Simbody  3.4
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Modules 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 
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 //------------------------------------------------------------------------------
107 //------------------------------------------------------------------------------
108 template <class P> // templatized by precision
109 class Rotation_ : public Mat<3,3,P> {
110 public:
111  typedef P RealP;
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
123 
124  // Default constructor and constructor-like methods
125  Rotation_() : Mat33P(1) {}
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 
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 
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 
180 
182  explicit Rotation_( const QuaternionP& q ) { setRotationFromQuaternion(q); }
185 
187  Rotation_( const Mat33P& m, bool ) : Mat33P(m) {}
188 
190  explicit Rotation_( const Mat33P& m ) { setRotationFromApproximateMat33(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 
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;
232 
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;
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 ; }
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(); }
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 
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 
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.
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.
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.
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 
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 
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.
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)
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.
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.
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 
735  (const Vec3P& q, const Vec3P& w_PB_B) {
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
749  (const Vec3P& cq, const Vec3P& sq, const Vec3P& w_PB_B)
750  { return calcNForBodyXYZInBodyFrame(cq,sq)*w_PB_B; }
751 
758  (const Vec3P& q, const Vec3P& qdot) {
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
772  (const Vec3P& cq, const Vec3P& sq, const Vec3P& qdot)
773  { return calcNInvForBodyXYZInBodyFrame(cq,sq)*qdot; }
774 
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.
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
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
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 
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:
1050 
1053 
1059 
1063  InverseRotation_() : BaseMat(1) {}
1064 
1066  InverseRotation_( const InverseRotation_& R ) : BaseMat(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 
Vec2P convertTwoAxesRotationToTwoAngles(BodyOrSpaceType bodyOrSpace, const CoordinateAxis &axis1, const CoordinateAxis &axis2) const
Converts rotation matrix to two orientation angles.
static Vec3P convertBodyXYZDotToAngVelInBodyFrame(const Vec3P &q, const Vec3P &qdot)
Inverse of the above routine.
Definition: Rotation.h:758
bool areAllRotationElementsSameToMachinePrecision(const Rotation_ &R) const
Definition: Rotation.h:263
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_ & setRotationFromAngleAboutY(RealP angle)
Set this Rotation_ object to a right-handed rotation by an angle (in radians) about a coordinate axis...
Definition: Rotation.h:144
BaseMat toMat33() const
Conversion from InverseRotation_ to BaseMat.
Definition: Rotation.h:1113
InverseRotation_< P > & updInvert()
Convert from Rotation_ to writable InverseRotation_ (no cost).
Definition: Rotation.h:274
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...
#define SimTK_SimTKCOMMON_EXPORT
Definition: SimTKcommon/include/SimTKcommon/internal/common.h:202
Rotation_(RealP angle, const CoordinateAxis &axis)
Constructor for right-handed rotation by an angle (in radians) about a coordinate axis...
Definition: Rotation.h:135
const RowType & row(int i) const
Definition: Rotation.h:303
const ColType & z() const
Access individual rows and columns of this InverseRotation; no cost or copying since suitably-cast re...
Definition: Rotation.h:1104
InverseRotation_< double > dInverseRotation
Definition: Rotation.h:55
bool areAllRotationElementsSameToEpsilon(const Rotation_ &R, RealP epsilon) const
Definition: Rotation.h:261
Vec3P convertRotationToBodyFixedXYZ() const
A convenient special case of convertThreeAxesRotationToThreeAngles().
Definition: Rotation.h:236
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:1101
UnitRow< P, Mat33P::ColSpacing > RowType
Definition: Rotation.h:302
const BaseMat & asMat33() const
Conversion from InverseRotation_ to BaseMat.
Definition: Rotation.h:1112
Defines the CoordinateAxis and CoordinateDirection classes.
ScalarNormSq normSqr() const
Definition: Vec.h:606
Rotation_(const Rotation_ &R)
Definition: Rotation.h:130
Rotation_(const Mat33P &m, bool)
Construct a Rotation_ directly from a Mat33P (we trust that m is a valid Rotation_!) ...
Definition: Rotation.h:187
Rotation_ & setRotationFromAngleAboutX(RealP cosAngle, RealP sinAngle)
Set this Rotation_ object to a right-handed rotation by an angle (in radians) about a coordinate axis...
Definition: Rotation.h:146
Rotation_ & setRotationFromMat33TrustMe(const Mat33P &m)
Set the Rotation_ matrix directly - but you had better know what you are doing!
Definition: Rotation.h:313
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:513
Rotation_(const QuaternionP &q)
Constructor for creating a rotation matrix from a quaternion.
Definition: Rotation.h:182
InverseRotation_(const InverseRotation_ &R)
This is an explicit implementation of the default copy constructor.
Definition: Rotation.h:1066
Rotation_ & setRotationFromAngleAboutUnitVector(RealP angle, const UnitVec3P &unitVector)
Set this Rotation_ object to a right-handed rotation of an angle (in radians) about an arbitrary vect...
Rotation_ & setRotationToNaN()
Definition: Rotation.h:127
Vec< 2, P > Vec2P
Definition: Rotation.h:117
RotationP & updTranspose()
Transpose, and transpose operators (override BaseMat versions of transpose).
Definition: Rotation.h:1090
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:347
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...
This is the top-level SimTK namespace into which all SimTK names are placed to avoid collision with o...
Definition: Assembler.h:37
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:142
const Mat33P & asMat33() const
Conversion from Rotation to its base class Mat33.
Definition: Rotation.h:296
Rotation_< double > dRotation
Definition: Rotation.h:51
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:332
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:556
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:1049
Rotation_ & setRotationFromUnitVecsTrustMe(const UnitVec3P &colA, const UnitVec3P &colB, const UnitVec3P &colC)
Set the Rotation_ matrix directly - but you had better know what you are doing!
Definition: Rotation.h:317
InverseRotation_< P > & operator~()
Transpose, and transpose operators.
Definition: Rotation.h:282
Definition: CoordinateAxis.h:196
Definition: CoordinateAxis.h:199
Mat< 2, 2, P > Mat22P
Definition: Rotation.h:112
const ColType & y() const
Definition: Rotation.h:306
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:1100
A Quaternion is a Vec4 with the following behavior:
Definition: Quaternion.h:41
Rotation_ & setRotationFromTwoAxes(const UnitVec3P &uveci, const CoordinateAxis &axisi, const Vec3P &vecjApprox, const CoordinateAxis &axisjApprox)
Calculate R_AB by knowing one of B's unit vectors u1 (could be Bx, By, or Bz) expressed in A and a ve...
InverseRotation_ & operator=(const InverseRotation_ &R)
This is an explicit implementation of the default copy assignment operator.
Definition: Rotation.h:1068
Rotation_ & setRotationColFromUnitVecTrustMe(int colj, const UnitVec3P &uvecj)
Set the Rotation_ matrix directly - but you had better know what you are doing!
Definition: Rotation.h:315
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:423
Definition: CoordinateAxis.h:202
Definition: Rotation.h:42
Rotation_()
Definition: Rotation.h:125
Rotation_ & setRotationFromAngleAboutZ(RealP angle)
Set this Rotation_ object to a right-handed rotation by an angle (in radians) about a coordinate axis...
Definition: Rotation.h:145
const ColType & x() const
Definition: Rotation.h:305
Vec3P convertThreeAxesRotationToThreeAngles(BodyOrSpaceType bodyOrSpace, const CoordinateAxis &axis1, const CoordinateAxis &axis2, const CoordinateAxis &axis3) const
Converts rotation matrix to three orientation angles.
const CoordinateAxis::ZCoordinateAxis ZAxis
Constant representing the Z coordinate axis; will implicitly convert to the integer 2 when used in a ...
const ColType & x() const
Access individual rows and columns of this InverseRotation; no cost or copying since suitably-cast re...
Definition: Rotation.h:1102
This class, along with its sister class CoordinateDirection, provides convenient manipulation of the ...
Definition: CoordinateAxis.h:53
Vec< 4, P > Vec4P
Definition: Rotation.h:119
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:828
const BaseRow & asRow3() const
Return a const reference to the Row3 underlying this UnitRow.
Definition: UnitVec.h:258
UnitRow< P, BaseMat::ColSpacing > RowType
This is the type of a row of this InverseRotation.
Definition: Rotation.h:1057
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:158
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:595
Vec< 3, P > Vec3P
Definition: Rotation.h:118
const ColType & col(int j) const
Definition: Rotation.h:304
Declares and defines the UnitVec and UnitRow classes.
const RotationP & invert() const
We can invert an InverseRotation just by recasting it to a Rotation at zero cost. ...
Definition: Rotation.h:1081
const CoordinateAxis::YCoordinateAxis YAxis
Constant representing the Y coordinate axis; will implicitly convert to the integer 1 when used in a ...
P RealP
These are just local abbreviations.
Definition: Rotation.h:111
Mat< 3, 4, P > Mat34P
Definition: Rotation.h:116
const RotationP & operator~() const
Transpose, and transpose operators (override BaseMat versions of transpose).
Definition: Rotation.h:1089
Rotation_< Real > Rotation
Definition: Rotation.h:47
This type is used for the transpose of UnitVec, and as the returned row type of a Rotation...
Definition: UnitVec.h:41
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:477
Vec4P convertRotationToAngleAxis() const
Converts rotation matrix to angle-axis form.
Definition: Rotation.h:231
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:735
bool isYAxis() const
Return true if this is the Y axis.
Definition: CoordinateAxis.h:92
void setRotationToBodyFixedXY(const Vec2P &v)
Set this Rotation_ to represent a rotation characterized by subsequent rotations of: +v[0] about the ...
Definition: Rotation.h:174
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
Rotation_(const Mat33P &m)
Constructs an (hopefully nearby) orthogonal rotation matrix from a generic Mat33P.
Definition: Rotation.h:190
Mat< 3, 3, P > Mat33P
Definition: Rotation.h:114
UnitVec< P, Mat33P::RowSpacing > ColType
The column and row unit vector types do not necessarily have unit spacing.
Definition: Rotation.h:301
InverseRotation_()
You should not ever construct one of these as they should only occur as expression intermediates resu...
Definition: Rotation.h:1063
Matrix_< E > operator*(const MatrixBase< E > &l, const typename CNT< E >::StdNumber &r)
Definition: BigMatrix.h:605
static Vec3P convertAngVelDotToBodyFixed321DotDot(const Vec3P &q, const Vec3P &w_PB_B, const Vec3P &wdot_PB_B)
Definition: Rotation.h:376
Definition: Rotation.h:42
Rotation_(RealP angle, const UnitVec3P &unitVector)
Constructor for right-handed rotation by an angle (in radians) about an arbitrary vector...
Definition: Rotation.h:153
SymMat33P reexpressSymMat33(const SymMat33P &S_BB) const
Assuming this InverseRotation_ is R_AB, and given a symmetric dyadic matrix S_BB expressed in B...
This class is a Vec3 plus an ironclad guarantee either that:
Definition: UnitVec.h:40
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:1106
static Mat33P calcNInvForBodyXYZInBodyFrame(const Vec3P &q)
Inverse of routine calcNForBodyXYZInBodyFrame().
Definition: Rotation.h:673
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:410
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:636
UnitVec< P, 1 > UnitVec3P
Definition: Rotation.h:120
Rotation_(const UnitVec3P &uveci, const CoordinateAxis &axisi, const Vec3P &vecjApprox, const CoordinateAxis &axisjApprox)
Calculate R_AB by knowing one of B's unit vectors u1 (could be Bx, By, or Bz) expressed in A and a ve...
Definition: Rotation.h:208
----------------------------------------------------------------------------- This InverseRotation cl...
Definition: Rotation.h:47
RotationP & operator~()
Transpose, and transpose operators (override BaseMat versions of transpose).
Definition: Rotation.h:1091
bool isSameRotationToWithinAngle(const Rotation_ &R, RealP okPointingAngleErrorRads) const
Return true if "this" Rotation is nearly identical to "R" within a specified pointing angle error...
static Mat33P calcNForBodyXYZInParentFrame(const Vec3P &cq, const Vec3P &sq)
This faster version of calcNForBodyXYZInParentFrame() assumes you have already calculated the cosine ...
Definition: Rotation.h:460
const ColType & operator()(int j) const
Definition: Rotation.h:309
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:858
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:533
This file is the user-includeable header to be included in user programs to provide fixed-length Vec ...
Rotation_(const UnitVec3P &uvec, const CoordinateAxis axis)
Calculate R_AB by knowing one of B's unit vector expressed in A.
Definition: Rotation.h:197
Rotation_(RealP angle, const CoordinateAxis::YCoordinateAxis)
Constructor for right-handed rotation by an angle (in radians) about a coordinate axis...
Definition: Rotation.h:137
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:1055
BodyOrSpaceType
Definition: Rotation.h:42
const ColType & z() const
Definition: Rotation.h:307
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:845
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:163
const RotationP & transpose() const
Transpose, and transpose operators (override BaseMat versions of transpose).
Definition: Rotation.h:1088
Vec2P convertRotationToBodyFixedXY() const
A convenient special case of convertTwoAxesRotationToTwoAngles().
Definition: Rotation.h:234
bool isXAxis() const
Return true if this is the X axis.
Definition: CoordinateAxis.h:90
QuaternionP convertRotationToQuaternion() const
Converts rotation matrix to a quaternion.
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:493
Rotation_ & setRotationFromQuaternion(const QuaternionP &q)
Method for creating a rotation matrix from a quaternion.
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:892
InverseRotation_< float > fInverseRotation
Definition: Rotation.h:54
This class represents a small matrix whose size is known at compile time, containing elements of any ...
Definition: SimTKcommon/include/SimTKcommon/internal/common.h:579
const InverseRotation_< P > & invert() const
Convert from Rotation_ to InverseRotation_ (no cost). Overrides base class invert().
Definition: Rotation.h:272
Rotation_(RealP angle, const CoordinateAxis::ZCoordinateAxis)
Constructor for right-handed rotation by an angle (in radians) about a coordinate axis...
Definition: Rotation.h:138
const BaseVec & asVec3() const
Return a reference to the underlying Vec3 (no copying here).
Definition: UnitVec.h:118
Mat< 3, 2, P > Mat32P
Definition: Rotation.h:113
Rotation_ & setRotationFromAngleAboutZ(RealP cosAngle, RealP sinAngle)
Set this Rotation_ object to a right-handed rotation by an angle (in radians) about a coordinate axis...
Definition: Rotation.h:148
Rotation_ & setRotationFromAngleAboutY(RealP cosAngle, RealP sinAngle)
Set this Rotation_ object to a right-handed rotation by an angle (in radians) about a coordinate axis...
Definition: Rotation.h:147
Mat< 4, 3, P > Mat43P
Definition: Rotation.h:115
const InverseRotation_< P > & transpose() const
Transpose, and transpose operators.
Definition: Rotation.h:279
void setRotationToBodyFixedXYZ(const Vec3P &v)
Set this Rotation_ to represent a rotation characterized by subsequent rotations of: +v[0] about the ...
Definition: Rotation.h:179
Rotation_ & setRotationToIdentityMatrix()
Definition: Rotation.h:126
RealP convertOneAxisRotationToOneAngle(const CoordinateAxis &axis1) const
Converts rotation matrix to a single orientation angle.
const RowType & operator[](int i) const
Definition: Rotation.h:308
Rotation_ & operator/=(const Rotation_< P > &R)
In-place composition of Rotation matrices.
Definition: Rotation.h:1146
This is a fixed-length column vector designed for no-overhead inline computation. ...
Definition: SimTKcommon/include/SimTKcommon/internal/common.h:577
Rotation_< float > fRotation
Definition: Rotation.h:50
Rotation_(RealP angle, const Vec3P &nonUnitVector)
Constructor for right-handed rotation by an angle (in radians) about an arbitrary vector...
Definition: Rotation.h:154
Rotation_ & setRotationFromAngleAboutX(RealP angle)
Set this Rotation_ object to a right-handed rotation by an angle (in radians) about a coordinate axis...
Definition: Rotation.h:143
static Vec4P OLDconvertAngVelDotToQuaternionDotDot(const Vec4P &q, const Vec3P &w_PB_P, const Vec3P &wdot_PB_P)
Given a quaternion q representing R_PB, angular velocity of B in P, and the time derivative of the an...
Definition: Rotation.h:876
RotationP & updInvert()
We can invert an InverseRotation just by recasting it to a Rotation at zero cost. ...
Definition: Rotation.h:1082
void setToNaN()
Definition: Mat.h:902
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:165
const ColType & y() const
Access individual rows and columns of this InverseRotation; no cost or copying since suitably-cast re...
Definition: Rotation.h:1103
RealP getMaxAbsDifferenceInRotationElements(const Rotation_ &R) const
Definition: Rotation.h:253
InverseRotation_< P > & updTranspose()
Transpose, and transpose operators.
Definition: Rotation.h:281
SymMat< 3, P > SymMat33P
Definition: Rotation.h:121
Vec4P convertQuaternionToAngleAxis() const
Returns [ a vx vy vz ] with (a,v) in canonical form, i.e., -180 < a <= 180 and |v|=1.
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:446
static Vec3P convertQuaternionDotToAngVel(const Vec4P &q, const Vec4P &qdot)
Inverse of the above routine.
Definition: Rotation.h:865
static Vec3P convertBodyFixed321DotToAngVel(const Vec3P &q, const Vec3P &qd)
Inverse of the above routine.
Definition: Rotation.h:361
Rotation_ & setRotationFromApproximateMat33(const Mat33P &m)
Set this Rotation_ object to an (hopefully nearby) orthogonal rotation matrix from a generic Mat33P...
static Vec3P convertAngVelDotInBodyFrameToBodyXYZDotDot(const Vec3P &q, const Vec3P &w_PB_B, const Vec3P &wdot_PB_B)
TODO: sherm: is this right? Warning: everything is measured in the *PARENT* frame, but has to be expressed in the *BODY* frame.
Definition: Rotation.h:781
bool isSameRotationToWithinAngleOfMachinePrecision(const Rotation_ &R) const
Return true if "this" Rotation is nearly identical to "R" within a specified pointing angle error...
Definition: Rotation.h:250
const InverseRotation_< P > & operator~() const
Transpose, and transpose operators.
Definition: Rotation.h:280
Quaternion_< P > QuaternionP
Definition: Rotation.h:122
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:813
Mat33P toMat33() const
Conversion from Rotation to its base class Mat33.
Definition: Rotation.h:297
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:1105
Rotation_(RealP angle, const CoordinateAxis::XCoordinateAxis)
Constructor for right-handed rotation by an angle (in radians) about a coordinate axis...
Definition: Rotation.h:136
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:572
Rotation_ & operator=(const Rotation_ &R)
Definition: Rotation.h:131
Definition: negator.h:64
Rotation_ & operator*=(const Rotation_< P > &R)
In-place composition of Rotation matrices.
Definition: Rotation.h:1144
Rotation_ & setRotationFromOneAxis(const UnitVec3P &uvec, const CoordinateAxis axis)
Calculate R_AB by knowing one of B's unit vector expressed in A.
static Mat33P calcNInvForBodyXYZInParentFrame(const Vec3P &q)
Inverse of the above routine.
Definition: Rotation.h:703
Rotation_ & setRotationFromThreeAnglesThreeAxes(BodyOrSpaceType bodyOrSpace, RealP angle1, const CoordinateAxis &axis1, RealP angle2, const CoordinateAxis &axis2, RealP angle3, const CoordinateAxis &axis3)
Set this Rotation_ object to a three-angle Body-fixed or Space-fixed rotation sequences (angles are i...
const CoordinateAxis::XCoordinateAxis XAxis
Constant representing the X coordinate axis; will implicitly convert to the integer 0 when used in a ...