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

implementation of GJK and EPA collision detection algorithm continued

Browse Source 
git-svn-id: https://reactphysics3d.googlecode.com/svn/trunk@421 92aac97c-a6ce-11dd-a772-7fcde58d38e6
This commit is contained in:
chappuis.daniel 2011-02-18 10:36:29 +00:00
parent cd5fda4396
commit 08ccec586a
11 changed files with 45 additions and 30 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

23
src/body/AABB.h
View File

@ -122,34 +122,37 @@ inline void AABB::setExtent(unsigned int index, double extent) throw(std::invali
}
// Return the minimum position value on the given axis
// We consider the object margin here
inline double AABB::getMinValueOnAxis(uint axis) const throw(std::invalid_argument) {
switch (axis) {
case 0: return center.getX() - extent[0];
case 1: return center.getY() - extent[1];
case 2: return center.getZ() - extent[2];
case 0: return center.getX() - extent[0] - OBJECT_MARGIN;
case 1: return center.getY() - extent[1] - OBJECT_MARGIN;
case 2: return center.getZ() - extent[2] - OBJECT_MARGIN;
default: // The index value is not valid, we throw an exception
throw std::invalid_argument("Exception : The index value has to be between 0 and 2");
}
}
// Return the maximum position value on the given axis
// We consider the object margin here
inline double AABB::getMaxValueOnAxis(uint axis) const throw(std::invalid_argument) {
switch (axis) {
case 0: return center.getX() + extent[0];
case 1: return center.getY() + extent[1];
case 2: return center.getZ() + extent[2];
case 0: return center.getX() + extent[0] + OBJECT_MARGIN;
case 1: return center.getY() + extent[1] + OBJECT_MARGIN;
case 2: return center.getZ() + extent[2] + OBJECT_MARGIN;
default: // The index value is not valid, we throw an exception
throw std::invalid_argument("Exception : The index value has to be between 0 and 2");
}
}
// Return true if the current AABB is overlapping is the AABB in argument
// Return true if the current AABB is overlapping with the AABB in argument
// Two AABB overlap if they overlap in the three x, y and z axis at the same time
inline bool AABB::testCollision(const AABB& aabb) const {
Vector3D center2 = aabb.getCenter();
if (std::abs(center.getX() - center2.getX()) > (extent[0] + aabb.getExtent(0))) return false;
if (std::abs(center.getY() - center2.getY()) > (extent[1] + aabb.getExtent(1))) return false;
if (std::abs(center.getZ() - center2.getZ()) > (extent[2] + aabb.getExtent(2))) return false;
double margin = 2 * OBJECT_MARGIN;
if (std::abs(center.getX() - center2.getX()) > (extent[0] + aabb.getExtent(0) + margin)) return false;
if (std::abs(center.getY() - center2.getY()) > (extent[1] + aabb.getExtent(1) + margin)) return false;
if (std::abs(center.getZ() - center2.getZ()) > (extent[2] + aabb.getExtent(2) + margin)) return false;
return true;
}

2
src/body/BoundingSphere.cpp
View File

@ -53,6 +53,8 @@ void BoundingSphere::draw() const {
// Draw in red
glColor3f(1.0, 0.0, 0.0);
glTranslatef(center.getX(), center.getY(), center.getZ());
// Draw the sphere
glutWireSphere(radius, 50, 50);
}

13
src/body/BoundingSphere.h
View File

@ -88,11 +88,18 @@ inline void BoundingSphere::update(const Vector3D& newCenter, const Quaternion&
// Return a support point in a given direction
inline Vector3D BoundingSphere::getSupportPoint(const Vector3D& direction, double margin) const {
assert(direction.length() > 0.0);
assert(margin >= 0.0);
double length = direction.length();
// Return the support point of the sphere in the given direction
return center + (radius + margin) * direction.getUnit();
// If the direction vector is not the zero vector
if (length > EPSILON) {
// Return the support point of the sphere in the given direction
return center + (radius + margin) * direction.getUnit();
}
// If the direction vector is the zero vector we return a point on the
// boundary of the sphere
return center + Vector3D(radius + margin, 0.0, 0.0);
}
}; // End of the ReactPhysics3D namespace

2
src/body/OBB.cpp
View File

@ -166,7 +166,7 @@ vector<Vector3D> OBB::getExtremeVertices(const Vector3D& directionAxis) const {
double projectionLength = directionAxis.dot(vertex-center) / directionAxis.length();
// If we found a bigger projection length
if (projectionLength > maxProjectionLength + EPSILON) {
if (projectionLength > maxProjectionLength + EPSILON_TEST) {
maxProjectionLength = projectionLength;
extremeVertices.clear();
extremeVertices.push_back(vertex);

5
src/collision/CollisionDetection.cpp
View File

@ -25,6 +25,7 @@
// Libraries
#include "CollisionDetection.h"
#include "SAPAlgorithm.h"
#include "GJK/GJKAlgorithm.h"
#include "SATAlgorithm.h"
#include "../body/Body.h"
#include "../body/OBB.h"
@ -44,7 +45,7 @@ CollisionDetection::CollisionDetection(PhysicsWorld* world) {
broadPhaseAlgorithm = new SAPAlgorithm();
// Create the narrow-phase algorithm that will be used (Separating axis algorithm)
narrowPhaseAlgorithm = new SATAlgorithm();
narrowPhaseAlgorithm = new GJKAlgorithm();
}
// Destructor
@ -107,7 +108,7 @@ void CollisionDetection::computeAllContacts() {
assert(contactInfo);
// Compute one or several new contacts and add them into the physics world
computeContactSAT(contactInfo);
computeContactGJK(contactInfo);
}
}

2
src/collision/EPA/EPAAlgorithm.cpp
View File

@ -30,6 +30,8 @@
// We want to use the ReactPhysics3D namespace
using namespace reactphysics3d;
// TODO : Check that allocated memory is correctly deleted
// Constructor
EPAAlgorithm::EPAAlgorithm() {

14
src/collision/GJK/GJKAlgorithm.cpp
View File

@ -37,6 +37,8 @@
// We want to use the ReactPhysics3D namespace
using namespace reactphysics3d;
// TODO : Check that allocated memory is correctly deleted
// Constructor
GJKAlgorithm::GJKAlgorithm() {
@ -115,9 +117,8 @@ bool GJKAlgorithm::testCollision(const NarrowBoundingVolume* const boundingVolum
pB = pB + (OBJECT_MARGIN / dist) * v;
// Compute the contact info
Vector3D normal = (pB - pA).getUnit();
Vector3D normal = v.getOpposite().getUnit();
double penetrationDepth = margin - dist;
assert(penetrationDepth > 0.0);
contactInfo = new ContactInfo(boundingVolume1->getBodyPointer(), boundingVolume2->getBodyPointer(),
normal, penetrationDepth, pA, pB);
@ -141,9 +142,8 @@ bool GJKAlgorithm::testCollision(const NarrowBoundingVolume* const boundingVolum
pB = pB + (OBJECT_MARGIN / dist) * v;
// Compute the contact info
Vector3D normal = (pB - pA).getUnit();
Vector3D normal = v.getOpposite().getUnit();
double penetrationDepth = margin - dist;
assert(penetrationDepth > 0.0);
contactInfo = new ContactInfo(boundingVolume1->getBodyPointer(), boundingVolume2->getBodyPointer(),
normal, penetrationDepth, pA, pB);
@ -165,9 +165,8 @@ bool GJKAlgorithm::testCollision(const NarrowBoundingVolume* const boundingVolum
pB = pB + (OBJECT_MARGIN / dist) * v;
// Compute the contact info
Vector3D normal = (pB - pA).getUnit();
Vector3D normal = v.getOpposite().getUnit();
double penetrationDepth = margin - dist;
assert(penetrationDepth > 0.0);
contactInfo = new ContactInfo(boundingVolume1->getBodyPointer(), boundingVolume2->getBodyPointer(),
normal, penetrationDepth, pA, pB);
@ -197,9 +196,8 @@ bool GJKAlgorithm::testCollision(const NarrowBoundingVolume* const boundingVolum
pB = pB + (OBJECT_MARGIN / dist) * v;
// Compute the contact info
Vector3D normal = (pB - pA).getUnit();
Vector3D normal = v.getOpposite().getUnit();
double penetrationDepth = margin - dist;
assert(penetrationDepth > 0.0);
contactInfo = new ContactInfo(boundingVolume1->getBodyPointer(), boundingVolume2->getBodyPointer(),
normal, penetrationDepth, pA, pB);

2
src/collision/SAPAlgorithm.cpp
View File

@ -142,7 +142,7 @@ void SAPAlgorithm::computePossibleCollisionPairs(vector<Body*> addedBodies, vect
variance[i] = esperanceSquare[i] - esperance[i] * esperance[i] / nbAABBs;
}
// Update the sorted Axis according to the axis with the largest variance
// Update the sorted axis according to the axis with the largest variance
sortAxis = 0;
if (variance[1] > variance[0]) sortAxis = 1;
if (variance[2] > variance[sortAxis]) sortAxis = 2;

2
src/collision/SATAlgorithm.h
View File

@ -63,7 +63,7 @@ class SATAlgorithm : public NarrowPhaseAlgorithm {
};
// Return the contact normal with the correct sign (from obb1 toward obb2). "axis" is the axis vector direction where the
// collision occur and "distanceOfOBBs" is the vector (obb2.center - obb1.center).
// collision occurs and "distanceOfOBBs" is the vector (obb2.center - obb1.center).
inline Vector3D SATAlgorithm::computeContactNormal(const Vector3D& axis, const Vector3D& distanceOfOBBs) const {
if (distanceOfOBBs.dot(axis) >= 0.0) {
return axis;

9
src/constants.h
View File

@ -33,17 +33,18 @@
typedef unsigned int uint;
// Mathematical constants
const double EPSILON = 0.00001; // Epsilon value to avoid numerical errors
const double ONE_MINUS_EPSILON = 0.99999; // 1 - espilon
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
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
const double OBJECT_MARGIN = 0.04; // Object margin for collision detection
// Contact constants
const double FRICTION_COEFFICIENT = 0.4; // Friction coefficient

1
src/reactphysics3d.h
View File

@ -40,6 +40,7 @@
#include "engine/PhysicsEngine.h"
#include "body/BoundingVolume.h"
#include "body/OBB.h"
#include "body/BoundingSphere.h"
#include "body/AABB.h"
// Alias to the ReactPhysics3D namespace