Transform.h

Go to the documentation of this file.

#ifndef SimTK_SimTKCOMMON_TRANSFORM_H 
#define SimTK_SimTKCOMMON_TRANSFORM_H 

/* -------------------------------------------------------------------------- *
 *                       Simbody(tm): SimTKcommon                             *
 * -------------------------------------------------------------------------- *
 * This is part of the SimTK 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/home/simbody.  *
 *                                                                            *
 * Portions copyright (c) 2005-12 Stanford University and the Authors.        *
 * Authors: Michael Sherman                                                   *
 * Contributors: Paul Mitiguy                                                 *
 *                                                                            *
 * Licensed under the Apache License, Version 2.0 (the "License"); you may    *
 * not use this file except in compliance with the License. You may obtain a  *
 * copy of the License at http://www.apache.org/licenses/LICENSE-2.0.         *
 *                                                                            *
 * Unless required by applicable law or agreed to in writing, software        *
 * distributed under the License is distributed on an "AS IS" BASIS,          *
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.   *
 * See the License for the specific language governing permissions and        *
 * limitations under the License.                                             *
 * -------------------------------------------------------------------------- */

//-----------------------------------------------------------------------------
#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 (everything is templatized by precision).
template <class P> class Transform_;
template <class P> class InverseTransform_;

typedef Transform_<Real>    Transform;
typedef Transform_<float>   fTransform;
typedef Transform_<double>  dTransform;


//-----------------------------------------------------------------------------
//-----------------------------------------------------------------------------
template <class P>
class Transform_ {
public:
    Transform_() : R_BF(),  p_BF(0) { }

    Transform_( const Rotation_<P>& R, const Vec<3,P>& p ) : R_BF(R), p_BF(p) { }

    Transform_( const Rotation_<P>& R ) : R_BF(R), p_BF(0) { }

    Transform_( const Vec<3,P>& p ) : R_BF(),  p_BF(p) { }

    // default copy, assignment, destructor

    // (Definition is below after InverseTransform is declared.)
    inline Transform_&  operator=( const InverseTransform_<P>& X );

    template <int S>
    Transform_& operator+=(const Vec<3,P,S>& offset_B)
    {   p_BF += offset_B; return *this; }

    template <int S>
    Transform_& operator-=(const Vec<3,P,S>& offset_B)
    {   p_BF -= offset_B; return *this; }

    Transform_&  set( const Rotation_<P>& R, const Vec<3,P>& p ) { p_BF=p; R_BF=R; return *this; }

    Transform_&  setToZero()  { R_BF.setRotationToIdentityMatrix();  p_BF = P(0);  return *this; }

    Transform_&  setToNaN()  { R_BF.setRotationToNaN();  p_BF.setToNaN();  return *this; }

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

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

    const InverseTransform_<P>&  operator~() const  {return invert();}

    InverseTransform_<P>&        operator~()        {return updInvert();}

    Transform_ compose(const Transform_& X_FY) const {
        return Transform_( R_BF * X_FY.R(),  p_BF + R_BF * X_FY.p() );
    }

    // (Definition is below after InverseTransform_ is declared.)
    inline Transform_  compose( const InverseTransform_<P>& X_FY ) const;

    Vec<3,P>  xformFrameVecToBase( const Vec<3,P>& vF ) const {return R_BF*vF;}

    Vec<3,P>  xformBaseVecToFrame( const Vec<3,P>& vB ) const  { return ~R_BF*vB; }

    Vec<3,P>  shiftFrameStationToBase( const Vec<3,P>& sF ) const 
    {   return p_BF + xformFrameVecToBase(sF); }

    Vec<3,P>  shiftBaseStationToFrame( const Vec<3,P>& sB ) const 
    {   return xformBaseVecToFrame(sB - p_BF); }

    const Rotation_<P>&  R() const  { return R_BF; }

    Rotation_<P>&  updR()           { return R_BF; }

    const typename Rotation_<P>::ColType&  x() const  { return R().x(); }
    const typename Rotation_<P>::ColType&  y() const  { return R().y(); }
    const typename Rotation_<P>::ColType&  z() const  { return R().z(); }

    const InverseRotation_<P>&  RInv() const  { return ~R_BF; }

    InverseRotation_<P>&  updRInv()  { return ~R_BF; }

    const Vec<3,P>&  p() const  { return p_BF; }

    Vec<3,P>&  updP()  { return p_BF; }

    Transform_<P>&  setP( const Vec<3,P>& p )  { p_BF=p; return *this; }

    Vec<3,P>  pInv() const  { return -(~R_BF*p_BF); }

    Transform_<P>&  setPInv( const Vec<3,P>& p_FB )  { p_BF = -(R_BF*p_FB); return *this; }

    const Mat<3,4,P>&  asMat34() const  { return Mat<3,4,P>::getAs(reinterpret_cast<const P*>(this)); }

    Mat<3,4,P>  toMat34() const  { return asMat34(); }

    Mat<4,4,P> toMat44() const {
        Mat<4,4,P> tmp;
        tmp.template updSubMat<3,4>(0,0) = asMat34();
        tmp[3]                  = Row<4,P>(0,0,0,1);
        return tmp;
    }

    // OBSOLETE -- alternate name for p
    const Vec<3,P>& T() const {return p();}
    Vec<3,P>&  updT()  {return updP();}

private:
    //TODO: these might not pack correctly; should use an array of 12 Reals.
    Rotation_<P> R_BF;   // rotation matrix that expresses F's axes in R
    Vec<3,P>     p_BF;   // location of F's origin measured from B's origin, expressed in B 
};


//-----------------------------------------------------------------------------
//-----------------------------------------------------------------------------
template <class P>
class InverseTransform_ {
public:
    InverseTransform_() : R_FB(), p_FB(0) { }

    // default copy, assignment, destructor

    operator Transform_<P>() const  { return Transform_<P>( R(), p() ); }

    // 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:
    //          p* == X.pInv()
    //          R* == X.RInv()
    // Cost: one frame conversion and a negation for pInv, 18 flops.
    InverseTransform_&  operator=( const Transform_<P>& 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.
        p_FB = X.pInv(); // This might change X.p ...
        R_FB = X.RInv(); // ... but this doesn't depend on X.p.
        return *this;
    }

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

    // Overload transpose to mean inversion.
    const Transform_<P>&  operator~() const  { return invert(); }
    Transform_<P>&        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_<P>  compose(const Transform_<P>& X_FY) const {
        return Transform_<P>( ~R_FB * X_FY.R(),  ~R_FB *(X_FY.p() - p_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_<P>  compose(const InverseTransform_<P>& X_FY) const { 
        return Transform_<P>(  ~R_FB * X_FY.R(),  ~R_FB *(X_FY.p() - p_FB)  ); 
    }

    // Forward and inverse vector transformations cost the same here as
    // for a Transform_<P> (or for that matter, a Rotation_<P>): 15 flops.
    Vec<3,P>  xformFrameVecToBase(const Vec<3,P>& vF) const {return ~R_FB*vF;}
    Vec<3,P>  xformBaseVecToFrame(const Vec<3,P>& vB) const {return  R_FB*vB;}

    // Forward and inverse station shift & transform cost the same here as for a Transform_<P>: 18 flops.
    Vec<3,P>  shiftFrameStationToBase(const Vec<3,P>& sF) const  { return ~R_FB*(sF-p_FB); }
    Vec<3,P>  shiftBaseStationToFrame(const Vec<3,P>& sB) const  { return R_FB*sB + p_FB; }
    
    const InverseRotation_<P>&  R() const  {return ~R_FB;}
    InverseRotation_<P>&        updR()     {return ~R_FB;}

    const typename InverseRotation_<P>::ColType&  x() const  {return R().x();}
    const typename InverseRotation_<P>::ColType&  y() const  {return R().y();}
    const typename InverseRotation_<P>::ColType&  z() const  {return R().z();}

    const Rotation_<P>&  RInv() const  {return R_FB;}
    Rotation_<P>&        updRInv()     {return R_FB;}

    Vec<3,P>  p() const  { return -(~R_FB*p_FB); }


    // no updP lvalue

    // Sorry, can't update translation as an lvalue, but here we
    // want -(R_BF*p_FB)=p_BF => p_FB=-(R_FB*p_BF). Cost: 18 flops.
    void  setP( const Vec<3,P>& p_BF )  { p_FB = -(R_FB*p_BF); }

    // Inverse translation is free.
    const Vec<3,P>&  pInv() const              { return p_FB; }
    void         setPInv( const Vec<3,P>& p )  { p_FB = p; }

    Mat<3,4,P>  toMat34() const  { return Transform_<P>(*this).asMat34(); }

    Mat<4,4,P>  toMat44() const  { return Transform_<P>(*this).toMat44(); }

    // OBSOLETE -- alternate name for p.
    Vec<3,P> T() const {return p();}

private:
    // DATA LAYOUT MUST BE IDENTICAL TO Transform_<P> !!
    // TODO: redo packing here when it is done for Transform_<P>.
    Rotation_<P> R_FB; // transpose of our rotation matrix, R_BF
    Vec<3,P>     p_FB; // our translation is -(R_BF*p_FB)=-(~R_FB*p_FB)
};

template <class P, int S> inline Vec<3,P>  
operator*(const Transform_<P>& X_BF,        const Vec<3,P,S>& s_F)  
{   return X_BF.shiftFrameStationToBase(s_F); }
template <class P, int S> inline Vec<3,P>  
operator*(const InverseTransform_<P>& X_BF, const Vec<3,P,S>& s_F)  
{   return X_BF.shiftFrameStationToBase(s_F); }
template <class P, int S> inline Vec<3,P>  
operator*(const Transform_<P>& X_BF,        const Vec<3,negator<P>,S>& s_F)  
{   return X_BF*Vec<3,P>(s_F); }
template <class P, int S> inline Vec<3,P>  
operator*(const InverseTransform_<P>& X_BF, const Vec<3,negator<P>,S>& s_F)  
{   return X_BF*Vec<3,P>(s_F); }

template <class P, int S> inline Transform_<P>
operator+(const Transform_<P>& X_BF, const Vec<3,P,S>& offset_B)
{   return Transform_<P>(X_BF) += offset_B; }
template <class P, int S> inline Transform_<P>
operator+(const Vec<3,P,S>& offset_B, const Transform_<P>& X_BF)
{   return Transform_<P>(X_BF) += offset_B; }

template <class P, int S> inline Transform_<P>
operator-(const Transform_<P>& X_BF, const Vec<3,P,S>& offset_B)
{   return Transform_<P>(X_BF) -= offset_B; }

//-----------------------------------------------------------------------------
template <class P, int S> inline Vec<4,P> 
operator*(const Transform_<P>& X_BF, const Vec<4,P,S>& a_F) {
    assert(a_F[3]==0 || a_F[3]==1);
    const Vec<3,P,S>& v_F = Vec<3,P,S>::getAs(&a_F[0]); // recast the 1st 3 elements as Vec3

    Vec<4,P> out;
    if( a_F[3] == 0 ) { Vec<3,P>::updAs(&out[0]) = X_BF.xformFrameVecToBase(v_F);      out[3] = 0; } 
    else              { Vec<3,P>::updAs(&out[0]) = X_BF.shiftFrameStationToBase(v_F);  out[3] = 1; }
    return out;
}

template <class P, int S> inline Vec<4,P> 
operator*(const InverseTransform_<P>& X_BF, const Vec<4,P,S>& a_F ) {
    assert(a_F[3]==0 || a_F[3]==1);
    const Vec<3,P,S>& v_F = Vec<3,P,S>::getAs(&a_F[0]); // recast the 1st 3 elements as Vec3

    Vec<4,P> out;
    if( a_F[3] == 0 ) { Vec<3,P>::updAs(&out[0]) = X_BF.xformFrameVecToBase(v_F);      out[3] = 0; } 
    else              { Vec<3,P>::updAs(&out[0]) = X_BF.shiftFrameStationToBase(v_F);  out[3] = 1; }
    return out;
}
template <class P, int S> inline Vec<4,P>  
operator*(const Transform_<P>& X_BF,        const Vec<4,negator<P>,S>& s_F)  {return X_BF*Vec<4,P>(s_F);}
template <class P, int S> inline Vec<4,P>  
operator*(const InverseTransform_<P>& X_BF, const Vec<4,negator<P>,S>& s_F)  {return X_BF*Vec<4,P>(s_F);}
//-----------------------------------------------------------------------------

template <class P, class E> inline Vector_<E> 
operator*(const Transform_<P>& X, const VectorBase<E>& v) {
    Vector_<E> result(v.size());
    for (int i = 0; i < v.size(); ++i)
        result[i] = X*v[i];
    return result;
}
template <class P, class E> inline Vector_<E> 
operator*(const VectorBase<E>& v, const Transform_<P>& X) {
    Vector_<E> result(v.size());
    for (int i = 0; i < v.size(); ++i)
        result[i] = X*v[i];
    return result;
}
template <class P, class E> inline RowVector_<E> 
operator*(const Transform_<P>& X, const RowVectorBase<E>& v) {
    RowVector_<E> result(v.size());
    for (int i = 0; i < v.size(); ++i)
        result[i] = X*v[i];
    return result;
}
template <class P, class E> inline RowVector_<E> 
operator*(const RowVectorBase<E>& v, const Transform_<P>& X) {
    RowVector_<E> result(v.size());
    for (int i = 0; i < v.size(); ++i)
        result[i] = X*v[i];
    return result;
}
template <class P, class E> inline Matrix_<E> 
operator*(const Transform_<P>& X, 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) = X*v(i, j);
    return result;
}
template <class P, class E> inline Matrix_<E> 
operator*(const MatrixBase<E>& v, const Transform_<P>& X) {
    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) = X*v(i, j);
    return result;
}
template <class P, int N, class E, int S> inline Vec<N,E> 
operator*(const Transform_<P>& X, const Vec<N,E,S>& v) {
    Vec<N,E> result;
    for (int i = 0; i < N; ++i)
        result[i] = X*v[i];
    return result;
}
template <class P, int N, class E, int S> inline Vec<N,E> 
operator*(const Vec<N,E,S>& v, const Transform_<P>& X) {
    Vec<N,E> result;
    for (int i = 0; i < N; ++i)
        result[i] = X*v[i];
    return result;
}
template <class P, int N, class E, int S> inline Row<N,E> 
operator*(const Transform_<P>& X, const Row<N,E,S>& v) {
    Row<N,E> result;
    for (int i = 0; i < N; ++i)
        result[i] = X*v[i];
    return result;
}
template <class P, int N, class E, int S> inline Row<N,E> 
operator*(const Row<N,E,S>& v, const Transform_<P>& X) {
    Row<N,E> result;
    for (int i = 0; i < N; ++i)
        result[i] = X*v[i];
    return result;
}
template <class P, int M, int N, class E, int CS, int RS> inline Mat<M,N,E> 
operator*(const Transform_<P>& X, const Mat<M,N,E,CS,RS>& v) {
    Mat<M,N,E> result;
    for (int i = 0; i < M; ++i)
        for (int j = 0; j < N; ++j)
            result(i, j) = X*v(i, j);
    return result;
}
template <class P, int M, int N, class E, int CS, int RS> inline Mat<M,N,E> 
operator*(const Mat<M,N,E,CS,RS>& v, const Transform_<P>& X) {
    Mat<M,N,E> result;
    for (int i = 0; i < M; ++i)
        for (int j = 0; j < N; ++j)
            result(i, j) = X*v(i, j);
    return result;
}

// These Transform definitions had to wait for InverseTransform to be declared.

template <class P> inline Transform_<P>&
Transform_<P>::operator=( const InverseTransform_<P>& 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.
    p_BF = X.p(); // This might change X.p ...
    R_BF = X.R(); // ... but this doesn't depend on X.p.
    return *this;
}

template <class P> inline Transform_<P>
Transform_<P>::compose( const InverseTransform_<P>& X_FY ) const {
    return Transform_<P>( R_BF * X_FY.R(), p_BF + R_BF * X_FY.p() );
}

template <class P> inline Transform_<P>
operator*(const Transform_<P>& X1,        const Transform_<P>& X2)         {return X1.compose(X2);}
template <class P> inline Transform_<P>
operator*(const Transform_<P>& X1,        const InverseTransform_<P>& X2)  {return X1.compose(X2);}
template <class P> inline Transform_<P>
operator*(const InverseTransform_<P>& X1, const Transform_<P>& X2)         {return X1.compose(X2);}
template <class P> inline Transform_<P>
operator*(const InverseTransform_<P>& X1, const InverseTransform_<P>& X2)  {return X1.compose(X2);}

template <class P> inline bool
operator==(const Transform_<P>& X1,        const Transform_<P>& X2)         {return X1.R()==X2.R() && X1.p()==X2.p();}
template <class P> inline bool
operator==(const InverseTransform_<P>& X1, const InverseTransform_<P>& X2)  {return X1.R()==X2.R() && X1.p()==X2.p();}
template <class P> inline bool
operator==(const Transform_<P>& X1,        const InverseTransform_<P>& X2)  {return X1.R()==X2.R() && X1.p()==X2.p();}
template <class P> inline bool
operator==(const InverseTransform_<P>& X1, const Transform_<P>& X2)         {return X1.R()==X2.R() && X1.p()==X2.p();}

template <class P> SimTK_SimTKCOMMON_EXPORT std::ostream&
operator<<(std::ostream&, const Transform_<P>&);
template <class P> SimTK_SimTKCOMMON_EXPORT std::ostream&
operator<<(std::ostream&, const InverseTransform_<P>&);



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

//--------------------------------------------------------------------------
#endif // SimTK_SimTKCOMMON_TRANSFORM_H
//--------------------------------------------------------------------------