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