1 #ifndef SimTK_SIMMATRIX_MASS_PROPERTIES_H_ 2 #define SimTK_SIMMATRIX_MASS_PROPERTIES_H_ 212 explicit Inertia_(
const RealP& moment) : I_OF_F(moment)
213 { errChk(
"Inertia::Inertia(moment)"); }
218 Inertia_(
const Vec3P& p,
const RealP& mass) : I_OF_F(pointMassAt(p,mass)) {}
224 explicit Inertia_(
const Vec3P& moments,
const Vec3P& products=Vec3P(0))
225 { I_OF_F.updDiag() = moments;
226 I_OF_F.updLower() = products;
227 errChk(
"Inertia::Inertia(moments,products)"); }
230 Inertia_(
const RealP& xx,
const RealP& yy,
const RealP& zz)
231 { I_OF_F = SymMat33P(xx,
234 errChk(
"Inertia::setInertia(xx,yy,zz)"); }
238 Inertia_(
const RealP& xx,
const RealP& yy,
const RealP& zz,
239 const RealP& xy,
const RealP& xz,
const RealP& yz)
240 { I_OF_F = SymMat33P(xx,
243 errChk(
"Inertia::setInertia(xx,yy,zz,xy,xz,yz)"); }
247 explicit Inertia_(
const SymMat33P& inertia) : I_OF_F(inertia)
248 { errChk(
"Inertia::Inertia(SymMat33)"); }
255 "Inertia(Mat33)",
"The supplied matrix was not symmetric.");
256 I_OF_F = SymMat33P(m);
257 errChk(
"Inertia(Mat33)"); }
263 I_OF_F = RealP(0); I_OF_F(0,0) = xx; I_OF_F(1,1) = yy; I_OF_F(2,2) = zz;
264 errChk(
"Inertia::setInertia(xx,yy,zz)");
271 I_OF_F.updDiag() = moments;
272 I_OF_F.updLower() = products;
273 errChk(
"Inertia::setInertia(moments,products)");
283 const RealP& xy,
const RealP& xz,
const RealP& yz) {
284 setInertia(Vec3P(xx,yy,zz), Vec3P(xy,xz,yz));
285 errChk(
"Inertia::setInertia(xx,yy,zz,xy,xz,yz)");
293 { I_OF_F += inertia.
I_OF_F;
294 errChk(
"Inertia::operator+=()");
300 { I_OF_F -= inertia.
I_OF_F;
301 errChk(
"Inertia::operator-=()");
321 {
Inertia_ I(*
this); I -= pointMassAt(CF, mass);
322 I.
errChk(
"Inertia::shiftToMassCenter()");
336 { (*this) -= pointMassAt(CF, mass);
337 errChk(
"Inertia::shiftToMassCenterInPlace()");
348 {
Inertia_ I(*
this); I += pointMassAt(p, mass);
349 I.
errChk(
"Inertia::shiftFromMassCenter()");
361 { (*this) += pointMassAt(p, mass);
362 errChk(
"Inertia::shiftFromMassCenterInPlace()");
376 {
return Inertia_((~R_FB).reexpressSymMat33(I_OF_F)); }
381 {
return Inertia_((~R_FB).reexpressSymMat33(I_OF_F)); }
388 { I_OF_F = (~R_FB).reexpressSymMat33(I_OF_F);
return *
this; }
393 { I_OF_F = (~R_FB).reexpressSymMat33(I_OF_F);
return *
this; }
395 RealP
trace()
const {
return I_OF_F.trace();}
398 operator const SymMat33P&()
const {
return I_OF_F;}
405 Mat33P
toMat33()
const {
return Mat33P(I_OF_F);}
414 bool isNaN()
const {
return I_OF_F.isNaN();}
415 bool isInf()
const {
return I_OF_F.isInf();}
422 {
return I_OF_F.isNumericallyEqual(other.
I_OF_F); }
428 {
return I_OF_F.isNumericallyEqual(other.
I_OF_F, tol); }
433 if (m.
isNaN())
return false;
435 const Vec3P& d = m.
diag();
436 if (!(d >= 0))
return false;
441 if (!(d[0]+d[1]+Slop>=d[2] && d[0]+d[2]+Slop>=d[1] && d[1]+d[2]+Slop>=d[0]))
463 const Vec3P mp = m*p;
464 const RealP mxx = mp[0]*p[0];
465 const RealP myy = mp[1]*p[1];
466 const RealP mzz = mp[2]*p[2];
467 const RealP nmx = -mp[0];
468 const RealP nmy = -mp[1];
469 return Inertia_( myy+mzz, mxx+mzz, mxx+myy,
470 nmx*p[1], nmx*p[2], nmy*p[2] );
482 inline static Inertia_ sphere(
const RealP& r);
486 inline static Inertia_ cylinderAlongZ(
const RealP& r,
const RealP& hz);
490 inline static Inertia_ cylinderAlongY(
const RealP& r,
const RealP& hy);
494 inline static Inertia_ cylinderAlongX(
const RealP& r,
const RealP& hx);
499 inline static Inertia_ brick(
const RealP& hx,
const RealP& hy,
const RealP& hz);
502 inline static Inertia_ brick(
const Vec3P& halfLengths);
505 inline static Inertia_ ellipsoid(
const RealP& hx,
const RealP& hy,
const RealP& hz);
508 inline static Inertia_ ellipsoid(
const Vec3P& halfLengths);
526 void errChk(
const char* methodName)
const {
529 "Inertia matrix contains a NaN.");
531 const Vec3P& d = I_OF_F.getDiag();
532 const Vec3P& p = I_OF_F.getLower();
533 const RealP Ixx = d[0], Iyy = d[1], Izz = d[2];
534 const RealP Ixy = p[0], Ixz = p[1], Iyz = p[2];
537 "Diagonals of an Inertia matrix must be nonnegative; got %g,%g,%g.",
538 (
double)Ixx,(
double)Iyy,(
double)Izz);
548 && Iyy+Izz+Slop>=Ixx,
550 "Diagonals of an Inertia matrix must satisfy the triangle " 551 "inequality; got %g,%g,%g.",
552 (
double)Ixx,(
double)Iyy,(
double)Izz);
559 "The magnitude of a product of inertia was too large to be physical.");
637 template <
class P>
inline bool 644 template <
class P>
inline std::ostream&
645 operator<<(std::ostream& o, const Inertia_<P>& inertia)
646 {
return o << inertia.toMat33(); }
696 explicit UnitInertia_(
const Vec3P& moments,
const Vec3P& products=Vec3P(0))
697 : InertiaP(moments,products) {}
701 : InertiaP(xx,yy,zz) {}
706 const RealP& xy,
const RealP& xz,
const RealP& yz)
707 : InertiaP(xx,yy,zz,xy,xz,yz) {}
728 { InertiaP::setInertia(xx,yy,zz);
return *
this; }
732 UnitInertia_&
setUnitInertia(
const Vec3P& moments,
const Vec3P& products=Vec3P(0))
733 { InertiaP::setInertia(moments,products);
return *
this; }
741 const RealP& xy,
const RealP& xz,
const RealP& yz)
742 { InertiaP::setInertia(xx,yy,zz,xy,xz,yz);
return *
this; }
758 { UnitInertia_ G(*
this);
759 G.
Inertia_<P>::operator-=(pointMassAt(CF));
775 { InertiaP::operator-=(pointMassAt(CF));
786 { UnitInertia_ G(*
this);
787 G.
Inertia_<P>::operator+=(pointMassAt(p));
799 { InertiaP::operator+=(pointMassAt(p));
813 {
return UnitInertia_((~R_FB).reexpressSymMat33(this->I_OF_F)); }
818 {
return UnitInertia_((~R_FB).reexpressSymMat33(this->I_OF_F)); }
825 { InertiaP::reexpressInPlace(R_FB);
return *
this; }
830 { InertiaP::reexpressInPlace(R_FB);
return *
this; }
834 operator const SymMat33P&()
const {
return this->I_OF_F;}
875 static UnitInertia_
sphere(
const RealP& r) {
return UnitInertia_(RealP(0.4)*r*r);}
880 const RealP Ixx = (r*r)/4 + (hz*hz)/3;
881 return UnitInertia_(Ixx,Ixx,(r*r)/2);
887 const RealP Ixx = (r*r)/4 + (hy*hy)/3;
888 return UnitInertia_(Ixx,(r*r)/2,Ixx);
894 const RealP Iyy = (r*r)/4 + (hx*hx)/3;
895 return UnitInertia_((r*r)/2,Iyy,Iyy);
901 static UnitInertia_
brick(
const RealP& hx,
const RealP& hy,
const RealP& hz) {
902 const RealP oo3 = RealP(1)/RealP(3);
903 const RealP hx2=hx*hx, hy2=hy*hy, hz2=hz*hz;
904 return UnitInertia_(oo3*(hy2+hz2), oo3*(hx2+hz2), oo3*(hx2+hy2));
908 static UnitInertia_
brick(
const Vec3P& halfLengths)
909 {
return brick(halfLengths[0],halfLengths[1],halfLengths[2]); }
912 static UnitInertia_
ellipsoid(
const RealP& hx,
const RealP& hy,
const RealP& hz) {
913 const RealP hx2=hx*hx, hy2=hy*hy, hz2=hz*hz;
914 return UnitInertia_((hy2+hz2)/5, (hx2+hz2)/5, (hx2+hy2)/5);
919 {
return ellipsoid(halfLengths[0],halfLengths[1],halfLengths[2]); }
928 void operator+=(
int) {}
929 void operator-=(
int) {}
930 void operator*=(
int) {}
931 void operator/=(
int) {}
949 brick(
const RealP& hx,
const RealP& hy,
const RealP& hz)
955 ellipsoid(
const RealP& hx,
const RealP& hy,
const RealP& hz)
1010 : m(nanP()), p(nanP()) {}
1012 : m(mass), p(com), G(gyration) {}
1018 "Negative mass %g is illegal.", (
double)mass);
1019 m=mass;
return *
this; }
1021 { p=com;
return *
this;}
1023 { G=gyration;
return *
this; }
1042 SimTK_ERRCHK(m+src.m != 0,
"SpatialInertia::operator+=()",
1043 "The combined mass cannot be zero.");
1044 const RealP mtot = m+src.m, oomtot = 1/mtot;
1046 G.setFromUnitInertia(oomtot*(calcInertia()+src.
calcInertia()));
1056 SimTK_ERRCHK(m != src.m,
"SpatialInertia::operator-=()",
1057 "The combined mass cannot be zero.");
1058 const RealP mtot = m-src.m, oomtot = 1/mtot;
1060 G.setFromUnitInertia(oomtot*(calcInertia()-src.
calcInertia()));
1067 SpatialInertia_&
operator*=(
const RealP& s) {m *= s;
return *
this;}
1071 SpatialInertia_&
operator/=(
const RealP& s) {m /= s;
return *
this;}
1076 {
return m*SpatialVecP(G*v[0]+p%v[1], v[1]-p%v[0]); }
1084 {
return SpatialInertia_(*this).reexpressInPlace(R_FB); }
1089 {
return SpatialInertia_(*this).reexpressInPlace(R_FB); }
1107 SpatialInertia_
shift(
const Vec3P& S)
const 1108 {
return SpatialInertia_(*this).shiftInPlace(S); }
1115 G.shiftToCentroidInPlace(p);
1116 const Vec3P pNew(p-S);
1117 G.shiftFromCentroidInPlace(pNew);
1131 {
return SpatialInertia_(*this).transformInPlace(X_FB); }
1136 {
return SpatialInertia_(*this).transformInPlace(X_FB); }
1148 shiftInPlace(X_FB.
p());
1149 reexpressInPlace(X_FB.
R());
1156 shiftInPlace(X_FB.
p());
1157 reexpressInPlace(X_FB.
R());
1163 return SpatialMatP(m*G.toMat33(), offDiag,
1164 -offDiag, Mat33P(m));
1256 : M(mass), J(inertia), F(massMoment) {}
1261 : M(rbi.getMass()), J(rbi.calcInertia()), F(
crossMat(rbi.calcMassMoment())) {}
1282 { M+=src.M; J+=src.J; F+=src.F;
return *
this; }
1286 { M-=src.M; J-=src.J; F-=src.F;
return *
this; }
1290 {
return SpatialVecP(J*v[0]+F*v[1], ~F*v[0]+M*v[1]); }
1296 for (
int j=0; j < N; ++j)
1297 res.
col(j) = (*this) * m.
col(j);
1320 return SpatialMatP( Mat33P(J), F,
1378 { setMassProperties(m,com,inertia); }
1382 { setMassProperties(m,com,gyration); }
1392 unitInertia_OB_B = UnitInertiaP(0);
1395 unitInertia_OB_B = UnitInertiaP(inertia*(1/m));
1402 MassProperties_& setMassProperties
1403 (
const RealP& m,
const Vec3P& com,
const UnitInertiaP& gyration)
1404 { mass=m; comInB=com; unitInertia_OB_B=gyration;
return *
this; }
1434 return mass*unitInertia_OB_B - InertiaP(comInB, mass);
1441 return calcCentralInertia() + InertiaP(newOriginB-comInB, mass);
1448 return calcShiftedInertia(X_BC.
p()).reexpress(X_BC.
R());
1456 return MassProperties_(mass, comInB-newOriginB,
1457 calcShiftedInertia(newOriginB));
1470 return MassProperties_(mass, ~X_BC*comInB, calcTransformedInertia(X_BC));
1482 return MassProperties_(mass, ~R_BC*comInB, unitInertia_OB_B.reexpress(R_BC));
1508 return comInB.normSqr() <= tol*tol;
1519 if (
isNaN())
return false;
1520 return SimTK::isInf(mass) || comInB.isInf() || unitInertia_OB_B.isInf();
1526 return SimTK::isFinite(mass) && comInB.isFinite() && unitInertia_OB_B.isFinite();
1534 M(0,0) = mass*unitInertia_OB_B.toMat33();
1548 M.template updSubMat<3,3>(0,0) = mass*unitInertia_OB_B.toMat33();
1549 M.template updSubMat<3,3>(0,3) = mass*
crossMat(comInB);
1550 M.template updSubMat<3,3>(3,0) = ~M.template getSubMat<3,3>(0,3);
1551 M.template updSubMat<3,3>(3,3) = mass;
1558 UnitInertiaP unitInertia_OB_B;
1570 template <
class P>
static inline std::ostream&
1571 operator<<(std::ostream& o, const MassProperties_<P>& mp) {
1572 return o <<
"{ mass=" << mp.getMass()
1573 <<
"\n com=" << mp.getMassCenter()
1574 <<
"\n Uxx,yy,zz=" << mp.getUnitInertia().getMoments()
1575 <<
"\n Uxy,xz,yz=" << mp.getUnitInertia().getProducts()
1581 #endif // SimTK_SIMMATRIX_MASS_PROPERTIES_H_ Inertia_ & shiftToMassCenterInPlace(const Vec3P &CF, const RealP &mass)
Assume that the current inertia is about the F frame's origin OF, and expressed in F...
Definition: MassProperties.h:335
bool isFinite() const
Definition: MassProperties.h:416
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
Inertia_< P > operator/(const Inertia_< P > &i, const P &r)
Divide an inertia matrix by a scalar.
Definition: MassProperties.h:616
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:1172
Vec< 2, Vec3 > SpatialVec
Spatial vectors are used for (rotation,translation) quantities and consist of a pair of Vec3 objects...
Definition: MassProperties.h:50
SpatialInertia_ transform(const InverseTransform_< P > &X_FB) const
Transform using an inverse transform to avoid having to convert it.
Definition: MassProperties.h:1135
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:824
Inertia_(const RealP &xx, const RealP &yy, const RealP &zz)
Create a principal inertia matrix (only non-zero on diagonal).
Definition: MassProperties.h:230
ArticulatedInertia_< double > dArticulatedInertia
An articulated body inertia matrix at double precision.
Definition: MassProperties.h:123
#define SimTK_SimTKCOMMON_EXPORT
Definition: SimTKcommon/include/SimTKcommon/internal/common.h:218
UnitInertia_ & setFromUnitInertia(const Inertia_< P > &inertia)
Set from a unit inertia matrix.
Definition: MassProperties.h:844
(Advanced) This InverseRotation class is the inverse of a Rotation.
Definition: Rotation.h:47
Inertia_ & setInertia(const RealP &xx, const RealP &yy, const RealP &zz, const RealP &xy, const RealP &xz, const RealP &yz)
Set this Inertia to a general matrix.
Definition: MassProperties.h:282
bool operator==(const Inertia_< P > &i1, const Inertia_< P > &i2)
Compare two inertia matrices for exact (bitwise) equality.
Definition: MassProperties.h:638
InertiaP calcCentralInertia() const
Return the inertia of this MassProperties object, but measured about the mass center rather than abou...
Definition: MassProperties.h:1433
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:757
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:912
const TLower & getLower() const
Definition: SymMat.h:825
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:630
const Vec3P & getMassCenter() const
Return the mass center currently stored in this MassProperties object; this is expressed in an implic...
Definition: MassProperties.h:1412
bool isExactlyMassless() const
Return true only if the mass stored here is exactly zero. If the mass resulted from a computation...
Definition: MassProperties.h:1488
SpatialInertia_< double > dSpatialInertia
A spatial (rigid body) inertia matrix at double precision.
Definition: MassProperties.h:116
UnitInertia_(const Inertia_< P > &inertia)
Construct a UnitInertia matrix from an Inertia matrix.
Definition: MassProperties.h:722
static Inertia_ cylinderAlongY(const RealP &r, const RealP &hy)
Unit-mass cylinder aligned along y axis; use radius and half-length.
Definition: MassProperties.h:943
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:1518
Inertia_ & operator+=(const Inertia_ &inertia)
Add in another inertia matrix.
Definition: MassProperties.h:292
SpatialInertia_ & reexpressInPlace(const Rotation_< P > &R_FB)
Re-express this SpatialInertia in another frame, modifying the original object.
Definition: MassProperties.h:1095
MassProperties_< float > fMassProperties
Rigid body mass properties at float precision.
Definition: MassProperties.h:107
Inertia_ & setInertia(const RealP &xx, const RealP &yy, const RealP &zz)
Set an inertia matrix to have only principal moments (that is, it will be diagonal).
Definition: MassProperties.h:262
This is the top-level SimTK namespace into which all SimTK names are placed to avoid collision with o...
Definition: Assembler.h:37
static UnitInertia_ brick(const Vec3P &halfLengths)
Alternate interface to brick() that takes a Vec3 for the half lengths.
Definition: MassProperties.h:908
Inertia_< P > operator*(const P &r, const Inertia_< P > &i)
Multiply an inertia matrix by a scalar.
Definition: MassProperties.h:594
These are numerical utility classes for dealing with the relative orientations of geometric objects...
Inertia_(const Vec3P &moments, const Vec3P &products=Vec3P(0))
Create an inertia matrix from a vector of the moments of inertia (the inertia matrix diagonal) and op...
Definition: MassProperties.h:224
SpatialMatP toSpatialMat() const
Convert this MassProperties object to a spatial inertia matrix and return it as a SpatialMat...
Definition: MassProperties.h:1532
UnitInertia_ reexpress(const InverseRotation_< P > &R_FB) const
Rexpress using an inverse rotation to avoid having to convert it.
Definition: MassProperties.h:817
ArticulatedInertia_< Real > ArticulatedInertia
An articulated body inertia matrix at default precision.
Definition: MassProperties.h:119
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
SpatialInertia_ reexpress(const InverseRotation_< P > &R_FB) const
Rexpress using an inverse rotation to avoid having to convert it.
Definition: MassProperties.h:1088
The Rotation class is a Mat33 that guarantees that the matrix can be interpreted as a legitimate 3x3 ...
Definition: Quaternion.h:40
const UnitInertiaP & getUnitInertia() const
Definition: MassProperties.h:1027
Inertia_< P > operator/(const Inertia_< P > &i, int r)
Divide an inertia matrix by a scalar provided as an int.
Definition: MassProperties.h:623
bool isFinite() const
Return true if none of the elements of this MassProperties object are NaN or Infinity. Note that Ground's mass properties are not finite.
Definition: MassProperties.h:1525
bool isNaN() const
Definition: MassProperties.h:414
bool isNearlyMassless(const RealP &tol=SignificantReal) const
Return true if the mass stored here is zero to within a small tolerance.
Definition: MassProperties.h:1494
SpatialInertia_ & reexpressInPlace(const InverseRotation_< P > &R_FB)
Rexpress using an inverse rotation to avoid having to convert it.
Definition: MassProperties.h:1100
ArticulatedInertia_()
Default construction produces uninitialized junk at zero cost; be sure to fill this in before referen...
Definition: MassProperties.h:1253
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:421
const SymMat33P & asSymMat33() const
Obtain a reference to the underlying symmetric matrix type.
Definition: MassProperties.h:401
UnitInertia_(const RealP &moment)
Create a principal unit inertia matrix with identical diagonal elements.
Definition: MassProperties.h:690
#define SimTK_ASSERT(cond, msg)
Definition: ExceptionMacros.h:374
const SymMat33P & getMass() const
Get the mass distribution matrix M from this ArticulatedInertia (symmetric).
Definition: MassProperties.h:1271
ArticulatedInertia_(const SpatialInertia_< P > &rbi)
Construct an articulated body inertia (ABI) from a rigid body spatial inertia (RBI).
Definition: MassProperties.h:1260
const RealP & getMass() const
Return the mass currently stored in this MassProperties object.
Definition: MassProperties.h:1407
static UnitInertia_ cylinderAlongZ(const RealP &r, const RealP &hz)
Unit-mass cylinder aligned along z axis; use radius and half-length.
Definition: MassProperties.h:879
UnitInertia_(const Mat33P &m)
Construct a UnitInertia from a 3x3 symmetric matrix.
Definition: MassProperties.h:716
Inertia_ & shiftFromMassCenterInPlace(const Vec3P &p, const RealP &mass)
Assuming that the current inertia I is a central inertia (that is, it is inertia about the body cente...
Definition: MassProperties.h:360
SpatialInertia_ & transformInPlace(const InverseTransform_< P > &X_FB)
Transform using an inverse transform to avoid having to convert it.
Definition: MassProperties.h:1155
SpatialInertia_ & operator/=(const RealP &s)
Divide a SpatialInertia by a scalar.
Definition: MassProperties.h:1071
This class contains the mass, center of mass, and unit inertia matrix of a rigid body B...
Definition: MassProperties.h:85
MassProperties_ & setMassProperties(const RealP &m, const Vec3P &com, const InertiaP &inertia)
Set mass, center of mass, and inertia.
Definition: MassProperties.h:1387
MassProperties_< double > dMassProperties
Rigid body mass properties at double precision.
Definition: MassProperties.h:109
const UnitInertia_< P > & getAsUnitInertia() const
Definition: MassProperties.h:517
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
static Inertia_ sphere(const RealP &r)
Create a UnitInertia matrix for a unit mass sphere of radius r centered at the origin.
Definition: MassProperties.h:937
const UnitInertiaP & getUnitInertia() const
Return the unit inertia currently stored in this MassProperties object; this is expressed in an impli...
Definition: MassProperties.h:1417
static Inertia_ ellipsoid(const RealP &hx, const RealP &hy, const RealP &hz)
Unit-mass ellipsoid given by half-lengths in each direction.
Definition: MassProperties.h:955
ArticulatedInertia_ & setMass(const SymMat33P &mass)
Set the mass distribution matrix M in this ArticulatedInertia (symmetric).
Definition: MassProperties.h:1264
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:774
Inertia_(const SymMat33P &inertia)
Construct an Inertia from a symmetric 3x3 matrix.
Definition: MassProperties.h:247
Inertia_ reexpress(const InverseRotation_< P > &R_FB) const
Rexpress using an inverse rotation to avoid having to convert it.
Definition: MassProperties.h:380
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:716
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:1107
MassProperties_ calcTransformedMassProps(const TransformP &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:1469
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:387
static bool isValidInertiaMatrix(const SymMat33P &m)
Test some conditions that must hold for a valid Inertia matrix.
Definition: MassProperties.h:432
UnitInertia_ & reexpressInPlace(const InverseRotation_< P > &R_FB)
Rexpress using an inverse rotation to avoid having to convert it.
Definition: MassProperties.h:829
UnitInertia_< P > & updAsUnitInertia()
Definition: MassProperties.h:519
Inertia_ shiftFromMassCenter(const Vec3P &p, const RealP &mass) const
Assuming that the current inertia I is a central inertia (that is, it is inertia about the body cente...
Definition: MassProperties.h:347
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:1147
UnitInertia_(const RealP &xx, const RealP &yy, const RealP &zz)
Create a principal unit inertia matrix (only non-zero on diagonal).
Definition: MassProperties.h:700
SpatialVecP operator*(const SpatialVecP &v) const
Multiply an ArticulatedIneria by a SpatialVec (66 flops).
Definition: MassProperties.h:1289
bool isFinite(const negator< float > &x)
Definition: negator.h:287
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:582
const Vec3P & getMassCenter() const
Definition: MassProperties.h:1026
static UnitInertia_ cylinderAlongX(const RealP &r, const RealP &hx)
Unit-mass cylinder aligned along x axis; use radius and half-length.
Definition: MassProperties.h:893
bool isInf() const
Definition: MassProperties.h:415
static Inertia_ cylinderAlongX(const RealP &r, const RealP &hx)
Unit-mass cylinder aligned along x axis; use radius and half-length.
Definition: MassProperties.h:946
const SymMat33P & getInertia() const
Get the mass second moment (inertia) matrix J from this ArticulatedInertia (symmetric).
Definition: MassProperties.h:1275
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:364
This is a fixed-length column vector designed for no-overhead inline computation. ...
Definition: SimTKcommon/include/SimTKcommon/internal/common.h:605
const Complex I
We only need one complex constant, i = sqrt(-1). For the rest just multiply the real constant by i...
SpatialInertia_< Real > SpatialInertia
A spatial (rigid body) inertia matrix at default precision.
Definition: MassProperties.h:112
SpatialVecP operator*(const SpatialVecP &v) const
Multiply a SpatialInertia by a SpatialVec to produce a SpatialVec result; 45 flops.
Definition: MassProperties.h:1075
SpatialInertia_< P > operator+(const SpatialInertia_< P > &l, const SpatialInertia_< P > &r)
Add two compatible spatial inertias.
Definition: MassProperties.h:1178
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:875
SymMat33P I_OF_F
Definition: MassProperties.h:565
const SpatialMatP toSpatialMat() const
Definition: MassProperties.h:1161
const InertiaP calcInertia() const
Return the inertia matrix for this MassProperties object; this is equal to the unit inertia times the...
Definition: MassProperties.h:1423
UnitInertia_(const SymMat33P &m)
Construct a UnitInertia from a symmetric 3x3 matrix.
Definition: MassProperties.h:711
static Inertia_ brick(const RealP &hx, const RealP &hy, const RealP &hz)
Unit-mass brick given by half-lengths in each direction.
Definition: MassProperties.h:949
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:696
const Inertia_< P > & asUnitInertia() const
Recast this UnitInertia matrix as a unit inertia matrix.
Definition: MassProperties.h:839
SpatialInertia_ & shiftInPlace(const Vec3P &S)
Change origin from OF to OF+S, modifying the original object in place.
Definition: MassProperties.h:1114
const TAsVec & getAsVec() const
Definition: SymMat.h:831
SpatialInertia_(RealP mass, const Vec3P &com, const UnitInertiaP &gyration)
Definition: MassProperties.h:1011
Inertia_< P > operator*(const Inertia_< P > &i, const P &r)
Multiply an inertia matrix by a scalar.
Definition: MassProperties.h:588
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:1255
Inertia_(const Vec3P &p, const RealP &mass)
Create an Inertia matrix for a point mass at a given location, measured from the origin OF of the imp...
Definition: MassProperties.h:218
Inertia_ & operator*=(const P &s)
Multiply this inertia matrix by a scalar. Cost is 6 flops.
Definition: MassProperties.h:305
const InertiaP getInertia() const
OBSOLETE – this is just here for compatibility with 2.2; since the UnitInertia is stored now the ful...
Definition: MassProperties.h:1428
#define SimTK_ERRCHK1(cond, whereChecked, fmt, a1)
Definition: ExceptionMacros.h:326
ArticulatedInertia_ & operator-=(const ArticulatedInertia_ &src)
Subtract a compatible ArticulatedInertia from this one.
Definition: MassProperties.h:1285
This is a user-includable header which includes everything needed to make use of SimMatrix Scalar cod...
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:727
static Inertia_ cylinderAlongZ(const RealP &r, const RealP &hz)
Unit-mass cylinder aligned along z axis; use radius and half-length.
Definition: MassProperties.h:940
MassProperties_ reexpress(const RotationP &R_BC) const
Re-express these mass properties from the current frame "B" to a new frame "C", given the orientation...
Definition: MassProperties.h:1481
ArticulatedInertia_ & operator+=(const ArticulatedInertia_ &src)
Add in a compatible ArticulatedInertia to this one.
Definition: MassProperties.h:1281
bool isNearlyCentral(const RealP &tol=SignificantReal) const
Return true if the mass center stored here is zero to within a small tolerance.
Definition: MassProperties.h:1507
SpatialInertia_ & operator-=(const SpatialInertia_ &src)
Subtract off a compatible SpatialInertia.
Definition: MassProperties.h:1055
ELEM max(const VectorBase< ELEM > &v)
Definition: VectorMath.h:251
SpatialInertia_< float > fSpatialInertia
A spatial (rigid body) inertia matrix at float precision.
Definition: MassProperties.h:114
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
Inertia_(const Mat33P &m)
Construct an Inertia matrix from a 3x3 symmetric matrix.
Definition: MassProperties.h:253
const Vec3P & getMoments() const
Obtain the inertia moments (diagonal of the Inertia matrix) as a Vec3 ordered xx, yy...
Definition: MassProperties.h:409
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:1083
UnitInertia Gyration
For backwards compatibility only; use UnitInertia instead.
Definition: MassProperties.h:126
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:1294
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:1130
MassProperties_< Real > MassProperties
Rigid body mass properties at default precision.
Definition: MassProperties.h:105
#define SimTK_ERRCHK(cond, whereChecked, msg)
Definition: ExceptionMacros.h:324
InertiaP calcTransformedInertia(const TransformP &X_BC) const
Return the inertia of this MassProperties object, but transformed to from the implicit B frame to a n...
Definition: MassProperties.h:1447
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:901
void errChk(const char *methodName) const
Definition: MassProperties.h:526
This file is the user-includeable header to be included in user programs to provide fixed-length Vec ...
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:870
Inertia_(const RealP &moment)
Create a principal inertia matrix with identical diagonal elements, like a sphere where moment=2/5 m ...
Definition: MassProperties.h:212
const SpatialMatP toSpatialMat() const
Convert the compactly-stored ArticulatedInertia (21 elements) into a full SpatialMat with 36 elements...
Definition: MassProperties.h:1319
ArticulatedInertia_< P > operator+(const ArticulatedInertia_< P > &l, const ArticulatedInertia_< P > &r)
Add two compatible articulated inertias.
Definition: MassProperties.h:1332
InertiaP calcShiftedInertia(const Vec3P &newOriginB) const
Return the inertia of this MassProperties object, but with the "measured about" point shifted from th...
Definition: MassProperties.h:1440
Inertia_< P > operator*(int r, const Inertia_< P > &i)
Multiply an inertia matrix by a scalar given as an int.
Definition: MassProperties.h:609
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:573
MassProperties_ calcShiftedMassProps(const Vec3P &newOriginB) const
Return a new MassProperties object that is the same as this one but with the origin point shifted fro...
Definition: MassProperties.h:1455
MassProperties_(const RealP &m, const Vec3P &com, const UnitInertiaP &gyration)
Create a mass properties object from individually supplied mass, mass center, and unit inertia (gyrat...
Definition: MassProperties.h:1381
Inertia_< P > operator*(const Inertia_< P > &i, int r)
Multiply an inertia matrix by a scalar given as an int.
Definition: MassProperties.h:602
RowVectorBase< typename CNT< ELEM >::TAbs > abs(const RowVectorBase< ELEM > &v)
Definition: VectorMath.h:120
bool isNaN(const negator< float > &x)
Definition: negator.h:273
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:740
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
SpatialInertia_ & setMassCenter(const Vec3P &com)
Definition: MassProperties.h:1020
const Mat33P & getMassMoment() const
Get the mass first moment distribution matrix F from this ArticulatedInertia (full).
Definition: MassProperties.h:1273
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
The physical meaning of an inertia is the distribution of a rigid body's mass about a particular poin...
Definition: MassProperties.h:82
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:1501
Inertia_ & reexpressInPlace(const InverseRotation_< P > &R_FB)
Rexpress in place using an inverse rotation to avoid having to convert it.
Definition: MassProperties.h:392
ArticulatedInertia_< float > fArticulatedInertia
An articulated body inertia matrix at float precision.
Definition: MassProperties.h:121
This is a fixed-length row vector designed for no-overhead inline computation.
Definition: SimTKcommon/include/SimTKcommon/internal/common.h:606
bool isInf(const negator< float > &x)
Definition: negator.h:301
A UnitInertia matrix is a unit-mass inertia matrix; you can convert it to an Inertia by multiplying i...
Definition: MassProperties.h:81
const TDiag & diag() const
Definition: SymMat.h:822
bool isNaN() const
Return true if any element of this MassProperties object is NaN.
Definition: MassProperties.h:1513
const Real SignificantReal
SignificantReal is the smallest value that we consider to be clearly distinct from roundoff error whe...
SpatialInertia_ & operator*=(const RealP &s)
Multiply a SpatialInertia by a scalar.
Definition: MassProperties.h:1067
Inertia_< float > fInertia
An inertia tensor at float precision.
Definition: MassProperties.h:100
Inertia_ & operator-=(const Inertia_ &inertia)
Subtract off another inertia matrix.
Definition: MassProperties.h:299
const TCol & col(int j) const
Definition: Mat.h:775
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
UnitInertia_< float > fUnitInertia
A unit inertia (gyration) tensor at float precision.
Definition: MassProperties.h:93
static Inertia_ pointMassAt(const Vec3P &p, const RealP &m)
Create an Inertia matrix for a point of a given mass, located at a given location measured from the o...
Definition: MassProperties.h:462
ArticulatedInertia_ & setInertia(const SymMat33P &inertia)
Set the mass second moment (inertia) matrix J in this ArticulatedInertia (symmetric).
Definition: MassProperties.h:1268
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
RealP getMass() const
Definition: MassProperties.h:1025
UnitInertia_ & setUnitInertia(const Vec3P &moments, const Vec3P &products=Vec3P(0))
Set principal moments and optionally off-diagonal terms.
Definition: MassProperties.h:732
SpatialInertia_< P > operator-(const SpatialInertia_< P > &l, const SpatialInertia_< P > &r)
Subtract one compatible spatial inertia from another.
Definition: MassProperties.h:1185
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:705
static UnitInertia_ ellipsoid(const Vec3P &halfLengths)
Alternate interface to ellipsoid() that takes a Vec3 for the half lengths.
Definition: MassProperties.h:918
SpatialInertia_ & setUnitInertia(const UnitInertiaP &gyration)
Definition: MassProperties.h:1022
static Inertia_ pointMassAtOrigin()
Create an Inertia matrix for a point located at the origin – that is, an all-zero matrix...
Definition: MassProperties.h:456
A spatial inertia contains the mass, center of mass point, and inertia matrix for a rigid body...
Definition: MassProperties.h:83
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
SpatialInertia_ & operator+=(const SpatialInertia_ &src)
Add in a compatible SpatialInertia.
Definition: MassProperties.h:1041
ArticulatedInertia_< P > operator-(const ArticulatedInertia_< P > &l, const ArticulatedInertia_< P > &r)
Subtract one compatible articulated inertia from another.
Definition: MassProperties.h:1339
UnitInertia_< double > dUnitInertia
A unit inertia (gyration) tensor at double precision.
Definition: MassProperties.h:95
SpatialInertia_ & setMass(RealP mass)
Definition: MassProperties.h:1016
static bool isValidUnitInertiaMatrix(const SymMat33P &m)
Test some conditions that must hold for a valid UnitInertia matrix.
Definition: MassProperties.h:851
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:798
Inertia_< double > dInertia
An inertia tensor at double precision.
Definition: MassProperties.h:102
MassProperties_()
Create a mass properties object in which the mass, mass center, and inertia are meaningless; you must...
Definition: MassProperties.h:1373
bool isNaN() const
Return true if any element of this SymMat contains a NaN anywhere.
Definition: SymMat.h:735
An articulated body inertia (ABI) matrix P(q) contains the spatial inertia properties that a body app...
Definition: MassProperties.h:84
Inertia_ & operator/=(const P &s)
Divide this inertia matrix by a scalar.
Definition: MassProperties.h:309
This class represents a small matrix whose size is known at compile time, containing elements of any ...
Definition: SimTKcommon/include/SimTKcommon/internal/common.h:607
InertiaP calcInertia() const
Calculate the inertia matrix (second mass moment, mass-weighted gyration matrix) from the mass and un...
Definition: MassProperties.h:1035
Mat33P toMat33() const
Expand the internal packed representation into a full 3x3 symmetric matrix with all elements set...
Definition: MassProperties.h:405
const Vec3P & getProducts() const
Obtain the inertia products (off-diagonals of the Inertia matrix) as a Vec3 with elements ordered xy...
Definition: MassProperties.h:412
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:427
Mat66P toMat66() const
Convert this MassProperties object to a spatial inertia matrix in the form of an ordinary 6x6 matrix...
Definition: MassProperties.h:1546
ArticulatedInertia_ & setMassMoment(const Mat33P &massMoment)
Set the mass first moment distribution matrix F in this ArticulatedInertia (full).
Definition: MassProperties.h:1266
Inertia_ shiftToMassCenter(const Vec3P &CF, const RealP &mass) const
Assume that the current inertia is about the F frame's origin OF, and expressed in F...
Definition: MassProperties.h:320
Inertia_ & setInertia(const Vec3P &moments, const Vec3P &products=Vec3P(0))
Set principal moments and optionally off-diagonal terms.
Definition: MassProperties.h:270
UnitInertia_< Real > UnitInertia
A unit inertia (gyration) tensor at default precision.
Definition: MassProperties.h:85
Vec3P calcMassMoment() const
Calculate the first mass moment (mass-weighted COM location) from the mass and COM vector...
Definition: MassProperties.h:1031
UnitInertia_()
Default is a NaN-ed out mess to avoid accidents, even in Release mode.
Definition: MassProperties.h:683
Inertia_()
Default is a NaN-ed out mess to avoid accidents, even in Release mode.
Definition: MassProperties.h:202
Inertia_(const RealP &xx, const RealP &yy, const RealP &zz, const RealP &xy, const RealP &xz, const RealP &yz)
This is a general inertia matrix.
Definition: MassProperties.h:238
MassProperties_(const RealP &m, const Vec3P &com, const InertiaP &inertia)
Create a mass properties object from individually supplied mass, mass center, and inertia matrix...
Definition: MassProperties.h:1377
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:812
#define SimTK_ERRCHK3(cond, whereChecked, fmt, a1, a2, a3)
Definition: ExceptionMacros.h:330
Inertia_< Real > Inertia
An inertia tensor at default precision.
Definition: MassProperties.h:98
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:785
static UnitInertia_ pointMassAtOrigin()
Create a UnitInertia matrix for a point located at the origin – that is, an all-zero matrix...
Definition: MassProperties.h:864
static UnitInertia_ cylinderAlongY(const RealP &r, const RealP &hy)
Unit-mass cylinder aligned along y axis; use radius and half-length.
Definition: MassProperties.h:886
RealP trace() const
Definition: MassProperties.h:395
SpatialInertia_()
The default constructor fills everything with NaN, even in Release mode.
Definition: MassProperties.h:1009
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:375