Add error correction using first world order projection (not active by default), change the Shape class into Collider class, add the new decimal type in order to easily switch between single and double precision

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

src/body/Body.cpp

@@ -25,16 +25,16 @@
 
  // Libraries
 #include "Body.h"
-#include "../shapes/Shape.h"
+#include "../shapes/Collider.h"
 
 // We want to use the ReactPhysics3D namespace
 using namespace reactphysics3d;
 
 // Constructor
-Body::Body(const Transform& transform, Shape* shape, double mass, long unsigned int id)
+Body::Body(const Transform& transform, Collider* collider, decimal mass, long unsigned int id)
-     : shape(shape), mass(mass), transform(transform), id(id) {
+     : collider(collider), mass(mass), transform(transform), id(id) {
     assert(mass > 0.0);
-    assert(shape);
+    assert(collider);
 
     isMotionEnabled = true;
     isCollisionEnabled = true;
@@ -44,7 +44,7 @@ Body::Body(const Transform& transform, Shape* shape, double mass, long unsigned
     oldTransform = transform;
 
     // Create the AABB for broad-phase collision detection
-    aabb = new AABB(transform, shape->getLocalExtents(OBJECT_MARGIN));
+    aabb = new AABB(transform, collider->getLocalExtents(OBJECT_MARGIN));
 }
 
 // Destructor

src/body/Body.h

@@ -31,8 +31,8 @@
 #include <cassert>
 #include "../mathematics/Transform.h"
 #include "../shapes/AABB.h"
-#include "../shapes/Shape.h"
+#include "../shapes/Collider.h"
-#include "../constants.h"
+#include "../configuration.h"
 
 // Namespace reactphysics3d
 namespace reactphysics3d {
@@ -46,36 +46,36 @@ namespace reactphysics3d {
 */
 class Body {
     protected :
-        Shape* shape;                   // Collision shape of the body
+        Collider* collider;             // Collider of the body
-        double mass;                    // Mass of the body
+        decimal mass;                   // Mass of the body
         Transform transform;            // Position and orientation of the body
         Transform oldTransform;         // Last position and orientation of the body
-        double interpolationFactor;     // Interpolation factor used for the state interpolation
+        decimal interpolationFactor;    // Interpolation factor used for the state interpolation
         bool isMotionEnabled;           // True if the body is able to move
         bool isCollisionEnabled;        // True if the body can collide with others bodies
         AABB* aabb;                     // Axis-Aligned Bounding Box for Broad-Phase collision detection
         luint id;                       // ID of the body
 
     public :
-        Body(const Transform& transform, Shape* shape, double mass, long unsigned int id);      // Constructor
+        Body(const Transform& transform, Collider* collider, decimal mass, long unsigned int id);       // Constructor
-        virtual ~Body();                                                                        // Destructor
+        virtual ~Body();                                                                                // Destructor
 
-        luint getID() const;                                            // Return the id of the body
+        luint getID() const;                                    // Return the id of the body
-        Shape* getShape() const;                                        // Return the collision shape
+        Collider* getCollider() const;                          // Return the collision collider
-        void setShape(Shape* shape);                                    // Set the collision shape
+        void setCollider(Collider* collider);                         // Set the collision collider
-        double getMass() const;                                         // Return the mass of the body
+        decimal getMass() const;                                // Return the mass of the body
-        void setMass(double mass);                                      // Set the mass of the body
+        void setMass(decimal mass);                             // Set the mass of the body
-        const Transform& getTransform() const;                          // Return the current position and orientation
+        const Transform& getTransform() const;                  // Return the current position and orientation
-        void setTransform(const Transform& transform);                  // Set the current position and orientation
+        void setTransform(const Transform& transform);          // Set the current position and orientation
-        const AABB* getAABB() const;                                    // Return the AAABB of the body
+        const AABB* getAABB() const;                            // Return the AAABB of the body
-        Transform getInterpolatedTransform() const;                     // Return the interpolated transform for rendering
+        Transform getInterpolatedTransform() const;             // Return the interpolated transform for rendering
-        void setInterpolationFactor(double factor);                     // Set the interpolation factor of the body
+        void setInterpolationFactor(decimal factor);            // Set the interpolation factor of the body
-        bool getIsMotionEnabled() const;                                // Return if the rigid body can move
+        bool getIsMotionEnabled() const;                        // Return if the rigid body can move
-        void setIsMotionEnabled(bool isMotionEnabled);                  // Set the value to true if the body can move
+        void setIsMotionEnabled(bool isMotionEnabled);          // Set the value to true if the body can move
-        bool getIsCollisionEnabled() const;                             // Return true if the body can collide with others bodies
+        bool getIsCollisionEnabled() const;                     // Return true if the body can collide with others bodies
-        void setIsCollisionEnabled(bool isCollisionEnabled);            // Set the isCollisionEnabled value
+        void setIsCollisionEnabled(bool isCollisionEnabled);    // Set the isCollisionEnabled value
-        void updateOldTransform();                                      // Update the old transform with the current one
+        void updateOldTransform();                              // Update the old transform with the current one
-        void updateAABB();                                              // Update the Axis-Aligned Bounding Box coordinates
+        void updateAABB();                                      // Update the Axis-Aligned Bounding Box coordinates
 };
 
 // Return the id of the body
@@ -83,20 +83,20 @@ inline luint Body::getID() const {
     return id;
 }                               
 
-// Return the collision shape
+// Return the collider
-inline Shape* Body::getShape() const {
+inline Collider* Body::getCollider() const {
-    assert(shape);
+    assert(collider);
-    return shape;
+    return collider;
 }
 
-// Set the collision shape
+// Set the collider
-inline void Body::setShape(Shape* shape) {
+inline void Body::setCollider(Collider* collider) {
-    assert(shape);
+    assert(collider);
-    this->shape = shape;
+    this->collider = collider;
 }
 
 // Method that return the mass of the body
-inline double Body::getMass() const {
+inline decimal Body::getMass() const {
     return mass;
 };
 
@@ -106,7 +106,7 @@ inline Transform Body::getInterpolatedTransform() const {
 }
 
 // Set the interpolation factor of the body
-inline void Body::setInterpolationFactor(double factor) {
+inline void Body::setInterpolationFactor(decimal factor) {
     // Set the factor
     interpolationFactor = factor;
 }
@@ -122,7 +122,7 @@ inline void Body::setIsMotionEnabled(bool isMotionEnabled) {
 }
 
 // Method that set the mass of the body
-inline void Body::setMass(double mass) {
+inline void Body::setMass(decimal mass) {
     this->mass = mass;
 }
 
@@ -160,7 +160,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(OBJECT_MARGIN));
+    aabb->update(transform, collider->getLocalExtents(OBJECT_MARGIN));
 }
 
 

src/body/RigidBody.cpp

@@ -25,23 +25,23 @@
 
 // Libraries
 #include "RigidBody.h"
-#include "../shapes/Shape.h"
+#include "../shapes/Collider.h"
 
 // We want to use the ReactPhysics3D namespace
 using namespace reactphysics3d;
 
  // Constructor
- RigidBody::RigidBody(const Transform& transform, double mass, const Matrix3x3& inertiaTensorLocal, Shape* shape, long unsigned id)
+ RigidBody::RigidBody(const Transform& transform, decimal mass, const Matrix3x3& inertiaTensorLocal, Collider* collider, long unsigned id)
-           : Body(transform, shape, mass, id), inertiaTensorLocal(inertiaTensorLocal),
+           : Body(transform, collider, mass, id), inertiaTensorLocal(inertiaTensorLocal),
              inertiaTensorLocalInverse(inertiaTensorLocal.getInverse()), massInverse(1.0/mass) {
 
     restitution = 1.0;
 
-    // Set the body pointer of the AABB and the shape
+    // Set the body pointer of the AABB and the collider
     aabb->setBodyPointer(this);
-    shape->setBodyPointer(this);
+    collider->setBodyPointer(this);
 
-    assert(shape);
+    assert(collider);
     assert(aabb);
 }
 

src/body/RigidBody.h

@@ -29,7 +29,7 @@
 // Libraries
 #include <cassert>
 #include "Body.h"
-#include "../shapes/Shape.h"
+#include "../shapes/Collider.h"
 #include "../mathematics/mathematics.h"
 
 // Namespace reactphysics3d
@@ -50,32 +50,32 @@ class RigidBody : public Body {
         Vector3 externalTorque;                 // Current external torque on the body
         Matrix3x3 inertiaTensorLocal;           // Local inertia tensor of the body (in body coordinates)
         Matrix3x3 inertiaTensorLocalInverse;    // Inverse of the inertia tensor of the body (in body coordinates)
-        double massInverse;                     // Inverse of the mass of the body
+        decimal massInverse;                     // Inverse of the mass of the body
-        double restitution;                     // Coefficient of restitution (between 0 and 1), 1 for a very boucing body
+        decimal restitution;                     // Coefficient of restitution (between 0 and 1), 1 for a very boucing body
 
     public :
-        RigidBody(const Transform& transform, double mass, const Matrix3x3& inertiaTensorLocal,
+        RigidBody(const Transform& transform, decimal mass, const Matrix3x3& inertiaTensorLocal,
-                  Shape* shape, long unsigned int id);                                          // Constructor                                                                                                         // Copy-constructor
+                  Collider* collider, long unsigned int id);                                          // Constructor                                                                                                         // Copy-constructor
         virtual ~RigidBody();                                                                   // Destructor
 
         Vector3 getLinearVelocity() const;                                     // Return the linear velocity
         void setLinearVelocity(const Vector3& linearVelocity);                 // Set the linear velocity of the body
         Vector3 getAngularVelocity() const;                                    // Return the angular velocity
         void setAngularVelocity(const Vector3& angularVelocity);               // Set the angular velocity
-        void setMassInverse(double massInverse);                                // Set the inverse of the mass
+        void setMassInverse(decimal massInverse);                              // Set the inverse of the mass
         Vector3 getExternalForce() const;                                      // Return the current external force of the body
         void setExternalForce(const Vector3& force);                           // Set the current external force on the body
         Vector3 getExternalTorque() const;                                     // Return the current external torque of the body
         void setExternalTorque(const Vector3& torque);                         // Set the current external torque of the body
-        double getMassInverse() const;                                          // Return the inverse of the mass of the body
+        decimal getMassInverse() const;                                        // Return the inverse of the mass of the body
-        Matrix3x3 getInertiaTensorLocal() const;                                // Return the local inertia tensor of the body (in body coordinates)
+        Matrix3x3 getInertiaTensorLocal() const;                               // Return the local inertia tensor of the body (in body coordinates)
-        void setInertiaTensorLocal(const Matrix3x3& inertiaTensorLocal);        // Set the local inertia tensor of the body (in body coordinates)
+        void setInertiaTensorLocal(const Matrix3x3& inertiaTensorLocal);       // Set the local inertia tensor of the body (in body coordinates)
-        Matrix3x3 getInertiaTensorLocalInverse() const;                         // Get the inverse of the inertia tensor
+        Matrix3x3 getInertiaTensorLocalInverse() const;                        // Get the inverse of the inertia tensor
-        Matrix3x3 getInertiaTensorWorld() const;                                // Return the inertia tensor in world coordinates
+        Matrix3x3 getInertiaTensorWorld() const;                               // Return the inertia tensor in world coordinates
-        Matrix3x3 getInertiaTensorInverseWorld() const;                         // Return the inverse of the inertia tensor in world coordinates
+        Matrix3x3 getInertiaTensorInverseWorld() const;                        // Return the inverse of the inertia tensor in world coordinates
         
-        double getRestitution() const;                                          // Get the restitution coefficient
+        decimal getRestitution() const;                                        // Get the restitution coefficient
-        void setRestitution(double restitution) throw(std::invalid_argument);   // Set the restitution coefficient
+        void setRestitution(decimal restitution) throw(std::invalid_argument); // Set the restitution coefficient
 };
 
 // Return the linear velocity
@@ -93,7 +93,7 @@ inline void RigidBody::setAngularVelocity(const Vector3& angularVelocity) {
 }
 
 // Set the inverse of the mass
-inline void RigidBody::setMassInverse(double massInverse) {
+inline void RigidBody::setMassInverse(decimal massInverse) {
     this->massInverse = massInverse;
 }
 
@@ -123,7 +123,7 @@ inline void RigidBody::setExternalTorque(const Vector3& torque) {
 }
 
 // Return the inverse of the mass of the body
-inline double RigidBody::getMassInverse() const {
+inline decimal RigidBody::getMassInverse() const {
     return massInverse;
 }
 
@@ -165,12 +165,12 @@ inline void RigidBody::setLinearVelocity(const Vector3& linearVelocity) {
 }
 
 // Get the restitution coeffficient of the rigid body
-inline double RigidBody::getRestitution() const {
+inline decimal RigidBody::getRestitution() const {
     return restitution;
 }
 
 // Set the restitution coefficient
-inline void RigidBody::setRestitution(double restitution) throw(std::invalid_argument) {
+inline void RigidBody::setRestitution(decimal restitution) throw(std::invalid_argument) {
     // Check if the restitution coefficient is between 0 and 1
     if (restitution >= 0.0 && restitution <= 1.0) {
         this->restitution = restitution;

src/collision/CollisionDetection.cpp

@@ -28,9 +28,9 @@
 #include "broadphase/SweepAndPruneAlgorithm.h"
 #include "narrowphase/GJK/GJKAlgorithm.h"
 #include "../body/Body.h"
-#include "../shapes/BoxShape.h"
+#include "../shapes/BoxCollider.h"
 #include "../body/RigidBody.h"
-#include "../constants.h"
+#include "../configuration.h"
 #include <cassert>
 #include <complex>
 #include <set>
@@ -99,7 +99,6 @@ void CollisionDetection::computeBroadPhase() {
         overlappingPairs.erase(*it);
     }
     
-    
     // The current overlapping pairs become the last step overlapping pairs
     lastStepOverlappingPairs = currentStepOverlappingPairs;
 }
@@ -120,14 +119,17 @@ bool CollisionDetection::computeNarrowPhase() {
         (*it).second->update();
         
         // Use the narrow-phase collision detection algorithm to check if there really are a contact
-        if (narrowPhaseAlgorithm->testCollision(body1->getShape(), body1->getTransform(),
+        if (narrowPhaseAlgorithm->testCollision(body1->getCollider(), body1->getTransform(),
-                                                body2->getShape(), body2->getTransform(), contactInfo)) {
+                                                body2->getCollider(), body2->getTransform(), contactInfo)) {
             assert(contactInfo);
             collisionExists = true;
 
             // Create a new contact
             Contact* contact = new(memoryPoolContacts.allocateObject()) Contact(contactInfo);
             
+            // Free the contact info memory
+            delete contactInfo;
+            
             // Add the contact to the contact cache of the corresponding overlapping pair
             (*it).second->addContact(contact);
             
@@ -146,7 +148,7 @@ bool CollisionDetection::computeNarrowPhase() {
 // Allow the broadphase to notify the collision detection about an overlapping pair
 // This method is called by a broad-phase collision detection algorithm
 void CollisionDetection::broadPhaseNotifyOverlappingPair(Body* body1, Body* body2) {
-    // Construct the pair of index
+    // Construct the pair of body index
     pair<luint, luint> indexPair = body1->getID() < body2->getID() ? make_pair(body1->getID(), body2->getID()) :
                                                                 make_pair(body2->getID(), body1->getID());
     assert(indexPair.first != indexPair.second);

src/collision/CollisionDetection.h

@@ -68,12 +68,12 @@ class CollisionDetection {
         bool computeNarrowPhase();                                              // Compute the narrow-phase collision detection
         
     public :
-        CollisionDetection(PhysicsWorld* physicsWorld);                             // Constructor
+        CollisionDetection(PhysicsWorld* physicsWorld);                         // Constructor
-        ~CollisionDetection();                                                      // Destructor
+        ~CollisionDetection();                                                  // Destructor
-                                                                                            
+                                                                                           
-        OverlappingPair* getOverlappingPair(luint body1ID, luint body2ID);          // Return an overlapping pair or null     
+        OverlappingPair* getOverlappingPair(luint body1ID, luint body2ID);      // Return an overlapping pair or null     
-        bool computeCollisionDetection();                                           // Compute the collision detection
+        bool computeCollisionDetection();                                       // Compute the collision detection
-        void broadPhaseNotifyOverlappingPair(Body* body1, Body* body2);             // Allow the broadphase to notify the collision detection about an overlapping pair
+        void broadPhaseNotifyOverlappingPair(Body* body1, Body* body2);         // Allow the broadphase to notify the collision detection about an overlapping pair
 };
 
 // Return an overlapping pair of bodies according to the given bodies ID

src/collision/ContactInfo.cpp

@@ -30,7 +30,7 @@ using namespace reactphysics3d;
 
 
 // Constructor for GJK
-ContactInfo::ContactInfo(Body* body1, Body* body2, const Vector3& normal, double penetrationDepth,
+ContactInfo::ContactInfo(Body* body1, Body* body2, const Vector3& normal, decimal penetrationDepth,
                          const Vector3& localPoint1, const Vector3& localPoint2,
                          const Transform& transform1, const Transform& transform2)
             : body1(body1), body2(body2), normal(normal), penetrationDepth(penetrationDepth),

src/collision/ContactInfo.h

@@ -27,7 +27,7 @@
 #define	CONTACT_INFO_H
 
 // Libraries
-#include "../shapes/BoxShape.h"
+#include "../shapes/BoxCollider.h"
 #include "../mathematics/mathematics.h"
 
 // ReactPhysics3D namespace
@@ -46,13 +46,13 @@ struct ContactInfo {
         Body* const body1;              // Pointer to the first body of the contact
         Body* const body2;              // Pointer to the second body of the contact
         const Vector3 normal;           // Normal vector the the collision contact in world space
-        const double penetrationDepth;  // Penetration depth of the contact
+        const decimal penetrationDepth;  // Penetration depth of the contact
         const Vector3 localPoint1;      // Contact point of body 1 in local space of body 1
         const Vector3 localPoint2;      // Contact point of body 2 in local space of body 2
         const Vector3 worldPoint1;      // Contact point of body 1 in world space
         const Vector3 worldPoint2;      // Contact point of body 2 in world space
         
-        ContactInfo(Body* body1, Body* body2, const Vector3& normal, double penetrationDepth,
+        ContactInfo(Body* body1, Body* body2, const Vector3& normal, decimal penetrationDepth,
                     const Vector3& localPoint1, const Vector3& localPoint2,
                     const Transform& transform1, const Transform& transform2);    // Constructor for GJK
 };

src/collision/PersistentContactCache.cpp

@@ -131,7 +131,7 @@ void PersistentContactCache::update(const Transform& transform1, const Transform
 int PersistentContactCache::getIndexOfDeepestPenetration(Contact* newContact) const {
     assert(nbContacts == MAX_CONTACTS_IN_CACHE);
     int indexMaxPenetrationDepth = -1;
-    double maxPenetrationDepth = newContact->getPenetrationDepth();
+    decimal maxPenetrationDepth = newContact->getPenetrationDepth();
 
     // For each contact in the cache
     for (uint i=0; i<nbContacts; i++) {
@@ -152,10 +152,10 @@ int PersistentContactCache::getIndexOfDeepestPenetration(Contact* newContact) co
 // kept.
 int PersistentContactCache::getIndexToRemove(int indexMaxPenetration, const Vector3& newPoint) const {
     assert(nbContacts == MAX_CONTACTS_IN_CACHE);
-    double area0 = 0.0;       // Area with contact 1,2,3 and newPoint
+    decimal area0 = 0.0;       // Area with contact 1,2,3 and newPoint
-    double area1 = 0.0;       // Area with contact 0,2,3 and newPoint
+    decimal area1 = 0.0;       // Area with contact 0,2,3 and newPoint
-    double area2 = 0.0;       // Area with contact 0,1,3 and newPoint
+    decimal area2 = 0.0;       // Area with contact 0,1,3 and newPoint
-    double area3 = 0.0;       // Area with contact 0,1,2 and newPoint
+    decimal area3 = 0.0;       // Area with contact 0,1,2 and newPoint
 
     if (indexMaxPenetration != 0) {
         // Compute the area
@@ -191,7 +191,7 @@ int PersistentContactCache::getIndexToRemove(int indexMaxPenetration, const Vect
 }
 
 // Return the index of maximum area
-int PersistentContactCache::getMaxArea(double area0, double area1, double area2, double area3) const {
+int PersistentContactCache::getMaxArea(decimal area0, decimal area1, decimal area2, decimal area3) const {
     if (area0 < area1) {
         if (area1 < area2) {
             if (area2 < area3) return 3;

src/collision/PersistentContactCache.h

@@ -59,11 +59,11 @@ class PersistentContactCache {
         uint nbContacts;                            // Number of contacts in the cache
         MemoryPool<Contact>& memoryPoolContacts;    // Reference to the memory pool with the contacts
 
-        int getMaxArea(double area0, double area1, double area2, double area3) const;   // Return the index of maximum area
+        int getMaxArea(decimal area0, decimal area1, decimal area2, decimal area3) const;   // Return the index of maximum area
-        int getIndexOfDeepestPenetration(Contact* newContact) const;                    // Return the index of the contact with the larger penetration depth
+        int getIndexOfDeepestPenetration(Contact* newContact) const;                        // Return the index of the contact with the larger penetration depth
-        int getIndexToRemove(int indexMaxPenetration, const Vector3& newPoint) const;   // Return the index that will be removed
+        int getIndexToRemove(int indexMaxPenetration, const Vector3& newPoint) const;       // Return the index that will be removed
-        void removeContact(int index);                                                  // Remove a contact from the cache
+        void removeContact(int index);                                                      // Remove a contact from the cache
-        bool isApproxEqual(const Vector3& vector1, const Vector3& vector2) const;       // Return true if two vectors are approximatively equal    
+        bool isApproxEqual(const Vector3& vector1, const Vector3& vector2) const;           // Return true if two vectors are approximatively equal    
         
     public:
         PersistentContactCache(Body* const body1, Body* const body2, MemoryPool<Contact>& memoryPoolContacts);  // Constructor
@@ -88,7 +88,7 @@ inline Contact* PersistentContactCache::getContact(uint index) const {
 
 // Return true if two vectors are approximatively equal
 inline bool PersistentContactCache::isApproxEqual(const Vector3& vector1, const Vector3& vector2) const {
-    const double epsilon = 0.1;
+    const decimal epsilon = 0.1;
     return (approxEqual(vector1.getX(), vector2.getX(), epsilon) &&
             approxEqual(vector1.getY(), vector2.getY(), epsilon) &&
             approxEqual(vector1.getZ(), vector2.getZ(), epsilon));

src/collision/broadphase/SweepAndPruneAlgorithm.cpp

@@ -79,13 +79,13 @@ void SweepAndPruneAlgorithm::notifyAddedBodies(vector<RigidBody*> bodies) {
 // the collision detection object about overlapping pairs using the
 // broadPhaseNotifyOverlappingPair() method from the CollisionDetection class
 void SweepAndPruneAlgorithm::computePossibleCollisionPairs() {
-    double variance[3];                             // Variance of the distribution of the AABBs on the three x, y and z axis
+    decimal variance[3];                            // Variance of the distribution of the AABBs on the three x, y and z axis
-    double esperance[] = {0.0, 0.0, 0.0};           // Esperance of the distribution of the AABBs on the three x, y and z axis
+    decimal esperance[] = {0.0, 0.0, 0.0};           // Esperance of the distribution of the AABBs on the three x, y and z axis
-    double esperanceSquare[] = {0.0, 0.0, 0.0};     // Esperance of the square of the distribution values of the AABBs on the three x, y and z axis
+    decimal esperanceSquare[] = {0.0, 0.0, 0.0};     // Esperance of the square of the distribution values of the AABBs on the three x, y and z axis
     vector<const AABB*>::iterator it;               // Iterator on the sortedAABBs set
     vector<const AABB*>::iterator it2;              // Second iterator
     Vector3 center3D;                               // Center of the current AABB
-    double center[3];                               // Coordinates of the center of the current AABB
+    decimal center[3];                               // Coordinates of the center of the current AABB
     int i;
     const Body* body;                               // Body pointer on the body corresponding to an AABB
     uint nbAABBs = sortedAABBs.size();              // Number of AABBs

src/collision/broadphase/SweepAndPruneAlgorithm.h

@@ -23,14 +23,13 @@
 *                                                                               *
 ********************************************************************************/
 
-#ifndef SAP_ALGORITHM_H
+#ifndef SWEEP_AND_PRUNE_ALGORITHM_H
-#define SAP_ALGORITHM_H
+#define SWEEP_AND_PRUNE_ALGORITHM_H
 
 // Libraries
 #include "BroadPhaseAlgorithm.h"
 #include "../../shapes/AABB.h"
 
-// TODO : Rename this class SweepAndPruneAlgorithm
 
 // Namespace ReactPhysics3D
 namespace reactphysics3d {

src/collision/narrowphase/EPA/EPAAlgorithm.cpp

@@ -80,26 +80,26 @@ int EPAAlgorithm::isOriginInTetrahedron(const Vector3& p1, const Vector3& p2, co
 // intersect. An initial simplex that contains origin has been computed with
 // GJK algorithm. The EPA Algorithm will extend this simplex polytope to find
 // the correct penetration depth
-bool EPAAlgorithm::computePenetrationDepthAndContactPoints(Simplex simplex, const Shape* shape1, const Transform& transform1,
+bool EPAAlgorithm::computePenetrationDepthAndContactPoints(Simplex simplex, const Collider* collider1, const Transform& transform1,
-                                                           const Shape* shape2, const Transform& transform2,
+                                                           const Collider* collider2, const Transform& transform2,
                                                            Vector3& v, ContactInfo*& contactInfo) {
     Vector3 suppPointsA[MAX_SUPPORT_POINTS];       // Support points of object A in local coordinates
     Vector3 suppPointsB[MAX_SUPPORT_POINTS];       // Support points of object B in local coordinates
     Vector3 points[MAX_SUPPORT_POINTS];            // Current points
-    TrianglesStore triangleStore;                   // Store the triangles
+    TrianglesStore triangleStore;                  // Store the triangles
-    TriangleEPA* triangleHeap[MAX_FACETS];          // Heap that contains the face candidate of the EPA algorithm
+    TriangleEPA* triangleHeap[MAX_FACETS];         // Heap that contains the face candidate of the EPA algorithm
 
-    // Transform a point from body space of shape 2 to body space of shape 1 (the GJK algorithm is done in body space of shape 1)
+    // Transform a point from local space of body 2 to local space of body 1 (the GJK algorithm is done in local space of body 1)
-    Transform shape2ToShape1 = transform1.inverse() * transform2;
+    Transform body2Tobody1 = transform1.inverse() * transform2;
 
-    // Matrix that transform a direction from body space of shape 1 into body space of shape 2
+    // Matrix that transform a direction from local space of body 1 into local space of body 2
-    Matrix3x3 rotateToShape2 = transform2.getOrientation().getMatrix().getTranspose() * transform1.getOrientation().getMatrix();
+    Matrix3x3 rotateToBody2 = transform2.getOrientation().getMatrix().getTranspose() * transform1.getOrientation().getMatrix();
 
     // Get the simplex computed previously by the GJK algorithm
     unsigned int nbVertices = simplex.getSimplex(suppPointsA, suppPointsB, points);
 
     // Compute the tolerance
-    double tolerance = MACHINE_EPSILON * simplex.getMaxLengthSquareOfAPoint();
+    decimal tolerance = MACHINE_EPSILON * simplex.getMaxLengthSquareOfAPoint();
 
     // Number of triangles in the polytope
     unsigned int nbTriangles = 0;
@@ -132,7 +132,7 @@ bool EPAAlgorithm::computePenetrationDepthAndContactPoints(Simplex simplex, cons
             int minAxis = d.getAbsoluteVector().getMinAxis();
 
             // Compute sin(60)
-            const double sin60 = sqrt(3.0) * 0.5;
+            const decimal sin60 = sqrt(3.0) * 0.5;
 
             // Create a rotation quaternion to rotate the vector v1 to get the vectors
             // v2 and v3
@@ -147,18 +147,18 @@ bool EPAAlgorithm::computePenetrationDepthAndContactPoints(Simplex simplex, cons
             Vector3 v3 = rotationMat * v2;
 
             // Compute the support point in the direction of v1
-            suppPointsA[2] = shape1->getLocalSupportPoint(v1, OBJECT_MARGIN);
+            suppPointsA[2] = collider1->getLocalSupportPoint(v1, OBJECT_MARGIN);
-            suppPointsB[2] = shape2ToShape1 * shape2->getLocalSupportPoint(rotateToShape2 * (-v1), OBJECT_MARGIN);
+            suppPointsB[2] = body2Tobody1 * collider2->getLocalSupportPoint(rotateToBody2 * (-v1), OBJECT_MARGIN);
             points[2] = suppPointsA[2] - suppPointsB[2];
 
             // Compute the support point in the direction of v2
-            suppPointsA[3] = shape1->getLocalSupportPoint(v2, OBJECT_MARGIN);
+            suppPointsA[3] = collider1->getLocalSupportPoint(v2, OBJECT_MARGIN);
-            suppPointsB[3] = shape2ToShape1 * shape2->getLocalSupportPoint(rotateToShape2 * (-v2), OBJECT_MARGIN);
+            suppPointsB[3] = body2Tobody1 * collider2->getLocalSupportPoint(rotateToBody2 * (-v2), OBJECT_MARGIN);
             points[3] = suppPointsA[3] - suppPointsB[3];
 
             // Compute the support point in the direction of v3
-            suppPointsA[4] = shape1->getLocalSupportPoint(v3, OBJECT_MARGIN);
+            suppPointsA[4] = collider1->getLocalSupportPoint(v3, OBJECT_MARGIN);
-            suppPointsB[4] = shape2ToShape1 * shape2->getLocalSupportPoint(rotateToShape2 * (-v3), OBJECT_MARGIN);
+            suppPointsB[4] = body2Tobody1 * collider2->getLocalSupportPoint(rotateToBody2 * (-v3), OBJECT_MARGIN);
             points[4] = suppPointsA[4] - suppPointsB[4];
 
             // Now we have an hexahedron (two tetrahedron glued together). We can simply keep the
@@ -221,10 +221,10 @@ bool EPAAlgorithm::computePenetrationDepthAndContactPoints(Simplex simplex, cons
                 link(EdgeEPA(face2, 1), EdgeEPA(face3, 1));
 
                 // Add the triangle faces in the candidate heap
-                addFaceCandidate(face0, triangleHeap, nbTriangles, DBL_MAX);
+                addFaceCandidate(face0, triangleHeap, nbTriangles, DECIMAL_MAX);
-                addFaceCandidate(face1, triangleHeap, nbTriangles, DBL_MAX);
+                addFaceCandidate(face1, triangleHeap, nbTriangles, DECIMAL_MAX);
-                addFaceCandidate(face2, triangleHeap, nbTriangles, DBL_MAX);
+                addFaceCandidate(face2, triangleHeap, nbTriangles, DECIMAL_MAX);
-                addFaceCandidate(face3, triangleHeap, nbTriangles, DBL_MAX);
+                addFaceCandidate(face3, triangleHeap, nbTriangles, DECIMAL_MAX);
 
                 break;
             }
@@ -252,11 +252,11 @@ bool EPAAlgorithm::computePenetrationDepthAndContactPoints(Simplex simplex, cons
             Vector3 n = v1.cross(v2);
 
             // Compute the two new vertices to obtain a hexahedron
-            suppPointsA[3] = shape1->getLocalSupportPoint(n, OBJECT_MARGIN);
+            suppPointsA[3] = collider1->getLocalSupportPoint(n, OBJECT_MARGIN);
-            suppPointsB[3] = shape2ToShape1 * shape2->getLocalSupportPoint(rotateToShape2 * (-n), OBJECT_MARGIN);
+            suppPointsB[3] = body2Tobody1 * collider2->getLocalSupportPoint(rotateToBody2 * (-n), OBJECT_MARGIN);
             points[3] = suppPointsA[3] - suppPointsB[3];
-            suppPointsA[4] = shape1->getLocalSupportPoint(-n, OBJECT_MARGIN);
+            suppPointsA[4] = collider1->getLocalSupportPoint(-n, OBJECT_MARGIN);
-            suppPointsB[4] = shape2ToShape1 * shape2->getLocalSupportPoint(rotateToShape2 * n, OBJECT_MARGIN);
+            suppPointsB[4] = body2Tobody1 * collider2->getLocalSupportPoint(rotateToBody2 * n, OBJECT_MARGIN);
             points[4] = suppPointsA[4] - suppPointsB[4];
 
             // Construct the triangle faces
@@ -287,12 +287,12 @@ bool EPAAlgorithm::computePenetrationDepthAndContactPoints(Simplex simplex, cons
             link(EdgeEPA(face5, 1), EdgeEPA(face3, 2));
 
             // Add the candidate faces in the heap
-            addFaceCandidate(face0, triangleHeap, nbTriangles, DBL_MAX);
+            addFaceCandidate(face0, triangleHeap, nbTriangles, DECIMAL_MAX);
-            addFaceCandidate(face1, triangleHeap, nbTriangles, DBL_MAX);
+            addFaceCandidate(face1, triangleHeap, nbTriangles, DECIMAL_MAX);
-            addFaceCandidate(face2, triangleHeap, nbTriangles, DBL_MAX);
+            addFaceCandidate(face2, triangleHeap, nbTriangles, DECIMAL_MAX);
-            addFaceCandidate(face3, triangleHeap, nbTriangles, DBL_MAX);
+            addFaceCandidate(face3, triangleHeap, nbTriangles, DECIMAL_MAX);
-            addFaceCandidate(face4, triangleHeap, nbTriangles, DBL_MAX);
+            addFaceCandidate(face4, triangleHeap, nbTriangles, DECIMAL_MAX);
-            addFaceCandidate(face5, triangleHeap, nbTriangles, DBL_MAX);
+            addFaceCandidate(face5, triangleHeap, nbTriangles, DECIMAL_MAX);
 
             nbVertices = 5;
         }
@@ -307,7 +307,7 @@ bool EPAAlgorithm::computePenetrationDepthAndContactPoints(Simplex simplex, cons
     }
 
     TriangleEPA* triangle = 0;
-    double upperBoundSquarePenDepth = DBL_MAX;
+    decimal upperBoundSquarePenDepth = DECIMAL_MAX;
 
     do {
         triangle = triangleHeap[0];
@@ -325,23 +325,23 @@ bool EPAAlgorithm::computePenetrationDepthAndContactPoints(Simplex simplex, cons
             }
 
             // Compute the support point of the Minkowski difference (A-B) in the closest point direction
-            suppPointsA[nbVertices] = shape1->getLocalSupportPoint(triangle->getClosestPoint(), OBJECT_MARGIN);
+            suppPointsA[nbVertices] = collider1->getLocalSupportPoint(triangle->getClosestPoint(), OBJECT_MARGIN);
-            suppPointsB[nbVertices] = shape2ToShape1 *shape2->getLocalSupportPoint(rotateToShape2 * (-triangle->getClosestPoint()), OBJECT_MARGIN);
+            suppPointsB[nbVertices] = body2Tobody1 * collider2->getLocalSupportPoint(rotateToBody2 * (-triangle->getClosestPoint()), OBJECT_MARGIN);
             points[nbVertices] = suppPointsA[nbVertices] - suppPointsB[nbVertices];
 
             int indexNewVertex = nbVertices;
             nbVertices++;
 
             // Update the upper bound of the penetration depth
-            double wDotv = points[indexNewVertex].dot(triangle->getClosestPoint());
+            decimal wDotv = points[indexNewVertex].dot(triangle->getClosestPoint());
             assert(wDotv > 0.0);
-            double wDotVSquare = wDotv * wDotv / triangle->getDistSquare();
+            decimal wDotVSquare = wDotv * wDotv / triangle->getDistSquare();
             if (wDotVSquare < upperBoundSquarePenDepth) {
                 upperBoundSquarePenDepth = wDotVSquare;
             }
 
             // Compute the error
-            double error = wDotv - triangle->getDistSquare();
+            decimal error = wDotv - triangle->getDistSquare();
             if (error <= std::max(tolerance, REL_ERROR_SQUARE * wDotv) ||
                 points[indexNewVertex] == points[(*triangle)[0]] ||
                 points[indexNewVertex] == points[(*triangle)[1]] ||
@@ -370,11 +370,11 @@ bool EPAAlgorithm::computePenetrationDepthAndContactPoints(Simplex simplex, cons
     // Compute the contact info
     v = transform1.getOrientation().getMatrix() * triangle->getClosestPoint();
     Vector3 pALocal = triangle->computeClosestPointOfObject(suppPointsA);
-    Vector3 pBLocal = shape2ToShape1.inverse() * triangle->computeClosestPointOfObject(suppPointsB);
+    Vector3 pBLocal = body2Tobody1.inverse() * triangle->computeClosestPointOfObject(suppPointsB);
     Vector3 normal = v.getUnit();
-    double penetrationDepth = v.length();
+    decimal penetrationDepth = v.length();
     assert(penetrationDepth > 0.0);
-    contactInfo = new ContactInfo(shape1->getBodyPointer(), shape2->getBodyPointer(), normal,
+    contactInfo = new ContactInfo(collider1->getBodyPointer(), collider2->getBodyPointer(), normal,
                                   penetrationDepth, pALocal, pBLocal, transform1, transform2);
     
     return true;

src/collision/narrowphase/EPA/EPAAlgorithm.h

@@ -28,7 +28,7 @@
 
 // Libraries
 #include "../GJK/Simplex.h"
-#include "../../../shapes/Shape.h"
+#include "../../../shapes/Collider.h"
 #include "../../ContactInfo.h"
 #include "../../../mathematics/mathematics.h"
 #include "TriangleEPA.h"
@@ -75,7 +75,7 @@ class EPAAlgorithm {
         TriangleComparison triangleComparison;           // Triangle comparison operator
 
         void addFaceCandidate(TriangleEPA* triangle, TriangleEPA** heap,
-                              uint& nbTriangles, double upperBoundSquarePenDepth);      // Add a triangle face in the candidate triangle heap
+                              uint& nbTriangles, decimal upperBoundSquarePenDepth);      // Add a triangle face in the candidate triangle heap
         int isOriginInTetrahedron(const Vector3& p1, const Vector3& p2,
                                   const Vector3& p3, const Vector3& p4) const;        // Decide if the origin is in the tetrahedron
 
@@ -83,14 +83,14 @@ class EPAAlgorithm {
         EPAAlgorithm();         // Constructor
         ~EPAAlgorithm();        // Destructor
 
-        bool computePenetrationDepthAndContactPoints(Simplex simplex, const Shape* shape1, const Transform& transform1,
+        bool computePenetrationDepthAndContactPoints(Simplex simplex, const Collider* collider1, const Transform& transform1,
-                                                     const Shape* shape2, const Transform& transform2,
+                                                     const Collider* collider2, const Transform& transform2,
                                                      Vector3& v, ContactInfo*& contactInfo);                         // Compute the penetration depth with EPA algorithm
 };
 
 // Add a triangle face in the candidate triangle heap in the EPA algorithm
 inline void EPAAlgorithm::addFaceCandidate(TriangleEPA* triangle, TriangleEPA** heap,
-                                           uint& nbTriangles, double upperBoundSquarePenDepth) {
+                                           uint& nbTriangles, decimal upperBoundSquarePenDepth) {
     
     // If the closest point of the affine hull of triangle points is internal to the triangle and
     // if the distance of the closest point from the origin is at most the penetration depth upper bound

src/collision/narrowphase/EPA/TriangleEPA.cpp

@@ -56,11 +56,11 @@ bool TriangleEPA::computeClosestPoint(const Vector3* vertices) {
 
     Vector3 v1 = vertices[indicesVertices[1]] - p0;
     Vector3 v2 = vertices[indicesVertices[2]] - p0;
-    double v1Dotv1 = v1.dot(v1);
+    decimal v1Dotv1 = v1.dot(v1);
-    double v1Dotv2 = v1.dot(v2);
+    decimal v1Dotv2 = v1.dot(v2);
-    double v2Dotv2 = v2.dot(v2);
+    decimal v2Dotv2 = v2.dot(v2);
-    double p0Dotv1 = p0.dot(v1);
+    decimal p0Dotv1 = p0.dot(v1);
-    double p0Dotv2 = p0.dot(v2);
+    decimal p0Dotv2 = p0.dot(v2);
 
     // Compute determinant
     det = v1Dotv1 * v2Dotv2 - v1Dotv2 * v1Dotv2;
@@ -126,7 +126,7 @@ bool TriangleEPA::computeSilhouette(const Vector3* vertices, uint indexNewVertex
     // Mark the current triangle as obsolete because it
     setIsObsolete(true);
 
-    // Execute recursively the silhouette algorithm for the ajdacent edges of neighbouring
+    // Execute recursively the silhouette algorithm for the adjacent edges of neighboring
     // triangles of the current triangle
     bool result = adjacentEdges[0].computeSilhouette(vertices, indexNewVertex, triangleStore) &&
                   adjacentEdges[1].computeSilhouette(vertices, indexNewVertex, triangleStore) &&

src/collision/narrowphase/EPA/TriangleEPA.h

@@ -28,7 +28,7 @@
 
 // Libraries
 #include "../../../mathematics/mathematics.h"
-#include "../../../constants.h"
+#include "../../../configuration.h"
 #include "EdgeEPA.h"
 #include <cassert>
 
@@ -51,11 +51,11 @@ class TriangleEPA {
         uint indicesVertices[3];    // Indices of the vertices y_i of the triangle
         EdgeEPA adjacentEdges[3];   // Three adjacent edges of the triangle (edges of other triangles)
         bool isObsolete;            // True if the triangle face is visible from the new support point
-        double det;                 // Determinant
+        decimal det;                // Determinant
-        Vector3 closestPoint;      // Point v closest to the origin on the affine hull of the triangle
+        Vector3 closestPoint;       // Point v closest to the origin on the affine hull of the triangle
-        double lambda1;             // Lambda1 value such that v = lambda0 * y_0 + lambda1 * y_1 + lambda2 * y_2
+        decimal lambda1;            // Lambda1 value such that v = lambda0 * y_0 + lambda1 * y_1 + lambda2 * y_2
-        double lambda2;             // Lambda1 value such that v = lambda0 * y_0 + lambda1 * y_1 + lambda2 * y_2
+        decimal lambda2;            // Lambda1 value such that v = lambda0 * y_0 + lambda1 * y_1 + lambda2 * y_2
-        double distSquare;          // Square distance of the point closest point v to the origin
+        decimal distSquare;         // Square distance of the point closest point v to the origin
 
     public:
         TriangleEPA();                              // Constructor
@@ -64,7 +64,7 @@ class TriangleEPA {
 
         EdgeEPA& getAdjacentEdge(int index);                                                // Return an adjacent edge of the triangle
         void setAdjacentEdge(int index, EdgeEPA& edge);                                     // Set an adjacent edge of the triangle
-        double getDistSquare() const;                                                       // Return the square distance  of the closest point to origin
+        decimal getDistSquare() const;                                                       // Return the square distance  of the closest point to origin
         void setIsObsolete(bool isObsolete);                                                // Set the isObsolete value
         bool getIsObsolete() const;                                                         // Return true if the triangle face is obsolete
         const Vector3& getClosestPoint() const;                                            // Return the point closest to the origin
@@ -93,7 +93,7 @@ inline void TriangleEPA::setAdjacentEdge(int index, EdgeEPA& edge) {
 }
 
 // Return the square distance  of the closest point to origin
-inline double TriangleEPA::getDistSquare() const {
+inline decimal TriangleEPA::getDistSquare() const {
     return distSquare;
 }
 

src/collision/narrowphase/EPA/TrianglesStore.h

@@ -89,7 +89,7 @@ inline TriangleEPA& TrianglesStore::last() {
 inline TriangleEPA* TrianglesStore::newTriangle(const Vector3* vertices, uint v0, uint v1, uint v2) {
     TriangleEPA* newTriangle = 0;
 
-    // If we have not reach the maximum number of triangles
+    // If we have not reached the maximum number of triangles
     if (nbTriangles != MAX_TRIANGLES) {
         newTriangle = &triangles[nbTriangles++];
         new (newTriangle) TriangleEPA(v0, v1, v2);

src/collision/narrowphase/GJK/GJKAlgorithm.cpp

@@ -27,7 +27,7 @@
 #include "GJKAlgorithm.h"
 #include "Simplex.h"
 #include "../../../constraint/Contact.h"
-#include "../../../constants.h"
+#include "../../../configuration.h"
 #include <algorithm>
 #include <cmath>
 #include <cfloat>
@@ -58,31 +58,31 @@ GJKAlgorithm::~GJKAlgorithm() {
 // algorithm on the enlarged object to obtain a simplex polytope that contains the
 // origin, they we give that simplex polytope to the EPA algorithm which will compute
 // the correct penetration depth and contact points between the enlarged objects.
-bool GJKAlgorithm::testCollision(const Shape* shape1, const Transform& transform1,
+bool GJKAlgorithm::testCollision(const Collider* collider1, const Transform& transform1,
-                                 const Shape* shape2,  const Transform& transform2,
+                                 const Collider* collider2,  const Transform& transform2,
 		                         ContactInfo*& contactInfo) {
 
-    assert(shape1 != shape2);
+    assert(collider1 != collider2);
     
     Vector3 suppA;             // Support point of object A
     Vector3 suppB;             // Support point of object B
     Vector3 w;                 // Support point of Minkowski difference A-B
     Vector3 pA;                // Closest point of object A
     Vector3 pB;                // Closest point of object B
-    double vDotw;
+    decimal vDotw;
-    double prevDistSquare;
+    decimal prevDistSquare;
-    Body* const body1 = shape1->getBodyPointer();
+    Body* const body1 = collider1->getBodyPointer();
-    Body* const body2 = shape2->getBodyPointer();
+    Body* const body2 = collider2->getBodyPointer();
 
-    // Transform a point from body space of shape 2 to body space of shape 1 (the GJK algorithm is done in body space of shape 1)
+    // Transform a point from local space of body 2 to local space of body 1 (the GJK algorithm is done in local space of body 1)
-    Transform shape2ToShape1 = transform1.inverse() * transform2;
+    Transform body2Tobody1 = transform1.inverse() * transform2;
 
-    // Matrix that transform a direction from body space of shape 1 into body space of shape 2
+    // Matrix that transform a direction from local space of body 1 into local space of body 2
-    Matrix3x3 rotateToShape2 = transform2.getOrientation().getMatrix().getTranspose() * transform1.getOrientation().getMatrix();
+    Matrix3x3 rotateToBody2 = transform2.getOrientation().getMatrix().getTranspose() * transform1.getOrientation().getMatrix();
 
     // Initialize the margin (sum of margins of both objects)
-    double margin = 2 * OBJECT_MARGIN;
+    decimal margin = 2 * OBJECT_MARGIN;
-    double marginSquare = margin * margin;
+    decimal marginSquare = margin * margin;
     assert(margin > 0.0);
 
     // Create a simplex set
@@ -93,12 +93,12 @@ bool GJKAlgorithm::testCollision(const Shape* shape1, const Transform& transform
 	Vector3 v = (overlappingPair) ? overlappingPair->getCachedSeparatingAxis() : Vector3(1.0, 1.0, 1.0);
 			
     // Initialize the upper bound for the square distance
-    double distSquare = DBL_MAX;
+    decimal distSquare = DECIMAL_MAX;
     
     do {
         // Compute the support points for original objects (without margins) A and B
-        suppA = shape1->getLocalSupportPoint(-v);
+        suppA = collider1->getLocalSupportPoint(-v);
-        suppB = shape2ToShape1 * shape2->getLocalSupportPoint(rotateToShape2 * v);
+        suppB = body2Tobody1 * collider2->getLocalSupportPoint(rotateToBody2 * v);
 
         // Compute the support point for the Minkowski difference A-B
         w = suppA - suppB;
@@ -118,14 +118,14 @@ bool GJKAlgorithm::testCollision(const Shape* shape1, const Transform& transform
 
             // Project those two points on the margins to have the closest points of both
             // object with the margins
-            double dist = sqrt(distSquare);
+            decimal dist = sqrt(distSquare);
             assert(dist > 0.0);
             pA = (pA - (OBJECT_MARGIN / dist) * v);
-            pB = shape2ToShape1.inverse() * (pB + (OBJECT_MARGIN / dist) * v);
+            pB = body2Tobody1.inverse() * (pB + (OBJECT_MARGIN / dist) * v);
 
             // Compute the contact info
             Vector3 normal = transform1.getOrientation().getMatrix() * (-v.getUnit());
-            double penetrationDepth = margin - dist;
+            decimal penetrationDepth = margin - dist;
 			
 			// Reject the contact if the penetration depth is negative (due too numerical errors)
 			if (penetrationDepth <= 0.0) return false;
@@ -146,14 +146,14 @@ bool GJKAlgorithm::testCollision(const Shape* shape1, const Transform& transform
 
             // Project those two points on the margins to have the closest points of both
             // object with the margins
-            double dist = sqrt(distSquare);
+            decimal dist = sqrt(distSquare);
             assert(dist > 0.0);
             pA = (pA - (OBJECT_MARGIN / dist) * v);
-            pB = shape2ToShape1.inverse() * (pB + (OBJECT_MARGIN / dist) * v);
+            pB = body2Tobody1.inverse() * (pB + (OBJECT_MARGIN / dist) * v);
 
             // Compute the contact info
             Vector3 normal = transform1.getOrientation().getMatrix() * (-v.getUnit());
-            double penetrationDepth = margin - dist;
+            decimal penetrationDepth = margin - dist;
 			
 			// Reject the contact if the penetration depth is negative (due too numerical errors)
 			if (penetrationDepth <= 0.0) return false;
@@ -172,14 +172,14 @@ bool GJKAlgorithm::testCollision(const Shape* shape1, const Transform& transform
 
             // Project those two points on the margins to have the closest points of both
             // object with the margins
-            double dist = sqrt(distSquare);
+            decimal dist = sqrt(distSquare);
             assert(dist > 0.0);
             pA = (pA - (OBJECT_MARGIN / dist) * v);
-            pB = shape2ToShape1.inverse() * (pB + (OBJECT_MARGIN / dist) * v);
+            pB = body2Tobody1.inverse() * (pB + (OBJECT_MARGIN / dist) * v);
 
             // Compute the contact info
             Vector3 normal = transform1.getOrientation().getMatrix() * (-v.getUnit());
-            double penetrationDepth = margin - dist;
+            decimal penetrationDepth = margin - dist;
 			
 			// Reject the contact if the penetration depth is negative (due too numerical errors)
 			if (penetrationDepth <= 0.0) return false;
@@ -206,14 +206,14 @@ bool GJKAlgorithm::testCollision(const Shape* shape1, const Transform& transform
 
             // Project those two points on the margins to have the closest points of both
             // object with the margins
-            double dist = sqrt(distSquare);
+            decimal dist = sqrt(distSquare);
             assert(dist > 0.0);
             pA = (pA - (OBJECT_MARGIN / dist) * v);
-            pB = shape2ToShape1.inverse() * (pB + (OBJECT_MARGIN / dist) * v);
+            pB = body2Tobody1.inverse() * (pB + (OBJECT_MARGIN / dist) * v);
 
             // Compute the contact info
             Vector3 normal = transform1.getOrientation().getMatrix() * (-v.getUnit());
-            double penetrationDepth = margin - dist;
+            decimal penetrationDepth = margin - dist;
 			
 			// Reject the contact if the penetration depth is negative (due too numerical errors)
 			if (penetrationDepth <= 0.0) return false;
@@ -229,7 +229,7 @@ bool GJKAlgorithm::testCollision(const Shape* shape1, const Transform& transform
     // enlarged objects to compute a simplex polytope that contains the origin. Then, we give that simplex
     // polytope to the EPA algorithm to compute the correct penetration depth and contact points between
     // the enlarged objects.
-    return computePenetrationDepthForEnlargedObjects(shape1, transform1, shape2, transform2, contactInfo, v);
+    return computePenetrationDepthForEnlargedObjects(collider1, transform1, collider2, transform2, contactInfo, v);
 }
 
 // This method runs the GJK algorithm on the two enlarged objects (with margin)
@@ -237,27 +237,27 @@ bool GJKAlgorithm::testCollision(const Shape* shape1, const Transform& transform
 // assumed to intersect in the original objects (without margin). Therefore such
 // a polytope must exist. Then, we give that polytope to the EPA algorithm to
 // compute the correct penetration depth and contact points of the enlarged objects.
-bool GJKAlgorithm::computePenetrationDepthForEnlargedObjects(const Shape* const shape1, const Transform& transform1,
+bool GJKAlgorithm::computePenetrationDepthForEnlargedObjects(const Collider* const collider1, const Transform& transform1,
-                                                             const Shape* const shape2, const Transform& transform2,
+                                                             const Collider* const collider2, const Transform& transform2,
                                                              ContactInfo*& contactInfo, Vector3& v) {
     Simplex simplex;
     Vector3 suppA;
     Vector3 suppB;
     Vector3 w;
-    double vDotw;
+    decimal vDotw;
-    double distSquare = DBL_MAX;
+    decimal distSquare = DECIMAL_MAX;
-    double prevDistSquare;
+    decimal prevDistSquare;
 
-    // Transform a point from body space of shape 2 to body space of shape 1 (the GJK algorithm is done in body space of shape 1)
+    // Transform a point from local space of body 2 to local space of body 1 (the GJK algorithm is done in local space of body 1)
-    Transform shape2ToShape1 = transform1.inverse() * transform2;
+    Transform body2ToBody1 = transform1.inverse() * transform2;
 
-    // Matrix that transform a direction from body space of shape 1 into body space of shape 2
+    // Matrix that transform a direction from local space of body 1 into local space of body 2
-    Matrix3x3 rotateToShape2 = transform2.getOrientation().getMatrix().getTranspose() * transform1.getOrientation().getMatrix();
+    Matrix3x3 rotateToBody2 = transform2.getOrientation().getMatrix().getTranspose() * transform1.getOrientation().getMatrix();
     
     do {
         // Compute the support points for the enlarged object A and B
-        suppA = shape1->getLocalSupportPoint(-v, OBJECT_MARGIN);
+        suppA = collider1->getLocalSupportPoint(-v, OBJECT_MARGIN);
-        suppB = shape2ToShape1 * shape2->getLocalSupportPoint(rotateToShape2 * v, OBJECT_MARGIN);
+        suppB = body2ToBody1 * collider2->getLocalSupportPoint(rotateToBody2 * v, OBJECT_MARGIN);
 
         // Compute the support point for the Minkowski difference A-B
         w = suppA - suppB;
@@ -293,5 +293,5 @@ bool GJKAlgorithm::computePenetrationDepthForEnlargedObjects(const Shape* const
 
     // Give the simplex computed with GJK algorithm to the EPA algorithm which will compute the correct
     // penetration depth and contact points between the two enlarged objects
-    return algoEPA.computePenetrationDepthAndContactPoints(simplex, shape1, transform1, shape2, transform2, v, contactInfo);
+    return algoEPA.computePenetrationDepthAndContactPoints(simplex, collider1, transform1, collider2, transform2, v, contactInfo);
 }

src/collision/narrowphase/GJK/GJKAlgorithm.h

@@ -29,7 +29,7 @@
 // Libraries
 #include "../NarrowPhaseAlgorithm.h"
 #include "../../ContactInfo.h"
-#include "../../../shapes/Shape.h"
+#include "../../../shapes/Collider.h"
 #include "../EPA/EPAAlgorithm.h"
 
 
@@ -37,8 +37,8 @@
 namespace reactphysics3d {
 
 // Constants
-const double REL_ERROR = 1.0e-3;
+const decimal REL_ERROR = 1.0e-3;
-const double REL_ERROR_SQUARE = REL_ERROR * REL_ERROR;
+const decimal REL_ERROR_SQUARE = REL_ERROR * REL_ERROR;
 
 /*  -------------------------------------------------------------------
     Class GJKAlgorithm :
@@ -61,16 +61,16 @@ class GJKAlgorithm : public NarrowPhaseAlgorithm {
     private :
         EPAAlgorithm algoEPA;             // EPA Algorithm
 
-        bool computePenetrationDepthForEnlargedObjects(const Shape* shape1, const Transform& transform1,
+        bool computePenetrationDepthForEnlargedObjects(const Collider* collider1, const Transform& transform1,
-                                                       const Shape* shape2, const Transform& transform2,
+                                                       const Collider* collider2, const Transform& transform2,
                                                        ContactInfo*& contactInfo, Vector3& v);             // Compute the penetration depth for enlarged objects
 
     public :
         GJKAlgorithm(CollisionDetection& collisionDetection);       // Constructor
         ~GJKAlgorithm();                                            // Destructor
 
-        virtual bool testCollision(const Shape* shape1, const Transform& transform1,
+        virtual bool testCollision(const Collider* collider1, const Transform& transform1,
-                                   const Shape* shape2,  const Transform& transform2,
+                                   const Collider* collider2,  const Transform& transform2,
                                    ContactInfo*& contactInfo);                      // Return true and compute a contact info if the two bounding volume collide
 };
 

src/collision/narrowphase/GJK/Simplex.cpp

@@ -222,7 +222,7 @@ bool Simplex::isProperSubset(Bits subset) const {
 // Return true if the set is affinely dependent meaning that a point of the set
 // is an affine combination of other points in the set
 bool Simplex::isAffinelyDependent() const {
-    double sum = 0.0;
+    decimal sum = 0.0;
     int i;
     Bits bit;
 
@@ -273,7 +273,7 @@ bool Simplex::isValidSubset(Bits subset) const {
 //      pB = sum(lambda_i * b_i)    where "b_i" are the support points of object B
 //      with lambda_i = deltaX_i / deltaX
 void Simplex::computeClosestPointsOfAandB(Vector3& pA, Vector3& pB) const {
-    double deltaX = 0.0;
+    decimal deltaX = 0.0;
     pA.setAllValues(0.0, 0.0, 0.0);
     pB.setAllValues(0.0, 0.0, 0.0);
     int i;
@@ -290,7 +290,7 @@ void Simplex::computeClosestPointsOfAandB(Vector3& pA, Vector3& pB) const {
     }
 
     assert(deltaX > 0.0);
-    double factor = 1.0 / deltaX;
+    decimal factor = 1.0 / deltaX;
     pA *= factor;
     pB *= factor;
 }
@@ -327,13 +327,13 @@ bool Simplex::computeClosestPoint(Vector3& v) {
 
 // Backup the closest point
 void Simplex::backupClosestPointInSimplex(Vector3& v) {
-    double minDistSquare = DBL_MAX;
+    decimal minDistSquare = DECIMAL_MAX;
     Bits bit;
 
     for (bit = allBits; bit != 0x0; bit--) {
         if (isSubset(bit, allBits) && isProperSubset(bit)) {
             Vector3 u = computeClosestPointForSubset(bit);
-            double distSquare = u.dot(u);
+            decimal distSquare = u.dot(u);
             if (distSquare < minDistSquare) {
                 minDistSquare = distSquare;
                 bitsCurrentSimplex = bit;
@@ -348,7 +348,7 @@ void Simplex::backupClosestPointInSimplex(Vector3& v) {
 Vector3 Simplex::computeClosestPointForSubset(Bits subset) {
     Vector3 v(0.0, 0.0, 0.0);      // Closet point v = sum(lambda_i * points[i])
     maxLengthSquare = 0.0;
-    double deltaX = 0.0;            // deltaX = sum of all det[subset][i]
+    decimal deltaX = 0.0;            // deltaX = sum of all det[subset][i]
     int i;
     Bits bit;
 

src/collision/narrowphase/GJK/Simplex.h

@@ -49,13 +49,13 @@ typedef unsigned int Bits;
 class Simplex {
     private:
         Vector3 points[4];                 // Current points
-        double pointsLengthSquare[4];       // pointsLengthSquare[i] = (points[i].length)^2
+        decimal pointsLengthSquare[4];       // pointsLengthSquare[i] = (points[i].length)^2
-        double maxLengthSquare;             // Maximum length of pointsLengthSquare[i]
+        decimal maxLengthSquare;             // Maximum length of pointsLengthSquare[i]
         Vector3 suppPointsA[4];            // Support points of object A in local coordinates
         Vector3 suppPointsB[4];            // Support points of object B in local coordinates
         Vector3 diffLength[4][4];          // diff[i][j] contains points[i] - points[j]
-        double det[16][4];                  // Cached determinant values
+        decimal det[16][4];                  // Cached determinant values
-        double normSquare[4][4];            // norm[i][j] = (diff[i][j].length())^2
+        decimal normSquare[4][4];            // norm[i][j] = (diff[i][j].length())^2
         Bits bitsCurrentSimplex;            // 4 bits that identify the current points of the simplex
                                             // For instance, 0101 means that points[1] and points[3] are in the simplex
         Bits lastFound;                     // Number between 1 and 4 that identify the last found support point
@@ -77,7 +77,7 @@ class Simplex {
         bool isFull() const;                                                                            // Return true if the simplex contains 4 points
         bool isEmpty() const;                                                                           // Return true if the simple is empty
         unsigned int getSimplex(Vector3* suppPointsA, Vector3* suppPointsB, Vector3* points) const;  // Return the points of the simplex
-        double getMaxLengthSquareOfAPoint() const;                                                      // Return the maximum squared length of a point
+        decimal getMaxLengthSquareOfAPoint() const;                                                      // Return the maximum squared length of a point
         void addPoint(const Vector3& point, const Vector3& suppPointA, const Vector3& suppPointB);   // Addd a point to the simplex
         bool isPointInSimplex(const Vector3& point) const;                                             // Return true if the point is in the simplex
         bool isAffinelyDependent() const;                                                               // Return true if the set is affinely dependent
@@ -107,7 +107,7 @@ inline bool Simplex::isEmpty() const {
 }
 
 // Return the maximum squared length of a point
-inline double Simplex::getMaxLengthSquareOfAPoint() const {
+inline decimal Simplex::getMaxLengthSquareOfAPoint() const {
     return maxLengthSquare;
 }
 

src/collision/narrowphase/NarrowPhaseAlgorithm.h

@@ -50,8 +50,8 @@ class NarrowPhaseAlgorithm {
         NarrowPhaseAlgorithm(CollisionDetection& collisionDetection);   // Constructor
         virtual ~NarrowPhaseAlgorithm();                                // Destructor
 
-        virtual bool testCollision(const Shape* shape1, const Transform& transform1,
+        virtual bool testCollision(const Collider* collider1, const Transform& transform1,
-                                   const Shape* shape2, const Transform& transform2,
+                                   const Collider* collider2, const Transform& transform2,
                                    ContactInfo*& contactInfo)=0;  // Return true and compute a contact info if the two bounding volume collide
 };
 

src/configuration.h

@@ -26,6 +26,11 @@
 #ifndef CONFIGURATION_H
 #define	CONFIGURATION_H
 
+// Libraries
+#include <limits>
+#include <cfloat>
+#include "decimal.h"
+
 // Windows platform
 #if defined(WIN32) || defined(_WIN32) || defined(_WIN64) || defined(__WIN32__) || defined(__WINDOWS__)
     #define WINDOWS_OS
@@ -35,4 +40,38 @@
     #define LINUX_OS
 #endif
 
+// Type definitions
+typedef unsigned int uint;
+typedef long unsigned int luint;
+
+// Mathematical constants
+const reactphysics3d::decimal DECIMAL_MIN = std::numeric_limits<reactphysics3d::decimal>::min();                // Minimun decimal value
+const reactphysics3d::decimal DECIMAL_MAX = std::numeric_limits<reactphysics3d::decimal>::max();               // Maximum decimal value
+const reactphysics3d::decimal MACHINE_EPSILON = std::numeric_limits<reactphysics3d::decimal>::epsilon();        // Machine epsilon
+const reactphysics3d::decimal DECIMAL_INFINITY = std::numeric_limits<reactphysics3d::decimal>::infinity();      // Infinity
+const reactphysics3d::decimal PI = 3.14159265;                                                                  // Pi constant
+
+// Physics Engine constants
+const reactphysics3d::decimal DEFAULT_TIMESTEP = 1.0 / 60.0;             // Default internal constant timestep in seconds
+
+// GJK Algorithm parameters
+const reactphysics3d::decimal OBJECT_MARGIN = 0.04;                      // Object margin for collision detection in cm
+
+// Contact constants
+const reactphysics3d::decimal FRICTION_COEFFICIENT = 0.4;                // Friction coefficient
+const reactphysics3d::decimal PERSISTENT_CONTACT_DIST_THRESHOLD = 0.02;  // Distance threshold for two contact points for a valid persistent contact
+
+// Constraint solver constants
+const int NB_MAX_BODIES = 100000;             // Maximum number of bodies
+const int NB_MAX_CONTACTS = 100000;           // Maximum number of contacts (for memory pool allocation)
+const int NB_MAX_CONSTRAINTS = 100000;        // Maximum number of constraints
+const int NB_MAX_COLLISION_PAIRS = 10000;     // Maximum number of collision pairs of bodies (for memory pool allocation)
+
+// Constraint solver constants
+const uint DEFAULT_LCP_ITERATIONS = 15;                                            // Number of iterations when solving a LCP problem
+const uint DEFAULT_LCP_ITERATIONS_ERROR_CORRECTION = 5;                            // Number of iterations when solving a LCP problem for error correction
+const bool ERROR_CORRECTION_PROJECTION_ENABLED = true;                             // True if the error correction projection (first order world) is active in the constraint solver
+const reactphysics3d::decimal PENETRATION_DEPTH_THRESHOLD_ERROR_CORRECTION = 0.20; // Contacts with penetration depth (in meters) larger that this use error correction with projection
+
+
 #endif

src/constants.h

@@ -1,72 +0,0 @@
-/********************************************************************************
-* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/      *
-* Copyright (c) 2010-2012 Daniel Chappuis                                       *
-*********************************************************************************
-*                                                                               *
-* This software is provided 'as-is', without any express or implied warranty.   *
-* In no event will the authors be held liable for any damages arising from the  *
-* use of this software.                                                         *
-*                                                                               *
-* Permission is granted to anyone to use this software for any purpose,         *
-* including commercial applications, and to alter it and redistribute it        *
-* freely, subject to the following restrictions:                                *
-*                                                                               *
-* 1. The origin of this software must not be misrepresented; you must not claim *
-*    that you wrote the original software. If you use this software in a        *
-*    product, an acknowledgment in the product documentation would be           *
-*    appreciated but is not required.                                           *
-*                                                                               *
-* 2. Altered source versions must be plainly marked as such, and must not be    *
-*    misrepresented as being the original software.                             *
-*                                                                               *
-* 3. This notice may not be removed or altered from any source distribution.    *
-*                                                                               *
-********************************************************************************/
-
-#ifndef CONSTANTS_H
-#define	CONSTANTS_H
-
-// Libraries
-#include <limits>
-#include <cfloat>
-
-// Type definitions
-typedef unsigned int uint;
-typedef long unsigned int luint;
-
-// Mathematical constants
-const double EPSILON = 1.0e-10;                                             // Epsilon value
-const double MACHINE_EPSILON = DBL_EPSILON;                                 // Machine epsilon
-const double EPSILON_TEST = 0.00001;                                        // TODO : Try not to use this value
-const double ONE_MINUS_EPSILON_TEST = 0.99999;                              // TODO : Try not to use this value
-const double INFINITY_CONST = std::numeric_limits<double>::infinity();      // Infinity constant
-const double PI = 3.14159265;                                               // Pi constant
-
-// Physics Engine constants
-const double DEFAULT_TIMESTEP = 1.0 / 60.0;             // Default internal constant timestep in seconds
-
-// GJK Algorithm parameters
-const double OBJECT_MARGIN = 0.04;                      // Object margin for collision detection in cm
-
-// Contact constants
-const double FRICTION_COEFFICIENT = 0.4;                // Friction coefficient
-const double DEFAULT_PENETRATION_FACTOR = 5.0;          // Penetration factor (between 0 and 1) which specify the importance of the
-                                                        // penetration depth in order to calculate the correct impulse for the contact
-const double PERSISTENT_CONTACT_DIST_THRESHOLD = 0.02;  // Distance threshold for two contact points for a valid persistent contact
-
-const int NB_MAX_BODIES = 100000;                       // Maximum number of bodies
-const int NB_MAX_CONTACTS = 100000;                     // Maximum number of contacts (for memory pool allocation)
-const int NB_MAX_CONSTRAINTS = 100000;                  // Maximum number of constraints
-const int NB_MAX_COLLISION_PAIRS = 10000;               // Maximum number of collision pairs of bodies (for memory pool allocation)
-
-// Constraint solver constants
-const uint DEFAULT_LCP_ITERATIONS = 15;                 // Number of iterations when solving a LCP problem
-const double AV_COUNTER_LIMIT = 500;                    // Maximum number value of the avBodiesCounter or avConstraintsCounter
-const double AV_PERCENT_TO_FREE = 0.5;                  // We will free the memory if the current nb of bodies (or constraints) is
-                                                        // less than AV_PERCENT_TO_FREE * bodiesCapacity (or constraintsCapacity). This
-                                                        // is used to avoid to keep to much memory for a long time if the system doesn't
-                                                        // need that memory. This value is between 0.0 and 1.0
-
-
-#endif
-

src/constraint/Constraint.cpp

@@ -30,8 +30,8 @@
 using namespace reactphysics3d;
 
 // Constructor
-Constraint::Constraint(Body* const body1, Body* const body2, uint nbConstraints, bool active)
+Constraint::Constraint(Body* const body1, Body* const body2, uint nbConstraints, bool active, ConstraintType type)
-           :body1(body1), body2(body2), active(active), nbConstraints(nbConstraints) {
+           :body1(body1), body2(body2), active(active), nbConstraints(nbConstraints), type(type) {
     
     // Initialize the cached lambda values
     for (int i=0; i<nbConstraints; i++) {

src/constraint/Constraint.h

@@ -32,6 +32,9 @@
 
 // ReactPhysics3D namespace
 namespace reactphysics3d {
+    
+// Enumeration for the type of a constraint
+enum ConstraintType {CONTACT};
 
 /*  -------------------------------------------------------------------
     Class Constraint :
@@ -47,21 +50,24 @@ class Constraint {
         Body* const body2;                      // Pointer to the second body of the constraint
         bool active;                            // True if the constraint is active
         uint nbConstraints;                     // Number mathematical constraints associated with this Constraint
-        std::vector<double> cachedLambdas;      // Cached lambda values of each mathematical constraint for more precise initializaton of LCP solver
+        const ConstraintType type;              // Type of the constraint
+        std::vector<decimal> cachedLambdas;     // Cached lambda values of each mathematical constraint for more precise initializaton of LCP solver
 
     public :
-        Constraint(Body* const body1, Body* const body2, uint nbConstraints, bool active);  // Constructor                                                                                                   // Constructor
+        Constraint(Body* const body1, Body* const body2, uint nbConstraints,
-        virtual ~Constraint();                                                              // Destructor
+                   bool active, ConstraintType type);                           // Constructor                                                                                                   // Constructor
-        Body* const getBody1() const;                                                       // Return the reference to the body 1
+        virtual ~Constraint();                                                  // Destructor
-        Body* const getBody2() const;                                                       // Return the reference to the body 2                                                                        // Evaluate the constraint
+        Body* const getBody1() const;                                           // Return the reference to the body 1
-        bool isActive() const;                                                                                      // Return true if the constraint is active                                                             // Return the jacobian matrix of body 2
+        Body* const getBody2() const;                                           // Return the reference to the body 2                                                                        // Evaluate the constraint
-        virtual void computeJacobian(int noConstraint, double J_sp[NB_MAX_CONSTRAINTS][2*6]) const=0;               // Compute the jacobian matrix for all mathematical constraints
+        bool isActive() const;                                                  // Return true if the constraint is active                                                             // Return the jacobian matrix of body 2
-        virtual void computeLowerBound(int noConstraint, double lowerBounds[NB_MAX_CONSTRAINTS]) const=0;           // Compute the lowerbounds values for all the mathematical constraints
+        ConstraintType getType() const;                                         // Return the type of the constraint                 
-        virtual void computeUpperBound(int noConstraint, double upperBounds[NB_MAX_CONSTRAINTS]) const=0;           // Compute the upperbounds values for all the mathematical constraints
+        virtual void computeJacobian(int noConstraint, decimal J_sp[NB_MAX_CONSTRAINTS][2*6]) const=0;               // Compute the jacobian matrix for all mathematical constraints
-        virtual void computeErrorValue(int noConstraint, double errorValues[], double penetrationFactor) const=0;   // Compute the error values for all the mathematical constraints
+        virtual void computeLowerBound(int noConstraint, decimal lowerBounds[NB_MAX_CONSTRAINTS]) const=0;           // Compute the lowerbounds values for all the mathematical constraints
+        virtual void computeUpperBound(int noConstraint, decimal upperBounds[NB_MAX_CONSTRAINTS]) const=0;           // Compute the upperbounds values for all the mathematical constraints
+        virtual void computeErrorValue(int noConstraint, decimal errorValues[]) const=0;   // Compute the error values for all the mathematical constraints
         unsigned int getNbConstraints() const;                                                                      // Return the number of mathematical constraints                                                                                                         // Return the number of auxiliary constraints
-        double getCachedLambda(int index) const;                                                                    // Get one cached lambda value
+        decimal getCachedLambda(int index) const;                                                                    // Get one cached lambda value
-        void setCachedLambda(int index, double lambda);                                                             // Set on cached lambda value  
+        void setCachedLambda(int index, decimal lambda);                                                             // Set on cached lambda value  
 };
 
 // Return the reference to the body 1
@@ -79,19 +85,25 @@ inline bool Constraint::isActive() const {
     return active;
 }
 
+// Return the type of the constraint
+inline ConstraintType Constraint::getType() const {
+    return type;
+}                                                          
+
+
 // Return the number auxiliary constraints
 inline uint Constraint::getNbConstraints() const {
     return nbConstraints;
 }
 
 // Get one previous lambda value
-inline double Constraint::getCachedLambda(int index) const {
+inline decimal Constraint::getCachedLambda(int index) const {
     assert(index >= 0 && index < nbConstraints);
     return cachedLambdas[index];
 } 
 
 // Set on cached lambda value  
-inline void Constraint::setCachedLambda(int index, double lambda) {
+inline void Constraint::setCachedLambda(int index, decimal lambda) {
     assert(index >= 0 && index < nbConstraints);
     cachedLambdas[index] = lambda;
 }                                                   

src/constraint/Contact.cpp

@@ -31,7 +31,7 @@ using namespace std;
 
 // Constructor
 Contact::Contact(const ContactInfo* contactInfo)
-        : Constraint(contactInfo->body1, contactInfo->body2, 3, true), normal(contactInfo->normal), penetrationDepth(contactInfo->penetrationDepth),
+        : Constraint(contactInfo->body1, contactInfo->body2, 3, true, CONTACT), normal(contactInfo->normal), penetrationDepth(contactInfo->penetrationDepth),
           localPointOnBody1(contactInfo->localPoint1), localPointOnBody2(contactInfo->localPoint2),
           worldPointOnBody1(contactInfo->worldPoint1), worldPointOnBody2(contactInfo->worldPoint2) {
     assert(penetrationDepth > 0.0);
@@ -55,7 +55,7 @@ Contact::~Contact() {
 // fill in this matrix with all the jacobian matrix of the mathematical constraint
 // of the contact. The argument "noConstraint", is the row were the method have
 // to start to fill in the J_sp matrix.
-void Contact::computeJacobian(int noConstraint, double J_sp[NB_MAX_CONSTRAINTS][2*6]) const {
+void Contact::computeJacobian(int noConstraint, decimal J_sp[NB_MAX_CONSTRAINTS][2*6]) const {
     assert(body1);
     assert(body2);
 
@@ -128,7 +128,7 @@ void Contact::computeJacobian(int noConstraint, double J_sp[NB_MAX_CONSTRAINTS][
 // Compute the lowerbounds values for all the mathematical constraints. The
 // argument "lowerBounds" is the lowerbounds values vector of the constraint solver and
 // this methods has to fill in this vector starting from the row "noConstraint"
-void Contact::computeLowerBound(int noConstraint, double lowerBounds[NB_MAX_CONSTRAINTS]) const {
+void Contact::computeLowerBound(int noConstraint, decimal lowerBounds[NB_MAX_CONSTRAINTS]) const {
     assert(noConstraint >= 0 && noConstraint + nbConstraints <= NB_MAX_CONSTRAINTS);
 
     lowerBounds[noConstraint] = 0.0;                // Lower bound for the contact constraint
@@ -139,10 +139,10 @@ void Contact::computeLowerBound(int noConstraint, double lowerBounds[NB_MAX_CONS
 // Compute the upperbounds values for all the mathematical constraints. The
 // argument "upperBounds" is the upperbounds values vector of the constraint solver and
 // this methods has to fill in this vector starting from the row "noConstraint"
-void Contact::computeUpperBound(int noConstraint, double upperBounds[NB_MAX_CONSTRAINTS]) const {
+void Contact::computeUpperBound(int noConstraint, decimal upperBounds[NB_MAX_CONSTRAINTS]) const {
     assert(noConstraint >= 0 && noConstraint + nbConstraints <= NB_MAX_CONSTRAINTS);
 
-    upperBounds[noConstraint] = INFINITY_CONST;    // Upper bound for the contact constraint
+    upperBounds[noConstraint] = DECIMAL_INFINITY;    // Upper bound for the contact constraint
     upperBounds[noConstraint + 1] = mu_mc_g;       // Upper bound for the first friction constraint
     upperBounds[noConstraint + 2] = mu_mc_g;       // Upper bound for the second friction constraint
 }
@@ -150,11 +150,11 @@ void Contact::computeUpperBound(int noConstraint, double upperBounds[NB_MAX_CONS
 // Compute the error values for all the mathematical constraints. The argument
 // "errorValues" is the error values vector of the constraint solver and this
 // method has to fill in this vector starting from the row "noConstraint"
-void Contact::computeErrorValue(int noConstraint, double errorValues[], double penetrationFactor) const {
+void Contact::computeErrorValue(int noConstraint, decimal errorValues[]) const {
     assert(body1);
     assert(body2);
 
-	// TODO : Do we need this casting anymore
+	// TODO : Do we need this casting anymore ?
     RigidBody* rigidBody1 = dynamic_cast<RigidBody*>(body1);
     RigidBody* rigidBody2 = dynamic_cast<RigidBody*>(body2);
 
@@ -163,9 +163,9 @@ void Contact::computeErrorValue(int noConstraint, double errorValues[], double p
     // Compute the error value for the contact constraint
     Vector3 velocity1 = rigidBody1->getLinearVelocity();
     Vector3 velocity2 = rigidBody2->getLinearVelocity();
-    double restitutionCoeff = rigidBody1->getRestitution() * rigidBody2->getRestitution();
+    decimal restitutionCoeff = rigidBody1->getRestitution() * rigidBody2->getRestitution();
-    double errorValue = restitutionCoeff * (normal.dot(velocity1) - normal.dot(velocity2)) + penetrationFactor * penetrationDepth;
+    decimal errorValue = restitutionCoeff * (normal.dot(velocity1) - normal.dot(velocity2));
-
+    
     // Assign the error value to the vector of error values
     errorValues[noConstraint] = errorValue;    // Error value for contact constraint
     errorValues[noConstraint + 1] = 0.0;       // Error value for friction constraint

src/constraint/Contact.h

@@ -30,11 +30,10 @@
 #include "Constraint.h"
 #include "../collision/ContactInfo.h"
 #include "../body/RigidBody.h"
-#include "../constants.h"
+#include "../configuration.h"
 #include "../mathematics/mathematics.h"
 #include "../memory/MemoryPool.h"
 #include "../configuration.h"
-#include <new>
 
 #if defined(VISUAL_DEBUG)
 	#if defined(APPLE_OS)
@@ -65,13 +64,13 @@ namespace reactphysics3d {
 class Contact : public Constraint {
     protected :
         const Vector3 normal;                   // Normal vector of the contact (From body1 toward body2) in world space
-        double penetrationDepth;                // Penetration depth
+        decimal penetrationDepth;                // Penetration depth
         const Vector3 localPointOnBody1;        // Contact point on body 1 in local space of body 1
         const Vector3 localPointOnBody2;        // Contact point on body 2 in local space of body 2
         Vector3 worldPointOnBody1;              // Contact point on body 1 in world space
         Vector3 worldPointOnBody2;              // Contact point on body 2 in world space
         std::vector<Vector3> frictionVectors;   // Two orthogonal vectors that span the tangential friction plane
-        double mu_mc_g;
+        decimal mu_mc_g;
         
         void computeFrictionVectors();                  // Compute the two friction vectors that span the tangential friction plane
 
@@ -80,18 +79,18 @@ class Contact : public Constraint {
         virtual ~Contact();                             // Destructor
 
         Vector3 getNormal() const;                                                     // Return the normal vector of the contact
-        void setPenetrationDepth(double penetrationDepth);                             // Set the penetration depth of the contact
+        void setPenetrationDepth(decimal penetrationDepth);                             // Set the penetration depth of the contact
         Vector3 getLocalPointOnBody1() const;                                          // Return the contact local point on body 1
         Vector3 getLocalPointOnBody2() const;                                          // Return the contact local point on body 2
         Vector3 getWorldPointOnBody1() const;                                          // Return the contact world point on body 1
         Vector3 getWorldPointOnBody2() const;                                          // Return the contact world point on body 2
         void setWorldPointOnBody1(const Vector3& worldPoint);                          // Set the contact world point on body 1
         void setWorldPointOnBody2(const Vector3& worldPoint);                                                   // Set the contact world point on body 2
-        virtual void computeJacobian(int noConstraint, double J_SP[NB_MAX_CONSTRAINTS][2*6]) const;             // Compute the jacobian matrix for all mathematical constraints
+        virtual void computeJacobian(int noConstraint, decimal J_SP[NB_MAX_CONSTRAINTS][2*6]) const;             // Compute the jacobian matrix for all mathematical constraints
-        virtual void computeLowerBound(int noConstraint, double lowerBounds[NB_MAX_CONSTRAINTS]) const;         // Compute the lowerbounds values for all the mathematical constraints
+        virtual void computeLowerBound(int noConstraint, decimal lowerBounds[NB_MAX_CONSTRAINTS]) const;         // Compute the lowerbounds values for all the mathematical constraints
-        virtual void computeUpperBound(int noConstraint, double upperBounds[NB_MAX_CONSTRAINTS]) const;         // Compute the upperbounds values for all the mathematical constraints
+        virtual void computeUpperBound(int noConstraint, decimal upperBounds[NB_MAX_CONSTRAINTS]) const;         // Compute the upperbounds values for all the mathematical constraints
-        virtual void computeErrorValue(int noConstraint, double errorValues[], double penetrationFactor) const; // Compute the error values for all the mathematical constraints
+        virtual void computeErrorValue(int noConstraint, decimal errorValues[]) const; // Compute the error values for all the mathematical constraints
-        double getPenetrationDepth() const;                                                                     // Return the penetration depth
+        decimal getPenetrationDepth() const;                                                                     // Return the penetration depth
         #ifdef VISUAL_DEBUG
            void draw() const;                                                                                   // Draw the contact (for debugging)
         #endif
@@ -117,7 +116,7 @@ inline Vector3 Contact::getNormal() const {
 }
 
 // Set the penetration depth of the contact
-inline void Contact::setPenetrationDepth(double penetrationDepth) {
+inline void Contact::setPenetrationDepth(decimal penetrationDepth) {
     this->penetrationDepth = penetrationDepth;
 }
 
@@ -152,7 +151,7 @@ inline void Contact::setWorldPointOnBody2(const Vector3& worldPoint) {
 }
 
 // Return the penetration depth of the contact
-inline double Contact::getPenetrationDepth() const {
+inline decimal Contact::getPenetrationDepth() const {
     return penetrationDepth;
 }
 
@@ -162,7 +161,7 @@ inline void Contact::draw() const {
     glColor3f(1.0, 0.0, 0.0);
     glutSolidSphere(0.3, 20, 20);
 }
-#endif
+#endif 
 
 } // End of the ReactPhysics3D namespace
 

src/engine/ConstraintSolver.cpp

@@ -34,7 +34,8 @@ using namespace std;
 // Constructor
 ConstraintSolver::ConstraintSolver(PhysicsWorld* world)
                  :physicsWorld(world), nbConstraints(0), nbIterationsLCP(DEFAULT_LCP_ITERATIONS),
-                  penetrationFactor(DEFAULT_PENETRATION_FACTOR) {
+                  nbIterationsLCPErrorCorrection(DEFAULT_LCP_ITERATIONS_ERROR_CORRECTION),
+                  isErrorCorrectionActive(false) {
 
 }
 
@@ -46,8 +47,9 @@ ConstraintSolver::~ConstraintSolver() {
  // Initialize the constraint solver before each solving
 void ConstraintSolver::initialize() {
     Constraint* constraint;
-
+    
     nbConstraints = 0;
+    nbConstraintsError = 0;
 
     // For each constraint
     vector<Constraint*>::iterator it;
@@ -68,6 +70,11 @@ void ConstraintSolver::initialize() {
 
             // Update the size of the jacobian matrix
             nbConstraints += constraint->getNbConstraints();
+            
+            // Update the size of the jacobian matrix for error correction projection
+            if (constraint->getType() == CONTACT) {
+                nbConstraintsError++;
+            }
         }
     }
 
@@ -75,16 +82,19 @@ void ConstraintSolver::initialize() {
     nbBodies = bodyNumberMapping.size();
 
     assert(nbConstraints > 0 && nbConstraints <= NB_MAX_CONSTRAINTS);
+    assert(nbConstraintsError > 0 && nbConstraintsError <= NB_MAX_CONSTRAINTS);
     assert(nbBodies > 0 && nbBodies <= NB_MAX_BODIES);
 }
 
 // Fill in all the matrices needed to solve the LCP problem
 // Notice that all the active constraints should have been evaluated first
-void ConstraintSolver::fillInMatrices() {
+void ConstraintSolver::fillInMatrices(decimal dt) {
     Constraint* constraint;
+    decimal oneOverDt = 1.0 / dt;
 
     // For each active constraint
     int noConstraint = 0;
+    int noConstraintError = 0;
     
     for (int c=0; c<activeConstraints.size(); c++) {
         
@@ -104,13 +114,46 @@ void ConstraintSolver::fillInMatrices() {
         constraint->computeUpperBound(noConstraint, upperBounds);
 
         // Fill in the error vector
-        constraint->computeErrorValue(noConstraint, errorValues, penetrationFactor);
+        constraint->computeErrorValue(noConstraint, errorValues);
 
         // Get the cached lambda values of the constraint
         for (int i=0; i<constraint->getNbConstraints(); i++) {
             lambdaInit[noConstraint + i] = constraint->getCachedLambda(i);
         }
 
+        // If the constraint is a contact
+        if (constraint->getType() == CONTACT) {
+            Contact* contact = dynamic_cast<Contact*>(constraint);
+            decimal penetrationDepth = contact->getPenetrationDepth();
+
+            // If error correction with projection is active
+            if (isErrorCorrectionActive) {
+                
+                // Fill in the error correction projection parameters
+                lowerBoundsError[noConstraintError] = lowerBounds[noConstraint];
+                upperBoundsError[noConstraintError] = upperBounds[noConstraint];
+                for (int i=0; i<12; i++) {
+                  J_spError[noConstraintError][i] = J_sp[noConstraint][i];
+                }
+                bodyMappingError[noConstraintError][0] = constraint->getBody1();
+                bodyMappingError[noConstraintError][1] = constraint->getBody2();
+                penetrationDepths[noConstraintError] = contact->getPenetrationDepth();
+                
+                // If the penetration depth is small
+                if (penetrationDepth < PENETRATION_DEPTH_THRESHOLD_ERROR_CORRECTION) {
+                    // Use the Baumgarte error correction term for contacts instead of
+                    // first order world projection
+                    errorValues[noConstraint] += 0.1 * oneOverDt * contact->getPenetrationDepth();
+                }
+
+                noConstraintError++;
+            }
+            else {  // If error correction with projection is not active
+                // Add the Baumgarte error correction term for contacts
+                errorValues[noConstraint] += 0.1 * oneOverDt * contact->getPenetrationDepth();
+            }
+        }
+        
         noConstraint += constraint->getNbConstraints();
     }
 
@@ -129,21 +172,31 @@ void ConstraintSolver::fillInMatrices() {
         // Compute the vector V1 with initial velocities values
         Vector3 linearVelocity = rigidBody->getLinearVelocity();
         Vector3 angularVelocity = rigidBody->getAngularVelocity();
-		int bodyIndexArray = 6 * bodyNumber;
+        int bodyIndexArray = 6 * bodyNumber;
-		V1[bodyIndexArray] = linearVelocity[0];
+        V1[bodyIndexArray] = linearVelocity[0];
-		V1[bodyIndexArray + 1] = linearVelocity[1];
+        V1[bodyIndexArray + 1] = linearVelocity[1];
-		V1[bodyIndexArray + 2] = linearVelocity[2];
+        V1[bodyIndexArray + 2] = linearVelocity[2];
-		V1[bodyIndexArray + 3] = angularVelocity[0];
+        V1[bodyIndexArray + 3] = angularVelocity[0];
-		V1[bodyIndexArray + 4] = angularVelocity[1];
+        V1[bodyIndexArray + 4] = angularVelocity[1];
-		V1[bodyIndexArray + 5] = angularVelocity[2];
+        V1[bodyIndexArray + 5] = angularVelocity[2];
 
         // Compute the vector Vconstraint with final constraint velocities
         Vconstraint[bodyIndexArray] = 0.0;
-		Vconstraint[bodyIndexArray + 1] = 0.0;
+        Vconstraint[bodyIndexArray + 1] = 0.0;
-		Vconstraint[bodyIndexArray + 2] = 0.0;
+        Vconstraint[bodyIndexArray + 2] = 0.0;
-		Vconstraint[bodyIndexArray + 3] = 0.0;
+        Vconstraint[bodyIndexArray + 3] = 0.0;
-		Vconstraint[bodyIndexArray + 4] = 0.0;
+        Vconstraint[bodyIndexArray + 4] = 0.0;
-		Vconstraint[bodyIndexArray + 5] = 0.0;
+        Vconstraint[bodyIndexArray + 5] = 0.0;
+        
+        // Compute the vector Vconstraint with final constraint velocities
+        if (isErrorCorrectionActive) {
+            VconstraintError[bodyIndexArray] = 0.0;
+            VconstraintError[bodyIndexArray + 1] = 0.0;
+            VconstraintError[bodyIndexArray + 2] = 0.0;
+            VconstraintError[bodyIndexArray + 3] = 0.0;
+            VconstraintError[bodyIndexArray + 4] = 0.0;
+            VconstraintError[bodyIndexArray + 5] = 0.0;
+        }
         
         // Compute the vector with forces and torques values
         Vector3 externalForce = rigidBody->getExternalForce();
@@ -155,63 +208,78 @@ void ConstraintSolver::fillInMatrices() {
         Fext[bodyIndexArray + 4] = externalTorque[1];
         Fext[bodyIndexArray + 5] = externalTorque[2];
 		
-		// Initialize the mass and inertia tensor matrices
+        // Initialize the mass and inertia tensor matrices
         Minv_sp_inertia[bodyNumber].setAllValues(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0);
-		Minv_sp_mass_diag[bodyNumber] = 0.0;
+        Minv_sp_mass_diag[bodyNumber] = 0.0;
-		
+
-		// If the motion of the rigid body is enabled
+        // If the motion of the rigid body is enabled
-		if (rigidBody->getIsMotionEnabled()) {
+        if (rigidBody->getIsMotionEnabled()) {
-			Minv_sp_inertia[bodyNumber] = rigidBody->getInertiaTensorInverseWorld();
+            Minv_sp_inertia[bodyNumber] = rigidBody->getInertiaTensorInverseWorld();
-			Minv_sp_mass_diag[bodyNumber] = rigidBody->getMassInverse();
+            Minv_sp_mass_diag[bodyNumber] = rigidBody->getMassInverse();
         }
-		
     }
 }
 
 // Compute the vector b
-void ConstraintSolver::computeVectorB(double dt) {
+void ConstraintSolver::computeVectorB(decimal dt) {
     uint indexBody1, indexBody2;
-    double oneOverDT = 1.0 / dt;
+    decimal oneOverDT = 1.0 / dt;
-    
-    //b = errorValues * oneOverDT;
 
     for (uint c = 0; c<nbConstraints; c++) {
-		b[c] = errorValues[c] * oneOverDT;
+        
+        // b = errorValues * oneOverDT;
+        b[c] = errorValues[c] * oneOverDT;
 		
         // Substract 1.0/dt*J*V to the vector b
         indexBody1 = bodyNumberMapping[bodyMapping[c][0]];
         indexBody2 = bodyNumberMapping[bodyMapping[c][1]];
-		double multiplication = 0.0;
+        decimal multiplication = 0.0;
-		int body1ArrayIndex = 6 * indexBody1;
+        int body1ArrayIndex = 6 * indexBody1;
-		int body2ArrayIndex = 6 * indexBody2;
+        int body2ArrayIndex = 6 * indexBody2;
-		for (uint i=0; i<6; i++) {
+        for (uint i=0; i<6; i++) {
-			multiplication += J_sp[c][i] * V1[body1ArrayIndex + i];
+                multiplication += J_sp[c][i] * V1[body1ArrayIndex + i];
-			multiplication += J_sp[c][6 + i] * V1[body2ArrayIndex + i];
+                multiplication += J_sp[c][6 + i] * V1[body2ArrayIndex + i];
-		}
+        }
-		b[c] -= multiplication * oneOverDT ;
+        b[c] -= multiplication * oneOverDT ;
 
         // Substract J*M^-1*F_ext to the vector b
-		double value1 = 0.0;
+        decimal value1 = 0.0;
-		double value2 = 0.0;
+        decimal value2 = 0.0;
-		double sum1, sum2;
+        decimal sum1, sum2;
-		value1 += J_sp[c][0] * Minv_sp_mass_diag[indexBody1] * Fext[body1ArrayIndex] +
+        value1 += J_sp[c][0] * Minv_sp_mass_diag[indexBody1] * Fext[body1ArrayIndex] +
-				  J_sp[c][1] * Minv_sp_mass_diag[indexBody1] * Fext[body1ArrayIndex + 1] +
+                          J_sp[c][1] * Minv_sp_mass_diag[indexBody1] * Fext[body1ArrayIndex + 1] +
-				  J_sp[c][2] * Minv_sp_mass_diag[indexBody1] * Fext[body1ArrayIndex + 2];
+                          J_sp[c][2] * Minv_sp_mass_diag[indexBody1] * Fext[body1ArrayIndex + 2];
-		value2 += J_sp[c][6] * Minv_sp_mass_diag[indexBody2] * Fext[body2ArrayIndex] +
+        value2 += J_sp[c][6] * Minv_sp_mass_diag[indexBody2] * Fext[body2ArrayIndex] +
-				   J_sp[c][7] * Minv_sp_mass_diag[indexBody2] * Fext[body2ArrayIndex + 1] +
+                           J_sp[c][7] * Minv_sp_mass_diag[indexBody2] * Fext[body2ArrayIndex + 1] +
-				   J_sp[c][8] * Minv_sp_mass_diag[indexBody2] * Fext[body2ArrayIndex + 2];
+                           J_sp[c][8] * Minv_sp_mass_diag[indexBody2] * Fext[body2ArrayIndex + 2];
-		for (uint i=0; i<3; i++) {
+        for (uint i=0; i<3; i++) {
-			sum1 = 0.0;
+                sum1 = 0.0;
-			sum2 = 0.0;
+                sum2 = 0.0;
-			for (uint j=0; j<3; j++) {
+                for (uint j=0; j<3; j++) {
-				sum1 += J_sp[c][3 + j] * Minv_sp_inertia[indexBody1].getValue(j, i);
+                        sum1 += J_sp[c][3 + j] * Minv_sp_inertia[indexBody1].getValue(j, i);
-				sum2 += J_sp[c][9 + j] * Minv_sp_inertia[indexBody2].getValue(j, i);
+                        sum2 += J_sp[c][9 + j] * Minv_sp_inertia[indexBody2].getValue(j, i);
-			}
+                }
-			value1 += sum1 * Fext[body1ArrayIndex + 3 + i];
+                value1 += sum1 * Fext[body1ArrayIndex + 3 + i];
-			value2 += sum2 * Fext[body2ArrayIndex + 3 + i];
+                value2 += sum2 * Fext[body2ArrayIndex + 3 + i];
-		}
+        }
-		
+
-		b[c] -= value1 + value2;	
+        b[c] -= value1 + value2;	
+    }
+}
+
+// Compute the vector b for error correction projection
+void ConstraintSolver::computeVectorBError(decimal dt) {
+    decimal oneOverDT = 1.0 / dt;
+
+    for (uint c = 0; c<nbConstraintsError; c++) {
+        
+        // b = errorValues * oneOverDT;
+        if (penetrationDepths[c] >= PENETRATION_DEPTH_THRESHOLD_ERROR_CORRECTION) {
+            bError[c] = penetrationDepths[c] * oneOverDT;
+        }
+        else {
+            bError[c] = 0.0;
+        }
     }
 }
 
@@ -222,24 +290,54 @@ void ConstraintSolver::computeMatrixB_sp() {
     // For each constraint
     for (uint c = 0; c<nbConstraints; c++) {
 
-		indexConstraintArray = 6 * c;
+        indexConstraintArray = 6 * c;
-		indexBody1 = bodyNumberMapping[bodyMapping[c][0]];
+        indexBody1 = bodyNumberMapping[bodyMapping[c][0]];
         indexBody2 = bodyNumberMapping[bodyMapping[c][1]];
-		B_sp[0][indexConstraintArray] = Minv_sp_mass_diag[indexBody1] * J_sp[c][0];
+        B_sp[0][indexConstraintArray] = Minv_sp_mass_diag[indexBody1] * J_sp[c][0];
-		B_sp[0][indexConstraintArray + 1] = Minv_sp_mass_diag[indexBody1] * J_sp[c][1];
+        B_sp[0][indexConstraintArray + 1] = Minv_sp_mass_diag[indexBody1] * J_sp[c][1];
-		B_sp[0][indexConstraintArray + 2] = Minv_sp_mass_diag[indexBody1] * J_sp[c][2];
+        B_sp[0][indexConstraintArray + 2] = Minv_sp_mass_diag[indexBody1] * J_sp[c][2];
-		B_sp[1][indexConstraintArray] = Minv_sp_mass_diag[indexBody2] * J_sp[c][6];
+        B_sp[1][indexConstraintArray] = Minv_sp_mass_diag[indexBody2] * J_sp[c][6];
-		B_sp[1][indexConstraintArray + 1] = Minv_sp_mass_diag[indexBody2] * J_sp[c][7];
+        B_sp[1][indexConstraintArray + 1] = Minv_sp_mass_diag[indexBody2] * J_sp[c][7];
-		B_sp[1][indexConstraintArray + 2] = Minv_sp_mass_diag[indexBody2] * J_sp[c][8];
+        B_sp[1][indexConstraintArray + 2] = Minv_sp_mass_diag[indexBody2] * J_sp[c][8];
-		
+
-		for (uint i=0; i<3; i++) {
+        for (uint i=0; i<3; i++) {
-			B_sp[0][indexConstraintArray + 3 + i] = 0.0;
+            B_sp[0][indexConstraintArray + 3 + i] = 0.0;
-			B_sp[1][indexConstraintArray + 3 + i] = 0.0;
+            B_sp[1][indexConstraintArray + 3 + i] = 0.0;
-			for (uint j=0; j<3; j++) {
+            for (uint j=0; j<3; j++) {
-				B_sp[0][indexConstraintArray + 3 + i] += Minv_sp_inertia[indexBody1].getValue(i, j) * J_sp[c][3 + j];
+                    B_sp[0][indexConstraintArray + 3 + i] += Minv_sp_inertia[indexBody1].getValue(i, j) * J_sp[c][3 + j];
-				B_sp[1][indexConstraintArray + 3 + i] += Minv_sp_inertia[indexBody2].getValue(i, j) * J_sp[c][9 + j];
+                    B_sp[1][indexConstraintArray + 3 + i] += Minv_sp_inertia[indexBody2].getValue(i, j) * J_sp[c][9 + j];
-			}
+            }
-		}
+        }
+    }
+}
+
+// Same as B_sp matrix but for error correction projection and
+// therefore, only for contact constraints
+void ConstraintSolver::computeMatrixB_spErrorCorrection() {
+    uint indexConstraintArray, indexBody1, indexBody2;
+	
+    // For each contact constraint
+    for (uint c = 0; c<nbConstraintsError; c++) {
+
+        indexConstraintArray = 6 * c;
+        indexBody1 = bodyNumberMapping[bodyMappingError[c][0]];
+        indexBody2 = bodyNumberMapping[bodyMappingError[c][1]];
+        B_spError[0][indexConstraintArray] = Minv_sp_mass_diag[indexBody1] * J_spError[c][0];
+        B_spError[0][indexConstraintArray + 1] = Minv_sp_mass_diag[indexBody1] * J_spError[c][1];
+        B_spError[0][indexConstraintArray + 2] = Minv_sp_mass_diag[indexBody1] * J_spError[c][2];
+        B_spError[1][indexConstraintArray] = Minv_sp_mass_diag[indexBody2] * J_spError[c][6];
+        B_spError[1][indexConstraintArray + 1] = Minv_sp_mass_diag[indexBody2] * J_spError[c][7];
+        B_spError[1][indexConstraintArray + 2] = Minv_sp_mass_diag[indexBody2] * J_spError[c][8];
+
+        for (uint i=0; i<3; i++) {
+            B_spError[0][indexConstraintArray + 3 + i] = 0.0;
+            B_spError[1][indexConstraintArray + 3 + i] = 0.0;
+
+            for (uint j=0; j<3; j++) {
+                B_spError[0][indexConstraintArray + 3 + i] += Minv_sp_inertia[indexBody1].getValue(i, j) * J_spError[c][3 + j];
+                B_spError[1][indexConstraintArray + 3 + i] += Minv_sp_inertia[indexBody2].getValue(i, j) * J_spError[c][9 + j];
+            }            
+        }
     }
 }
 
@@ -249,8 +347,8 @@ void ConstraintSolver::computeMatrixB_sp() {
 // Note that we use the vector V to store both V1 and V_constraint.
 // Note that M^-1 * J^T = B.
 // This method is called after that the LCP solver has computed lambda
-void ConstraintSolver::computeVectorVconstraint(double dt) {
+void ConstraintSolver::computeVectorVconstraint(decimal dt) {
-    uint indexBody1, indexBody2, indexBody1Array, indexBody2Array, indexConstraintArray;
+    uint indexBody1Array, indexBody2Array, indexConstraintArray;
 	uint j;
 	
     // Compute dt * (M^-1 * J^T * lambda
@@ -265,18 +363,35 @@ void ConstraintSolver::computeVectorVconstraint(double dt) {
     }
 }
 
+// Same as computeVectorVconstraint() but for error correction projection
+void ConstraintSolver::computeVectorVconstraintError(decimal dt) {
+    uint indexBody1Array, indexBody2Array, indexConstraintArray;
+	uint j;
+	
+    // Compute M^-1 * J^T * lambda
+    for (uint i=0; i<nbConstraintsError; i++) {
+        indexBody1Array = 6 * bodyNumberMapping[bodyMappingError[i][0]];
+        indexBody2Array = 6 * bodyNumberMapping[bodyMappingError[i][1]];
+		indexConstraintArray = 6 * i;
+		for (j=0; j<6; j++) {
+			VconstraintError[indexBody1Array + j] += B_spError[0][indexConstraintArray + j] * lambdaError[i];
+			VconstraintError[indexBody2Array + j] += B_spError[1][indexConstraintArray + j] * lambdaError[i];
+		}
+    }
+}
+
 
 // Solve a LCP problem using the Projected-Gauss-Seidel algorithm
 // This method outputs the result in the lambda vector
 void ConstraintSolver::solveLCP() {
 
-	for (uint i=0; i<nbConstraints; i++) {
+    for (uint i=0; i<nbConstraints; i++) {
-		lambda[i] = lambdaInit[i];
+            lambda[i] = lambdaInit[i];
-	}
+    }
 
     uint indexBody1Array, indexBody2Array;
-    double deltaLambda;
+    decimal deltaLambda;
-    double lambdaTemp;
+    decimal lambdaTemp;
     uint i, iter;
 
     // Compute the vector a
@@ -285,30 +400,78 @@ void ConstraintSolver::solveLCP() {
 
     // For each constraint
     for (i=0; i<nbConstraints; i++) {
-		uint indexConstraintArray = 6 * i;
+        uint indexConstraintArray = 6 * i;
-		d[i] = 0.0;
+        d[i] = 0.0;
-		for (uint j=0; j<6; j++) {
+        for (uint j=0; j<6; j++) {
-			d[i] += J_sp[i][j] * B_sp[0][indexConstraintArray + j] + J_sp[i][6 + j] * B_sp[1][indexConstraintArray + j];
+                d[i] += J_sp[i][j] * B_sp[0][indexConstraintArray + j] + J_sp[i][6 + j] * B_sp[1][indexConstraintArray + j];
-		}
+        }
     }
 
     for(iter=0; iter<nbIterationsLCP; iter++) {
         for (i=0; i<nbConstraints; i++) {
             indexBody1Array = 6 * bodyNumberMapping[bodyMapping[i][0]];
             indexBody2Array = 6 * bodyNumberMapping[bodyMapping[i][1]];
-			uint indexConstraintArray = 6 * i;
+            uint indexConstraintArray = 6 * i;
-			deltaLambda = b[i];
+            deltaLambda = b[i];
-			for (uint j=0; j<6; j++) {
+            for (uint j=0; j<6; j++) {
-				deltaLambda -= (J_sp[i][j] * a[indexBody1Array + j] + J_sp[i][6 + j] * a[indexBody2Array + j]);
+                    deltaLambda -= (J_sp[i][j] * a[indexBody1Array + j] + J_sp[i][6 + j] * a[indexBody2Array + j]);
-			}
+            }
-			deltaLambda /= d[i];
+            deltaLambda /= d[i];
             lambdaTemp = lambda[i];
             lambda[i] = std::max(lowerBounds[i], std::min(lambda[i] + deltaLambda, upperBounds[i]));
             deltaLambda = lambda[i] - lambdaTemp;
-			for (uint j=0; j<6; j++) {
+            for (uint j=0; j<6; j++) {
-				a[indexBody1Array + j] += B_sp[0][indexConstraintArray + j] * deltaLambda;
+                    a[indexBody1Array + j] += B_sp[0][indexConstraintArray + j] * deltaLambda;
-				a[indexBody2Array + j] += B_sp[1][indexConstraintArray + j] * deltaLambda;
+                    a[indexBody2Array + j] += B_sp[1][indexConstraintArray + j] * deltaLambda;
-			}
+            }
+        }
+    }
+}
+
+// Solve a LCP problem for error correction projection
+// using the Projected-Gauss-Seidel algorithm
+// This method outputs the result in the lambda vector
+void ConstraintSolver::solveLCPErrorCorrection() {
+
+    for (uint i=0; i<nbConstraintsError; i++) {
+        lambdaError[i] = 0;
+    }
+
+    uint indexBody1Array, indexBody2Array;
+    decimal deltaLambda;
+    decimal lambdaTemp;
+    uint i, iter;
+
+    // Compute the vector a
+    computeVectorAError();
+
+
+    // For each constraint
+    for (i=0; i<nbConstraintsError; i++) {
+        uint indexConstraintArray = 6 * i;
+        d[i] = 0.0;
+        for (uint j=0; j<6; j++) {
+           d[i] += J_spError[i][j] * B_spError[0][indexConstraintArray + j] + J_spError[i][6 + j] * B_spError[1][indexConstraintArray + j];
+        }
+    }
+
+    for(iter=0; iter<nbIterationsLCPErrorCorrection; iter++) {
+        for (i=0; i<nbConstraintsError; i++) {
+            indexBody1Array = 6 * bodyNumberMapping[bodyMappingError[i][0]];
+            indexBody2Array = 6 * bodyNumberMapping[bodyMappingError[i][1]];
+            uint indexConstraintArray = 6 * i;
+            deltaLambda = bError[i];
+            for (uint j=0; j<6; j++) {
+                deltaLambda -= (J_spError[i][j] * aError[indexBody1Array + j] + J_spError[i][6 + j] * aError[indexBody2Array + j]);
+            }
+            deltaLambda /= d[i];
+            lambdaTemp = lambdaError[i];
+            lambdaError[i] = std::max(lowerBoundsError[i], std::min(lambdaError[i] + deltaLambda, upperBoundsError[i]));
+            deltaLambda = lambdaError[i] - lambdaTemp;
+            for (uint j=0; j<6; j++) {
+                    aError[indexBody1Array + j] += B_spError[0][indexConstraintArray + j] * deltaLambda;
+                    aError[indexBody2Array + j] += B_spError[1][indexConstraintArray + j] * deltaLambda;
+            }
         }
     }
 }
@@ -335,6 +498,27 @@ void ConstraintSolver::computeVectorA() {
     }
 }
 
+// Same as computeVectorA() but for error correction projection
+void ConstraintSolver::computeVectorAError() {
+    uint i;
+    uint indexBody1Array, indexBody2Array;
+    
+    // Init the vector a with zero values
+    for (i=0; i<6*nbBodies; i++) {
+       aError[i] = 0.0;
+    }
+
+    for(i=0; i<nbConstraintsError; i++) {
+        indexBody1Array = 6 * bodyNumberMapping[bodyMappingError[i][0]];
+        indexBody2Array = 6 * bodyNumberMapping[bodyMappingError[i][1]];
+		uint indexConstraintArray = 6 * i;
+		for (uint j=0; j<6; j++) {
+			aError[indexBody1Array + j] += B_spError[0][indexConstraintArray + j] * lambdaError[i];
+			aError[indexBody2Array + j] += B_spError[1][indexConstraintArray + j] * lambdaError[i];
+		}
+    }
+}
+
 
 // Cache the lambda values in order to reuse them in the next step
 // to initialize the lambda vector

src/engine/ConstraintSolver.h

@@ -27,7 +27,7 @@
 #define CONSTRAINT_SOLVER_H
 
 // Libraries
-#include "../constants.h"
+#include "../configuration.h"
 #include "../constraint/Constraint.h"
 #include "PhysicsWorld.h"
 #include <map>
@@ -44,63 +44,99 @@ namespace reactphysics3d {
         Temporal Coherence" from Erin Catto. We keep the same notations as
         in the paper. The idea is to construct a LCP problem and then solve
         it using a Projected Gauss Seidel (PGS) solver.
+ 
+        The idea is to solve the following system for lambda :
+                JM^-1J^T * lamdba - 1/dt * b_error + 1/dt * JV^1 + JM^-1F_ext >= 0
+        
+        By default, error correction using first world order projections (described
+        by Erleben in section 4.16 in his PhD thesis "Stable, Robust, and
+        Versatile Multibody Dynamics Animation") is used for very large penetration
+        depths. Error correction with projection requires to solve another LCP problem
+        that is simpler than the above one and by considering only the contact
+        constraints. The LCP problem for error correction is the following one :
+                J_contact M^-1 J_contact^T * lambda + 1/dt * -d >= 0
+ 
+        where "d" is a vector with penetration depths. If the penetration depth of
+       a given contact is not very large, we use the Baumgarte error correction (see
+       the paper from Erin Catto).
+        
     -------------------------------------------------------------------
 */
 class ConstraintSolver {
     private:
         PhysicsWorld* physicsWorld;                     // Reference to the physics world
         std::vector<Constraint*> activeConstraints;     // Current active constraints in the physics world
+        bool isErrorCorrectionActive;                   // True if error correction (with world order) is active
         uint nbIterationsLCP;                           // Number of iterations of the LCP solver
+        uint nbIterationsLCPErrorCorrection;            // Number of iterations of the LCP solver for error correction
         uint nbConstraints;                             // Total number of constraints (with the auxiliary constraints)
+        uint nbConstraintsError;                        // Number of constraints for error correction projection (only contact constraints)
         uint nbBodies;                                  // Current number of bodies in the physics world
-        double penetrationFactor;                       // Penetration factor "beta" for penetration correction
         std::set<Body*> constraintBodies;               // Bodies that are implied in some constraint
         std::map<Body*, uint> bodyNumberMapping;        // Map a body pointer with its index number
         Body* bodyMapping[NB_MAX_CONSTRAINTS][2];       // 2-dimensional array that contains the mapping of body reference
                                                         // in the J_sp and B_sp matrices. For instance the cell bodyMapping[i][j] contains
                                                         // the pointer to the body that correspond to the 1x6 J_ij matrix in the
                                                         // J_sp matrix. An integer body index refers to its index in the "bodies" std::vector
-        double J_sp[NB_MAX_CONSTRAINTS][2*6];           // 2-dimensional array that correspond to the sparse representation of the jacobian matrix of all constraints
+        Body* bodyMappingError[NB_MAX_CONSTRAINTS][2];  // Same as bodyMapping but for error correction projection
+        decimal J_sp[NB_MAX_CONSTRAINTS][2*6];          // 2-dimensional array that correspond to the sparse representation of the jacobian matrix of all constraints
                                                         // This array contains for each constraint two 1x6 Jacobian matrices (one for each body of the constraint)
                                                         // a 1x6 matrix
-        double B_sp[2][6*NB_MAX_CONSTRAINTS];           // 2-dimensional array that correspond to a useful matrix in sparse representation
+        decimal B_sp[2][6*NB_MAX_CONSTRAINTS];          // 2-dimensional array that correspond to a useful matrix in sparse representation
                                                         // This array contains for each constraint two 6x1 matrices (one for each body of the constraint)
                                                         // a 6x1 matrix
-        double b[NB_MAX_CONSTRAINTS];                   // Vector "b" of the LCP problem
+        decimal J_spError[NB_MAX_CONSTRAINTS][2*6];     // Same as J_sp for error correction projection (only contact constraints)
-        double d[NB_MAX_CONSTRAINTS];                   // Vector "d"
+        decimal B_spError[2][6*NB_MAX_CONSTRAINTS];     // Same as B_sp for error correction projection (only contact constraints)
-        double a[6*NB_MAX_BODIES];                      // Vector "a"
+        decimal b[NB_MAX_CONSTRAINTS];                  // Vector "b" of the LCP problem
-        double lambda[NB_MAX_CONSTRAINTS];              // Lambda vector of the LCP problem
+        decimal bError[NB_MAX_CONSTRAINTS];             // Vector "b" of the LCP problem for error correction projection
-        double lambdaInit[NB_MAX_CONSTRAINTS];          // Lambda init vector for the LCP solver
+        decimal d[NB_MAX_CONSTRAINTS];                  // Vector "d"
-        double errorValues[NB_MAX_CONSTRAINTS];         // Error vector of all constraints
+        decimal a[6*NB_MAX_BODIES];                     // Vector "a"
-        double lowerBounds[NB_MAX_CONSTRAINTS];         // Vector that contains the low limits for the variables of the LCP problem
+        decimal aError[6*NB_MAX_BODIES];                // Vector "a" for error correction projection
-        double upperBounds[NB_MAX_CONSTRAINTS];         // Vector that contains the high limits for the variables of the LCP problem
+        decimal penetrationDepths[NB_MAX_CONSTRAINTS];  // Array of penetration depths for error correction projection
+        decimal lambda[NB_MAX_CONSTRAINTS];             // Lambda vector of the LCP problem
+        decimal lambdaError[NB_MAX_CONSTRAINTS];        // Lambda vector of the LCP problem for error correction projections
+        decimal lambdaInit[NB_MAX_CONSTRAINTS];         // Lambda init vector for the LCP solver
+        decimal errorValues[NB_MAX_CONSTRAINTS];        // Error vector of all constraints
+        decimal lowerBounds[NB_MAX_CONSTRAINTS];        // Vector that contains the low limits for the variables of the LCP problem
+        decimal upperBounds[NB_MAX_CONSTRAINTS];        // Vector that contains the high limits for the variables of the LCP problem
+        decimal lowerBoundsError[NB_MAX_CONSTRAINTS];   // Same as "lowerBounds" but for error correction projections
+        decimal upperBoundsError[NB_MAX_CONSTRAINTS];   // Same as "upperBounds" but for error correction projections
         Matrix3x3 Minv_sp_inertia[NB_MAX_BODIES];       // 3x3 world inertia tensor matrix I for each body (from the Minv_sp matrix)
-        double Minv_sp_mass_diag[NB_MAX_BODIES];        // Array that contains for each body the inverse of its mass
+        decimal Minv_sp_mass_diag[NB_MAX_BODIES];       // Array that contains for each body the inverse of its mass
                                                         // This is an array of size nbBodies that contains in each cell a 6x6 matrix
-        double V1[6*NB_MAX_BODIES];                     // Array that contains for each body the 6x1 vector that contains linear and angular velocities
+        decimal V1[6*NB_MAX_BODIES];                    // Array that contains for each body the 6x1 vector that contains linear and angular velocities
                                                         // Each cell contains a 6x1 vector with linear and angular velocities
-        double Vconstraint[6*NB_MAX_BODIES];            // Same kind of vector as V1 but contains the final constraint velocities
+        decimal Vconstraint[6*NB_MAX_BODIES];           // Same kind of vector as V1 but contains the final constraint velocities
-        double Fext[6*NB_MAX_BODIES];                   // Array that contains for each body the 6x1 vector that contains external forces and torques
+        decimal VconstraintError[6*NB_MAX_BODIES];      // Same kind of vector as V1 but contains the final constraint velocities
+        decimal Fext[6*NB_MAX_BODIES];                  // Array that contains for each body the 6x1 vector that contains external forces and torques
                                                         // Each cell contains a 6x1 vector with external force and torque.
         void initialize();                              // Initialize the constraint solver before each solving
-        void fillInMatrices();                          // Fill in all the matrices needed to solve the LCP problem
+        void fillInMatrices(decimal dt);                // Fill in all the matrices needed to solve the LCP problem
-        void computeVectorB(double dt);                 // Compute the vector b
+        void computeVectorB(decimal dt);                // Compute the vector b
+        void computeVectorBError(decimal dt);           // Compute the vector b for error correction projection
         void computeMatrixB_sp();                       // Compute the matrix B_sp
-        void computeVectorVconstraint(double dt);       // Compute the vector V2
+        void computeMatrixB_spErrorCorrection();        // Compute the matrix B_spError for error correction projection
+        void computeVectorVconstraint(decimal dt);      // Compute the vector V2
+        void computeVectorVconstraintError(decimal dt); // Same as computeVectorVconstraint() but for error correction projection
         void cacheLambda();                             // Cache the lambda values in order to reuse them in the next step to initialize the lambda vector
         void computeVectorA();                          // Compute the vector a used in the solve() method
+        void computeVectorAError();                     // Same as computeVectorA() but for error correction projection
         void solveLCP();                                // Solve a LCP problem using Projected-Gauss-Seidel algorithm
-        
+        void solveLCPErrorCorrection();                 // Solve the LCP problem for error correction projection
-    public:
+    
-        ConstraintSolver(PhysicsWorld* world);                      // Constructor
+   public:
-        virtual ~ConstraintSolver();                                // Destructor
+        ConstraintSolver(PhysicsWorld* world);                         // Constructor
-        void solve(double dt);                                      // Solve the current LCP problem
+        virtual ~ConstraintSolver();                                   // Destructor
-        bool isConstrainedBody(Body* body) const;                   // Return true if the body is in at least one constraint
+        void solve(decimal dt);                                         // Solve the current LCP problem
-        Vector3 getConstrainedLinearVelocityOfBody(Body* body);     // Return the constrained linear velocity of a body after solving the LCP problem
+        bool isConstrainedBody(Body* body) const;                      // Return true if the body is in at least one constraint
-        Vector3 getConstrainedAngularVelocityOfBody(Body* body);    // Return the constrained angular velocity of a body after solving the LCP problem
+        Vector3 getConstrainedLinearVelocityOfBody(Body* body);        // Return the constrained linear velocity of a body after solving the LCP problem
-        void cleanup();                                             // Cleanup of the constraint solver
+        Vector3 getConstrainedAngularVelocityOfBody(Body* body);       // Return the constrained angular velocity of a body after solving the LCP problem
-        void setPenetrationFactor(double penetrationFactor);        // Set the penetration factor 
+        Vector3 getErrorConstrainedLinearVelocityOfBody(Body* body);   // Return the constrained linear velocity of a body after solving the LCP problem for error correction
-        void setNbLCPIterations(uint nbIterations);                 // Set the number of iterations of the LCP solver
+        Vector3 getErrorConstrainedAngularVelocityOfBody(Body* body);  // Return the constrained angular velocity of a body after solving the LCP problem for error correction
+        void cleanup();                                                 // Cleanup of the constraint solver
+        void setPenetrationFactor(decimal penetrationFactor);           // Set the penetration factor 
+        void setNbLCPIterations(uint nbIterations);                     // Set the number of iterations of the LCP solver
+        void setIsErrorCorrectionActive(bool isErrorCorrectionActive);  // Set the isErrorCorrectionActive value
 };
 
 // Return true if the body is in at least one constraint
@@ -118,7 +154,6 @@ inline Vector3 ConstraintSolver::getConstrainedLinearVelocityOfBody(Body* body)
     return Vector3(Vconstraint[indexBodyArray], Vconstraint[indexBodyArray + 1], Vconstraint[indexBodyArray + 2]);
 }
 
-
 // Return the constrained angular velocity of a body after solving the LCP problem
 inline Vector3 ConstraintSolver::getConstrainedAngularVelocityOfBody(Body* body) {
     assert(isConstrainedBody(body));
@@ -126,6 +161,20 @@ inline Vector3 ConstraintSolver::getConstrainedAngularVelocityOfBody(Body* body)
     return Vector3(Vconstraint[indexBodyArray + 3], Vconstraint[indexBodyArray + 4], Vconstraint[indexBodyArray + 5]);
 }
 
+// Return the constrained linear velocity of a body after solving the LCP problem for error correction
+inline Vector3 ConstraintSolver::getErrorConstrainedLinearVelocityOfBody(Body* body) {
+    //assert(isConstrainedBody(body));
+    uint indexBodyArray = 6 * bodyNumberMapping[body];
+    return Vector3(VconstraintError[indexBodyArray], VconstraintError[indexBodyArray + 1], VconstraintError[indexBodyArray + 2]);
+}  
+
+// Return the constrained angular velocity of a body after solving the LCP problem for error correction
+inline Vector3 ConstraintSolver::getErrorConstrainedAngularVelocityOfBody(Body* body) {
+    //assert(isConstrainedBody(body));
+    uint indexBodyArray = 6 * bodyNumberMapping[body];
+    return Vector3(VconstraintError[indexBodyArray + 3], VconstraintError[indexBodyArray + 4], VconstraintError[indexBodyArray + 5]);
+} 
+
 // Cleanup of the constraint solver
 inline void ConstraintSolver::cleanup() {
     bodyNumberMapping.clear();
@@ -133,39 +182,53 @@ inline void ConstraintSolver::cleanup() {
     activeConstraints.clear();
 }
 
-// Set the penetration factor 
-inline void ConstraintSolver::setPenetrationFactor(double factor) {
-    penetrationFactor = factor;
-}   
-
 // Set the number of iterations of the LCP solver
 inline void ConstraintSolver::setNbLCPIterations(uint nbIterations) {
     nbIterationsLCP = nbIterations;
-}                 
+}         
+
+
+// Set the isErrorCorrectionActive value
+inline void ConstraintSolver::setIsErrorCorrectionActive(bool isErrorCorrectionActive) {
+    this->isErrorCorrectionActive = isErrorCorrectionActive;
+}
+
 
 // Solve the current LCP problem
-inline void ConstraintSolver::solve(double dt) {
+inline void ConstraintSolver::solve(decimal dt) {
     
-    // Allocate memory for the matrices
+    // Initialize the solver
     initialize();
 
     // Fill-in all the matrices needed to solve the LCP problem
-    fillInMatrices();
+    fillInMatrices(dt);
 
     // Compute the vector b
     computeVectorB(dt);
+    if (isErrorCorrectionActive) {
+        computeVectorBError(dt);
+    }
 
     // Compute the matrix B
     computeMatrixB_sp();
+    if (isErrorCorrectionActive) {
+        computeMatrixB_spErrorCorrection();
+    }
 
     // Solve the LCP problem (computation of lambda)
     solveLCP();
+    if (isErrorCorrectionActive) {
+        solveLCPErrorCorrection();
+    }
 
     // Cache the lambda values in order to use them in the next step
     cacheLambda();
     
     // Compute the vector Vconstraint
     computeVectorVconstraint(dt);
+    if (isErrorCorrectionActive) {
+        computeVectorVconstraintError(dt);
+    }
 }
 
 } // End of ReactPhysics3D namespace

src/engine/PhysicsEngine.cpp

@@ -31,7 +31,7 @@ using namespace reactphysics3d;
 using namespace std;
 
 // Constructor
-PhysicsEngine::PhysicsEngine(PhysicsWorld* world, double timeStep = DEFAULT_TIMESTEP)
+PhysicsEngine::PhysicsEngine(PhysicsWorld* world, decimal timeStep = DEFAULT_TIMESTEP)
               : world(world), timer(timeStep), collisionDetection(world), constraintSolver(world) {
     assert(world);
     assert(timeStep > 0.0);
@@ -57,7 +57,6 @@ void PhysicsEngine::update() {
     // While the time accumulator is not empty
     while(timer.isPossibleToTakeStep()) {
         existCollision = false;
-
 		
         // Compute the collision detection
         if (collisionDetection.computeCollisionDetection()) {
@@ -96,10 +95,12 @@ void PhysicsEngine::update() {
 // This method uses the semi-implicit Euler method to update the position and
 // orientation of the body
 void PhysicsEngine::updateAllBodiesMotion() {
-    double dt = timer.getTimeStep();
+    decimal dt = timer.getTimeStep();
     Vector3 newLinearVelocity;
     Vector3 newAngularVelocity;
-
+    Vector3 linearVelocityErrorCorrection;
+    Vector3 angularVelocityErrorCorrection;
+    
     // For each body of thephysics world
     for (vector<RigidBody*>::iterator it=world->getRigidBodiesBeginIterator(); it != world->getRigidBodiesEndIterator(); ++it) {
 
@@ -110,12 +111,17 @@ void PhysicsEngine::updateAllBodiesMotion() {
         if (rigidBody->getIsMotionEnabled()) {
             newLinearVelocity.setAllValues(0.0, 0.0, 0.0);
             newAngularVelocity.setAllValues(0.0, 0.0, 0.0);
+            linearVelocityErrorCorrection.setAllValues(0.0, 0.0, 0.0);
+            angularVelocityErrorCorrection.setAllValues(0.0, 0.0, 0.0);
 
             // If it's a constrained body
             if (constraintSolver.isConstrainedBody(*it)) {
                 // Get the constrained linear and angular velocities from the constraint solver
                 newLinearVelocity = constraintSolver.getConstrainedLinearVelocityOfBody(*it);
                 newAngularVelocity = constraintSolver.getConstrainedAngularVelocityOfBody(*it);
+                
+                linearVelocityErrorCorrection = constraintSolver.getErrorConstrainedLinearVelocityOfBody(rigidBody);
+                angularVelocityErrorCorrection = constraintSolver.getErrorConstrainedAngularVelocityOfBody(rigidBody);
             }
 
             // Compute V_forces = dt * (M^-1 * F_ext) which is the velocity of the body due to the
@@ -126,9 +132,10 @@ void PhysicsEngine::updateAllBodiesMotion() {
             // Add the velocity V1 to the new velocity
             newLinearVelocity += rigidBody->getLinearVelocity();
             newAngularVelocity += rigidBody->getAngularVelocity();
-
+            
             // Update the position and the orientation of the body according to the new velocity
-            updatePositionAndOrientationOfBody(*it, newLinearVelocity, newAngularVelocity);
+            updatePositionAndOrientationOfBody(*it, newLinearVelocity, newAngularVelocity,
+                                               linearVelocityErrorCorrection, angularVelocityErrorCorrection);
 
             // Update the AABB of the rigid body
             rigidBody->updateAABB();
@@ -139,8 +146,9 @@ void PhysicsEngine::updateAllBodiesMotion() {
 // Update the position and orientation of a body
 // Use the Semi-Implicit Euler (Sympletic Euler) method to compute the new position and the new
 // orientation of the body
-void PhysicsEngine::updatePositionAndOrientationOfBody(RigidBody* rigidBody, const Vector3& newLinVelocity, const Vector3& newAngVelocity) {
+void PhysicsEngine::updatePositionAndOrientationOfBody(RigidBody* rigidBody, const Vector3& newLinVelocity, const Vector3& newAngVelocity,
-    double dt = timer.getTimeStep();
+                                                       const Vector3& linearVelocityErrorCorrection, const Vector3& angularVelocityErrorCorrection) {
+    decimal dt = timer.getTimeStep();
 
     assert(rigidBody);
 
@@ -150,21 +158,27 @@ void PhysicsEngine::updatePositionAndOrientationOfBody(RigidBody* rigidBody, con
     // Update the linear and angular velocity of the body
     rigidBody->setLinearVelocity(newLinVelocity);
     rigidBody->setAngularVelocity(newAngVelocity);
-
+    
     // Get current position and orientation of the body
     const Vector3& currentPosition = rigidBody->getTransform().getPosition();
     const Quaternion& currentOrientation = rigidBody->getTransform().getOrientation();
+        
+    // Error correction projection
+    Vector3 correctedPosition = currentPosition + dt * linearVelocityErrorCorrection;
+    Quaternion correctedOrientation = currentOrientation + Quaternion(angularVelocityErrorCorrection.getX(), angularVelocityErrorCorrection.getY(), angularVelocityErrorCorrection.getZ(), 0) * currentOrientation * 0.5 * dt;
+    
+    Vector3 newPosition = correctedPosition + newLinVelocity * dt;
+    Quaternion newOrientation = correctedOrientation + Quaternion(newAngVelocity.getX(), newAngVelocity.getY(), newAngVelocity.getZ(), 0) * correctedOrientation * 0.5 * dt;
 
-    Vector3 newPosition = currentPosition + newLinVelocity * dt;
-    Quaternion newOrientation = currentOrientation + Quaternion(newAngVelocity.getX(), newAngVelocity.getY(), newAngVelocity.getZ(), 0) * currentOrientation * 0.5 * dt;
     Transform newTransform(newPosition, newOrientation.getUnit());
     rigidBody->setTransform(newTransform);
 }
 
 // Compute and set the interpolation factor to all bodies
 void PhysicsEngine::setInterpolationFactorToAllBodies() {
+    
     // Compute the interpolation factor
-    double factor = timer.computeInterpolationFactor();
+    decimal factor = timer.computeInterpolationFactor();
     assert(factor >= 0.0 && factor <= 1.0);
 
     // Set the factor to all bodies

src/engine/PhysicsEngine.h

@@ -32,7 +32,7 @@
 #include "ConstraintSolver.h"
 #include "../body/RigidBody.h"
 #include "Timer.h"
-#include "../constants.h"
+#include "../configuration.h"
 
 // Namespace ReactPhysics3D
 namespace reactphysics3d {
@@ -50,20 +50,23 @@ class PhysicsEngine {
         CollisionDetection collisionDetection;          // Collision detection
         ConstraintSolver constraintSolver;              // Constraint solver
 
-        void updateAllBodiesMotion();                                                                                           // Compute the motion of all bodies and update their positions and orientations
+        void updateAllBodiesMotion();                                                                                                            // Compute the motion of all bodies and update their positions and orientations
-        void updatePositionAndOrientationOfBody(RigidBody* body, const Vector3& newLinVelocity, const Vector3& newAngVelocity);    // Update the position and orientation of a body
+        void updatePositionAndOrientationOfBody(RigidBody* body, const Vector3& newLinVelocity, const Vector3& newAngVelocity,
-        void setInterpolationFactorToAllBodies();                                                                                // Compute and set the interpolation factor to all bodies
+                                                const Vector3& linearVelocityErrorCorrection, const Vector3& angularVelocityErrorCorrection);    // Update the position and orientation of a body
-        void applyGravity();                                                                                                    // Apply the gravity force to all bodies
+        void setInterpolationFactorToAllBodies();                                                                                                // Compute and set the interpolation factor to all bodies
+        void applyGravity();                                                                                                                     // Apply the gravity force to all bodies
 
 public :
-        PhysicsEngine(PhysicsWorld* world, double timeStep);    // Constructor
+        PhysicsEngine(PhysicsWorld* world, decimal timeStep);    // Constructor
         ~PhysicsEngine();                                       // Destructor
 
-        void start();                                           // Start the physics simulation
+        void start();                                                   // Start the physics simulation
-        void stop();                                            // Stop the physics simulation
+        void stop();                                                    // Stop the physics simulation
-        void update();                                          // Update the physics simulation
+        void update();                                                  // Update the physics simulation
-        void setPenetrationFactor(double factor);               // Set the penetration factor for the constraint solver
+        void setPenetrationFactor(decimal factor);                      // Set the penetration factor for the constraint solver
-        void setNbLCPIterations(uint nbIterations);             // Set the number of iterations of the LCP solver
+        void setNbLCPIterations(uint nbIterations);                     // Set the number of iterations of the LCP solver
+        void setIsErrorCorrectionActive(bool isErrorCorrectionActive);  // Set the isErrorCorrectionActive value
+
 };
 
 // --- Inline functions --- //
@@ -78,14 +81,20 @@ inline void PhysicsEngine::stop() {
 }      
 
 // Set the penetration factor for the constraint solver
-inline void PhysicsEngine::setPenetrationFactor(double factor) {
+inline void PhysicsEngine::setPenetrationFactor(decimal factor) {
     constraintSolver.setPenetrationFactor(factor);
 }               
 
 // Set the number of iterations of the LCP solver
 inline void PhysicsEngine::setNbLCPIterations(uint nbIterations) {
     constraintSolver.setNbLCPIterations(nbIterations);
-}             
+}   
+
+// Set the isErrorCorrectionActive value
+inline void PhysicsEngine::setIsErrorCorrectionActive(bool isErrorCorrectionActive) {
+    constraintSolver.setIsErrorCorrectionActive(isErrorCorrectionActive);
+}
+
 
 }
 

src/engine/PhysicsWorld.cpp

@@ -44,10 +44,10 @@ PhysicsWorld::~PhysicsWorld() {
 }
 
 // Create a rigid body into the physics world
-RigidBody* PhysicsWorld::createRigidBody(const Transform& transform, double mass, const Matrix3x3& inertiaTensorLocal, Shape* shape) {
+RigidBody* PhysicsWorld::createRigidBody(const Transform& transform, decimal mass, const Matrix3x3& inertiaTensorLocal, Collider* collider) {
     
     // Create the rigid body
-    RigidBody* rigidBody = new (memoryPoolRigidBodies.allocateObject()) RigidBody(transform, mass, inertiaTensorLocal, shape, currentBodyID);
+    RigidBody* rigidBody = new (memoryPoolRigidBodies.allocateObject()) RigidBody(transform, mass, inertiaTensorLocal, collider, currentBodyID);
     
     currentBodyID++;
     
@@ -60,11 +60,11 @@ RigidBody* PhysicsWorld::createRigidBody(const Transform& transform, double mass
 void PhysicsWorld::destroyRigidBody(RigidBody* rigidBody) {
     removeRigidBody(rigidBody);
 	
-	// Call the constructor of the rigid body
+    // Call the constructor of the rigid body
-	rigidBody->RigidBody::~RigidBody();
+    rigidBody->RigidBody::~RigidBody();
-	
+
-	// Free the object from the memory pool
+    // Free the object from the memory pool
-	memoryPoolRigidBodies.freeObject(rigidBody);
+    memoryPoolRigidBodies.freeObject(rigidBody);
 }  
 
 // Remove all collision contacts constraints

src/engine/PhysicsWorld.h

@@ -64,8 +64,8 @@ class PhysicsWorld {
         PhysicsWorld(const Vector3& gravity);      // Constructor
         virtual ~PhysicsWorld();                   // Destructor
 
-        RigidBody* createRigidBody(const Transform& transform, double mass,
+        RigidBody* createRigidBody(const Transform& transform, decimal mass,
-                                   const Matrix3x3& inertiaTensorLocal, Shape* shape);  // Create a rigid body into the physics world
+                                   const Matrix3x3& inertiaTensorLocal, Collider* collider);  // Create a rigid body into the physics world
         void destroyRigidBody(RigidBody* rigidBody);                                    // Destroy a rigid body
         void clearAddedAndRemovedBodies();                                              // Clear the addedBodies and removedBodies sets
         Vector3 getGravity() const;                                                     // Return the gravity vector of the world

src/mathematics/Matrix3x3.cpp

@@ -37,12 +37,12 @@ Matrix3x3::Matrix3x3() {
 }
 
 // Constructor
-Matrix3x3::Matrix3x3(double value) {
+Matrix3x3::Matrix3x3(decimal value) {
     setAllValues(value, value, value, value, value, value, value, value, value);
 }
 
 // Constructor with arguments
-Matrix3x3::Matrix3x3(double a1, double a2, double a3, double b1, double b2, double b3, double c1, double c2, double c3) {
+Matrix3x3::Matrix3x3(decimal a1, decimal a2, decimal a3, decimal b1, decimal b2, decimal b3, decimal c1, decimal c2, decimal c3) {
     // Initialize the matrix with the values
     setAllValues(a1, a2, a3, b1, b2, b3, c1, c2, c3);
 }
@@ -55,11 +55,11 @@ Matrix3x3::~Matrix3x3() {
 // Return the inverse matrix
 Matrix3x3 Matrix3x3::getInverse() const {
     // Compute the determinant of the matrix
-    double determinant = getDeterminant();
+    decimal determinant = getDeterminant();
 
     // Check if the determinant is equal to zero
     assert(determinant != 0.0);
-    double invDeterminant = 1.0 / determinant;
+    decimal invDeterminant = 1.0 / determinant;
     Matrix3x3 tempMatrix;
 
     // Compute the inverse of the matrix

src/mathematics/Matrix3x3.h

@@ -42,23 +42,23 @@ namespace reactphysics3d {
 */
 class Matrix3x3 {
     private :
-        double array[3][3];                                                                     // Array with the values of the matrix
+        decimal array[3][3];                                                                    // Array with the values of the matrix
 
     public :
         Matrix3x3();                                                                            // Constructor
-        Matrix3x3(double value);                                                                // Constructor
+        Matrix3x3(decimal value);                                                               // Constructor
-        Matrix3x3(double a1, double a2, double a3, double b1, double b2, double b3,
+        Matrix3x3(decimal a1, decimal a2, decimal a3, decimal b1, decimal b2, decimal b3,
-                  double c1, double c2, double c3);                                             // Constructor
+                  decimal c1, decimal c2, decimal c3);                                          // Constructor
         virtual ~Matrix3x3();                                                                   // Destructor
 
-        double getValue(int i, int j) const;                                                    // Get a value in the matrix
+        decimal getValue(int i, int j) const;                                                   // Get a value in the matrix
-        void setValue(int i, int j, double value);                                              // Set a value in the matrix
+        void setValue(int i, int j, decimal value);                                             // Set a value in the matrix
-        void setAllValues(double a1, double a2, double a3, double b1, double b2, double b3,
+        void setAllValues(decimal a1, decimal a2, decimal a3, decimal b1, decimal b2, decimal b3,
-                  double c1, double c2, double c3);                                             // Set all the values in the matrix
+                  decimal c1, decimal c2, decimal c3);                                          // Set all the values in the matrix
         Vector3 getColumn(int i) const;                                                         // Return a column
         Matrix3x3 getTranspose() const;                                                         // Return the transpose matrix
-        double getDeterminant() const;                                                          // Return the determinant of the matrix
+        decimal getDeterminant() const;                                                         // Return the determinant of the matrix
-        double getTrace() const;                                                                // Return the trace of the matrix
+        decimal getTrace() const;                                                               // Return the trace of the matrix
         Matrix3x3 getInverse() const;                                                           // Return the inverse matrix
         Matrix3x3 getAbsoluteMatrix() const;                                                    // Return the matrix with absolute values
         void setToIdentity();                                                                   // Set the matrix to the identity matrix
@@ -68,8 +68,8 @@ class Matrix3x3 {
         friend Matrix3x3 operator+(const Matrix3x3& matrix1, const Matrix3x3& matrix2);         // Overloaded operator for addition
         friend Matrix3x3 operator-(const Matrix3x3& matrix1, const Matrix3x3& matrix2);         // Overloaded operator for substraction
         friend Matrix3x3 operator-(const Matrix3x3& matrix);                                    // Overloaded operator for the negative of the matrix
-        friend Matrix3x3 operator*(double nb, const Matrix3x3& matrix);                         // Overloaded operator for multiplication with a number
+        friend Matrix3x3 operator*(decimal nb, const Matrix3x3& matrix);                        // Overloaded operator for multiplication with a number
-        friend Matrix3x3 operator*(const Matrix3x3& matrix, double nb);                         // Overloaded operator for multiplication with a matrix
+        friend Matrix3x3 operator*(const Matrix3x3& matrix, decimal nb);                        // Overloaded operator for multiplication with a matrix
         friend Matrix3x3 operator*(const Matrix3x3& matrix1, const Matrix3x3& matrix2);         // Overloaded operator for matrix multiplication
         friend Vector3 operator*(const Matrix3x3& matrix, const Vector3& vector);               // Overloaded operator for multiplication with a vector
 
@@ -77,25 +77,25 @@ class Matrix3x3 {
         bool operator!= (const Matrix3x3& matrix) const;                                        // Overloaded operator for the is different condition
         Matrix3x3& operator+=(const Matrix3x3& matrix);                                         // Overloaded operator for addition with assignment
         Matrix3x3& operator-=(const Matrix3x3& matrix);                                         // Overloaded operator for substraction with assignment
-        Matrix3x3& operator*=(double nb);                                                      // Overloaded operator for multiplication with a number with assignment
+        Matrix3x3& operator*=(decimal nb);                                                      // Overloaded operator for multiplication with a number with assignment
 };
 
 
 // Method to get a value in the matrix (inline)
-inline double Matrix3x3::getValue(int i, int j) const {
+inline decimal Matrix3x3::getValue(int i, int j) const {
     assert(i>=0 && i<3 && j>=0 && j<3);
     return array[i][j];
 }
 
 // Method to set a value in the matrix (inline)
-inline void Matrix3x3::setValue(int i, int j, double value) {
+inline void Matrix3x3::setValue(int i, int j, decimal value) {
     assert(i>=0 && i<3 && j>=0 && j<3);
     array[i][j] = value;
 }
 
 // Method to set all the values in the matrix
-inline void Matrix3x3::setAllValues(double a1, double a2, double a3, double b1, double b2, double b3,
+inline void Matrix3x3::setAllValues(decimal a1, decimal a2, decimal a3, decimal b1, decimal b2, decimal b3,
-                                    double c1, double c2, double c3) {
+                                    decimal c1, decimal c2, decimal c3) {
     array[0][0] = a1; array[0][1] = a2; array[0][2] = a3;
     array[1][0] = b1; array[1][1] = b2; array[1][2] = b3;
     array[2][0] = c1; array[2][1] = c2; array[2][2] = c3;
@@ -116,14 +116,14 @@ inline Matrix3x3 Matrix3x3::getTranspose() const {
 }
 
 // Return the determinant of the matrix
-inline double Matrix3x3::getDeterminant() const {
+inline decimal Matrix3x3::getDeterminant() const {
     // Compute and return the determinant of the matrix
     return (array[0][0]*(array[1][1]*array[2][2]-array[2][1]*array[1][2]) - array[0][1]*(array[1][0]*array[2][2]-array[2][0]*array[1][2]) +
             array[0][2]*(array[1][0]*array[2][1]-array[2][0]*array[1][1]));
 }
 
 // Return the trace of the matrix
-inline double Matrix3x3::getTrace() const {
+inline decimal Matrix3x3::getTrace() const {
     // Compute and return the trace
     return (array[0][0] + array[1][1] + array[2][2]);
 }
@@ -170,14 +170,14 @@ inline Matrix3x3 operator-(const Matrix3x3& matrix) {
 }
 
 // Overloaded operator for multiplication with a number
-inline Matrix3x3 operator*(double nb, const Matrix3x3& matrix) {
+inline Matrix3x3 operator*(decimal nb, const Matrix3x3& matrix) {
     return Matrix3x3(matrix.array[0][0] * nb, matrix.array[0][1] * nb, matrix.array[0][2] * nb,
                      matrix.array[1][0] * nb, matrix.array[1][1] * nb, matrix.array[1][2] * nb,
                      matrix.array[2][0] * nb, matrix.array[2][1] * nb, matrix.array[2][2] * nb);
 }
 
 // Overloaded operator for multiplication with a matrix
-inline Matrix3x3 operator*(const Matrix3x3& matrix, double nb) {
+inline Matrix3x3 operator*(const Matrix3x3& matrix, decimal nb) {
     return nb * matrix;
 }
 
@@ -230,7 +230,7 @@ inline Matrix3x3& Matrix3x3::operator-=(const Matrix3x3& matrix) {
 }
 
 // Overloaded operator for multiplication with a number with assignment
-inline Matrix3x3& Matrix3x3::operator*=(double nb) {
+inline Matrix3x3& Matrix3x3::operator*=(decimal nb) {
    array[0][0] *= nb; array[0][1] *= nb; array[0][2] *= nb;
    array[1][0] *= nb; array[1][1] *= nb; array[1][2] *= nb;
    array[2][0] *= nb; array[2][1] *= nb; array[2][2] *= nb;

src/mathematics/Quaternion.cpp

@@ -38,13 +38,13 @@ Quaternion::Quaternion()
 }
 
 // Constructor with arguments
-Quaternion::Quaternion(double x, double y, double z, double w)
+Quaternion::Quaternion(decimal x, decimal y, decimal z, decimal w)
            :x(x), y(y), z(z), w(w) {
 
 }
 
 // Constructor with the component w and the vector v=(x y z)
-Quaternion::Quaternion(double w, const Vector3& v)
+Quaternion::Quaternion(decimal w, const Vector3& v)
            :x(v.getX()), y(v.getY()), z(v.getZ()), w(w) {
 
 }
@@ -59,17 +59,17 @@ Quaternion::Quaternion(const Quaternion& quaternion)
 Quaternion::Quaternion(const Matrix3x3& matrix) {
 
     // Get the trace of the matrix
-    double trace = matrix.getTrace();
+    decimal trace = matrix.getTrace();
 
-    double array[3][3];
+    decimal array[3][3];
     for (int i=0; i<3; i++) {
         for (int j=0; j<3; j++) {
             array[i][j] = matrix.getValue(i, j);
         }
     }
 
-    double r;
+    decimal r;
-    double s;
+    decimal s;
 
     if (trace < 0.0) {
         if (array[1][1] > array[0][0]) {
@@ -135,7 +135,7 @@ Quaternion::~Quaternion() {
 // Compute the rotation angle (in radians) and the 3D rotation axis
 // This method is used to get the rotation angle (in radian) and the unit
 // rotation axis of an orientation quaternion.
-void Quaternion::getRotationAngleAxis(double& angle, Vector3& axis) const {
+void Quaternion::getRotationAngleAxis(decimal& angle, Vector3& axis) const {
     Quaternion quaternion;
 
     // If the quaternion is unit
@@ -163,26 +163,26 @@ void Quaternion::getRotationAngleAxis(double& angle, Vector3& axis) const {
 // Return the orientation matrix corresponding to this quaternion
 Matrix3x3 Quaternion::getMatrix() const {
 
-    double nQ = x*x + y*y + z*z + w*w;
+    decimal nQ = x*x + y*y + z*z + w*w;
-    double s = 0.0;
+    decimal s = 0.0;
 
     if (nQ > 0.0) {
         s = 2.0/nQ;
     }
 
     // Computations used for optimization (less multiplications)
-    double xs = x*s;
+    decimal xs = x*s;
-    double ys = y*s;
+    decimal ys = y*s;
-    double zs = z*s;
+    decimal zs = z*s;
-    double wxs = w*xs;
+    decimal wxs = w*xs;
-    double wys = w*ys;
+    decimal wys = w*ys;
-    double wzs = w*zs;
+    decimal wzs = w*zs;
-    double xxs = x*xs;
+    decimal xxs = x*xs;
-    double xys = x*ys;
+    decimal xys = x*ys;
-    double xzs = x*zs;
+    decimal xzs = x*zs;
-    double yys = y*ys;
+    decimal yys = y*ys;
-    double yzs = y*zs;
+    decimal yzs = y*zs;
-    double zzs = z*zs;
+    decimal zzs = z*zs;
 
     // Create the matrix corresponding to the quaternion
     return Matrix3x3(1.0-yys-zzs, xys-wzs, xzs + wys,
@@ -192,13 +192,13 @@ Matrix3x3 Quaternion::getMatrix() const {
 
 // Compute the spherical linear interpolation between two quaternions.
 // The t argument has to be such that 0 <= t <= 1. This method is static.
-Quaternion Quaternion::slerp(const Quaternion& quaternion1, const Quaternion& quaternion2, double t) {
+Quaternion Quaternion::slerp(const Quaternion& quaternion1, const Quaternion& quaternion2, decimal t) {
     assert(t >= 0.0 && t <= 1.0);
 
-    double invert = 1.0;
+    decimal invert = 1.0;
 
     // Compute cos(theta) using the quaternion scalar product
-    double cosineTheta = quaternion1.dot(quaternion2);
+    decimal cosineTheta = quaternion1.dot(quaternion2);
 
     // Take care of the sign of cosineTheta
     if (cosineTheta < 0.0) {
@@ -208,19 +208,20 @@ Quaternion Quaternion::slerp(const Quaternion& quaternion1, const Quaternion& qu
 
     // Because of precision, if cos(theta) is nearly 1, therefore theta is nearly 0 and we can write
     // sin((1-t)*theta) as (1-t) and sin(t*theta) as t
-    if(1-cosineTheta < EPSILON_TEST) {
+    const decimal epsilon = 0.00001;
+    if(1-cosineTheta < epsilon) {
         return quaternion1 * (1.0-t) + quaternion2 * (t * invert);
     }
 
     // Compute the theta angle
-    double theta = acos(cosineTheta);
+    decimal theta = acos(cosineTheta);
 
     // Compute sin(theta)
-    double sineTheta = sin(theta);
+    decimal sineTheta = sin(theta);
 
     // Compute the two coefficients that are in the spherical linear interpolation formula
-    double coeff1 = sin((1.0-t)*theta) / sineTheta;
+    decimal coeff1 = sin((1.0-t)*theta) / sineTheta;
-    double coeff2 = sin(t*theta) / sineTheta * invert;
+    decimal coeff2 = sin(t*theta) / sineTheta * invert;
 
     // Compute and return the interpolated quaternion
     return quaternion1 * coeff1 + quaternion2 * coeff2;

src/mathematics/Quaternion.h

@@ -30,6 +30,7 @@
 #include <cmath>
 #include "Vector3.h"
 #include "Matrix3x3.h"
+#include "../decimal.h"
 
 // ReactPhysics3D namespace
 namespace reactphysics3d {
@@ -42,86 +43,86 @@ namespace reactphysics3d {
 */
 class Quaternion {
     private :
-        double x;                       // Component x of the quaternion
+        decimal x;      // Component x of the quaternion
-        double y;                       // Component y of the quaternion
+        decimal y;      // Component y of the quaternion
-        double z;                       // Component z of the quaternion
+        decimal z;      // Component z of the quaternion
-        double w;                       // Component w of the quaternion
+        decimal w;      // Component w of the quaternion
 
     public :
-        Quaternion();                                                       // Constructor
+        Quaternion();                                                   // Constructor
-        Quaternion(double x, double y, double z, double w);                 // Constructor with arguments
+        Quaternion(decimal x, decimal y, decimal z, decimal w);         // Constructor with arguments
-        Quaternion(double w, const Vector3& v);                             // Constructor with the component w and the vector v=(x y z)
+        Quaternion(decimal w, const Vector3& v);                        // Constructor with the component w and the vector v=(x y z)
-        Quaternion(const Quaternion& quaternion);                           // Copy-constructor
+        Quaternion(const Quaternion& quaternion);                       // Copy-constructor
-        Quaternion(const Matrix3x3& matrix);                                // Create a unit quaternion from a rotation matrix
+        Quaternion(const Matrix3x3& matrix);                            // Create a unit quaternion from a rotation matrix
-        ~Quaternion();                                                      // Destructor
+        ~Quaternion();                                                  // Destructor
-        double getX() const;                                                // Return the component x of the quaternion
+        decimal getX() const;                                           // Return the component x of the quaternion
-        double getY() const;                                                // Return the component y of the quaternion
+        decimal getY() const;                                           // Return the component y of the quaternion
-        double getZ() const;                                                // Return the component z of the quaternion
+        decimal getZ() const;                                           // Return the component z of the quaternion
-        double getW() const;                                                // Return the component w of the quaternion
+        decimal getW() const;                                           // Return the component w of the quaternion
-        void setX(double x);                                                // Set the value x
+        void setX(decimal x);                                           // Set the value x
-        void setY(double y);                                                // Set the value y
+        void setY(decimal y);                                           // Set the value y
-        void setZ(double z);                                                // Set the value z
+        void setZ(decimal z);                                           // Set the value z
-        void setW(double w);                                                // Set the value w
+        void setW(decimal w);                                           // Set the value w
-        Vector3 vectorV() const;                                            // Return the vector v=(x y z) of the quaternion
+        Vector3 vectorV() const;                                        // Return the vector v=(x y z) of the quaternion
-        double length() const;                                              // Return the length of the quaternion
+        decimal length() const;                                         // Return the length of the quaternion
-        Quaternion getUnit() const;                                         // Return the unit quaternion
+        Quaternion getUnit() const;                                     // Return the unit quaternion
-        Quaternion getConjugate() const;                                    // Return the conjugate quaternion
+        Quaternion getConjugate() const;                                // Return the conjugate quaternion
-        Quaternion getInverse() const;                                      // Return the inverse of the quaternion
+        Quaternion getInverse() const;                                  // Return the inverse of the quaternion
-        Matrix3x3 getMatrix() const;                                        // Return the orientation matrix corresponding to this quaternion
+        Matrix3x3 getMatrix() const;                                    // Return the orientation matrix corresponding to this quaternion
-        static Quaternion identity();                                       // Return the identity quaternion
+        static Quaternion identity();                                   // Return the identity quaternion
-        double dot(const Quaternion& quaternion) const;                     // Dot product between two quaternions
+        decimal dot(const Quaternion& quaternion) const;                // Dot product between two quaternions
-        void getRotationAngleAxis(double& angle, Vector3& axis) const;      // Compute the rotation angle (in radians) and the axis
+        void getRotationAngleAxis(decimal& angle, Vector3& axis) const; // Compute the rotation angle (in radians) and the axis
         static Quaternion slerp(const Quaternion& quaternion1,
-                                const Quaternion& quaternion2, double t);   // Compute the spherical linear interpolation between two quaternions
+                                const Quaternion& quaternion2, decimal t);   // Compute the spherical linear interpolation between two quaternions
 
         // --- Overloaded operators --- //
-        Quaternion operator+(const Quaternion& quaternion) const;             // Overloaded operator for the addition
+        Quaternion operator+(const Quaternion& quaternion) const;       // Overloaded operator for the addition
-        Quaternion operator-(const Quaternion& quaternion) const;             // Overloaded operator for the substraction
+        Quaternion operator-(const Quaternion& quaternion) const;       // Overloaded operator for the substraction
-        Quaternion operator*(double nb) const;                                // Overloaded operator for the multiplication with a constant
+        Quaternion operator*(decimal nb) const;                         // Overloaded operator for the multiplication with a constant
-        Quaternion operator*(const Quaternion& quaternion) const;             // Overloaded operator for the multiplication
+        Quaternion operator*(const Quaternion& quaternion) const;       // Overloaded operator for the multiplication
-        Quaternion& operator=(const Quaternion& quaternion);                  // Overloaded operator for assignment
+        Quaternion& operator=(const Quaternion& quaternion);            // Overloaded operator for assignment
-        bool operator==(const Quaternion& quaternion) const;                  // Overloaded operator for equality condition
+        bool operator==(const Quaternion& quaternion) const;            // Overloaded operator for equality condition
 };
 
 // --- Inline functions --- //
 
 // Get the value x (inline)
-inline double Quaternion::getX() const {
+inline decimal Quaternion::getX() const {
     return x;
 }
 
 // Get the value y (inline)
-inline double Quaternion::getY() const {
+inline decimal Quaternion::getY() const {
     return y;
 }
 
 // Get the value z (inline)
-inline double Quaternion::getZ() const {
+inline decimal Quaternion::getZ() const {
     return z;
 }
 
 // Get the value w (inline)
-inline double Quaternion::getW() const {
+inline decimal Quaternion::getW() const {
     return w;
 }
 
 // Set the value x (inline)
-inline void Quaternion::setX(double x) {
+inline void Quaternion::setX(decimal x) {
     this->x = x;
 }
 
 // Set the value y (inline)
-inline void Quaternion::setY(double y) {
+inline void Quaternion::setY(decimal y) {
     this->y = y;
 }
 
 // Set the value z (inline)
-inline void Quaternion::setZ(double z) {
+inline void Quaternion::setZ(decimal z) {
     this->z = z;
 }
 
 // Set the value w (inline)
-inline void Quaternion::setW(double w) {
+inline void Quaternion::setW(decimal w) {
     this->w = w;
 }
 
@@ -132,13 +133,13 @@ inline Vector3 Quaternion::vectorV() const {
 }
 
 // Return the length of the quaternion (inline)
-inline double Quaternion::length() const {
+inline decimal Quaternion::length() const {
     return sqrt(x*x + y*y + z*z + w*w);
 }
 
 // Return the unit quaternion
 inline Quaternion Quaternion::getUnit() const {
-    double lengthQuaternion = length();
+    decimal lengthQuaternion = length();
 
     // Check if the length is not equal to zero
     assert (lengthQuaternion != 0.0);
@@ -159,7 +160,7 @@ inline Quaternion Quaternion::getConjugate() const {
 
 // Return the inverse of the quaternion (inline)
 inline Quaternion Quaternion::getInverse() const {
-    double lengthQuaternion = length();
+    decimal lengthQuaternion = length();
     lengthQuaternion = lengthQuaternion * lengthQuaternion;
 
     assert (lengthQuaternion != 0.0);
@@ -169,7 +170,7 @@ inline Quaternion Quaternion::getInverse() const {
 }
 
 // Scalar product between two quaternions
-inline double Quaternion::dot(const Quaternion& quaternion) const {
+inline decimal Quaternion::dot(const Quaternion& quaternion) const {
     return (x*quaternion.x + y*quaternion.y + z*quaternion.z + w*quaternion.w);
 }
 
@@ -186,7 +187,7 @@ inline Quaternion Quaternion::operator-(const Quaternion& quaternion) const {
 }
 
 // Overloaded operator for the multiplication with a constant
-inline Quaternion Quaternion::operator*(double nb) const {
+inline Quaternion Quaternion::operator*(decimal nb) const {
     // Return the result
     return Quaternion(nb*x, nb*y, nb*z, nb*w);
 }

src/mathematics/Transform.h

@@ -46,22 +46,22 @@ class Transform {
         Quaternion orientation;     // Orientation
 
     public :
-        Transform();                                                            // Constructor
+        Transform();                                                           // Constructor
         Transform(const Vector3& position, const Matrix3x3& orientation);      // Constructor
         Transform(const Vector3& position, const Quaternion& orientation);     // Constructor
-        ~Transform();                                                           // Destructor
+        ~Transform();                                                          // Destructor
 
-        const Vector3& getPosition() const;                                    // Return the origin of the transform
+        const Vector3& getPosition() const;                                     // Return the origin of the transform
-        void setPosition(const Vector3& position);                             // Set the origin of the transform
+        void setPosition(const Vector3& position);                              // Set the origin of the transform
         const Quaternion& getOrientation() const;                               // Return the orientation quaternion
         void setOrientation(const Quaternion& orientation);                     // Set the rotation quaternion
         void setToIdentity();                                                   // Set the transform to the identity transform
-        void setFromOpenGL(double* openglMatrix);                               // Set the transform from an OpenGL transform matrix
+        void setFromOpenGL(decimal* openglMatrix);                              // Set the transform from an OpenGL transform matrix
-        void getOpenGLMatrix(double* openglMatrix) const;                       // Get the OpenGL matrix of the transform
+        void getOpenGLMatrix(decimal* openglMatrix) const;                      // Get the OpenGL matrix of the transform
         Transform inverse() const;                                              // Return the inverse of the transform
         static Transform interpolateTransforms(const Transform& oldTransform,
                                                const Transform& newTransform,
-                                               double interpolationFactor);     // Return an interpolated transform
+                                               decimal interpolationFactor);    // Return an interpolated transform
 
         Vector3 operator*(const Vector3& vector) const;           // Return the transformed vector
         Transform operator*(const Transform& transform2) const;     // Operator of multiplication of a transform with another one
@@ -94,7 +94,7 @@ inline void Transform::setToIdentity() {
 }
 
 // Set the transform from an OpenGL transform matrix
-inline void Transform::setFromOpenGL(double* openglMatrix) {
+inline void Transform::setFromOpenGL(decimal* openglMatrix) {
     Matrix3x3 matrix(openglMatrix[0], openglMatrix[4], openglMatrix[8],
                      openglMatrix[1], openglMatrix[5], openglMatrix[9],
                      openglMatrix[2], openglMatrix[6], openglMatrix[10]);
@@ -103,7 +103,7 @@ inline void Transform::setFromOpenGL(double* openglMatrix) {
 }
 
 // Get the OpenGL matrix of the transform
-inline void Transform::getOpenGLMatrix(double* openglMatrix) const {
+inline void Transform::getOpenGLMatrix(decimal* openglMatrix) const {
     const Matrix3x3& matrix = orientation.getMatrix();
     openglMatrix[0] = matrix.getValue(0, 0); openglMatrix[1] = matrix.getValue(1, 0); openglMatrix[2] = matrix.getValue(2, 0); openglMatrix[3] = 0.0;
     openglMatrix[4] = matrix.getValue(0, 1); openglMatrix[5] = matrix.getValue(1, 1); openglMatrix[6] = matrix.getValue(2, 1); openglMatrix[7] = 0.0;
@@ -119,7 +119,7 @@ inline Transform Transform::inverse() const {
 }
 
 // Return an interpolated transform
-inline Transform Transform::interpolateTransforms(const Transform& oldTransform, const Transform& newTransform, double interpolationFactor) {
+inline Transform Transform::interpolateTransforms(const Transform& oldTransform, const Transform& newTransform, decimal interpolationFactor) {
     Vector3 interPosition = oldTransform.position * (1.0 - interpolationFactor) + newTransform.position * interpolationFactor;
     Quaternion interOrientation = Quaternion::slerp(oldTransform.orientation, newTransform.orientation, interpolationFactor);
     return Transform(interPosition, interOrientation);

src/mathematics/Vector3.cpp

@@ -40,7 +40,7 @@ Vector3::Vector3() {
 }
 
 // Constructor with arguments
-Vector3::Vector3(double x, double y, double z) {
+Vector3::Vector3(decimal x, decimal y, decimal z) {
     values[0] = x;
     values[1] = y;
     values[2] = z;
@@ -60,12 +60,12 @@ Vector3::~Vector3() {
 
 // Return the corresponding unit vector
 Vector3 Vector3::getUnit() const {
-    double lengthVector = length();
+    decimal lengthVector = length();
 
     assert(lengthVector != 0.0);
 
     // Compute and return the unit vector
-    double lengthInv = 1.0 / lengthVector;
+    decimal lengthInv = 1.0 / lengthVector;
     return Vector3(values[0] * lengthInv, values[1] * lengthInv, values[2] * lengthInv);
 }
 
@@ -91,6 +91,6 @@ Vector3 Vector3::getOneOrthogonalVector() const {
         vector1.setZ((-values[0]*values[2]-values[1]*vector1.getY())/values[2]);
     }
 
-    assert(vector1.isUnit());
+    //assert(vector1.isUnit());
     return vector1;
 }

src/mathematics/Vector3.h

@@ -29,6 +29,7 @@
 // Libraries
 #include <cmath>
 #include "mathematics_functions.h"
+#include "../decimal.h"
 
 
 // ReactPhysics3D namespace
@@ -36,32 +37,32 @@ namespace reactphysics3d {
 
 /*  -------------------------------------------------------------------
     Class Vector3 :
-        This classrepresents 3 dimensionnal vector in space.
+        This class represents 3D vector in space.
     -------------------------------------------------------------------
 */
 class Vector3 {
     private :
-        double values[3];                                       // Values of the 3D vector
+        decimal values[3];                                      // Values of the 3D vector
 
     public :
         Vector3();                                              // Constructor of the class Vector3D
-        Vector3(double x, double y, double z);                  // Constructor with arguments
+        Vector3(decimal x, decimal y, decimal z);               // Constructor with arguments
         Vector3(const Vector3& vector);                         // Copy-constructor
         virtual ~Vector3();                                     // Destructor
-        double getX() const;                                    // Get the x component of the vector
+        decimal getX() const;                                   // Get the x component of the vector
-        double getY() const;                                    // Get the y component of the vector
+        decimal getY() const;                                   // Get the y component of the vector
-        double getZ() const;                                    // Get the z component of the vector
+        decimal getZ() const;                                   // Get the z component of the vector
-        void setX(double x);                                    // Set the x component of the vector
+        void setX(decimal x);                                   // Set the x component of the vector
-        void setY(double y);                                    // Set the y component of the vector
+        void setY(decimal y);                                   // Set the y component of the vector
-        void setZ(double z);                                    // Set the z component of the vector
+        void setZ(decimal z);                                   // Set the z component of the vector
-        void setAllValues(double x, double y, double z);        // Set all the values of the vector
+        void setAllValues(decimal x, decimal y, decimal z);     // Set all the values of the vector
-        double length() const;                                  // Return the lenght of the vector
+        decimal length() const;                                 // Return the lenght of the vector
-        double lengthSquare() const;                            // Return the square of the length of the vector
+        decimal lengthSquare() const;                           // Return the square of the length of the vector
         Vector3 getUnit() const;                                // 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
         Vector3 getOneOrthogonalVector() const;                 // Return one unit orthogonal vectors of the current vector
-        double dot(const Vector3& vector) const;                // Dot product of two vectors
+        decimal dot(const Vector3& vector) const;               // Dot product of two vectors
         Vector3 cross(const Vector3& vector) const;             // Cross product of two vectors
         Vector3 getAbsoluteVector() const;                      // Return the corresponding absolute value vector
         int getMinAxis() const;                                 // Return the axis with the minimal value
@@ -73,68 +74,68 @@ class Vector3 {
         bool operator!= (const Vector3& vector) const;          // Overloaded operator for the is different condition
         Vector3& operator+=(const Vector3& vector);             // Overloaded operator for addition with assignment
         Vector3& operator-=(const Vector3& vector);             // Overloaded operator for substraction with assignment
-        Vector3& operator*=(double number);                     // Overloaded operator for multiplication with a number with assignment
+        Vector3& operator*=(decimal number);                    // Overloaded operator for multiplication with a number with assignment
-        double& operator[] (int index);                         // Overloaded operator for value access
+        decimal& operator[] (int index);                        // Overloaded operator for value access
-        const double& operator[] (int index) const;             // Overloaded operator for value access
+        const decimal& operator[] (int index) const;            // Overloaded operator for value access
         
         // Friend functions
         friend Vector3 operator+(const Vector3& vector1, const Vector3& vector2);
         friend Vector3 operator-(const Vector3& vector1, const Vector3& vector2);
         friend Vector3 operator-(const Vector3& vector);
-        friend Vector3 operator*(const Vector3& vector, double number);
+        friend Vector3 operator*(const Vector3& vector, decimal number);
-        friend Vector3 operator*(double number, const Vector3& vector);
+        friend Vector3 operator*(decimal number, const Vector3& vector);
 };
 
 // Get the x component of the vector
-inline double Vector3::getX() const {
+inline decimal Vector3::getX() const {
     return values[0];
 }
 
 // Get the y component of the vector
-inline double Vector3::getY() const {
+inline decimal Vector3::getY() const {
     return values[1];
 }
 
 // Get the z component of the vector
-inline double Vector3::getZ() const {
+inline decimal Vector3::getZ() const {
     return values[2];
 }
 
 // Set the x component of the vector
-inline void Vector3::setX(double x) {
+inline void Vector3::setX(decimal x) {
     this->values[0] = x;
 }
 
 // Set the y component of the vector
-inline void Vector3::setY(double y) {
+inline void Vector3::setY(decimal y) {
     this->values[1] = y;
 }
 
 // Set the z component of the vector
-inline void Vector3::setZ(double z) {
+inline void Vector3::setZ(decimal z) {
     this->values[2] = z;
 }
 
 // Set all the values of the vector (inline)
-inline void Vector3::setAllValues(double x, double y, double z) {
+inline void Vector3::setAllValues(decimal x, decimal y, decimal z) {
     values[0]= x;
     values[1] = y;
     values[2] = z;
 }
 
 // Return the length of the vector (inline)
-inline double Vector3::length() const {
+inline decimal Vector3::length() const {
     // Compute and return the length of the vector
     return sqrt(values[0]*values[0] + values[1]*values[1] + values[2]*values[2]);
 }
 
 // Return the square of the length of the vector
-inline double Vector3::lengthSquare() const {
+inline decimal Vector3::lengthSquare() const {
     return values[0]*values[0] + values[1]*values[1] + values[2]*values[2];
 }
 
 // Scalar product of two vectors (inline)
-inline double Vector3::dot(const Vector3& vector) const {
+inline decimal Vector3::dot(const Vector3& vector) const {
     // Compute and return the result of the scalar product
     return (values[0] * vector.values[0] + values[1] * vector.values[1] + values[2] * vector.values[2]);
 }
@@ -154,7 +155,7 @@ inline Vector3 Vector3::getAbsoluteVector() const {
 
 // Return true if two vectors are parallel
 inline bool Vector3::isParallelWith(const Vector3& vector) const {
-    double scalarProd = this->dot(vector);
+    decimal scalarProd = this->dot(vector);
     return approxEqual(std::abs(scalarProd), length() * vector.length());
 }    
 
@@ -206,7 +207,7 @@ inline Vector3& Vector3::operator-=(const Vector3& vector) {
 }
 
 // Overloaded operator for multiplication with a number with assignment
-inline Vector3& Vector3::operator*=(double number) {
+inline Vector3& Vector3::operator*=(decimal number) {
     values[0] *= number;
     values[1] *= number;
     values[2] *= number;
@@ -214,12 +215,12 @@ inline Vector3& Vector3::operator*=(double number) {
 }
 
 // Overloaded operator for value access
-inline double& Vector3::operator[] (int index) {
+inline decimal& Vector3::operator[] (int index) {
     return values[index];
 }
 
 // Overloaded operator for value access
-inline const double& Vector3::operator[] (int index) const {
+inline const decimal& Vector3::operator[] (int index) const {
     return values[index];
 }
 
@@ -239,12 +240,12 @@ inline Vector3 operator-(const Vector3& vector) {
 }
 
 // Overloaded operator for multiplication with a number
-inline Vector3 operator*(const Vector3& vector, double number) {
+inline Vector3 operator*(const Vector3& vector, decimal number) {
     return Vector3(number * vector.values[0], number * vector.values[1], number * vector.values[2]);
 }
 
 // Overloaded operator for multiplication with a number
-inline Vector3 operator*(double number, const Vector3& vector) {
+inline Vector3 operator*(decimal number, const Vector3& vector) {
     return vector * number;
 }
 

src/mathematics/mathematics.h

@@ -31,7 +31,7 @@
 #include "Quaternion.h"
 #include "Vector3.h"
 #include "Transform.h"
-#include "../constants.h"
+#include "../configuration.h"
 #include "mathematics_functions.h"
 #include <vector>
 #include <cstdio>
@@ -61,15 +61,15 @@ inline reactphysics3d::Vector3 rotateVectorWithQuaternion(const reactphysics3d::
 // P1 = point1 + alpha * d1
 // P2 = point2 + beta * d2
 inline void closestPointsBetweenTwoLines(const reactphysics3d::Vector3& point1, const reactphysics3d::Vector3& d1, const reactphysics3d::Vector3& point2,
-                                         const reactphysics3d::Vector3& d2, double* alpha, double* beta) {
+                                         const reactphysics3d::Vector3& d2, decimal* alpha, decimal* beta) {
 
     reactphysics3d::Vector3 r = point1 - point2;
-    double a = d1.dot(d1);
+    decimal a = d1.dot(d1);
-    double b = d1.dot(d2);
+    decimal b = d1.dot(d2);
-    double c = d1.dot(r);
+    decimal c = d1.dot(r);
-    double e = d2.dot(d2);
+    decimal e = d2.dot(d2);
-    double f = d2.dot(r);
+    decimal f = d2.dot(r);
-    double d = a*e-b*b;
+    decimal d = a*e-b*b;
 
     // The two lines must not be parallel
     assert(!reactphysics3d::approxEqual(d, 0.0));
@@ -90,11 +90,11 @@ inline bool isPointOnSegment(const reactphysics3d::Vector3& segPointA, const rea
     }
 
     // Compute the length of the segment
-    double segmentLength = d.length();
+    decimal segmentLength = d.length();
 
     // Compute the distance from point "P" to points "segPointA" and "segPointB"
-    double distA = dP.length();
+    decimal distA = dP.length();
-    double distB = (P - segPointB).length();
+    decimal distB = (P - segPointB).length();
 
     // If one of the "distA" and "distB" is greather than the length of the segment, then P is not on the segment
     if (distA > segmentLength || distB > segmentLength) {
@@ -111,7 +111,7 @@ inline bool isPointOnSegment(const reactphysics3d::Vector3& segPointA, const rea
 inline reactphysics3d::Vector3 computeLinesIntersection(const reactphysics3d::Vector3& p1, const reactphysics3d::Vector3& d1,
                                                          const reactphysics3d::Vector3& p2, const reactphysics3d::Vector3& d2) {
     // Computes the two closest points on the lines
-    double alpha, beta;
+    decimal alpha, beta;
     closestPointsBetweenTwoLines(p1, d1, p2, d2, &alpha, &beta);
     reactphysics3d::Vector3 point1 = p1 + alpha * d1;
     reactphysics3d::Vector3 point2 = p2 + beta * d2;
@@ -136,7 +136,7 @@ inline reactphysics3d::Vector3 computeNonParallelSegmentsIntersection(const reac
     assert(!d1.isParallelWith(d2));
 
     // Compute the closet points between the two lines
-    double alpha, beta;
+    decimal alpha, beta;
     closestPointsBetweenTwoLines(seg1PointA, d1, seg2PointA, d2, &alpha, &beta);
     reactphysics3d::Vector3 point1 = seg1PointA + alpha * d1;
     reactphysics3d::Vector3 point2 = seg2PointA + beta * d2;
@@ -147,7 +147,8 @@ inline reactphysics3d::Vector3 computeNonParallelSegmentsIntersection(const reac
 
     // If the two closest point aren't very close, there is no intersection between the segments
     reactphysics3d::Vector3 d = point2 - point1;
-    assert(d.length() <= EPSILON_TEST);
+    const decimal epsilon = 0.00001;
+    assert(d.length() <= epsilon);
 
     // They are very close so we return the intersection point (halfway between "point1" and "point2"
     return 0.5 * (point1 + point2);
@@ -192,7 +193,7 @@ inline std::vector<reactphysics3d::Vector3> projectPointsOntoPlane(const std::ve
 
 
 // Compute the distance between a point "P" and a line (given by a point "A" and a vector "v")
-inline double computeDistanceBetweenPointAndLine(const reactphysics3d::Vector3& P, const reactphysics3d::Vector3& A, const reactphysics3d::Vector3& v) {
+inline decimal computeDistanceBetweenPointAndLine(const reactphysics3d::Vector3& P, const reactphysics3d::Vector3& A, const reactphysics3d::Vector3& v) {
     assert(v.length() != 0);
     return ((P-A).cross(v).length() / (v.length()));
 }
@@ -208,12 +209,12 @@ inline reactphysics3d::Vector3 computeOrthogonalProjectionOfPointOntoALine(const
 // The result point Q will be in a segment of the rectangle
 inline reactphysics3d::Vector3 computeNearestPointOnRectangle(const reactphysics3d::Vector3& P, const std::vector<reactphysics3d::Vector3> rectangle) {
     assert(rectangle.size() == 4);
-    double distPSegment1 = computeDistanceBetweenPointAndLine(P, rectangle[0], rectangle[1] - rectangle[0]);
+    decimal distPSegment1 = computeDistanceBetweenPointAndLine(P, rectangle[0], rectangle[1] - rectangle[0]);
-    double distPSegment2 = computeDistanceBetweenPointAndLine(P, rectangle[1], rectangle[2] - rectangle[1]);
+    decimal distPSegment2 = computeDistanceBetweenPointAndLine(P, rectangle[1], rectangle[2] - rectangle[1]);
-    double distPSegment3 = computeDistanceBetweenPointAndLine(P, rectangle[2], rectangle[3] - rectangle[2]);
+    decimal distPSegment3 = computeDistanceBetweenPointAndLine(P, rectangle[2], rectangle[3] - rectangle[2]);
-    double distPSegment4 = computeDistanceBetweenPointAndLine(P, rectangle[3], rectangle[0] - rectangle[3]);
+    decimal distPSegment4 = computeDistanceBetweenPointAndLine(P, rectangle[3], rectangle[0] - rectangle[3]);
-    double distSegment1Segment3 = computeDistanceBetweenPointAndLine(rectangle[0], rectangle[3], rectangle[3] - rectangle[2]);
+    decimal distSegment1Segment3 = computeDistanceBetweenPointAndLine(rectangle[0], rectangle[3], rectangle[3] - rectangle[2]);
-    double distSegment2Segment4 = computeDistanceBetweenPointAndLine(rectangle[1], rectangle[3], rectangle[0] - rectangle[3]);
+    decimal distSegment2Segment4 = computeDistanceBetweenPointAndLine(rectangle[1], rectangle[3], rectangle[0] - rectangle[3]);
     Vector3 resultPoint;
     
     // Check if P is between the lines of the first pair of parallel segments of the rectangle
@@ -280,8 +281,8 @@ inline void computeParallelSegmentsIntersection(const reactphysics3d::Vector3& s
     assert(d1.isParallelWith(d2));
 
     // Compute the projection of the two points of the second segment onto the vector of segment 1
-    double projSeg2PointA = d1.getUnit().dot(seg2PointA - seg1PointA);
+    decimal projSeg2PointA = d1.getUnit().dot(seg2PointA - seg1PointA);
-    double projSeg2PointB = d1.getUnit().dot(seg2PointB - seg1PointA);
+    decimal projSeg2PointB = d1.getUnit().dot(seg2PointB - seg1PointA);
 
     // The projections intervals should intersect
     assert(!(projSeg2PointA < 0.0 && projSeg2PointB < 0.0));
@@ -319,7 +320,7 @@ inline void computeParallelSegmentsIntersection(const reactphysics3d::Vector3& s
 // The segment is given by the two vertices in "segment" and the rectangle is given by the ordered vertices in "clipRectangle".
 // This method returns the clipped segment.
 inline std::vector<reactphysics3d::Vector3> clipSegmentWithRectangleInPlane(const std::vector<reactphysics3d::Vector3>& segment, const std::vector<reactphysics3d::Vector3> clipRectangle) {
-    double const epsilon = 0.01;
+    decimal const epsilon = 0.01;
 
     assert(segment.size() == 2);
     assert(clipRectangle.size() == 4);
@@ -375,7 +376,7 @@ inline std::vector<reactphysics3d::Vector3> clipSegmentWithRectangleInPlane(cons
 // a rectangle polygon (given by the ordered 3D vertices in "clipRectangle"). The subject polygon and the clip rectangle are in 3D but we assumed that
 // they are on a same plane in 3D. The method returns the ordered 3D vertices of the subject polygon clipped using the rectangle polygon.
 inline std::vector<reactphysics3d::Vector3> clipPolygonWithRectangleInPlane(const std::vector<reactphysics3d::Vector3>& subjectPolygon, const std::vector<reactphysics3d::Vector3>& clipRectangle) {
-    double const epsilon = 0.1;
+    decimal const epsilon = 0.1;
     assert(clipRectangle.size() == 4);
 
     std::vector<reactphysics3d::Vector3> outputPolygon;
@@ -432,10 +433,10 @@ inline reactphysics3d::Vector3 intersectLineWithPlane(const reactphysics3d::Vect
     assert(!approxEqual(lineVector.dot(planeNormal), 0.0));
 
     // The plane is represented by the equation planeNormal dot X = d where X is a point of the plane
-    double d  = planeNormal.dot(planePoint);
+    decimal d  = planeNormal.dot(planePoint);
 
     // Compute the parameter t
-    double t = (d - planeNormal.dot(linePoint)) / planeNormal.dot(lineVector);
+    decimal t = (d - planeNormal.dot(linePoint)) / planeNormal.dot(lineVector);
 
     // Compute the intersection point
     return linePoint + lineVector * t;

src/mathematics/mathematics_functions.h

@@ -27,7 +27,8 @@
 #define MATHEMATICS_FUNCTIONS_H
 
 // Libraries
-#include "../constants.h"
+#include "../configuration.h"
+#include "../decimal.h"
 
 // ReactPhysics3D namespace
 namespace reactphysics3d {
@@ -36,8 +37,8 @@ namespace reactphysics3d {
 
 // function to test if two real numbers are (almost) equal
 // We test if two numbers a and b are such that (a-b) are in [-EPSILON; EPSILON]
-inline bool approxEqual(double a, double b, double epsilon = EPSILON) {
+inline bool approxEqual(decimal a, decimal b, decimal epsilon = 1.0e-10) {
-    double difference = a - b;
+    decimal difference = a - b;
     return (difference < epsilon && difference > -epsilon);
 }
 

src/memory/MemoryPool.h

@@ -27,7 +27,7 @@
 #define	MEMORY_POOL_H
 
 // Libraries
-#include "../constants.h"
+#include "../configuration.h"
 #include <cstddef>
 #include <cstdlib>
 #include <cassert>
@@ -115,6 +115,10 @@ MemoryPool<T>::MemoryPool(uint nbObjectsToAllocate) throw(std::bad_alloc)
 // Deallocate the block of memory that has been allocated previously
 template<class T>
 MemoryPool<T>::~MemoryPool() {
+    
+    // Check if we have a memory leak
+    assert(currentNbObjects == 0);
+    
     // Release the whole memory block
     free(pMemoryBlock);
 }

src/reactphysics3d.h

@@ -40,11 +40,11 @@
 #include "body/RigidBody.h"
 #include "engine/PhysicsWorld.h"
 #include "engine/PhysicsEngine.h"
-#include "shapes/Shape.h"
+#include "shapes/Collider.h"
-#include "shapes/BoxShape.h"
+#include "shapes/BoxCollider.h"
-#include "shapes/SphereShape.h"
+#include "shapes/SphereCollider.h"
-#include "shapes/ConeShape.h"
+#include "shapes/ConeCollider.h"
-#include "shapes/CylinderShape.h"
+#include "shapes/CylinderCollider.h"
 #include "shapes/AABB.h"
 
 // Alias to the ReactPhysics3D namespace

src/shapes/AABB.h

@@ -47,22 +47,22 @@ class AABB {
     private :
         Vector3 minCoordinates;        // Minimum world coordinates of the AABB on the x,y and z axis
         Vector3 maxCoordinates;        // Maximum world coordinates of the AABB on the x,y and z axis
-        Body* bodyPointer;              // Pointer to the owner body (not the abstract class Body but its derivative which is instanciable)
+        Body* bodyPointer;             // Pointer to the owner body (not the abstract class Body but its derivative which is instanciable)
 
     public :
-        AABB();                                                           // Constructor
+        AABB();                                                          // Constructor
         AABB(const Transform& transform, const Vector3& extents);        // Constructor
-        virtual ~AABB();                                                  // Destructor
+        virtual ~AABB();                                                 // Destructor
 
         Vector3 getCenter() const;                                                     // Return the center point
         const Vector3& getMinCoordinates() const;                                      // Return the minimum coordinates of the AABB
         const Vector3& getMaxCoordinates() const;                                      // Return the maximum coordinates of the AABB
-        Body* getBodyPointer() const;                                                   // Return a pointer to the owner body
+        Body* getBodyPointer() const;                                                  // Return a pointer to the owner body
-        void setBodyPointer(Body* bodyPointer);                                         // Set the body pointer
+        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
+        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 Vector3& 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)
+       virtual void draw() const;                                                      // Draw the AABB (only for testing purpose)
 #endif
 };
 

src/shapes/BoxCollider.cpp

@@ -24,7 +24,7 @@
 ********************************************************************************/
 
 // Libraries
-#include "BoxShape.h"
+#include "BoxCollider.h"
 #include "../configuration.h"
 #include <vector>
 #include <cassert>
@@ -46,21 +46,21 @@ using namespace reactphysics3d;
 using namespace std;
 
 // Constructor
-BoxShape::BoxShape(const Vector3& extent) : extent(extent) {
+BoxCollider::BoxCollider(const Vector3& extent) : extent(extent) {
 
 }
 
 // Destructor
-BoxShape::~BoxShape() {
+BoxCollider::~BoxCollider() {
 
 }
 
-// Return the local inertia tensor of the shape
+// Return the local inertia tensor of the collider
-void BoxShape::computeLocalInertiaTensor(Matrix3x3& tensor, double mass) const {
+void BoxCollider::computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const {
-    double factor = (1.0 / 3.0) * mass;
+    decimal factor = (1.0 / 3.0) * mass;
-    double xSquare = extent.getX() * extent.getX();
+    decimal xSquare = extent.getX() * extent.getX();
-    double ySquare = extent.getY() * extent.getY();
+    decimal ySquare = extent.getY() * extent.getY();
-    double zSquare = extent.getZ() * extent.getZ();
+    decimal 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));
@@ -68,10 +68,10 @@ void BoxShape::computeLocalInertiaTensor(Matrix3x3& tensor, double mass) const {
 
 #ifdef VISUAL_DEBUG
 // Draw the Box (only for testing purpose)
-void BoxShape::draw() const {
+void BoxCollider::draw() const {
-    double e1 = extent.getX();
+    decimal e1 = extent.getX();
-    double e2 = extent.getY();
+    decimal e2 = extent.getY();
-    double e3 = extent.getZ();
+    decimal e3 = extent.getZ();
 
     // Draw in red
     glColor3f(1.0, 0.0, 0.0);

src/shapes/BoxCollider.h

@@ -23,12 +23,12 @@
 *                                                                               *
 ********************************************************************************/
 
-#ifndef BOX_SHAPE_H
+#ifndef BOX_COLLIDER_H
-#define BOX_SHAPE_H
+#define BOX_COLLIDER_H
 
 // Libraries
 #include <cfloat>
-#include "Shape.h"
+#include "Collider.h"
 #include "../mathematics/mathematics.h"
 
 
@@ -36,26 +36,26 @@
 namespace reactphysics3d {
 
 /*  -------------------------------------------------------------------
-    Class BoxShape :
+    Class BoxCollider :
-        This class represents a 3D box. Those axis are unit length.
+        This class represents a 3D box collider. 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 gives an orientation and a position to the box.
     -------------------------------------------------------------------
 */
-class BoxShape : public Shape {
+class BoxCollider : public Collider {
     private :
         Vector3 extent;           // Extent sizes of the box in the x, y and z direction
 
     public :
-        BoxShape(const Vector3& extent);        // Constructor
+        BoxCollider(const Vector3& extent);        // Constructor
-        virtual ~BoxShape();                     // Destructor
+        virtual ~BoxCollider();                     // 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 getLocalExtents(decimal 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 getLocalSupportPoint(const Vector3& direction, decimal 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
+        virtual void computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const;              // Return the local inertia tensor of the collider
 
 #ifdef VISUAL_DEBUG
             virtual void draw() const;                                                                      // Draw the Box (only for testing purpose)
@@ -63,23 +63,23 @@ class BoxShape : public Shape {
 };
 
 // Return the extents of the box
-inline const Vector3& BoxShape::getExtent() const {
+inline const Vector3& BoxCollider::getExtent() const {
     return extent;
 }
 
  // Set the extents of the box
-inline void BoxShape::setExtent(const Vector3& extent) {
+inline void BoxCollider::setExtent(const Vector3& extent) {
     this->extent = extent;
 }
 
-// Return the local extents of the shape (half-width) in x,y and z local direction
+// 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(double margin) const {
+inline Vector3 BoxCollider::getLocalExtents(decimal margin) const {
     return extent + Vector3(margin, margin, margin);
 }
 
 // Return a local support point in a given direction
-inline Vector3 BoxShape::getLocalSupportPoint(const Vector3& direction, double margin) const {
+inline Vector3 BoxCollider::getLocalSupportPoint(const Vector3& direction, decimal margin) const {
     assert(margin >= 0.0);
     
     return Vector3(direction.getX() < 0.0 ? -extent.getX()-margin : extent.getX()+margin,

src/shapes/Collider.cpp

@@ -24,17 +24,17 @@
 ********************************************************************************/
 
 // Libraries
-#include "Shape.h"
+#include "Collider.h"
 
 // We want to use the ReactPhysics3D namespace
 using namespace reactphysics3d;
 
 // Constructor
-Shape::Shape() {
+Collider::Collider() {
     
 }
 
 // Destructor
-Shape::~Shape() {
+Collider::~Collider() {
 
 }

src/shapes/Collider.h

@@ -23,8 +23,8 @@
 *                                                                               *
 ********************************************************************************/
 
-#ifndef SHAPE_H
+#ifndef COLLIDER_H
-#define SHAPE_H
+#define COLLIDER_H
 
 // Libraries
 #include <cassert>
@@ -38,34 +38,34 @@ namespace reactphysics3d {
 class Body;
 
 /*  -------------------------------------------------------------------
-    Class Shape :
+    Class Collider :
-        This abstract class represents the shape of a body that is used during
+        This abstract class represents the collider associated with a
-        the narrow-phase collision detection.
+        body that is used during the narrow-phase collision detection.
     -------------------------------------------------------------------
 */
-class Shape {
+class Collider {
     protected :
         Body* bodyPointer;              // Pointer to the owner body (not the abstract class Body but its derivative which is instanciable)
 
     public :
-        Shape();                        // Constructor
+        Collider();                        // Constructor
-        virtual ~Shape();               // Destructor
+        virtual ~Collider();               // Destructor
 
         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 getLocalSupportPoint(const Vector3& direction, decimal 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 getLocalExtents(decimal 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
+        virtual void computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const=0;                // Return the local inertia tensor of the collider
 };
 
 // Return the body pointer
-inline Body* Shape::getBodyPointer() const {
+inline Body* Collider::getBodyPointer() const {
     assert(bodyPointer != 0);
     return bodyPointer;
 }
 
 // Set the body pointer
-inline void Shape::setBodyPointer(Body* bodyPointer) {
+inline void Collider::setBodyPointer(Body* bodyPointer) {
     this->bodyPointer = bodyPointer;
 }
 

src/shapes/ConeCollider..cpp

@@ -26,7 +26,7 @@
 // Libraries
 #include <complex>
 #include "../configuration.h"
-#include "ConeShape.h"
+#include "ConeCollider.h"
 
 #if defined(VISUAL_DEBUG)
 	#if defined(APPLE_OS)
@@ -44,7 +44,7 @@
 using namespace reactphysics3d;
 
 // Constructor
-ConeShape::ConeShape(double radius, double height) : radius(radius), halfHeight(height/2.0) {
+ConeCollider::ConeCollider(decimal radius, decimal height) : radius(radius), halfHeight(height/2.0) {
     assert(radius > 0.0);
     assert(halfHeight > 0.0);
     
@@ -53,25 +53,25 @@ ConeShape::ConeShape(double radius, double height) : radius(radius), halfHeight(
 }
 
 // Destructor
-ConeShape::~ConeShape() {
+ConeCollider::~ConeCollider() {
 
 }
 
 // Return a local support point in a given direction
-inline Vector3 ConeShape::getLocalSupportPoint(const Vector3& direction, double margin) const {
+inline Vector3 ConeCollider::getLocalSupportPoint(const Vector3& direction, decimal margin) const {
     assert(margin >= 0.0);
 
     const Vector3& v = direction;
-    double sinThetaTimesLengthV = sinTheta * v.length();
+    decimal sinThetaTimesLengthV = sinTheta * v.length();
     Vector3 supportPoint;
 
     if (v.getY() >= sinThetaTimesLengthV) {
         supportPoint = Vector3(0.0, halfHeight, 0.0);
     }
     else {
-        double projectedLength = sqrt(v.getX() * v.getX() + v.getZ() * v.getZ());
+        decimal projectedLength = sqrt(v.getX() * v.getX() + v.getZ() * v.getZ());
         if (projectedLength > MACHINE_EPSILON) {
-            double d = radius / projectedLength;
+            decimal d = radius / projectedLength;
             supportPoint = Vector3(v.getX() * d, -halfHeight, v.getZ() * d);
         }
         else {
@@ -93,7 +93,7 @@ inline Vector3 ConeShape::getLocalSupportPoint(const Vector3& direction, double
 
 #ifdef VISUAL_DEBUG
 // Draw the cone (only for debuging purpose)
-void ConeShape::draw() const {
+void ConeCollider::draw() const {
 
     // Draw in red
     glColor3f(1.0, 0.0, 0.0);

src/shapes/ConeCollider.h

@@ -23,19 +23,19 @@
 *                                                                               *
 ********************************************************************************/
 
-#ifndef CONE_SHAPE_H
+#ifndef CONE_COLLIDER_H
-#define CONE_SHAPE_H
+#define CONE_COLLIDER_H
 
 // Libraries
-#include "Shape.h"
+#include "Collider.h"
 #include "../mathematics/mathematics.h"
 
 // ReactPhysics3D namespace
 namespace reactphysics3d {
 
 /*  -------------------------------------------------------------------
-    Class ConeShape :
+    Class ConeCollider :
-        This class represents a cone collision shape centered at the
+        This class represents a cone collider 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
@@ -43,23 +43,23 @@ namespace reactphysics3d {
         to the cone.
     -------------------------------------------------------------------
 */
-class ConeShape : public Shape {
+class ConeCollider : public Collider {
     private :
-        double radius;              // Radius of the base
+        decimal radius;              // Radius of the base
-        double halfHeight;          // Half height of the cone
+        decimal halfHeight;          // Half height of the cone
-        double sinTheta;            // sine of the semi angle at the apex point
+        decimal sinTheta;            // sine of the semi angle at the apex point
         
     public :
-        ConeShape(double radius, double height);        // Constructor
+        ConeCollider(decimal radius, decimal height);      // Constructor
-        virtual ~ConeShape();                           // Destructor
+        virtual ~ConeCollider();                           // Destructor
 
-        double getRadius() const;                                                                   // Return the radius
+        decimal getRadius() const;                                                                   // Return the radius
-        void setRadius(double radius);                                                              // Set the radius
+        void setRadius(decimal radius);                                                              // Set the radius
-        double getHeight() const;                                                                   // Return the height
+        decimal getHeight() const;                                                                   // Return the height
-        void setHeight(double height);                                                              // Set the height
+        void setHeight(decimal 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 getLocalSupportPoint(const Vector3& direction, decimal 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 getLocalExtents(decimal 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
+        virtual void computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const;               // Return the local inertia tensor of the collider
 
 
 #ifdef VISUAL_DEBUG
@@ -68,12 +68,12 @@ class ConeShape : public Shape {
 };
 
 // Return the radius
-inline double ConeShape::getRadius() const {
+inline decimal ConeCollider::getRadius() const {
     return radius;
 }
 
 // Set the radius
-inline void ConeShape::setRadius(double radius) {
+inline void ConeCollider::setRadius(decimal radius) {
     this->radius = radius;
 
     // Update sine of the semi-angle at the apex point
@@ -81,12 +81,12 @@ inline void ConeShape::setRadius(double radius) {
 }
 
 // Return the height
-inline double ConeShape::getHeight() const {
+inline decimal ConeCollider::getHeight() const {
     return 2.0 * halfHeight;
 }
 
 // Set the height
-inline void ConeShape::setHeight(double height) {
+inline void ConeCollider::setHeight(decimal height) {
     this->halfHeight = height / 2.0;
 
     // Update the sine of the semi-angle at the apex point
@@ -94,14 +94,14 @@ inline void ConeShape::setHeight(double height) {
 }
 
 // Return the local extents in x,y and z direction
-inline Vector3 ConeShape::getLocalExtents(double margin) const {
+inline Vector3 ConeCollider::getLocalExtents(decimal margin) const {
     return Vector3(radius + margin, halfHeight + margin, radius + margin);
 }
 
-// Return the local inertia tensor of the shape
+// Return the local inertia tensor of the collider
-inline void ConeShape::computeLocalInertiaTensor(Matrix3x3& tensor, double mass) const {
+inline void ConeCollider::computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const {
-    double rSquare = radius * radius;
+    decimal rSquare = radius * radius;
-    double diagXZ = 0.15 * mass * (rSquare + halfHeight);
+    decimal 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);
 }
 

src/shapes/CylinderCollider.cpp

@@ -24,7 +24,7 @@
 ********************************************************************************/
 
 // Libraries
-#include "CylinderShape.h"
+#include "CylinderCollider.h"
 #include "../configuration.h"
 
 #if defined(VISUAL_DEBUG)
@@ -43,23 +43,23 @@
 using namespace reactphysics3d;
 
 // Constructor
-CylinderShape::CylinderShape(double radius, double height) : radius(radius), halfHeight(height/2.0) {
+CylinderCollider::CylinderCollider(decimal radius, decimal height) : radius(radius), halfHeight(height/2.0) {
 
 }
 
 // Destructor
-CylinderShape::~CylinderShape() {
+CylinderCollider::~CylinderCollider() {
 
 }
 
 // Return a local support point in a given direction
-Vector3 CylinderShape::getLocalSupportPoint(const Vector3& direction, double margin) const {
+Vector3 CylinderCollider::getLocalSupportPoint(const Vector3& direction, decimal margin) const {
     assert(margin >= 0.0);
 
     Vector3 supportPoint(0.0, 0.0, 0.0);
-    double uDotv = direction.getY();
+    decimal uDotv = direction.getY();
     Vector3 w(direction.getX(), 0.0, direction.getZ());
-    double lengthW = sqrt(direction.getX() * direction.getX() + direction.getZ() * direction.getZ());
+    decimal lengthW = sqrt(direction.getX() * direction.getX() + direction.getZ() * direction.getZ());
 
     if (lengthW != 0.0) {
         if (uDotv < 0.0) supportPoint.setY(-halfHeight);
@@ -85,7 +85,7 @@ Vector3 CylinderShape::getLocalSupportPoint(const Vector3& direction, double mar
 
 #ifdef VISUAL_DEBUG
 // Draw the cone (only for debuging purpose)
-void CylinderShape::draw() const {
+void CylinderCollider::draw() const {
 
     // Draw in red
     glColor3f(1.0, 0.0, 0.0);

src/shapes/CylinderCollider.h

@@ -23,11 +23,11 @@
 *                                                                               *
 ********************************************************************************/
 
-#ifndef CYLINDER_SHAPE_H
+#ifndef CYLINDER_COLLIDER_H
-#define CYLINDER_SHAPE_H
+#define CYLINDER_COLLIDER_H
 
 // Libraries
-#include "Shape.h"
+#include "Collider.h"
 #include "../mathematics/mathematics.h"
 
 
@@ -35,29 +35,29 @@
 namespace reactphysics3d {
 
 /*  -------------------------------------------------------------------
-    Class CylinderShape :
+    Class CylinderCollider :
-        This class represents a cylinder collision shape around the Y axis
+        This class represents a cylinder collision collider 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 {
+class CylinderCollider : public Collider {
     private :
-        double radius;              // Radius of the base
+        decimal radius;              // Radius of the base
-        double halfHeight;          // Half height of the cone
+        decimal halfHeight;          // Half height of the cone
 
     public :
-        CylinderShape(double radius, double height);        // Constructor
+        CylinderCollider(decimal radius, decimal height);      // Constructor
-        virtual ~CylinderShape();                           // Destructor
+        virtual ~CylinderCollider();                           // Destructor
 
-        double getRadius() const;                                                                   // Return the radius
+        decimal getRadius() const;                                                                   // Return the radius
-        void setRadius(double radius);                                                              // Set the radius
+        void setRadius(decimal radius);                                                              // Set the radius
-        double getHeight() const;                                                                   // Return the height
+        decimal getHeight() const;                                                                   // Return the height
-        void setHeight(double height);                                                              // Set the height
+        void setHeight(decimal 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 getLocalSupportPoint(const Vector3& direction, decimal 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 getLocalExtents(decimal 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
+        virtual void computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const;               // Return the local inertia tensor of the collider
 
 #ifdef VISUAL_DEBUG
         virtual void draw() const;                              // Draw the sphere (only for testing purpose)
@@ -65,34 +65,34 @@ class CylinderShape : public Shape {
 };
 
 // Return the radius
-inline double CylinderShape::getRadius() const {
+inline decimal CylinderCollider::getRadius() const {
     return radius;
 }
 
 // Set the radius
-inline void CylinderShape::setRadius(double radius) {
+inline void CylinderCollider::setRadius(decimal radius) {
     this->radius = radius;
 }
 
 // Return the height
-inline double CylinderShape::getHeight() const {
+inline decimal CylinderCollider::getHeight() const {
     return halfHeight * 2.0;
 }
 
 // Set the height
-inline void CylinderShape::setHeight(double height) {
+inline void CylinderCollider::setHeight(decimal height) {
     this->halfHeight = height / 2.0;
 }
 
 // Return the local extents in x,y and z direction
-inline Vector3 CylinderShape::getLocalExtents(double margin) const {
+inline Vector3 CylinderCollider::getLocalExtents(decimal 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 {
+inline void CylinderCollider::computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const {
-    double height = 2.0 * halfHeight;
+    decimal height = 2.0 * halfHeight;
-    double diag = (1.0 / 12.0) * mass * (3 * radius * radius + height * height);
+    decimal 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);
 }
 

src/shapes/SphereCollider.cpp

@@ -24,7 +24,7 @@
 ********************************************************************************/
 
 // Libraries
-#include "SphereShape.h"
+#include "SphereCollider.h"
 #include "../configuration.h"
 #include <cassert>
 
@@ -45,18 +45,18 @@ using namespace reactphysics3d;
 using namespace std;
 
 // Constructor
-SphereShape::SphereShape(double radius) : radius(radius) {
+SphereCollider::SphereCollider(decimal radius) : radius(radius) {
 
 }
 
 // Destructor
-SphereShape::~SphereShape() {
+SphereCollider::~SphereCollider() {
 
 }
 
 #ifdef VISUAL_DEBUG
 // Draw the sphere (only for testing purpose)
-void SphereShape::draw() const {
+void SphereCollider::draw() const {
 
     // Draw in red
     glColor3f(1.0, 0.0, 0.0);

src/shapes/SphereCollider.h

@@ -23,35 +23,35 @@
 *                                                                               *
 ********************************************************************************/
 
-#ifndef SPHERE_SHAPE_H
+#ifndef SPHERE_COLLIDER_H
-#define SPHERE_SHAPE_H
+#define SPHERE_COLLIDER_H
 
 // Libraries
-#include "Shape.h"
+#include "Collider.h"
 #include "../mathematics/mathematics.h"
 
 // ReactPhysics3D namespace
 namespace reactphysics3d {
 
 /*  -------------------------------------------------------------------
-    Class SphereShape :
+    Class SphereCollider :
-        This class represents a sphere collision shape that is centered
+        This class represents a sphere collider that is centered
         at the origin and defined by its radius.
     -------------------------------------------------------------------
 */
-class SphereShape : public Shape {
+class SphereCollider : public Collider {
     private :
-        double radius;              // Radius of the sphere
+        decimal radius;              // Radius of the sphere
 
     public :
-        SphereShape(double radius);                 // Constructor
+        SphereCollider(decimal radius);                 // Constructor
-        virtual ~SphereShape();                     // Destructor
+        virtual ~SphereCollider();                     // Destructor
 
-        double getRadius() const;                                                                   // Return the radius of the sphere
+        decimal getRadius() const;                                                                  // Return the radius of the sphere
-        void setRadius(double radius);                                                              // Set the radius of the sphere
+        void setRadius(decimal 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 getLocalSupportPoint(const Vector3& direction, decimal 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 getLocalExtents(decimal 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
+        virtual void computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const;              // Return the local inertia tensor of the collider
 
 #ifdef VISUAL_DEBUG
             virtual void draw() const;                              // Draw the sphere (only for testing purpose)
@@ -59,19 +59,19 @@ class SphereShape : public Shape {
 };
 
 // Get the radius of the sphere
-inline double SphereShape::getRadius() const {
+inline decimal SphereCollider::getRadius() const {
     return radius;
 }
 
 // Set the radius of the sphere
-inline void SphereShape::setRadius(double radius) {
+inline void SphereCollider::setRadius(decimal radius) {
     this->radius = radius;
 }
 
 // Return a local support point in a given direction
-inline Vector3 SphereShape::getLocalSupportPoint(const Vector3& direction, double margin) const {
+inline Vector3 SphereCollider::getLocalSupportPoint(const Vector3& direction, decimal margin) const {
     assert(margin >= 0.0);
-    double length = direction.length();
+    decimal length = direction.length();
 
     // If the direction vector is not the zero vector
     if (length > 0.0) {
@@ -84,15 +84,15 @@ inline Vector3 SphereShape::getLocalSupportPoint(const Vector3& direction, doubl
     return Vector3(0, radius + margin, 0);
 }
 
-// Return the local extents of the shape (half-width) in x,y and z local direction
+// Return the local extents of the collider (half-width) in x,y and z local direction
 // This method is used to compute the AABB of the box
-inline Vector3 SphereShape::getLocalExtents(double margin) const {
+inline Vector3 SphereCollider::getLocalExtents(decimal 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 {
+inline void SphereCollider::computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const {
-    double diag = 0.4 * mass * radius * radius;
+    decimal diag = 0.4 * mass * radius * radius;
     tensor.setAllValues(diag, 0.0, 0.0, 0.0, diag, 0.0, 0.0, 0.0, diag);
 }