MatX jointFrame(const Vect3 &loc, const Vect3 &axis_, const Vect3 &ref_) { Vect3 axis, ref, other; Mat3 R; axis.normalize(axis_); ref.displace(ref_, axis, - ref_.dot(axis)); // in case ref & axis not perp ref.normalize(ref); other.cross(axis, ref); R.setXcol(ref); R.setYcol(other); R.setZcol(axis); return MatX(R, loc); }