MassProperties.h

Go to the documentation of this file.
00001 #ifndef SimTK_SIMMATRIX_MASS_PROPERTIES_H_
00002 #define SimTK_SIMMATRIX_MASS_PROPERTIES_H_
00003 
00004 /* -------------------------------------------------------------------------- *
00005  *                      SimTK Core: SimTK Simmatrix(tm)                       *
00006  * -------------------------------------------------------------------------- *
00007  * This is part of the SimTK Core biosimulation toolkit originating from      *
00008  * Simbios, the NIH National Center for Physics-Based Simulation of           *
00009  * Biological Structures at Stanford, funded under the NIH Roadmap for        *
00010  * Medical Research, grant U54 GM072970. See https://simtk.org.               *
00011  *                                                                            *
00012  * Portions copyright (c) 2005-7 Stanford University and the Authors.         *
00013  * Authors: Michael Sherman                                                   *
00014  * Contributors:                                                              *
00015  *                                                                            *
00016  * Permission is hereby granted, free of charge, to any person obtaining a    *
00017  * copy of this software and associated documentation files (the "Software"), *
00018  * to deal in the Software without restriction, including without limitation  *
00019  * the rights to use, copy, modify, merge, publish, distribute, sublicense,   *
00020  * and/or sell copies of the Software, and to permit persons to whom the      *
00021  * Software is furnished to do so, subject to the following conditions:       *
00022  *                                                                            *
00023  * The above copyright notice and this permission notice shall be included in *
00024  * all copies or substantial portions of the Software.                        *
00025  *                                                                            *
00026  * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR *
00027  * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,   *
00028  * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL    *
00029  * THE AUTHORS, CONTRIBUTORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM,    *
00030  * DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR      *
00031  * OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE  *
00032  * USE OR OTHER DEALINGS IN THE SOFTWARE.                                     *
00033  * -------------------------------------------------------------------------- */
00034 
00041 #include "SimTKcommon/Scalar.h"
00042 #include "SimTKcommon/SmallMatrix.h"
00043 #include "SimTKcommon/Orientation.h"
00044 
00045 #include <iostream>
00046 
00047 namespace SimTK {
00048 typedef Mat<2,2, Mat33> SpatialMat;
00049 
00050 // Spatial vectors are used for (orientation,translation) quantities.
00051 // These include
00052 //      spatial velocity     = (angularVelocity,linearVelocity)
00053 //      spatial acceleration = (angularAcceleration,linearAcceleration)
00054 //      generalized forces   = (torque,force)
00055 // Spatial configuration has to be handled differently though since
00056 // orientation is not a vector quantity. (We use "Transform" for this concept
00057 // which includes an orientation matrix and a translation vector.)
00058 typedef Vec<2,   Vec3>  SpatialVec;
00059 typedef Row<2,   Row3>  SpatialRow;
00060 typedef Mat<2,2, Mat33> SpatialMat;
00061 
00111 class Inertia {
00112 public:
00114     Inertia() : I_OF_F(NaN) {}
00115 
00121     explicit Inertia(const Real& r) : I_OF_F(r) { }
00122 
00127     explicit Inertia(const Vec3& moments, const Vec3& products=Vec3(0)) {
00128         setInertia(moments,products);
00129     }
00131     Inertia(const Real& xx, const Real& yy, const Real& zz) {
00132         setInertia(Vec3(xx,yy,zz)); 
00133     }
00136     Inertia(const Real& xx, const Real& yy, const Real& zz,
00137             const Real& xy, const Real& xz, const Real& yz) {
00138         setInertia(xx,yy,zz,xy,xz,yz); 
00139     }
00140 
00148     Inertia(const Vec3& p, const Real& m) {
00149         Mat33& t = I_OF_F;
00150         const Real& x = p[0]; const Real mx=m*x, mxx=mx*x;
00151         const Real& y = p[1]; const Real my=m*y, myy=my*y;
00152         const Real& z = p[2]; const Real mz=m*z, mzz=mz*z;
00153 
00154         t(0,0)          =  myy + mzz;
00155         t(1,1)          =  mxx + mzz;
00156         t(2,2)          =  mxx + myy;
00157         t(0,1) = t(1,0) = -mx*y;
00158         t(0,2) = t(2,0) = -mx*z;
00159         t(1,2) = t(2,1) = -my*z;
00160     }
00161 
00163     Inertia(const Inertia& src) {
00164         Mat33&       t = I_OF_F;
00165         const Mat33& s = src.I_OF_F;
00166         t(0,0) = s(0,0); t(1,1) = s(1,1);  t(2,2) = s(2,2);
00167         t(0,1) = (t(1,0) = s(1,0));
00168         t(0,2) = (t(2,0) = s(2,0));
00169         t(1,2) = (t(2,1) = s(2,1));
00170     }
00171 
00175     explicit Inertia(const Mat33& s) {
00176         assert(close(s(0,1),s(1,0),s.diag().norm()) 
00177             && close(s(0,2),s(2,0),s.diag().norm())
00178             && close(s(1,2),s(2,1),s.diag().norm()));
00179         setInertia(s(0,0),s(1,1),s(2,2),
00180             0.5*(s(1,0)+s(0,1)),0.5*(s(2,0)+s(0,2)),0.5*(s(2,1)+s(1,2)));
00181     }
00182 
00184     Inertia& operator=(const Inertia& src) {
00185         Mat33&       t = I_OF_F;
00186         const Mat33& s = src.I_OF_F;
00187         t(0,0) = s(0,0); t(1,1) = s(1,1);  t(2,2) = s(2,2);
00188         t(0,1) = (t(1,0) = s(1,0));
00189         t(0,2) = (t(2,0) = s(2,0));
00190         t(1,2) = (t(2,1) = s(2,1));
00191         return *this;
00192     }
00193 
00196     Inertia& operator+=(const Inertia& src) {
00197         Mat33&       t = I_OF_F;
00198         const Mat33& s = src.I_OF_F;
00199         t(0,0) += s(0,0); t(1,1) += s(1,1);  t(2,2) += s(2,2);
00200         t(0,1) = (t(1,0) += s(1,0));
00201         t(0,2) = (t(2,0) += s(2,0));
00202         t(1,2) = (t(2,1) += s(2,1));
00203         return *this;
00204     }
00205 
00208     Inertia& operator-=(const Inertia& src) {
00209         Mat33&       t = I_OF_F;
00210         const Mat33& s = src.I_OF_F;
00211         t(0,0) -= s(0,0); t(1,1) -= s(1,1);  t(2,2) -= s(2,2);
00212         t(0,1) = (t(1,0) -= s(1,0));
00213         t(0,2) = (t(2,0) -= s(2,0));
00214         t(1,2) = (t(2,1) -= s(2,1));
00215         return *this;
00216     }
00217 
00219     Inertia& operator*=(const Real& r) {
00220         I_OF_F *= r;
00221         return *this;
00222     }
00224     Inertia& operator/=(const Real& r) {
00225         I_OF_F /= r;
00226         return *this;
00227     }
00230     Inertia& setInertia(const Real& xx, const Real& yy, const Real& zz) {
00231         I_OF_F = 0.; I_OF_F(0,0) = xx; I_OF_F(1,1) = yy;  I_OF_F(2,2) = zz;
00232         return *this;
00233     }
00234 
00237     Inertia& setInertia(const Vec3& moments, const Vec3& products=Vec3(0)) {
00238         Mat33& t = I_OF_F;
00239         t.diag() = moments;
00240         t(0,1) = t(1,0) = products[0];
00241         t(0,2) = t(2,0) = products[1];
00242         t(1,2) = t(2,1) = products[2];
00243         return *this;
00244     }
00245 
00249     Inertia& setInertia(const Real& xx, const Real& yy, const Real& zz,
00250                     const Real& xy, const Real& xz, const Real& yz) {
00251         Mat33& t = I_OF_F;
00252         t(0,0) = xx; t(1,1) = yy;  t(2,2) = zz;
00253         t(0,1) = t(1,0) = xy;
00254         t(0,2) = t(2,0) = xz;
00255         t(1,2) = t(2,1) = yz;
00256         return *this;
00257     }
00258 
00266     inline Inertia shiftToMassCenter(const Vec3& CF, const Real& mtot) const;
00267 
00273     inline Inertia shiftFromMassCenter(const Vec3& p, const Real& mtot) const;
00274 
00278     Inertia reexpress(const Rotation& R_FB) const {
00279         return Inertia(~R_FB * I_OF_F * R_FB); // TODO can do better due to symmetry
00280     }
00281 
00282     Real trace() const {return I_OF_F(0,0) + I_OF_F(1,1) + I_OF_F(2,2);}
00283 
00284     // Note that we are copying into the Mat33 here so this will still
00285     // work if we decide to switch to a SymMat33 when they exist.
00286     Mat33 toMat33() const {
00287         return I_OF_F;
00288     }
00289     Vec3 getMoments()  const {return I_OF_F.diag();}
00290     Vec3 getProducts() const {return Vec3(I_OF_F(1,0), I_OF_F(2,0), I_OF_F(2,1));}
00291 
00292     // Inertia factory for some common mass elements. Each defines its own
00293     // frame aligned (when possible) with principal moments. Each has unit
00294     // mass and its center of mass located at the origin. Use this with shiftFromCOM()
00295     // to move it somewhere else, and with xform() to express the inertia
00296     // in another frame. Mass enters linearly in inertia, so just multiply
00297     // by the actual mass to scale these properly.
00298     static Inertia pointMass() {return Inertia(0.);}
00299 
00300     // This is an exception -- the mass center will be at p.
00301     static Inertia pointMassAt(const Vec3& p) {
00302         const Real& x = p[0]; const Real xx=x*x;
00303         const Real& y = p[1]; const Real yy=y*y;
00304         const Real& z = p[2]; const Real zz=z*z;
00305         return Inertia(yy + zz, xx + zz, xx + yy,
00306                        -x*y, -x*z, -y*z);
00307     }
00308 
00309     static Inertia sphere(const Real& r) {return Inertia(0.4*r*r);}
00310 
00311     // Cylinder is aligned along z axis, use radius and half-length.
00312     // If r==0 this is a thin rod; hz=0 it is a thin disk.
00313     static Inertia cylinderAlongZ(const Real& r, const Real& hz) {
00314         const Real Ixx = 0.25*r*r + (Real(1)/Real(3))*hz*hz;
00315         return Inertia(Ixx,Ixx,0.5*r*r);
00316     }
00317 
00318     // Cylinder is aligned along y axis, use radius and half-length.
00319     // If r==0 this is a thin rod; hy=0 it is a thin disk.
00320     static Inertia cylinderAlongY(const Real& r, const Real& hy) {
00321         const Real Ixx = 0.25*r*r + (Real(1)/Real(3))*hy*hy;
00322         return Inertia(Ixx,0.5*r*r,Ixx);
00323     }
00324 
00325     // Cylinder is aligned along x axis, use radius and half-length.
00326     // If r==0 this is a thin rod; hx=0 it is a thin disk.
00327     static Inertia cylinderAlongX(const Real& r, const Real& hx) {
00328         const Real Iyy = 0.25*r*r + (Real(1)/Real(3))*hx*hx;
00329         return Inertia(0.5*r*r,Iyy,Iyy);
00330     }
00331 
00332     // Brick given by half-lengths in each direction. One dimension zero
00333     // gives inertia of a thin rectangular sheet; two zero gives inertia
00334     // of a thin rod in the remaining direction.
00335     static Inertia brick(const Real& hx, const Real& hy, const Real& hz) {
00336         const Real oo3 = Real(1)/Real(3);
00337         const Real hx2=hx*hx, hy2=hy*hy, hz2=hz*hz;
00338         return Inertia(oo3*(hy2+hz2), oo3*(hx2+hz2), oo3*(hx2+hy2));
00339     }
00340 
00341     // Ellipsoid given by half-lengths in each direction.
00342     static Inertia ellipsoid(const Real& hx, const Real& hy, const Real& hz) {
00343         const Real hx2=hx*hx, hy2=hy*hy, hz2=hz*hz;
00344         return Inertia(0.2*(hy2+hz2), 0.2*(hx2+hz2), 0.2*(hx2+hy2));
00345     }
00346 
00347 
00348 private:
00349     // Check whether a and b are the same except for numerical error which
00350     // is a reasonable fraction of the overall scale, which is passed in.
00351     static bool close(const Real& a, const Real& b, const Real& scale) {
00352         const Real okErr = SignificantReal*std::abs(scale);
00353         const Real err = std::abs(a-b);
00354         return err <= okErr;
00355     }
00356 
00357 private:
00358     // Inertia expressed in frame F and about F's origin OF. Note that frame F
00359     // is implicit here; all we actually have are the inertia scalars. This is 
00360     // a symmetric matrix but we keep all the elements here, and manage them
00361     // so that the reflected elements are *exactly* equal.
00362     // TODO: should use a SymMat33 type.
00363     Mat33 I_OF_F;
00364     friend Vec3 operator*(const Inertia& i, const Vec3& w);
00365 };
00366 
00367 inline Inertia operator+(const Inertia& l, const Inertia& r) {
00368     return Inertia(l) += r;
00369 }
00370 inline Inertia operator-(const Inertia& l, const Inertia& r) {
00371     return Inertia(l) -= r;
00372 }
00373 inline Inertia operator*(const Inertia& i, const Real& r) {
00374     return Inertia(i) *= r;
00375 }
00376 inline Inertia operator*(const Real& r, const Inertia& i) {
00377     return Inertia(i) *= r;
00378 }
00379 inline Vec3 operator*(const Inertia& i, const Vec3& w) {
00380     return i.I_OF_F * w;
00381 }
00382 inline Inertia operator/(const Inertia& i, const Real& r) {
00383     return Inertia(i) /= r;
00384 }
00385 inline Inertia Inertia::shiftToMassCenter(const Vec3& CF, const Real& mtot) const {
00386     return *this - Inertia(CF, mtot);
00387 }
00388 inline Inertia Inertia::shiftFromMassCenter(const Vec3& p, const Real& mtot) const {
00389     return *this + Inertia(p, mtot);
00390 }
00391 
00392 inline bool
00393 operator==(const Inertia& i1, const Inertia& i2) {
00394     return i1.toMat33() == i2.toMat33();    // TODO should use underlying rep
00395 }
00396 
00397 SimTK_SimTKCOMMON_EXPORT std::ostream& 
00398 operator<<(std::ostream& o, const Inertia&);
00399 
00400 
00406 class MassProperties {
00407 public:
00408     MassProperties() { setMassProperties(0.,Vec3(0.),Inertia()); }
00409     MassProperties(const Real& m, const Vec3& com, const Inertia& inertia)
00410       { setMassProperties(m,com,inertia); }
00411 
00414     MassProperties& setMassProperties(const Real& m, const Vec3& com, const Inertia& inertia)
00415       { mass=m; comInB=com; inertia_OB_B=inertia; return *this; }
00416 
00417     const Real&    getMass()       const { return mass; }
00418     const Vec3&    getMassCenter() const { return comInB; }
00419     const Inertia& getInertia()    const { return inertia_OB_B; }
00420 
00421     Inertia calcCentralInertia() const {
00422         return inertia_OB_B - Inertia(comInB, mass);
00423     }
00424     Inertia calcShiftedInertia(const Vec3& newOriginB) const {
00425         return calcCentralInertia() + Inertia(newOriginB-comInB, mass);
00426     }
00427     Inertia calcTransformedInertia(const Transform& X_BC) const {
00428         return calcShiftedInertia(X_BC.T()).reexpress(X_BC.R());
00429     }
00430     MassProperties calcShiftedMassProps(const Vec3& newOriginB) const {
00431         return MassProperties(mass, comInB-newOriginB,
00432                               calcShiftedInertia(newOriginB));
00433     }
00434     // Transform a body's mass properties from the (implicit) B frame
00435     // to a new frame C.
00436     MassProperties calcTransformedMassProps(const Transform& X_BC) const {
00437         return MassProperties(mass, ~X_BC*comInB, calcTransformedInertia(X_BC));
00438     }
00439 
00440     // Re-express these mass properties in frame C. Currently the mass properties
00441     // are expressed in the (implicit) frame B, so we need the Rotation matrix
00442     // that takes us from B to C.
00443     MassProperties reexpress(const Rotation& R_BC) const {
00444         return MassProperties(mass, ~R_BC*comInB, inertia_OB_B.reexpress(R_BC));
00445     }
00446 
00447     bool isExactlyMassless()   const { return mass==0.; }
00448     bool isNearlyMassless(const Real& tol=SignificantReal) const { 
00449         return mass <= tol; 
00450     }
00451 
00452     bool isExactlyCentral() const { return comInB==Vec3(0); }
00453     bool isNearlyCentral(const Real& tol=SignificantReal) const {
00454         return comInB.normSqr() <= tol*tol;
00455     }
00456 
00457     SpatialMat toSpatialMat() const {
00458         SpatialMat M;
00459         M(0,0) = inertia_OB_B.toMat33();
00460         M(0,1) = crossMat(comInB);
00461         M(1,0) = ~M(0,1);
00462         M(1,1) = mass; // a diagonal matrix
00463         return M;
00464     }
00465 
00468     Mat66 toMat66() const {
00469         Mat66 M;
00470         M.updSubMat<3,3>(0,0) = inertia_OB_B.toMat33();
00471         M.updSubMat<3,3>(0,3) = crossMat(comInB);
00472         M.updSubMat<3,3>(3,0) = ~M.getSubMat<3,3>(0,3);
00473         M.updSubMat<3,3>(3,3) = mass; // a diagonal matrix
00474         return M;
00475     }
00476 
00477 private:
00478     Real     mass;
00479     Vec3     comInB;         // meas. from B origin, expr. in B
00480     Inertia  inertia_OB_B;   // about B origin, expr. in B
00481 };
00482 
00483 SimTK_SimTKCOMMON_EXPORT std::ostream& 
00484 operator<<(std::ostream& o, const MassProperties&);
00485 
00486 } // namespace SimTK
00487 
00488 #endif // SimTK_SIMMATRIX_MASS_PROPERTIES_H_

Generated on Fri Sep 26 07:44:13 2008 for SimTKcore by  doxygen 1.5.6