|
Simbody
3.5
|
|
Simbody
3.5
|
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 RowType & | row (int i) const |
| Return a reference to the ith row of this Rotation matrix as a UnitRow3. More... | |
| const RowType & | operator[] (int i) const |
| Same as row(i) but nicer to look at. More... | |
| const ColType & | col (int j) const |
| Return a reference to the jth column of this Rotation matrix as a UnitVec3. More... | |
| const ColType & | operator() (int j) const |
| Same as col(j) but nicer to look at. More... | |
| const ColType & | x () const |
| Return col(0) of this Rotation matrix as a UnitVec3. More... | |
| const ColType & | y () const |
| Return col(1) of this Rotation matrix as a UnitVec3. More... | |
| const ColType & | z () const |
| Return col(2) of this Rotation matrix as a UnitVec3. More... | |
| const ColType & | getAxisUnitVec (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 Mat33P & | asMat33 () 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) | |
| Mat & | operator= (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... | |
| Mat & | operator= (const Mat< M, N, EE, CSS, RSS > &mm) |
| Mat & | operator= (const EE *p) |
| Mat & | operator= (const EE &e) |
| Mat & | operator+= (const Mat< M, N, EE, CSS, RSS > &mm) |
| Mat & | operator+= (const Mat< M, N, negator< EE >, CSS, RSS > &mm) |
| Mat & | operator+= (const EE &e) |
| Mat & | operator-= (const Mat< M, N, EE, CSS, RSS > &mm) |
| Mat & | operator-= (const Mat< M, N, negator< EE >, CSS, RSS > &mm) |
| Mat & | operator-= (const EE &e) |
| Mat & | operator*= (const Mat< N, N, EE, CSS, RSS > &mm) |
| Mat & | operator*= (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 TRow & | operator[] (int i) const |
| TRow & | operator[] (int i) |
| const TCol & | operator() (int j) const |
| TCol & | operator() (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 Mat & | operator+ () const |
| const TNeg & | operator- () const |
| TNeg & | operator- () |
| const THerm & | operator~ () const |
| THerm & | operator~ () |
| const TNeg & | negate () const |
| TNeg & | updNegate () |
| const THerm & | transpose () const |
| THerm & | updTranspose () |
| const TPosTrans & | positionalTranspose () const |
| TPosTrans & | updPositionalTranspose () |
| const TReal & | real () const |
| TReal & | real () |
| const TImag & | imag () const |
| TImag & | imag () |
| const TWithoutNegator & | castAwayNegatorIfAny () const |
| TWithoutNegator & | updCastAwayNegatorIfAny () |
| const TRow & | row (int i) const |
| TRow & | row (int i) |
| const TCol & | col (int j) const |
| TCol & | col (int j) |
| const E & | elt (int i, int j) const |
| E & | elt (int i, int j) |
| const TDiag & | diag () 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... | |
| TDiag & | diag () |
| This non-const version of diag() is an alternate name for updDiag() available for historical reasons. More... | |
| TDiag & | updDiag () |
| 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 |
| Mat & | operator/= (const EE &e) |
| Mat & | scalarEq (const EE &ee) |
| Mat & | scalarPlusEq (const EE &ee) |
| Mat & | scalarMinusEq (const EE &ee) |
| Mat & | scalarMinusEqFromLeft (const EE &ee) |
| Mat & | scalarTimesEq (const EE &ee) |
| Mat & | scalarTimesEqFromLeft (const EE &ee) |
| Mat & | scalarDivideEq (const EE &ee) |
| Mat & | scalarDivideEqFromLeft (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 Mat & | getAs (const P *p) |
| static Mat & | updAs (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... | |
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.
| typedef P SimTK::Rotation_< P >::RealP |
These are just local abbreviations.
| typedef Mat<2,2,P> SimTK::Rotation_< P >::Mat22P |
| typedef Mat<3,2,P> SimTK::Rotation_< P >::Mat32P |
| typedef Mat<3,3,P> SimTK::Rotation_< P >::Mat33P |
| typedef Mat<4,3,P> SimTK::Rotation_< P >::Mat43P |
| typedef Mat<3,4,P> SimTK::Rotation_< P >::Mat34P |
| typedef Vec<2,P> SimTK::Rotation_< P >::Vec2P |
| typedef Vec<3,P> SimTK::Rotation_< P >::Vec3P |
| typedef Vec<4,P> SimTK::Rotation_< P >::Vec4P |
| typedef UnitVec<P,1> SimTK::Rotation_< P >::UnitVec3P |
| typedef SymMat<3,P> SimTK::Rotation_< P >::SymMat33P |
| typedef Quaternion_<P> SimTK::Rotation_< P >::QuaternionP |
| 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.
| 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.
|
inline |
Default constructor.
|
inline |
Copy constructor.
|
inline |
Like copy constructor but for inverse rotation.
This allows implicit conversion from InverseRotation_ to Rotation_.
|
inline |
Constructor for right-handed rotation by an angle (in radians) about a coordinate axis.
|
inline |
Constructor for right-handed rotation by an angle (in radians) about the X-axis.
|
inline |
Constructor for right-handed rotation by an angle (in radians) about the Y-axis.
|
inline |
Constructor for right-handed rotation by an angle (in radians) about the Z-axis.
|
inline |
Constructor for right-handed rotation by an angle (in radians) about an arbitrary unit vector.
|
inline |
Constructor for right-handed rotation by an angle (in radians) about an arbitrary vector of arbitrary length.
|
inline |
Constructor for two-angle, two-axes, Body-fixed or Space-fixed rotation sequences (angles are in radians).
|
inline |
Constructor for three-angle Body-fixed or Space-fixed rotation sequences (angles are in radians).
|
inlineexplicit |
Constructor for creating a rotation matrix from a quaternion.
|
inlineexplicit |
Constructs an (hopefully nearby) orthogonal rotation matrix from a generic Mat33P.
|
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.
|
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).
|
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.
|
inline |
Assignment operator.
|
inline |
Like copy assignment but for inverse rotation.
|
inline |
Construct Rotation_ filled with NaNs.
|
inline |
Construct identity Rotation_.
|
inline |
Set this Rotation_ object to a right-handed rotation by an angle (in radians) about a coordinate axis.
|
inline |
Set this Rotation_ object to a right-handed rotation by an angle (in radians) about the X-axis.
|
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.
|
inline |
Set this Rotation_ object to a right-handed rotation by an angle (in radians) about the Y-axis.
|
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.
|
inline |
Set this Rotation_ object to a right-handed rotation by an angle (in radians) about the Z-axis.
|
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.
| 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.
|
inline |
Set this Rotation_ object to a right-handed rotation of an angle (in radians) about an arbitrary vector of arbitrary length.
| 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).
| 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).
| Rotation_& SimTK::Rotation_< P >::setRotationFromQuaternion | ( | const QuaternionP & | q | ) |
Method for creating a rotation matrix from a quaternion.
| Rotation_& SimTK::Rotation_< P >::setRotationFromApproximateMat33 | ( | const Mat33P & | m | ) |
Set this Rotation_ object to an (hopefully nearby) orthogonal rotation matrix from a generic Mat33P.
|
inline |
(Advanced) Set the Rotation_ matrix directly - but you had better know what you are doing!
|
inline |
(Advanced) Set the Rotation_ matrix directly - but you had better know what you are doing!
|
inline |
(Advanced) Set the Rotation_ matrix directly - but you had better know what you are doing!
| 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).
| 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.
|
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.
|
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.
|
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.
|
inline |
Transpose operator.
For an orthogonal matrix like this one, transpose is the same thing as inversion.
|
inline |
Transpose operator.
For an orthogonal matrix like this one, transpose is the same thing as inversion.
|
inline |
Transpose.
For an orthogonal matrix like this one, transpose is the same thing as inversion. Overrides the base class transpose method.
|
inline |
Transpose.
For an orthogonal matrix like this one, transpose is the same thing as inversion. Overrides the base class transpose method.
|
inline |
Convert from Rotation_ to InverseRotation_ (no cost).
Overrides base class invert() method.
|
inline |
Convert from Rotation_ to writable InverseRotation_ (no cost).
|
inline |
In-place composition of Rotation matrices.
|
inline |
In-place composition of Rotation matrices.
|
inline |
In-place composition of Rotation matrices.
|
inline |
In-place composition of Rotation matrices.
|
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.
|
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.
|
inlinestatic |
Fastest way to form the product w_PB=NInv_P*qdot.
This is never singular. Cost is 9 flops.
|
inlinestatic |
Fastest way to form the product q=~NInv_P*v_P=~(~v_P*NInv_P).
This is never singular. Cost is 10 flops.
|
inline |
Return a reference to the ith row of this Rotation matrix as a UnitRow3.
|
inline |
Same as row(i) but nicer to look at.
|
inline |
Return a reference to the jth column of this Rotation matrix as a UnitVec3.
|
inline |
Same as col(j) but nicer to look at.
|
inline |
Return col(0) of this Rotation matrix as a UnitVec3.
|
inline |
Return col(1) of this Rotation matrix as a UnitVec3.
|
inline |
Return col(2) of this Rotation matrix as a UnitVec3.
|
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.
|
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.
|
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!
Cost: about 100 flops for sin/cos plus 12 to calculate N_B.
|
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.
|
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!
Cost: about 100 flops for sin/cos plus 12 to calculate N_P.
|
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.
|
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.
|
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.
|
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.
|
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.
|
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.
|
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.
|
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.
|
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.
|
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.
|
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.
|
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.
|
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.
|
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.
| 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.
| 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.
| 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.
| 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.
| 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.
|
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.
|
inline |
A convenient special case of convertTwoAxesRotationToTwoAngles().
|
inline |
A convenient special case of convertThreeAxesRotationToThreeAngles().
|
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.
|
inlinestatic |
Inverse of convertAngVelToBodyFixed321Dot.
Returned angular velocity is B in P, expressed in *B*: w_PB_B.
|
inlinestatic |
Caution: needs testing.
|
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!
|
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.
|
inlinestatic |
Inverse of the above routine.
Returned angular velocity is B in P, expressed in *B*: w_PB_B.
|
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.
|
inlinestatic |
Warning: everything is measured in the *PARENT* frame, but has to be expressed in the *BODY* frame.
|
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.
|
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.
|
inlinestatic |
Inverse of the above routine.
Returned AngVel is expressed in the *PARENT* frame: w_PB_P. Cost is 28 flops.
|
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.
|
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().
| cosxy | cos(qx), cos(qy) |
| sinxy | sin(qx), sin(qy) |
| oocosy | 1/cos(qy) |
| w_PB | angular velocity of B in P, exp. in P |
|
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.
| cosxy | cos(qx), cos(qy) |
| sinxy | sin(qx), sin(qy) |
| oocosy | 1/cos(qy) |
| qdot | previously calculated BodyXYZDot |
| b_PB | angular acceleration, a.k.a. wdot_PB |
| 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.
|
inline |
Return true if "this" Rotation is nearly identical to "R" within machine precision.
|
inline |
Returns maximum absolute difference between elements in "this" Rotation and elements in "R".
|
inline |
Returns true if each element of "this" Rotation is within epsilon of the corresponding element of "R".
|
inline |
Returns true if each element of "this" Rotation is within machine precision of the corresponding element of "R".
1.8.8