diff --git a/testbed/scenes/collisiondetection/CollisionDetectionScene.cpp b/testbed/scenes/collisiondetection/CollisionDetectionScene.cpp index 3d692501..8c0196b3 100644 --- a/testbed/scenes/collisiondetection/CollisionDetectionScene.cpp +++ b/testbed/scenes/collisiondetection/CollisionDetectionScene.cpp @@ -82,6 +82,17 @@ CollisionDetectionScene::CollisionDetectionScene(const std::string& name) mCapsule1->setColor(mGreyColorDemo); mCapsule1->setSleepingColor(mRedColorDemo); + // ---------- Capsule 2 ---------- // + openglframework::Vector3 position4(-4, 0, 0); + + // Create a cylinder and a corresponding collision body in the dynamics world + mCapsule2 = new Capsule(CAPSULE_RADIUS, CAPSULE_HEIGHT, position4, mCollisionWorld, mMeshFolderPath); + mAllShapes.push_back(mCapsule2); + + // Set the color + mCapsule2->setColor(mGreyColorDemo); + mCapsule2->setSleepingColor(mRedColorDemo); + // ---------- Cone ---------- // //openglframework::Vector3 position4(0, 0, 0); @@ -163,6 +174,12 @@ CollisionDetectionScene::~CollisionDetectionScene() { mCollisionWorld->destroyCollisionBody(mSphere2->getCollisionBody()); delete mSphere2; + mCollisionWorld->destroyCollisionBody(mCapsule1->getCollisionBody()); + delete mCapsule1; + + mCollisionWorld->destroyCollisionBody(mCapsule2->getCollisionBody()); + delete mCapsule2; + /* // Destroy the corresponding rigid body from the dynamics world mCollisionWorld->destroyCollisionBody(mCone->getCollisionBody()); @@ -285,6 +302,7 @@ void CollisionDetectionScene::renderSinglePass(openglframework::Shader& shader, if (mSphere1->getCollisionBody()->isActive()) mSphere1->render(shader, worldToCameraMatrix, mIsWireframeEnabled); if (mSphere2->getCollisionBody()->isActive()) mSphere2->render(shader, worldToCameraMatrix, mIsWireframeEnabled); if (mCapsule1->getCollisionBody()->isActive()) mCapsule1->render(shader, worldToCameraMatrix, mIsWireframeEnabled); + if (mCapsule2->getCollisionBody()->isActive()) mCapsule2->render(shader, worldToCameraMatrix, mIsWireframeEnabled); /* if (mBox->getCollisionBody()->isActive()) mBox->render(shader, worldToCameraMatrix); diff --git a/testbed/scenes/collisiondetection/CollisionDetectionScene.h b/testbed/scenes/collisiondetection/CollisionDetectionScene.h index c2aec0cd..2c33b3ac 100644 --- a/testbed/scenes/collisiondetection/CollisionDetectionScene.h +++ b/testbed/scenes/collisiondetection/CollisionDetectionScene.h @@ -51,8 +51,8 @@ const float CONE_RADIUS = 3.0f; const float CONE_HEIGHT = 5.0f; const float CYLINDER_RADIUS = 3.0f; const float CYLINDER_HEIGHT = 5.0f; -const float CAPSULE_RADIUS = 3.0f; -const float CAPSULE_HEIGHT = 5.0f; +const float CAPSULE_RADIUS = 1.0f; +const float CAPSULE_HEIGHT = 1.0f; const float DUMBBELL_HEIGHT = 5.0f; const int NB_RAYS = 100; const float RAY_LENGTH = 30.0f; @@ -83,21 +83,29 @@ class ContactManager : public rp3d::CollisionCallback { /// This method will be called for each reported contact point virtual void notifyContact(const CollisionCallbackInfo& collisionCallbackInfo) override { - rp3d::Vector3 point1 = collisionCallbackInfo.contactPoint.localPoint1; - point1 = collisionCallbackInfo.proxyShape1->getLocalToWorldTransform() * point1; - openglframework::Vector3 position1(point1.x, point1.y, point1.z); - mContactPoints.push_back(ContactPoint(position1)); + // For each contact point + rp3d::ContactPointInfo* contactPointInfo = collisionCallbackInfo.contactManifold.getFirstContactPointInfo(); + while (contactPointInfo != nullptr) { - rp3d::Vector3 point2 = collisionCallbackInfo.contactPoint.localPoint2; - point2 = collisionCallbackInfo.proxyShape2->getLocalToWorldTransform() * point2; - openglframework::Vector3 position2(point2.x, point2.y, point2.z); - mContactPoints.push_back(ContactPoint(position2)); + rp3d::Vector3 point1 = contactPointInfo->localPoint1; + point1 = collisionCallbackInfo.proxyShape1->getLocalToWorldTransform() * point1; + openglframework::Vector3 position1(point1.x, point1.y, point1.z); + mContactPoints.push_back(ContactPoint(position1)); - // Create a line to display the normal at hit point - rp3d::Vector3 n = collisionCallbackInfo.contactPoint.normal; - openglframework::Vector3 normal(n.x, n.y, n.z); - Line* normalLine = new Line(position1, position1 + normal); - mNormals.push_back(normalLine); + + rp3d::Vector3 point2 = contactPointInfo->localPoint2; + point2 = collisionCallbackInfo.proxyShape2->getLocalToWorldTransform() * point2; + openglframework::Vector3 position2(point2.x, point2.y, point2.z); + mContactPoints.push_back(ContactPoint(position2)); + + // Create a line to display the normal at hit point + rp3d::Vector3 n = contactPointInfo->normal; + openglframework::Vector3 normal(n.x, n.y, n.z); + Line* normalLine = new Line(position1, position1 + normal); + mNormals.push_back(normalLine); + + contactPointInfo = contactPointInfo->next; + } } void resetPoints() {