Esempio n. 1
0
/**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;
}
Esempio n. 2
0
/**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;
}
Esempio n. 3
0
Vector3D Frame3D::getEulerXYZ() const
{
	Transform3D t;
	t = mAngleAxis;
	Vector3D ea = t.matrix().block<3, 3> (0, 0).eulerAngles(0, 1, 2);
	return ea;
}