Simbody 3.7
Loading...
Searching...
No Matches
MassProperties.h
Go to the documentation of this file.
1#ifndef SimTK_SIMMATRIX_MASS_PROPERTIES_H_
2#define SimTK_SIMMATRIX_MASS_PROPERTIES_H_
3
4/* -------------------------------------------------------------------------- *
5 * Simbody(tm): SimTKcommon *
6 * -------------------------------------------------------------------------- *
7 * This is part of the SimTK biosimulation toolkit originating from *
8 * Simbios, the NIH National Center for Physics-Based Simulation of *
9 * Biological Structures at Stanford, funded under the NIH Roadmap for *
10 * Medical Research, grant U54 GM072970. See https://simtk.org/home/simbody. *
11 * *
12 * Portions copyright (c) 2005-14 Stanford University and the Authors. *
13 * Authors: Michael Sherman *
14 * Contributors: Paul Mitiguy *
15 * *
16 * Licensed under the Apache License, Version 2.0 (the "License"); you may *
17 * not use this file except in compliance with the License. You may obtain a *
18 * copy of the License at http://www.apache.org/licenses/LICENSE-2.0. *
19 * *
20 * Unless required by applicable law or agreed to in writing, software *
21 * distributed under the License is distributed on an "AS IS" BASIS, *
22 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. *
23 * See the License for the specific language governing permissions and *
24 * limitations under the License. *
25 * -------------------------------------------------------------------------- */
26
33#include "SimTKcommon/Scalar.h"
36
37#include <iostream>
38
39namespace SimTK {
57
67
79
80// These are templatized by precision (float or double).
81template <class P> class UnitInertia_;
82template <class P> class Inertia_;
83template <class P> class SpatialInertia_;
84template <class P> class ArticulatedInertia_;
85template <class P> class MassProperties_;
86
87// The "no trailing underscore" typedefs use whatever the
88// compile-time precision is set to.
89
96
103
110
117
124
127
128// -----------------------------------------------------------------------------
129// INERTIA MATRIX
130// -----------------------------------------------------------------------------
192template <class P>
194 typedef Rotation_<P> RotationP;
195public:
198Inertia_() : I_OF_F(NTraits<P>::getNaN()) {}
199
200// Default copy constructor, copy assignment, destructor.
201
208explicit Inertia_(const P& moment) : I_OF_F(moment)
209{ errChk("Inertia::Inertia(moment)"); }
210
214Inertia_(const Vec<3,P>& p, const P& mass) : I_OF_F(pointMassAt(p,mass)) {}
215
220explicit Inertia_(const Vec<3,P>& moments, const Vec<3,P>& products=Vec<3,P>(0))
221{ I_OF_F.updDiag() = moments;
222 I_OF_F.updLower() = products;
223 errChk("Inertia::Inertia(moments,products)"); }
224
226Inertia_(const P& xx, const P& yy, const P& zz)
227{ I_OF_F = SymMat<3,P>(xx,
228 0, yy,
229 0, 0, zz);
230 errChk("Inertia::setInertia(xx,yy,zz)"); }
231
234Inertia_(const P& xx, const P& yy, const P& zz,
235 const P& xy, const P& xz, const P& yz)
236{ I_OF_F = SymMat<3,P>(xx,
237 xy, yy,
238 xz, yz, zz);
239 errChk("Inertia::setInertia(xx,yy,zz,xy,xz,yz)"); }
240
243explicit Inertia_(const SymMat<3,P>& inertia) : I_OF_F(inertia)
244{ errChk("Inertia::Inertia(SymMat33)"); }
245
249explicit Inertia_(const Mat<3,3,P>& m)
251 "Inertia(Mat33)", "The supplied matrix was not symmetric.");
252 I_OF_F = SymMat<3,P>(m);
253 errChk("Inertia(Mat33)"); }
254
255
258Inertia_& setInertia(const P& xx, const P& yy, const P& zz) {
259 I_OF_F = P(0); I_OF_F(0,0) = xx; I_OF_F(1,1) = yy; I_OF_F(2,2) = zz;
260 errChk("Inertia::setInertia(xx,yy,zz)");
261 return *this;
262}
263
266Inertia_& setInertia(const Vec<3,P>& moments, const Vec<3,P>& products=Vec<3,P>(0)) {
267 I_OF_F.updDiag() = moments;
268 I_OF_F.updLower() = products;
269 errChk("Inertia::setInertia(moments,products)");
270 return *this;
271}
272
278Inertia_& setInertia(const P& xx, const P& yy, const P& zz,
279 const P& xy, const P& xz, const P& yz) {
280 setInertia(Vec<3,P>(xx,yy,zz), Vec<3,P>(xy,xz,yz));
281 errChk("Inertia::setInertia(xx,yy,zz,xy,xz,yz)");
282 return *this;
283}
284
285
288Inertia_& operator+=(const Inertia_& inertia)
289{ I_OF_F += inertia.I_OF_F;
290 errChk("Inertia::operator+=()");
291 return *this; }
292
295Inertia_& operator-=(const Inertia_& inertia)
296{ I_OF_F -= inertia.I_OF_F;
297 errChk("Inertia::operator-=()");
298 return *this; }
299
301Inertia_& operator*=(const P& s) {I_OF_F *= s; return *this;}
302
305Inertia_& operator/=(const P& s) {I_OF_F /= s; return *this;}
306
316Inertia_ shiftToMassCenter(const Vec<3,P>& CF, const P& mass) const
317{ Inertia_ I(*this); I -= pointMassAt(CF, mass);
318 I.errChk("Inertia::shiftToMassCenter()");
319 return I; }
320
331Inertia_& shiftToMassCenterInPlace(const Vec<3,P>& CF, const P& mass)
332{ (*this) -= pointMassAt(CF, mass);
333 errChk("Inertia::shiftToMassCenterInPlace()");
334 return *this; }
335
343Inertia_ shiftFromMassCenter(const Vec<3,P>& p, const P& mass) const
344{ Inertia_ I(*this); I += pointMassAt(p, mass);
345 I.errChk("Inertia::shiftFromMassCenter()");
346 return I; }
347
357{ (*this) += pointMassAt(p, mass);
358 errChk("Inertia::shiftFromMassCenterInPlace()");
359 return *this; }
360
371Inertia_ reexpress(const Rotation_<P>& R_FB) const
372{ return Inertia_((~R_FB).reexpressSymMat33(I_OF_F)); }
373
377{ return Inertia_((~R_FB).reexpressSymMat33(I_OF_F)); }
378
384{ I_OF_F = (~R_FB).reexpressSymMat33(I_OF_F); return *this; }
385
389{ I_OF_F = (~R_FB).reexpressSymMat33(I_OF_F); return *this; }
390
391P trace() const {return I_OF_F.trace();}
392
394operator const SymMat<3,P>&() const {return I_OF_F;}
395
397const SymMat<3,P>& asSymMat33() const {return I_OF_F;}
398
401Mat<3,3,P> toMat33() const {return Mat<3,3,P>(I_OF_F);}
402
405const Vec<3,P>& getMoments() const {return I_OF_F.getDiag();}
408const Vec<3,P>& getProducts() const {return I_OF_F.getLower();}
409
410bool isNaN() const {return I_OF_F.isNaN();}
411bool isInf() const {return I_OF_F.isInf();}
412bool isFinite() const {return I_OF_F.isFinite();}
413
417bool isNumericallyEqual(const Inertia_<P>& other) const
418{ return I_OF_F.isNumericallyEqual(other.I_OF_F); }
419
423bool isNumericallyEqual(const Inertia_<P>& other, double tol) const
424{ return I_OF_F.isNumericallyEqual(other.I_OF_F, tol); }
425
428static bool isValidInertiaMatrix(const SymMat<3,P>& m) {
429 if (m.isNaN()) return false;
430
431 const Vec<3,P>& d = m.diag();
432 if (!(d >= 0)) return false; // diagonals must be nonnegative
433
434 const P Slop = std::max(d.sum(),P(1))
436
437 if (!(d[0]+d[1]+Slop>=d[2] && d[0]+d[2]+Slop>=d[1] && d[1]+d[2]+Slop>=d[0]))
438 return false; // must satisfy triangle inequality
439
440 // Thanks to Paul Mitiguy for this condition on products of inertia.
441 const Vec<3,P>& p = m.getLower();
442 if (!( d[0]+Slop>=std::abs(2*p[2])
443 && d[1]+Slop>=std::abs(2*p[1])
444 && d[2]+Slop>=std::abs(2*p[0])))
445 return false; // max products are limited by moments
446
447 return true;
448}
449
453
458static Inertia_ pointMassAt(const Vec<3,P>& p, const P& m) {
459 const Vec<3,P> mp = m*p; // 3 flops
460 const P mxx = mp[0]*p[0];
461 const P myy = mp[1]*p[1];
462 const P mzz = mp[2]*p[2];
463 const P nmx = -mp[0];
464 const P nmy = -mp[1];
465 return Inertia_( myy+mzz, mxx+mzz, mxx+myy,
466 nmx*p[1], nmx*p[2], nmy*p[2] );
467}
468
474
475
478inline static Inertia_ sphere(const P& r);
479
482inline static Inertia_ cylinderAlongZ(const P& r, const P& hz);
483
486inline static Inertia_ cylinderAlongY(const P& r, const P& hy);
487
490inline static Inertia_ cylinderAlongX(const P& r, const P& hx);
491
495inline static Inertia_ brick(const P& hx, const P& hy, const P& hz);
496
498inline static Inertia_ brick(const Vec<3,P>& halfLengths);
499
501inline static Inertia_ ellipsoid(const P& hx, const P& hy, const P& hz);
502
504inline static Inertia_ ellipsoid(const Vec<3,P>& halfLengths);
505
507
508protected:
509// Reinterpret this Inertia matrix as a UnitInertia matrix, that is, as the
510// inertia of something with unit mass. This is useful in implementing
511// methods of the UnitInertia class in terms of Inertia methods. Be sure you
512// know that this is a unit-mass inertia!
514{ return *static_cast<const UnitInertia_<P>*>(this); }
516{ return *static_cast<UnitInertia_<P>*>(this); }
517
518// If error checking is enabled (only in Debug mode), this
519// method will run some tests on the current contents of this Inertia
520// matrix and throw an error message if it is not valid. This should be
521// the same set of tests as run by the isValidInertiaMatrix() method above.
522void errChk(const char* methodName) const {
523#ifndef NDEBUG
524 SimTK_ERRCHK(!isNaN(), methodName,
525 "Inertia matrix contains a NaN.");
526
527 const Vec<3,P>& d = I_OF_F.getDiag(); // moments
528 const Vec<3,P>& p = I_OF_F.getLower(); // products
529 const P Ixx = d[0], Iyy = d[1], Izz = d[2];
530 const P Ixy = p[0], Ixz = p[1], Iyz = p[2];
531
532 SimTK_ERRCHK3(d >= -SignificantReal, methodName,
533 "Diagonals of an Inertia matrix must be nonnegative; got %g,%g,%g.",
534 (double)Ixx,(double)Iyy,(double)Izz);
535
536 // TODO: This is looser than it should be as a workaround for distorted
537 // rotation matrices that were produced by an 11,000 body chain that
538 // Sam Flores encountered.
539 const P Slop = std::max(d.sum(),P(1))
540 * std::sqrt(NTraits<P>::getEps());
541
542 SimTK_ERRCHK3( Ixx+Iyy+Slop>=Izz
543 && Ixx+Izz+Slop>=Iyy
544 && Iyy+Izz+Slop>=Ixx,
545 methodName,
546 "Diagonals of an Inertia matrix must satisfy the triangle "
547 "inequality; got %g,%g,%g.",
548 (double)Ixx,(double)Iyy,(double)Izz);
549
550 // Thanks to Paul Mitiguy for this condition on products of inertia.
551 SimTK_ERRCHK( Ixx+Slop>=std::abs(2*Iyz)
552 && Iyy+Slop>=std::abs(2*Ixz)
553 && Izz+Slop>=std::abs(2*Ixy),
554 methodName,
555 "The magnitude of a product of inertia was too large to be physical.");
556#endif
557}
558
559// Inertia expressed in frame F and about F's origin OF. Note that frame F
560// is implicit here; all we actually have are the inertia scalars.
562};
563
568template <class P> inline Inertia_<P>
570{ return Inertia_<P>(l) += r; }
571
577template <class P> inline Inertia_<P>
579{ return Inertia_<P>(l) -= r; }
580
583template <class P> inline Inertia_<P>
584operator*(const Inertia_<P>& i, const P& r)
585{ return Inertia_<P>(i) *= r; }
586
589template <class P> inline Inertia_<P>
590operator*(const P& r, const Inertia_<P>& i)
591{ return Inertia_<P>(i) *= r; }
592
593
597template <class P> inline Inertia_<P>
598operator*(const Inertia_<P>& i, int r)
599{ return Inertia_<P>(i) *= P(r); }
600
604template <class P> inline Inertia_<P>
605operator*(int r, const Inertia_<P>& i)
606{ return Inertia_<P>(i) *= P(r); }
607
611template <class P> inline Inertia_<P>
612operator/(const Inertia_<P>& i, const P& r)
613{ return Inertia_<P>(i) /= r; }
614
618template <class P> inline Inertia_<P>
619operator/(const Inertia_<P>& i, int r)
620{ return Inertia_<P>(i) /= P(r); }
621
625template <class P> inline Vec<3,P>
626operator*(const Inertia_<P>& I, const Vec<3,P>& w)
627{ return I.asSymMat33() * w; }
628
633template <class P> inline bool
634operator==(const Inertia_<P>& i1, const Inertia_<P>& i2)
635{ return i1.asSymMat33() == i2.asSymMat33(); }
636
640template <class P> inline std::ostream&
641operator<<(std::ostream& o, const Inertia_<P>& inertia)
642{ return o << inertia.toMat33(); }
643
644
645// -----------------------------------------------------------------------------
646// UNIT INERTIA MATRIX
647// -----------------------------------------------------------------------------
668template <class P>
670 typedef P RealP;
671 typedef Vec<3,P> Vec3P;
672 typedef SymMat<3,P> SymMat33P;
673 typedef Mat<3,3,P> Mat33P;
674 typedef Rotation_<P> RotationP;
675 typedef Inertia_<P> InertiaP;
676public:
680
681// Default copy constructor, copy assignment, destructor.
682
686explicit UnitInertia_(const RealP& moment) : InertiaP(moment) {}
687
692explicit UnitInertia_(const Vec3P& moments, const Vec3P& products=Vec3P(0))
693: InertiaP(moments,products) {}
694
696UnitInertia_(const RealP& xx, const RealP& yy, const RealP& zz)
697: InertiaP(xx,yy,zz) {}
698
701UnitInertia_(const RealP& xx, const RealP& yy, const RealP& zz,
702 const RealP& xy, const RealP& xz, const RealP& yz)
703: InertiaP(xx,yy,zz,xy,xz,yz) {}
704
707explicit UnitInertia_(const SymMat33P& m) : InertiaP(m) {}
708
712explicit UnitInertia_(const Mat33P& m) : InertiaP(m) {}
713
718explicit UnitInertia_(const Inertia_<P>& inertia) : InertiaP(inertia) {}
719
723UnitInertia_& setUnitInertia(const RealP& xx, const RealP& yy, const RealP& zz)
724{ InertiaP::setInertia(xx,yy,zz); return *this; }
725
728UnitInertia_& setUnitInertia(const Vec3P& moments, const Vec3P& products=Vec3P(0))
729{ InertiaP::setInertia(moments,products); return *this; }
730
736UnitInertia_& setUnitInertia(const RealP& xx, const RealP& yy, const RealP& zz,
737 const RealP& xy, const RealP& xz, const RealP& yz)
738{ InertiaP::setInertia(xx,yy,zz,xy,xz,yz); return *this; }
739
740
741// No +=, -=, etc. operators because those don't result in a UnitInertia
742// matrix. The parent class ones are suppressed below.
743
754{ UnitInertia_ G(*this);
755 G.Inertia_<P>::operator-=(pointMassAt(CF));
756 return G; }
757
771{ InertiaP::operator-=(pointMassAt(CF));
772 return *this; }
773
782{ UnitInertia_ G(*this);
783 G.Inertia_<P>::operator+=(pointMassAt(p));
784 return G; }
785
795{ InertiaP::operator+=(pointMassAt(p));
796 return *this; }
797
809{ return UnitInertia_((~R_FB).reexpressSymMat33(this->I_OF_F)); }
810
814{ return UnitInertia_((~R_FB).reexpressSymMat33(this->I_OF_F)); }
815
821{ InertiaP::reexpressInPlace(R_FB); return *this; }
822
826{ InertiaP::reexpressInPlace(R_FB); return *this; }
827
828
830operator const SymMat33P&() const {return this->I_OF_F;}
831
836{ return *static_cast<const Inertia_<P>*>(this); }
837
841{ Inertia_<P>::operator=(inertia);
842 return *this; }
843
849
856
857
861
867{ return UnitInertia_(crossMatSq(p)); }
868
871static UnitInertia_ sphere(const RealP& r) {return UnitInertia_(RealP(0.4)*r*r);}
872
875static UnitInertia_ cylinderAlongZ(const RealP& r, const RealP& hz) {
876 const RealP Ixx = (r*r)/4 + (hz*hz)/3;
877 return UnitInertia_(Ixx,Ixx,(r*r)/2);
878}
879
882static UnitInertia_ cylinderAlongY(const RealP& r, const RealP& hy) {
883 const RealP Ixx = (r*r)/4 + (hy*hy)/3;
884 return UnitInertia_(Ixx,(r*r)/2,Ixx);
885}
886
889static UnitInertia_ cylinderAlongX(const RealP& r, const RealP& hx) {
890 const RealP Iyy = (r*r)/4 + (hx*hx)/3;
891 return UnitInertia_((r*r)/2,Iyy,Iyy);
892}
893
897static UnitInertia_ brick(const RealP& hx, const RealP& hy, const RealP& hz) {
898 const RealP oo3 = RealP(1)/RealP(3);
899 const RealP hx2=hx*hx, hy2=hy*hy, hz2=hz*hz;
900 return UnitInertia_(oo3*(hy2+hz2), oo3*(hx2+hz2), oo3*(hx2+hy2));
901}
902
904static UnitInertia_ brick(const Vec3P& halfLengths)
905{ return brick(halfLengths[0],halfLengths[1],halfLengths[2]); }
906
908static UnitInertia_ ellipsoid(const RealP& hx, const RealP& hy, const RealP& hz) {
909 const RealP hx2=hx*hx, hy2=hy*hy, hz2=hz*hz;
910 return UnitInertia_((hy2+hz2)/5, (hx2+hz2)/5, (hx2+hy2)/5);
911}
912
914static UnitInertia_ ellipsoid(const Vec3P& halfLengths)
915{ return ellipsoid(halfLengths[0],halfLengths[1],halfLengths[2]); }
916
918private:
919// Suppress Inertia_ methods which are not allowed for UnitInertia_.
920
921// These kill all flavors of Inertia_::setInertia() and the
922// Inertia_ assignment ops.
923void setInertia() {}
924void operator+=(int) {}
925void operator-=(int) {}
926void operator*=(int) {}
927void operator/=(int) {}
928};
929
930// Implement Inertia methods which are pass-throughs to UnitInertia methods.
931
932template <class P> inline Inertia_<P> Inertia_<P>::
933sphere(const P& r)
934{ return UnitInertia_<P>::sphere(r); }
935template <class P> inline Inertia_<P> Inertia_<P>::
936cylinderAlongZ(const P& r, const P& hz)
937{ return UnitInertia_<P>::cylinderAlongZ(r,hz); }
938template <class P> inline Inertia_<P> Inertia_<P>::
939cylinderAlongY(const P& r, const P& hy)
940{ return UnitInertia_<P>::cylinderAlongY(r,hy); }
941template <class P> inline Inertia_<P> Inertia_<P>::
942cylinderAlongX(const P& r, const P& hx)
943{ return UnitInertia_<P>::cylinderAlongX(r,hx); }
944template <class P> inline Inertia_<P> Inertia_<P>::
945brick(const P& hx, const P& hy, const P& hz)
946{ return UnitInertia_<P>::brick(hx,hy,hz); }
947template <class P> inline Inertia_<P> Inertia_<P>::
948brick(const Vec<3,P>& halfLengths)
949{ return UnitInertia_<P>::brick(halfLengths); }
950template <class P> inline Inertia_<P> Inertia_<P>::
951ellipsoid(const P& hx, const P& hy, const P& hz)
952{ return UnitInertia_<P>::ellipsoid(hx,hy,hz); }
953template <class P> inline Inertia_<P> Inertia_<P>::
954ellipsoid(const Vec<3,P>& halfLengths)
955{ return UnitInertia_<P>::ellipsoid(halfLengths); }
956
957
958// -----------------------------------------------------------------------------
959// SPATIAL INERTIA MATRIX
960// -----------------------------------------------------------------------------
992template <class P>
994 typedef P RealP;
995 typedef Vec<3,P> Vec3P;
997 typedef Mat<3,3,P> Mat33P;
1000 typedef Rotation_<P> RotationP;
1001 typedef Transform_<P> TransformP;
1002 typedef Inertia_<P> InertiaP;
1003public:
1006: m(nanP()), p(nanP()) {} // inertia is already NaN
1007SpatialInertia_(RealP mass, const Vec3P& com, const UnitInertiaP& gyration)
1008: m(mass), p(com), G(gyration) {}
1009
1010// default copy constructor, copy assignment, destructor
1011
1013{ SimTK_ERRCHK1(mass >= 0, "SpatialInertia::setMass()",
1014 "Negative mass %g is illegal.", (double)mass);
1015 m=mass; return *this; }
1017{ p=com; return *this;}
1019{ G=gyration; return *this; }
1020
1021RealP getMass() const {return m;}
1022const Vec3P& getMassCenter() const {return p;}
1023const UnitInertiaP& getUnitInertia() const {return G;}
1024
1027Vec3P calcMassMoment() const {return m*p;}
1028
1031InertiaP calcInertia() const {return m*G;}
1032
1038 SimTK_ERRCHK(m+src.m != 0, "SpatialInertia::operator+=()",
1039 "The combined mass cannot be zero.");
1040 const RealP mtot = m+src.m, oomtot = 1/mtot; // ~11 flops
1041 p = oomtot*(calcMassMoment() + src.calcMassMoment()); // 10 flops
1042 G.setFromUnitInertia(oomtot*(calcInertia()+src.calcInertia())); // 19 flops
1043 m = mtot; // must do this last
1044 return *this;
1045}
1046
1052 SimTK_ERRCHK(m != src.m, "SpatialInertia::operator-=()",
1053 "The combined mass cannot be zero.");
1054 const RealP mtot = m-src.m, oomtot = 1/mtot; // ~11 flops
1055 p = oomtot*(calcMassMoment() - src.calcMassMoment()); // 10 flops
1056 G.setFromUnitInertia(oomtot*(calcInertia()-src.calcInertia())); // 19 flops
1057 m = mtot; // must do this last
1058 return *this;
1059}
1060
1063SpatialInertia_& operator*=(const RealP& s) {m *= s; return *this;}
1064
1067SpatialInertia_& operator/=(const RealP& s) {m /= s; return *this;}
1068
1072{ return m*SpatialVecP(G*v[0]+p%v[1], v[1]-p%v[0]); }
1073
1080{ return SpatialInertia_(*this).reexpressInPlace(R_FB); }
1081
1085{ return SpatialInertia_(*this).reexpressInPlace(R_FB); }
1086
1092{ p = (~R_FB)*p; G.reexpressInPlace(R_FB); return *this; }
1093
1097{ p = (~R_FB)*p; G.reexpressInPlace(R_FB); return *this; }
1098
1104{ return SpatialInertia_(*this).shiftInPlace(S); }
1105
1111 G.shiftToCentroidInPlace(p); // change to central inertia
1112 const Vec3P pNew(p-S); // vec from new origin to com (3 flops)
1113 G.shiftFromCentroidInPlace(pNew); // shift to S (pNew sign doesn't matter)
1114 p = pNew; // had p=com-OF; want p=com-(OF+S)=p-S
1115 return *this;
1116}
1117
1127{ return SpatialInertia_(*this).transformInPlace(X_FB); }
1128
1133
1144 shiftInPlace(X_FB.p()); // shift to the new origin OB.
1145 reexpressInPlace(X_FB.R()); // get everything in B
1146 return *this;
1147}
1148
1152 shiftInPlace(X_FB.p()); // shift to the new origin OB.
1153 reexpressInPlace(X_FB.R()); // get everything in B
1154 return *this;
1155}
1156
1158 Mat33P offDiag = crossMat(m*p);
1159 return SpatialMatP(m*G.toMat33(), offDiag,
1160 -offDiag, Mat33P(m));
1161}
1162
1163private:
1164RealP m; // mass of this rigid body F
1165Vec3P p; // location of body's COM from OF, expressed in F
1166UnitInertiaP G; // mass distribution; inertia is mass*gyration
1167
1168static P nanP() {return NTraits<P>::getNaN();}
1169};
1170
1173template <class P> inline SpatialInertia_<P>
1175{ return SpatialInertia_<P>(l) += r; }
1176
1180template <class P> inline SpatialInertia_<P>
1182{ return SpatialInertia_<P>(l) -= r; }
1183
1184
1185// -----------------------------------------------------------------------------
1186// ARTICULATED BODY INERTIA MATRIX
1187// -----------------------------------------------------------------------------
1234template <class P>
1236 typedef P RealP;
1237 typedef Vec<3,P> Vec3P;
1239 typedef Mat<3,3,P> Mat33P;
1240 typedef SymMat<3,P> SymMat33P;
1241 typedef Vec<2, Vec3P> SpatialVecP;
1243 typedef Rotation_<P> RotationP;
1244 typedef Transform_<P> TransformP;
1245 typedef Inertia_<P> InertiaP;
1246public:
1251ArticulatedInertia_(const SymMat33P& mass, const Mat33P& massMoment, const SymMat33P& inertia)
1252: M(mass), J(inertia), F(massMoment) {}
1253
1257: M(rbi.getMass()), J(rbi.calcInertia()), F(crossMat(rbi.calcMassMoment())) {}
1258
1260ArticulatedInertia_& setMass (const SymMat33P& mass) {M=mass; return *this;}
1262ArticulatedInertia_& setMassMoment(const Mat33P& massMoment) {F=massMoment; return *this;}
1264ArticulatedInertia_& setInertia (const SymMat33P& inertia) {J=inertia; return *this;}
1265
1267const SymMat33P& getMass() const {return M;}
1269const Mat33P& getMassMoment() const {return F;}
1271const SymMat33P& getInertia() const {return J;}
1272
1273// default destructor, copy constructor, copy assignment
1274
1278{ M+=src.M; J+=src.J; F+=src.F; return *this; }
1282{ M-=src.M; J-=src.J; F-=src.F; return *this; }
1283
1286{ return SpatialVecP(J*v[0]+F*v[1], ~F*v[0]+M*v[1]); }
1287
1289template <int N>
1291 Mat<2,N,Vec3P> res;
1292 for (int j=0; j < N; ++j)
1293 res.col(j) = (*this) * m.col(j); // above this*SpatialVec operator
1294 return res;
1295}
1296
1308
1312
1316 // This more-beautiful code ran into an optimization bug Microsoft
1317 // introduced in Update 2 to Visual C++ 2015.
1318 //return SpatialMatP( Mat33P(J), F,
1319 // ~F, Mat33P(M) );
1320
1321 // Uglier but functional.
1322 SpatialMatP smat;
1323 smat(0,0) = Mat33P(J); smat(0,1) = F;
1324 smat(1,0) = ~F; smat(1,1) = Mat33P(M);
1325 return smat;
1326}
1327private:
1328SymMat33P M;
1329SymMat33P J;
1330Mat33P F;
1331};
1332
1335template <class P> inline ArticulatedInertia_<P>
1338
1342template <class P> inline ArticulatedInertia_<P>
1345
1346
1347// -----------------------------------------------------------------------------
1348// MASS PROPERTIES
1349// -----------------------------------------------------------------------------
1362template <class P>
1364public:
1367MassProperties_() { setMassProperties(0,Vec<3,P>(0),UnitInertia_<P>()); }
1371MassProperties_(const P& m, const Vec<3,P>& com, const Inertia_<P>& inertia)
1372 { setMassProperties(m,com,inertia); }
1375MassProperties_(const P& m, const Vec<3,P>& com, const UnitInertia_<P>& gyration)
1376 { setMassProperties(m,com,gyration); }
1377
1381MassProperties_& setMassProperties(const P& m, const Vec<3,P>& com, const Inertia_<P>& inertia) {
1382 mass = m;
1383 comInB = com;
1384 if (m == 0) {
1385 SimTK_ASSERT(inertia.asSymMat33().getAsVec() == 0, "Mass is zero but inertia is nonzero");
1386 unitInertia_OB_B = UnitInertia_<P>(0);
1387 }
1388 else {
1389 unitInertia_OB_B = UnitInertia_<P>(inertia*(1/m));
1390 }
1391 return *this;
1392}
1393
1397 (const P& m, const Vec<3,P>& com, const UnitInertia_<P>& gyration)
1398{ mass=m; comInB=com; unitInertia_OB_B=gyration; return *this; }
1399
1401const P& getMass() const {return mass;}
1406const Vec<3,P>& getMassCenter() const {return comInB;}
1411const UnitInertia_<P>& getUnitInertia() const {return unitInertia_OB_B;}
1417const Inertia_<P> calcInertia() const {return mass*unitInertia_OB_B;}
1422const Inertia_<P> getInertia() const {return calcInertia();}
1423
1428 return mass*unitInertia_OB_B - Inertia_<P>(comInB, mass);
1429}
1434Inertia_<P> calcShiftedInertia(const Vec<3,P>& newOriginB) const {
1435 return calcCentralInertia() + Inertia_<P>(newOriginB-comInB, mass);
1436}
1442 return calcShiftedInertia(X_BC.p()).reexpress(X_BC.R());
1443}
1450 return MassProperties_(mass, comInB-newOriginB,
1451 calcShiftedInertia(newOriginB));
1452}
1453
1464 return MassProperties_(mass, ~X_BC*comInB, calcTransformedInertia(X_BC));
1465}
1466
1476 return MassProperties_(mass, ~R_BC*comInB, unitInertia_OB_B.reexpress(R_BC));
1477}
1478
1482bool isExactlyMassless() const { return mass==0; }
1488bool isNearlyMassless(const P& tol=SignificantReal) const {
1489 return mass <= tol;
1490}
1491
1495bool isExactlyCentral() const { return comInB==Vec<3,P>(0); }
1501bool isNearlyCentral(const P& tol=SignificantReal) const {
1502 return comInB.normSqr() <= tol*tol;
1503}
1504
1507bool isNaN() const {return SimTK::isNaN(mass) || comInB.isNaN() || unitInertia_OB_B.isNaN();}
1512bool isInf() const {
1513 if (isNaN()) return false;
1514 return SimTK::isInf(mass) || comInB.isInf() || unitInertia_OB_B.isInf();
1515}
1519bool isFinite() const {
1520 return SimTK::isFinite(mass) && comInB.isFinite() && unitInertia_OB_B.isFinite();
1521}
1522
1528 M(0,0) = mass*unitInertia_OB_B.toMat33();
1529 M(0,1) = mass*crossMat(comInB);
1530 M(1,0) = ~M(0,1);
1531 M(1,1) = mass; // a diagonal matrix
1532 return M;
1533}
1534
1541 Mat<6,6,P> M;
1542 M.template updSubMat<3,3>(0,0) = mass*unitInertia_OB_B.toMat33();
1543 M.template updSubMat<3,3>(0,3) = mass*crossMat(comInB);
1544 M.template updSubMat<3,3>(3,0) = ~M.template getSubMat<3,3>(0,3);
1545 M.template updSubMat<3,3>(3,3) = mass; // a diagonal matrix
1546 return M;
1547}
1548
1549private:
1550P mass;
1551Vec<3,P> comInB; // meas. from B origin, expr. in B
1552UnitInertia_<P> unitInertia_OB_B; // about B origin, expr. in B
1553};
1554
1564template <class P> static inline std::ostream&
1565operator<<(std::ostream& o, const MassProperties_<P>& mp) {
1566 return o << "{ mass=" << mp.getMass()
1567 << "\n com=" << mp.getMassCenter()
1568 << "\n Uxx,yy,zz=" << mp.getUnitInertia().getMoments()
1569 << "\n Uxy,xz,yz=" << mp.getUnitInertia().getProducts()
1570 << "\n}\n";
1571}
1572
1573} // namespace SimTK
1574
1575#endif // SimTK_SIMMATRIX_MASS_PROPERTIES_H_
#define SimTK_ASSERT(cond, msg)
Definition ExceptionMacros.h:373
#define SimTK_ERRCHK3(cond, whereChecked, fmt, a1, a2, a3)
Definition ExceptionMacros.h:330
#define SimTK_ERRCHK(cond, whereChecked, msg)
Definition ExceptionMacros.h:324
#define SimTK_ERRCHK1(cond, whereChecked, fmt, a1)
Definition ExceptionMacros.h:326
These are numerical utility classes for dealing with the relative orientations of geometric objects.
This is a user-includable header which includes everything needed to make use of SimMatrix Scalar cod...
#define SimTK_SimTKCOMMON_EXPORT
Definition SimTKcommon/include/SimTKcommon/internal/common.h:224
This file is the user-includeable header to be included in user programs to provide fixed-length Vec ...
An articulated body inertia (ABI) matrix P(q) contains the spatial inertia properties that a body app...
Definition MassProperties.h:1235
const Mat33P & getMassMoment() const
Get the mass first moment distribution matrix F from this ArticulatedInertia (full).
Definition MassProperties.h:1269
ArticulatedInertia_()
Default construction produces uninitialized junk at zero cost; be sure to fill this in before referen...
Definition MassProperties.h:1249
const SymMat33P & getMass() const
Get the mass distribution matrix M from this ArticulatedInertia (symmetric).
Definition MassProperties.h:1267
const SymMat33P & getInertia() const
Get the mass second moment (inertia) matrix J from this ArticulatedInertia (symmetric).
Definition MassProperties.h:1271
ArticulatedInertia_ & setMassMoment(const Mat33P &massMoment)
Set the mass first moment distribution matrix F in this ArticulatedInertia (full).
Definition MassProperties.h:1262
SpatialVecP operator*(const SpatialVecP &v) const
Multiply an ArticulatedIneria by a SpatialVec (66 flops).
Definition MassProperties.h:1285
const SpatialMatP toSpatialMat() const
Convert the compactly-stored ArticulatedInertia (21 elements) into a full SpatialMat with 36 elements...
Definition MassProperties.h:1315
Mat< 2, N, Vec3P > operator*(const Mat< 2, N, Vec3P > &m) const
Multiply an ArticulatedInertia by a row of SpatialVecs (66*N flops).
Definition MassProperties.h:1290
ArticulatedInertia_< P > operator-(const ArticulatedInertia_< P > &l, const ArticulatedInertia_< P > &r)
Subtract one compatible articulated inertia from another.
Definition MassProperties.h:1343
ArticulatedInertia_(const SpatialInertia_< P > &rbi)
Construct an articulated body inertia (ABI) from a rigid body spatial inertia (RBI).
Definition MassProperties.h:1256
ArticulatedInertia_ shift(const Vec3P &s) const
Rigid-shift the origin of this Articulated Body Inertia P by a shift vector -s to produce a new ABI P...
ArticulatedInertia_ & setInertia(const SymMat33P &inertia)
Set the mass second moment (inertia) matrix J in this ArticulatedInertia (symmetric).
Definition MassProperties.h:1264
ArticulatedInertia_ & operator+=(const ArticulatedInertia_ &src)
Add in a compatible ArticulatedInertia to this one.
Definition MassProperties.h:1277
ArticulatedInertia_ & operator-=(const ArticulatedInertia_ &src)
Subtract a compatible ArticulatedInertia from this one.
Definition MassProperties.h:1281
ArticulatedInertia_ & shiftInPlace(const Vec3P &s)
Rigid-shift this ABI in place by -s.
ArticulatedInertia_ & setMass(const SymMat33P &mass)
Set the mass distribution matrix M in this ArticulatedInertia (symmetric).
Definition MassProperties.h:1260
ArticulatedInertia_(const SymMat33P &mass, const Mat33P &massMoment, const SymMat33P &inertia)
Construct an ArticulatedInertia from the mass, first moment, and inertia matrices it contains.
Definition MassProperties.h:1251
ArticulatedInertia_< P > operator+(const ArticulatedInertia_< P > &l, const ArticulatedInertia_< P > &r)
Add two compatible articulated inertias.
Definition MassProperties.h:1336
The physical meaning of an inertia is the distribution of a rigid body's mass about a particular poin...
Definition MassProperties.h:193
Inertia_< P > operator*(const Inertia_< P > &i, const P &r)
Multiply an inertia matrix by a scalar.
Definition MassProperties.h:584
const SymMat< 3, P > & asSymMat33() const
Obtain a reference to the underlying symmetric matrix type.
Definition MassProperties.h:397
Inertia_< P > operator*(int r, const Inertia_< P > &i)
Multiply an inertia matrix by a scalar given as an int.
Definition MassProperties.h:605
Mat< 3, 3, P > toMat33() const
Expand the internal packed representation into a full 3x3 symmetric matrix with all elements set.
Definition MassProperties.h:401
Inertia_ & reexpressInPlace(const InverseRotation_< P > &R_FB)
Rexpress in place using an inverse rotation to avoid having to convert it.
Definition MassProperties.h:388
static Inertia_ brick(const P &hx, const P &hy, const P &hz)
Unit-mass brick given by half-lengths in each direction.
Definition MassProperties.h:945
void errChk(const char *methodName) const
Create a UnitInertia matrix for a unit mass sphere of radius r centered at the origin.
Definition MassProperties.h:522
Inertia_(const Mat< 3, 3, P > &m)
Construct an Inertia matrix from a 3x3 symmetric matrix.
Definition MassProperties.h:249
Inertia_(const Vec< 3, P > &moments, const Vec< 3, P > &products=Vec< 3, P >(0))
Create an inertia matrix from a vector of the moments of inertia (the inertia matrix diagonal) and op...
Definition MassProperties.h:220
SymMat< 3, P > I_OF_F
Create a UnitInertia matrix for a unit mass sphere of radius r centered at the origin.
Definition MassProperties.h:561
const Vec< 3, P > & getProducts() const
Obtain the inertia products (off-diagonals of the Inertia matrix) as a Vec3 with elements ordered xy,...
Definition MassProperties.h:408
Inertia_(const Vec< 3, P > &p, const P &mass)
Create an Inertia matrix for a point mass at a given location, measured from the origin OF of the imp...
Definition MassProperties.h:214
Inertia_ & operator*=(const P &s)
Multiply this inertia matrix by a scalar. Cost is 6 flops.
Definition MassProperties.h:301
Inertia_< P > operator/(const Inertia_< P > &i, const P &r)
Divide an inertia matrix by a scalar.
Definition MassProperties.h:612
Inertia_< P > operator*(const Inertia_< P > &i, int r)
Multiply an inertia matrix by a scalar given as an int.
Definition MassProperties.h:598
Inertia_ & reexpressInPlace(const Rotation_< P > &R_FB)
Re-express this inertia matrix in another frame, changing the object in place; see reexpress() if you...
Definition MassProperties.h:383
Inertia_< P > operator/(const Inertia_< P > &i, int r)
Divide an inertia matrix by a scalar provided as an int.
Definition MassProperties.h:619
Inertia_ shiftFromMassCenter(const Vec< 3, P > &p, const P &mass) const
Assuming that the current inertia I is a central inertia (that is, it is inertia about the body cente...
Definition MassProperties.h:343
bool isNumericallyEqual(const Inertia_< P > &other, double tol) const
Compare this inertia matrix with another one and return true if they are close to within a specified ...
Definition MassProperties.h:423
Vec< 3, P > operator*(const Inertia_< P > &I, const Vec< 3, P > &w)
Multiply an inertia matrix I on the right by a vector w giving the vector result I*w.
Definition MassProperties.h:626
Inertia_ & operator-=(const Inertia_ &inertia)
Subtract off another inertia matrix.
Definition MassProperties.h:295
Inertia_ & operator/=(const P &s)
Divide this inertia matrix by a scalar.
Definition MassProperties.h:305
Inertia_ & setInertia(const P &xx, const P &yy, const P &zz)
Set an inertia matrix to have only principal moments (that is, it will be diagonal).
Definition MassProperties.h:258
Inertia_ & operator+=(const Inertia_ &inertia)
Add in another inertia matrix.
Definition MassProperties.h:288
bool isNumericallyEqual(const Inertia_< P > &other) const
Compare this inertia matrix with another one and return true if they are close to within a default nu...
Definition MassProperties.h:417
Inertia_(const P &xx, const P &yy, const P &zz)
Create a principal inertia matrix (only non-zero on diagonal).
Definition MassProperties.h:226
Inertia_< P > operator*(const P &r, const Inertia_< P > &i)
Multiply an inertia matrix by a scalar.
Definition MassProperties.h:590
bool isFinite() const
Definition MassProperties.h:412
Inertia_ & setInertia(const P &xx, const P &yy, const P &zz, const P &xy, const P &xz, const P &yz)
Set this Inertia to a general matrix.
Definition MassProperties.h:278
Inertia_(const P &moment)
Create a principal inertia matrix with identical diagonal elements, like a sphere where moment=2/5 m ...
Definition MassProperties.h:208
Inertia_< P > operator+(const Inertia_< P > &l, const Inertia_< P > &r)
Add two compatible inertia matrices, meaning they must be taken about the same point and expressed in...
Definition MassProperties.h:569
Inertia_()
Default is a NaN-ed out mess to avoid accidents, even in Release mode.
Definition MassProperties.h:198
Inertia_ reexpress(const Rotation_< P > &R_FB) const
Return a new inertia matrix like this one but re-expressed in another frame (leaving the origin point...
Definition MassProperties.h:371
Inertia_ reexpress(const InverseRotation_< P > &R_FB) const
Rexpress using an inverse rotation to avoid having to convert it.
Definition MassProperties.h:376
static Inertia_ cylinderAlongZ(const P &r, const P &hz)
Unit-mass cylinder aligned along z axis; use radius and half-length.
Definition MassProperties.h:936
bool isInf() const
Definition MassProperties.h:411
static bool isValidInertiaMatrix(const SymMat< 3, P > &m)
Test some conditions that must hold for a valid Inertia matrix.
Definition MassProperties.h:428
static Inertia_ cylinderAlongY(const P &r, const P &hy)
Unit-mass cylinder aligned along y axis; use radius and half-length.
Definition MassProperties.h:939
const Vec< 3, P > & getMoments() const
Obtain the inertia moments (diagonal of the Inertia matrix) as a Vec3 ordered xx, yy,...
Definition MassProperties.h:405
Inertia_ & shiftToMassCenterInPlace(const Vec< 3, P > &CF, const P &mass)
Assume that the current inertia is about the F frame's origin OF, and expressed in F.
Definition MassProperties.h:331
static Inertia_ ellipsoid(const P &hx, const P &hy, const P &hz)
Unit-mass ellipsoid given by half-lengths in each direction.
Definition MassProperties.h:951
Inertia_(const SymMat< 3, P > &inertia)
Construct an Inertia from a symmetric 3x3 matrix.
Definition MassProperties.h:243
bool isNaN() const
Definition MassProperties.h:410
P trace() const
Definition MassProperties.h:391
Inertia_ & setInertia(const Vec< 3, P > &moments, const Vec< 3, P > &products=Vec< 3, P >(0))
Set principal moments and optionally off-diagonal terms.
Definition MassProperties.h:266
Inertia_< P > operator-(const Inertia_< P > &l, const Inertia_< P > &r)
Subtract from one inertia matrix another one which is compatible, meaning that both must be taken abo...
Definition MassProperties.h:578
bool operator==(const Inertia_< P > &i1, const Inertia_< P > &i2)
Compare two inertia matrices for exact (bitwise) equality.
Definition MassProperties.h:634
static Inertia_ pointMassAtOrigin()
Create an Inertia matrix for a point located at the origin – that is, an all-zero matrix.
Definition MassProperties.h:452
Inertia_ & shiftFromMassCenterInPlace(const Vec< 3, P > &p, const P &mass)
Assuming that the current inertia I is a central inertia (that is, it is inertia about the body cente...
Definition MassProperties.h:356
Inertia_(const P &xx, const P &yy, const P &zz, const P &xy, const P &xz, const P &yz)
This is a general inertia matrix.
Definition MassProperties.h:234
UnitInertia_< P > & updAsUnitInertia()
Create a UnitInertia matrix for a unit mass sphere of radius r centered at the origin.
Definition MassProperties.h:515
static Inertia_ pointMassAt(const Vec< 3, P > &p, const P &m)
Create an Inertia matrix for a point of a given mass, located at a given location measured from the o...
Definition MassProperties.h:458
static Inertia_ cylinderAlongX(const P &r, const P &hx)
Unit-mass cylinder aligned along x axis; use radius and half-length.
Definition MassProperties.h:942
static Inertia_ sphere(const P &r)
Create a UnitInertia matrix for a unit mass sphere of radius r centered at the origin.
Definition MassProperties.h:933
const UnitInertia_< P > & getAsUnitInertia() const
Create a UnitInertia matrix for a unit mass sphere of radius r centered at the origin.
Definition MassProperties.h:513
Inertia_ shiftToMassCenter(const Vec< 3, P > &CF, const P &mass) const
Assume that the current inertia is about the F frame's origin OF, and expressed in F.
Definition MassProperties.h:316
(Advanced) This InverseRotation class is the inverse of a Rotation.
Definition Rotation.h:1283
Transform from frame B to frame F, but with the internal representation inverted.
Definition Transform.h:306
const InverseRotation_< P > & R() const
Definition Transform.h:360
Vec< 3, P > p() const
Calculate the actual translation vector at a cost of 18 flops.
Definition Transform.h:373
This class contains the mass, center of mass, and unit inertia matrix of a rigid body B.
Definition MassProperties.h:1363
Inertia_< P > calcShiftedInertia(const Vec< 3, P > &newOriginB) const
Return the inertia of this MassProperties object, but with the "measured about" point shifted from th...
Definition MassProperties.h:1434
MassProperties_ calcTransformedMassProps(const Transform_< P > &X_BC) const
Transform these mass properties from the current frame "B" to a new frame "C", given the pose of C in...
Definition MassProperties.h:1463
MassProperties_ calcShiftedMassProps(const Vec< 3, P > &newOriginB) const
Return a new MassProperties object that is the same as this one but with the origin point shifted fro...
Definition MassProperties.h:1449
const UnitInertia_< P > & getUnitInertia() const
Return the unit inertia currently stored in this MassProperties object; this is expressed in an impli...
Definition MassProperties.h:1411
MassProperties_ reexpress(const Rotation_< P > &R_BC) const
Re-express these mass properties from the current frame "B" to a new frame "C", given the orientation...
Definition MassProperties.h:1475
MassProperties_(const P &m, const Vec< 3, P > &com, const Inertia_< P > &inertia)
Create a mass properties object from individually supplied mass, mass center, and inertia matrix.
Definition MassProperties.h:1371
MassProperties_ & setMassProperties(const P &m, const Vec< 3, P > &com, const Inertia_< P > &inertia)
Set mass, center of mass, and inertia.
Definition MassProperties.h:1381
bool isNaN() const
Return true if any element of this MassProperties object is NaN.
Definition MassProperties.h:1507
bool isNearlyCentral(const P &tol=SignificantReal) const
Return true if the mass center stored here is zero to within a small tolerance.
Definition MassProperties.h:1501
Inertia_< P > calcTransformedInertia(const Transform_< P > &X_BC) const
Return the inertia of this MassProperties object, but transformed to from the implicit B frame to a n...
Definition MassProperties.h:1441
Mat< 2, 2, Mat< 3, 3, P > > toSpatialMat() const
Convert this MassProperties object to a spatial inertia matrix and return it as a SpatialMat,...
Definition MassProperties.h:1526
const Inertia_< P > calcInertia() const
Return the inertia matrix for this MassProperties object; this is equal to the unit inertia times the...
Definition MassProperties.h:1417
MassProperties_()
Create a mass properties object in which the mass, mass center, and inertia are meaningless; you must...
Definition MassProperties.h:1367
MassProperties_(const P &m, const Vec< 3, P > &com, const UnitInertia_< P > &gyration)
Create a mass properties object from individually supplied mass, mass center, and unit inertia (gyrat...
Definition MassProperties.h:1375
bool isInf() const
Return true only if there are no NaN's in this MassProperties object, and at least one of the element...
Definition MassProperties.h:1512
bool isExactlyMassless() const
Return true only if the mass stored here is exactly zero. If the mass resulted from a computation,...
Definition MassProperties.h:1482
bool isFinite() const
Return true if none of the elements of this MassProperties object are NaN or Infinity....
Definition MassProperties.h:1519
const P & getMass() const
Return the mass currently stored in this MassProperties object.
Definition MassProperties.h:1401
bool isExactlyCentral() const
Return true only if the mass center stored here is exactly zero. If the mass center resulted from a c...
Definition MassProperties.h:1495
MassProperties_ & setMassProperties(const P &m, const Vec< 3, P > &com, const UnitInertia_< P > &gyration)
Set mass, center of mass, and unit inertia.
Definition MassProperties.h:1397
Mat< 6, 6, P > toMat66() const
Convert this MassProperties object to a spatial inertia matrix in the form of an ordinary 6x6 matrix,...
Definition MassProperties.h:1540
Inertia_< P > calcCentralInertia() const
Return the inertia of this MassProperties object, but measured about the mass center rather than abou...
Definition MassProperties.h:1427
bool isNearlyMassless(const P &tol=SignificantReal) const
Return true if the mass stored here is zero to within a small tolerance.
Definition MassProperties.h:1488
const Inertia_< P > getInertia() const
OBSOLETE – this is just here for compatibility with 2.2; since the UnitInertia is stored now the full...
Definition MassProperties.h:1422
const Vec< 3, P > & getMassCenter() const
Return the mass center currently stored in this MassProperties object; this is expressed in an implic...
Definition MassProperties.h:1406
This class represents a small matrix whose size is known at compile time, containing elements of any ...
Definition Mat.h:97
const TCol & col(int j) const
Definition Mat.h:777
bool isNumericallySymmetric(double tol=getDefaultTolerance()) const
A Matrix is symmetric (actually Hermitian) if it is square and each element (i,j) is the Hermitian tr...
Definition Mat.h:1174
Definition NTraits.h:436
The Rotation class is a Mat33 that guarantees that the matrix can be interpreted as a legitimate 3x3 ...
Definition Rotation.h:111
This is a fixed-length row vector designed for no-overhead inline computation.
Definition Row.h:132
A spatial inertia contains the mass, center of mass point, and inertia matrix for a rigid body.
Definition MassProperties.h:993
SpatialInertia_ & setMassCenter(const Vec3P &com)
Definition MassProperties.h:1016
Vec3P calcMassMoment() const
Calculate the first mass moment (mass-weighted COM location) from the mass and COM vector.
Definition MassProperties.h:1027
SpatialInertia_ & operator/=(const RealP &s)
Divide a SpatialInertia by a scalar.
Definition MassProperties.h:1067
SpatialInertia_ & setUnitInertia(const UnitInertiaP &gyration)
Definition MassProperties.h:1018
RealP getMass() const
Definition MassProperties.h:1021
SpatialInertia_< P > operator-(const SpatialInertia_< P > &l, const SpatialInertia_< P > &r)
Subtract one compatible spatial inertia from another.
Definition MassProperties.h:1181
SpatialInertia_ reexpress(const InverseRotation_< P > &R_FB) const
Rexpress using an inverse rotation to avoid having to convert it.
Definition MassProperties.h:1084
SpatialInertia_ & reexpressInPlace(const Rotation_< P > &R_FB)
Re-express this SpatialInertia in another frame, modifying the original object.
Definition MassProperties.h:1091
SpatialInertia_ & shiftInPlace(const Vec3P &S)
Change origin from OF to OF+S, modifying the original object in place.
Definition MassProperties.h:1110
SpatialInertia_ & setMass(RealP mass)
Definition MassProperties.h:1012
InertiaP calcInertia() const
Calculate the inertia matrix (second mass moment, mass-weighted gyration matrix) from the mass and un...
Definition MassProperties.h:1031
SpatialInertia_ & transformInPlace(const Transform_< P > &X_FB)
Transform this SpatialInertia object so that it is measured about and expressed in a new frame,...
Definition MassProperties.h:1143
SpatialInertia_()
The default constructor fills everything with NaN, even in Release mode.
Definition MassProperties.h:1005
SpatialInertia_ reexpress(const Rotation_< P > &R_FB) const
Return a new SpatialInertia object which is the same as this one except re-expressed in another coord...
Definition MassProperties.h:1079
const Vec3P & getMassCenter() const
Definition MassProperties.h:1022
SpatialInertia_ & transformInPlace(const InverseTransform_< P > &X_FB)
Transform using an inverse transform to avoid having to convert it.
Definition MassProperties.h:1151
SpatialInertia_ transform(const Transform_< P > &X_FB) const
Return a new SpatialInertia object which is the same as this one but measured about and expressed in ...
Definition MassProperties.h:1126
SpatialInertia_ & operator-=(const SpatialInertia_ &src)
Subtract off a compatible SpatialInertia.
Definition MassProperties.h:1051
const SpatialMatP toSpatialMat() const
Definition MassProperties.h:1157
SpatialInertia_ transform(const InverseTransform_< P > &X_FB) const
Transform using an inverse transform to avoid having to convert it.
Definition MassProperties.h:1131
SpatialInertia_ shift(const Vec3P &S) const
Return a new SpatialInertia object which is the same as this one except the origin ("taken about" poi...
Definition MassProperties.h:1103
SpatialInertia_< P > operator+(const SpatialInertia_< P > &l, const SpatialInertia_< P > &r)
Add two compatible spatial inertias.
Definition MassProperties.h:1174
SpatialVecP operator*(const SpatialVecP &v) const
Multiply a SpatialInertia by a SpatialVec to produce a SpatialVec result; 45 flops.
Definition MassProperties.h:1071
SpatialInertia_(RealP mass, const Vec3P &com, const UnitInertiaP &gyration)
Definition MassProperties.h:1007
const UnitInertiaP & getUnitInertia() const
Definition MassProperties.h:1023
SpatialInertia_ & reexpressInPlace(const InverseRotation_< P > &R_FB)
Rexpress using an inverse rotation to avoid having to convert it.
Definition MassProperties.h:1096
SpatialInertia_ & operator+=(const SpatialInertia_ &src)
Add in a compatible SpatialInertia.
Definition MassProperties.h:1037
SpatialInertia_ & operator*=(const RealP &s)
Multiply a SpatialInertia by a scalar.
Definition MassProperties.h:1063
This is a small, fixed-size symmetric or Hermitian matrix designed for no-overhead inline computation...
Definition SymMat.h:87
bool isNaN() const
Return true if any element of this SymMat contains a NaN anywhere.
Definition SymMat.h:735
const TLower & getLower() const
Definition SymMat.h:825
const TDiag & diag() const
Definition SymMat.h:822
const TAsVec & getAsVec() const
Definition SymMat.h:831
This class represents the rotate-and-shift transform which gives the location and orientation of a ne...
Definition Transform.h:108
const Rotation_< P > & R() const
Return a read-only reference to the contained rotation R_BF.
Definition Transform.h:215
const Vec< 3, P > & p() const
Return a read-only reference to our translation vector p_BF.
Definition Transform.h:239
A UnitInertia matrix is a unit-mass inertia matrix; you can convert it to an Inertia by multiplying i...
Definition MassProperties.h:669
UnitInertia_ & setUnitInertia(const Vec3P &moments, const Vec3P &products=Vec3P(0))
Set principal moments and optionally off-diagonal terms.
Definition MassProperties.h:728
static bool isValidUnitInertiaMatrix(const SymMat33P &m)
Test some conditions that must hold for a valid UnitInertia matrix.
Definition MassProperties.h:847
static UnitInertia_ pointMassAtOrigin()
Create a UnitInertia matrix for a point located at the origin – that is, an all-zero matrix.
Definition MassProperties.h:860
static UnitInertia_ cylinderAlongZ(const RealP &r, const RealP &hz)
Unit-mass cylinder aligned along z axis; use radius and half-length.
Definition MassProperties.h:875
UnitInertia_ & setUnitInertia(const RealP &xx, const RealP &yy, const RealP &zz)
Set a UnitInertia matrix to have only principal moments (that is, it will be diagonal).
Definition MassProperties.h:723
const Inertia_< P > & asUnitInertia() const
Recast this UnitInertia matrix as a unit inertia matrix.
Definition MassProperties.h:835
UnitInertia_ & reexpressInPlace(const InverseRotation_< P > &R_FB)
Rexpress using an inverse rotation to avoid having to convert it.
Definition MassProperties.h:825
UnitInertia_(const SymMat33P &m)
Construct a UnitInertia from a symmetric 3x3 matrix.
Definition MassProperties.h:707
UnitInertia_ & shiftFromCentroidInPlace(const Vec3P &p)
Assuming that the current UnitInertia G is a central inertia (that is, it is inertia about the body c...
Definition MassProperties.h:794
UnitInertia_(const RealP &moment)
Create a principal unit inertia matrix with identical diagonal elements.
Definition MassProperties.h:686
UnitInertia_(const Vec3P &moments, const Vec3P &products=Vec3P(0))
Create a unit inertia matrix from a vector of the moments of inertia (the inertia matrix diagonal) an...
Definition MassProperties.h:692
static UnitInertia_ cylinderAlongY(const RealP &r, const RealP &hy)
Unit-mass cylinder aligned along y axis; use radius and half-length.
Definition MassProperties.h:882
static UnitInertia_ ellipsoid(const Vec3P &halfLengths)
Alternate interface to ellipsoid() that takes a Vec3 for the half lengths.
Definition MassProperties.h:914
static UnitInertia_ brick(const Vec3P &halfLengths)
Alternate interface to brick() that takes a Vec3 for the half lengths.
Definition MassProperties.h:904
UnitInertia_ shiftFromCentroid(const Vec3P &p) const
Assuming that the current UnitInertia G is a central inertia (that is, it is inertia about the body c...
Definition MassProperties.h:781
static UnitInertia_ cylinderAlongX(const RealP &r, const RealP &hx)
Unit-mass cylinder aligned along x axis; use radius and half-length.
Definition MassProperties.h:889
UnitInertia_(const Inertia_< P > &inertia)
Construct a UnitInertia matrix from an Inertia matrix.
Definition MassProperties.h:718
UnitInertia_ reexpress(const InverseRotation_< P > &R_FB) const
Rexpress using an inverse rotation to avoid having to convert it.
Definition MassProperties.h:813
UnitInertia_ & reexpressInPlace(const Rotation_< P > &R_FB)
Re-express this unit inertia matrix in another frame, changing the object in place; see reexpress() i...
Definition MassProperties.h:820
UnitInertia_ & shiftToCentroidInPlace(const Vec3P &CF)
Assuming that this unit inertia matrix is currently taken about some (implicit) frame F's origin OF,...
Definition MassProperties.h:770
static UnitInertia_ brick(const RealP &hx, const RealP &hy, const RealP &hz)
Unit-mass brick given by half-lengths in each direction.
Definition MassProperties.h:897
UnitInertia_ & setUnitInertia(const RealP &xx, const RealP &yy, const RealP &zz, const RealP &xy, const RealP &xz, const RealP &yz)
Set this UnitInertia to a general matrix.
Definition MassProperties.h:736
UnitInertia_(const RealP &xx, const RealP &yy, const RealP &zz)
Create a principal unit inertia matrix (only non-zero on diagonal).
Definition MassProperties.h:696
static UnitInertia_ pointMassAt(const Vec3P &p)
Create a UnitInertia matrix for a point of unit mass located at a given location measured from origin...
Definition MassProperties.h:866
UnitInertia_ & setFromUnitInertia(const Inertia_< P > &inertia)
Set from a unit inertia matrix.
Definition MassProperties.h:840
UnitInertia_(const Mat33P &m)
Construct a UnitInertia from a 3x3 symmetric matrix.
Definition MassProperties.h:712
UnitInertia_ reexpress(const Rotation_< P > &R_FB) const
Return a new unit inertia matrix like this one but re-expressed in another frame (leaving the origin ...
Definition MassProperties.h:808
UnitInertia_()
Default is a NaN-ed out mess to avoid accidents, even in Release mode.
Definition MassProperties.h:679
static UnitInertia_ sphere(const RealP &r)
Create a UnitInertia matrix for a unit mass sphere of radius r centered at the origin.
Definition MassProperties.h:871
UnitInertia_ shiftToCentroid(const Vec3P &CF) const
Assuming that this unit inertia matrix is currently taken about some (implicit) frame F's origin OF,...
Definition MassProperties.h:753
UnitInertia_(const RealP &xx, const RealP &yy, const RealP &zz, const RealP &xy, const RealP &xz, const RealP &yz)
This is a general unit inertia matrix.
Definition MassProperties.h:701
static UnitInertia_ ellipsoid(const RealP &hx, const RealP &hy, const RealP &hz)
Unit-mass ellipsoid given by half-lengths in each direction.
Definition MassProperties.h:908
This is a fixed-length column vector designed for no-overhead inline computation.
Definition Vec.h:184
EStandard sum() const
Sum just adds up all the elements into a single return element that is the same type as this Vec's el...
Definition Vec.h:366
Vec< 2, Vec3 > SpatialVec
Spatial vectors are used for (rotation,translation) quantities and consist of a pair of Vec3 objects,...
Definition MassProperties.h:50
Mat< 2, 2, Mat33 > SpatialMat
Spatial matrices are used to hold 6x6 matrices that are best viewed as 2x2 matrices of 3x3 matrices; ...
Definition MassProperties.h:72
Row< 2, Row3 > SpatialRow
This is the type of a transposed SpatialVec; it does not usually appear explicitly in user programs.
Definition MassProperties.h:60
const Real SignificantReal
SignificantReal is the smallest value that we consider to be clearly distinct from roundoff error whe...
const Complex I
We only need one complex constant, i = sqrt(-1). For the rest just multiply the real constant by i,...
This is the top-level SimTK namespace into which all SimTK names are placed to avoid collision with o...
Definition Assembler.h:37
SpatialInertia_< double > dSpatialInertia
A spatial (rigid body) inertia matrix at double precision.
Definition MassProperties.h:116
SpatialInertia_< Real > SpatialInertia
A spatial (rigid body) inertia matrix at default precision.
Definition MassProperties.h:112
SymMat< 3, E > crossMatSq(const Vec< 3, E, S > &v)
Calculate matrix S(v) such that S(v)*w = -v % (v % w) = (v % w) % v.
Definition SmallMatrixMixed.h:717
MassProperties_< float > fMassProperties
Rigid body mass properties at float precision.
Definition MassProperties.h:107
bool isInf(const negator< float > &x)
Definition negator.h:298
Row< 2, Row< 3, double > > dSpatialRow
A SpatialRow that is always double precision regardless of the compiled-in precision of Real.
Definition MassProperties.h:66
Vec< 2, Vec< 3, double > > dSpatialVec
A SpatialVec that is always double precision regardless of the compiled-in precision of Real.
Definition MassProperties.h:56
UnitInertia_< float > fUnitInertia
A unit inertia (gyration) tensor at float precision.
Definition MassProperties.h:93
Mat< 2, 2, Mat< 3, 3, float > > fSpatialMat
A SpatialMat that is always single (float) precision regardless of the compiled-in precision of Real.
Definition MassProperties.h:75
UnitInertia Gyration
For backwards compatibility only; use UnitInertia instead.
Definition MassProperties.h:126
ArticulatedInertia_< float > fArticulatedInertia
An articulated body inertia matrix at float precision.
Definition MassProperties.h:121
Inertia_< Real > Inertia
An inertia tensor at default precision.
Definition MassProperties.h:98
std::ostream & operator<<(std::ostream &o, const ContactForce &f)
Definition CompliantContactSubsystem.h:387
ArticulatedInertia_< double > dArticulatedInertia
An articulated body inertia matrix at double precision.
Definition MassProperties.h:123
Inertia_< double > dInertia
An inertia tensor at double precision.
Definition MassProperties.h:102
Vec< 2, Vec< 3, float > > fSpatialVec
A SpatialVec that is always single (float) precision regardless of the compiled-in precision of Real.
Definition MassProperties.h:53
SpatialInertia_< float > fSpatialInertia
A spatial (rigid body) inertia matrix at float precision.
Definition MassProperties.h:114
MassProperties_< Real > MassProperties
Rigid body mass properties at default precision.
Definition MassProperties.h:105
bool isNaN(const negator< float > &x)
Definition negator.h:272
bool isFinite(const negator< float > &x)
Definition negator.h:285
ArticulatedInertia_< Real > ArticulatedInertia
An articulated body inertia matrix at default precision.
Definition MassProperties.h:119
Inertia_< float > fInertia
An inertia tensor at float precision.
Definition MassProperties.h:100
Mat< 2, 2, Mat< 3, 3, double > > dSpatialMat
A SpatialMat that is always double precision regardless of the compiled-in precision of Real.
Definition MassProperties.h:78
UnitInertia_< double > dUnitInertia
A unit inertia (gyration) tensor at double precision.
Definition MassProperties.h:95
Mat< 3, 3, E > crossMat(const Vec< 3, E, S > &v)
Calculate matrix M(v) such that M(v)*w = v % w.
Definition SmallMatrixMixed.h:649
MassProperties_< double > dMassProperties
Rigid body mass properties at double precision.
Definition MassProperties.h:109
Row< 2, Row< 3, float > > fSpatialRow
A SpatialRow that is always single (float) precision regardless of the compiled-in precision of Real.
Definition MassProperties.h:63
UnitInertia_< Real > UnitInertia
A unit inertia (gyration) tensor at default precision.
Definition MassProperties.h:91