The Rotation class is a Mat33 that guarantees that the matrix is a legitimate 3x3 array associated with the relative orientation of two right-handed, orthogonal, unit vector bases. More...

#include <Rotation.h>

Inheritance diagram for SimTK::Rotation_:

List of all members.

Public Types
typedef UnitVec< P, Mat33P::RowSpacing >	ColType
	The column and row unit vector types do not necessarily have unit spacing.
typedef UnitRow< P, Mat33P::ColSpacing >	RowType
Public Member Functions
	Rotation_ ()
Rotation_ &	setRotationToIdentityMatrix ()
Rotation_ &	setRotationToNaN ()
	Rotation_ (const Rotation_ &R)
Rotation_ &	operator= (const Rotation_ &R)
	Rotation_ (BodyOrSpaceType bodyOrSpace, RealP angle1, const CoordinateAxis &axis1, RealP angle2, const CoordinateAxis &axis2)
	Constructor for two-angle, two-axes, Body-fixed or Space-fixed rotation sequences (angles are in radians)
	Rotation_ (BodyOrSpaceType bodyOrSpace, RealP angle1, const CoordinateAxis &axis1, RealP angle2, const CoordinateAxis &axis2, RealP angle3, const CoordinateAxis &axis3)
	Constructor for three-angle Body-fixed or Space-fixed rotation sequences (angles are in radians)
Rotation_ &	setRotationFromTwoAnglesTwoAxes (BodyOrSpaceType bodyOrSpace, RealP angle1, const CoordinateAxis &axis1, RealP angle2, const CoordinateAxis &axis2)
	Set this Rotation_ object to a two-angle, two-axes, Body-fixed or Space-fixed rotation sequences (angles are in radians)
Rotation_ &	setRotationFromThreeAnglesThreeAxes (BodyOrSpaceType bodyOrSpace, RealP angle1, const CoordinateAxis &axis1, RealP angle2, const CoordinateAxis &axis2, RealP angle3, const CoordinateAxis &axis3)
	Set this Rotation_ object to a three-angle Body-fixed or Space-fixed rotation sequences (angles are in radians)
void	setRotationToBodyFixedXY (const Vec2P &v)
	Set this Rotation_ to represent a rotation characterized by subsequent rotations of: +v[0] about the body frame's X axis, followed by a rotation of +v[1] about the body frame's NEW Y axis.
void	setRotationToBodyFixedXYZ (const Vec3P &v)
	Set this Rotation_ to represent a rotation characterized by subsequent rotations of: +v[0] about the body frame's X axis, followed by a rotation of +v[1] about the body frame's NEW Y axis, followed by a rotation of +v[2] about the body frame's NEW Z axis.
	Rotation_ (const QuaternionP &q)
	Constructor for creating a rotation matrix from a quaternion.
Rotation_ &	setRotationFromQuaternion (const QuaternionP &q)
	Method for creating a rotation matrix from a quaternion.
	Rotation_ (const Mat33P &m, bool)
	Construct a Rotation_ directly from a Mat33P (we trust that m is a valid Rotation_!)
	Rotation_ (const Mat33P &m)
	Constructs an (hopefully nearby) orthogonal rotation matrix from a generic Mat33P.
Rotation_ &	setRotationFromApproximateMat33 (const Mat33P &m)
	Set this Rotation_ object to an (hopefully nearby) orthogonal rotation matrix from a generic Mat33P.
RealP	convertOneAxisRotationToOneAngle (const CoordinateAxis &axis1) const
	Converts rotation matrix to a single orientation angle.
Vec2P	convertTwoAxesRotationToTwoAngles (BodyOrSpaceType bodyOrSpace, const CoordinateAxis &axis1, const CoordinateAxis &axis2) const
	Converts rotation matrix to two orientation angles.
Vec3P	convertThreeAxesRotationToThreeAngles (BodyOrSpaceType bodyOrSpace, const CoordinateAxis &axis1, const CoordinateAxis &axis2, const CoordinateAxis &axis3) const
	Converts rotation matrix to three orientation angles.
QuaternionP	convertRotationToQuaternion () const
	Converts rotation matrix to a quaternion.
Vec4P	convertRotationToAngleAxis () const
	Converts rotation matrix to angle-axis form.
Vec2P	convertRotationToBodyFixedXY () const
	A convenient special case of convertTwoAxesRotationToTwoAngles().
Vec3P	convertRotationToBodyFixedXYZ () const
	A convenient special case of convertThreeAxesRotationToThreeAngles().
SymMat33P	reexpressSymMat33 (const SymMat33P &S_BB) const
	Perform an efficient transform of a symmetric matrix that must be re-expressed with a multiply from both left and right, such as an inertia matrix.
RealP	getMaxAbsDifferenceInRotationElements (const Rotation_ &R) const
bool	areAllRotationElementsSameToEpsilon (const Rotation_ &R, RealP epsilon) const
bool	areAllRotationElementsSameToMachinePrecision (const Rotation_ &R) const
	Rotation_ (const InverseRotation_< P > &)
	Like copy constructor but for inverse rotation. This allows implicit conversion from InverseRotation_ to Rotation_.
Rotation_ &	operator= (const InverseRotation_< P > &)
	Like copy assignment but for inverse rotation.
const InverseRotation_< P > &	invert () const
	Convert from Rotation_ to InverseRotation_ (no cost). Overrides base class invert().
InverseRotation_< P > &	updInvert ()
	Convert from Rotation_ to writable InverseRotation_ (no cost).
const RowType &	row (int i) const
const ColType &	col (int j) const
const ColType &	x () const
const ColType &	y () const
const ColType &	z () const
const RowType &	operator[] (int i) const
const ColType &	operator() (int j) const

	Rotation_ (RealP angle, const CoordinateAxis &axis)
	Constructor for right-handed rotation by an angle (in radians) about a coordinate axis.
	Rotation_ (RealP angle, const CoordinateAxis::XCoordinateAxis)
	Constructor for right-handed rotation by an angle (in radians) about a coordinate axis.
	Rotation_ (RealP angle, const CoordinateAxis::YCoordinateAxis)
	Constructor for right-handed rotation by an angle (in radians) about a coordinate axis.
	Rotation_ (RealP angle, const CoordinateAxis::ZCoordinateAxis)
	Constructor for right-handed rotation by an angle (in radians) about a coordinate axis.

Rotation_ &	setRotationFromAngleAboutAxis (RealP angle, const CoordinateAxis &axis)
	Set this Rotation_ object to a right-handed rotation by an angle (in radians) about a coordinate axis.
Rotation_ &	setRotationFromAngleAboutX (RealP angle)
	Set this Rotation_ object to a right-handed rotation by an angle (in radians) about a coordinate axis.
Rotation_ &	setRotationFromAngleAboutY (RealP angle)
	Set this Rotation_ object to a right-handed rotation by an angle (in radians) about a coordinate axis.
Rotation_ &	setRotationFromAngleAboutZ (RealP angle)
	Set this Rotation_ object to a right-handed rotation by an angle (in radians) about a coordinate axis.
Rotation_ &	setRotationFromAngleAboutX (RealP cosAngle, RealP sinAngle)
	Set this Rotation_ object to a right-handed rotation by an angle (in radians) about a coordinate axis.
Rotation_ &	setRotationFromAngleAboutY (RealP cosAngle, RealP sinAngle)
	Set this Rotation_ object to a right-handed rotation by an angle (in radians) about a coordinate axis.
Rotation_ &	setRotationFromAngleAboutZ (RealP cosAngle, RealP sinAngle)
	Set this Rotation_ object to a right-handed rotation by an angle (in radians) about a coordinate axis.

	Rotation_ (RealP angle, const UnitVec3P &unitVector)
	Constructor for right-handed rotation by an angle (in radians) about an arbitrary vector.
	Rotation_ (RealP angle, const Vec3P &nonUnitVector)
	Constructor for right-handed rotation by an angle (in radians) about an arbitrary vector.

Rotation_ &	setRotationFromAngleAboutNonUnitVector (RealP angle, const Vec3P &nonUnitVector)
	Set this Rotation_ object to a right-handed rotation of an angle (in radians) about an arbitrary vector.
Rotation_ &	setRotationFromAngleAboutUnitVector (RealP angle, const UnitVec3P &unitVector)
	Set this Rotation_ object to a right-handed rotation of an angle (in radians) about an arbitrary vector.

	Rotation_ (const UnitVec3P &uvec, const CoordinateAxis axis)
	Calculate R_AB by knowing one of B's unit vector expressed in A.
Rotation_ &	setRotationFromOneAxis (const UnitVec3P &uvec, const CoordinateAxis axis)
	Calculate R_AB by knowing one of B's unit vector expressed in A.

	Rotation_ (const UnitVec3P &uveci, const CoordinateAxis &axisi, const Vec3P &vecjApprox, const CoordinateAxis &axisjApprox)
	Calculate R_AB by knowing one of B's unit vectors u1 (could be Bx, By, or Bz) expressed in A and a vector v (also expressed in A) that is approximately in the desired direction for a second one of B's unit vectors, u2 (!= u1).
Rotation_ &	setRotationFromTwoAxes (const UnitVec3P &uveci, const CoordinateAxis &axisi, const Vec3P &vecjApprox, const CoordinateAxis &axisjApprox)
	Calculate R_AB by knowing one of B's unit vectors u1 (could be Bx, By, or Bz) expressed in A and a vector v (also expressed in A) that is approximately in the desired direction for a second one of B's unit vectors, u2 (!= u1).

bool	isSameRotationToWithinAngle (const Rotation_ &R, RealP okPointingAngleErrorRads) const
	Return true if "this" Rotation is nearly identical to "R" within a specified pointing angle error.
bool	isSameRotationToWithinAngleOfMachinePrecision (const Rotation_ &R) const
	Return true if "this" Rotation is nearly identical to "R" within a specified pointing angle error.

const InverseRotation_< P > &	transpose () const
	Transpose, and transpose operators.
const InverseRotation_< P > &	operator~ () const
	Transpose, and transpose operators.
InverseRotation_< P > &	updTranspose ()
	Transpose, and transpose operators.
InverseRotation_< P > &	operator~ ()
	Transpose, and transpose operators.

Rotation_ &	operator*= (const Rotation_< P > &R)
	In-place composition of Rotation matrices.
Rotation_ &	operator/= (const Rotation_< P > &R)
	In-place composition of Rotation matrices.
Rotation_ &	operator*= (const InverseRotation_< P > &)
	In-place composition of Rotation matrices.
Rotation_ &	operator/= (const InverseRotation_< P > &)
	In-place composition of Rotation matrices.

const Mat33P &	asMat33 () const
	Conversion from Rotation to its base class Mat33.
Mat33P	toMat33 () const
	Conversion from Rotation to its base class Mat33.

Rotation_ &	setRotationFromMat33TrustMe (const Mat33P &m)
	Set the Rotation_ matrix directly - but you had better know what you are doing!
Rotation_ &	setRotationColFromUnitVecTrustMe (int colj, const UnitVec3P &uvecj)
	Set the Rotation_ matrix directly - but you had better know what you are doing!
Rotation_ &	setRotationFromUnitVecsTrustMe (const UnitVec3P &colA, const UnitVec3P &colB, const UnitVec3P &colC)
	Set the Rotation_ matrix directly - but you had better know what you are doing!
Static Public Member Functions
static Vec3P	convertAngVelToBodyFixed321Dot (const Vec3P &q, const Vec3P &w_PB_B)
	Given Euler angles forming a body-fixed 3-2-1 sequence, and the relative angular velocity vector of B in the parent frame, BUT EXPRESSED IN THE BODY FRAME, return the Euler angle derivatives.
static Vec3P	convertBodyFixed321DotToAngVel (const Vec3P &q, const Vec3P &qd)
	Inverse of the above routine.
static Vec3P	convertAngVelDotToBodyFixed321DotDot (const Vec3P &q, const Vec3P &w_PB_B, const Vec3P &wdot)
static Mat33P	calcNForBodyXYZInBodyFrame (const Vec3P &q)
	Given Euler angles q forming a body-fixed X-Y-Z sequence return the block of the N matrix such that qdot=N(q)w where w is the angular velocity of B in P EXPRESSED IN B*!!! This matrix will be singular if Y (q[1]) gets near 90 degrees!
static Mat33P	calcNForBodyXYZInBodyFrame (const Vec3P &cq, const Vec3P &sq)
	This fast version of calcNForBodyXYZInBodyFrame() assumes you have already calculated the cosine and sine of the three q's.
static Mat33P	calcNDotForBodyXYZInBodyFrame (const Vec3P &q, const Vec3P &qdot)
	Given Euler angles forming a body-fixed X-Y-Z sequence q, and their time derivatives qdot, return the block of the NDot matrix such that qdotdot=N(q)wdot + NDot(q,u)w where w is the angular velocity of B in P EXPRESSED IN B!!! This matrix will be singular if Y (q[1]) gets near 90 degrees! See calcNForBodyXYZInBodyFrame() for the matrix we're differentiating here.
static Mat33P	calcNDotForBodyXYZInBodyFrame (const Vec3P &cq, const Vec3P &sq, const Vec3P &qdot)
	This fast version of calcNDotForBodyXYZInBodyFrame() assumes you have already calculated the cosine and sine of the three q's.
static Mat33P	calcNInvForBodyXYZInBodyFrame (const Vec3P &q)
	Inverse of the above routine.
static Mat33P	calcNInvForBodyXYZInBodyFrame (const Vec3P &cq, const Vec3P &sq)
	This fast version of calcNInvForBodyXYZInBodyFrame() assumes you have already calculated the cosine and sine of the three q's.
static Vec3P	convertAngVelInBodyFrameToBodyXYZDot (const Vec3P &q, const Vec3P &w_PB_B)
	Given Euler angles forming a body-fixed 1-2-3 sequence, and the relative angular velocity vector of B in the parent frame, BUT EXPRESSED IN THE BODY FRAME, return the Euler angle derivatives.
static Vec3P	convertAngVelInBodyFrameToBodyXYZDot (const Vec3P &cq, const Vec3P &sq, const Vec3P &w_PB_B)
	This fast version of convertAngVelInBodyFrameToBodyXYZDot() assumes you have already calculated the cosine and sine of the three q's.
static Vec3P	convertBodyXYZDotToAngVelInBodyFrame (const Vec3P &q, const Vec3P &qdot)
	Inverse of the above routine.
static Vec3P	convertBodyXYZDotToAngVelInBodyFrame (const Vec3P &cq, const Vec3P &sq, const Vec3P &qdot)
	This fast version of convertBodyXYZDotToAngVelInBodyFrame() assumes you have already calculated the cosine and sine of the three q's.
static Vec3P	convertAngVelDotInBodyFrameToBodyXYZDotDot (const Vec3P &q, const Vec3P &w_PB_B, const Vec3P &wdot_PB_B)
	TODO: sherm: is this right? Warning: everything is measured in the PARENT* frame, but has to be expressed in the BODY frame.
static Vec3P	convertAngVelDotInBodyFrameToBodyXYZDotDot (const Vec3P &cq, const Vec3P &sq, const Vec3P &w_PB_B, const Vec3P &wdot_PB_B)
	This faster version of convertAngVelDotInBodyFrameToBodyXYZDotDot() assumes you have already calculated the cosine and sine of the three q's.
static Mat< 4, 3, P >	calcUnnormalizedNForQuaternion (const Vec4P &q)
	Given a possibly unnormalized quaternion q, calculate the 4x3 matrix N which maps angular velocity w to quaternion derivatives qdot.
static Mat< 4, 3, P >	calcUnnormalizedNDotForQuaternion (const Vec4P &qdot)
	Given the time derivative qdot of a possibly unnormalized quaternion q, calculate the 4x3 matrix NDot which is the time derivative of the matrix N as described in calcUnnormalizedNForQuaternion().
static Mat< 3, 4, P >	calcUnnormalizedNInvForQuaternion (const Vec4P &q)
	Given a (possibly unnormalized) quaternion q, calculate the 3x4 matrix NInv (= N^-1) which maps quaternion derivatives qdot to angular velocity w, where the angular velocity is in the parent frame, i.e.
static Vec4P	convertAngVelToQuaternionDot (const Vec4P &q, const Vec3P &w_PB_P)
	Given a possibly unnormalized quaternion (0th element is the scalar) and the relative angular velocity vector of B in its parent, expressed in the PARENT, return the quaternion derivatives.
static Vec3P	convertQuaternionDotToAngVel (const Vec4P &q, const Vec4P &qdot)
	Inverse of the above routine.
static Vec4P	convertAngVelDotToQuaternionDotDot (const Vec4P &q, const Vec3P &w_PB_P, const Vec3P &wdot_PB_P)
	Given a quaternion q representing R_PB, angular velocity of B in P, and the time derivative of the angular velocity, return the second time derivative qdotdot of the quaternion.

Detailed Description

template<class P>
class SimTK::Rotation_

The Rotation class is a Mat33 that guarantees that the matrix is a legitimate 3x3 array associated with the relative orientation of two right-handed, orthogonal, unit vector bases.

The Rotation class takes advantage of known properties of orthogonal matrices. For example, multiplication by a rotation matrix preserves a vector's length so unit vectors are still unit vectors afterwards and don't need to be re-normalized.

A rotation is an orthogonal matrix whose columns and rows are directions (that is, unit vectors) that are mutually orthogonal. Furthermore, if the columns (or rows) are labeled x,y,z it always holds that z = x X y (rather than -(x X y)) ensuring that this is a right-handed rotation matrix and not a reflection. This is equivalent to saying that the determinant of a rotation matrix is 1, not -1.

Suppose there is a vector v_F expressed in terms of the right-handed, orthogonal unit vectors Fx, Fy, Fz and one would like to express v instead as v_G, in terms of a right-handed, orthogonal unit vectors Gx, Gy, Gz. To calculate it, we form a rotation matrix R_GF whose columns are the F unit vectors re-expressed in G:

             G F   (      |      |      )
      R_GF =  R  = ( Fx_G | Fy_G | Fz_G )
                   (      |      |      )
 where
      Fx_G = ~( ~Fx*Gx, ~Fx*Gy, ~Fx*Gz ), etc.

(~Fx*Gx means dot(Fx,Gx)). Note that we use "monogram" notation R_GF in code to represent the more typographically demanding superscripted notation for rotation matrices. Now we can re-express the vector v from frame F to frame G via

      v_G = R_GF * v_F.

Because a rotation is orthogonal, its transpose is its inverse. Hence R_FG = ~R_GF (where ~ is the SimTK "transpose" operator). This transpose matrix can be used to expressed v_G in terms of Fx, Fy, Fz as

      v_F = R_FG * v_G  or  v_F = ~R_GF * v_G

In either direction, correct behavior can be obtained by using the recommended notation and then matching up the frame labels (after interpreting the "~" operator as reversing the labels).

Member Typedef Documentation

template<class P>

typedef UnitVec<P,Mat33P::RowSpacing> SimTK::Rotation_::ColType

The column and row unit vector types do not necessarily have unit spacing.

template<class P>

typedef UnitRow<P,Mat33P::ColSpacing> SimTK::Rotation_::RowType

Constructor & Destructor Documentation

template<class P>

SimTK::Rotation_::Rotation_ ( ) [inline]

template<class P>

SimTK::Rotation_::Rotation_ ( const Rotation_ & R ) [inline]

template<class P>

SimTK::Rotation_< P >::Rotation_	(	RealP	angle,
		const CoordinateAxis &	axis
	)		`[inline]`

Constructor for right-handed rotation by an angle (in radians) about a coordinate axis.

template<class P>

SimTK::Rotation_< P >::Rotation_	(	RealP	angle,
		const CoordinateAxis::XCoordinateAxis
	)		`[inline]`

Constructor for right-handed rotation by an angle (in radians) about a coordinate axis.

template<class P>

SimTK::Rotation_< P >::Rotation_	(	RealP	angle,
		const CoordinateAxis::YCoordinateAxis
	)		`[inline]`

Constructor for right-handed rotation by an angle (in radians) about a coordinate axis.

template<class P>

SimTK::Rotation_< P >::Rotation_	(	RealP	angle,
		const CoordinateAxis::ZCoordinateAxis
	)		`[inline]`

Constructor for right-handed rotation by an angle (in radians) about a coordinate axis.

template<class P>

SimTK::Rotation_< P >::Rotation_	(	RealP	angle,
		const UnitVec3P &	unitVector
	)		`[inline]`

Constructor for right-handed rotation by an angle (in radians) about an arbitrary vector.

template<class P>

SimTK::Rotation_< P >::Rotation_	(	RealP	angle,
		const Vec3P &	nonUnitVector
	)		`[inline]`

Constructor for right-handed rotation by an angle (in radians) about an arbitrary vector.

template<class P>

SimTK::Rotation_< P >::Rotation_	(	BodyOrSpaceType	bodyOrSpace,
		RealP	angle1,
		const CoordinateAxis &	axis1,
		RealP	angle2,
		const CoordinateAxis &	axis2
	)		`[inline]`

Constructor for two-angle, two-axes, Body-fixed or Space-fixed rotation sequences (angles are in radians)

template<class P>

SimTK::Rotation_< P >::Rotation_	(	BodyOrSpaceType	bodyOrSpace,
		RealP	angle1,
		const CoordinateAxis &	axis1,
		RealP	angle2,
		const CoordinateAxis &	axis2,
		RealP	angle3,
		const CoordinateAxis &	axis3
	)		`[inline]`

Constructor for three-angle Body-fixed or Space-fixed rotation sequences (angles are in radians)

template<class P>

SimTK::Rotation_::Rotation_ ( const QuaternionP & q ) [inline, explicit]

Constructor for creating a rotation matrix from a quaternion.

template<class P>

SimTK::Rotation_< P >::Rotation_	(	const Mat33P &	m,
		bool
	)		`[inline]`

Construct a Rotation_ directly from a Mat33P (we trust that m is a valid Rotation_!)

template<class P>

SimTK::Rotation_::Rotation_ ( const Mat33P & m ) [inline, explicit]

Constructs an (hopefully nearby) orthogonal rotation matrix from a generic Mat33P.

template<class P>

SimTK::Rotation_< P >::Rotation_	(	const UnitVec3P &	uvec,
		const CoordinateAxis	axis
	)		`[inline]`

Calculate R_AB by knowing one of B's unit vector expressed in A.

Note: The other vectors are perpendicular (but somewhat arbitrarily so).

template<class P>

SimTK::Rotation_< P >::Rotation_	(	const UnitVec3P &	uveci,
		const CoordinateAxis &	axisi,
		const Vec3P &	vecjApprox,
		const CoordinateAxis &	axisjApprox
	)		`[inline]`

Calculate R_AB by knowing one of B's unit vectors u1 (could be Bx, By, or Bz) expressed in A and a vector v (also expressed in A) that is approximately in the desired direction for a second one of B's unit vectors, u2 (!= u1).

If v is not perpendicular to u1, no worries - we'll find a direction for u2 that is perpendicular to u1 and comes closest to v. The third vector u3 is +/- u1 X u2, as appropriate for a right-handed rotation matrix.

template<class P>

SimTK::Rotation_::Rotation_ ( const InverseRotation_ & R ) [inline]

Like copy constructor but for inverse rotation. This allows implicit conversion from InverseRotation_ to Rotation_.

Member Function Documentation

template<class P>

Rotation_& SimTK::Rotation_::setRotationToIdentityMatrix ( ) [inline]

template<class P>

Rotation_& SimTK::Rotation_::setRotationToNaN ( ) [inline]

template<class P>

Rotation_& SimTK::Rotation_::operator= ( const Rotation_ & R ) [inline]

template<class P>

Rotation_& SimTK::Rotation_< P >::setRotationFromAngleAboutAxis	(	RealP	angle,
		const CoordinateAxis &	axis
	)		`[inline]`