Simbody  3.5
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Modules Pages
SimTK::Rotation_< P > Class Template Reference

The Rotation class is a Mat33 that guarantees that the matrix can be interpreted as a legitimate 3x3 rotation matrix giving the relative orientation of two right-handed, orthogonal, unit vector bases. More...

+ Inheritance diagram for SimTK::Rotation_< P >:

Public Types

typedef P RealP
 These are just local abbreviations. More...
 
typedef Mat< 2, 2, P > Mat22P
 
typedef Mat< 3, 2, P > Mat32P
 
typedef Mat< 3, 3, P > Mat33P
 
typedef Mat< 4, 3, P > Mat43P
 
typedef Mat< 3, 4, P > Mat34P
 
typedef Vec< 2, P > Vec2P
 
typedef Vec< 3, P > Vec3P
 
typedef Vec< 4, P > Vec4P
 
typedef UnitVec< P, 1 > UnitVec3P
 
typedef SymMat< 3, P > SymMat33P
 
typedef Quaternion_< P > QuaternionP
 
typedef UnitVec< P,
Mat33P::RowSpacing
ColType
 This is the type of a column of this Rotation matrix. More...
 
typedef UnitRow< P,
Mat33P::ColSpacing
RowType
 This is the type of a row of this Rotation matrix. More...
 
- Public Types inherited from SimTK::Mat< 3, 3, P >
enum  
 Every Composite Numerical Type (CNT) must define these values. More...
 
typedef Mat< M, N, E, CS, RS > T
 
typedef Mat< M, N, ENeg, CS, RS > TNeg
 
typedef Mat< M, N,
EWithoutNegator, CS, RS > 
TWithoutNegator
 
typedef Mat< M, N, EReal, CS
*CNT< E >::RealStrideFactor,
RS *CNT< E >::RealStrideFactor
TReal
 
typedef Mat< M, N, EImag, CS
*CNT< E >::RealStrideFactor,
RS *CNT< E >::RealStrideFactor
TImag
 
typedef Mat< M, N, EComplex,
CS, RS > 
TComplex
 
typedef Mat< N, M, EHerm, RS, CS > THerm
 
typedef Mat< N, M, E, RS, CS > TPosTrans
 
typedef E TElement
 
typedef Row< N, E, CS > TRow
 
typedef Vec< M, E, RS > TCol
 
typedef Vec< MinDim, E, RS+CS > TDiag
 
typedef Mat< M, N, ESqrt, M, 1 > TSqrt
 
typedef Mat< M, N, EAbs, M, 1 > TAbs
 
typedef Mat< M, N, EStandard,
M, 1 > 
TStandard
 
typedef Mat< N, M, EInvert, N, 1 > TInvert
 
typedef Mat< M, N, ENormalize,
M, 1 > 
TNormalize
 
typedef SymMat< N, ESqHermT > TSqHermT
 
typedef SymMat< M, ESqTHerm > TSqTHerm
 
typedef Mat< M, N, E, M, 1 > TPacked
 
typedef Mat< M-1, N, E, M-1, 1 > TDropRow
 
typedef Mat< M, N-1, E, M, 1 > TDropCol
 
typedef Mat< M-1, N-1, E, M-1, 1 > TDropRowCol
 
typedef Mat< M+1, N, E, M+1, 1 > TAppendRow
 
typedef Mat< M, N+1, E, M, 1 > TAppendCol
 
typedef Mat< M+1, N+1, E, M+1, 1 > TAppendRowCol
 
typedef EScalar Scalar
 
typedef EULessScalar ULessScalar
 
typedef ENumber Number
 
typedef EStdNumber StdNumber
 
typedef EPrecision Precision
 
typedef EScalarNormSq ScalarNormSq
 
typedef THerm TransposeType
 

Constructors, Mutators, and Assignment

 Rotation_ ()
 Default constructor. More...
 
 Rotation_ (const Rotation_ &R)
 Copy constructor. More...
 
 Rotation_ (const InverseRotation_< P > &)
 Like copy constructor but for inverse rotation. More...
 
Rotation_operator= (const Rotation_ &R)
 Assignment operator. More...
 
Rotation_operator= (const InverseRotation_< P > &)
 Like copy assignment but for inverse rotation. More...
 
Rotation_setRotationToNaN ()
 Construct Rotation_ filled with NaNs. More...
 
Rotation_setRotationToIdentityMatrix ()
 Construct identity Rotation_. More...
 
 Rotation_ (RealP angle, const CoordinateAxis &axis)
 Constructor for right-handed rotation by an angle (in radians) about a coordinate axis. More...
 
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. More...
 
 Rotation_ (RealP angle, const CoordinateAxis::XCoordinateAxis)
 Constructor for right-handed rotation by an angle (in radians) about the X-axis. More...
 
Rotation_setRotationFromAngleAboutX (RealP angle)
 Set this Rotation_ object to a right-handed rotation by an angle (in radians) about the X-axis. More...
 
Rotation_setRotationFromAngleAboutX (RealP cosAngle, RealP sinAngle)
 Set this Rotation_ object to a right-handed rotation by an angle about the X-axis, where the cosine and sine of the angle are specified. More...
 
 Rotation_ (RealP angle, const CoordinateAxis::YCoordinateAxis)
 Constructor for right-handed rotation by an angle (in radians) about the Y-axis. More...
 
Rotation_setRotationFromAngleAboutY (RealP angle)
 Set this Rotation_ object to a right-handed rotation by an angle (in radians) about the Y-axis. More...
 
Rotation_setRotationFromAngleAboutY (RealP cosAngle, RealP sinAngle)
 Set this Rotation_ object to a right-handed rotation by an angle about the Y-axis, where the cosine and sine of the angle are specified. More...
 
 Rotation_ (RealP angle, const CoordinateAxis::ZCoordinateAxis)
 Constructor for right-handed rotation by an angle (in radians) about the Z-axis. More...
 
Rotation_setRotationFromAngleAboutZ (RealP angle)
 Set this Rotation_ object to a right-handed rotation by an angle (in radians) about the Z-axis. More...
 
Rotation_setRotationFromAngleAboutZ (RealP cosAngle, RealP sinAngle)
 Set this Rotation_ object to a right-handed rotation by an angle about the Z-axis, where the cosine and sine of the angle are specified. More...
 
 Rotation_ (RealP angle, const UnitVec3P &unitVector)
 Constructor for right-handed rotation by an angle (in radians) about an arbitrary unit vector. More...
 
Rotation_setRotationFromAngleAboutUnitVector (RealP angle, const UnitVec3P &unitVector)
 Set this Rotation_ object to a right-handed rotation of an angle (in radians) about an arbitrary unit vector. More...
 
 Rotation_ (RealP angle, const Vec3P &nonUnitVector)
 Constructor for right-handed rotation by an angle (in radians) about an arbitrary vector of arbitrary length. More...
 
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 of arbitrary length. More...
 
 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). More...
 
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). More...
 
 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). More...
 
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). More...
 
 Rotation_ (const QuaternionP &q)
 Constructor for creating a rotation matrix from a quaternion. More...
 
Rotation_setRotationFromQuaternion (const QuaternionP &q)
 Method for creating a rotation matrix from a quaternion. More...
 
 Rotation_ (const Mat33P &m)
 Constructs an (hopefully nearby) orthogonal rotation matrix from a generic Mat33P. More...
 
Rotation_setRotationFromApproximateMat33 (const Mat33P &m)
 Set this Rotation_ object to an (hopefully nearby) orthogonal rotation matrix from a generic Mat33P. More...
 
 Rotation_ (const Mat33P &m, bool)
 (Advanced) Construct a Rotation_ directly from a Mat33P (we trust that m is a valid Rotation_!) Things will not go well for you if it is not. More...
 
Rotation_setRotationFromMat33TrustMe (const Mat33P &m)
 (Advanced) Set the Rotation_ matrix directly - but you had better know what you are doing! More...
 
Rotation_setRotationColFromUnitVecTrustMe (int colj, const UnitVec3P &uvecj)
 (Advanced) Set the Rotation_ matrix directly - but you had better know what you are doing! More...
 
Rotation_setRotationFromUnitVecsTrustMe (const UnitVec3P &colA, const UnitVec3P &colB, const UnitVec3P &colC)
 (Advanced) Set the Rotation_ matrix directly - but you had better know what you are doing! More...
 
 Rotation_ (const UnitVec3P &uvec, CoordinateAxis axis)
 Calculate R_AB by knowing one of B's unit vectors expressed in A. More...
 
Rotation_setRotationFromOneAxis (const UnitVec3P &uvec, CoordinateAxis axis)
 Calculate R_AB by knowing one of B's unit vectors expressed in A. More...
 
 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). More...
 
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). More...
 
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. More...
 
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 (twice rotated) Z axis. More...
 
void setRotationToBodyFixedXYZ (const Vec3P &c, const Vec3P &s)
 Given cosines and sines (in that order) of three angles, set this Rotation matrix to the body-fixed 1-2-3 sequence of those angles. More...
 

Operators and Arithmetic

const InverseRotation_< P > & operator~ () const
 Transpose operator. More...
 
InverseRotation_< P > & operator~ ()
 Transpose operator. More...
 
const InverseRotation_< P > & transpose () const
 Transpose. More...
 
InverseRotation_< P > & updTranspose ()
 Transpose. More...
 
const InverseRotation_< P > & invert () const
 Convert from Rotation_ to InverseRotation_ (no cost). More...
 
InverseRotation_< P > & updInvert ()
 Convert from Rotation_ to writable InverseRotation_ (no cost). More...
 
Rotation_operator*= (const Rotation_< P > &R)
 In-place composition of Rotation matrices. More...
 
Rotation_operator*= (const InverseRotation_< P > &)
 In-place composition of Rotation matrices. More...
 
Rotation_operator/= (const Rotation_< P > &R)
 In-place composition of Rotation matrices. More...
 
Rotation_operator/= (const InverseRotation_< P > &)
 In-place composition of Rotation matrices. More...
 
static Vec3P multiplyByBodyXYZ_N_P (const Vec2P &cosxy, const Vec2P &sinxy, RealP oocosy, const Vec3P &w_PB)
 This is the fastest way to form the product qdot=N_P*w_PB for a body-fixed XYZ sequence where angular velocity of child in parent is expected to be expressed in the parent. More...
 
static Vec3P multiplyByBodyXYZ_NT_P (const Vec2P &cosxy, const Vec2P &sinxy, RealP oocosy, const Vec3P &q)
 This is the fastest way to form the product v_P=~N_P*q=~(~q*N_P); see the untransposed method multiplyByBodyXYZ_N_P() for information. More...
 
static Vec3P multiplyByBodyXYZ_NInv_P (const Vec2P &cosxy, const Vec2P &sinxy, const Vec3P &qdot)
 Fastest way to form the product w_PB=NInv_P*qdot. More...
 
static Vec3P multiplyByBodyXYZ_NInvT_P (const Vec2P &cosxy, const Vec2P &sinxy, const Vec3P &v_P)
 Fastest way to form the product q=~NInv_P*v_P=~(~v_P*NInv_P). More...
 

Accessors

const RowTyperow (int i) const
 Return a reference to the ith row of this Rotation matrix as a UnitRow3. More...
 
const RowTypeoperator[] (int i) const
 Same as row(i) but nicer to look at. More...
 
const ColTypecol (int j) const
 Return a reference to the jth column of this Rotation matrix as a UnitVec3. More...
 
const ColTypeoperator() (int j) const
 Same as col(j) but nicer to look at. More...
 
const ColTypex () const
 Return col(0) of this Rotation matrix as a UnitVec3. More...
 
const ColTypey () const
 Return col(1) of this Rotation matrix as a UnitVec3. More...
 
const ColTypez () const
 Return col(2) of this Rotation matrix as a UnitVec3. More...
 
const ColTypegetAxisUnitVec (CoordinateAxis axis) const
 Given a CoordinateAxis (XAxis,YAxis, or ZAxis) return a reference to the corresponding column of this Rotation matrix. More...
 
const UnitVec< P, 1 > getAxisUnitVec (CoordinateDirection dir) const
 Given a CoordinateDirection (+/-XAxis, etc.) return a unit vector in that direction. More...
 

Calculations

static Mat33P calcNForBodyXYZInBodyFrame (const Vec3P &q)
 Given Euler angles q forming a body-fixed X-Y-Z sequence return the block N_B of the system N matrix such that qdot=N_B(q)*w_PB_B where w_PB_B is the angular velocity of B in P EXPRESSED IN *B*!!! Note that N_B=N_P*R_PB. More...
 
static Mat33P calcNForBodyXYZInBodyFrame (const Vec3P &cq, const Vec3P &sq)
 This faster version of calcNForBodyXYZInBodyFrame() assumes you have already calculated the cosine and sine of the three q's. More...
 
static Mat33P calcNForBodyXYZInParentFrame (const Vec3P &q)
 Given Euler angles q forming a body-fixed X-Y-Z (123) sequence return the block N_P of the system N matrix such that qdot=N_P(q)*w_PB where w_PB is the angular velocity of B in P expressed in P (not the convention that Kane uses, where angular velocities are expressed in the outboard body B). More...
 
static Mat33P calcNForBodyXYZInParentFrame (const Vec3P &cq, const Vec3P &sq)
 This faster version of calcNForBodyXYZInParentFrame() assumes you have already calculated the cosine and sine of the three q's. More...
 
static Mat33P calcNDotForBodyXYZInBodyFrame (const Vec3P &q, const Vec3P &qdot)
 Given Euler angles forming a body-fixed X-Y-Z (123) 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. More...
 
static Mat33P calcNDotForBodyXYZInBodyFrame (const Vec3P &cq, const Vec3P &sq, const Vec3P &qdot)
 This faster version of calcNDotForBodyXYZInBodyFrame() assumes you have already calculated the cosine and sine of the three q's. More...
 
static Mat33P calcNDotForBodyXYZInParentFrame (const Vec3P &q, const Vec3P &qdot)
 Given Euler angles forming a body-fixed X-Y-Z (123) 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 P. More...
 
static Mat33P calcNDotForBodyXYZInParentFrame (const Vec2P &cq, const Vec2P &sq, RealP ooc1, const Vec3P &qdot)
 This faster version of calcNDotForBodyXYZInParentFrame() assumes you have already calculated the cosine and sine of the three q's. More...
 
static Mat33P calcNInvForBodyXYZInBodyFrame (const Vec3P &q)
 Inverse of routine calcNForBodyXYZInBodyFrame(). More...
 
static Mat33P calcNInvForBodyXYZInBodyFrame (const Vec3P &cq, const Vec3P &sq)
 This faster version of calcNInvForBodyXYZInBodyFrame() assumes you have already calculated the cosine and sine of the three q's. More...
 
static Mat33P calcNInvForBodyXYZInParentFrame (const Vec3P &q)
 Inverse of the above routine. More...
 
static Mat33P calcNInvForBodyXYZInParentFrame (const Vec3P &cq, const Vec3P &sq)
 This faster version of calcNInvForBodyXYZInParentFrame() assumes you have already calculated the cosine and sine of the three q's. More...
 
static Mat43P calcUnnormalizedNForQuaternion (const Vec4P &q)
 Given a possibly unnormalized quaternion q, calculate the 4x3 matrix N which maps angular velocity w to quaternion derivatives qdot. More...
 
static Mat43P 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(). More...
 
static Mat34P 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. More...
 

Conversions

const Mat33PasMat33 () const
 Conversion from Rotation to its base class Mat33. More...
 
Mat33P toMat33 () const
 Conversion from Rotation to its base class Mat33. More...
 
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. More...
 
RealP convertOneAxisRotationToOneAngle (const CoordinateAxis &axis1) const
 Converts rotation matrix to a single orientation angle. More...
 
Vec2P convertTwoAxesRotationToTwoAngles (BodyOrSpaceType bodyOrSpace, const CoordinateAxis &axis1, const CoordinateAxis &axis2) const
 Converts rotation matrix to two orientation angles. More...
 
Vec3P convertThreeAxesRotationToThreeAngles (BodyOrSpaceType bodyOrSpace, const CoordinateAxis &axis1, const CoordinateAxis &axis2, const CoordinateAxis &axis3) const
 Converts rotation matrix to three orientation angles. More...
 
QuaternionP convertRotationToQuaternion () const
 Converts rotation matrix to an equivalent quaternion in canonical form (meaning its scalar element is nonnegative). More...
 
Vec4P convertRotationToAngleAxis () const
 Converts rotation matrix to an equivalent angle-axis representation in canonicalized form. More...
 
Vec2P convertRotationToBodyFixedXY () const
 A convenient special case of convertTwoAxesRotationToTwoAngles(). More...
 
Vec3P convertRotationToBodyFixedXYZ () const
 A convenient special case of convertThreeAxesRotationToThreeAngles(). More...
 
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. More...
 
static Vec3P convertBodyFixed321DotToAngVel (const Vec3P &q, const Vec3P &qd)
 Inverse of convertAngVelToBodyFixed321Dot. More...
 
static Vec3P convertAngVelDotToBodyFixed321DotDot (const Vec3P &q, const Vec3P &w_PB_B, const Vec3P &wdot_PB_B)
 Caution: needs testing. More...
 
static Vec3P convertAngVelInBodyFrameToBodyXYZDot (const Vec3P &q, const Vec3P &w_PB_B)
 Given Euler angles forming a body-fixed X-Y-Z (123) sequence, and the relative angular velocity vector w_PB_B of B in the parent frame, BUT EXPRESSED IN THE BODY FRAME, return the Euler angle derivatives. More...
 
static Vec3P convertAngVelInBodyFrameToBodyXYZDot (const Vec3P &cq, const Vec3P &sq, const Vec3P &w_PB_B)
 This faster version of convertAngVelInBodyFrameToBodyXYZDot() assumes you have already calculated the cosine and sine of the three q's. More...
 
static Vec3P convertBodyXYZDotToAngVelInBodyFrame (const Vec3P &q, const Vec3P &qdot)
 Inverse of the above routine. More...
 
static Vec3P convertBodyXYZDotToAngVelInBodyFrame (const Vec3P &cq, const Vec3P &sq, const Vec3P &qdot)
 This faster version of convertBodyXYZDotToAngVelInBodyFrame() assumes you have already calculated the cosine and sine of the three q's. More...
 
static Vec3P convertAngVelDotInBodyFrameToBodyXYZDotDot (const Vec3P &q, const Vec3P &w_PB_B, const Vec3P &wdot_PB_B)
 Warning: everything is measured in the *PARENT* frame, but has to be expressed in the *BODY* frame. More...
 
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. More...
 
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. More...
 
static Vec3P convertQuaternionDotToAngVel (const Vec4P &q, const Vec4P &qdot)
 Inverse of the above routine. More...
 
static Vec4P convertAngVelDotToQuaternionDotDot (const Vec4P &q, const Vec3P &w_PB, const Vec3P &b_PB)
 We want to differentiate qdot=N(q)*w to get qdotdot=N*b+NDot*w where b is angular acceleration wdot. More...
 
static Vec3P convertAngVelInParentToBodyXYZDot (const Vec2P &cosxy, const Vec2P &sinxy, RealP oocosy, const Vec3P &w_PB)
 Calculate first time derivative qdot of body-fixed XYZ Euler angles q given sines and cosines of the Euler angles and the angular velocity w_PB of child B in parent P, expressed in P. More...
 
static Vec3P convertAngAccInParentToBodyXYZDotDot (const Vec2P &cosxy, const Vec2P &sinxy, RealP oocosy, const Vec3P &qdot, const Vec3P &b_PB)
 Calculate second time derivative qdotdot of body-fixed XYZ Euler angles q given sines and cosines of the Euler angles, the first derivative qdot and the angular acceleration b_PB of child B in parent P, expressed in P. More...
 

Queries

bool isSameRotationToWithinAngle (const Rotation_ &R, RealP okPointingAngleErrorRads) const
 Return true if "this" Rotation is nearly identical to "R" within a specified pointing angle error. More...
 
bool isSameRotationToWithinAngleOfMachinePrecision (const Rotation_ &R) const
 Return true if "this" Rotation is nearly identical to "R" within machine precision. More...
 
RealP getMaxAbsDifferenceInRotationElements (const Rotation_ &R) const
 Returns maximum absolute difference between elements in "this" Rotation and elements in "R". More...
 
bool areAllRotationElementsSameToEpsilon (const Rotation_ &R, RealP epsilon) const
 Returns true if each element of "this" Rotation is within epsilon of the corresponding element of "R". More...
 
bool areAllRotationElementsSameToMachinePrecision (const Rotation_ &R) const
 Returns true if each element of "this" Rotation is within machine precision of the corresponding element of "R". More...
 

Additional Inherited Members

- Public Member Functions inherited from SimTK::Mat< 3, 3, P >
ScalarNormSq scalarNormSqr () const
 Scalar norm square is the sum of squares of all the scalars that comprise the value of this Mat. More...
 
TSqrt sqrt () const
 Elementwise square root; that is, the return value has the same dimensions as this Mat but with each element replaced by whatever it thinks its square root is. More...
 
TAbs abs () const
 Elementwise absolute value; that is, the return value has the same dimensions as this Mat but with each element replaced by whatever it thinks its absolute value is. More...
 
TStandard standardize () const
 
 Mat ()
 Default construction initializes to NaN when debugging but is left uninitialized otherwise to ensure that there is no overhead. More...
 
 Mat (const Mat &src)
 Copy constructor copies only the elements that are present and does not touch any unused memory space between them if they are not packed. More...
 
 Mat (const SymMat< M, P > &src)
 Explicit construction of a Mat from a SymMat (symmetric/Hermitian matrix). More...
 
 Mat (const Mat< M, N, E, CSS, RSS > &src)
 This provides an implicit conversion from a Mat of the same dimensions and element type but with different element spacing. More...
 
 Mat (const Mat< M, N, ENeg, CSS, RSS > &src)
 This provides an implicit conversion from a Mat of the same dimensions and negated element type, possibly with different element spacing. More...
 
 Mat (const Mat< M, N, EE, CSS, RSS > &mm)
 Explicit construction of a Mat from a source Mat of the same dimensions and an assignment-compatible element type, with any element spacing allowed. More...
 
 Mat (const E &e)
 Explicit construction from a single element e of this Mat's element type E sets all the main diagonal elements to e but sets the rest of the elements to zero. More...
 
 Mat (const ENeg &e)
 Explicit construction from a single element e whose type is negator<E> (abbreviated ENeg here) where E is this Mat's element type sets all the main diagonal elements to e but sets the rest of the elements to zero. More...
 
 Mat (int i)
 Explicit construction from an int value means we convert the int into an object of this Mat's element type E, and then apply the single-element constructor above which sets the Mat to zero except for its main diagonal elements which will all be set to the given value. More...
 
 Mat (const E &e0, const E &e1)
 
 Mat (const E &e0, const E &e1, const E &e2)
 
 Mat (const E &e0, const E &e1, const E &e2, const E &e3)
 
 Mat (const E &e0, const E &e1, const E &e2, const E &e3, const E &e4)
 
 Mat (const E &e0, const E &e1, const E &e2, const E &e3, const E &e4, const E &e5)
 
 Mat (const E &e0, const E &e1, const E &e2, const E &e3, const E &e4, const E &e5, const E &e6)
 
 Mat (const E &e0, const E &e1, const E &e2, const E &e3, const E &e4, const E &e5, const E &e6, const E &e7)
 
 Mat (const E &e0, const E &e1, const E &e2, const E &e3, const E &e4, const E &e5, const E &e6, const E &e7, const E &e8)
 
 Mat (const E &e0, const E &e1, const E &e2, const E &e3, const E &e4, const E &e5, const E &e6, const E &e7, const E &e8, const E &e9)
 
 Mat (const E &e0, const E &e1, const E &e2, const E &e3, const E &e4, const E &e5, const E &e6, const E &e7, const E &e8, const E &e9, const E &e10)
 
 Mat (const E &e0, const E &e1, const E &e2, const E &e3, const E &e4, const E &e5, const E &e6, const E &e7, const E &e8, const E &e9, const E &e10, const E &e11)
 
 Mat (const E &e0, const E &e1, const E &e2, const E &e3, const E &e4, const E &e5, const E &e6, const E &e7, const E &e8, const E &e9, const E &e10, const E &e11, const E &e12)
 
 Mat (const E &e0, const E &e1, const E &e2, const E &e3, const E &e4, const E &e5, const E &e6, const E &e7, const E &e8, const E &e9, const E &e10, const E &e11, const E &e12, const E &e13)
 
 Mat (const E &e0, const E &e1, const E &e2, const E &e3, const E &e4, const E &e5, const E &e6, const E &e7, const E &e8, const E &e9, const E &e10, const E &e11, const E &e12, const E &e13, const E &e14)
 
 Mat (const E &e0, const E &e1, const E &e2, const E &e3, const E &e4, const E &e5, const E &e6, const E &e7, const E &e8, const E &e9, const E &e10, const E &e11, const E &e12, const E &e13, const E &e14, const E &e15)
 
 Mat (const TRow &r0)
 
 Mat (const TRow &r0, const TRow &r1)
 
 Mat (const TRow &r0, const TRow &r1, const TRow &r2)
 
 Mat (const TRow &r0, const TRow &r1, const TRow &r2, const TRow &r3)
 
 Mat (const TRow &r0, const TRow &r1, const TRow &r2, const TRow &r3, const TRow &r4)
 
 Mat (const TRow &r0, const TRow &r1, const TRow &r2, const TRow &r3, const TRow &r4, const TRow &r5)
 
 Mat (const Row< N, EE, SS > &r0)
 
 Mat (const Row< N, EE, SS > &r0, const Row< N, EE, SS > &r1)
 
 Mat (const Row< N, EE, SS > &r0, const Row< N, EE, SS > &r1, const Row< N, EE, SS > &r2)
 
 Mat (const Row< N, EE, SS > &r0, const Row< N, EE, SS > &r1, const Row< N, EE, SS > &r2, const Row< N, EE, SS > &r3)
 
 Mat (const Row< N, EE, SS > &r0, const Row< N, EE, SS > &r1, const Row< N, EE, SS > &r2, const Row< N, EE, SS > &r3, const Row< N, EE, SS > &r4)
 
 Mat (const Row< N, EE, SS > &r0, const Row< N, EE, SS > &r1, const Row< N, EE, SS > &r2, const Row< N, EE, SS > &r3, const Row< N, EE, SS > &r4, const Row< N, EE, SS > &r5)
 
 Mat (const TCol &r0)
 
 Mat (const TCol &r0, const TCol &r1)
 
 Mat (const TCol &r0, const TCol &r1, const TCol &r2)
 
 Mat (const TCol &r0, const TCol &r1, const TCol &r2, const TCol &r3)
 
 Mat (const TCol &r0, const TCol &r1, const TCol &r2, const TCol &r3, const TCol &r4)
 
 Mat (const TCol &r0, const TCol &r1, const TCol &r2, const TCol &r3, const TCol &r4, const TCol &r5)
 
 Mat (const Vec< M, EE, SS > &r0)
 
 Mat (const Vec< M, EE, SS > &r0, const Vec< M, EE, SS > &r1)
 
 Mat (const Vec< M, EE, SS > &r0, const Vec< M, EE, SS > &r1, const Vec< M, EE, SS > &r2)
 
 Mat (const Vec< M, EE, SS > &r0, const Vec< M, EE, SS > &r1, const Vec< M, EE, SS > &r2, const Vec< M, EE, SS > &r3)
 
 Mat (const Vec< M, EE, SS > &r0, const Vec< M, EE, SS > &r1, const Vec< M, EE, SS > &r2, const Vec< M, EE, SS > &r3, const Vec< M, EE, SS > &r4)
 
 Mat (const Vec< M, EE, SS > &r0, const Vec< M, EE, SS > &r1, const Vec< M, EE, SS > &r2, const Vec< M, EE, SS > &r3, const Vec< M, EE, SS > &r4, const Vec< M, EE, SS > &r5)
 
 Mat (const EE *p)
 
Matoperator= (const Mat &src)
 Copy assignment copies only the elements that are present and does not touch any unused memory space between them if they are not packed. More...
 
Matoperator= (const Mat< M, N, EE, CSS, RSS > &mm)
 
Matoperator= (const EE *p)
 
Matoperator= (const EE &e)
 
Matoperator+= (const Mat< M, N, EE, CSS, RSS > &mm)
 
Matoperator+= (const Mat< M, N, negator< EE >, CSS, RSS > &mm)
 
Matoperator+= (const EE &e)
 
Matoperator-= (const Mat< M, N, EE, CSS, RSS > &mm)
 
Matoperator-= (const Mat< M, N, negator< EE >, CSS, RSS > &mm)
 
Matoperator-= (const EE &e)
 
Matoperator*= (const Mat< N, N, EE, CSS, RSS > &mm)
 
Matoperator*= (const EE &e)
 
Result< Mat< M, N, E2, CS2,
RS2 > >::Add 
conformingAdd (const Mat< M, N, E2, CS2, RS2 > &r) const
 
Result< SymMat< M, E2, RS2 > >::Add conformingAdd (const SymMat< M, E2, RS2 > &sy) const
 
Result< Mat< M, N, E2, CS2,
RS2 > >::Sub 
conformingSubtract (const Mat< M, N, E2, CS2, RS2 > &r) const
 
Result< SymMat< M, E2, RS2 > >::Sub conformingSubtract (const SymMat< M, E2, RS2 > &sy) const
 
Mat< M, N, E2, CS2, RS2 >
::template Result< Mat >::Sub 
conformingSubtractFromLeft (const Mat< M, N, E2, CS2, RS2 > &l) const
 
SymMat< M, E2, RS2 >::template
Result< Mat >::Sub 
conformingSubtractFromLeft (const SymMat< M, E2, RS2 > &sy) const
 
EltResult< E2 >::Mul elementwiseMultiply (const Mat< M, N, E2, CS2, RS2 > &r) const
 
EltResult< E2 >::Dvd elementwiseDivide (const Mat< M, N, E2, CS2, RS2 > &r) const
 
Result< Mat< N, N2, E2, CS2,
RS2 > >::Mul 
conformingMultiply (const Mat< N, N2, E2, CS2, RS2 > &m) const
 
Mat< M2, M, E2, CS2, RS2 >
::template Result< Mat >::Mul 
conformingMultiplyFromLeft (const Mat< M2, M, E2, CS2, RS2 > &m) const
 
Result< Mat< M2, N, E2, CS2,
RS2 > >::Dvd 
conformingDivide (const Mat< M2, N, E2, CS2, RS2 > &m) const
 
Mat< M2, N, E2, CS2, RS2 >
::template Result< Mat >::Dvd 
conformingDivideFromLeft (const Mat< M2, N, E2, CS2, RS2 > &m) const
 
const TRowoperator[] (int i) const
 
TRowoperator[] (int i)
 
const TColoperator() (int j) const
 
TColoperator() (int j)
 
const E & operator() (int i, int j) const
 
E & operator() (int i, int j)
 
ScalarNormSq normSqr () const
 
CNT< ScalarNormSq >::TSqrt norm () const
 
TNormalize normalize () const
 
TInvert invert () const
 
const Matoperator+ () const
 
const TNegoperator- () const
 
TNegoperator- ()
 
const THermoperator~ () const
 
THermoperator~ ()
 
const TNegnegate () const
 
TNegupdNegate ()
 
const THermtranspose () const
 
THermupdTranspose ()
 
const TPosTranspositionalTranspose () const
 
TPosTransupdPositionalTranspose ()
 
const TRealreal () const
 
TRealreal ()
 
const TImagimag () const
 
TImagimag ()
 
const TWithoutNegatorcastAwayNegatorIfAny () const
 
TWithoutNegatorupdCastAwayNegatorIfAny ()
 
const TRowrow (int i) const
 
TRowrow (int i)
 
const TColcol (int j) const
 
TColcol (int j)
 
const E & elt (int i, int j) const
 
E & elt (int i, int j)
 
const TDiagdiag () const
 Select main diagonal (of largest leading square if rectangular) and return it as a read-only view (as a Vec) of the diagonal elements of this Mat. More...
 
TDiagdiag ()
 This non-const version of diag() is an alternate name for updDiag() available for historical reasons. More...
 
TDiagupdDiag ()
 Select main diagonal (of largest leading square if rectangular) and return it as a writable view (as a Vec) of the diagonal elements of this Mat. More...
 
EStandard trace () const
 
Mat< M, N, typename CNT< E >
::template Result< EE >::Mul > 
scalarMultiply (const EE &e) const
 
Mat< M, N, typename CNT< EE >
::template Result< E >::Mul > 
scalarMultiplyFromLeft (const EE &e) const
 
Mat< M, N, typename CNT< E >
::template Result< EE >::Dvd > 
scalarDivide (const EE &e) const
 
Mat< M, N, typename CNT< EE >
::template Result< E >::Dvd > 
scalarDivideFromLeft (const EE &e) const
 
Mat< M, N, typename CNT< E >
::template Result< EE >::Add > 
scalarAdd (const EE &e) const
 
Mat< M, N, typename CNT< E >
::template Result< EE >::Sub > 
scalarSubtract (const EE &e) const
 
Mat< M, N, typename CNT< EE >
::template Result< E >::Sub > 
scalarSubtractFromLeft (const EE &e) const
 
Matoperator/= (const EE &e)
 
MatscalarEq (const EE &ee)
 
MatscalarPlusEq (const EE &ee)
 
MatscalarMinusEq (const EE &ee)
 
MatscalarMinusEqFromLeft (const EE &ee)
 
MatscalarTimesEq (const EE &ee)
 
MatscalarTimesEqFromLeft (const EE &ee)
 
MatscalarDivideEq (const EE &ee)
 
MatscalarDivideEqFromLeft (const EE &ee)
 
void setToNaN ()
 
void setToZero ()
 
const SubMat< MM, NN >::Type & getSubMat (int i, int j) const
 
SubMat< MM, NN >::Type & updSubMat (int i, int j)
 
void setSubMat (int i, int j, const typename SubMat< MM, NN >::Type &value)
 
TDropRow dropRow (int i) const
 Return a matrix one row smaller than this one by dropping row i. More...
 
TDropCol dropCol (int j) const
 Return a matrix one column smaller than this one by dropping column j. More...
 
TDropRowCol dropRowCol (int i, int j) const
 Return a matrix one row and one column smaller than this one by dropping row i and column j. More...
 
TAppendRow appendRow (const Row< N, EE, SS > &row) const
 Return a matrix one row larger than this one by adding a row to the end. More...
 
TAppendCol appendCol (const Vec< M, EE, SS > &col) const
 Return a matrix one column larger than this one by adding a column to the end. More...
 
TAppendRowCol appendRowCol (const Row< N+1, ER, SR > &row, const Vec< M+1, EC, SC > &col) const
 Return a matrix one row and one column larger than this one by adding a row to the bottom and a column to the right. More...
 
TAppendRow insertRow (int i, const Row< N, EE, SS > &row) const
 Return a matrix one row larger than this one by inserting a row *before* row i. More...
 
TAppendCol insertCol (int j, const Vec< M, EE, SS > &col) const
 Return a matrix one column larger than this one by inserting a column *before* column j. More...
 
TAppendRowCol insertRowCol (int i, int j, const Row< N+1, ER, SR > &row, const Vec< M+1, EC, SC > &col) const
 Return a matrix one row and one column larger than this one by inserting a row *before* row i and a column *before* column j. More...
 
bool isNaN () const
 Return true if any element of this Mat contains a NaN anywhere. More...
 
bool isInf () const
 Return true if any element of this Mat contains a +Inf or -Inf somewhere but no element contains a NaN anywhere. More...
 
bool isFinite () const
 Return true if no element contains an Infinity or a NaN. More...
 
bool isNumericallyEqual (const Mat< M, N, E2, CS2, RS2 > &m, double tol) const
 Test whether this matrix is numerically equal to some other matrix with the same shape, using a specified tolerance. More...
 
bool isNumericallyEqual (const Mat< M, N, E2, CS2, RS2 > &m) const
 Test whether this matrix is numerically equal to some other matrix with the same shape, using a default tolerance which is the looser of the default tolerances of the two objects being compared. More...
 
bool isNumericallyEqual (const P &e, double tol=getDefaultTolerance()) const
 Test whether this is numerically a "scalar" matrix, meaning that it is a diagonal matrix in which each diagonal element is numerically equal to the same scalar, using either a specified tolerance or the matrix's default tolerance (which is always the same or looser than the default tolerance for one of its elements). More...
 
bool isNumericallySymmetric (double tol=getDefaultTolerance()) const
 A Matrix is symmetric (actually Hermitian) if it is square and each element (i,j) is the Hermitian transpose of element (j,i). More...
 
bool isExactlySymmetric () const
 A Matrix is symmetric (actually Hermitian) if it is square and each element (i,j) is the Hermitian (conjugate) transpose of element (j,i). More...
 
TRow colSum () const
 Returns a row vector (Row) containing the column sums of this matrix. More...
 
TRow sum () const
 This is an alternate name for colSum(); behaves like the Matlab function of the same name. More...
 
TCol rowSum () const
 Returns a column vector (Vec) containing the row sums of this matrix. More...
 
std::string toString () const
 toString() returns a string representation of the Mat. More...
 
const P & get (int i, int j) const
 Variant of indexing operator that's scripting friendly to get entry (i, j) More...
 
void set (int i, int j, const P &value)
 Variant of indexing operator that's scripting friendly to set entry (i, j) More...
 
- Static Public Member Functions inherited from SimTK::Mat< 3, 3, P >
static int size ()
 Return the total number of elements M*N contained in this Mat. More...
 
static int nrow ()
 Return the number of rows in this Mat, echoing the value supplied for the template paramter M. More...
 
static int ncol ()
 Return the number of columns in this Mat, echoing the value supplied for the template paramter N. More...
 
static const MatgetAs (const P *p)
 
static MatupdAs (P *p)
 
static Mat< M, N, P, M, 1 > getNaN ()
 
static double getDefaultTolerance ()
 For approximate comparisions, the default tolerance to use for a matrix is its shortest dimension times its elements' default tolerance. More...
 

Detailed Description

template<class P>
class SimTK::Rotation_< P >

The Rotation class is a Mat33 that guarantees that the matrix can be interpreted as a legitimate 3x3 rotation matrix giving the relative orientation of two right-handed, orthogonal, unit vector bases.

A rotation matrix, also known as a direction cosine matrix, 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.

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.

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).

The Rotation_ class is templatized by the precision P, which should be float or double. A typedef defining type Rotation as Rotation_<Real> is always defined and is normally used in user programs rather than the templatized class.

Member Typedef Documentation

template<class P>
typedef P SimTK::Rotation_< P >::RealP

These are just local abbreviations.

template<class P>
typedef Mat<2,2,P> SimTK::Rotation_< P >::Mat22P
template<class P>
typedef Mat<3,2,P> SimTK::Rotation_< P >::Mat32P
template<class P>
typedef Mat<3,3,P> SimTK::Rotation_< P >::Mat33P
template<class P>
typedef Mat<4,3,P> SimTK::Rotation_< P >::Mat43P
template<class P>
typedef Mat<3,4,P> SimTK::Rotation_< P >::Mat34P
template<class P>
typedef Vec<2,P> SimTK::Rotation_< P >::Vec2P
template<class P>
typedef Vec<3,P> SimTK::Rotation_< P >::Vec3P
template<class P>
typedef Vec<4,P> SimTK::Rotation_< P >::Vec4P
template<class P>
typedef UnitVec<P,1> SimTK::Rotation_< P >::UnitVec3P
template<class P>
typedef SymMat<3,P> SimTK::Rotation_< P >::SymMat33P
template<class P>
typedef Quaternion_<P> SimTK::Rotation_< P >::QuaternionP
template<class P>
typedef UnitVec<P,Mat33P::RowSpacing> SimTK::Rotation_< P >::ColType

This is the type of a column of this Rotation matrix.

It will be a Vec<3> but will not necessarily have unit spacing.

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

This is the type of a row of this Rotation matrix.

It will be a Row<3> but will not necessarily have unit spacing.

Constructor & Destructor Documentation

template<class P>
SimTK::Rotation_< P >::Rotation_ ( )
inline

Default constructor.

template<class P>
SimTK::Rotation_< P >::Rotation_ ( const Rotation_< P > &  R)
inline

Copy constructor.

template<class P>
SimTK::Rotation_< P >::Rotation_ ( const InverseRotation_< P > &  R)
inline

Like copy constructor but for inverse rotation.

This allows implicit conversion from InverseRotation_ to Rotation_.

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 the X-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 the Y-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 the Z-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 unit 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 of arbitrary length.

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_< P >::Rotation_ ( const QuaternionP q)
inlineexplicit

Constructor for creating a rotation matrix from a quaternion.

template<class P>
SimTK::Rotation_< P >::Rotation_ ( const Mat33P m)
inlineexplicit

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

template<class P>
SimTK::Rotation_< P >::Rotation_ ( const Mat33P m,
bool   
)
inline

(Advanced) Construct a Rotation_ directly from a Mat33P (we trust that m is a valid Rotation_!) Things will not go well for you if it is not.

template<class P>
SimTK::Rotation_< P >::Rotation_ ( const UnitVec3P uvec,
CoordinateAxis  axis 
)
inline

Calculate R_AB by knowing one of B's unit vectors 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.

Member Function Documentation

template<class P>
Rotation_& SimTK::Rotation_< P >::operator= ( const Rotation_< P > &  R)
inline

Assignment operator.

template<class P>
Rotation_< P > & SimTK::Rotation_< P >::operator= ( const InverseRotation_< P > &  R)
inline

Like copy assignment but for inverse rotation.

template<class P>
Rotation_& SimTK::Rotation_< P >::setRotationToNaN ( )
inline

Construct Rotation_ filled with NaNs.

template<class P>
Rotation_& SimTK::Rotation_< P >::setRotationToIdentityMatrix ( )
inline

Construct identity Rotation_.

template<class P>
Rotation_& SimTK::Rotation_< P >::setRotationFromAngleAboutAxis ( RealP  angle,
const CoordinateAxis axis 
)
inline

Set this Rotation_ object to a right-handed rotation by an angle (in radians) about a coordinate axis.

template<class P>
Rotation_& SimTK::Rotation_< P >::setRotationFromAngleAboutX ( RealP  angle)
inline

Set this Rotation_ object to a right-handed rotation by an angle (in radians) about the X-axis.

template<class P>
Rotation_& SimTK::Rotation_< P >::setRotationFromAngleAboutX ( RealP  cosAngle,
RealP  sinAngle 
)
inline

Set this Rotation_ object to a right-handed rotation by an angle about the X-axis, where the cosine and sine of the angle are specified.

template<class P>
Rotation_& SimTK::Rotation_< P >::setRotationFromAngleAboutY ( RealP  angle)
inline

Set this Rotation_ object to a right-handed rotation by an angle (in radians) about the Y-axis.

template<class P>
Rotation_& SimTK::Rotation_< P >::setRotationFromAngleAboutY ( RealP  cosAngle,
RealP  sinAngle 
)
inline

Set this Rotation_ object to a right-handed rotation by an angle about the Y-axis, where the cosine and sine of the angle are specified.

template<class P>
Rotation_& SimTK::Rotation_< P >::setRotationFromAngleAboutZ ( RealP  angle)
inline

Set this Rotation_ object to a right-handed rotation by an angle (in radians) about the Z-axis.

template<class P>
Rotation_& SimTK::Rotation_< P >::setRotationFromAngleAboutZ ( RealP  cosAngle,
RealP  sinAngle 
)
inline

Set this Rotation_ object to a right-handed rotation by an angle about the Z-axis, where the cosine and sine of the angle are specified.

template<class P>
Rotation_& SimTK::Rotation_< P >::setRotationFromAngleAboutUnitVector ( RealP  angle,
const UnitVec3P unitVector 
)

Set this Rotation_ object to a right-handed rotation of an angle (in radians) about an arbitrary unit vector.

template<class P>
Rotation_& SimTK::Rotation_< P >::setRotationFromAngleAboutNonUnitVector ( RealP  angle,
const Vec3P nonUnitVector 
)
inline

Set this Rotation_ object to a right-handed rotation of an angle (in radians) about an arbitrary vector of arbitrary length.

template<class P>
Rotation_& SimTK::Rotation_< P >::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).

template<class P>
Rotation_& SimTK::Rotation_< P >::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).

template<class P>
Rotation_& SimTK::Rotation_< P >::setRotationFromQuaternion ( const QuaternionP q)

Method for creating a rotation matrix from a quaternion.

template<class P>
Rotation_& SimTK::Rotation_< P >::setRotationFromApproximateMat33 ( const Mat33P m)

Set this Rotation_ object to an (hopefully nearby) orthogonal rotation matrix from a generic Mat33P.

template<class P>
Rotation_& SimTK::Rotation_< P >::setRotationFromMat33TrustMe ( const Mat33P m)
inline

(Advanced) Set the Rotation_ matrix directly - but you had better know what you are doing!

template<class P>
Rotation_& SimTK::Rotation_< P >::setRotationColFromUnitVecTrustMe ( int  colj,
const UnitVec3P uvecj 
)
inline

(Advanced) Set the Rotation_ matrix directly - but you had better know what you are doing!

template<class P>
Rotation_& SimTK::Rotation_< P >::setRotationFromUnitVecsTrustMe ( const UnitVec3P colA,
const UnitVec3P colB,
const UnitVec3P colC 
)
inline

(Advanced) Set the Rotation_ matrix directly - but you had better know what you are doing!

template<class P>
Rotation_& SimTK::Rotation_< P >::setRotationFromOneAxis ( const UnitVec3P uvec,
CoordinateAxis  axis 
)

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

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

template<class P>
Rotation_& SimTK::Rotation_< P >::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).

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>
void SimTK::Rotation_< P >::setRotationToBodyFixedXY ( const Vec2P v)
inline

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.

See Kane, Spacecraft Dynamics, pg. 423, body-three: 1-2-3, but the last rotation is zero.

template<class P>
void SimTK::Rotation_< P >::setRotationToBodyFixedXYZ ( const Vec3P v)
inline

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 (twice rotated) Z axis.

See Kane, Spacecraft Dynamics, pg. 423, body-three: 1-2-3.

template<class P>
void SimTK::Rotation_< P >::setRotationToBodyFixedXYZ ( const Vec3P c,
const Vec3P s 
)
inline

Given cosines and sines (in that order) of three angles, set this Rotation matrix to the body-fixed 1-2-3 sequence of those angles.

Cost is 18 flops.

template<class P>
const InverseRotation_<P>& SimTK::Rotation_< P >::operator~ ( ) const
inline

Transpose operator.

For an orthogonal matrix like this one, transpose is the same thing as inversion.

template<class P>
InverseRotation_<P>& SimTK::Rotation_< P >::operator~ ( )
inline

Transpose operator.

For an orthogonal matrix like this one, transpose is the same thing as inversion.

template<class P>
const InverseRotation_<P>& SimTK::Rotation_< P >::transpose ( ) const
inline

Transpose.

For an orthogonal matrix like this one, transpose is the same thing as inversion. Overrides the base class transpose method.

template<class P>
InverseRotation_<P>& SimTK::Rotation_< P >::updTranspose ( )
inline

Transpose.

For an orthogonal matrix like this one, transpose is the same thing as inversion. Overrides the base class transpose method.

template<class P>
const InverseRotation_<P>& SimTK::Rotation_< P >::invert ( ) const
inline

Convert from Rotation_ to InverseRotation_ (no cost).

Overrides base class invert() method.

template<class P>
InverseRotation_<P>& SimTK::Rotation_< P >::updInvert ( )
inline

Convert from Rotation_ to writable InverseRotation_ (no cost).

template<class P>
Rotation_< P > & SimTK::Rotation_< P >::operator*= ( const Rotation_< P > &  R)
inline

In-place composition of Rotation matrices.

template<class P>
Rotation_< P > & SimTK::Rotation_< P >::operator*= ( const InverseRotation_< P > &  R)
inline

In-place composition of Rotation matrices.

template<class P>
Rotation_< P > & SimTK::Rotation_< P >::operator/= ( const Rotation_< P > &  R)
inline

In-place composition of Rotation matrices.

template<class P>
Rotation_< P > & SimTK::Rotation_< P >::operator/= ( const InverseRotation_< P > &  R)
inline

In-place composition of Rotation matrices.

template<class P>
static Vec3P SimTK::Rotation_< P >::multiplyByBodyXYZ_N_P ( const Vec2P cosxy,
const Vec2P sinxy,
RealP  oocosy,
const Vec3P w_PB 
)
inlinestatic

This is the fastest way to form the product qdot=N_P*w_PB for a body-fixed XYZ sequence where angular velocity of child in parent is expected to be expressed in the parent.

Here we assume you have previously calculated sincos(qx), sincos(qy), and 1/cos(qy). Cost is 10 flops, faster even than the 15 it would take if you had saved N_P and then formed the N_P*w_PB product explicitly.

template<class P>
static Vec3P SimTK::Rotation_< P >::multiplyByBodyXYZ_NT_P ( const Vec2P cosxy,
const Vec2P sinxy,
RealP  oocosy,
const Vec3P q 
)
inlinestatic

This is the fastest way to form the product v_P=~N_P*q=~(~q*N_P); see the untransposed method multiplyByBodyXYZ_N_P() for information.

Cost is 9 flops.

template<class P>
static Vec3P SimTK::Rotation_< P >::multiplyByBodyXYZ_NInv_P ( const Vec2P cosxy,
const Vec2P sinxy,
const Vec3P qdot 
)
inlinestatic

Fastest way to form the product w_PB=NInv_P*qdot.

This is never singular. Cost is 9 flops.

template<class P>
static Vec3P SimTK::Rotation_< P >::multiplyByBodyXYZ_NInvT_P ( const Vec2P cosxy,
const Vec2P sinxy,
const Vec3P v_P 
)
inlinestatic

Fastest way to form the product q=~NInv_P*v_P=~(~v_P*NInv_P).

This is never singular. Cost is 10 flops.

template<class P>
const RowType& SimTK::Rotation_< P >::row ( int  i) const
inline

Return a reference to the ith row of this Rotation matrix as a UnitRow3.

template<class P>
const RowType& SimTK::Rotation_< P >::operator[] ( int  i) const
inline

Same as row(i) but nicer to look at.

template<class P>
const ColType& SimTK::Rotation_< P >::col ( int  j) const
inline

Return a reference to the jth column of this Rotation matrix as a UnitVec3.

template<class P>
const ColType& SimTK::Rotation_< P >::operator() ( int  j) const
inline

Same as col(j) but nicer to look at.

template<class P>
const ColType& SimTK::Rotation_< P >::x ( ) const
inline

Return col(0) of this Rotation matrix as a UnitVec3.

template<class P>
const ColType& SimTK::Rotation_< P >::y ( ) const
inline

Return col(1) of this Rotation matrix as a UnitVec3.

template<class P>
const ColType& SimTK::Rotation_< P >::z ( ) const
inline

Return col(2) of this Rotation matrix as a UnitVec3.

template<class P>
const ColType& SimTK::Rotation_< P >::getAxisUnitVec ( CoordinateAxis  axis) const
inline

Given a CoordinateAxis (XAxis,YAxis, or ZAxis) return a reference to the corresponding column of this Rotation matrix.

The result is equivalent to multiplying R_AB*v_B where v_B is [1,0,0],[0,1,0], or [0,0,1], which would cost 15 flops, but requires no computation.

template<class P>
const UnitVec<P,1> SimTK::Rotation_< P >::getAxisUnitVec ( CoordinateDirection  dir) const
inline

Given a CoordinateDirection (+/-XAxis, etc.) return a unit vector in that direction.

The result is equivalent to multiplying R_AB*v_B where v_B is [+/-1,0,0], [0,+/-1,0], or [0,0,+/-1], which would cost 15 flops, but this method requires at most 3 flops.

template<class P>
static Mat33P SimTK::Rotation_< P >::calcNForBodyXYZInBodyFrame ( const Vec3P q)
inlinestatic

Given Euler angles q forming a body-fixed X-Y-Z sequence return the block N_B of the system N matrix such that qdot=N_B(q)*w_PB_B where w_PB_B is the angular velocity of B in P EXPRESSED IN *B*!!! Note that N_B=N_P*R_PB.

This matrix will be singular if Y (q[1]) gets near 90 degrees!

Note
This version is very expensive because it has to calculate sines and cosines. If you already have those, use the alternate form of this method.

Cost: about 100 flops for sin/cos plus 12 to calculate N_B.

See also
Kane's Spacecraft Dynamics, page 427, body-three: 1-2-3.
template<class P>
static Mat33P SimTK::Rotation_< P >::calcNForBodyXYZInBodyFrame ( const Vec3P cq,
const Vec3P sq 
)
inlinestatic

This faster version of calcNForBodyXYZInBodyFrame() assumes you have already calculated the cosine and sine of the three q's.

Note that we only look at the cosines and sines of q[1] and q[2]; q[0] does not matter so you don't have to fill in the 0'th element of cq and sq. Cost is one divide plus 6 flops, say 12 flops.

template<class P>
static Mat33P SimTK::Rotation_< P >::calcNForBodyXYZInParentFrame ( const Vec3P q)
inlinestatic

Given Euler angles q forming a body-fixed X-Y-Z (123) sequence return the block N_P of the system N matrix such that qdot=N_P(q)*w_PB where w_PB is the angular velocity of B in P expressed in P (not the convention that Kane uses, where angular velocities are expressed in the outboard body B).

Note that N_P = N_B*~R_PB. This matrix will be singular if Y (q[1]) gets near 90 degrees!

Note
This version is very expensive because it has to calculate sines and cosines. If you already have those, use the alternate form of this method.

Cost: about 100 flops for sin/cos plus 12 to calculate N_P.

template<class P>
static Mat33P SimTK::Rotation_< P >::calcNForBodyXYZInParentFrame ( const Vec3P cq,
const Vec3P sq 
)
inlinestatic

This faster version of calcNForBodyXYZInParentFrame() assumes you have already calculated the cosine and sine of the three q's.

Note that we only look at the cosines and sines of q[0] and q[1]; q[2] does not matter so you don't have to fill in the 3rd element of cq and sq. Cost is one divide plus 6 flops, say 12 flops.

See also
Paul Mitiguy
template<class P>
static Mat33P SimTK::Rotation_< P >::calcNDotForBodyXYZInBodyFrame ( const Vec3P q,
const Vec3P qdot 
)
inlinestatic

Given Euler angles forming a body-fixed X-Y-Z (123) 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.

Note
This version is very expensive because it has to calculate sines and cosines. If you already have those, use the alternate form of this method.
template<class P>
static Mat33P SimTK::Rotation_< P >::calcNDotForBodyXYZInBodyFrame ( const Vec3P cq,
const Vec3P sq,
const Vec3P qdot 
)
inlinestatic

This faster version of calcNDotForBodyXYZInBodyFrame() assumes you have already calculated the cosine and sine of the three q's.

Note that we only look at the cosines and sines of q[1] and q[2]; q[0] does not matter so you don't have to fill in the 0'th element of cq and sq. Cost is one divide plus 21 flops, say 30 flops.

template<class P>
static Mat33P SimTK::Rotation_< P >::calcNDotForBodyXYZInParentFrame ( const Vec3P q,
const Vec3P qdot 
)
inlinestatic

Given Euler angles forming a body-fixed X-Y-Z (123) 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 P.

This matrix will be singular if Y (q[1]) gets near 90 degrees! See calcNForBodyXYZInParentFrame() for the matrix we're differentiating here.

Note
This version is very expensive because it has to calculate sines and cosines. If you already have those, use the alternate form of this method.
template<class P>
static Mat33P SimTK::Rotation_< P >::calcNDotForBodyXYZInParentFrame ( const Vec2P cq,
const Vec2P sq,
RealP  ooc1,
const Vec3P qdot 
)
inlinestatic

This faster version of calcNDotForBodyXYZInParentFrame() assumes you have already calculated the cosine and sine of the three q's.

Note that we only look at the cosines and sines of q[0] and q[1]. Cost is 21 flops.

template<class P>
static Mat33P SimTK::Rotation_< P >::calcNInvForBodyXYZInBodyFrame ( const Vec3P q)
inlinestatic

Inverse of routine calcNForBodyXYZInBodyFrame().

Return the inverse NInv_B of the N_B block computed above, such that w_PB_B=NInv_B(q)*qdot where w_PB_B is the angular velocity of B in P EXPRESSED IN *B*!!! (Kane's convention.) Note that NInv_B=~R_PB*NInv_P. This matrix is never singular.

Note
This version is very expensive because it has to calculate sines and cosines. If you already have those, use the alternate form of this method.
template<class P>
static Mat33P SimTK::Rotation_< P >::calcNInvForBodyXYZInBodyFrame ( const Vec3P cq,
const Vec3P sq 
)
inlinestatic

This faster version of calcNInvForBodyXYZInBodyFrame() assumes you have already calculated the cosine and sine of the three q's.

Note that we only look at the cosines and sines of q[1] and q[2]; q[0] does not matter so you don't have to fill in the 0'th element of cq and sq. Cost is 3 flops.

template<class P>
static Mat33P SimTK::Rotation_< P >::calcNInvForBodyXYZInParentFrame ( const Vec3P q)
inlinestatic

Inverse of the above routine.

Return the inverse NInv_P of the N_P block computed above, such that w_PB=NInv_P(q)*qdot where w_PB is the angular velocity of B in P (expressed in P). Note that NInv_P=R_PB*NInv_B. This matrix is never singular.

Note
This version is very expensive because it has to calculate sines and cosines. If you already have those, use the alternate form of this method.
template<class P>
static Mat33P SimTK::Rotation_< P >::calcNInvForBodyXYZInParentFrame ( const Vec3P cq,
const Vec3P sq 
)
inlinestatic

This faster version of calcNInvForBodyXYZInParentFrame() assumes you have already calculated the cosine and sine of the three q's.

Note that we only look at the cosines and sines of q[0] and q[1]; q[2] does not matter so you don't have to fill in the 3rd element of cq and sq. Cost is 3 flops.

template<class P>
static Mat43P SimTK::Rotation_< P >::calcUnnormalizedNForQuaternion ( const Vec4P q)
inlinestatic

Given a possibly unnormalized quaternion q, calculate the 4x3 matrix N which maps angular velocity w to quaternion derivatives qdot.

We expect the angular velocity in the parent frame, i.e. w==w_PB_P. We don't normalize, so N=|q|N' where N' is the normalized version. Cost is 7 flops.

template<class P>
static Mat43P SimTK::Rotation_< P >::calcUnnormalizedNDotForQuaternion ( const Vec4P qdot)
inlinestatic

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().

Note that NDot = d/dt N = d/dt (|q|N') = |q|(d/dt N'), where N' is the normalized matrix, since the length of the quaternion should be a constant. Cost is 7 flops.

template<class P>
static Mat34P SimTK::Rotation_< P >::calcUnnormalizedNInvForQuaternion ( const Vec4P q)
inlinestatic

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.

w==w_PB_P. Note: when the quaternion is not normalized, this is not precisely the (pseudo)inverse of N. inv(N)=inv(N')/|q| but we're returning |q|*inv(N')=|q|^2*inv(N). That is, NInv*N =|q|^2*I, which is I if the original q was normalized. (Note: N*NInv != I, not even close.) Cost is 7 flops.

template<class P>
const Mat33P& SimTK::Rotation_< P >::asMat33 ( ) const
inline

Conversion from Rotation to its base class Mat33.

Note: asMat33 is more efficient than toMat33() (no copy), but you have to know the internal layout.

template<class P>
Mat33P SimTK::Rotation_< P >::toMat33 ( ) const
inline

Conversion from Rotation to its base class Mat33.

Note: asMat33 is more efficient than toMat33() (no copy), but you have to know the internal layout.

template<class P>
SymMat33P SimTK::Rotation_< P >::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.

Details: assuming this Rotation is R_AB, and given a symmetric dyadic matrix S_BB expressed in B, we can reexpress it in A using S_AA=R_AB*S_BB*R_BA. The matrix should be one that is formed as products of vectors expressed in A, such as inertia, gyration or covariance matrices. This can be done efficiently exploiting properties of R (orthogonal) and S (symmetric). Total cost is 57 flops.

template<class P>
RealP SimTK::Rotation_< P >::convertOneAxisRotationToOneAngle ( const CoordinateAxis axis1) const

Converts rotation matrix to a single orientation angle.

Note: The result is most meaningful if the Rotation_ matrix is one that can be produced by such a sequence.

template<class P>
Vec2P SimTK::Rotation_< P >::convertTwoAxesRotationToTwoAngles ( BodyOrSpaceType  bodyOrSpace,
const CoordinateAxis axis1,
const CoordinateAxis axis2 
) const

Converts rotation matrix to two orientation angles.

Note: The result is most meaningful if the Rotation_ matrix is one that can be produced by such a sequence.

template<class P>
Vec3P SimTK::Rotation_< P >::convertThreeAxesRotationToThreeAngles ( BodyOrSpaceType  bodyOrSpace,
const CoordinateAxis axis1,
const CoordinateAxis axis2,
const CoordinateAxis axis3 
) const

Converts rotation matrix to three orientation angles.

Note: The result is most meaningful if the Rotation_ matrix is one that can be produced by such a sequence.

template<class P>
QuaternionP SimTK::Rotation_< P >::convertRotationToQuaternion ( ) const

Converts rotation matrix to an equivalent quaternion in canonical form (meaning its scalar element is nonnegative).

This uses a robust, singularity-free method due to Richard Spurrier. The cost is about 40 flops.

Reference
Spurrier, R.A., "Comment on 'Singularity-Free Extraction of a Quaternion from a Direction-Cosine Matrix'", J. Spacecraft and Rockets, 15(4):255, 1977.
See also
Quaternion_
template<class P>
Vec4P SimTK::Rotation_< P >::convertRotationToAngleAxis ( ) const
inline

Converts rotation matrix to an equivalent angle-axis representation in canonicalized form.

The result (a,v) is returned packed into a Vec4 [a vx vy vz], with -Pi < a <= Pi and |v|=1. Cost is about 140 flops.

If the rotation angle is zero (or very very close to zero) then the returned unit vector is arbitrary.

Theory
Euler's Rotation Theorem (1776) guarantees that any rigid body rotation is equivalent to a rotation by an angle about a fixed axis. This method finds such an angle and axis. Numerically, this is a very tricky computation to get correct in all cases. We use Spurrier's method to obtain a numerically-robust quaternion equivalent to this rotation matrix, then carefully extract and canonicalize the angle-axis form from the quaternion.
See also
convertRotationToQuaternion()
template<class P>
Vec2P SimTK::Rotation_< P >::convertRotationToBodyFixedXY ( ) const
inline

A convenient special case of convertTwoAxesRotationToTwoAngles().

template<class P>
Vec3P SimTK::Rotation_< P >::convertRotationToBodyFixedXYZ ( ) const
inline

A convenient special case of convertThreeAxesRotationToThreeAngles().

template<class P>
static Vec3P SimTK::Rotation_< P >::convertAngVelToBodyFixed321Dot ( const Vec3P q,
const Vec3P w_PB_B 
)
inlinestatic

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.

You are dead if q[1] gets near 90 degrees! See Kane's Spacecraft Dynamics, page 428, body-three: 3-2-1.

template<class P>
static Vec3P SimTK::Rotation_< P >::convertBodyFixed321DotToAngVel ( const Vec3P q,
const Vec3P qd 
)
inlinestatic

Inverse of convertAngVelToBodyFixed321Dot.

Returned angular velocity is B in P, expressed in *B*: w_PB_B.

template<class P>
static Vec3P SimTK::Rotation_< P >::convertAngVelDotToBodyFixed321DotDot ( const Vec3P q,
const Vec3P w_PB_B,
const Vec3P wdot_PB_B 
)
inlinestatic

Caution: needs testing.

template<class P>
static Vec3P SimTK::Rotation_< P >::convertAngVelInBodyFrameToBodyXYZDot ( const Vec3P q,
const Vec3P w_PB_B 
)
inlinestatic

Given Euler angles forming a body-fixed X-Y-Z (123) sequence, and the relative angular velocity vector w_PB_B of B in the parent frame, BUT EXPRESSED IN THE BODY FRAME, return the Euler angle derivatives.

You are dead if q[1] gets near 90 degrees!

Note
This version is very expensive because it has to calculate sines and cosines. If you already have those, use the alternate form of this method.
See also
Kane's Spacecraft Dynamics, page 427, body-three: 1-2-3.
template<class P>
static Vec3P SimTK::Rotation_< P >::convertAngVelInBodyFrameToBodyXYZDot ( const Vec3P cq,
const Vec3P sq,
const Vec3P w_PB_B 
)
inlinestatic

This faster version of convertAngVelInBodyFrameToBodyXYZDot() assumes you have already calculated the cosine and sine of the three q's.

Note that we only look at the cosines and sines of q[1] and q[2]; q[0] does not matter so you don't have to fill in the 0'th element of cq and sq. Cost is 27 flops.

template<class P>
static Vec3P SimTK::Rotation_< P >::convertBodyXYZDotToAngVelInBodyFrame ( const Vec3P q,
const Vec3P qdot 
)
inlinestatic

Inverse of the above routine.

Returned angular velocity is B in P, expressed in *B*: w_PB_B.

Note
This version is very expensive because it has to calculate sines and cosines. If you already have those, use the alternate form of this method.
template<class P>
static Vec3P SimTK::Rotation_< P >::convertBodyXYZDotToAngVelInBodyFrame ( const Vec3P cq,
const Vec3P sq,
const Vec3P qdot 
)
inlinestatic

This faster version of convertBodyXYZDotToAngVelInBodyFrame() assumes you have already calculated the cosine and sine of the three q's.

Note that we only look at the cosines and sines of q[1] and q[2]; q[0] does not matter so you don't have to fill in the 0'th element of cq and sq. Cost is 18 flops.

template<class P>
static Vec3P SimTK::Rotation_< P >::convertAngVelDotInBodyFrameToBodyXYZDotDot ( const Vec3P q,
const Vec3P w_PB_B,
const Vec3P wdot_PB_B 
)
inlinestatic

Warning: everything is measured in the *PARENT* frame, but has to be expressed in the *BODY* frame.

Note
This version is very expensive because it has to calculate sines and cosines. If you already have those, use the alternate form of this method. Caution: needs testing.
template<class P>
static Vec3P SimTK::Rotation_< P >::convertAngVelDotInBodyFrameToBodyXYZDotDot ( const Vec3P cq,
const Vec3P sq,
const Vec3P w_PB_B,
const Vec3P wdot_PB_B 
)
inlinestatic

This faster version of convertAngVelDotInBodyFrameToBodyXYZDotDot() assumes you have already calculated the cosine and sine of the three q's.

Note that we only look at the cosines and sines of q[1] and q[2]; q[0] does not matter so you don't have to fill in the 0'th element of cq and sq. Cost is about 93 flops.

template<class P>
static Vec4P SimTK::Rotation_< P >::convertAngVelToQuaternionDot ( const Vec4P q,
const Vec3P w_PB_P 
)
inlinestatic

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.

This is never singular. Cost is 27 flops.

template<class P>
static Vec3P SimTK::Rotation_< P >::convertQuaternionDotToAngVel ( const Vec4P q,
const Vec4P qdot 
)
inlinestatic

Inverse of the above routine.

Returned AngVel is expressed in the *PARENT* frame: w_PB_P. Cost is 28 flops.

template<class P>
static Vec4P SimTK::Rotation_< P >::convertAngVelDotToQuaternionDotDot ( const Vec4P q,
const Vec3P w_PB,
const Vec3P b_PB 
)
inlinestatic

We want to differentiate qdot=N(q)*w to get qdotdot=N*b+NDot*w where b is angular acceleration wdot.

Note that NDot=NDot(qdot), but it is far better to calculate the matrix-vector product NDot(N*w)*w directly rather than calculate NDot separately. That gives

    NDot*w = -(w^2)/4 * q

Cost is 41 flops.

template<class P>
static Vec3P SimTK::Rotation_< P >::convertAngVelInParentToBodyXYZDot ( const Vec2P cosxy,
const Vec2P sinxy,
RealP  oocosy,
const Vec3P w_PB 
)
inlinestatic

Calculate first time derivative qdot of body-fixed XYZ Euler angles q given sines and cosines of the Euler angles and the angular velocity w_PB of child B in parent P, expressed in P.

Cost is 10 flops.

Theory: calculate qdot=N_P(q)*w_PB using multiplyByBodyXYZ_N_P().

See also
multiplyByBodyXYZ_N_P()
Parameters
cosxycos(qx), cos(qy)
sinxysin(qx), sin(qy)
oocosy1/cos(qy)
w_PBangular velocity of B in P, exp. in P
template<class P>
static Vec3P SimTK::Rotation_< P >::convertAngAccInParentToBodyXYZDotDot ( const Vec2P cosxy,
const Vec2P sinxy,
RealP  oocosy,
const Vec3P qdot,
const Vec3P b_PB 
)
inlinestatic

Calculate second time derivative qdotdot of body-fixed XYZ Euler angles q given sines and cosines of the Euler angles, the first derivative qdot and the angular acceleration b_PB of child B in parent P, expressed in P.

Cost is 22 flops.

Theory: we have qdot=N_P*w_PB, which we differentiate in P to get qdotdot=N_P*b_PB + NDot_P*w_PB. Note that NDot_P=NDot_P(q,qdot) and w_PB=NInv_P*qdot (because N_P is invertible). We can then rewrite qdotdot=N_P*b_PB + NDot_P*(NInv_P*qdot) which can be calculated very efficiently. The second term is just an acceleration remainder term quadratic in qdot.

Parameters
cosxycos(qx), cos(qy)
sinxysin(qx), sin(qy)
oocosy1/cos(qy)
qdotpreviously calculated BodyXYZDot
b_PBangular acceleration, a.k.a. wdot_PB
template<class P>
bool SimTK::Rotation_< P >::isSameRotationToWithinAngle ( const Rotation_< P > &  R,
RealP  okPointingAngleErrorRads 
) const

Return true if "this" Rotation is nearly identical to "R" within a specified pointing angle error.

template<class P>
bool SimTK::Rotation_< P >::isSameRotationToWithinAngleOfMachinePrecision ( const Rotation_< P > &  R) const
inline

Return true if "this" Rotation is nearly identical to "R" within machine precision.

template<class P>
RealP SimTK::Rotation_< P >::getMaxAbsDifferenceInRotationElements ( const Rotation_< P > &  R) const
inline

Returns maximum absolute difference between elements in "this" Rotation and elements in "R".

template<class P>
bool SimTK::Rotation_< P >::areAllRotationElementsSameToEpsilon ( const Rotation_< P > &  R,
RealP  epsilon 
) const
inline

Returns true if each element of "this" Rotation is within epsilon of the corresponding element of "R".

template<class P>
bool SimTK::Rotation_< P >::areAllRotationElementsSameToMachinePrecision ( const Rotation_< P > &  R) const
inline

Returns true if each element of "this" Rotation is within machine precision of the corresponding element of "R".


The documentation for this class was generated from the following files: