Simbody 3.7
Loading...
Searching...
No Matches
Rotation.h
Go to the documentation of this file.
1#ifndef SimTK_SimTKCOMMON_ROTATION_H_
2#define SimTK_SimTKCOMMON_ROTATION_H_
3
4/* -------------------------------------------------------------------------- *
5 * Simbody(tm): SimTKcommon *
6 * -------------------------------------------------------------------------- *
7 * This is part of the SimTK biosimulation toolkit originating from *
8 * Simbios, the NIH National Center for Physics-Based Simulation of *
9 * Biological Structures at Stanford, funded under the NIH Roadmap for *
10 * Medical Research, grant U54 GM072970. See https://simtk.org/home/simbody. *
11 * *
12 * Portions copyright (c) 2005-14 Stanford University and the Authors. *
13 * Authors: Paul Mitiguy, Michael Sherman *
14 * Contributors: *
15 * *
16 * Licensed under the Apache License, Version 2.0 (the "License"); you may *
17 * not use this file except in compliance with the License. You may obtain a *
18 * copy of the License at http://www.apache.org/licenses/LICENSE-2.0. *
19 * *
20 * Unless required by applicable law or agreed to in writing, software *
21 * distributed under the License is distributed on an "AS IS" BASIS, *
22 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. *
23 * See the License for the specific language governing permissions and *
24 * limitations under the License. *
25 * -------------------------------------------------------------------------- */
26
27//------------------------------------------------------------------------------
28
33
34//------------------------------------------------------------------------------
35#include <iosfwd> // Forward declaration of iostream
36//------------------------------------------------------------------------------
37
38//------------------------------------------------------------------------------
39namespace SimTK {
40
41
43
44//------------------------------------------------------------------------------
45// Forward declarations
46template <class P> class Rotation_;
47template <class P> class InverseRotation_;
48
52
56
57//------------------------------------------------------------------------------
109//------------------------------------------------------------------------------
110template <class P> // templatized by precision
111class Rotation_ : public Mat<3,3,P> {
112public:
113typedef P RealP;
122typedef UnitVec<P,1> UnitVec3P; // stride is 1 here, length is always 3
125
132
133//------------------------------------------------------------------------------
138
140Rotation_( const Rotation_& R ) : Mat33P(R) {}
144
147{ Mat33P::operator=( R.asMat33() ); return *this; }
150
153{ Mat33P::setToNaN(); return *this; }
154
158
169
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);
183 return *this; }
184
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);
198 return *this; }
199
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);
213 return *this; }
214
223
230Rotation_&
233
246
262
268
276
279Rotation_(const Mat33P& m, bool) : Mat33P(m) {}
283{ Mat33P& R = *this; R=m; return *this; }
287{ Mat33P& R = *this; R(colj)=uvecj.asVec3(); return *this; }
291 (const UnitVec3P& colA, const UnitVec3P& colB, const UnitVec3P& colC)
292{ Mat33P& R = *this; R(0)=colA.asVec3(); R(1)=colB.asVec3(); R(2)=colC.asVec3();
293 return *this; }
294
303
322
330
343 Mat33P& R = *this;
344 const RealP s0s1=s[0]*s[1], s2c0=s[2]*c[0], c0c2=c[0]*c[2], nc1= -c[1];
345
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] );
349}
351
352//------------------------------------------------------------------------------
357const InverseRotation_<P>& operator~() const { return invert(); }
361
365const InverseRotation_<P>& transpose() const { return invert(); }
370
374{ return *reinterpret_cast<const InverseRotation_<P>*>(this); }
377{ return *reinterpret_cast<InverseRotation_<P>*>(this); }
378
383
388
396 const Vec2P& sinxy,
398 const Vec3P& w_PB)
399{
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];
403
404 const RealP t = (s0*w1-c0*w2)*oocosy;
405 return Vec3P( w0 + t*s1, c0*w1 + s0*w2, -t ); // qdot
406}
407
412 const Vec2P& sinxy,
414 const Vec3P& q)
415{
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];
419
420 const RealP t = (q0*s1-q2) * oocosy;
421 return Vec3P( q0, c0*q1 + t*s0, s0*q1 - t*c0 ); // v_P
422}
423
427 const Vec2P& sinxy,
428 const Vec3P& qdot)
429{
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;
434
435 return Vec3P( q0 + s1*q2, // w_PB
436 c0*q1 - s0*c1q2,
437 s0*q1 + c0*c1q2 );
438}
439
443 const Vec2P& sinxy,
444 const Vec3P& v_P)
445{
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];
449
450 return Vec3P( w0, // qdot-like
451 c0*w1 + s0*w2,
452 s1*w0 - s0*c1*w1 + c0*c1*w2);
453}
455
456//------------------------------------------------------------------------------
461const RowType& row( int i ) const
462{ return reinterpret_cast<const RowType&>(asMat33()[i]); }
464const RowType& operator[]( int i ) const { return row(i); }
465
468const ColType& col( int j ) const
469{ return reinterpret_cast<const ColType&>(asMat33()(j)); }
471const ColType& operator()( int j ) const { return col(j); }
472
474const ColType& x() const { return col(0); }
476const ColType& y() const { return col(1); }
478const ColType& z() const { return col(2); }
479
485{ return col(axis); }
486
492 const ColType& axDir = getAxisUnitVec(dir.getAxis());
493 return dir.getDirection() > 0 ? UnitVec<P,1>( axDir)
494 : UnitVec<P,1>(-axDir); // cheap
495}
497
498//------------------------------------------------------------------------------
512 // Note: q[0] is not referenced so we won't waste time calculating
513 // its cosine and sine here.
515 (Vec3P(0, std::cos(q[1]), std::cos(q[2])),
516 Vec3P(0, std::sin(q[1]), std::sin(q[2])));
517}
518
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;
529
530 return Mat33P( c2oc1 , -s2oc1 , 0,
531 s2 , c2 , 0,
532 -s1*c2oc1 , s1*s2oc1, 1 );
533}
534
546 // Note: q[2] is not referenced so we won't waste time calculating
547 // its cosine and sine here.
549 (Vec3P(std::cos(q[0]), std::cos(q[1]), 0),
550 Vec3P(std::sin(q[0]), std::sin(q[1]), 0));
551}
552
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;
564
565 return Mat33P( 1 , s1*s0oc1 , -s1*c0oc1,
566 0 , c0 , s0,
567 0 , -s0oc1 , c0oc1 );
568}
569
580 (const Vec3P& q, const Vec3P& qdot) {
581 // Note: q[0] is not referenced so we won't waste time calculating
582 // its cosine and sine here.
584 (Vec3P(0, std::cos(q[1]), std::cos(q[2])),
585 Vec3P(0, std::sin(q[1]), std::sin(q[2])),
586 qdot);
587}
588
595 (const Vec3P& cq, const Vec3P& sq, const Vec3P& qdot)
596{
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;
601
602 const RealP t = qdot[1]*s1*ooc1;
603 const RealP a = t*s2oc1 + qdot[2]*c2oc1; // d/dt s2oc1
604 const RealP b = t*c2oc1 - qdot[2]*s2oc1; // d/dt c2oc1
605
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 );
609}
610
621 (const Vec3P& q, const Vec3P& qdot) {
622 // Note: q[2] is not referenced so we won't waste time calculating
623 // its cosine and sine here.
624 const RealP cy = std::cos(q[1]); // cos(y)
626 (Vec2P(std::cos(q[0]), cy),
627 Vec2P(std::sin(q[0]), std::sin(q[1])),
628 1/cy, qdot);
629}
630
635 (const Vec2P& cq, const Vec2P& sq, RealP ooc1, const Vec3P& qdot) {
636 const RealP s0 = sq[0], c0 = cq[0];
637 const RealP s1 = sq[1];
638 const RealP s0oc1 = s0*ooc1, c0oc1 = c0*ooc1;
639
640 const RealP t = qdot[1]*s1*ooc1;
641 const RealP a = t*s0oc1 + qdot[0]*c0oc1; // d/dt s0oc1
642 const RealP b = t*c0oc1 - qdot[0]*s0oc1; // d/dt c0oc1
643
644 return Mat33P( 0, s1*a + qdot[1]*s0, -(s1*b + qdot[1]*c0),
645 0, -qdot[0]*s0 , qdot[0]*c0 ,
646 0, -a , b );
647}
648
658 // Note: q[0] is not referenced so we won't waste time calculating
659 // its cosine and sine here.
661 (Vec3P(0, std::cos(q[1]), std::cos(q[2])),
662 Vec3P(0, std::sin(q[1]), std::sin(q[2])));
663}
664
670 (const Vec3P& cq, const Vec3P& sq) {
671 const RealP s1 = sq[1], c1 = cq[1];
672 const RealP s2 = sq[2], c2 = cq[2];
673
674 return Mat33P( c1*c2 , s2 , 0 ,
675 -c1*s2 , c2 , 0 ,
676 s1 , 0 , 1 );
677}
678
687 // Note: q[0] is not referenced so we won't waste time calculating
688 // its cosine and sine here.
690 (Vec3P(std::cos(q[0]), std::cos(q[1]), 0),
691 Vec3P(std::sin(q[0]), std::sin(q[1]), 0));
692}
693
699 (const Vec3P& cq, const Vec3P& sq) {
700 const RealP s0 = sq[0], c0 = cq[0];
701 const RealP s1 = sq[1], c1 = cq[1];
702
703 return Mat33P( 1 , 0 , s1 ,
704 0 , c0 , -s0*c1 ,
705 0 , s0 , c0*c1 );
706}
707
713 const Vec4P e = q/2;
714 const RealP ne1 = -e[1], ne2 = -e[2], ne3 = -e[3];
715 return Mat43P( ne1, ne2, ne3,
716 e[0], e[3], ne2,
717 ne3, e[0], e[1],
718 e[2], ne1, e[0]);
719}
720
727 const Vec4P ed = qdot/2;
728 const RealP ned1 = -ed[1], ned2 = -ed[2], ned3 = -ed[3];
729 return Mat43P( ned1, ned2, ned3,
730 ed[0], ed[3], ned2,
731 ned3, ed[0], ed[1],
732 ed[2], ned1, ed[0]);
733}
734
743 const Vec4P e = 2*q;
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]);
748}
750
751//------------------------------------------------------------------------------
757const Mat33P& asMat33() const { return *static_cast<const Mat33P*>(this); }
761Mat33P toMat33() const { return asMat33(); }
762
773
774// Converts rotation matrix to one or two or three orientation angles.
775// Note: The result is most meaningful if the Rotation_ matrix is one that can
776// be produced by such a sequence.
777// Use1: someRotation.convertOneAxisRotationToOneAngle( XAxis );
778// Use2: someRotation.convertTwoAxesRotationToTwoAngles
779// (SpaceRotationSequence, YAxis, ZAxis );
780// Use3: someRotation.convertThreeAxesRotationToThreeAngles
781// (SpaceRotationSequence, ZAxis, YAxis, XAxis );
782// Use4: someRotation.convertRotationToAngleAxis();
783// Return: [angleInRadians, unitVectorX, unitVectorY, unitVectorZ].
784
790
796 const CoordinateAxis& axis1,
797 const CoordinateAxis& axis2) const;
798
805 const CoordinateAxis& axis2, const CoordinateAxis& axis3 ) const;
806
818
838
846
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]);
854 const RealP ooc1 = RealP(1)/c1;
855 const RealP s2oc1 = s2*ooc1, c2oc1 = c2*ooc1;
856
857 const Mat33P E( 0, s2oc1 , c2oc1 ,
858 0, c2 , -s2 ,
859 1, s1*s2oc1 , s1*c2oc1 );
860 return E * w_PB_B;
861}
862
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]);
868
869 const Mat33P Einv( -s1 , 0 , 1 ,
870 c1*s2 , c2 , 0 ,
871 c1*c2 , -s2 , 0 );
872 return Einv*qd;
873}
874
875// TODO: sherm: is this right? Warning: everything is measured in the
876// *PARENT* frame, but angular velocities and accelerations are
877// expressed in the *BODY* frame.
878// TODO: this is not an efficient way to do this computation.
881 (const Vec3P& q, const Vec3P& w_PB_B, const Vec3P& wdot_PB_B)
882{
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;
887
888 const Mat33P E( 0 , s2oc1 , c2oc1 ,
889 0 , c2 , -s2 ,
890 1 , s1*s2oc1 , s1*c2oc1 );
891 const Vec3P qdot = E * w_PB_B;
892
893 const RealP t = qdot[1]*s1oc1;
894 const RealP a = t*s2oc1 + qdot[2]*c2oc1; // d/dt s2oc1
895 const RealP b = t*c2oc1 - qdot[2]*s2oc1; // d/dt c2oc1
896
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 );
900
901 return E*wdot_PB_B + Edot*w_PB_B;
902}
903
913 (const Vec3P& q, const Vec3P& 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])),
917 w_PB_B);
918}
919
925//TODO: reimplement
929
936 (const Vec3P& q, const Vec3P& qdot) {
938 (Vec3P(0, std::cos(q[1]), std::cos(q[2])),
939 Vec3P(0, std::sin(q[1]), std::sin(q[2])),
940 qdot);
941}
942
948// TODO: reimplement
952
953// TODO: sherm: is this right?
960 (const Vec3P& q, const Vec3P& w_PB_B, const Vec3P& wdot_PB_B)
961{
962 // Note: q[0] is not referenced so we won't waste time calculating
963 // its cosine and sine here.
965 (Vec3P(0, std::cos(q[1]), std::cos(q[2])),
966 Vec3P(0, std::sin(q[1]), std::sin(q[2])),
968}
969
975// TODO: reimplement
977 (const Vec3P& cq, const Vec3P& sq,
978 const Vec3P& w_PB_B, const Vec3P& wdot_PB_B)
979{
980 const Mat33P N = calcNForBodyXYZInBodyFrame(cq,sq); // ~12 flops
981 const Vec3P qdot = N * w_PB_B; // 15 flops
982 const Mat33P NDot = calcNDotForBodyXYZInBodyFrame(cq,sq,qdot); // ~30 flops
983
984 return N*wdot_PB_B + NDot*w_PB_B; // 33 flops
985}
986
993
999
1007 (const Vec4P& q, const Vec3P& w_PB, const Vec3P& b_PB)
1008{
1009 const Mat43P N = calcUnnormalizedNForQuaternion(q); // 7 flops
1010 const Vec4P Nb = N*b_PB; // 20 flops
1011 const Vec4P NDotw = RealP(-.25)*w_PB.normSqr()*q; // 10 flops
1012 return Nb + NDotw; // 4 flops
1013}
1014
1022 (const Vec2P& cosxy,
1023 const Vec2P& sinxy,
1024 RealP oocosy,
1025 const Vec3P& w_PB)
1026{
1028}
1029
1042 (const Vec2P& cosxy,
1043 const Vec2P& sinxy,
1044 RealP oocosy,
1045 const Vec3P& qdot,
1046 const Vec3P& b_PB)
1047{
1048 const RealP s1 = sinxy[1], c1 = cosxy[1];
1049 const RealP q0 = qdot[0], q1 = qdot[1], q2 = qdot[2];
1050
1051 // 10 flops
1053
1054 const RealP q1oc1 = q1*oocosy;
1055 const Vec3P NDotw((q0*s1-q2)*q1oc1, // NDot_P*w_PB
1056 q0*q2*c1, // = NDot_P*(NInv_P*qdot)
1057 (q2*s1-q0)*q1oc1 ); // (9 flops)
1058
1059 return Nb + NDotw; // 3 flops
1060}
1062
1063//------------------------------------------------------------------------------
1070 const;
1071
1076
1080 const Mat33P& A=asMat33(); const Mat33P& B=R.asMat33(); RealP maxDiff=0;
1081 for( int i=0; i<=2; i++ ) for( int j=0; j<=2; j++ ) {
1082 const RealP absDiff = std::abs(A[i][j] - B[i][j]);
1083 if( absDiff > maxDiff ) maxDiff = absDiff;
1084 }
1085 return maxDiff;
1086}
1087
1092
1098
1099
1100private:
1101// This is only for the most trustworthy of callers, that is, methods of
1102// the Rotation_ class. There are a lot of ways for this NOT to be a
1103// legitimate rotation matrix -- be careful!!
1104// Note that these are supplied in rows.
1105Rotation_( const RealP& xx, const RealP& xy, const RealP& xz,
1106 const RealP& yx, const RealP& yy, const RealP& yz,
1107 const RealP& zx, const RealP& zy, const RealP& zz )
1108: Mat33P( xx,xy,xz, yx,yy,yz, zx,zy,zz ) {}
1109
1110// These next methods are highly-efficient power-user methods. Read the
1111// code to understand them.
1113setTwoAngleTwoAxesBodyFixedForwardCyclicalRotation
1114 (RealP cosAngle1, RealP sinAngle1, const CoordinateAxis& axis1,
1115 RealP cosAngle2, RealP sinAngle2, const CoordinateAxis& axis2 );
1117setThreeAngleTwoAxesBodyFixedForwardCyclicalRotation
1118 (RealP cosAngle1, RealP sinAngle1, const CoordinateAxis& axis1,
1119 RealP cosAngle2, RealP sinAngle2, const CoordinateAxis& axis2,
1122setThreeAngleThreeAxesBodyFixedForwardCyclicalRotation
1123 (RealP cosAngle1, RealP sinAngle1, const CoordinateAxis& axis1,
1124 RealP cosAngle2, RealP sinAngle2, const CoordinateAxis& axis2,
1125 RealP cosAngle3, RealP sinAngle3, const CoordinateAxis& axis3 );
1126
1127// These next methods are highly-efficient power-user methods to convert
1128// Rotation matrices to orientation angles. Read the code to understand them.
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;
1139
1140//------------------------------------------------------------------------------
1141// These are obsolete names from a previous release, listed here so that
1142// users will get a decipherable compilation error. (sherm 091101)
1143//------------------------------------------------------------------------------
1144private:
1145// REPLACED BY: calcNForBodyXYZInBodyFrame()
1146static Mat33P calcQBlockForBodyXYZInBodyFrame(const Vec3P& a)
1147{ return calcNForBodyXYZInBodyFrame(a); }
1148// REPLACED BY: calcNInvForBodyXYZInBodyFrame()
1149static Mat33P calcQInvBlockForBodyXYZInBodyFrame(const Vec3P& a)
1150{ return calcNInvForBodyXYZInBodyFrame(a); }
1151// REPLACED BY: calcUnnormalizedNForQuaternion()
1152static Mat<4,3,P> calcUnnormalizedQBlockForQuaternion(const Vec4P& q)
1154// REPLACED BY: calcUnnormalizedNInvForQuaternion()
1155static Mat<3,4,P> calcUnnormalizedQInvBlockForQuaternion(const Vec4P& q)
1157// REPLACED BY: convertAngVelInBodyFrameToBodyXYZDot
1158static Vec3P convertAngVelToBodyFixed123Dot(const Vec3P& q, const Vec3P& w_PB_B)
1160// REPLACED BY: convertBodyXYZDotToAngVelInBodyFrame
1161static Vec3P convertBodyFixed123DotToAngVel(const Vec3P& q, const Vec3P& qdot)
1163// REPLACED BY: convertAngVelDotInBodyFrameToBodyXYZDotDot
1164static Vec3P convertAngVelDotToBodyFixed123DotDot
1165 (const Vec3P& q, const Vec3P& w_PB_B, const Vec3P& wdot_PB_B)
1167
1168//------------------------------------------------------------------------------
1169// The following code is obsolete - it is here temporarily for backward
1170// compatibility (Mitiguy 9/5/2007)
1171//------------------------------------------------------------------------------
1172private:
1173// These static methods are like constructors with friendlier names.
1174static Rotation_ zero() { return Rotation_(); }
1175static Rotation_ NaN() { Rotation_ r; r.setRotationToNaN(); return r; }
1176
1178Rotation_& setToZero() { return setRotationToIdentityMatrix(); }
1179Rotation_& setToIdentityMatrix() { return setRotationToIdentityMatrix(); }
1180Rotation_& setToNaN() { return setRotationToNaN(); }
1181static Rotation_ trustMe( const Mat33P& m ) { return Rotation_(m,true); }
1182
1183// One-angle rotations.
1184static Rotation_ aboutX( const RealP& angleInRad )
1185{ return Rotation_( angleInRad, XAxis ); }
1186static Rotation_ aboutY( const RealP& angleInRad )
1187{ return Rotation_( angleInRad, YAxis ); }
1188static Rotation_ aboutZ( const RealP& angleInRad )
1189{ return Rotation_( angleInRad, ZAxis ); }
1190static Rotation_ aboutAxis( const RealP& angleInRad, const UnitVec3P& axis )
1191{ return Rotation_(angleInRad,axis); }
1192static Rotation_ aboutAxis( const RealP& angleInRad, const Vec3P& axis )
1193{ return Rotation_(angleInRad,axis); }
1194void setToRotationAboutZ( const RealP& q ) { setRotationFromAngleAboutZ( q ); }
1195
1196// Two-angle space-fixed rotations.
1197static Rotation_ aboutXThenOldY(const RealP& xInRad, const RealP& yInRad)
1199static Rotation_ aboutYThenOldX(const RealP& yInRad, const RealP& xInRad)
1201static Rotation_ aboutXThenOldZ(const RealP& xInRad, const RealP& zInRad)
1203static Rotation_ aboutZThenOldX(const RealP& zInRad, const RealP& xInRad)
1205static Rotation_ aboutYThenOldZ(const RealP& yInRad, const RealP& zInRad)
1207static Rotation_ aboutZThenOldY(const RealP& zInRad, const RealP& yInRad)
1209
1210// Two-angle body fixed rotations (reversed space-fixed ones).
1211static Rotation_ aboutXThenNewY(const RealP& xInRad, const RealP& yInRad)
1213static Rotation_ aboutYThenNewX(const RealP& yInRad, const RealP& xInRad)
1214{ return aboutXThenOldY(xInRad, yInRad); }
1215static Rotation_ aboutXThenNewZ(const RealP& xInRad, const RealP& zInRad)
1216{ return aboutZThenOldX(zInRad, xInRad); }
1217static Rotation_ aboutZThenNewX(const RealP& zInRad, const RealP& xInRad)
1218{ return aboutXThenOldZ(xInRad, zInRad); }
1219static Rotation_ aboutYThenNewZ(const RealP& yInRad, const RealP& zInRad)
1220{ return aboutZThenOldY(zInRad, yInRad); }
1221static Rotation_ aboutZThenNewY(const RealP& zInRad, const RealP& yInRad)
1222{ return aboutYThenOldZ(yInRad, zInRad); }
1223
1224// Create a Rotation_ matrix by specifying only its z axis.
1225// This will work for any stride UnitVec because there is always an implicit
1226// conversion available to the packed form used as the argument.
1227explicit Rotation_( const UnitVec3P& uvecZ )
1229
1230// Create a Rotation_ matrix by specifying its x axis, and a "y like" axis.
1231// We will take x seriously after normalizing, but use the y only to create
1232// z = normalize(x X y), then y = z X x. Bad things happen if x and y are
1233// aligned but we may not catch it.
1234Rotation_( const Vec3P& x, const Vec3P& yish )
1236
1237// Set this Rotation_ to represent the same rotation as the passed-in quaternion.
1238void setToQuaternion( const QuaternionP& q ) { setRotationFromQuaternion(q); }
1239
1240// Set this Rotation_ to represent a rotation of +q0 about body frame's Z axis,
1241// followed by a rotation of +q1 about the body frame's NEW Y axis,
1242// followed by a rotation of +q3 about the body frame's NEW X axis.
1243// See Kane, Spacecraft Dynamics, pg. 423, body-three: 3-2-1.
1244// Similarly for BodyFixed123.
1245void setToBodyFixed321( const Vec3P& v)
1247 v[0], ZAxis, v[1], YAxis, v[2], XAxis ); }
1248void setToBodyFixed123( const Vec3P& v)
1250
1251// Convert this Rotation_ matrix to an equivalent (angle,axis) representation:
1252// Returned Vec4P is [angleInRadians, unitVectorX, unitVectorY, unitVectorZ].
1253Vec4P convertToAngleAxis() const
1254{ return convertRotationToAngleAxis(); }
1255
1256// Convert this Rotation_ matrix to equivalent quaternion representation.
1257QuaternionP convertToQuaternion() const
1258{ return convertRotationToQuaternion(); }
1259
1260// Set this Rotation_ to represent a rotation of +q0 about base frame's X axis,
1261// followed by a rotation of +q1 about the base frame's (unchanged) Y axis.
1262void setToSpaceFixed12( const Vec2P& q )
1264
1265// Convert this Rotation_ matrix to the equivalent 1-2-3 body fixed Euler angle
1266// sequence. Similarly, convert Rotation_ matrix to the equivalent 1-2 body
1267// fixed Euler angle sequence. Similarly, convert Rotation_ matrix to the
1268// equivalent 1-2 space fixed Euler angle sequence.
1269Vec3P convertToBodyFixed123() const
1271Vec2P convertToBodyFixed12() const
1272{ return convertRotationToBodyFixedXY(); }
1273Vec2P convertToSpaceFixed12() const
1275};
1276
1277
1278//-----------------------------------------------------------------------------
1281//-----------------------------------------------------------------------------
1282template <class P>
1283class InverseRotation_ : public Mat<3,3,P>::TransposeType {
1284typedef Mat<3,3,P> Mat33P; // not the base type!
1285typedef Mat<2,2,P> Mat22P;
1286typedef Mat<3,2,P> Mat32P;
1287typedef Vec<2,P> Vec2P;
1288typedef Vec<3,P> Vec3P;
1289typedef Vec<4,P> Vec4P;
1291public:
1295
1304
1309
1314{ BaseMat::operator=(R.asMat33()); return *this; }
1315
1324
1328const Rotation_<P>& invert() const
1329{return *reinterpret_cast<const Rotation_<P>*>(this);}
1330Rotation_<P>& updInvert() {return *reinterpret_cast<Rotation_<P>*>(this);}
1332
1336const Rotation_<P>& transpose() const { return invert(); }
1337const Rotation_<P>& operator~() const { return invert(); }
1341
1348const RowType& row( int i ) const
1349{ return reinterpret_cast<const RowType&>(asMat33()[i]); }
1350const RowType& operator[]( int i ) const { return row(i); }
1351const ColType& col( int j ) const
1352{ return reinterpret_cast<const ColType&>(asMat33()(j)); }
1353const ColType& operator()( int j ) const { return col(j); }
1354const ColType& x() const { return col(0); }
1355const ColType& y() const { return col(1); }
1356const ColType& z() const { return col(2); }
1358
1359
1365{ return col(axis); }
1366
1372 const ColType& axDir = getAxisUnitVec(dir.getAxis());
1373 return dir.getDirection() > 0 ? UnitVec<P,1>( axDir)
1374 : UnitVec<P,1>(-axDir); // cheap
1375}
1376
1381const BaseMat& asMat33() const { return *static_cast<const BaseMat*>(this); }
1382BaseMat toMat33() const { return asMat33(); }
1384};
1385
1388template <class P> SimTK_SimTKCOMMON_EXPORT std::ostream&
1389operator<<(std::ostream&, const Rotation_<P>&);
1392template <class P> SimTK_SimTKCOMMON_EXPORT std::ostream&
1393operator<<(std::ostream&, const InverseRotation_<P>&);
1394
1399template <class P, int S> inline UnitVec<P,1>
1401{return UnitVec<P,1>(R.asMat33()* v.asVec3(), true);}
1402template <class P, int S> inline UnitRow<P,1>
1404{return UnitRow<P,1>(r.asRow3() * R.asMat33(), true);}
1405template <class P, int S> inline UnitVec<P,1>
1407{return UnitVec<P,1>(R.asMat33()* v.asVec3(), true);}
1408template <class P, int S> inline UnitRow<P,1>
1410{return UnitRow<P,1>(r.asRow3() * R.asMat33(), true);}
1412
1413// Couldn't implement these Rotation_ methods until InverseRotation_ was defined.
1414template <class P> inline
1416: Mat<3,3,P>( R.asMat33() ) {}
1417
1418template <class P> inline Rotation_<P>&
1420{static_cast<Mat<3,3,P>&>(*this) = R.asMat33(); return *this;}
1421template <class P> inline Rotation_<P>&
1423{static_cast<Mat<3,3,P>&>(*this) *= R.asMat33(); return *this;}
1424template <class P> inline Rotation_<P>&
1426{static_cast<Mat<3,3,P>&>(*this) *= (~R).asMat33(); return *this;}
1427template <class P> inline Rotation_<P>&
1429{static_cast<Mat<3,3,P>&>(*this) *= R.asMat33(); return *this;}
1430template <class P> inline Rotation_<P>&
1432{static_cast<Mat<3,3,P>&>(*this) *= (~R).asMat33(); return *this;}
1433
1435
1436template <class P> inline Rotation_<P>
1438{return Rotation_<P>(R1) *= R2;}
1439template <class P> inline Rotation_<P>
1441{return Rotation_<P>(R1) *= R2;}
1442template <class P> inline Rotation_<P>
1444{return Rotation_<P>(R1) *= R2;}
1445template <class P> inline Rotation_<P>
1447{return Rotation_<P>(R1) *= R2;}
1449
1452
1453template <class P> inline Rotation_<P>
1454operator/( const Rotation_<P>& R1, const Rotation_<P>& R2 )
1455{return Rotation_<P>(R1) /= R2;}
1456template <class P> inline Rotation_<P>
1458{return Rotation_<P>(R1) /= R2;}
1459template <class P> inline Rotation_<P>
1461{return Rotation_<P>(R1) /= R2;}
1462template <class P> inline Rotation_<P>
1464{return Rotation_<P>(R1) /= R2;}
1466
1467
1468//------------------------------------------------------------------------------
1469} // End of namespace SimTK
1470
1471#endif // SimTK_SimTKCOMMON_ROTATION_H_
1472
1473
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
P E
Definition Mat.h:99
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
Definition NTraits.h:436
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 ...