Use a customize algorithm for Sphere vs Sphere collision detection instead of using GJK

git-svn-id: https://reactphysics3d.googlecode.com/svn/trunk@463 92aac97c-a6ce-11dd-a772-7fcde58d38e6
This commit is contained in:
chappuis.daniel 2012-01-25 22:57:27 +00:00
parent ea2d922ae5
commit 9333d0e690
21 changed files with 233 additions and 57 deletions

View File

@ -32,7 +32,7 @@ using namespace reactphysics3d;
// Constructor
Body::Body(const Transform& transform, Collider* collider, decimal mass, long unsigned int id)
: collider(collider), mass(mass), transform(transform), id(id) {
: collider(collider), mass(mass), transform(transform), isActive(true), id(id) {
assert(mass > 0.0);
assert(collider);

View File

@ -51,6 +51,7 @@ class Body {
Transform transform; // Position and orientation of the body
Transform oldTransform; // Last position and orientation of the body
decimal interpolationFactor; // Interpolation factor used for the state interpolation
bool isActive; // True if the body is active (not sleeping because of deactivation)
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
@ -65,6 +66,8 @@ class Body {
void setCollider(Collider* collider); // Set the collision collider
decimal getMass() const; // Return the mass of the body
void setMass(decimal mass); // Set the mass of the body
bool getIsActive() const; // Return true if the body is active
void setIsActive(bool isActive); // Set the isActive variable
const Transform& getTransform() const; // Return 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
@ -100,6 +103,21 @@ inline decimal Body::getMass() const {
return mass;
};
// Method that set the mass of the body
inline void Body::setMass(decimal mass) {
this->mass = mass;
}
// Return true if the body is active
inline bool Body::getIsActive() const {
return isActive;
}
// Set the isActive variable
inline void Body::setIsActive(bool isActive) {
this->isActive = isActive;
}
// Return the interpolated transform for rendering
inline Transform Body::getInterpolatedTransform() const {
return Transform::interpolateTransforms(oldTransform, transform, interpolationFactor);
@ -121,11 +139,6 @@ inline void Body::setIsMotionEnabled(bool isMotionEnabled) {
this->isMotionEnabled = isMotionEnabled;
}
// Method that set the mass of the body
inline void Body::setMass(decimal mass) {
this->mass = mass;
}
// Return the current position and orientation
inline const Transform& Body::getTransform() const {
return transform;

View File

@ -46,7 +46,7 @@ using namespace reactphysics3d;
using namespace std;
// Constructor
BoxCollider::BoxCollider(const Vector3& extent) : extent(extent) {
BoxCollider::BoxCollider(const Vector3& extent) : Collider(BOX), extent(extent) {
}

View File

@ -30,7 +30,7 @@
using namespace reactphysics3d;
// Constructor
Collider::Collider() {
Collider::Collider(ColliderType type) : type(type) {
}

View File

@ -33,6 +33,9 @@
// ReactPhysics3D namespace
namespace reactphysics3d {
// Enumeration
enum ColliderType {BOX, SPHERE, CONE, CYLINDER}; // Type of the collider
// Declarations
class Body;
@ -44,15 +47,18 @@ class Body;
-------------------------------------------------------------------
*/
class Collider {
protected :
Body* bodyPointer; // Pointer to the owner body (not the abstract class Body but its derivative which is instanciable)
ColliderType type; // Type of the collider
public :
Collider(); // Constructor
virtual ~Collider(); // Destructor
Collider(ColliderType type); // Constructor
virtual ~Collider(); // Destructor
Body* getBodyPointer() const; // Return the body pointer
void setBodyPointer(Body* bodyPointer); // Set the body pointer
ColliderType getType() const; // Return the type of the collider
virtual Vector3 getLocalSupportPoint(const Vector3& direction, decimal margin=0.0) const=0; // Return a local support point in a given 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, decimal mass) const=0; // Return the local inertia tensor of the collider
@ -69,6 +75,11 @@ inline void Collider::setBodyPointer(Body* bodyPointer) {
this->bodyPointer = bodyPointer;
}
// Return the type of the collider
inline ColliderType Collider::getType() const {
return type;
}
}
#endif

View File

@ -44,7 +44,7 @@
using namespace reactphysics3d;
// Constructor
ConeCollider::ConeCollider(decimal radius, decimal height) : radius(radius), halfHeight(height/2.0) {
ConeCollider::ConeCollider(decimal radius, decimal height) : Collider(CONE), radius(radius), halfHeight(height/2.0) {
assert(radius > 0.0);
assert(halfHeight > 0.0);

View File

@ -43,7 +43,8 @@
using namespace reactphysics3d;
// Constructor
CylinderCollider::CylinderCollider(decimal radius, decimal height) : radius(radius), halfHeight(height/2.0) {
CylinderCollider::CylinderCollider(decimal radius, decimal height)
: Collider(CYLINDER), radius(radius), halfHeight(height/2.0) {
}

View File

@ -45,7 +45,7 @@ using namespace reactphysics3d;
using namespace std;
// Constructor
SphereCollider::SphereCollider(decimal radius) : radius(radius) {
SphereCollider::SphereCollider(decimal radius) : Collider(SPHERE), radius(radius) {
}

View File

@ -26,7 +26,6 @@
// Libraries
#include "CollisionDetection.h"
#include "broadphase/SweepAndPruneAlgorithm.h"
#include "narrowphase/GJK/GJKAlgorithm.h"
#include "../body/Body.h"
#include "../colliders/BoxCollider.h"
#include "../body/RigidBody.h"
@ -42,13 +41,14 @@ using namespace std;
// Constructor
CollisionDetection::CollisionDetection(PhysicsWorld* world)
: world(world), memoryPoolContacts(NB_MAX_CONTACTS), memoryPoolOverlappingPairs(NB_MAX_COLLISION_PAIRS) {
: world(world), narrowPhaseGJKAlgorithm(*this), narrowPhaseSphereVsSphereAlgorithm(*this),
memoryPoolContacts(NB_MAX_CONTACTS), memoryPoolOverlappingPairs(NB_MAX_COLLISION_PAIRS) {
// Create the broad-phase algorithm that will be used (Sweep and Prune with AABB)
broadPhaseAlgorithm = new SweepAndPruneAlgorithm(*this);
// Create the narrow-phase algorithm that will be used (Separating axis algorithm)
narrowPhaseAlgorithm = new GJKAlgorithm(*this);
// Create the narrow-phase algorithm that will be used
//narrowPhaseAlgorithm = new GJKAlgorithm(*this);
}
// Destructor
@ -118,9 +118,12 @@ bool CollisionDetection::computeNarrowPhase() {
// Update the contact cache of the overlapping pair
(*it).second->update();
// Select the narrow phase algorithm to use according to the two colliders
NarrowPhaseAlgorithm& narrowPhaseAlgorithm = SelectNarrowPhaseAlgorithm(body1->getCollider(), body2->getCollider());
// Use the narrow-phase collision detection algorithm to check if there really are a contact
if (narrowPhaseAlgorithm->testCollision(body1->getCollider(), body1->getTransform(),
body2->getCollider(), body2->getTransform(), contactInfo)) {
if (narrowPhaseAlgorithm.testCollision(body1->getCollider(), body1->getTransform(),
body2->getCollider(), body2->getTransform(), contactInfo)) {
assert(contactInfo);
collisionExists = true;

View File

@ -31,6 +31,8 @@
#include "OverlappingPair.h"
#include "../engine/PhysicsWorld.h"
#include "../memory/MemoryPool.h"
#include "narrowphase/GJK/GJKAlgorithm.h"
#include "narrowphase/SphereVsSphereAlgorithm.h"
#include "ContactInfo.h"
#include <vector>
#include <map>
@ -43,37 +45,39 @@ namespace reactphysics3d {
// Declarations
class BroadPhaseAlgorithm;
class NarrowPhaseAlgorithm;
/* -------------------------------------------------------------------
Class CollisionDetection :
This class computes the collision detection algorithms. We first
perfom a broad-phase algorithm to know which pairs of bodies can
perform a broad-phase algorithm to know which pairs of bodies can
collide and then we run a narrow-phase algorithm to compute the
collision contacts between bodies.
-------------------------------------------------------------------
*/
class CollisionDetection {
private :
PhysicsWorld* world; // Pointer to the physics world
std::map<std::pair<luint, luint>, OverlappingPair*> overlappingPairs; // Broad-phase overlapping pairs of bodies
std::set<std::pair<luint, luint> > currentStepOverlappingPairs; // Overlapping pairs of bodies at the current collision detection step
std::set<std::pair<luint, luint> > lastStepOverlappingPairs; // Overlapping pairs of bodies at the last collision detection step
BroadPhaseAlgorithm* broadPhaseAlgorithm; // Broad-phase algorithm
NarrowPhaseAlgorithm* narrowPhaseAlgorithm; // Narrow-phase algorithm
MemoryPool<Contact> memoryPoolContacts; // Memory pool for the contacts
MemoryPool<OverlappingPair> memoryPoolOverlappingPairs; // Memory pool for the overlapping pairs
void computeBroadPhase(); // Compute the broad-phase collision detection
bool computeNarrowPhase(); // Compute the narrow-phase collision detection
PhysicsWorld* world; // Pointer to the physics world
std::map<std::pair<luint, luint>, OverlappingPair*> overlappingPairs; // Broad-phase overlapping pairs of bodies
std::set<std::pair<luint, luint> > currentStepOverlappingPairs; // Overlapping pairs of bodies at the current collision detection step
std::set<std::pair<luint, luint> > lastStepOverlappingPairs; // Overlapping pairs of bodies at the last collision detection step
BroadPhaseAlgorithm* broadPhaseAlgorithm; // Broad-phase algorithm
GJKAlgorithm narrowPhaseGJKAlgorithm; // Narrow-phase GJK algorithm
SphereVsSphereAlgorithm narrowPhaseSphereVsSphereAlgorithm; // Narrow-phase Sphere vs Sphere algorithm
MemoryPool<Contact> memoryPoolContacts; // Memory pool for the contacts
MemoryPool<OverlappingPair> memoryPoolOverlappingPairs; // Memory pool for the overlapping pairs
void computeBroadPhase(); // Compute the broad-phase collision detection
bool computeNarrowPhase(); // Compute the narrow-phase collision detection
NarrowPhaseAlgorithm& SelectNarrowPhaseAlgorithm(Collider* collider1,
Collider* collider2); // Select the narrow phase algorithm to use given two colliders
public :
CollisionDetection(PhysicsWorld* physicsWorld); // Constructor
~CollisionDetection(); // Destructor
CollisionDetection(PhysicsWorld* physicsWorld); // Constructor
~CollisionDetection(); // Destructor
OverlappingPair* getOverlappingPair(luint body1ID, luint body2ID); // Return an overlapping pair or null
bool computeCollisionDetection(); // Compute the collision detection
void broadPhaseNotifyOverlappingPair(Body* body1, Body* body2); // Allow the broadphase to notify the collision detection about an overlapping pair
OverlappingPair* getOverlappingPair(luint body1ID, luint body2ID); // Return an overlapping pair or null
bool computeCollisionDetection(); // Compute the collision detection
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
@ -86,6 +90,18 @@ inline OverlappingPair* CollisionDetection::getOverlappingPair(luint body1ID, lu
return NULL;
}
// Select the narrow-phase collision algorithm to use given two colliders
inline NarrowPhaseAlgorithm& CollisionDetection::SelectNarrowPhaseAlgorithm(Collider* collider1, Collider* collider2) {
// Sphere vs Sphere algorithm
if (collider1->getType() == SPHERE && collider2->getType() == SPHERE) {
return narrowPhaseSphereVsSphereAlgorithm;
}
else { // GJK algorithm
return narrowPhaseGJKAlgorithm;
}
}
} // End of the ReactPhysics3D namespace
#endif

View File

@ -36,7 +36,7 @@ namespace reactphysics3d {
/* -------------------------------------------------------------------
Structure ContactInfo :
This structure contains informations about a collision contact
computed durring the narow phase collision detection. Those
computed during the narrow-phase collision detection. Those
informations are use to compute the contact set for a contact
between two bodies.
-------------------------------------------------------------------
@ -46,7 +46,7 @@ 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 decimal 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
@ -54,7 +54,7 @@ struct ContactInfo {
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
const Transform& transform1, const Transform& transform2); // Constructor
};
} // End of the ReactPhysics3D namespace

View File

@ -28,6 +28,8 @@
#include "Simplex.h"
#include "../../../constraint/Contact.h"
#include "../../../configuration.h"
#include "../../OverlappingPair.h"
#include "../../CollisionDetection.h"
#include <algorithm>
#include <cmath>
#include <cfloat>

View File

@ -29,10 +29,12 @@
// Libraries
#include "../../body/Body.h"
#include "../ContactInfo.h"
#include "../CollisionDetection.h"
// Namespace ReactPhysics3D
namespace reactphysics3d {
// Class declarations
class CollisionDetection;
/* -------------------------------------------------------------------
Class NarrowPhaseAlgorithm :

View File

@ -0,0 +1,74 @@
/********************************************************************************
* 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. *
* *
********************************************************************************/
// Libraries
#include "SphereVsSphereAlgorithm.h"
#include "../../colliders/SphereCollider.h"
// We want to use the ReactPhysics3D namespace
using namespace reactphysics3d;
// Constructor
SphereVsSphereAlgorithm::SphereVsSphereAlgorithm(CollisionDetection& collisionDetection)
:NarrowPhaseAlgorithm(collisionDetection) {
}
// Destructor
SphereVsSphereAlgorithm::~SphereVsSphereAlgorithm() {
}
bool SphereVsSphereAlgorithm::testCollision(const Collider* collider1, const Transform& transform1,
const Collider* collider2, const Transform& transform2, ContactInfo*& contactInfo) {
assert(collider1 != collider2);
// Get the sphere colliders
const SphereCollider* sphereCollider1 = dynamic_cast<const SphereCollider*>(collider1);
const SphereCollider* sphereCollider2 = dynamic_cast<const SphereCollider*>(collider2);
// Compute the distance between the centers
Vector3 vectorBetweenCenters = transform2.getPosition() - transform1.getPosition();
decimal squaredDistanceBetweenCenters = vectorBetweenCenters.lengthSquare();
// Compute the sum of the radius
decimal sumRadius = sphereCollider1->getRadius() + sphereCollider2->getRadius();
// If the sphere colliders intersect
if (squaredDistanceBetweenCenters <= sumRadius * sumRadius) {
Vector3 centerSphere2InBody1LocalSpace = transform1.inverse() * transform2.getPosition();
Vector3 centerSphere1InBody2LocalSpace = transform2.inverse() * transform1.getPosition();
Vector3 intersectionOnBody1 = sphereCollider1->getRadius() * centerSphere2InBody1LocalSpace.getUnit();
Vector3 intersectionOnBody2 = sphereCollider2->getRadius() * centerSphere1InBody2LocalSpace.getUnit();
decimal penetrationDepth = sumRadius - std::sqrt(squaredDistanceBetweenCenters);
contactInfo = new ContactInfo(collider1->getBodyPointer(), collider2->getBodyPointer(), vectorBetweenCenters.getUnit(),
penetrationDepth, intersectionOnBody1, intersectionOnBody2, transform1, transform2);
return true;
}
return false;
}

View File

@ -0,0 +1,59 @@
/********************************************************************************
* 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 SPHERE_VS_SPHERE_ALGORITHM_H
#define SPHERE_VS_SPHERE_ALGORITHM_H
// Libraries
#include "../../body/Body.h"
#include "../ContactInfo.h"
#include "NarrowPhaseAlgorithm.h"
// Namespace ReactPhysics3D
namespace reactphysics3d {
/* -------------------------------------------------------------------
Class SphereVsSphereAlgorithm :
This class is used to compute the narrow-phase collision detection
between two sphere colliders.
-------------------------------------------------------------------
*/
class SphereVsSphereAlgorithm : public NarrowPhaseAlgorithm {
protected :
public :
SphereVsSphereAlgorithm(CollisionDetection& collisionDetection); // Constructor
virtual ~SphereVsSphereAlgorithm(); // Destructor
virtual bool testCollision(const Collider* collider1, const Transform& transform1,
const Collider* collider2, const Transform& transform2,
ContactInfo*& contactInfo); // Return true and compute a contact info if the two bounding volume collide
};
} // End of reactphysics3d namespace
#endif

View File

@ -52,7 +52,8 @@ const reactphysics3d::decimal DECIMAL_INFINITY = std::numeric_limits<reactphysic
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
const reactphysics3d::decimal DEFAULT_TIMESTEP = 1.0 / 60.0; // Default internal constant timestep in seconds
const bool DEACTIVATION_ENABLED = true; // True if the deactivation (sleeping) of inactive bodies is enabled
// GJK Algorithm parameters
const reactphysics3d::decimal OBJECT_MARGIN = 0.04; // Object margin for collision detection in cm

View File

@ -64,7 +64,7 @@ namespace reactphysics3d {
class Contact : public Constraint {
protected :
const Vector3 normal; // Normal vector of the contact (From body1 toward body2) in world space
decimal 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

View File

@ -134,7 +134,6 @@ class ConstraintSolver {
Vector3 getErrorConstrainedLinearVelocityOfBody(Body* body); // Return the constrained linear velocity of a body after solving the LCP problem for error correction
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
};

View File

@ -32,7 +32,8 @@ using namespace std;
// Constructor
PhysicsEngine::PhysicsEngine(PhysicsWorld* world, decimal timeStep = DEFAULT_TIMESTEP)
: world(world), timer(timeStep), collisionDetection(world), constraintSolver(world) {
: world(world), timer(timeStep), collisionDetection(world), constraintSolver(world),
isDeactivationActive(DEACTIVATION_ENABLED) {
assert(world);
assert(timeStep > 0.0);
}

View File

@ -44,12 +44,12 @@ namespace reactphysics3d {
-------------------------------------------------------------------
*/
class PhysicsEngine {
protected :
private :
PhysicsWorld* world; // Pointer to the physics world of the physics engine
Timer timer; // Timer of the physics engine
CollisionDetection collisionDetection; // Collision detection
ConstraintSolver constraintSolver; // Constraint solver
bool isDeactivationActive; // True if the deactivation (sleeping) of inactive bodies is enabled
void updateAllBodiesMotion(); // Compute the motion of all bodies and update their positions and orientations
void updatePositionAndOrientationOfBody(RigidBody* body, const Vector3& newLinVelocity, const Vector3& newAngVelocity,
const Vector3& linearVelocityErrorCorrection, const Vector3& angularVelocityErrorCorrection); // Update the position and orientation of a body
@ -63,7 +63,6 @@ public :
void start(); // Start the physics simulation
void stop(); // Stop the physics simulation
void update(); // Update the physics simulation
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 setIsErrorCorrectionActive(bool isErrorCorrectionActive); // Set the isErrorCorrectionActive value
@ -78,12 +77,7 @@ inline void PhysicsEngine::start() {
inline void PhysicsEngine::stop() {
timer.stop();
}
// Set the penetration factor for the constraint solver
inline void PhysicsEngine::setPenetrationFactor(decimal factor) {
constraintSolver.setPenetrationFactor(factor);
}
}
// Set the number of iterations of the LCP solver
inline void PhysicsEngine::setNbLCPIterations(uint nbIterations) {

View File

@ -63,8 +63,8 @@ class Transform {
const Transform& newTransform,
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
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
};
// Return the position of the transform