KrisLibrary  1.0.0
Inertia.h
1 #ifndef ROBOTICS_INERTIA_H
2 #define ROBOTICS_INERTIA_H
3 
5 #include <KrisLibrary/math3d/geometry3d.h>
6 
7 using namespace Math;
8 using namespace Math3D;
9 
10 //computes the inertia matrix I about the point refPt for a rigid body with com at com,
11 //with mass mass, and inertia matrix Icom (about the center of mass).
12 inline void InertiaMatrixAboutPoint(const Vector3& com,Real mass,const Matrix3& Icom,const Vector3& refPt,Matrix3& I)
13 {
14  //I = Icom - mass*[com-refPt]^2
15  Matrix3 temp;
16  temp.setCrossProduct(com-refPt);
17  I.mul(temp,temp);
18  I.inplaceMul(-mass);
19  I += Icom;
20 }
21 
22 //computes the inertia matrix for an axis-aligned box with dims (x,y,z) and mass m
23 inline void BoxInertiaMatrix(Real x,Real y,Real z,Real m,Matrix3& I)
24 {
25  I.setZero();
26  I(0,0) = m*(Sqr(y)+Sqr(z))/12.0;
27  I(1,1) = m*(Sqr(x)+Sqr(z))/12.0;
28  I(2,2) = m*(Sqr(x)+Sqr(y))/12.0;
29 }
30 
31 //computes the inertia matrix for a solid sphere with radius r and mass m
32 inline void SphereInertiaMatrix(Real r,Real m,Matrix3& I)
33 {
34  I.setZero();
35  I(0,0) = I(1,1) = I(2,2) = 0.4*Sqr(r)*m;
36 }
37 
38 //computes the inertia matrix for a hollow sphere with radius r and mass m
39 inline void HollowSphereInertiaMatrix(Real r,Real m,Matrix3& I)
40 {
41  I.setZero();
42  I(0,0) = I(1,1) = I(2,2) = 2.0/3.0*Sqr(r)*m;
43 }
44 
45 //computes the inertia matrix for an axis-aligned ellipsoid with dimensions x,y,z and mass m
46 inline void EllipsoidInertiaMatrix(Real x,Real y,Real z,Real m,Matrix3& I)
47 {
48  I.setZero();
49  I(0,0) = m*(Sqr(y)+Sqr(z))*0.2;
50  I(1,1) = m*(Sqr(x)+Sqr(z))*0.2;
51  I(2,2) = m*(Sqr(x)+Sqr(y))*0.2;
52 }
53 
54 //computes the inertia matrix for z-oriented cylinder with radius r, height h, and mass m
55 inline void CylinderInertiaMatrix(Real r,Real h,Real m,Matrix3& I)
56 {
57  I.setZero();
58  I(0,0) = I(1,1) = m*(Sqr(h)/12.0+Sqr(r)/4.0);
59  I(2,2) = m*Sqr(r)*0.5;
60 }
61 
62 //computes the inertia matrix for z-oriented capsule with radius r, height h, and mass m
63 inline void CapsuleInertiaMatrix(Real r,Real h,Real m,Matrix3& I)
64 {
65  I.setZero();
66  //mass = density*(Pi*r^2*h+4/3*Pi*r^3) = d*(a+b) = M1 + M2
67  //=> d = mass/(a+b)
68  Real a = Pi*r*r*h;
69  Real b = 4.0/3.0*Pi*r*r*r;
70  Real density = m/(a+b);
71  Real M1 = density*a;
72  Real M2 = density*b;
73  I(0,0) = I(1,1) = M1*(0.25*Sqr(r) + Sqr(h)/12.0) + M2*(0.4*Sqr(r) + 0.375*r*h + 0.25*Sqr(h));
74  I(2,2) = (M1*0.5 + M2*0.4)*Sqr(r);
75 }
76 
77 Vector3 CenterOfMass(const GeometricPrimitive3D& geom);
78 Matrix3 InertiaMatrix(const GeometricPrimitive3D& geom,Real mass);
79 
80 #endif
A generic geometric primitive class.
Definition: geometry3d.h:32
A 3D vector class.
Definition: math3d/primitives.h:136
Class declarations for useful 3D math types.
Contains all the definitions in the Math3D package.
Definition: AnyGeometry.h:12
void setCrossProduct(const Vector3 &)
sets the matrix that performs the vector cross product
Definition: math3d/primitives.h:1898
A 3x3 matrix class.
Definition: math3d/primitives.h:469
Contains all definitions in the Math package.
Definition: WorkspaceBound.h:12