RelativeQuatConstraint re-implemented in MATLAB