Fix issue in the collision detection for compound shapes

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

View File

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

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
mRigidBody->addCollisionShape(sphereCollisionShape, massSphere, transformSphereShape1);
mRigidBody->addCollisionShape(sphereCollisionShape, massSphere, transformSphereShape2);
//mRigidBody->addCollisionShape(cylinderCollisionShape, massCylinder, transformCylinderShape);
mRigidBody->addCollisionShape(cylinderCollisionShape, massCylinder, transformCylinderShape);
}
// Destructor

View File

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

View File

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

View File

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

View File

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

View File

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

View File

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

View File

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

View File

@ -133,7 +133,7 @@ inline void OverlappingPair::addContact(ContactPoint* contact) {
// Update the contact manifold
inline void OverlappingPair::update() {
mContactManifold.update(mShape1->getBody()->getTransform() *mShape1->getLocalToBodyTransform(),
mContactManifold.update(mShape1->getBody()->getTransform() * mShape1->getLocalToBodyTransform(),
mShape2->getBody()->getTransform() *mShape2->getLocalToBodyTransform());
}