Add a method to compute the inertia tensor from the collision shape of the rigid body

git-svn-id: https://reactphysics3d.googlecode.com/svn/trunk@438 92aac97c-a6ce-11dd-a772-7fcde58d38e6

src/shapes/BoxShape.cpp

@@ -45,6 +45,17 @@ BoxShape::~BoxShape() {
 
 }
 
+// Return the local inertia tensor of the shape
+void BoxShape::computeLocalInertiaTensor(Matrix3x3& tensor, double mass) const {
+    double factor = (1.0 / 3.0) * mass;
+    double xSquare = extent.getX() * extent.getX();
+    double ySquare = extent.getY() * extent.getY();
+    double zSquare = extent.getZ() * extent.getZ();
+    tensor.setAllValues(factor * (ySquare + zSquare), 0.0, 0.0,
+                        0.0, factor * (xSquare + zSquare), 0.0,
+                        0.0, 0.0, factor * (xSquare + ySquare));
+}
+
 #ifdef VISUAL_DEBUG
 // Draw the Box (only for testing purpose)
 void BoxShape::draw() const {

src/shapes/BoxShape.h

@@ -46,7 +46,7 @@ namespace reactphysics3d {
 */
 class BoxShape : public Shape {
     private :
-        Vector3 extent;           // Extent sizes of the box
+        Vector3 extent;           // Extent sizes of the box in the x, y and z direction
 
     public :
         BoxShape(const Vector3& extent);        // Constructor
@@ -56,6 +56,7 @@ class BoxShape : public Shape {
         void setExtent(const Vector3& extent);                                                      // Set the extents of the box
         virtual Vector3 getLocalExtents(double margin=0.0) const;                                   // Return the local extents in x,y and z direction
         virtual Vector3 getLocalSupportPoint(const Vector3& direction, double margin=0.0) const;    // Return a local support point in a given direction
+        virtual void computeLocalInertiaTensor(Matrix3x3& tensor, double mass) const;               // Return the local inertia tensor of the shape
 
 #ifdef VISUAL_DEBUG
             virtual void draw() const;                                                                      // Draw the Box (only for testing purpose)

src/shapes/ConeShape.cpp

@@ -75,7 +75,7 @@ inline Vector3 ConeShape::getLocalSupportPoint(const Vector3& direction, double
         if (v.lengthSquare() > MACHINE_EPSILON * MACHINE_EPSILON) {
             unitVec = v.getUnit();
         }
-        supportPoint = supportPoint + unitVec * margin;
+        supportPoint += unitVec * margin;
     }
 
     return supportPoint;

src/shapes/ConeShape.h

@@ -58,6 +58,7 @@ class ConeShape : public Shape {
         void setHeight(double height);                                                              // Set the height
         virtual Vector3 getLocalSupportPoint(const Vector3& direction, double margin=0.0) const;    // Return a support point in a given direction
         virtual Vector3 getLocalExtents(double margin=0.0) const;                                   // Return the local extents in x,y and z direction
+        virtual void computeLocalInertiaTensor(Matrix3x3& tensor, double mass) const;               // Return the local inertia tensor of the shape
 
 
 #ifdef VISUAL_DEBUG
@@ -96,6 +97,13 @@ inline Vector3 ConeShape::getLocalExtents(double margin) const {
     return Vector3(radius + margin, halfHeight + margin, radius + margin);
 }
 
+// Return the local inertia tensor of the shape
+inline void ConeShape::computeLocalInertiaTensor(Matrix3x3& tensor, double mass) const {
+    double rSquare = radius * radius;
+    double diagXZ = 0.15 * mass * (rSquare + halfHeight);
+    tensor.setAllValues(diagXZ, 0.0, 0.0, 0.0, 0.3 * mass * rSquare, 0.0, 0.0, 0.0, diagXZ);
+}
+
 }; // End of the ReactPhysics3D namespace
 
 #endif

src/shapes/CylinderShape.cpp

@@ -53,7 +53,7 @@ Vector3 CylinderShape::getLocalSupportPoint(const Vector3& direction, double mar
     if (lengthW != 0.0) {
         if (uDotv < 0.0) supportPoint.setY(-halfHeight);
         else supportPoint.setY(halfHeight);
-        supportPoint = supportPoint + (radius / lengthW) * w;
+        supportPoint += (radius / lengthW) * w;
     }
     else {
          if (uDotv < 0.0) supportPoint.setY(-halfHeight);
@@ -66,7 +66,7 @@ Vector3 CylinderShape::getLocalSupportPoint(const Vector3& direction, double mar
         if (direction.lengthSquare() > MACHINE_EPSILON * MACHINE_EPSILON) {
             unitVec = direction.getUnit();
         }
-        supportPoint = supportPoint + unitVec * margin;
+        supportPoint += unitVec * margin;
     }
 
     return supportPoint;

src/shapes/CylinderShape.h

@@ -58,7 +58,7 @@ class CylinderShape : public Shape {
         void setHeight(double height);                                                              // Set the height
         virtual Vector3 getLocalSupportPoint(const Vector3& direction, double margin=0.0) const;    // Return a support point in a given direction
         virtual Vector3 getLocalExtents(double margin=0.0) const;                                   // Return the local extents in x,y and z direction
-
+        virtual void computeLocalInertiaTensor(Matrix3x3& tensor, double mass) const;               // Return the local inertia tensor of the shape
 
 #ifdef VISUAL_DEBUG
         virtual void draw() const;                              // Draw the sphere (only for testing purpose)
@@ -90,6 +90,13 @@ inline Vector3 CylinderShape::getLocalExtents(double margin) const {
     return Vector3(radius + margin, halfHeight + margin, radius + margin);
 }
 
+// Return the local inertia tensor of the cylinder
+inline void CylinderShape::computeLocalInertiaTensor(Matrix3x3& tensor, double mass) const {
+    double height = 2.0 * halfHeight;
+    double diag = (1.0 / 12.0) * mass * (3 * radius * radius + height * height);
+    tensor.setAllValues(diag, 0.0, 0.0, 0.0, 0.5 * mass * radius * radius, 0.0, 0.0, 0.0, diag);
+}
+
 }; // End of the ReactPhysics3D namespace
 
 #endif

src/shapes/Shape.h

@@ -29,6 +29,7 @@
 // Libraries
 #include <cassert>
 #include "../mathematics/Vector3.h"
+#include "../mathematics/Matrix3x3.h"
 
 // ReactPhysics3D namespace
 namespace reactphysics3d {
@@ -54,6 +55,7 @@ class Shape {
         void setBodyPointer(Body* bodyPointer);                                                         // Set the body pointer
         virtual Vector3 getLocalSupportPoint(const Vector3& direction, double margin=0.0) const=0;      // Return a local support point in a given direction
         virtual Vector3 getLocalExtents(double margin=0.0) const=0;                                     // Return the local extents in x,y and z direction
+        virtual void computeLocalInertiaTensor(Matrix3x3& tensor, double mass) const=0;                 // Return the local inertia tensor of the shape
 };
 
 // Return the body pointer

src/shapes/SphereShape.h

@@ -50,6 +50,7 @@ class SphereShape : public Shape {
         void setRadius(double radius);                                                              // Set the radius of the sphere
         virtual Vector3 getLocalSupportPoint(const Vector3& direction, double margin=0.0) const;    // Return a local support point in a given direction
         virtual Vector3 getLocalExtents(double margin=0.0) const;                                   // Return the local extents in x,y and z direction
+        virtual void computeLocalInertiaTensor(Matrix3x3& tensor, double mass) const;               // Return the local inertia tensor of the shape
 
 #ifdef VISUAL_DEBUG
             virtual void draw() const;                              // Draw the sphere (only for testing purpose)
@@ -88,6 +89,12 @@ inline Vector3 SphereShape::getLocalExtents(double margin) const {
     return Vector3(radius + margin, radius + margin, radius + margin);
 }
 
+// Return the local inertia tensor of the sphere
+inline void SphereShape::computeLocalInertiaTensor(Matrix3x3& tensor, double mass) const {
+    double diag = 0.4 * mass * radius * radius;
+    tensor.setAllValues(diag, 0.0, 0.0, 0.0, diag, 0.0, 0.0, 0.0, diag);
+}
+
 }; // End of the ReactPhysics3D namespace
 
 #endif