Rotation.h

Go to the documentation of this file.

#ifndef SimTK_SimTKCOMMON_ROTATION_H_ 
#define SimTK_SimTKCOMMON_ROTATION_H_ 

/* -------------------------------------------------------------------------- *
 *                       Simbody(tm): SimTKcommon                             *
 * -------------------------------------------------------------------------- *
 * This is part of the SimTK biosimulation toolkit originating from           *
 * Simbios, the NIH National Center for Physics-Based Simulation of           *
 * Biological Structures at Stanford, funded under the NIH Roadmap for        *
 * Medical Research, grant U54 GM072970. See https://simtk.org/home/simbody.  *
 *                                                                            *
 * Portions copyright (c) 2005-14 Stanford University and the Authors.        *
 * Authors: Paul Mitiguy, Michael Sherman                                     *
 * Contributors:                                                              *
 *                                                                            *
 * Licensed under the Apache License, Version 2.0 (the "License"); you may    *
 * not use this file except in compliance with the License. You may obtain a  *
 * copy of the License at http://www.apache.org/licenses/LICENSE-2.0.         *
 *                                                                            *
 * Unless required by applicable law or agreed to in writing, software        *
 * distributed under the License is distributed on an "AS IS" BASIS,          *
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.   *
 * See the License for the specific language governing permissions and        *
 * limitations under the License.                                             *
 * -------------------------------------------------------------------------- */

//------------------------------------------------------------------------------

#include "SimTKcommon/SmallMatrix.h"
#include "SimTKcommon/internal/CoordinateAxis.h"
#include "SimTKcommon/internal/UnitVec.h"
#include "SimTKcommon/internal/Quaternion.h"

//------------------------------------------------------------------------------
#include <iosfwd>  // Forward declaration of iostream
//------------------------------------------------------------------------------

//------------------------------------------------------------------------------
namespace SimTK {


enum BodyOrSpaceType { BodyRotationSequence=0, SpaceRotationSequence=1 };

//------------------------------------------------------------------------------
// Forward declarations
template <class P> class Rotation_;
template <class P> class InverseRotation_;

typedef Rotation_<Real>             Rotation;
typedef Rotation_<float>           fRotation;
typedef Rotation_<double>          dRotation;

typedef InverseRotation_<Real>      InverseRotation;
typedef InverseRotation_<float>    fInverseRotation;
typedef InverseRotation_<double>   dInverseRotation;

//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
template <class P> // templatized by precision
class Rotation_ : public Mat<3,3,P> {
public:
typedef P               RealP; 
typedef Mat<2,2,P>      Mat22P;
typedef Mat<3,2,P>      Mat32P;
typedef Mat<3,3,P>      Mat33P;
typedef Mat<4,3,P>      Mat43P;
typedef Mat<3,4,P>      Mat34P;
typedef Vec<2,P>        Vec2P;
typedef Vec<3,P>        Vec3P;
typedef Vec<4,P>        Vec4P;
typedef UnitVec<P,1>    UnitVec3P; // stride is 1 here, length is always 3
typedef SymMat<3,P>     SymMat33P;
typedef Quaternion_<P>  QuaternionP;

typedef UnitVec<P,Mat33P::RowSpacing> ColType;
typedef UnitRow<P,Mat33P::ColSpacing> RowType;

//------------------------------------------------------------------------------
Rotation_() : Mat33P(1) {}    

Rotation_( const Rotation_& R ) : Mat33P(R)  {}
inline Rotation_( const InverseRotation_<P>& );

Rotation_&  operator=( const Rotation_& R )  
{ Mat33P::operator=( R.asMat33() );  return *this; }
inline Rotation_& operator=( const InverseRotation_<P>& );

Rotation_&  setRotationToNaN()             
{ Mat33P::setToNaN();    return *this; } 

Rotation_&  setRotationToIdentityMatrix()  
{ Mat33P::operator=(RealP(1));  return *this; }

Rotation_( RealP angle, const CoordinateAxis& axis )             
{ setRotationFromAngleAboutAxis( angle, axis ); }
Rotation_& setRotationFromAngleAboutAxis(RealP angle, const CoordinateAxis& axis)  
{ return axis.isXAxis() ? setRotationFromAngleAboutX(angle) 
      : (axis.isYAxis() ? setRotationFromAngleAboutY(angle) 
      : setRotationFromAngleAboutZ(angle) ); }

Rotation_( RealP angle, const CoordinateAxis::XCoordinateAxis )  
{ setRotationFromAngleAboutX( std::cos(angle), std::sin(angle) ); }
Rotation_&  setRotationFromAngleAboutX( RealP angle )  
{ return setRotationFromAngleAboutX( std::cos(angle), std::sin(angle) ); }
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; }

Rotation_( RealP angle, const CoordinateAxis::YCoordinateAxis )  
{ setRotationFromAngleAboutY( std::cos(angle), std::sin(angle) ); }
Rotation_&  setRotationFromAngleAboutY( RealP angle )  
{ return setRotationFromAngleAboutY( std::cos(angle), std::sin(angle) ); }
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; }

Rotation_( RealP angle, const CoordinateAxis::ZCoordinateAxis )  
{ setRotationFromAngleAboutZ( std::cos(angle), std::sin(angle) ); }
Rotation_&  setRotationFromAngleAboutZ( RealP angle )  
{ return setRotationFromAngleAboutZ( std::cos(angle), std::sin(angle) ); }
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; }

Rotation_( RealP angle, const UnitVec3P& unitVector ) 
{ setRotationFromAngleAboutUnitVector(angle,unitVector); }
SimTK_SimTKCOMMON_EXPORT Rotation_&  
setRotationFromAngleAboutUnitVector(RealP angle, const UnitVec3P& unitVector);

Rotation_( RealP angle, const Vec3P& nonUnitVector )  
{ setRotationFromAngleAboutNonUnitVector(angle,nonUnitVector); }
Rotation_&  
setRotationFromAngleAboutNonUnitVector(RealP angle, const Vec3P& nonUnitVector)  
{ return setRotationFromAngleAboutUnitVector( angle, UnitVec3P(nonUnitVector) ); }

Rotation_(BodyOrSpaceType bodyOrSpace, 
          RealP angle1, const CoordinateAxis& axis1, 
          RealP angle2, const CoordinateAxis& axis2)
{ setRotationFromTwoAnglesTwoAxes(bodyOrSpace,angle1,axis1,angle2,axis2); }
SimTK_SimTKCOMMON_EXPORT Rotation_&  
setRotationFromTwoAnglesTwoAxes(BodyOrSpaceType bodyOrSpace, 
                                RealP angle1, const CoordinateAxis& axis1, 
                                RealP angle2, const CoordinateAxis& axis2); 

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); }
SimTK_SimTKCOMMON_EXPORT Rotation_&  
setRotationFromThreeAnglesThreeAxes(BodyOrSpaceType bodyOrSpace, 
                                    RealP angle1, const CoordinateAxis& axis1, 
                                    RealP angle2, const CoordinateAxis& axis2, 
                                    RealP angle3, const CoordinateAxis& axis3);

explicit Rotation_( const QuaternionP& q )  { setRotationFromQuaternion(q); }
SimTK_SimTKCOMMON_EXPORT Rotation_&  
setRotationFromQuaternion( const QuaternionP& q );

explicit Rotation_( const Mat33P& m )  { setRotationFromApproximateMat33(m); }
SimTK_SimTKCOMMON_EXPORT Rotation_&  
setRotationFromApproximateMat33( const Mat33P& m );

Rotation_(const Mat33P& m, bool) : Mat33P(m) {}
Rotation_& setRotationFromMat33TrustMe(const Mat33P& m)  
{ Mat33P& R = *this; R=m;  return *this; }   
Rotation_& setRotationColFromUnitVecTrustMe(int colj, const UnitVec3P& uvecj)  
{ Mat33P& R = *this; R(colj)=uvecj.asVec3(); return *this; }   
Rotation_& setRotationFromUnitVecsTrustMe
   (const UnitVec3P& colA, const UnitVec3P& colB, const UnitVec3P& colC)  
{ Mat33P& R = *this; R(0)=colA.asVec3(); R(1)=colB.asVec3(); R(2)=colC.asVec3();
  return *this; }  

Rotation_(const UnitVec3P& uvec, CoordinateAxis axis)  
{ setRotationFromOneAxis(uvec,axis); }
SimTK_SimTKCOMMON_EXPORT Rotation_&  
setRotationFromOneAxis(const UnitVec3P& uvec, CoordinateAxis axis);

Rotation_(const UnitVec3P& uveci, const CoordinateAxis& axisi, 
          const Vec3P& vecjApprox, const CoordinateAxis& axisjApprox )  
{ setRotationFromTwoAxes(uveci,axisi,vecjApprox,axisjApprox); }
SimTK_SimTKCOMMON_EXPORT Rotation_&  
setRotationFromTwoAxes(const UnitVec3P& uveci, const CoordinateAxis& axisi, 
                       const Vec3P& vecjApprox, const CoordinateAxis& axisjApprox );

void setRotationToBodyFixedXY( const Vec2P& v)   
{ setRotationFromTwoAnglesTwoAxes(BodyRotationSequence,
                                  v[0],XAxis, v[1],YAxis); }

void setRotationToBodyFixedXYZ( const Vec3P& v)  
{ setRotationFromThreeAnglesThreeAxes(BodyRotationSequence, 
                                      v[0],XAxis, v[1],YAxis, v[2],ZAxis ); }
void setRotationToBodyFixedXYZ(const Vec3P& c, const Vec3P& s) {
    Mat33P& R = *this;
    const RealP s0s1=s[0]*s[1], s2c0=s[2]*c[0], c0c2=c[0]*c[2], nc1= -c[1];

    R = Mat33P(     c[1]*c[2]      ,         s[2]*nc1       ,    s[1]  ,
                s2c0 + s0s1*c[2]   ,     c0c2 - s0s1*s[2]   , s[0]*nc1 ,
                s[0]*s[2] - s[1]*c0c2 ,  s[0]*c[2] + s[1]*s2c0 , c[0]*c[1] );
}

//------------------------------------------------------------------------------
const InverseRotation_<P>&  operator~() const  { return invert(); }
InverseRotation_<P>&        operator~()        { return updInvert(); }

const InverseRotation_<P>&  transpose() const  { return invert(); }
InverseRotation_<P>&        updTranspose()     { return updInvert(); }

const InverseRotation_<P>& invert() const  
{ return *reinterpret_cast<const InverseRotation_<P>*>(this); }
InverseRotation_<P>& updInvert()     
{ return *reinterpret_cast<InverseRotation_<P>*>(this); }

inline Rotation_&  operator*=( const Rotation_<P>& R );
inline Rotation_&  operator*=( const InverseRotation_<P>& );

inline Rotation_&  operator/=( const Rotation_<P>& R );
inline Rotation_&  operator/=( const InverseRotation_<P>& );

static Vec3P multiplyByBodyXYZ_N_P(const Vec2P& cosxy,
                                    const Vec2P& sinxy,
                                    RealP        oocosy,
                                    const Vec3P& w_PB)
{
    const RealP s0 = sinxy[0], c0 = cosxy[0];
    const RealP s1 = sinxy[1];
    const RealP w0 = w_PB[0], w1 = w_PB[1], w2 = w_PB[2];

    const RealP t = (s0*w1-c0*w2)*oocosy;
    return Vec3P( w0 + t*s1, c0*w1 + s0*w2, -t ); // qdot
}

static Vec3P multiplyByBodyXYZ_NT_P(const Vec2P& cosxy,
                                    const Vec2P& sinxy,
                                    RealP        oocosy,
                                    const Vec3P& q)
{
    const RealP s0 = sinxy[0], c0 = cosxy[0];
    const RealP s1 = sinxy[1];
    const RealP q0 = q[0], q1 = q[1], q2 = q[2];

    const RealP t = (q0*s1-q2) * oocosy;
    return Vec3P( q0, c0*q1 + t*s0, s0*q1 - t*c0 ); // v_P
}

static Vec3P multiplyByBodyXYZ_NInv_P(const Vec2P& cosxy,
                                      const Vec2P& sinxy,
                                      const Vec3P& qdot)
{
    const RealP s0 = sinxy[0], c0 = cosxy[0];
    const RealP s1 = sinxy[1], c1 = cosxy[1];
    const RealP q0 = qdot[0], q1 = qdot[1], q2 = qdot[2];
    const RealP c1q2 = c1*q2;

    return Vec3P( q0 + s1*q2,           // w_PB
                  c0*q1 - s0*c1q2, 
                  s0*q1 + c0*c1q2 );
}

static Vec3P multiplyByBodyXYZ_NInvT_P(const Vec2P& cosxy,
                                       const Vec2P& sinxy,
                                       const Vec3P& v_P)
{
    const RealP s0 = sinxy[0], c0 = cosxy[0];
    const RealP s1 = sinxy[1], c1 = cosxy[1];
    const RealP w0 = v_P[0], w1 = v_P[1], w2 = v_P[2];

    return Vec3P( w0,                           // qdot-like
                  c0*w1 + s0*w2,
                  s1*w0 - s0*c1*w1 + c0*c1*w2);
}

//------------------------------------------------------------------------------
const RowType&  row( int i ) const         
{ return reinterpret_cast<const RowType&>(asMat33()[i]); }
const RowType&  operator[]( int i ) const  { return row(i); }

const ColType&  col( int j ) const         
{ return reinterpret_cast<const ColType&>(asMat33()(j)); }
const ColType&  operator()( int j ) const  { return col(j); }

const ColType&  x() const                  { return col(0); }
const ColType&  y() const                  { return col(1); }
const ColType&  z() const                  { return col(2); }

const ColType& getAxisUnitVec(CoordinateAxis axis) const 
{   return col(axis); }

const UnitVec<P,1> getAxisUnitVec(CoordinateDirection dir) const {
    const ColType& axDir = getAxisUnitVec(dir.getAxis());
    return dir.getDirection() > 0 ? UnitVec<P,1>( axDir) 
                                  : UnitVec<P,1>(-axDir); // cheap 
}

//------------------------------------------------------------------------------
static Mat33P calcNForBodyXYZInBodyFrame(const Vec3P& q) {
    // Note: q[0] is not referenced so we won't waste time calculating
    // its cosine and sine here.
    return calcNForBodyXYZInBodyFrame
        (Vec3P(0, std::cos(q[1]), std::cos(q[2])),
        Vec3P(0, std::sin(q[1]), std::sin(q[2])));
}

static Mat33P calcNForBodyXYZInBodyFrame(const Vec3P& cq, const Vec3P& sq) {
    const RealP s1 = sq[1], c1 = cq[1];
    const RealP s2 = sq[2], c2 = cq[2];
    const RealP ooc1  = 1/c1;
    const RealP s2oc1 = s2*ooc1, c2oc1 = c2*ooc1;

    return Mat33P(    c2oc1  , -s2oc1  , 0,
                        s2   ,    c2   , 0,
                    -s1*c2oc1 , s1*s2oc1, 1 );
}

static Mat33P calcNForBodyXYZInParentFrame(const Vec3P& q) {
    // Note: q[2] is not referenced so we won't waste time calculating
    // its cosine and sine here.
    return calcNForBodyXYZInParentFrame
        (Vec3P(std::cos(q[0]), std::cos(q[1]), 0),
        Vec3P(std::sin(q[0]), std::sin(q[1]), 0));
}

static Mat33P calcNForBodyXYZInParentFrame(const Vec3P& cq, const Vec3P& sq) {
    const RealP s0 = sq[0], c0 = cq[0];
    const RealP s1 = sq[1], c1 = cq[1];
    const RealP ooc1  = 1/c1;
    const RealP s0oc1 = s0*ooc1, c0oc1 = c0*ooc1;

    return Mat33P( 1 , s1*s0oc1 , -s1*c0oc1,
                    0 ,    c0    ,    s0,
                    0 ,  -s0oc1  ,  c0oc1 );
}

static Mat33P calcNDotForBodyXYZInBodyFrame
   (const Vec3P& q, const Vec3P& qdot) {
    // Note: q[0] is not referenced so we won't waste time calculating
    // its cosine and sine here.
    return calcNDotForBodyXYZInBodyFrame
        (Vec3P(0, std::cos(q[1]), std::cos(q[2])),
        Vec3P(0, std::sin(q[1]), std::sin(q[2])),
        qdot);
}

static Mat33P calcNDotForBodyXYZInBodyFrame
   (const Vec3P& cq, const Vec3P& sq, const Vec3P& qdot) 
{
    const RealP s1 = sq[1], c1 = cq[1];
    const RealP s2 = sq[2], c2 = cq[2];
    const RealP ooc1  = 1/c1;
    const RealP s2oc1 = s2*ooc1, c2oc1 = c2*ooc1;

    const RealP t = qdot[1]*s1*ooc1;
    const RealP a = t*s2oc1 + qdot[2]*c2oc1; // d/dt s2oc1
    const RealP b = t*c2oc1 - qdot[2]*s2oc1; // d/dt c2oc1

    return Mat33P(       b             ,        -a         , 0,
                        qdot[2]*c2       ,    -qdot[2]*s2    , 0,
                    -(s1*b + qdot[1]*c2) , s1*a + qdot[1]*s2 , 0 );
}

static Mat33P calcNDotForBodyXYZInParentFrame
   (const Vec3P& q, const Vec3P& qdot) {
    // Note: q[2] is not referenced so we won't waste time calculating
    // its cosine and sine here.
    const RealP cy = std::cos(q[1]); // cos(y)
    return calcNDotForBodyXYZInParentFrame
        (Vec2P(std::cos(q[0]), cy), 
        Vec2P(std::sin(q[0]), std::sin(q[1])),
        1/cy, qdot);
}

static Mat33P calcNDotForBodyXYZInParentFrame
   (const Vec2P& cq, const Vec2P& sq, RealP ooc1, const Vec3P& qdot) {
    const RealP s0 = sq[0], c0 = cq[0];
    const RealP s1 = sq[1];
    const RealP s0oc1 = s0*ooc1, c0oc1 = c0*ooc1;

    const RealP t = qdot[1]*s1*ooc1;
    const RealP a = t*s0oc1 + qdot[0]*c0oc1; // d/dt s0oc1
    const RealP b = t*c0oc1 - qdot[0]*s0oc1; // d/dt c0oc1

    return Mat33P( 0,  s1*a + qdot[1]*s0, -(s1*b + qdot[1]*c0), 
                   0,    -qdot[0]*s0    ,     qdot[0]*c0      ,
                   0,        -a         ,         b            );
}

static Mat33P calcNInvForBodyXYZInBodyFrame(const Vec3P& q) {
    // Note: q[0] is not referenced so we won't waste time calculating
    // its cosine and sine here.
    return calcNInvForBodyXYZInBodyFrame
       (Vec3P(0, std::cos(q[1]), std::cos(q[2])),
        Vec3P(0, std::sin(q[1]), std::sin(q[2])));
}

static Mat33P calcNInvForBodyXYZInBodyFrame
   (const Vec3P& cq, const Vec3P& sq) {
    const RealP s1 = sq[1], c1 = cq[1];
    const RealP s2 = sq[2], c2 = cq[2];

    return Mat33P( c1*c2 ,  s2 , 0 ,
                  -c1*s2 ,  c2 , 0 ,
                    s1   ,  0  , 1 );
}

static Mat33P calcNInvForBodyXYZInParentFrame(const Vec3P& q) {
    // Note: q[0] is not referenced so we won't waste time calculating
    // its cosine and sine here.
    return calcNInvForBodyXYZInParentFrame
       (Vec3P(std::cos(q[0]), std::cos(q[1]), 0),
        Vec3P(std::sin(q[0]), std::sin(q[1]), 0));
}

static Mat33P calcNInvForBodyXYZInParentFrame
    (const Vec3P& cq, const Vec3P& sq) {
    const RealP s0 = sq[0], c0 = cq[0];
    const RealP s1 = sq[1], c1 = cq[1];

    return Mat33P( 1 ,  0  ,   s1   ,
                    0 ,  c0 , -s0*c1 ,
                    0 ,  s0 ,  c0*c1 );
}

static Mat43P calcUnnormalizedNForQuaternion(const Vec4P& q) {
    const Vec4P e = q/2;
    const RealP ne1 = -e[1], ne2 = -e[2], ne3 = -e[3];
    return Mat43P( ne1,  ne2,  ne3,
                    e[0], e[3], ne2,
                    ne3,  e[0], e[1],
                    e[2], ne1,  e[0]);
}

static Mat43P calcUnnormalizedNDotForQuaternion(const Vec4P& qdot) {
    const Vec4P ed = qdot/2;
    const RealP ned1 = -ed[1], ned2 = -ed[2], ned3 = -ed[3];
    return Mat43P( ned1,  ned2,  ned3,
                    ed[0], ed[3], ned2,
                    ned3,  ed[0], ed[1],
                    ed[2], ned1,  ed[0]);
}

static Mat34P calcUnnormalizedNInvForQuaternion(const Vec4P& q) {
    const Vec4P e = 2*q;
    const RealP ne1 = -e[1], ne2 = -e[2], ne3 = -e[3];
    return Mat34P(ne1, e[0], ne3,  e[2],
                    ne2, e[3], e[0], ne1,
                    ne3, ne2,  e[1], e[0]);
}

//------------------------------------------------------------------------------
const Mat33P&  asMat33() const  { return *static_cast<const Mat33P*>(this); }
Mat33P         toMat33() const  { return asMat33(); }

SimTK_SimTKCOMMON_EXPORT SymMat33P 
reexpressSymMat33(const SymMat33P& S_BB) const;

// Converts rotation matrix to one or two or three orientation angles.
// Note:  The result is most meaningful if the Rotation_ matrix is one that can 
// be produced by such a sequence.
// Use1:  someRotation.convertOneAxisRotationToOneAngle( XAxis );
// Use2:  someRotation.convertTwoAxesRotationToTwoAngles
//                                (SpaceRotationSequence, YAxis, ZAxis );
// Use3:  someRotation.convertThreeAxesRotationToThreeAngles
//                                (SpaceRotationSequence, ZAxis, YAxis, XAxis );
// Use4:  someRotation.convertRotationToAngleAxis();   
//        Return: [angleInRadians, unitVectorX, unitVectorY, unitVectorZ].

SimTK_SimTKCOMMON_EXPORT RealP  
convertOneAxisRotationToOneAngle( const CoordinateAxis& axis1 ) const;

SimTK_SimTKCOMMON_EXPORT Vec2P  
convertTwoAxesRotationToTwoAngles(BodyOrSpaceType bodyOrSpace, 
                                  const CoordinateAxis& axis1, 
                                  const CoordinateAxis& axis2) const;

SimTK_SimTKCOMMON_EXPORT Vec3P  
convertThreeAxesRotationToThreeAngles
   (BodyOrSpaceType bodyOrSpace, const CoordinateAxis& axis1, 
    const CoordinateAxis& axis2, const CoordinateAxis& axis3 ) const;

SimTK_SimTKCOMMON_EXPORT QuaternionP convertRotationToQuaternion() const;

Vec4P convertRotationToAngleAxis() const  
{ return convertRotationToQuaternion().convertQuaternionToAngleAxis(); }

Vec2P convertRotationToBodyFixedXY() const   
{ return convertTwoAxesRotationToTwoAngles(BodyRotationSequence,XAxis,YAxis); }
Vec3P convertRotationToBodyFixedXYZ() const  
{ return convertThreeAxesRotationToThreeAngles( BodyRotationSequence, 
                                                XAxis, YAxis, ZAxis ); }

static Vec3P convertAngVelToBodyFixed321Dot(const Vec3P& q, const Vec3P& w_PB_B) {
    const RealP s1 = std::sin(q[1]), c1 = std::cos(q[1]);
    const RealP s2 = std::sin(q[2]), c2 = std::cos(q[2]);
    const RealP ooc1 = RealP(1)/c1;
    const RealP s2oc1 = s2*ooc1, c2oc1 = c2*ooc1;

    const Mat33P E( 0,    s2oc1  ,  c2oc1  ,
                    0,      c2   ,   -s2   ,
                    1,  s1*s2oc1 , s1*c2oc1 );
    return E * w_PB_B;
}

static Vec3P convertBodyFixed321DotToAngVel(const Vec3P& q, const Vec3P& qd) {
    const RealP s1 = std::sin(q[1]), c1 = std::cos(q[1]);
    const RealP s2 = std::sin(q[2]), c2 = std::cos(q[2]);

    const Mat33P Einv(  -s1  ,  0  ,  1 ,
                        c1*s2 ,  c2 ,  0 ,
                        c1*c2 , -s2 ,  0 );
    return Einv*qd;
}

// TODO: sherm: is this right? Warning: everything is measured in the
// *PARENT* frame, but angular velocities and accelerations are
// expressed in the *BODY* frame.
// TODO: this is not an efficient way to do this computation.
static Vec3P convertAngVelDotToBodyFixed321DotDot
    (const Vec3P& q, const Vec3P& w_PB_B, const Vec3P& wdot_PB_B)
{
    const RealP s1 = std::sin(q[1]), c1 = std::cos(q[1]);
    const RealP s2 = std::sin(q[2]), c2 = std::cos(q[2]);
    const RealP ooc1  = 1/c1;
    const RealP s2oc1 = s2*ooc1, c2oc1 = c2*ooc1, s1oc1 = s1*ooc1;

    const Mat33P E( 0 ,   s2oc1  ,  c2oc1  ,
                    0 ,     c2   ,   -s2   ,
                    1 , s1*s2oc1 , s1*c2oc1 );
    const Vec3P qdot = E * w_PB_B;

    const RealP t = qdot[1]*s1oc1;
    const RealP a = t*s2oc1 + qdot[2]*c2oc1; // d/dt s2oc1
    const RealP b = t*c2oc1 - qdot[2]*s2oc1; // d/dt c2oc1

    const Mat33P Edot( 0 ,       a           ,         b         ,
                        0 ,   -qdot[2]*s2     ,    -qdot[2]*c2    ,
                        0 , s1*a + qdot[1]*s2 , s1*b + qdot[1]*c2 );

    return E*wdot_PB_B + Edot*w_PB_B;
}

static Vec3P convertAngVelInBodyFrameToBodyXYZDot
    (const Vec3P& q, const Vec3P& w_PB_B) {  
    return convertAngVelInBodyFrameToBodyXYZDot
        (Vec3P(0, std::cos(q[1]), std::cos(q[2])),
        Vec3P(0, std::sin(q[1]), std::sin(q[2])),
        w_PB_B); 
}

//TODO: reimplement
static Vec3P convertAngVelInBodyFrameToBodyXYZDot
   (const Vec3P& cq, const Vec3P& sq, const Vec3P& w_PB_B) 
{   return calcNForBodyXYZInBodyFrame(cq,sq)*w_PB_B; }

static Vec3P convertBodyXYZDotToAngVelInBodyFrame
   (const Vec3P& q, const Vec3P& qdot) {   
        return convertBodyXYZDotToAngVelInBodyFrame
                   (Vec3P(0, std::cos(q[1]), std::cos(q[2])),
                    Vec3P(0, std::sin(q[1]), std::sin(q[2])),
                    qdot); 
}

// TODO: reimplement
static Vec3P convertBodyXYZDotToAngVelInBodyFrame
    (const Vec3P& cq, const Vec3P& sq, const Vec3P& qdot) 
{   return calcNInvForBodyXYZInBodyFrame(cq,sq)*qdot; }

// TODO: sherm: is this right?
static Vec3P convertAngVelDotInBodyFrameToBodyXYZDotDot
   (const Vec3P& q, const Vec3P& w_PB_B, const Vec3P& wdot_PB_B)
{
    // Note: q[0] is not referenced so we won't waste time calculating
    // its cosine and sine here.
    return convertAngVelDotInBodyFrameToBodyXYZDotDot
               (Vec3P(0, std::cos(q[1]), std::cos(q[2])),
                Vec3P(0, std::sin(q[1]), std::sin(q[2])),
                w_PB_B, wdot_PB_B);
}

// TODO: reimplement
static Vec3P convertAngVelDotInBodyFrameToBodyXYZDotDot
   (const Vec3P& cq, const Vec3P& sq, 
    const Vec3P& w_PB_B, const Vec3P& wdot_PB_B)
{
    const Mat33P N    = calcNForBodyXYZInBodyFrame(cq,sq);         // ~12 flops
    const Vec3P  qdot = N * w_PB_B;                                //  15 flops
    const Mat33P NDot = calcNDotForBodyXYZInBodyFrame(cq,sq,qdot); // ~30 flops

    return N*wdot_PB_B + NDot*w_PB_B;                              //  33 flops
}

static Vec4P convertAngVelToQuaternionDot(const Vec4P& q, const Vec3P& w_PB_P) {
    return calcUnnormalizedNForQuaternion(q)*w_PB_P;
}

static Vec3P convertQuaternionDotToAngVel(const Vec4P& q, const Vec4P& qdot) {
    return calcUnnormalizedNInvForQuaternion(q)*qdot;
}

static Vec4P convertAngVelDotToQuaternionDotDot
    (const Vec4P& q, const Vec3P& w_PB, const Vec3P& b_PB)
{
    const Mat43P N     = calcUnnormalizedNForQuaternion(q); //  7 flops
    const Vec4P  Nb    = N*b_PB;                            // 20 flops
    const Vec4P  NDotw = RealP(-.25)*w_PB.normSqr()*q;      // 10 flops
    return Nb + NDotw;                                      //  4 flops
}

static Vec3P convertAngVelInParentToBodyXYZDot
    (const Vec2P& cosxy,  
    const Vec2P& sinxy,  
    RealP        oocosy, 
    const Vec3P& w_PB)   
{
    return multiplyByBodyXYZ_N_P(cosxy,sinxy,oocosy,w_PB);
}

static Vec3P convertAngAccInParentToBodyXYZDotDot
    (const Vec2P& cosxy,  
    const Vec2P& sinxy,  
    RealP        oocosy, 
    const Vec3P& qdot,   
    const Vec3P& b_PB)   
{
    const RealP s1 = sinxy[1], c1 = cosxy[1];
    const RealP q0 = qdot[0], q1 = qdot[1], q2 = qdot[2];

    // 10 flops
    const Vec3P Nb = multiplyByBodyXYZ_N_P(cosxy,sinxy,oocosy,b_PB);

    const RealP q1oc1 = q1*oocosy;
    const Vec3P NDotw((q0*s1-q2)*q1oc1,     //   NDot_P*w_PB
                        q0*q2*c1,            // = NDot_P*(NInv_P*qdot)
                        (q2*s1-q0)*q1oc1 );   // (9 flops)

    return Nb + NDotw; // 3 flops
}

//------------------------------------------------------------------------------
SimTK_SimTKCOMMON_EXPORT bool  
isSameRotationToWithinAngle(const Rotation_& R, RealP okPointingAngleErrorRads) 
                                                                        const;

bool isSameRotationToWithinAngleOfMachinePrecision(const Rotation_& R) const       
{ return isSameRotationToWithinAngle( R, NTraits<P>::getSignificant() ); }

RealP getMaxAbsDifferenceInRotationElements( const Rotation_& R ) const {            
    const Mat33P& A=asMat33(); const Mat33P& B=R.asMat33(); RealP maxDiff=0;  
    for( int i=0;  i<=2; i++ ) for( int j=0; j<=2; j++ ) {
        const RealP absDiff = std::abs(A[i][j] - B[i][j]);  
        if( absDiff > maxDiff ) maxDiff = absDiff; 
    }
    return maxDiff; 
} 

bool areAllRotationElementsSameToEpsilon(const Rotation_& R, RealP epsilon) const 
{ return getMaxAbsDifferenceInRotationElements(R) <= epsilon; }

bool areAllRotationElementsSameToMachinePrecision( const Rotation_& R ) const       
{ return areAllRotationElementsSameToEpsilon(R, NTraits<P>::getSignificant()); } 


private:
// This is only for the most trustworthy of callers, that is, methods of 
// the Rotation_ class.  There are a lot of ways for this NOT to be a 
// legitimate rotation matrix -- be careful!!
// Note that these are supplied in rows.
Rotation_( const RealP& xx, const RealP& xy, const RealP& xz,
            const RealP& yx, const RealP& yy, const RealP& yz,
            const RealP& zx, const RealP& zy, const RealP& zz )
:   Mat33P( xx,xy,xz, yx,yy,yz, zx,zy,zz ) {}

// These next methods are highly-efficient power-user methods. Read the 
// code to understand them.
SimTK_SimTKCOMMON_EXPORT Rotation_&  
setTwoAngleTwoAxesBodyFixedForwardCyclicalRotation
   (RealP cosAngle1, RealP sinAngle1, const CoordinateAxis& axis1, 
    RealP cosAngle2, RealP sinAngle2, const CoordinateAxis& axis2 );
SimTK_SimTKCOMMON_EXPORT Rotation_&  
setThreeAngleTwoAxesBodyFixedForwardCyclicalRotation
   (RealP cosAngle1, RealP sinAngle1, const CoordinateAxis& axis1, 
    RealP cosAngle2, RealP sinAngle2, const CoordinateAxis& axis2, 
    RealP cosAngle3, RealP sinAngle3 );
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 );

// These next methods are highly-efficient power-user methods to convert 
// Rotation matrices to orientation angles.  Read the code to understand them.
SimTK_SimTKCOMMON_EXPORT Vec2P  
convertTwoAxesBodyFixedRotationToTwoAngles
   (const CoordinateAxis& axis1, const CoordinateAxis& axis2 ) const;
SimTK_SimTKCOMMON_EXPORT Vec3P  
convertTwoAxesBodyFixedRotationToThreeAngles
   (const CoordinateAxis& axis1, const CoordinateAxis& axis2 ) const;
SimTK_SimTKCOMMON_EXPORT Vec3P  
convertThreeAxesBodyFixedRotationToThreeAngles
   (const CoordinateAxis& axis1, const CoordinateAxis& axis2, 
    const CoordinateAxis& axis3 ) const;

//------------------------------------------------------------------------------
// These are obsolete names from a previous release, listed here so that 
// users will get a decipherable compilation error. (sherm 091101)
//------------------------------------------------------------------------------
private:
// REPLACED BY: calcNForBodyXYZInBodyFrame()
static Mat33P calcQBlockForBodyXYZInBodyFrame(const Vec3P& a)
{   return calcNForBodyXYZInBodyFrame(a); }
// REPLACED BY: calcNInvForBodyXYZInBodyFrame()
static Mat33P calcQInvBlockForBodyXYZInBodyFrame(const Vec3P& a)
{   return calcNInvForBodyXYZInBodyFrame(a); }
// REPLACED BY: calcUnnormalizedNForQuaternion()
static Mat<4,3,P> calcUnnormalizedQBlockForQuaternion(const Vec4P& q)
{   return calcUnnormalizedNForQuaternion(q); }
// REPLACED BY: calcUnnormalizedNInvForQuaternion()
static Mat<3,4,P> calcUnnormalizedQInvBlockForQuaternion(const Vec4P& q)
{   return calcUnnormalizedNInvForQuaternion(q); }
// REPLACED BY: convertAngVelInBodyFrameToBodyXYZDot
static Vec3P convertAngVelToBodyFixed123Dot(const Vec3P& q, const Vec3P& w_PB_B) 
{   return convertAngVelInBodyFrameToBodyXYZDot(q,w_PB_B); }
// REPLACED BY: convertBodyXYZDotToAngVelInBodyFrame
static Vec3P convertBodyFixed123DotToAngVel(const Vec3P& q, const Vec3P& qdot) 
{   return convertBodyXYZDotToAngVelInBodyFrame(q,qdot); }
// REPLACED BY: convertAngVelDotInBodyFrameToBodyXYZDotDot
static Vec3P convertAngVelDotToBodyFixed123DotDot
    (const Vec3P& q, const Vec3P& w_PB_B, const Vec3P& wdot_PB_B)
{   return convertAngVelDotInBodyFrameToBodyXYZDotDot(q,w_PB_B,wdot_PB_B); }

//------------------------------------------------------------------------------
// The following code is obsolete - it is here temporarily for backward 
// compatibility (Mitiguy 9/5/2007)
//------------------------------------------------------------------------------
private:
// These static methods are like constructors with friendlier names.
static Rotation_ zero() { return Rotation_(); }
static Rotation_ NaN()  { Rotation_ r;  r.setRotationToNaN();  return r; }

Rotation_&  setToZero()            { return setRotationToIdentityMatrix(); }
Rotation_&  setToIdentityMatrix()  { return setRotationToIdentityMatrix(); }
Rotation_&  setToNaN()             { return setRotationToNaN(); }
static Rotation_  trustMe( const Mat33P& m )  { return Rotation_(m,true); }

// One-angle rotations.
static Rotation_ aboutX( const RealP& angleInRad ) 
{ return Rotation_( angleInRad, XAxis ); }
static Rotation_ aboutY( const RealP& angleInRad ) 
{ return Rotation_( angleInRad, YAxis ); }
static Rotation_ aboutZ( const RealP& angleInRad ) 
{ return Rotation_( angleInRad, ZAxis ); }
static Rotation_ aboutAxis( const RealP& angleInRad, const UnitVec3P& axis ) 
{ return Rotation_(angleInRad,axis); }
static Rotation_ aboutAxis( const RealP& angleInRad, const Vec3P& axis )     
{ return Rotation_(angleInRad,axis); }
void setToRotationAboutZ( const RealP& q ) { setRotationFromAngleAboutZ( q ); }

// Two-angle space-fixed rotations.
static Rotation_ aboutXThenOldY(const RealP& xInRad, const RealP& yInRad) 
{ return Rotation_( SpaceRotationSequence, xInRad, XAxis, yInRad, YAxis ); }
static Rotation_ aboutYThenOldX(const RealP& yInRad, const RealP& xInRad) 
{ return Rotation_( SpaceRotationSequence, yInRad, YAxis, xInRad, XAxis ); }
static Rotation_ aboutXThenOldZ(const RealP& xInRad, const RealP& zInRad) 
{ return Rotation_( SpaceRotationSequence, xInRad, XAxis, zInRad, ZAxis ); }
static Rotation_ aboutZThenOldX(const RealP& zInRad, const RealP& xInRad) 
{ return Rotation_( SpaceRotationSequence, zInRad, ZAxis, xInRad, XAxis ); }
static Rotation_ aboutYThenOldZ(const RealP& yInRad, const RealP& zInRad) 
{ return Rotation_( SpaceRotationSequence, yInRad, YAxis, zInRad, ZAxis ); }
static Rotation_ aboutZThenOldY(const RealP& zInRad, const RealP& yInRad) 
{ return Rotation_( SpaceRotationSequence, zInRad, ZAxis, yInRad, YAxis ); }

// Two-angle body fixed rotations (reversed space-fixed ones).
static Rotation_ aboutXThenNewY(const RealP& xInRad, const RealP& yInRad) 
{ return Rotation_( BodyRotationSequence, xInRad, XAxis, yInRad, YAxis ); }
static Rotation_ aboutYThenNewX(const RealP& yInRad, const RealP& xInRad)
{ return aboutXThenOldY(xInRad, yInRad); }
static Rotation_ aboutXThenNewZ(const RealP& xInRad, const RealP& zInRad)
{ return aboutZThenOldX(zInRad, xInRad); }
static Rotation_ aboutZThenNewX(const RealP& zInRad, const RealP& xInRad)
{ return aboutXThenOldZ(xInRad, zInRad); }
static Rotation_ aboutYThenNewZ(const RealP& yInRad, const RealP& zInRad)
{ return aboutZThenOldY(zInRad, yInRad); }
static Rotation_ aboutZThenNewY(const RealP& zInRad, const RealP& yInRad)
{ return aboutYThenOldZ(yInRad, zInRad); }

// Create a Rotation_ matrix by specifying only its z axis. 
// This will work for any stride UnitVec because there is always an implicit 
// conversion available to the packed form used as the argument.
explicit Rotation_( const UnitVec3P& uvecZ )  
{ setRotationFromOneAxis(uvecZ,ZAxis); }

// Create a Rotation_ matrix by specifying its x axis, and a "y like" axis. 
// We will take x seriously after normalizing, but use the y only to create 
// z = normalize(x X y), then y = z X x. Bad things happen if x and y are 
// aligned but we may not catch it.
Rotation_( const Vec3P& x, const Vec3P& yish )  
{ setRotationFromTwoAxes( UnitVec3P(x), XAxis, yish, YAxis ); }

// Set this Rotation_ to represent the same rotation as the passed-in quaternion.
void setToQuaternion( const QuaternionP& q )  { setRotationFromQuaternion(q); }

// Set this Rotation_ to represent a rotation of +q0 about body frame's Z axis, 
// followed by a rotation of +q1 about the body frame's NEW Y axis, 
// followed by a rotation of +q3 about the body frame's NEW X axis.
// See Kane, Spacecraft Dynamics, pg. 423, body-three: 3-2-1.
//  Similarly for BodyFixed123.
void setToBodyFixed321( const Vec3P& v)  
{ setRotationFromThreeAnglesThreeAxes(BodyRotationSequence, 
                                      v[0], ZAxis, v[1], YAxis, v[2], XAxis ); }
void setToBodyFixed123( const Vec3P& v)  
{ setRotationToBodyFixedXYZ(v); }

// Convert this Rotation_ matrix to an equivalent (angle,axis) representation: 
// Returned Vec4P is [angleInRadians, unitVectorX, unitVectorY, unitVectorZ].
Vec4P convertToAngleAxis() const  
{ return convertRotationToAngleAxis(); }

// Convert this Rotation_ matrix to equivalent quaternion representation.
QuaternionP convertToQuaternion() const  
{ return convertRotationToQuaternion(); }

// Set this Rotation_ to represent a rotation of +q0 about base frame's X axis, 
// followed by a rotation of +q1 about the base frame's (unchanged) Y axis.
void setToSpaceFixed12( const Vec2P& q ) 
{ setRotationFromTwoAnglesTwoAxes(SpaceRotationSequence,q[0],XAxis,q[1],YAxis);}

// Convert this Rotation_ matrix to the equivalent 1-2-3 body fixed Euler angle 
// sequence. Similarly, convert Rotation_ matrix to the equivalent 1-2 body  
// fixed Euler angle sequence. Similarly, convert Rotation_ matrix to the 
// equivalent 1-2 space fixed Euler angle sequence. 
Vec3P  convertToBodyFixed123() const  
{ return convertRotationToBodyFixedXYZ(); }
Vec2P  convertToBodyFixed12() const   
{ return convertRotationToBodyFixedXY(); }
Vec2P  convertToSpaceFixed12() const  
{ return convertTwoAxesRotationToTwoAngles(SpaceRotationSequence,XAxis,YAxis); }
};


//-----------------------------------------------------------------------------
//-----------------------------------------------------------------------------
template <class P>
class InverseRotation_ : public Mat<3,3,P>::TransposeType {
typedef P               RealP;
typedef Rotation_<P>    RotationP;
typedef Mat<3,3,P>      Mat33P; // not the base type!
typedef SymMat<3,P>     SymMat33P;
typedef Mat<2,2,P>      Mat22P;
typedef Mat<3,2,P>      Mat32P;
typedef Vec<2,P>        Vec2P;
typedef Vec<3,P>        Vec3P;
typedef Vec<4,P>        Vec4P;
typedef Quaternion_<P>  QuaternionP;
public:
typedef typename Mat<3,3,P>::TransposeType  BaseMat;

typedef  UnitVec<P,BaseMat::RowSpacing>  ColType;
typedef  UnitRow<P,BaseMat::ColSpacing>  RowType;

InverseRotation_() : BaseMat(1) {}

InverseRotation_( const InverseRotation_& R ) : BaseMat(R) {}
InverseRotation_&  operator=( const InverseRotation_& R )  
{   BaseMat::operator=(R.asMat33());  return *this; }

SimTK_SimTKCOMMON_EXPORT SymMat33P 
reexpressSymMat33(const SymMat33P& S_BB) const;

const RotationP&  invert() const 
{return *reinterpret_cast<const RotationP*>(this);}
RotationP&        updInvert() {return *reinterpret_cast<RotationP*>(this);}

const RotationP&  transpose() const  { return invert(); }
const RotationP&  operator~() const  { return invert(); }
RotationP&        updTranspose()     { return updInvert(); }
RotationP&        operator~()        { return updInvert(); }

const RowType&  row( int i ) const         
{ return reinterpret_cast<const RowType&>(asMat33()[i]); }
const RowType&  operator[]( int i ) const  { return row(i); }
const ColType&  col( int j ) const         
{ return reinterpret_cast<const ColType&>(asMat33()(j)); }
const ColType&  operator()( int j ) const  { return col(j); }
const ColType&  x() const                  { return col(0); }
const ColType&  y() const                  { return col(1); }
const ColType&  z() const                  { return col(2); }


const ColType& getAxisUnitVec(CoordinateAxis axis) const 
{   return col(axis); }

const UnitVec<P,1> getAxisUnitVec(CoordinateDirection dir) const {
    const ColType& axDir = getAxisUnitVec(dir.getAxis());
    return dir.getDirection() > 0 ? UnitVec<P,1>( axDir) 
                                  : UnitVec<P,1>(-axDir); // cheap 
}

const BaseMat&  asMat33() const  { return *static_cast<const BaseMat*>(this); }
BaseMat         toMat33() const  { return asMat33(); }
};

template <class P> SimTK_SimTKCOMMON_EXPORT std::ostream& 
operator<<(std::ostream&, const Rotation_<P>&);
template <class P> SimTK_SimTKCOMMON_EXPORT std::ostream& 
operator<<(std::ostream&, const InverseRotation_<P>&);

template <class P, int S> inline UnitVec<P,1>  
operator*(const Rotation_<P>& R, const UnitVec<P,S>& v)        
{return UnitVec<P,1>(R.asMat33()* v.asVec3(),  true);}
template <class P, int S> inline UnitRow<P,1>  
operator*(const UnitRow<P,S>& r, const Rotation_<P>& R)        
{return UnitRow<P,1>(r.asRow3() * R.asMat33(), true);}
template <class P, int S> inline UnitVec<P,1>  
operator*(const InverseRotation_<P>& R, const UnitVec<P,S>& v) 
{return UnitVec<P,1>(R.asMat33()* v.asVec3(),  true);}
template <class P, int S> inline UnitRow<P,1>  
operator*(const UnitRow<P,S>& r, const InverseRotation_<P>& R) 
{return UnitRow<P,1>(r.asRow3() * R.asMat33(), true);}

// Couldn't implement these Rotation_ methods until InverseRotation_ was defined.
template <class P> inline
Rotation_<P>::Rotation_(const InverseRotation_<P>& R) 
:   Mat<3,3,P>( R.asMat33() ) {}

template <class P> inline Rotation_<P>&  
Rotation_<P>::operator=(const InverseRotation_<P>& R)  
{static_cast<Mat<3,3,P>&>(*this)  = R.asMat33();    return *this;}
template <class P> inline Rotation_<P>&  
Rotation_<P>::operator*=(const Rotation_<P>& R)        
{static_cast<Mat<3,3,P>&>(*this) *= R.asMat33();    return *this;}
template <class P> inline Rotation_<P>&  
Rotation_<P>::operator/=(const Rotation_<P>& R)        
{static_cast<Mat<3,3,P>&>(*this) *= (~R).asMat33(); return *this;}
template <class P> inline Rotation_<P>&  
Rotation_<P>::operator*=(const InverseRotation_<P>& R) 
{static_cast<Mat<3,3,P>&>(*this) *= R.asMat33();    return *this;}
template <class P> inline Rotation_<P>&  
Rotation_<P>::operator/=(const InverseRotation_<P>& R) 
{static_cast<Mat<3,3,P>&>(*this) *= (~R).asMat33(); return *this;}


template <class P> inline Rotation_<P>
operator*(const Rotation_<P>&        R1, const Rotation_<P>&        R2)  
{return Rotation_<P>(R1) *= R2;}
template <class P> inline Rotation_<P>
operator*(const Rotation_<P>&        R1, const InverseRotation_<P>& R2)  
{return Rotation_<P>(R1) *= R2;}
template <class P> inline Rotation_<P>
operator*(const InverseRotation_<P>& R1, const Rotation_<P>&        R2)  
{return Rotation_<P>(R1) *= R2;}
template <class P> inline Rotation_<P>
operator*(const InverseRotation_<P>& R1, const InverseRotation_<P>& R2)  
{return Rotation_<P>(R1) *= R2;}


template <class P> inline Rotation_<P>
operator/( const Rotation_<P>&        R1, const Rotation_<P>&        R2 )  
{return Rotation_<P>(R1) /= R2;}
template <class P> inline Rotation_<P>
operator/( const Rotation_<P>&        R1, const InverseRotation&     R2 )  
{return Rotation_<P>(R1) /= R2;}
template <class P> inline Rotation_<P>
operator/( const InverseRotation_<P>& R1, const Rotation_<P>&        R2 )  
{return Rotation_<P>(R1) /= R2;}
template <class P> inline Rotation_<P>
operator/( const InverseRotation_<P>& R1, const InverseRotation_<P>& R2 )  
{return Rotation_<P>(R1) /= R2;}


//------------------------------------------------------------------------------
}  // End of namespace SimTK

#endif // SimTK_SimTKCOMMON_ROTATION_H_