Simbody  3.4
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
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-12 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 
29 #include "SimTKcommon/SmallMatrix.h"
30 #include "SimTKcommon/internal/CoordinateAxis.h"
31 #include "SimTKcommon/internal/UnitVec.h"
32 #include "SimTKcommon/internal/Quaternion.h"
33 
34 //------------------------------------------------------------------------------
35 #include <iosfwd> // Forward declaration of iostream
36 //------------------------------------------------------------------------------
37 
38 //------------------------------------------------------------------------------
39 namespace SimTK {
40 
41 
42 enum BodyOrSpaceType { BodyRotationSequence=0, SpaceRotationSequence=1 };
43 
44 //------------------------------------------------------------------------------
45 // Forward declarations
46 template <class P> class Rotation_;
47 template <class P> class InverseRotation_;
48 
49 typedef Rotation_<Real> Rotation;
50 typedef Rotation_<float> fRotation;
51 typedef Rotation_<double> dRotation;
52 
53 typedef InverseRotation_<Real> InverseRotation;
54 typedef InverseRotation_<float> fInverseRotation;
55 typedef InverseRotation_<double> dInverseRotation;
56 
57 //------------------------------------------------------------------------------
107 //------------------------------------------------------------------------------
108 template <class P> // templatized by precision
109 class Rotation_ : public Mat<3,3,P> {
110 public:
111  typedef P RealP;
112  typedef Mat<2,2,P> Mat22P;
113  typedef Mat<3,2,P> Mat32P;
114  typedef Mat<3,3,P> Mat33P;
115  typedef Mat<4,3,P> Mat43P;
116  typedef Mat<3,4,P> Mat34P;
117  typedef Vec<2,P> Vec2P;
118  typedef Vec<3,P> Vec3P;
119  typedef Vec<4,P> Vec4P;
120  typedef UnitVec<P,1> UnitVec3P; // stride is 1 here, length is always 3
121  typedef SymMat<3,P> SymMat33P;
122  typedef Quaternion_<P> QuaternionP;
123 
124  // Default constructor and constructor-like methods
125  Rotation_() : Mat33P(1) {}
126  Rotation_& setRotationToIdentityMatrix() { Mat33P::operator=(RealP(1)); return *this; }
127  Rotation_& setRotationToNaN() { Mat33P::setToNaN(); return *this; }
128 
129  // Default copy constructor and assignment operator
130  Rotation_( const Rotation_& R ) : Mat33P(R) {}
131  Rotation_& operator=( const Rotation_& R ) { Mat33P::operator=( R.asMat33() ); return *this; }
132 
134 
135  Rotation_( RealP angle, const CoordinateAxis& axis ) { setRotationFromAngleAboutAxis( angle, axis ); }
136  Rotation_( RealP angle, const CoordinateAxis::XCoordinateAxis ) { setRotationFromAngleAboutX( std::cos(angle), std::sin(angle) ); }
137  Rotation_( RealP angle, const CoordinateAxis::YCoordinateAxis ) { setRotationFromAngleAboutY( std::cos(angle), std::sin(angle) ); }
138  Rotation_( RealP angle, const CoordinateAxis::ZCoordinateAxis ) { setRotationFromAngleAboutZ( std::cos(angle), std::sin(angle) ); }
140 
141 
142  Rotation_& setRotationFromAngleAboutAxis( RealP angle, const CoordinateAxis& axis ) { return axis.isXAxis() ? setRotationFromAngleAboutX(angle) : (axis.isYAxis() ? setRotationFromAngleAboutY(angle) : setRotationFromAngleAboutZ(angle) ); }
143  Rotation_& setRotationFromAngleAboutX( RealP angle ) { return setRotationFromAngleAboutX( std::cos(angle), std::sin(angle) ); }
144  Rotation_& setRotationFromAngleAboutY( RealP angle ) { return setRotationFromAngleAboutY( std::cos(angle), std::sin(angle) ); }
145  Rotation_& setRotationFromAngleAboutZ( RealP angle ) { return setRotationFromAngleAboutZ( std::cos(angle), std::sin(angle) ); }
146  Rotation_& setRotationFromAngleAboutX( RealP cosAngle, RealP sinAngle ) { Mat33P& R = *this; R[0][0] = 1; R[0][1] = R[0][2] = R[1][0] = R[2][0] = 0; R[1][1] = R[2][2] = cosAngle; R[1][2] = -(R[2][1] = sinAngle); return *this; }
147  Rotation_& setRotationFromAngleAboutY( RealP cosAngle, RealP sinAngle ) { Mat33P& R = *this; R[1][1] = 1; R[0][1] = R[1][0] = R[1][2] = R[2][1] = 0; R[0][0] = R[2][2] = cosAngle; R[2][0] = -(R[0][2] = sinAngle); return *this; }
148  Rotation_& setRotationFromAngleAboutZ( RealP cosAngle, RealP sinAngle ) { Mat33P& R = *this; R[2][2] = 1; R[0][2] = R[1][2] = R[2][0] = R[2][1] = 0; R[0][0] = R[1][1] = cosAngle; R[0][1] = -(R[1][0] = sinAngle); return *this; }
150 
152 
153  Rotation_( RealP angle, const UnitVec3P& unitVector ) { setRotationFromAngleAboutUnitVector(angle,unitVector); }
154  Rotation_( RealP angle, const Vec3P& nonUnitVector ) { setRotationFromAngleAboutNonUnitVector(angle,nonUnitVector); }
156 
157 
158  Rotation_& setRotationFromAngleAboutNonUnitVector( RealP angle, const Vec3P& nonUnitVector ) { return setRotationFromAngleAboutUnitVector( angle, UnitVec3P(nonUnitVector) ); }
159  SimTK_SimTKCOMMON_EXPORT Rotation_& setRotationFromAngleAboutUnitVector( RealP angle, const UnitVec3P& unitVector );
161 
163  Rotation_( BodyOrSpaceType bodyOrSpace, RealP angle1, const CoordinateAxis& axis1, RealP angle2, const CoordinateAxis& axis2 ) { setRotationFromTwoAnglesTwoAxes( bodyOrSpace,angle1,axis1,angle2,axis2); }
165  Rotation_( BodyOrSpaceType bodyOrSpace, RealP angle1, const CoordinateAxis& axis1, RealP angle2, const CoordinateAxis& axis2, RealP angle3, const CoordinateAxis& axis3 ) { setRotationFromThreeAnglesThreeAxes(bodyOrSpace,angle1,axis1,angle2,axis2,angle3,axis3); }
167  SimTK_SimTKCOMMON_EXPORT Rotation_& setRotationFromTwoAnglesTwoAxes( BodyOrSpaceType bodyOrSpace, RealP angle1, const CoordinateAxis& axis1, RealP angle2, const CoordinateAxis& axis2 );
169  SimTK_SimTKCOMMON_EXPORT Rotation_& setRotationFromThreeAnglesThreeAxes( BodyOrSpaceType bodyOrSpace, RealP angle1, const CoordinateAxis& axis1, RealP angle2, const CoordinateAxis& axis2, RealP angle3, const CoordinateAxis& axis3 );
170 
174  void setRotationToBodyFixedXY( const Vec2P& v) { setRotationFromTwoAnglesTwoAxes( BodyRotationSequence, v[0], XAxis, v[1], YAxis ); }
179  void setRotationToBodyFixedXYZ( const Vec3P& v) { setRotationFromThreeAnglesThreeAxes( BodyRotationSequence, v[0], XAxis, v[1], YAxis, v[2], ZAxis ); }
180 
182  explicit Rotation_( const QuaternionP& q ) { setRotationFromQuaternion(q); }
184  SimTK_SimTKCOMMON_EXPORT Rotation_& setRotationFromQuaternion( const QuaternionP& q );
185 
187  Rotation_( const Mat33P& m, bool ) : Mat33P(m) {}
188 
190  explicit Rotation_( const Mat33P& m ) { setRotationFromApproximateMat33(m); }
192  SimTK_SimTKCOMMON_EXPORT Rotation_& setRotationFromApproximateMat33( const Mat33P& m );
193 
196 
197  Rotation_( const UnitVec3P& uvec, const CoordinateAxis axis ) { setRotationFromOneAxis(uvec,axis); }
198  SimTK_SimTKCOMMON_EXPORT Rotation_& setRotationFromOneAxis( const UnitVec3P& uvec, const CoordinateAxis axis );
200 
207 
208  Rotation_( const UnitVec3P& uveci, const CoordinateAxis& axisi, const Vec3P& vecjApprox, const CoordinateAxis& axisjApprox ) { setRotationFromTwoAxes(uveci,axisi,vecjApprox,axisjApprox); }
209  SimTK_SimTKCOMMON_EXPORT Rotation_& setRotationFromTwoAxes( const UnitVec3P& uveci, const CoordinateAxis& axisi, const Vec3P& vecjApprox, const CoordinateAxis& axisjApprox );
211 
212  // Converts rotation matrix to one or two or three orientation angles.
213  // Note: The result is most meaningful if the Rotation_ matrix is one that can be produced by such a sequence.
214  // Use1: someRotation.convertOneAxisRotationToOneAngle( XAxis );
215  // Use2: someRotation.convertTwoAxesRotationToTwoAngles( SpaceRotationSequence, YAxis, ZAxis );
216  // Use3: someRotation.convertThreeAxesRotationToThreeAngles( SpaceRotationSequence, ZAxis, YAxis, XAxis );
217  // Use4: someRotation.convertRotationToAngleAxis(); Return: [angleInRadians, unitVectorX, unitVectorY, unitVectorZ].
218 
221  SimTK_SimTKCOMMON_EXPORT RealP convertOneAxisRotationToOneAngle( const CoordinateAxis& axis1 ) const;
224  SimTK_SimTKCOMMON_EXPORT Vec2P convertTwoAxesRotationToTwoAngles( BodyOrSpaceType bodyOrSpace, const CoordinateAxis& axis1, const CoordinateAxis& axis2 ) const;
227  SimTK_SimTKCOMMON_EXPORT Vec3P convertThreeAxesRotationToThreeAngles( BodyOrSpaceType bodyOrSpace, const CoordinateAxis& axis1, const CoordinateAxis& axis2, const CoordinateAxis& axis3 ) const;
229  SimTK_SimTKCOMMON_EXPORT QuaternionP convertRotationToQuaternion() const;
231  Vec4P convertRotationToAngleAxis() const { return convertRotationToQuaternion().convertQuaternionToAngleAxis(); }
232 
234  Vec2P convertRotationToBodyFixedXY() const { return convertTwoAxesRotationToTwoAngles( BodyRotationSequence, XAxis, YAxis ); }
236  Vec3P convertRotationToBodyFixedXYZ() const { return convertThreeAxesRotationToThreeAngles( BodyRotationSequence, XAxis, YAxis, ZAxis ); }
237 
245  SimTK_SimTKCOMMON_EXPORT SymMat33P reexpressSymMat33(const SymMat33P& S_BB) const;
246 
248 
249  SimTK_SimTKCOMMON_EXPORT bool isSameRotationToWithinAngle( const Rotation_& R, RealP okPointingAngleErrorRads ) const;
250  bool isSameRotationToWithinAngleOfMachinePrecision( const Rotation_& R) const
251  { return isSameRotationToWithinAngle( R, NTraits<P>::getSignificant() ); }
253  RealP getMaxAbsDifferenceInRotationElements( const Rotation_& R ) const {
254  const Mat33P& A = asMat33(); const Mat33P& B = R.asMat33(); RealP maxDiff = 0;
255  for( int i=0; i<=2; i++ ) for( int j=0; j<=2; j++ )
256  { RealP absDiff = std::fabs(A[i][j] - B[i][j]);
257  if( absDiff > maxDiff ) maxDiff = absDiff; }
258  return maxDiff;
259  }
260 
261  bool areAllRotationElementsSameToEpsilon( const Rotation_& R, RealP epsilon ) const
262  { return getMaxAbsDifferenceInRotationElements(R) <= epsilon ; }
263  bool areAllRotationElementsSameToMachinePrecision( const Rotation_& R ) const
264  { return areAllRotationElementsSameToEpsilon( R, NTraits<P>::getSignificant() ); }
265 
267  inline Rotation_( const InverseRotation_<P>& );
269  inline Rotation_& operator=( const InverseRotation_<P>& );
270 
272  const InverseRotation_<P>& invert() const { return *reinterpret_cast<const InverseRotation_<P>*>(this); }
274  InverseRotation_<P>& updInvert() { return *reinterpret_cast<InverseRotation_<P>*>(this); }
275 
278 
279  const InverseRotation_<P>& transpose() const { return invert(); }
280  const InverseRotation_<P>& operator~() const { return invert(); }
281  InverseRotation_<P>& updTranspose() { return updInvert(); }
282  InverseRotation_<P>& operator~() { return updInvert(); }
284 
286 
287  inline Rotation_& operator*=( const Rotation_<P>& R );
288  inline Rotation_& operator/=( const Rotation_<P>& R );
289  inline Rotation_& operator*=( const InverseRotation_<P>& );
290  inline Rotation_& operator/=( const InverseRotation_<P>& );
292 
295 
296  const Mat33P& asMat33() const { return *static_cast<const Mat33P*>(this); }
297  Mat33P toMat33() const { return asMat33(); }
299 
301  typedef UnitVec<P,Mat33P::RowSpacing> ColType;
302  typedef UnitRow<P,Mat33P::ColSpacing> RowType;
303  const RowType& row( int i ) const { return reinterpret_cast<const RowType&>(asMat33()[i]); }
304  const ColType& col( int j ) const { return reinterpret_cast<const ColType&>(asMat33()(j)); }
305  const ColType& x() const { return col(0); }
306  const ColType& y() const { return col(1); }
307  const ColType& z() const { return col(2); }
308  const RowType& operator[]( int i ) const { return row(i); }
309  const ColType& operator()( int j ) const { return col(j); }
310 
312 
313  Rotation_& setRotationFromMat33TrustMe( const Mat33P& m )
314  { Mat33P& R = *this; R=m; return *this; }
315  Rotation_& setRotationColFromUnitVecTrustMe( int colj, const UnitVec3P& uvecj )
316  { Mat33P& R = *this; R(colj)=uvecj.asVec3(); return *this; }
317  Rotation_& setRotationFromUnitVecsTrustMe( const UnitVec3P& colA, const UnitVec3P& colB, const UnitVec3P& colC )
318  { Mat33P& R = *this; R(0)=colA.asVec3(); R(1)=colB.asVec3(); R(2)=colC.asVec3(); return *this; }
320 
321 //--------------------------- PAUL CONTINUE FROM HERE --------------------------
322 public:
323 //------------------------------------------------------------------------------
324 
325  // These are ad hoc routines that don't match the nice API Paul Mitiguy
326  // implemented above.
327 
328 
332  void setRotationToBodyFixedXYZ(const Vec3P& c, const Vec3P& s) {
333  Mat33P& R = *this;
334  const RealP s0s1=s[0]*s[1], s2c0=s[2]*c[0], c0c2=c[0]*c[2], nc1= -c[1];
335 
336  R = Mat33P( c[1]*c[2] , s[2]*nc1 , s[1] ,
337  s2c0 + s0s1*c[2] , c0c2 - s0s1*s[2] , s[0]*nc1 ,
338  s[0]*s[2] - s[1]*c0c2 , s[0]*c[2] + s[1]*s2c0 , c[0]*c[1] );
339  }
340 
341 
347  static Vec3P convertAngVelToBodyFixed321Dot(const Vec3P& q, const Vec3P& w_PB_B) {
348  const RealP s1 = std::sin(q[1]), c1 = std::cos(q[1]);
349  const RealP s2 = std::sin(q[2]), c2 = std::cos(q[2]);
350  const RealP ooc1 = RealP(1)/c1;
351  const RealP s2oc1 = s2*ooc1, c2oc1 = c2*ooc1;
352 
353  const Mat33P E( 0, s2oc1 , c2oc1 ,
354  0, c2 , -s2 ,
355  1, s1*s2oc1 , s1*c2oc1 );
356  return E * w_PB_B;
357  }
358 
361  static Vec3P convertBodyFixed321DotToAngVel(const Vec3P& q, const Vec3P& qd) {
362  const RealP s1 = std::sin(q[1]), c1 = std::cos(q[1]);
363  const RealP s2 = std::sin(q[2]), c2 = std::cos(q[2]);
364 
365  const Mat33P Einv( -s1 , 0 , 1 ,
366  c1*s2 , c2 , 0 ,
367  c1*c2 , -s2 , 0 );
368  return Einv*qd;
369  }
370 
371  // TODO: sherm: is this right? Warning: everything is measured in the
372  // *PARENT* frame, but angular velocities and accelerations are
373  // expressed in the *BODY* frame.
374  // TODO: this is not an efficient way to do this computation.
375  static Vec3P convertAngVelDotToBodyFixed321DotDot
376  (const Vec3P& q, const Vec3P& w_PB_B, const Vec3P& wdot_PB_B)
377  {
378  const RealP s1 = std::sin(q[1]), c1 = std::cos(q[1]);
379  const RealP s2 = std::sin(q[2]), c2 = std::cos(q[2]);
380  const RealP ooc1 = 1/c1;
381  const RealP s2oc1 = s2*ooc1, c2oc1 = c2*ooc1, s1oc1 = s1*ooc1;
382 
383  const Mat33P E( 0 , s2oc1 , c2oc1 ,
384  0 , c2 , -s2 ,
385  1 , s1*s2oc1 , s1*c2oc1 );
386  const Vec3P qdot = E * w_PB_B;
387 
388  const RealP t = qdot[1]*s1oc1;
389  const RealP a = t*s2oc1 + qdot[2]*c2oc1; // d/dt s2oc1
390  const RealP b = t*c2oc1 - qdot[2]*s2oc1; // d/dt c2oc1
391 
392  const Mat33P Edot( 0 , a , b ,
393  0 , -qdot[2]*s2 , -qdot[2]*c2 ,
394  0 , s1*a + qdot[1]*s2 , s1*b + qdot[1]*c2 );
395 
396  return E*wdot_PB_B + Edot*w_PB_B;
397  }
398 
410  static Mat33P calcNForBodyXYZInBodyFrame(const Vec3P& q) {
411  // Note: q[0] is not referenced so we won't waste time calculating
412  // its cosine and sine here.
413  return calcNForBodyXYZInBodyFrame
414  (Vec3P(0, std::cos(q[1]), std::cos(q[2])),
415  Vec3P(0, std::sin(q[1]), std::sin(q[2])));
416  }
417 
423  static Mat33P calcNForBodyXYZInBodyFrame(const Vec3P& cq, const Vec3P& sq) {
424  const RealP s1 = sq[1], c1 = cq[1];
425  const RealP s2 = sq[2], c2 = cq[2];
426  const RealP ooc1 = 1/c1;
427  const RealP s2oc1 = s2*ooc1, c2oc1 = c2*ooc1;
428 
429  return Mat33P( c2oc1 , -s2oc1 , 0,
430  s2 , c2 , 0,
431  -s1*c2oc1 , s1*s2oc1, 1 );
432  }
433 
446  static Mat33P calcNForBodyXYZInParentFrame(const Vec3P& q) {
447  // Note: q[2] is not referenced so we won't waste time calculating
448  // its cosine and sine here.
449  return calcNForBodyXYZInParentFrame
450  (Vec3P(std::cos(q[0]), std::cos(q[1]), 0),
451  Vec3P(std::sin(q[0]), std::sin(q[1]), 0));
452  }
453 
460  static Mat33P calcNForBodyXYZInParentFrame(const Vec3P& cq, const Vec3P& sq) {
461  const RealP s0 = sq[0], c0 = cq[0];
462  const RealP s1 = sq[1], c1 = cq[1];
463  const RealP ooc1 = 1/c1;
464  const RealP s0oc1 = s0*ooc1, c0oc1 = c0*ooc1;
465 
466  return Mat33P( 1 , s1*s0oc1 , -s1*c0oc1,
467  0 , c0 , s0,
468  0 , -s0oc1 , c0oc1 );
469  }
470 
477  static Vec3P multiplyByBodyXYZ_N_P(const Vec2P& cosxy,
478  const Vec2P& sinxy,
479  RealP oocosy,
480  const Vec3P& w_PB)
481  {
482  const RealP s0 = sinxy[0], c0 = cosxy[0];
483  const RealP s1 = sinxy[1];
484  const RealP w0 = w_PB[0], w1 = w_PB[1], w2 = w_PB[2];
485 
486  const RealP t = (s0*w1-c0*w2)*oocosy;
487  return Vec3P( w0 + t*s1, c0*w1 + s0*w2, -t ); // qdot
488  }
489 
493  static Vec3P multiplyByBodyXYZ_NT_P(const Vec2P& cosxy,
494  const Vec2P& sinxy,
495  RealP oocosy,
496  const Vec3P& q)
497  {
498  const RealP s0 = sinxy[0], c0 = cosxy[0];
499  const RealP s1 = sinxy[1];
500  const RealP q0 = q[0], q1 = q[1], q2 = q[2];
501 
502  const RealP t = (q0*s1-q2) * oocosy;
503  return Vec3P( q0, c0*q1 + t*s0, s0*q1 - t*c0 ); // v_P
504  }
505 
512  static Vec3P convertAngVelInParentToBodyXYZDot
513  (const Vec2P& cosxy,
514  const Vec2P& sinxy,
515  RealP oocosy,
516  const Vec3P& w_PB)
517  {
518  return multiplyByBodyXYZ_N_P(cosxy,sinxy,oocosy,w_PB);
519  }
520 
532  static Vec3P convertAngAccInParentToBodyXYZDotDot
533  (const Vec2P& cosxy,
534  const Vec2P& sinxy,
535  RealP oocosy,
536  const Vec3P& qdot,
537  const Vec3P& b_PB)
538  {
539  const RealP s1 = sinxy[1], c1 = cosxy[1];
540  const RealP q0 = qdot[0], q1 = qdot[1], q2 = qdot[2];
541 
542  // 10 flops
543  const Vec3P Nb = multiplyByBodyXYZ_N_P(cosxy,sinxy,oocosy,b_PB);
544 
545  const RealP q1oc1 = q1*oocosy;
546  const Vec3P NDotw((q0*s1-q2)*q1oc1, // NDot_P*w_PB
547  q0*q2*c1, // = NDot_P*(NInv_P*qdot)
548  (q2*s1-q0)*q1oc1 ); // (9 flops)
549 
550  return Nb + NDotw; // 3 flops
551  }
552 
553 
556  static Vec3P multiplyByBodyXYZ_NInv_P(const Vec2P& cosxy,
557  const Vec2P& sinxy,
558  const Vec3P& qdot)
559  {
560  const RealP s0 = sinxy[0], c0 = cosxy[0];
561  const RealP s1 = sinxy[1], c1 = cosxy[1];
562  const RealP q0 = qdot[0], q1 = qdot[1], q2 = qdot[2];
563  const RealP c1q2 = c1*q2;
564 
565  return Vec3P( q0 + s1*q2, // w_PB
566  c0*q1 - s0*c1q2,
567  s0*q1 + c0*c1q2 );
568  }
569 
572  static Vec3P multiplyByBodyXYZ_NInvT_P(const Vec2P& cosxy,
573  const Vec2P& sinxy,
574  const Vec3P& v_P)
575  {
576  const RealP s0 = sinxy[0], c0 = cosxy[0];
577  const RealP s1 = sinxy[1], c1 = cosxy[1];
578  const RealP w0 = v_P[0], w1 = v_P[1], w2 = v_P[2];
579 
580  return Vec3P( w0, // qdot-like
581  c0*w1 + s0*w2,
582  s1*w0 - s0*c1*w1 + c0*c1*w2);
583  }
584 
594  static Mat33P calcNDotForBodyXYZInBodyFrame
595  (const Vec3P& q, const Vec3P& qdot) {
596  // Note: q[0] is not referenced so we won't waste time calculating
597  // its cosine and sine here.
598  return calcNDotForBodyXYZInBodyFrame
599  (Vec3P(0, std::cos(q[1]), std::cos(q[2])),
600  Vec3P(0, std::sin(q[1]), std::sin(q[2])),
601  qdot);
602  }
603 
609  static Mat33P calcNDotForBodyXYZInBodyFrame
610  (const Vec3P& cq, const Vec3P& sq, const Vec3P& qdot)
611  {
612  const RealP s1 = sq[1], c1 = cq[1];
613  const RealP s2 = sq[2], c2 = cq[2];
614  const RealP ooc1 = 1/c1;
615  const RealP s2oc1 = s2*ooc1, c2oc1 = c2*ooc1;
616 
617  const RealP t = qdot[1]*s1*ooc1;
618  const RealP a = t*s2oc1 + qdot[2]*c2oc1; // d/dt s2oc1
619  const RealP b = t*c2oc1 - qdot[2]*s2oc1; // d/dt c2oc1
620 
621  return Mat33P( b , -a , 0,
622  qdot[2]*c2 , -qdot[2]*s2 , 0,
623  -(s1*b + qdot[1]*c2) , s1*a + qdot[1]*s2 , 0 );
624  }
625 
635  static Mat33P calcNDotForBodyXYZInParentFrame
636  (const Vec3P& q, const Vec3P& qdot) {
637  // Note: q[2] is not referenced so we won't waste time calculating
638  // its cosine and sine here.
639  const RealP cy = std::cos(q[1]); // cos(y)
640  return calcNDotForBodyXYZInParentFrame
641  (Vec2P(std::cos(q[0]), cy),
642  Vec2P(std::sin(q[0]), std::sin(q[1])),
643  1/cy, qdot);
644  }
645 
650  static Mat33P calcNDotForBodyXYZInParentFrame
651  (const Vec2P& cq, const Vec2P& sq, RealP ooc1, const Vec3P& qdot) {
652  const RealP s0 = sq[0], c0 = cq[0];
653  const RealP s1 = sq[1], c1 = cq[1];
654  const RealP s0oc1 = s0*ooc1, c0oc1 = c0*ooc1;
655 
656  const RealP t = qdot[1]*s1*ooc1;
657  const RealP a = t*s0oc1 + qdot[0]*c0oc1; // d/dt s0oc1
658  const RealP b = t*c0oc1 - qdot[0]*s0oc1; // d/dt c0oc1
659 
660  return Mat33P( 0, s1*a + qdot[1]*s0, -(s1*b + qdot[1]*c0),
661  0, -qdot[0]*s0 , qdot[0]*c0 ,
662  0, -a , b );
663  }
664 
673  static Mat33P calcNInvForBodyXYZInBodyFrame(const Vec3P& q) {
674  // Note: q[0] is not referenced so we won't waste time calculating
675  // its cosine and sine here.
676  return calcNInvForBodyXYZInBodyFrame
677  (Vec3P(0, std::cos(q[1]), std::cos(q[2])),
678  Vec3P(0, std::sin(q[1]), std::sin(q[2])));
679  }
680 
686  static Mat33P calcNInvForBodyXYZInBodyFrame
687  (const Vec3P& cq, const Vec3P& sq) {
688  const RealP s1 = sq[1], c1 = cq[1];
689  const RealP s2 = sq[2], c2 = cq[2];
690 
691  return Mat33P( c1*c2 , s2 , 0 ,
692  -c1*s2 , c2 , 0 ,
693  s1 , 0 , 1 );
694  }
695 
703  static Mat33P calcNInvForBodyXYZInParentFrame(const Vec3P& q) {
704  // Note: q[0] is not referenced so we won't waste time calculating
705  // its cosine and sine here.
706  return calcNInvForBodyXYZInParentFrame
707  (Vec3P(std::cos(q[0]), std::cos(q[1]), 0),
708  Vec3P(std::sin(q[0]), std::sin(q[1]), 0));
709  }
710 
716  static Mat33P calcNInvForBodyXYZInParentFrame
717  (const Vec3P& cq, const Vec3P& sq) {
718  const RealP s0 = sq[0], c0 = cq[0];
719  const RealP s1 = sq[1], c1 = cq[1];
720 
721  return Mat33P( 1 , 0 , s1 ,
722  0 , c0 , -s0*c1 ,
723  0 , s0 , c0*c1 );
724  }
725 
734  static Vec3P convertAngVelInBodyFrameToBodyXYZDot
735  (const Vec3P& q, const Vec3P& w_PB_B) {
736  return convertAngVelInBodyFrameToBodyXYZDot
737  (Vec3P(0, std::cos(q[1]), std::cos(q[2])),
738  Vec3P(0, std::sin(q[1]), std::sin(q[2])),
739  w_PB_B);
740  }
741 
747  //TODO: reimplement
748  static Vec3P convertAngVelInBodyFrameToBodyXYZDot
749  (const Vec3P& cq, const Vec3P& sq, const Vec3P& w_PB_B)
750  { return calcNForBodyXYZInBodyFrame(cq,sq)*w_PB_B; }
751 
757  static Vec3P convertBodyXYZDotToAngVelInBodyFrame
758  (const Vec3P& q, const Vec3P& qdot) {
759  return convertBodyXYZDotToAngVelInBodyFrame
760  (Vec3P(0, std::cos(q[1]), std::cos(q[2])),
761  Vec3P(0, std::sin(q[1]), std::sin(q[2])),
762  qdot);
763  }
764 
770  // TODO: reimplement
771  static Vec3P convertBodyXYZDotToAngVelInBodyFrame
772  (const Vec3P& cq, const Vec3P& sq, const Vec3P& qdot)
773  { return calcNInvForBodyXYZInBodyFrame(cq,sq)*qdot; }
774 
780  static Vec3P convertAngVelDotInBodyFrameToBodyXYZDotDot
781  (const Vec3P& q, const Vec3P& w_PB_B, const Vec3P& wdot_PB_B)
782  {
783  // Note: q[0] is not referenced so we won't waste time calculating
784  // its cosine and sine here.
785  return convertAngVelDotInBodyFrameToBodyXYZDotDot
786  (Vec3P(0, std::cos(q[1]), std::cos(q[2])),
787  Vec3P(0, std::sin(q[1]), std::sin(q[2])),
788  w_PB_B, wdot_PB_B);
789  }
790 
797  // TODO: reimplement
798  static Vec3P convertAngVelDotInBodyFrameToBodyXYZDotDot
799  (const Vec3P& cq, const Vec3P& sq, const Vec3P& w_PB_B, const Vec3P& wdot_PB_B)
800  {
801  const Mat33P N = calcNForBodyXYZInBodyFrame(cq,sq);
802  const Vec3P qdot = N * w_PB_B; // 15 flops
803  const Mat33P NDot = calcNDotForBodyXYZInBodyFrame(cq,sq,qdot);
804 
805  return N*wdot_PB_B + NDot*w_PB_B; // 33 flops
806  }
807 
813  static Mat43P calcUnnormalizedNForQuaternion(const Vec4P& q) {
814  const Vec4P e = q/2;
815  const RealP ne1 = -e[1], ne2 = -e[2], ne3 = -e[3];
816  return Mat43P( ne1, ne2, ne3,
817  e[0], e[3], ne2,
818  ne3, e[0], e[1],
819  e[2], ne1, e[0]);
820  }
821 
828  static Mat43P calcUnnormalizedNDotForQuaternion(const Vec4P& qdot) {
829  const Vec4P ed = qdot/2;
830  const RealP ned1 = -ed[1], ned2 = -ed[2], ned3 = -ed[3];
831  return Mat43P( ned1, ned2, ned3,
832  ed[0], ed[3], ned2,
833  ned3, ed[0], ed[1],
834  ed[2], ned1, ed[0]);
835  }
836 
845  static Mat34P calcUnnormalizedNInvForQuaternion(const Vec4P& q) {
846  const Vec4P e = 2*q;
847  const RealP ne1 = -e[1], ne2 = -e[2], ne3 = -e[3];
848  return Mat34P(ne1, e[0], ne3, e[2],
849  ne2, e[3], e[0], ne1,
850  ne3, ne2, e[1], e[0]);
851  }
852 
853 
858  static Vec4P convertAngVelToQuaternionDot(const Vec4P& q, const Vec3P& w_PB_P) {
859  return calcUnnormalizedNForQuaternion(q)*w_PB_P;
860  }
861 
865  static Vec3P convertQuaternionDotToAngVel(const Vec4P& q, const Vec4P& qdot) {
866  return calcUnnormalizedNInvForQuaternion(q)*qdot;
867  }
868 
874  // TODO: reimplement
875  static Vec4P OLDconvertAngVelDotToQuaternionDotDot
876  (const Vec4P& q, const Vec3P& w_PB_P, const Vec3P& wdot_PB_P)
877  {
878  const Mat43P N = calcUnnormalizedNForQuaternion(q);
879  const Vec4P qdot = N*w_PB_P; // 20 flops
880  const Mat43P NDot = calcUnnormalizedNDotForQuaternion(qdot);
881 
882  return N*wdot_PB_P + NDot*w_PB_P; // 44 flops
883  }
884 
891  static Vec4P convertAngVelDotToQuaternionDotDot
892  (const Vec4P& q, const Vec3P& w_PB, const Vec3P& b_PB)
893  {
894  const Mat43P N = calcUnnormalizedNForQuaternion(q); // 7 flops
895  const Vec4P Nb = N*b_PB; // 20 flops
896  const Vec4P NDotw = RealP(-.25)*w_PB.normSqr()*q; // 10 flops
897  return Nb + NDotw; // 4 flops
898  }
899 
900 
901 private:
902  // This is only for the most trustworthy of callers, that is, methods of
903  // the Rotation_ class. There are a lot of ways for this NOT to be a
904  // legitimate rotation matrix -- be careful!!
905  // Note that these are supplied in rows.
906  Rotation_( const RealP& xx, const RealP& xy, const RealP& xz,
907  const RealP& yx, const RealP& yy, const RealP& yz,
908  const RealP& zx, const RealP& zy, const RealP& zz )
909  : Mat33P( xx,xy,xz, yx,yy,yz, zx,zy,zz ) {}
910 
911  // These next methods are highly-efficient power-user methods. Read the
912  // code to understand them.
913  SimTK_SimTKCOMMON_EXPORT Rotation_& setTwoAngleTwoAxesBodyFixedForwardCyclicalRotation( RealP cosAngle1, RealP sinAngle1, const CoordinateAxis& axis1, RealP cosAngle2, RealP sinAngle2, const CoordinateAxis& axis2 );
914  SimTK_SimTKCOMMON_EXPORT Rotation_& setThreeAngleTwoAxesBodyFixedForwardCyclicalRotation( RealP cosAngle1, RealP sinAngle1, const CoordinateAxis& axis1, RealP cosAngle2, RealP sinAngle2, const CoordinateAxis& axis2, RealP cosAngle3, RealP sinAngle3 );
915  SimTK_SimTKCOMMON_EXPORT Rotation_& setThreeAngleThreeAxesBodyFixedForwardCyclicalRotation( RealP cosAngle1, RealP sinAngle1, const CoordinateAxis& axis1, RealP cosAngle2, RealP sinAngle2, const CoordinateAxis& axis2, RealP cosAngle3, RealP sinAngle3, const CoordinateAxis& axis3 );
916 
917  // These next methods are highly-efficient power-user methods to convert
918  // Rotation matrices to orientation angles. Read the code to understand them.
919  SimTK_SimTKCOMMON_EXPORT Vec2P convertTwoAxesBodyFixedRotationToTwoAngles( const CoordinateAxis& axis1, const CoordinateAxis& axis2 ) const;
920  SimTK_SimTKCOMMON_EXPORT Vec3P convertTwoAxesBodyFixedRotationToThreeAngles( const CoordinateAxis& axis1, const CoordinateAxis& axis2 ) const;
921  SimTK_SimTKCOMMON_EXPORT Vec3P convertThreeAxesBodyFixedRotationToThreeAngles( const CoordinateAxis& axis1, const CoordinateAxis& axis2, const CoordinateAxis& axis3 ) const;
922 
923 //------------------------------------------------------------------------------
924 // These are obsolete names from a previous release, listed here so that
925 // users will get a decipherable compilation error. (sherm 091101)
926 //------------------------------------------------------------------------------
927 private:
928  // REPLACED BY: calcNForBodyXYZInBodyFrame()
929  static Mat33P calcQBlockForBodyXYZInBodyFrame(const Vec3P& a)
930  { return calcNForBodyXYZInBodyFrame(a); }
931  // REPLACED BY: calcNInvForBodyXYZInBodyFrame()
932  static Mat33P calcQInvBlockForBodyXYZInBodyFrame(const Vec3P& a)
933  { return calcNInvForBodyXYZInBodyFrame(a); }
934  // REPLACED BY: calcUnnormalizedNForQuaternion()
935  static Mat<4,3,P> calcUnnormalizedQBlockForQuaternion(const Vec4P& q)
936  { return calcUnnormalizedNForQuaternion(q); }
937  // REPLACED BY: calcUnnormalizedNInvForQuaternion()
938  static Mat<3,4,P> calcUnnormalizedQInvBlockForQuaternion(const Vec4P& q)
939  { return calcUnnormalizedNInvForQuaternion(q); }
940  // REPLACED BY: convertAngVelInBodyFrameToBodyXYZDot
941  static Vec3P convertAngVelToBodyFixed123Dot(const Vec3P& q, const Vec3P& w_PB_B)
942  { return convertAngVelInBodyFrameToBodyXYZDot(q,w_PB_B); }
943  // REPLACED BY: convertBodyXYZDotToAngVelInBodyFrame
944  static Vec3P convertBodyFixed123DotToAngVel(const Vec3P& q, const Vec3P& qdot)
945  { return convertBodyXYZDotToAngVelInBodyFrame(q,qdot); }
946  // REPLACED BY: convertAngVelDotInBodyFrameToBodyXYZDotDot
947  static Vec3P convertAngVelDotToBodyFixed123DotDot
948  (const Vec3P& q, const Vec3P& w_PB_B, const Vec3P& wdot_PB_B)
949  { return convertAngVelDotInBodyFrameToBodyXYZDotDot(q,w_PB_B,wdot_PB_B); }
950 
951 //------------------------------------------------------------------------------
952 // The following code is obsolete - it is here temporarily for backward
953 // compatibility (Mitiguy 9/5/2007)
954 //------------------------------------------------------------------------------
955 private:
956  // These static methods are like constructors with friendlier names.
957  static Rotation_ zero() { return Rotation_(); }
958  static Rotation_ NaN() { Rotation_ r; r.setRotationToNaN(); return r; }
959 
961  Rotation_& setToZero() { return setRotationToIdentityMatrix(); }
962  Rotation_& setToIdentityMatrix() { return setRotationToIdentityMatrix(); }
963  Rotation_& setToNaN() { return setRotationToNaN(); }
964  static Rotation_ trustMe( const Mat33P& m ) { return Rotation_(m,true); }
965 
966  // One-angle rotations.
967  static Rotation_ aboutX( const RealP& angleInRad ) { return Rotation_( angleInRad, XAxis ); }
968  static Rotation_ aboutY( const RealP& angleInRad ) { return Rotation_( angleInRad, YAxis ); }
969  static Rotation_ aboutZ( const RealP& angleInRad ) { return Rotation_( angleInRad, ZAxis ); }
970  static Rotation_ aboutAxis( const RealP& angleInRad, const UnitVec3P& axis ) { return Rotation_(angleInRad,axis); }
971  static Rotation_ aboutAxis( const RealP& angleInRad, const Vec3P& axis ) { return Rotation_(angleInRad,axis); }
972  void setToRotationAboutZ( const RealP& q ) { setRotationFromAngleAboutZ( q ); }
973 
974  // Two-angle space-fixed rotations.
975  static Rotation_ aboutXThenOldY(const RealP& xInRad, const RealP& yInRad) { return Rotation_( SpaceRotationSequence, xInRad, XAxis, yInRad, YAxis ); }
976  static Rotation_ aboutYThenOldX(const RealP& yInRad, const RealP& xInRad) { return Rotation_( SpaceRotationSequence, yInRad, YAxis, xInRad, XAxis ); }
977  static Rotation_ aboutXThenOldZ(const RealP& xInRad, const RealP& zInRad) { return Rotation_( SpaceRotationSequence, xInRad, XAxis, zInRad, ZAxis ); }
978  static Rotation_ aboutZThenOldX(const RealP& zInRad, const RealP& xInRad) { return Rotation_( SpaceRotationSequence, zInRad, ZAxis, xInRad, XAxis ); }
979  static Rotation_ aboutYThenOldZ(const RealP& yInRad, const RealP& zInRad) { return Rotation_( SpaceRotationSequence, yInRad, YAxis, zInRad, ZAxis ); }
980  static Rotation_ aboutZThenOldY(const RealP& zInRad, const RealP& yInRad) { return Rotation_( SpaceRotationSequence, zInRad, ZAxis, yInRad, YAxis ); }
981 
982  // Two-angle body fixed rotations (reversed space-fixed ones).
983  static Rotation_ aboutXThenNewY(const RealP& xInRad, const RealP& yInRad) { return Rotation_( BodyRotationSequence, xInRad, XAxis, yInRad, YAxis ); }
984  static Rotation_ aboutYThenNewX(const RealP& yInRad, const RealP& xInRad) { return aboutXThenOldY(xInRad, yInRad); }
985  static Rotation_ aboutXThenNewZ(const RealP& xInRad, const RealP& zInRad) { return aboutZThenOldX(zInRad, xInRad); }
986  static Rotation_ aboutZThenNewX(const RealP& zInRad, const RealP& xInRad) { return aboutXThenOldZ(xInRad, zInRad); }
987  static Rotation_ aboutYThenNewZ(const RealP& yInRad, const RealP& zInRad) { return aboutZThenOldY(zInRad, yInRad); }
988  static Rotation_ aboutZThenNewY(const RealP& zInRad, const RealP& yInRad) { return aboutYThenOldZ(yInRad, zInRad); }
989 
992  explicit Rotation_( const UnitVec3P& uvecZ ) { setRotationFromOneAxis(uvecZ,ZAxis); }
993 
995  // We will take x seriously after normalizing, but use the y only to create z = normalize(x X y),
996  // then y = z X x. Bad things happen if x and y are aligned but we may not catch it.
997  Rotation_( const Vec3P& x, const Vec3P& yish ) { setRotationFromTwoAxes( UnitVec3P(x), XAxis, yish, YAxis ); }
998 
1000  void setToQuaternion( const QuaternionP& q ) { setRotationFromQuaternion(q); }
1001 
1006  // Similarly for BodyFixed123.
1007  void setToBodyFixed321( const Vec3P& v) { setRotationFromThreeAnglesThreeAxes( BodyRotationSequence, v[0], ZAxis, v[1], YAxis, v[2], XAxis ); }
1008  void setToBodyFixed123( const Vec3P& v) { setRotationToBodyFixedXYZ(v); }
1009 
1012  Vec4P convertToAngleAxis() const { return convertRotationToAngleAxis(); }
1013 
1015  QuaternionP convertToQuaternion() const { return convertRotationToQuaternion(); }
1016 
1019  void setToSpaceFixed12( const Vec2P& q ) { setRotationFromTwoAnglesTwoAxes( SpaceRotationSequence, q[0], XAxis, q[1], YAxis ); }
1020 
1024  Vec3P convertToBodyFixed123() const { return convertRotationToBodyFixedXYZ(); }
1025  Vec2P convertToBodyFixed12() const { return convertRotationToBodyFixedXY(); }
1026  Vec2P convertToSpaceFixed12() const { return convertTwoAxesRotationToTwoAngles( SpaceRotationSequence, XAxis, YAxis ); }
1027 };
1028 
1029 
1034 template <class P>
1035 class InverseRotation_ : public Mat<3,3,P>::TransposeType {
1036  typedef P RealP;
1037  typedef Rotation_<P> RotationP;
1038  typedef Mat<3,3,P> Mat33P; // not the base type!
1039  typedef SymMat<3,P> SymMat33P;
1040  typedef Mat<2,2,P> Mat22P;
1041  typedef Mat<3,2,P> Mat32P;
1042  typedef Vec<2,P> Vec2P;
1043  typedef Vec<3,P> Vec3P;
1044  typedef Vec<4,P> Vec4P;
1045  typedef Quaternion_<P> QuaternionP;
1046 public:
1049  typedef typename Mat<3,3,P>::TransposeType BaseMat;
1050 
1053 
1054 
1055  typedef UnitVec<P,BaseMat::RowSpacing> ColType;
1057  typedef UnitRow<P,BaseMat::ColSpacing> RowType;
1059 
1063  InverseRotation_() : BaseMat(1) {}
1064 
1066  InverseRotation_( const InverseRotation_& R ) : BaseMat(R) {}
1068  InverseRotation_& operator=( const InverseRotation_& R )
1069  { BaseMat::operator=(R.asMat33()); return *this; }
1070 
1077  SimTK_SimTKCOMMON_EXPORT SymMat33P reexpressSymMat33(const SymMat33P& S_BB) const;
1078 
1080 
1081  const RotationP& invert() const {return *reinterpret_cast<const RotationP*>(this);}
1082  RotationP& updInvert() {return *reinterpret_cast<RotationP*>(this);}
1084 
1087 
1088  const RotationP& transpose() const { return invert(); }
1089  const RotationP& operator~() const { return invert(); }
1090  RotationP& updTranspose() { return updInvert(); }
1091  RotationP& operator~() { return updInvert(); }
1093 
1099 
1100  const RowType& row( int i ) const { return reinterpret_cast<const RowType&>(asMat33()[i]); }
1101  const ColType& col( int j ) const { return reinterpret_cast<const ColType&>(asMat33()(j)); }
1102  const ColType& x() const { return col(0); }
1103  const ColType& y() const { return col(1); }
1104  const ColType& z() const { return col(2); }
1105  const RowType& operator[]( int i ) const { return row(i); }
1106  const ColType& operator()( int j ) const { return col(j); }
1108 
1111 
1112  const BaseMat& asMat33() const { return *static_cast<const BaseMat*>(this); }
1113  BaseMat toMat33() const { return asMat33(); }
1115 };
1116 
1118 template <class P> SimTK_SimTKCOMMON_EXPORT std::ostream&
1119 operator<<(std::ostream&, const Rotation_<P>&);
1121 template <class P> SimTK_SimTKCOMMON_EXPORT std::ostream&
1122 operator<<(std::ostream&, const InverseRotation_<P>&);
1123 
1127 
1128 template <class P, int S> inline UnitVec<P,1>
1129 operator*(const Rotation_<P>& R, const UnitVec<P,S>& v) {return UnitVec<P,1>(R.asMat33()* v.asVec3(), true);}
1130 template <class P, int S> inline UnitRow<P,1>
1131 operator*(const UnitRow<P,S>& r, const Rotation_<P>& R) {return UnitRow<P,1>(r.asRow3() * R.asMat33(), true);}
1132 template <class P, int S> inline UnitVec<P,1>
1133 operator*(const InverseRotation_<P>& R, const UnitVec<P,S>& v) {return UnitVec<P,1>(R.asMat33()* v.asVec3(), true);}
1134 template <class P, int S> inline UnitRow<P,1>
1135 operator*(const UnitRow<P,S>& r, const InverseRotation_<P>& R) {return UnitRow<P,1>(r.asRow3() * R.asMat33(), true);}
1137 
1138 // Couldn't implement these Rotation_ methods until InverseRotation_ was defined.
1139 template <class P> inline
1140 Rotation_<P>::Rotation_(const InverseRotation_<P>& R) : Mat<3,3,P>( R.asMat33() ) {}
1141 template <class P> inline Rotation_<P>&
1142 Rotation_<P>::operator=(const InverseRotation_<P>& R) {static_cast<Mat<3,3,P>&>(*this) = R.asMat33(); return *this;}
1143 template <class P> inline Rotation_<P>&
1144 Rotation_<P>::operator*=(const Rotation_<P>& R) {static_cast<Mat<3,3,P>&>(*this) *= R.asMat33(); return *this;}
1145 template <class P> inline Rotation_<P>&
1146 Rotation_<P>::operator/=(const Rotation_<P>& R) {static_cast<Mat<3,3,P>&>(*this) *= (~R).asMat33(); return *this;}
1147 template <class P> inline Rotation_<P>&
1148 Rotation_<P>::operator*=(const InverseRotation_<P>& R) {static_cast<Mat<3,3,P>&>(*this) *= R.asMat33(); return *this;}
1149 template <class P> inline Rotation_<P>&
1150 Rotation_<P>::operator/=(const InverseRotation_<P>& R) {static_cast<Mat<3,3,P>&>(*this) *= (~R).asMat33(); return *this;}
1151 
1153 
1154 template <class P> inline Rotation_<P>
1155 operator*(const Rotation_<P>& R1, const Rotation_<P>& R2) {return Rotation_<P>(R1) *= R2;}
1156 template <class P> inline Rotation_<P>
1157 operator*(const Rotation_<P>& R1, const InverseRotation_<P>& R2) {return Rotation_<P>(R1) *= R2;}
1158 template <class P> inline Rotation_<P>
1159 operator*(const InverseRotation_<P>& R1, const Rotation_<P>& R2) {return Rotation_<P>(R1) *= R2;}
1160 template <class P> inline Rotation_<P>
1161 operator*(const InverseRotation_<P>& R1, const InverseRotation_<P>& R2) {return Rotation_<P>(R1) *= R2;}
1163 
1166 
1167 template <class P> inline Rotation_<P>
1168 operator/( const Rotation_<P>& R1, const Rotation_<P>& R2 ) {return Rotation_<P>(R1) /= R2;}
1169 template <class P> inline Rotation_<P>
1170 operator/( const Rotation_<P>& R1, const InverseRotation& R2 ) {return Rotation_<P>(R1) /= R2;}
1171 template <class P> inline Rotation_<P>
1172 operator/( const InverseRotation_<P>& R1, const Rotation_<P>& R2 ) {return Rotation_<P>(R1) /= R2;}
1173 template <class P> inline Rotation_<P>
1174 operator/( const InverseRotation_<P>& R1, const InverseRotation_<P>& R2 ) {return Rotation_<P>(R1) /= R2;}
1176 
1177 
1178 //------------------------------------------------------------------------------
1179 } // End of namespace SimTK
1180 
1181 //--------------------------------------------------------------------------
1182 #endif // SimTK_SimTKCOMMON_ROTATION_H_
1183 //--------------------------------------------------------------------------
1184 
1185