/**Create a transform to a space defined by an origin and two perpendicular unit vectors that
* for the x-y plane.
* The original space is A and the space defined by ijc are B
* The returned transform M_AB converts a point in B to A:
* p_A = M_AB * p_B
*/
Transform3D createTransformIJC(const Vector3D& ivec, const Vector3D& jvec, const Vector3D& center)
{
Transform3D t = Transform3D::Identity();
t.matrix().col(0).head(3) = ivec;
t.matrix().col(1).head(3) = jvec;
t.matrix().col(2).head(3) = cross(ivec, jvec);
t.matrix().col(3).head(3) = center;
return t;
}
/**Create from affine matrix
*
*/
Frame3D Frame3D::create(const Transform3D& T)
{
Frame3D retval;
Eigen::Matrix3d R = T.matrix().block<3, 3> (0, 0); // extract rotational part
retval.mAngleAxis = Eigen::AngleAxisd(R); // construct angle axis from R
retval.mPos = T.matrix().block<3, 1> (0, 3); // extract translational part
return retval;
}
Vector3D Frame3D::getEulerXYZ() const
{
Transform3D t;
t = mAngleAxis;
Vector3D ea = t.matrix().block<3, 3> (0, 0).eulerAngles(0, 1, 2);
return ea;
}