1#ifndef SimTK_SIMMATRIX_MASS_PROPERTIES_H_
2#define SimTK_SIMMATRIX_MASS_PROPERTIES_H_
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; }
391P
trace()
const {
return I_OF_F.trace();}
410bool isNaN()
const {
return I_OF_F.isNaN();}
411bool 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;
434 const P Slop = std::max(d.
sum(),P(1))
437 if (!(d[0]+d[1]+Slop>=d[2] && d[0]+d[2]+Slop>=d[1] && d[1]+d[2]+Slop>=d[0]))
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])))
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] );
478inline static Inertia_ sphere(
const P& r);
482inline static Inertia_ cylinderAlongZ(
const P& r,
const P& hz);
486inline static Inertia_ cylinderAlongY(
const P& r,
const P& hy);
490inline static Inertia_ cylinderAlongX(
const P& r,
const P& hx);
495inline static Inertia_ brick(
const P& hx,
const P& hy,
const P& hz);
501inline static Inertia_ ellipsoid(
const P& hx,
const P& hy,
const P& hz);
522void 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);
539 const P Slop = std::max(d.
sum(),P(1))
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);
552 && Iyy+Slop>=std::abs(2*Ixz)
553 && Izz+Slop>=std::abs(2*Ixy),
555 "The magnitude of a product of inertia was too large to be physical.");
627{
return I.asSymMat33() * w; }
633template <
class P>
inline bool
640template <
class P>
inline std::ostream&
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; }
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]); }
924void operator+=(
int) {}
925void operator-=(
int) {}
926void operator*=(
int) {}
927void operator/=(
int) {}
945brick(
const P& hx,
const P& hy,
const P& hz)
951ellipsoid(
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());
1173template <
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);
1335template <
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;
1564template <
class P>
static inline std::ostream&
1566 return o <<
"{ mass=" << mp.
getMass()
#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
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
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
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