Transform.h

Go to the documentation of this file.
00001 //-----------------------------------------------------------------------------
00002 // File:     Transform.h
00003 // Class:    Transform and InverseTransform 
00004 // Parent:   None:  Data contains Rotation (for orientation) and Vec3 (translation)
00005 // Purpose:  Transform (orientation and translation) relating two right-handed orthogonal bases
00006 //-----------------------------------------------------------------------------
00007 #ifndef SimTK_TRANSFORM_H 
00008 #define SimTK_TRANSFORM_H 
00009 
00010 /* -------------------------------------------------------------------------- *
00011  *                      SimTK Core: SimTK Simmatrix(tm)                       *
00012  * -------------------------------------------------------------------------- *
00013  * This is part of the SimTK Core biosimulation toolkit originating from      *
00014  * Simbios, the NIH National Center for Physics-Based Simulation of           *
00015  * Biological Structures at Stanford, funded under the NIH Roadmap for        *
00016  * Medical Research, grant U54 GM072970. See https://simtk.org.               *
00017  *                                                                            *
00018  * Portions copyright (c) 2005-7 Stanford University and the Authors.         *
00019  * Authors: Michael Sherman                                                   *
00020  * Contributors: Paul Mitiguy                                                 *
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 #include "SimTKcommon/SmallMatrix.h"
00043 #include "SimTKcommon/internal/BigMatrix.h"
00044 #include "SimTKcommon/internal/UnitVec.h"
00045 #include "SimTKcommon/internal/Quaternion.h"
00046 #include "SimTKcommon/internal/Rotation.h"
00047 //-----------------------------------------------------------------------------
00048 #include <iosfwd>  // Forward declaration of iostream
00049 //-----------------------------------------------------------------------------
00050 
00051 
00052 //-----------------------------------------------------------------------------
00053 namespace SimTK {
00054 
00055 //-----------------------------------------------------------------------------
00056 // Forward declarations
00057 class InverseTransform;
00058 
00059 
00060 //-----------------------------------------------------------------------------
00119 //-----------------------------------------------------------------------------
00120 class Transform {
00121 public:
00123     Transform() : R_BF(),  p_BF(0) { }
00124 
00126     Transform( const Rotation& R, const Vec3& p ) : R_BF(R), p_BF(p) { }
00127 
00130     Transform( const Rotation& R ) : R_BF(R), p_BF(0) { }
00131 
00134     Transform( const Vec3& p ) : R_BF(),  p_BF(p) { }
00135 
00136     // default copy, assignment, destructor
00137 
00145     // (Definition is below after InverseTransform is declared.)
00146     inline Transform&  operator=( const InverseTransform& X );
00147 
00151     Transform&  set( const Rotation& R, const Vec3& p ) { p_BF=p; R_BF=R; return *this; }
00152 
00156     Transform&  setToZero()  { R_BF.setRotationToIdentityMatrix();  p_BF = 0.;  return *this; }
00157 
00162     Transform&  setToNaN()  { R_BF.setRotationToNaN();  p_BF.setToNaN();  return *this; }
00163 
00166     const InverseTransform&  invert() const  { return *reinterpret_cast<const InverseTransform*>(this); }
00167 
00170     InverseTransform&  updInvert()  { return *reinterpret_cast<InverseTransform*>(this); }
00171 
00173     const InverseTransform&  operator~() const  {return invert();}
00174 
00176     InverseTransform&        operator~()        {return updInvert();}
00177 
00180     Transform compose(const Transform& X_FY) const {
00181         return Transform( R_BF * X_FY.R(),  p_BF + R_BF * X_FY.p() );
00182     }
00183 
00189     // (Definition is below after InverseTransform is declared.)
00190     inline Transform  compose( const InverseTransform& X_FY ) const;
00191 
00195     Vec3  xformFrameVecToBase( const Vec3& vF ) const {return R_BF*vF;}
00196 
00200     Vec3  xformBaseVecToFrame( const Vec3& vB ) const  { return ~R_BF*vB; }
00201 
00205     Vec3  shiftFrameStationToBase( const Vec3& sF ) const { return p_BF + xformFrameVecToBase(sF); }
00206 
00210     Vec3  shiftBaseStationToFrame( const Vec3& sB ) const { return xformBaseVecToFrame(sB - p_BF); }
00211 
00213     const Rotation&  R() const  { return R_BF; }
00214 
00216     Rotation&  updR()           { return R_BF; }
00217 
00220     const Rotation::ColType&  x() const  { return R().x(); }
00223     const Rotation::ColType&  y() const  { return R().y(); }
00226     const Rotation::ColType&  z() const  { return R().z(); }
00227 
00230     const InverseRotation&  RInv() const  { return ~R_BF; }
00231 
00234     InverseRotation&  updRInv()  { return ~R_BF; }
00235 
00237     const Vec3&  p() const  { return p_BF; }
00238 
00241     Vec3&  updP()  { return p_BF; }
00242 
00247     Transform&  setP( const Vec3& p )  { p_BF=p; return *this; }
00248 
00252     Vec3  pInv() const  { return -(~R_BF*p_BF); }
00253 
00261     Transform&  setPInv( const Vec3& p_FB )  { p_BF = -(R_BF*p_FB); return *this; }
00262 
00266     const Mat34&  asMat34() const  { return Mat34::getAs(reinterpret_cast<const Real*>(this)); }
00267 
00269     Mat34  toMat34() const  { return asMat34(); }
00270 
00272     Mat44 toMat44() const {
00273         Mat44 tmp;
00274         tmp.updSubMat<3,4>(0,0) = asMat34();
00275         tmp[3]                  = Row4(0,0,0,1);
00276         return tmp;
00277     }
00278 
00279     // OBSOLETE -- alternate name for p
00280     const Vec3& T() const {return p();}
00281     Vec3&  updT()  {return updP();}
00282 
00283 private:
00284     //TODO: these might not pack correctly; should use an array of 12 Reals.
00285     Rotation R_BF;   // rotation matrix that expresses F's axes in R
00286     Vec3     p_BF;   // location of F's origin measured from B's origin, expressed in B 
00287 };
00288 
00289 
00290 //-----------------------------------------------------------------------------
00303 //-----------------------------------------------------------------------------
00304 class InverseTransform {
00305 public:
00306     InverseTransform() : R_FB(), p_FB(0) { }
00307     // default copy, assignment, destructor
00308 
00309     // Implicit conversion to Transform
00310     operator Transform() const  { return Transform( R(), p() ); }
00311 
00312     // Assignment from Transform. This means that the inverse
00313     // transform we're assigning to must end up with the same meaning
00314     // as the inverse transform X has, so we'll need:
00315     //          p* == X.pInv()
00316     //          R* == X.RInv()
00317     // Cost: one frame conversion and a negation for pInv, 18 flops.
00318     InverseTransform&  operator=( const Transform& X ) {
00319         // Be careful to do this in the right order in case X and this
00320         // are the same object, i.e. ~X = X which is weird but has
00321         // the same meaning as X = ~X, i.e. invert X in place.
00322         p_FB = X.pInv(); // This might change X.p ...
00323         R_FB = X.RInv(); // ... but this doesn't depend on X.p.
00324         return *this;
00325     }
00326 
00327     // Inverting one of these just recasts it back to a Transform.
00328     const Transform&  invert() const  { return *reinterpret_cast<const Transform*>(this); }
00329     Transform&  updInvert()           { return *reinterpret_cast<Transform*>(this); }
00330 
00331     // Overload transpose to mean inversion.
00332     const Transform&  operator~() const  { return invert(); }
00333     Transform&        operator~()        { return updInvert(); }
00334 
00335     // Return X_BY=X_BF*X_FY, where X_BF (this) is represented here as ~X_FB. This
00336     // costs exactly the same as a composition of two Transforms (63 flops).
00337     Transform  compose(const Transform& X_FY) const {
00338         return Transform( ~R_FB * X_FY.R(),  ~R_FB *(X_FY.p() - p_FB) );
00339     }
00340     // Return X_BY=X_BF*X_FY, but now both xforms are represented by their inverses.
00341     // This costs one extra vector transformation and a negation (18 flops) more
00342     // than a composition of two Transforms, for a total of 81 flops.
00343     Transform  compose(const InverseTransform& X_FY) const { 
00344         return Transform(  ~R_FB * X_FY.R(),  ~R_FB *(X_FY.p() - p_FB)  ); 
00345     }
00346 
00347     // Forward and inverse vector transformations cost the same here as
00348     // for a Transform (or for that matter, a Rotation): 15 flops.
00349     Vec3  xformFrameVecToBase(const Vec3& vF) const {return ~R_FB*vF;}
00350     Vec3  xformBaseVecToFrame(const Vec3& vB) const {return  R_FB*vB;}
00351 
00352     // Forward and inverse station shift & transform cost the same here as for a Transform: 18 flops.
00353     Vec3  shiftFrameStationToBase(const Vec3& sF) const  { return ~R_FB*(sF-p_FB); }
00354     Vec3  shiftBaseStationToFrame(const Vec3& sB) const  { return R_FB*sB + p_FB; }
00355     
00356     const InverseRotation&  R() const  {return ~R_FB;}
00357     InverseRotation&        updR()     {return ~R_FB;}
00358 
00359     const InverseRotation::ColType&  x() const  {return R().x();}
00360     const InverseRotation::ColType&  y() const  {return R().y();}
00361     const InverseRotation::ColType&  z() const  {return R().z();}
00362 
00363     const Rotation&  RInv() const  {return R_FB;}
00364     Rotation&        updRInv()     {return R_FB;}
00365 
00366     // Costs 18 flops to look at the real translation vector.
00367     Vec3  p() const  { return -(~R_FB*p_FB); }
00368 
00369 
00370     // no updP lvalue
00371 
00372     // Sorry, can't update translation as an lvalue, but here we
00373     // want -(R_BF*p_FB)=p_BF => p_FB=-(R_FB*p_BF). Cost: 18 flops.
00374     void  setP( const Vec3& p_BF )  { p_FB = -(R_FB*p_BF); }
00375 
00376     // Inverse translation is free.
00377     const Vec3&  pInv() const              { return p_FB; }
00378     void         setPInv( const Vec3& p )  { p_FB = p; }
00379 
00382     Mat34  toMat34() const  { return Transform(*this).asMat34(); }
00383 
00385     Mat44  toMat44() const  { return Transform(*this).toMat44(); }
00386 
00387     // OBSOLETE -- alternate name for p.
00388     Vec3 T() const {return p();}
00389 
00390 private:
00391     // DATA LAYOUT MUST BE IDENTICAL TO Transform !!
00392     // TODO: redo packing here when it is done for Transform.
00393     Rotation R_FB; // transpose of our rotation matrix, R_BF
00394     Vec3     p_FB; // our translation is -(R_BF*p_FB)=-(~R_FB*p_FB)
00395 };
00396 
00397 
00400 inline Vec3  operator*( const Transform& X_BF,        const Vec3& s_F )  { return X_BF.shiftFrameStationToBase(s_F); }
00401 inline Vec3  operator*( const InverseTransform& X_BF, const Vec3& s_F )  { return X_BF.shiftFrameStationToBase(s_F); }
00402 
00406 //-----------------------------------------------------------------------
00407 inline Vec4  operator*( const Transform& X_BF, const Vec4& a_F ) {
00408     assert(a_F[3]==0 || a_F[3]==1);
00409     const Vec3& v_F = Vec3::getAs(&a_F[0]); // recast the 1st 3 elements as Vec3
00410 
00411     Vec4 out;
00412     if( a_F[3] == 0 ) { Vec3::updAs(&out[0]) = X_BF.xformFrameVecToBase(v_F);      out[3] = 0; } 
00413     else              { Vec3::updAs(&out[0]) = X_BF.shiftFrameStationToBase(v_F);  out[3] = 1; }
00414     return out;
00415 }
00416 
00417 //-----------------------------------------------------------------------
00418 inline Vec4  operator*( const InverseTransform& X_BF, const Vec4& a_F ) {
00419     assert(a_F[3]==0 || a_F[3]==1);
00420     const Vec3& v_F = Vec3::getAs(&a_F[0]); // recast the 1st 3 elements as Vec3
00421 
00422     Vec4 out;
00423     if( a_F[3] == 0 ) { Vec3::updAs(&out[0]) = X_BF.xformFrameVecToBase(v_F);      out[3] = 0; } 
00424     else              { Vec3::updAs(&out[0]) = X_BF.shiftFrameStationToBase(v_F);  out[3] = 1; }
00425     return out;
00426 }
00427 
00429 
00430 template <class E>
00431 inline Vector_<E> operator*(const Transform& X, const VectorBase<E>& v) {
00432     Vector_<E> result(v.size());
00433     for (int i = 0; i < v.size(); ++i)
00434         result[i] = X*v[i];
00435     return result;
00436 }
00437 template <class E>
00438 inline Vector_<E> operator*(const VectorBase<E>& v, const Transform& X) {
00439     Vector_<E> result(v.size());
00440     for (int i = 0; i < v.size(); ++i)
00441         result[i] = X*v[i];
00442     return result;
00443 }
00444 template <class E>
00445 inline RowVector_<E> operator*(const Transform& X, const RowVectorBase<E>& v) {
00446     RowVector_<E> result(v.size());
00447     for (int i = 0; i < v.size(); ++i)
00448         result[i] = X*v[i];
00449     return result;
00450 }
00451 template <class E>
00452 inline RowVector_<E> operator*(const RowVectorBase<E>& v, const Transform& X) {
00453     RowVector_<E> result(v.size());
00454     for (int i = 0; i < v.size(); ++i)
00455         result[i] = X*v[i];
00456     return result;
00457 }
00458 template <class E>
00459 inline Matrix_<E> operator*(const Transform& X, const MatrixBase<E>& v) {
00460     Matrix_<E> result(v.nrow(), v.ncol());
00461     for (int i = 0; i < v.nrow(); ++i)
00462         for (int j = 0; j < v.ncol(); ++j)
00463             result(i, j) = X*v(i, j);
00464     return result;
00465 }
00466 template <class E>
00467 inline Matrix_<E> operator*(const MatrixBase<E>& v, const Transform& X) {
00468     Matrix_<E> result(v.nrow(), v.ncol());
00469     for (int i = 0; i < v.nrow(); ++i)
00470         for (int j = 0; j < v.ncol(); ++j)
00471             result(i, j) = X*v(i, j);
00472     return result;
00473 }
00474 template <int N, class E>
00475 inline Vec<N,E> operator*(const Transform& X, const Vec<N,E>& v) {
00476     Vec<N,E> result;
00477     for (int i = 0; i < N; ++i)
00478         result[i] = X*v[i];
00479     return result;
00480 }
00481 template <int N, class E>
00482 inline Vec<N,E> operator*(const Vec<N,E>& v, const Transform& X) {
00483     Vec<N,E> result;
00484     for (int i = 0; i < N; ++i)
00485         result[i] = X*v[i];
00486     return result;
00487 }
00488 template <int N, class E>
00489 inline Row<N,E> operator*(const Transform& X, const Row<N,E>& v) {
00490     Row<N,E> result;
00491     for (int i = 0; i < N; ++i)
00492         result[i] = X*v[i];
00493     return result;
00494 }
00495 template <int N, class E>
00496 inline Row<N,E> operator*(const Row<N,E>& v, const Transform& X) {
00497     Row<N,E> result;
00498     for (int i = 0; i < N; ++i)
00499         result[i] = X*v[i];
00500     return result;
00501 }
00502 template <int M, int N, class E>
00503 inline Mat<M,N,E> operator*(const Transform& X, const Mat<M,N,E>& v) {
00504     Mat<M,N,E> result;
00505     for (int i = 0; i < M; ++i)
00506         for (int j = 0; j < N; ++j)
00507             result(i, j) = X*v(i, j);
00508     return result;
00509 }
00510 template <int M, int N, class E>
00511 inline Mat<M,N,E> operator*(const Mat<M,N,E>& v, const Transform& X) {
00512     Mat<M,N,E> result;
00513     for (int i = 0; i < M; ++i)
00514         for (int j = 0; j < N; ++j)
00515             result(i, j) = X*v(i, j);
00516     return result;
00517 }
00518 
00520 
00521 // These Transform definitions had to wait for InverseTransform to be declared.
00522 inline Transform&  Transform::operator=( const InverseTransform& X ) {
00523     // Be careful to do this in the right order in case X and this
00524     // are the same object, i.e. we're doing X = ~X, inverting X in place.
00525     p_BF = X.p(); // This might change X.p ...
00526     R_BF = X.R(); // ... but this doesn't depend on X.p.
00527     return *this;
00528 }
00529 
00530 inline Transform  Transform::compose( const InverseTransform& X_FY ) const {
00531     return Transform( R_BF * X_FY.R(), p_BF + R_BF * X_FY.p() );
00532 }
00533 
00534 inline Transform  operator*( const Transform& X1,        const Transform& X2 )         { return X1.compose(X2); }
00535 inline Transform  operator*( const Transform& X1,        const InverseTransform& X2 )  { return X1.compose(X2); }
00536 inline Transform  operator*( const InverseTransform& X1, const Transform& X2 )         { return X1.compose(X2); }
00537 inline Transform  operator*( const InverseTransform& X1, const InverseTransform& X2 )  { return X1.compose(X2); }
00538 
00539 inline bool  operator==( const Transform& X1,        const Transform& X2 )         { return X1.R()==X2.R() && X1.p()==X2.p(); }
00540 inline bool  operator==( const InverseTransform& X1, const InverseTransform& X2 )  { return X1.R()==X2.R() && X1.p()==X2.p(); }
00541 inline bool  operator==( const Transform& X1,        const InverseTransform& X2 )  { return X1.R()==X2.R() && X1.p()==X2.p(); }
00542 inline bool  operator==( const InverseTransform& X1, const Transform& X2 )         { return X1.R()==X2.R() && X1.p()==X2.p(); }
00543 
00544 SimTK_SimTKCOMMON_EXPORT std::ostream&  operator<<( std::ostream& o, const Transform& );
00545 
00546 
00547 //------------------------------------------------------------------------------
00548 }  // End of namespace SimTK
00549 
00550 //--------------------------------------------------------------------------
00551 #endif // SimTK_TRANSFORM_H_
00552 //--------------------------------------------------------------------------
00553 

Generated by  doxygen 1.6.2