Transform.h

Go to the documentation of this file.

//-----------------------------------------------------------------------------
// File:     Transform.h
// Class:    Transform and InverseTransform 
// Parent:   None:  Data contains Rotation (for orientation) and Vec3 (translation)
// Purpose:  Transform (orientation and translation) relating two right-handed orthogonal bases
//-----------------------------------------------------------------------------
#ifndef SimTK_TRANSFORM_H 
#define SimTK_TRANSFORM_H 

/* -------------------------------------------------------------------------- *
 *                      SimTK Core: SimTK Simmatrix(tm)                       *
 * -------------------------------------------------------------------------- *
 * This is part of the SimTK Core biosimulation toolkit originating from      *
 * Simbios, the NIH National Center for Physics-Based Simulation of           *
 * Biological Structures at Stanford, funded under the NIH Roadmap for        *
 * Medical Research, grant U54 GM072970. See https://simtk.org.               *
 *                                                                            *
 * Portions copyright (c) 2005-7 Stanford University and the Authors.         *
 * Authors: Michael Sherman                                                   *
 * Contributors: Paul Mitiguy                                                 *
 *                                                                            *
 * Permission is hereby granted, free of charge, to any person obtaining a    *
 * copy of this software and associated documentation files (the "Software"), *
 * to deal in the Software without restriction, including without limitation  *
 * the rights to use, copy, modify, merge, publish, distribute, sublicense,   *
 * and/or sell copies of the Software, and to permit persons to whom the      *
 * Software is furnished to do so, subject to the following conditions:       *
 *                                                                            *
 * The above copyright notice and this permission notice shall be included in *
 * all copies or substantial portions of the Software.                        *
 *                                                                            *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR *
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,   *
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL    *
 * THE AUTHORS, CONTRIBUTORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM,    *
 * DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR      *
 * OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE  *
 * USE OR OTHER DEALINGS IN THE SOFTWARE.                                     *
 * -------------------------------------------------------------------------- */

//-----------------------------------------------------------------------------
#include "SimTKcommon/SmallMatrix.h"
#include "SimTKcommon/internal/BigMatrix.h"
#include "SimTKcommon/internal/UnitVec.h"
#include "SimTKcommon/internal/Quaternion.h"
#include "SimTKcommon/internal/Rotation.h"
//-----------------------------------------------------------------------------
#include <iosfwd>  // Forward declaration of iostream
//-----------------------------------------------------------------------------


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

//-----------------------------------------------------------------------------
// Forward declarations
class InverseTransform;


//-----------------------------------------------------------------------------
//-----------------------------------------------------------------------------
class Transform {
public:
    Transform() : R_BF(),  T_BF(0) { }

    Transform( const Rotation& R, const Vec3& T ) : R_BF(R), T_BF(T) { }

    Transform( const Rotation& R ) : R_BF(R), T_BF(0) { }

    Transform( const Vec3& T ) : R_BF(),  T_BF(T) { }

    // default copy, assignment, destructor

    // (Definition is below after InverseTransform is declared.)
    inline Transform&  operator=( const InverseTransform& X );

    Transform&  set( const Rotation& R, const Vec3& T ) { T_BF=T; R_BF=R; return *this; }

    Transform&  setToZero()  { R_BF.setRotationToIdentityMatrix();  T_BF = 0.;  return *this; }

    Transform&  setToNaN()  { R_BF.setRotationToNaN();  T_BF.setToNaN();  return *this; }

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

    InverseTransform&  updInvert()  { return *reinterpret_cast<InverseTransform*>(this); }

    const InverseTransform&  operator~() const  {return invert();}

    InverseTransform&        operator~()        {return updInvert();}

    Transform compose(const Transform& X_FY) const {
        return Transform( R_BF * X_FY.R(),  T_BF + R_BF * X_FY.T() );
    }

    // (Definition is below after InverseTransform is declared.)
    inline Transform  compose( const InverseTransform& X_FY ) const;

    Vec3  xformFrameVecToBase( const Vec3& vF ) const {return R_BF*vF;}

    Vec3  xformBaseVecToFrame( const Vec3& vB ) const  { return ~R_BF*vB; }

    Vec3  shiftFrameStationToBase( const Vec3& sF ) const { return T_BF + xformFrameVecToBase(sF); }

    Vec3  shiftBaseStationToFrame( const Vec3& sB ) const { return xformBaseVecToFrame(sB - T_BF); }

    const Rotation&  R() const  { return R_BF; }

    Rotation&  updR()           { return R_BF; }

    const Rotation::ColType&  x() const  { return R().x(); }
    const Rotation::ColType&  y() const  { return R().y(); }
    const Rotation::ColType&  z() const  { return R().z(); }

    const InverseRotation&  RInv() const  { return ~R_BF; }

    InverseRotation&  updRInv()  { return ~R_BF; }

    const Vec3&  T() const  { return T_BF; }

    Vec3&  updT()  { return T_BF; }

    Transform&  setT( const Vec3& T )  { T_BF=T; return *this; }

    Vec3  TInv() const  { return -(~R_BF*T_BF); }

    Transform&  setTInv( const Vec3& T_FB )  { T_BF = -(R_BF*T_FB); return *this; }

    const Mat34&  asMat34() const  { return Mat34::getAs(reinterpret_cast<const Real*>(this)); }

    Mat34  toMat34() const  { return asMat34(); }

    Mat44 toMat44() const {
        Mat44 tmp;
        tmp.updSubMat<3,4>(0,0) = asMat34();
        tmp[3]                  = Row4(0,0,0,1);
        return tmp;
    }
private:
    //TODO: these might not pack correctly; should use an array of 12 Reals.
    Rotation R_BF;   // rotation matrix that expresses F's axes in R
    Vec3     T_BF;   // location of F's origin measured from B's origin, expressed in B 
};


//-----------------------------------------------------------------------------
//-----------------------------------------------------------------------------
class InverseTransform {
public:
    InverseTransform() : R_FB(), T_FB(0) { }
    // default copy, assignment, destructor

    // Implicit conversion to Transform
    operator Transform() const  { return Transform( R(), T() ); }

    // Assignment from Transform. This means that the inverse
    // transform we're assigning to must end up with the same meaning
    // as the inverse transform X has, so we'll need:
    //          T* == X.TInv()
    //          R* == X.RInv()
    // Cost: one frame conversion and a negation for TInv, 18 flops.
    InverseTransform&  operator=( const Transform& X ) {
        // Be careful to do this in the right order in case X and this
        // are the same object, i.e. ~X = X which is weird but has
        // the same meaning as X = ~X, i.e. invert X in place.
        T_FB = X.TInv(); // This might change X.T ...
        R_FB = X.RInv(); // ... but this doesn't depend on X.T.
        return *this;
    }

    // Inverting one of these just recasts it back to a Transform.
    const Transform&  invert() const  { return *reinterpret_cast<const Transform*>(this); }
    Transform&  updInvert()           { return *reinterpret_cast<Transform*>(this); }

    // Overload transpose to mean inversion.
    const Transform&  operator~() const  { return invert(); }
    Transform&        operator~()        { return updInvert(); }

    // Return X_BY=X_BF*X_FY, where X_BF (this) is represented here as ~X_FB. This
    // costs exactly the same as a composition of two Transforms (63 flops).
    Transform  compose(const Transform& X_FY) const {
        return Transform( ~R_FB * X_FY.R(),  ~R_FB *(X_FY.T() - T_FB) );
    }
    // Return X_BY=X_BF*X_FY, but now both xforms are represented by their inverses.
    // This costs one extra vector transformation and a negation (18 flops) more
    // than a composition of two Transforms, for a total of 81 flops.
    Transform  compose(const InverseTransform& X_FY) const { 
        return Transform(  ~R_FB * X_FY.R(),  ~R_FB *(X_FY.T() - T_FB)  ); 
    }

    // Forward and inverse vector transformations cost the same here as
    // for a Transform (or for that matter, a Rotation): 15 flops.
    Vec3  xformFrameVecToBase(const Vec3& vF) const {return ~R_FB*vF;}
    Vec3  xformBaseVecToFrame(const Vec3& vB) const {return  R_FB*vB;}

    // Forward and inverse station shift & transform cost the same here as for a Transform: 18 flops.
    Vec3  shiftFrameStationToBase(const Vec3& sF) const  { return ~R_FB*(sF-T_FB); }
    Vec3  shiftBaseStationToFrame(const Vec3& sB) const  { return R_FB*sB + T_FB; }
    
    const InverseRotation&  R() const  {return ~R_FB;}
    InverseRotation&        updR()     {return ~R_FB;}

    const InverseRotation::ColType&  x() const  {return R().x();}
    const InverseRotation::ColType&  y() const  {return R().y();}
    const InverseRotation::ColType&  z() const  {return R().z();}

    const Rotation&  RInv() const  {return R_FB;}
    Rotation&        updRInv()     {return R_FB;}

    // Costs 18 flops to look at the real translation vector.
    Vec3  T() const  { return -(~R_FB*T_FB); }
    // no updT lvalue

    // Sorry, can't update translation as an lvalue, but here we
    // want -(R_BF*T_FB)=T_BF => T_FB=-(R_FB*T_BF). Cost: 18 flops.
    void  setT( const Vec3& T_BF )  { T_FB = -(R_FB*T_BF); }

    // Inverse translation is free.
    const Vec3&  TInv() const              { return T_FB; }
    void         setTInv( const Vec3& T )  { T_FB = T; }

    Mat34  toMat34() const  { return Transform(*this).asMat34(); }

    Mat44  toMat44() const  { return Transform(*this).toMat44(); }

private:
    // DATA LAYOUT MUST BE IDENTICAL TO Transform !!
    // TODO: redo packing here when it is done for Transform.
    Rotation R_FB; // transpose of our rotation matrix, R_BF
    Vec3     T_FB; // our translation is -(R_BF*T_FB)=-(~R_FB*T_FB)
};


inline Vec3  operator*( const Transform& X_BF,        const Vec3& s_F )  { return X_BF.shiftFrameStationToBase(s_F); }
inline Vec3  operator*( const InverseTransform& X_BF, const Vec3& s_F )  { return X_BF.shiftFrameStationToBase(s_F); }

//-----------------------------------------------------------------------
inline Vec4  operator*( const Transform& X_BF, const Vec4& a_F ) {
    assert(a_F[3]==0 || a_F[3]==1);
    const Vec3& v_F = Vec3::getAs(&a_F[0]); // recast the 1st 3 elements as Vec3

    Vec4 out;
    if( a_F[3] == 0 ) { Vec3::updAs(&out[0]) = X_BF.xformFrameVecToBase(v_F);      out[3] = 0; } 
    else              { Vec3::updAs(&out[0]) = X_BF.shiftFrameStationToBase(v_F);  out[3] = 1; }
    return out;
}

//-----------------------------------------------------------------------
inline Vec4  operator*( const InverseTransform& X_BF, const Vec4& a_F ) {
    assert(a_F[3]==0 || a_F[3]==1);
    const Vec3& v_F = Vec3::getAs(&a_F[0]); // recast the 1st 3 elements as Vec3

    Vec4 out;
    if( a_F[3] == 0 ) { Vec3::updAs(&out[0]) = X_BF.xformFrameVecToBase(v_F);      out[3] = 0; } 
    else              { Vec3::updAs(&out[0]) = X_BF.shiftFrameStationToBase(v_F);  out[3] = 1; }
    return out;
}


template <class E>
inline Vector_<E> operator*(const Transform& T, const VectorBase<E>& v) {
    Vector_<E> result(v.size());
    for (int i = 0; i < v.size(); ++i)
        result[i] = T*v[i];
    return result;
}
template <class E>
inline Vector_<E> operator*(const VectorBase<E>& v, const Transform& T) {
    Vector_<E> result(v.size());
    for (int i = 0; i < v.size(); ++i)
        result[i] = T*v[i];
    return result;
}
template <class E>
inline RowVector_<E> operator*(const Transform& T, const RowVectorBase<E>& v) {
    RowVector_<E> result(v.size());
    for (int i = 0; i < v.size(); ++i)
        result[i] = T*v[i];
    return result;
}
template <class E>
inline RowVector_<E> operator*(const RowVectorBase<E>& v, const Transform& T) {
    RowVector_<E> result(v.size());
    for (int i = 0; i < v.size(); ++i)
        result[i] = T*v[i];
    return result;
}
template <class E>
inline Matrix_<E> operator*(const Transform& T, const MatrixBase<E>& v) {
    Matrix_<E> result(v.nrow(), v.ncol());
    for (int i = 0; i < v.nrow(); ++i)
        for (int j = 0; j < v.ncol(); ++j)
            result(i, j) = T*v(i, j);
    return result;
}
template <class E>
inline Matrix_<E> operator*(const MatrixBase<E>& v, const Transform& T) {
    Matrix_<E> result(v.nrow(), v.ncol());
    for (int i = 0; i < v.nrow(); ++i)
        for (int j = 0; j < v.ncol(); ++j)
            result(i, j) = T*v(i, j);
    return result;
}
template <int N, class E>
inline Vec<N,E> operator*(const Transform& T, const Vec<N,E>& v) {
    Vec<N,E> result;
    for (int i = 0; i < N; ++i)
        result[i] = T*v[i];
    return result;
}
template <int N, class E>
inline Vec<N,E> operator*(const Vec<N,E>& v, const Transform& T) {
    Vec<N,E> result;
    for (int i = 0; i < N; ++i)
        result[i] = T*v[i];
    return result;
}
template <int N, class E>
inline Row<N,E> operator*(const Transform& T, const Row<N,E>& v) {
    Row<N,E> result;
    for (int i = 0; i < N; ++i)
        result[i] = T*v[i];
    return result;
}
template <int N, class E>
inline Row<N,E> operator*(const Row<N,E>& v, const Transform& T) {
    Row<N,E> result;
    for (int i = 0; i < N; ++i)
        result[i] = T*v[i];
    return result;
}
template <int M, int N, class E>
inline Mat<M,N,E> operator*(const Transform& T, const Mat<M,N,E>& v) {
    Mat<M,N,E> result;
    for (int i = 0; i < M; ++i)
        for (int j = 0; j < N; ++j)
            result(i, j) = T*v(i, j);
    return result;
}
template <int M, int N, class E>
inline Mat<M,N,E> operator*(const Mat<M,N,E>& v, const Transform& T) {
    Mat<M,N,E> result;
    for (int i = 0; i < M; ++i)
        for (int j = 0; j < N; ++j)
            result(i, j) = T*v(i, j);
    return result;
}


// These Transform definitions had to wait for InverseTransform to be declared.
inline Transform&  Transform::operator=( const InverseTransform& X ) {
    // Be careful to do this in the right order in case X and this
    // are the same object, i.e. we're doing X = ~X, inverting X in place.
    T_BF = X.T(); // This might change X.T ...
    R_BF = X.R(); // ... but this doesn't depend on X.T.
    return *this;
}

inline Transform  Transform::compose( const InverseTransform& X_FY ) const {
    return Transform( R_BF * X_FY.R(), T_BF + R_BF * X_FY.T() );
}

inline Transform  operator*( const Transform& X1,        const Transform& X2 )         { return X1.compose(X2); }
inline Transform  operator*( const Transform& X1,        const InverseTransform& X2 )  { return X1.compose(X2); }
inline Transform  operator*( const InverseTransform& X1, const Transform& X2 )         { return X1.compose(X2); }
inline Transform  operator*( const InverseTransform& X1, const InverseTransform& X2 )  { return X1.compose(X2); }

inline bool  operator==( const Transform& X1,        const Transform& X2 )         { return X1.R()==X2.R() && X1.T()==X2.T(); }
inline bool  operator==( const InverseTransform& X1, const InverseTransform& X2 )  { return X1.R()==X2.R() && X1.T()==X2.T(); }
inline bool  operator==( const Transform& X1,        const InverseTransform& X2 )  { return X1.R()==X2.R() && X1.T()==X2.T(); }
inline bool  operator==( const InverseTransform& X1, const Transform& X2 )         { return X1.R()==X2.R() && X1.T()==X2.T(); }

SimTK_SimTKCOMMON_EXPORT std::ostream&  operator<<( std::ostream& o, const Transform& );


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

//--------------------------------------------------------------------------
#endif // SimTK_TRANSFORM_H_
//--------------------------------------------------------------------------

---