mrq/reactphysics3d
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

implementation of GJK collision detection algorithm continued

Browse Source 
git-svn-id: https://reactphysics3d.googlecode.com/svn/trunk@416 92aac97c-a6ce-11dd-a772-7fcde58d38e6
This commit is contained in:
chappuis.daniel 2011-02-01 11:03:54 +00:00
parent 2acf563508
commit fdbb661df5
14 changed files with 378 additions and 36 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

26
src/body/BoundingSphere.h
View File

@ -46,16 +46,18 @@ class BoundingSphere : public NarrowBoundingVolume {
BoundingSphere(const Vector3D& center, double radius); // Constructor
virtual ~BoundingSphere(); // Destructor
Vector3D getCenter() const; // Return the center point of the sphere
void setCenter(const Vector3D& center); // Set the center point of the sphere
double getRadius() const; // Return the radius of the sphere
void setRadius(double radius); // Set the radius of the sphere
Vector3D getCenter() const; // Return the center point of the sphere
void setCenter(const Vector3D& center); // Set the center point of the sphere
double getRadius() const; // Return the radius of the sphere
void setRadius(double radius); // Set the radius of the sphere
virtual void update(const Vector3D& newCenter,
const Quaternion& rotationQuaternion); // Update the sphere orientation according to a new orientation of the rigid body
virtual AABB* computeAABB() const; // Return the corresponding AABB
#ifdef VISUAL_DEBUG
const Quaternion& rotationQuaternion); // Update the sphere orientation according to a new orientation of the rigid body
virtual AABB* computeAABB() const; // Return the corresponding AABB
virtual Vector3D getSupportPoint(const Vector3D& direction) const; // Return a support point in a given direction
#ifdef VISUAL_DEBUG
virtual void draw() const; // Draw the sphere (only for testing purpose)
#endif
#endif
};
// Return the center point of the sphere
@ -84,6 +86,14 @@ inline void BoundingSphere::update(const Vector3D& newCenter, const Quaternion&
center = newCenter;
}
// Return a support point in a given direction
inline Vector3D BoundingSphere::getSupportPoint(const Vector3D& direction) const {
assert(direction.length() > 0.0);
// Return the support point of the sphere in the given direction
return center + radius * direction.getUnit();
}
}; // End of the ReactPhysics3D namespace
#endif

7
src/body/NarrowBoundingVolume.h
View File

@ -45,10 +45,11 @@ class NarrowBoundingVolume : public BoundingVolume {
protected :
public :
NarrowBoundingVolume(); // Constructor
virtual ~NarrowBoundingVolume(); // Destructor
NarrowBoundingVolume(); // Constructor
virtual ~NarrowBoundingVolume(); // Destructor
virtual AABB* computeAABB() const=0; // Return the corresponding AABB
virtual AABB* computeAABB() const=0; // Return the corresponding AABB
virtual Vector3D getSupportPoint(const Vector3D& direction) const=0; // Return a support point in a given direction
};
}

13
src/body/OBB.h
View File

@ -66,9 +66,11 @@ class OBB : public NarrowBoundingVolume {
virtual std::vector<Vector3D> getExtremeVertices(const Vector3D& axis) const; // Return all the vertices that are projected at the extreme of the projection of the bouding volume on the axis
virtual void update(const Vector3D& newCenter, const Quaternion& rotationQuaternion); // Update the oriented bounding box orientation according to a new orientation of the rigid body
virtual AABB* computeAABB() const; // Return the corresponding AABB
#ifdef VISUAL_DEBUG
virtual Vector3D getSupportPoint(const Vector3D& direction) const; // Return a support point in a given direction
#ifdef VISUAL_DEBUG
virtual void draw() const; // Draw the OBB (only for testing purpose)
#endif
#endif
static OBB* computeFromVertices(const std::vector<Vector3D>& vertices, const Vector3D& center); // Compute an OBB from a set of vertices
};
@ -225,6 +227,13 @@ inline void OBB::update(const Vector3D& newCenter, const Quaternion& rotationQua
axis[2] = rotateVectorWithQuaternion(oldAxis[2], rotationQuaternion).getUnit();
}
// Return a support point in a given direction
inline Vector3D OBB::getSupportPoint(const Vector3D& direction) const {
// TODO : Implement this method
assert(false);
return Vector3D();
}
}; // End of the ReactPhysics3D namespace
#endif

30
src/collision/CollisionDetection.cpp
View File

@ -107,16 +107,33 @@ void CollisionDetection::computeAllContacts() {
assert(contactInfo);
// Compute one or several new contacts and add them into the physics world
computeContact(contactInfo);
computeContactSAT(contactInfo);
}
}
// Compute a contact (and add it to the physics world) for two colliding bodies
void CollisionDetection::computeContact(const ContactInfo* const contactInfo) {
// Compute a contact for GJK (and add it to the physics world)
void CollisionDetection::computeContactGJK(const ContactInfo* const contactInfo) {
// TODO : Compute PersisentContact here instead
// Compute the set of contact points
vector<Vector3D> contactPoints;
contactPoints.push_back(contactInfo->point1);
contactPoints.push_back(contactInfo->point2);
// Create a new contact
Contact* contact = new Contact(contactInfo->body1, contactInfo->body2, contactInfo->normal, contactInfo->penetrationDepth, contactPoints);
// Add the contact to the physics world
world->addConstraint(contact);
}
// Compute a contact for the SAT algorithm (and add it to the physics world) for two colliding bodies
// This method only works for collision between two OBB bounding volumes
void CollisionDetection::computeContactSAT(const ContactInfo* const contactInfo) {
// Extract informations from the contact info structure
const OBB* const obb1 = contactInfo->obb1;
const OBB* const obb2 = contactInfo->obb2;
const OBB* const obb1 = dynamic_cast<const OBB* const>(contactInfo->body1->getNarrowBoundingVolume());
const OBB* const obb2 = dynamic_cast<const OBB* const>(contactInfo->body2->getNarrowBoundingVolume());
Vector3D normal = contactInfo->normal;
double penetrationDepth = contactInfo->penetrationDepth;
@ -238,4 +255,5 @@ void CollisionDetection::computeContact(const ContactInfo* const contactInfo) {
// Create a new contact and add it to the physics world
world->addConstraint(new Contact(obb1->getBodyPointer(), obb2->getBodyPointer(), normal, penetrationDepth, clippedFace));
}
}
}

4
src/collision/CollisionDetection.h
View File

@ -55,8 +55,8 @@ class CollisionDetection {
void computeBroadPhase(); // Compute the broad-phase collision detection
void computeNarrowPhase(); // Compute the narrow-phase collision detection
void computeAllContacts(); // Compute all the contacts from the collision info list
void computeContact(const ContactInfo* const contactInfo); // Compute a contact (and add it to the physics world) for two colliding bodies
void computeContact2(const ContactInfo* const contactInfo); // Compute a contact (and add it to the physics world) for two colliding bodies
void computeContactSAT(const ContactInfo* const contactInfo); // Compute a contact for SAT (and add it to the physics world) for two colliding bodies
void computeContactGJK(const ContactInfo* const contactInfo); // Compute a contact for GJK (and add it to the physics world)
public :
CollisionDetection(PhysicsWorld* physicsWorld); // Constructor

13
src/collision/ContactInfo.cpp
View File

@ -27,8 +27,15 @@
using namespace reactphysics3d;
// Constructor
ContactInfo::ContactInfo(const OBB* const obb1, const OBB* const obb2, const Vector3D& normal, double penetrationDepth)
: obb1(obb1), obb2(obb2), normal(normal), penetrationDepth(penetrationDepth) {
// Constructor for SAT
ContactInfo::ContactInfo(Body* const body1, Body* const body2, const Vector3D& normal, double penetrationDepth)
: body1(body1), body2(body2), normal(normal), penetrationDepth(penetrationDepth), point1(0.0, 0.0, 0.0), point2(0.0, 0.0, 0.0) {
}
// Constructor for GJK
ContactInfo::ContactInfo(Body* const body1, Body* const body2, const Vector3D& normal,
double penetrationDepth, const Vector3D& point1, const Vector3D& point2)
: body1(body1), body2(body2), normal(normal), penetrationDepth(penetrationDepth), point1(point1), point2(point2) {
}

16
src/collision/ContactInfo.h
View File

@ -42,13 +42,17 @@ namespace reactphysics3d {
*/
struct ContactInfo {
public:
// TODO : Use polymorphism here (change OBB into BoundingVolume to be more general)
const OBB* const obb1; // Body pointer of the first bounding volume
const OBB* const obb2; // Body pointer of the second bounding volume
const Vector3D normal; // Normal vector the the collision contact
const double penetrationDepth; // Penetration depth of the contact
Body* const body1; // Pointer to the first body of the contact
Body* const body2; // Pointer to the second body of the contact
const Vector3D point1; // Contact point of body 1
const Vector3D point2; // Contact point of body 2
const Vector3D normal; // Normal vector the the collision contact
const double penetrationDepth; // Penetration depth of the contact
ContactInfo(const OBB* const obb1, const OBB* const obb2, const Vector3D& normal, double penetrationDepth); // Constructor
ContactInfo(Body* const body1, Body* const body2, const Vector3D& normal,
double penetrationDepth); // Constructor for SAT
ContactInfo(Body* const body1, Body* const body2, const Vector3D& normal,
double penetrationDepth, const Vector3D& point1, const Vector3D& point2); // Constructor for GJK
};
} // End of the ReactPhysics3D namespace

207
src/collision/GJKAlgorithm.cpp Normal file
View File

@ -0,0 +1,207 @@
/********************************************************************************
* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ *
* Copyright (c) 2010 Daniel Chappuis *
*********************************************************************************
* *
* Permission is hereby granted, free of charge, to any person obtaining a copy *
* of this software and associated documentation files (the "Software"), to deal *
* in the Software without restriction, including without limitation the rights *
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell *
* copies of the Software, and to permit persons to whom the Software is *
* furnished to do so, subject to the following conditions: *
* *
* The above copyright notice and this permission notice shall be included in *
* all copies or substantial portions of the Software. *
* *
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR *
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, *
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE *
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER *
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, *
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN *
* THE SOFTWARE. *
********************************************************************************/
// Libraries
#include "GJKAlgorithm.h"
#include "Simplex.h"
#include "../constraint/Contact.h"
#include "../constants.h"
#include <algorithm>
#include <cmath>
#include <cfloat>
#include <cassert>
// We want to use the ReactPhysics3D namespace
using namespace reactphysics3d;
// Constructor
GJKAlgorithm::GJKAlgorithm() : lastSeparatingAxis(1.0, 1.0, 1.0) {
// TODO : Check if we can initialize the last separating axis as above
}
// Destructor
GJKAlgorithm::~GJKAlgorithm() {
}
// Return true and compute a contact info if the two bounding volume collide.
// The method returns false if there is no collision between the two bounding volumes.
bool GJKAlgorithm::testCollision(const NarrowBoundingVolume* const boundingVolume1,
const NarrowBoundingVolume* const boundingVolume2,
ContactInfo*& contactInfo) {
Vector3D suppA; // Support point of object A
Vector3D suppB; // Support point of object B
Vector3D w; // Support point of Minkowski difference A-B
Vector3D pA; // Closest point of object A
Vector3D pB; // Closest point of object B
double vDotw;
double prevDistSquare;
assert(boundingVolume1 != boundingVolume2);
// Initialize the margin (sum of margins of both objects)
double margin = 2 * OBJECT_MARGIN;
double marginSquare = margin * margin;
assert(margin > 0.0);
// Create a simplex set
Simplex simplex;
// Get the last point V (last separating axis)
Vector3D v = lastSeparatingAxis;
// Initialize the upper bound for the square distance
double distSquare = DBL_MAX;
do {
// Compute the support points for object A and B
suppA = boundingVolume1->getSupportPoint(v.getOpposite());
suppB = boundingVolume2->getSupportPoint(v);
// Compute the support point for the Minkowski difference A-B
w = suppA - suppB;
vDotw = v.scalarProduct(w);
// If the enlarge objects (with margins) do not intersect
if (vDotw > 0.0 && vDotw * vDotw > distSquare * marginSquare) {
// No intersection, we return false
return false;
}
// If the objects intersect only in the margins
if (simplex.isPointInSimplex(w) || distSquare - vDotw <= distSquare * REL_ERROR_SQUARE) {
// Compute the closet points of both objects (without the margins)
simplex.computeClosestPointsOfAandB(pA, pB);
// Project those two points on the margins to have the closest points of both
// object with the margins
double dist = sqrt(distSquare);
assert(dist > 0.0);
pA = pA - (OBJECT_MARGIN / dist) * v;
pB = pB + (OBJECT_MARGIN / dist) * v;
// Compute the contact info
Vector3D normal = (pB - pA).getUnit();
double penetrationDepth = margin - dist;
assert(penetrationDepth > 0.0);
contactInfo = new ContactInfo(boundingVolume1->getBodyPointer(), boundingVolume2->getBodyPointer(),
normal, penetrationDepth, pA, pB);
// There is an intersection, therefore we return true
return true;
}
// Add the current point to the simplex
simplex.addPoint(w, suppA, suppB);
// If the simplex is affinely dependent
if (simplex.isAffinelyDependent()) {
// Compute the closet points of both objects (without the margins)
simplex.computeClosestPointsOfAandB(pA, pB);
// Project those two points on the margins to have the closest points of both
// object with the margins
double dist = sqrt(distSquare);
assert(dist > 0.0);
pA = pA - (OBJECT_MARGIN / dist) * v;
pB = pB + (OBJECT_MARGIN / dist) * v;
// Compute the contact info
Vector3D normal = (pB - pA).getUnit();
double penetrationDepth = margin - dist;
assert(penetrationDepth > 0.0);
contactInfo = new ContactInfo(boundingVolume1->getBodyPointer(), boundingVolume2->getBodyPointer(),
normal, penetrationDepth, pA, pB);
// There is an intersection, therefore we return true
return true;
}
// Compute the point of the simplex closest to the origin
// If the computation of the closest point fail
if (!simplex.computeClosestPoint(v)) {
// Compute the closet points of both objects (without the margins)
simplex.computeClosestPointsOfAandB(pA, pB);
// Project those two points on the margins to have the closest points of both
// object with the margins
double dist = sqrt(distSquare);
assert(dist > 0.0);
pA = pA - (OBJECT_MARGIN / dist) * v;
pB = pB + (OBJECT_MARGIN / dist) * v;
// Compute the contact info
Vector3D normal = (pB - pA).getUnit();
double penetrationDepth = margin - dist;
assert(penetrationDepth > 0.0);
contactInfo = new ContactInfo(boundingVolume1->getBodyPointer(), boundingVolume2->getBodyPointer(),
normal, penetrationDepth, pA, pB);
// There is an intersection, therefore we return true
return true;
}
// Store and update the squared distance of the closest point
prevDistSquare = distSquare;
distSquare = v.scalarProduct(v);
// If the distance to the closest point doesn't improve a lot
if (prevDistSquare - distSquare <= MACHINE_EPSILON * prevDistSquare) {
simplex.backupClosestPointInSimplex(v);
// Get the new squared distance
distSquare = v.scalarProduct(v);
// Compute the closet points of both objects (without the margins)
simplex.computeClosestPointsOfAandB(pA, pB);
// Project those two points on the margins to have the closest points of both
// object with the margins
double dist = sqrt(distSquare);
assert(dist > 0.0);
pA = pA - (OBJECT_MARGIN / dist) * v;
pB = pB + (OBJECT_MARGIN / dist) * v;
// Compute the contact info
Vector3D normal = (pB - pA).getUnit();
double penetrationDepth = margin - dist;
assert(penetrationDepth > 0.0);
contactInfo = new ContactInfo(boundingVolume1->getBodyPointer(), boundingVolume2->getBodyPointer(),
normal, penetrationDepth, pA, pB);
// There is an intersection, therefore we return true
return true;
}
} while(!simplex.isFull() && distSquare > MACHINE_EPSILON * simplex.getMaxLengthSquareOfAPoint());
// The objects (without margins) intersect
}

69
src/collision/GJKAlgorithm.h Normal file
View File

@ -0,0 +1,69 @@
/********************************************************************************
* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ *
* Copyright (c) 2010 Daniel Chappuis *
*********************************************************************************
* *
* Permission is hereby granted, free of charge, to any person obtaining a copy *
* of this software and associated documentation files (the "Software"), to deal *
* in the Software without restriction, including without limitation the rights *
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell *
* copies of the Software, and to permit persons to whom the Software is *
* furnished to do so, subject to the following conditions: *
* *
* The above copyright notice and this permission notice shall be included in *
* all copies or substantial portions of the Software. *
* *
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR *
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, *
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE *
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER *
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, *
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN *
* THE SOFTWARE. *
********************************************************************************/
#ifndef GJKALGORITHM_H
#define GJKALGORITHM_H
// Libraries
#include "NarrowPhaseAlgorithm.h"
#include "ContactInfo.h"
#include "../body/NarrowBoundingVolume.h"
// ReactPhysics3D namespace
namespace reactphysics3d {
// Constants
const double REL_ERROR = 1.0e-3;
const double REL_ERROR_SQUARE = REL_ERROR * REL_ERROR;
// TODO : Improve the description here
/* -------------------------------------------------------------------
Class GJKAlgorithm :
This class implements a narrow-phase collision detection algorithm. This
algorithm uses the ISA-GJK algorithm and the EPA algorithm. This
implementation is based on the implementation discussed in the book
"Collision Detection in 3D Environments".
-------------------------------------------------------------------
*/
class GJKAlgorithm : public NarrowPhaseAlgorithm {
private :
// TODO : Implement frame coherence. For each pair of body, store
// the last separating axis and use it to initialize the v vector
Vector3D lastSeparatingAxis;
public :
GJKAlgorithm(); // Constructor
~GJKAlgorithm(); // Destructor
virtual bool testCollision(const NarrowBoundingVolume* const boundingVolume1,
const NarrowBoundingVolume* const boundingVolume2,
ContactInfo*& contactInfo); // Return true and compute a contact info if the two bounding volume collide
};
} // End of the ReactPhysics3D namespace
// TODO : Check what would be a correct value for the OBJECT_MARGIN constant
#endif

4
src/collision/NarrowPhaseAlgorithm.h
View File

@ -47,7 +47,9 @@ class NarrowPhaseAlgorithm {
NarrowPhaseAlgorithm(); // Constructor
virtual ~NarrowPhaseAlgorithm(); // Destructor
virtual bool testCollision(const BoundingVolume* const boundingVolume1, const BoundingVolume* const boundingVolume2, ContactInfo*& contactInfo)=0; // Return true and compute a contact info if the two bounding volume collide
virtual bool testCollision(const NarrowBoundingVolume* const boundingVolume1,
const NarrowBoundingVolume* const boundingVolume2,
ContactInfo*& contactInfo)=0; // Return true and compute a contact info if the two bounding volume collide
};
} // End of reactphysics3d namespace

8
src/collision/SATAlgorithm.cpp
View File

@ -48,7 +48,9 @@ SATAlgorithm::~SATAlgorithm() {
// Return true and compute a contact info if the two bounding volume collide.
// The method returns false if there is no collision between the two bounding volumes.
bool SATAlgorithm::testCollision(const BoundingVolume* const boundingVolume1, const BoundingVolume* const boundingVolume2, ContactInfo*& contactInfo) {
bool SATAlgorithm::testCollision(const NarrowBoundingVolume* const boundingVolume1,
const NarrowBoundingVolume* const boundingVolume2,
ContactInfo*& contactInfo) {
assert(boundingVolume1 != boundingVolume2);
@ -239,7 +241,7 @@ bool SATAlgorithm::computeCollisionTest(const OBB* const obb1, const OBB* const
// normals for separation. Therefore the OBBs must intersect
// Compute the contact info
contactInfo = new ContactInfo(obb1, obb2, normal.getUnit(), minPenetrationDepth);
contactInfo = new ContactInfo(obb1->getBodyPointer(), obb2->getBodyPointer(), normal.getUnit(), minPenetrationDepth);
return true;
}
@ -398,7 +400,7 @@ bool SATAlgorithm::computeCollisionTest(const OBB* const obb1, const OBB* const
}
// Compute the contact info
contactInfo = new ContactInfo(obb1, obb2, normal.getUnit(), minPenetrationDepth);
contactInfo = new ContactInfo(obb1->getBodyPointer(), obb2->getBodyPointer(), normal.getUnit(), minPenetrationDepth);
return true;
}

4
src/collision/SATAlgorithm.h
View File

@ -57,7 +57,9 @@ class SATAlgorithm : public NarrowPhaseAlgorithm {
SATAlgorithm(); // Constructor
~SATAlgorithm(); // Destructor
virtual bool testCollision(const BoundingVolume* const boundingVolume1, const BoundingVolume* const boundingVolume2, ContactInfo*& contactInfo); // Return true and compute a contact info if the two bounding volume collide
virtual bool testCollision(const NarrowBoundingVolume* const boundingVolume1,
const NarrowBoundingVolume* const boundingVolume2,
ContactInfo*& contactInfo); // Return true and compute a contact info if the two bounding volume collide
};
// Return the contact normal with the correct sign (from obb1 toward obb2). "axis" is the axis vector direction where the

8
src/collision/Simplex.h
View File

@ -67,7 +67,6 @@ class Simplex {
bool isProperSubset(Bits subset) const; // Return true if the subset is a proper subset
void updateCache(); // Update the cached values used during the GJK algorithm
void computeDeterminants(); // Compute the cached determinant values
void backupClosestPointInSimplex(Vector3D& point); // Backup the closest point
Vector3D computeClosestPointForSubset(Bits subset) const; // Return the closest point "v" in the convex hull of a subset of points
public:
@ -76,9 +75,11 @@ class Simplex {
bool isFull() const; // Return true if the simplex contains 4 points
bool isEmpty() const; // Return true if the simple is empty
double getMaxLengthSquareOfAPoint() const; // Return the maximum squared length of a point
void addPoint(const Vector3D& point, const Vector3D& suppPointA, const Vector3D& suppPointB); // Addd a point to the simplex
bool isPointInSimplex(const Vector3D& point) const; // Return true if the point is in the simplex
bool isAffinelyDependent() const; // Return true if the set is affinely dependent
void backupClosestPointInSimplex(Vector3D& point); // Backup the closest point
void computeClosestPointsOfAandB(Vector3D& pA, Vector3D& pB) const; // Compute the closest points of object A and B
bool computeClosestPoint(Vector3D& v); // Compute the closest point to the origin of the current simplex
};
@ -103,6 +104,11 @@ inline bool Simplex::isEmpty() const {
return (bitsCurrentSimplex == 0x0);
}
// Return the maximum squared length of a point
inline double Simplex::getMaxLengthSquareOfAPoint() const {
return maxLengthSquare;
}
} // End of the ReactPhysics3D namespace
#endif

5
src/constants.h
View File

@ -27,6 +27,7 @@
// Libraries
#include <limits>
#include <cfloat>
// Type definitions
typedef unsigned int uint;
@ -36,10 +37,14 @@ const double EPSILON = 0.00001; // E
const double ONE_MINUS_EPSILON = 0.99999; // 1 - espilon
const double INFINITY_CONST = std::numeric_limits<double>::infinity(); // Infinity constant
const double PI = 3.14159265; // Pi constant
const double MACHINE_EPSILON = DBL_EPSILON;
// Physics Engine constants
const double DEFAULT_TIMESTEP = 0.002;
// GJK Algorithm parameters
const double OBJECT_MARGIN = 0.1; // Object margin for the hybrid GJK algorithm
// Contact constants
const double FRICTION_COEFFICIENT = 0.4; // Friction coefficient
const double PENETRATION_FACTOR = 0.2; // Penetration factor (between 0 and 1) which specify the importance of the