|
Simbody
3.4 (development)
|
|
Simbody
3.4 (development)
|
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-12 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 //------------------------------------------------------------------------------ 00102 //------------------------------------------------------------------------------ 00103 template <class P> // templatized by precision 00104 class Rotation_ : public Mat<3,3,P> { 00105 typedef P RealP; 00106 typedef Mat<2,2,P> Mat22P; 00107 typedef Mat<3,2,P> Mat32P; 00108 typedef Mat<3,3,P> Mat33P; 00109 typedef Mat<4,3,P> Mat43P; 00110 typedef Mat<3,4,P> Mat34P; 00111 typedef Vec<2,P> Vec2P; 00112 typedef Vec<3,P> Vec3P; 00113 typedef Vec<4,P> Vec4P; 00114 typedef UnitVec<P,1> UnitVec3P; // stride is 1 here, length is always 3 00115 typedef SymMat<3,P> SymMat33P; 00116 typedef Quaternion_<P> QuaternionP; 00117 public: 00118 // Default constructor and constructor-like methods 00119 Rotation_() : Mat33P(1) {} 00120 Rotation_& setRotationToIdentityMatrix() { Mat33P::operator=(RealP(1)); return *this; } 00121 Rotation_& setRotationToNaN() { Mat33P::setToNaN(); return *this; } 00122 00123 // Default copy constructor and assignment operator 00124 Rotation_( const Rotation_& R ) : Mat33P(R) {} 00125 Rotation_& operator=( const Rotation_& R ) { Mat33P::operator=( R.asMat33() ); return *this; } 00126 00128 00129 Rotation_( RealP angle, const CoordinateAxis& axis ) { setRotationFromAngleAboutAxis( angle, axis ); } 00130 Rotation_( RealP angle, const CoordinateAxis::XCoordinateAxis ) { setRotationFromAngleAboutX( std::cos(angle), std::sin(angle) ); } 00131 Rotation_( RealP angle, const CoordinateAxis::YCoordinateAxis ) { setRotationFromAngleAboutY( std::cos(angle), std::sin(angle) ); } 00132 Rotation_( RealP angle, const CoordinateAxis::ZCoordinateAxis ) { setRotationFromAngleAboutZ( std::cos(angle), std::sin(angle) ); } 00134 00135 00136 Rotation_& setRotationFromAngleAboutAxis( RealP angle, const CoordinateAxis& axis ) { return axis.isXAxis() ? setRotationFromAngleAboutX(angle) : (axis.isYAxis() ? setRotationFromAngleAboutY(angle) : setRotationFromAngleAboutZ(angle) ); } 00137 Rotation_& setRotationFromAngleAboutX( RealP angle ) { return setRotationFromAngleAboutX( std::cos(angle), std::sin(angle) ); } 00138 Rotation_& setRotationFromAngleAboutY( RealP angle ) { return setRotationFromAngleAboutY( std::cos(angle), std::sin(angle) ); } 00139 Rotation_& setRotationFromAngleAboutZ( RealP angle ) { return setRotationFromAngleAboutZ( std::cos(angle), std::sin(angle) ); } 00140 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; } 00141 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; } 00142 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; } 00144 00146 00147 Rotation_( RealP angle, const UnitVec3P& unitVector ) { setRotationFromAngleAboutUnitVector(angle,unitVector); } 00148 Rotation_( RealP angle, const Vec3P& nonUnitVector ) { setRotationFromAngleAboutNonUnitVector(angle,nonUnitVector); } 00150 00151 00152 Rotation_& setRotationFromAngleAboutNonUnitVector( RealP angle, const Vec3P& nonUnitVector ) { return setRotationFromAngleAboutUnitVector( angle, UnitVec3P(nonUnitVector) ); } 00153 SimTK_SimTKCOMMON_EXPORT Rotation_& setRotationFromAngleAboutUnitVector( RealP angle, const UnitVec3P& unitVector ); 00155 00157 Rotation_( BodyOrSpaceType bodyOrSpace, RealP angle1, const CoordinateAxis& axis1, RealP angle2, const CoordinateAxis& axis2 ) { setRotationFromTwoAnglesTwoAxes( bodyOrSpace,angle1,axis1,angle2,axis2); } 00159 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); } 00161 SimTK_SimTKCOMMON_EXPORT Rotation_& setRotationFromTwoAnglesTwoAxes( BodyOrSpaceType bodyOrSpace, RealP angle1, const CoordinateAxis& axis1, RealP angle2, const CoordinateAxis& axis2 ); 00163 SimTK_SimTKCOMMON_EXPORT Rotation_& setRotationFromThreeAnglesThreeAxes( BodyOrSpaceType bodyOrSpace, RealP angle1, const CoordinateAxis& axis1, RealP angle2, const CoordinateAxis& axis2, RealP angle3, const CoordinateAxis& axis3 ); 00164 00168 void setRotationToBodyFixedXY( const Vec2P& v) { setRotationFromTwoAnglesTwoAxes( BodyRotationSequence, v[0], XAxis, v[1], YAxis ); } 00173 void setRotationToBodyFixedXYZ( const Vec3P& v) { setRotationFromThreeAnglesThreeAxes( BodyRotationSequence, v[0], XAxis, v[1], YAxis, v[2], ZAxis ); } 00174 00176 explicit Rotation_( const QuaternionP& q ) { setRotationFromQuaternion(q); } 00178 SimTK_SimTKCOMMON_EXPORT Rotation_& setRotationFromQuaternion( const QuaternionP& q ); 00179 00181 Rotation_( const Mat33P& m, bool ) : Mat33P(m) {} 00182 00184 explicit Rotation_( const Mat33P& m ) { setRotationFromApproximateMat33(m); } 00186 SimTK_SimTKCOMMON_EXPORT Rotation_& setRotationFromApproximateMat33( const Mat33P& m ); 00187 00190 00191 Rotation_( const UnitVec3P& uvec, const CoordinateAxis axis ) { setRotationFromOneAxis(uvec,axis); } 00192 SimTK_SimTKCOMMON_EXPORT Rotation_& setRotationFromOneAxis( const UnitVec3P& uvec, const CoordinateAxis axis ); 00194 00201 00202 Rotation_( const UnitVec3P& uveci, const CoordinateAxis& axisi, const Vec3P& vecjApprox, const CoordinateAxis& axisjApprox ) { setRotationFromTwoAxes(uveci,axisi,vecjApprox,axisjApprox); } 00203 SimTK_SimTKCOMMON_EXPORT Rotation_& setRotationFromTwoAxes( const UnitVec3P& uveci, const CoordinateAxis& axisi, const Vec3P& vecjApprox, const CoordinateAxis& axisjApprox ); 00205 00206 // Converts rotation matrix to one or two or three orientation angles. 00207 // Note: The result is most meaningful if the Rotation_ matrix is one that can be produced by such a sequence. 00208 // Use1: someRotation.convertOneAxisRotationToOneAngle( XAxis ); 00209 // Use2: someRotation.convertTwoAxesRotationToTwoAngles( SpaceRotationSequence, YAxis, ZAxis ); 00210 // Use3: someRotation.convertThreeAxesRotationToThreeAngles( SpaceRotationSequence, ZAxis, YAxis, XAxis ); 00211 // Use4: someRotation.convertRotationToAngleAxis(); Return: [angleInRadians, unitVectorX, unitVectorY, unitVectorZ]. 00212 00215 SimTK_SimTKCOMMON_EXPORT RealP convertOneAxisRotationToOneAngle( const CoordinateAxis& axis1 ) const; 00218 SimTK_SimTKCOMMON_EXPORT Vec2P convertTwoAxesRotationToTwoAngles( BodyOrSpaceType bodyOrSpace, const CoordinateAxis& axis1, const CoordinateAxis& axis2 ) const; 00221 SimTK_SimTKCOMMON_EXPORT Vec3P convertThreeAxesRotationToThreeAngles( BodyOrSpaceType bodyOrSpace, const CoordinateAxis& axis1, const CoordinateAxis& axis2, const CoordinateAxis& axis3 ) const; 00223 SimTK_SimTKCOMMON_EXPORT QuaternionP convertRotationToQuaternion() const; 00225 Vec4P convertRotationToAngleAxis() const { return convertRotationToQuaternion().convertQuaternionToAngleAxis(); } 00226 00228 Vec2P convertRotationToBodyFixedXY() const { return convertTwoAxesRotationToTwoAngles( BodyRotationSequence, XAxis, YAxis ); } 00230 Vec3P convertRotationToBodyFixedXYZ() const { return convertThreeAxesRotationToThreeAngles( BodyRotationSequence, XAxis, YAxis, ZAxis ); } 00231 00239 SimTK_SimTKCOMMON_EXPORT SymMat33P reexpressSymMat33(const SymMat33P& S_BB) const; 00240 00242 00243 SimTK_SimTKCOMMON_EXPORT bool isSameRotationToWithinAngle( const Rotation_& R, RealP okPointingAngleErrorRads ) const; 00244 bool isSameRotationToWithinAngleOfMachinePrecision( const Rotation_& R) const 00245 { return isSameRotationToWithinAngle( R, NTraits<P>::getSignificant() ); } 00247 RealP getMaxAbsDifferenceInRotationElements( const Rotation_& R ) const { 00248 const Mat33P& A = asMat33(); const Mat33P& B = R.asMat33(); RealP maxDiff = 0; 00249 for( int i=0; i<=2; i++ ) for( int j=0; j<=2; j++ ) 00250 { RealP absDiff = std::fabs(A[i][j] - B[i][j]); 00251 if( absDiff > maxDiff ) maxDiff = absDiff; } 00252 return maxDiff; 00253 } 00254 00255 bool areAllRotationElementsSameToEpsilon( const Rotation_& R, RealP epsilon ) const 00256 { return getMaxAbsDifferenceInRotationElements(R) <= epsilon ; } 00257 bool areAllRotationElementsSameToMachinePrecision( const Rotation_& R ) const 00258 { return areAllRotationElementsSameToEpsilon( R, NTraits<P>::getSignificant() ); } 00259 00261 inline Rotation_( const InverseRotation_<P>& ); 00263 inline Rotation_& operator=( const InverseRotation_<P>& ); 00264 00266 const InverseRotation_<P>& invert() const { return *reinterpret_cast<const InverseRotation_<P>*>(this); } 00268 InverseRotation_<P>& updInvert() { return *reinterpret_cast<InverseRotation_<P>*>(this); } 00269 00272 00273 const InverseRotation_<P>& transpose() const { return invert(); } 00274 const InverseRotation_<P>& operator~() const { return invert(); } 00275 InverseRotation_<P>& updTranspose() { return updInvert(); } 00276 InverseRotation_<P>& operator~() { return updInvert(); } 00278 00280 00281 inline Rotation_& operator*=( const Rotation_<P>& R ); 00282 inline Rotation_& operator/=( const Rotation_<P>& R ); 00283 inline Rotation_& operator*=( const InverseRotation_<P>& ); 00284 inline Rotation_& operator/=( const InverseRotation_<P>& ); 00286 00289 00290 const Mat33P& asMat33() const { return *static_cast<const Mat33P*>(this); } 00291 Mat33P toMat33() const { return asMat33(); } 00293 00295 typedef UnitVec<P,Mat33P::RowSpacing> ColType; 00296 typedef UnitRow<P,Mat33P::ColSpacing> RowType; 00297 const RowType& row( int i ) const { return reinterpret_cast<const RowType&>(asMat33()[i]); } 00298 const ColType& col( int j ) const { return reinterpret_cast<const ColType&>(asMat33()(j)); } 00299 const ColType& x() const { return col(0); } 00300 const ColType& y() const { return col(1); } 00301 const ColType& z() const { return col(2); } 00302 const RowType& operator[]( int i ) const { return row(i); } 00303 const ColType& operator()( int j ) const { return col(j); } 00304 00306 00307 Rotation_& setRotationFromMat33TrustMe( const Mat33P& m ) 00308 { Mat33P& R = *this; R=m; return *this; } 00309 Rotation_& setRotationColFromUnitVecTrustMe( int colj, const UnitVec3P& uvecj ) 00310 { Mat33P& R = *this; R(colj)=uvecj.asVec3(); return *this; } 00311 Rotation_& setRotationFromUnitVecsTrustMe( const UnitVec3P& colA, const UnitVec3P& colB, const UnitVec3P& colC ) 00312 { Mat33P& R = *this; R(0)=colA.asVec3(); R(1)=colB.asVec3(); R(2)=colC.asVec3(); return *this; } 00314 00315 //--------------------------- PAUL CONTINUE FROM HERE -------------------------- 00316 public: 00317 //------------------------------------------------------------------------------ 00318 00319 // These are ad hoc routines that don't match the nice API Paul Mitiguy 00320 // implemented above. 00321 00322 00326 void setRotationToBodyFixedXYZ(const Vec3P& c, const Vec3P& s) { 00327 Mat33P& R = *this; 00328 const RealP s0s1=s[0]*s[1], s2c0=s[2]*c[0], c0c2=c[0]*c[2], nc1= -c[1]; 00329 00330 R = Mat33P( c[1]*c[2] , s[2]*nc1 , s[1] , 00331 s2c0 + s0s1*c[2] , c0c2 - s0s1*s[2] , s[0]*nc1 , 00332 s[0]*s[2] - s[1]*c0c2 , s[0]*c[2] + s[1]*s2c0 , c[0]*c[1] ); 00333 } 00334 00335 00341 static Vec3P convertAngVelToBodyFixed321Dot(const Vec3P& q, const Vec3P& w_PB_B) { 00342 const RealP s1 = std::sin(q[1]), c1 = std::cos(q[1]); 00343 const RealP s2 = std::sin(q[2]), c2 = std::cos(q[2]); 00344 const RealP ooc1 = RealP(1)/c1; 00345 const RealP s2oc1 = s2*ooc1, c2oc1 = c2*ooc1; 00346 00347 const Mat33P E( 0, s2oc1 , c2oc1 , 00348 0, c2 , -s2 , 00349 1, s1*s2oc1 , s1*c2oc1 ); 00350 return E * w_PB_B; 00351 } 00352 00355 static Vec3P convertBodyFixed321DotToAngVel(const Vec3P& q, const Vec3P& qd) { 00356 const RealP s1 = std::sin(q[1]), c1 = std::cos(q[1]); 00357 const RealP s2 = std::sin(q[2]), c2 = std::cos(q[2]); 00358 00359 const Mat33P Einv( -s1 , 0 , 1 , 00360 c1*s2 , c2 , 0 , 00361 c1*c2 , -s2 , 0 ); 00362 return Einv*qd; 00363 } 00364 00365 // TODO: sherm: is this right? Warning: everything is measured in the 00366 // *PARENT* frame, but angular velocities and accelerations are 00367 // expressed in the *BODY* frame. 00368 // TODO: this is not an efficient way to do this computation. 00369 static Vec3P convertAngVelDotToBodyFixed321DotDot 00370 (const Vec3P& q, const Vec3P& w_PB_B, const Vec3P& wdot_PB_B) 00371 { 00372 const RealP s1 = std::sin(q[1]), c1 = std::cos(q[1]); 00373 const RealP s2 = std::sin(q[2]), c2 = std::cos(q[2]); 00374 const RealP ooc1 = 1/c1; 00375 const RealP s2oc1 = s2*ooc1, c2oc1 = c2*ooc1, s1oc1 = s1*ooc1; 00376 00377 const Mat33P E( 0 , s2oc1 , c2oc1 , 00378 0 , c2 , -s2 , 00379 1 , s1*s2oc1 , s1*c2oc1 ); 00380 const Vec3P qdot = E * w_PB_B; 00381 00382 const RealP t = qdot[1]*s1oc1; 00383 const RealP a = t*s2oc1 + qdot[2]*c2oc1; // d/dt s2oc1 00384 const RealP b = t*c2oc1 - qdot[2]*s2oc1; // d/dt c2oc1 00385 00386 const Mat33P 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_PB_B + Edot*w_PB_B; 00391 } 00392 00404 static Mat33P calcNForBodyXYZInBodyFrame(const Vec3P& q) { 00405 // Note: q[0] is not referenced so we won't waste time calculating 00406 // its cosine and sine here. 00407 return calcNForBodyXYZInBodyFrame 00408 (Vec3P(0, std::cos(q[1]), std::cos(q[2])), 00409 Vec3P(0, std::sin(q[1]), std::sin(q[2]))); 00410 } 00411 00417 static Mat33P calcNForBodyXYZInBodyFrame(const Vec3P& cq, const Vec3P& sq) { 00418 const RealP s1 = sq[1], c1 = cq[1]; 00419 const RealP s2 = sq[2], c2 = cq[2]; 00420 const RealP ooc1 = 1/c1; 00421 const RealP s2oc1 = s2*ooc1, c2oc1 = c2*ooc1; 00422 00423 return Mat33P( c2oc1 , -s2oc1 , 0, 00424 s2 , c2 , 0, 00425 -s1*c2oc1 , s1*s2oc1, 1 ); 00426 } 00427 00440 static Mat33P calcNForBodyXYZInParentFrame(const Vec3P& q) { 00441 // Note: q[2] is not referenced so we won't waste time calculating 00442 // its cosine and sine here. 00443 return calcNForBodyXYZInParentFrame 00444 (Vec3P(std::cos(q[0]), std::cos(q[1]), 0), 00445 Vec3P(std::sin(q[0]), std::sin(q[1]), 0)); 00446 } 00447 00454 static Mat33P calcNForBodyXYZInParentFrame(const Vec3P& cq, const Vec3P& sq) { 00455 const RealP s0 = sq[0], c0 = cq[0]; 00456 const RealP s1 = sq[1], c1 = cq[1]; 00457 const RealP ooc1 = 1/c1; 00458 const RealP s0oc1 = s0*ooc1, c0oc1 = c0*ooc1; 00459 00460 return Mat33P( 1 , s1*s0oc1 , -s1*c0oc1, 00461 0 , c0 , s0, 00462 0 , -s0oc1 , c0oc1 ); 00463 } 00464 00471 static Vec3P multiplyByBodyXYZ_N_P(const Vec2P& cosxy, 00472 const Vec2P& sinxy, 00473 RealP oocosy, 00474 const Vec3P& w_PB) 00475 { 00476 const RealP s0 = sinxy[0], c0 = cosxy[0]; 00477 const RealP s1 = sinxy[1]; 00478 const RealP w0 = w_PB[0], w1 = w_PB[1], w2 = w_PB[2]; 00479 00480 const RealP t = (s0*w1-c0*w2)*oocosy; 00481 return Vec3P( w0 + t*s1, c0*w1 + s0*w2, -t ); // qdot 00482 } 00483 00487 static Vec3P multiplyByBodyXYZ_NT_P(const Vec2P& cosxy, 00488 const Vec2P& sinxy, 00489 RealP oocosy, 00490 const Vec3P& q) 00491 { 00492 const RealP s0 = sinxy[0], c0 = cosxy[0]; 00493 const RealP s1 = sinxy[1]; 00494 const RealP q0 = q[0], q1 = q[1], q2 = q[2]; 00495 00496 const RealP t = (q0*s1-q2) * oocosy; 00497 return Vec3P( q0, c0*q1 + t*s0, s0*q1 - t*c0 ); // v_P 00498 } 00499 00506 static Vec3P convertAngVelInParentToBodyXYZDot 00507 (const Vec2P& cosxy, 00508 const Vec2P& sinxy, 00509 RealP oocosy, 00510 const Vec3P& w_PB) 00511 { 00512 return multiplyByBodyXYZ_N_P(cosxy,sinxy,oocosy,w_PB); 00513 } 00514 00526 static Vec3P convertAngAccInParentToBodyXYZDotDot 00527 (const Vec2P& cosxy, 00528 const Vec2P& sinxy, 00529 RealP oocosy, 00530 const Vec3P& qdot, 00531 const Vec3P& b_PB) 00532 { 00533 const RealP s1 = sinxy[1], c1 = cosxy[1]; 00534 const RealP q0 = qdot[0], q1 = qdot[1], q2 = qdot[2]; 00535 00536 // 10 flops 00537 const Vec3P Nb = multiplyByBodyXYZ_N_P(cosxy,sinxy,oocosy,b_PB); 00538 00539 const RealP q1oc1 = q1*oocosy; 00540 const Vec3P NDotw((q0*s1-q2)*q1oc1, // NDot_P*w_PB 00541 q0*q2*c1, // = NDot_P*(NInv_P*qdot) 00542 (q2*s1-q0)*q1oc1 ); // (9 flops) 00543 00544 return Nb + NDotw; // 3 flops 00545 } 00546 00547 00550 static Vec3P multiplyByBodyXYZ_NInv_P(const Vec2P& cosxy, 00551 const Vec2P& sinxy, 00552 const Vec3P& qdot) 00553 { 00554 const RealP s0 = sinxy[0], c0 = cosxy[0]; 00555 const RealP s1 = sinxy[1], c1 = cosxy[1]; 00556 const RealP q0 = qdot[0], q1 = qdot[1], q2 = qdot[2]; 00557 const RealP c1q2 = c1*q2; 00558 00559 return Vec3P( q0 + s1*q2, // w_PB 00560 c0*q1 - s0*c1q2, 00561 s0*q1 + c0*c1q2 ); 00562 } 00563 00566 static Vec3P multiplyByBodyXYZ_NInvT_P(const Vec2P& cosxy, 00567 const Vec2P& sinxy, 00568 const Vec3P& v_P) 00569 { 00570 const RealP s0 = sinxy[0], c0 = cosxy[0]; 00571 const RealP s1 = sinxy[1], c1 = cosxy[1]; 00572 const RealP w0 = v_P[0], w1 = v_P[1], w2 = v_P[2]; 00573 00574 return Vec3P( w0, // qdot-like 00575 c0*w1 + s0*w2, 00576 s1*w0 - s0*c1*w1 + c0*c1*w2); 00577 } 00578 00588 static Mat33P calcNDotForBodyXYZInBodyFrame 00589 (const Vec3P& q, const Vec3P& qdot) { 00590 // Note: q[0] is not referenced so we won't waste time calculating 00591 // its cosine and sine here. 00592 return calcNDotForBodyXYZInBodyFrame 00593 (Vec3P(0, std::cos(q[1]), std::cos(q[2])), 00594 Vec3P(0, std::sin(q[1]), std::sin(q[2])), 00595 qdot); 00596 } 00597 00603 static Mat33P calcNDotForBodyXYZInBodyFrame 00604 (const Vec3P& cq, const Vec3P& sq, const Vec3P& qdot) 00605 { 00606 const RealP s1 = sq[1], c1 = cq[1]; 00607 const RealP s2 = sq[2], c2 = cq[2]; 00608 const RealP ooc1 = 1/c1; 00609 const RealP s2oc1 = s2*ooc1, c2oc1 = c2*ooc1; 00610 00611 const RealP t = qdot[1]*s1*ooc1; 00612 const RealP a = t*s2oc1 + qdot[2]*c2oc1; // d/dt s2oc1 00613 const RealP b = t*c2oc1 - qdot[2]*s2oc1; // d/dt c2oc1 00614 00615 return Mat33P( b , -a , 0, 00616 qdot[2]*c2 , -qdot[2]*s2 , 0, 00617 -(s1*b + qdot[1]*c2) , s1*a + qdot[1]*s2 , 0 ); 00618 } 00619 00629 static Mat33P calcNDotForBodyXYZInParentFrame 00630 (const Vec3P& q, const Vec3P& qdot) { 00631 // Note: q[2] is not referenced so we won't waste time calculating 00632 // its cosine and sine here. 00633 const RealP cy = std::cos(q[1]); // cos(y) 00634 return calcNDotForBodyXYZInParentFrame 00635 (Vec2P(std::cos(q[0]), cy), 00636 Vec2P(std::sin(q[0]), std::sin(q[1])), 00637 1/cy, qdot); 00638 } 00639 00644 static Mat33P calcNDotForBodyXYZInParentFrame 00645 (const Vec2P& cq, const Vec2P& sq, RealP ooc1, const Vec3P& qdot) { 00646 const RealP s0 = sq[0], c0 = cq[0]; 00647 const RealP s1 = sq[1], c1 = cq[1]; 00648 const RealP s0oc1 = s0*ooc1, c0oc1 = c0*ooc1; 00649 00650 const RealP t = qdot[1]*s1*ooc1; 00651 const RealP a = t*s0oc1 + qdot[0]*c0oc1; // d/dt s0oc1 00652 const RealP b = t*c0oc1 - qdot[0]*s0oc1; // d/dt c0oc1 00653 00654 return Mat33P( 0, s1*a + qdot[1]*s0, -(s1*b + qdot[1]*c0), 00655 0, -qdot[0]*s0 , qdot[0]*c0 , 00656 0, -a , b ); 00657 } 00658 00667 static Mat33P calcNInvForBodyXYZInBodyFrame(const Vec3P& q) { 00668 // Note: q[0] is not referenced so we won't waste time calculating 00669 // its cosine and sine here. 00670 return calcNInvForBodyXYZInBodyFrame 00671 (Vec3P(0, std::cos(q[1]), std::cos(q[2])), 00672 Vec3P(0, std::sin(q[1]), std::sin(q[2]))); 00673 } 00674 00680 static Mat33P calcNInvForBodyXYZInBodyFrame 00681 (const Vec3P& cq, const Vec3P& sq) { 00682 const RealP s1 = sq[1], c1 = cq[1]; 00683 const RealP s2 = sq[2], c2 = cq[2]; 00684 00685 return Mat33P( c1*c2 , s2 , 0 , 00686 -c1*s2 , c2 , 0 , 00687 s1 , 0 , 1 ); 00688 } 00689 00697 static Mat33P calcNInvForBodyXYZInParentFrame(const Vec3P& q) { 00698 // Note: q[0] is not referenced so we won't waste time calculating 00699 // its cosine and sine here. 00700 return calcNInvForBodyXYZInParentFrame 00701 (Vec3P(std::cos(q[0]), std::cos(q[1]), 0), 00702 Vec3P(std::sin(q[0]), std::sin(q[1]), 0)); 00703 } 00704 00710 static Mat33P calcNInvForBodyXYZInParentFrame 00711 (const Vec3P& cq, const Vec3P& sq) { 00712 const RealP s0 = sq[0], c0 = cq[0]; 00713 const RealP s1 = sq[1], c1 = cq[1]; 00714 00715 return Mat33P( 1 , 0 , s1 , 00716 0 , c0 , -s0*c1 , 00717 0 , s0 , c0*c1 ); 00718 } 00719 00728 static Vec3P convertAngVelInBodyFrameToBodyXYZDot 00729 (const Vec3P& q, const Vec3P& w_PB_B) { 00730 return convertAngVelInBodyFrameToBodyXYZDot 00731 (Vec3P(0, std::cos(q[1]), std::cos(q[2])), 00732 Vec3P(0, std::sin(q[1]), std::sin(q[2])), 00733 w_PB_B); 00734 } 00735 00741 //TODO: reimplement 00742 static Vec3P convertAngVelInBodyFrameToBodyXYZDot 00743 (const Vec3P& cq, const Vec3P& sq, const Vec3P& w_PB_B) 00744 { return calcNForBodyXYZInBodyFrame(cq,sq)*w_PB_B; } 00745 00751 static Vec3P convertBodyXYZDotToAngVelInBodyFrame 00752 (const Vec3P& q, const Vec3P& qdot) { 00753 return convertBodyXYZDotToAngVelInBodyFrame 00754 (Vec3P(0, std::cos(q[1]), std::cos(q[2])), 00755 Vec3P(0, std::sin(q[1]), std::sin(q[2])), 00756 qdot); 00757 } 00758 00764 // TODO: reimplement 00765 static Vec3P convertBodyXYZDotToAngVelInBodyFrame 00766 (const Vec3P& cq, const Vec3P& sq, const Vec3P& qdot) 00767 { return calcNInvForBodyXYZInBodyFrame(cq,sq)*qdot; } 00768 00774 static Vec3P convertAngVelDotInBodyFrameToBodyXYZDotDot 00775 (const Vec3P& q, const Vec3P& w_PB_B, const Vec3P& wdot_PB_B) 00776 { 00777 // Note: q[0] is not referenced so we won't waste time calculating 00778 // its cosine and sine here. 00779 return convertAngVelDotInBodyFrameToBodyXYZDotDot 00780 (Vec3P(0, std::cos(q[1]), std::cos(q[2])), 00781 Vec3P(0, std::sin(q[1]), std::sin(q[2])), 00782 w_PB_B, wdot_PB_B); 00783 } 00784 00791 // TODO: reimplement 00792 static Vec3P convertAngVelDotInBodyFrameToBodyXYZDotDot 00793 (const Vec3P& cq, const Vec3P& sq, const Vec3P& w_PB_B, const Vec3P& wdot_PB_B) 00794 { 00795 const Mat33P N = calcNForBodyXYZInBodyFrame(cq,sq); 00796 const Vec3P qdot = N * w_PB_B; // 15 flops 00797 const Mat33P NDot = calcNDotForBodyXYZInBodyFrame(cq,sq,qdot); 00798 00799 return N*wdot_PB_B + NDot*w_PB_B; // 33 flops 00800 } 00801 00807 static Mat43P calcUnnormalizedNForQuaternion(const Vec4P& q) { 00808 const Vec4P e = q/2; 00809 const RealP ne1 = -e[1], ne2 = -e[2], ne3 = -e[3]; 00810 return Mat43P( ne1, ne2, ne3, 00811 e[0], e[3], ne2, 00812 ne3, e[0], e[1], 00813 e[2], ne1, e[0]); 00814 } 00815 00822 static Mat43P calcUnnormalizedNDotForQuaternion(const Vec4P& qdot) { 00823 const Vec4P ed = qdot/2; 00824 const RealP ned1 = -ed[1], ned2 = -ed[2], ned3 = -ed[3]; 00825 return Mat43P( ned1, ned2, ned3, 00826 ed[0], ed[3], ned2, 00827 ned3, ed[0], ed[1], 00828 ed[2], ned1, ed[0]); 00829 } 00830 00839 static Mat34P calcUnnormalizedNInvForQuaternion(const Vec4P& q) { 00840 const Vec4P e = 2*q; 00841 const RealP ne1 = -e[1], ne2 = -e[2], ne3 = -e[3]; 00842 return Mat34P(ne1, e[0], ne3, e[2], 00843 ne2, e[3], e[0], ne1, 00844 ne3, ne2, e[1], e[0]); 00845 } 00846 00847 00852 static Vec4P convertAngVelToQuaternionDot(const Vec4P& q, const Vec3P& w_PB_P) { 00853 return calcUnnormalizedNForQuaternion(q)*w_PB_P; 00854 } 00855 00859 static Vec3P convertQuaternionDotToAngVel(const Vec4P& q, const Vec4P& qdot) { 00860 return calcUnnormalizedNInvForQuaternion(q)*qdot; 00861 } 00862 00868 // TODO: reimplement 00869 static Vec4P OLDconvertAngVelDotToQuaternionDotDot 00870 (const Vec4P& q, const Vec3P& w_PB_P, const Vec3P& wdot_PB_P) 00871 { 00872 const Mat43P N = calcUnnormalizedNForQuaternion(q); 00873 const Vec4P qdot = N*w_PB_P; // 20 flops 00874 const Mat43P NDot = calcUnnormalizedNDotForQuaternion(qdot); 00875 00876 return N*wdot_PB_P + NDot*w_PB_P; // 44 flops 00877 } 00878 00885 static Vec4P convertAngVelDotToQuaternionDotDot 00886 (const Vec4P& q, const Vec3P& w_PB, const Vec3P& b_PB) 00887 { 00888 const Mat43P N = calcUnnormalizedNForQuaternion(q); // 7 flops 00889 const Vec4P Nb = N*b_PB; // 20 flops 00890 const Vec4P NDotw = RealP(-.25)*w_PB.normSqr()*q; // 10 flops 00891 return Nb + NDotw; // 4 flops 00892 } 00893 00894 00895 private: 00896 // This is only for the most trustworthy of callers, that is, methods of 00897 // the Rotation_ class. There are a lot of ways for this NOT to be a 00898 // legitimate rotation matrix -- be careful!! 00899 // Note that these are supplied in rows. 00900 Rotation_( const RealP& xx, const RealP& xy, const RealP& xz, 00901 const RealP& yx, const RealP& yy, const RealP& yz, 00902 const RealP& zx, const RealP& zy, const RealP& zz ) 00903 : Mat33P( xx,xy,xz, yx,yy,yz, zx,zy,zz ) {} 00904 00905 // These next methods are highly-efficient power-user methods. Read the 00906 // code to understand them. 00907 SimTK_SimTKCOMMON_EXPORT Rotation_& setTwoAngleTwoAxesBodyFixedForwardCyclicalRotation( RealP cosAngle1, RealP sinAngle1, const CoordinateAxis& axis1, RealP cosAngle2, RealP sinAngle2, const CoordinateAxis& axis2 ); 00908 SimTK_SimTKCOMMON_EXPORT Rotation_& setThreeAngleTwoAxesBodyFixedForwardCyclicalRotation( RealP cosAngle1, RealP sinAngle1, const CoordinateAxis& axis1, RealP cosAngle2, RealP sinAngle2, const CoordinateAxis& axis2, RealP cosAngle3, RealP sinAngle3 ); 00909 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 ); 00910 00911 // These next methods are highly-efficient power-user methods to convert 00912 // Rotation matrices to orientation angles. Read the code to understand them. 00913 SimTK_SimTKCOMMON_EXPORT Vec2P convertTwoAxesBodyFixedRotationToTwoAngles( const CoordinateAxis& axis1, const CoordinateAxis& axis2 ) const; 00914 SimTK_SimTKCOMMON_EXPORT Vec3P convertTwoAxesBodyFixedRotationToThreeAngles( const CoordinateAxis& axis1, const CoordinateAxis& axis2 ) const; 00915 SimTK_SimTKCOMMON_EXPORT Vec3P convertThreeAxesBodyFixedRotationToThreeAngles( const CoordinateAxis& axis1, const CoordinateAxis& axis2, const CoordinateAxis& axis3 ) const; 00916 00917 //------------------------------------------------------------------------------ 00918 // These are obsolete names from a previous release, listed here so that 00919 // users will get a decipherable compilation error. (sherm 091101) 00920 //------------------------------------------------------------------------------ 00921 private: 00922 // REPLACED BY: calcNForBodyXYZInBodyFrame() 00923 static Mat33P calcQBlockForBodyXYZInBodyFrame(const Vec3P& a) 00924 { return calcNForBodyXYZInBodyFrame(a); } 00925 // REPLACED BY: calcNInvForBodyXYZInBodyFrame() 00926 static Mat33P calcQInvBlockForBodyXYZInBodyFrame(const Vec3P& a) 00927 { return calcNInvForBodyXYZInBodyFrame(a); } 00928 // REPLACED BY: calcUnnormalizedNForQuaternion() 00929 static Mat<4,3,P> calcUnnormalizedQBlockForQuaternion(const Vec4P& q) 00930 { return calcUnnormalizedNForQuaternion(q); } 00931 // REPLACED BY: calcUnnormalizedNInvForQuaternion() 00932 static Mat<3,4,P> calcUnnormalizedQInvBlockForQuaternion(const Vec4P& q) 00933 { return calcUnnormalizedNInvForQuaternion(q); } 00934 // REPLACED BY: convertAngVelInBodyFrameToBodyXYZDot 00935 static Vec3P convertAngVelToBodyFixed123Dot(const Vec3P& q, const Vec3P& w_PB_B) 00936 { return convertAngVelInBodyFrameToBodyXYZDot(q,w_PB_B); } 00937 // REPLACED BY: convertBodyXYZDotToAngVelInBodyFrame 00938 static Vec3P convertBodyFixed123DotToAngVel(const Vec3P& q, const Vec3P& qdot) 00939 { return convertBodyXYZDotToAngVelInBodyFrame(q,qdot); } 00940 // REPLACED BY: convertAngVelDotInBodyFrameToBodyXYZDotDot 00941 static Vec3P convertAngVelDotToBodyFixed123DotDot 00942 (const Vec3P& q, const Vec3P& w_PB_B, const Vec3P& wdot_PB_B) 00943 { return convertAngVelDotInBodyFrameToBodyXYZDotDot(q,w_PB_B,wdot_PB_B); } 00944 00945 //------------------------------------------------------------------------------ 00946 // The following code is obsolete - it is here temporarily for backward 00947 // compatibility (Mitiguy 9/5/2007) 00948 //------------------------------------------------------------------------------ 00949 private: 00950 // These static methods are like constructors with friendlier names. 00951 static Rotation_ zero() { return Rotation_(); } 00952 static Rotation_ NaN() { Rotation_ r; r.setRotationToNaN(); return r; } 00953 00955 Rotation_& setToZero() { return setRotationToIdentityMatrix(); } 00956 Rotation_& setToIdentityMatrix() { return setRotationToIdentityMatrix(); } 00957 Rotation_& setToNaN() { return setRotationToNaN(); } 00958 static Rotation_ trustMe( const Mat33P& m ) { return Rotation_(m,true); } 00959 00960 // One-angle rotations. 00961 static Rotation_ aboutX( const RealP& angleInRad ) { return Rotation_( angleInRad, XAxis ); } 00962 static Rotation_ aboutY( const RealP& angleInRad ) { return Rotation_( angleInRad, YAxis ); } 00963 static Rotation_ aboutZ( const RealP& angleInRad ) { return Rotation_( angleInRad, ZAxis ); } 00964 static Rotation_ aboutAxis( const RealP& angleInRad, const UnitVec3P& axis ) { return Rotation_(angleInRad,axis); } 00965 static Rotation_ aboutAxis( const RealP& angleInRad, const Vec3P& axis ) { return Rotation_(angleInRad,axis); } 00966 void setToRotationAboutZ( const RealP& q ) { setRotationFromAngleAboutZ( q ); } 00967 00968 // Two-angle space-fixed rotations. 00969 static Rotation_ aboutXThenOldY(const RealP& xInRad, const RealP& yInRad) { return Rotation_( SpaceRotationSequence, xInRad, XAxis, yInRad, YAxis ); } 00970 static Rotation_ aboutYThenOldX(const RealP& yInRad, const RealP& xInRad) { return Rotation_( SpaceRotationSequence, yInRad, YAxis, xInRad, XAxis ); } 00971 static Rotation_ aboutXThenOldZ(const RealP& xInRad, const RealP& zInRad) { return Rotation_( SpaceRotationSequence, xInRad, XAxis, zInRad, ZAxis ); } 00972 static Rotation_ aboutZThenOldX(const RealP& zInRad, const RealP& xInRad) { return Rotation_( SpaceRotationSequence, zInRad, ZAxis, xInRad, XAxis ); } 00973 static Rotation_ aboutYThenOldZ(const RealP& yInRad, const RealP& zInRad) { return Rotation_( SpaceRotationSequence, yInRad, YAxis, zInRad, ZAxis ); } 00974 static Rotation_ aboutZThenOldY(const RealP& zInRad, const RealP& yInRad) { return Rotation_( SpaceRotationSequence, zInRad, ZAxis, yInRad, YAxis ); } 00975 00976 // Two-angle body fixed rotations (reversed space-fixed ones). 00977 static Rotation_ aboutXThenNewY(const RealP& xInRad, const RealP& yInRad) { return Rotation_( BodyRotationSequence, xInRad, XAxis, yInRad, YAxis ); } 00978 static Rotation_ aboutYThenNewX(const RealP& yInRad, const RealP& xInRad) { return aboutXThenOldY(xInRad, yInRad); } 00979 static Rotation_ aboutXThenNewZ(const RealP& xInRad, const RealP& zInRad) { return aboutZThenOldX(zInRad, xInRad); } 00980 static Rotation_ aboutZThenNewX(const RealP& zInRad, const RealP& xInRad) { return aboutXThenOldZ(xInRad, zInRad); } 00981 static Rotation_ aboutYThenNewZ(const RealP& yInRad, const RealP& zInRad) { return aboutZThenOldY(zInRad, yInRad); } 00982 static Rotation_ aboutZThenNewY(const RealP& zInRad, const RealP& yInRad) { return aboutYThenOldZ(yInRad, zInRad); } 00983 00986 explicit Rotation_( const UnitVec3P& uvecZ ) { setRotationFromOneAxis(uvecZ,ZAxis); } 00987 00989 // We will take x seriously after normalizing, but use the y only to create z = normalize(x X y), 00990 // then y = z X x. Bad things happen if x and y are aligned but we may not catch it. 00991 Rotation_( const Vec3P& x, const Vec3P& yish ) { setRotationFromTwoAxes( UnitVec3P(x), XAxis, yish, YAxis ); } 00992 00994 void setToQuaternion( const QuaternionP& q ) { setRotationFromQuaternion(q); } 00995 01000 // Similarly for BodyFixed123. 01001 void setToBodyFixed321( const Vec3P& v) { setRotationFromThreeAnglesThreeAxes( BodyRotationSequence, v[0], ZAxis, v[1], YAxis, v[2], XAxis ); } 01002 void setToBodyFixed123( const Vec3P& v) { setRotationToBodyFixedXYZ(v); } 01003 01006 Vec4P convertToAngleAxis() const { return convertRotationToAngleAxis(); } 01007 01009 QuaternionP convertToQuaternion() const { return convertRotationToQuaternion(); } 01010 01013 void setToSpaceFixed12( const Vec2P& q ) { setRotationFromTwoAnglesTwoAxes( SpaceRotationSequence, q[0], XAxis, q[1], YAxis ); } 01014 01018 Vec3P convertToBodyFixed123() const { return convertRotationToBodyFixedXYZ(); } 01019 Vec2P convertToBodyFixed12() const { return convertRotationToBodyFixedXY(); } 01020 Vec2P convertToSpaceFixed12() const { return convertTwoAxesRotationToTwoAngles( SpaceRotationSequence, XAxis, YAxis ); } 01021 }; 01022 01023 01028 template <class P> 01029 class InverseRotation_ : public Mat<3,3,P>::TransposeType { 01030 typedef P RealP; 01031 typedef Rotation_<P> RotationP; 01032 typedef Mat<3,3,P> Mat33P; // not the base type! 01033 typedef SymMat<3,P> SymMat33P; 01034 typedef Mat<2,2,P> Mat22P; 01035 typedef Mat<3,2,P> Mat32P; 01036 typedef Vec<2,P> Vec2P; 01037 typedef Vec<3,P> Vec3P; 01038 typedef Vec<4,P> Vec4P; 01039 typedef Quaternion_<P> QuaternionP; 01040 public: 01043 typedef typename Mat<3,3,P>::TransposeType BaseMat; 01044 01047 01048 01049 typedef UnitVec<P,BaseMat::RowSpacing> ColType; 01051 typedef UnitRow<P,BaseMat::ColSpacing> RowType; 01053 01057 InverseRotation_() : BaseMat(1) {} 01058 01060 InverseRotation_( const InverseRotation_& R ) : BaseMat(R) {} 01062 InverseRotation_& operator=( const InverseRotation_& R ) 01063 { BaseMat::operator=(R.asMat33()); return *this; } 01064 01071 SimTK_SimTKCOMMON_EXPORT SymMat33P reexpressSymMat33(const SymMat33P& S_BB) const; 01072 01074 01075 const RotationP& invert() const {return *reinterpret_cast<const RotationP*>(this);} 01076 RotationP& updInvert() {return *reinterpret_cast<RotationP*>(this);} 01078 01081 01082 const RotationP& transpose() const { return invert(); } 01083 const RotationP& operator~() const { return invert(); } 01084 RotationP& updTranspose() { return updInvert(); } 01085 RotationP& operator~() { return updInvert(); } 01087 01093 01094 const RowType& row( int i ) const { return reinterpret_cast<const RowType&>(asMat33()[i]); } 01095 const ColType& col( int j ) const { return reinterpret_cast<const ColType&>(asMat33()(j)); } 01096 const ColType& x() const { return col(0); } 01097 const ColType& y() const { return col(1); } 01098 const ColType& z() const { return col(2); } 01099 const RowType& operator[]( int i ) const { return row(i); } 01100 const ColType& operator()( int j ) const { return col(j); } 01102 01105 01106 const BaseMat& asMat33() const { return *static_cast<const BaseMat*>(this); } 01107 BaseMat toMat33() const { return asMat33(); } 01109 }; 01110 01112 template <class P> SimTK_SimTKCOMMON_EXPORT std::ostream& 01113 operator<<(std::ostream&, const Rotation_<P>&); 01115 template <class P> SimTK_SimTKCOMMON_EXPORT std::ostream& 01116 operator<<(std::ostream&, const InverseRotation_<P>&); 01117 01121 01122 template <class P, int S> inline UnitVec<P,1> 01123 operator*(const Rotation_<P>& R, const UnitVec<P,S>& v) {return UnitVec<P,1>(R.asMat33()* v.asVec3(), true);} 01124 template <class P, int S> inline UnitRow<P,1> 01125 operator*(const UnitRow<P,S>& r, const Rotation_<P>& R) {return UnitRow<P,1>(r.asRow3() * R.asMat33(), true);} 01126 template <class P, int S> inline UnitVec<P,1> 01127 operator*(const InverseRotation_<P>& R, const UnitVec<P,S>& v) {return UnitVec<P,1>(R.asMat33()* v.asVec3(), true);} 01128 template <class P, int S> inline UnitRow<P,1> 01129 operator*(const UnitRow<P,S>& r, const InverseRotation_<P>& R) {return UnitRow<P,1>(r.asRow3() * R.asMat33(), true);} 01131 01132 // Couldn't implement these Rotation_ methods until InverseRotation_ was defined. 01133 template <class P> inline 01134 Rotation_<P>::Rotation_(const InverseRotation_<P>& R) : Mat<3,3,P>( R.asMat33() ) {} 01135 template <class P> inline Rotation_<P>& 01136 Rotation_<P>::operator=(const InverseRotation_<P>& R) {static_cast<Mat<3,3,P>&>(*this) = R.asMat33(); return *this;} 01137 template <class P> inline Rotation_<P>& 01138 Rotation_<P>::operator*=(const Rotation_<P>& R) {static_cast<Mat<3,3,P>&>(*this) *= R.asMat33(); return *this;} 01139 template <class P> inline Rotation_<P>& 01140 Rotation_<P>::operator/=(const Rotation_<P>& R) {static_cast<Mat<3,3,P>&>(*this) *= (~R).asMat33(); return *this;} 01141 template <class P> inline Rotation_<P>& 01142 Rotation_<P>::operator*=(const InverseRotation_<P>& R) {static_cast<Mat<3,3,P>&>(*this) *= R.asMat33(); return *this;} 01143 template <class P> inline Rotation_<P>& 01144 Rotation_<P>::operator/=(const InverseRotation_<P>& R) {static_cast<Mat<3,3,P>&>(*this) *= (~R).asMat33(); return *this;} 01145 01147 01148 template <class P> inline Rotation_<P> 01149 operator*(const Rotation_<P>& R1, const Rotation_<P>& R2) {return Rotation_<P>(R1) *= R2;} 01150 template <class P> inline Rotation_<P> 01151 operator*(const Rotation_<P>& R1, const InverseRotation_<P>& R2) {return Rotation_<P>(R1) *= R2;} 01152 template <class P> inline Rotation_<P> 01153 operator*(const InverseRotation_<P>& R1, const Rotation_<P>& R2) {return Rotation_<P>(R1) *= R2;} 01154 template <class P> inline Rotation_<P> 01155 operator*(const InverseRotation_<P>& R1, const InverseRotation_<P>& R2) {return Rotation_<P>(R1) *= R2;} 01157 01160 01161 template <class P> inline Rotation_<P> 01162 operator/( const Rotation_<P>& R1, const Rotation_<P>& R2 ) {return Rotation_<P>(R1) /= R2;} 01163 template <class P> inline Rotation_<P> 01164 operator/( const Rotation_<P>& R1, const InverseRotation& R2 ) {return Rotation_<P>(R1) /= R2;} 01165 template <class P> inline Rotation_<P> 01166 operator/( const InverseRotation_<P>& R1, const Rotation_<P>& R2 ) {return Rotation_<P>(R1) /= R2;} 01167 template <class P> inline Rotation_<P> 01168 operator/( const InverseRotation_<P>& R1, const InverseRotation_<P>& R2 ) {return Rotation_<P>(R1) /= R2;} 01170 01171 01172 //------------------------------------------------------------------------------ 01173 } // End of namespace SimTK 01174 01175 //-------------------------------------------------------------------------- 01176 #endif // SimTK_SimTKCOMMON_ROTATION_H_ 01177 //-------------------------------------------------------------------------- 01178 01179
1.7.6.1 
