MassProperties.h

Go to the documentation of this file.

#ifndef SimTK_SIMMATRIX_MASS_PROPERTIES_H_
#define SimTK_SIMMATRIX_MASS_PROPERTIES_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-9 Stanford University and the Authors.         *
 * Authors: Michael Sherman                                                   *
 * Contributors:                                                              *
 *                                                                            *
 * 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/Scalar.h"
#include "SimTKcommon/SmallMatrix.h"
#include "SimTKcommon/Orientation.h"

#include <iostream>

namespace SimTK {
// Spatial vectors are used for (orientation,translation) quantities.
// These include
//      spatial velocity     = (angularVelocity,linearVelocity)
//      spatial acceleration = (angularAcceleration,linearAcceleration)
//      generalized forces   = (torque,force)
// Spatial configuration has to be handled differently though since
// orientation is not a vector quantity. (We use "Transform" for this concept
// which includes an orientation matrix and a translation vector.)
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 Gyration_;
template <class P> class Inertia_;
template <class P> class SpatialInertia_;
template <class P> class ArticulatedInertia_;

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

typedef Gyration_<Real>              Gyration;
typedef Gyration_<float>            fGyration;
typedef Gyration_<double>           dGyration;

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

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

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


// -----------------------------------------------------------------------------
//                             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) {
        const RealP Slop = NTraits<P>::getSignificant();
        if (m.isNaN()) return false;
        const Vec3P& d = m.diag();
        if (!(d >= 0)) return false; // diagonals must be nonnegative
        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
        //TODO: what else?
        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 Gyration matrix, that is, as the
    // inertia of something with unit mass. This is useful in implementing
    // methods of the Gyration class in terms of Inertia methods. Be sure you
    // know that this is a unit-mass inertia!
    const Gyration_<P>& getAsGyration() const
    {   return *reinterpret_cast<const Gyration_<P>*>(this); }
    Gyration_<P>& updAsGyration()
    {   return *reinterpret_cast<Gyration_<P>*>(this); }

    // If error checking is enabled (typically 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 {
        const RealP Slop = NTraits<P>::getSignificant();
        SimTK_ERRCHK(!isNaN(), methodName,
            "Inertia matrix contains a NaN.");
        const Vec3P& d = I_OF_F.diag();
        SimTK_ERRCHK3(d >= 0, methodName,
            "Diagonals of an Inertia matrix must be nonnegative; got %g,%g,%g.",
            (double)d[0],(double)d[1],(double)d[2]);
        SimTK_ERRCHK3(d[0]+d[1]+Slop>=d[2] && d[0]+d[2]+Slop>=d[1] && d[1]+d[2]+Slop>=d[0],
            methodName,
            "Diagonals of an Inertia matrix must satisfy the triangle "
            "inequality; got %g,%g,%g.",
            (double)d[0],(double)d[1],(double)d[2]);
    }

    // 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(); }


// -----------------------------------------------------------------------------
//                             GYRATION MATRIX
// -----------------------------------------------------------------------------

template <class P>
class SimTK_SimTKCOMMON_EXPORT Gyration_ : 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:
    Gyration_() {}

    // Default copy constructor, copy assignment, destructor.

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

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

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

    Gyration_(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 Gyration_(const SymMat33P& m) : InertiaP(m) {}

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

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

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

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

    Gyration_& setGyration(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 Gyration matrix.
    // The parent class ones are suppressed below.

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

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

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

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

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

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

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

    Gyration_& 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); }

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

    static bool isValidGyrationMatrix(const SymMat33P& m) 
    {   return isValidInertiaMatrix(m); }



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

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

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

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

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

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

    static Gyration_ 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 Gyration_(oo3*(hy2+hz2), oo3*(hx2+hz2), oo3*(hx2+hy2));
    }

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

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

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

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

    // 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 Gyration methods.

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


// -----------------------------------------------------------------------------
//                           SPATIAL INERTIA MATRIX
// -----------------------------------------------------------------------------

template <class P> 
class SimTK_SimTKCOMMON_EXPORT SpatialInertia_ {
    typedef P               RealP;
    typedef Vec<3,P>        Vec3P;
    typedef Gyration_<P>    GyrationP;
    typedef Mat<3,3,P>      Mat33P;
    typedef Rotation_<P>    RotationP;  // TODO: need template argument
    typedef Transform_<P>   TransformP; //   "
    typedef Inertia_<P>     InertiaP;   //   "
public:
    SpatialInertia_() 
    :   m(nanP()), p(nanP()) {} // gyration is already NaN
    SpatialInertia_(RealP mass, const Vec3P& com, const GyrationP& 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_& setGyration(const GyrationP& gyration) 
    {   G=gyration; return *this; }

    RealP            getMass()       const {return m;}
    const Vec3P&     getMassCenter() const {return p;}
    const GyrationP& getGyration()   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;}

    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 gyration
        G.shiftFromCentroidInPlace(S);  // now gyration 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;
    }

private:
    RealP       m;  
    Vec3P       p;  
    GyrationP   G;  

    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 Gyration_<P>    GyrationP;
    typedef Mat<3,3,P>      Mat33P;
    typedef SymMat<3,P>     SymMat33P;
    typedef Mat<2,2,Mat33P> SpatialMatP;
    typedef Rotation_<P>    RotationP;  // TODO: need template argument
    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; }

    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;
};


// -----------------------------------------------------------------------------
//                              MASS PROPERTIES
// -----------------------------------------------------------------------------
   
class SimTK_SimTKCOMMON_EXPORT MassProperties {
public:
    MassProperties() { setMassProperties(0.,Vec3(0.),Inertia()); }
    MassProperties(const Real& m, const Vec3& com, const Inertia& inertia)
      { setMassProperties(m,com,inertia); }

    MassProperties& setMassProperties(const Real& m, const Vec3& com, const Inertia& inertia)
      { mass=m; comInB=com; inertia_OB_B=inertia; return *this; }

    const Real&    getMass()       const { return mass; }
    const Vec3&    getMassCenter() const { return comInB; }
    const Inertia& getInertia()    const { return inertia_OB_B; }

    Inertia calcCentralInertia() const {
        return inertia_OB_B - Inertia(comInB, mass);
    }
    Inertia calcShiftedInertia(const Vec3& newOriginB) const {
        return calcCentralInertia() + Inertia(newOriginB-comInB, mass);
    }
    Inertia calcTransformedInertia(const Transform& X_BC) const {
        return calcShiftedInertia(X_BC.p()).reexpress(X_BC.R());
    }
    MassProperties calcShiftedMassProps(const Vec3& newOriginB) const {
        return MassProperties(mass, comInB-newOriginB,
                              calcShiftedInertia(newOriginB));
    }
    // Transform a body's mass properties from the (implicit) B frame
    // to a new frame C.
    MassProperties calcTransformedMassProps(const Transform& X_BC) const {
        return MassProperties(mass, ~X_BC*comInB, calcTransformedInertia(X_BC));
    }

    // Re-express these mass properties in frame C. Currently the mass properties
    // are expressed in the (implicit) frame B, so we need the Rotation matrix
    // that takes us from B to C.
    MassProperties reexpress(const Rotation& R_BC) const {
        return MassProperties(mass, ~R_BC*comInB, inertia_OB_B.reexpress(R_BC));
    }

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

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

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

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

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

private:
    Real     mass;
    Vec3     comInB;         // meas. from B origin, expr. in B
    Inertia  inertia_OB_B;   // about B origin, expr. in B
};

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

} // namespace SimTK

#endif // SimTK_SIMMATRIX_MASS_PROPERTIES_H_