Simbody  3.5
Rotation.h
Go to the documentation of this file.
00001 #ifndef SimTK_SimTKCOMMON_ROTATION_H_ 
00002 #define SimTK_SimTKCOMMON_ROTATION_H_ 
00003 
00004 /* -------------------------------------------------------------------------- *
00005  *                       Simbody(tm): SimTKcommon                             *
00006  * -------------------------------------------------------------------------- *
00007  * This is part of the SimTK biosimulation toolkit originating from           *
00008  * Simbios, the NIH National Center for Physics-Based Simulation of           *
00009  * Biological Structures at Stanford, funded under the NIH Roadmap for        *
00010  * Medical Research, grant U54 GM072970. See https://simtk.org/home/simbody.  *
00011  *                                                                            *
00012  * Portions copyright (c) 2005-14 Stanford University and the Authors.        *
00013  * Authors: Paul Mitiguy, Michael Sherman                                     *
00014  * Contributors:                                                              *
00015  *                                                                            *
00016  * Licensed under the Apache License, Version 2.0 (the "License"); you may    *
00017  * not use this file except in compliance with the License. You may obtain a  *
00018  * copy of the License at http://www.apache.org/licenses/LICENSE-2.0.         *
00019  *                                                                            *
00020  * Unless required by applicable law or agreed to in writing, software        *
00021  * distributed under the License is distributed on an "AS IS" BASIS,          *
00022  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.   *
00023  * See the License for the specific language governing permissions and        *
00024  * limitations under the License.                                             *
00025  * -------------------------------------------------------------------------- */
00026 
00027 //------------------------------------------------------------------------------
00028 
00029 #include "SimTKcommon/SmallMatrix.h"
00030 #include "SimTKcommon/internal/CoordinateAxis.h"
00031 #include "SimTKcommon/internal/UnitVec.h"
00032 #include "SimTKcommon/internal/Quaternion.h"
00033 
00034 //------------------------------------------------------------------------------
00035 #include <iosfwd>  // Forward declaration of iostream
00036 //------------------------------------------------------------------------------
00037 
00038 //------------------------------------------------------------------------------
00039 namespace SimTK {
00040 
00041 
00042 enum BodyOrSpaceType { BodyRotationSequence=0, SpaceRotationSequence=1 };
00043 
00044 //------------------------------------------------------------------------------
00045 // Forward declarations
00046 template <class P> class Rotation_;
00047 template <class P> class InverseRotation_;
00048 
00049 typedef Rotation_<Real>             Rotation;
00050 typedef Rotation_<float>           fRotation;
00051 typedef Rotation_<double>          dRotation;
00052 
00053 typedef InverseRotation_<Real>      InverseRotation;
00054 typedef InverseRotation_<float>    fInverseRotation;
00055 typedef InverseRotation_<double>   dInverseRotation;
00056 
00057 //------------------------------------------------------------------------------
00109 //------------------------------------------------------------------------------
00110 template <class P> // templatized by precision
00111 class Rotation_ : public Mat<3,3,P> {
00112 public:
00113 typedef P               RealP; 
00114 typedef Mat<2,2,P>      Mat22P;
00115 typedef Mat<3,2,P>      Mat32P;
00116 typedef Mat<3,3,P>      Mat33P;
00117 typedef Mat<4,3,P>      Mat43P;
00118 typedef Mat<3,4,P>      Mat34P;
00119 typedef Vec<2,P>        Vec2P;
00120 typedef Vec<3,P>        Vec3P;
00121 typedef Vec<4,P>        Vec4P;
00122 typedef UnitVec<P,1>    UnitVec3P; // stride is 1 here, length is always 3
00123 typedef SymMat<3,P>     SymMat33P;
00124 typedef Quaternion_<P>  QuaternionP;
00125 
00128 typedef UnitVec<P,Mat33P::RowSpacing> ColType;
00131 typedef UnitRow<P,Mat33P::ColSpacing> RowType;
00132 
00133 //------------------------------------------------------------------------------
00137 Rotation_() : Mat33P(1) {}    
00138 
00140 Rotation_( const Rotation_& R ) : Mat33P(R)  {}
00143 inline Rotation_( const InverseRotation_<P>& );
00144 
00146 Rotation_&  operator=( const Rotation_& R )  
00147 { Mat33P::operator=( R.asMat33() );  return *this; }
00149 inline Rotation_& operator=( const InverseRotation_<P>& );
00150 
00152 Rotation_&  setRotationToNaN()             
00153 { Mat33P::setToNaN();    return *this; } 
00154 
00156 Rotation_&  setRotationToIdentityMatrix()  
00157 { Mat33P::operator=(RealP(1));  return *this; }
00158 
00161 Rotation_( RealP angle, const CoordinateAxis& axis )             
00162 { setRotationFromAngleAboutAxis( angle, axis ); }
00165 Rotation_& setRotationFromAngleAboutAxis(RealP angle, const CoordinateAxis& axis)  
00166 { return axis.isXAxis() ? setRotationFromAngleAboutX(angle) 
00167       : (axis.isYAxis() ? setRotationFromAngleAboutY(angle) 
00168       : setRotationFromAngleAboutZ(angle) ); }
00169 
00172 Rotation_( RealP angle, const CoordinateAxis::XCoordinateAxis )  
00173 { setRotationFromAngleAboutX( std::cos(angle), std::sin(angle) ); }
00176 Rotation_&  setRotationFromAngleAboutX( RealP angle )  
00177 { return setRotationFromAngleAboutX( std::cos(angle), std::sin(angle) ); }
00180 Rotation_&  setRotationFromAngleAboutX( RealP cosAngle, RealP sinAngle )  
00181 { Mat33P& R = *this;  R[0][0] = 1;   R[0][1] = R[0][2] = R[1][0] = R[2][0] = 0;   
00182   R[1][1] = R[2][2] = cosAngle;  R[1][2] = -(R[2][1] = sinAngle);  
00183   return *this; }
00184 
00187 Rotation_( RealP angle, const CoordinateAxis::YCoordinateAxis )  
00188 { setRotationFromAngleAboutY( std::cos(angle), std::sin(angle) ); }
00191 Rotation_&  setRotationFromAngleAboutY( RealP angle )  
00192 { return setRotationFromAngleAboutY( std::cos(angle), std::sin(angle) ); }
00195 Rotation_&  setRotationFromAngleAboutY( RealP cosAngle, RealP sinAngle )  
00196 { Mat33P& R = *this;  R[1][1] = 1;   R[0][1] = R[1][0] = R[1][2] = R[2][1] = 0;   
00197   R[0][0] = R[2][2] = cosAngle;  R[2][0] = -(R[0][2] = sinAngle);  
00198   return *this; }
00199 
00202 Rotation_( RealP angle, const CoordinateAxis::ZCoordinateAxis )  
00203 { setRotationFromAngleAboutZ( std::cos(angle), std::sin(angle) ); }
00206 Rotation_&  setRotationFromAngleAboutZ( RealP angle )  
00207 { return setRotationFromAngleAboutZ( std::cos(angle), std::sin(angle) ); }
00210 Rotation_&  setRotationFromAngleAboutZ( RealP cosAngle, RealP sinAngle )  
00211 { Mat33P& R = *this;  R[2][2] = 1;   R[0][2] = R[1][2] = R[2][0] = R[2][1] = 0;   
00212   R[0][0] = R[1][1] = cosAngle;  R[0][1] = -(R[1][0] = sinAngle);  
00213   return *this; }
00214 
00217 Rotation_( RealP angle, const UnitVec3P& unitVector ) 
00218 { setRotationFromAngleAboutUnitVector(angle,unitVector); }
00221 SimTK_SimTKCOMMON_EXPORT Rotation_&  
00222 setRotationFromAngleAboutUnitVector(RealP angle, const UnitVec3P& unitVector);
00223 
00226 Rotation_( RealP angle, const Vec3P& nonUnitVector )  
00227 { setRotationFromAngleAboutNonUnitVector(angle,nonUnitVector); }
00230 Rotation_&  
00231 setRotationFromAngleAboutNonUnitVector(RealP angle, const Vec3P& nonUnitVector)  
00232 { return setRotationFromAngleAboutUnitVector( angle, UnitVec3P(nonUnitVector) ); }
00233 
00236 Rotation_(BodyOrSpaceType bodyOrSpace, 
00237           RealP angle1, const CoordinateAxis& axis1, 
00238           RealP angle2, const CoordinateAxis& axis2)
00239 { setRotationFromTwoAnglesTwoAxes(bodyOrSpace,angle1,axis1,angle2,axis2); }
00242 SimTK_SimTKCOMMON_EXPORT Rotation_&  
00243 setRotationFromTwoAnglesTwoAxes(BodyOrSpaceType bodyOrSpace, 
00244                                 RealP angle1, const CoordinateAxis& axis1, 
00245                                 RealP angle2, const CoordinateAxis& axis2); 
00246 
00249 Rotation_(BodyOrSpaceType bodyOrSpace, 
00250           RealP angle1, const CoordinateAxis& axis1, 
00251           RealP angle2, const CoordinateAxis& axis2, 
00252           RealP angle3, const CoordinateAxis& axis3 )  
00253 { setRotationFromThreeAnglesThreeAxes
00254    (bodyOrSpace,angle1,axis1,angle2,axis2,angle3,axis3); }
00257 SimTK_SimTKCOMMON_EXPORT Rotation_&  
00258 setRotationFromThreeAnglesThreeAxes(BodyOrSpaceType bodyOrSpace, 
00259                                     RealP angle1, const CoordinateAxis& axis1, 
00260                                     RealP angle2, const CoordinateAxis& axis2, 
00261                                     RealP angle3, const CoordinateAxis& axis3);
00262 
00264 explicit Rotation_( const QuaternionP& q )  { setRotationFromQuaternion(q); }
00266 SimTK_SimTKCOMMON_EXPORT Rotation_&  
00267 setRotationFromQuaternion( const QuaternionP& q );
00268 
00271 explicit Rotation_( const Mat33P& m )  { setRotationFromApproximateMat33(m); }
00274 SimTK_SimTKCOMMON_EXPORT Rotation_&  
00275 setRotationFromApproximateMat33( const Mat33P& m );
00276 
00279 Rotation_(const Mat33P& m, bool) : Mat33P(m) {}
00282 Rotation_& setRotationFromMat33TrustMe(const Mat33P& m)  
00283 { Mat33P& R = *this; R=m;  return *this; }   
00286 Rotation_& setRotationColFromUnitVecTrustMe(int colj, const UnitVec3P& uvecj)  
00287 { Mat33P& R = *this; R(colj)=uvecj.asVec3(); return *this; }   
00290 Rotation_& setRotationFromUnitVecsTrustMe
00291    (const UnitVec3P& colA, const UnitVec3P& colB, const UnitVec3P& colC)  
00292 { Mat33P& R = *this; R(0)=colA.asVec3(); R(1)=colB.asVec3(); R(2)=colC.asVec3();
00293   return *this; }  
00294 
00297 Rotation_(const UnitVec3P& uvec, CoordinateAxis axis)  
00298 { setRotationFromOneAxis(uvec,axis); }
00301 SimTK_SimTKCOMMON_EXPORT Rotation_&  
00302 setRotationFromOneAxis(const UnitVec3P& uvec, CoordinateAxis axis);
00303 
00310 Rotation_(const UnitVec3P& uveci, const CoordinateAxis& axisi, 
00311           const Vec3P& vecjApprox, const CoordinateAxis& axisjApprox )  
00312 { setRotationFromTwoAxes(uveci,axisi,vecjApprox,axisjApprox); }
00319 SimTK_SimTKCOMMON_EXPORT Rotation_&  
00320 setRotationFromTwoAxes(const UnitVec3P& uveci, const CoordinateAxis& axisi, 
00321                        const Vec3P& vecjApprox, const CoordinateAxis& axisjApprox );
00322 
00327 void setRotationToBodyFixedXY( const Vec2P& v)   
00328 { setRotationFromTwoAnglesTwoAxes(BodyRotationSequence,
00329                                   v[0],XAxis, v[1],YAxis); }
00330 
00336 void setRotationToBodyFixedXYZ( const Vec3P& v)  
00337 { setRotationFromThreeAnglesThreeAxes(BodyRotationSequence, 
00338                                       v[0],XAxis, v[1],YAxis, v[2],ZAxis ); }
00342 void setRotationToBodyFixedXYZ(const Vec3P& c, const Vec3P& s) {
00343     Mat33P& R = *this;
00344     const RealP s0s1=s[0]*s[1], s2c0=s[2]*c[0], c0c2=c[0]*c[2], nc1= -c[1];
00345 
00346     R = Mat33P(     c[1]*c[2]      ,         s[2]*nc1       ,    s[1]  ,
00347                 s2c0 + s0s1*c[2]   ,     c0c2 - s0s1*s[2]   , s[0]*nc1 ,
00348                 s[0]*s[2] - s[1]*c0c2 ,  s[0]*c[2] + s[1]*s2c0 , c[0]*c[1] );
00349 }
00351 
00352 //------------------------------------------------------------------------------
00357 const InverseRotation_<P>&  operator~() const  { return invert(); }
00360 InverseRotation_<P>&        operator~()        { return updInvert(); }
00361 
00365 const InverseRotation_<P>&  transpose() const  { return invert(); }
00369 InverseRotation_<P>&        updTranspose()     { return updInvert(); }
00370 
00373 const InverseRotation_<P>& invert() const  
00374 { return *reinterpret_cast<const InverseRotation_<P>*>(this); }
00376 InverseRotation_<P>& updInvert()     
00377 { return *reinterpret_cast<InverseRotation_<P>*>(this); }
00378 
00380 inline Rotation_&  operator*=( const Rotation_<P>& R );
00382 inline Rotation_&  operator*=( const InverseRotation_<P>& );
00383 
00385 inline Rotation_&  operator/=( const Rotation_<P>& R );
00387 inline Rotation_&  operator/=( const InverseRotation_<P>& );
00388 
00395 static Vec3P multiplyByBodyXYZ_N_P(const Vec2P& cosxy,
00396                                     const Vec2P& sinxy,
00397                                     RealP        oocosy,
00398                                     const Vec3P& w_PB)
00399 {
00400     const RealP s0 = sinxy[0], c0 = cosxy[0];
00401     const RealP s1 = sinxy[1];
00402     const RealP w0 = w_PB[0], w1 = w_PB[1], w2 = w_PB[2];
00403 
00404     const RealP t = (s0*w1-c0*w2)*oocosy;
00405     return Vec3P( w0 + t*s1, c0*w1 + s0*w2, -t ); // qdot
00406 }
00407 
00411 static Vec3P multiplyByBodyXYZ_NT_P(const Vec2P& cosxy,
00412                                     const Vec2P& sinxy,
00413                                     RealP        oocosy,
00414                                     const Vec3P& q)
00415 {
00416     const RealP s0 = sinxy[0], c0 = cosxy[0];
00417     const RealP s1 = sinxy[1];
00418     const RealP q0 = q[0], q1 = q[1], q2 = q[2];
00419 
00420     const RealP t = (q0*s1-q2) * oocosy;
00421     return Vec3P( q0, c0*q1 + t*s0, s0*q1 - t*c0 ); // v_P
00422 }
00423 
00426 static Vec3P multiplyByBodyXYZ_NInv_P(const Vec2P& cosxy,
00427                                       const Vec2P& sinxy,
00428                                       const Vec3P& qdot)
00429 {
00430     const RealP s0 = sinxy[0], c0 = cosxy[0];
00431     const RealP s1 = sinxy[1], c1 = cosxy[1];
00432     const RealP q0 = qdot[0], q1 = qdot[1], q2 = qdot[2];
00433     const RealP c1q2 = c1*q2;
00434 
00435     return Vec3P( q0 + s1*q2,           // w_PB
00436                   c0*q1 - s0*c1q2, 
00437                   s0*q1 + c0*c1q2 );
00438 }
00439 
00442 static Vec3P multiplyByBodyXYZ_NInvT_P(const Vec2P& cosxy,
00443                                        const Vec2P& sinxy,
00444                                        const Vec3P& v_P)
00445 {
00446     const RealP s0 = sinxy[0], c0 = cosxy[0];
00447     const RealP s1 = sinxy[1], c1 = cosxy[1];
00448     const RealP w0 = v_P[0], w1 = v_P[1], w2 = v_P[2];
00449 
00450     return Vec3P( w0,                           // qdot-like
00451                   c0*w1 + s0*w2,
00452                   s1*w0 - s0*c1*w1 + c0*c1*w2);
00453 }
00455 
00456 //------------------------------------------------------------------------------
00461 const RowType&  row( int i ) const         
00462 { return reinterpret_cast<const RowType&>(asMat33()[i]); }
00464 const RowType&  operator[]( int i ) const  { return row(i); }
00465 
00468 const ColType&  col( int j ) const         
00469 { return reinterpret_cast<const ColType&>(asMat33()(j)); }
00471 const ColType&  operator()( int j ) const  { return col(j); }
00472 
00474 const ColType&  x() const                  { return col(0); }
00476 const ColType&  y() const                  { return col(1); }
00478 const ColType&  z() const                  { return col(2); }
00479 
00484 const ColType& getAxisUnitVec(CoordinateAxis axis) const 
00485 {   return col(axis); }
00486 
00491 const UnitVec<P,1> getAxisUnitVec(CoordinateDirection dir) const {
00492     const ColType& axDir = getAxisUnitVec(dir.getAxis());
00493     return dir.getDirection() > 0 ? UnitVec<P,1>( axDir) 
00494                                   : UnitVec<P,1>(-axDir); // cheap 
00495 }
00497 
00498 //------------------------------------------------------------------------------
00511 static Mat33P calcNForBodyXYZInBodyFrame(const Vec3P& q) {
00512     // Note: q[0] is not referenced so we won't waste time calculating
00513     // its cosine and sine here.
00514     return calcNForBodyXYZInBodyFrame
00515         (Vec3P(0, std::cos(q[1]), std::cos(q[2])),
00516         Vec3P(0, std::sin(q[1]), std::sin(q[2])));
00517 }
00518 
00524 static Mat33P calcNForBodyXYZInBodyFrame(const Vec3P& cq, const Vec3P& sq) {
00525     const RealP s1 = sq[1], c1 = cq[1];
00526     const RealP s2 = sq[2], c2 = cq[2];
00527     const RealP ooc1  = 1/c1;
00528     const RealP s2oc1 = s2*ooc1, c2oc1 = c2*ooc1;
00529 
00530     return Mat33P(    c2oc1  , -s2oc1  , 0,
00531                         s2   ,    c2   , 0,
00532                     -s1*c2oc1 , s1*s2oc1, 1 );
00533 }
00534 
00545 static Mat33P calcNForBodyXYZInParentFrame(const Vec3P& q) {
00546     // Note: q[2] is not referenced so we won't waste time calculating
00547     // its cosine and sine here.
00548     return calcNForBodyXYZInParentFrame
00549         (Vec3P(std::cos(q[0]), std::cos(q[1]), 0),
00550         Vec3P(std::sin(q[0]), std::sin(q[1]), 0));
00551 }
00552 
00559 static Mat33P calcNForBodyXYZInParentFrame(const Vec3P& cq, const Vec3P& sq) {
00560     const RealP s0 = sq[0], c0 = cq[0];
00561     const RealP s1 = sq[1], c1 = cq[1];
00562     const RealP ooc1  = 1/c1;
00563     const RealP s0oc1 = s0*ooc1, c0oc1 = c0*ooc1;
00564 
00565     return Mat33P( 1 , s1*s0oc1 , -s1*c0oc1,
00566                     0 ,    c0    ,    s0,
00567                     0 ,  -s0oc1  ,  c0oc1 );
00568 }
00569 
00579 static Mat33P calcNDotForBodyXYZInBodyFrame
00580    (const Vec3P& q, const Vec3P& qdot) {
00581     // Note: q[0] is not referenced so we won't waste time calculating
00582     // its cosine and sine here.
00583     return calcNDotForBodyXYZInBodyFrame
00584         (Vec3P(0, std::cos(q[1]), std::cos(q[2])),
00585         Vec3P(0, std::sin(q[1]), std::sin(q[2])),
00586         qdot);
00587 }
00588 
00594 static Mat33P calcNDotForBodyXYZInBodyFrame
00595    (const Vec3P& cq, const Vec3P& sq, const Vec3P& qdot) 
00596 {
00597     const RealP s1 = sq[1], c1 = cq[1];
00598     const RealP s2 = sq[2], c2 = cq[2];
00599     const RealP ooc1  = 1/c1;
00600     const RealP s2oc1 = s2*ooc1, c2oc1 = c2*ooc1;
00601 
00602     const RealP t = qdot[1]*s1*ooc1;
00603     const RealP a = t*s2oc1 + qdot[2]*c2oc1; // d/dt s2oc1
00604     const RealP b = t*c2oc1 - qdot[2]*s2oc1; // d/dt c2oc1
00605 
00606     return Mat33P(       b             ,        -a         , 0,
00607                         qdot[2]*c2       ,    -qdot[2]*s2    , 0,
00608                     -(s1*b + qdot[1]*c2) , s1*a + qdot[1]*s2 , 0 );
00609 }
00610 
00620 static Mat33P calcNDotForBodyXYZInParentFrame
00621    (const Vec3P& q, const Vec3P& qdot) {
00622     // Note: q[2] is not referenced so we won't waste time calculating
00623     // its cosine and sine here.
00624     const RealP cy = std::cos(q[1]); // cos(y)
00625     return calcNDotForBodyXYZInParentFrame
00626         (Vec2P(std::cos(q[0]), cy), 
00627         Vec2P(std::sin(q[0]), std::sin(q[1])),
00628         1/cy, qdot);
00629 }
00630 
00634 static Mat33P calcNDotForBodyXYZInParentFrame
00635    (const Vec2P& cq, const Vec2P& sq, RealP ooc1, const Vec3P& qdot) {
00636     const RealP s0 = sq[0], c0 = cq[0];
00637     const RealP s1 = sq[1];
00638     const RealP s0oc1 = s0*ooc1, c0oc1 = c0*ooc1;
00639 
00640     const RealP t = qdot[1]*s1*ooc1;
00641     const RealP a = t*s0oc1 + qdot[0]*c0oc1; // d/dt s0oc1
00642     const RealP b = t*c0oc1 - qdot[0]*s0oc1; // d/dt c0oc1
00643 
00644     return Mat33P( 0,  s1*a + qdot[1]*s0, -(s1*b + qdot[1]*c0), 
00645                    0,    -qdot[0]*s0    ,     qdot[0]*c0      ,
00646                    0,        -a         ,         b            );
00647 }
00648 
00657 static Mat33P calcNInvForBodyXYZInBodyFrame(const Vec3P& q) {
00658     // Note: q[0] is not referenced so we won't waste time calculating
00659     // its cosine and sine here.
00660     return calcNInvForBodyXYZInBodyFrame
00661        (Vec3P(0, std::cos(q[1]), std::cos(q[2])),
00662         Vec3P(0, std::sin(q[1]), std::sin(q[2])));
00663 }
00664 
00669 static Mat33P calcNInvForBodyXYZInBodyFrame
00670    (const Vec3P& cq, const Vec3P& sq) {
00671     const RealP s1 = sq[1], c1 = cq[1];
00672     const RealP s2 = sq[2], c2 = cq[2];
00673 
00674     return Mat33P( c1*c2 ,  s2 , 0 ,
00675                   -c1*s2 ,  c2 , 0 ,
00676                     s1   ,  0  , 1 );
00677 }
00678 
00686 static Mat33P calcNInvForBodyXYZInParentFrame(const Vec3P& q) {
00687     // Note: q[0] is not referenced so we won't waste time calculating
00688     // its cosine and sine here.
00689     return calcNInvForBodyXYZInParentFrame
00690        (Vec3P(std::cos(q[0]), std::cos(q[1]), 0),
00691         Vec3P(std::sin(q[0]), std::sin(q[1]), 0));
00692 }
00693 
00698 static Mat33P calcNInvForBodyXYZInParentFrame
00699     (const Vec3P& cq, const Vec3P& sq) {
00700     const RealP s0 = sq[0], c0 = cq[0];
00701     const RealP s1 = sq[1], c1 = cq[1];
00702 
00703     return Mat33P( 1 ,  0  ,   s1   ,
00704                     0 ,  c0 , -s0*c1 ,
00705                     0 ,  s0 ,  c0*c1 );
00706 }
00707 
00712 static Mat43P calcUnnormalizedNForQuaternion(const Vec4P& q) {
00713     const Vec4P e = q/2;
00714     const RealP ne1 = -e[1], ne2 = -e[2], ne3 = -e[3];
00715     return Mat43P( ne1,  ne2,  ne3,
00716                     e[0], e[3], ne2,
00717                     ne3,  e[0], e[1],
00718                     e[2], ne1,  e[0]);
00719 }
00720 
00726 static Mat43P calcUnnormalizedNDotForQuaternion(const Vec4P& qdot) {
00727     const Vec4P ed = qdot/2;
00728     const RealP ned1 = -ed[1], ned2 = -ed[2], ned3 = -ed[3];
00729     return Mat43P( ned1,  ned2,  ned3,
00730                     ed[0], ed[3], ned2,
00731                     ned3,  ed[0], ed[1],
00732                     ed[2], ned1,  ed[0]);
00733 }
00734 
00742 static Mat34P calcUnnormalizedNInvForQuaternion(const Vec4P& q) {
00743     const Vec4P e = 2*q;
00744     const RealP ne1 = -e[1], ne2 = -e[2], ne3 = -e[3];
00745     return Mat34P(ne1, e[0], ne3,  e[2],
00746                     ne2, e[3], e[0], ne1,
00747                     ne3, ne2,  e[1], e[0]);
00748 }
00750 
00751 //------------------------------------------------------------------------------
00757 const Mat33P&  asMat33() const  { return *static_cast<const Mat33P*>(this); }
00761 Mat33P         toMat33() const  { return asMat33(); }
00762 
00771 SimTK_SimTKCOMMON_EXPORT SymMat33P 
00772 reexpressSymMat33(const SymMat33P& S_BB) const;
00773 
00774 // Converts rotation matrix to one or two or three orientation angles.
00775 // Note:  The result is most meaningful if the Rotation_ matrix is one that can 
00776 // be produced by such a sequence.
00777 // Use1:  someRotation.convertOneAxisRotationToOneAngle( XAxis );
00778 // Use2:  someRotation.convertTwoAxesRotationToTwoAngles
00779 //                                (SpaceRotationSequence, YAxis, ZAxis );
00780 // Use3:  someRotation.convertThreeAxesRotationToThreeAngles
00781 //                                (SpaceRotationSequence, ZAxis, YAxis, XAxis );
00782 // Use4:  someRotation.convertRotationToAngleAxis();   
00783 //        Return: [angleInRadians, unitVectorX, unitVectorY, unitVectorZ].
00784 
00788 SimTK_SimTKCOMMON_EXPORT RealP  
00789 convertOneAxisRotationToOneAngle( const CoordinateAxis& axis1 ) const;
00790 
00794 SimTK_SimTKCOMMON_EXPORT Vec2P  
00795 convertTwoAxesRotationToTwoAngles(BodyOrSpaceType bodyOrSpace, 
00796                                   const CoordinateAxis& axis1, 
00797                                   const CoordinateAxis& axis2) const;
00798 
00802 SimTK_SimTKCOMMON_EXPORT Vec3P  
00803 convertThreeAxesRotationToThreeAngles
00804    (BodyOrSpaceType bodyOrSpace, const CoordinateAxis& axis1, 
00805     const CoordinateAxis& axis2, const CoordinateAxis& axis3 ) const;
00806 
00817 SimTK_SimTKCOMMON_EXPORT QuaternionP convertRotationToQuaternion() const;
00818 
00836 Vec4P convertRotationToAngleAxis() const  
00837 { return convertRotationToQuaternion().convertQuaternionToAngleAxis(); }
00838 
00840 Vec2P convertRotationToBodyFixedXY() const   
00841 { return convertTwoAxesRotationToTwoAngles(BodyRotationSequence,XAxis,YAxis); }
00843 Vec3P convertRotationToBodyFixedXYZ() const  
00844 { return convertThreeAxesRotationToThreeAngles( BodyRotationSequence, 
00845                                                 XAxis, YAxis, ZAxis ); }
00846 
00851 static Vec3P convertAngVelToBodyFixed321Dot(const Vec3P& q, const Vec3P& w_PB_B) {
00852     const RealP s1 = std::sin(q[1]), c1 = std::cos(q[1]);
00853     const RealP s2 = std::sin(q[2]), c2 = std::cos(q[2]);
00854     const RealP ooc1 = RealP(1)/c1;
00855     const RealP s2oc1 = s2*ooc1, c2oc1 = c2*ooc1;
00856 
00857     const Mat33P E( 0,    s2oc1  ,  c2oc1  ,
00858                     0,      c2   ,   -s2   ,
00859                     1,  s1*s2oc1 , s1*c2oc1 );
00860     return E * w_PB_B;
00861 }
00862 
00865 static Vec3P convertBodyFixed321DotToAngVel(const Vec3P& q, const Vec3P& qd) {
00866     const RealP s1 = std::sin(q[1]), c1 = std::cos(q[1]);
00867     const RealP s2 = std::sin(q[2]), c2 = std::cos(q[2]);
00868 
00869     const Mat33P Einv(  -s1  ,  0  ,  1 ,
00870                         c1*s2 ,  c2 ,  0 ,
00871                         c1*c2 , -s2 ,  0 );
00872     return Einv*qd;
00873 }
00874 
00875 // TODO: sherm: is this right? Warning: everything is measured in the
00876 // *PARENT* frame, but angular velocities and accelerations are
00877 // expressed in the *BODY* frame.
00878 // TODO: this is not an efficient way to do this computation.
00880 static Vec3P convertAngVelDotToBodyFixed321DotDot
00881     (const Vec3P& q, const Vec3P& w_PB_B, const Vec3P& wdot_PB_B)
00882 {
00883     const RealP s1 = std::sin(q[1]), c1 = std::cos(q[1]);
00884     const RealP s2 = std::sin(q[2]), c2 = std::cos(q[2]);
00885     const RealP ooc1  = 1/c1;
00886     const RealP s2oc1 = s2*ooc1, c2oc1 = c2*ooc1, s1oc1 = s1*ooc1;
00887 
00888     const Mat33P E( 0 ,   s2oc1  ,  c2oc1  ,
00889                     0 ,     c2   ,   -s2   ,
00890                     1 , s1*s2oc1 , s1*c2oc1 );
00891     const Vec3P qdot = E * w_PB_B;
00892 
00893     const RealP t = qdot[1]*s1oc1;
00894     const RealP a = t*s2oc1 + qdot[2]*c2oc1; // d/dt s2oc1
00895     const RealP b = t*c2oc1 - qdot[2]*s2oc1; // d/dt c2oc1
00896 
00897     const Mat33P Edot( 0 ,       a           ,         b         ,
00898                         0 ,   -qdot[2]*s2     ,    -qdot[2]*c2    ,
00899                         0 , s1*a + qdot[1]*s2 , s1*b + qdot[1]*c2 );
00900 
00901     return E*wdot_PB_B + Edot*w_PB_B;
00902 }
00903 
00912 static Vec3P convertAngVelInBodyFrameToBodyXYZDot
00913     (const Vec3P& q, const Vec3P& w_PB_B) {  
00914     return convertAngVelInBodyFrameToBodyXYZDot
00915         (Vec3P(0, std::cos(q[1]), std::cos(q[2])),
00916         Vec3P(0, std::sin(q[1]), std::sin(q[2])),
00917         w_PB_B); 
00918 }
00919 
00925 //TODO: reimplement
00926 static Vec3P convertAngVelInBodyFrameToBodyXYZDot
00927    (const Vec3P& cq, const Vec3P& sq, const Vec3P& w_PB_B) 
00928 {   return calcNForBodyXYZInBodyFrame(cq,sq)*w_PB_B; }
00929 
00935 static Vec3P convertBodyXYZDotToAngVelInBodyFrame
00936    (const Vec3P& q, const Vec3P& qdot) {   
00937         return convertBodyXYZDotToAngVelInBodyFrame
00938                    (Vec3P(0, std::cos(q[1]), std::cos(q[2])),
00939                     Vec3P(0, std::sin(q[1]), std::sin(q[2])),
00940                     qdot); 
00941 }
00942 
00948 // TODO: reimplement
00949 static Vec3P convertBodyXYZDotToAngVelInBodyFrame
00950     (const Vec3P& cq, const Vec3P& sq, const Vec3P& qdot) 
00951 {   return calcNInvForBodyXYZInBodyFrame(cq,sq)*qdot; }
00952 
00953 // TODO: sherm: is this right?
00959 static Vec3P convertAngVelDotInBodyFrameToBodyXYZDotDot
00960    (const Vec3P& q, const Vec3P& w_PB_B, const Vec3P& wdot_PB_B)
00961 {
00962     // Note: q[0] is not referenced so we won't waste time calculating
00963     // its cosine and sine here.
00964     return convertAngVelDotInBodyFrameToBodyXYZDotDot
00965                (Vec3P(0, std::cos(q[1]), std::cos(q[2])),
00966                 Vec3P(0, std::sin(q[1]), std::sin(q[2])),
00967                 w_PB_B, wdot_PB_B);
00968 }
00969 
00975 // TODO: reimplement
00976 static Vec3P convertAngVelDotInBodyFrameToBodyXYZDotDot
00977    (const Vec3P& cq, const Vec3P& sq, 
00978     const Vec3P& w_PB_B, const Vec3P& wdot_PB_B)
00979 {
00980     const Mat33P N    = calcNForBodyXYZInBodyFrame(cq,sq);         // ~12 flops
00981     const Vec3P  qdot = N * w_PB_B;                                //  15 flops
00982     const Mat33P NDot = calcNDotForBodyXYZInBodyFrame(cq,sq,qdot); // ~30 flops
00983 
00984     return N*wdot_PB_B + NDot*w_PB_B;                              //  33 flops
00985 }
00986 
00990 static Vec4P convertAngVelToQuaternionDot(const Vec4P& q, const Vec3P& w_PB_P) {
00991     return calcUnnormalizedNForQuaternion(q)*w_PB_P;
00992 }
00993 
00996 static Vec3P convertQuaternionDotToAngVel(const Vec4P& q, const Vec4P& qdot) {
00997     return calcUnnormalizedNInvForQuaternion(q)*qdot;
00998 }
00999 
01006 static Vec4P convertAngVelDotToQuaternionDotDot
01007     (const Vec4P& q, const Vec3P& w_PB, const Vec3P& b_PB)
01008 {
01009     const Mat43P N     = calcUnnormalizedNForQuaternion(q); //  7 flops
01010     const Vec4P  Nb    = N*b_PB;                            // 20 flops
01011     const Vec4P  NDotw = RealP(-.25)*w_PB.normSqr()*q;      // 10 flops
01012     return Nb + NDotw;                                      //  4 flops
01013 }
01014 
01021 static Vec3P convertAngVelInParentToBodyXYZDot
01022     (const Vec2P& cosxy,  
01023     const Vec2P& sinxy,  
01024     RealP        oocosy, 
01025     const Vec3P& w_PB)   
01026 {
01027     return multiplyByBodyXYZ_N_P(cosxy,sinxy,oocosy,w_PB);
01028 }
01029 
01041 static Vec3P convertAngAccInParentToBodyXYZDotDot
01042     (const Vec2P& cosxy,  
01043     const Vec2P& sinxy,  
01044     RealP        oocosy, 
01045     const Vec3P& qdot,   
01046     const Vec3P& b_PB)   
01047 {
01048     const RealP s1 = sinxy[1], c1 = cosxy[1];
01049     const RealP q0 = qdot[0], q1 = qdot[1], q2 = qdot[2];
01050 
01051     // 10 flops
01052     const Vec3P Nb = multiplyByBodyXYZ_N_P(cosxy,sinxy,oocosy,b_PB);
01053 
01054     const RealP q1oc1 = q1*oocosy;
01055     const Vec3P NDotw((q0*s1-q2)*q1oc1,     //   NDot_P*w_PB
01056                         q0*q2*c1,            // = NDot_P*(NInv_P*qdot)
01057                         (q2*s1-q0)*q1oc1 );   // (9 flops)
01058 
01059     return Nb + NDotw; // 3 flops
01060 }
01062 
01063 //------------------------------------------------------------------------------
01068 SimTK_SimTKCOMMON_EXPORT bool  
01069 isSameRotationToWithinAngle(const Rotation_& R, RealP okPointingAngleErrorRads) 
01070                                                                         const;
01071 
01074 bool isSameRotationToWithinAngleOfMachinePrecision(const Rotation_& R) const       
01075 { return isSameRotationToWithinAngle( R, NTraits<P>::getSignificant() ); }
01076 
01079 RealP getMaxAbsDifferenceInRotationElements( const Rotation_& R ) const {            
01080     const Mat33P& A=asMat33(); const Mat33P& B=R.asMat33(); RealP maxDiff=0;  
01081     for( int i=0;  i<=2; i++ ) for( int j=0; j<=2; j++ ) {
01082         const RealP absDiff = std::abs(A[i][j] - B[i][j]);  
01083         if( absDiff > maxDiff ) maxDiff = absDiff; 
01084     }
01085     return maxDiff; 
01086 } 
01087 
01090 bool areAllRotationElementsSameToEpsilon(const Rotation_& R, RealP epsilon) const 
01091 { return getMaxAbsDifferenceInRotationElements(R) <= epsilon; }
01092 
01095 bool areAllRotationElementsSameToMachinePrecision( const Rotation_& R ) const       
01096 { return areAllRotationElementsSameToEpsilon(R, NTraits<P>::getSignificant()); } 
01098 
01099 
01100 private:
01101 // This is only for the most trustworthy of callers, that is, methods of 
01102 // the Rotation_ class.  There are a lot of ways for this NOT to be a 
01103 // legitimate rotation matrix -- be careful!!
01104 // Note that these are supplied in rows.
01105 Rotation_( const RealP& xx, const RealP& xy, const RealP& xz,
01106             const RealP& yx, const RealP& yy, const RealP& yz,
01107             const RealP& zx, const RealP& zy, const RealP& zz )
01108 :   Mat33P( xx,xy,xz, yx,yy,yz, zx,zy,zz ) {}
01109 
01110 // These next methods are highly-efficient power-user methods. Read the 
01111 // code to understand them.
01112 SimTK_SimTKCOMMON_EXPORT Rotation_&  
01113 setTwoAngleTwoAxesBodyFixedForwardCyclicalRotation
01114    (RealP cosAngle1, RealP sinAngle1, const CoordinateAxis& axis1, 
01115     RealP cosAngle2, RealP sinAngle2, const CoordinateAxis& axis2 );
01116 SimTK_SimTKCOMMON_EXPORT Rotation_&  
01117 setThreeAngleTwoAxesBodyFixedForwardCyclicalRotation
01118    (RealP cosAngle1, RealP sinAngle1, const CoordinateAxis& axis1, 
01119     RealP cosAngle2, RealP sinAngle2, const CoordinateAxis& axis2, 
01120     RealP cosAngle3, RealP sinAngle3 );
01121 SimTK_SimTKCOMMON_EXPORT Rotation_&  
01122 setThreeAngleThreeAxesBodyFixedForwardCyclicalRotation
01123    (RealP cosAngle1, RealP sinAngle1, const CoordinateAxis& axis1, 
01124     RealP cosAngle2, RealP sinAngle2, const CoordinateAxis& axis2, 
01125     RealP cosAngle3, RealP sinAngle3, const CoordinateAxis& axis3 );
01126 
01127 // These next methods are highly-efficient power-user methods to convert 
01128 // Rotation matrices to orientation angles.  Read the code to understand them.
01129 SimTK_SimTKCOMMON_EXPORT Vec2P  
01130 convertTwoAxesBodyFixedRotationToTwoAngles
01131    (const CoordinateAxis& axis1, const CoordinateAxis& axis2 ) const;
01132 SimTK_SimTKCOMMON_EXPORT Vec3P  
01133 convertTwoAxesBodyFixedRotationToThreeAngles
01134    (const CoordinateAxis& axis1, const CoordinateAxis& axis2 ) const;
01135 SimTK_SimTKCOMMON_EXPORT Vec3P  
01136 convertThreeAxesBodyFixedRotationToThreeAngles
01137    (const CoordinateAxis& axis1, const CoordinateAxis& axis2, 
01138     const CoordinateAxis& axis3 ) const;
01139 
01140 //------------------------------------------------------------------------------
01141 // These are obsolete names from a previous release, listed here so that 
01142 // users will get a decipherable compilation error. (sherm 091101)
01143 //------------------------------------------------------------------------------
01144 private:
01145 // REPLACED BY: calcNForBodyXYZInBodyFrame()
01146 static Mat33P calcQBlockForBodyXYZInBodyFrame(const Vec3P& a)
01147 {   return calcNForBodyXYZInBodyFrame(a); }
01148 // REPLACED BY: calcNInvForBodyXYZInBodyFrame()
01149 static Mat33P calcQInvBlockForBodyXYZInBodyFrame(const Vec3P& a)
01150 {   return calcNInvForBodyXYZInBodyFrame(a); }
01151 // REPLACED BY: calcUnnormalizedNForQuaternion()
01152 static Mat<4,3,P> calcUnnormalizedQBlockForQuaternion(const Vec4P& q)
01153 {   return calcUnnormalizedNForQuaternion(q); }
01154 // REPLACED BY: calcUnnormalizedNInvForQuaternion()
01155 static Mat<3,4,P> calcUnnormalizedQInvBlockForQuaternion(const Vec4P& q)
01156 {   return calcUnnormalizedNInvForQuaternion(q); }
01157 // REPLACED BY: convertAngVelInBodyFrameToBodyXYZDot
01158 static Vec3P convertAngVelToBodyFixed123Dot(const Vec3P& q, const Vec3P& w_PB_B) 
01159 {   return convertAngVelInBodyFrameToBodyXYZDot(q,w_PB_B); }
01160 // REPLACED BY: convertBodyXYZDotToAngVelInBodyFrame
01161 static Vec3P convertBodyFixed123DotToAngVel(const Vec3P& q, const Vec3P& qdot) 
01162 {   return convertBodyXYZDotToAngVelInBodyFrame(q,qdot); }
01163 // REPLACED BY: convertAngVelDotInBodyFrameToBodyXYZDotDot
01164 static Vec3P convertAngVelDotToBodyFixed123DotDot
01165     (const Vec3P& q, const Vec3P& w_PB_B, const Vec3P& wdot_PB_B)
01166 {   return convertAngVelDotInBodyFrameToBodyXYZDotDot(q,w_PB_B,wdot_PB_B); }
01167 
01168 //------------------------------------------------------------------------------
01169 // The following code is obsolete - it is here temporarily for backward 
01170 // compatibility (Mitiguy 9/5/2007)
01171 //------------------------------------------------------------------------------
01172 private:
01173 // These static methods are like constructors with friendlier names.
01174 static Rotation_ zero() { return Rotation_(); }
01175 static Rotation_ NaN()  { Rotation_ r;  r.setRotationToNaN();  return r; }
01176 
01178 Rotation_&  setToZero()            { return setRotationToIdentityMatrix(); }
01179 Rotation_&  setToIdentityMatrix()  { return setRotationToIdentityMatrix(); }
01180 Rotation_&  setToNaN()             { return setRotationToNaN(); }
01181 static Rotation_  trustMe( const Mat33P& m )  { return Rotation_(m,true); }
01182 
01183 // One-angle rotations.
01184 static Rotation_ aboutX( const RealP& angleInRad ) 
01185 { return Rotation_( angleInRad, XAxis ); }
01186 static Rotation_ aboutY( const RealP& angleInRad ) 
01187 { return Rotation_( angleInRad, YAxis ); }
01188 static Rotation_ aboutZ( const RealP& angleInRad ) 
01189 { return Rotation_( angleInRad, ZAxis ); }
01190 static Rotation_ aboutAxis( const RealP& angleInRad, const UnitVec3P& axis ) 
01191 { return Rotation_(angleInRad,axis); }
01192 static Rotation_ aboutAxis( const RealP& angleInRad, const Vec3P& axis )     
01193 { return Rotation_(angleInRad,axis); }
01194 void setToRotationAboutZ( const RealP& q ) { setRotationFromAngleAboutZ( q ); }
01195 
01196 // Two-angle space-fixed rotations.
01197 static Rotation_ aboutXThenOldY(const RealP& xInRad, const RealP& yInRad) 
01198 { return Rotation_( SpaceRotationSequence, xInRad, XAxis, yInRad, YAxis ); }
01199 static Rotation_ aboutYThenOldX(const RealP& yInRad, const RealP& xInRad) 
01200 { return Rotation_( SpaceRotationSequence, yInRad, YAxis, xInRad, XAxis ); }
01201 static Rotation_ aboutXThenOldZ(const RealP& xInRad, const RealP& zInRad) 
01202 { return Rotation_( SpaceRotationSequence, xInRad, XAxis, zInRad, ZAxis ); }
01203 static Rotation_ aboutZThenOldX(const RealP& zInRad, const RealP& xInRad) 
01204 { return Rotation_( SpaceRotationSequence, zInRad, ZAxis, xInRad, XAxis ); }
01205 static Rotation_ aboutYThenOldZ(const RealP& yInRad, const RealP& zInRad) 
01206 { return Rotation_( SpaceRotationSequence, yInRad, YAxis, zInRad, ZAxis ); }
01207 static Rotation_ aboutZThenOldY(const RealP& zInRad, const RealP& yInRad) 
01208 { return Rotation_( SpaceRotationSequence, zInRad, ZAxis, yInRad, YAxis ); }
01209 
01210 // Two-angle body fixed rotations (reversed space-fixed ones).
01211 static Rotation_ aboutXThenNewY(const RealP& xInRad, const RealP& yInRad) 
01212 { return Rotation_( BodyRotationSequence, xInRad, XAxis, yInRad, YAxis ); }
01213 static Rotation_ aboutYThenNewX(const RealP& yInRad, const RealP& xInRad)
01214 { return aboutXThenOldY(xInRad, yInRad); }
01215 static Rotation_ aboutXThenNewZ(const RealP& xInRad, const RealP& zInRad)
01216 { return aboutZThenOldX(zInRad, xInRad); }
01217 static Rotation_ aboutZThenNewX(const RealP& zInRad, const RealP& xInRad)
01218 { return aboutXThenOldZ(xInRad, zInRad); }
01219 static Rotation_ aboutYThenNewZ(const RealP& yInRad, const RealP& zInRad)
01220 { return aboutZThenOldY(zInRad, yInRad); }
01221 static Rotation_ aboutZThenNewY(const RealP& zInRad, const RealP& yInRad)
01222 { return aboutYThenOldZ(yInRad, zInRad); }
01223 
01224 // Create a Rotation_ matrix by specifying only its z axis. 
01225 // This will work for any stride UnitVec because there is always an implicit 
01226 // conversion available to the packed form used as the argument.
01227 explicit Rotation_( const UnitVec3P& uvecZ )  
01228 { setRotationFromOneAxis(uvecZ,ZAxis); }
01229 
01230 // Create a Rotation_ matrix by specifying its x axis, and a "y like" axis. 
01231 // We will take x seriously after normalizing, but use the y only to create 
01232 // z = normalize(x X y), then y = z X x. Bad things happen if x and y are 
01233 // aligned but we may not catch it.
01234 Rotation_( const Vec3P& x, const Vec3P& yish )  
01235 { setRotationFromTwoAxes( UnitVec3P(x), XAxis, yish, YAxis ); }
01236 
01237 // Set this Rotation_ to represent the same rotation as the passed-in quaternion.
01238 void setToQuaternion( const QuaternionP& q )  { setRotationFromQuaternion(q); }
01239 
01240 // Set this Rotation_ to represent a rotation of +q0 about body frame's Z axis, 
01241 // followed by a rotation of +q1 about the body frame's NEW Y axis, 
01242 // followed by a rotation of +q3 about the body frame's NEW X axis.
01243 // See Kane, Spacecraft Dynamics, pg. 423, body-three: 3-2-1.
01244 //  Similarly for BodyFixed123.
01245 void setToBodyFixed321( const Vec3P& v)  
01246 { setRotationFromThreeAnglesThreeAxes(BodyRotationSequence, 
01247                                       v[0], ZAxis, v[1], YAxis, v[2], XAxis ); }
01248 void setToBodyFixed123( const Vec3P& v)  
01249 { setRotationToBodyFixedXYZ(v); }
01250 
01251 // Convert this Rotation_ matrix to an equivalent (angle,axis) representation: 
01252 // Returned Vec4P is [angleInRadians, unitVectorX, unitVectorY, unitVectorZ].
01253 Vec4P convertToAngleAxis() const  
01254 { return convertRotationToAngleAxis(); }
01255 
01256 // Convert this Rotation_ matrix to equivalent quaternion representation.
01257 QuaternionP convertToQuaternion() const  
01258 { return convertRotationToQuaternion(); }
01259 
01260 // Set this Rotation_ to represent a rotation of +q0 about base frame's X axis, 
01261 // followed by a rotation of +q1 about the base frame's (unchanged) Y axis.
01262 void setToSpaceFixed12( const Vec2P& q ) 
01263 { setRotationFromTwoAnglesTwoAxes(SpaceRotationSequence,q[0],XAxis,q[1],YAxis);}
01264 
01265 // Convert this Rotation_ matrix to the equivalent 1-2-3 body fixed Euler angle 
01266 // sequence. Similarly, convert Rotation_ matrix to the equivalent 1-2 body  
01267 // fixed Euler angle sequence. Similarly, convert Rotation_ matrix to the 
01268 // equivalent 1-2 space fixed Euler angle sequence. 
01269 Vec3P  convertToBodyFixed123() const  
01270 { return convertRotationToBodyFixedXYZ(); }
01271 Vec2P  convertToBodyFixed12() const   
01272 { return convertRotationToBodyFixedXY(); }
01273 Vec2P  convertToSpaceFixed12() const  
01274 { return convertTwoAxesRotationToTwoAngles(SpaceRotationSequence,XAxis,YAxis); }
01275 };
01276 
01277 
01278 //-----------------------------------------------------------------------------
01281 //-----------------------------------------------------------------------------
01282 template <class P>
01283 class InverseRotation_ : public Mat<3,3,P>::TransposeType {
01284 typedef P               RealP;
01285 typedef Rotation_<P>    RotationP;
01286 typedef Mat<3,3,P>      Mat33P; // not the base type!
01287 typedef SymMat<3,P>     SymMat33P;
01288 typedef Mat<2,2,P>      Mat22P;
01289 typedef Mat<3,2,P>      Mat32P;
01290 typedef Vec<2,P>        Vec2P;
01291 typedef Vec<3,P>        Vec3P;
01292 typedef Vec<4,P>        Vec4P;
01293 typedef Quaternion_<P>  QuaternionP;
01294 public:
01297 typedef typename Mat<3,3,P>::TransposeType  BaseMat;
01298 
01303 typedef  UnitVec<P,BaseMat::RowSpacing>  ColType;
01305 typedef  UnitRow<P,BaseMat::ColSpacing>  RowType;
01307 
01311 InverseRotation_() : BaseMat(1) {}
01312 
01314 InverseRotation_( const InverseRotation_& R ) : BaseMat(R) {}
01316 InverseRotation_&  operator=( const InverseRotation_& R )  
01317 {   BaseMat::operator=(R.asMat33());  return *this; }
01318 
01325 SimTK_SimTKCOMMON_EXPORT SymMat33P 
01326 reexpressSymMat33(const SymMat33P& S_BB) const;
01327 
01331 const RotationP&  invert() const 
01332 {return *reinterpret_cast<const RotationP*>(this);}
01333 RotationP&        updInvert() {return *reinterpret_cast<RotationP*>(this);}
01335 
01339 const RotationP&  transpose() const  { return invert(); }
01340 const RotationP&  operator~() const  { return invert(); }
01341 RotationP&        updTranspose()     { return updInvert(); }
01342 RotationP&        operator~()        { return updInvert(); }
01344 
01351 const RowType&  row( int i ) const         
01352 { return reinterpret_cast<const RowType&>(asMat33()[i]); }
01353 const RowType&  operator[]( int i ) const  { return row(i); }
01354 const ColType&  col( int j ) const         
01355 { return reinterpret_cast<const ColType&>(asMat33()(j)); }
01356 const ColType&  operator()( int j ) const  { return col(j); }
01357 const ColType&  x() const                  { return col(0); }
01358 const ColType&  y() const                  { return col(1); }
01359 const ColType&  z() const                  { return col(2); }
01361 
01362 
01367 const ColType& getAxisUnitVec(CoordinateAxis axis) const 
01368 {   return col(axis); }
01369 
01374 const UnitVec<P,1> getAxisUnitVec(CoordinateDirection dir) const {
01375     const ColType& axDir = getAxisUnitVec(dir.getAxis());
01376     return dir.getDirection() > 0 ? UnitVec<P,1>( axDir) 
01377                                   : UnitVec<P,1>(-axDir); // cheap 
01378 }
01379 
01384 const BaseMat&  asMat33() const  { return *static_cast<const BaseMat*>(this); }
01385 BaseMat         toMat33() const  { return asMat33(); }
01387 };
01388 
01391 template <class P> SimTK_SimTKCOMMON_EXPORT std::ostream& 
01392 operator<<(std::ostream&, const Rotation_<P>&);
01395 template <class P> SimTK_SimTKCOMMON_EXPORT std::ostream& 
01396 operator<<(std::ostream&, const InverseRotation_<P>&);
01397 
01402 template <class P, int S> inline UnitVec<P,1>  
01403 operator*(const Rotation_<P>& R, const UnitVec<P,S>& v)        
01404 {return UnitVec<P,1>(R.asMat33()* v.asVec3(),  true);}
01405 template <class P, int S> inline UnitRow<P,1>  
01406 operator*(const UnitRow<P,S>& r, const Rotation_<P>& R)        
01407 {return UnitRow<P,1>(r.asRow3() * R.asMat33(), true);}
01408 template <class P, int S> inline UnitVec<P,1>  
01409 operator*(const InverseRotation_<P>& R, const UnitVec<P,S>& v) 
01410 {return UnitVec<P,1>(R.asMat33()* v.asVec3(),  true);}
01411 template <class P, int S> inline UnitRow<P,1>  
01412 operator*(const UnitRow<P,S>& r, const InverseRotation_<P>& R) 
01413 {return UnitRow<P,1>(r.asRow3() * R.asMat33(), true);}
01415 
01416 // Couldn't implement these Rotation_ methods until InverseRotation_ was defined.
01417 template <class P> inline
01418 Rotation_<P>::Rotation_(const InverseRotation_<P>& R) 
01419 :   Mat<3,3,P>( R.asMat33() ) {}
01420 
01421 template <class P> inline Rotation_<P>&  
01422 Rotation_<P>::operator=(const InverseRotation_<P>& R)  
01423 {static_cast<Mat<3,3,P>&>(*this)  = R.asMat33();    return *this;}
01424 template <class P> inline Rotation_<P>&  
01425 Rotation_<P>::operator*=(const Rotation_<P>& R)        
01426 {static_cast<Mat<3,3,P>&>(*this) *= R.asMat33();    return *this;}
01427 template <class P> inline Rotation_<P>&  
01428 Rotation_<P>::operator/=(const Rotation_<P>& R)        
01429 {static_cast<Mat<3,3,P>&>(*this) *= (~R).asMat33(); return *this;}
01430 template <class P> inline Rotation_<P>&  
01431 Rotation_<P>::operator*=(const InverseRotation_<P>& R) 
01432 {static_cast<Mat<3,3,P>&>(*this) *= R.asMat33();    return *this;}
01433 template <class P> inline Rotation_<P>&  
01434 Rotation_<P>::operator/=(const InverseRotation_<P>& R) 
01435 {static_cast<Mat<3,3,P>&>(*this) *= (~R).asMat33(); return *this;}
01436 
01438 
01439 template <class P> inline Rotation_<P>
01440 operator*(const Rotation_<P>&        R1, const Rotation_<P>&        R2)  
01441 {return Rotation_<P>(R1) *= R2;}
01442 template <class P> inline Rotation_<P>
01443 operator*(const Rotation_<P>&        R1, const InverseRotation_<P>& R2)  
01444 {return Rotation_<P>(R1) *= R2;}
01445 template <class P> inline Rotation_<P>
01446 operator*(const InverseRotation_<P>& R1, const Rotation_<P>&        R2)  
01447 {return Rotation_<P>(R1) *= R2;}
01448 template <class P> inline Rotation_<P>
01449 operator*(const InverseRotation_<P>& R1, const InverseRotation_<P>& R2)  
01450 {return Rotation_<P>(R1) *= R2;}
01452 
01455 
01456 template <class P> inline Rotation_<P>
01457 operator/( const Rotation_<P>&        R1, const Rotation_<P>&        R2 )  
01458 {return Rotation_<P>(R1) /= R2;}
01459 template <class P> inline Rotation_<P>
01460 operator/( const Rotation_<P>&        R1, const InverseRotation&     R2 )  
01461 {return Rotation_<P>(R1) /= R2;}
01462 template <class P> inline Rotation_<P>
01463 operator/( const InverseRotation_<P>& R1, const Rotation_<P>&        R2 )  
01464 {return Rotation_<P>(R1) /= R2;}
01465 template <class P> inline Rotation_<P>
01466 operator/( const InverseRotation_<P>& R1, const InverseRotation_<P>& R2 )  
01467 {return Rotation_<P>(R1) /= R2;}
01469 
01470 
01471 //------------------------------------------------------------------------------
01472 }  // End of namespace SimTK
01473 
01474 #endif // SimTK_SimTKCOMMON_ROTATION_H_
01475 
01476 
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines