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

Fix issue in the collision detection for compound shapes

Browse Source
This commit is contained in:
Daniel Chappuis 2014-06-10 23:37:11 +02:00
parent cbeeec21f3
commit bc4de62e75
10 changed files with 57 additions and 46 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
examples/collisionshapes/Scene.h
View File

@ -40,13 +40,13 @@
#include "../common/Viewer.h" #include "../common/Viewer.h"
// Constants // Constants
const int NB_BOXES = 3; const int NB_BOXES = 4;
const int NB_SPHERES = 2; const int NB_SPHERES = 2;
const int NB_CONES = 0; const int NB_CONES = 3;
const int NB_CYLINDERS = 0; const int NB_CYLINDERS = 1;
const int NB_CAPSULES = 0; const int NB_CAPSULES = 1;
const int NB_MESHES = 0; const int NB_MESHES = 2;
const int NB_COMPOUND_SHAPES = 2; const int NB_COMPOUND_SHAPES = 3;
const openglframework::Vector3 BOX_SIZE(2, 2, 2); const openglframework::Vector3 BOX_SIZE(2, 2, 2);
const float SPHERE_RADIUS = 1.5f; const float SPHERE_RADIUS = 1.5f;
const float CONE_RADIUS = 2.0f; const float CONE_RADIUS = 2.0f;

2
examples/common/Dumbbell.cpp
View File

@ -79,7 +79,7 @@ Dumbbell::Dumbbell(const openglframework::Vector3 &position,
// Add the three collision shapes to the body and specify the mass and transform of the shapes // Add the three collision shapes to the body and specify the mass and transform of the shapes
mRigidBody->addCollisionShape(sphereCollisionShape, massSphere, transformSphereShape1); mRigidBody->addCollisionShape(sphereCollisionShape, massSphere, transformSphereShape1);
mRigidBody->addCollisionShape(sphereCollisionShape, massSphere, transformSphereShape2); mRigidBody->addCollisionShape(sphereCollisionShape, massSphere, transformSphereShape2);
//mRigidBody->addCollisionShape(cylinderCollisionShape, massCylinder, transformCylinderShape); mRigidBody->addCollisionShape(cylinderCollisionShape, massCylinder, transformCylinderShape);
} }
// Destructor // Destructor

2
src/collision/CollisionDetection.cpp
View File

@ -142,11 +142,13 @@ void CollisionDetection::computeNarrowPhase() {
if (narrowPhaseAlgorithm.testCollision(shape1, shape2, contactInfo)) { if (narrowPhaseAlgorithm.testCollision(shape1, shape2, contactInfo)) {
assert(contactInfo != NULL); assert(contactInfo != NULL);
/*
// Set the bodies of the contact // Set the bodies of the contact
contactInfo->body1 = dynamic_cast<RigidBody*>(body1); contactInfo->body1 = dynamic_cast<RigidBody*>(body1);
contactInfo->body2 = dynamic_cast<RigidBody*>(body2); contactInfo->body2 = dynamic_cast<RigidBody*>(body2);
assert(contactInfo->body1 != NULL); assert(contactInfo->body1 != NULL);
assert(contactInfo->body2 != NULL); assert(contactInfo->body2 != NULL);
*/
// Create a new contact // Create a new contact
createContact(pair, contactInfo); createContact(pair, contactInfo);

6
src/collision/narrowphase/EPA/EPAAlgorithm.cpp
View File

@ -400,9 +400,9 @@ bool EPAAlgorithm::computePenetrationDepthAndContactPoints(const Simplex& simple
assert(penetrationDepth > 0.0); assert(penetrationDepth > 0.0);
// Create the contact info object // Create the contact info object
contactInfo = new (mMemoryAllocator.allocate(sizeof(ContactPointInfo))) ContactPointInfo(normal, contactInfo = new (mMemoryAllocator.allocate(sizeof(ContactPointInfo)))
penetrationDepth, ContactPointInfo(collisionShape1, collisionShape2, normal,
pALocal, pBLocal); penetrationDepth, pALocal, pBLocal);
return true; return true;
} }

24
src/collision/narrowphase/GJK/GJKAlgorithm.cpp
View File

@ -140,9 +140,9 @@ bool GJKAlgorithm::testCollision(ProxyShape* collisionShape1, ProxyShape* collis
if (penetrationDepth <= 0.0) return false; if (penetrationDepth <= 0.0) return false;
// Create the contact info object // Create the contact info object
contactInfo = new (mMemoryAllocator.allocate(sizeof(ContactPointInfo))) ContactPointInfo(normal, contactInfo = new (mMemoryAllocator.allocate(sizeof(ContactPointInfo)))
penetrationDepth, ContactPointInfo(collisionShape1, collisionShape2, normal,
pA, pB); penetrationDepth, pA, pB);
// There is an intersection, therefore we return true // There is an intersection, therefore we return true
return true; return true;
@ -172,9 +172,9 @@ bool GJKAlgorithm::testCollision(ProxyShape* collisionShape1, ProxyShape* collis
if (penetrationDepth <= 0.0) return false; if (penetrationDepth <= 0.0) return false;
// Create the contact info object // Create the contact info object
contactInfo = new (mMemoryAllocator.allocate(sizeof(ContactPointInfo))) ContactPointInfo(normal, contactInfo = new (mMemoryAllocator.allocate(sizeof(ContactPointInfo)))
penetrationDepth, ContactPointInfo(collisionShape1, collisionShape2, normal,
pA, pB); penetrationDepth, pA, pB);
// There is an intersection, therefore we return true // There is an intersection, therefore we return true
return true; return true;
@ -202,9 +202,9 @@ bool GJKAlgorithm::testCollision(ProxyShape* collisionShape1, ProxyShape* collis
if (penetrationDepth <= 0.0) return false; if (penetrationDepth <= 0.0) return false;
// Create the contact info object // Create the contact info object
contactInfo = new (mMemoryAllocator.allocate(sizeof(ContactPointInfo))) ContactPointInfo(normal, contactInfo = new (mMemoryAllocator.allocate(sizeof(ContactPointInfo)))
penetrationDepth, ContactPointInfo(collisionShape1, collisionShape2, normal,
pA, pB); penetrationDepth, pA, pB);
// There is an intersection, therefore we return true // There is an intersection, therefore we return true
return true; return true;
@ -239,9 +239,9 @@ bool GJKAlgorithm::testCollision(ProxyShape* collisionShape1, ProxyShape* collis
if (penetrationDepth <= 0.0) return false; if (penetrationDepth <= 0.0) return false;
// Create the contact info object // Create the contact info object
contactInfo = new (mMemoryAllocator.allocate(sizeof(ContactPointInfo))) ContactPointInfo(normal, contactInfo = new (mMemoryAllocator.allocate(sizeof(ContactPointInfo)))
penetrationDepth, ContactPointInfo(collisionShape1, collisionShape2, normal,
pA, pB); penetrationDepth, pA, pB);
// There is an intersection, therefore we return true // There is an intersection, therefore we return true
return true; return true;

3
src/collision/narrowphase/SphereVsSphereAlgorithm.cpp
View File

@ -75,7 +75,8 @@ bool SphereVsSphereAlgorithm::testCollision(ProxyShape* collisionShape1,
decimal penetrationDepth = sumRadius - std::sqrt(squaredDistanceBetweenCenters); decimal penetrationDepth = sumRadius - std::sqrt(squaredDistanceBetweenCenters);
// Create the contact info object // Create the contact info object
contactInfo = new (mMemoryAllocator.allocate(sizeof(ContactPointInfo))) ContactPointInfo( contactInfo = new (mMemoryAllocator.allocate(sizeof(ContactPointInfo)))
ContactPointInfo(collisionShape1, collisionShape2,
vectorBetweenCenters.getUnit(), penetrationDepth, vectorBetweenCenters.getUnit(), penetrationDepth,
intersectionOnBody1, intersectionOnBody2); intersectionOnBody1, intersectionOnBody2);

10
src/constraint/ContactPoint.cpp
View File

@ -31,13 +31,17 @@ using namespace std;
// Constructor // Constructor
ContactPoint::ContactPoint(const ContactPointInfo& contactInfo) ContactPoint::ContactPoint(const ContactPointInfo& contactInfo)
: mBody1(contactInfo.body1), mBody2(contactInfo.body2), : mBody1(contactInfo.shape1->getBody()), mBody2(contactInfo.shape2->getBody()),
mNormal(contactInfo.normal), mNormal(contactInfo.normal),
mPenetrationDepth(contactInfo.penetrationDepth), mPenetrationDepth(contactInfo.penetrationDepth),
mLocalPointOnBody1(contactInfo.localPoint1), mLocalPointOnBody1(contactInfo.localPoint1),
mLocalPointOnBody2(contactInfo.localPoint2), mLocalPointOnBody2(contactInfo.localPoint2),
mWorldPointOnBody1(contactInfo.body1->getTransform() * contactInfo.localPoint1), mWorldPointOnBody1(contactInfo.shape1->getBody()->getTransform() *
mWorldPointOnBody2(contactInfo.body2->getTransform() * contactInfo.localPoint2), contactInfo.shape1->getLocalToBodyTransform() *
contactInfo.localPoint1),
mWorldPointOnBody2(contactInfo.shape2->getBody()->getTransform() *
contactInfo.shape2->getLocalToBodyTransform() *
contactInfo.localPoint2),
mIsRestingContact(false) { mIsRestingContact(false) {
mFrictionVectors[0] = Vector3(0, 0, 0); mFrictionVectors[0] = Vector3(0, 0, 0);

32
src/constraint/ContactPoint.h
View File

@ -27,7 +27,7 @@
#define REACTPHYSICS3D_CONTACT_POINT_H #define REACTPHYSICS3D_CONTACT_POINT_H
// Libraries // Libraries
#include "../body/RigidBody.h" #include "../body/CollisionBody.h"
#include "../configuration.h" #include "../configuration.h"
#include "../mathematics/mathematics.h" #include "../mathematics/mathematics.h"
#include "../configuration.h" #include "../configuration.h"
@ -58,11 +58,11 @@ struct ContactPointInfo {
// -------------------- Attributes -------------------- // // -------------------- Attributes -------------------- //
/// First rigid body of the constraint /// First proxy collision shape of the contact
RigidBody* body1; ProxyShape* shape1;
/// Second rigid body of the constraint /// Second proxy collision shape of the contact
RigidBody* body2; ProxyShape* shape2;
/// Normal vector the the collision contact in world space /// Normal vector the the collision contact in world space
const Vector3 normal; const Vector3 normal;
@ -79,10 +79,12 @@ struct ContactPointInfo {
// -------------------- Methods -------------------- // // -------------------- Methods -------------------- //
/// Constructor /// Constructor
ContactPointInfo(const Vector3& normal, decimal penetrationDepth, ContactPointInfo(ProxyShape* proxyShape1, ProxyShape* proxyShape2, const Vector3& normal,
const Vector3& localPoint1, const Vector3& localPoint2) decimal penetrationDepth, const Vector3& localPoint1,
: normal(normal), penetrationDepth(penetrationDepth), const Vector3& localPoint2)
localPoint1(localPoint1), localPoint2(localPoint2) { : shape1(proxyShape1), shape2(proxyShape2), normal(normal),
penetrationDepth(penetrationDepth), localPoint1(localPoint1),
localPoint2(localPoint2) {
} }
}; };
@ -99,10 +101,10 @@ class ContactPoint {
// -------------------- Attributes -------------------- // // -------------------- Attributes -------------------- //
/// First rigid body of the contact /// First rigid body of the contact
RigidBody* mBody1; CollisionBody* mBody1;
/// Second rigid body of the contact /// Second rigid body of the contact
RigidBody* mBody2; CollisionBody* mBody2;
/// Normal vector of the contact (From body1 toward body2) in world space /// Normal vector of the contact (From body1 toward body2) in world space
const Vector3 mNormal; const Vector3 mNormal;
@ -156,10 +158,10 @@ class ContactPoint {
~ContactPoint(); ~ContactPoint();
/// Return the reference to the body 1 /// Return the reference to the body 1
RigidBody* const getBody1() const; CollisionBody* const getBody1() const;
/// Return the reference to the body 2 /// Return the reference to the body 2
RigidBody* const getBody2() const; CollisionBody* const getBody2() const;
/// Return the normal vector of the contact /// Return the normal vector of the contact
Vector3 getNormal() const; Vector3 getNormal() const;
@ -229,12 +231,12 @@ class ContactPoint {
}; };
// Return the reference to the body 1 // Return the reference to the body 1
inline RigidBody* const ContactPoint::getBody1() const { inline CollisionBody* const ContactPoint::getBody1() const {
return mBody1; return mBody1;
} }
// Return the reference to the body 2 // Return the reference to the body 2
inline RigidBody* const ContactPoint::getBody2() const { inline CollisionBody* const ContactPoint::getBody2() const {
return mBody2; return mBody2;
} }

6
src/engine/ContactSolver.cpp
View File

@ -83,8 +83,10 @@ void ContactSolver::initializeForIsland(decimal dt, Island* island) {
assert(externalManifold->getNbContactPoints() > 0); assert(externalManifold->getNbContactPoints() > 0);
// Get the two bodies of the contact // Get the two bodies of the contact
RigidBody* body1 = externalManifold->getContactPoint(0)->getBody1(); RigidBody* body1 = dynamic_cast<RigidBody*>(externalManifold->getContactPoint(0)->getBody1());
RigidBody* body2 = externalManifold->getContactPoint(0)->getBody2(); RigidBody* body2 = dynamic_cast<RigidBody*>(externalManifold->getContactPoint(0)->getBody2());
assert(body1 != NULL);
assert(body2 != NULL);
// Get the position of the two bodies // Get the position of the two bodies
const Vector3& x1 = body1->mCenterOfMassWorld; const Vector3& x1 = body1->mCenterOfMassWorld;