|
Simbody
|
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-9 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/CoordinateAxis.h" 00045 #include "SimTKcommon/internal/UnitVec.h" 00046 #include "SimTKcommon/internal/Quaternion.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 template <class P> class Rotation_; 00061 template <class P> class InverseRotation_; 00062 00063 typedef Rotation_<Real> Rotation; 00064 typedef Rotation_<float> fRotation; 00065 typedef Rotation_<double> dRotation; 00066 00067 typedef InverseRotation_<Real> InverseRotation; 00068 typedef InverseRotation_<float> fInverseRotation; 00069 typedef InverseRotation_<double> dInverseRotation; 00070 00071 //------------------------------------------------------------------------------ 00116 //------------------------------------------------------------------------------ 00117 template <class P> // templatized by precision 00118 class Rotation_ : public Mat<3,3,P> { 00119 typedef P RealP; 00120 typedef Mat<2,2,P> Mat22P; 00121 typedef Mat<3,2,P> Mat32P; 00122 typedef Mat<3,3,P> Mat33P; 00123 typedef Vec<2,P> Vec2P; 00124 typedef Vec<3,P> Vec3P; 00125 typedef Vec<4,P> Vec4P; 00126 typedef UnitVec<P,1> UnitVec3P; // stride is 1 here, length is always 3 00127 typedef SymMat<3,P> SymMat33P; 00128 typedef Quaternion_<P> QuaternionP; 00129 public: 00130 // Default constructor and constructor-like methods 00131 Rotation_() : Mat33P(1) {} 00132 Rotation_& setRotationToIdentityMatrix() { Mat33P::operator=(RealP(1)); return *this; } 00133 Rotation_& setRotationToNaN() { Mat33P::setToNaN(); return *this; } 00134 00135 // Default copy constructor and assignment operator 00136 Rotation_( const Rotation_& R ) : Mat33P(R) {} 00137 Rotation_& operator=( const Rotation_& R ) { Mat33P::operator=( R.asMat33() ); return *this; } 00138 00140 00141 Rotation_( RealP angle, const CoordinateAxis& axis ) { setRotationFromAngleAboutAxis( angle, axis ); } 00142 Rotation_( RealP angle, const CoordinateAxis::XCoordinateAxis ) { setRotationFromAngleAboutX( std::cos(angle), std::sin(angle) ); } 00143 Rotation_( RealP angle, const CoordinateAxis::YCoordinateAxis ) { setRotationFromAngleAboutY( std::cos(angle), std::sin(angle) ); } 00144 Rotation_( RealP angle, const CoordinateAxis::ZCoordinateAxis ) { setRotationFromAngleAboutZ( std::cos(angle), std::sin(angle) ); } 00146 00147 00148 Rotation_& setRotationFromAngleAboutAxis( RealP angle, const CoordinateAxis& axis ) { return axis.isXAxis() ? setRotationFromAngleAboutX(angle) : (axis.isYAxis() ? setRotationFromAngleAboutY(angle) : setRotationFromAngleAboutZ(angle) ); } 00149 Rotation_& setRotationFromAngleAboutX( RealP angle ) { return setRotationFromAngleAboutX( std::cos(angle), std::sin(angle) ); } 00150 Rotation_& setRotationFromAngleAboutY( RealP angle ) { return setRotationFromAngleAboutY( std::cos(angle), std::sin(angle) ); } 00151 Rotation_& setRotationFromAngleAboutZ( RealP angle ) { return setRotationFromAngleAboutZ( std::cos(angle), std::sin(angle) ); } 00152 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; } 00153 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; } 00154 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; } 00156 00158 00159 Rotation_( RealP angle, const UnitVec3P& unitVector ) { setRotationFromAngleAboutUnitVector(angle,unitVector); } 00160 Rotation_( RealP angle, const Vec3P& nonUnitVector ) { setRotationFromAngleAboutNonUnitVector(angle,nonUnitVector); } 00162 00163 00164 Rotation_& setRotationFromAngleAboutNonUnitVector( RealP angle, const Vec3P& nonUnitVector ) { return setRotationFromAngleAboutUnitVector( angle, UnitVec3P(nonUnitVector) ); } 00165 SimTK_SimTKCOMMON_EXPORT Rotation_& setRotationFromAngleAboutUnitVector( RealP angle, const UnitVec3P& unitVector ); 00167 00169 Rotation_( BodyOrSpaceType bodyOrSpace, RealP angle1, const CoordinateAxis& axis1, RealP angle2, const CoordinateAxis& axis2 ) { setRotationFromTwoAnglesTwoAxes( bodyOrSpace,angle1,axis1,angle2,axis2); } 00171 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); } 00173 SimTK_SimTKCOMMON_EXPORT Rotation_& setRotationFromTwoAnglesTwoAxes( BodyOrSpaceType bodyOrSpace, RealP angle1, const CoordinateAxis& axis1, RealP angle2, const CoordinateAxis& axis2 ); 00175 SimTK_SimTKCOMMON_EXPORT Rotation_& setRotationFromThreeAnglesThreeAxes( BodyOrSpaceType bodyOrSpace, RealP angle1, const CoordinateAxis& axis1, RealP angle2, const CoordinateAxis& axis2, RealP angle3, const CoordinateAxis& axis3 ); 00176 00180 void setRotationToBodyFixedXY( const Vec2P& v) { setRotationFromTwoAnglesTwoAxes( BodyRotationSequence, v[0], XAxis, v[1], YAxis ); } 00185 void setRotationToBodyFixedXYZ( const Vec3P& v) { setRotationFromThreeAnglesThreeAxes( BodyRotationSequence, v[0], XAxis, v[1], YAxis, v[2], ZAxis ); } 00186 00188 explicit Rotation_( const QuaternionP& q ) { setRotationFromQuaternion(q); } 00190 SimTK_SimTKCOMMON_EXPORT Rotation_& setRotationFromQuaternion( const QuaternionP& q ); 00191 00193 Rotation_( const Mat33P& m, bool ) : Mat33P(m) {} 00194 00196 explicit Rotation_( const Mat33P& m ) { setRotationFromApproximateMat33(m); } 00198 SimTK_SimTKCOMMON_EXPORT Rotation_& setRotationFromApproximateMat33( const Mat33P& m ); 00199 00202 00203 Rotation_( const UnitVec3P& uvec, const CoordinateAxis axis ) { setRotationFromOneAxis(uvec,axis); } 00204 SimTK_SimTKCOMMON_EXPORT Rotation_& setRotationFromOneAxis( const UnitVec3P& uvec, const CoordinateAxis axis ); 00206 00213 00214 Rotation_( const UnitVec3P& uveci, const CoordinateAxis& axisi, const Vec3P& vecjApprox, const CoordinateAxis& axisjApprox ) { setRotationFromTwoAxes(uveci,axisi,vecjApprox,axisjApprox); } 00215 SimTK_SimTKCOMMON_EXPORT Rotation_& setRotationFromTwoAxes( const UnitVec3P& uveci, const CoordinateAxis& axisi, const Vec3P& vecjApprox, const CoordinateAxis& axisjApprox ); 00217 00218 // Converts rotation matrix to one or two or three orientation angles. 00219 // Note: The result is most meaningful if the Rotation_ matrix is one that can be produced by such a sequence. 00220 // Use1: someRotation.convertOneAxisRotationToOneAngle( XAxis ); 00221 // Use2: someRotation.convertTwoAxesRotationToTwoAngles( SpaceRotationSequence, YAxis, ZAxis ); 00222 // Use3: someRotation.convertThreeAxesRotationToThreeAngles( SpaceRotationSequence, ZAxis, YAxis, XAxis ); 00223 // Use4: someRotation.convertRotationToAngleAxis(); Return: [angleInRadians, unitVectorX, unitVectorY, unitVectorZ]. 00224 00227 SimTK_SimTKCOMMON_EXPORT RealP convertOneAxisRotationToOneAngle( const CoordinateAxis& axis1 ) const; 00230 SimTK_SimTKCOMMON_EXPORT Vec2P convertTwoAxesRotationToTwoAngles( BodyOrSpaceType bodyOrSpace, const CoordinateAxis& axis1, const CoordinateAxis& axis2 ) const; 00233 SimTK_SimTKCOMMON_EXPORT Vec3P convertThreeAxesRotationToThreeAngles( BodyOrSpaceType bodyOrSpace, const CoordinateAxis& axis1, const CoordinateAxis& axis2, const CoordinateAxis& axis3 ) const; 00235 SimTK_SimTKCOMMON_EXPORT QuaternionP convertRotationToQuaternion() const; 00237 Vec4P convertRotationToAngleAxis() const { return convertRotationToQuaternion().convertQuaternionToAngleAxis(); } 00238 00240 Vec2P convertRotationToBodyFixedXY() const { return convertTwoAxesRotationToTwoAngles( BodyRotationSequence, XAxis, YAxis ); } 00242 Vec3P convertRotationToBodyFixedXYZ() const { return convertThreeAxesRotationToThreeAngles( BodyRotationSequence, XAxis, YAxis, ZAxis ); } 00243 00251 SimTK_SimTKCOMMON_EXPORT SymMat33P reexpressSymMat33(const SymMat33P& S_BB) const; 00252 00254 00255 SimTK_SimTKCOMMON_EXPORT bool isSameRotationToWithinAngle( const Rotation_& R, RealP okPointingAngleErrorRads ) const; 00256 bool isSameRotationToWithinAngleOfMachinePrecision( const Rotation_& R) const 00257 { return isSameRotationToWithinAngle( R, NTraits<P>::getSignificant() ); } 00259 RealP getMaxAbsDifferenceInRotationElements( const Rotation_& R ) const { 00260 const Mat33P& A = asMat33(); const Mat33P& B = R.asMat33(); RealP maxDiff = 0; 00261 for( int i=0; i<=2; i++ ) for( int j=0; j<=2; j++ ) 00262 { RealP absDiff = std::fabs(A[i][j] - B[i][j]); 00263 if( absDiff > maxDiff ) maxDiff = absDiff; } 00264 return maxDiff; 00265 } 00266 00267 bool areAllRotationElementsSameToEpsilon( const Rotation_& R, RealP epsilon ) const 00268 { return getMaxAbsDifferenceInRotationElements(R) <= epsilon ; } 00269 bool areAllRotationElementsSameToMachinePrecision( const Rotation_& R ) const 00270 { return areAllRotationElementsSameToEpsilon( R, NTraits<P>::getSignificant() ); } 00271 00273 inline Rotation_( const InverseRotation_<P>& ); 00275 inline Rotation_& operator=( const InverseRotation_<P>& ); 00276 00278 const InverseRotation_<P>& invert() const { return *reinterpret_cast<const InverseRotation_<P>*>(this); } 00280 InverseRotation_<P>& updInvert() { return *reinterpret_cast<InverseRotation_<P>*>(this); } 00281 00284 00285 const InverseRotation_<P>& transpose() const { return invert(); } 00286 const InverseRotation_<P>& operator~() const { return invert(); } 00287 InverseRotation_<P>& updTranspose() { return updInvert(); } 00288 InverseRotation_<P>& operator~() { return updInvert(); } 00290 00292 00293 inline Rotation_& operator*=( const Rotation_<P>& R ); 00294 inline Rotation_& operator/=( const Rotation_<P>& R ); 00295 inline Rotation_& operator*=( const InverseRotation_<P>& ); 00296 inline Rotation_& operator/=( const InverseRotation_<P>& ); 00298 00301 00302 const Mat33P& asMat33() const { return *static_cast<const Mat33P*>(this); } 00303 Mat33P toMat33() const { return asMat33(); } 00305 00307 typedef UnitVec<P,Mat33P::RowSpacing> ColType; 00308 typedef UnitRow<P,Mat33P::ColSpacing> RowType; 00309 const RowType& row( int i ) const { return reinterpret_cast<const RowType&>(asMat33()[i]); } 00310 const ColType& col( int j ) const { return reinterpret_cast<const ColType&>(asMat33()(j)); } 00311 const ColType& x() const { return col(0); } 00312 const ColType& y() const { return col(1); } 00313 const ColType& z() const { return col(2); } 00314 const RowType& operator[]( int i ) const { return row(i); } 00315 const ColType& operator()( int j ) const { return col(j); } 00316 00318 00319 Rotation_& setRotationFromMat33TrustMe( const Mat33P& m ) 00320 { Mat33P& R = *this; R=m; return *this; } 00321 Rotation_& setRotationColFromUnitVecTrustMe( int colj, const UnitVec3P& uvecj ) 00322 { Mat33P& R = *this; R(colj)=uvecj.asVec3(); return *this; } 00323 Rotation_& setRotationFromUnitVecsTrustMe( const UnitVec3P& colA, const UnitVec3P& colB, const UnitVec3P& colC ) 00324 { Mat33P& R = *this; R(0)=colA.asVec3(); R(1)=colB.asVec3(); R(2)=colC.asVec3(); return *this; } 00326 00327 00328 //--------------------------------- PAUL CONTINUE FROM HERE ---------------------------------- 00329 public: 00330 //-------------------------------------------------------------------------------------------- 00336 static Vec3P convertAngVelToBodyFixed321Dot(const Vec3P& q, const Vec3P& w_PB_B) { 00337 const RealP s1 = std::sin(q[1]), c1 = std::cos(q[1]); 00338 const RealP s2 = std::sin(q[2]), c2 = std::cos(q[2]); 00339 const RealP ooc1 = RealP(1)/c1; 00340 const RealP s2oc1 = s2*ooc1, c2oc1 = c2*ooc1; 00341 00342 const Mat33P E( 0, s2oc1 , c2oc1 , 00343 0, c2 , -s2 , 00344 1, s1*s2oc1 , s1*c2oc1 ); 00345 return E * w_PB_B; 00346 } 00347 00350 static Vec3P convertBodyFixed321DotToAngVel(const Vec3P& q, const Vec3P& qd) { 00351 const RealP s1 = std::sin(q[1]), c1 = std::cos(q[1]); 00352 const RealP s2 = std::sin(q[2]), c2 = std::cos(q[2]); 00353 00354 const Mat33P Einv( -s1 , 0 , 1 , 00355 c1*s2 , c2 , 0 , 00356 c1*c2 , -s2 , 0 ); 00357 return Einv*qd; 00358 } 00359 00360 // TODO: sherm: is this right? Warning: everything is measured in the 00361 // *PARENT* frame, but has to be expressed in the *BODY* frame. 00362 static Vec3P convertAngVelDotToBodyFixed321DotDot 00363 (const Vec3P& q, const Vec3P& w_PB_B, const Vec3P& wdot) 00364 { 00365 const RealP s1 = std::sin(q[1]), c1 = std::cos(q[1]); 00366 const RealP s2 = std::sin(q[2]), c2 = std::cos(q[2]); 00367 const RealP ooc1 = 1/c1; 00368 const RealP s2oc1 = s2*ooc1, c2oc1 = c2*ooc1; 00369 00370 const Mat33P E( 0 , s2oc1 , c2oc1 , 00371 0 , c2 , -s2 , 00372 1 , s1*s2oc1 , s1*c2oc1 ); 00373 const Vec3P qdot = E * w_PB_B; 00374 00375 const RealP t = qdot[1]*qdot[2]*s1*ooc1; 00376 const RealP a = t*c2oc1; // d/dt s2oc1 00377 const RealP b = -t*s2oc1; // d/dt c2oc1 00378 00379 const Mat33P Edot( 0 , a , b , 00380 0 , -qdot[2]*s2 , -qdot[2]*c2 , 00381 0 , s1*a + qdot[1]*s2 , s1*b + qdot[1]*c2 ); 00382 00383 return E*wdot + Edot*w_PB_B; 00384 } 00385 00394 static Mat33P calcNForBodyXYZInBodyFrame(const Vec3P& q) { 00395 // Note: q[0] is not referenced so we won't waste time calculating 00396 // its cosine and sine here. 00397 return calcNForBodyXYZInBodyFrame(Vec3P(0,std::cos(q[1]),std::cos(q[2])), 00398 Vec3P(0,std::sin(q[1]),std::sin(q[2]))); 00399 } 00400 00406 static Mat33P calcNForBodyXYZInBodyFrame(const Vec3P& cq, const Vec3P& sq) { 00407 const RealP s1 = sq[1], c1 = cq[1]; 00408 const RealP s2 = sq[2], c2 = cq[2]; 00409 const RealP ooc1 = 1/c1; 00410 const RealP s2oc1 = s2*ooc1, c2oc1 = c2*ooc1; 00411 00412 return Mat33P( c2oc1 , -s2oc1 , 0, 00413 s2 , c2 , 0, 00414 -s1*c2oc1 , s1*s2oc1, 1 ); 00415 } 00416 00426 static Mat33P calcNDotForBodyXYZInBodyFrame(const Vec3P& q, const Vec3P& qdot) { 00427 // Note: q[0] is not referenced so we won't waste time calculating 00428 // its cosine and sine here. 00429 return calcNDotForBodyXYZInBodyFrame(Vec3P(0,std::cos(q[1]),std::cos(q[2])), 00430 Vec3P(0,std::sin(q[1]),std::sin(q[2])), 00431 qdot); 00432 } 00433 00439 static Mat33P calcNDotForBodyXYZInBodyFrame(const Vec3P& cq, const Vec3P& sq, 00440 const Vec3P& qdot) 00441 { 00442 const RealP s1 = sq[1], c1 = cq[1]; 00443 const RealP s2 = sq[2], c2 = cq[2]; 00444 const RealP ooc1 = 1/c1; 00445 const RealP s2oc1 = s2*ooc1, c2oc1 = c2*ooc1; 00446 00447 const RealP t = qdot[1]*qdot[2]*s1*ooc1; 00448 const RealP a = t*c2oc1; // d/dt s2oc1 00449 const RealP b = -t*s2oc1; // d/dt c2oc1 00450 00451 return Mat33P( b , -a , 0, 00452 qdot[2]*c2 , -qdot[2]*s2 , 0, 00453 -s1*b - qdot[1]*c2 , s1*a + qdot[1]*s2 , 0 ); 00454 } 00455 00462 static Mat33P calcNInvForBodyXYZInBodyFrame(const Vec3P& q) { 00463 // Note: q[0] is not referenced so we won't waste time calculating 00464 // its cosine and sine here. 00465 return calcNInvForBodyXYZInBodyFrame(Vec3P(0,std::cos(q[1]),std::cos(q[2])), 00466 Vec3P(0,std::sin(q[1]),std::sin(q[2]))); 00467 } 00468 00474 static Mat33P calcNInvForBodyXYZInBodyFrame(const Vec3P& cq, const Vec3P& sq) 00475 { 00476 const RealP s1 = sq[1], c1 = cq[1]; 00477 const RealP s2 = sq[2], c2 = cq[2]; 00478 00479 return Mat33P( c1*c2 , s2 , 0 , 00480 -c1*s2 , c2 , 0 , 00481 s1 , 0 , 1 ); 00482 } 00483 00492 static Vec3P convertAngVelInBodyFrameToBodyXYZDot(const Vec3P& q, const Vec3P& w_PB_B) 00493 { return convertAngVelInBodyFrameToBodyXYZDot(Vec3P(0,std::cos(q[1]),std::cos(q[2])), 00494 Vec3P(0,std::sin(q[1]),std::sin(q[2])), 00495 w_PB_B); } 00496 00502 static Vec3P convertAngVelInBodyFrameToBodyXYZDot 00503 (const Vec3P& cq, const Vec3P& sq, const Vec3P& w_PB_B) 00504 { return calcNForBodyXYZInBodyFrame(cq,sq)*w_PB_B; } 00505 00511 static Vec3P convertBodyXYZDotToAngVelInBodyFrame(const Vec3P& q, const Vec3P& qdot) 00512 { return convertBodyXYZDotToAngVelInBodyFrame(Vec3P(0,std::cos(q[1]),std::cos(q[2])), 00513 Vec3P(0,std::sin(q[1]),std::sin(q[2])), 00514 qdot); } 00515 00521 static Vec3P convertBodyXYZDotToAngVelInBodyFrame 00522 (const Vec3P& cq, const Vec3P& sq, const Vec3P& qdot) 00523 { return calcNInvForBodyXYZInBodyFrame(cq,sq)*qdot; } 00524 00530 static Vec3P convertAngVelDotInBodyFrameToBodyXYZDotDot 00531 (const Vec3P& q, const Vec3P& w_PB_B, const Vec3P& wdot_PB_B) 00532 { 00533 // Note: q[0] is not referenced so we won't waste time calculating 00534 // its cosine and sine here. 00535 return convertAngVelDotInBodyFrameToBodyXYZDotDot 00536 (Vec3P(0,std::cos(q[1]),std::cos(q[2])), 00537 Vec3P(0,std::sin(q[1]),std::sin(q[2])), 00538 w_PB_B, wdot_PB_B); 00539 } 00540 00546 static Vec3P convertAngVelDotInBodyFrameToBodyXYZDotDot 00547 (const Vec3P& cq, const Vec3P& sq, const Vec3P& w_PB_B, const Vec3P& wdot_PB_B) 00548 { 00549 const Mat33P N = calcNForBodyXYZInBodyFrame(cq,sq); 00550 const Vec3P qdot = N * w_PB_B; // 15 flops 00551 const Mat33P NDot = calcNDotForBodyXYZInBodyFrame(cq,sq,qdot); 00552 00553 return N*wdot_PB_B + NDot*w_PB_B; // 33 flops 00554 } 00555 00561 static Mat<4,3,P> calcUnnormalizedNForQuaternion(const Vec4P& q) { 00562 const Vec4P e = q/2; 00563 const RealP ne1 = -e[1], ne2 = -e[2], ne3 = -e[3]; 00564 return Mat<4,3,P>( ne1, ne2, ne3, 00565 e[0], e[3], ne2, 00566 ne3, e[0], e[1], 00567 e[2], ne1, e[0]); 00568 } 00569 00576 static Mat<4,3,P> calcUnnormalizedNDotForQuaternion(const Vec4P& qdot) { 00577 const Vec4P ed = qdot/2; 00578 const RealP ned1 = -ed[1], ned2 = -ed[2], ned3 = -ed[3]; 00579 return Mat<4,3,P>( ned1, ned2, ned3, 00580 ed[0], ed[3], ned2, 00581 ned3, ed[0], ed[1], 00582 ed[2], ned1, ed[0]); 00583 } 00584 00593 static Mat<3,4,P> calcUnnormalizedNInvForQuaternion(const Vec4P& q) { 00594 const Vec4P e = 2*q; 00595 const RealP ne1 = -e[1], ne2 = -e[2], ne3 = -e[3]; 00596 return Mat<3,4,P>(ne1, e[0], ne3, e[2], 00597 ne2, e[3], e[0], ne1, 00598 ne3, ne2, e[1], e[0]); 00599 } 00600 00601 00606 static Vec4P convertAngVelToQuaternionDot(const Vec4P& q, const Vec3P& w_PB_P) { 00607 return calcUnnormalizedNForQuaternion(q)*w_PB_P; 00608 } 00609 00613 static Vec3P convertQuaternionDotToAngVel(const Vec4P& q, const Vec4P& qdot) { 00614 return calcUnnormalizedNInvForQuaternion(q)*qdot; 00615 } 00616 00622 static Vec4P convertAngVelDotToQuaternionDotDot 00623 (const Vec4P& q, const Vec3P& w_PB_P, const Vec3P& wdot_PB_P) 00624 { 00625 const Mat<4,3,P> N = calcUnnormalizedNForQuaternion(q); 00626 const Vec4P qdot = N*w_PB_P; // 20 flops 00627 const Mat<4,3,P> NDot = calcUnnormalizedNDotForQuaternion(qdot); 00628 00629 return N*wdot_PB_P + NDot*w_PB_P; // 44 flops 00630 } 00631 00632 00633 private: 00634 // This is only for the most trustworthy of callers, that is, methods of 00635 // the Rotation_ class. There are a lot of ways for this NOT to be a 00636 // legitimate rotation matrix -- be careful!! 00637 // Note that these are supplied in rows. 00638 Rotation_( const RealP& xx, const RealP& xy, const RealP& xz, 00639 const RealP& yx, const RealP& yy, const RealP& yz, 00640 const RealP& zx, const RealP& zy, const RealP& zz ) 00641 : Mat33P( xx,xy,xz, yx,yy,yz, zx,zy,zz ) {} 00642 00643 // These next methods are highly-efficient power-user methods. Read the 00644 // code to understand them. 00645 SimTK_SimTKCOMMON_EXPORT Rotation_& setTwoAngleTwoAxesBodyFixedForwardCyclicalRotation( RealP cosAngle1, RealP sinAngle1, const CoordinateAxis& axis1, RealP cosAngle2, RealP sinAngle2, const CoordinateAxis& axis2 ); 00646 SimTK_SimTKCOMMON_EXPORT Rotation_& setThreeAngleTwoAxesBodyFixedForwardCyclicalRotation( RealP cosAngle1, RealP sinAngle1, const CoordinateAxis& axis1, RealP cosAngle2, RealP sinAngle2, const CoordinateAxis& axis2, RealP cosAngle3, RealP sinAngle3 ); 00647 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 ); 00648 00649 // These next methods are highly-efficient power-user methods to convert 00650 // Rotation matrices to orientation angles. Read the code to understand them. 00651 SimTK_SimTKCOMMON_EXPORT Vec2P convertTwoAxesBodyFixedRotationToTwoAngles( const CoordinateAxis& axis1, const CoordinateAxis& axis2 ) const; 00652 SimTK_SimTKCOMMON_EXPORT Vec3P convertTwoAxesBodyFixedRotationToThreeAngles( const CoordinateAxis& axis1, const CoordinateAxis& axis2 ) const; 00653 SimTK_SimTKCOMMON_EXPORT Vec3P convertThreeAxesBodyFixedRotationToThreeAngles( const CoordinateAxis& axis1, const CoordinateAxis& axis2, const CoordinateAxis& axis3 ) const; 00654 00655 //------------------------------------------------------------------------------ 00656 // These are obsolete names from a previous release, listed here so that 00657 // users will get a decipherable compilation error. (sherm 091101) 00658 //------------------------------------------------------------------------------ 00659 private: 00660 // REPLACED BY: calcNForBodyXYZInBodyFrame() 00661 static Mat33P calcQBlockForBodyXYZInBodyFrame(const Vec3P& a) 00662 { return calcNForBodyXYZInBodyFrame(a); } 00663 // REPLACED BY: calcNInvForBodyXYZInBodyFrame() 00664 static Mat33P calcQInvBlockForBodyXYZInBodyFrame(const Vec3P& a) 00665 { return calcNInvForBodyXYZInBodyFrame(a); } 00666 // REPLACED BY: calcUnnormalizedNForQuaternion() 00667 static Mat<4,3,P> calcUnnormalizedQBlockForQuaternion(const Vec4P& q) 00668 { return calcUnnormalizedNForQuaternion(q); } 00669 // REPLACED BY: calcUnnormalizedNInvForQuaternion() 00670 static Mat<3,4,P> calcUnnormalizedQInvBlockForQuaternion(const Vec4P& q) 00671 { return calcUnnormalizedNInvForQuaternion(q); } 00672 // REPLACED BY: convertAngVelInBodyFrameToBodyXYZDot 00673 static Vec3P convertAngVelToBodyFixed123Dot(const Vec3P& q, const Vec3P& w_PB_B) 00674 { return convertAngVelInBodyFrameToBodyXYZDot(q,w_PB_B); } 00675 // REPLACED BY: convertBodyXYZDotToAngVelInBodyFrame 00676 static Vec3P convertBodyFixed123DotToAngVel(const Vec3P& q, const Vec3P& qdot) 00677 { return convertBodyXYZDotToAngVelInBodyFrame(q,qdot); } 00678 // REPLACED BY: convertAngVelDotInBodyFrameToBodyXYZDotDot 00679 static Vec3P convertAngVelDotToBodyFixed123DotDot 00680 (const Vec3P& q, const Vec3P& w_PB_B, const Vec3P& wdot_PB_B) 00681 { return convertAngVelDotInBodyFrameToBodyXYZDotDot(q,w_PB_B,wdot_PB_B); } 00682 00683 //------------------------------------------------------------------------------ 00684 // The following code is obsolete - it is here temporarily for backward 00685 // compatibility (Mitiguy 9/5/2007) 00686 //------------------------------------------------------------------------------ 00687 private: 00688 // These static methods are like constructors with friendlier names. 00689 static Rotation_ zero() { return Rotation_(); } 00690 static Rotation_ NaN() { Rotation_ r; r.setRotationToNaN(); return r; } 00691 00693 Rotation_& setToZero() { return setRotationToIdentityMatrix(); } 00694 Rotation_& setToIdentityMatrix() { return setRotationToIdentityMatrix(); } 00695 Rotation_& setToNaN() { return setRotationToNaN(); } 00696 static Rotation_ trustMe( const Mat33P& m ) { return Rotation_(m,true); } 00697 00698 // One-angle rotations. 00699 static Rotation_ aboutX( const RealP& angleInRad ) { return Rotation_( angleInRad, XAxis ); } 00700 static Rotation_ aboutY( const RealP& angleInRad ) { return Rotation_( angleInRad, YAxis ); } 00701 static Rotation_ aboutZ( const RealP& angleInRad ) { return Rotation_( angleInRad, ZAxis ); } 00702 static Rotation_ aboutAxis( const RealP& angleInRad, const UnitVec3P& axis ) { return Rotation_(angleInRad,axis); } 00703 static Rotation_ aboutAxis( const RealP& angleInRad, const Vec3P& axis ) { return Rotation_(angleInRad,axis); } 00704 void setToRotationAboutZ( const RealP& q ) { setRotationFromAngleAboutZ( q ); } 00705 00706 // Two-angle space-fixed rotations. 00707 static Rotation_ aboutXThenOldY(const RealP& xInRad, const RealP& yInRad) { return Rotation_( SpaceRotationSequence, xInRad, XAxis, yInRad, YAxis ); } 00708 static Rotation_ aboutYThenOldX(const RealP& yInRad, const RealP& xInRad) { return Rotation_( SpaceRotationSequence, yInRad, YAxis, xInRad, XAxis ); } 00709 static Rotation_ aboutXThenOldZ(const RealP& xInRad, const RealP& zInRad) { return Rotation_( SpaceRotationSequence, xInRad, XAxis, zInRad, ZAxis ); } 00710 static Rotation_ aboutZThenOldX(const RealP& zInRad, const RealP& xInRad) { return Rotation_( SpaceRotationSequence, zInRad, ZAxis, xInRad, XAxis ); } 00711 static Rotation_ aboutYThenOldZ(const RealP& yInRad, const RealP& zInRad) { return Rotation_( SpaceRotationSequence, yInRad, YAxis, zInRad, ZAxis ); } 00712 static Rotation_ aboutZThenOldY(const RealP& zInRad, const RealP& yInRad) { return Rotation_( SpaceRotationSequence, zInRad, ZAxis, yInRad, YAxis ); } 00713 00714 // Two-angle body fixed rotations (reversed space-fixed ones). 00715 static Rotation_ aboutXThenNewY(const RealP& xInRad, const RealP& yInRad) { return Rotation_( BodyRotationSequence, xInRad, XAxis, yInRad, YAxis ); } 00716 static Rotation_ aboutYThenNewX(const RealP& yInRad, const RealP& xInRad) { return aboutXThenOldY(xInRad, yInRad); } 00717 static Rotation_ aboutXThenNewZ(const RealP& xInRad, const RealP& zInRad) { return aboutZThenOldX(zInRad, xInRad); } 00718 static Rotation_ aboutZThenNewX(const RealP& zInRad, const RealP& xInRad) { return aboutXThenOldZ(xInRad, zInRad); } 00719 static Rotation_ aboutYThenNewZ(const RealP& yInRad, const RealP& zInRad) { return aboutZThenOldY(zInRad, yInRad); } 00720 static Rotation_ aboutZThenNewY(const RealP& zInRad, const RealP& yInRad) { return aboutYThenOldZ(yInRad, zInRad); } 00721 00724 explicit Rotation_( const UnitVec3P& uvecZ ) { setRotationFromOneAxis(uvecZ,ZAxis); } 00725 00727 // We will take x seriously after normalizing, but use the y only to create z = normalize(x X y), 00728 // then y = z X x. Bad things happen if x and y are aligned but we may not catch it. 00729 Rotation_( const Vec3P& x, const Vec3P& yish ) { setRotationFromTwoAxes( UnitVec3P(x), XAxis, yish, YAxis ); } 00730 00732 void setToQuaternion( const QuaternionP& q ) { setRotationFromQuaternion(q); } 00733 00738 // Similarly for BodyFixed123. 00739 void setToBodyFixed321( const Vec3P& v) { setRotationFromThreeAnglesThreeAxes( BodyRotationSequence, v[0], ZAxis, v[1], YAxis, v[2], XAxis ); } 00740 void setToBodyFixed123( const Vec3P& v) { setRotationToBodyFixedXYZ(v); } 00741 00744 Vec4P convertToAngleAxis() const { return convertRotationToAngleAxis(); } 00745 00747 QuaternionP convertToQuaternion() const { return convertRotationToQuaternion(); } 00748 00751 void setToSpaceFixed12( const Vec2P& q ) { setRotationFromTwoAnglesTwoAxes( SpaceRotationSequence, q[0], XAxis, q[1], YAxis ); } 00752 00756 Vec3P convertToBodyFixed123() const { return convertRotationToBodyFixedXYZ(); } 00757 Vec2P convertToBodyFixed12() const { return convertRotationToBodyFixedXY(); } 00758 Vec2P convertToSpaceFixed12() const { return convertTwoAxesRotationToTwoAngles( SpaceRotationSequence, XAxis, YAxis ); } 00759 }; 00760 00761 00766 template <class P> 00767 class InverseRotation_ : public Mat<3,3,P>::TransposeType { 00768 typedef P RealP; 00769 typedef Rotation_<P> RotationP; 00770 typedef Mat<3,3,P> Mat33P; // not the base type! 00771 typedef SymMat<3,P> SymMat33P; 00772 typedef Mat<2,2,P> Mat22P; 00773 typedef Mat<3,2,P> Mat32P; 00774 typedef Vec<2,P> Vec2P; 00775 typedef Vec<3,P> Vec3P; 00776 typedef Vec<4,P> Vec4P; 00777 typedef Quaternion_<P> QuaternionP; 00778 public: 00781 typedef typename Mat<3,3,P>::TransposeType BaseMat; 00782 00785 00786 00787 typedef UnitVec<P,BaseMat::RowSpacing> ColType; 00789 typedef UnitRow<P,BaseMat::ColSpacing> RowType; 00791 00795 InverseRotation_() : BaseMat(1) {} 00796 00798 InverseRotation_( const InverseRotation_& R ) : BaseMat(R) {} 00800 InverseRotation_& operator=( const InverseRotation_& R ) 00801 { BaseMat::operator=(R.asMat33()); return *this; } 00802 00809 SimTK_SimTKCOMMON_EXPORT SymMat33P reexpressSymMat33(const SymMat33P& S_BB) const; 00810 00812 00813 const RotationP& invert() const {return *reinterpret_cast<const RotationP*>(this);} 00814 RotationP& updInvert() {return *reinterpret_cast<RotationP*>(this);} 00816 00819 00820 const RotationP& transpose() const { return invert(); } 00821 const RotationP& operator~() const { return invert(); } 00822 RotationP& updTranspose() { return updInvert(); } 00823 RotationP& operator~() { return updInvert(); } 00825 00831 00832 const RowType& row( int i ) const { return reinterpret_cast<const RowType&>(asMat33()[i]); } 00833 const ColType& col( int j ) const { return reinterpret_cast<const ColType&>(asMat33()(j)); } 00834 const ColType& x() const { return col(0); } 00835 const ColType& y() const { return col(1); } 00836 const ColType& z() const { return col(2); } 00837 const RowType& operator[]( int i ) const { return row(i); } 00838 const ColType& operator()( int j ) const { return col(j); } 00840 00843 00844 const BaseMat& asMat33() const { return *static_cast<const BaseMat*>(this); } 00845 BaseMat toMat33() const { return asMat33(); } 00847 }; 00848 00850 template <class P> SimTK_SimTKCOMMON_EXPORT std::ostream& 00851 operator<<(std::ostream&, const Rotation_<P>&); 00853 template <class P> SimTK_SimTKCOMMON_EXPORT std::ostream& 00854 operator<<(std::ostream&, const InverseRotation_<P>&); 00855 00859 00860 template <class P, int S> inline UnitVec<P,1> 00861 operator*(const Rotation_<P>& R, const UnitVec<P,S>& v) {return UnitVec<P,1>(R.asMat33()* v.asVec3(), true);} 00862 template <class P, int S> inline UnitRow<P,1> 00863 operator*(const UnitRow<P,S>& r, const Rotation_<P>& R) {return UnitRow<P,1>(r.asRow3() * R.asMat33(), true);} 00864 template <class P, int S> inline UnitVec<P,1> 00865 operator*(const InverseRotation_<P>& R, const UnitVec<P,S>& v) {return UnitVec<P,1>(R.asMat33()* v.asVec3(), true);} 00866 template <class P, int S> inline UnitRow<P,1> 00867 operator*(const UnitRow<P,S>& r, const InverseRotation_<P>& R) {return UnitRow<P,1>(r.asRow3() * R.asMat33(), true);} 00869 00870 // Couldn't implement these Rotation_ methods until InverseRotation_ was defined. 00871 template <class P> inline 00872 Rotation_<P>::Rotation_(const InverseRotation_<P>& R) : Mat<3,3,P>( R.asMat33() ) {} 00873 template <class P> inline Rotation_<P>& 00874 Rotation_<P>::operator=(const InverseRotation_<P>& R) {static_cast<Mat<3,3,P>&>(*this) = R.asMat33(); return *this;} 00875 template <class P> inline Rotation_<P>& 00876 Rotation_<P>::operator*=(const Rotation_<P>& R) {static_cast<Mat<3,3,P>&>(*this) *= R.asMat33(); return *this;} 00877 template <class P> inline Rotation_<P>& 00878 Rotation_<P>::operator/=(const Rotation_<P>& R) {static_cast<Mat<3,3,P>&>(*this) *= (~R).asMat33(); return *this;} 00879 template <class P> inline Rotation_<P>& 00880 Rotation_<P>::operator*=(const InverseRotation_<P>& R) {static_cast<Mat<3,3,P>&>(*this) *= R.asMat33(); return *this;} 00881 template <class P> inline Rotation_<P>& 00882 Rotation_<P>::operator/=(const InverseRotation_<P>& R) {static_cast<Mat<3,3,P>&>(*this) *= (~R).asMat33(); return *this;} 00883 00885 00886 template <class P> inline Rotation_<P> 00887 operator*(const Rotation_<P>& R1, const Rotation_<P>& R2) {return Rotation_<P>(R1) *= R2;} 00888 template <class P> inline Rotation_<P> 00889 operator*(const Rotation_<P>& R1, const InverseRotation_<P>& R2) {return Rotation_<P>(R1) *= R2;} 00890 template <class P> inline Rotation_<P> 00891 operator*(const InverseRotation_<P>& R1, const Rotation_<P>& R2) {return Rotation_<P>(R1) *= R2;} 00892 template <class P> inline Rotation_<P> 00893 operator*(const InverseRotation_<P>& R1, const InverseRotation_<P>& R2) {return Rotation_<P>(R1) *= R2;} 00895 00898 00899 template <class P> inline Rotation_<P> 00900 operator/( const Rotation_<P>& R1, const Rotation_<P>& R2 ) {return Rotation_<P>(R1) /= R2;} 00901 template <class P> inline Rotation_<P> 00902 operator/( const Rotation_<P>& R1, const InverseRotation& R2 ) {return Rotation_<P>(R1) /= R2;} 00903 template <class P> inline Rotation_<P> 00904 operator/( const InverseRotation_<P>& R1, const Rotation_<P>& R2 ) {return Rotation_<P>(R1) /= R2;} 00905 template <class P> inline Rotation_<P> 00906 operator/( const InverseRotation_<P>& R1, const InverseRotation_<P>& R2 ) {return Rotation_<P>(R1) /= R2;} 00908 00909 00910 //------------------------------------------------------------------------------ 00911 } // End of namespace SimTK 00912 00913 //-------------------------------------------------------------------------- 00914 #endif // SIMTK_ROTATION_H_ 00915 //-------------------------------------------------------------------------- 00916 00917
