Rotation.h

Go to the documentation of this file.
00001 //-----------------------------------------------------------------------------
00002 // File:     Rotation.h
00003 // Class:    Rotation and InverseRotation 
00004 // Parent:   Mat33
00005 // Purpose:  3x3 rotation class relating two right-handed orthogonal bases
00006 //-----------------------------------------------------------------------------
00007 #ifndef SIMTK_ROTATION_H 
00008 #define SIMTK_ROTATION_H 
00009 
00010 /* -------------------------------------------------------------------------- *
00011  *                      SimTK Core: SimTK Simmatrix(tm)                       *
00012  * -------------------------------------------------------------------------- *
00013  * This is part of the SimTK Core biosimulation toolkit originating from      *
00014  * Simbios, the NIH National Center for Physics-Based Simulation of           *
00015  * Biological Structures at Stanford, funded under the NIH Roadmap for        *
00016  * Medical Research, grant U54 GM072970. See https://simtk.org.               *
00017  *                                                                            *
00018  * Portions copyright (c) 2005-7 Stanford University and the Authors.         *
00019  * Authors: Paul Mitiguy and Michael Sherman                                  *
00020  * Contributors:                                                              *
00021  *                                                                            *
00022  * Permission is hereby granted, free of charge, to any person obtaining a    *
00023  * copy of this software and associated documentation files (the "Software"), *
00024  * to deal in the Software without restriction, including without limitation  *
00025  * the rights to use, copy, modify, merge, publish, distribute, sublicense,   *
00026  * and/or sell copies of the Software, and to permit persons to whom the      *
00027  * Software is furnished to do so, subject to the following conditions:       *
00028  *                                                                            *
00029  * The above copyright notice and this permission notice shall be included in *
00030  * all copies or substantial portions of the Software.                        *
00031  *                                                                            *
00032  * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR *
00033  * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,   *
00034  * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL    *
00035  * THE AUTHORS, CONTRIBUTORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM,    *
00036  * DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR      *
00037  * OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE  *
00038  * USE OR OTHER DEALINGS IN THE SOFTWARE.                                     *
00039  * -------------------------------------------------------------------------- */
00040 
00041 //-----------------------------------------------------------------------------
00042 
00043 #include "SimTKcommon/SmallMatrix.h"
00044 #include "SimTKcommon/internal/UnitVec.h"
00045 #include "SimTKcommon/internal/Quaternion.h"
00046 #include "SimTKcommon/internal/CoordinateAxis.h"
00047 
00048 //-----------------------------------------------------------------------------
00049 #include <iosfwd>  // Forward declaration of iostream
00050 //-----------------------------------------------------------------------------
00051 
00052 //-----------------------------------------------------------------------------
00053 namespace SimTK {
00054 
00055 
00056 enum BodyOrSpaceType { BodyRotationSequence=0, SpaceRotationSequence=1 };
00057 
00058 //-----------------------------------------------------------------------------
00059 // Forward declarations
00060 class InverseRotation;
00061 
00062 
00063 //-----------------------------------------------------------------------------
00088 //------------------------------------------------------------------------------
00089 class Rotation : public Mat33 {
00090 public:
00091     // Default constructor and constructor-like methods
00092     Rotation() : Mat33(1) {}    
00093     Rotation&  setRotationToIdentityMatrix()  { Mat33::operator=(1);  return *this; }
00094     Rotation&  setRotationToNaN()             { Mat33::setToNaN();    return *this; } 
00095 
00096     // Default copy constructor and assignment operator
00097     Rotation( const Rotation& R ) : Mat33(R)  {}
00098     Rotation&  operator=( const Rotation& R )  { Mat33::operator=( R.asMat33() );  return *this; }
00099 
00101 
00102     Rotation( Real angle, const CoordinateAxis& axis )             { setRotationFromAngleAboutAxis( angle, axis ); }
00103     Rotation( Real angle, const CoordinateAxis::XCoordinateAxis )  { setRotationFromAngleAboutX( std::cos(angle), std::sin(angle) ); }
00104     Rotation( Real angle, const CoordinateAxis::YCoordinateAxis )  { setRotationFromAngleAboutY( std::cos(angle), std::sin(angle) ); }
00105     Rotation( Real angle, const CoordinateAxis::ZCoordinateAxis )  { setRotationFromAngleAboutZ( std::cos(angle), std::sin(angle) ); }
00107 
00108 
00109     Rotation&  setRotationFromAngleAboutAxis( Real angle, const CoordinateAxis& axis )  { return axis.isXAxis() ? setRotationFromAngleAboutX(angle) : (axis.isYAxis() ? setRotationFromAngleAboutY(angle) : setRotationFromAngleAboutZ(angle) ); }
00110     Rotation&  setRotationFromAngleAboutX( Real angle )  { return setRotationFromAngleAboutX( std::cos(angle), std::sin(angle) ); }
00111     Rotation&  setRotationFromAngleAboutY( Real angle )  { return setRotationFromAngleAboutY( std::cos(angle), std::sin(angle) ); }
00112     Rotation&  setRotationFromAngleAboutZ( Real angle )  { return setRotationFromAngleAboutZ( std::cos(angle), std::sin(angle) ); }
00113     Rotation&  setRotationFromAngleAboutX( Real cosAngle, Real sinAngle )  { Mat33& 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; }
00114     Rotation&  setRotationFromAngleAboutY( Real cosAngle, Real sinAngle )  { Mat33& 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; }
00115     Rotation&  setRotationFromAngleAboutZ( Real cosAngle, Real sinAngle )  { Mat33& 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; }
00117 
00119 
00120     Rotation( Real angle, const UnitVec3& unitVector ) { setRotationFromAngleAboutUnitVector(angle,unitVector); }
00121     Rotation( Real angle, const Vec3& nonUnitVector )  { setRotationFromAngleAboutNonUnitVector(angle,nonUnitVector); }
00123 
00124 
00125     Rotation&  setRotationFromAngleAboutNonUnitVector( Real angle, const Vec3& nonUnitVector )  { return setRotationFromAngleAboutUnitVector( angle, UnitVec3(nonUnitVector) ); }
00126     SimTK_SimTKCOMMON_EXPORT Rotation&  setRotationFromAngleAboutUnitVector( Real angle, const UnitVec3& unitVector );
00128 
00130     Rotation( BodyOrSpaceType bodyOrSpace, Real angle1, const CoordinateAxis& axis1, Real angle2, const CoordinateAxis& axis2 )                                            { setRotationFromTwoAnglesTwoAxes(    bodyOrSpace,angle1,axis1,angle2,axis2); }
00132     Rotation( BodyOrSpaceType bodyOrSpace, Real angle1, const CoordinateAxis& axis1, Real angle2, const CoordinateAxis& axis2, Real angle3, const CoordinateAxis& axis3 )  { setRotationFromThreeAnglesThreeAxes(bodyOrSpace,angle1,axis1,angle2,axis2,angle3,axis3); }
00134     SimTK_SimTKCOMMON_EXPORT Rotation&  setRotationFromTwoAnglesTwoAxes(     BodyOrSpaceType bodyOrSpace, Real angle1, const CoordinateAxis& axis1, Real angle2, const CoordinateAxis& axis2 ); 
00136     SimTK_SimTKCOMMON_EXPORT Rotation&  setRotationFromThreeAnglesThreeAxes( BodyOrSpaceType bodyOrSpace, Real angle1, const CoordinateAxis& axis1, Real angle2, const CoordinateAxis& axis2, Real angle3, const CoordinateAxis& axis3 );
00137 
00141     void setRotationToBodyFixedXY( const Vec2& v)   { setRotationFromTwoAnglesTwoAxes(     BodyRotationSequence, v[0], XAxis, v[1], YAxis ); }
00146     void setRotationToBodyFixedXYZ( const Vec3& v)  { setRotationFromThreeAnglesThreeAxes( BodyRotationSequence, v[0], XAxis, v[1], YAxis, v[2], ZAxis ); }
00147 
00149     explicit Rotation( const Quaternion& q )  { setRotationFromQuaternion(q); }
00151     SimTK_SimTKCOMMON_EXPORT Rotation&  setRotationFromQuaternion( const Quaternion& q );
00152 
00154     Rotation( const Mat33& m, bool ) : Mat33(m) {}
00155 
00157     explicit Rotation( const Mat33& m )  { setRotationFromApproximateMat33(m); }
00159     SimTK_SimTKCOMMON_EXPORT Rotation&  setRotationFromApproximateMat33( const Mat33& m );
00160 
00163 
00164     Rotation( const UnitVec3& uvec, const CoordinateAxis axis )  { setRotationFromOneAxis(uvec,axis); }
00165     SimTK_SimTKCOMMON_EXPORT Rotation&  setRotationFromOneAxis( const UnitVec3& uvec, const CoordinateAxis axis );
00167 
00170 
00171     Rotation( const UnitVec3& uveci, const CoordinateAxis& axisi, const Vec3& vecjApprox, const CoordinateAxis& axisjApprox )  { setRotationFromTwoAxes(uveci,axisi,vecjApprox,axisjApprox); }
00172     SimTK_SimTKCOMMON_EXPORT Rotation&  setRotationFromTwoAxes( const UnitVec3& uveci, const CoordinateAxis& axisi, const Vec3& vecjApprox, const CoordinateAxis& axisjApprox );
00174 
00175     // Converts rotation matrix to one or two or three orientation angles.
00176     // Note:  The result is most meaningful if the Rotation matrix is one that can be produced by such a sequence.
00177     // Use1:  someRotation.convertOneAxisRotationToOneAngle( XAxis );
00178     // Use2:  someRotation.convertTwoAxesRotationToTwoAngles(     SpaceRotationSequence, YAxis, ZAxis );
00179     // Use3:  someRotation.convertThreeAxesRotationToThreeAngles( SpaceRotationSequence, ZAxis, YAxis, XAxis );
00180     // Use4:  someRotation.convertRotationToAngleAxis();   Return: [angleInRadians, unitVectorX, unitVectorY, unitVectorZ].
00181 
00184     SimTK_SimTKCOMMON_EXPORT Real  convertOneAxisRotationToOneAngle( const CoordinateAxis& axis1 ) const;
00187     SimTK_SimTKCOMMON_EXPORT Vec2  convertTwoAxesRotationToTwoAngles(     BodyOrSpaceType bodyOrSpace, const CoordinateAxis& axis1, const CoordinateAxis& axis2 ) const;
00190     SimTK_SimTKCOMMON_EXPORT Vec3  convertThreeAxesRotationToThreeAngles( BodyOrSpaceType bodyOrSpace, const CoordinateAxis& axis1, const CoordinateAxis& axis2, const CoordinateAxis& axis3 ) const;
00192     SimTK_SimTKCOMMON_EXPORT Quaternion  convertRotationToQuaternion() const;
00194     Vec4  convertRotationToAngleAxis() const  { return convertRotationToQuaternion().convertQuaternionToAngleAxis(); }
00195 
00197     Vec2  convertRotationToBodyFixedXY() const   { return convertTwoAxesRotationToTwoAngles( BodyRotationSequence, XAxis, YAxis ); }
00199     Vec3  convertRotationToBodyFixedXYZ() const  { return convertThreeAxesRotationToThreeAngles( BodyRotationSequence, XAxis, YAxis, ZAxis ); }
00200 
00202 
00203     SimTK_SimTKCOMMON_EXPORT bool  isSameRotationToWithinAngle( const Rotation& R, Real okPointingAngleErrorRads ) const;
00204     bool  isSameRotationToWithinAngleOfMachinePrecision( const Rotation& R) const      { return isSameRotationToWithinAngle( R, SignificantReal ); }
00206     Real  getMaxAbsDifferenceInRotationElements( const Rotation& R ) const             { const Mat33& A = asMat33();  const Mat33& B = R.asMat33();  Real maxDiff = 0.0;  for( int i=0;  i<=2; i++ ) for( int j=0; j<=2; j++ ) { Real absDiff = std::fabs(A[i][j] - B[i][j]);  if( absDiff > maxDiff ) maxDiff = absDiff; }  return maxDiff; } 
00207     bool  areAllRotationElementsSameToEpsilon( const Rotation& R, Real epsilon ) const { return getMaxAbsDifferenceInRotationElements(R) <= epsilon ; }
00208     bool  areAllRotationElementsSameToMachinePrecision( const Rotation& R ) const      { return areAllRotationElementsSameToEpsilon( R, SignificantReal ); } 
00209 
00211     inline Rotation( const InverseRotation& );
00213     inline Rotation& operator=( const InverseRotation& );
00214 
00216     const InverseRotation&  invert() const  { return *reinterpret_cast<const InverseRotation*>(this); }
00218     InverseRotation&        updInvert()     { return *reinterpret_cast<InverseRotation*>(this); }
00219 
00220     // Transpose, and transpose operators (override Mat33 versions of transpose).
00221     const InverseRotation&  transpose() const  { return invert(); }
00222     const InverseRotation&  operator~() const  { return invert(); }
00223     InverseRotation&        updTranspose()     { return updInvert(); }
00224     InverseRotation&        operator~()        { return updInvert(); }
00225 
00226     // Multiply-equals and divide-equals operators 
00227     inline Rotation&  operator*=( const Rotation& R );
00228     inline Rotation&  operator/=( const Rotation& R );
00229     inline Rotation&  operator*=( const InverseRotation& );
00230     inline Rotation&  operator/=( const InverseRotation& );
00231 
00234 
00235     const Mat33&  asMat33() const  { return *static_cast<const Mat33*>(this); }
00236     Mat33         toMat33() const  { return asMat33(); }
00238 
00239     // Note: This does not have unit stride.
00240     typedef  UnitVec<Mat33::RowSpacing>  ColType;
00241     typedef  UnitRow<Mat33::ColSpacing>  RowType;
00242     const RowType&  row( int i ) const         { return reinterpret_cast<const RowType&>(asMat33()[i]); }
00243     const ColType&  col( int j ) const         { return reinterpret_cast<const ColType&>(asMat33()(j)); }
00244     const ColType&  x() const                  { return col(0); }
00245     const ColType&  y() const                  { return col(1); }
00246     const ColType&  z() const                  { return col(2); }
00247     const RowType&  operator[]( int i ) const  { return row(i); }
00248     const ColType&  operator()( int j ) const  { return col(j); }
00249 
00251 
00252     Rotation&  setRotationFromMat33TrustMe( const Mat33& m )  { Mat33& R = *this;  R[0][0]=m[0][0];  R[0][1]=m[0][1];  R[0][2]=m[0][2];  R[1][0]=m[1][0];  R[1][1]=m[1][1];  R[1][2]=m[1][2];  R[2][0]=m[2][0];  R[2][1]=m[2][1];  R[2][2]=m[2][2];  return *this; }   
00253     Rotation&  setRotationColFromUnitVecTrustMe( int coli, const UnitVec3& uveci )  { Mat33& R = *this;   R[0][coli]=uveci[0];  R[1][coli]=uveci[1];  R[2][coli]=uveci[2];  return *this; }   
00254     Rotation&  setRotationFromUnitVecsTrustMe( const UnitVec3& colA, const UnitVec3& colB, const UnitVec3& colC )  { setRotationColFromUnitVecTrustMe(0,colA);  setRotationColFromUnitVecTrustMe(1,colB);  return setRotationColFromUnitVecTrustMe(2,colC); }   
00256 
00257 //----------------------------------------------------------------------------------------------------
00258 // The following code is obsolete - it is here temporarily for backward compatibility (Mitiguy 9/5/2007)
00259 //----------------------------------------------------------------------------------------------------
00260 private:
00261     // These static methods are like constructors with friendlier names.
00262     static Rotation zero() { return Rotation(); }
00263     static Rotation NaN()  { Rotation r;  r.setRotationToNaN();  return r; }
00264 
00266     Rotation&  setToZero()            { return setRotationToIdentityMatrix(); }
00267     Rotation&  setToIdentityMatrix()  { return setRotationToIdentityMatrix(); }
00268     Rotation&  setToNaN()             { return setRotationToNaN(); }
00269     static Rotation  trustMe( const Mat33& m )  { return Rotation(m,true); }
00270 
00271     // One-angle rotations.
00272     static Rotation aboutX( const Real& angleInRad ) { return Rotation( angleInRad, XAxis ); }
00273     static Rotation aboutY( const Real& angleInRad ) { return Rotation( angleInRad, YAxis ); }
00274     static Rotation aboutZ( const Real& angleInRad ) { return Rotation( angleInRad, ZAxis ); }
00275     static Rotation aboutAxis( const Real& angleInRad, const UnitVec3& axis ) { return Rotation(angleInRad,axis); }
00276     static Rotation aboutAxis( const Real& angleInRad, const Vec3& axis )     { return Rotation(angleInRad,axis); }
00277     void  setToRotationAboutZ( const Real& q ) { setRotationFromAngleAboutZ( q ); }
00278 
00279     // Two-angle space-fixed rotations.
00280     static Rotation aboutXThenOldY(const Real& xInRad, const Real& yInRad) { return Rotation( SpaceRotationSequence, xInRad, XAxis, yInRad, YAxis ); }
00281     static Rotation aboutYThenOldX(const Real& yInRad, const Real& xInRad) { return Rotation( SpaceRotationSequence, yInRad, YAxis, xInRad, XAxis ); }
00282     static Rotation aboutXThenOldZ(const Real& xInRad, const Real& zInRad) { return Rotation( SpaceRotationSequence, xInRad, XAxis, zInRad, ZAxis ); }
00283     static Rotation aboutZThenOldX(const Real& zInRad, const Real& xInRad) { return Rotation( SpaceRotationSequence, zInRad, ZAxis, xInRad, XAxis ); }
00284     static Rotation aboutYThenOldZ(const Real& yInRad, const Real& zInRad) { return Rotation( SpaceRotationSequence, yInRad, YAxis, zInRad, ZAxis ); }
00285     static Rotation aboutZThenOldY(const Real& zInRad, const Real& yInRad) { return Rotation( SpaceRotationSequence, zInRad, ZAxis, yInRad, YAxis ); }
00286 
00287     // Two-angle body fixed rotations (reversed space-fixed ones).
00288     static Rotation aboutXThenNewY(const Real& xInRad, const Real& yInRad) { return Rotation( BodyRotationSequence, xInRad, XAxis, yInRad, YAxis ); }
00289     static Rotation aboutYThenNewX(const Real& yInRad, const Real& xInRad) { return aboutXThenOldY(xInRad, yInRad); }
00290     static Rotation aboutXThenNewZ(const Real& xInRad, const Real& zInRad) { return aboutZThenOldX(zInRad, xInRad); }
00291     static Rotation aboutZThenNewX(const Real& zInRad, const Real& xInRad) { return aboutXThenOldZ(xInRad, zInRad); }
00292     static Rotation aboutYThenNewZ(const Real& yInRad, const Real& zInRad) { return aboutZThenOldY(zInRad, yInRad); }
00293     static Rotation aboutZThenNewY(const Real& zInRad, const Real& yInRad) { return aboutYThenOldZ(yInRad, zInRad); }
00294 
00297     explicit Rotation( const UnitVec3& uvecZ )  { setRotationFromOneAxis(uvecZ,ZAxis); }
00298 
00300     //  We will take x seriously after normalizing, but use the y only to create z = normalize(x X y), 
00301     //  then y = z X x. Bad things happen if x and y are aligned but we may not catch it.
00302     Rotation( const Vec3& x, const Vec3& yish )  { setRotationFromTwoAxes( UnitVec3(x), XAxis, yish, YAxis ); }
00303 
00305     void setToQuaternion( const Quaternion& q )  { setRotationFromQuaternion(q); }
00306 
00311     //  Similarly for BodyFixed123.
00312     void setToBodyFixed321( const Vec3& v)  { setRotationFromThreeAnglesThreeAxes( BodyRotationSequence, v[0], ZAxis, v[1], YAxis, v[2], XAxis ); }
00313     void setToBodyFixed123( const Vec3& v)  { setRotationToBodyFixedXYZ(v); }
00314 
00317     Vec4  convertToAngleAxis() const  { return convertRotationToAngleAxis(); }
00318 
00320     Quaternion  convertToQuaternion() const  { return convertRotationToQuaternion(); }
00321 
00324     void setToSpaceFixed12( const Vec2& q ) { setRotationFromTwoAnglesTwoAxes( SpaceRotationSequence, q[0], XAxis, q[1], YAxis ); }
00325 
00329     Vec3  convertToBodyFixed123() const  { return convertRotationToBodyFixedXYZ(); }
00330     Vec2  convertToBodyFixed12() const   { return convertRotationToBodyFixedXY(); }
00331     Vec2  convertToSpaceFixed12() const  { return convertTwoAxesRotationToTwoAngles( SpaceRotationSequence, XAxis, YAxis ); }
00332 
00333 //--------------------------------- PAUL CONTINUE FROM HERE ----------------------------------
00334 public:
00335 //--------------------------------------------------------------------------------------------
00341     static Vec3 convertAngVelToBodyFixed321Dot(const Vec3& q, const Vec3& w_PB_B) {
00342         const Real s1 = std::sin(q[1]), c1 = std::cos(q[1]);
00343         const Real s2 = std::sin(q[2]), c2 = std::cos(q[2]);
00344         const Real ooc1 = Real(1)/c1;
00345         const Real s2oc1 = s2*ooc1, c2oc1 = c2*ooc1;
00346 
00347         const Mat33 E( 0,    s2oc1  ,  c2oc1  ,
00348                        0,      c2   ,   -s2   ,
00349                        1,  s1*s2oc1 , s1*c2oc1 );
00350         return E * w_PB_B;
00351     }
00352 
00355     static Vec3 convertBodyFixed321DotToAngVel(const Vec3& q, const Vec3& qd) {
00356         const Real s1 = std::sin(q[1]), c1 = std::cos(q[1]);
00357         const Real s2 = std::sin(q[2]), c2 = std::cos(q[2]);
00358         const Real ooc1  = 1/c1;
00359         const Real s2oc1 = s2*ooc1, c2oc1 = c2*ooc1;
00360 
00361         const Mat33 Einv(  -s1  ,  0  ,  1 ,
00362                           c1*s2 ,  c2 ,  0 ,
00363                           c1*c2 , -s2 ,  0 );
00364         return Einv*qd;
00365     }
00366 
00367     // TODO: sherm: is this right? Warning: everything is measured in the
00368     // *PARENT* frame, but has to be expressed in the *BODY* frame.
00369     static Vec3 convertAngVelDotToBodyFixed321DotDot
00370         (const Vec3& q, const Vec3& w_PB_B, const Vec3& wdot)
00371     {
00372         const Real s1 = std::sin(q[1]), c1 = std::cos(q[1]);
00373         const Real s2 = std::sin(q[2]), c2 = std::cos(q[2]);
00374         const Real ooc1  = 1/c1;
00375         const Real s2oc1 = s2*ooc1, c2oc1 = c2*ooc1;
00376 
00377         const Mat33 E( 0 ,   s2oc1  ,  c2oc1  ,
00378                        0 ,     c2   ,   -s2   ,
00379                        1 , s1*s2oc1 , s1*c2oc1 );
00380         const Vec3 qdot = E * w_PB_B;
00381 
00382         const Real t =  qdot[1]*qdot[2]*s1*ooc1;
00383         const Real a =  t*c2oc1; // d/dt s2oc1
00384         const Real b = -t*s2oc1; // d/dt c2oc1
00385 
00386         const Mat33 Edot( 0 ,       a           ,         b         ,
00387                           0 ,   -qdot[2]*s2     ,    -qdot[2]*c2    ,
00388                           0 , s1*a + qdot[1]*s2 , s1*b + qdot[1]*c2 );
00389 
00390         return E*wdot + Edot*w_PB_B;
00391     }
00392     
00398     static Mat33 calcQBlockForBodyXYZInBodyFrame(const Vec3& q) {
00399         const Real s1 = std::sin(q[1]), c1 = std::cos(q[1]);
00400         const Real s2 = std::sin(q[2]), c2 = std::cos(q[2]);
00401         const Real ooc1  = 1/c1;
00402         const Real s2oc1 = s2*ooc1, c2oc1 = c2*ooc1;
00403 
00404         return Mat33(    c2oc1  , -s2oc1  , 0,
00405                            s2   ,    c2   , 0,
00406                       -s1*c2oc1 , s1*s2oc1, 1 );
00407     }
00408 
00412     static Mat33 calcQInvBlockForBodyXYZInBodyFrame(const Vec3& q) {
00413         const Real s1 = std::sin(q[1]), c1 = std::cos(q[1]);
00414         const Real s2 = std::sin(q[2]), c2 = std::cos(q[2]);
00415 
00416         return Mat33( c1*c2 ,  s2 , 0 ,
00417                      -c1*s2 ,  c2 , 0 ,
00418                        s1   ,  0  , 1 );
00419     }
00420 
00426     static Vec3 convertAngVelToBodyFixed123Dot(const Vec3& q, const Vec3& w_PB_B) {
00427         return calcQBlockForBodyXYZInBodyFrame(q)*w_PB_B;
00428     }
00429 
00432     static Vec3 convertBodyFixed123DotToAngVel(const Vec3& q, const Vec3& qd) {
00433         return calcQInvBlockForBodyXYZInBodyFrame(q)*qd;
00434     }
00435 
00436 
00437     // TODO: sherm: is this right? Warning: everything is measured in the
00438     // *PARENT* frame, but has to be expressed in the *BODY* frame.
00439     static Vec3 convertAngVelDotToBodyFixed123DotDot
00440         (const Vec3& q, const Vec3& w_PB_B, const Vec3& wdot)
00441     {
00442         const Real s1 = std::sin(q[1]), c1 = std::cos(q[1]);
00443         const Real s2 = std::sin(q[2]), c2 = std::cos(q[2]);
00444         const Real ooc1  = 1/c1;
00445         const Real s2oc1 = s2*ooc1, c2oc1 = c2*ooc1;
00446 
00447         const Mat33 Q(    c2oc1  , -s2oc1  , 0,
00448                             s2   ,    c2   , 0,
00449                        -s1*c2oc1 , s1*s2oc1, 1 );
00450         const Vec3 qdot = Q * w_PB_B;
00451 
00452         const Real t =  qdot[1]*qdot[2]*s1*ooc1;
00453         const Real a =  t*c2oc1; // d/dt s2oc1
00454         const Real b = -t*s2oc1; // d/dt c2oc1
00455 
00456         const Mat33 QDot(       b           ,        -a         , 0,
00457                              qdot[2]*c2     ,    -qdot[2]*s2    , 0,
00458                          -s1*b - qdot[1]*c2 , s1*a + qdot[1]*s2 , 0 );
00459 
00460         return Q*wdot + QDot*w_PB_B;
00461     }
00462 
00467     static Mat43 calcUnnormalizedQBlockForQuaternion(const Vec4& q) {
00468         const Vec4 e = q/2;
00469         return Mat43(-e[1],-e[2],-e[3],
00470                       e[0], e[3],-e[2],
00471                      -e[3], e[0], e[1],
00472                       e[2],-e[1], e[0]);
00473     }
00474 
00482     static Mat34 calcUnnormalizedQInvBlockForQuaternion(const Vec4& q) {
00483         const Vec4 e = 2*q;
00484         return Mat34(-e[1], e[0],-e[3], e[2],
00485                      -e[2], e[3], e[0],-e[1],
00486                      -e[3],-e[2], e[1], e[0]);
00487     }
00488 
00489 
00493     static Vec4 convertAngVelToQuaternionDot(const Vec4& q, const Vec3& w_PB_P) {
00494         return calcUnnormalizedQBlockForQuaternion(q)*w_PB_P;
00495     }
00496 
00499     static Vec3 convertQuaternionDotToAngVel(const Vec4& q, const Vec4& qd) {
00500         return calcUnnormalizedQInvBlockForQuaternion(q)*qd;
00501     }
00502 
00504     static Vec4 convertAngVelDotToQuaternionDotDot
00505         (const Vec4& q, const Vec3& w_PB_P, const Vec3& wdot)
00506     {
00507         const Mat43 Q = calcUnnormalizedQBlockForQuaternion(q);
00508         const Vec4  edot = (Q*w_PB_P)/2; // i.e., edot=qdot/2
00509         const Mat43 QDot(-edot[1],-edot[2],-edot[3],
00510                           edot[0], edot[3],-edot[2],
00511                          -edot[3], edot[0], edot[1],
00512                           edot[2],-edot[1], edot[0]);
00513 
00514         return  Q*wdot + QDot*w_PB_P;
00515     }
00516 
00517 
00518 private:
00519     // This is only for the most trustworthy of callers, that is, methods of the Rotation class. 
00520     // There are a lot of ways for this NOT to be a legitimate rotation matrix -- be careful!!
00521     // Note that these are supplied in rows.
00522     Rotation( const Real& xx, const Real& xy, const Real& xz,
00523               const Real& yx, const Real& yy, const Real& yz,
00524               const Real& zx, const Real& zy, const Real& zz )
00525             : Mat33( xx,xy,xz, yx,yy,yz, zx,zy,zz ) {}
00526 
00527     // These next methods are highly-efficient power-user methods.  Read the documentation to understand them.
00528     SimTK_SimTKCOMMON_EXPORT Rotation&  setTwoAngleTwoAxesBodyFixedForwardCyclicalRotation(     Real cosAngle1, Real sinAngle1, const CoordinateAxis& axis1, Real cosAngle2, Real sinAngle2, const CoordinateAxis& axis2 );
00529     SimTK_SimTKCOMMON_EXPORT Rotation&  setThreeAngleTwoAxesBodyFixedForwardCyclicalRotation(   Real cosAngle1, Real sinAngle1, const CoordinateAxis& axis1, Real cosAngle2, Real sinAngle2, const CoordinateAxis& axis2, Real cosAngle3, Real sinAngle3 );
00530     SimTK_SimTKCOMMON_EXPORT Rotation&  setThreeAngleThreeAxesBodyFixedForwardCyclicalRotation( Real cosAngle1, Real sinAngle1, const CoordinateAxis& axis1, Real cosAngle2, Real sinAngle2, const CoordinateAxis& axis2, Real cosAngle3, Real sinAngle3, const CoordinateAxis& axis3 );
00531 
00532     // These next methods highly-efficient power-user methods convert Rotation matrices to orientation angles are highly-efficient power-user methods.  Read the documentation to understand them.
00533     SimTK_SimTKCOMMON_EXPORT Vec2  convertTwoAxesBodyFixedRotationToTwoAngles(     const CoordinateAxis& axis1, const CoordinateAxis& axis2 ) const;
00534     SimTK_SimTKCOMMON_EXPORT Vec3  convertTwoAxesBodyFixedRotationToThreeAngles(   const CoordinateAxis& axis1, const CoordinateAxis& axis2 ) const;
00535     SimTK_SimTKCOMMON_EXPORT Vec3  convertThreeAxesBodyFixedRotationToThreeAngles( const CoordinateAxis& axis1, const CoordinateAxis& axis2, const CoordinateAxis& axis3 ) const;
00536 
00537 };
00538 
00539 
00544 class InverseRotation : public Mat33::TransposeType {
00545 public:
00546     // Convenient shortcut name
00547     typedef  Mat33::TransposeType  BaseMat;
00548 
00549     // Should not usually construct one of these as they should only occur as expression intermediates.
00550     // But if you must ...
00551     InverseRotation() : BaseMat(1) {}
00552 
00553     // Default constructor and copy constructor
00554     InverseRotation( const InverseRotation& R ) : BaseMat(R) {}
00555     InverseRotation&  operator=( const InverseRotation& R )  { BaseMat::operator=( R.asMat33() );  return *this; }
00556 
00557     // Convert from InverseRotation to Rotation (no cost)
00558     const Rotation&  invert() const { return *reinterpret_cast<const Rotation*>(this); }
00559     Rotation&  updInvert()          { return *reinterpret_cast<Rotation*>(this); }
00560 
00561     // Transpose, and transpose operators (override BaseMat versions of transpose).
00562     const Rotation&  transpose() const  { return invert(); }
00563     const Rotation&  operator~() const  { return invert(); }
00564     Rotation&        updTranspose()     { return updInvert(); }
00565     Rotation&        operator~()        { return updInvert(); }
00566 
00567     // Note that this does not have unit stride.
00568     typedef  UnitVec<BaseMat::RowSpacing>  ColType;
00569     typedef  UnitRow<BaseMat::ColSpacing>  RowType;
00570     const RowType&  row( int i ) const         { return reinterpret_cast<const RowType&>(asMat33()[i]); }
00571     const ColType&  col( int j ) const         { return reinterpret_cast<const ColType&>(asMat33()(j)); }
00572     const ColType&  x() const                  { return col(0); }
00573     const ColType&  y() const                  { return col(1); }
00574     const ColType&  z() const                  { return col(2); }
00575     const RowType&  operator[]( int i ) const  { return row(i); }
00576     const ColType&  operator()( int j ) const  { return col(j); }
00577 
00580     const BaseMat&  asMat33() const  { return *static_cast<const BaseMat*>(this); }
00581     BaseMat         toMat33() const  { return asMat33(); }
00582 
00583 };
00584 
00585 
00586 SimTK_SimTKCOMMON_EXPORT std::ostream&  operator<<( std::ostream& o, const Rotation& m );
00587 
00588 template <int S> inline UnitVec<1>  operator*( const Rotation& R,        const UnitVec<S>& v )       { return UnitVec<1>(R.asMat33()* v.asVec3(),  true); }
00589 template <int S> inline UnitRow<1>  operator*( const UnitRow<S>& r,      const Rotation& R   )       { return UnitRow<1>(r.asRow3() * R.asMat33(), true); }
00590 template <int S> inline UnitVec<1>  operator*( const InverseRotation& R, const UnitVec<S>& v )       { return UnitVec<1>(R.asMat33()* v.asVec3(),  true); }
00591 template <int S> inline UnitRow<1>  operator*( const UnitRow<S>& r,      const InverseRotation& R )  { return UnitRow<1>(r.asRow3() * R.asMat33(), true); }
00592 
00593 inline Rotation::Rotation( const InverseRotation& R) : Mat33( R.asMat33() )  {}
00594 
00595 inline Rotation&  Rotation::operator=(  const InverseRotation& R )  { static_cast<Mat33&>(*this)  = R.asMat33();    return *this; }
00596 inline Rotation&  Rotation::operator*=( const Rotation& R )         { static_cast<Mat33&>(*this) *= R.asMat33();    return *this; }
00597 inline Rotation&  Rotation::operator/=( const Rotation& R )         { static_cast<Mat33&>(*this) *= (~R).asMat33(); return *this; }
00598 inline Rotation&  Rotation::operator*=( const InverseRotation& R )  { static_cast<Mat33&>(*this) *= R.asMat33();    return *this; }
00599 inline Rotation&  Rotation::operator/=( const InverseRotation& R )  { static_cast<Mat33&>(*this) *= (~R).asMat33(); return *this; }
00600 
00601 inline Rotation  operator*( const Rotation&        R1, const Rotation&        R2 )  { return Rotation(R1) *= R2; }
00602 inline Rotation  operator*( const Rotation&        R1, const InverseRotation& R2 )  { return Rotation(R1) *= R2; }
00603 inline Rotation  operator*( const InverseRotation& R1, const Rotation&        R2 )  { return Rotation(R1) *= R2; }
00604 inline Rotation  operator*( const InverseRotation& R1, const InverseRotation& R2 )  { return Rotation(R1) *= R2; }
00605 
00606 inline Rotation operator/( const Rotation&        R1, const Rotation&        R2 )  {return Rotation(R1) /= R2;}
00607 inline Rotation operator/( const Rotation&        R1, const InverseRotation& R2 )  {return Rotation(R1) /= R2;}
00608 inline Rotation operator/( const InverseRotation& R1, const Rotation&        R2 )  {return Rotation(R1) /= R2;}
00609 inline Rotation operator/( const InverseRotation& R1, const InverseRotation& R2 )  {return Rotation(R1) /= R2;}
00610 
00611 
00612 //------------------------------------------------------------------------------
00613 }  // End of namespace SimTK
00614 
00615 //--------------------------------------------------------------------------
00616 #endif // SIMTK_ROTATION_H_
00617 //--------------------------------------------------------------------------
00618 
00619 

Generated on Fri Sep 26 07:44:15 2008 for SimTKcore by  doxygen 1.5.6