1#ifndef SimTK_SimTKCOMMON_ROTATION_H_
2#define SimTK_SimTKCOMMON_ROTATION_H_
46template <
class P>
class Rotation_;
47template <
class P>
class InverseRotation_;
181{
Mat33P& R = *
this; R[0][0] = 1; R[0][1] = R[0][2] = R[1][0] = R[2][0] = 0;
196{
Mat33P& R = *
this; R[1][1] = 1; R[0][1] = R[1][0] = R[1][2] = R[2][1] = 0;
211{
Mat33P& R = *
this; R[2][2] = 1; R[0][2] = R[1][2] = R[2][0] = R[2][1] = 0;
283{
Mat33P& R = *
this; R=m;
return *
this; }
515 (
Vec3P(0, std::cos(
q[1]), std::cos(
q[2])),
516 Vec3P(0, std::sin(
q[1]), std::sin(
q[2])));
549 (
Vec3P(std::cos(
q[0]), std::cos(
q[1]), 0),
550 Vec3P(std::sin(
q[0]), std::sin(
q[1]), 0));
584 (
Vec3P(0, std::cos(
q[1]), std::cos(
q[2])),
585 Vec3P(0, std::sin(
q[1]), std::sin(
q[2])),
627 Vec2P(std::sin(
q[0]), std::sin(
q[1])),
661 (
Vec3P(0, std::cos(
q[1]), std::cos(
q[2])),
662 Vec3P(0, std::sin(
q[1]), std::sin(
q[2])));
690 (
Vec3P(std::cos(
q[0]), std::cos(
q[1]), 0),
691 Vec3P(std::sin(
q[0]), std::sin(
q[1]), 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]);
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]);
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]);
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])),
1081 for(
int i=0;
i<=2;
i++ )
for(
int j=0;
j<=2;
j++ ) {
1113setTwoAngleTwoAxesBodyFixedForwardCyclicalRotation
1117setThreeAngleTwoAxesBodyFixedForwardCyclicalRotation
1122setThreeAngleThreeAxesBodyFixedForwardCyclicalRotation
1130convertTwoAxesBodyFixedRotationToTwoAngles
1131 (
const CoordinateAxis&
axis1,
const CoordinateAxis&
axis2 )
const;
1133convertTwoAxesBodyFixedRotationToThreeAngles
1134 (
const CoordinateAxis&
axis1,
const CoordinateAxis&
axis2 )
const;
1136convertThreeAxesBodyFixedRotationToThreeAngles
1137 (
const CoordinateAxis&
axis1,
const CoordinateAxis&
axis2,
1138 const CoordinateAxis&
axis3 )
const;
1146static Mat33P calcQBlockForBodyXYZInBodyFrame(
const Vec3P& a)
1149static Mat33P calcQInvBlockForBodyXYZInBodyFrame(
const Vec3P& a)
1164static Vec3P convertAngVelDotToBodyFixed123DotDot
1245void setToBodyFixed321(
const Vec3P& v)
1248void setToBodyFixed123(
const Vec3P& v)
1253Vec4P convertToAngleAxis()
const
1262void setToSpaceFixed12(
const Vec2P&
q )
1269Vec3P convertToBodyFixed123()
const
1271Vec2P convertToBodyFixed12()
const
1273Vec2P convertToSpaceFixed12()
const
1314{ BaseMat::operator=(R.
asMat33());
return *
this; }
1402template <
class P,
int S>
inline UnitRow<P,1>
1405template <
class P,
int S>
inline UnitVec<P,1>
1408template <
class P,
int S>
inline UnitRow<P,1>
1414template <
class P>
inline
1416:
Mat<3,3,P>( R.asMat33() ) {}
1426{
static_cast<Mat<3,3,P>&
>(*this) *= (~R).asMat33();
return *
this;}
1432{
static_cast<Mat<3,3,P>&
>(*this) *= (~R).asMat33();
return *
this;}
1439template <
class P>
inline Rotation_<P>
1442template <
class P>
inline Rotation_<P>
1445template <
class P>
inline Rotation_<P>
1453template <
class P>
inline Rotation_<P>
1456template <
class P>
inline Rotation_<P>
1459template <
class P>
inline Rotation_<P>
1462template <
class P>
inline Rotation_<P>
Defines the CoordinateAxis and CoordinateDirection classes.
#define SimTK_SimTKCOMMON_EXPORT
Definition SimTKcommon/include/SimTKcommon/internal/common.h:224
This file is the user-includeable header to be included in user programs to provide fixed-length Vec ...
Declares and defines the UnitVec and UnitRow classes.
Definition CoordinateAxis.h:194
Definition CoordinateAxis.h:197
Definition CoordinateAxis.h:200
This class, along with its sister class CoordinateDirection, provides convenient manipulation of the ...
Definition CoordinateAxis.h:53
A CoordinateDirection is a CoordinateAxis plus a direction indicating the positive or negative direct...
Definition CoordinateAxis.h:244
(Advanced) This InverseRotation class is the inverse of a Rotation.
Definition Rotation.h:1283
Rotation_< P > & updInvert()
Definition Rotation.h:1330
const ColType & col(int j) const
Definition Rotation.h:1351
const ColType & z() const
Definition Rotation.h:1356
const Rotation_< P > & invert() const
We can invert an InverseRotation just by recasting it to a Rotation at zero cost.
Definition Rotation.h:1328
const Rotation_< P > & transpose() const
Transpose, and transpose operators (override BaseMat versions of transpose).
Definition Rotation.h:1336
InverseRotation_(const InverseRotation_ &R)
An explicit implementation of the default copy constructor.
Definition Rotation.h:1311
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
Rotation_< P > & updTranspose()
Definition Rotation.h:1338
const ColType & operator()(int j) const
Definition Rotation.h:1353
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
Rotation_< P > & operator~()
Definition Rotation.h:1339
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,...
BaseMat toMat33() const
Definition Rotation.h:1382
const ColType & y() const
Definition Rotation.h:1355
InverseRotation_()
You should not ever construct one of these as they should only occur as expression intermediates resu...
Definition Rotation.h:1308
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
const BaseMat & asMat33() const
Conversion from InverseRotation_ to BaseMat.
Definition Rotation.h:1381
InverseRotation_ & operator=(const InverseRotation_ &R)
An explicit implementation of the default copy assignment operator.
Definition Rotation.h:1313
UnitRow< P, BaseMat::ColSpacing > RowType
This is the type of a row of this InverseRotation.
Definition Rotation.h:1302
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 & x() const
Definition Rotation.h:1354
const RowType & operator[](int i) const
Definition Rotation.h:1350
const Rotation_< P > & operator~() const
Definition Rotation.h:1337
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
This class represents a small matrix whose size is known at compile time, containing elements of any ...
Definition Mat.h:97
void setToNaN()
Definition Mat.h:904
void set(int i, int j, const P &value)
Variant of indexing operator that's scripting friendly to set entry (i, j)
Definition Mat.h:1227
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
A Quaternion is a Vec4 with the following behavior:
Definition Quaternion.h:65
Vec4P convertQuaternionToAngleAxis() const
Returns [ a vx vy vz ] with (a,v) in canonical form, i.e., -180 < a <= 180 and |v|=1.
The Rotation class is a Mat33 that guarantees that the matrix can be interpreted as a legitimate 3x3 ...
Definition Rotation.h:111
Mat< 3, 2, P > Mat32P
Definition Rotation.h:115
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
static Mat33P calcNInvForBodyXYZInParentFrame(const Vec3P &cq, const Vec3P &sq)
This faster version of calcNInvForBodyXYZInParentFrame() assumes you have already calculated the cosi...
Definition Rotation.h:699
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...
static Mat33P calcNInvForBodyXYZInBodyFrame(const Vec3P &cq, const Vec3P &sq)
This faster version of calcNInvForBodyXYZInBodyFrame() assumes you have already calculated the cosine...
Definition Rotation.h:670
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
Vec3P convertThreeAxesRotationToThreeAngles(BodyOrSpaceType bodyOrSpace, const CoordinateAxis &axis1, const CoordinateAxis &axis2, const CoordinateAxis &axis3) const
Converts rotation matrix to three orientation angles.
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
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...
static Vec3P convertBodyFixed321DotToAngVel(const Vec3P &q, const Vec3P &qd)
Inverse of convertAngVelToBodyFixed321Dot.
Definition Rotation.h:865
UnitRow< P, Mat33P::ColSpacing > RowType
This is the type of a row of this Rotation matrix.
Definition Rotation.h:131
InverseRotation_< P > & operator~()
Transpose operator.
Definition Rotation.h:360
Rotation_(const InverseRotation_< P > &)
Like copy constructor but for inverse rotation.
Definition Rotation.h:1415
Rotation_ & setRotationFromOneAxis(const UnitVec3P &uvec, CoordinateAxis axis)
Calculate R_AB by knowing one of B's unit vectors expressed in A.
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_ & 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
Rotation_(RealP angle, const CoordinateAxis &axis)
Constructor for right-handed rotation by an angle (in radians) about a coordinate axis.
Definition Rotation.h:161
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
SymMat< 3, P > SymMat33P
Definition Rotation.h:123
Rotation_ & setRotationFromAngleAboutY(RealP cosAngle, RealP sinAngle)
Set this Rotation_ object to a right-handed rotation by an angle about the Y-axis,...
Definition Rotation.h:195
Rotation_ & operator*=(const Rotation_< P > &R)
In-place composition of Rotation matrices.
Definition Rotation.h:1422
Rotation_ & setRotationFromAngleAboutZ(RealP cosAngle, RealP sinAngle)
Set this Rotation_ object to a right-handed rotation by an angle about the Z-axis,...
Definition Rotation.h:210
InverseRotation_< P > & updInvert()
Convert from Rotation_ to writable InverseRotation_ (no cost).
Definition Rotation.h:376
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
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
Mat< 4, 3, P > Mat43P
Definition Rotation.h:117
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 calcNInvForBodyXYZInParentFrame(const Vec3P &q)
Inverse of the above routine.
Definition Rotation.h:686
const InverseRotation_< P > & transpose() const
Transpose.
Definition Rotation.h:365
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
P RealP
These are just local abbreviations.
Definition Rotation.h:113
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
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 convertAngVelDotToBodyFixed321DotDot(const Vec3P &q, const Vec3P &w_PB_B, const Vec3P &wdot_PB_B)
Caution: needs testing.
Definition Rotation.h:881
bool isSameRotationToWithinAngle(const Rotation_ &R, RealP okPointingAngleErrorRads) const
Return true if "this" Rotation is nearly identical to "R" within a specified pointing angle error.
Vec< 4, P > Vec4P
Definition Rotation.h:121
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
const ColType & y() const
Return col(1) of this Rotation matrix as a UnitVec3.
Definition Rotation.h:476
Rotation_ & operator=(const InverseRotation_< P > &)
Like copy assignment but for inverse rotation.
Definition Rotation.h:1419
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 calculat...
Definition Rotation.h:977
const ColType & x() const
Return col(0) of this Rotation matrix as a UnitVec3.
Definition Rotation.h:474
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
const RowType & operator[](int i) const
Same as row(i) but nicer to look at.
Definition Rotation.h:464
RealP convertOneAxisRotationToOneAngle(const CoordinateAxis &axis1) const
Converts rotation matrix to a single orientation angle.
Rotation_(const QuaternionP &q)
Constructor for creating a rotation matrix from a quaternion.
Definition Rotation.h:264
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
Rotation_(const UnitVec3P &uvec, CoordinateAxis axis)
Calculate R_AB by knowing one of B's unit vectors expressed in A.
Definition Rotation.h:297
Rotation_ & setRotationToIdentityMatrix()
Construct identity Rotation_.
Definition Rotation.h:156
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 & getAxisUnitVec(CoordinateAxis axis) const
Given a CoordinateAxis (XAxis,YAxis, or ZAxis) return a reference to the corresponding column of this...
Definition Rotation.h:484
const ColType & operator()(int j) const
Same as col(j) but nicer to look at.
Definition Rotation.h:471
Rotation_(RealP angle, const CoordinateAxis::YCoordinateAxis)
Constructor for right-handed rotation by an angle (in radians) about the Y-axis.
Definition Rotation.h:187
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
Vec2P convertRotationToBodyFixedXY() const
A convenient special case of convertTwoAxesRotationToTwoAngles().
Definition Rotation.h:840
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
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 cosi...
Definition Rotation.h:635
Rotation_ & operator=(const Rotation_ &R)
Assignment operator.
Definition Rotation.h:146
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...
const InverseRotation_< P > & invert() const
Convert from Rotation_ to InverseRotation_ (no cost).
Definition Rotation.h:373
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
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
Mat33P toMat33() const
Conversion from Rotation to its base class Mat33.
Definition Rotation.h:761
Rotation_()
Default constructor.
Definition Rotation.h:137
InverseRotation_< P > & updTranspose()
Transpose.
Definition Rotation.h:369
Rotation_ & operator*=(const InverseRotation_< P > &)
In-place composition of Rotation matrices.
Definition Rotation.h:1428
UnitVec< P, Mat33P::RowSpacing > ColType
This is the type of a column of this Rotation matrix.
Definition Rotation.h:128
const ColType & z() const
Return col(2) of this Rotation matrix as a UnitVec3.
Definition Rotation.h:478
const InverseRotation_< P > & operator~() const
Transpose operator.
Definition Rotation.h:357
Vec3P convertRotationToBodyFixedXYZ() const
A convenient special case of convertThreeAxesRotationToThreeAngles().
Definition Rotation.h:843
Rotation_(const Mat33P &m)
Constructs an (hopefully nearby) orthogonal rotation matrix from a generic Mat33P.
Definition Rotation.h:271
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_ & operator/=(const InverseRotation_< P > &)
In-place composition of Rotation matrices.
Definition Rotation.h:1431
QuaternionP convertRotationToQuaternion() const
Converts rotation matrix to an equivalent quaternion in canonical form (meaning its scalar element is...
Rotation_ & setRotationFromQuaternion(const QuaternionP &q)
Method for creating a rotation matrix from a quaternion.
Rotation_(const Rotation_ &R)
Copy constructor.
Definition Rotation.h:140
UnitVec< P, 1 > UnitVec3P
Definition Rotation.h:122
Rotation_(const Mat33P &m, bool)
(Advanced) Construct a Rotation_ directly from a Mat33P (we trust that m is a valid Rotation_!...
Definition Rotation.h:279
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...
Definition Rotation.h:927
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 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
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
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 convertBodyXYZDotToAngVelInBodyFrame(const Vec3P &cq, const Vec3P &sq, const Vec3P &qdot)
This faster version of convertBodyXYZDotToAngVelInBodyFrame() assumes you have already calculated the...
Definition Rotation.h:950
Rotation_(RealP angle, const CoordinateAxis::ZCoordinateAxis)
Constructor for right-handed rotation by an angle (in radians) about the Z-axis.
Definition Rotation.h:202
Rotation_ & operator/=(const Rotation_< P > &R)
In-place composition of Rotation matrices.
Definition Rotation.h:1425
const Mat33P & asMat33() const
Conversion from Rotation to its base class Mat33.
Definition Rotation.h:757
static Mat33P calcNInvForBodyXYZInBodyFrame(const Vec3P &q)
Inverse of routine calcNForBodyXYZInBodyFrame().
Definition Rotation.h:657
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
Mat< 3, 3, P > Mat33P
Definition Rotation.h:116
Rotation_ & setRotationFromApproximateMat33(const Mat33P &m)
Set this Rotation_ object to an (hopefully nearby) orthogonal rotation matrix from a generic Mat33P.
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
Vec2P convertTwoAxesRotationToTwoAngles(BodyOrSpaceType bodyOrSpace, const CoordinateAxis &axis1, const CoordinateAxis &axis2) const
Converts rotation matrix to two orientation angles.
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
Mat< 3, 4, P > Mat34P
Definition Rotation.h:118
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
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
static Vec3P convertQuaternionDotToAngVel(const Vec4P &q, const Vec4P &qdot)
Inverse of the above routine.
Definition Rotation.h:996
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
RealP getMaxAbsDifferenceInRotationElements(const Rotation_ &R) const
Returns maximum absolute difference between elements in "this" Rotation and elements in "R".
Definition Rotation.h:1079
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...
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
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
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
Quaternion_< P > QuaternionP
Definition Rotation.h:124
Vec< 2, P > Vec2P
Definition Rotation.h:119
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
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
Rotation_(RealP angle, const CoordinateAxis::XCoordinateAxis)
Constructor for right-handed rotation by an angle (in radians) about the X-axis.
Definition Rotation.h:172
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
Rotation_ & setRotationFromMat33TrustMe(const Mat33P &m)
(Advanced) Set the Rotation_ matrix directly - but you had better know what you are doing!
Definition Rotation.h:282
bool isSameRotationToWithinAngleOfMachinePrecision(const Rotation_ &R) const
Return true if "this" Rotation is nearly identical to "R" within machine precision.
Definition Rotation.h:1074
Rotation_ & setRotationToNaN()
Construct Rotation_ filled with NaNs.
Definition Rotation.h:152
static Mat33P calcNDotForBodyXYZInBodyFrame(const Vec3P &cq, const Vec3P &sq, const Vec3P &qdot)
This faster version of calcNDotForBodyXYZInBodyFrame() assumes you have already calculated the cosine...
Definition Rotation.h:595
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
Vec4P convertRotationToAngleAxis() const
Converts rotation matrix to an equivalent angle-axis representation in canonicalized form.
Definition Rotation.h:836
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
const UnitVec< P, 1 > getAxisUnitVec(CoordinateDirection dir) const
Given a CoordinateDirection (+/-XAxis, etc.) return a unit vector in that direction.
Definition Rotation.h:491
Mat< 2, 2, P > Mat22P
Definition Rotation.h:114
const ColType & col(int j) const
Return a reference to the jth column of this Rotation matrix as a UnitVec3.
Definition Rotation.h:468
Rotation_ & setRotationFromAngleAboutX(RealP cosAngle, RealP sinAngle)
Set this Rotation_ object to a right-handed rotation by an angle about the X-axis,...
Definition Rotation.h:180
static Vec3P convertBodyXYZDotToAngVelInBodyFrame(const Vec3P &q, const Vec3P &qdot)
Inverse of the above routine.
Definition Rotation.h:936
Vec< 3, P > Vec3P
Definition Rotation.h:120
This is a small, fixed-size symmetric or Hermitian matrix designed for no-overhead inline computation...
Definition SymMat.h:87
This type is used for the transpose of UnitVec, and as the returned row type of a Rotation.
Definition UnitVec.h:211
const BaseRow & asRow3() const
Return a const reference to the Row3 underlying this UnitRow.
Definition UnitVec.h:256
This class is a Vec3 plus an ironclad guarantee either that:
Definition UnitVec.h:56
const BaseVec & asVec3() const
Return a reference to the underlying Vec3 (no copying here).
Definition UnitVec.h:117
This is a fixed-length column vector designed for no-overhead inline computation.
Definition Vec.h:184
ScalarNormSq normSqr() const
Definition Vec.h:608
This is the top-level SimTK namespace into which all SimTK names are placed to avoid collision with o...
Definition Assembler.h:37
Rotation_< Real > Rotation
Definition Rotation.h:49
Rotation_< float > fRotation
Definition Rotation.h:50
BodyOrSpaceType
Definition Rotation.h:42
@ BodyRotationSequence
Definition Rotation.h:42
@ SpaceRotationSequence
Definition Rotation.h:42
Matrix_< E > operator*(const MatrixBase< E > &l, const typename CNT< E >::StdNumber &r)
Definition BigMatrix.h:605
const CoordinateAxis::ZCoordinateAxis ZAxis
Constant representing the Z coordinate axis; will implicitly convert to the integer 2 when used in a ...
Matrix_< E > operator/(const MatrixBase< E > &l, const typename CNT< E >::StdNumber &r)
Definition BigMatrix.h:613
const CoordinateAxis::YCoordinateAxis YAxis
Constant representing the Y coordinate axis; will implicitly convert to the integer 1 when used in a ...
InverseRotation_< double > dInverseRotation
Definition Rotation.h:55
std::ostream & operator<<(std::ostream &o, const ContactForce &f)
Definition CompliantContactSubsystem.h:387
Rotation_< double > dRotation
Definition Rotation.h:51
InverseRotation_< Real > InverseRotation
Definition Rotation.h:53
InverseRotation_< float > fInverseRotation
Definition Rotation.h:54
const CoordinateAxis::XCoordinateAxis XAxis
Constant representing the X coordinate axis; will implicitly convert to the integer 0 when used in a ...