1 #ifndef SimTK_SimTKCOMMON_ROTATION_H_ 2 #define SimTK_SimTKCOMMON_ROTATION_H_ 46 template <
class P>
class Rotation_;
181 {
Mat33P& R = *
this; R[0][0] = 1; R[0][1] = R[0][2] = R[1][0] = R[2][0] = 0;
182 R[1][1] = R[2][2] = cosAngle; R[1][2] = -(R[2][1] = sinAngle);
196 {
Mat33P& R = *
this; R[1][1] = 1; R[0][1] = R[1][0] = R[1][2] = R[2][1] = 0;
197 R[0][0] = R[2][2] = cosAngle; R[2][0] = -(R[0][2] = sinAngle);
211 {
Mat33P& R = *
this; R[2][2] = 1; R[0][2] = R[1][2] = R[2][0] = R[2][1] = 0;
212 R[0][0] = R[1][1] = cosAngle; R[0][1] = -(R[1][0] = sinAngle);
254 (bodyOrSpace,angle1,axis1,angle2,axis2,angle3,axis3); }
283 {
Mat33P& R = *
this; R=m;
return *
this; }
287 {
Mat33P& R = *
this; R(colj)=uvecj.
asVec3();
return *
this; }
344 const RealP s0s1=s[0]*s[1], s2c0=s[2]*c[0], c0c2=c[0]*c[2], nc1= -c[1];
346 R =
Mat33P( c[1]*c[2] , s[2]*nc1 , s[1] ,
347 s2c0 + s0s1*c[2] , c0c2 - s0s1*s[2] , s[0]*nc1 ,
348 s[0]*s[2] - s[1]*c0c2 , s[0]*c[2] + s[1]*s2c0 , c[0]*c[1] );
400 const RealP s0 = sinxy[0], c0 = cosxy[0];
401 const RealP s1 = sinxy[1];
402 const RealP w0 = w_PB[0], w1 = w_PB[1], w2 = w_PB[2];
404 const RealP t = (s0*w1-c0*w2)*oocosy;
405 return Vec3P( w0 + t*s1, c0*w1 + s0*w2, -t );
416 const RealP s0 = sinxy[0], c0 = cosxy[0];
417 const RealP s1 = sinxy[1];
418 const RealP q0 = q[0], q1 = q[1], q2 = q[2];
420 const RealP t = (q0*s1-q2) * oocosy;
421 return Vec3P( q0, c0*q1 + t*s0, s0*q1 - t*c0 );
430 const RealP s0 = sinxy[0], c0 = cosxy[0];
431 const RealP s1 = sinxy[1], c1 = cosxy[1];
432 const RealP q0 = qdot[0], q1 = qdot[1], q2 = qdot[2];
433 const RealP c1q2 = c1*q2;
435 return Vec3P( q0 + s1*q2,
446 const RealP s0 = sinxy[0], c0 = cosxy[0];
447 const RealP s1 = sinxy[1], c1 = cosxy[1];
448 const RealP w0 = v_P[0], w1 = v_P[1], w2 = v_P[2];
452 s1*w0 - s0*c1*w1 + c0*c1*w2);
485 {
return col(axis); }
515 (
Vec3P(0, std::cos(q[1]), std::cos(q[2])),
516 Vec3P(0, std::sin(q[1]), std::sin(q[2])));
525 const RealP s1 = sq[1], c1 = cq[1];
526 const RealP s2 = sq[2], c2 = cq[2];
527 const RealP ooc1 = 1/c1;
528 const RealP s2oc1 = s2*ooc1, c2oc1 = c2*ooc1;
530 return Mat33P( c2oc1 , -s2oc1 , 0,
532 -s1*c2oc1 , s1*s2oc1, 1 );
549 (
Vec3P(std::cos(q[0]), std::cos(q[1]), 0),
550 Vec3P(std::sin(q[0]), std::sin(q[1]), 0));
560 const RealP s0 = sq[0], c0 = cq[0];
561 const RealP s1 = sq[1], c1 = cq[1];
562 const RealP ooc1 = 1/c1;
563 const RealP s0oc1 = s0*ooc1, c0oc1 = c0*ooc1;
565 return Mat33P( 1 , s1*s0oc1 , -s1*c0oc1,
567 0 , -s0oc1 , c0oc1 );
584 (
Vec3P(0, std::cos(q[1]), std::cos(q[2])),
585 Vec3P(0, std::sin(q[1]), std::sin(q[2])),
597 const RealP s1 = sq[1], c1 = cq[1];
598 const RealP s2 = sq[2], c2 = cq[2];
599 const RealP ooc1 = 1/c1;
600 const RealP s2oc1 = s2*ooc1, c2oc1 = c2*ooc1;
602 const RealP t = qdot[1]*s1*ooc1;
603 const RealP a = t*s2oc1 + qdot[2]*c2oc1;
604 const RealP b = t*c2oc1 - qdot[2]*s2oc1;
606 return Mat33P( b , -a , 0,
607 qdot[2]*c2 , -qdot[2]*s2 , 0,
608 -(s1*b + qdot[1]*c2) , s1*a + qdot[1]*s2 , 0 );
624 const RealP cy = std::cos(q[1]);
626 (
Vec2P(std::cos(q[0]), cy),
627 Vec2P(std::sin(q[0]), std::sin(q[1])),
636 const RealP s0 = sq[0], c0 = cq[0];
637 const RealP s1 = sq[1];
638 const RealP s0oc1 = s0*ooc1, c0oc1 = c0*ooc1;
640 const RealP t = qdot[1]*s1*ooc1;
641 const RealP a = t*s0oc1 + qdot[0]*c0oc1;
642 const RealP b = t*c0oc1 - qdot[0]*s0oc1;
644 return Mat33P( 0, s1*a + qdot[1]*s0, -(s1*b + qdot[1]*c0),
645 0, -qdot[0]*s0 , qdot[0]*c0 ,
661 (
Vec3P(0, std::cos(q[1]), std::cos(q[2])),
662 Vec3P(0, std::sin(q[1]), std::sin(q[2])));
671 const RealP s1 = sq[1], c1 = cq[1];
672 const RealP s2 = sq[2], c2 = cq[2];
674 return Mat33P( c1*c2 , s2 , 0 ,
690 (
Vec3P(std::cos(q[0]), std::cos(q[1]), 0),
691 Vec3P(std::sin(q[0]), std::sin(q[1]), 0));
700 const RealP s0 = sq[0], c0 = cq[0];
701 const RealP s1 = sq[1], c1 = cq[1];
703 return Mat33P( 1 , 0 , s1 ,
714 const RealP ne1 = -e[1], ne2 = -e[2], ne3 = -e[3];
715 return Mat43P( ne1, ne2, ne3,
727 const Vec4P ed = qdot/2;
728 const RealP ned1 = -ed[1], ned2 = -ed[2], ned3 = -ed[3];
729 return Mat43P( ned1, ned2, ned3,
744 const RealP ne1 = -e[1], ne2 = -e[2], ne3 = -e[3];
745 return Mat34P(ne1, e[0], ne3, e[2],
746 ne2, e[3], e[0], ne1,
747 ne3, ne2, e[1], e[0]);
852 const RealP s1 = std::sin(q[1]), c1 = std::cos(q[1]);
853 const RealP s2 = std::sin(q[2]), c2 = std::cos(q[2]);
855 const RealP s2oc1 = s2*ooc1, c2oc1 = c2*ooc1;
857 const Mat33P E( 0, s2oc1 , c2oc1 ,
859 1, s1*s2oc1 , s1*c2oc1 );
866 const RealP s1 = std::sin(q[1]), c1 = std::cos(q[1]);
867 const RealP s2 = std::sin(q[2]), c2 = std::cos(q[2]);
869 const Mat33P Einv( -s1 , 0 , 1 ,
883 const RealP s1 = std::sin(q[1]), c1 = std::cos(q[1]);
884 const RealP s2 = std::sin(q[2]), c2 = std::cos(q[2]);
885 const RealP ooc1 = 1/c1;
886 const RealP s2oc1 = s2*ooc1, c2oc1 = c2*ooc1, s1oc1 = s1*ooc1;
888 const Mat33P E( 0 , s2oc1 , c2oc1 ,
890 1 , s1*s2oc1 , s1*c2oc1 );
891 const Vec3P qdot =
E * w_PB_B;
893 const RealP t = qdot[1]*s1oc1;
894 const RealP a = t*s2oc1 + qdot[2]*c2oc1;
895 const RealP b = t*c2oc1 - qdot[2]*s2oc1;
897 const Mat33P Edot( 0 , a , b ,
898 0 , -qdot[2]*s2 , -qdot[2]*c2 ,
899 0 , s1*a + qdot[1]*s2 , s1*b + qdot[1]*c2 );
901 return E*wdot_PB_B + Edot*w_PB_B;
915 (
Vec3P(0, std::cos(q[1]), std::cos(q[2])),
916 Vec3P(0, std::sin(q[1]), std::sin(q[2])),
938 (
Vec3P(0, std::cos(q[1]), std::cos(q[2])),
939 Vec3P(0, std::sin(q[1]), std::sin(q[2])),
965 (
Vec3P(0, std::cos(q[1]), std::cos(q[2])),
966 Vec3P(0, std::sin(q[1]), std::sin(q[2])),
981 const Vec3P qdot = N * w_PB_B;
984 return N*wdot_PB_B + NDot*w_PB_B;
1010 const Vec4P Nb = N*b_PB;
1048 const RealP s1 = sinxy[1], c1 = cosxy[1];
1049 const RealP q0 = qdot[0], q1 = qdot[1], q2 = qdot[2];
1054 const RealP q1oc1 = q1*oocosy;
1055 const Vec3P NDotw((q0*s1-q2)*q1oc1,
1081 for(
int i=0; i<=2; i++ )
for(
int j=0; j<=2; j++ ) {
1083 if( absDiff > maxDiff ) maxDiff = absDiff;
1108 :
Mat33P( xx,xy,xz, yx,yy,yz, zx,zy,zz ) {}
1113 setTwoAngleTwoAxesBodyFixedForwardCyclicalRotation
1114 (
RealP cosAngle1,
RealP sinAngle1,
const CoordinateAxis& axis1,
1115 RealP cosAngle2,
RealP sinAngle2,
const CoordinateAxis& axis2 );
1117 setThreeAngleTwoAxesBodyFixedForwardCyclicalRotation
1118 (
RealP cosAngle1,
RealP sinAngle1,
const CoordinateAxis& axis1,
1119 RealP cosAngle2,
RealP sinAngle2,
const CoordinateAxis& axis2,
1122 setThreeAngleThreeAxesBodyFixedForwardCyclicalRotation
1123 (
RealP cosAngle1,
RealP sinAngle1,
const CoordinateAxis& axis1,
1124 RealP cosAngle2,
RealP sinAngle2,
const CoordinateAxis& axis2,
1125 RealP cosAngle3,
RealP sinAngle3,
const CoordinateAxis& axis3 );
1130 convertTwoAxesBodyFixedRotationToTwoAngles
1131 (
const CoordinateAxis& axis1,
const CoordinateAxis& axis2 )
const;
1133 convertTwoAxesBodyFixedRotationToThreeAngles
1134 (
const CoordinateAxis& axis1,
const CoordinateAxis& axis2 )
const;
1136 convertThreeAxesBodyFixedRotationToThreeAngles
1137 (
const CoordinateAxis& axis1,
const CoordinateAxis& axis2,
1138 const CoordinateAxis& axis3 )
const;
1146 static Mat33P calcQBlockForBodyXYZInBodyFrame(
const Vec3P& a)
1149 static Mat33P calcQInvBlockForBodyXYZInBodyFrame(
const Vec3P& a)
1152 static Mat<4,3,P> calcUnnormalizedQBlockForQuaternion(
const Vec4P& q)
1155 static Mat<3,4,P> calcUnnormalizedQInvBlockForQuaternion(
const Vec4P& q)
1158 static Vec3P convertAngVelToBodyFixed123Dot(
const Vec3P& q,
const Vec3P& w_PB_B)
1161 static Vec3P convertBodyFixed123DotToAngVel(
const Vec3P& q,
const Vec3P& qdot)
1164 static Vec3P convertAngVelDotToBodyFixed123DotDot
1214 {
return aboutXThenOldY(xInRad, yInRad); }
1216 {
return aboutZThenOldX(zInRad, xInRad); }
1218 {
return aboutXThenOldZ(xInRad, zInRad); }
1220 {
return aboutZThenOldY(zInRad, yInRad); }
1222 {
return aboutYThenOldZ(yInRad, zInRad); }
1245 void setToBodyFixed321(
const Vec3P& v)
1248 void setToBodyFixed123(
const Vec3P& v)
1253 Vec4P convertToAngleAxis()
const 1262 void setToSpaceFixed12(
const Vec2P& q )
1269 Vec3P convertToBodyFixed123()
const 1271 Vec2P convertToBodyFixed12()
const 1273 Vec2P convertToSpaceFixed12()
const 1283 class InverseRotation_ :
public Mat<3,3,P>::TransposeType {
1284 typedef Mat<3,3,P> Mat33P;
1285 typedef Mat<2,2,P> Mat22P;
1286 typedef Mat<3,2,P> Mat32P;
1287 typedef Vec<2,P> Vec2P;
1288 typedef Vec<3,P> Vec3P;
1289 typedef Vec<4,P> Vec4P;
1290 typedef Quaternion_<P> QuaternionP;
1314 { BaseMat::operator=(R.asMat33());
return *
this; }
1365 {
return col(axis); }
1389 operator<<(std::ostream&, const Rotation_<P>&);
1393 operator<<(std::ostream&, const InverseRotation_<P>&);
1399 template <
class P,
int S>
inline UnitVec<P,1>
1402 template <
class P,
int S>
inline UnitRow<P,1>
1405 template <
class P,
int S>
inline UnitVec<P,1>
1408 template <
class P,
int S>
inline UnitRow<P,1>
1414 template <
class P>
inline 1416 :
Mat<3,3,P>( R.asMat33() ) {}
1420 {
static_cast<Mat<3,3,P>&
>(*this) = R.asMat33();
return *
this;}
1426 {
static_cast<Mat<3,3,P>&
>(*this) *= (~R).asMat33();
return *
this;}
1429 {
static_cast<Mat<3,3,P>&
>(*this) *= R.asMat33();
return *
this;}
1432 {
static_cast<Mat<3,3,P>&
>(*this) *= (~R).asMat33();
return *
this;}
1439 template <
class P>
inline Rotation_<P>
1442 template <
class P>
inline Rotation_<P>
1445 template <
class P>
inline Rotation_<P>
1453 template <
class P>
inline Rotation_<P>
1456 template <
class P>
inline Rotation_<P>
1459 template <
class P>
inline Rotation_<P>
1462 template <
class P>
inline Rotation_<P>
1471 #endif // SimTK_SimTKCOMMON_ROTATION_H_ 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.
Definition: Rotation.h:210
Rotation_(const Rotation_ &R)
Copy constructor.
Definition: Rotation.h:140
Vec< 3, P > Vec3P
Definition: Rotation.h:120
Inertia_< P > operator/(const Inertia_< P > &i, const P &r)
Divide an inertia matrix by a scalar.
Definition: MassProperties.h:612
InverseRotation_< Real > InverseRotation
Definition: Rotation.h:53
Rotation_ & setRotationFromQuaternion(const QuaternionP &q)
Method for creating a rotation matrix from a quaternion.
#define SimTK_SimTKCOMMON_EXPORT
Definition: SimTKcommon/include/SimTKcommon/internal/common.h:224
Vec< 4, P > Vec4P
Definition: Rotation.h:121
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).
Definition: Rotation.h:442
(Advanced) This InverseRotation class is the inverse of a Rotation.
Definition: Rotation.h:47
InverseRotation_< double > dInverseRotation
Definition: Rotation.h:55
const ColType & getAxisUnitVec(CoordinateAxis axis) const
Given a CoordinateAxis (XAxis,YAxis, or ZAxis) return a reference to the corresponding column of this...
Definition: Rotation.h:1364
const UnitVec< P, 1 > getAxisUnitVec(CoordinateDirection dir) const
Given a CoordinateDirection (+/-XAxis, etc.) return a unit vector in that direction.
Definition: Rotation.h:491
Defines the CoordinateAxis and CoordinateDirection classes.
const InverseRotation_< P > & transpose() const
Transpose.
Definition: Rotation.h:365
Rotation_(RealP angle, const Vec3P &nonUnitVector)
Constructor for right-handed rotation by an angle (in radians) about an arbitrary vector of arbitrary...
Definition: Rotation.h:226
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...
Definition: Rotation.h:621
This class is a Vec3 plus an ironclad guarantee either that:
Definition: UnitVec.h:40
const Rotation_< P > & transpose() const
Transpose, and transpose operators (override BaseMat versions of transpose).
Definition: Rotation.h:1336
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...
Definition: Rotation.h:960
static Vec3P convertBodyXYZDotToAngVelInBodyFrame(const Vec3P &q, const Vec3P &qdot)
Inverse of the above routine.
Definition: Rotation.h:936
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)...
Definition: Rotation.h:249
This is the top-level SimTK namespace into which all SimTK names are placed to avoid collision with o...
Definition: Assembler.h:37
Rotation_ & operator=(const Rotation_ &R)
Assignment operator.
Definition: Rotation.h:146
Rotation_< double > dRotation
Definition: Rotation.h:51
static Vec3P convertAngVelDotToBodyFixed321DotDot(const Vec3P &q, const Vec3P &w_PB_B, const Vec3P &wdot_PB_B)
Caution: needs testing.
Definition: Rotation.h:881
Rotation_ & setRotationFromOneAxis(const UnitVec3P &uvec, CoordinateAxis axis)
Calculate R_AB by knowing one of B's unit vectors expressed in A.
SymMat33P reexpressSymMat33(const SymMat33P &S_BB) const
Perform an efficient transform of a symmetric matrix that must be re-expressed with a multiply from b...
Definition: CoordinateAxis.h:194
A CoordinateDirection is a CoordinateAxis plus a direction indicating the positive or negative direct...
Definition: CoordinateAxis.h:244
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"...
Definition: Rotation.h:1090
Definition: CoordinateAxis.h:197
Quaternion_< P > QuaternionP
Definition: Rotation.h:124
QuaternionP convertRotationToQuaternion() const
Converts rotation matrix to an equivalent quaternion in canonical form (meaning its scalar element is...
bool isSameRotationToWithinAngle(const Rotation_ &R, RealP okPointingAngleErrorRads) const
Return true if "this" Rotation is nearly identical to "R" within a specified pointing angle error...
Rotation_(const UnitVec3P &uvec, CoordinateAxis axis)
Calculate R_AB by knowing one of B's unit vectors expressed in A.
Definition: Rotation.h:297
static Mat34P calcUnnormalizedNInvForQuaternion(const Vec4P &q)
Given a (possibly unnormalized) quaternion q, calculate the 3x4 matrix NInv (= N^-1) which maps quate...
Definition: Rotation.h:742
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.
Definition: Rotation.h:195
bool areAllRotationElementsSameToMachinePrecision(const Rotation_ &R) const
Returns true if each element of "this" Rotation is within machine precision of the corresponding elem...
Definition: Rotation.h:1095
Definition: CoordinateAxis.h:200
Definition: Rotation.h:42
CoordinateAxis getAxis() const
This is the coordinate axis XAxis, YAxis, or ZAxis contained in this CoordinateDirection. Use getDirection() to determine whether this is the positive or negative direction.
Definition: CoordinateAxis.h:274
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 ...
Definition: Rotation.h:1042
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 velocit...
Definition: Rotation.h:990
InverseRotation_< P > & updInvert()
Convert from Rotation_ to writable InverseRotation_ (no cost).
Definition: Rotation.h:376
SymMat< 3, P > SymMat33P
Definition: Rotation.h:123
const RowType & operator[](int i) const
Same as row(i) but nicer to look at.
Definition: Rotation.h:464
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...
InverseRotation_< P > & updTranspose()
Transpose.
Definition: Rotation.h:369
bool isSameRotationToWithinAngleOfMachinePrecision(const Rotation_ &R) const
Return true if "this" Rotation is nearly identical to "R" within machine precision.
Definition: Rotation.h:1074
Rotation_ & setRotationFromApproximateMat33(const Mat33P &m)
Set this Rotation_ object to an (hopefully nearby) orthogonal rotation matrix from a generic Mat33P...
Rotation_(RealP angle, const CoordinateAxis::YCoordinateAxis)
Constructor for right-handed rotation by an angle (in radians) about the Y-axis.
Definition: Rotation.h:187
InverseRotation_< P > & operator~()
Transpose operator.
Definition: Rotation.h:360
const CoordinateAxis::ZCoordinateAxis ZAxis
Constant representing the Z coordinate axis; will implicitly convert to the integer 2 when used in a ...
This class, along with its sister class CoordinateDirection, provides convenient manipulation of the ...
Definition: CoordinateAxis.h:53
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 i...
const ColType & z() const
Access individual rows and columns of this InverseRotation; no cost or copying since suitably-cast re...
Definition: Rotation.h:1356
const BaseMat & asMat33() const
Conversion from InverseRotation_ to BaseMat.
Definition: Rotation.h:1381
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...
Definition: Rotation.h:165
const ColType & col(int j) const
Return a reference to the jth column of this Rotation matrix as a UnitVec3.
Definition: Rotation.h:468
P RealP
These are just local abbreviations.
Definition: Rotation.h:113
static Mat43P calcUnnormalizedNDotForQuaternion(const Vec4P &qdot)
Given the time derivative qdot of a possibly unnormalized quaternion q, calculate the 4x3 matrix NDot...
Definition: Rotation.h:726
static Mat33P calcNInvForBodyXYZInParentFrame(const Vec3P &q)
Inverse of the above routine.
Definition: Rotation.h:686
Vec4P convertQuaternionToAngleAxis() const
Returns [ a vx vy vz ] with (a,v) in canonical form, i.e., -180 < a <= 180 and |v|=1.
Mat< 4, 3, P > Mat43P
Definition: Rotation.h:117
Declares and defines the UnitVec and UnitRow classes.
const CoordinateAxis::YCoordinateAxis YAxis
Constant representing the Y coordinate axis; will implicitly convert to the integer 1 when used in a ...
UnitVec< P, BaseMat::RowSpacing > ColType
Note that the unit vectors representing the rows and columns of this matrix do not necessarily have u...
Definition: Rotation.h:1300
SymMat< 3, P > reexpressSymMat33(const SymMat< 3, P > &S_BB) const
Assuming this InverseRotation_ is R_AB, and given a symmetric dyadic matrix S_BB expressed in B...
const ColType & operator()(int j) const
Same as col(j) but nicer to look at.
Definition: Rotation.h:471
Rotation_ & setRotationToIdentityMatrix()
Construct identity Rotation_.
Definition: Rotation.h:156
InverseRotation_()
You should not ever construct one of these as they should only occur as expression intermediates resu...
Definition: Rotation.h:1308
Rotation_< Real > Rotation
Definition: Rotation.h:47
const Rotation_< P > & invert() const
We can invert an InverseRotation just by recasting it to a Rotation at zero cost. ...
Definition: Rotation.h:1328
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 m...
Definition: Rotation.h:545
Vec< 2, P > Vec2P
Definition: Rotation.h:119
int getDirection() const
Returns 1 or -1 to indicate the direction along the coordinate axis returned by getAxis().
Definition: CoordinateAxis.h:277
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.
Definition: Rotation.h:426
This is a fixed-length column vector designed for no-overhead inline computation. ...
Definition: SimTKcommon/include/SimTKcommon/internal/common.h:618
void setRotationToBodyFixedXY(const Vec2P &v)
Set this Rotation_ to represent a rotation characterized by subsequent rotations of: +v[0] about the ...
Definition: Rotation.h:327
ScalarNormSq normSqr() const
Definition: Vec.h:608
InverseRotation_ & operator=(const InverseRotation_ &R)
An explicit implementation of the default copy assignment operator.
Definition: Rotation.h:1313
Vec2P convertRotationToBodyFixedXY() const
A convenient special case of convertTwoAxesRotationToTwoAngles().
Definition: Rotation.h:840
const ColType & y() const
Return col(1) of this Rotation matrix as a UnitVec3.
Definition: Rotation.h:476
Matrix_< E > operator*(const MatrixBase< E > &l, const typename CNT< E >::StdNumber &r)
Definition: BigMatrix.h:605
Mat< 3, 4, P > Mat34P
Definition: Rotation.h:118
const ColType & x() const
Return col(0) of this Rotation matrix as a UnitVec3.
Definition: Rotation.h:474
Rotation_ & setRotationToNaN()
Construct Rotation_ filled with NaNs.
Definition: Rotation.h:152
Definition: Rotation.h:42
bool isYAxis() const
Return true if this is the Y axis.
Definition: CoordinateAxis.h:93
Inertia_< P > operator*(const Inertia_< P > &i, const P &r)
Multiply an inertia matrix by a scalar.
Definition: MassProperties.h:584
const InverseRotation_< P > & operator~() const
Transpose operator.
Definition: Rotation.h:357
BaseMat toMat33() const
Conversion from InverseRotation_ to BaseMat.
Definition: Rotation.h:1382
const Rotation_< P > & operator~() const
Transpose, and transpose operators (override BaseMat versions of transpose).
Definition: Rotation.h:1337
Mat< 3, 3, P >::TransposeType BaseMat
This is the type of the underlying 3x3 matrix; note that it will have unusual row and column spacing ...
Definition: Rotation.h:1294
Rotation_ & setRotationColFromUnitVecTrustMe(int colj, const UnitVec3P &uvecj)
(Advanced) Set the Rotation_ matrix directly - but you had better know what you are doing! ...
Definition: Rotation.h:286
const ColType & x() const
Access individual rows and columns of this InverseRotation; no cost or copying since suitably-cast re...
Definition: Rotation.h:1354
Mat< 2, 2, P > Mat22P
Definition: Rotation.h:114
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 radi...
Definition: Rotation.h:236
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...
Definition: Rotation.h:1007
const BaseVec & asVec3() const
Return a reference to the underlying Vec3 (no copying here).
Definition: UnitVec.h:117
Mat33P toMat33() const
Conversion from Rotation to its base class Mat33.
Definition: Rotation.h:761
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...
Definition: Rotation.h:395
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 (ang...
Rotation_(RealP angle, const UnitVec3P &unitVector)
Constructor for right-handed rotation by an angle (in radians) about an arbitrary unit vector...
Definition: Rotation.h:217
Rotation_ & setRotationFromAngleAboutNonUnitVector(RealP angle, const Vec3P &nonUnitVector)
Set this Rotation_ object to a right-handed rotation of an angle (in radians) about an arbitrary vect...
Definition: Rotation.h:231
const RowType & operator[](int i) const
Access individual rows and columns of this InverseRotation; no cost or copying since suitably-cast re...
Definition: Rotation.h:1350
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...
Definition: Rotation.h:580
const ColType & z() const
Return col(2) of this Rotation matrix as a UnitVec3.
Definition: Rotation.h:478
RealP convertOneAxisRotationToOneAngle(const CoordinateAxis &axis1) const
Converts rotation matrix to a single orientation angle.
This file is the user-includeable header to be included in user programs to provide fixed-length Vec ...
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.
Definition: Rotation.h:180
void setToNaN()
Definition: Mat.h:904
Rotation_(const Mat33P &m, bool)
(Advanced) Construct a Rotation_ directly from a Mat33P (we trust that m is a valid Rotation_!) Thing...
Definition: Rotation.h:279
Mat< 3, 2, P > Mat32P
Definition: Rotation.h:115
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 ve...
Definition: Rotation.h:310
Rotation_(RealP angle, const CoordinateAxis::XCoordinateAxis)
Constructor for right-handed rotation by an angle (in radians) about the X-axis.
Definition: Rotation.h:172
const ColType & y() const
Access individual rows and columns of this InverseRotation; no cost or copying since suitably-cast re...
Definition: Rotation.h:1355
Vec3P convertThreeAxesRotationToThreeAngles(BodyOrSpaceType bodyOrSpace, const CoordinateAxis &axis1, const CoordinateAxis &axis2, const CoordinateAxis &axis3) const
Converts rotation matrix to three orientation angles.
static Vec3P convertQuaternionDotToAngVel(const Vec4P &q, const Vec4P &qdot)
Inverse of the above routine.
Definition: Rotation.h:996
RowVectorBase< typename CNT< ELEM >::TAbs > abs(const RowVectorBase< ELEM > &v)
Definition: VectorMath.h:120
BodyOrSpaceType
Definition: Rotation.h:42
Rotation_< P > & updTranspose()
Transpose, and transpose operators (override BaseMat versions of transpose).
Definition: Rotation.h:1338
const BaseRow & asRow3() const
Return a const reference to the Row3 underlying this UnitRow.
Definition: UnitVec.h:256
Rotation_()
Default constructor.
Definition: Rotation.h:137
const RowType & row(int i) const
Return a reference to the ith row of this Rotation matrix as a UnitRow3.
Definition: Rotation.h:461
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! ...
Definition: Rotation.h:291
Rotation_ & setRotationFromAngleAboutX(RealP angle)
Set this Rotation_ object to a right-handed rotation by an angle (in radians) about the X-axis...
Definition: Rotation.h:176
UnitRow< P, Mat33P::ColSpacing > RowType
This is the type of a row of this Rotation matrix.
Definition: Rotation.h:131
Rotation_(RealP angle, const CoordinateAxis &axis)
Constructor for right-handed rotation by an angle (in radians) about a coordinate axis...
Definition: Rotation.h:161
UnitVec< P, Mat33P::RowSpacing > ColType
This is the type of a column of this Rotation matrix.
Definition: Rotation.h:128
Mat & operator=(const Mat &src)
Copy assignment copies only the elements that are present and does not touch any unused memory space ...
Definition: Mat.h:306
InverseRotation_< float > fInverseRotation
Definition: Rotation.h:54
InverseRotation_(const InverseRotation_ &R)
An explicit implementation of the default copy constructor.
Definition: Rotation.h:1311
bool isXAxis() const
Return true if this is the X axis.
Definition: CoordinateAxis.h:91
static Mat43P calcUnnormalizedNForQuaternion(const Vec4P &q)
Given a possibly unnormalized quaternion q, calculate the 4x3 matrix N which maps angular velocity w ...
Definition: Rotation.h:712
RealP getMaxAbsDifferenceInRotationElements(const Rotation_ &R) const
Returns maximum absolute difference between elements in "this" Rotation and elements in "R"...
Definition: Rotation.h:1079
Rotation_ & setRotationFromMat33TrustMe(const Mat33P &m)
(Advanced) Set the Rotation_ matrix directly - but you had better know what you are doing! ...
Definition: Rotation.h:282
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 multipl...
Definition: Rotation.h:411
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 ...
Definition: Rotation.h:511
static Vec3P convertBodyFixed321DotToAngVel(const Vec3P &q, const Vec3P &qd)
Inverse of convertAngVelToBodyFixed321Dot.
Definition: Rotation.h:865
Rotation_ & operator*=(const Rotation_< P > &R)
In-place composition of Rotation matrices.
Definition: Rotation.h:1422
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 ve...
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 ...
Definition: Rotation.h:1022
Rotation_< float > fRotation
Definition: Rotation.h:50
Mat< 3, 3, P > Mat33P
Definition: Rotation.h:116
const ColType & operator()(int j) const
Access individual rows and columns of this InverseRotation; no cost or copying since suitably-cast re...
Definition: Rotation.h:1353
A Quaternion is a Vec4 with the following behavior:
Definition: Quaternion.h:41
Rotation_(RealP angle, const CoordinateAxis::ZCoordinateAxis)
Constructor for right-handed rotation by an angle (in radians) about the Z-axis.
Definition: Rotation.h:202
const Mat33P & asMat33() const
Conversion from Rotation to its base class Mat33.
Definition: Rotation.h:757
Rotation_ & setRotationFromAngleAboutZ(RealP angle)
Set this Rotation_ object to a right-handed rotation by an angle (in radians) about the Z-axis...
Definition: Rotation.h:206
static Mat33P calcNForBodyXYZInBodyFrame(const Vec3P &cq, const Vec3P &sq)
This faster version of calcNForBodyXYZInBodyFrame() assumes you have already calculated the cosine an...
Definition: Rotation.h:524
UnitRow< P, BaseMat::ColSpacing > RowType
This is the type of a row of this InverseRotation.
Definition: Rotation.h:1302
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...
Definition: Rotation.h:851
Vec2P convertTwoAxesRotationToTwoAngles(BodyOrSpaceType bodyOrSpace, const CoordinateAxis &axis1, const CoordinateAxis &axis2) const
Converts rotation matrix to two orientation angles.
const RowType & row(int i) const
Access individual rows and columns of this InverseRotation; no cost or copying since suitably-cast re...
Definition: Rotation.h:1348
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...
Definition: Rotation.h:342
Vec4P convertRotationToAngleAxis() const
Converts rotation matrix to an equivalent angle-axis representation in canonicalized form...
Definition: Rotation.h:836
Rotation_< P > & operator~()
Transpose, and transpose operators (override BaseMat versions of transpose).
Definition: Rotation.h:1339
Rotation_(const Mat33P &m)
Constructs an (hopefully nearby) orthogonal rotation matrix from a generic Mat33P.
Definition: Rotation.h:271
const ColType & getAxisUnitVec(CoordinateAxis axis) const
Given a CoordinateAxis (XAxis,YAxis, or ZAxis) return a reference to the corresponding column of this...
Definition: Rotation.h:484
This class represents a small matrix whose size is known at compile time, containing elements of any ...
Definition: SimTKcommon/include/SimTKcommon/internal/common.h:620
This type is used for the transpose of UnitVec, and as the returned row type of a Rotation...
Definition: UnitVec.h:41
void setRotationToBodyFixedXYZ(const Vec3P &v)
Set this Rotation_ to represent a rotation characterized by subsequent rotations of: +v[0] about the ...
Definition: Rotation.h:336
Rotation_(const QuaternionP &q)
Constructor for creating a rotation matrix from a quaternion.
Definition: Rotation.h:264
Rotation_ & operator/=(const Rotation_< P > &R)
In-place composition of Rotation matrices.
Definition: Rotation.h:1425
static Mat33P calcNForBodyXYZInParentFrame(const Vec3P &cq, const Vec3P &sq)
This faster version of calcNForBodyXYZInParentFrame() assumes you have already calculated the cosine ...
Definition: Rotation.h:559
UnitVec< P, 1 > UnitVec3P
Definition: Rotation.h:122
const UnitVec< P, 1 > getAxisUnitVec(CoordinateDirection dir) const
Given a CoordinateDirection (+/-XAxis, etc.) return a unit vector in that direction.
Definition: Rotation.h:1371
const ColType & col(int j) const
Access individual rows and columns of this InverseRotation; no cost or copying since suitably-cast re...
Definition: Rotation.h:1351
Rotation_ & setRotationFromAngleAboutY(RealP angle)
Set this Rotation_ object to a right-handed rotation by an angle (in radians) about the Y-axis...
Definition: Rotation.h:191
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 vecto...
Definition: Rotation.h:913
Rotation_< P > & updInvert()
We can invert an InverseRotation just by recasting it to a Rotation at zero cost. ...
Definition: Rotation.h:1330
const InverseRotation_< P > & invert() const
Convert from Rotation_ to InverseRotation_ (no cost).
Definition: Rotation.h:373
Vec3P convertRotationToBodyFixedXYZ() const
A convenient special case of convertThreeAxesRotationToThreeAngles().
Definition: Rotation.h:843
static Mat33P calcNInvForBodyXYZInBodyFrame(const Vec3P &q)
Inverse of routine calcNForBodyXYZInBodyFrame().
Definition: Rotation.h:657
const CoordinateAxis::XCoordinateAxis XAxis
Constant representing the X coordinate axis; will implicitly convert to the integer 0 when used in a ...