1 #ifndef SimTK_SIMMATRIX_MASS_PROPERTIES_H_ 2 #define SimTK_SIMMATRIX_MASS_PROPERTIES_H_ 208 explicit Inertia_(
const P& moment) : I_OF_F(moment)
209 { errChk(
"Inertia::Inertia(moment)"); }
221 { I_OF_F.updDiag() = moments;
222 I_OF_F.updLower() = products;
223 errChk(
"Inertia::Inertia(moments,products)"); }
230 errChk(
"Inertia::setInertia(xx,yy,zz)"); }
235 const P& xy,
const P& xz,
const P& yz)
239 errChk(
"Inertia::setInertia(xx,yy,zz,xy,xz,yz)"); }
244 { errChk(
"Inertia::Inertia(SymMat33)"); }
251 "Inertia(Mat33)",
"The supplied matrix was not symmetric.");
253 errChk(
"Inertia(Mat33)"); }
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)");
267 I_OF_F.updDiag() = moments;
268 I_OF_F.updLower() = products;
269 errChk(
"Inertia::setInertia(moments,products)");
279 const P& xy,
const P& xz,
const P& yz) {
281 errChk(
"Inertia::setInertia(xx,yy,zz,xy,xz,yz)");
289 { I_OF_F += inertia.
I_OF_F;
290 errChk(
"Inertia::operator+=()");
296 { I_OF_F -= inertia.
I_OF_F;
297 errChk(
"Inertia::operator-=()");
317 {
Inertia_ I(*
this);
I -= pointMassAt(CF, mass);
318 I.errChk(
"Inertia::shiftToMassCenter()");
332 { (*this) -= pointMassAt(CF, mass);
333 errChk(
"Inertia::shiftToMassCenterInPlace()");
344 {
Inertia_ I(*
this);
I += pointMassAt(p, mass);
345 I.errChk(
"Inertia::shiftFromMassCenter()");
357 { (*this) += pointMassAt(p, mass);
358 errChk(
"Inertia::shiftFromMassCenterInPlace()");
372 {
return Inertia_((~R_FB).reexpressSymMat33(I_OF_F)); }
377 {
return Inertia_((~R_FB).reexpressSymMat33(I_OF_F)); }
384 { I_OF_F = (~R_FB).reexpressSymMat33(I_OF_F);
return *
this; }
389 { I_OF_F = (~R_FB).reexpressSymMat33(I_OF_F);
return *
this; }
391 P
trace()
const {
return I_OF_F.trace();}
410 bool isNaN()
const {
return I_OF_F.isNaN();}
411 bool isInf()
const {
return I_OF_F.isInf();}
418 {
return I_OF_F.isNumericallyEqual(other.
I_OF_F); }
424 {
return I_OF_F.isNumericallyEqual(other.
I_OF_F, tol); }
429 if (m.
isNaN())
return false;
432 if (!(d >= 0))
return false;
437 if (!(d[0]+d[1]+Slop>=d[2] && d[0]+d[2]+Slop>=d[1] && d[1]+d[2]+Slop>=d[0]))
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] );
478 inline static Inertia_ sphere(
const P& r);
482 inline static Inertia_ cylinderAlongZ(
const P& r,
const P& hz);
486 inline static Inertia_ cylinderAlongY(
const P& r,
const P& hy);
490 inline static Inertia_ cylinderAlongX(
const P& r,
const P& hx);
495 inline static Inertia_ brick(
const P& hx,
const P& hy,
const P& hz);
501 inline static Inertia_ ellipsoid(
const P& hx,
const P& hy,
const P& hz);
522 void errChk(
const char* methodName)
const {
525 "Inertia matrix contains a NaN.");
527 const Vec<3,P>& d = I_OF_F.getDiag();
528 const Vec<3,P>& p = I_OF_F.getLower();
529 const P Ixx = d[0], Iyy = d[1], Izz = d[2];
530 const P Ixy = p[0], Ixz = p[1], Iyz = p[2];
533 "Diagonals of an Inertia matrix must be nonnegative; got %g,%g,%g.",
534 (
double)Ixx,(
double)Iyy,(
double)Izz);
544 && Iyy+Izz+Slop>=Ixx,
546 "Diagonals of an Inertia matrix must satisfy the triangle " 547 "inequality; got %g,%g,%g.",
548 (
double)Ixx,(
double)Iyy,(
double)Izz);
555 "The magnitude of a product of inertia was too large to be physical.");
627 {
return I.asSymMat33() * w; }
633 template <
class P>
inline bool 640 template <
class P>
inline std::ostream&
641 operator<<(std::ostream& o, const Inertia_<P>& inertia)
642 {
return o << inertia.toMat33(); }
702 const RealP& xy,
const RealP& xz,
const RealP& yz)
724 { InertiaP::setInertia(xx,yy,zz);
return *
this; }
729 { InertiaP::setInertia(moments,products);
return *
this; }
737 const RealP& xy,
const RealP& xz,
const RealP& yz)
738 { InertiaP::setInertia(xx,yy,zz,xy,xz,yz);
return *
this; }
755 G.
Inertia_<P>::operator-=(pointMassAt(CF));
771 { InertiaP::operator-=(pointMassAt(CF));
783 G.
Inertia_<P>::operator+=(pointMassAt(p));
795 { InertiaP::operator+=(pointMassAt(p));
809 {
return UnitInertia_((~R_FB).reexpressSymMat33(this->I_OF_F)); }
814 {
return UnitInertia_((~R_FB).reexpressSymMat33(this->I_OF_F)); }
821 { InertiaP::reexpressInPlace(R_FB);
return *
this; }
826 { InertiaP::reexpressInPlace(R_FB);
return *
this; }
830 operator const SymMat33P&()
const {
return this->I_OF_F;}
876 const RealP Ixx = (r*r)/4 + (hz*hz)/3;
883 const RealP Ixx = (r*r)/4 + (hy*hy)/3;
890 const RealP Iyy = (r*r)/4 + (hx*hx)/3;
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));
905 {
return brick(halfLengths[0],halfLengths[1],halfLengths[2]); }
909 const RealP hx2=hx*hx, hy2=hy*hy, hz2=hz*hz;
910 return UnitInertia_((hy2+hz2)/5, (hx2+hz2)/5, (hx2+hy2)/5);
915 {
return ellipsoid(halfLengths[0],halfLengths[1],halfLengths[2]); }
924 void operator+=(
int) {}
925 void operator-=(
int) {}
926 void operator*=(
int) {}
927 void operator/=(
int) {}
945 brick(
const P& hx,
const P& hy,
const P& hz)
1006 : m(nanP()), p(nanP()) {}
1008 : m(mass), p(com), G(gyration) {}
1014 "Negative mass %g is illegal.", (
double)mass);
1015 m=mass;
return *
this; }
1017 { p=com;
return *
this;}
1019 { G=gyration;
return *
this; }
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;
1042 G.setFromUnitInertia(oomtot*(calcInertia()+src.
calcInertia()));
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;
1056 G.setFromUnitInertia(oomtot*(calcInertia()-src.
calcInertia()));
1072 {
return m*
SpatialVecP(G*v[0]+p%v[1], v[1]-p%v[0]); }
1111 G.shiftToCentroidInPlace(p);
1112 const Vec3P pNew(p-S);
1113 G.shiftFromCentroidInPlace(pNew);
1144 shiftInPlace(X_FB.
p());
1145 reexpressInPlace(X_FB.
R());
1152 shiftInPlace(X_FB.
p());
1153 reexpressInPlace(X_FB.
R());
1173 template <
class P>
inline SpatialInertia_<P>
1252 : M(mass), J(inertia), F(massMoment) {}
1257 : M(rbi.
getMass()), J(rbi.calcInertia()), F(
crossMat(rbi.calcMassMoment())) {}
1278 { M+=src.M; J+=src.J; F+=src.F;
return *
this; }
1282 { M-=src.M; J-=src.J; F-=src.F;
return *
this; }
1286 {
return SpatialVecP(J*v[0]+F*v[1], ~F*v[0]+M*v[1]); }
1292 for (
int j=0; j < N; ++j)
1293 res.
col(j) = (*this) * m.
col(j);
1323 smat(0,0) =
Mat33P(J); smat(0,1) = F;
1324 smat(1,0) = ~F; smat(1,1) =
Mat33P(M);
1335 template <
class P>
inline ArticulatedInertia_<P>
1372 { setMassProperties(m,com,inertia); }
1376 { setMassProperties(m,com,gyration); }
1398 { mass=m; comInB=com; unitInertia_OB_B=gyration;
return *
this; }
1428 return mass*unitInertia_OB_B -
Inertia_<P>(comInB, mass);
1435 return calcCentralInertia() +
Inertia_<P>(newOriginB-comInB, mass);
1442 return calcShiftedInertia(X_BC.
p()).reexpress(X_BC.
R());
1451 calcShiftedInertia(newOriginB));
1464 return MassProperties_(mass, ~X_BC*comInB, calcTransformedInertia(X_BC));
1476 return MassProperties_(mass, ~R_BC*comInB, unitInertia_OB_B.reexpress(R_BC));
1502 return comInB.normSqr() <= tol*tol;
1513 if (
isNaN())
return false;
1514 return SimTK::isInf(mass) || comInB.isInf() || unitInertia_OB_B.isInf();
1520 return SimTK::isFinite(mass) && comInB.isFinite() && unitInertia_OB_B.isFinite();
1528 M(0,0) = mass*unitInertia_OB_B.toMat33();
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;
1564 template <
class P>
static inline std::ostream&
1565 operator<<(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()
1575 #endif // SimTK_SIMMATRIX_MASS_PROPERTIES_H_ 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
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:612
static bool isValidInertiaMatrix(const SymMat< 3, P > &m)
Test some conditions that must hold for a valid Inertia matrix.
Definition: MassProperties.h:428
const UnitInertia_< P > & getAsUnitInertia() const
Definition: MassProperties.h:513
RealP getMass() const
Definition: MassProperties.h:1021
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
Vec< 2, Vec3 > SpatialVec
Spatial vectors are used for (rotation,translation) quantities and consist of a pair of Vec3 objects...
Definition: MassProperties.h:50
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
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
const SymMat33P & getMass() const
Get the mass distribution matrix M from this ArticulatedInertia (symmetric).
Definition: MassProperties.h:1267
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
SpatialVecP operator*(const SpatialVecP &v) const
Multiply an ArticulatedIneria by a SpatialVec (66 flops).
Definition: MassProperties.h:1285
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:224
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
UnitInertia_ & setFromUnitInertia(const Inertia_< P > &inertia)
Set from a unit inertia matrix.
Definition: MassProperties.h:840
(Advanced) This InverseRotation class is the inverse of a Rotation.
Definition: Rotation.h:47
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
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
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
bool operator==(const Inertia_< P > &i1, const Inertia_< P > &i2)
Compare two inertia matrices for exact (bitwise) equality.
Definition: MassProperties.h:634
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
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
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
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:718
Inertia_ & operator+=(const Inertia_ &inertia)
Add in another inertia matrix.
Definition: MassProperties.h:288
SpatialInertia_ & reexpressInPlace(const Rotation_< P > &R_FB)
Re-express this SpatialInertia in another frame, modifying the original object.
Definition: MassProperties.h:1091
MassProperties_< float > fMassProperties
Rigid body mass properties at float precision.
Definition: MassProperties.h:107
This is the top-level SimTK namespace into which all SimTK names are placed to avoid collision with o...
Definition: Assembler.h:37
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
static UnitInertia_ brick(const Vec3P &halfLengths)
Alternate interface to brick() that takes a Vec3 for the half lengths.
Definition: MassProperties.h:904
Inertia_< P > calcCentralInertia() const
Return the inertia of this MassProperties object, but measured about the mass center rather than abou...
Definition: MassProperties.h:1427
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
Inertia_< P > operator*(const P &r, const Inertia_< P > &i)
Multiply an inertia matrix by a scalar.
Definition: MassProperties.h:590
These are numerical utility classes for dealing with the relative orientations of geometric objects...
ArticulatedInertia_ & shiftInPlace(const Vec3P &s)
Rigid-shift this ABI in place by -s.
P trace() const
Definition: MassProperties.h:391
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
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
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
The Rotation class is a Mat33 that guarantees that the matrix can be interpreted as a legitimate 3x3 ...
Definition: Quaternion.h:40
const SpatialMatP toSpatialMat() const
Definition: MassProperties.h:1157
Inertia_< P > operator/(const Inertia_< P > &i, int r)
Divide an inertia matrix by a scalar provided as an int.
Definition: MassProperties.h:619
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 Mat< 3, 3, P > &m)
Construct an Inertia matrix from a 3x3 symmetric matrix.
Definition: MassProperties.h:249
const TCol & col(int j) const
Definition: Mat.h:777
const SpatialMatP toSpatialMat() const
Convert the compactly-stored ArticulatedInertia (21 elements) into a full SpatialMat with 36 elements...
Definition: MassProperties.h:1315
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
SpatialInertia_ & reexpressInPlace(const InverseRotation_< P > &R_FB)
Rexpress using an inverse rotation to avoid having to convert it.
Definition: MassProperties.h:1096
ArticulatedInertia_()
Default construction produces uninitialized junk at zero cost; be sure to fill this in before referen...
Definition: MassProperties.h:1249
#define SimTK_ERRCHK1(cond, whereChecked, fmt, a1)
Definition: ExceptionMacros.h:326
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
UnitInertia_(const RealP &moment)
Create a principal unit inertia matrix with identical diagonal elements.
Definition: MassProperties.h:686
#define SimTK_ASSERT(cond, msg)
Definition: ExceptionMacros.h:373
ArticulatedInertia_(const SpatialInertia_< P > &rbi)
Construct an articulated body inertia (ABI) from a rigid body spatial inertia (RBI).
Definition: MassProperties.h:1256
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_ & 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
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_(const Mat33P &m)
Construct a UnitInertia from a 3x3 symmetric matrix.
Definition: MassProperties.h:712
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
SpatialInertia_ & transformInPlace(const InverseTransform_< P > &X_FB)
Transform using an inverse transform to avoid having to convert it.
Definition: MassProperties.h:1151
SpatialInertia_ & operator/=(const RealP &s)
Divide a SpatialInertia by a scalar.
Definition: MassProperties.h:1067
#define SimTK_ERRCHK3(cond, whereChecked, fmt, a1, a2, a3)
Definition: ExceptionMacros.h:330
Inertia_(const P &xx, const P &yy, const P &zz)
Create a principal inertia matrix (only non-zero on diagonal).
Definition: MassProperties.h:226
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
This class contains the mass, center of mass, and unit inertia matrix of a rigid body B...
Definition: MassProperties.h:85
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_< double > dMassProperties
Rigid body mass properties at double precision.
Definition: MassProperties.h:109
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
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
ArticulatedInertia_ & setMass(const SymMat33P &mass)
Set the mass distribution matrix M in this ArticulatedInertia (symmetric).
Definition: MassProperties.h:1260
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
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
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
SpatialVecP operator*(const SpatialVecP &v) const
Multiply a SpatialInertia by a SpatialVec to produce a SpatialVec result; 45 flops.
Definition: MassProperties.h:1071
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
const P & getMass() const
Return the mass currently stored in this MassProperties object.
Definition: MassProperties.h:1401
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
UnitInertia_ & reexpressInPlace(const InverseRotation_< P > &R_FB)
Rexpress using an inverse rotation to avoid having to convert it.
Definition: MassProperties.h:825
const TAsVec & getAsVec() const
Definition: SymMat.h:831
UnitInertia_< P > & updAsUnitInertia()
Definition: MassProperties.h:515
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
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
bool isFinite(const negator< float > &x)
Definition: negator.h:285
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
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 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
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
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
bool isInf() const
Definition: MassProperties.h:411
InertiaP calcInertia() const
Calculate the inertia matrix (second mass moment, mass-weighted gyration matrix) from the mass and un...
Definition: MassProperties.h:1031
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
const Inertia_< P > & asUnitInertia() const
Recast this UnitInertia matrix as a unit inertia matrix.
Definition: MassProperties.h:835
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
const SymMat33P & getInertia() const
Get the mass second moment (inertia) matrix J from this ArticulatedInertia (symmetric).
Definition: MassProperties.h:1271
SpatialInertia_< P > operator+(const SpatialInertia_< P > &l, const SpatialInertia_< P > &r)
Add two compatible spatial inertias.
Definition: MassProperties.h:1174
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
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
SpatialInertia_ transform(const InverseTransform_< P > &X_FB) const
Transform using an inverse transform to avoid having to convert it.
Definition: MassProperties.h:1131
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
UnitInertia_(const SymMat33P &m)
Construct a UnitInertia from a symmetric 3x3 matrix.
Definition: MassProperties.h:707
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
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
SpatialInertia_ & shiftInPlace(const Vec3P &S)
Change origin from OF to OF+S, modifying the original object in place.
Definition: MassProperties.h:1110
SpatialInertia_(RealP mass, const Vec3P &com, const UnitInertiaP &gyration)
Definition: MassProperties.h:1007
Inertia_< P > operator*(const Inertia_< P > &i, const P &r)
Multiply an inertia matrix by a scalar.
Definition: MassProperties.h:584
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
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
Inertia_ & operator*=(const P &s)
Multiply this inertia matrix by a scalar. Cost is 6 flops.
Definition: MassProperties.h:301
ArticulatedInertia_ & operator-=(const ArticulatedInertia_ &src)
Subtract a compatible ArticulatedInertia from this one.
Definition: MassProperties.h:1281
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:723
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
ArticulatedInertia_ & operator+=(const ArticulatedInertia_ &src)
Add in a compatible ArticulatedInertia to this one.
Definition: MassProperties.h:1277
SpatialInertia_ & operator-=(const SpatialInertia_ &src)
Subtract off a compatible SpatialInertia.
Definition: MassProperties.h:1051
ELEM max(const VectorBase< ELEM > &v)
Definition: VectorMath.h:251
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
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
UnitInertia Gyration
For backwards compatibility only; use UnitInertia instead.
Definition: MassProperties.h:126
const TLower & getLower() const
Definition: SymMat.h:825
Inertia_ reexpress(const InverseRotation_< P > &R_FB) const
Rexpress using an inverse rotation to avoid having to convert it.
Definition: MassProperties.h:376
MassProperties_< Real > MassProperties
Rigid body mass properties at default precision.
Definition: MassProperties.h:105
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
Inertia_(const SymMat< 3, P > &inertia)
Construct an Inertia from a symmetric 3x3 matrix.
Definition: MassProperties.h:243
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:866
const Mat33P & getMassMoment() const
Get the mass first moment distribution matrix F from this ArticulatedInertia (full).
Definition: MassProperties.h:1269
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
ArticulatedInertia_< P > operator+(const ArticulatedInertia_< P > &l, const ArticulatedInertia_< P > &r)
Add two compatible articulated inertias.
Definition: MassProperties.h:1336
Inertia_< P > operator*(int r, const Inertia_< P > &i)
Multiply an inertia matrix by a scalar given as an int.
Definition: MassProperties.h:605
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
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
void errChk(const char *methodName) const
Definition: MassProperties.h:522
Inertia_< P > operator*(const Inertia_< P > &i, int r)
Multiply an inertia matrix by a scalar given as an int.
Definition: MassProperties.h:598
RowVectorBase< typename CNT< ELEM >::TAbs > abs(const RowVectorBase< ELEM > &v)
Definition: VectorMath.h:120
bool isNaN(const negator< float > &x)
Definition: negator.h:272
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 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:1519
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
#define SimTK_ERRCHK(cond, whereChecked, msg)
Definition: ExceptionMacros.h:324
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:1016
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 isNaN() const
Definition: MassProperties.h:410
const SymMat< 3, P > & asSymMat33() const
Obtain a reference to the underlying symmetric matrix type.
Definition: MassProperties.h:397
bool isFinite() const
Definition: MassProperties.h:412
UnitInertia_ reexpress(const InverseRotation_< P > &R_FB) const
Rexpress using an inverse rotation to avoid having to convert it.
Definition: MassProperties.h:813
const UnitInertiaP & getUnitInertia() const
Definition: MassProperties.h:1023
Inertia_ & reexpressInPlace(const InverseRotation_< P > &R_FB)
Rexpress in place using an inverse rotation to avoid having to convert it.
Definition: MassProperties.h:388
ArticulatedInertia_< float > fArticulatedInertia
An articulated body inertia matrix at float precision.
Definition: MassProperties.h:121
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
This is a fixed-length row vector designed for no-overhead inline computation.
Definition: SimTKcommon/include/SimTKcommon/internal/common.h:619
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
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
bool isInf(const negator< float > &x)
Definition: negator.h:298
A UnitInertia matrix is a unit-mass inertia matrix; you can convert it to an Inertia by multiplying i...
Definition: MassProperties.h:81
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:1063
const Inertia_< P > getInertia() const
OBSOLETE – this is just here for compatibility with 2.2; since the UnitInertia is stored now the ful...
Definition: MassProperties.h:1422
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:295
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
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
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
ArticulatedInertia_ & setInertia(const SymMat33P &inertia)
Set the mass second moment (inertia) matrix J in this ArticulatedInertia (symmetric).
Definition: MassProperties.h:1264
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
Vec3P calcMassMoment() const
Calculate the first mass moment (mass-weighted COM location) from the mass and COM vector...
Definition: MassProperties.h:1027
UnitInertia_ & setUnitInertia(const Vec3P &moments, const Vec3P &products=Vec3P(0))
Set principal moments and optionally off-diagonal terms.
Definition: MassProperties.h:728
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
SpatialInertia_< P > operator-(const SpatialInertia_< P > &l, const SpatialInertia_< P > &r)
Subtract one compatible spatial inertia from another.
Definition: MassProperties.h:1181
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
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
static UnitInertia_ ellipsoid(const Vec3P &halfLengths)
Alternate interface to ellipsoid() that takes a Vec3 for the half lengths.
Definition: MassProperties.h:914
SpatialInertia_ & setUnitInertia(const UnitInertiaP &gyration)
Definition: MassProperties.h:1018
static Inertia_ pointMassAtOrigin()
Create an Inertia matrix for a point located at the origin – that is, an all-zero matrix...
Definition: MassProperties.h:452
A spatial inertia contains the mass, center of mass point, and inertia matrix for a rigid body...
Definition: MassProperties.h:83
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
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:1037
ArticulatedInertia_< P > operator-(const ArticulatedInertia_< P > &l, const ArticulatedInertia_< P > &r)
Subtract one compatible articulated inertia from another.
Definition: MassProperties.h:1343
UnitInertia_< double > dUnitInertia
A unit inertia (gyration) tensor at double precision.
Definition: MassProperties.h:95
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
SpatialInertia_ & setMass(RealP mass)
Definition: MassProperties.h:1012
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
static bool isValidUnitInertiaMatrix(const SymMat33P &m)
Test some conditions that must hold for a valid UnitInertia matrix.
Definition: MassProperties.h:847
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
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:1367
An articulated body inertia (ABI) matrix P(q) contains the spatial inertia properties that a body app...
Definition: MassProperties.h:84
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...
Inertia_ & operator/=(const P &s)
Divide this inertia matrix by a scalar.
Definition: MassProperties.h:305
This class represents a small matrix whose size is known at compile time, containing elements of any ...
Definition: SimTKcommon/include/SimTKcommon/internal/common.h:620
const Vec3P & getMassCenter() const
Definition: MassProperties.h:1022
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
SymMat< 3, P > I_OF_F
Definition: MassProperties.h:561
ArticulatedInertia_ & setMassMoment(const Mat33P &massMoment)
Set the mass first moment distribution matrix F in this ArticulatedInertia (full).
Definition: MassProperties.h:1262
UnitInertia_< Real > UnitInertia
A unit inertia (gyration) tensor at default precision.
Definition: MassProperties.h:85
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
UnitInertia_()
Default is a NaN-ed out mess to avoid accidents, even in Release mode.
Definition: MassProperties.h:679
Inertia_()
Default is a NaN-ed out mess to avoid accidents, even in Release mode.
Definition: MassProperties.h:198
bool isNaN() const
Return true if any element of this MassProperties object is NaN.
Definition: MassProperties.h:1507
bool isNaN() const
Return true if any element of this SymMat contains a NaN anywhere.
Definition: SymMat.h:735
Inertia_< Real > Inertia
An inertia tensor at default precision.
Definition: MassProperties.h:98
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_ cylinderAlongY(const RealP &r, const RealP &hy)
Unit-mass cylinder aligned along y axis; use radius and half-length.
Definition: MassProperties.h:882
const TDiag & diag() const
Definition: SymMat.h:822
SpatialInertia_ reexpress(const InverseRotation_< P > &R_FB) const
Rexpress using an inverse rotation to avoid having to convert it.
Definition: MassProperties.h:1084
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
SpatialInertia_()
The default constructor fills everything with NaN, even in Release mode.
Definition: MassProperties.h:1005
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