diff --git a/src/body/Body.cpp b/src/body/Body.cpp
index 2219949f..fba08e3a 100644
--- a/src/body/Body.cpp
+++ b/src/body/Body.cpp
@@ -43,7 +43,7 @@ Body::Body(const Transform& transform, Shape* shape, double mass)
oldTransform = transform;
// Create the AABB for broad-phase collision detection
- aabb = new AABB(transform, shape->getLocalExtents());
+ aabb = new AABB(transform, shape->getLocalExtents(OBJECT_MARGIN));
}
// Destructor
diff --git a/src/body/Body.h b/src/body/Body.h
index 185892aa..7ac925b4 100644
--- a/src/body/Body.h
+++ b/src/body/Body.h
@@ -151,7 +151,7 @@ inline void Body::updateOldTransform() {
// Update the rigid body in order to reflect a change in the body state
inline void Body::updateAABB() {
// Update the AABB
- aabb->update(transform, shape->getLocalExtents());
+ aabb->update(transform, shape->getLocalExtents(OBJECT_MARGIN));
}
diff --git a/src/collision/EPA/EPAAlgorithm.cpp b/src/collision/EPA/EPAAlgorithm.cpp
index 77ba25e8..db244a08 100644
--- a/src/collision/EPA/EPAAlgorithm.cpp
+++ b/src/collision/EPA/EPAAlgorithm.cpp
@@ -146,18 +146,18 @@ bool EPAAlgorithm::computePenetrationDepthAndContactPoints(Simplex simplex, cons
Vector3D v3 = rotationMat * v2;
// Compute the support point in the direction of v1
- suppPointsA[2] = shape1->getSupportPoint(v1, OBJECT_MARGIN);
- suppPointsB[2] = shape2ToShape1 * shape2->getSupportPoint(rotateToShape2 * v1.getOpposite(), OBJECT_MARGIN);
+ suppPointsA[2] = shape1->getLocalSupportPoint(v1, OBJECT_MARGIN);
+ suppPointsB[2] = shape2ToShape1 * shape2->getLocalSupportPoint(rotateToShape2 * v1.getOpposite(), OBJECT_MARGIN);
points[2] = suppPointsA[2] - suppPointsB[2];
// Compute the support point in the direction of v2
- suppPointsA[3] = shape1->getSupportPoint(v2, OBJECT_MARGIN);
- suppPointsB[3] = shape2ToShape1 * shape2->getSupportPoint(rotateToShape2 * v2.getOpposite(), OBJECT_MARGIN);
+ suppPointsA[3] = shape1->getLocalSupportPoint(v2, OBJECT_MARGIN);
+ suppPointsB[3] = shape2ToShape1 * shape2->getLocalSupportPoint(rotateToShape2 * v2.getOpposite(), OBJECT_MARGIN);
points[3] = suppPointsA[3] - suppPointsB[3];
// Compute the support point in the direction of v3
- suppPointsA[4] = shape1->getSupportPoint(v3, OBJECT_MARGIN);
- suppPointsB[4] = shape2ToShape1 * shape2->getSupportPoint(rotateToShape2 * v3.getOpposite(), OBJECT_MARGIN);
+ suppPointsA[4] = shape1->getLocalSupportPoint(v3, OBJECT_MARGIN);
+ suppPointsB[4] = shape2ToShape1 * shape2->getLocalSupportPoint(rotateToShape2 * v3.getOpposite(), OBJECT_MARGIN);
points[4] = suppPointsA[4] - suppPointsB[4];
// Now we have an hexahedron (two tetrahedron glued together). We can simply keep the
@@ -173,9 +173,9 @@ bool EPAAlgorithm::computePenetrationDepthAndContactPoints(Simplex simplex, cons
}
else if (isOriginInTetrahedron(points[1], points[2], points[3], points[4]) == 0) { // If the origin is in the tetrahedron of points 1, 2, 3, 4
// We use the point 4 instead of point 0 for the initial tetrahedron
- suppPointsA[0] = suppPointsA[0];
- suppPointsB[0] = suppPointsB[0];
- points[0] = points[0];
+ suppPointsA[0] = suppPointsA[4];
+ suppPointsB[0] = suppPointsB[4];
+ points[0] = points[4];
}
else {
// The origin is not in the initial polytope
@@ -251,11 +251,11 @@ bool EPAAlgorithm::computePenetrationDepthAndContactPoints(Simplex simplex, cons
Vector3D n = v1.cross(v2);
// Compute the two new vertices to obtain a hexahedron
- suppPointsA[3] = shape1->getSupportPoint(n, OBJECT_MARGIN);
- suppPointsB[3] = shape2ToShape1 * shape2->getSupportPoint(rotateToShape2 * n.getOpposite(), OBJECT_MARGIN);
+ suppPointsA[3] = shape1->getLocalSupportPoint(n, OBJECT_MARGIN);
+ suppPointsB[3] = shape2ToShape1 * shape2->getLocalSupportPoint(rotateToShape2 * n.getOpposite(), OBJECT_MARGIN);
points[3] = suppPointsA[3] - suppPointsB[3];
- suppPointsA[4] = shape1->getSupportPoint(n.getOpposite(), OBJECT_MARGIN);
- suppPointsB[4] = shape2ToShape1 * shape2->getSupportPoint(rotateToShape2 * n, OBJECT_MARGIN);
+ suppPointsA[4] = shape1->getLocalSupportPoint(n.getOpposite(), OBJECT_MARGIN);
+ suppPointsB[4] = shape2ToShape1 * shape2->getLocalSupportPoint(rotateToShape2 * n, OBJECT_MARGIN);
points[4] = suppPointsA[4] - suppPointsB[4];
// Construct the triangle faces
@@ -324,8 +324,8 @@ bool EPAAlgorithm::computePenetrationDepthAndContactPoints(Simplex simplex, cons
}
// Compute the support point of the Minkowski difference (A-B) in the closest point direction
- suppPointsA[nbVertices] = shape1->getSupportPoint(triangle->getClosestPoint(), OBJECT_MARGIN);
- suppPointsB[nbVertices] = shape2ToShape1 *shape2->getSupportPoint(rotateToShape2 * triangle->getClosestPoint().getOpposite(), OBJECT_MARGIN);
+ suppPointsA[nbVertices] = shape1->getLocalSupportPoint(triangle->getClosestPoint(), OBJECT_MARGIN);
+ suppPointsB[nbVertices] = shape2ToShape1 *shape2->getLocalSupportPoint(rotateToShape2 * triangle->getClosestPoint().getOpposite(), OBJECT_MARGIN);
points[nbVertices] = suppPointsA[nbVertices] - suppPointsB[nbVertices];
int indexNewVertex = nbVertices;
@@ -364,7 +364,6 @@ bool EPAAlgorithm::computePenetrationDepthAndContactPoints(Simplex simplex, cons
i++;
}
}
-
} while(nbTriangles > 0 && triangleHeap[0]->getDistSquare() <= upperBoundSquarePenDepth);
// Compute the contact info (in world-space)
diff --git a/src/collision/GJK/GJKAlgorithm.cpp b/src/collision/GJK/GJKAlgorithm.cpp
index 146361de..f9e2118f 100644
--- a/src/collision/GJK/GJKAlgorithm.cpp
+++ b/src/collision/GJK/GJKAlgorithm.cpp
@@ -37,8 +37,6 @@
// We want to use the ReactPhysics3D namespace
using namespace reactphysics3d;
-// TODO : Check that allocated memory is correctly deleted
-
// Constructor
GJKAlgorithm::GJKAlgorithm() {
@@ -91,15 +89,15 @@ bool GJKAlgorithm::testCollision(const Shape* shape1, const Transform& transform
// Get the last point V (last separating axis)
// TODO : Implement frame coherence. For each pair of body, store
// the last separating axis and use it to initialize the v vector
- Vector3D v(0.0, 1.0, 0.0);
+ Vector3D v(1.0, 1.0, 1.0);
// Initialize the upper bound for the square distance
double distSquare = DBL_MAX;
do {
// Compute the support points for original objects (without margins) A and B
- suppA = shape1->getSupportPoint(v.getOpposite());
- suppB = shape2ToShape1 * shape2->getSupportPoint(rotateToShape2 * v);
+ suppA = shape1->getLocalSupportPoint(v.getOpposite());
+ suppB = shape2ToShape1 * shape2->getLocalSupportPoint(rotateToShape2 * v);
// Compute the support point for the Minkowski difference A-B
w = suppA - suppB;
@@ -208,10 +206,6 @@ bool GJKAlgorithm::testCollision(const Shape* shape1, const Transform& transform
// There is an intersection, therefore we return true
return true;
}
-
- double test = simplex.getMaxLengthSquareOfAPoint(); // TODO : Remove this
- test = 4.5;
-
} while(!simplex.isFull() && distSquare > MACHINE_EPSILON * simplex.getMaxLengthSquareOfAPoint());
// The objects (without margins) intersect. Therefore, we run the GJK algorithm again but on the
@@ -245,8 +239,8 @@ bool GJKAlgorithm::computePenetrationDepthForEnlargedObjects(const Shape* const
do {
// Compute the support points for the enlarged object A and B
- suppA = shape1->getSupportPoint(v.getOpposite(), OBJECT_MARGIN);
- suppB = shape2ToShape1 * shape2->getSupportPoint(rotateToShape2 * v, OBJECT_MARGIN);
+ suppA = shape1->getLocalSupportPoint(v.getOpposite(), OBJECT_MARGIN);
+ suppB = shape2ToShape1 * shape2->getLocalSupportPoint(rotateToShape2 * v, OBJECT_MARGIN);
// Compute the support point for the Minkowski difference A-B
w = suppA - suppB;
diff --git a/src/mathematics/Vector3D.h b/src/mathematics/Vector3D.h
index 35253ac5..96b4be2a 100644
--- a/src/mathematics/Vector3D.h
+++ b/src/mathematics/Vector3D.h
@@ -63,6 +63,7 @@ class Vector3D {
void setZ(double z); // Set the z component of the vector
void setAllValues(double x, double y, double z); // Set all the values of the vector
double length() const; // Return the lenght of the vector
+ double lengthSquare() const; // Return the square of the length of the vector
Vector3D getUnit() const throw(MathematicsException); // Return the corresponding unit vector
bool isUnit() const; // Return true if the vector is unit and false otherwise
bool isZero() const; // Return true if the current vector is the zero vector
@@ -148,6 +149,11 @@ inline double Vector3D::length() const {
return sqrt(x*x + y*y + z*z);
}
+// Return the square of the length of the vector
+inline double Vector3D::lengthSquare() const {
+ return x*x + y*y + z*z;
+}
+
// Return the vector in the opposite direction
inline Vector3D Vector3D::getOpposite() const {
return (Vector3D(0.0, 0.0, 0.0) - *this);
diff --git a/src/reactphysics3d.h b/src/reactphysics3d.h
index 7ae39519..e1bb167c 100644
--- a/src/reactphysics3d.h
+++ b/src/reactphysics3d.h
@@ -42,6 +42,7 @@
#include "shapes/BoxShape.h"
#include "shapes/SphereShape.h"
#include "shapes/ConeShape.h"
+#include "shapes/CylinderShape.h"
#include "shapes/AABB.h"
// Alias to the ReactPhysics3D namespace
diff --git a/src/shapes/AABB.h b/src/shapes/AABB.h
index d6a1b8af..30342fbc 100644
--- a/src/shapes/AABB.h
+++ b/src/shapes/AABB.h
@@ -60,9 +60,9 @@ class AABB {
void setBodyPointer(Body* bodyPointer); // Set the body pointer
bool testCollision(const AABB& aabb) const; // Return true if the current AABB is overlapping is the AABB in argument
virtual void update(const Transform& newTransform, const Vector3D& extents); // Update the oriented bounding box orientation according to a new orientation of the rigid body
- #ifdef VISUAL_DEBUG
- virtual void draw() const; // Draw the AABB (only for testing purpose)
- #endif
+#ifdef VISUAL_DEBUG
+ virtual void draw() const; // Draw the AABB (only for testing purpose)
+#endif
};
// Return the center point of the AABB in world coordinates
@@ -93,22 +93,18 @@ inline void AABB::setBodyPointer(Body* bodyPointer) {
// Return true if the current AABB is overlapping with the AABB in argument
// Two AABB overlap if they overlap in the three x, y and z axis at the same time
inline bool AABB::testCollision(const AABB& aabb) const {
- if (maxCoordinates.getX() + OBJECT_MARGIN < aabb.minCoordinates.getX() - OBJECT_MARGIN ||
- aabb.maxCoordinates.getX() + OBJECT_MARGIN < minCoordinates.getX() - OBJECT_MARGIN) return false;
- if (maxCoordinates.getY() + OBJECT_MARGIN < aabb.minCoordinates.getY() - OBJECT_MARGIN ||
- aabb.maxCoordinates.getY() + OBJECT_MARGIN < minCoordinates.getY() - OBJECT_MARGIN) return false;
- if (maxCoordinates.getZ() + OBJECT_MARGIN < aabb.minCoordinates.getZ() - OBJECT_MARGIN ||
- aabb.maxCoordinates.getZ() + OBJECT_MARGIN < minCoordinates.getZ() - OBJECT_MARGIN) return false;
+ if (maxCoordinates.getX() < aabb.minCoordinates.getX() || aabb.maxCoordinates.getX() < minCoordinates.getX()) return false;
+ if (maxCoordinates.getY() < aabb.minCoordinates.getY() || aabb.maxCoordinates.getY() < minCoordinates.getY()) return false;
+ if (maxCoordinates.getZ() < aabb.minCoordinates.getZ() || aabb.maxCoordinates.getZ() < minCoordinates.getZ()) return false;
return true;
}
// Update the world minimum and maximum coordinates of the AABB on the three x,y and z axis
inline void AABB::update(const Transform& newTransform, const Vector3D& extents) {
- Vector3D localExtents = extents + Vector3D(OBJECT_MARGIN, OBJECT_MARGIN, OBJECT_MARGIN);
Matrix3x3 worldAxis = newTransform.getOrientation().getAbsoluteMatrix();
- Vector3D worldExtents = Vector3D(worldAxis.getColumn(0).dot(localExtents),
- worldAxis.getColumn(1).dot(localExtents),
- worldAxis.getColumn(2).dot(localExtents));
+ Vector3D worldExtents = Vector3D(worldAxis.getColumn(0).dot(extents),
+ worldAxis.getColumn(1).dot(extents),
+ worldAxis.getColumn(2).dot(extents));
minCoordinates = newTransform.getPosition() - worldExtents;
maxCoordinates = newTransform.getPosition() + worldExtents;
}
diff --git a/src/shapes/BoxShape.h b/src/shapes/BoxShape.h
index 949ca118..8ac194f4 100644
--- a/src/shapes/BoxShape.h
+++ b/src/shapes/BoxShape.h
@@ -30,6 +30,9 @@
#include "Shape.h"
#include "../mathematics/mathematics.h"
+// TODO : CHECK THAT THE AABB IS CORRECT
+// TODO : TEST THIS SHAPE WITH GJK AND EPA ALGORITHMS
+
// ReactPhysics3D namespace
namespace reactphysics3d {
@@ -38,7 +41,7 @@ namespace reactphysics3d {
This class represents a 3D box. Those axis are unit length.
The three extents are half-widths of the box along the three
axis x, y, z local axis. The "transform" of the corresponding
- rigid body given an orientation and a position to the box
+ rigid body gives an orientation and a position to the box.
-------------------------------------------------------------------
*/
class BoxShape : public Shape {
@@ -51,8 +54,8 @@ class BoxShape : public Shape {
const Vector3D& getExtent() const; // Return the extents of the box
void setExtent(const Vector3D& extent); // Set the extents of the box
- virtual Vector3D getLocalExtents() const; // Return the local extents in x,y and z direction
- virtual Vector3D getSupportPoint(const Vector3D& direction, double margin=0.0) const; // Return a support point in a given direction
+ virtual Vector3D getLocalExtents(double margin=0.0) const; // Return the local extents in x,y and z direction
+ virtual Vector3D getLocalSupportPoint(const Vector3D& direction, double margin=0.0) const; // Return a local support point in a given direction
#ifdef VISUAL_DEBUG
virtual void draw() const; // Draw the Box (only for testing purpose)
@@ -71,12 +74,12 @@ inline void BoxShape::setExtent(const Vector3D& extent) {
// Return the local extents of the shape (half-width) in x,y and z local direction
// This method is used to compute the AABB of the box
-inline Vector3D BoxShape::getLocalExtents() const {
- return extent;
+inline Vector3D BoxShape::getLocalExtents(double margin) const {
+ return extent + Vector3D(margin, margin, margin);
}
-// Return a support point in a given direction
-inline Vector3D BoxShape::getSupportPoint(const Vector3D& direction, double margin) const {
+// Return a local support point in a given direction
+inline Vector3D BoxShape::getLocalSupportPoint(const Vector3D& direction, double margin) const {
assert(margin >= 0.0);
return Vector3D(direction.getX() < 0.0 ? -extent.getX()-margin : extent.getX()+margin,
diff --git a/src/shapes/ConeShape.cpp b/src/shapes/ConeShape.cpp
index 8b137891..c0861ba7 100644
--- a/src/shapes/ConeShape.cpp
+++ b/src/shapes/ConeShape.cpp
@@ -1 +1,94 @@
+/********************************************************************************
+* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ *
+* Copyright (c) 2011 Daniel Chappuis *
+*********************************************************************************
+* *
+* Permission is hereby granted, free of charge, to any person obtaining a copy *
+* of this software and associated documentation files (the "Software"), to deal *
+* in the Software without restriction, including without limitation the rights *
+* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell *
+* copies of the Software, and to permit persons to whom the Software is *
+* furnished to do so, subject to the following conditions: *
+* *
+* The above copyright notice and this permission notice shall be included in *
+* all copies or substantial portions of the Software. *
+* *
+* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR *
+* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, *
+* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE *
+* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER *
+* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, *
+* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN *
+* THE SOFTWARE. *
+********************************************************************************/
+// Libraries
+#include <complex>
+
+#include "ConeShape.h"
+#ifdef VISUAL_DEBUG
+ #include <GL/freeglut.h> // TODO : Remove this in the final version
+ #include <GL/gl.h> // TODO : Remove this in the final version
+#endif
+
+using namespace reactphysics3d;
+
+// Constructor
+ConeShape::ConeShape(double radius, double height) : radius(radius), halfHeight(height/2.0) {
+ assert(radius > 0.0);
+ assert(halfHeight > 0.0);
+
+ // Compute the sine of the semi-angle at the apex point
+ sinTheta = radius / (sqrt(radius * radius + height * height));
+}
+
+// Destructor
+ConeShape::~ConeShape() {
+
+}
+
+// Return a local support point in a given direction
+inline Vector3D ConeShape::getLocalSupportPoint(const Vector3D& direction, double margin) const {
+ assert(margin >= 0.0);
+
+ const Vector3D& v = direction;
+ double sinThetaTimesLengthV = sinTheta * v.length();
+ Vector3D supportPoint;
+
+ if (v.getY() >= sinThetaTimesLengthV) {
+ supportPoint = Vector3D(0.0, halfHeight, 0.0);
+ }
+ else {
+ double projectedLength = sqrt(v.getX() * v.getX() + v.getZ() * v.getZ());
+ if (projectedLength > MACHINE_EPSILON) {
+ double d = radius / projectedLength;
+ supportPoint = Vector3D(v.getX() * d, -halfHeight, v.getZ() * d);
+ }
+ else {
+ supportPoint = Vector3D(radius, -halfHeight, 0.0);
+ }
+ }
+
+ // Add the margin to the support point
+ if (margin != 0.0) {
+ Vector3D unitVec(0.0, -1.0, 0.0);
+ if (v.lengthSquare() > MACHINE_EPSILON * MACHINE_EPSILON) {
+ unitVec = v.getUnit();
+ }
+ supportPoint = supportPoint + unitVec * margin;
+ }
+
+ return supportPoint;
+}
+
+#ifdef VISUAL_DEBUG
+// Draw the cone (only for debuging purpose)
+void ConeShape::draw() const {
+
+ // Draw in red
+ glColor3f(1.0, 0.0, 0.0);
+
+ // Draw the sphere
+ glutWireCone(radius, 2.0 * halfHeight, 50, 50);
+}
+#endif
diff --git a/src/shapes/ConeShape.h b/src/shapes/ConeShape.h
index c0cd1afe..3fd58cfc 100644
--- a/src/shapes/ConeShape.h
+++ b/src/shapes/ConeShape.h
@@ -29,27 +29,76 @@
#include "Shape.h"
#include "../mathematics/mathematics.h"
+// TODO : CHECK THAT THE AABB IS CORRECT
+// TODO : TEST THIS SHAPE WITH GJK AND EPA ALGORITHMS
+
// ReactPhysics3D namespace
namespace reactphysics3d {
/* -------------------------------------------------------------------
Class ConeShape :
- This class represents a cone bounding volume.
+ This class represents a cone collision shape centered at the
+ origin and alligned with the Y axis. The cone is defined
+ by its height and by the radius of its base. The center of the
+ cone is at the half of the height. The "transform" of the
+ corresponding rigid body gives an orientation and a position
+ to the cone.
-------------------------------------------------------------------
*/
class ConeShape : public Shape {
private :
-
+ double radius; // Radius of the base
+ double halfHeight; // Half height of the cone
+ double sinTheta; // sine of the semi angle at the apex point
+
public :
- ConeShape(); // Constructor
- virtual ~ConeShape(); // Destructor
+ ConeShape(double radius, double height); // Constructor
+ virtual ~ConeShape(); // Destructor
+
+ double getRadius() const; // Return the radius
+ void setRadius(double radius); // Set the radius
+ double getHeight() const; // Return the height
+ void setHeight(double height); // Set the height
+ virtual Vector3D getLocalSupportPoint(const Vector3D& direction, double margin=0.0) const; // Return a support point in a given direction
+ virtual Vector3D getLocalExtents(double margin=0.0) const; // Return the local extents in x,y and z direction
#ifdef VISUAL_DEBUG
- virtual void draw() const; // Draw the sphere (only for testing purpose)
+ virtual void draw() const; // Draw the sphere (only for testing purpose)
#endif
};
+// Return the radius
+inline double ConeShape::getRadius() const {
+ return radius;
+}
+
+// Set the radius
+inline void ConeShape::setRadius(double radius) {
+ this->radius = radius;
+
+ // Update sine of the semi-angle at the apex point
+ sinTheta = radius / (sqrt(radius * radius + 4 * halfHeight * halfHeight));
+}
+
+// Return the height
+inline double ConeShape::getHeight() const {
+ return 2.0 * halfHeight;
+}
+
+// Set the height
+inline void ConeShape::setHeight(double height) {
+ this->halfHeight = height / 2.0;
+
+ // Update the sine of the semi-angle at the apex point
+ sinTheta = radius / (sqrt(radius * radius + height * height));
+}
+
+// Return the local extents in x,y and z direction
+inline Vector3D ConeShape::getLocalExtents(double margin) const {
+ return Vector3D(radius + margin, halfHeight + margin, radius + margin);
+}
+
}; // End of the ReactPhysics3D namespace
#endif
\ No newline at end of file
diff --git a/src/shapes/CylinderShape.cpp b/src/shapes/CylinderShape.cpp
new file mode 100644
index 00000000..b78984c8
--- /dev/null
+++ b/src/shapes/CylinderShape.cpp
@@ -0,0 +1,85 @@
+/********************************************************************************
+* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ *
+* Copyright (c) 2011 Daniel Chappuis *
+*********************************************************************************
+* *
+* Permission is hereby granted, free of charge, to any person obtaining a copy *
+* of this software and associated documentation files (the "Software"), to deal *
+* in the Software without restriction, including without limitation the rights *
+* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell *
+* copies of the Software, and to permit persons to whom the Software is *
+* furnished to do so, subject to the following conditions: *
+* *
+* The above copyright notice and this permission notice shall be included in *
+* all copies or substantial portions of the Software. *
+* *
+* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR *
+* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, *
+* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE *
+* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER *
+* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, *
+* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN *
+* THE SOFTWARE. *
+********************************************************************************/
+
+// Libraries
+#include "CylinderShape.h"
+#ifdef VISUAL_DEBUG
+ #include <GL/freeglut.h> // TODO : Remove this in the final version
+ #include <GL/gl.h> // TODO : Remove this in the final version
+#endif
+
+using namespace reactphysics3d;
+
+// Constructor
+CylinderShape::CylinderShape(double radius, double height) : radius(radius), halfHeight(height/2.0) {
+
+}
+
+// Destructor
+CylinderShape::~CylinderShape() {
+
+}
+
+// Return a local support point in a given direction
+Vector3D CylinderShape::getLocalSupportPoint(const Vector3D& direction, double margin) const {
+ assert(margin >= 0.0);
+
+ Vector3D supportPoint(0.0, 0.0, 0.0);
+ double uDotv = direction.getY();
+ Vector3D w(direction.getX(), 0.0, direction.getZ());
+ double lengthW = sqrt(direction.getX() * direction.getX() + direction.getZ() * direction.getZ());
+
+ if (lengthW != 0.0) {
+ if (uDotv < 0.0) supportPoint.setY(-halfHeight);
+ else supportPoint.setY(halfHeight);
+ supportPoint = supportPoint + (radius / lengthW) * w;
+ }
+ else {
+ if (uDotv < 0.0) supportPoint.setY(-halfHeight);
+ else supportPoint.setY(halfHeight);
+ }
+
+ // Add the margin to the support point
+ if (margin != 0.0) {
+ Vector3D unitVec(0.0, 1.0, 0.0);
+ if (direction.lengthSquare() > MACHINE_EPSILON * MACHINE_EPSILON) {
+ unitVec = direction.getUnit();
+ }
+ supportPoint = supportPoint + unitVec * margin;
+ }
+
+ return supportPoint;
+}
+
+#ifdef VISUAL_DEBUG
+// Draw the cone (only for debuging purpose)
+void CylinderShape::draw() const {
+
+ // Draw in red
+ glColor3f(1.0, 0.0, 0.0);
+
+ // Draw the sphere
+ glutWireSphere(radius, 50, 50);
+}
+#endif
diff --git a/src/shapes/CylinderShape.h b/src/shapes/CylinderShape.h
new file mode 100644
index 00000000..8b1180fb
--- /dev/null
+++ b/src/shapes/CylinderShape.h
@@ -0,0 +1,96 @@
+/********************************************************************************
+* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ *
+* Copyright (c) 2011 Daniel Chappuis *
+*********************************************************************************
+* *
+* Permission is hereby granted, free of charge, to any person obtaining a copy *
+* of this software and associated documentation files (the "Software"), to deal *
+* in the Software without restriction, including without limitation the rights *
+* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell *
+* copies of the Software, and to permit persons to whom the Software is *
+* furnished to do so, subject to the following conditions: *
+* *
+* The above copyright notice and this permission notice shall be included in *
+* all copies or substantial portions of the Software. *
+* *
+* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR *
+* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, *
+* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE *
+* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER *
+* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, *
+* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN *
+* THE SOFTWARE. *
+********************************************************************************/
+
+#ifndef CYLINDER_SHAPE_H
+#define CYLINDER_SHAPE_H
+
+// Libraries
+#include "Shape.h"
+#include "../mathematics/mathematics.h"
+
+// TODO : CHECK THAT THE AABB IS CORRECT
+// TODO : TEST THIS SHAPE WITH GJK AND EPA ALGORITHMS
+
+// ReactPhysics3D namespace
+namespace reactphysics3d {
+
+/* -------------------------------------------------------------------
+ Class CylinderShape :
+ This class represents a cylinder collision shape around the Y axis
+ and centered at the origin. The cylinder is defined by its height
+ and the radius of its base. The "transform" of the corresponding
+ rigid body gives an orientation and a position to the cylinder.
+ -------------------------------------------------------------------
+*/
+class CylinderShape : public Shape {
+ private :
+ double radius; // Radius of the base
+ double halfHeight; // Half height of the cone
+
+ public :
+ CylinderShape(double radius, double height); // Constructor
+ virtual ~CylinderShape(); // Destructor
+
+ double getRadius() const; // Return the radius
+ void setRadius(double radius); // Set the radius
+ double getHeight() const; // Return the height
+ void setHeight(double height); // Set the height
+ virtual Vector3D getLocalSupportPoint(const Vector3D& direction, double margin=0.0) const; // Return a support point in a given direction
+ virtual Vector3D getLocalExtents(double margin=0.0) const; // Return the local extents in x,y and z direction
+
+
+#ifdef VISUAL_DEBUG
+ virtual void draw() const; // Draw the sphere (only for testing purpose)
+#endif
+};
+
+// Return the radius
+inline double CylinderShape::getRadius() const {
+ return radius;
+}
+
+// Set the radius
+inline void CylinderShape::setRadius(double radius) {
+ this->radius = radius;
+}
+
+// Return the height
+inline double CylinderShape::getHeight() const {
+ return halfHeight * 2.0;
+}
+
+// Set the height
+inline void CylinderShape::setHeight(double height) {
+ this->halfHeight = height / 2.0;
+}
+
+// Return the local extents in x,y and z direction
+inline Vector3D CylinderShape::getLocalExtents(double margin) const {
+ return Vector3D(radius + margin, halfHeight + margin, radius + margin);
+}
+
+}; // End of the ReactPhysics3D namespace
+
+#endif
+
diff --git a/src/shapes/Shape.h b/src/shapes/Shape.h
index be31d2ad..e6fafb8a 100644
--- a/src/shapes/Shape.h
+++ b/src/shapes/Shape.h
@@ -50,10 +50,10 @@ class Shape {
Shape(); // Constructor
virtual ~Shape(); // Destructor
- Body* getBodyPointer() const; // Return the body pointer
- void setBodyPointer(Body* bodyPointer); // Set the body pointer
- virtual Vector3D getSupportPoint(const Vector3D& direction, double margin=0.0) const=0; // Return a support point in a given direction
- virtual Vector3D getLocalExtents() const=0; // Return the local extents in x,y and z direction
+ Body* getBodyPointer() const; // Return the body pointer
+ void setBodyPointer(Body* bodyPointer); // Set the body pointer
+ virtual Vector3D getLocalSupportPoint(const Vector3D& direction, double margin=0.0) const=0; // Return a local support point in a given direction
+ virtual Vector3D getLocalExtents(double margin=0.0) const=0; // Return the local extents in x,y and z direction
};
// Return the body pointer
diff --git a/src/shapes/SphereShape.cpp b/src/shapes/SphereShape.cpp
index 56a8d1bc..70e0b64a 100644
--- a/src/shapes/SphereShape.cpp
+++ b/src/shapes/SphereShape.cpp
@@ -36,9 +36,8 @@ using namespace reactphysics3d;
using namespace std;
// Constructor
-SphereShape::SphereShape(const Vector3D& center, double radius) {
- this->center = center;
- this->radius = radius;
+SphereShape::SphereShape(double radius) : radius(radius) {
+
}
// Destructor
@@ -53,17 +52,7 @@ void SphereShape::draw() const {
// Draw in red
glColor3f(1.0, 0.0, 0.0);
- glTranslatef(center.getX(), center.getY(), center.getZ());
-
// Draw the sphere
glutWireSphere(radius, 50, 50);
}
-#endif
-
-/*TODO: DELETE THIS
-// Return the corresponding AABB
-AABB* BoundingSphere::computeAABB() const {
- // Create and return the AABB
- return new AABB(center, radius, radius, radius);
-}
-*/
\ No newline at end of file
+#endif
\ No newline at end of file
diff --git a/src/shapes/SphereShape.h b/src/shapes/SphereShape.h
index dc07935d..6dc876ca 100644
--- a/src/shapes/SphereShape.h
+++ b/src/shapes/SphereShape.h
@@ -34,43 +34,28 @@ namespace reactphysics3d {
/* -------------------------------------------------------------------
Class SphereShape :
- This class represents a sphere bounding volume.
+ This class represents a sphere collision shape that is centered
+ at the origin and defined by its radius.
-------------------------------------------------------------------
*/
class SphereShape : public Shape {
private :
- Vector3D center; // Center point of the sphere
double radius; // Radius of the sphere
public :
- SphereShape(const Vector3D& center, double radius); // Constructor
- virtual ~SphereShape(); // Destructor
+ SphereShape(double radius); // Constructor
+ virtual ~SphereShape(); // Destructor
- Vector3D getCenter() const; // Return the center point of the sphere
- void setCenter(const Vector3D& center); // Set the center point of the sphere
- double getRadius() const; // Return the radius of the sphere
- void setRadius(double radius); // Set the radius of the sphere
- virtual void update(const Vector3D& newCenter,
- const Quaternion& rotationQuaternion); // Update the sphere orientation according to a new orientation of the rigid body
- // TODO : DELETE virtual AABB* computeAABB() const; // Return the corresponding AABB
- virtual Vector3D getSupportPoint(const Vector3D& direction, double margin=0.0) const; // Return a support point in a given direction
- virtual Vector3D getLocalExtents() const; // Return the local extents in x,y and z direction
+ double getRadius() const; // Return the radius of the sphere
+ void setRadius(double radius); // Set the radius of the sphere
+ virtual Vector3D getLocalSupportPoint(const Vector3D& direction, double margin=0.0) const; // Return a local support point in a given direction
+ virtual Vector3D getLocalExtents(double margin=0.0) const; // Return the local extents in x,y and z direction
#ifdef VISUAL_DEBUG
virtual void draw() const; // Draw the sphere (only for testing purpose)
#endif
};
-// Return the center point of the sphere
-inline Vector3D SphereShape::getCenter() const {
- return center;
-}
-
-// Set the center point of the sphere
-inline void SphereShape::setCenter(const Vector3D& center) {
- this->center = center;
-}
-
// Get the radius of the sphere
inline double SphereShape::getRadius() const {
return radius;
@@ -81,14 +66,8 @@ inline void SphereShape::setRadius(double radius) {
this->radius = radius;
}
-// Update the orientation of the shere according to the orientation of the rigid body
-inline void SphereShape::update(const Vector3D& newCenter, const Quaternion& rotationQuaternion) {
- // Update the center of the sphere
- center = newCenter;
-}
-
-// Return a support point in a given direction
-inline Vector3D SphereShape::getSupportPoint(const Vector3D& direction, double margin) const {
+// Return a local support point in a given direction
+inline Vector3D SphereShape::getLocalSupportPoint(const Vector3D& direction, double margin) const {
assert(margin >= 0.0);
double length = direction.length();
@@ -105,8 +84,8 @@ inline Vector3D SphereShape::getSupportPoint(const Vector3D& direction, double m
// Return the local extents of the shape (half-width) in x,y and z local direction
// This method is used to compute the AABB of the box
-inline Vector3D SphereShape::getLocalExtents() const {
- return Vector3D(radius, radius, radius);
+inline Vector3D SphereShape::getLocalExtents(double margin) const {
+ return Vector3D(radius + margin, radius + margin, radius + margin);
}
}; // End of the ReactPhysics3D namespace