Constrained nonlinear optimizacion

I have encountered this optimization problem while trying to implement the method proposed in this very interesting paper:

http://www.mae.cuhk.edu.hk/~cwang/pubs/JCISERealTimeSkeleton.pdf

the thing is that the optimization is a constrained nonlineal optimization. So far my colleagues and I have found a great solution using fmincon, a Matlab function which is part of the optimization toolbox. However we need to implement our solution in c++. While working with Matlab we got a warning saying that the software was going to use an active-set method, and here

http://www.mathworks.com/help/optim/ug/constrained-nonlinear-optimization-algorithms.html

it says that the way it handles the optimization is by using SQP (sequential quadratic programming), but for that it solves a quadratic subproblem , and I quote : "Here you simplify Equation 2-1 by assuming that bound constraints have been expressed as inequality constraints. You obtain the QP subproblem by linearizing the nonlinear constraints."

My question to this community is: how can I linearize the constraints, knowing that they are expressed in the form of a distance between two points that should be equal to a number that is well known ?(basically the constraint says that the lengths of the bones of a person must not change between different frames while being acquired with the kinect).

Any help in this matter will be greatly appreciated, I just need you to point me to the correct literature on how to solve this kind of optimization problems, since fmincon source code is not very clear.

Happy Holidays

Regarding the linearization done inside fmincon. It depends on how you express $||x-y||-l=0$. This form is probably bad as it is non-smooth. I would write it as
$$(x-y)^T(x-y)-l^2=0$$ The gradient is given by $2(x-y)$. Hence, the linearization at an iterate $(x_k,y_k)$ would be $$(x_k-y_k)^T(x_k-y_k) + 2(x_k-y_k)^T((x-x_k) - (y-y_k)) - l^2 = 0$$