From 6c6b6c6c86772fdf686ce34b07ffec99536aaebd Mon Sep 17 00:00:00 2001 From: "chappuis.daniel" Date: Tue, 23 Aug 2011 21:53:01 +0000 Subject: [PATCH] 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 | 11 +++++++++++ src/shapes/BoxShape.h | 11 ++++++----- src/shapes/ConeShape.cpp | 2 +- src/shapes/ConeShape.h | 12 ++++++++++-- src/shapes/CylinderShape.cpp | 4 ++-- src/shapes/CylinderShape.h | 13 ++++++++++--- src/shapes/Shape.h | 6 ++++-- src/shapes/SphereShape.h | 11 +++++++++-- 8 files changed, 53 insertions(+), 17 deletions(-) diff --git a/src/shapes/BoxShape.cpp b/src/shapes/BoxShape.cpp index c43d6510..427c32e3 100644 --- a/src/shapes/BoxShape.cpp +++ b/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 { diff --git a/src/shapes/BoxShape.h b/src/shapes/BoxShape.h index 97be5ca9..e67eac28 100644 --- a/src/shapes/BoxShape.h +++ b/src/shapes/BoxShape.h @@ -46,16 +46,17 @@ 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 virtual ~BoxShape(); // Destructor - const Vector3& getExtent() const; // Return the extents of the box - 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 + const Vector3& getExtent() const; // Return the extents of the box + 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) diff --git a/src/shapes/ConeShape.cpp b/src/shapes/ConeShape.cpp index de556ba4..6e5410d0 100644 --- a/src/shapes/ConeShape.cpp +++ b/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; diff --git a/src/shapes/ConeShape.h b/src/shapes/ConeShape.h index f9eca349..2be4b391 100644 --- a/src/shapes/ConeShape.h +++ b/src/shapes/ConeShape.h @@ -56,8 +56,9 @@ class ConeShape : public Shape { void setRadius(double radius); // Set the radius double getHeight() const; // Return the height 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 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 \ No newline at end of file diff --git a/src/shapes/CylinderShape.cpp b/src/shapes/CylinderShape.cpp index 9a39027b..2a5019b1 100644 --- a/src/shapes/CylinderShape.cpp +++ b/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; diff --git a/src/shapes/CylinderShape.h b/src/shapes/CylinderShape.h index de67bc35..b11db675 100644 --- a/src/shapes/CylinderShape.h +++ b/src/shapes/CylinderShape.h @@ -56,9 +56,9 @@ class CylinderShape : public Shape { void setRadius(double radius); // Set the radius double getHeight() const; // Return the height 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 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 diff --git a/src/shapes/Shape.h b/src/shapes/Shape.h index cac4ce49..97543929 100644 --- a/src/shapes/Shape.h +++ b/src/shapes/Shape.h @@ -29,6 +29,7 @@ // Libraries #include #include "../mathematics/Vector3.h" +#include "../mathematics/Matrix3x3.h" // ReactPhysics3D namespace namespace reactphysics3d { @@ -52,8 +53,9 @@ class Shape { Body* getBodyPointer() const; // Return the body pointer 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 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 diff --git a/src/shapes/SphereShape.h b/src/shapes/SphereShape.h index d57d8739..ef58ffb4 100644 --- a/src/shapes/SphereShape.h +++ b/src/shapes/SphereShape.h @@ -48,8 +48,9 @@ class SphereShape : public Shape { double getRadius() const; // Return the radius of the sphere 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 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 \ No newline at end of file