Simbody  3.5
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 
126 typedef UnitInertia Gyration;
127 
128 // -----------------------------------------------------------------------------
129 // INERTIA MATRIX
130 // -----------------------------------------------------------------------------
192 template <class P>
194  typedef P RealP;
195  typedef Vec<3,P> Vec3P;
196  typedef SymMat<3,P> SymMat33P;
197  typedef Mat<3,3,P> Mat33P;
198  typedef Rotation_<P> RotationP;
199 public:
202 Inertia_() : I_OF_F(NTraits<P>::getNaN()) {}
203 
204 // Default copy constructor, copy assignment, destructor.
205 
212 explicit Inertia_(const RealP& moment) : I_OF_F(moment)
213 { errChk("Inertia::Inertia(moment)"); }
214 
218 Inertia_(const Vec3P& p, const RealP& mass) : I_OF_F(pointMassAt(p,mass)) {}
219 
224 explicit Inertia_(const Vec3P& moments, const Vec3P& products=Vec3P(0))
225 { I_OF_F.updDiag() = moments;
226  I_OF_F.updLower() = products;
227  errChk("Inertia::Inertia(moments,products)"); }
228 
230 Inertia_(const RealP& xx, const RealP& yy, const RealP& zz)
231 { I_OF_F = SymMat33P(xx,
232  0, yy,
233  0, 0, zz);
234  errChk("Inertia::setInertia(xx,yy,zz)"); }
235 
238 Inertia_(const RealP& xx, const RealP& yy, const RealP& zz,
239  const RealP& xy, const RealP& xz, const RealP& yz)
240 { I_OF_F = SymMat33P(xx,
241  xy, yy,
242  xz, yz, zz);
243  errChk("Inertia::setInertia(xx,yy,zz,xy,xz,yz)"); }
244 
247 explicit Inertia_(const SymMat33P& inertia) : I_OF_F(inertia)
248 { errChk("Inertia::Inertia(SymMat33)"); }
249 
253 explicit Inertia_(const Mat33P& m)
255  "Inertia(Mat33)", "The supplied matrix was not symmetric.");
256  I_OF_F = SymMat33P(m);
257  errChk("Inertia(Mat33)"); }
258 
259 
262 Inertia_& setInertia(const RealP& xx, const RealP& yy, const RealP& zz) {
263  I_OF_F = RealP(0); I_OF_F(0,0) = xx; I_OF_F(1,1) = yy; I_OF_F(2,2) = zz;
264  errChk("Inertia::setInertia(xx,yy,zz)");
265  return *this;
266 }
267 
270 Inertia_& setInertia(const Vec3P& moments, const Vec3P& products=Vec3P(0)) {
271  I_OF_F.updDiag() = moments;
272  I_OF_F.updLower() = products;
273  errChk("Inertia::setInertia(moments,products)");
274  return *this;
275 }
276 
282 Inertia_& setInertia(const RealP& xx, const RealP& yy, const RealP& zz,
283  const RealP& xy, const RealP& xz, const RealP& yz) {
284  setInertia(Vec3P(xx,yy,zz), Vec3P(xy,xz,yz));
285  errChk("Inertia::setInertia(xx,yy,zz,xy,xz,yz)");
286  return *this;
287 }
288 
289 
292 Inertia_& operator+=(const Inertia_& inertia)
293 { I_OF_F += inertia.I_OF_F;
294  errChk("Inertia::operator+=()");
295  return *this; }
296 
299 Inertia_& operator-=(const Inertia_& inertia)
300 { I_OF_F -= inertia.I_OF_F;
301  errChk("Inertia::operator-=()");
302  return *this; }
303 
305 Inertia_& operator*=(const P& s) {I_OF_F *= s; return *this;}
306 
309 Inertia_& operator/=(const P& s) {I_OF_F /= s; return *this;}
310 
320 Inertia_ shiftToMassCenter(const Vec3P& CF, const RealP& mass) const
321 { Inertia_ I(*this); I -= pointMassAt(CF, mass);
322  I.errChk("Inertia::shiftToMassCenter()");
323  return I; }
324 
335 Inertia_& shiftToMassCenterInPlace(const Vec3P& CF, const RealP& mass)
336 { (*this) -= pointMassAt(CF, mass);
337  errChk("Inertia::shiftToMassCenterInPlace()");
338  return *this; }
339 
347 Inertia_ shiftFromMassCenter(const Vec3P& p, const RealP& mass) const
348 { Inertia_ I(*this); I += pointMassAt(p, mass);
349  I.errChk("Inertia::shiftFromMassCenter()");
350  return I; }
351 
360 Inertia_& shiftFromMassCenterInPlace(const Vec3P& p, const RealP& mass)
361 { (*this) += pointMassAt(p, mass);
362  errChk("Inertia::shiftFromMassCenterInPlace()");
363  return *this; }
364 
375 Inertia_ reexpress(const Rotation_<P>& R_FB) const
376 { return Inertia_((~R_FB).reexpressSymMat33(I_OF_F)); }
377 
381 { return Inertia_((~R_FB).reexpressSymMat33(I_OF_F)); }
382 
388 { I_OF_F = (~R_FB).reexpressSymMat33(I_OF_F); return *this; }
389 
393 { I_OF_F = (~R_FB).reexpressSymMat33(I_OF_F); return *this; }
394 
395 RealP trace() const {return I_OF_F.trace();}
396 
398 operator const SymMat33P&() const {return I_OF_F;}
399 
401 const SymMat33P& asSymMat33() const {return I_OF_F;}
402 
405 Mat33P toMat33() const {return Mat33P(I_OF_F);}
406 
409 const Vec3P& getMoments() const {return I_OF_F.getDiag();}
412 const Vec3P& getProducts() const {return I_OF_F.getLower();}
413 
414 bool isNaN() const {return I_OF_F.isNaN();}
415 bool isInf() const {return I_OF_F.isInf();}
416 bool isFinite() const {return I_OF_F.isFinite();}
417 
421 bool isNumericallyEqual(const Inertia_<P>& other) const
422 { return I_OF_F.isNumericallyEqual(other.I_OF_F); }
423 
427 bool isNumericallyEqual(const Inertia_<P>& other, double tol) const
428 { return I_OF_F.isNumericallyEqual(other.I_OF_F, tol); }
429 
432 static bool isValidInertiaMatrix(const SymMat33P& m) {
433  if (m.isNaN()) return false;
434 
435  const Vec3P& d = m.diag();
436  if (!(d >= 0)) return false; // diagonals must be nonnegative
437 
438  const RealP Slop = std::max(d.sum(),RealP(1))
440 
441  if (!(d[0]+d[1]+Slop>=d[2] && d[0]+d[2]+Slop>=d[1] && d[1]+d[2]+Slop>=d[0]))
442  return false; // must satisfy triangle inequality
443 
444  // Thanks to Paul Mitiguy for this condition on products of inertia.
445  const Vec3P& p = m.getLower();
446  if (!( d[0]+Slop>=std::abs(2*p[2])
447  && d[1]+Slop>=std::abs(2*p[1])
448  && d[2]+Slop>=std::abs(2*p[0])))
449  return false; // max products are limited by moments
450 
451  return true;
452 }
453 
456 static Inertia_ pointMassAtOrigin() {return Inertia_(0);}
457 
462 static Inertia_ pointMassAt(const Vec3P& p, const RealP& m) {
463  const Vec3P mp = m*p; // 3 flops
464  const RealP mxx = mp[0]*p[0];
465  const RealP myy = mp[1]*p[1];
466  const RealP mzz = mp[2]*p[2];
467  const RealP nmx = -mp[0];
468  const RealP nmy = -mp[1];
469  return Inertia_( myy+mzz, mxx+mzz, mxx+myy,
470  nmx*p[1], nmx*p[2], nmy*p[2] );
471 }
472 
478 
479 
482 inline static Inertia_ sphere(const RealP& r);
483 
486 inline static Inertia_ cylinderAlongZ(const RealP& r, const RealP& hz);
487 
490 inline static Inertia_ cylinderAlongY(const RealP& r, const RealP& hy);
491 
494 inline static Inertia_ cylinderAlongX(const RealP& r, const RealP& hx);
495 
499 inline static Inertia_ brick(const RealP& hx, const RealP& hy, const RealP& hz);
500 
502 inline static Inertia_ brick(const Vec3P& halfLengths);
503 
505 inline static Inertia_ ellipsoid(const RealP& hx, const RealP& hy, const RealP& hz);
506 
508 inline static Inertia_ ellipsoid(const Vec3P& halfLengths);
509 
511 
512 protected:
513 // Reinterpret this Inertia matrix as a UnitInertia matrix, that is, as the
514 // inertia of something with unit mass. This is useful in implementing
515 // methods of the UnitInertia class in terms of Inertia methods. Be sure you
516 // know that this is a unit-mass inertia!
518 { return *static_cast<const UnitInertia_<P>*>(this); }
520 { return *static_cast<UnitInertia_<P>*>(this); }
521 
522 // If error checking is enabled (only in Debug mode), this
523 // method will run some tests on the current contents of this Inertia
524 // matrix and throw an error message if it is not valid. This should be
525 // the same set of tests as run by the isValidInertiaMatrix() method above.
526 void errChk(const char* methodName) const {
527 #ifndef NDEBUG
528  SimTK_ERRCHK(!isNaN(), methodName,
529  "Inertia matrix contains a NaN.");
530 
531  const Vec3P& d = I_OF_F.getDiag(); // moments
532  const Vec3P& p = I_OF_F.getLower(); // products
533  const RealP Ixx = d[0], Iyy = d[1], Izz = d[2];
534  const RealP Ixy = p[0], Ixz = p[1], Iyz = p[2];
535 
536  SimTK_ERRCHK3(d >= -SignificantReal, methodName,
537  "Diagonals of an Inertia matrix must be nonnegative; got %g,%g,%g.",
538  (double)Ixx,(double)Iyy,(double)Izz);
539 
540  // TODO: This is looser than it should be as a workaround for distorted
541  // rotation matrices that were produced by an 11,000 body chain that
542  // Sam Flores encountered.
543  const RealP Slop = std::max(d.sum(),RealP(1))
544  * std::sqrt(NTraits<P>::getEps());
545 
546  SimTK_ERRCHK3( Ixx+Iyy+Slop>=Izz
547  && Ixx+Izz+Slop>=Iyy
548  && Iyy+Izz+Slop>=Ixx,
549  methodName,
550  "Diagonals of an Inertia matrix must satisfy the triangle "
551  "inequality; got %g,%g,%g.",
552  (double)Ixx,(double)Iyy,(double)Izz);
553 
554  // Thanks to Paul Mitiguy for this condition on products of inertia.
555  SimTK_ERRCHK( Ixx+Slop>=std::abs(2*Iyz)
556  && Iyy+Slop>=std::abs(2*Ixz)
557  && Izz+Slop>=std::abs(2*Ixy),
558  methodName,
559  "The magnitude of a product of inertia was too large to be physical.");
560 #endif
561 }
562 
563 // Inertia expressed in frame F and about F's origin OF. Note that frame F
564 // is implicit here; all we actually have are the inertia scalars.
565 SymMat33P I_OF_F;
566 };
567 
572 template <class P> inline Inertia_<P>
573 operator+(const Inertia_<P>& l, const Inertia_<P>& r)
574 { return Inertia_<P>(l) += r; }
575 
581 template <class P> inline Inertia_<P>
582 operator-(const Inertia_<P>& l, const Inertia_<P>& r)
583 { return Inertia_<P>(l) -= r; }
584 
587 template <class P> inline Inertia_<P>
588 operator*(const Inertia_<P>& i, const P& r)
589 { return Inertia_<P>(i) *= r; }
590 
593 template <class P> inline Inertia_<P>
594 operator*(const P& r, const Inertia_<P>& i)
595 { return Inertia_<P>(i) *= r; }
596 
597 
601 template <class P> inline Inertia_<P>
602 operator*(const Inertia_<P>& i, int r)
603 { return Inertia_<P>(i) *= P(r); }
604 
608 template <class P> inline Inertia_<P>
609 operator*(int r, const Inertia_<P>& i)
610 { return Inertia_<P>(i) *= P(r); }
611 
615 template <class P> inline Inertia_<P>
616 operator/(const Inertia_<P>& i, const P& r)
617 { return Inertia_<P>(i) /= r; }
618 
622 template <class P> inline Inertia_<P>
623 operator/(const Inertia_<P>& i, int r)
624 { return Inertia_<P>(i) /= P(r); }
625 
629 template <class P> inline Vec<3,P>
630 operator*(const Inertia_<P>& I, const Vec<3,P>& w)
631 { return I.asSymMat33() * w; }
632 
637 template <class P> inline bool
638 operator==(const Inertia_<P>& i1, const Inertia_<P>& i2)
639 { return i1.asSymMat33() == i2.asSymMat33(); }
640 
644 template <class P> inline std::ostream&
645 operator<<(std::ostream& o, const Inertia_<P>& inertia)
646 { return o << inertia.toMat33(); }
647 
648 
649 // -----------------------------------------------------------------------------
650 // UNIT INERTIA MATRIX
651 // -----------------------------------------------------------------------------
672 template <class P>
673 class SimTK_SimTKCOMMON_EXPORT UnitInertia_ : public Inertia_<P> {
674  typedef P RealP;
675  typedef Vec<3,P> Vec3P;
676  typedef SymMat<3,P> SymMat33P;
677  typedef Mat<3,3,P> Mat33P;
678  typedef Rotation_<P> RotationP;
679  typedef Inertia_<P> InertiaP;
680 public:
684 
685 // Default copy constructor, copy assignment, destructor.
686 
690 explicit UnitInertia_(const RealP& moment) : InertiaP(moment) {}
691 
696 explicit UnitInertia_(const Vec3P& moments, const Vec3P& products=Vec3P(0))
697 : InertiaP(moments,products) {}
698 
700 UnitInertia_(const RealP& xx, const RealP& yy, const RealP& zz)
701 : InertiaP(xx,yy,zz) {}
702 
705 UnitInertia_(const RealP& xx, const RealP& yy, const RealP& zz,
706  const RealP& xy, const RealP& xz, const RealP& yz)
707 : InertiaP(xx,yy,zz,xy,xz,yz) {}
708 
711 explicit UnitInertia_(const SymMat33P& m) : InertiaP(m) {}
712 
716 explicit UnitInertia_(const Mat33P& m) : InertiaP(m) {}
717 
722 explicit UnitInertia_(const Inertia_<P>& inertia) : InertiaP(inertia) {}
723 
727 UnitInertia_& setUnitInertia(const RealP& xx, const RealP& yy, const RealP& zz)
728 { InertiaP::setInertia(xx,yy,zz); return *this; }
729 
732 UnitInertia_& setUnitInertia(const Vec3P& moments, const Vec3P& products=Vec3P(0))
733 { InertiaP::setInertia(moments,products); return *this; }
734 
740 UnitInertia_& setUnitInertia(const RealP& xx, const RealP& yy, const RealP& zz,
741  const RealP& xy, const RealP& xz, const RealP& yz)
742 { InertiaP::setInertia(xx,yy,zz,xy,xz,yz); return *this; }
743 
744 
745 // No +=, -=, etc. operators because those don't result in a UnitInertia
746 // matrix. The parent class ones are suppressed below.
747 
757 UnitInertia_ shiftToCentroid(const Vec3P& CF) const
758 { UnitInertia_ G(*this);
759  G.Inertia_<P>::operator-=(pointMassAt(CF));
760  return G; }
761 
774 UnitInertia_& shiftToCentroidInPlace(const Vec3P& CF)
775 { InertiaP::operator-=(pointMassAt(CF));
776  return *this; }
777 
785 UnitInertia_ shiftFromCentroid(const Vec3P& p) const
786 { UnitInertia_ G(*this);
787  G.Inertia_<P>::operator+=(pointMassAt(p));
788  return G; }
789 
798 UnitInertia_& shiftFromCentroidInPlace(const Vec3P& p)
799 { InertiaP::operator+=(pointMassAt(p));
800  return *this; }
801 
812 UnitInertia_ reexpress(const Rotation_<P>& R_FB) const
813 { return UnitInertia_((~R_FB).reexpressSymMat33(this->I_OF_F)); }
814 
817 UnitInertia_ reexpress(const InverseRotation_<P>& R_FB) const
818 { return UnitInertia_((~R_FB).reexpressSymMat33(this->I_OF_F)); }
819 
824 UnitInertia_& reexpressInPlace(const Rotation_<P>& R_FB)
825 { InertiaP::reexpressInPlace(R_FB); return *this; }
826 
829 UnitInertia_& reexpressInPlace(const InverseRotation_<P>& R_FB)
830 { InertiaP::reexpressInPlace(R_FB); return *this; }
831 
832 
834 operator const SymMat33P&() const {return this->I_OF_F;}
835 
839 const Inertia_<P>& asUnitInertia() const
840 { return *static_cast<const Inertia_<P>*>(this); }
841 
844 UnitInertia_& setFromUnitInertia(const Inertia_<P>& inertia)
845 { Inertia_<P>::operator=(inertia);
846  return *this; }
847 
851 static bool isValidUnitInertiaMatrix(const SymMat33P& m)
853 
860 
861 
864 static UnitInertia_ pointMassAtOrigin() {return UnitInertia_(0);}
865 
870 static UnitInertia_ pointMassAt(const Vec3P& p)
871 { return UnitInertia_(crossMatSq(p)); }
872 
875 static UnitInertia_ sphere(const RealP& r) {return UnitInertia_(RealP(0.4)*r*r);}
876 
879 static UnitInertia_ cylinderAlongZ(const RealP& r, const RealP& hz) {
880  const RealP Ixx = (r*r)/4 + (hz*hz)/3;
881  return UnitInertia_(Ixx,Ixx,(r*r)/2);
882 }
883 
886 static UnitInertia_ cylinderAlongY(const RealP& r, const RealP& hy) {
887  const RealP Ixx = (r*r)/4 + (hy*hy)/3;
888  return UnitInertia_(Ixx,(r*r)/2,Ixx);
889 }
890 
893 static UnitInertia_ cylinderAlongX(const RealP& r, const RealP& hx) {
894  const RealP Iyy = (r*r)/4 + (hx*hx)/3;
895  return UnitInertia_((r*r)/2,Iyy,Iyy);
896 }
897 
901 static UnitInertia_ brick(const RealP& hx, const RealP& hy, const RealP& hz) {
902  const RealP oo3 = RealP(1)/RealP(3);
903  const RealP hx2=hx*hx, hy2=hy*hy, hz2=hz*hz;
904  return UnitInertia_(oo3*(hy2+hz2), oo3*(hx2+hz2), oo3*(hx2+hy2));
905 }
906 
908 static UnitInertia_ brick(const Vec3P& halfLengths)
909 { return brick(halfLengths[0],halfLengths[1],halfLengths[2]); }
910 
912 static UnitInertia_ ellipsoid(const RealP& hx, const RealP& hy, const RealP& hz) {
913  const RealP hx2=hx*hx, hy2=hy*hy, hz2=hz*hz;
914  return UnitInertia_((hy2+hz2)/5, (hx2+hz2)/5, (hx2+hy2)/5);
915 }
916 
918 static UnitInertia_ ellipsoid(const Vec3P& halfLengths)
919 { return ellipsoid(halfLengths[0],halfLengths[1],halfLengths[2]); }
920 
922 private:
923 // Suppress Inertia_ methods which are not allowed for UnitInertia_.
924 
925 // These kill all flavors of Inertia_::setInertia() and the
926 // Inertia_ assignment ops.
927 void setInertia() {}
928 void operator+=(int) {}
929 void operator-=(int) {}
930 void operator*=(int) {}
931 void operator/=(int) {}
932 };
933 
934 // Implement Inertia methods which are pass-throughs to UnitInertia methods.
935 
936 template <class P> inline Inertia_<P> Inertia_<P>::
937 sphere(const RealP& r)
938 { return UnitInertia_<P>::sphere(r); }
939 template <class P> inline Inertia_<P> Inertia_<P>::
940 cylinderAlongZ(const RealP& r, const RealP& hz)
941 { return UnitInertia_<P>::cylinderAlongZ(r,hz); }
942 template <class P> inline Inertia_<P> Inertia_<P>::
943 cylinderAlongY(const RealP& r, const RealP& hy)
944 { return UnitInertia_<P>::cylinderAlongY(r,hy); }
945 template <class P> inline Inertia_<P> Inertia_<P>::
946 cylinderAlongX(const RealP& r, const RealP& hx)
947 { return UnitInertia_<P>::cylinderAlongX(r,hx); }
948 template <class P> inline Inertia_<P> Inertia_<P>::
949 brick(const RealP& hx, const RealP& hy, const RealP& hz)
950 { return UnitInertia_<P>::brick(hx,hy,hz); }
951 template <class P> inline Inertia_<P> Inertia_<P>::
952 brick(const Vec3P& halfLengths)
953 { return UnitInertia_<P>::brick(halfLengths); }
954 template <class P> inline Inertia_<P> Inertia_<P>::
955 ellipsoid(const RealP& hx, const RealP& hy, const RealP& hz)
956 { return UnitInertia_<P>::ellipsoid(hx,hy,hz); }
957 template <class P> inline Inertia_<P> Inertia_<P>::
958 ellipsoid(const Vec3P& halfLengths)
959 { return UnitInertia_<P>::ellipsoid(halfLengths); }
960 
961 
962 // -----------------------------------------------------------------------------
963 // SPATIAL INERTIA MATRIX
964 // -----------------------------------------------------------------------------
996 template <class P>
997 class SimTK_SimTKCOMMON_EXPORT SpatialInertia_ {
998  typedef P RealP;
999  typedef Vec<3,P> Vec3P;
1000  typedef UnitInertia_<P> UnitInertiaP;
1001  typedef Mat<3,3,P> Mat33P;
1002  typedef Vec<2, Vec3P> SpatialVecP;
1003  typedef Mat<2,2,Mat33P> SpatialMatP;
1004  typedef Rotation_<P> RotationP;
1005  typedef Transform_<P> TransformP;
1006  typedef Inertia_<P> InertiaP;
1007 public:
1010 : m(nanP()), p(nanP()) {} // inertia is already NaN
1011 SpatialInertia_(RealP mass, const Vec3P& com, const UnitInertiaP& gyration)
1012 : m(mass), p(com), G(gyration) {}
1013 
1014 // default copy constructor, copy assignment, destructor
1015 
1016 SpatialInertia_& setMass(RealP mass)
1017 { SimTK_ERRCHK1(mass >= 0, "SpatialInertia::setMass()",
1018  "Negative mass %g is illegal.", (double)mass);
1019  m=mass; return *this; }
1020 SpatialInertia_& setMassCenter(const Vec3P& com)
1021 { p=com; return *this;}
1022 SpatialInertia_& setUnitInertia(const UnitInertiaP& gyration)
1023 { G=gyration; return *this; }
1024 
1025 RealP getMass() const {return m;}
1026 const Vec3P& getMassCenter() const {return p;}
1027 const UnitInertiaP& getUnitInertia() const {return G;}
1028 
1031 Vec3P calcMassMoment() const {return m*p;}
1032 
1035 InertiaP calcInertia() const {return m*G;}
1036 
1041 SpatialInertia_& operator+=(const SpatialInertia_& src) {
1042  SimTK_ERRCHK(m+src.m != 0, "SpatialInertia::operator+=()",
1043  "The combined mass cannot be zero.");
1044  const RealP mtot = m+src.m, oomtot = 1/mtot; // ~11 flops
1045  p = oomtot*(calcMassMoment() + src.calcMassMoment()); // 10 flops
1046  G.setFromUnitInertia(oomtot*(calcInertia()+src.calcInertia())); // 19 flops
1047  m = mtot; // must do this last
1048  return *this;
1049 }
1050 
1055 SpatialInertia_& operator-=(const SpatialInertia_& src) {
1056  SimTK_ERRCHK(m != src.m, "SpatialInertia::operator-=()",
1057  "The combined mass cannot be zero.");
1058  const RealP mtot = m-src.m, oomtot = 1/mtot; // ~11 flops
1059  p = oomtot*(calcMassMoment() - src.calcMassMoment()); // 10 flops
1060  G.setFromUnitInertia(oomtot*(calcInertia()-src.calcInertia())); // 19 flops
1061  m = mtot; // must do this last
1062  return *this;
1063 }
1064 
1067 SpatialInertia_& operator*=(const RealP& s) {m *= s; return *this;}
1068 
1071 SpatialInertia_& operator/=(const RealP& s) {m /= s; return *this;}
1072 
1075 SpatialVecP operator*(const SpatialVecP& v) const
1076 { return m*SpatialVecP(G*v[0]+p%v[1], v[1]-p%v[0]); }
1077 
1083 SpatialInertia_ reexpress(const Rotation_<P>& R_FB) const
1084 { return SpatialInertia_(*this).reexpressInPlace(R_FB); }
1085 
1088 SpatialInertia_ reexpress(const InverseRotation_<P>& R_FB) const
1089 { return SpatialInertia_(*this).reexpressInPlace(R_FB); }
1090 
1095 SpatialInertia_& reexpressInPlace(const Rotation_<P>& R_FB)
1096 { p = (~R_FB)*p; G.reexpressInPlace(R_FB); return *this; }
1097 
1100 SpatialInertia_& reexpressInPlace(const InverseRotation_<P>& R_FB)
1101 { p = (~R_FB)*p; G.reexpressInPlace(R_FB); return *this; }
1102 
1107 SpatialInertia_ shift(const Vec3P& S) const
1108 { return SpatialInertia_(*this).shiftInPlace(S); }
1109 
1114 SpatialInertia_& shiftInPlace(const Vec3P& S) {
1115  G.shiftToCentroidInPlace(p); // change to central inertia
1116  const Vec3P pNew(p-S); // vec from new origin to com (3 flops)
1117  G.shiftFromCentroidInPlace(pNew); // shift to S (pNew sign doesn't matter)
1118  p = pNew; // had p=com-OF; want p=com-(OF+S)=p-S
1119  return *this;
1120 }
1121 
1130 SpatialInertia_ transform(const Transform_<P>& X_FB) const
1131 { return SpatialInertia_(*this).transformInPlace(X_FB); }
1132 
1135 SpatialInertia_ transform(const InverseTransform_<P>& X_FB) const
1136 { return SpatialInertia_(*this).transformInPlace(X_FB); }
1137 
1147 SpatialInertia_& transformInPlace(const Transform_<P>& X_FB) {
1148  shiftInPlace(X_FB.p()); // shift to the new origin OB.
1149  reexpressInPlace(X_FB.R()); // get everything in B
1150  return *this;
1151 }
1152 
1155 SpatialInertia_& transformInPlace(const InverseTransform_<P>& X_FB) {
1156  shiftInPlace(X_FB.p()); // shift to the new origin OB.
1157  reexpressInPlace(X_FB.R()); // get everything in B
1158  return *this;
1159 }
1160 
1161 const SpatialMatP toSpatialMat() const {
1162  Mat33P offDiag = crossMat(m*p);
1163  return SpatialMatP(m*G.toMat33(), offDiag,
1164  -offDiag, Mat33P(m));
1165 }
1166 
1167 private:
1168 RealP m; // mass of this rigid body F
1169 Vec3P p; // location of body's COM from OF, expressed in F
1170 UnitInertiaP G; // mass distribution; inertia is mass*gyration
1171 
1172 static P nanP() {return NTraits<P>::getNaN();}
1173 };
1174 
1177 template <class P> inline SpatialInertia_<P>
1179 { return SpatialInertia_<P>(l) += r; }
1180 
1184 template <class P> inline SpatialInertia_<P>
1186 { return SpatialInertia_<P>(l) -= r; }
1187 
1188 
1189 // -----------------------------------------------------------------------------
1190 // ARTICULATED BODY INERTIA MATRIX
1191 // -----------------------------------------------------------------------------
1238 template <class P>
1239 class ArticulatedInertia_ {
1240  typedef P RealP;
1241  typedef Vec<3,P> Vec3P;
1242  typedef UnitInertia_<P> UnitInertiaP;
1243  typedef Mat<3,3,P> Mat33P;
1244  typedef SymMat<3,P> SymMat33P;
1245  typedef Vec<2, Vec3P> SpatialVecP;
1246  typedef Mat<2,2,Mat33P> SpatialMatP;
1247  typedef Rotation_<P> RotationP;
1248  typedef Transform_<P> TransformP;
1249  typedef Inertia_<P> InertiaP;
1250 public:
1255 ArticulatedInertia_(const SymMat33P& mass, const Mat33P& massMoment, const SymMat33P& inertia)
1256 : M(mass), J(inertia), F(massMoment) {}
1257 
1261 : M(rbi.getMass()), J(rbi.calcInertia()), F(crossMat(rbi.calcMassMoment())) {}
1262 
1264 ArticulatedInertia_& setMass (const SymMat33P& mass) {M=mass; return *this;}
1266 ArticulatedInertia_& setMassMoment(const Mat33P& massMoment) {F=massMoment; return *this;}
1268 ArticulatedInertia_& setInertia (const SymMat33P& inertia) {J=inertia; return *this;}
1269 
1271 const SymMat33P& getMass() const {return M;}
1273 const Mat33P& getMassMoment() const {return F;}
1275 const SymMat33P& getInertia() const {return J;}
1276 
1277 // default destructor, copy constructor, copy assignment
1278 
1282 { M+=src.M; J+=src.J; F+=src.F; return *this; }
1286 { M-=src.M; J-=src.J; F-=src.F; return *this; }
1287 
1289 SpatialVecP operator*(const SpatialVecP& v) const
1290 { return SpatialVecP(J*v[0]+F*v[1], ~F*v[0]+M*v[1]); }
1291 
1293 template <int N>
1295  Mat<2,N,Vec3P> res;
1296  for (int j=0; j < N; ++j)
1297  res.col(j) = (*this) * m.col(j); // above this*SpatialVec operator
1298  return res;
1299 }
1300 
1311 SimTK_SimTKCOMMON_EXPORT ArticulatedInertia_ shift(const Vec3P& s) const;
1312 
1315 SimTK_SimTKCOMMON_EXPORT ArticulatedInertia_& shiftInPlace(const Vec3P& s);
1316 
1319 const SpatialMatP toSpatialMat() const {
1320  return SpatialMatP( Mat33P(J), F,
1321  ~F, Mat33P(M) );
1322 }
1323 private:
1324 SymMat33P M;
1325 SymMat33P J;
1326 Mat33P F;
1327 };
1328 
1331 template <class P> inline ArticulatedInertia_<P>
1333 { return ArticulatedInertia_<P>(l) += r; }
1334 
1338 template <class P> inline ArticulatedInertia_<P>
1340 { return ArticulatedInertia_<P>(l) -= r; }
1341 
1342 
1343 // -----------------------------------------------------------------------------
1344 // MASS PROPERTIES
1345 // -----------------------------------------------------------------------------
1358 template <class P>
1359 class SimTK_SimTKCOMMON_EXPORT MassProperties_ {
1360  typedef P RealP;
1361  typedef Vec<3,P> Vec3P;
1362  typedef UnitInertia_<P> UnitInertiaP;
1363  typedef Mat<3,3,P> Mat33P;
1364  typedef Mat<6,6,P> Mat66P;
1365  typedef SymMat<3,P> SymMat33P;
1366  typedef Mat<2,2,Mat33P> SpatialMatP;
1367  typedef Rotation_<P> RotationP;
1368  typedef Transform_<P> TransformP;
1369  typedef Inertia_<P> InertiaP;
1370 public:
1373 MassProperties_() { setMassProperties(0,Vec3P(0),UnitInertiaP()); }
1377 MassProperties_(const RealP& m, const Vec3P& com, const InertiaP& inertia)
1378  { setMassProperties(m,com,inertia); }
1381 MassProperties_(const RealP& m, const Vec3P& com, const UnitInertiaP& gyration)
1382  { setMassProperties(m,com,gyration); }
1383 
1387 MassProperties_& setMassProperties(const RealP& m, const Vec3P& com, const InertiaP& inertia) {
1388  mass = m;
1389  comInB = com;
1390  if (m == 0) {
1391  SimTK_ASSERT(inertia.asSymMat33().getAsVec() == 0, "Mass is zero but inertia is nonzero");
1392  unitInertia_OB_B = UnitInertiaP(0);
1393  }
1394  else {
1395  unitInertia_OB_B = UnitInertiaP(inertia*(1/m));
1396  }
1397  return *this;
1398 }
1399 
1402 MassProperties_& setMassProperties
1403  (const RealP& m, const Vec3P& com, const UnitInertiaP& gyration)
1404 { mass=m; comInB=com; unitInertia_OB_B=gyration; return *this; }
1405 
1407 const RealP& getMass() const {return mass;}
1412 const Vec3P& getMassCenter() const {return comInB;}
1417 const UnitInertiaP& getUnitInertia() const {return unitInertia_OB_B;}
1423 const InertiaP calcInertia() const {return mass*unitInertia_OB_B;}
1428 const InertiaP getInertia() const {return calcInertia();}
1429 
1433 InertiaP calcCentralInertia() const {
1434  return mass*unitInertia_OB_B - InertiaP(comInB, mass);
1435 }
1440 InertiaP calcShiftedInertia(const Vec3P& newOriginB) const {
1441  return calcCentralInertia() + InertiaP(newOriginB-comInB, mass);
1442 }
1447 InertiaP calcTransformedInertia(const TransformP& X_BC) const {
1448  return calcShiftedInertia(X_BC.p()).reexpress(X_BC.R());
1449 }
1455 MassProperties_ calcShiftedMassProps(const Vec3P& newOriginB) const {
1456  return MassProperties_(mass, comInB-newOriginB,
1457  calcShiftedInertia(newOriginB));
1458 }
1459 
1469 MassProperties_ calcTransformedMassProps(const TransformP& X_BC) const {
1470  return MassProperties_(mass, ~X_BC*comInB, calcTransformedInertia(X_BC));
1471 }
1472 
1481 MassProperties_ reexpress(const RotationP& R_BC) const {
1482  return MassProperties_(mass, ~R_BC*comInB, unitInertia_OB_B.reexpress(R_BC));
1483 }
1484 
1488 bool isExactlyMassless() const { return mass==0; }
1494 bool isNearlyMassless(const RealP& tol=SignificantReal) const {
1495  return mass <= tol;
1496 }
1497 
1501 bool isExactlyCentral() const { return comInB==Vec3P(0); }
1507 bool isNearlyCentral(const RealP& tol=SignificantReal) const {
1508  return comInB.normSqr() <= tol*tol;
1509 }
1510 
1513 bool isNaN() const {return SimTK::isNaN(mass) || comInB.isNaN() || unitInertia_OB_B.isNaN();}
1518 bool isInf() const {
1519  if (isNaN()) return false;
1520  return SimTK::isInf(mass) || comInB.isInf() || unitInertia_OB_B.isInf();
1521 }
1525 bool isFinite() const {
1526  return SimTK::isFinite(mass) && comInB.isFinite() && unitInertia_OB_B.isFinite();
1527 }
1528 
1532 SpatialMatP toSpatialMat() const {
1533  SpatialMatP M;
1534  M(0,0) = mass*unitInertia_OB_B.toMat33();
1535  M(0,1) = mass*crossMat(comInB);
1536  M(1,0) = ~M(0,1);
1537  M(1,1) = mass; // a diagonal matrix
1538  return M;
1539 }
1540 
1546 Mat66P toMat66() const {
1547  Mat66P M;
1548  M.template updSubMat<3,3>(0,0) = mass*unitInertia_OB_B.toMat33();
1549  M.template updSubMat<3,3>(0,3) = mass*crossMat(comInB);
1550  M.template updSubMat<3,3>(3,0) = ~M.template getSubMat<3,3>(0,3);
1551  M.template updSubMat<3,3>(3,3) = mass; // a diagonal matrix
1552  return M;
1553 }
1554 
1555 private:
1556 RealP mass;
1557 Vec3P comInB; // meas. from B origin, expr. in B
1558 UnitInertiaP unitInertia_OB_B; // about B origin, expr. in B
1559 };
1560 
1570 template <class P> static inline std::ostream&
1571 operator<<(std::ostream& o, const MassProperties_<P>& mp) {
1572  return o << "{ mass=" << mp.getMass()
1573  << "\n com=" << mp.getMassCenter()
1574  << "\n Uxx,yy,zz=" << mp.getUnitInertia().getMoments()
1575  << "\n Uxy,xz,yz=" << mp.getUnitInertia().getProducts()
1576  << "\n}\n";
1577 }
1578 
1579 } // namespace SimTK
1580 
1581 #endif // SimTK_SIMMATRIX_MASS_PROPERTIES_H_
Inertia_ & shiftToMassCenterInPlace(const Vec3P &CF, const RealP &mass)
Assume that the current inertia is about the F frame&#39;s origin OF, and expressed in F...
Definition: MassProperties.h:335
bool isFinite() const
Definition: MassProperties.h:416
Vec< 2, Vec< 3, float > > fSpatialVec
A SpatialVec that is always single (float) precision regardless of the compiled-in precision of Real...
Definition: MassProperties.h:53
Vec< 3, P > p() const
Calculate the actual translation vector at a cost of 18 flops.
Definition: Transform.h:373
const InverseRotation_< P > & R() const
Definition: Transform.h:360
Inertia_< P > operator/(const Inertia_< P > &i, const P &r)
Divide an inertia matrix by a scalar.
Definition: MassProperties.h:616
bool isNumericallySymmetric(double tol=getDefaultTolerance()) const
A Matrix is symmetric (actually Hermitian) if it is square and each element (i,j) is the Hermitian tr...
Definition: Mat.h:1172
Vec< 2, Vec3 > SpatialVec
Spatial vectors are used for (rotation,translation) quantities and consist of a pair of Vec3 objects...
Definition: MassProperties.h:50
SpatialInertia_ transform(const InverseTransform_< P > &X_FB) const
Transform using an inverse transform to avoid having to convert it.
Definition: MassProperties.h:1135
UnitInertia_ & reexpressInPlace(const Rotation_< P > &R_FB)
Re-express this unit inertia matrix in another frame, changing the object in place; see reexpress() i...
Definition: MassProperties.h:824
Inertia_(const RealP &xx, const RealP &yy, const RealP &zz)
Create a principal inertia matrix (only non-zero on diagonal).
Definition: MassProperties.h:230
ArticulatedInertia_< double > dArticulatedInertia
An articulated body inertia matrix at double precision.
Definition: MassProperties.h:123
#define SimTK_SimTKCOMMON_EXPORT
Definition: SimTKcommon/include/SimTKcommon/internal/common.h:218
UnitInertia_ & setFromUnitInertia(const Inertia_< P > &inertia)
Set from a unit inertia matrix.
Definition: MassProperties.h:844
(Advanced) This InverseRotation class is the inverse of a Rotation.
Definition: Rotation.h:47
Inertia_ & setInertia(const RealP &xx, const RealP &yy, const RealP &zz, const RealP &xy, const RealP &xz, const RealP &yz)
Set this Inertia to a general matrix.
Definition: MassProperties.h:282
bool operator==(const Inertia_< P > &i1, const Inertia_< P > &i2)
Compare two inertia matrices for exact (bitwise) equality.
Definition: MassProperties.h:638
InertiaP calcCentralInertia() const
Return the inertia of this MassProperties object, but measured about the mass center rather than abou...
Definition: MassProperties.h:1433
UnitInertia_ shiftToCentroid(const Vec3P &CF) const
Assuming that this unit inertia matrix is currently taken about some (implicit) frame F&#39;s origin OF...
Definition: MassProperties.h:757
static UnitInertia_ ellipsoid(const RealP &hx, const RealP &hy, const RealP &hz)
Unit-mass ellipsoid given by half-lengths in each direction.
Definition: MassProperties.h:912
const TLower & getLower() const
Definition: SymMat.h:825
Vec< 3, P > operator*(const Inertia_< P > &I, const Vec< 3, P > &w)
Multiply an inertia matrix I on the right by a vector w giving the vector result I*w.
Definition: MassProperties.h:630
const Vec3P & getMassCenter() const
Return the mass center currently stored in this MassProperties object; this is expressed in an implic...
Definition: MassProperties.h:1412
bool isExactlyMassless() const
Return true only if the mass stored here is exactly zero. If the mass resulted from a computation...
Definition: MassProperties.h:1488
SpatialInertia_< double > dSpatialInertia
A spatial (rigid body) inertia matrix at double precision.
Definition: MassProperties.h:116
UnitInertia_(const Inertia_< P > &inertia)
Construct a UnitInertia matrix from an Inertia matrix.
Definition: MassProperties.h:722
static Inertia_ cylinderAlongY(const RealP &r, const RealP &hy)
Unit-mass cylinder aligned along y axis; use radius and half-length.
Definition: MassProperties.h:943
bool isInf() const
Return true only if there are no NaN&#39;s in this MassProperties object, and at least one of the element...
Definition: MassProperties.h:1518
Inertia_ & operator+=(const Inertia_ &inertia)
Add in another inertia matrix.
Definition: MassProperties.h:292
SpatialInertia_ & reexpressInPlace(const Rotation_< P > &R_FB)
Re-express this SpatialInertia in another frame, modifying the original object.
Definition: MassProperties.h:1095
MassProperties_< float > fMassProperties
Rigid body mass properties at float precision.
Definition: MassProperties.h:107
Inertia_ & setInertia(const RealP &xx, const RealP &yy, const RealP &zz)
Set an inertia matrix to have only principal moments (that is, it will be diagonal).
Definition: MassProperties.h:262
This is the top-level SimTK namespace into which all SimTK names are placed to avoid collision with o...
Definition: Assembler.h:37
static UnitInertia_ brick(const Vec3P &halfLengths)
Alternate interface to brick() that takes a Vec3 for the half lengths.
Definition: MassProperties.h:908
Inertia_< P > operator*(const P &r, const Inertia_< P > &i)
Multiply an inertia matrix by a scalar.
Definition: MassProperties.h:594
These are numerical utility classes for dealing with the relative orientations of geometric objects...
Inertia_(const Vec3P &moments, const Vec3P &products=Vec3P(0))
Create an inertia matrix from a vector of the moments of inertia (the inertia matrix diagonal) and op...
Definition: MassProperties.h:224
SpatialMatP toSpatialMat() const
Convert this MassProperties object to a spatial inertia matrix and return it as a SpatialMat...
Definition: MassProperties.h:1532
UnitInertia_ reexpress(const InverseRotation_< P > &R_FB) const
Rexpress using an inverse rotation to avoid having to convert it.
Definition: MassProperties.h:817
Transform from frame B to frame F, but with the internal representation inverted. ...
Definition: Transform.h:44
ArticulatedInertia_< Real > ArticulatedInertia
An articulated body inertia matrix at default precision.
Definition: MassProperties.h:119
Row< 2, Row< 3, float > > fSpatialRow
A SpatialRow that is always single (float) precision regardless of the compiled-in precision of Real...
Definition: MassProperties.h:63
SpatialInertia_ reexpress(const InverseRotation_< P > &R_FB) const
Rexpress using an inverse rotation to avoid having to convert it.
Definition: MassProperties.h:1088
The Rotation class is a Mat33 that guarantees that the matrix can be interpreted as a legitimate 3x3 ...
Definition: Quaternion.h:40
const UnitInertiaP & getUnitInertia() const
Definition: MassProperties.h:1027
Inertia_< P > operator/(const Inertia_< P > &i, int r)
Divide an inertia matrix by a scalar provided as an int.
Definition: MassProperties.h:623
bool isFinite() const
Return true if none of the elements of this MassProperties object are NaN or Infinity. Note that Ground&#39;s mass properties are not finite.
Definition: MassProperties.h:1525
bool isNaN() const
Definition: MassProperties.h:414
bool isNearlyMassless(const RealP &tol=SignificantReal) const
Return true if the mass stored here is zero to within a small tolerance.
Definition: MassProperties.h:1494
SpatialInertia_ & reexpressInPlace(const InverseRotation_< P > &R_FB)
Rexpress using an inverse rotation to avoid having to convert it.
Definition: MassProperties.h:1100
ArticulatedInertia_()
Default construction produces uninitialized junk at zero cost; be sure to fill this in before referen...
Definition: MassProperties.h:1253
bool isNumericallyEqual(const Inertia_< P > &other) const
Compare this inertia matrix with another one and return true if they are close to within a default nu...
Definition: MassProperties.h:421
const SymMat33P & asSymMat33() const
Obtain a reference to the underlying symmetric matrix type.
Definition: MassProperties.h:401
UnitInertia_(const RealP &moment)
Create a principal unit inertia matrix with identical diagonal elements.
Definition: MassProperties.h:690
#define SimTK_ASSERT(cond, msg)
Definition: ExceptionMacros.h:374
const SymMat33P & getMass() const
Get the mass distribution matrix M from this ArticulatedInertia (symmetric).
Definition: MassProperties.h:1271
ArticulatedInertia_(const SpatialInertia_< P > &rbi)
Construct an articulated body inertia (ABI) from a rigid body spatial inertia (RBI).
Definition: MassProperties.h:1260
const RealP & getMass() const
Return the mass currently stored in this MassProperties object.
Definition: MassProperties.h:1407
static UnitInertia_ cylinderAlongZ(const RealP &r, const RealP &hz)
Unit-mass cylinder aligned along z axis; use radius and half-length.
Definition: MassProperties.h:879
UnitInertia_(const Mat33P &m)
Construct a UnitInertia from a 3x3 symmetric matrix.
Definition: MassProperties.h:716
Inertia_ & shiftFromMassCenterInPlace(const Vec3P &p, const RealP &mass)
Assuming that the current inertia I is a central inertia (that is, it is inertia about the body cente...
Definition: MassProperties.h:360
SpatialInertia_ & transformInPlace(const InverseTransform_< P > &X_FB)
Transform using an inverse transform to avoid having to convert it.
Definition: MassProperties.h:1155
SpatialInertia_ & operator/=(const RealP &s)
Divide a SpatialInertia by a scalar.
Definition: MassProperties.h:1071
This class contains the mass, center of mass, and unit inertia matrix of a rigid body B...
Definition: MassProperties.h:85
MassProperties_ & setMassProperties(const RealP &m, const Vec3P &com, const InertiaP &inertia)
Set mass, center of mass, and inertia.
Definition: MassProperties.h:1387
MassProperties_< double > dMassProperties
Rigid body mass properties at double precision.
Definition: MassProperties.h:109
const UnitInertia_< P > & getAsUnitInertia() const
Definition: MassProperties.h:517
Row< 2, Row3 > SpatialRow
This is the type of a transposed SpatialVec; it does not usually appear explicitly in user programs...
Definition: MassProperties.h:60
static Inertia_ sphere(const RealP &r)
Create a UnitInertia matrix for a unit mass sphere of radius r centered at the origin.
Definition: MassProperties.h:937
const UnitInertiaP & getUnitInertia() const
Return the unit inertia currently stored in this MassProperties object; this is expressed in an impli...
Definition: MassProperties.h:1417
static Inertia_ ellipsoid(const RealP &hx, const RealP &hy, const RealP &hz)
Unit-mass ellipsoid given by half-lengths in each direction.
Definition: MassProperties.h:955
ArticulatedInertia_ & setMass(const SymMat33P &mass)
Set the mass distribution matrix M in this ArticulatedInertia (symmetric).
Definition: MassProperties.h:1264
UnitInertia_ & shiftToCentroidInPlace(const Vec3P &CF)
Assuming that this unit inertia matrix is currently taken about some (implicit) frame F&#39;s origin OF...
Definition: MassProperties.h:774
Inertia_(const SymMat33P &inertia)
Construct an Inertia from a symmetric 3x3 matrix.
Definition: MassProperties.h:247
Inertia_ reexpress(const InverseRotation_< P > &R_FB) const
Rexpress using an inverse rotation to avoid having to convert it.
Definition: MassProperties.h:380
SymMat< 3, E > crossMatSq(const Vec< 3, E, S > &v)
Calculate matrix S(v) such that S(v)*w = -v % (v % w) = (v % w) % v.
Definition: SmallMatrixMixed.h:716
SpatialInertia_ shift(const Vec3P &S) const
Return a new SpatialInertia object which is the same as this one except the origin ("taken about" poi...
Definition: MassProperties.h:1107
MassProperties_ calcTransformedMassProps(const TransformP &X_BC) const
Transform these mass properties from the current frame "B" to a new frame "C", given the pose of C in...
Definition: MassProperties.h:1469
Inertia_ & reexpressInPlace(const Rotation_< P > &R_FB)
Re-express this inertia matrix in another frame, changing the object in place; see reexpress() if you...
Definition: MassProperties.h:387
static bool isValidInertiaMatrix(const SymMat33P &m)
Test some conditions that must hold for a valid Inertia matrix.
Definition: MassProperties.h:432
UnitInertia_ & reexpressInPlace(const InverseRotation_< P > &R_FB)
Rexpress using an inverse rotation to avoid having to convert it.
Definition: MassProperties.h:829
UnitInertia_< P > & updAsUnitInertia()
Definition: MassProperties.h:519
Inertia_ shiftFromMassCenter(const Vec3P &p, const RealP &mass) const
Assuming that the current inertia I is a central inertia (that is, it is inertia about the body cente...
Definition: MassProperties.h:347
SpatialInertia_ & transformInPlace(const Transform_< P > &X_FB)
Transform this SpatialInertia object so that it is measured about and expressed in a new frame...
Definition: MassProperties.h:1147
UnitInertia_(const RealP &xx, const RealP &yy, const RealP &zz)
Create a principal unit inertia matrix (only non-zero on diagonal).
Definition: MassProperties.h:700
SpatialVecP operator*(const SpatialVecP &v) const
Multiply an ArticulatedIneria by a SpatialVec (66 flops).
Definition: MassProperties.h:1289
bool isFinite(const negator< float > &x)
Definition: negator.h:287
Inertia_< P > operator-(const Inertia_< P > &l, const Inertia_< P > &r)
Subtract from one inertia matrix another one which is compatible, meaning that both must be taken abo...
Definition: MassProperties.h:582
const Vec3P & getMassCenter() const
Definition: MassProperties.h:1026
static UnitInertia_ cylinderAlongX(const RealP &r, const RealP &hx)
Unit-mass cylinder aligned along x axis; use radius and half-length.
Definition: MassProperties.h:893
bool isInf() const
Definition: MassProperties.h:415
static Inertia_ cylinderAlongX(const RealP &r, const RealP &hx)
Unit-mass cylinder aligned along x axis; use radius and half-length.
Definition: MassProperties.h:946
const SymMat33P & getInertia() const
Get the mass second moment (inertia) matrix J from this ArticulatedInertia (symmetric).
Definition: MassProperties.h:1275
EStandard sum() const
Sum just adds up all the elements into a single return element that is the same type as this Vec&#39;s el...
Definition: Vec.h:364
This is a fixed-length column vector designed for no-overhead inline computation. ...
Definition: SimTKcommon/include/SimTKcommon/internal/common.h:605
const Complex I
We only need one complex constant, i = sqrt(-1). For the rest just multiply the real constant by i...
SpatialInertia_< Real > SpatialInertia
A spatial (rigid body) inertia matrix at default precision.
Definition: MassProperties.h:112
SpatialVecP operator*(const SpatialVecP &v) const
Multiply a SpatialInertia by a SpatialVec to produce a SpatialVec result; 45 flops.
Definition: MassProperties.h:1075
SpatialInertia_< P > operator+(const SpatialInertia_< P > &l, const SpatialInertia_< P > &r)
Add two compatible spatial inertias.
Definition: MassProperties.h:1178
static UnitInertia_ sphere(const RealP &r)
Create a UnitInertia matrix for a unit mass sphere of radius r centered at the origin.
Definition: MassProperties.h:875
SymMat33P I_OF_F
Definition: MassProperties.h:565
const SpatialMatP toSpatialMat() const
Definition: MassProperties.h:1161
const InertiaP calcInertia() const
Return the inertia matrix for this MassProperties object; this is equal to the unit inertia times the...
Definition: MassProperties.h:1423
UnitInertia_(const SymMat33P &m)
Construct a UnitInertia from a symmetric 3x3 matrix.
Definition: MassProperties.h:711
static Inertia_ brick(const RealP &hx, const RealP &hy, const RealP &hz)
Unit-mass brick given by half-lengths in each direction.
Definition: MassProperties.h:949
UnitInertia_(const Vec3P &moments, const Vec3P &products=Vec3P(0))
Create a unit inertia matrix from a vector of the moments of inertia (the inertia matrix diagonal) an...
Definition: MassProperties.h:696
const Inertia_< P > & asUnitInertia() const
Recast this UnitInertia matrix as a unit inertia matrix.
Definition: MassProperties.h:839
SpatialInertia_ & shiftInPlace(const Vec3P &S)
Change origin from OF to OF+S, modifying the original object in place.
Definition: MassProperties.h:1114
const TAsVec & getAsVec() const
Definition: SymMat.h:831
SpatialInertia_(RealP mass, const Vec3P &com, const UnitInertiaP &gyration)
Definition: MassProperties.h:1011
Inertia_< P > operator*(const Inertia_< P > &i, const P &r)
Multiply an inertia matrix by a scalar.
Definition: MassProperties.h:588
ArticulatedInertia_(const SymMat33P &mass, const Mat33P &massMoment, const SymMat33P &inertia)
Construct an ArticulatedInertia from the mass, first moment, and inertia matrices it contains...
Definition: MassProperties.h:1255
Inertia_(const Vec3P &p, const RealP &mass)
Create an Inertia matrix for a point mass at a given location, measured from the origin OF of the imp...
Definition: MassProperties.h:218
Inertia_ & operator*=(const P &s)
Multiply this inertia matrix by a scalar. Cost is 6 flops.
Definition: MassProperties.h:305
const InertiaP getInertia() const
OBSOLETE – this is just here for compatibility with 2.2; since the UnitInertia is stored now the ful...
Definition: MassProperties.h:1428
#define SimTK_ERRCHK1(cond, whereChecked, fmt, a1)
Definition: ExceptionMacros.h:326
ArticulatedInertia_ & operator-=(const ArticulatedInertia_ &src)
Subtract a compatible ArticulatedInertia from this one.
Definition: MassProperties.h:1285
This is a user-includable header which includes everything needed to make use of SimMatrix Scalar cod...
UnitInertia_ & setUnitInertia(const RealP &xx, const RealP &yy, const RealP &zz)
Set a UnitInertia matrix to have only principal moments (that is, it will be diagonal).
Definition: MassProperties.h:727
static Inertia_ cylinderAlongZ(const RealP &r, const RealP &hz)
Unit-mass cylinder aligned along z axis; use radius and half-length.
Definition: MassProperties.h:940
MassProperties_ reexpress(const RotationP &R_BC) const
Re-express these mass properties from the current frame "B" to a new frame "C", given the orientation...
Definition: MassProperties.h:1481
ArticulatedInertia_ & operator+=(const ArticulatedInertia_ &src)
Add in a compatible ArticulatedInertia to this one.
Definition: MassProperties.h:1281
bool isNearlyCentral(const RealP &tol=SignificantReal) const
Return true if the mass center stored here is zero to within a small tolerance.
Definition: MassProperties.h:1507
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:1055
ELEM max(const VectorBase< ELEM > &v)
Definition: VectorMath.h:251
SpatialInertia_< float > fSpatialInertia
A spatial (rigid body) inertia matrix at float precision.
Definition: MassProperties.h:114
Mat< 2, 2, Mat< 3, 3, float > > fSpatialMat
A SpatialMat that is always single (float) precision regardless of the compiled-in precision of Real...
Definition: MassProperties.h:75
Inertia_(const Mat33P &m)
Construct an Inertia matrix from a 3x3 symmetric matrix.
Definition: MassProperties.h:253
const Vec3P & getMoments() const
Obtain the inertia moments (diagonal of the Inertia matrix) as a Vec3 ordered xx, yy...
Definition: MassProperties.h:409
SpatialInertia_ reexpress(const Rotation_< P > &R_FB) const
Return a new SpatialInertia object which is the same as this one except re-expressed in another coord...
Definition: MassProperties.h:1083
UnitInertia Gyration
For backwards compatibility only; use UnitInertia instead.
Definition: MassProperties.h:126
Mat< 2, N, Vec3P > operator*(const Mat< 2, N, Vec3P > &m) const
Multiply an ArticulatedInertia by a row of SpatialVecs (66*N flops).
Definition: MassProperties.h:1294
SpatialInertia_ transform(const Transform_< P > &X_FB) const
Return a new SpatialInertia object which is the same as this one but measured about and expressed in ...
Definition: MassProperties.h:1130
MassProperties_< Real > MassProperties
Rigid body mass properties at default precision.
Definition: MassProperties.h:105
#define SimTK_ERRCHK(cond, whereChecked, msg)
Definition: ExceptionMacros.h:324
InertiaP calcTransformedInertia(const TransformP &X_BC) const
Return the inertia of this MassProperties object, but transformed to from the implicit B frame to a n...
Definition: MassProperties.h:1447
static UnitInertia_ brick(const RealP &hx, const RealP &hy, const RealP &hz)
Unit-mass brick given by half-lengths in each direction.
Definition: MassProperties.h:901
void errChk(const char *methodName) const
Definition: MassProperties.h:526
This file is the user-includeable header to be included in user programs to provide fixed-length Vec ...
static UnitInertia_ pointMassAt(const Vec3P &p)
Create a UnitInertia matrix for a point of unit mass located at a given location measured from origin...
Definition: MassProperties.h:870
Inertia_(const RealP &moment)
Create a principal inertia matrix with identical diagonal elements, like a sphere where moment=2/5 m ...
Definition: MassProperties.h:212
const SpatialMatP toSpatialMat() const
Convert the compactly-stored ArticulatedInertia (21 elements) into a full SpatialMat with 36 elements...
Definition: MassProperties.h:1319
ArticulatedInertia_< P > operator+(const ArticulatedInertia_< P > &l, const ArticulatedInertia_< P > &r)
Add two compatible articulated inertias.
Definition: MassProperties.h:1332
InertiaP calcShiftedInertia(const Vec3P &newOriginB) const
Return the inertia of this MassProperties object, but with the "measured about" point shifted from th...
Definition: MassProperties.h:1440
Inertia_< P > operator*(int r, const Inertia_< P > &i)
Multiply an inertia matrix by a scalar given as an int.
Definition: MassProperties.h:609
const Vec< 3, P > & p() const
Return a read-only reference to our translation vector p_BF.
Definition: Transform.h:239
Inertia_< P > operator+(const Inertia_< P > &l, const Inertia_< P > &r)
Add two compatible inertia matrices, meaning they must be taken about the same point and expressed in...
Definition: MassProperties.h:573
MassProperties_ calcShiftedMassProps(const Vec3P &newOriginB) const
Return a new MassProperties object that is the same as this one but with the origin point shifted fro...
Definition: MassProperties.h:1455
MassProperties_(const RealP &m, const Vec3P &com, const UnitInertiaP &gyration)
Create a mass properties object from individually supplied mass, mass center, and unit inertia (gyrat...
Definition: MassProperties.h:1381
Inertia_< P > operator*(const Inertia_< P > &i, int r)
Multiply an inertia matrix by a scalar given as an int.
Definition: MassProperties.h:602
RowVectorBase< typename CNT< ELEM >::TAbs > abs(const RowVectorBase< ELEM > &v)
Definition: VectorMath.h:120
bool isNaN(const negator< float > &x)
Definition: negator.h:273
UnitInertia_ & setUnitInertia(const RealP &xx, const RealP &yy, const RealP &zz, const RealP &xy, const RealP &xz, const RealP &yz)
Set this UnitInertia to a general matrix.
Definition: MassProperties.h:740
Mat< 2, 2, Mat33 > SpatialMat
Spatial matrices are used to hold 6x6 matrices that are best viewed as 2x2 matrices of 3x3 matrices; ...
Definition: MassProperties.h:72
SpatialInertia_ & setMassCenter(const Vec3P &com)
Definition: MassProperties.h:1020
const Mat33P & getMassMoment() const
Get the mass first moment distribution matrix F from this ArticulatedInertia (full).
Definition: MassProperties.h:1273
Mat< 3, 3, E > crossMat(const Vec< 3, E, S > &v)
Calculate matrix M(v) such that M(v)*w = v % w.
Definition: SmallMatrixMixed.h:649
The physical meaning of an inertia is the distribution of a rigid body&#39;s mass about a particular poin...
Definition: MassProperties.h:82
bool isExactlyCentral() const
Return true only if the mass center stored here is exactly zero. If the mass center resulted from a c...
Definition: MassProperties.h:1501
Inertia_ & reexpressInPlace(const InverseRotation_< P > &R_FB)
Rexpress in place using an inverse rotation to avoid having to convert it.
Definition: MassProperties.h:392
ArticulatedInertia_< float > fArticulatedInertia
An articulated body inertia matrix at float precision.
Definition: MassProperties.h:121
This is a fixed-length row vector designed for no-overhead inline computation.
Definition: SimTKcommon/include/SimTKcommon/internal/common.h:606
bool isInf(const negator< float > &x)
Definition: negator.h:301
A UnitInertia matrix is a unit-mass inertia matrix; you can convert it to an Inertia by multiplying i...
Definition: MassProperties.h:81
const TDiag & diag() const
Definition: SymMat.h:822
bool isNaN() const
Return true if any element of this MassProperties object is NaN.
Definition: MassProperties.h:1513
const Real SignificantReal
SignificantReal is the smallest value that we consider to be clearly distinct from roundoff error whe...
SpatialInertia_ & operator*=(const RealP &s)
Multiply a SpatialInertia by a scalar.
Definition: MassProperties.h:1067
Inertia_< float > fInertia
An inertia tensor at float precision.
Definition: MassProperties.h:100
Inertia_ & operator-=(const Inertia_ &inertia)
Subtract off another inertia matrix.
Definition: MassProperties.h:299
const TCol & col(int j) const
Definition: Mat.h:775
Row< 2, Row< 3, double > > dSpatialRow
A SpatialRow that is always double precision regardless of the compiled-in precision of Real...
Definition: MassProperties.h:66
UnitInertia_< float > fUnitInertia
A unit inertia (gyration) tensor at float precision.
Definition: MassProperties.h:93
static Inertia_ pointMassAt(const Vec3P &p, const RealP &m)
Create an Inertia matrix for a point of a given mass, located at a given location measured from the o...
Definition: MassProperties.h:462
ArticulatedInertia_ & setInertia(const SymMat33P &inertia)
Set the mass second moment (inertia) matrix J in this ArticulatedInertia (symmetric).
Definition: MassProperties.h:1268
Mat< 2, 2, Mat< 3, 3, double > > dSpatialMat
A SpatialMat that is always double precision regardless of the compiled-in precision of Real...
Definition: MassProperties.h:78
RealP getMass() const
Definition: MassProperties.h:1025
UnitInertia_ & setUnitInertia(const Vec3P &moments, const Vec3P &products=Vec3P(0))
Set principal moments and optionally off-diagonal terms.
Definition: MassProperties.h:732
SpatialInertia_< P > operator-(const SpatialInertia_< P > &l, const SpatialInertia_< P > &r)
Subtract one compatible spatial inertia from another.
Definition: MassProperties.h:1185
UnitInertia_(const RealP &xx, const RealP &yy, const RealP &zz, const RealP &xy, const RealP &xz, const RealP &yz)
This is a general unit inertia matrix.
Definition: MassProperties.h:705
static UnitInertia_ ellipsoid(const Vec3P &halfLengths)
Alternate interface to ellipsoid() that takes a Vec3 for the half lengths.
Definition: MassProperties.h:918
SpatialInertia_ & setUnitInertia(const UnitInertiaP &gyration)
Definition: MassProperties.h:1022
static Inertia_ pointMassAtOrigin()
Create an Inertia matrix for a point located at the origin – that is, an all-zero matrix...
Definition: MassProperties.h:456
A spatial inertia contains the mass, center of mass point, and inertia matrix for a rigid body...
Definition: MassProperties.h:83
Vec< 2, Vec< 3, double > > dSpatialVec
A SpatialVec that is always double precision regardless of the compiled-in precision of Real...
Definition: MassProperties.h:56
SpatialInertia_ & operator+=(const SpatialInertia_ &src)
Add in a compatible SpatialInertia.
Definition: MassProperties.h:1041
ArticulatedInertia_< P > operator-(const ArticulatedInertia_< P > &l, const ArticulatedInertia_< P > &r)
Subtract one compatible articulated inertia from another.
Definition: MassProperties.h:1339
UnitInertia_< double > dUnitInertia
A unit inertia (gyration) tensor at double precision.
Definition: MassProperties.h:95
SpatialInertia_ & setMass(RealP mass)
Definition: MassProperties.h:1016
static bool isValidUnitInertiaMatrix(const SymMat33P &m)
Test some conditions that must hold for a valid UnitInertia matrix.
Definition: MassProperties.h:851
UnitInertia_ & shiftFromCentroidInPlace(const Vec3P &p)
Assuming that the current UnitInertia G is a central inertia (that is, it is inertia about the body c...
Definition: MassProperties.h:798
Inertia_< double > dInertia
An inertia tensor at double precision.
Definition: MassProperties.h:102
MassProperties_()
Create a mass properties object in which the mass, mass center, and inertia are meaningless; you must...
Definition: MassProperties.h:1373
bool isNaN() const
Return true if any element of this SymMat contains a NaN anywhere.
Definition: SymMat.h:735
An articulated body inertia (ABI) matrix P(q) contains the spatial inertia properties that a body app...
Definition: MassProperties.h:84
Inertia_ & operator/=(const P &s)
Divide this inertia matrix by a scalar.
Definition: MassProperties.h:309
This class represents a small matrix whose size is known at compile time, containing elements of any ...
Definition: SimTKcommon/include/SimTKcommon/internal/common.h:607
InertiaP calcInertia() const
Calculate the inertia matrix (second mass moment, mass-weighted gyration matrix) from the mass and un...
Definition: MassProperties.h:1035
Mat33P toMat33() const
Expand the internal packed representation into a full 3x3 symmetric matrix with all elements set...
Definition: MassProperties.h:405
const Vec3P & getProducts() const
Obtain the inertia products (off-diagonals of the Inertia matrix) as a Vec3 with elements ordered xy...
Definition: MassProperties.h:412
bool isNumericallyEqual(const Inertia_< P > &other, double tol) const
Compare this inertia matrix with another one and return true if they are close to within a specified ...
Definition: MassProperties.h:427
Mat66P toMat66() const
Convert this MassProperties object to a spatial inertia matrix in the form of an ordinary 6x6 matrix...
Definition: MassProperties.h:1546
ArticulatedInertia_ & setMassMoment(const Mat33P &massMoment)
Set the mass first moment distribution matrix F in this ArticulatedInertia (full).
Definition: MassProperties.h:1266
Inertia_ shiftToMassCenter(const Vec3P &CF, const RealP &mass) const
Assume that the current inertia is about the F frame&#39;s origin OF, and expressed in F...
Definition: MassProperties.h:320
Inertia_ & setInertia(const Vec3P &moments, const Vec3P &products=Vec3P(0))
Set principal moments and optionally off-diagonal terms.
Definition: MassProperties.h:270
UnitInertia_< Real > UnitInertia
A unit inertia (gyration) tensor at default precision.
Definition: MassProperties.h:85
Vec3P calcMassMoment() const
Calculate the first mass moment (mass-weighted COM location) from the mass and COM vector...
Definition: MassProperties.h:1031
UnitInertia_()
Default is a NaN-ed out mess to avoid accidents, even in Release mode.
Definition: MassProperties.h:683
Inertia_()
Default is a NaN-ed out mess to avoid accidents, even in Release mode.
Definition: MassProperties.h:202
Inertia_(const RealP &xx, const RealP &yy, const RealP &zz, const RealP &xy, const RealP &xz, const RealP &yz)
This is a general inertia matrix.
Definition: MassProperties.h:238
MassProperties_(const RealP &m, const Vec3P &com, const InertiaP &inertia)
Create a mass properties object from individually supplied mass, mass center, and inertia matrix...
Definition: MassProperties.h:1377
const Rotation_< P > & R() const
Return a read-only reference to the contained rotation R_BF.
Definition: Transform.h:215
UnitInertia_ reexpress(const Rotation_< P > &R_FB) const
Return a new unit inertia matrix like this one but re-expressed in another frame (leaving the origin ...
Definition: MassProperties.h:812
#define SimTK_ERRCHK3(cond, whereChecked, fmt, a1, a2, a3)
Definition: ExceptionMacros.h:330
Inertia_< Real > Inertia
An inertia tensor at default precision.
Definition: MassProperties.h:98
UnitInertia_ shiftFromCentroid(const Vec3P &p) const
Assuming that the current UnitInertia G is a central inertia (that is, it is inertia about the body c...
Definition: MassProperties.h:785
static UnitInertia_ pointMassAtOrigin()
Create a UnitInertia matrix for a point located at the origin – that is, an all-zero matrix...
Definition: MassProperties.h:864
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:886
RealP trace() const
Definition: MassProperties.h:395
SpatialInertia_()
The default constructor fills everything with NaN, even in Release mode.
Definition: MassProperties.h:1009
Inertia_ reexpress(const Rotation_< P > &R_FB) const
Return a new inertia matrix like this one but re-expressed in another frame (leaving the origin point...
Definition: MassProperties.h:375