MassProperties.h

Go to the documentation of this file.

#ifndef SimTK_SIMMATRIX_MASS_PROPERTIES_H_
#define SimTK_SIMMATRIX_MASS_PROPERTIES_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-14 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/Scalar.h"
#include "SimTKcommon/SmallMatrix.h"
#include "SimTKcommon/Orientation.h"

#include <iostream>

namespace SimTK {
typedef Vec<2,   Vec3>              SpatialVec;
typedef Vec<2,   Vec<3,float> >    fSpatialVec;
typedef Vec<2,   Vec<3,double> >   dSpatialVec;

typedef Row<2,   Row3>              SpatialRow;
typedef Row<2,   Row<3,float> >    fSpatialRow;
typedef Row<2,   Row<3,double> >   dSpatialRow;

typedef Mat<2,2, Mat33>             SpatialMat;
typedef Mat<2,2, Mat<3,3,float> >  fSpatialMat;
typedef Mat<2,2, Mat<3,3,double> > dSpatialMat;

// These are templatized by precision (float or double).
template <class P> class UnitInertia_;
template <class P> class Inertia_;
template <class P> class SpatialInertia_;
template <class P> class ArticulatedInertia_;
template <class P> class MassProperties_;

// The "no trailing underscore" typedefs use whatever the 
// compile-time precision is set to.

typedef UnitInertia_<Real>          UnitInertia;
typedef UnitInertia_<float>        fUnitInertia;
typedef UnitInertia_<double>       dUnitInertia;

typedef Inertia_<Real>              Inertia;
typedef Inertia_<float>            fInertia;
typedef Inertia_<double>           dInertia;

typedef MassProperties_<Real>       MassProperties;
typedef MassProperties_<float>     fMassProperties;
typedef MassProperties_<double>    dMassProperties;

typedef SpatialInertia_<Real>       SpatialInertia;
typedef SpatialInertia_<float>     fSpatialInertia;
typedef SpatialInertia_<double>    dSpatialInertia;

typedef ArticulatedInertia_<Real>    ArticulatedInertia;
typedef ArticulatedInertia_<float>  fArticulatedInertia;
typedef ArticulatedInertia_<double> dArticulatedInertia;

typedef UnitInertia  Gyration;

// -----------------------------------------------------------------------------
//                             INERTIA MATRIX
// -----------------------------------------------------------------------------
template <class P>
class SimTK_SimTKCOMMON_EXPORT Inertia_ {
    typedef P               RealP;
    typedef Vec<3,P>        Vec3P;
    typedef SymMat<3,P>     SymMat33P;
    typedef Mat<3,3,P>      Mat33P;
    typedef Rotation_<P>    RotationP;
public:
Inertia_() : I_OF_F(NTraits<P>::getNaN()) {}

// Default copy constructor, copy assignment, destructor.

explicit Inertia_(const RealP& moment) : I_OF_F(moment) 
{   errChk("Inertia::Inertia(moment)"); }

Inertia_(const Vec3P& p, const RealP& mass) : I_OF_F(pointMassAt(p,mass)) {}

explicit Inertia_(const Vec3P& moments, const Vec3P& products=Vec3P(0)) 
{   I_OF_F.updDiag()  = moments;
    I_OF_F.updLower() = products;
    errChk("Inertia::Inertia(moments,products)"); }

Inertia_(const RealP& xx, const RealP& yy, const RealP& zz) 
{   I_OF_F = SymMat33P(xx,
                        0, yy,
                        0,  0, zz);
    errChk("Inertia::setInertia(xx,yy,zz)"); }

Inertia_(const RealP& xx, const RealP& yy, const RealP& zz,
            const RealP& xy, const RealP& xz, const RealP& yz) 
{   I_OF_F = SymMat33P(xx,
                        xy, yy,
                        xz, yz, zz);
    errChk("Inertia::setInertia(xx,yy,zz,xy,xz,yz)"); }

explicit Inertia_(const SymMat33P& I) : I_OF_F(I) 
{   errChk("Inertia::Inertia(SymMat33)"); }

explicit Inertia_(const Mat33P& m)
{   SimTK_ERRCHK(m.isNumericallySymmetric(), 
                    "Inertia(Mat33)", "The supplied matrix was not symmetric.");
    I_OF_F = SymMat33P(m);
    errChk("Inertia(Mat33)"); }


Inertia_& setInertia(const RealP& xx, const RealP& yy, const RealP& zz) {
    I_OF_F = RealP(0); I_OF_F(0,0) = xx; I_OF_F(1,1) = yy;  I_OF_F(2,2) = zz;
    errChk("Inertia::setInertia(xx,yy,zz)");
    return *this;
}

Inertia_& setInertia(const Vec3P& moments, const Vec3P& products=Vec3P(0)) {
    I_OF_F.updDiag()  = moments;
    I_OF_F.updLower() = products;
    errChk("Inertia::setInertia(moments,products)");
    return *this;
}

Inertia_& setInertia(const RealP& xx, const RealP& yy, const RealP& zz,
                        const RealP& xy, const RealP& xz, const RealP& yz) {
    setInertia(Vec3P(xx,yy,zz), Vec3P(xy,xz,yz));
    errChk("Inertia::setInertia(xx,yy,zz,xy,xz,yz)");
    return *this;
}


Inertia_& operator+=(const Inertia_& I) 
{   I_OF_F += I.I_OF_F; 
    errChk("Inertia::operator+=()");
    return *this; }

Inertia_& operator-=(const Inertia_& I) 
{   I_OF_F -= I.I_OF_F; 
    errChk("Inertia::operator-=()");
    return *this; }

Inertia_& operator*=(const P& s) {I_OF_F *= s; return *this;}

Inertia_& operator/=(const P& s) {I_OF_F /= s; return *this;}

Inertia_ shiftToMassCenter(const Vec3P& CF, const RealP& mass) const 
{   Inertia_ I(*this); I -= pointMassAt(CF, mass);
    I.errChk("Inertia::shiftToMassCenter()");
    return I; }

Inertia_& shiftToMassCenterInPlace(const Vec3P& CF, const RealP& mass) 
{   (*this) -= pointMassAt(CF, mass);
    errChk("Inertia::shiftToMassCenterInPlace()");
    return *this; }

Inertia_ shiftFromMassCenter(const Vec3P& p, const RealP& mass) const
{   Inertia_ I(*this); I += pointMassAt(p, mass);
    I.errChk("Inertia::shiftFromMassCenter()");
    return I; }

Inertia_& shiftFromMassCenterInPlace(const Vec3P& p, const RealP& mass)
{   (*this) += pointMassAt(p, mass);
    errChk("Inertia::shiftFromMassCenterInPlace()");
    return *this; }

Inertia_ reexpress(const Rotation_<P>& R_FB) const 
{   return Inertia_((~R_FB).reexpressSymMat33(I_OF_F)); }

Inertia_ reexpress(const InverseRotation_<P>& R_FB) const 
{   return Inertia_((~R_FB).reexpressSymMat33(I_OF_F)); }

Inertia_& reexpressInPlace(const Rotation_<P>& R_FB)
{   I_OF_F = (~R_FB).reexpressSymMat33(I_OF_F); return *this; }

Inertia_& reexpressInPlace(const InverseRotation_<P>& R_FB)
{   I_OF_F = (~R_FB).reexpressSymMat33(I_OF_F); return *this; }

RealP trace() const {return I_OF_F.trace();}

operator const SymMat33P&() const {return I_OF_F;}

const SymMat33P& asSymMat33() const {return I_OF_F;}

Mat33P toMat33() const {return Mat33P(I_OF_F);}

const Vec3P& getMoments()  const {return I_OF_F.getDiag();}
const Vec3P& getProducts() const {return I_OF_F.getLower();}

bool isNaN()    const {return I_OF_F.isNaN();}
bool isInf()    const {return I_OF_F.isInf();}
bool isFinite() const {return I_OF_F.isFinite();}

bool isNumericallyEqual(const Inertia_<P>& other) const 
{   return I_OF_F.isNumericallyEqual(other.I_OF_F); }

bool isNumericallyEqual(const Inertia_<P>& other, double tol) const 
{   return I_OF_F.isNumericallyEqual(other.I_OF_F, tol); }

static bool isValidInertiaMatrix(const SymMat33P& m) {
    if (m.isNaN()) return false;

    const Vec3P& d = m.diag();
    if (!(d >= 0)) return false;    // diagonals must be nonnegative

    const RealP Slop = std::max(d.sum(),RealP(1))
                       * NTraits<P>::getSignificant();

    if (!(d[0]+d[1]+Slop>=d[2] && d[0]+d[2]+Slop>=d[1] && d[1]+d[2]+Slop>=d[0]))
        return false;               // must satisfy triangle inequality

    // Thanks to Paul Mitiguy for this condition on products of inertia.
    const Vec3P& p = m.getLower();
    if (!( d[0]+Slop>=std::abs(2*p[2])
        && d[1]+Slop>=std::abs(2*p[1])
        && d[2]+Slop>=std::abs(2*p[0])))
        return false;               // max products are limited by moments

    return true;
}

static Inertia_ pointMassAtOrigin() {return Inertia_(0);}

static Inertia_ pointMassAt(const Vec3P& p, const RealP& m) {
    const Vec3P mp = m*p;       // 3 flops
    const RealP mxx = mp[0]*p[0];
    const RealP myy = mp[1]*p[1];
    const RealP mzz = mp[2]*p[2];
    const RealP nmx = -mp[0];
    const RealP nmy = -mp[1];
    return Inertia_( myy+mzz,  mxx+mzz,  mxx+myy,
                        nmx*p[1], nmx*p[2], nmy*p[2] );
}



inline static Inertia_ sphere(const RealP& r);

inline static Inertia_ cylinderAlongZ(const RealP& r, const RealP& hz);

inline static Inertia_ cylinderAlongY(const RealP& r, const RealP& hy);

inline static Inertia_ cylinderAlongX(const RealP& r, const RealP& hx);

inline static Inertia_ brick(const RealP& hx, const RealP& hy, const RealP& hz);

inline static Inertia_ brick(const Vec3P& halfLengths);

inline static Inertia_ ellipsoid(const RealP& hx, const RealP& hy, const RealP& hz);

inline static Inertia_ ellipsoid(const Vec3P& halfLengths);


protected:
// Reinterpret this Inertia matrix as a UnitInertia matrix, that is, as the
// inertia of something with unit mass. This is useful in implementing
// methods of the UnitInertia class in terms of Inertia methods. Be sure you
// know that this is a unit-mass inertia!
const UnitInertia_<P>& getAsUnitInertia() const
{   return *static_cast<const UnitInertia_<P>*>(this); }
UnitInertia_<P>& updAsUnitInertia()
{   return *static_cast<UnitInertia_<P>*>(this); }

// If error checking is enabled (only in Debug mode), this 
// method will run some tests on the current contents of this Inertia 
// matrix and throw an error message if it is not valid. This should be 
// the same set of tests as run by the isValidInertiaMatrix() method above.
void errChk(const char* methodName) const {
#ifndef NDEBUG
    SimTK_ERRCHK(!isNaN(), methodName,
        "Inertia matrix contains a NaN.");

    const Vec3P& d = I_OF_F.getDiag();  // moments
    const Vec3P& p = I_OF_F.getLower(); // products
    const RealP Ixx = d[0], Iyy = d[1], Izz = d[2];
    const RealP Ixy = p[0], Ixz = p[1], Iyz = p[2];

    SimTK_ERRCHK3(d >= -SignificantReal, methodName,
        "Diagonals of an Inertia matrix must be nonnegative; got %g,%g,%g.",
        (double)Ixx,(double)Iyy,(double)Izz);

    // TODO: This is looser than it should be as a workaround for distorted
    // rotation matrices that were produced by an 11,000 body chain that
    // Sam Flores encountered. 
    const RealP Slop = std::max(d.sum(),RealP(1))
                       * std::sqrt(NTraits<P>::getEps());

    SimTK_ERRCHK3(   Ixx+Iyy+Slop>=Izz 
                  && Ixx+Izz+Slop>=Iyy 
                  && Iyy+Izz+Slop>=Ixx,
        methodName,
        "Diagonals of an Inertia matrix must satisfy the triangle "
        "inequality; got %g,%g,%g.",
        (double)Ixx,(double)Iyy,(double)Izz);

    // Thanks to Paul Mitiguy for this condition on products of inertia.
    SimTK_ERRCHK(   Ixx+Slop>=std::abs(2*Iyz) 
                 && Iyy+Slop>=std::abs(2*Ixz)
                 && Izz+Slop>=std::abs(2*Ixy),
        methodName,
        "The magnitude of a product of inertia was too large to be physical.");
#endif
}

// Inertia expressed in frame F and about F's origin OF. Note that frame F
// is implicit here; all we actually have are the inertia scalars.
SymMat33P I_OF_F; 
};

template <class P> inline Inertia_<P>
operator+(const Inertia_<P>& l, const Inertia_<P>& r) 
{   return Inertia_<P>(l) += r; }

template <class P> inline Inertia_<P>
operator-(const Inertia_<P>& l, const Inertia_<P>& r) 
{   return Inertia_<P>(l) -= r; }

template <class P> inline Inertia_<P>
operator*(const Inertia_<P>& i, const P& r) 
{   return Inertia_<P>(i) *= r; }

template <class P> inline Inertia_<P>
operator*(const P& r, const Inertia_<P>& i) 
{   return Inertia_<P>(i) *= r; }


template <class P> inline Inertia_<P>
operator*(const Inertia_<P>& i, int r) 
{   return Inertia_<P>(i) *= P(r); }

template <class P> inline Inertia_<P>
operator*(int r, const Inertia_<P>& i) 
{   return Inertia_<P>(i) *= P(r); }

template <class P> inline Inertia_<P>
operator/(const Inertia_<P>& i, const P& r) 
{   return Inertia_<P>(i) /= r; }

template <class P> inline Inertia_<P>
operator/(const Inertia_<P>& i, int r) 
{   return Inertia_<P>(i) /= P(r); }

template <class P> inline Vec<3,P>
operator*(const Inertia_<P>& I, const Vec<3,P>& w) 
{   return I.asSymMat33() * w; }

template <class P> inline bool
operator==(const Inertia_<P>& i1, const Inertia_<P>& i2) 
{   return i1.asSymMat33() == i2.asSymMat33(); }

template <class P> inline std::ostream& 
operator<<(std::ostream& o, const Inertia_<P>& inertia)
{   return o << inertia.toMat33(); }


// -----------------------------------------------------------------------------
//                            UNIT INERTIA MATRIX
// -----------------------------------------------------------------------------
template <class P>
class SimTK_SimTKCOMMON_EXPORT UnitInertia_ : public Inertia_<P> {
    typedef P               RealP;
    typedef Vec<3,P>        Vec3P;
    typedef SymMat<3,P>     SymMat33P;
    typedef Mat<3,3,P>      Mat33P;
    typedef Rotation_<P>    RotationP;
    typedef Inertia_<P>     InertiaP;
public:
UnitInertia_() {}

// Default copy constructor, copy assignment, destructor.

explicit UnitInertia_(const RealP& moment) : InertiaP(moment) {}

explicit UnitInertia_(const Vec3P& moments, const Vec3P& products=Vec3P(0))
:   InertiaP(moments,products) {}

UnitInertia_(const RealP& xx, const RealP& yy, const RealP& zz)
:   InertiaP(xx,yy,zz) {}   

UnitInertia_(const RealP& xx, const RealP& yy, const RealP& zz,
            const RealP& xy, const RealP& xz, const RealP& yz)
:   InertiaP(xx,yy,zz,xy,xz,yz) {}

explicit UnitInertia_(const SymMat33P& m) : InertiaP(m) {}

explicit UnitInertia_(const Mat33P& m) : InertiaP(m) {}

explicit UnitInertia_(const Inertia_<P>& I) : InertiaP(I) {}

UnitInertia_& setUnitInertia(const RealP& xx, const RealP& yy, const RealP& zz) 
{   InertiaP::setInertia(xx,yy,zz); return *this; }

UnitInertia_& setUnitInertia(const Vec3P& moments, const Vec3P& products=Vec3P(0)) 
{   InertiaP::setInertia(moments,products); return *this; }

UnitInertia_& setUnitInertia(const RealP& xx, const RealP& yy, const RealP& zz,
                        const RealP& xy, const RealP& xz, const RealP& yz) 
{   InertiaP::setInertia(xx,yy,zz,xy,xz,yz); return *this; }


// No +=, -=, etc. operators because those don't result in a UnitInertia 
// matrix. The parent class ones are suppressed below.

UnitInertia_ shiftToCentroid(const Vec3P& CF) const 
{   UnitInertia_ G(*this); 
    G.Inertia_<P>::operator-=(pointMassAt(CF));
    return G; }

UnitInertia_& shiftToCentroidInPlace(const Vec3P& CF) 
{   InertiaP::operator-=(pointMassAt(CF));
    return *this; }

UnitInertia_ shiftFromCentroid(const Vec3P& p) const
{   UnitInertia_ G(*this); 
    G.Inertia_<P>::operator+=(pointMassAt(p));
    return G; }

UnitInertia_& shiftFromCentroidInPlace(const Vec3P& p)
{   InertiaP::operator+=(pointMassAt(p));
    return *this; }

UnitInertia_ reexpress(const Rotation_<P>& R_FB) const 
{   return UnitInertia_((~R_FB).reexpressSymMat33(this->I_OF_F)); }

UnitInertia_ reexpress(const InverseRotation_<P>& R_FB) const 
{   return UnitInertia_((~R_FB).reexpressSymMat33(this->I_OF_F)); }

UnitInertia_& reexpressInPlace(const Rotation_<P>& R_FB)
{   InertiaP::reexpressInPlace(R_FB); return *this; }

UnitInertia_& reexpressInPlace(const InverseRotation_<P>& R_FB)
{   InertiaP::reexpressInPlace(R_FB); return *this; }


operator const SymMat33P&() const {return this->I_OF_F;}

const Inertia_<P>& asUnitInertia() const 
{   return *static_cast<const Inertia_<P>*>(this); }

UnitInertia_& setFromUnitInertia(const Inertia_<P>& I)
{   Inertia_<P>::operator=(I);
    return *this; }

static bool isValidUnitInertiaMatrix(const SymMat33P& m) 
{   return Inertia_<P>::isValidInertiaMatrix(m); }



static UnitInertia_ pointMassAtOrigin() {return UnitInertia_(0);}

static UnitInertia_ pointMassAt(const Vec3P& p) 
{   return UnitInertia_(crossMatSq(p)); }

static UnitInertia_ sphere(const RealP& r) {return UnitInertia_(RealP(0.4)*r*r);}

static UnitInertia_ cylinderAlongZ(const RealP& r, const RealP& hz) {
    const RealP Ixx = (r*r)/4 + (hz*hz)/3;
    return UnitInertia_(Ixx,Ixx,(r*r)/2);
}

static UnitInertia_ cylinderAlongY(const RealP& r, const RealP& hy) {
    const RealP Ixx = (r*r)/4 + (hy*hy)/3;
    return UnitInertia_(Ixx,(r*r)/2,Ixx);
}

static UnitInertia_ cylinderAlongX(const RealP& r, const RealP& hx) {
    const RealP Iyy = (r*r)/4 + (hx*hx)/3;
    return UnitInertia_((r*r)/2,Iyy,Iyy);
}

static UnitInertia_ brick(const RealP& hx, const RealP& hy, const RealP& hz) {
    const RealP oo3 = RealP(1)/RealP(3);
    const RealP hx2=hx*hx, hy2=hy*hy, hz2=hz*hz;
    return UnitInertia_(oo3*(hy2+hz2), oo3*(hx2+hz2), oo3*(hx2+hy2));
}

static UnitInertia_ brick(const Vec3P& halfLengths)
{   return brick(halfLengths[0],halfLengths[1],halfLengths[2]); }

static UnitInertia_ ellipsoid(const RealP& hx, const RealP& hy, const RealP& hz) {
    const RealP hx2=hx*hx, hy2=hy*hy, hz2=hz*hz;
    return UnitInertia_((hy2+hz2)/5, (hx2+hz2)/5, (hx2+hy2)/5);
}

static UnitInertia_ ellipsoid(const Vec3P& halfLengths)
{   return ellipsoid(halfLengths[0],halfLengths[1],halfLengths[2]); }

private:
// Suppress Inertia_ methods which are not allowed for UnitInertia_.

// These kill all flavors of Inertia_::setInertia() and the
// Inertia_ assignment ops.
void setInertia() {}
void operator+=(int) {}
void operator-=(int) {}
void operator*=(int) {}
void operator/=(int) {}
};

// Implement Inertia methods which are pass-throughs to UnitInertia methods.

template <class P> inline Inertia_<P> Inertia_<P>::
sphere(const RealP& r) 
{   return UnitInertia_<P>::sphere(r); }
template <class P> inline Inertia_<P> Inertia_<P>::
cylinderAlongZ(const RealP& r, const RealP& hz)
{   return UnitInertia_<P>::cylinderAlongZ(r,hz); }
template <class P> inline Inertia_<P> Inertia_<P>::
cylinderAlongY(const RealP& r, const RealP& hy)
{   return UnitInertia_<P>::cylinderAlongY(r,hy); }
template <class P> inline Inertia_<P> Inertia_<P>::
cylinderAlongX(const RealP& r, const RealP& hx)
{   return UnitInertia_<P>::cylinderAlongX(r,hx); }
template <class P> inline Inertia_<P> Inertia_<P>::
brick(const RealP& hx, const RealP& hy, const RealP& hz)
{   return UnitInertia_<P>::brick(hx,hy,hz); }
template <class P> inline Inertia_<P> Inertia_<P>::
brick(const Vec3P& halfLengths)
{   return UnitInertia_<P>::brick(halfLengths); }
template <class P> inline Inertia_<P> Inertia_<P>::
ellipsoid(const RealP& hx, const RealP& hy, const RealP& hz)
{   return UnitInertia_<P>::ellipsoid(hx,hy,hz); }
template <class P> inline Inertia_<P> Inertia_<P>::
ellipsoid(const Vec3P& halfLengths)
{   return UnitInertia_<P>::ellipsoid(halfLengths); }


// -----------------------------------------------------------------------------
//                           SPATIAL INERTIA MATRIX
// -----------------------------------------------------------------------------
template <class P> 
class SimTK_SimTKCOMMON_EXPORT SpatialInertia_ {
    typedef P               RealP;
    typedef Vec<3,P>        Vec3P;
    typedef UnitInertia_<P> UnitInertiaP;
    typedef Mat<3,3,P>      Mat33P;
    typedef Vec<2, Vec3P>   SpatialVecP;
    typedef Mat<2,2,Mat33P> SpatialMatP;
    typedef Rotation_<P>    RotationP;
    typedef Transform_<P>   TransformP;
    typedef Inertia_<P>     InertiaP;
public:
SpatialInertia_() 
:   m(nanP()), p(nanP()) {} // inertia is already NaN
SpatialInertia_(RealP mass, const Vec3P& com, const UnitInertiaP& gyration) 
:   m(mass), p(com), G(gyration) {}

// default copy constructor, copy assignment, destructor

SpatialInertia_& setMass(RealP mass)
{   SimTK_ERRCHK1(mass >= 0, "SpatialInertia::setMass()",
        "Negative mass %g is illegal.", (double)mass);
    m=mass; return *this; }
SpatialInertia_& setMassCenter(const Vec3P& com)
{   p=com; return *this;} 
SpatialInertia_& setUnitInertia(const UnitInertiaP& gyration) 
{   G=gyration; return *this; }

RealP               getMass()        const {return m;}
const Vec3P&        getMassCenter()  const {return p;}
const UnitInertiaP& getUnitInertia() const {return G;}

Vec3P calcMassMoment() const {return m*p;}

InertiaP calcInertia() const {return m*G;}

SpatialInertia_& operator+=(const SpatialInertia_& src) {
    SimTK_ERRCHK(m+src.m != 0, "SpatialInertia::operator+=()",
        "The combined mass cannot be zero.");
    const RealP mtot = m+src.m, oomtot = 1/mtot;                    // ~11 flops
    p = oomtot*(calcMassMoment() + src.calcMassMoment());           // 10 flops
    G.setFromUnitInertia(oomtot*(calcInertia()+src.calcInertia())); // 19 flops
    m = mtot; // must do this last
    return *this;
}

SpatialInertia_& operator-=(const SpatialInertia_& src) {
    SimTK_ERRCHK(m != src.m, "SpatialInertia::operator-=()",
        "The combined mass cannot be zero.");
    const RealP mtot = m-src.m, oomtot = 1/mtot;                    // ~11 flops
    p = oomtot*(calcMassMoment() - src.calcMassMoment());           // 10 flops
    G.setFromUnitInertia(oomtot*(calcInertia()-src.calcInertia())); // 19 flops
    m = mtot; // must do this last
    return *this;
}

SpatialInertia_& operator*=(const RealP& s) {m *= s; return *this;}

SpatialInertia_& operator/=(const RealP& s) {m /= s; return *this;}

SpatialVecP operator*(const SpatialVecP& v) const
{   return m*SpatialVecP(G*v[0]+p%v[1], v[1]-p%v[0]); }

SpatialInertia_ reexpress(const Rotation_<P>& R_FB) const
{   return SpatialInertia_(*this).reexpressInPlace(R_FB); }

SpatialInertia_ reexpress(const InverseRotation_<P>& R_FB) const
{   return SpatialInertia_(*this).reexpressInPlace(R_FB); }

SpatialInertia_& reexpressInPlace(const Rotation_<P>& R_FB)
{   p = (~R_FB)*p; G.reexpressInPlace(R_FB); return *this; }

SpatialInertia_& reexpressInPlace(const InverseRotation_<P>& R_FB)
{   p = (~R_FB)*p; G.reexpressInPlace(R_FB); return *this; }

SpatialInertia_ shift(const Vec3P& S) const 
{   return SpatialInertia_(*this).shiftInPlace(S); }

SpatialInertia_& shiftInPlace(const Vec3P& S) {
    G.shiftToCentroidInPlace(p);    // change to central inertia
    G.shiftFromCentroidInPlace(S);  // now inertia is about S
    p -= S; // was p=com-OF, now want p'=com-(OF+S)=p-S
    return *this;
}

SpatialInertia_ transform(const Transform_<P>& X_FB) const 
{   return SpatialInertia_(*this).transformInPlace(X_FB); }

SpatialInertia_ transform(const InverseTransform_<P>& X_FB) const 
{   return SpatialInertia_(*this).transformInPlace(X_FB); }

SpatialInertia_& transformInPlace(const Transform_<P>& X_FB) {
    shiftInPlace(X_FB.p());     // shift to the new origin OB.
    reexpressInPlace(X_FB.R()); // get everything in B
    return *this;
}

SpatialInertia_& transformInPlace(const InverseTransform_<P>& X_FB) {
    shiftInPlace(X_FB.p());     // shift to the new origin OB.
    reexpressInPlace(X_FB.R()); // get everything in B
    return *this;
}

const SpatialMatP toSpatialMat() const {
    Mat33P offDiag = crossMat(m*p);
    return SpatialMatP(m*G.toMat33(), offDiag,
                       -offDiag,      Mat33P(m));
}

private:
RealP           m;  // mass of this rigid body F
Vec3P           p;  // location of body's COM from OF, expressed in F
UnitInertiaP    G;  // mass distribution; inertia is mass*gyration

static P nanP() {return NTraits<P>::getNaN();} 
};

template <class P> inline SpatialInertia_<P> 
operator+(const SpatialInertia_<P>& l, const SpatialInertia_<P>& r)
{   return SpatialInertia_<P>(l) += r; } 

template <class P> inline SpatialInertia_<P> 
operator-(const SpatialInertia_<P>& l, const SpatialInertia_<P>& r)
{   return SpatialInertia_<P>(l) -= r; } 


// -----------------------------------------------------------------------------
//                        ARTICULATED BODY INERTIA MATRIX
// -----------------------------------------------------------------------------
template <class P> 
class ArticulatedInertia_ {
    typedef P               RealP;
    typedef Vec<3,P>        Vec3P;
    typedef UnitInertia_<P> UnitInertiaP;
    typedef Mat<3,3,P>      Mat33P;
    typedef SymMat<3,P>     SymMat33P;
    typedef Vec<2, Vec3P>   SpatialVecP;
    typedef Mat<2,2,Mat33P> SpatialMatP;
    typedef Rotation_<P>    RotationP;
    typedef Transform_<P>   TransformP;
    typedef Inertia_<P>     InertiaP;
public:
ArticulatedInertia_() {}
ArticulatedInertia_(const SymMat33P& mass, const Mat33P& massMoment, const SymMat33P& inertia)
:   M(mass), J(inertia), F(massMoment) {}

explicit ArticulatedInertia_(const SpatialInertia_<P>& rbi)
:   M(rbi.getMass()), J(rbi.calcInertia()), F(crossMat(rbi.calcMassMoment())) {}

ArticulatedInertia_& setMass      (const SymMat33P& mass)       {M=mass;       return *this;}
ArticulatedInertia_& setMassMoment(const Mat33P&    massMoment) {F=massMoment; return *this;}
ArticulatedInertia_& setInertia   (const SymMat33P& inertia)    {J=inertia;    return *this;}

const SymMat33P& getMass()       const {return M;}
const Mat33P&    getMassMoment() const {return F;}
const SymMat33P& getInertia()    const {return J;}

// default destructor, copy constructor, copy assignment

ArticulatedInertia_& operator+=(const ArticulatedInertia_& src)
{   M+=src.M; J+=src.J; F+=src.F; return *this; }
ArticulatedInertia_& operator-=(const ArticulatedInertia_& src)
{   M-=src.M; J-=src.J; F-=src.F; return *this; }

SpatialVecP operator*(const SpatialVecP& v) const
{   return SpatialVecP(J*v[0]+F*v[1], ~F*v[0]+M*v[1]); }

template <int N>
Mat<2,N,Vec3P> operator*(const Mat<2,N,Vec3P>& m) const {
    Mat<2,N,Vec3P> res;
    for (int j=0; j < N; ++j)
        res.col(j) = (*this) * m.col(j); // above this*SpatialVec operator
    return res;
}

SimTK_SimTKCOMMON_EXPORT ArticulatedInertia_ shift(const Vec3P& s) const;

SimTK_SimTKCOMMON_EXPORT ArticulatedInertia_& shiftInPlace(const Vec3P& s);

const SpatialMatP toSpatialMat() const {
    return SpatialMatP( Mat33P(J),     F,
                            ~F,       Mat33P(M) );
}
private:
SymMat33P M;
SymMat33P J;
Mat33P    F;
};

template <class P> inline ArticulatedInertia_<P>
operator+(const ArticulatedInertia_<P>& l, const ArticulatedInertia_<P>& r)
{   return ArticulatedInertia_<P>(l) += r; }

template <class P> inline ArticulatedInertia_<P>
operator-(const ArticulatedInertia_<P>& l, const ArticulatedInertia_<P>& r)
{   return ArticulatedInertia_<P>(l) -= r; }


// -----------------------------------------------------------------------------
//                              MASS PROPERTIES
// -----------------------------------------------------------------------------
template <class P>
class SimTK_SimTKCOMMON_EXPORT MassProperties_ {
    typedef P               RealP;
    typedef Vec<3,P>        Vec3P;
    typedef UnitInertia_<P> UnitInertiaP;
    typedef Mat<3,3,P>      Mat33P;
    typedef Mat<6,6,P>      Mat66P;
    typedef SymMat<3,P>     SymMat33P;
    typedef Mat<2,2,Mat33P> SpatialMatP;
    typedef Rotation_<P>    RotationP;
    typedef Transform_<P>   TransformP;
    typedef Inertia_<P>     InertiaP;
public:
MassProperties_() { setMassProperties(0,Vec3P(0),UnitInertiaP()); }
MassProperties_(const RealP& m, const Vec3P& com, const InertiaP& inertia)
    { setMassProperties(m,com,inertia); }
MassProperties_(const RealP& m, const Vec3P& com, const UnitInertiaP& gyration)
    { setMassProperties(m,com,gyration); }

MassProperties_& setMassProperties(const RealP& m, const Vec3P& com, const InertiaP& inertia) {
    mass = m;
    comInB = com;
    if (m == 0) {
        SimTK_ASSERT(inertia.asSymMat33().getAsVec() == 0, "Mass is zero but inertia is nonzero");
        unitInertia_OB_B = UnitInertiaP(0);
    }
    else {
        unitInertia_OB_B = UnitInertiaP(inertia*(1/m));
    }
    return *this;
}

MassProperties_& setMassProperties
   (const RealP& m, const Vec3P& com, const UnitInertiaP& gyration)
{   mass=m; comInB=com; unitInertia_OB_B=gyration; return *this; }

const RealP& getMass() const {return mass;}
const Vec3P& getMassCenter() const {return comInB;}
const UnitInertiaP& getUnitInertia() const {return unitInertia_OB_B;}
const InertiaP calcInertia() const {return mass*unitInertia_OB_B;}
const InertiaP getInertia() const {return calcInertia();}

InertiaP calcCentralInertia() const {
    return mass*unitInertia_OB_B - InertiaP(comInB, mass);
}
InertiaP calcShiftedInertia(const Vec3P& newOriginB) const {
    return calcCentralInertia() + InertiaP(newOriginB-comInB, mass);
}
InertiaP calcTransformedInertia(const TransformP& X_BC) const {
    return calcShiftedInertia(X_BC.p()).reexpress(X_BC.R());
}
MassProperties_ calcShiftedMassProps(const Vec3P& newOriginB) const {
    return MassProperties_(mass, comInB-newOriginB,
                            calcShiftedInertia(newOriginB));
}

MassProperties_ calcTransformedMassProps(const TransformP& X_BC) const {
    return MassProperties_(mass, ~X_BC*comInB, calcTransformedInertia(X_BC));
}

MassProperties_ reexpress(const RotationP& R_BC) const {
    return MassProperties_(mass, ~R_BC*comInB, unitInertia_OB_B.reexpress(R_BC));
}

bool isExactlyMassless()   const { return mass==0; }
bool isNearlyMassless(const RealP& tol=SignificantReal) const { 
    return mass <= tol; 
}

bool isExactlyCentral() const { return comInB==Vec3P(0); }
bool isNearlyCentral(const RealP& tol=SignificantReal) const {
    return comInB.normSqr() <= tol*tol;
}

bool isNaN() const {return SimTK::isNaN(mass) || comInB.isNaN() || unitInertia_OB_B.isNaN();}
bool isInf() const {
    if (isNaN()) return false;
    return SimTK::isInf(mass) || comInB.isInf() || unitInertia_OB_B.isInf();
}
bool isFinite() const {
    return SimTK::isFinite(mass) && comInB.isFinite() && unitInertia_OB_B.isFinite();
}

SpatialMatP toSpatialMat() const {
    SpatialMatP M;
    M(0,0) = mass*unitInertia_OB_B.toMat33();
    M(0,1) = mass*crossMat(comInB);
    M(1,0) = ~M(0,1);
    M(1,1) = mass; // a diagonal matrix
    return M;
}

Mat66P toMat66() const {
    Mat66P M;
    M.template updSubMat<3,3>(0,0) = mass*unitInertia_OB_B.toMat33();
    M.template updSubMat<3,3>(0,3) = mass*crossMat(comInB);
    M.template updSubMat<3,3>(3,0) = ~M.template getSubMat<3,3>(0,3);
    M.template updSubMat<3,3>(3,3) = mass; // a diagonal matrix
    return M;
}

private:
RealP        mass;
Vec3P        comInB;         // meas. from B origin, expr. in B
UnitInertiaP unitInertia_OB_B;   // about B origin, expr. in B
};

template <class P> static inline std::ostream& 
operator<<(std::ostream& o, const MassProperties_<P>& mp) {
    return o << "{ mass=" << mp.getMass() 
             << "\n  com=" << mp.getMassCenter()
             << "\n  Uxx,yy,zz=" << mp.getUnitInertia().getMoments()
             << "\n  Uxy,xz,yz=" << mp.getUnitInertia().getProducts()
             << "\n}\n";
}

} // namespace SimTK

#endif // SimTK_SIMMATRIX_MASS_PROPERTIES_H_