Remove the margin gap for the BoxShape and make possible to choose the collision margin when creating a collision shape

src/collision/narrowphase/GJK/GJKAlgorithm.h

@@ -45,12 +45,12 @@ const decimal REL_ERROR_SQUARE = REL_ERROR * REL_ERROR;
  * This class implements a narrow-phase collision detection algorithm. This
  * algorithm uses the ISA-GJK algorithm and the EPA algorithm. This
  * implementation is based on the implementation discussed in the book
- * "Collision Detection in 3D Environments".
+ * "Collision Detection in Interactive 3D Environments" by Gino van den Bergen.
  * This method implements the Hybrid Technique for calculating the
  * penetration depth. The two objects are enlarged with a small margin. If
- * the object intersection, the penetration depth is quickly computed using
+ * the object intersects in their margins, the penetration depth is quickly
- * GJK algorithm on the original objects (without margin). If the
+ * computed using the GJK algorithm on the original objects (without margin).
- * original objects (without margin) intersect, we run again the GJK
+ * If the original objects (without margin) intersect, we run again the GJK
  * algorithm on the enlarged objects (with margin) to compute simplex
  * polytope that contains the origin and give it to the EPA (Expanding
  * Polytope Algorithm) to compute the correct penetration depth between the

src/collision/shapes/AABB.cpp

@@ -28,19 +28,6 @@
 #include "../../configuration.h"
 #include <cassert>
 
-#if defined(VISUAL_DEBUG)
-	#if defined(APPLE_OS)
-		#include <GLUT/glut.h>
-		#include <OpenGL/gl.h>
-	#elif defined(WINDOWS_OS)
-		#include <GL/glut.h>
-		#include <GL/gl.h>
-	#elif defined(LINUX_OS)
-		#include <GL/freeglut.h>
-		#include <GL/gl.h>
-	#endif
-#endif
-
 using namespace reactphysics3d;
 using namespace std;
 
@@ -60,52 +47,3 @@ AABB::~AABB() {
 
 }
 
-#ifdef VISUAL_DEBUG
-// Draw the AABB (only for testing purpose)
-void AABB::draw() const {
-
-    // Draw in red
-    glColor3f(1.0, 0.0, 0.0);
-
-    // Draw the AABB
-    glBegin(GL_LINES);
-        glVertex3f(mMaxCoordinates.x, mMinCoordinates.y, mMinCoordinates.z);
-        glVertex3f(mMaxCoordinates.x, mMaxCoordinates.y, mMinCoordinates.z);
-
-        glVertex3f(mMaxCoordinates.x, mMinCoordinates.y, mMinCoordinates.z);
-        glVertex3f(mMaxCoordinates.x, mMinCoordinates.y, mMaxCoordinates.z);
-
-        glVertex3f(mMaxCoordinates.x, mMinCoordinates.y, mMaxCoordinates.z);
-        glVertex3f(mMaxCoordinates.x, mMaxCoordinates.y, mMaxCoordinates.z);
-
-        glVertex3f(mMaxCoordinates.x, mMaxCoordinates.y, mMinCoordinates.z);
-        glVertex3f(mMaxCoordinates.x, mMaxCoordinates.y, mMaxCoordinates.z);
-
-        glVertex3f(mMinCoordinates.x, mMinCoordinates.y, mMinCoordinates.z);
-        glVertex3f(mMinCoordinates.x, mMaxCoordinates.y, mMinCoordinates.z);
-
-        glVertex3f(mMinCoordinates.x, mMinCoordinates.y, mMinCoordinates.z);
-        glVertex3f(mMinCoordinates.x, mMinCoordinates.y, mMaxCoordinates.z);
-
-        glVertex3f(mMinCoordinates.x, mMinCoordinates.y, mMaxCoordinates.z);
-        glVertex3f(mMinCoordinates.x, mMaxCoordinates.y, mMaxCoordinates.z);
-
-        glVertex3f(mMinCoordinates.x, mMaxCoordinates.y, mMinCoordinates.z);
-        glVertex3f(mMinCoordinates.x, mMaxCoordinates.y, mMaxCoordinates.z);
-
-        glVertex3f(mMinCoordinates.x, mMinCoordinates.y, mMinCoordinates.z);
-        glVertex3f(mMaxCoordinates.x, mMinCoordinates.y, mMinCoordinates.z);
-
-        glVertex3f(mMinCoordinates.x, mMaxCoordinates.y, mMinCoordinates.z);
-        glVertex3f(mMaxCoordinates.x, mMaxCoordinates.y, mMinCoordinates.z);
-
-        glVertex3f(mMinCoordinates.x, mMaxCoordinates.y, mMaxCoordinates.z);
-        glVertex3f(mMaxCoordinates.x, mMaxCoordinates.y, mMaxCoordinates.z);
-
-        glVertex3f(mMinCoordinates.x, mMinCoordinates.y, mMaxCoordinates.z);
-        glVertex3f(mMaxCoordinates.x, mMinCoordinates.y, mMaxCoordinates.z);
-
-    glEnd();
-}
-#endif
-

src/collision/shapes/AABB.h

@@ -97,11 +97,6 @@ class AABB {
 
         /// Return true if the current AABB is overlapping with the AABB in argument
         bool testCollision(const AABB& aabb) const;
-
-#ifdef VISUAL_DEBUG
-       /// Draw the AABB (only for testing purpose)
-       virtual void draw() const;
-#endif
 };
 
 // Return the center point of the AABB in world coordinates

src/collision/shapes/BoxShape.cpp

@@ -29,25 +29,16 @@
 #include <vector>
 #include <cassert>
 
-#if defined(VISUAL_DEBUG)
-	#if defined(APPLE_OS)
-		#include <GLUT/glut.h>
-		#include <OpenGL/gl.h>
-	#elif defined(WINDOWS_OS)
-		#include <GL/glut.h>
-		#include <GL/gl.h>
-	#elif defined(LINUX_OS)
-		#include <GL/freeglut.h>
-		#include <GL/gl.h>
-	#endif
-#endif
-
 using namespace reactphysics3d;
 using namespace std;
 
 // Constructor
-BoxShape::BoxShape(const Vector3& extent) : CollisionShape(BOX), mExtent(extent) {
+BoxShape::BoxShape(const Vector3& extent, decimal margin)
-
+         : CollisionShape(BOX, margin), mExtent(extent - Vector3(margin, margin, margin)) {
+    assert(extent.x > decimal(0.0) && extent.x > margin);
+    assert(extent.y > decimal(0.0) && extent.y > margin);
+    assert(extent.z > decimal(0.0) && extent.z > margin);
+    assert(margin > decimal(0.0));
 }
 
 // Private copy-constructor
@@ -63,62 +54,11 @@ BoxShape::~BoxShape() {
 // Return the local inertia tensor of the collision shape
 void BoxShape::computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const {
     decimal factor = (decimal(1.0) / decimal(3.0)) * mass;
-    decimal xSquare = mExtent.x * mExtent.x;
+    Vector3 realExtent = mExtent + Vector3(mMargin, mMargin, mMargin);
-    decimal ySquare = mExtent.y * mExtent.y;
+    decimal xSquare = realExtent.x * realExtent.x;
-    decimal zSquare = mExtent.z * mExtent.z;
+    decimal ySquare = realExtent.y * realExtent.y;
+    decimal zSquare = realExtent.z * realExtent.z;
     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 {
-    decimal e1 = mExtent.x;
-    decimal e2 = mExtent.y;
-    decimal e3 = mExtent.z;
-
-    // Draw in red
-    glColor3f(1.0, 0.0, 0.0);
-
-    // Draw the Box
-    glBegin(GL_LINES);
-        glVertex3f(e1, -e2, -e3);
-        glVertex3f(e1, e2, -e3);
-
-        glVertex3f(e1, -e2, -e3);
-        glVertex3f(e1, -e2, e3);
-
-        glVertex3f(e1, -e2, e3);
-        glVertex3f(e1, e2, e3);
-
-        glVertex3f(e1, e2, e3);
-        glVertex3f(e1, e2, -e3);
-
-        glVertex3f(-e1, -e2, -e3);
-        glVertex3f(-e1, e2, -e3);
-
-        glVertex3f(-e1, -e2, -e3);
-        glVertex3f(-e1, -e2, e3);
-
-        glVertex3f(-e1, -e2, e3);
-        glVertex3f(-e1, e2, e3);
-
-        glVertex3f(-e1, e2, e3);
-        glVertex3f(-e1, e2, -e3);
-
-        glVertex3f(e1, -e2, -e3);
-        glVertex3f(-e1, -e2, -e3);
-
-        glVertex3f(e1, -e2, -e3);
-        glVertex3f(-e1, -e2, -e3);
-
-        glVertex3f(e1, -e2, e3);
-        glVertex3f(-e1, -e2, e3);
-
-        glVertex3f(e1, e2, e3);
-        glVertex3f(-e1, e2, e3);
-
-    glEnd();
-}
-#endif

src/collision/shapes/BoxShape.h

@@ -64,7 +64,7 @@ class BoxShape : public CollisionShape {
         // -------------------- Methods -------------------- //
 
         /// Constructor
-        BoxShape(const Vector3& extent);
+        BoxShape(const Vector3& extent, decimal margin = OBJECT_MARGIN);
 
         /// Destructor
         virtual ~BoxShape();
@@ -75,14 +75,8 @@ class BoxShape : public CollisionShape {
         /// Return the extents of the box
         const Vector3& getExtent() const;
 
-        /// Set the extents of the box
-        void setExtent(const Vector3& extent);
-
         /// Return the local extents in x,y and z direction.
-        virtual Vector3 getLocalExtents(decimal margin=0.0) const;
+        virtual Vector3 getLocalExtents() const;
-
-        /// Return the margin distance around the shape
-        virtual decimal getMargin() const;
 
         /// Return the number of bytes used by the collision shape
         virtual size_t getSizeInBytes() const;
@@ -98,11 +92,6 @@ class BoxShape : public CollisionShape {
 
         /// Test equality between two box shapes
         virtual bool isEqualTo(const CollisionShape& otherCollisionShape) const;
-
-#ifdef VISUAL_DEBUG
-        /// Draw the Box (only for testing purpose)
-        virtual void draw() const;
-#endif
 };
 
 // Allocate and return a copy of the object
@@ -112,23 +101,13 @@ inline BoxShape* BoxShape::clone(void* allocatedMemory) const {
 
 // Return the extents of the box
 inline const Vector3& BoxShape::getExtent() const {
-    return mExtent;
+    return mExtent + Vector3(mMargin, mMargin, mMargin);
-}
-
- // Set the extents of the box
-inline void BoxShape::setExtent(const Vector3& extent) {
-    this->mExtent = extent;
 }
 
 // Return the local extents of the box (half-width) in x,y and z local direction.
 /// This method is used to compute the AABB of the box
-inline Vector3 BoxShape::getLocalExtents(decimal margin) const {
+inline Vector3 BoxShape::getLocalExtents() const {
-    return mExtent + Vector3(getMargin(), getMargin(), getMargin());
+    return mExtent + Vector3(mMargin, mMargin, mMargin);
-}
-
-// Return the margin distance around the shape
-inline decimal BoxShape::getMargin() const {
-    return OBJECT_MARGIN;
 }
 
 // Return the number of bytes used by the collision shape
@@ -139,12 +118,11 @@ inline size_t BoxShape::getSizeInBytes() const {
 // Return a local support point in a given direction with the object margin
 inline Vector3 BoxShape::getLocalSupportPointWithMargin(const Vector3& direction) const {
 
-    decimal margin = getMargin();
+    assert(mMargin > 0.0);
-    assert(margin >= 0.0);
     
-    return Vector3(direction.x < 0.0 ? -mExtent.x - margin : mExtent.x + margin,
+    return Vector3(direction.x < 0.0 ? -mExtent.x - mMargin : mExtent.x + mMargin,
-                   direction.y < 0.0 ? -mExtent.y - margin : mExtent.y + margin,
+                   direction.y < 0.0 ? -mExtent.y - mMargin : mExtent.y + mMargin,
-                   direction.z < 0.0 ? -mExtent.z - margin : mExtent.z + margin);
+                   direction.z < 0.0 ? -mExtent.z - mMargin : mExtent.z + mMargin);
 }
 
 // Return a local support point in a given direction without the objec margin

src/collision/shapes/CollisionShape.cpp

@@ -30,14 +30,15 @@
 using namespace reactphysics3d;
 
 // Constructor
-CollisionShape::CollisionShape(CollisionShapeType type)
+CollisionShape::CollisionShape(CollisionShapeType type, decimal margin)
-               : mType(type), mNbSimilarCreatedShapes(0) {
+               : mType(type), mNbSimilarCreatedShapes(0), mMargin(margin) {
     
 }
 
 // Private copy-constructor
 CollisionShape::CollisionShape(const CollisionShape& shape)
-               : mType(shape.mType), mNbSimilarCreatedShapes(shape.mNbSimilarCreatedShapes){
+               : mType(shape.mType), mNbSimilarCreatedShapes(shape.mNbSimilarCreatedShapes),
+                 mMargin(shape.mMargin) {
 
 }
 
@@ -50,7 +51,7 @@ CollisionShape::~CollisionShape() {
 inline void CollisionShape::updateAABB(AABB& aabb, const Transform& transform) {
 
     // Get the local extents in x,y and z direction
-    Vector3 extents = getLocalExtents(OBJECT_MARGIN);
+    Vector3 extents = getLocalExtents();
 
     // Rotate the local extents according to the orientation of the body
     Matrix3x3 worldAxis = transform.getOrientation().getMatrix().getAbsoluteMatrix();

src/collision/shapes/CollisionShape.h

@@ -36,7 +36,7 @@
 namespace reactphysics3d {
     
 /// Type of the collision shape
-enum CollisionShapeType {BOX, SPHERE, CONE, CYLINDER};
+enum CollisionShapeType {BOX, SPHERE, CONE, CYLINDER, CAPSULE};
 
 // Declarations
 class Body;
@@ -58,6 +58,9 @@ class CollisionShape {
         /// Current number of similar created shapes
         uint mNbSimilarCreatedShapes;
 
+        /// Margin used for the GJK collision detection algorithm
+        decimal mMargin;
+        
         // -------------------- Methods -------------------- //
 
         /// Private copy-constructor
@@ -71,7 +74,7 @@ class CollisionShape {
         // -------------------- Methods -------------------- //
 
         /// Constructor
-        CollisionShape(CollisionShapeType type);
+        CollisionShape(CollisionShapeType type, decimal margin);
 
         /// Destructor
         virtual ~CollisionShape();
@@ -85,6 +88,9 @@ class CollisionShape {
         /// Return the number of similar created shapes
         uint getNbSimilarCreatedShapes() const;
 
+        /// Return the current object margin
+        decimal getMargin() const;
+
         /// Return the number of bytes used by the collision shape
         virtual size_t getSizeInBytes() const = 0;
 
@@ -95,10 +101,7 @@ class CollisionShape {
         virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction) const=0;
 
         /// Return the local extents in x,y and z direction
-        virtual Vector3 getLocalExtents(decimal margin=0.0) const=0;
+        virtual Vector3 getLocalExtents() const=0;
-
-        /// Return the margin distance around the shape
-        virtual decimal getMargin() const=0;
 
         /// Return the local inertia tensor of the collision shapes
         virtual void computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const=0;
@@ -129,6 +132,11 @@ inline uint CollisionShape::getNbSimilarCreatedShapes() const {
     return mNbSimilarCreatedShapes;
 }
 
+// Return the current object margin
+inline decimal CollisionShape::getMargin() const {
+    return mMargin;
+}
+
 // Increment the number of similar allocated collision shapes
 inline void CollisionShape::incrementNbSimilarCreatedShapes() {
     mNbSimilarCreatedShapes++;
@@ -150,6 +158,8 @@ inline bool CollisionShape::operator==(const CollisionShape& otherCollisionShape
 
     assert(typeid(*this) == typeid(otherCollisionShape));
 
+    if (mMargin != otherCollisionShape.mMargin) return false;
+
     // Check if the two shapes are equal
     return otherCollisionShape.isEqualTo(*this);
 }

src/collision/shapes/ConeShape.cpp

@@ -28,26 +28,14 @@
 #include "../../configuration.h"
 #include "ConeShape.h"
 
-#if defined(VISUAL_DEBUG)
-	#if defined(APPLE_OS)
-		#include <GLUT/glut.h>
-		#include <OpenGL/gl.h>
-	#elif defined(WINDOWS_OS)
-		#include <GL/glut.h>
-		#include <GL/gl.h>
-	#elif defined(LINUX_OS)
-		#include <GL/freeglut.h>
-		#include <GL/gl.h>
-	#endif
-#endif
-
 using namespace reactphysics3d;
 
 // Constructor
-ConeShape::ConeShape(decimal radius, decimal height)
+ConeShape::ConeShape(decimal radius, decimal height, decimal margin)
-          : CollisionShape(CONE), mRadius(radius), mHalfHeight(height * decimal(0.5)) {
+          : CollisionShape(CONE, margin), mRadius(radius), mHalfHeight(height * decimal(0.5)) {
-    assert(mRadius > 0.0);
+    assert(mRadius > decimal(0.0));
-    assert(mHalfHeight > 0.0);
+    assert(mHalfHeight > decimal(0.0));
+    assert(margin > decimal(0.0));
     
     // Compute the sine of the semi-angle at the apex point
     mSinTheta = mRadius / (sqrt(mRadius * mRadius + height * height));
@@ -76,7 +64,7 @@ inline Vector3 ConeShape::getLocalSupportPointWithMargin(const Vector3& directio
     if (direction.lengthSquare() > MACHINE_EPSILON * MACHINE_EPSILON) {
         unitVec = direction.getUnit();
     }
-    supportPoint += unitVec * getMargin();
+    supportPoint += unitVec * mMargin;
 
     return supportPoint;
 }
@@ -104,15 +92,3 @@ inline Vector3 ConeShape::getLocalSupportPointWithoutMargin(const Vector3& direc
 
     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(mRadius, 2.0 * mHalfHeight, 50, 50);
-}
-#endif

src/collision/shapes/ConeShape.h

@@ -70,7 +70,7 @@ class ConeShape : public CollisionShape {
         // -------------------- Methods -------------------- //
 
         /// Constructor
-        ConeShape(decimal mRadius, decimal height);
+        ConeShape(decimal mRadius, decimal height, decimal margin = OBJECT_MARGIN);
 
         /// Destructor
         virtual ~ConeShape();
@@ -81,15 +81,9 @@ class ConeShape : public CollisionShape {
         /// Return the radius
         decimal getRadius() const;
 
-        /// Set the radius
-        void setRadius(decimal radius);
-
         /// Return the height
         decimal getHeight() const;
 
-        /// Set the height
-        void setHeight(decimal height);
-
         /// Return the number of bytes used by the collision shape
         virtual size_t getSizeInBytes() const;
 
@@ -100,21 +94,13 @@ class ConeShape : public CollisionShape {
         virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction) const;
 
         /// Return the local extents in x,y and z direction
-        virtual Vector3 getLocalExtents(decimal margin=0.0) const;
+        virtual Vector3 getLocalExtents() const;
 
         /// Return the local inertia tensor of the collision shape
         virtual void computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const;
 
-        /// Return the margin distance around the shape
-        virtual decimal getMargin() const;
-
         /// Test equality between two cone shapes
         virtual bool isEqualTo(const CollisionShape& otherCollisionShape) const;
-
-#ifdef VISUAL_DEBUG
-        /// Draw the sphere (only for testing purpose)
-        virtual void draw() const;
-#endif
 };
 
 // Allocate and return a copy of the object
@@ -127,35 +113,19 @@ inline decimal ConeShape::getRadius() const {
     return mRadius;
 }
 
-// Set the radius
-inline void ConeShape::setRadius(decimal radius) {
-    mRadius = radius;
-
-    // Update sine of the semi-angle at the apex point
-    mSinTheta = radius / (sqrt(radius * radius + 4 * mHalfHeight * mHalfHeight));
-}
-
 // Return the height
 inline decimal ConeShape::getHeight() const {
     return decimal(2.0) * mHalfHeight;
 }
 
-// Set the height
-inline void ConeShape::setHeight(decimal height) {
-    mHalfHeight = height * decimal(0.5);
-
-    // Update the sine of the semi-angle at the apex point
-    mSinTheta = mRadius / (sqrt(mRadius * mRadius + height * height));
-}
-
 // Return the number of bytes used by the collision shape
 inline size_t ConeShape::getSizeInBytes() const {
     return sizeof(ConeShape);
 }
 
 // Return the local extents in x,y and z direction
-inline Vector3 ConeShape::getLocalExtents(decimal margin) const {
+inline Vector3 ConeShape::getLocalExtents() const {
-    return Vector3(mRadius + margin, mHalfHeight + margin, mRadius + margin);
+    return Vector3(mRadius + mMargin, mHalfHeight + mMargin, mRadius + mMargin);
 }
 
 // Return the local inertia tensor of the collision shape
@@ -167,11 +137,6 @@ inline void ConeShape::computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass
                         0.0, 0.0, 0.0, diagXZ);
 }
 
-// Return the margin distance around the shape
-inline decimal ConeShape::getMargin() const {
-    return OBJECT_MARGIN;
-}
-
 // Test equality between two cone shapes
 inline bool ConeShape::isEqualTo(const CollisionShape& otherCollisionShape) const {
     const ConeShape& otherShape = dynamic_cast<const ConeShape&>(otherCollisionShape);

src/collision/shapes/CylinderShape.cpp

@@ -27,25 +27,15 @@
 #include "CylinderShape.h"
 #include "../../configuration.h"
 
-#if defined(VISUAL_DEBUG)
-	#if defined(APPLE_OS)
-		#include <GLUT/glut.h>
-		#include <OpenGL/gl.h>
-	#elif defined(WINDOWS_OS)
-		#include <GL/glut.h>
-		#include <GL/gl.h>
-	#elif defined(LINUX_OS)
-		#include <GL/freeglut.h>
-		#include <GL/gl.h>
-	#endif
-#endif
-
 using namespace reactphysics3d;
 
 // Constructor
-CylinderShape::CylinderShape(decimal radius, decimal height)
+CylinderShape::CylinderShape(decimal radius, decimal height, decimal margin)
-                 : CollisionShape(CYLINDER), mRadius(radius), mHalfHeight(height/decimal(2.0)) {
+              : CollisionShape(CYLINDER, margin), mRadius(radius),
-
+                mHalfHeight(height/decimal(2.0)) {
+    assert(radius > decimal(0.0));
+    assert(height > decimal(0.0));
+    assert(margin > decimal(0.0));
 }
 
 // Private copy-constructor
@@ -70,7 +60,7 @@ Vector3 CylinderShape::getLocalSupportPointWithMargin(const Vector3& direction)
     if (direction.lengthSquare() > MACHINE_EPSILON * MACHINE_EPSILON) {
         unitVec = direction.getUnit();
     }
-    supportPoint += unitVec * getMargin();
+    supportPoint += unitVec * mMargin;
 
     return supportPoint;
 }
@@ -95,15 +85,3 @@ Vector3 CylinderShape::getLocalSupportPointWithoutMargin(const Vector3& directio
 
     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(mRadius, 50, 50);
-}
-#endif

src/collision/shapes/CylinderShape.h

@@ -66,7 +66,7 @@ class CylinderShape : public CollisionShape {
         // -------------------- Methods -------------------- //
 
         /// Constructor
-        CylinderShape(decimal radius, decimal height);
+        CylinderShape(decimal radius, decimal height, decimal margin = OBJECT_MARGIN);
 
         /// Destructor
         virtual ~CylinderShape();
@@ -77,15 +77,9 @@ class CylinderShape : public CollisionShape {
         /// Return the radius
         decimal getRadius() const;
 
-        /// Set the radius
-        void setRadius(decimal mRadius);
-
         /// Return the height
         decimal getHeight() const;
 
-        /// Set the height
-        void setHeight(decimal height);
-
         /// Return the number of bytes used by the collision shape
         virtual size_t getSizeInBytes() const;
 
@@ -96,21 +90,13 @@ class CylinderShape : public CollisionShape {
         virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction) const;
 
         /// Return the local extents in x,y and z direction
-        virtual Vector3 getLocalExtents(decimal margin=0.0) const;
+        virtual Vector3 getLocalExtents() const;
 
         /// Return the local inertia tensor of the collision shape
         virtual void computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const;
 
-        /// Return the margin distance around the shape
-        virtual decimal getMargin() const;
-
         /// Test equality between two cylinder shapes
         virtual bool isEqualTo(const CollisionShape& otherCollisionShape) const;
-
-#ifdef VISUAL_DEBUG
-        /// Draw the sphere (only for testing purpose)
-        virtual void draw() const;
-#endif
 };
 
 /// Allocate and return a copy of the object
@@ -123,29 +109,19 @@ inline decimal CylinderShape::getRadius() const {
     return mRadius;
 }
 
-// Set the radius
-inline void CylinderShape::setRadius(decimal radius) {
-    this->mRadius = radius;
-}
-
 // Return the height
 inline decimal CylinderShape::getHeight() const {
     return mHalfHeight * decimal(2.0);
 }
 
-// Set the height
-inline void CylinderShape::setHeight(decimal height) {
-    mHalfHeight = height * decimal(0.5);
-}
-
 // Return the number of bytes used by the collision shape
 inline size_t CylinderShape::getSizeInBytes() const {
     return sizeof(CylinderShape);
 }
 
 // Return the local extents in x,y and z direction
-inline Vector3 CylinderShape::getLocalExtents(decimal margin) const {
+inline Vector3 CylinderShape::getLocalExtents() const {
-    return Vector3(mRadius + margin, mHalfHeight + margin, mRadius + margin);
+    return Vector3(mRadius + mMargin, mHalfHeight + mMargin, mRadius + mMargin);
 }
 
 // Return the local inertia tensor of the cylinder
@@ -157,11 +133,6 @@ inline void CylinderShape::computeLocalInertiaTensor(Matrix3x3& tensor, decimal
                         0.0, 0.0, diag);
 }
 
-// Return the margin distance around the shape
-inline decimal CylinderShape::getMargin() const {
-   return OBJECT_MARGIN;
-}
-
 // Test equality between two cylinder shapes
 inline bool CylinderShape::isEqualTo(const CollisionShape& otherCollisionShape) const {
     const CylinderShape& otherShape = dynamic_cast<const CylinderShape&>(otherCollisionShape);

src/collision/shapes/SphereShape.cpp

@@ -28,25 +28,12 @@
 #include "../../configuration.h"
 #include <cassert>
 
-#if defined(VISUAL_DEBUG)
-	#if defined(APPLE_OS)
-		#include <GLUT/glut.h>
-		#include <OpenGL/gl.h>
-	#elif defined(WINDOWS_OS)
-		#include <GL/glut.h>
-		#include <GL/gl.h>
-	#elif defined(LINUX_OS)
-		#include <GL/freeglut.h>
-		#include <GL/gl.h>
-	#endif
-#endif
-
 using namespace reactphysics3d;
 using namespace std;
 
 // Constructor
-SphereShape::SphereShape(decimal radius): CollisionShape(SPHERE), mRadius(radius) {
+SphereShape::SphereShape(decimal radius) : CollisionShape(SPHERE, radius), mRadius(radius) {
-
+    assert(radius > decimal(0.0));
 }
 
 // Private copy-constructor
@@ -59,15 +46,3 @@ SphereShape::SphereShape(const SphereShape& shape)
 SphereShape::~SphereShape() {
 
 }
-
-#ifdef VISUAL_DEBUG
-// Draw the sphere (only for testing purpose)
-void SphereShape::draw() const {
-
-    // Draw in red
-    glColor3f(1.0, 0.0, 0.0);
-
-    // Draw the sphere
-    glutWireSphere(mRadius, 50, 50);
-}
-#endif

src/collision/shapes/SphereShape.h

@@ -71,9 +71,6 @@ class SphereShape : public CollisionShape {
         /// Return the radius of the sphere
         decimal getRadius() const;
 
-        /// Set the radius of the sphere
-        void setRadius(decimal radius);
-
         /// Return the number of bytes used by the collision shape
         virtual size_t getSizeInBytes() const;
 
@@ -84,24 +81,16 @@ class SphereShape : public CollisionShape {
         virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction) const;
 
         /// Return the local extents in x,y and z direction
-        virtual Vector3 getLocalExtents(decimal margin=0.0) const;
+        virtual Vector3 getLocalExtents() const;
 
         /// Return the local inertia tensor of the collision shape
         virtual void computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const;
 
-        /// Return the margin distance around the shape
-        virtual decimal getMargin() const;
-
         /// Update the AABB of a body using its collision shape
         virtual void updateAABB(AABB& aabb, const Transform& transform);
 
         /// Test equality between two sphere shapes
         virtual bool isEqualTo(const CollisionShape& otherCollisionShape) const;
-
-#ifdef VISUAL_DEBUG
-        /// Draw the sphere (only for testing purpose)
-        virtual void draw() const;
-#endif
 };
 
 /// Allocate and return a copy of the object
@@ -114,11 +103,6 @@ inline decimal SphereShape::getRadius() const {
     return mRadius;
 }
 
-// Set the radius of the sphere
-inline void SphereShape::setRadius(decimal radius) {
-    mRadius = radius;
-}
-
 // Return the number of bytes used by the collision shape
 inline size_t SphereShape::getSizeInBytes() const {
     return sizeof(SphereShape);
@@ -127,18 +111,16 @@ inline size_t SphereShape::getSizeInBytes() const {
 // Return a local support point in a given direction with the object margin
 inline Vector3 SphereShape::getLocalSupportPointWithMargin(const Vector3& direction) const {
 
-    decimal margin = getMargin();
-
     // If the direction vector is not the zero vector
     if (direction.lengthSquare() >= MACHINE_EPSILON * MACHINE_EPSILON) {
 
         // Return the support point of the sphere in the given direction
-        return margin * direction.getUnit();
+        return mMargin * direction.getUnit();
     }
 
     // If the direction vector is the zero vector we return a point on the
     // boundary of the sphere
-    return Vector3(0, margin, 0);
+    return Vector3(0, mMargin, 0);
 }
 
 // Return a local support point in a given direction without the object margin
@@ -150,8 +132,8 @@ inline Vector3 SphereShape::getLocalSupportPointWithoutMargin(const Vector3& dir
 
 // Return the local extents of the collision shape (half-width) in x,y and z local direction
 // This method is used to compute the AABB of the box
-inline Vector3 SphereShape::getLocalExtents(decimal margin) const {
+inline Vector3 SphereShape::getLocalExtents() const {
-    return Vector3(mRadius + margin, mRadius + margin, mRadius + margin);
+    return Vector3(mRadius, mRadius, mRadius);
 }
 
 // Return the local inertia tensor of the sphere
@@ -162,16 +144,11 @@ inline void SphereShape::computeLocalInertiaTensor(Matrix3x3& tensor, decimal ma
                         0.0, 0.0, diag);
 }
 
-// Return the margin distance around the shape
-inline decimal SphereShape::getMargin() const {
-    return mRadius + OBJECT_MARGIN;
-}
-
 // Update the AABB of a body using its collision shape
 inline void SphereShape::updateAABB(AABB& aabb, const Transform& transform) {
 
     // Get the local extents in x,y and z direction
-    Vector3 extents = getLocalExtents(OBJECT_MARGIN);
+    Vector3 extents = getLocalExtents();
 
     // Compute the minimum and maximum coordinates of the rotated extents
     Vector3 minCoordinates = transform.getPosition() - extents;

src/configuration.h

@@ -92,7 +92,7 @@ const decimal DEFAULT_FRICTION_COEFFICIENT = decimal(0.3);
 /// True if the deactivation (sleeping) of inactive bodies is enabled
 const bool DEACTIVATION_ENABLED = true;
 
-/// Object margin for collision detection in cm (For GJK-EPA Algorithm)
+/// Object margin for collision detection in meters (for the GJK-EPA Algorithm)
 const decimal OBJECT_MARGIN = decimal(0.04);
 
 /// Distance threshold for two contact points for a valid persistent contact (in meters)

src/constraint/ContactPoint.h

@@ -33,19 +33,6 @@
 #include "../mathematics/mathematics.h"
 #include "../configuration.h"
 
-#if defined(VISUAL_DEBUG)
-	#if defined(APPLE_OS)
-		#include <GLUT/glut.h>
-		#include <OpenGL/gl.h>
-	#elif defined(WINDOWS_OS)
-		#include <GL/glut.h>
-		#include <GL/gl.h>
-	#elif defined(LINUX_OS)
-        #include <GL/freeglut.h>
-        #include <GL/gl.h>
-    #endif
-#endif
-
 /// ReactPhysics3D namespace
 namespace reactphysics3d {
 
@@ -235,11 +222,6 @@ class ContactPoint : public Constraint {
 
         /// Solve the position constraint
         virtual void solvePositionConstraint(const ConstraintSolverData& constraintSolverData);
-
-        #ifdef VISUAL_DEBUG
-            /// Draw the contact (for debugging)
-           void draw() const;
-        #endif
 };
 
 // Return the normal vector of the contact
@@ -352,13 +334,6 @@ inline size_t ContactPoint::getSizeInBytes() const {
     return sizeof(ContactPoint);
 }
 
-#ifdef VISUAL_DEBUG
-inline void ContactPoint::draw() const {
-    glColor3f(1.0, 0.0, 0.0);
-    glutSolidSphere(0.3, 20, 20);
-}
-#endif 
-
 }
 
 #endif