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-7 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 {
typedef Mat<2,2, Mat33> SpatialMat;

// 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 Row<2,   Row3>  SpatialRow;
typedef Mat<2,2, Mat33> SpatialMat;

class Inertia {
public:
    Inertia() : I_OF_F(NaN) {}

    explicit Inertia(const Real& r) : I_OF_F(r) { }

    explicit Inertia(const Vec3& moments, const Vec3& products=Vec3(0)) {
        setInertia(moments,products);
    }
    Inertia(const Real& xx, const Real& yy, const Real& zz) {
        setInertia(Vec3(xx,yy,zz)); 
    }
    Inertia(const Real& xx, const Real& yy, const Real& zz,
            const Real& xy, const Real& xz, const Real& yz) {
        setInertia(xx,yy,zz,xy,xz,yz); 
    }

    Inertia(const Vec3& p, const Real& m) {
        Mat33& t = I_OF_F;
        const Real& x = p[0]; const Real mx=m*x, mxx=mx*x;
        const Real& y = p[1]; const Real my=m*y, myy=my*y;
        const Real& z = p[2]; const Real mz=m*z, mzz=mz*z;

        t(0,0)          =  myy + mzz;
        t(1,1)          =  mxx + mzz;
        t(2,2)          =  mxx + myy;
        t(0,1) = t(1,0) = -mx*y;
        t(0,2) = t(2,0) = -mx*z;
        t(1,2) = t(2,1) = -my*z;
    }

    Inertia(const Inertia& src) {
        Mat33&       t = I_OF_F;
        const Mat33& s = src.I_OF_F;
        t(0,0) = s(0,0); t(1,1) = s(1,1);  t(2,2) = s(2,2);
        t(0,1) = (t(1,0) = s(1,0));
        t(0,2) = (t(2,0) = s(2,0));
        t(1,2) = (t(2,1) = s(2,1));
    }

    explicit Inertia(const Mat33& s) {
        assert(close(s(0,1),s(1,0),s.diag().norm()) 
            && close(s(0,2),s(2,0),s.diag().norm())
            && close(s(1,2),s(2,1),s.diag().norm()));
        setInertia(s(0,0),s(1,1),s(2,2),
            0.5*(s(1,0)+s(0,1)),0.5*(s(2,0)+s(0,2)),0.5*(s(2,1)+s(1,2)));
    }

    Inertia& operator=(const Inertia& src) {
        Mat33&       t = I_OF_F;
        const Mat33& s = src.I_OF_F;
        t(0,0) = s(0,0); t(1,1) = s(1,1);  t(2,2) = s(2,2);
        t(0,1) = (t(1,0) = s(1,0));
        t(0,2) = (t(2,0) = s(2,0));
        t(1,2) = (t(2,1) = s(2,1));
        return *this;
    }

    Inertia& operator+=(const Inertia& src) {
        Mat33&       t = I_OF_F;
        const Mat33& s = src.I_OF_F;
        t(0,0) += s(0,0); t(1,1) += s(1,1);  t(2,2) += s(2,2);
        t(0,1) = (t(1,0) += s(1,0));
        t(0,2) = (t(2,0) += s(2,0));
        t(1,2) = (t(2,1) += s(2,1));
        return *this;
    }

    Inertia& operator-=(const Inertia& src) {
        Mat33&       t = I_OF_F;
        const Mat33& s = src.I_OF_F;
        t(0,0) -= s(0,0); t(1,1) -= s(1,1);  t(2,2) -= s(2,2);
        t(0,1) = (t(1,0) -= s(1,0));
        t(0,2) = (t(2,0) -= s(2,0));
        t(1,2) = (t(2,1) -= s(2,1));
        return *this;
    }

    Inertia& operator*=(const Real& r) {
        I_OF_F *= r;
        return *this;
    }
    Inertia& operator/=(const Real& r) {
        I_OF_F /= r;
        return *this;
    }
    Inertia& setInertia(const Real& xx, const Real& yy, const Real& zz) {
        I_OF_F = 0.; I_OF_F(0,0) = xx; I_OF_F(1,1) = yy;  I_OF_F(2,2) = zz;
        return *this;
    }

    Inertia& setInertia(const Vec3& moments, const Vec3& products=Vec3(0)) {
        Mat33& t = I_OF_F;
        t.diag() = moments;
        t(0,1) = t(1,0) = products[0];
        t(0,2) = t(2,0) = products[1];
        t(1,2) = t(2,1) = products[2];
        return *this;
    }

    Inertia& setInertia(const Real& xx, const Real& yy, const Real& zz,
                    const Real& xy, const Real& xz, const Real& yz) {
        Mat33& t = I_OF_F;
        t(0,0) = xx; t(1,1) = yy;  t(2,2) = zz;
        t(0,1) = t(1,0) = xy;
        t(0,2) = t(2,0) = xz;
        t(1,2) = t(2,1) = yz;
        return *this;
    }

    inline Inertia shiftToMassCenter(const Vec3& CF, const Real& mtot) const;

    inline Inertia shiftFromMassCenter(const Vec3& p, const Real& mtot) const;

    Inertia reexpress(const Rotation& R_FB) const {
        return Inertia(~R_FB * I_OF_F * R_FB); // TODO can do better due to symmetry
    }

    Real trace() const {return I_OF_F(0,0) + I_OF_F(1,1) + I_OF_F(2,2);}

    // Note that we are copying into the Mat33 here so this will still
    // work if we decide to switch to a SymMat33 when they exist.
    Mat33 toMat33() const {
        return I_OF_F;
    }
    Vec3 getMoments()  const {return I_OF_F.diag();}
    Vec3 getProducts() const {return Vec3(I_OF_F(1,0), I_OF_F(2,0), I_OF_F(2,1));}

    // Inertia factory for some common mass elements. Each defines its own
    // frame aligned (when possible) with principal moments. Each has unit
    // mass and its center of mass located at the origin. Use this with shiftFromCOM()
    // to move it somewhere else, and with xform() to express the inertia
    // in another frame. Mass enters linearly in inertia, so just multiply
    // by the actual mass to scale these properly.
    static Inertia pointMass() {return Inertia(0.);}

    // This is an exception -- the mass center will be at p.
    static Inertia pointMassAt(const Vec3& p) {
        const Real& x = p[0]; const Real xx=x*x;
        const Real& y = p[1]; const Real yy=y*y;
        const Real& z = p[2]; const Real zz=z*z;
        return Inertia(yy + zz, xx + zz, xx + yy,
                       -x*y, -x*z, -y*z);
    }

    static Inertia sphere(const Real& r) {return Inertia(0.4*r*r);}

    // Cylinder is aligned along z axis, use radius and half-length.
    // If r==0 this is a thin rod; hz=0 it is a thin disk.
    static Inertia cylinderAlongZ(const Real& r, const Real& hz) {
        const Real Ixx = 0.25*r*r + (Real(1)/Real(3))*hz*hz;
        return Inertia(Ixx,Ixx,0.5*r*r);
    }

    // Cylinder is aligned along y axis, use radius and half-length.
    // If r==0 this is a thin rod; hy=0 it is a thin disk.
    static Inertia cylinderAlongY(const Real& r, const Real& hy) {
        const Real Ixx = 0.25*r*r + (Real(1)/Real(3))*hy*hy;
        return Inertia(Ixx,0.5*r*r,Ixx);
    }

    // Cylinder is aligned along x axis, use radius and half-length.
    // If r==0 this is a thin rod; hx=0 it is a thin disk.
    static Inertia cylinderAlongX(const Real& r, const Real& hx) {
        const Real Iyy = 0.25*r*r + (Real(1)/Real(3))*hx*hx;
        return Inertia(0.5*r*r,Iyy,Iyy);
    }

    // Brick given by half-lengths in each direction. One dimension zero
    // gives inertia of a thin rectangular sheet; two zero gives inertia
    // of a thin rod in the remaining direction.
    static Inertia brick(const Real& hx, const Real& hy, const Real& hz) {
        const Real oo3 = Real(1)/Real(3);
        const Real hx2=hx*hx, hy2=hy*hy, hz2=hz*hz;
        return Inertia(oo3*(hy2+hz2), oo3*(hx2+hz2), oo3*(hx2+hy2));
    }

    // Ellipsoid given by half-lengths in each direction.
    static Inertia ellipsoid(const Real& hx, const Real& hy, const Real& hz) {
        const Real hx2=hx*hx, hy2=hy*hy, hz2=hz*hz;
        return Inertia(0.2*(hy2+hz2), 0.2*(hx2+hz2), 0.2*(hx2+hy2));
    }


private:
    // Check whether a and b are the same except for numerical error which
    // is a reasonable fraction of the overall scale, which is passed in.
    static bool close(const Real& a, const Real& b, const Real& scale) {
        const Real okErr = SignificantReal*std::abs(scale);
        const Real err = std::abs(a-b);
        return err <= okErr;
    }

private:
    // 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. This is 
    // a symmetric matrix but we keep all the elements here, and manage them
    // so that the reflected elements are *exactly* equal.
    // TODO: should use a SymMat33 type.
    Mat33 I_OF_F;
    friend Vec3 operator*(const Inertia& i, const Vec3& w);
};

inline Inertia operator+(const Inertia& l, const Inertia& r) {
    return Inertia(l) += r;
}
inline Inertia operator-(const Inertia& l, const Inertia& r) {
    return Inertia(l) -= r;
}
inline Inertia operator*(const Inertia& i, const Real& r) {
    return Inertia(i) *= r;
}
inline Inertia operator*(const Real& r, const Inertia& i) {
    return Inertia(i) *= r;
}
inline Vec3 operator*(const Inertia& i, const Vec3& w) {
    return i.I_OF_F * w;
}
inline Inertia operator/(const Inertia& i, const Real& r) {
    return Inertia(i) /= r;
}
inline Inertia Inertia::shiftToMassCenter(const Vec3& CF, const Real& mtot) const {
    return *this - Inertia(CF, mtot);
}
inline Inertia Inertia::shiftFromMassCenter(const Vec3& p, const Real& mtot) const {
    return *this + Inertia(p, mtot);
}

inline bool
operator==(const Inertia& i1, const Inertia& i2) {
    return i1.toMat33() == i2.toMat33();    // TODO should use underlying rep
}

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


class 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.T()).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;
    }

    SpatialMat toSpatialMat() const {
        SpatialMat M;
        M(0,0) = inertia_OB_B.toMat33();
        M(0,1) = 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) = 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_