Simbody  3.5
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 //------------------------------------------------------------------------------
39 namespace SimTK {
40 
41 
43 
44 //------------------------------------------------------------------------------
45 // Forward declarations
46 template <class P> class Rotation_;
47 template <class P> class InverseRotation_;
48 
52 
56 
57 //------------------------------------------------------------------------------
109 //------------------------------------------------------------------------------
110 template <class P> // templatized by precision
111 class Rotation_ : public Mat<3,3,P> {
112 public:
113 typedef P RealP;
119 typedef Vec<2,P> Vec2P;
120 typedef Vec<3,P> Vec3P;
121 typedef Vec<4,P> Vec4P;
122 typedef UnitVec<P,1> UnitVec3P; // stride is 1 here, length is always 3
125 
132 
133 //------------------------------------------------------------------------------
137 Rotation_() : Mat33P(1) {}
138 
140 Rotation_( const Rotation_& R ) : Mat33P(R) {}
143 inline Rotation_( const InverseRotation_<P>& );
144 
147 { Mat33P::operator=( R.asMat33() ); return *this; }
149 inline Rotation_& operator=( const InverseRotation_<P>& );
150 
153 { Mat33P::setToNaN(); return *this; }
154 
157 { Mat33P::operator=(RealP(1)); return *this; }
158 
161 Rotation_( RealP angle, const CoordinateAxis& axis )
162 { setRotationFromAngleAboutAxis( angle, axis ); }
166 { return axis.isXAxis() ? setRotationFromAngleAboutX(angle)
167  : (axis.isYAxis() ? setRotationFromAngleAboutY(angle)
168  : setRotationFromAngleAboutZ(angle) ); }
169 
173 { setRotationFromAngleAboutX( std::cos(angle), std::sin(angle) ); }
177 { return setRotationFromAngleAboutX( std::cos(angle), std::sin(angle) ); }
180 Rotation_& setRotationFromAngleAboutX( RealP cosAngle, RealP sinAngle )
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 
188 { setRotationFromAngleAboutY( std::cos(angle), std::sin(angle) ); }
192 { return setRotationFromAngleAboutY( std::cos(angle), std::sin(angle) ); }
195 Rotation_& setRotationFromAngleAboutY( RealP cosAngle, RealP 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);
198  return *this; }
199 
203 { setRotationFromAngleAboutZ( std::cos(angle), std::sin(angle) ); }
207 { return setRotationFromAngleAboutZ( std::cos(angle), std::sin(angle) ); }
210 Rotation_& setRotationFromAngleAboutZ( RealP cosAngle, RealP 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);
213  return *this; }
214 
217 Rotation_( RealP angle, const UnitVec3P& unitVector )
218 { setRotationFromAngleAboutUnitVector(angle,unitVector); }
222 setRotationFromAngleAboutUnitVector(RealP angle, const UnitVec3P& unitVector);
223 
226 Rotation_( RealP angle, const Vec3P& nonUnitVector )
227 { setRotationFromAngleAboutNonUnitVector(angle,nonUnitVector); }
230 Rotation_&
231 setRotationFromAngleAboutNonUnitVector(RealP angle, const Vec3P& nonUnitVector)
232 { return setRotationFromAngleAboutUnitVector( angle, UnitVec3P(nonUnitVector) ); }
233 
237  RealP angle1, const CoordinateAxis& axis1,
238  RealP angle2, const CoordinateAxis& axis2)
239 { setRotationFromTwoAnglesTwoAxes(bodyOrSpace,angle1,axis1,angle2,axis2); }
244  RealP angle1, const CoordinateAxis& axis1,
245  RealP angle2, const CoordinateAxis& axis2);
246 
250  RealP angle1, const CoordinateAxis& axis1,
251  RealP angle2, const CoordinateAxis& axis2,
252  RealP angle3, const CoordinateAxis& axis3 )
254  (bodyOrSpace,angle1,axis1,angle2,axis2,angle3,axis3); }
259  RealP angle1, const CoordinateAxis& axis1,
260  RealP angle2, const CoordinateAxis& axis2,
261  RealP angle3, const CoordinateAxis& axis3);
262 
264 explicit Rotation_( const QuaternionP& q ) { setRotationFromQuaternion(q); }
267 setRotationFromQuaternion( const QuaternionP& q );
268 
271 explicit Rotation_( const Mat33P& m ) { setRotationFromApproximateMat33(m); }
275 setRotationFromApproximateMat33( const Mat33P& m );
276 
279 Rotation_(const Mat33P& m, bool) : Mat33P(m) {}
283 { Mat33P& R = *this; R=m; return *this; }
286 Rotation_& setRotationColFromUnitVecTrustMe(int colj, const UnitVec3P& uvecj)
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 
297 Rotation_(const UnitVec3P& uvec, CoordinateAxis axis)
298 { setRotationFromOneAxis(uvec,axis); }
302 setRotationFromOneAxis(const UnitVec3P& uvec, CoordinateAxis axis);
303 
310 Rotation_(const UnitVec3P& uveci, const CoordinateAxis& axisi,
311  const Vec3P& vecjApprox, const CoordinateAxis& axisjApprox )
312 { setRotationFromTwoAxes(uveci,axisi,vecjApprox,axisjApprox); }
320 setRotationFromTwoAxes(const UnitVec3P& uveci, const CoordinateAxis& axisi,
321  const Vec3P& vecjApprox, const CoordinateAxis& axisjApprox );
322 
327 void setRotationToBodyFixedXY( const Vec2P& v)
329  v[0],XAxis, v[1],YAxis); }
330 
336 void setRotationToBodyFixedXYZ( const Vec3P& v)
338  v[0],XAxis, v[1],YAxis, v[2],ZAxis ); }
342 void setRotationToBodyFixedXYZ(const Vec3P& c, const Vec3P& s) {
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 //------------------------------------------------------------------------------
357 const InverseRotation_<P>& operator~() const { return invert(); }
361 
365 const InverseRotation_<P>& transpose() const { return invert(); }
370 
374 { return *reinterpret_cast<const InverseRotation_<P>*>(this); }
377 { return *reinterpret_cast<InverseRotation_<P>*>(this); }
378 
380 inline Rotation_& operator*=( const Rotation_<P>& R );
382 inline Rotation_& operator*=( const InverseRotation_<P>& );
383 
385 inline Rotation_& operator/=( const Rotation_<P>& R );
387 inline Rotation_& operator/=( const InverseRotation_<P>& );
388 
395 static Vec3P multiplyByBodyXYZ_N_P(const Vec2P& cosxy,
396  const Vec2P& sinxy,
397  RealP oocosy,
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 
411 static Vec3P multiplyByBodyXYZ_NT_P(const Vec2P& cosxy,
412  const Vec2P& sinxy,
413  RealP oocosy,
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 
426 static Vec3P multiplyByBodyXYZ_NInv_P(const Vec2P& cosxy,
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 
442 static Vec3P multiplyByBodyXYZ_NInvT_P(const Vec2P& cosxy,
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 //------------------------------------------------------------------------------
461 const RowType& row( int i ) const
462 { return reinterpret_cast<const RowType&>(asMat33()[i]); }
464 const RowType& operator[]( int i ) const { return row(i); }
465 
468 const ColType& col( int j ) const
469 { return reinterpret_cast<const ColType&>(asMat33()(j)); }
471 const ColType& operator()( int j ) const { return col(j); }
472 
474 const ColType& x() const { return col(0); }
476 const ColType& y() const { return col(1); }
478 const ColType& z() const { return col(2); }
479 
484 const ColType& getAxisUnitVec(CoordinateAxis axis) const
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 //------------------------------------------------------------------------------
511 static Mat33P calcNForBodyXYZInBodyFrame(const Vec3P& q) {
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 
524 static Mat33P calcNForBodyXYZInBodyFrame(const Vec3P& cq, const Vec3P& sq) {
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 
545 static Mat33P calcNForBodyXYZInParentFrame(const Vec3P& q) {
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 
559 static Mat33P calcNForBodyXYZInParentFrame(const Vec3P& cq, const Vec3P& sq) {
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 
579 static Mat33P calcNDotForBodyXYZInBodyFrame
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 
594 static Mat33P calcNDotForBodyXYZInBodyFrame
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 
657 static Mat33P calcNInvForBodyXYZInBodyFrame(const Vec3P& q) {
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 
669 static Mat33P calcNInvForBodyXYZInBodyFrame
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 
686 static Mat33P calcNInvForBodyXYZInParentFrame(const Vec3P& q) {
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 
712 static Mat43P calcUnnormalizedNForQuaternion(const Vec4P& q) {
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 
726 static Mat43P calcUnnormalizedNDotForQuaternion(const Vec4P& qdot) {
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 
742 static Mat34P calcUnnormalizedNInvForQuaternion(const Vec4P& q) {
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 //------------------------------------------------------------------------------
757 const Mat33P& asMat33() const { return *static_cast<const Mat33P*>(this); }
761 Mat33P toMat33() const { return asMat33(); }
762 
771 SimTK_SimTKCOMMON_EXPORT SymMat33P
772 reexpressSymMat33(const SymMat33P& S_BB) const;
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 
804  (BodyOrSpaceType bodyOrSpace, const CoordinateAxis& axis1,
805  const CoordinateAxis& axis2, const CoordinateAxis& axis3 ) const;
806 
818 
838 
845  XAxis, YAxis, ZAxis ); }
846 
851 static Vec3P convertAngVelToBodyFixed321Dot(const Vec3P& q, const Vec3P& w_PB_B) {
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 
865 static Vec3P convertBodyFixed321DotToAngVel(const Vec3P& q, const Vec3P& qd) {
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
927  (const Vec3P& cq, const Vec3P& sq, const Vec3P& w_PB_B)
928 { return calcNForBodyXYZInBodyFrame(cq,sq)*w_PB_B; }
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
950  (const Vec3P& cq, const Vec3P& sq, const Vec3P& qdot)
951 { return calcNInvForBodyXYZInBodyFrame(cq,sq)*qdot; }
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])),
967  w_PB_B, wdot_PB_B);
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 
990 static Vec4P convertAngVelToQuaternionDot(const Vec4P& q, const Vec3P& w_PB_P) {
991  return calcUnnormalizedNForQuaternion(q)*w_PB_P;
992 }
993 
996 static Vec3P convertQuaternionDotToAngVel(const Vec4P& q, const Vec4P& qdot) {
997  return calcUnnormalizedNInvForQuaternion(q)*qdot;
998 }
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 {
1027  return multiplyByBodyXYZ_N_P(cosxy,sinxy,oocosy,w_PB);
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
1052  const Vec3P Nb = multiplyByBodyXYZ_N_P(cosxy,sinxy,oocosy,b_PB);
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 //------------------------------------------------------------------------------
1069 isSameRotationToWithinAngle(const Rotation_& R, RealP okPointingAngleErrorRads)
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 
1090 bool areAllRotationElementsSameToEpsilon(const Rotation_& R, RealP epsilon) const
1091 { return getMaxAbsDifferenceInRotationElements(R) <= epsilon; }
1092 
1098 
1099 
1100 private:
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.
1105 Rotation_( 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.
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,
1120  RealP cosAngle3, RealP sinAngle3 );
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 );
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.
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;
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 //------------------------------------------------------------------------------
1144 private:
1145 // REPLACED BY: calcNForBodyXYZInBodyFrame()
1146 static Mat33P calcQBlockForBodyXYZInBodyFrame(const Vec3P& a)
1147 { return calcNForBodyXYZInBodyFrame(a); }
1148 // REPLACED BY: calcNInvForBodyXYZInBodyFrame()
1149 static Mat33P calcQInvBlockForBodyXYZInBodyFrame(const Vec3P& a)
1150 { return calcNInvForBodyXYZInBodyFrame(a); }
1151 // REPLACED BY: calcUnnormalizedNForQuaternion()
1152 static Mat<4,3,P> calcUnnormalizedQBlockForQuaternion(const Vec4P& q)
1153 { return calcUnnormalizedNForQuaternion(q); }
1154 // REPLACED BY: calcUnnormalizedNInvForQuaternion()
1155 static Mat<3,4,P> calcUnnormalizedQInvBlockForQuaternion(const Vec4P& q)
1156 { return calcUnnormalizedNInvForQuaternion(q); }
1157 // REPLACED BY: convertAngVelInBodyFrameToBodyXYZDot
1158 static Vec3P convertAngVelToBodyFixed123Dot(const Vec3P& q, const Vec3P& w_PB_B)
1159 { return convertAngVelInBodyFrameToBodyXYZDot(q,w_PB_B); }
1160 // REPLACED BY: convertBodyXYZDotToAngVelInBodyFrame
1161 static Vec3P convertBodyFixed123DotToAngVel(const Vec3P& q, const Vec3P& qdot)
1162 { return convertBodyXYZDotToAngVelInBodyFrame(q,qdot); }
1163 // REPLACED BY: convertAngVelDotInBodyFrameToBodyXYZDotDot
1164 static Vec3P convertAngVelDotToBodyFixed123DotDot
1165  (const Vec3P& q, const Vec3P& w_PB_B, const Vec3P& wdot_PB_B)
1166 { return convertAngVelDotInBodyFrameToBodyXYZDotDot(q,w_PB_B,wdot_PB_B); }
1167 
1168 //------------------------------------------------------------------------------
1169 // The following code is obsolete - it is here temporarily for backward
1170 // compatibility (Mitiguy 9/5/2007)
1171 //------------------------------------------------------------------------------
1172 private:
1173 // These static methods are like constructors with friendlier names.
1174 static Rotation_ zero() { return Rotation_(); }
1175 static Rotation_ NaN() { Rotation_ r; r.setRotationToNaN(); return r; }
1176 
1178 Rotation_& setToZero() { return setRotationToIdentityMatrix(); }
1179 Rotation_& setToIdentityMatrix() { return setRotationToIdentityMatrix(); }
1180 Rotation_& setToNaN() { return setRotationToNaN(); }
1181 static Rotation_ trustMe( const Mat33P& m ) { return Rotation_(m,true); }
1182 
1183 // One-angle rotations.
1184 static Rotation_ aboutX( const RealP& angleInRad )
1185 { return Rotation_( angleInRad, XAxis ); }
1186 static Rotation_ aboutY( const RealP& angleInRad )
1187 { return Rotation_( angleInRad, YAxis ); }
1188 static Rotation_ aboutZ( const RealP& angleInRad )
1189 { return Rotation_( angleInRad, ZAxis ); }
1190 static Rotation_ aboutAxis( const RealP& angleInRad, const UnitVec3P& axis )
1191 { return Rotation_(angleInRad,axis); }
1192 static Rotation_ aboutAxis( const RealP& angleInRad, const Vec3P& axis )
1193 { return Rotation_(angleInRad,axis); }
1194 void setToRotationAboutZ( const RealP& q ) { setRotationFromAngleAboutZ( q ); }
1195 
1196 // Two-angle space-fixed rotations.
1197 static Rotation_ aboutXThenOldY(const RealP& xInRad, const RealP& yInRad)
1198 { return Rotation_( SpaceRotationSequence, xInRad, XAxis, yInRad, YAxis ); }
1199 static Rotation_ aboutYThenOldX(const RealP& yInRad, const RealP& xInRad)
1200 { return Rotation_( SpaceRotationSequence, yInRad, YAxis, xInRad, XAxis ); }
1201 static Rotation_ aboutXThenOldZ(const RealP& xInRad, const RealP& zInRad)
1202 { return Rotation_( SpaceRotationSequence, xInRad, XAxis, zInRad, ZAxis ); }
1203 static Rotation_ aboutZThenOldX(const RealP& zInRad, const RealP& xInRad)
1204 { return Rotation_( SpaceRotationSequence, zInRad, ZAxis, xInRad, XAxis ); }
1205 static Rotation_ aboutYThenOldZ(const RealP& yInRad, const RealP& zInRad)
1206 { return Rotation_( SpaceRotationSequence, yInRad, YAxis, zInRad, ZAxis ); }
1207 static Rotation_ aboutZThenOldY(const RealP& zInRad, const RealP& yInRad)
1208 { return Rotation_( SpaceRotationSequence, zInRad, ZAxis, yInRad, YAxis ); }
1209 
1210 // Two-angle body fixed rotations (reversed space-fixed ones).
1211 static Rotation_ aboutXThenNewY(const RealP& xInRad, const RealP& yInRad)
1212 { return Rotation_( BodyRotationSequence, xInRad, XAxis, yInRad, YAxis ); }
1213 static Rotation_ aboutYThenNewX(const RealP& yInRad, const RealP& xInRad)
1214 { return aboutXThenOldY(xInRad, yInRad); }
1215 static Rotation_ aboutXThenNewZ(const RealP& xInRad, const RealP& zInRad)
1216 { return aboutZThenOldX(zInRad, xInRad); }
1217 static Rotation_ aboutZThenNewX(const RealP& zInRad, const RealP& xInRad)
1218 { return aboutXThenOldZ(xInRad, zInRad); }
1219 static Rotation_ aboutYThenNewZ(const RealP& yInRad, const RealP& zInRad)
1220 { return aboutZThenOldY(zInRad, yInRad); }
1221 static 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.
1227 explicit Rotation_( const UnitVec3P& uvecZ )
1228 { setRotationFromOneAxis(uvecZ,ZAxis); }
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.
1234 Rotation_( const Vec3P& x, const Vec3P& yish )
1235 { setRotationFromTwoAxes( UnitVec3P(x), XAxis, yish, YAxis ); }
1236 
1237 // Set this Rotation_ to represent the same rotation as the passed-in quaternion.
1238 void 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.
1245 void setToBodyFixed321( const Vec3P& v)
1247  v[0], ZAxis, v[1], YAxis, v[2], XAxis ); }
1248 void setToBodyFixed123( const Vec3P& v)
1250 
1251 // Convert this Rotation_ matrix to an equivalent (angle,axis) representation:
1252 // Returned Vec4P is [angleInRadians, unitVectorX, unitVectorY, unitVectorZ].
1253 Vec4P convertToAngleAxis() const
1254 { return convertRotationToAngleAxis(); }
1255 
1256 // Convert this Rotation_ matrix to equivalent quaternion representation.
1257 QuaternionP 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.
1262 void 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.
1269 Vec3P convertToBodyFixed123() const
1270 { return convertRotationToBodyFixedXYZ(); }
1271 Vec2P convertToBodyFixed12() const
1272 { return convertRotationToBodyFixedXY(); }
1273 Vec2P convertToSpaceFixed12() const
1275 };
1276 
1277 
1278 //-----------------------------------------------------------------------------
1281 //-----------------------------------------------------------------------------
1282 template <class P>
1283 class InverseRotation_ : public Mat<3,3,P>::TransposeType {
1284 typedef P RealP;
1285 typedef Rotation_<P> RotationP;
1286 typedef Mat<3,3,P> Mat33P; // not the base type!
1287 typedef SymMat<3,P> SymMat33P;
1288 typedef Mat<2,2,P> Mat22P;
1289 typedef Mat<3,2,P> Mat32P;
1290 typedef Vec<2,P> Vec2P;
1291 typedef Vec<3,P> Vec3P;
1292 typedef Vec<4,P> Vec4P;
1293 typedef Quaternion_<P> QuaternionP;
1294 public:
1298 
1307 
1311 InverseRotation_() : BaseMat(1) {}
1312 
1314 InverseRotation_( const InverseRotation_& R ) : BaseMat(R) {}
1317 { BaseMat::operator=(R.asMat33()); return *this; }
1318 
1325 SimTK_SimTKCOMMON_EXPORT SymMat33P
1326 reexpressSymMat33(const SymMat33P& S_BB) const;
1327 
1331 const RotationP& invert() const
1332 {return *reinterpret_cast<const RotationP*>(this);}
1333 RotationP& updInvert() {return *reinterpret_cast<RotationP*>(this);}
1335 
1339 const RotationP& transpose() const { return invert(); }
1340 const RotationP& operator~() const { return invert(); }
1341 RotationP& updTranspose() { return updInvert(); }
1342 RotationP& operator~() { return updInvert(); }
1344 
1351 const RowType& row( int i ) const
1352 { return reinterpret_cast<const RowType&>(asMat33()[i]); }
1353 const RowType& operator[]( int i ) const { return row(i); }
1354 const ColType& col( int j ) const
1355 { return reinterpret_cast<const ColType&>(asMat33()(j)); }
1356 const ColType& operator()( int j ) const { return col(j); }
1357 const ColType& x() const { return col(0); }
1358 const ColType& y() const { return col(1); }
1359 const ColType& z() const { return col(2); }
1361 
1362 
1367 const ColType& getAxisUnitVec(CoordinateAxis axis) const
1368 { return col(axis); }
1369 
1375  const ColType& axDir = getAxisUnitVec(dir.getAxis());
1376  return dir.getDirection() > 0 ? UnitVec<P,1>( axDir)
1377  : UnitVec<P,1>(-axDir); // cheap
1378 }
1379 
1384 const BaseMat& asMat33() const { return *static_cast<const BaseMat*>(this); }
1385 BaseMat toMat33() const { return asMat33(); }
1387 };
1388 
1391 template <class P> SimTK_SimTKCOMMON_EXPORT std::ostream&
1392 operator<<(std::ostream&, const Rotation_<P>&);
1395 template <class P> SimTK_SimTKCOMMON_EXPORT std::ostream&
1396 operator<<(std::ostream&, const InverseRotation_<P>&);
1397 
1402 template <class P, int S> inline UnitVec<P,1>
1404 {return UnitVec<P,1>(R.asMat33()* v.asVec3(), true);}
1405 template <class P, int S> inline UnitRow<P,1>
1407 {return UnitRow<P,1>(r.asRow3() * R.asMat33(), true);}
1408 template <class P, int S> inline UnitVec<P,1>
1410 {return UnitVec<P,1>(R.asMat33()* v.asVec3(), true);}
1411 template <class P, int S> inline UnitRow<P,1>
1413 {return UnitRow<P,1>(r.asRow3() * R.asMat33(), true);}
1415 
1416 // Couldn't implement these Rotation_ methods until InverseRotation_ was defined.
1417 template <class P> inline
1419 : Mat<3,3,P>( R.asMat33() ) {}
1420 
1421 template <class P> inline Rotation_<P>&
1423 {static_cast<Mat<3,3,P>&>(*this) = R.asMat33(); return *this;}
1424 template <class P> inline Rotation_<P>&
1426 {static_cast<Mat<3,3,P>&>(*this) *= R.asMat33(); return *this;}
1427 template <class P> inline Rotation_<P>&
1429 {static_cast<Mat<3,3,P>&>(*this) *= (~R).asMat33(); return *this;}
1430 template <class P> inline Rotation_<P>&
1432 {static_cast<Mat<3,3,P>&>(*this) *= R.asMat33(); return *this;}
1433 template <class P> inline Rotation_<P>&
1435 {static_cast<Mat<3,3,P>&>(*this) *= (~R).asMat33(); return *this;}
1436 
1438 
1439 template <class P> inline Rotation_<P>
1440 operator*(const Rotation_<P>& R1, const Rotation_<P>& R2)
1441 {return Rotation_<P>(R1) *= R2;}
1442 template <class P> inline Rotation_<P>
1444 {return Rotation_<P>(R1) *= R2;}
1445 template <class P> inline Rotation_<P>
1447 {return Rotation_<P>(R1) *= R2;}
1448 template <class P> inline Rotation_<P>
1450 {return Rotation_<P>(R1) *= R2;}
1452 
1455 
1456 template <class P> inline Rotation_<P>
1457 operator/( const Rotation_<P>& R1, const Rotation_<P>& R2 )
1458 {return Rotation_<P>(R1) /= R2;}
1459 template <class P> inline Rotation_<P>
1460 operator/( const Rotation_<P>& R1, const InverseRotation& R2 )
1461 {return Rotation_<P>(R1) /= R2;}
1462 template <class P> inline Rotation_<P>
1464 {return Rotation_<P>(R1) /= R2;}
1465 template <class P> inline Rotation_<P>
1467 {return Rotation_<P>(R1) /= R2;}
1469 
1470 
1471 //------------------------------------------------------------------------------
1472 } // End of namespace SimTK
1473 
1474 #endif // SimTK_SimTKCOMMON_ROTATION_H_
1475 
1476 
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
Matrix_< E > operator/(const MatrixBase< E > &l, const typename CNT< E >::StdNumber &r)
Definition: BigMatrix.h:613
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:218
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
Defines the CoordinateAxis and CoordinateDirection classes.
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
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 RowType & operator[](int i) const
Same as row(i) but nicer to look at.
Definition: Rotation.h:464
QuaternionP convertRotationToQuaternion() const
Converts rotation matrix to an equivalent quaternion in canonical form (meaning its scalar element is...
const UnitVec< P, 1 > getAxisUnitVec(CoordinateDirection dir) const
Given a CoordinateDirection (+/-XAxis, etc.) return a unit vector in that direction.
Definition: Rotation.h:1374
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
const RotationP & transpose() const
Transpose, and transpose operators (override BaseMat versions of transpose).
Definition: Rotation.h:1339
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
const UnitVec< P, 1 > getAxisUnitVec(CoordinateDirection dir) const
Given a CoordinateDirection (+/-XAxis, etc.) return a unit vector in that direction.
Definition: Rotation.h:491
Vec3P convertRotationToBodyFixedXYZ() const
A convenient special case of convertThreeAxesRotationToThreeAngles().
Definition: Rotation.h:843
const RowType & row(int i) const
Return a reference to the ith row of this Rotation matrix as a UnitRow3.
Definition: Rotation.h:461
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&#39;s unit vectors expressed in A.
RotationP & operator~()
Transpose, and transpose operators (override BaseMat versions of transpose).
Definition: Rotation.h:1342
Definition: CoordinateAxis.h:194
A CoordinateDirection is a CoordinateAxis plus a direction indicating the positive or negative direct...
Definition: CoordinateAxis.h:244
Definition: CoordinateAxis.h:197
Quaternion_< P > QuaternionP
Definition: Rotation.h:124
The Rotation class is a Mat33 that guarantees that the matrix can be interpreted as a legitimate 3x3 ...
Definition: Quaternion.h:40
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&#39;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
Definition: CoordinateAxis.h:200
Definition: Rotation.h:42
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
const InverseRotation_< P > & operator~() const
Transpose operator.
Definition: Rotation.h:357
SymMat< 3, P > SymMat33P
Definition: Rotation.h:123
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
const ColType & y() const
Access individual rows and columns of this InverseRotation; no cost or copying since suitably-cast re...
Definition: Rotation.h:1358
Rotation_ & setRotationFromApproximateMat33(const Mat33P &m)
Set this Rotation_ object to an (hopefully nearby) orthogonal rotation matrix from a generic Mat33P...
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...
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 ...
const ColType & z() const
Access individual rows and columns of this InverseRotation; no cost or copying since suitably-cast re...
Definition: Rotation.h:1359
Vec4P convertQuaternionToAngleAxis() const
Returns [ a vx vy vz ] with (a,v) in canonical form, i.e., -180 < a <= 180 and |v|=1.
This class, along with its sister class CoordinateDirection, provides convenient manipulation of the ...
Definition: CoordinateAxis.h:53
RealP convertOneAxisRotationToOneAngle(const CoordinateAxis &axis1) const
Converts rotation matrix to a single orientation angle.
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_ & 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 & y() const
Return col(1) of this Rotation matrix as a UnitVec3.
Definition: Rotation.h:476
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
Mat< 4, 3, P > Mat43P
Definition: Rotation.h:117
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:1351
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:1303
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:1311
Rotation_< Real > Rotation
Definition: Rotation.h:47
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
bool isYAxis() const
Return true if this is the Y axis.
Definition: CoordinateAxis.h:93
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
Vec< 2, P > Vec2P
Definition: Rotation.h:119
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
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:605
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
const BaseVec & asVec3() const
Return a reference to the underlying Vec3 (no copying here).
Definition: UnitVec.h:118
BaseMat toMat33() const
Conversion from InverseRotation_ to BaseMat.
Definition: Rotation.h:1385
const ColType & x() const
Access individual rows and columns of this InverseRotation; no cost or copying since suitably-cast re...
Definition: Rotation.h:1357
const BaseRow & asRow3() const
Return a const reference to the Row3 underlying this UnitRow.
Definition: UnitVec.h:258
RotationP & updTranspose()
Transpose, and transpose operators (override BaseMat versions of transpose).
Definition: Rotation.h:1341
InverseRotation_ & operator=(const InverseRotation_ &R)
An explicit implementation of the default copy assignment operator.
Definition: Rotation.h:1316
Matrix_< E > operator*(const MatrixBase< E > &l, const typename CNT< E >::StdNumber &r)
Definition: BigMatrix.h:605
RealP getMaxAbsDifferenceInRotationElements(const Rotation_ &R) const
Returns maximum absolute difference between elements in "this" Rotation and elements in "R"...
Definition: Rotation.h:1079
Mat< 3, 4, P > Mat34P
Definition: Rotation.h:118
Rotation_ & setRotationToNaN()
Construct Rotation_ filled with NaNs.
Definition: Rotation.h:152
Definition: Rotation.h:42
Vec4P convertRotationToAngleAxis() const
Converts rotation matrix to an equivalent angle-axis representation in canonicalized form...
Definition: Rotation.h:836
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:1297
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
Mat< 2, 2, P > Mat22P
Definition: Rotation.h:114
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_(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 RotationP & invert() const
We can invert an InverseRotation just by recasting it to a Rotation at zero cost. ...
Definition: Rotation.h:1331
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
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
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
const ColType & z() const
Return col(2) of this Rotation matrix as a UnitVec3.
Definition: Rotation.h:478
void setToNaN()
Definition: Mat.h:902
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&#39;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 & col(int j) const
Return a reference to the jth column of this Rotation matrix as a UnitVec3.
Definition: Rotation.h:468
static Vec3P convertQuaternionDotToAngVel(const Vec4P &q, const Vec4P &qdot)
Inverse of the above routine.
Definition: Rotation.h:996
const ColType & operator()(int j) const
Same as col(j) but nicer to look at.
Definition: Rotation.h:471
RowVectorBase< typename CNT< ELEM >::TAbs > abs(const RowVectorBase< ELEM > &v)
Definition: VectorMath.h:120
BodyOrSpaceType
Definition: Rotation.h:42
const Mat33P & asMat33() const
Conversion from Rotation to its base class Mat33.
Definition: Rotation.h:757
Vec3P convertThreeAxesRotationToThreeAngles(BodyOrSpaceType bodyOrSpace, const CoordinateAxis &axis1, const CoordinateAxis &axis2, const CoordinateAxis &axis3) const
Converts rotation matrix to three orientation angles.
Rotation_()
Default constructor.
Definition: Rotation.h:137
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
bool isSameRotationToWithinAngleOfMachinePrecision(const Rotation_ &R) const
Return true if "this" Rotation is nearly identical to "R" within machine precision.
Definition: Rotation.h:1074
Mat33P toMat33() const
Conversion from Rotation to its base class Mat33.
Definition: Rotation.h:761
bool isXAxis() const
Return true if this is the X axis.
Definition: CoordinateAxis.h:91
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:304
const ColType & x() const
Return col(0) of this Rotation matrix as a UnitVec3.
Definition: Rotation.h:474
InverseRotation_< float > fInverseRotation
Definition: Rotation.h:54
InverseRotation_(const InverseRotation_ &R)
An explicit implementation of the default copy constructor.
Definition: Rotation.h:1314
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
Rotation_ & setRotationFromMat33TrustMe(const Mat33P &m)
(Advanced) Set the Rotation_ matrix directly - but you had better know what you are doing! ...
Definition: Rotation.h:282
const RotationP & operator~() const
Transpose, and transpose operators (override BaseMat versions of transpose).
Definition: Rotation.h:1340
Vec2P convertRotationToBodyFixedXY() const
A convenient special case of convertTwoAxesRotationToTwoAngles().
Definition: Rotation.h:840
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
int getDirection() const
Returns 1 or -1 to indicate the direction along the coordinate axis returned by getAxis().
Definition: CoordinateAxis.h:277
ScalarNormSq normSqr() const
Definition: Vec.h:606
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:1425
Rotation_ & setRotationFromTwoAxes(const UnitVec3P &uveci, const CoordinateAxis &axisi, const Vec3P &vecjApprox, const CoordinateAxis &axisjApprox)
Calculate R_AB by knowing one of B&#39;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
const BaseMat & asMat33() const
Conversion from InverseRotation_ to BaseMat.
Definition: Rotation.h:1384
Mat< 3, 3, P > Mat33P
Definition: Rotation.h:116
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
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:1305
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
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:1367
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
Rotation_(const Mat33P &m)
Constructs an (hopefully nearby) orthogonal rotation matrix from a generic Mat33P.
Definition: Rotation.h:271
Vec2P convertTwoAxesRotationToTwoAngles(BodyOrSpaceType bodyOrSpace, const CoordinateAxis &axis1, const CoordinateAxis &axis2) const
Converts rotation matrix to two orientation angles.
This class represents a small matrix whose size is known at compile time, containing elements of any ...
Definition: SimTKcommon/include/SimTKcommon/internal/common.h:607
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
const InverseRotation_< P > & invert() const
Convert from Rotation_ to InverseRotation_ (no cost).
Definition: Rotation.h:373
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:1428
const InverseRotation_< P > & transpose() const
Transpose.
Definition: Rotation.h:365
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:1353
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
Definition: negator.h:64
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
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:1356
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
RotationP & updInvert()
We can invert an InverseRotation just by recasting it to a Rotation at zero cost. ...
Definition: Rotation.h:1333
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:1354
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 ...