Simbody  3.7
MassProperties.h
Go to the documentation of this file.
1 #ifndef SimTK_SIMMATRIX_MASS_PROPERTIES_H_
2 #define SimTK_SIMMATRIX_MASS_PROPERTIES_H_
3 
4 /* -------------------------------------------------------------------------- *
5  * Simbody(tm): SimTKcommon *
6  * -------------------------------------------------------------------------- *
7  * This is part of the SimTK biosimulation toolkit originating from *
8  * Simbios, the NIH National Center for Physics-Based Simulation of *
9  * Biological Structures at Stanford, funded under the NIH Roadmap for *
10  * Medical Research, grant U54 GM072970. See https://simtk.org/home/simbody. *
11  * *
12  * Portions copyright (c) 2005-14 Stanford University and the Authors. *
13  * Authors: Michael Sherman *
14  * Contributors: Paul Mitiguy *
15  * *
16  * Licensed under the Apache License, Version 2.0 (the "License"); you may *
17  * not use this file except in compliance with the License. You may obtain a *
18  * copy of the License at http://www.apache.org/licenses/LICENSE-2.0. *
19  * *
20  * Unless required by applicable law or agreed to in writing, software *
21  * distributed under the License is distributed on an "AS IS" BASIS, *
22  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. *
23  * See the License for the specific language governing permissions and *
24  * limitations under the License. *
25  * -------------------------------------------------------------------------- */
26 
33 #include "SimTKcommon/Scalar.h"
36 
37 #include <iostream>
38 
39 namespace SimTK {
57 
67 
79 
80 // These are templatized by precision (float or double).
81 template <class P> class UnitInertia_;
82 template <class P> class Inertia_;
83 template <class P> class SpatialInertia_;
84 template <class P> class ArticulatedInertia_;
85 template <class P> class MassProperties_;
86 
87 // The "no trailing underscore" typedefs use whatever the
88 // compile-time precision is set to.
89 
96 
103 
110 
117 
124 
127 
128 // -----------------------------------------------------------------------------
129 // INERTIA MATRIX
130 // -----------------------------------------------------------------------------
192 template <class P>
194  typedef Rotation_<P> RotationP;
195 public:
198 Inertia_() : I_OF_F(NTraits<P>::getNaN()) {}
199 
200 // Default copy constructor, copy assignment, destructor.
201 
208 explicit Inertia_(const P& moment) : I_OF_F(moment)
209 { errChk("Inertia::Inertia(moment)"); }
210 
214 Inertia_(const Vec<3,P>& p, const P& mass) : I_OF_F(pointMassAt(p,mass)) {}
215 
220 explicit Inertia_(const Vec<3,P>& moments, const Vec<3,P>& products=Vec<3,P>(0))
221 { I_OF_F.updDiag() = moments;
222  I_OF_F.updLower() = products;
223  errChk("Inertia::Inertia(moments,products)"); }
224 
226 Inertia_(const P& xx, const P& yy, const P& zz)
227 { I_OF_F = SymMat<3,P>(xx,
228  0, yy,
229  0, 0, zz);
230  errChk("Inertia::setInertia(xx,yy,zz)"); }
231 
234 Inertia_(const P& xx, const P& yy, const P& zz,
235  const P& xy, const P& xz, const P& yz)
236 { I_OF_F = SymMat<3,P>(xx,
237  xy, yy,
238  xz, yz, zz);
239  errChk("Inertia::setInertia(xx,yy,zz,xy,xz,yz)"); }
240 
243 explicit Inertia_(const SymMat<3,P>& inertia) : I_OF_F(inertia)
244 { errChk("Inertia::Inertia(SymMat33)"); }
245 
249 explicit Inertia_(const Mat<3,3,P>& m)
251  "Inertia(Mat33)", "The supplied matrix was not symmetric.");
252  I_OF_F = SymMat<3,P>(m);
253  errChk("Inertia(Mat33)"); }
254 
255 
258 Inertia_& setInertia(const P& xx, const P& yy, const P& zz) {
259  I_OF_F = P(0); I_OF_F(0,0) = xx; I_OF_F(1,1) = yy; I_OF_F(2,2) = zz;
260  errChk("Inertia::setInertia(xx,yy,zz)");
261  return *this;
262 }
263 
266 Inertia_& setInertia(const Vec<3,P>& moments, const Vec<3,P>& products=Vec<3,P>(0)) {
267  I_OF_F.updDiag() = moments;
268  I_OF_F.updLower() = products;
269  errChk("Inertia::setInertia(moments,products)");
270  return *this;
271 }
272 
278 Inertia_& setInertia(const P& xx, const P& yy, const P& zz,
279  const P& xy, const P& xz, const P& yz) {
280  setInertia(Vec<3,P>(xx,yy,zz), Vec<3,P>(xy,xz,yz));
281  errChk("Inertia::setInertia(xx,yy,zz,xy,xz,yz)");
282  return *this;
283 }
284 
285 
288 Inertia_& operator+=(const Inertia_& inertia)
289 { I_OF_F += inertia.I_OF_F;
290  errChk("Inertia::operator+=()");
291  return *this; }
292 
295 Inertia_& operator-=(const Inertia_& inertia)
296 { I_OF_F -= inertia.I_OF_F;
297  errChk("Inertia::operator-=()");
298  return *this; }
299 
301 Inertia_& operator*=(const P& s) {I_OF_F *= s; return *this;}
302 
305 Inertia_& operator/=(const P& s) {I_OF_F /= s; return *this;}
306 
316 Inertia_ shiftToMassCenter(const Vec<3,P>& CF, const P& mass) const
317 { Inertia_ I(*this); I -= pointMassAt(CF, mass);
318  I.errChk("Inertia::shiftToMassCenter()");
319  return I; }
320 
331 Inertia_& shiftToMassCenterInPlace(const Vec<3,P>& CF, const P& mass)
332 { (*this) -= pointMassAt(CF, mass);
333  errChk("Inertia::shiftToMassCenterInPlace()");
334  return *this; }
335 
343 Inertia_ shiftFromMassCenter(const Vec<3,P>& p, const P& mass) const
344 { Inertia_ I(*this); I += pointMassAt(p, mass);
345  I.errChk("Inertia::shiftFromMassCenter()");
346  return I; }
347 
357 { (*this) += pointMassAt(p, mass);
358  errChk("Inertia::shiftFromMassCenterInPlace()");
359  return *this; }
360 
371 Inertia_ reexpress(const Rotation_<P>& R_FB) const
372 { return Inertia_((~R_FB).reexpressSymMat33(I_OF_F)); }
373 
377 { return Inertia_((~R_FB).reexpressSymMat33(I_OF_F)); }
378 
384 { I_OF_F = (~R_FB).reexpressSymMat33(I_OF_F); return *this; }
385 
389 { I_OF_F = (~R_FB).reexpressSymMat33(I_OF_F); return *this; }
390 
391 P trace() const {return I_OF_F.trace();}
392 
394 operator const SymMat<3,P>&() const {return I_OF_F;}
395 
397 const SymMat<3,P>& asSymMat33() const {return I_OF_F;}
398 
401 Mat<3,3,P> toMat33() const {return Mat<3,3,P>(I_OF_F);}
402 
405 const Vec<3,P>& getMoments() const {return I_OF_F.getDiag();}
408 const Vec<3,P>& getProducts() const {return I_OF_F.getLower();}
409 
410 bool isNaN() const {return I_OF_F.isNaN();}
411 bool isInf() const {return I_OF_F.isInf();}
412 bool isFinite() const {return I_OF_F.isFinite();}
413 
417 bool isNumericallyEqual(const Inertia_<P>& other) const
418 { return I_OF_F.isNumericallyEqual(other.I_OF_F); }
419 
423 bool isNumericallyEqual(const Inertia_<P>& other, double tol) const
424 { return I_OF_F.isNumericallyEqual(other.I_OF_F, tol); }
425 
428 static bool isValidInertiaMatrix(const SymMat<3,P>& m) {
429  if (m.isNaN()) return false;
430 
431  const Vec<3,P>& d = m.diag();
432  if (!(d >= 0)) return false; // diagonals must be nonnegative
433 
434  const P Slop = std::max(d.sum(),P(1))
436 
437  if (!(d[0]+d[1]+Slop>=d[2] && d[0]+d[2]+Slop>=d[1] && d[1]+d[2]+Slop>=d[0]))
438  return false; // must satisfy triangle inequality
439 
440  // Thanks to Paul Mitiguy for this condition on products of inertia.
441  const Vec<3,P>& p = m.getLower();
442  if (!( d[0]+Slop>=std::abs(2*p[2])
443  && d[1]+Slop>=std::abs(2*p[1])
444  && d[2]+Slop>=std::abs(2*p[0])))
445  return false; // max products are limited by moments
446 
447  return true;
448 }
449 
452 static Inertia_ pointMassAtOrigin() {return Inertia_(0);}
453 
458 static Inertia_ pointMassAt(const Vec<3,P>& p, const P& m) {
459  const Vec<3,P> mp = m*p; // 3 flops
460  const P mxx = mp[0]*p[0];
461  const P myy = mp[1]*p[1];
462  const P mzz = mp[2]*p[2];
463  const P nmx = -mp[0];
464  const P nmy = -mp[1];
465  return Inertia_( myy+mzz, mxx+mzz, mxx+myy,
466  nmx*p[1], nmx*p[2], nmy*p[2] );
467 }
468 
474 
475 
478 inline static Inertia_ sphere(const P& r);
479 
482 inline static Inertia_ cylinderAlongZ(const P& r, const P& hz);
483 
486 inline static Inertia_ cylinderAlongY(const P& r, const P& hy);
487 
490 inline static Inertia_ cylinderAlongX(const P& r, const P& hx);
491 
495 inline static Inertia_ brick(const P& hx, const P& hy, const P& hz);
496 
498 inline static Inertia_ brick(const Vec<3,P>& halfLengths);
499 
501 inline static Inertia_ ellipsoid(const P& hx, const P& hy, const P& hz);
502 
504 inline static Inertia_ ellipsoid(const Vec<3,P>& halfLengths);
505 
507 
508 protected:
509 // Reinterpret this Inertia matrix as a UnitInertia matrix, that is, as the
510 // inertia of something with unit mass. This is useful in implementing
511 // methods of the UnitInertia class in terms of Inertia methods. Be sure you
512 // know that this is a unit-mass inertia!
514 { return *static_cast<const UnitInertia_<P>*>(this); }
516 { return *static_cast<UnitInertia_<P>*>(this); }
517 
518 // If error checking is enabled (only in Debug mode), this
519 // method will run some tests on the current contents of this Inertia
520 // matrix and throw an error message if it is not valid. This should be
521 // the same set of tests as run by the isValidInertiaMatrix() method above.
522 void errChk(const char* methodName) const {
523 #ifndef NDEBUG
524  SimTK_ERRCHK(!isNaN(), methodName,
525  "Inertia matrix contains a NaN.");
526 
527  const Vec<3,P>& d = I_OF_F.getDiag(); // moments
528  const Vec<3,P>& p = I_OF_F.getLower(); // products
529  const P Ixx = d[0], Iyy = d[1], Izz = d[2];
530  const P Ixy = p[0], Ixz = p[1], Iyz = p[2];
531 
532  SimTK_ERRCHK3(d >= -SignificantReal, methodName,
533  "Diagonals of an Inertia matrix must be nonnegative; got %g,%g,%g.",
534  (double)Ixx,(double)Iyy,(double)Izz);
535 
536  // TODO: This is looser than it should be as a workaround for distorted
537  // rotation matrices that were produced by an 11,000 body chain that
538  // Sam Flores encountered.
539  const P Slop = std::max(d.sum(),P(1))
540  * std::sqrt(NTraits<P>::getEps());
541 
542  SimTK_ERRCHK3( Ixx+Iyy+Slop>=Izz
543  && Ixx+Izz+Slop>=Iyy
544  && Iyy+Izz+Slop>=Ixx,
545  methodName,
546  "Diagonals of an Inertia matrix must satisfy the triangle "
547  "inequality; got %g,%g,%g.",
548  (double)Ixx,(double)Iyy,(double)Izz);
549 
550  // Thanks to Paul Mitiguy for this condition on products of inertia.
551  SimTK_ERRCHK( Ixx+Slop>=std::abs(2*Iyz)
552  && Iyy+Slop>=std::abs(2*Ixz)
553  && Izz+Slop>=std::abs(2*Ixy),
554  methodName,
555  "The magnitude of a product of inertia was too large to be physical.");
556 #endif
557 }
558 
559 // Inertia expressed in frame F and about F's origin OF. Note that frame F
560 // is implicit here; all we actually have are the inertia scalars.
562 };
563 
568 template <class P> inline Inertia_<P>
569 operator+(const Inertia_<P>& l, const Inertia_<P>& r)
570 { return Inertia_<P>(l) += r; }
571 
577 template <class P> inline Inertia_<P>
578 operator-(const Inertia_<P>& l, const Inertia_<P>& r)
579 { return Inertia_<P>(l) -= r; }
580 
583 template <class P> inline Inertia_<P>
584 operator*(const Inertia_<P>& i, const P& r)
585 { return Inertia_<P>(i) *= r; }
586 
589 template <class P> inline Inertia_<P>
590 operator*(const P& r, const Inertia_<P>& i)
591 { return Inertia_<P>(i) *= r; }
592 
593 
597 template <class P> inline Inertia_<P>
598 operator*(const Inertia_<P>& i, int r)
599 { return Inertia_<P>(i) *= P(r); }
600 
604 template <class P> inline Inertia_<P>
605 operator*(int r, const Inertia_<P>& i)
606 { return Inertia_<P>(i) *= P(r); }
607 
611 template <class P> inline Inertia_<P>
612 operator/(const Inertia_<P>& i, const P& r)
613 { return Inertia_<P>(i) /= r; }
614 
618 template <class P> inline Inertia_<P>
619 operator/(const Inertia_<P>& i, int r)
620 { return Inertia_<P>(i) /= P(r); }
621 
625 template <class P> inline Vec<3,P>
626 operator*(const Inertia_<P>& I, const Vec<3,P>& w)
627 { return I.asSymMat33() * w; }
628 
633 template <class P> inline bool
634 operator==(const Inertia_<P>& i1, const Inertia_<P>& i2)
635 { return i1.asSymMat33() == i2.asSymMat33(); }
636 
640 template <class P> inline std::ostream&
641 operator<<(std::ostream& o, const Inertia_<P>& inertia)
642 { return o << inertia.toMat33(); }
643 
644 
645 // -----------------------------------------------------------------------------
646 // UNIT INERTIA MATRIX
647 // -----------------------------------------------------------------------------
668 template <class P>
670  typedef P RealP;
671  typedef Vec<3,P> Vec3P;
672  typedef SymMat<3,P> SymMat33P;
673  typedef Mat<3,3,P> Mat33P;
674  typedef Rotation_<P> RotationP;
675  typedef Inertia_<P> InertiaP;
676 public:
680 
681 // Default copy constructor, copy assignment, destructor.
682 
686 explicit UnitInertia_(const RealP& moment) : InertiaP(moment) {}
687 
692 explicit UnitInertia_(const Vec3P& moments, const Vec3P& products=Vec3P(0))
693 : InertiaP(moments,products) {}
694 
696 UnitInertia_(const RealP& xx, const RealP& yy, const RealP& zz)
697 : InertiaP(xx,yy,zz) {}
698 
701 UnitInertia_(const RealP& xx, const RealP& yy, const RealP& zz,
702  const RealP& xy, const RealP& xz, const RealP& yz)
703 : InertiaP(xx,yy,zz,xy,xz,yz) {}
704 
707 explicit UnitInertia_(const SymMat33P& m) : InertiaP(m) {}
708 
712 explicit UnitInertia_(const Mat33P& m) : InertiaP(m) {}
713 
718 explicit UnitInertia_(const Inertia_<P>& inertia) : InertiaP(inertia) {}
719 
723 UnitInertia_& setUnitInertia(const RealP& xx, const RealP& yy, const RealP& zz)
724 { InertiaP::setInertia(xx,yy,zz); return *this; }
725 
728 UnitInertia_& setUnitInertia(const Vec3P& moments, const Vec3P& products=Vec3P(0))
729 { InertiaP::setInertia(moments,products); return *this; }
730 
736 UnitInertia_& setUnitInertia(const RealP& xx, const RealP& yy, const RealP& zz,
737  const RealP& xy, const RealP& xz, const RealP& yz)
738 { InertiaP::setInertia(xx,yy,zz,xy,xz,yz); return *this; }
739 
740 
741 // No +=, -=, etc. operators because those don't result in a UnitInertia
742 // matrix. The parent class ones are suppressed below.
743 
754 { UnitInertia_ G(*this);
755  G.Inertia_<P>::operator-=(pointMassAt(CF));
756  return G; }
757 
771 { InertiaP::operator-=(pointMassAt(CF));
772  return *this; }
773 
782 { UnitInertia_ G(*this);
783  G.Inertia_<P>::operator+=(pointMassAt(p));
784  return G; }
785 
795 { InertiaP::operator+=(pointMassAt(p));
796  return *this; }
797 
809 { return UnitInertia_((~R_FB).reexpressSymMat33(this->I_OF_F)); }
810 
814 { return UnitInertia_((~R_FB).reexpressSymMat33(this->I_OF_F)); }
815 
821 { InertiaP::reexpressInPlace(R_FB); return *this; }
822 
826 { InertiaP::reexpressInPlace(R_FB); return *this; }
827 
828 
830 operator const SymMat33P&() const {return this->I_OF_F;}
831 
835 const Inertia_<P>& asUnitInertia() const
836 { return *static_cast<const Inertia_<P>*>(this); }
837 
841 { Inertia_<P>::operator=(inertia);
842  return *this; }
843 
847 static bool isValidUnitInertiaMatrix(const SymMat33P& m)
849 
856 
857 
861 
866 static UnitInertia_ pointMassAt(const Vec3P& p)
867 { return UnitInertia_(crossMatSq(p)); }
868 
871 static UnitInertia_ sphere(const RealP& r) {return UnitInertia_(RealP(0.4)*r*r);}
872 
875 static UnitInertia_ cylinderAlongZ(const RealP& r, const RealP& hz) {
876  const RealP Ixx = (r*r)/4 + (hz*hz)/3;
877  return UnitInertia_(Ixx,Ixx,(r*r)/2);
878 }
879 
882 static UnitInertia_ cylinderAlongY(const RealP& r, const RealP& hy) {
883  const RealP Ixx = (r*r)/4 + (hy*hy)/3;
884  return UnitInertia_(Ixx,(r*r)/2,Ixx);
885 }
886 
889 static UnitInertia_ cylinderAlongX(const RealP& r, const RealP& hx) {
890  const RealP Iyy = (r*r)/4 + (hx*hx)/3;
891  return UnitInertia_((r*r)/2,Iyy,Iyy);
892 }
893 
897 static UnitInertia_ brick(const RealP& hx, const RealP& hy, const RealP& hz) {
898  const RealP oo3 = RealP(1)/RealP(3);
899  const RealP hx2=hx*hx, hy2=hy*hy, hz2=hz*hz;
900  return UnitInertia_(oo3*(hy2+hz2), oo3*(hx2+hz2), oo3*(hx2+hy2));
901 }
902 
904 static UnitInertia_ brick(const Vec3P& halfLengths)
905 { return brick(halfLengths[0],halfLengths[1],halfLengths[2]); }
906 
908 static UnitInertia_ ellipsoid(const RealP& hx, const RealP& hy, const RealP& hz) {
909  const RealP hx2=hx*hx, hy2=hy*hy, hz2=hz*hz;
910  return UnitInertia_((hy2+hz2)/5, (hx2+hz2)/5, (hx2+hy2)/5);
911 }
912 
914 static UnitInertia_ ellipsoid(const Vec3P& halfLengths)
915 { return ellipsoid(halfLengths[0],halfLengths[1],halfLengths[2]); }
916 
918 private:
919 // Suppress Inertia_ methods which are not allowed for UnitInertia_.
920 
921 // These kill all flavors of Inertia_::setInertia() and the
922 // Inertia_ assignment ops.
923 void setInertia() {}
924 void operator+=(int) {}
925 void operator-=(int) {}
926 void operator*=(int) {}
927 void operator/=(int) {}
928 };
929 
930 // Implement Inertia methods which are pass-throughs to UnitInertia methods.
931 
932 template <class P> inline Inertia_<P> Inertia_<P>::
933 sphere(const P& r)
934 { return UnitInertia_<P>::sphere(r); }
935 template <class P> inline Inertia_<P> Inertia_<P>::
936 cylinderAlongZ(const P& r, const P& hz)
937 { return UnitInertia_<P>::cylinderAlongZ(r,hz); }
938 template <class P> inline Inertia_<P> Inertia_<P>::
939 cylinderAlongY(const P& r, const P& hy)
940 { return UnitInertia_<P>::cylinderAlongY(r,hy); }
941 template <class P> inline Inertia_<P> Inertia_<P>::
942 cylinderAlongX(const P& r, const P& hx)
943 { return UnitInertia_<P>::cylinderAlongX(r,hx); }
944 template <class P> inline Inertia_<P> Inertia_<P>::
945 brick(const P& hx, const P& hy, const P& hz)
946 { return UnitInertia_<P>::brick(hx,hy,hz); }
947 template <class P> inline Inertia_<P> Inertia_<P>::
948 brick(const Vec<3,P>& halfLengths)
949 { return UnitInertia_<P>::brick(halfLengths); }
950 template <class P> inline Inertia_<P> Inertia_<P>::
951 ellipsoid(const P& hx, const P& hy, const P& hz)
952 { return UnitInertia_<P>::ellipsoid(hx,hy,hz); }
953 template <class P> inline Inertia_<P> Inertia_<P>::
954 ellipsoid(const Vec<3,P>& halfLengths)
955 { return UnitInertia_<P>::ellipsoid(halfLengths); }
956 
957 
958 // -----------------------------------------------------------------------------
959 // SPATIAL INERTIA MATRIX
960 // -----------------------------------------------------------------------------
992 template <class P>
994  typedef P RealP;
995  typedef Vec<3,P> Vec3P;
996  typedef UnitInertia_<P> UnitInertiaP;
997  typedef Mat<3,3,P> Mat33P;
998  typedef Vec<2, Vec3P> SpatialVecP;
999  typedef Mat<2,2,Mat33P> SpatialMatP;
1000  typedef Rotation_<P> RotationP;
1001  typedef Transform_<P> TransformP;
1002  typedef Inertia_<P> InertiaP;
1003 public:
1006 : m(nanP()), p(nanP()) {} // inertia is already NaN
1007 SpatialInertia_(RealP mass, const Vec3P& com, const UnitInertiaP& gyration)
1008 : m(mass), p(com), G(gyration) {}
1009 
1010 // default copy constructor, copy assignment, destructor
1011 
1013 { SimTK_ERRCHK1(mass >= 0, "SpatialInertia::setMass()",
1014  "Negative mass %g is illegal.", (double)mass);
1015  m=mass; return *this; }
1017 { p=com; return *this;}
1019 { G=gyration; return *this; }
1020 
1021 RealP getMass() const {return m;}
1022 const Vec3P& getMassCenter() const {return p;}
1023 const UnitInertiaP& getUnitInertia() const {return G;}
1024 
1027 Vec3P calcMassMoment() const {return m*p;}
1028 
1031 InertiaP calcInertia() const {return m*G;}
1032 
1038  SimTK_ERRCHK(m+src.m != 0, "SpatialInertia::operator+=()",
1039  "The combined mass cannot be zero.");
1040  const RealP mtot = m+src.m, oomtot = 1/mtot; // ~11 flops
1041  p = oomtot*(calcMassMoment() + src.calcMassMoment()); // 10 flops
1042  G.setFromUnitInertia(oomtot*(calcInertia()+src.calcInertia())); // 19 flops
1043  m = mtot; // must do this last
1044  return *this;
1045 }
1046 
1052  SimTK_ERRCHK(m != src.m, "SpatialInertia::operator-=()",
1053  "The combined mass cannot be zero.");
1054  const RealP mtot = m-src.m, oomtot = 1/mtot; // ~11 flops
1055  p = oomtot*(calcMassMoment() - src.calcMassMoment()); // 10 flops
1056  G.setFromUnitInertia(oomtot*(calcInertia()-src.calcInertia())); // 19 flops
1057  m = mtot; // must do this last
1058  return *this;
1059 }
1060 
1063 SpatialInertia_& operator*=(const RealP& s) {m *= s; return *this;}
1064 
1067 SpatialInertia_& operator/=(const RealP& s) {m /= s; return *this;}
1068 
1072 { return m*SpatialVecP(G*v[0]+p%v[1], v[1]-p%v[0]); }
1073 
1080 { return SpatialInertia_(*this).reexpressInPlace(R_FB); }
1081 
1085 { return SpatialInertia_(*this).reexpressInPlace(R_FB); }
1086 
1092 { p = (~R_FB)*p; G.reexpressInPlace(R_FB); return *this; }
1093 
1097 { p = (~R_FB)*p; G.reexpressInPlace(R_FB); return *this; }
1098 
1103 SpatialInertia_ shift(const Vec3P& S) const
1104 { return SpatialInertia_(*this).shiftInPlace(S); }
1105 
1111  G.shiftToCentroidInPlace(p); // change to central inertia
1112  const Vec3P pNew(p-S); // vec from new origin to com (3 flops)
1113  G.shiftFromCentroidInPlace(pNew); // shift to S (pNew sign doesn't matter)
1114  p = pNew; // had p=com-OF; want p=com-(OF+S)=p-S
1115  return *this;
1116 }
1117 
1127 { return SpatialInertia_(*this).transformInPlace(X_FB); }
1128 
1132 { return SpatialInertia_(*this).transformInPlace(X_FB); }
1133 
1144  shiftInPlace(X_FB.p()); // shift to the new origin OB.
1145  reexpressInPlace(X_FB.R()); // get everything in B
1146  return *this;
1147 }
1148 
1152  shiftInPlace(X_FB.p()); // shift to the new origin OB.
1153  reexpressInPlace(X_FB.R()); // get everything in B
1154  return *this;
1155 }
1156 
1157 const SpatialMatP toSpatialMat() const {
1158  Mat33P offDiag = crossMat(m*p);
1159  return SpatialMatP(m*G.toMat33(), offDiag,
1160  -offDiag, Mat33P(m));
1161 }
1162 
1163 private:
1164 RealP m; // mass of this rigid body F
1165 Vec3P p; // location of body's COM from OF, expressed in F
1166 UnitInertiaP G; // mass distribution; inertia is mass*gyration
1167 
1168 static P nanP() {return NTraits<P>::getNaN();}
1169 };
1170 
1173 template <class P> inline SpatialInertia_<P>
1175 { return SpatialInertia_<P>(l) += r; }
1176 
1180 template <class P> inline SpatialInertia_<P>
1182 { return SpatialInertia_<P>(l) -= r; }
1183 
1184 
1185 // -----------------------------------------------------------------------------
1186 // ARTICULATED BODY INERTIA MATRIX
1187 // -----------------------------------------------------------------------------
1234 template <class P>
1235 class ArticulatedInertia_ {
1236  typedef P RealP;
1237  typedef Vec<3,P> Vec3P;
1238  typedef UnitInertia_<P> UnitInertiaP;
1239  typedef Mat<3,3,P> Mat33P;
1240  typedef SymMat<3,P> SymMat33P;
1241  typedef Vec<2, Vec3P> SpatialVecP;
1242  typedef Mat<2,2,Mat33P> SpatialMatP;
1243  typedef Rotation_<P> RotationP;
1244  typedef Transform_<P> TransformP;
1245  typedef Inertia_<P> InertiaP;
1246 public:
1251 ArticulatedInertia_(const SymMat33P& mass, const Mat33P& massMoment, const SymMat33P& inertia)
1252 : M(mass), J(inertia), F(massMoment) {}
1253 
1257 : M(rbi.getMass()), J(rbi.calcInertia()), F(crossMat(rbi.calcMassMoment())) {}
1258 
1260 ArticulatedInertia_& setMass (const SymMat33P& mass) {M=mass; return *this;}
1262 ArticulatedInertia_& setMassMoment(const Mat33P& massMoment) {F=massMoment; return *this;}
1264 ArticulatedInertia_& setInertia (const SymMat33P& inertia) {J=inertia; return *this;}
1265 
1267 const SymMat33P& getMass() const {return M;}
1269 const Mat33P& getMassMoment() const {return F;}
1271 const SymMat33P& getInertia() const {return J;}
1272 
1273 // default destructor, copy constructor, copy assignment
1274 
1278 { M+=src.M; J+=src.J; F+=src.F; return *this; }
1282 { M-=src.M; J-=src.J; F-=src.F; return *this; }
1283 
1286 { return SpatialVecP(J*v[0]+F*v[1], ~F*v[0]+M*v[1]); }
1287 
1289 template <int N>
1291  Mat<2,N,Vec3P> res;
1292  for (int j=0; j < N; ++j)
1293  res.col(j) = (*this) * m.col(j); // above this*SpatialVec operator
1294  return res;
1295 }
1296 
1307 SimTK_SimTKCOMMON_EXPORT ArticulatedInertia_ shift(const Vec3P& s) const;
1308 
1312 
1315 const SpatialMatP toSpatialMat() const {
1316  // This more-beautiful code ran into an optimization bug Microsoft
1317  // introduced in Update 2 to Visual C++ 2015.
1318  //return SpatialMatP( Mat33P(J), F,
1319  // ~F, Mat33P(M) );
1320 
1321  // Uglier but functional.
1322  SpatialMatP smat;
1323  smat(0,0) = Mat33P(J); smat(0,1) = F;
1324  smat(1,0) = ~F; smat(1,1) = Mat33P(M);
1325  return smat;
1326 }
1327 private:
1328 SymMat33P M;
1329 SymMat33P J;
1330 Mat33P F;
1331 };
1332 
1335 template <class P> inline ArticulatedInertia_<P>
1337 { return ArticulatedInertia_<P>(l) += r; }
1338 
1342 template <class P> inline ArticulatedInertia_<P>
1344 { return ArticulatedInertia_<P>(l) -= r; }
1345 
1346 
1347 // -----------------------------------------------------------------------------
1348 // MASS PROPERTIES
1349 // -----------------------------------------------------------------------------
1362 template <class P>
1364 public:
1367 MassProperties_() { setMassProperties(0,Vec<3,P>(0),UnitInertia_<P>()); }
1371 MassProperties_(const P& m, const Vec<3,P>& com, const Inertia_<P>& inertia)
1372  { setMassProperties(m,com,inertia); }
1375 MassProperties_(const P& m, const Vec<3,P>& com, const UnitInertia_<P>& gyration)
1376  { setMassProperties(m,com,gyration); }
1377 
1381 MassProperties_& setMassProperties(const P& m, const Vec<3,P>& com, const Inertia_<P>& inertia) {
1382  mass = m;
1383  comInB = com;
1384  if (m == 0) {
1385  SimTK_ASSERT(inertia.asSymMat33().getAsVec() == 0, "Mass is zero but inertia is nonzero");
1386  unitInertia_OB_B = UnitInertia_<P>(0);
1387  }
1388  else {
1389  unitInertia_OB_B = UnitInertia_<P>(inertia*(1/m));
1390  }
1391  return *this;
1392 }
1393 
1396 MassProperties_& setMassProperties
1397  (const P& m, const Vec<3,P>& com, const UnitInertia_<P>& gyration)
1398 { mass=m; comInB=com; unitInertia_OB_B=gyration; return *this; }
1399 
1401 const P& getMass() const {return mass;}
1406 const Vec<3,P>& getMassCenter() const {return comInB;}
1411 const UnitInertia_<P>& getUnitInertia() const {return unitInertia_OB_B;}
1417 const Inertia_<P> calcInertia() const {return mass*unitInertia_OB_B;}
1422 const Inertia_<P> getInertia() const {return calcInertia();}
1423 
1428  return mass*unitInertia_OB_B - Inertia_<P>(comInB, mass);
1429 }
1434 Inertia_<P> calcShiftedInertia(const Vec<3,P>& newOriginB) const {
1435  return calcCentralInertia() + Inertia_<P>(newOriginB-comInB, mass);
1436 }
1442  return calcShiftedInertia(X_BC.p()).reexpress(X_BC.R());
1443 }
1450  return MassProperties_(mass, comInB-newOriginB,
1451  calcShiftedInertia(newOriginB));
1452 }
1453 
1464  return MassProperties_(mass, ~X_BC*comInB, calcTransformedInertia(X_BC));
1465 }
1466 
1476  return MassProperties_(mass, ~R_BC*comInB, unitInertia_OB_B.reexpress(R_BC));
1477 }
1478 
1482 bool isExactlyMassless() const { return mass==0; }
1488 bool isNearlyMassless(const P& tol=SignificantReal) const {
1489  return mass <= tol;
1490 }
1491 
1495 bool isExactlyCentral() const { return comInB==Vec<3,P>(0); }
1501 bool isNearlyCentral(const P& tol=SignificantReal) const {
1502  return comInB.normSqr() <= tol*tol;
1503 }
1504 
1507 bool isNaN() const {return SimTK::isNaN(mass) || comInB.isNaN() || unitInertia_OB_B.isNaN();}
1512 bool isInf() const {
1513  if (isNaN()) return false;
1514  return SimTK::isInf(mass) || comInB.isInf() || unitInertia_OB_B.isInf();
1515 }
1519 bool isFinite() const {
1520  return SimTK::isFinite(mass) && comInB.isFinite() && unitInertia_OB_B.isFinite();
1521 }
1522 
1527  Mat<2,2,Mat<3,3,P>> M;
1528  M(0,0) = mass*unitInertia_OB_B.toMat33();
1529  M(0,1) = mass*crossMat(comInB);
1530  M(1,0) = ~M(0,1);
1531  M(1,1) = mass; // a diagonal matrix
1532  return M;
1533 }
1534 
1541  Mat<6,6,P> M;
1542  M.template updSubMat<3,3>(0,0) = mass*unitInertia_OB_B.toMat33();
1543  M.template updSubMat<3,3>(0,3) = mass*crossMat(comInB);
1544  M.template updSubMat<3,3>(3,0) = ~M.template getSubMat<3,3>(0,3);
1545  M.template updSubMat<3,3>(3,3) = mass; // a diagonal matrix
1546  return M;
1547 }
1548 
1549 private:
1550 P mass;
1551 Vec<3,P> comInB; // meas. from B origin, expr. in B
1552 UnitInertia_<P> unitInertia_OB_B; // about B origin, expr. in B
1553 };
1554 
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()
1570  << "\n}\n";
1571 }
1572 
1573 } // namespace SimTK
1574 
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
Vec< 3, P > p() const
Calculate the actual translation vector at a cost of 18 flops.
Definition: Transform.h:373
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
Transform from frame B to frame F, but with the internal representation inverted. ...
Definition: Transform.h:44
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
const Rotation_< P > & R() const
Return a read-only reference to the contained rotation R_BF.
Definition: Transform.h:215
EStandard sum() const
Sum just adds up all the elements into a single return element that is the same type as this Vec&#39;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&#39;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
const InverseRotation_< P > & R() const
Definition: Transform.h:360
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
This class represents the rotate-and-shift transform which gives the location and orientation of a ne...
Definition: Transform.h:43
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&#39;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&#39;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&#39;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&#39;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&#39;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&#39;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
const Vec< 3, P > & p() const
Return a read-only reference to our translation vector p_BF.
Definition: Transform.h:239
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
Definition: negator.h:64
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