atria-soft/ephysics
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

[DEV] continue integration new ETK

Browse Source
This commit is contained in:
Edouard DUPIN 2017-09-29 00:37:47 +02:00
parent d22ad84b61
commit d7470ad277
57 changed files with 825 additions and 1632 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
ephysics/body/Body.hpp
View File

@ -5,8 +5,9 @@
*/ */
#pragma once #pragma once
#include <stdexcept> extern "C" {
#include <cassert> #include <assert.h>
}
#include <ephysics/configuration.hpp> #include <ephysics/configuration.hpp>
namespace ephysics { namespace ephysics {

2
ephysics/body/CollisionBody.hpp
View File

@ -6,8 +6,6 @@
#pragma once #pragma once
// Libraries // Libraries
#include <stdexcept>
#include <cassert>
#include <ephysics/body/Body.hpp> #include <ephysics/body/Body.hpp>
#include <etk/math/Transform3D.hpp> #include <etk/math/Transform3D.hpp>
#include <ephysics/collision/shapes/AABB.hpp> #include <ephysics/collision/shapes/AABB.hpp>

1
ephysics/body/RigidBody.hpp
View File

@ -5,7 +5,6 @@
*/ */
#pragma once #pragma once
#include <cassert>
#include <ephysics/body/CollisionBody.hpp> #include <ephysics/body/CollisionBody.hpp>
#include <ephysics/engine/Material.hpp> #include <ephysics/engine/Material.hpp>
#include <ephysics/mathematics/mathematics.hpp> #include <ephysics/mathematics/mathematics.hpp>

309
ephysics/collision/CollisionDetection.cpp
View File

@ -11,106 +11,85 @@
#include <ephysics/collision/shapes/BoxShape.hpp> #include <ephysics/collision/shapes/BoxShape.hpp>
#include <ephysics/body/RigidBody.hpp> #include <ephysics/body/RigidBody.hpp>
#include <ephysics/configuration.hpp> #include <ephysics/configuration.hpp>
#include <cassert>
#include <complex>
#include <set>
#include <utility>
#include <utility>
// We want to use the ReactPhysics3D namespace // We want to use the ReactPhysics3D namespace
using namespace ephysics; using namespace ephysics;
using namespace std; using namespace std;
// Constructor // Constructor
CollisionDetection::CollisionDetection(CollisionWorld* world, MemoryAllocator& memoryAllocator) CollisionDetection::CollisionDetection(CollisionWorld* _world, MemoryAllocator& _memoryAllocator):
: m_memoryAllocator(memoryAllocator), m_memoryAllocator(_memoryAllocator),
m_world(world), m_broadPhaseAlgorithm(*this), m_world(_world),
m_isCollisionShapesAdded(false) { m_broadPhaseAlgorithm(*this),
m_isCollisionShapesAdded(false) {
// Set the default collision dispatch configuration // Set the default collision dispatch configuration
setCollisionDispatch(&m_defaultCollisionDispatch); setCollisionDispatch(&m_defaultCollisionDispatch);
// Fill-in the collision detection matrix with algorithms // Fill-in the collision detection matrix with algorithms
fillInCollisionMatrix(); fillInCollisionMatrix();
} }
// Destructor
CollisionDetection::~CollisionDetection() { CollisionDetection::~CollisionDetection() {
} }
// Compute the collision detection
void CollisionDetection::computeCollisionDetection() { void CollisionDetection::computeCollisionDetection() {
PROFILE("CollisionDetection::computeCollisionDetection()"); PROFILE("CollisionDetection::computeCollisionDetection()");
// Compute the broad-phase collision detection // Compute the broad-phase collision detection
computeBroadPhase(); computeBroadPhase();
// Compute the narrow-phase collision detection // Compute the narrow-phase collision detection
computeNarrowPhase(); computeNarrowPhase();
} }
// Compute the collision detection
void CollisionDetection::testCollisionBetweenShapes(CollisionCallback* callback, void CollisionDetection::testCollisionBetweenShapes(CollisionCallback* callback,
const std::set<uint32_t>& shapes1, const etk::Set<uint32_t>& shapes1,
const std::set<uint32_t>& shapes2) { const etk::Set<uint32_t>& shapes2) {
// Compute the broad-phase collision detection // Compute the broad-phase collision detection
computeBroadPhase(); computeBroadPhase();
// Delete all the contact points in the currently overlapping pairs // Delete all the contact points in the currently overlapping pairs
clearContactPoints(); clearContactPoints();
// Compute the narrow-phase collision detection among given sets of shapes // Compute the narrow-phase collision detection among given sets of shapes
computeNarrowPhaseBetweenShapes(callback, shapes1, shapes2); computeNarrowPhaseBetweenShapes(callback, shapes1, shapes2);
} }
// Report collision between two sets of shapes
void CollisionDetection::reportCollisionBetweenShapes(CollisionCallback* callback, void CollisionDetection::reportCollisionBetweenShapes(CollisionCallback* callback,
const std::set<uint32_t>& shapes1, const etk::Set<uint32_t>& shapes1,
const std::set<uint32_t>& shapes2) { const etk::Set<uint32_t>& shapes2) {
// For each possible collision pair of bodies // For each possible collision pair of bodies
map<overlappingpairid, OverlappingPair*>::iterator it; etk::Map<overlappingpairid, OverlappingPair*>::Iterator it;
for (it = m_overlappingPairs.begin(); it != m_overlappingPairs.end(); ++it) { for (it = m_overlappingPairs.begin(); it != m_overlappingPairs.end(); ++it) {
OverlappingPair* pair = it->second; OverlappingPair* pair = it->second;
const ProxyShape* shape1 = pair->getShape1(); const ProxyShape* shape1 = pair->getShape1();
const ProxyShape* shape2 = pair->getShape2(); const ProxyShape* shape2 = pair->getShape2();
assert(shape1->m_broadPhaseID != shape2->m_broadPhaseID); assert(shape1->m_broadPhaseID != shape2->m_broadPhaseID);
// If both shapes1 and shapes2 sets are non-empty, we check that // If both shapes1 and shapes2 sets are non-empty, we check that
// shape1 is among on set and shape2 is among the other one // shape1 is among on set and shape2 is among the other one
if (!shapes1.empty() && !shapes2.empty() && if ( !shapes1.empty()
(shapes1.count(shape1->m_broadPhaseID) == 0 || shapes2.count(shape2->m_broadPhaseID) == 0) && && !shapes2.empty()
(shapes1.count(shape2->m_broadPhaseID) == 0 || shapes2.count(shape1->m_broadPhaseID) == 0)) { && ( shapes1.count(shape1->m_broadPhaseID) == 0
|| shapes2.count(shape2->m_broadPhaseID) == 0 )
&& ( shapes1.count(shape2->m_broadPhaseID) == 0
|| shapes2.count(shape1->m_broadPhaseID) == 0 ) ) {
continue; continue;
} }
if (!shapes1.empty() && shapes2.empty() && if ( !shapes1.empty()
shapes1.count(shape1->m_broadPhaseID) == 0 && shapes1.count(shape2->m_broadPhaseID) == 0) && shapes2.empty()
{ && shapes1.count(shape1->m_broadPhaseID) == 0
&& shapes1.count(shape2->m_broadPhaseID) == 0) {
continue; continue;
} }
if (!shapes2.empty() && shapes1.empty() && if ( !shapes2.empty()
shapes2.count(shape1->m_broadPhaseID) == 0 && shapes2.count(shape2->m_broadPhaseID) == 0) && shapes1.empty()
{ && shapes2.count(shape1->m_broadPhaseID) == 0
&& shapes2.count(shape2->m_broadPhaseID) == 0) {
continue; continue;
} }
// For each contact manifold set of the overlapping pair // For each contact manifold set of the overlapping pair
const ContactManifoldSet& manifoldSet = pair->getContactManifoldSet(); const ContactManifoldSet& manifoldSet = pair->getContactManifoldSet();
for (int32_t j=0; j<manifoldSet.getNbContactManifolds(); j++) { for (int32_t j=0; j<manifoldSet.getNbContactManifolds(); j++) {
const ContactManifold* manifold = manifoldSet.getContactManifold(j); const ContactManifold* manifold = manifoldSet.getContactManifold(j);
// For each contact manifold of the manifold set // For each contact manifold of the manifold set
for (uint32_t i=0; i<manifold->getNbContactPoints(); i++) { for (uint32_t i=0; i<manifold->getNbContactPoints(); i++) {
ContactPoint* contactPoint = manifold->getContactPoint(i); ContactPoint* contactPoint = manifold->getContactPoint(i);
// Create the contact info object for the contact // Create the contact info object for the contact
ContactPointInfo contactInfo(manifold->getShape1(), manifold->getShape2(), ContactPointInfo contactInfo(manifold->getShape1(), manifold->getShape2(),
manifold->getShape1()->getCollisionShape(), manifold->getShape1()->getCollisionShape(),
@ -119,22 +98,19 @@ void CollisionDetection::reportCollisionBetweenShapes(CollisionCallback* callbac
contactPoint->getPenetrationDepth(), contactPoint->getPenetrationDepth(),
contactPoint->getLocalPointOnBody1(), contactPoint->getLocalPointOnBody1(),
contactPoint->getLocalPointOnBody2()); contactPoint->getLocalPointOnBody2());
// Notify the collision callback about this new contact // Notify the collision callback about this new contact
if (callback != NULL) callback->notifyContact(contactInfo); if (callback != nullptr) {
callback->notifyContact(contactInfo);
}
} }
} }
} }
} }
// Compute the broad-phase collision detection
void CollisionDetection::computeBroadPhase() { void CollisionDetection::computeBroadPhase() {
PROFILE("CollisionDetection::computeBroadPhase()"); PROFILE("CollisionDetection::computeBroadPhase()");
// If new collision shapes have been added to bodies // If new collision shapes have been added to bodies
if (m_isCollisionShapesAdded) { if (m_isCollisionShapesAdded) {
// Ask the broad-phase to recompute the overlapping pairs of collision // Ask the broad-phase to recompute the overlapping pairs of collision
// shapes. This call can only add new overlapping pairs in the collision // shapes. This call can only add new overlapping pairs in the collision
// detection. // detection.
@ -142,73 +118,59 @@ void CollisionDetection::computeBroadPhase() {
} }
} }
// Compute the narrow-phase collision detection
void CollisionDetection::computeNarrowPhase() { void CollisionDetection::computeNarrowPhase() {
PROFILE("CollisionDetection::computeNarrowPhase()"); PROFILE("CollisionDetection::computeNarrowPhase()");
// Clear the set of overlapping pairs in narrow-phase contact // Clear the set of overlapping pairs in narrow-phase contact
m_contactOverlappingPairs.clear(); m_contactOverlappingPairs.clear();
// For each possible collision pair of bodies // For each possible collision pair of bodies
map<overlappingpairid, OverlappingPair*>::iterator it; etk::Map<overlappingpairid, OverlappingPair*>::Iterator it;
for (it = m_overlappingPairs.begin(); it != m_overlappingPairs.end(); ) { for (it = m_overlappingPairs.begin(); it != m_overlappingPairs.end(); ) {
OverlappingPair* pair = it->second; OverlappingPair* pair = it->second;
ProxyShape* shape1 = pair->getShape1(); ProxyShape* shape1 = pair->getShape1();
ProxyShape* shape2 = pair->getShape2(); ProxyShape* shape2 = pair->getShape2();
assert(shape1->m_broadPhaseID != shape2->m_broadPhaseID); assert(shape1->m_broadPhaseID != shape2->m_broadPhaseID);
// Check if the collision filtering allows collision between the two shapes and // Check if the collision filtering allows collision between the two shapes and
// that the two shapes are still overlapping. Otherwise, we destroy the // that the two shapes are still overlapping. Otherwise, we destroy the
// overlapping pair // overlapping pair
if (((shape1->getCollideWithMaskBits() & shape2->getCollisionCategoryBits()) == 0 || if (((shape1->getCollideWithMaskBits() & shape2->getCollisionCategoryBits()) == 0 ||
(shape1->getCollisionCategoryBits() & shape2->getCollideWithMaskBits()) == 0) || (shape1->getCollisionCategoryBits() & shape2->getCollideWithMaskBits()) == 0) ||
!m_broadPhaseAlgorithm.testOverlappingShapes(shape1, shape2)) { !m_broadPhaseAlgorithm.testOverlappingShapes(shape1, shape2)) {
etk::Map<overlappingpairid, OverlappingPair*>::Iterator itToRemove = it;
etk::Map<overlappingpairid, OverlappingPair*>::iterator itToRemove = it;
++it; ++it;
// TODO : Remove all the contact manifold of the overlapping pair from the contact manifolds list of the two bodies involved // TODO : Remove all the contact manifold of the overlapping pair from the contact manifolds list of the two bodies involved
// Destroy the overlapping pair // Destroy the overlapping pair
itToRemove->second->~OverlappingPair(); itToRemove->second->~OverlappingPair();
m_world->m_memoryAllocator.release(itToRemove->second, sizeof(OverlappingPair)); m_world->m_memoryAllocator.release(itToRemove->second, sizeof(OverlappingPair));
m_overlappingPairs.erase(itToRemove); m_overlappingPairs.erase(itToRemove);
continue; continue;
} } else {
else {
++it; ++it;
} }
CollisionBody* const body1 = shape1->getBody(); CollisionBody* const body1 = shape1->getBody();
CollisionBody* const body2 = shape2->getBody(); CollisionBody* const body2 = shape2->getBody();
// Update the contact cache of the overlapping pair // Update the contact cache of the overlapping pair
pair->update(); pair->update();
// Check that at least one body is awake and not static // Check that at least one body is awake and not static
bool isBody1Active = !body1->isSleeping() && body1->getType() != STATIC; bool isBody1Active = !body1->isSleeping() && body1->getType() != STATIC;
bool isBody2Active = !body2->isSleeping() && body2->getType() != STATIC; bool isBody2Active = !body2->isSleeping() && body2->getType() != STATIC;
if (!isBody1Active && !isBody2Active) continue; if (!isBody1Active && !isBody2Active) {
continue;
}
// Check if the bodies are in the set of bodies that cannot collide between each other // Check if the bodies are in the set of bodies that cannot collide between each other
bodyindexpair bodiesIndex = OverlappingPair::computeBodiesIndexPair(body1, body2); bodyindexpair bodiesIndex = OverlappingPair::computeBodiesIndexPair(body1, body2);
if (m_noCollisionPairs.count(bodiesIndex) > 0) continue; if (m_noCollisionPairs.count(bodiesIndex) > 0) {
continue;
}
// Select the narrow phase algorithm to use according to the two collision shapes // Select the narrow phase algorithm to use according to the two collision shapes
const CollisionShapeType shape1Type = shape1->getCollisionShape()->getType(); const CollisionShapeType shape1Type = shape1->getCollisionShape()->getType();
const CollisionShapeType shape2Type = shape2->getCollisionShape()->getType(); const CollisionShapeType shape2Type = shape2->getCollisionShape()->getType();
NarrowPhaseAlgorithm* narrowPhaseAlgorithm = m_collisionMatrix[shape1Type][shape2Type]; NarrowPhaseAlgorithm* narrowPhaseAlgorithm = m_collisionMatrix[shape1Type][shape2Type];
// If there is no collision algorithm between those two kinds of shapes // If there is no collision algorithm between those two kinds of shapes
if (narrowPhaseAlgorithm == NULL) continue; if (narrowPhaseAlgorithm == nullptr) {
continue;
}
// Notify the narrow-phase algorithm about the overlapping pair we are going to test // Notify the narrow-phase algorithm about the overlapping pair we are going to test
narrowPhaseAlgorithm->setCurrentOverlappingPair(pair); narrowPhaseAlgorithm->setCurrentOverlappingPair(pair);
// Create the CollisionShapeInfo objects // Create the CollisionShapeInfo objects
CollisionShapeInfo shape1Info(shape1, shape1->getCollisionShape(), shape1->getLocalToWorldTransform(), CollisionShapeInfo shape1Info(shape1, shape1->getCollisionShape(), shape1->getLocalToWorldTransform(),
pair, shape1->getCachedCollisionData()); pair, shape1->getCachedCollisionData());
@ -220,166 +182,140 @@ void CollisionDetection::computeNarrowPhase() {
// notifyContact() callback method will be called. // notifyContact() callback method will be called.
narrowPhaseAlgorithm->testCollision(shape1Info, shape2Info, this); narrowPhaseAlgorithm->testCollision(shape1Info, shape2Info, this);
} }
// Add all the contact manifolds (between colliding bodies) to the bodies // Add all the contact manifolds (between colliding bodies) to the bodies
addAllContactManifoldsToBodies(); addAllContactManifoldsToBodies();
} }
// Compute the narrow-phase collision detection
void CollisionDetection::computeNarrowPhaseBetweenShapes(CollisionCallback* callback, void CollisionDetection::computeNarrowPhaseBetweenShapes(CollisionCallback* callback,
const std::set<uint32_t>& shapes1, const etk::Set<uint32_t>& shapes1,
const std::set<uint32_t>& shapes2) { const etk::Set<uint32_t>& shapes2) {
m_contactOverlappingPairs.clear(); m_contactOverlappingPairs.clear();
// For each possible collision pair of bodies // For each possible collision pair of bodies
map<overlappingpairid, OverlappingPair*>::iterator it; etk::Map<overlappingpairid, OverlappingPair*>::Iterator it;
for (it = m_overlappingPairs.begin(); it != m_overlappingPairs.end(); ) { for (it = m_overlappingPairs.begin(); it != m_overlappingPairs.end(); ) {
OverlappingPair* pair = it->second; OverlappingPair* pair = it->second;
ProxyShape* shape1 = pair->getShape1(); ProxyShape* shape1 = pair->getShape1();
ProxyShape* shape2 = pair->getShape2(); ProxyShape* shape2 = pair->getShape2();
assert(shape1->m_broadPhaseID != shape2->m_broadPhaseID); assert(shape1->m_broadPhaseID != shape2->m_broadPhaseID);
// If both shapes1 and shapes2 sets are non-empty, we check that // If both shapes1 and shapes2 sets are non-empty, we check that
// shape1 is among on set and shape2 is among the other one // shape1 is among on set and shape2 is among the other one
if (!shapes1.empty() && !shapes2.empty() && if ( !shapes1.empty()
(shapes1.count(shape1->m_broadPhaseID) == 0 || shapes2.count(shape2->m_broadPhaseID) == 0) && && !shapes2.empty()
(shapes1.count(shape2->m_broadPhaseID) == 0 || shapes2.count(shape1->m_broadPhaseID) == 0)) { && ( shapes1.count(shape1->m_broadPhaseID) == 0
|| shapes2.count(shape2->m_broadPhaseID) == 0 )
&& ( shapes1.count(shape2->m_broadPhaseID) == 0
|| shapes2.count(shape1->m_broadPhaseID) == 0 ) ) {
++it; ++it;
continue; continue;
} }
if (!shapes1.empty() && shapes2.empty() && if ( !shapes1.empty()
shapes1.count(shape1->m_broadPhaseID) == 0 && shapes1.count(shape2->m_broadPhaseID) == 0) && shapes2.empty()
{ && shapes1.count(shape1->m_broadPhaseID) == 0
&& shapes1.count(shape2->m_broadPhaseID) == 0) {
++it; ++it;
continue; continue;
} }
if (!shapes2.empty() && shapes1.empty() && if ( !shapes2.empty()
shapes2.count(shape1->m_broadPhaseID) == 0 && shapes2.count(shape2->m_broadPhaseID) == 0) && shapes1.empty()
{ && shapes2.count(shape1->m_broadPhaseID) == 0
&& shapes2.count(shape2->m_broadPhaseID) == 0) {
++it; ++it;
continue; continue;
} }
// Check if the collision filtering allows collision between the two shapes and // Check if the collision filtering allows collision between the two shapes and
// that the two shapes are still overlapping. Otherwise, we destroy the // that the two shapes are still overlapping. Otherwise, we destroy the
// overlapping pair // overlapping pair
if (((shape1->getCollideWithMaskBits() & shape2->getCollisionCategoryBits()) == 0 || if (((shape1->getCollideWithMaskBits() & shape2->getCollisionCategoryBits()) == 0 ||
(shape1->getCollisionCategoryBits() & shape2->getCollideWithMaskBits()) == 0) || (shape1->getCollisionCategoryBits() & shape2->getCollideWithMaskBits()) == 0) ||
!m_broadPhaseAlgorithm.testOverlappingShapes(shape1, shape2)) { !m_broadPhaseAlgorithm.testOverlappingShapes(shape1, shape2)) {
etk::Map<overlappingpairid, OverlappingPair*>::Iterator itToRemove = it;
etk::Map<overlappingpairid, OverlappingPair*>::iterator itToRemove = it;
++it; ++it;
// TODO : Remove all the contact manifold of the overlapping pair from the contact manifolds list of the two bodies involved // TODO : Remove all the contact manifold of the overlapping pair from the contact manifolds list of the two bodies involved
// Destroy the overlapping pair // Destroy the overlapping pair
itToRemove->second->~OverlappingPair(); itToRemove->second->~OverlappingPair();
m_world->m_memoryAllocator.release(itToRemove->second, sizeof(OverlappingPair)); m_world->m_memoryAllocator.release(itToRemove->second, sizeof(OverlappingPair));
m_overlappingPairs.erase(itToRemove); m_overlappingPairs.erase(itToRemove);
continue; continue;
} } else {
else {
++it; ++it;
} }
CollisionBody* const body1 = shape1->getBody(); CollisionBody* const body1 = shape1->getBody();
CollisionBody* const body2 = shape2->getBody(); CollisionBody* const body2 = shape2->getBody();
// Update the contact cache of the overlapping pair // Update the contact cache of the overlapping pair
pair->update(); pair->update();
// Check if the two bodies are allowed to collide, otherwise, we do not test for collision // Check if the two bodies are allowed to collide, otherwise, we do not test for collision
if (body1->getType() != DYNAMIC && body2->getType() != DYNAMIC) continue; if (body1->getType() != DYNAMIC && body2->getType() != DYNAMIC) {
continue;
}
bodyindexpair bodiesIndex = OverlappingPair::computeBodiesIndexPair(body1, body2); bodyindexpair bodiesIndex = OverlappingPair::computeBodiesIndexPair(body1, body2);
if (m_noCollisionPairs.count(bodiesIndex) > 0) continue; if (m_noCollisionPairs.count(bodiesIndex) > 0) {
continue;
}
// Check if the two bodies are sleeping, if so, we do no test collision between them // Check if the two bodies are sleeping, if so, we do no test collision between them
if (body1->isSleeping() && body2->isSleeping()) continue; if (body1->isSleeping() && body2->isSleeping()) {
continue;
}
// Select the narrow phase algorithm to use according to the two collision shapes // Select the narrow phase algorithm to use according to the two collision shapes
const CollisionShapeType shape1Type = shape1->getCollisionShape()->getType(); const CollisionShapeType shape1Type = shape1->getCollisionShape()->getType();
const CollisionShapeType shape2Type = shape2->getCollisionShape()->getType(); const CollisionShapeType shape2Type = shape2->getCollisionShape()->getType();
NarrowPhaseAlgorithm* narrowPhaseAlgorithm = m_collisionMatrix[shape1Type][shape2Type]; NarrowPhaseAlgorithm* narrowPhaseAlgorithm = m_collisionMatrix[shape1Type][shape2Type];
// If there is no collision algorithm between those two kinds of shapes // If there is no collision algorithm between those two kinds of shapes
if (narrowPhaseAlgorithm == NULL) continue; if (narrowPhaseAlgorithm == nullptr) {
continue;
}
// Notify the narrow-phase algorithm about the overlapping pair we are going to test // Notify the narrow-phase algorithm about the overlapping pair we are going to test
narrowPhaseAlgorithm->setCurrentOverlappingPair(pair); narrowPhaseAlgorithm->setCurrentOverlappingPair(pair);
// Create the CollisionShapeInfo objects // Create the CollisionShapeInfo objects
CollisionShapeInfo shape1Info(shape1, shape1->getCollisionShape(), shape1->getLocalToWorldTransform(), CollisionShapeInfo shape1Info(shape1, shape1->getCollisionShape(), shape1->getLocalToWorldTransform(),
pair, shape1->getCachedCollisionData()); pair, shape1->getCachedCollisionData());
CollisionShapeInfo shape2Info(shape2, shape2->getCollisionShape(), shape2->getLocalToWorldTransform(), CollisionShapeInfo shape2Info(shape2, shape2->getCollisionShape(), shape2->getLocalToWorldTransform(),
pair, shape2->getCachedCollisionData()); pair, shape2->getCachedCollisionData());
TestCollisionBetweenShapesCallback narrowPhaseCallback(callback); TestCollisionBetweenShapesCallback narrowPhaseCallback(callback);
// Use the narrow-phase collision detection algorithm to check // Use the narrow-phase collision detection algorithm to check
// if there really is a collision // if there really is a collision
narrowPhaseAlgorithm->testCollision(shape1Info, shape2Info, &narrowPhaseCallback); narrowPhaseAlgorithm->testCollision(shape1Info, shape2Info, &narrowPhaseCallback);
} }
// Add all the contact manifolds (between colliding bodies) to the bodies // Add all the contact manifolds (between colliding bodies) to the bodies
addAllContactManifoldsToBodies(); addAllContactManifoldsToBodies();
} }
// Allow the broadphase to notify the collision detection about an overlapping pair.
/// This method is called by the broad-phase collision detection algorithm
void CollisionDetection::broadPhaseNotifyOverlappingPair(ProxyShape* shape1, ProxyShape* shape2) { void CollisionDetection::broadPhaseNotifyOverlappingPair(ProxyShape* shape1, ProxyShape* shape2) {
assert(shape1->m_broadPhaseID != shape2->m_broadPhaseID); assert(shape1->m_broadPhaseID != shape2->m_broadPhaseID);
// If the two proxy collision shapes are from the same body, skip it // If the two proxy collision shapes are from the same body, skip it
if (shape1->getBody()->getID() == shape2->getBody()->getID()) return; if (shape1->getBody()->getID() == shape2->getBody()->getID()) {
return;
}
// Check if the collision filtering allows collision between the two shapes // Check if the collision filtering allows collision between the two shapes
if ((shape1->getCollideWithMaskBits() & shape2->getCollisionCategoryBits()) == 0 || if ( (shape1->getCollideWithMaskBits() & shape2->getCollisionCategoryBits()) == 0
(shape1->getCollisionCategoryBits() & shape2->getCollideWithMaskBits()) == 0) return; || (shape1->getCollisionCategoryBits() & shape2->getCollideWithMaskBits()) == 0) {
return;
}
// Compute the overlapping pair ID // Compute the overlapping pair ID
overlappingpairid pairID = OverlappingPair::computeID(shape1, shape2); overlappingpairid pairID = OverlappingPair::computeID(shape1, shape2);
// Check if the overlapping pair already exists // Check if the overlapping pair already exists
if (m_overlappingPairs.find(pairID) != m_overlappingPairs.end()) return; if (m_overlappingPairs.find(pairID) != m_overlappingPairs.end()) return;
// Compute the maximum number of contact manifolds for this pair // Compute the maximum number of contact manifolds for this pair
int32_t nbMaxManifolds = CollisionShape::computeNbMaxContactManifolds(shape1->getCollisionShape()->getType(), int32_t nbMaxManifolds = CollisionShape::computeNbMaxContactManifolds(shape1->getCollisionShape()->getType(),
shape2->getCollisionShape()->getType()); shape2->getCollisionShape()->getType());
// Create the overlapping pair and add it int32_to the set of overlapping pairs // Create the overlapping pair and add it int32_to the set of overlapping pairs
OverlappingPair* newPair = new (m_world->m_memoryAllocator.allocate(sizeof(OverlappingPair))) OverlappingPair* newPair = new (m_world->m_memoryAllocator.allocate(sizeof(OverlappingPair)))
OverlappingPair(shape1, shape2, nbMaxManifolds, m_world->m_memoryAllocator); OverlappingPair(shape1, shape2, nbMaxManifolds, m_world->m_memoryAllocator);
assert(newPair != NULL); assert(newPair != nullptr);
m_overlappingPairs.set(pairID, newPair);
#ifndef NDEBUG
etk::Pair<map<overlappingpairid, OverlappingPair*>::iterator, bool> check =
#endif
m_overlappingPairs.insert(make_pair(pairID, newPair));
assert(check.second);
// Wake up the two bodies // Wake up the two bodies
shape1->getBody()->setIsSleeping(false); shape1->getBody()->setIsSleeping(false);
shape2->getBody()->setIsSleeping(false); shape2->getBody()->setIsSleeping(false);
} }
// Remove a body from the collision detection
void CollisionDetection::removeProxyCollisionShape(ProxyShape* proxyShape) { void CollisionDetection::removeProxyCollisionShape(ProxyShape* proxyShape) {
// Remove all the overlapping pairs involving this proxy shape // Remove all the overlapping pairs involving this proxy shape
etk::Map<overlappingpairid, OverlappingPair*>::iterator it; etk::Map<overlappingpairid, OverlappingPair*>::Iterator it;
for (it = m_overlappingPairs.begin(); it != m_overlappingPairs.end(); ) { for (it = m_overlappingPairs.begin(); it != m_overlappingPairs.end(); ) {
if (it->second->getShape1()->m_broadPhaseID == proxyShape->m_broadPhaseID|| if (it->second->getShape1()->m_broadPhaseID == proxyShape->m_broadPhaseID||
it->second->getShape2()->m_broadPhaseID == proxyShape->m_broadPhaseID) { it->second->getShape2()->m_broadPhaseID == proxyShape->m_broadPhaseID) {
etk::Map<overlappingpairid, OverlappingPair*>::iterator itToRemove = it; etk::Map<overlappingpairid, OverlappingPair*>::Iterator itToRemove = it;
++it; ++it;
// TODO : Remove all the contact manifold of the overlapping pair from the contact manifolds list of the two bodies involved // TODO : Remove all the contact manifold of the overlapping pair from the contact manifolds list of the two bodies involved
// Destroy the overlapping pair // Destroy the overlapping pair
itToRemove->second->~OverlappingPair(); itToRemove->second->~OverlappingPair();
m_world->m_memoryAllocator.release(itToRemove->second, sizeof(OverlappingPair)); m_world->m_memoryAllocator.release(itToRemove->second, sizeof(OverlappingPair));
@ -389,39 +325,28 @@ void CollisionDetection::removeProxyCollisionShape(ProxyShape* proxyShape) {
++it; ++it;
} }
} }
// Remove the body from the broad-phase // Remove the body from the broad-phase
m_broadPhaseAlgorithm.removeProxyCollisionShape(proxyShape); m_broadPhaseAlgorithm.removeProxyCollisionShape(proxyShape);
} }
// Called by a narrow-phase collision algorithm when a new contact has been found
void CollisionDetection::notifyContact(OverlappingPair* overlappingPair, const ContactPointInfo& contactInfo) { void CollisionDetection::notifyContact(OverlappingPair* overlappingPair, const ContactPointInfo& contactInfo) {
// If it is the first contact since the pairs are overlapping // If it is the first contact since the pairs are overlapping
if (overlappingPair->getNbContactPoints() == 0) { if (overlappingPair->getNbContactPoints() == 0) {
// Trigger a callback event // Trigger a callback event
if (m_world->m_eventListener != NULL) m_world->m_eventListener->beginContact(contactInfo); if (m_world->m_eventListener != NULL) m_world->m_eventListener->beginContact(contactInfo);
} }
// Create a new contact // Create a new contact
createContact(overlappingPair, contactInfo); createContact(overlappingPair, contactInfo);
// Trigger a callback event for the new contact // Trigger a callback event for the new contact
if (m_world->m_eventListener != NULL) m_world->m_eventListener->newContact(contactInfo); if (m_world->m_eventListener != NULL) m_world->m_eventListener->newContact(contactInfo);
} }
// Create a new contact void CollisionDetection::createContact(OverlappingPair* overlappingPair, const ContactPointInfo& contactInfo) {
void CollisionDetection::createContact(OverlappingPair* overlappingPair,
const ContactPointInfo& contactInfo) {
// Create a new contact // Create a new contact
ContactPoint* contact = new (m_world->m_memoryAllocator.allocate(sizeof(ContactPoint))) ContactPoint* contact = new (m_world->m_memoryAllocator.allocate(sizeof(ContactPoint)))
ContactPoint(contactInfo); ContactPoint(contactInfo);
// Add the contact to the contact manifold set of the corresponding overlapping pair // Add the contact to the contact manifold set of the corresponding overlapping pair
overlappingPair->addContact(contact); overlappingPair->addContact(contact);
// Add the overlapping pair int32_to the set of pairs in contact during narrow-phase // Add the overlapping pair int32_to the set of pairs in contact during narrow-phase
overlappingpairid pairId = OverlappingPair::computeID(overlappingPair->getShape1(), overlappingpairid pairId = OverlappingPair::computeID(overlappingPair->getShape1(),
overlappingPair->getShape2()); overlappingPair->getShape2());
@ -429,34 +354,24 @@ void CollisionDetection::createContact(OverlappingPair* overlappingPair,
} }
void CollisionDetection::addAllContactManifoldsToBodies() { void CollisionDetection::addAllContactManifoldsToBodies() {
// For each overlapping pairs in contact during the narrow-phase // For each overlapping pairs in contact during the narrow-phase
etk::Map<overlappingpairid, OverlappingPair*>::iterator it; etk::Map<overlappingpairid, OverlappingPair*>::Iterator it;
for (it = m_contactOverlappingPairs.begin(); it != m_contactOverlappingPairs.end(); ++it) { for (it = m_contactOverlappingPairs.begin(); it != m_contactOverlappingPairs.end(); ++it) {
// Add all the contact manifolds of the pair int32_to the list of contact manifolds // Add all the contact manifolds of the pair int32_to the list of contact manifolds
// of the two bodies involved in the contact // of the two bodies involved in the contact
addContactManifoldToBody(it->second); addContactManifoldToBody(it->second);
} }
} }
// Add a contact manifold to the linked list of contact manifolds of the two bodies involved
// in the corresponding contact
void CollisionDetection::addContactManifoldToBody(OverlappingPair* pair) { void CollisionDetection::addContactManifoldToBody(OverlappingPair* pair) {
assert(pair != nullptr);
assert(pair != NULL);
CollisionBody* body1 = pair->getShape1()->getBody(); CollisionBody* body1 = pair->getShape1()->getBody();
CollisionBody* body2 = pair->getShape2()->getBody(); CollisionBody* body2 = pair->getShape2()->getBody();
const ContactManifoldSet& manifoldSet = pair->getContactManifoldSet(); const ContactManifoldSet& manifoldSet = pair->getContactManifoldSet();
// For each contact manifold in the set of manifolds in the pair // For each contact manifold in the set of manifolds in the pair
for (int32_t i=0; i<manifoldSet.getNbContactManifolds(); i++) { for (int32_t i=0; i<manifoldSet.getNbContactManifolds(); i++) {
ContactManifold* contactManifold = manifoldSet.getContactManifold(i); ContactManifold* contactManifold = manifoldSet.getContactManifold(i);
assert(contactManifold->getNbContactPoints() > 0); assert(contactManifold->getNbContactPoints() > 0);
// Add the contact manifold at the beginning of the linked // Add the contact manifold at the beginning of the linked
// list of contact manifolds of the first body // list of contact manifolds of the first body
void* allocatedMemory1 = m_world->m_memoryAllocator.allocate(sizeof(ContactManifoldListElement)); void* allocatedMemory1 = m_world->m_memoryAllocator.allocate(sizeof(ContactManifoldListElement));
@ -464,7 +379,6 @@ void CollisionDetection::addContactManifoldToBody(OverlappingPair* pair) {
ContactManifoldListElement(contactManifold, ContactManifoldListElement(contactManifold,
body1->m_contactManifoldsList); body1->m_contactManifoldsList);
body1->m_contactManifoldsList = listElement1; body1->m_contactManifoldsList = listElement1;
// Add the contact manifold at the beginning of the linked // Add the contact manifold at the beginning of the linked
// list of the contact manifolds of the second body // list of the contact manifolds of the second body
void* allocatedMemory2 = m_world->m_memoryAllocator.allocate(sizeof(ContactManifoldListElement)); void* allocatedMemory2 = m_world->m_memoryAllocator.allocate(sizeof(ContactManifoldListElement));
@ -475,19 +389,16 @@ void CollisionDetection::addContactManifoldToBody(OverlappingPair* pair) {
} }
} }
// Delete all the contact points in the currently overlapping pairs
void CollisionDetection::clearContactPoints() { void CollisionDetection::clearContactPoints() {
// For each overlapping pair // For each overlapping pair
etk::Map<overlappingpairid, OverlappingPair*>::iterator it; etk::Map<overlappingpairid, OverlappingPair*>::Iterator it;
for (it = m_overlappingPairs.begin(); it != m_overlappingPairs.end(); ++it) { for (it = m_overlappingPairs.begin(); it != m_overlappingPairs.end(); ++it) {
it->second->clearContactPoints(); it->second->clearContactPoints();
} }
} }
// Fill-in the collision detection matrix
void CollisionDetection::fillInCollisionMatrix() { void CollisionDetection::fillInCollisionMatrix() {
// For each possible type of collision shape // For each possible type of collision shape
for (int32_t i=0; i<NB_COLLISION_SHAPE_TYPES; i++) { for (int32_t i=0; i<NB_COLLISION_SHAPE_TYPES; i++) {
for (int32_t j=0; j<NB_COLLISION_SHAPE_TYPES; j++) { for (int32_t j=0; j<NB_COLLISION_SHAPE_TYPES; j++) {
@ -496,99 +407,73 @@ void CollisionDetection::fillInCollisionMatrix() {
} }
} }
// Return the world event listener
EventListener* CollisionDetection::getWorldEventListener() { EventListener* CollisionDetection::getWorldEventListener() {
return m_world->m_eventListener; return m_world->m_eventListener;
} }
/// Return a reference to the world memory allocator
MemoryAllocator& CollisionDetection::getWorldMemoryAllocator() { MemoryAllocator& CollisionDetection::getWorldMemoryAllocator() {
return m_world->m_memoryAllocator; return m_world->m_memoryAllocator;
} }
// Called by a narrow-phase collision algorithm when a new contact has been found void TestCollisionBetweenShapesCallback::notifyContact(OverlappingPair* _overlappingPair,
void TestCollisionBetweenShapesCallback::notifyContact(OverlappingPair* overlappingPair, const ContactPointInfo& _contactInfo) {
const ContactPointInfo& contactInfo) { m_collisionCallback->notifyContact(_contactInfo);
m_collisionCallback->notifyContact(contactInfo);
} }
// Return the Narrow-phase collision detection algorithm to use between two types of shapes NarrowPhaseAlgorithm* CollisionDetection::getCollisionAlgorithm(CollisionShapeType _shape1Type, CollisionShapeType _shape2Type) const {
NarrowPhaseAlgorithm* CollisionDetection::getCollisionAlgorithm(CollisionShapeType shape1Type, CollisionShapeType shape2Type) const { return m_collisionMatrix[_shape1Type][_shape2Type];
return m_collisionMatrix[shape1Type][shape2Type];
} }
// Set the collision dispatch configuration void CollisionDetection::setCollisionDispatch(CollisionDispatch* _collisionDispatch) {
void CollisionDetection::setCollisionDispatch(CollisionDispatch* collisionDispatch) { m_collisionDispatch = _collisionDispatch;
m_collisionDispatch = collisionDispatch;
m_collisionDispatch->init(this, &m_memoryAllocator); m_collisionDispatch->init(this, &m_memoryAllocator);
// Fill-in the collision matrix with the new algorithms to use // Fill-in the collision matrix with the new algorithms to use
fillInCollisionMatrix(); fillInCollisionMatrix();
} }
// Add a body to the collision detection void CollisionDetection::addProxyCollisionShape(ProxyShape* _proxyShape, const AABB& _aabb) {
void CollisionDetection::addProxyCollisionShape(ProxyShape* proxyShape,
const AABB& aabb) {
// Add the body to the broad-phase // Add the body to the broad-phase
m_broadPhaseAlgorithm.addProxyCollisionShape(proxyShape, aabb); m_broadPhaseAlgorithm.addProxyCollisionShape(_proxyShape, _aabb);
m_isCollisionShapesAdded = true; m_isCollisionShapesAdded = true;
} }
// Add a pair of bodies that cannot collide with each other void CollisionDetection::addNoCollisionPair(CollisionBody* body1, CollisionBody* body2) {
void CollisionDetection::addNoCollisionPair(CollisionBody* body1, m_noCollisionPairs.set(OverlappingPair::computeBodiesIndexPair(body1, body2));
CollisionBody* body2) {
m_noCollisionPairs.insert(OverlappingPair::computeBodiesIndexPair(body1, body2));
} }
// Remove a pair of bodies that cannot collide with each other
void CollisionDetection::removeNoCollisionPair(CollisionBody* body1, void CollisionDetection::removeNoCollisionPair(CollisionBody* body1,
CollisionBody* body2) { CollisionBody* body2) {
m_noCollisionPairs.erase(OverlappingPair::computeBodiesIndexPair(body1, body2)); m_noCollisionPairs.erase(m_noCollisionPairs.find(OverlappingPair::computeBodiesIndexPair(body1, body2)));
} }
// Ask for a collision shape to be tested again during broad-phase.
/// We simply put the shape in the list of collision shape that have moved in the
/// previous frame so that it is tested for collision again in the broad-phase.
void CollisionDetection::askForBroadPhaseCollisionCheck(ProxyShape* shape) { void CollisionDetection::askForBroadPhaseCollisionCheck(ProxyShape* shape) {
m_broadPhaseAlgorithm.addMovedCollisionShape(shape->m_broadPhaseID); m_broadPhaseAlgorithm.addMovedCollisionShape(shape->m_broadPhaseID);
} }
// Update a proxy collision shape (that has moved for instance)
void CollisionDetection::updateProxyCollisionShape(ProxyShape* shape, const AABB& aabb, void CollisionDetection::updateProxyCollisionShape(ProxyShape* shape, const AABB& aabb,
const vec3& displacement, bool forceReinsert) { const vec3& displacement, bool forceReinsert) {
m_broadPhaseAlgorithm.updateProxyCollisionShape(shape, aabb, displacement); m_broadPhaseAlgorithm.updateProxyCollisionShape(shape, aabb, displacement);
} }
// Ray casting method
void CollisionDetection::raycast(RaycastCallback* raycastCallback, void CollisionDetection::raycast(RaycastCallback* raycastCallback,
const Ray& ray, const Ray& ray,
unsigned short raycastWithCategoryMaskBits) const { unsigned short raycastWithCategoryMaskBits) const {
PROFILE("CollisionDetection::raycast()"); PROFILE("CollisionDetection::raycast()");
RaycastTest rayCastTest(raycastCallback); RaycastTest rayCastTest(raycastCallback);
// Ask the broad-phase algorithm to call the testRaycastAgainstShape() // Ask the broad-phase algorithm to call the testRaycastAgainstShape()
// callback method for each proxy shape hit by the ray in the broad-phase // callback method for each proxy shape hit by the ray in the broad-phase
m_broadPhaseAlgorithm.raycast(ray, rayCastTest, raycastWithCategoryMaskBits); m_broadPhaseAlgorithm.raycast(ray, rayCastTest, raycastWithCategoryMaskBits);
} }
// Test if the AABBs of two proxy shapes overlap
bool CollisionDetection::testAABBOverlap(const ProxyShape* shape1, bool CollisionDetection::testAABBOverlap(const ProxyShape* shape1,
const ProxyShape* shape2) const { const ProxyShape* shape2) const {
// If one of the shape's body is not active, we return no overlap // If one of the shape's body is not active, we return no overlap
if (!shape1->getBody()->isActive() || !shape2->getBody()->isActive()) { if (!shape1->getBody()->isActive() || !shape2->getBody()->isActive()) {
return false; return false;
} }
return m_broadPhaseAlgorithm.testOverlappingShapes(shape1, shape2); return m_broadPhaseAlgorithm.testOverlappingShapes(shape1, shape2);
} }
// Return a pointer to the world
CollisionWorld* CollisionDetection::getWorld() { CollisionWorld* CollisionDetection::getWorld() {
return m_world; return m_world;
} }

22
ephysics/collision/CollisionDetection.hpp
View File

@ -14,8 +14,7 @@
#include <ephysics/constraint/ContactPoint.hpp> #include <ephysics/constraint/ContactPoint.hpp>
#include <etk/Vector.hpp> #include <etk/Vector.hpp>
#include <etk/Map.hpp> #include <etk/Map.hpp>
#include <set> #include <etk/Set.hpp>
#include <utility>
namespace ephysics { namespace ephysics {
@ -56,7 +55,7 @@ namespace ephysics {
BroadPhaseAlgorithm m_broadPhaseAlgorithm; //!< Broad-phase algorithm BroadPhaseAlgorithm m_broadPhaseAlgorithm; //!< Broad-phase algorithm
// TODO : Delete this // TODO : Delete this
GJKAlgorithm m_narrowPhaseGJKAlgorithm; //!< Narrow-phase GJK algorithm GJKAlgorithm m_narrowPhaseGJKAlgorithm; //!< Narrow-phase GJK algorithm
std::set<bodyindexpair> m_noCollisionPairs; //!< Set of pair of bodies that cannot collide between each other etk::Set<bodyindexpair> m_noCollisionPairs; //!< Set of pair of bodies that cannot collide between each other
bool m_isCollisionShapesAdded; //!< True if some collision shapes have been added previously bool m_isCollisionShapesAdded; //!< True if some collision shapes have been added previously
/// Private copy-constructor /// Private copy-constructor
CollisionDetection(const CollisionDetection& _collisionDetection); CollisionDetection(const CollisionDetection& _collisionDetection);
@ -98,18 +97,20 @@ namespace ephysics {
void addNoCollisionPair(CollisionBody* _body1, CollisionBody* _body2); void addNoCollisionPair(CollisionBody* _body1, CollisionBody* _body2);
/// Remove a pair of bodies that cannot collide with each other /// Remove a pair of bodies that cannot collide with each other
void removeNoCollisionPair(CollisionBody* _body1, CollisionBody* _body2); void removeNoCollisionPair(CollisionBody* _body1, CollisionBody* _body2);
/// Ask for a collision shape to be tested again during broad-phase. // Ask for a collision shape to be tested again during broad-phase.
/// We simply put the shape in the list of collision shape that have moved in the
/// previous frame so that it is tested for collision again in the broad-phase.
void askForBroadPhaseCollisionCheck(ProxyShape* _shape); void askForBroadPhaseCollisionCheck(ProxyShape* _shape);
/// Compute the collision detection /// Compute the collision detection
void computeCollisionDetection(); void computeCollisionDetection();
/// Compute the collision detection /// Compute the collision detection
void testCollisionBetweenShapes(CollisionCallback* _callback, void testCollisionBetweenShapes(CollisionCallback* _callback,
const std::set<uint32_t>& _shapes1, const etk::Set<uint32_t>& _shapes1,
const std::set<uint32_t>& _shapes2); const etk::Set<uint32_t>& _shapes2);
/// Report collision between two sets of shapes /// Report collision between two sets of shapes
void reportCollisionBetweenShapes(CollisionCallback* _callback, void reportCollisionBetweenShapes(CollisionCallback* _callback,
const std::set<uint32_t>& _shapes1, const etk::Set<uint32_t>& _shapes1,
const std::set<uint32_t>& _shapes2) ; const etk::Set<uint32_t>& _shapes2) ;
/// Ray casting method /// Ray casting method
void raycast(RaycastCallback* _raycastCallback, void raycast(RaycastCallback* _raycastCallback,
const Ray& _ray, const Ray& _ray,
@ -121,11 +122,12 @@ namespace ephysics {
bool testAABBOverlap(const ProxyShape* _shape1, bool testAABBOverlap(const ProxyShape* _shape1,
const ProxyShape* _shape2) const; const ProxyShape* _shape2) const;
/// Allow the broadphase to notify the collision detection about an overlapping pair. /// Allow the broadphase to notify the collision detection about an overlapping pair.
/// This method is called by the broad-phase collision detection algorithm
void broadPhaseNotifyOverlappingPair(ProxyShape* _shape1, ProxyShape* _shape2); void broadPhaseNotifyOverlappingPair(ProxyShape* _shape1, ProxyShape* _shape2);
/// Compute the narrow-phase collision detection /// Compute the narrow-phase collision detection
void computeNarrowPhaseBetweenShapes(CollisionCallback* _callback, void computeNarrowPhaseBetweenShapes(CollisionCallback* _callback,
const std::set<uint32_t>& _shapes1, const etk::Set<uint32_t>& _shapes1,
const std::set<uint32_t>& _shapes2); const etk::Set<uint32_t>& _shapes2);
/// Return a pointer to the world /// Return a pointer to the world
CollisionWorld* getWorld(); CollisionWorld* getWorld();
/// Return the world event listener /// Return the world event listener

1
ephysics/collision/ContactManifold.cpp
View File

@ -6,7 +6,6 @@
// Libraries // Libraries
#include <iostream>
#include <ephysics/collision/ContactManifold.hpp> #include <ephysics/collision/ContactManifold.hpp>
using namespace ephysics; using namespace ephysics;

79
ephysics/collision/ContactManifoldSet.cpp
View File

@ -4,12 +4,10 @@
* @license BSD 3 clauses (see license file) * @license BSD 3 clauses (see license file)
*/ */
// Libraries
#include <ephysics/collision/ContactManifoldSet.hpp> #include <ephysics/collision/ContactManifoldSet.hpp>
using namespace ephysics; using namespace ephysics;
// Constructor
ContactManifoldSet::ContactManifoldSet(ProxyShape* shape1, ProxyShape* shape2, ContactManifoldSet::ContactManifoldSet(ProxyShape* shape1, ProxyShape* shape2,
MemoryAllocator& memoryAllocator, int32_t nbMaxManifolds) MemoryAllocator& memoryAllocator, int32_t nbMaxManifolds)
: m_nbMaxManifolds(nbMaxManifolds), m_nbManifolds(0), m_shape1(shape1), : m_nbMaxManifolds(nbMaxManifolds), m_nbManifolds(0), m_shape1(shape1),
@ -17,61 +15,45 @@ ContactManifoldSet::ContactManifoldSet(ProxyShape* shape1, ProxyShape* shape2,
assert(nbMaxManifolds >= 1); assert(nbMaxManifolds >= 1);
} }
// Destructor
ContactManifoldSet::~ContactManifoldSet() { ContactManifoldSet::~ContactManifoldSet() {
// Clear all the contact manifolds
clear(); clear();
} }
// Add a contact point to the manifold set
void ContactManifoldSet::addContactPoint(ContactPoint* contact) { void ContactManifoldSet::addContactPoint(ContactPoint* contact) {
// Compute an Id corresponding to the normal direction (using a cubemap) // Compute an Id corresponding to the normal direction (using a cubemap)
int16_t normalDirectionId = computeCubemapNormalId(contact->getNormal()); int16_t normalDirectionId = computeCubemapNormalId(contact->getNormal());
// If there is no contact manifold yet // If there is no contact manifold yet
if (m_nbManifolds == 0) { if (m_nbManifolds == 0) {
createManifold(normalDirectionId); createManifold(normalDirectionId);
m_manifolds[0]->addContactPoint(contact); m_manifolds[0]->addContactPoint(contact);
assert(m_manifolds[m_nbManifolds-1]->getNbContactPoints() > 0); assert(m_manifolds[m_nbManifolds-1]->getNbContactPoints() > 0);
for (int32_t i=0; i<m_nbManifolds; i++) { for (int32_t i=0; i<m_nbManifolds; i++) {
assert(m_manifolds[i]->getNbContactPoints() > 0); assert(m_manifolds[i]->getNbContactPoints() > 0);
} }
return; return;
} }
// Select the manifold with the most similar normal (if exists) // Select the manifold with the most similar normal (if exists)
int32_t similarManifoldIndex = 0; int32_t similarManifoldIndex = 0;
if (m_nbMaxManifolds > 1) { if (m_nbMaxManifolds > 1) {
similarManifoldIndex = selectManifoldWithSimilarNormal(normalDirectionId); similarManifoldIndex = selectManifoldWithSimilarNormal(normalDirectionId);
} }
// If a similar manifold has been found // If a similar manifold has been found
if (similarManifoldIndex != -1) { if (similarManifoldIndex != -1) {
// Add the contact point to that similar manifold // Add the contact point to that similar manifold
m_manifolds[similarManifoldIndex]->addContactPoint(contact); m_manifolds[similarManifoldIndex]->addContactPoint(contact);
assert(m_manifolds[similarManifoldIndex]->getNbContactPoints() > 0); assert(m_manifolds[similarManifoldIndex]->getNbContactPoints() > 0);
return; return;
} }
// If the maximum number of manifold has not been reached yet // If the maximum number of manifold has not been reached yet
if (m_nbManifolds < m_nbMaxManifolds) { if (m_nbManifolds < m_nbMaxManifolds) {
// Create a new manifold for the contact point // Create a new manifold for the contact point
createManifold(normalDirectionId); createManifold(normalDirectionId);
m_manifolds[m_nbManifolds-1]->addContactPoint(contact); m_manifolds[m_nbManifolds-1]->addContactPoint(contact);
for (int32_t i=0; i<m_nbManifolds; i++) { for (int32_t i=0; i<m_nbManifolds; i++) {
assert(m_manifolds[i]->getNbContactPoints() > 0); assert(m_manifolds[i]->getNbContactPoints() > 0);
} }
return; return;
} }
// The contact point will be in a new contact manifold, we now have too much // The contact point will be in a new contact manifold, we now have too much
// manifolds condidates. We need to remove one. We choose to keep the manifolds // manifolds condidates. We need to remove one. We choose to keep the manifolds
// with the largest contact depth among their points // with the largest contact depth among their points
@ -85,20 +67,15 @@ void ContactManifoldSet::addContactPoint(ContactPoint* contact) {
smallestDepthIndex = i; smallestDepthIndex = i;
} }
} }
// If we do not want to keep to new manifold (not created yet) with the // If we do not want to keep to new manifold (not created yet) with the
// new contact point // new contact point
if (smallestDepthIndex == -1) { if (smallestDepthIndex == -1) {
// Delete the new contact // Delete the new contact
contact->~ContactPoint(); contact->~ContactPoint();
m_memoryAllocator.release(contact, sizeof(ContactPoint)); m_memoryAllocator.release(contact, sizeof(ContactPoint));
return; return;
} }
assert(smallestDepthIndex >= 0 && smallestDepthIndex < m_nbManifolds); assert(smallestDepthIndex >= 0 && smallestDepthIndex < m_nbManifolds);
// Here we need to replace an existing manifold with a new one (that contains // Here we need to replace an existing manifold with a new one (that contains
// the new contact point) // the new contact point)
removeManifold(smallestDepthIndex); removeManifold(smallestDepthIndex);
@ -108,70 +85,55 @@ void ContactManifoldSet::addContactPoint(ContactPoint* contact) {
for (int32_t i=0; i<m_nbManifolds; i++) { for (int32_t i=0; i<m_nbManifolds; i++) {
assert(m_manifolds[i]->getNbContactPoints() > 0); assert(m_manifolds[i]->getNbContactPoints() > 0);
} }
return; return;
} }
// Return the index of the contact manifold with a similar average normal.
// If no manifold has close enough average normal, it returns -1
int32_t ContactManifoldSet::selectManifoldWithSimilarNormal(int16_t normalDirectionId) const { int32_t ContactManifoldSet::selectManifoldWithSimilarNormal(int16_t normalDirectionId) const {
// Return the Id of the manifold with the same normal direction id (if exists) // Return the Id of the manifold with the same normal direction id (if exists)
for (int32_t i=0; i<m_nbManifolds; i++) { for (int32_t i=0; i<m_nbManifolds; i++) {
if (normalDirectionId == m_manifolds[i]->getNormalDirectionId()) { if (normalDirectionId == m_manifolds[i]->getNormalDirectionId()) {
return i; return i;
} }
} }
return -1; return -1;
} }
// Map the normal vector int32_to a cubemap face bucket (a face contains 4x4 buckets)
// Each face of the cube is divided int32_to 4x4 buckets. This method maps the
// normal vector int32_to of the of the bucket and returns a unique Id for the bucket
int16_t ContactManifoldSet::computeCubemapNormalId(const vec3& normal) const { int16_t ContactManifoldSet::computeCubemapNormalId(const vec3& normal) const {
assert(normal.length2() > FLT_EPSILON);
assert(normal.length2() > MACHINE_EPSILON);
int32_t faceNo; int32_t faceNo;
float u, v; float u, v;
float max = max3(fabs(normal.x()), fabs(normal.y()), fabs(normal.z())); float max = max3(fabs(normal.x()), fabs(normal.y()), fabs(normal.z()));
vec3 normalScaled = normal / max; vec3 normalScaled = normal / max;
if (normalScaled.x() >= normalScaled.y() && normalScaled.x() >= normalScaled.z()) { if (normalScaled.x() >= normalScaled.y() && normalScaled.x() >= normalScaled.z()) {
faceNo = normalScaled.x() > 0 ? 0 : 1; faceNo = normalScaled.x() > 0 ? 0 : 1;
u = normalScaled.y(); u = normalScaled.y();
v = normalScaled.z(); v = normalScaled.z();
} } else if (normalScaled.y() >= normalScaled.x() && normalScaled.y() >= normalScaled.z()) {
else if (normalScaled.y() >= normalScaled.x() && normalScaled.y() >= normalScaled.z()) {
faceNo = normalScaled.y() > 0 ? 2 : 3; faceNo = normalScaled.y() > 0 ? 2 : 3;
u = normalScaled.x(); u = normalScaled.x();
v = normalScaled.z(); v = normalScaled.z();
} } else {
else {
faceNo = normalScaled.z() > 0 ? 4 : 5; faceNo = normalScaled.z() > 0 ? 4 : 5;
u = normalScaled.x(); u = normalScaled.x();
v = normalScaled.y(); v = normalScaled.y();
} }
int32_t indexU = floor(((u + 1)/2) * CONTACT_CUBEMAP_FACE_NB_SUBDIVISIONS); int32_t indexU = floor(((u + 1)/2) * CONTACT_CUBEMAP_FACE_NB_SUBDIVISIONS);
int32_t indexV = floor(((v + 1)/2) * CONTACT_CUBEMAP_FACE_NB_SUBDIVISIONS); int32_t indexV = floor(((v + 1)/2) * CONTACT_CUBEMAP_FACE_NB_SUBDIVISIONS);
if (indexU == CONTACT_CUBEMAP_FACE_NB_SUBDIVISIONS) indexU--; if (indexU == CONTACT_CUBEMAP_FACE_NB_SUBDIVISIONS) {
if (indexV == CONTACT_CUBEMAP_FACE_NB_SUBDIVISIONS) indexV--; indexU--;
}
if (indexV == CONTACT_CUBEMAP_FACE_NB_SUBDIVISIONS) {
indexV--;
}
const int32_t nbSubDivInFace = CONTACT_CUBEMAP_FACE_NB_SUBDIVISIONS * CONTACT_CUBEMAP_FACE_NB_SUBDIVISIONS; const int32_t nbSubDivInFace = CONTACT_CUBEMAP_FACE_NB_SUBDIVISIONS * CONTACT_CUBEMAP_FACE_NB_SUBDIVISIONS;
return faceNo * 200 + indexU * nbSubDivInFace + indexV; return faceNo * 200 + indexU * nbSubDivInFace + indexV;
} }
// Update the contact manifolds
void ContactManifoldSet::update() { void ContactManifoldSet::update() {
for (int32_t i=m_nbManifolds-1; i>=0; i--) { for (int32_t i=m_nbManifolds-1; i>=0; i--) {
// Update the contact manifold // Update the contact manifold
m_manifolds[i]->update(m_shape1->getBody()->getTransform() * m_shape1->getLocalToBodyTransform(), m_manifolds[i]->update(m_shape1->getBody()->getTransform() * m_shape1->getLocalToBodyTransform(),
m_shape2->getBody()->getTransform() * m_shape2->getLocalToBodyTransform()); m_shape2->getBody()->getTransform() * m_shape2->getLocalToBodyTransform());
// Remove the contact manifold if has no contact points anymore // Remove the contact manifold if has no contact points anymore
if (m_manifolds[i]->getNbContactPoints() == 0) { if (m_manifolds[i]->getNbContactPoints() == 0) {
removeManifold(i); removeManifold(i);
@ -179,65 +141,48 @@ void ContactManifoldSet::update() {
} }
} }
// Clear the contact manifold set
void ContactManifoldSet::clear() { void ContactManifoldSet::clear() {
// Destroy all the contact manifolds
for (int32_t i=m_nbManifolds-1; i>=0; i--) { for (int32_t i=m_nbManifolds-1; i>=0; i--) {
removeManifold(i); removeManifold(i);
} }
assert(m_nbManifolds == 0); assert(m_nbManifolds == 0);
} }
// Create a new contact manifold and add it to the set
void ContactManifoldSet::createManifold(int16_t normalDirectionId) { void ContactManifoldSet::createManifold(int16_t normalDirectionId) {
assert(m_nbManifolds < m_nbMaxManifolds); assert(m_nbManifolds < m_nbMaxManifolds);
m_manifolds[m_nbManifolds] = new ContactManifold(m_shape1, m_shape2, m_memoryAllocator, normalDirectionId);
m_manifolds[m_nbManifolds] = new (m_memoryAllocator.allocate(sizeof(ContactManifold)))
ContactManifold(m_shape1, m_shape2, m_memoryAllocator, normalDirectionId);
m_nbManifolds++; m_nbManifolds++;
} }
// Remove a contact manifold from the set
void ContactManifoldSet::removeManifold(int32_t index) { void ContactManifoldSet::removeManifold(int32_t index) {
assert(m_nbManifolds > 0); assert(m_nbManifolds > 0);
assert(index >= 0 && index < m_nbManifolds); assert(index >= 0 && index < m_nbManifolds);
// Delete the new contact // Delete the new contact
m_manifolds[index]->~ContactManifold(); delete m_manifolds[index];
m_memoryAllocator.release(m_manifolds[index], sizeof(ContactManifold)); m_manifolds[index] = nullptr;
for (int32_t i=index; (i+1) < m_nbManifolds; i++) { for (int32_t i=index; (i+1) < m_nbManifolds; i++) {
m_manifolds[i] = m_manifolds[i+1]; m_manifolds[i] = m_manifolds[i+1];
} }
m_nbManifolds--; m_nbManifolds--;
} }
// Return the first proxy shape
ProxyShape* ContactManifoldSet::getShape1() const { ProxyShape* ContactManifoldSet::getShape1() const {
return m_shape1; return m_shape1;
} }
// Return the second proxy shape
ProxyShape* ContactManifoldSet::getShape2() const { ProxyShape* ContactManifoldSet::getShape2() const {
return m_shape2; return m_shape2;
} }
// Return the number of manifolds in the set
int32_t ContactManifoldSet::getNbContactManifolds() const { int32_t ContactManifoldSet::getNbContactManifolds() const {
return m_nbManifolds; return m_nbManifolds;
} }
// Return a given contact manifold
ContactManifold* ContactManifoldSet::getContactManifold(int32_t index) const { ContactManifold* ContactManifoldSet::getContactManifold(int32_t index) const {
assert(index >= 0 && index < m_nbManifolds); assert(index >= 0 && index < m_nbManifolds);
return m_manifolds[index]; return m_manifolds[index];
} }
// Return the total number of contact points in the set of manifolds
int32_t ContactManifoldSet::getTotalNbContactPoints() const { int32_t ContactManifoldSet::getTotalNbContactPoints() const {
int32_t nbPoints = 0; int32_t nbPoints = 0;
for (int32_t i=0; i<m_nbManifolds; i++) { for (int32_t i=0; i<m_nbManifolds; i++) {

1
ephysics/collision/TriangleMesh.hpp
View File

@ -5,7 +5,6 @@
*/ */
#pragma once #pragma once
#include <etk/Vector.hpp> #include <etk/Vector.hpp>
#include <cassert>
#include <ephysics/collision/TriangleVertexArray.hpp> #include <ephysics/collision/TriangleVertexArray.hpp>
namespace ephysics { namespace ephysics {

146
ephysics/collision/narrowphase/ConcaveVsConvexAlgorithm.cpp
View File

@ -4,50 +4,42 @@
* @license BSD 3 clauses (see license file) * @license BSD 3 clauses (see license file)
*/ */
// Libraries
#include <ephysics/collision/shapes/ConcaveShape.hpp> #include <ephysics/collision/shapes/ConcaveShape.hpp>
#include <ephysics/collision/shapes/TriangleShape.hpp> #include <ephysics/collision/shapes/TriangleShape.hpp>
#include <ephysics/collision/narrowphase/ConcaveVsConvexAlgorithm.hpp> #include <ephysics/collision/narrowphase/ConcaveVsConvexAlgorithm.hpp>
#include <ephysics/collision/CollisionDetection.hpp> #include <ephysics/collision/CollisionDetection.hpp>
#include <ephysics/engine/CollisionWorld.hpp> #include <ephysics/engine/CollisionWorld.hpp>
#include <algorithm>
using namespace ephysics; using namespace ephysics;
// Constructor
ConcaveVsConvexAlgorithm::ConcaveVsConvexAlgorithm() { ConcaveVsConvexAlgorithm::ConcaveVsConvexAlgorithm() {
} }
// Destructor
ConcaveVsConvexAlgorithm::~ConcaveVsConvexAlgorithm() { ConcaveVsConvexAlgorithm::~ConcaveVsConvexAlgorithm() {
} }
// Return true and compute a contact info if the two bounding volumes collide
void ConcaveVsConvexAlgorithm::testCollision(const CollisionShapeInfo& shape1Info, void ConcaveVsConvexAlgorithm::testCollision(const CollisionShapeInfo& shape1Info,
const CollisionShapeInfo& shape2Info, const CollisionShapeInfo& shape2Info,
NarrowPhaseCallback* narrowPhaseCallback) { NarrowPhaseCallback* narrowPhaseCallback) {
ProxyShape* convexProxyShape; ProxyShape* convexProxyShape;
ProxyShape* concaveProxyShape; ProxyShape* concaveProxyShape;
const ConvexShape* convexShape; const ConvexShape* convexShape;
const ConcaveShape* concaveShape; const ConcaveShape* concaveShape;
// Collision shape 1 is convex, collision shape 2 is concave // Collision shape 1 is convex, collision shape 2 is concave
if (shape1Info.collisionShape->isConvex()) { if (shape1Info.collisionShape->isConvex()) {
convexProxyShape = shape1Info.proxyShape; convexProxyShape = shape1Info.proxyShape;
convexShape = static_cast<const ConvexShape*>(shape1Info.collisionShape); convexShape = static_cast<const ConvexShape*>(shape1Info.collisionShape);
concaveProxyShape = shape2Info.proxyShape; concaveProxyShape = shape2Info.proxyShape;
concaveShape = static_cast<const ConcaveShape*>(shape2Info.collisionShape); concaveShape = static_cast<const ConcaveShape*>(shape2Info.collisionShape);
} } else {
else { // Collision shape 2 is convex, collision shape 1 is concave // Collision shape 2 is convex, collision shape 1 is concave
convexProxyShape = shape2Info.proxyShape; convexProxyShape = shape2Info.proxyShape;
convexShape = static_cast<const ConvexShape*>(shape2Info.collisionShape); convexShape = static_cast<const ConvexShape*>(shape2Info.collisionShape);
concaveProxyShape = shape1Info.proxyShape; concaveProxyShape = shape1Info.proxyShape;
concaveShape = static_cast<const ConcaveShape*>(shape1Info.collisionShape); concaveShape = static_cast<const ConcaveShape*>(shape1Info.collisionShape);
} }
// Set the parameters of the callback object // Set the parameters of the callback object
ConvexVsTriangleCallback convexVsTriangleCallback; ConvexVsTriangleCallback convexVsTriangleCallback;
convexVsTriangleCallback.setCollisionDetection(m_collisionDetection); convexVsTriangleCallback.setCollisionDetection(m_collisionDetection);
@ -55,83 +47,64 @@ void ConcaveVsConvexAlgorithm::testCollision(const CollisionShapeInfo& shape1Inf
convexVsTriangleCallback.setConcaveShape(concaveShape); convexVsTriangleCallback.setConcaveShape(concaveShape);
convexVsTriangleCallback.setProxyShapes(convexProxyShape, concaveProxyShape); convexVsTriangleCallback.setProxyShapes(convexProxyShape, concaveProxyShape);
convexVsTriangleCallback.setOverlappingPair(shape1Info.overlappingPair); convexVsTriangleCallback.setOverlappingPair(shape1Info.overlappingPair);
// Compute the convex shape AABB in the local-space of the convex shape // Compute the convex shape AABB in the local-space of the convex shape
AABB aabb; AABB aabb;
convexShape->computeAABB(aabb, convexProxyShape->getLocalToWorldTransform()); convexShape->computeAABB(aabb, convexProxyShape->getLocalToWorldTransform());
// If smooth mesh collision is enabled for the concave mesh // If smooth mesh collision is enabled for the concave mesh
if (concaveShape->getIsSmoothMeshCollisionEnabled()) { if (concaveShape->getIsSmoothMeshCollisionEnabled()) {
etk::Vector<SmoothMeshContactInfo> contactPoints; etk::Vector<SmoothMeshContactInfo> contactPoints;
SmoothCollisionNarrowPhaseCallback smoothNarrowPhaseCallback(contactPoints); SmoothCollisionNarrowPhaseCallback smoothNarrowPhaseCallback(contactPoints);
convexVsTriangleCallback.setNarrowPhaseCallback(&smoothNarrowPhaseCallback); convexVsTriangleCallback.setNarrowPhaseCallback(&smoothNarrowPhaseCallback);
// Call the convex vs triangle callback for each triangle of the concave shape // Call the convex vs triangle callback for each triangle of the concave shape
concaveShape->testAllTriangles(convexVsTriangleCallback, aabb); concaveShape->testAllTriangles(convexVsTriangleCallback, aabb);
// Run the smooth mesh collision algorithm // Run the smooth mesh collision algorithm
processSmoothMeshCollision(shape1Info.overlappingPair, contactPoints, narrowPhaseCallback); processSmoothMeshCollision(shape1Info.overlappingPair, contactPoints, narrowPhaseCallback);
} } else {
else {
convexVsTriangleCallback.setNarrowPhaseCallback(narrowPhaseCallback); convexVsTriangleCallback.setNarrowPhaseCallback(narrowPhaseCallback);
// Call the convex vs triangle callback for each triangle of the concave shape // Call the convex vs triangle callback for each triangle of the concave shape
concaveShape->testAllTriangles(convexVsTriangleCallback, aabb); concaveShape->testAllTriangles(convexVsTriangleCallback, aabb);
} }
} }
// Test collision between a triangle and the convex mesh shape
void ConvexVsTriangleCallback::testTriangle(const vec3* trianglePoints) { void ConvexVsTriangleCallback::testTriangle(const vec3* trianglePoints) {
// Create a triangle collision shape // Create a triangle collision shape
float margin = m_concaveShape->getTriangleMargin(); float margin = m_concaveShape->getTriangleMargin();
TriangleShape triangleShape(trianglePoints[0], trianglePoints[1], trianglePoints[2], margin); TriangleShape triangleShape(trianglePoints[0], trianglePoints[1], trianglePoints[2], margin);
// Select the collision algorithm to use between the triangle and the convex shape // Select the collision algorithm to use between the triangle and the convex shape
NarrowPhaseAlgorithm* algo = m_collisionDetection->getCollisionAlgorithm(triangleShape.getType(), NarrowPhaseAlgorithm* algo = m_collisionDetection->getCollisionAlgorithm(triangleShape.getType(),
m_convexShape->getType()); m_convexShape->getType());
// If there is no collision algorithm between those two kinds of shapes // If there is no collision algorithm between those two kinds of shapes
if (algo == NULL) return; if (algo == nullptr) {
return;
}
// Notify the narrow-phase algorithm about the overlapping pair we are going to test // Notify the narrow-phase algorithm about the overlapping pair we are going to test
algo->setCurrentOverlappingPair(m_overlappingPair); algo->setCurrentOverlappingPair(m_overlappingPair);
// Create the CollisionShapeInfo objects // Create the CollisionShapeInfo objects
CollisionShapeInfo shapeConvexInfo(m_convexProxyShape, m_convexShape, m_convexProxyShape->getLocalToWorldTransform(), CollisionShapeInfo shapeConvexInfo(m_convexProxyShape, m_convexShape, m_convexProxyShape->getLocalToWorldTransform(),
m_overlappingPair, m_convexProxyShape->getCachedCollisionData()); m_overlappingPair, m_convexProxyShape->getCachedCollisionData());
CollisionShapeInfo shapeConcaveInfo(m_concaveProxyShape, &triangleShape, CollisionShapeInfo shapeConcaveInfo(m_concaveProxyShape, &triangleShape,
m_concaveProxyShape->getLocalToWorldTransform(), m_concaveProxyShape->getLocalToWorldTransform(),
m_overlappingPair, m_concaveProxyShape->getCachedCollisionData()); m_overlappingPair, m_concaveProxyShape->getCachedCollisionData());
// Use the collision algorithm to test collision between the triangle and the other convex shape // Use the collision algorithm to test collision between the triangle and the other convex shape
algo->testCollision(shapeConvexInfo, shapeConcaveInfo, m_narrowPhaseCallback); algo->testCollision(shapeConvexInfo, shapeConcaveInfo, m_narrowPhaseCallback);
} }
// Process the concave triangle mesh collision using the smooth mesh collision algorithm described
// by Pierre Terdiman (http://www.codercorner.com/MeshContacts.pdf). This is used to avoid the collision
// issue with some int32_ternal edges.
void ConcaveVsConvexAlgorithm::processSmoothMeshCollision(OverlappingPair* overlappingPair, void ConcaveVsConvexAlgorithm::processSmoothMeshCollision(OverlappingPair* overlappingPair,
etk::Vector<SmoothMeshContactInfo> contactPoints, etk::Vector<SmoothMeshContactInfo> contactPoints,
NarrowPhaseCallback* narrowPhaseCallback) { NarrowPhaseCallback* narrowPhaseCallback) {
// Set with the triangle vertices already processed to void further contacts with same triangle // Set with the triangle vertices already processed to void further contacts with same triangle
std::unordered_multimap<int32_t, vec3> processTriangleVertices; etk::Vector<etk::Pair<int32_t, vec3>> processTriangleVertices;
// Sort the list of narrow-phase contacts according to their penetration depth // Sort the list of narrow-phase contacts according to their penetration depth
std::sort(contactPoints.begin(), contactPoints.end(), ContactsDepthCompare()); contactPoints.sort(0,
contactPoints.size()-1,
[](const SmoothMeshContactInfo& _contact1, const SmoothMeshContactInfo& _contact2) {
return _contact1.contactInfo.penetrationDepth < _contact2.contactInfo.penetrationDepth;
});
// For each contact point (from smaller penetration depth to larger) // For each contact point (from smaller penetration depth to larger)
etk::Vector<SmoothMeshContactInfo>::const_iterator it; etk::Vector<SmoothMeshContactInfo>::Iterator it;
for (it = contactPoints.begin(); it != contactPoints.end(); ++it) { for (it = contactPoints.begin(); it != contactPoints.end(); ++it) {
const SmoothMeshContactInfo info = *it; const SmoothMeshContactInfo info = *it;
const vec3& contactPoint = info.isFirstShapeTriangle ? info.contactInfo.localPoint1 : info.contactInfo.localPoint2; const vec3& contactPoint = info.isFirstShapeTriangle ? info.contactInfo.localPoint1 : info.contactInfo.localPoint2;
// Compute the barycentric coordinates of the point in the triangle // Compute the barycentric coordinates of the point in the triangle
float u, v, w; float u, v, w;
computeBarycentricCoordinatesInTriangle(info.triangleVertices[0], computeBarycentricCoordinatesInTriangle(info.triangleVertices[0],
@ -142,51 +115,45 @@ void ConcaveVsConvexAlgorithm::processSmoothMeshCollision(OverlappingPair* overl
bool isUZero = approxEqual(u, 0, 0.0001); bool isUZero = approxEqual(u, 0, 0.0001);
bool isVZero = approxEqual(v, 0, 0.0001); bool isVZero = approxEqual(v, 0, 0.0001);
bool isWZero = approxEqual(w, 0, 0.0001); bool isWZero = approxEqual(w, 0, 0.0001);
if (isUZero) nbZeros++; if (isUZero) {
if (isVZero) nbZeros++; nbZeros++;
if (isWZero) nbZeros++; }
if (isVZero) {
nbZeros++;
}
if (isWZero) {
nbZeros++;
}
// If it is a vertex contact // If it is a vertex contact
if (nbZeros == 2) { if (nbZeros == 2) {
vec3 contactVertex = !isUZero ? info.triangleVertices[0] : (!isVZero ? info.triangleVertices[1] : info.triangleVertices[2]); vec3 contactVertex = !isUZero ? info.triangleVertices[0] : (!isVZero ? info.triangleVertices[1] : info.triangleVertices[2]);
// Check that this triangle vertex has not been processed yet // Check that this triangle vertex has not been processed yet
if (!hasVertexBeenProcessed(processTriangleVertices, contactVertex)) { if (!hasVertexBeenProcessed(processTriangleVertices, contactVertex)) {
// Keep the contact as it is and report it // Keep the contact as it is and report it
narrowPhaseCallback->notifyContact(overlappingPair, info.contactInfo); narrowPhaseCallback->notifyContact(overlappingPair, info.contactInfo);
} }
} } else if (nbZeros == 1) {
else if (nbZeros == 1) { // If it is an edge contact // If it is an edge contact
vec3 contactVertex1 = isUZero ? info.triangleVertices[1] : (isVZero ? info.triangleVertices[0] : info.triangleVertices[0]); vec3 contactVertex1 = isUZero ? info.triangleVertices[1] : (isVZero ? info.triangleVertices[0] : info.triangleVertices[0]);
vec3 contactVertex2 = isUZero ? info.triangleVertices[2] : (isVZero ? info.triangleVertices[2] : info.triangleVertices[1]); vec3 contactVertex2 = isUZero ? info.triangleVertices[2] : (isVZero ? info.triangleVertices[2] : info.triangleVertices[1]);
// Check that this triangle edge has not been processed yet // Check that this triangle edge has not been processed yet
if (!hasVertexBeenProcessed(processTriangleVertices, contactVertex1) && if (!hasVertexBeenProcessed(processTriangleVertices, contactVertex1) &&
!hasVertexBeenProcessed(processTriangleVertices, contactVertex2)) { !hasVertexBeenProcessed(processTriangleVertices, contactVertex2)) {
// Keep the contact as it is and report it // Keep the contact as it is and report it
narrowPhaseCallback->notifyContact(overlappingPair, info.contactInfo); narrowPhaseCallback->notifyContact(overlappingPair, info.contactInfo);
} }
} else {
} // If it is a face contact
else { // If it is a face contact
ContactPointInfo newContactInfo(info.contactInfo); ContactPointInfo newContactInfo(info.contactInfo);
ProxyShape* firstShape; ProxyShape* firstShape;
ProxyShape* secondShape; ProxyShape* secondShape;
if (info.isFirstShapeTriangle) { if (info.isFirstShapeTriangle) {
firstShape = overlappingPair->getShape1(); firstShape = overlappingPair->getShape1();
secondShape = overlappingPair->getShape2(); secondShape = overlappingPair->getShape2();
} } else {
else {
firstShape = overlappingPair->getShape2(); firstShape = overlappingPair->getShape2();
secondShape = overlappingPair->getShape1(); secondShape = overlappingPair->getShape1();
} }
// We use the triangle normal as the contact normal // We use the triangle normal as the contact normal
vec3 a = info.triangleVertices[1] - info.triangleVertices[0]; vec3 a = info.triangleVertices[1] - info.triangleVertices[0];
vec3 b = info.triangleVertices[2] - info.triangleVertices[0]; vec3 b = info.triangleVertices[2] - info.triangleVertices[0];
@ -198,7 +165,6 @@ void ConcaveVsConvexAlgorithm::processSmoothMeshCollision(OverlappingPair* overl
if (newContactInfo.normal.dot(info.contactInfo.normal) < 0) { if (newContactInfo.normal.dot(info.contactInfo.normal) < 0) {
newContactInfo.normal = -newContactInfo.normal; newContactInfo.normal = -newContactInfo.normal;
} }
// We recompute the contact point on the second body with the new normal as described in // We recompute the contact point on the second body with the new normal as described in
// the Smooth Mesh Contacts with GJK of the Game Physics Pearls book (from Gino van Den Bergen and // the Smooth Mesh Contacts with GJK of the Game Physics Pearls book (from Gino van Den Bergen and
// Dirk Gregorius) to avoid adding torque // Dirk Gregorius) to avoid adding torque
@ -206,16 +172,13 @@ void ConcaveVsConvexAlgorithm::processSmoothMeshCollision(OverlappingPair* overl
if (info.isFirstShapeTriangle) { if (info.isFirstShapeTriangle) {
vec3 newSecondWorldPoint = firstWorldPoint + newContactInfo.normal; vec3 newSecondWorldPoint = firstWorldPoint + newContactInfo.normal;
newContactInfo.localPoint2 = worldToLocalSecondPoint * newSecondWorldPoint; newContactInfo.localPoint2 = worldToLocalSecondPoint * newSecondWorldPoint;
} } else {
else {
vec3 newSecondWorldPoint = firstWorldPoint - newContactInfo.normal; vec3 newSecondWorldPoint = firstWorldPoint - newContactInfo.normal;
newContactInfo.localPoint1 = worldToLocalSecondPoint * newSecondWorldPoint; newContactInfo.localPoint1 = worldToLocalSecondPoint * newSecondWorldPoint;
} }
// Report the contact // Report the contact
narrowPhaseCallback->notifyContact(overlappingPair, newContactInfo); narrowPhaseCallback->notifyContact(overlappingPair, newContactInfo);
} }
// Add the three vertices of the triangle to the set of processed // Add the three vertices of the triangle to the set of processed
// triangle vertices // triangle vertices
addProcessedVertex(processTriangleVertices, info.triangleVertices[0]); addProcessedVertex(processTriangleVertices, info.triangleVertices[0]);
@ -224,48 +187,51 @@ void ConcaveVsConvexAlgorithm::processSmoothMeshCollision(OverlappingPair* overl
} }
} }
// Return true if the vertex is in the set of already processed vertices bool ConcaveVsConvexAlgorithm::hasVertexBeenProcessed(const etk::Vector<etk::Pair<int32_t, vec3>>& _processTriangleVertices, const vec3& _vertex) const {
bool ConcaveVsConvexAlgorithm::hasVertexBeenProcessed(const std::unordered_multimap<int32_t, vec3>& processTriangleVertices, const vec3& vertex) const { /* TODO : etk::Vector<etk::Pair<int32_t, vec3>> was an unordered map ... ==> stupid idee... I replace code because I do not have enouth time to do something good...
int32_t key = int32_t(_vertex.x() * _vertex.y() * _vertex.z());
int32_t key = int32_t(vertex.x() * vertex.y() * vertex.z()); auto range = _processTriangleVertices.equal_range(key);
auto range = processTriangleVertices.equal_range(key);
for (auto it = range.first; it != range.second; ++it) { for (auto it = range.first; it != range.second; ++it) {
if (vertex.x() == it->second.x() && vertex.y() == it->second.y() && vertex.z() == it->second.z()) return true; if ( _vertex.x() == it->second.x()
&& _vertex.y() == it->second.y()
&& _vertex.z() == it->second.z()) {
return true;
}
}
return false;
*/
// TODO : This is not really the same ...
for (auto &it: _processTriangleVertices) {
if ( _vertex.x() == it.second.x()
&& _vertex.y() == it.second.y()
&& _vertex.z() == it.second.z()) {
return true;
}
} }
return false; return false;
} }
// Called by a narrow-phase collision algorithm when a new contact has been found void SmoothCollisionNarrowPhaseCallback::notifyContact(OverlappingPair* _overlappingPair,
void SmoothCollisionNarrowPhaseCallback::notifyContact(OverlappingPair* overlappingPair, const ContactPointInfo& _contactInfo) {
const ContactPointInfo& contactInfo) {
vec3 triangleVertices[3]; vec3 triangleVertices[3];
bool isFirstShapeTriangle; bool isFirstShapeTriangle;
// If the collision shape 1 is the triangle // If the collision shape 1 is the triangle
if (contactInfo.collisionShape1->getType() == TRIANGLE) { if (_contactInfo.collisionShape1->getType() == TRIANGLE) {
assert(contactInfo.collisionShape2->getType() != TRIANGLE); assert(_contactInfo.collisionShape2->getType() != TRIANGLE);
const TriangleShape* triangleShape = static_cast<const TriangleShape*>(_contactInfo.collisionShape1);
const TriangleShape* triangleShape = static_cast<const TriangleShape*>(contactInfo.collisionShape1);
triangleVertices[0] = triangleShape->getVertex(0); triangleVertices[0] = triangleShape->getVertex(0);
triangleVertices[1] = triangleShape->getVertex(1); triangleVertices[1] = triangleShape->getVertex(1);
triangleVertices[2] = triangleShape->getVertex(2); triangleVertices[2] = triangleShape->getVertex(2);
isFirstShapeTriangle = true; isFirstShapeTriangle = true;
} } else { // If the collision shape 2 is the triangle
else { // If the collision shape 2 is the triangle assert(_contactInfo.collisionShape2->getType() == TRIANGLE);
assert(contactInfo.collisionShape2->getType() == TRIANGLE); const TriangleShape* triangleShape = static_cast<const TriangleShape*>(_contactInfo.collisionShape2);
const TriangleShape* triangleShape = static_cast<const TriangleShape*>(contactInfo.collisionShape2);
triangleVertices[0] = triangleShape->getVertex(0); triangleVertices[0] = triangleShape->getVertex(0);
triangleVertices[1] = triangleShape->getVertex(1); triangleVertices[1] = triangleShape->getVertex(1);
triangleVertices[2] = triangleShape->getVertex(2); triangleVertices[2] = triangleShape->getVertex(2);
isFirstShapeTriangle = false; isFirstShapeTriangle = false;
} }
SmoothMeshContactInfo smoothContactInfo(contactInfo, isFirstShapeTriangle, triangleVertices[0], triangleVertices[1], triangleVertices[2]); SmoothMeshContactInfo smoothContactInfo(_contactInfo, isFirstShapeTriangle, triangleVertices[0], triangleVertices[1], triangleVertices[2]);
// Add the narrow-phase contact int32_to the list of contact to process for // Add the narrow-phase contact int32_to the list of contact to process for
// smooth mesh collision // smooth mesh collision
m_contactPoints.pushBack(smoothContactInfo); m_contactPoints.pushBack(smoothContactInfo);

12
ephysics/collision/narrowphase/ConcaveVsConvexAlgorithm.hpp
View File

@ -8,7 +8,6 @@
#include <ephysics/collision/narrowphase/NarrowPhaseAlgorithm.hpp> #include <ephysics/collision/narrowphase/NarrowPhaseAlgorithm.hpp>
#include <ephysics/collision/shapes/ConvexShape.hpp> #include <ephysics/collision/shapes/ConvexShape.hpp>
#include <ephysics/collision/shapes/ConcaveShape.hpp> #include <ephysics/collision/shapes/ConcaveShape.hpp>
#include <unordered_map>
namespace ephysics { namespace ephysics {
@ -79,13 +78,18 @@ namespace ephysics {
triangleVertices[1] = _trianglePoint2; triangleVertices[1] = _trianglePoint2;
triangleVertices[2] = _trianglePoint3; triangleVertices[2] = _trianglePoint3;
} }
SmoothMeshContactInfo() {
// TODO: add it for etk::Vector
}
}; };
/*
struct ContactsDepthCompare { struct ContactsDepthCompare {
bool operator()(const SmoothMeshContactInfo& _contact1, const SmoothMeshContactInfo& _contact2) { bool operator()(const SmoothMeshContactInfo& _contact1, const SmoothMeshContactInfo& _contact2) {
return _contact1.contactInfo.penetrationDepth < _contact2.contactInfo.penetrationDepth; return _contact1.contactInfo.penetrationDepth < _contact2.contactInfo.penetrationDepth;
} }
}; };
*/
/** /**
* @brief This class is used as a narrow-phase callback to get narrow-phase contacts * @brief This class is used as a narrow-phase callback to get narrow-phase contacts
@ -122,11 +126,11 @@ namespace ephysics {
etk::Vector<SmoothMeshContactInfo> _contactPoints, etk::Vector<SmoothMeshContactInfo> _contactPoints,
NarrowPhaseCallback* _narrowPhaseCallback); NarrowPhaseCallback* _narrowPhaseCallback);
/// Add a triangle vertex int32_to the set of processed triangles /// Add a triangle vertex int32_to the set of processed triangles
void addProcessedVertex(std::unordered_multimap<int32_t, vec3>& _processTriangleVertices, const vec3& _vertex) { void addProcessedVertex(etk::Vector<etk::Pair<int32_t, vec3>>& _processTriangleVertices, const vec3& _vertex) {
_processTriangleVertices.insert(etk::makePair(int32_t(_vertex.x() * _vertex.y() * _vertex.z()), _vertex)); _processTriangleVertices.pushBack(etk::makePair(int32_t(_vertex.x() * _vertex.y() * _vertex.z()), _vertex));
} }
/// Return true if the vertex is in the set of already processed vertices /// Return true if the vertex is in the set of already processed vertices
bool hasVertexBeenProcessed(const std::unordered_multimap<int32_t, vec3>& _processTriangleVertices, bool hasVertexBeenProcessed(const etk::Vector<etk::Pair<int32_t, vec3>>& _processTriangleVertices,
const vec3& _vertex) const; const vec3& _vertex) const;
public : public :
/// Constructor /// Constructor

113
ephysics/collision/narrowphase/EPA/EPAAlgorithm.cpp
View File

@ -3,66 +3,46 @@
* @copyright 2010-2016 Daniel Chappuis * @copyright 2010-2016 Daniel Chappuis
* @license BSD 3 clauses (see license file) * @license BSD 3 clauses (see license file)
*/ */
// Libraries
#include <ephysics/collision/narrowphase/EPA/EPAAlgorithm.hpp> #include <ephysics/collision/narrowphase/EPA/EPAAlgorithm.hpp>
#include <ephysics/engine/Profiler.hpp> #include <ephysics/engine/Profiler.hpp>
#include <ephysics/collision/narrowphase/GJK/GJKAlgorithm.hpp> #include <ephysics/collision/narrowphase/GJK/GJKAlgorithm.hpp>
#include <ephysics/collision/narrowphase/EPA/TrianglesStore.hpp> #include <ephysics/collision/narrowphase/EPA/TrianglesStore.hpp>
// We want to use the ReactPhysics3D namespace
using namespace ephysics; using namespace ephysics;
// Constructor
EPAAlgorithm::EPAAlgorithm() { EPAAlgorithm::EPAAlgorithm() {
} }
// Destructor
EPAAlgorithm::~EPAAlgorithm() { EPAAlgorithm::~EPAAlgorithm() {
} }
// Decide if the origin is in the tetrahedron. int32_t EPAAlgorithm::isOriginInTetrahedron(const vec3& p1, const vec3& p2, const vec3& p3, const vec3& p4) const {
/// Return 0 if the origin is in the tetrahedron and return the number (1,2,3 or 4) of
/// the vertex that is wrong if the origin is not in the tetrahedron
int32_t EPAAlgorithm::isOriginInTetrahedron(const vec3& p1, const vec3& p2,
const vec3& p3, const vec3& p4) const {
// Check vertex 1 // Check vertex 1
vec3 normal1 = (p2-p1).cross(p3-p1); vec3 normal1 = (p2-p1).cross(p3-p1);
if ((normal1.dot(p1) > 0.0) == (normal1.dot(p4) > 0.0)) { if ((normal1.dot(p1) > 0.0) == (normal1.dot(p4) > 0.0)) {
return 4; return 4;
} }
// Check vertex 2 // Check vertex 2
vec3 normal2 = (p4-p2).cross(p3-p2); vec3 normal2 = (p4-p2).cross(p3-p2);
if ((normal2.dot(p2) > 0.0) == (normal2.dot(p1) > 0.0)) { if ((normal2.dot(p2) > 0.0) == (normal2.dot(p1) > 0.0)) {
return 1; return 1;
} }
// Check vertex 3 // Check vertex 3
vec3 normal3 = (p4-p3).cross(p1-p3); vec3 normal3 = (p4-p3).cross(p1-p3);
if ((normal3.dot(p3) > 0.0) == (normal3.dot(p2) > 0.0)) { if ((normal3.dot(p3) > 0.0) == (normal3.dot(p2) > 0.0)) {
return 2; return 2;
} }
// Check vertex 4 // Check vertex 4
vec3 normal4 = (p2-p4).cross(p1-p4); vec3 normal4 = (p2-p4).cross(p1-p4);
if ((normal4.dot(p4) > 0.0) == (normal4.dot(p3) > 0.0)) { if ((normal4.dot(p4) > 0.0) == (normal4.dot(p3) > 0.0)) {
return 3; return 3;
} }
// The origin is in the tetrahedron, we return 0 // The origin is in the tetrahedron, we return 0
return 0; return 0;
} }
// Compute the penetration depth with the EPA algorithm.
/// This method computes the penetration depth and contact points between two
/// enlarged objects (with margin) where the original objects (without margin)
/// int32_tersect. An initial simplex that contains origin has been computed with
/// GJK algorithm. The EPA Algorithm will extend this simplex polytope to find
/// the correct penetration depth
void EPAAlgorithm::computePenetrationDepthAndContactPoints(const Simplex& simplex, void EPAAlgorithm::computePenetrationDepthAndContactPoints(const Simplex& simplex,
CollisionShapeInfo shape1Info, CollisionShapeInfo shape1Info,
const etk::Transform3D& transform1, const etk::Transform3D& transform1,
@ -70,46 +50,34 @@ void EPAAlgorithm::computePenetrationDepthAndContactPoints(const Simplex& simple
const etk::Transform3D& transform2, const etk::Transform3D& transform2,
vec3& v, vec3& v,
NarrowPhaseCallback* narrowPhaseCallback) { NarrowPhaseCallback* narrowPhaseCallback) {
PROFILE("EPAAlgorithm::computePenetrationDepthAndContactPoints()"); PROFILE("EPAAlgorithm::computePenetrationDepthAndContactPoints()");
assert(shape1Info.collisionShape->isConvex()); assert(shape1Info.collisionShape->isConvex());
assert(shape2Info.collisionShape->isConvex()); assert(shape2Info.collisionShape->isConvex());
const ConvexShape* shape1 = static_cast<const ConvexShape*>(shape1Info.collisionShape); const ConvexShape* shape1 = static_cast<const ConvexShape*>(shape1Info.collisionShape);
const ConvexShape* shape2 = static_cast<const ConvexShape*>(shape2Info.collisionShape); const ConvexShape* shape2 = static_cast<const ConvexShape*>(shape2Info.collisionShape);
void** shape1CachedCollisionData = shape1Info.cachedCollisionData; void** shape1CachedCollisionData = shape1Info.cachedCollisionData;
void** shape2CachedCollisionData = shape2Info.cachedCollisionData; void** shape2CachedCollisionData = shape2Info.cachedCollisionData;
vec3 suppPointsA[MAX_SUPPORT_POINTS]; // Support points of object A in local coordinates vec3 suppPointsA[MAX_SUPPORT_POINTS]; // Support points of object A in local coordinates
vec3 suppPointsB[MAX_SUPPORT_POINTS]; // Support points of object B in local coordinates vec3 suppPointsB[MAX_SUPPORT_POINTS]; // Support points of object B in local coordinates
vec3 points[MAX_SUPPORT_POINTS]; // Current points vec3 points[MAX_SUPPORT_POINTS]; // Current points
TrianglesStore triangleStore; // Store the triangles TrianglesStore triangleStore; // Store the triangles
TriangleEPA* triangleHeap[MAX_FACETS]; // Heap that contains the face TriangleEPA* triangleHeap[MAX_FACETS]; // Heap that contains the face
// candidate of the EPA algorithm // candidate of the EPA algorithm
// etk::Transform3D a point from local space of body 2 to local // etk::Transform3D a point from local space of body 2 to local
// space of body 1 (the GJK algorithm is done in local space of body 1) // space of body 1 (the GJK algorithm is done in local space of body 1)
etk::Transform3D body2Tobody1 = transform1.getInverse() * transform2; etk::Transform3D body2Tobody1 = transform1.getInverse() * transform2;
// Matrix that transform a direction from local // Matrix that transform a direction from local
// space of body 1 int32_to local space of body 2 // space of body 1 int32_to local space of body 2
etk::Quaternion rotateToBody2 = transform2.getOrientation().getInverse() * etk::Quaternion rotateToBody2 = transform2.getOrientation().getInverse() *
transform1.getOrientation(); transform1.getOrientation();
// Get the simplex computed previously by the GJK algorithm // Get the simplex computed previously by the GJK algorithm
uint32_t nbVertices = simplex.getSimplex(suppPointsA, suppPointsB, points); uint32_t nbVertices = simplex.getSimplex(suppPointsA, suppPointsB, points);
// Compute the tolerance // Compute the tolerance
float tolerance = MACHINE_EPSILON * simplex.getMaxLengthSquareOfAPoint(); float tolerance = FLT_EPSILON * simplex.getMaxLengthSquareOfAPoint();
// Number of triangles in the polytope // Number of triangles in the polytope
uint32_t nbTriangles = 0; uint32_t nbTriangles = 0;
// Clear the storing of triangles // Clear the storing of triangles
triangleStore.clear(); triangleStore.clear();
// Select an action according to the number of points in the simplex // Select an action according to the number of points in the simplex
// computed with GJK algorithm in order to obtain an initial polytope for // computed with GJK algorithm in order to obtain an initial polytope for
// The EPA algorithm. // The EPA algorithm.
@ -119,7 +87,6 @@ void EPAAlgorithm::computePenetrationDepthAndContactPoints(const Simplex& simple
// We have a touching contact with zero penetration depth. // We have a touching contact with zero penetration depth.
// We drop that kind of contact. Therefore, we return false // We drop that kind of contact. Therefore, we return false
return; return;
case 2: { case 2: {
// The simplex returned by GJK is a line segment d containing the origin. // The simplex returned by GJK is a line segment d containing the origin.
// We add two additional support points to construct a hexahedron (two tetrahedron // We add two additional support points to construct a hexahedron (two tetrahedron
@ -128,47 +95,37 @@ void EPAAlgorithm::computePenetrationDepthAndContactPoints(const Simplex& simple
// rotated of 120 degree around the d segment. The the three new points to // rotated of 120 degree around the d segment. The the three new points to
// construct the polytope are the three support points in those three directions // construct the polytope are the three support points in those three directions
// v1, v2 and v3. // v1, v2 and v3.
// Direction of the segment // Direction of the segment
vec3 d = (points[1] - points[0]).safeNormalized(); vec3 d = (points[1] - points[0]).safeNormalized();
// Choose the coordinate axis from the minimal absolute component of the vector d // Choose the coordinate axis from the minimal absolute component of the vector d
int32_t minAxis = d.absolute().getMinAxis(); int32_t minAxis = d.absolute().getMinAxis();
// Compute sin(60) // Compute sin(60)
const float sin60 = float(sqrt(3.0)) * 0.5f; const float sin60 = float(sqrt(3.0)) * 0.5f;
// Create a rotation quaternion to rotate the vector v1 to get the vectors // Create a rotation quaternion to rotate the vector v1 to get the vectors
// v2 and v3 // v2 and v3
etk::Quaternion rotationQuat(d.x() * sin60, d.y() * sin60, d.z() * sin60, 0.5); etk::Quaternion rotationQuat(d.x() * sin60, d.y() * sin60, d.z() * sin60, 0.5);
// Compute the vector v1, v2, v3 // Compute the vector v1, v2, v3
vec3 v1 = d.cross(vec3(minAxis == 0, minAxis == 1, minAxis == 2)); vec3 v1 = d.cross(vec3(minAxis == 0, minAxis == 1, minAxis == 2));
vec3 v2 = rotationQuat * v1; vec3 v2 = rotationQuat * v1;
vec3 v3 = rotationQuat * v2; vec3 v3 = rotationQuat * v2;
// Compute the support point in the direction of v1 // Compute the support point in the direction of v1
suppPointsA[2] = shape1->getLocalSupportPointWithMargin(v1, shape1CachedCollisionData); suppPointsA[2] = shape1->getLocalSupportPointWithMargin(v1, shape1CachedCollisionData);
suppPointsB[2] = body2Tobody1 * suppPointsB[2] = body2Tobody1 *
shape2->getLocalSupportPointWithMargin(rotateToBody2 * (-v1), shape2CachedCollisionData); shape2->getLocalSupportPointWithMargin(rotateToBody2 * (-v1), shape2CachedCollisionData);
points[2] = suppPointsA[2] - suppPointsB[2]; points[2] = suppPointsA[2] - suppPointsB[2];
// Compute the support point in the direction of v2 // Compute the support point in the direction of v2
suppPointsA[3] = shape1->getLocalSupportPointWithMargin(v2, shape1CachedCollisionData); suppPointsA[3] = shape1->getLocalSupportPointWithMargin(v2, shape1CachedCollisionData);
suppPointsB[3] = body2Tobody1 * suppPointsB[3] = body2Tobody1 *
shape2->getLocalSupportPointWithMargin(rotateToBody2 * (-v2), shape2CachedCollisionData); shape2->getLocalSupportPointWithMargin(rotateToBody2 * (-v2), shape2CachedCollisionData);
points[3] = suppPointsA[3] - suppPointsB[3]; points[3] = suppPointsA[3] - suppPointsB[3];
// Compute the support point in the direction of v3 // Compute the support point in the direction of v3
suppPointsA[4] = shape1->getLocalSupportPointWithMargin(v3, shape1CachedCollisionData); suppPointsA[4] = shape1->getLocalSupportPointWithMargin(v3, shape1CachedCollisionData);
suppPointsB[4] = body2Tobody1 * suppPointsB[4] = body2Tobody1 *
shape2->getLocalSupportPointWithMargin(rotateToBody2 * (-v3), shape2CachedCollisionData); shape2->getLocalSupportPointWithMargin(rotateToBody2 * (-v3), shape2CachedCollisionData);
points[4] = suppPointsA[4] - suppPointsB[4]; points[4] = suppPointsA[4] - suppPointsB[4];
// Now we have an hexahedron (two tetrahedron glued together). We can simply keep the // Now we have an hexahedron (two tetrahedron glued together). We can simply keep the
// tetrahedron that contains the origin in order that the initial polytope of the // tetrahedron that contains the origin in order that the initial polytope of the
// EPA algorithm is a tetrahedron, which is simpler to deal with. // EPA algorithm is a tetrahedron, which is simpler to deal with.
// If the origin is in the tetrahedron of points 0, 2, 3, 4 // If the origin is in the tetrahedron of points 0, 2, 3, 4
if (isOriginInTetrahedron(points[0], points[2], points[3], points[4]) == 0) { if (isOriginInTetrahedron(points[0], points[2], points[3], points[4]) == 0) {
// We use the point 4 instead of point 1 for the initial tetrahedron // We use the point 4 instead of point 1 for the initial tetrahedron
@ -187,7 +144,6 @@ void EPAAlgorithm::computePenetrationDepthAndContactPoints(const Simplex& simple
// The origin is not in the initial polytope // The origin is not in the initial polytope
return; return;
} }
// The polytope contains now 4 vertices // The polytope contains now 4 vertices
nbVertices = 4; nbVertices = 4;
} }
@ -196,28 +152,23 @@ void EPAAlgorithm::computePenetrationDepthAndContactPoints(const Simplex& simple
// if this tetrahedron contains the origin. If it is the case, we keep it and // if this tetrahedron contains the origin. If it is the case, we keep it and
// otherwise we remove the wrong vertex of the tetrahedron and go in the case // otherwise we remove the wrong vertex of the tetrahedron and go in the case
// where the GJK algorithm compute a simplex of three vertices. // where the GJK algorithm compute a simplex of three vertices.
// Check if the tetrahedron contains the origin (or wich is the wrong vertex otherwise) // Check if the tetrahedron contains the origin (or wich is the wrong vertex otherwise)
int32_t badVertex = isOriginInTetrahedron(points[0], points[1], points[2], points[3]); int32_t badVertex = isOriginInTetrahedron(points[0], points[1], points[2], points[3]);
// If the origin is in the tetrahedron // If the origin is in the tetrahedron
if (badVertex == 0) { if (badVertex == 0) {
// The tetrahedron is a correct initial polytope for the EPA algorithm. // The tetrahedron is a correct initial polytope for the EPA algorithm.
// Therefore, we construct the tetrahedron. // Therefore, we construct the tetrahedron.
// Comstruct the 4 triangle faces of the tetrahedron // Comstruct the 4 triangle faces of the tetrahedron
TriangleEPA* face0 = triangleStore.newTriangle(points, 0, 1, 2); TriangleEPA* face0 = triangleStore.newTriangle(points, 0, 1, 2);
TriangleEPA* face1 = triangleStore.newTriangle(points, 0, 3, 1); TriangleEPA* face1 = triangleStore.newTriangle(points, 0, 3, 1);
TriangleEPA* face2 = triangleStore.newTriangle(points, 0, 2, 3); TriangleEPA* face2 = triangleStore.newTriangle(points, 0, 2, 3);
TriangleEPA* face3 = triangleStore.newTriangle(points, 1, 3, 2); TriangleEPA* face3 = triangleStore.newTriangle(points, 1, 3, 2);
// If the constructed tetrahedron is not correct // If the constructed tetrahedron is not correct
if (!((face0 != NULL) && (face1 != NULL) && (face2 != NULL) && (face3 != NULL) if (!((face0 != NULL) && (face1 != NULL) && (face2 != NULL) && (face3 != NULL)
&& face0->getDistSquare() > 0.0 && face1->getDistSquare() > 0.0 && face0->getDistSquare() > 0.0 && face1->getDistSquare() > 0.0
&& face2->getDistSquare() > 0.0 && face3->getDistSquare() > 0.0)) { && face2->getDistSquare() > 0.0 && face3->getDistSquare() > 0.0)) {
return; return;
} }
// Associate the edges of neighbouring triangle faces // Associate the edges of neighbouring triangle faces
link(EdgeEPA(face0, 0), EdgeEPA(face1, 2)); link(EdgeEPA(face0, 0), EdgeEPA(face1, 2));
link(EdgeEPA(face0, 1), EdgeEPA(face3, 2)); link(EdgeEPA(face0, 1), EdgeEPA(face3, 2));
@ -225,26 +176,21 @@ void EPAAlgorithm::computePenetrationDepthAndContactPoints(const Simplex& simple
link(EdgeEPA(face1, 0), EdgeEPA(face2, 2)); link(EdgeEPA(face1, 0), EdgeEPA(face2, 2));
link(EdgeEPA(face1, 1), EdgeEPA(face3, 0)); link(EdgeEPA(face1, 1), EdgeEPA(face3, 0));
link(EdgeEPA(face2, 1), EdgeEPA(face3, 1)); link(EdgeEPA(face2, 1), EdgeEPA(face3, 1));
// Add the triangle faces in the candidate heap // Add the triangle faces in the candidate heap
addFaceCandidate(face0, triangleHeap, nbTriangles, DECIMAL_LARGEST); addFaceCandidate(face0, triangleHeap, nbTriangles, FLT_MAX);
addFaceCandidate(face1, triangleHeap, nbTriangles, DECIMAL_LARGEST); addFaceCandidate(face1, triangleHeap, nbTriangles, FLT_MAX);
addFaceCandidate(face2, triangleHeap, nbTriangles, DECIMAL_LARGEST); addFaceCandidate(face2, triangleHeap, nbTriangles, FLT_MAX);
addFaceCandidate(face3, triangleHeap, nbTriangles, DECIMAL_LARGEST); addFaceCandidate(face3, triangleHeap, nbTriangles, FLT_MAX);
break; break;
} }
// The tetrahedron contains a wrong vertex (the origin is not inside the tetrahedron) // The tetrahedron contains a wrong vertex (the origin is not inside the tetrahedron)
// Remove the wrong vertex and continue to the next case with the // Remove the wrong vertex and continue to the next case with the
// three remaining vertices // three remaining vertices
if (badVertex < 4) { if (badVertex < 4) {
suppPointsA[badVertex-1] = suppPointsA[3]; suppPointsA[badVertex-1] = suppPointsA[3];
suppPointsB[badVertex-1] = suppPointsB[3]; suppPointsB[badVertex-1] = suppPointsB[3];
points[badVertex-1] = points[3]; points[badVertex-1] = points[3];
} }
// We have removed the wrong vertex // We have removed the wrong vertex
nbVertices = 3; nbVertices = 3;
} }
@ -254,12 +200,10 @@ void EPAAlgorithm::computePenetrationDepthAndContactPoints(const Simplex& simple
// vertices are the support points in the "n" and "-n" direction // vertices are the support points in the "n" and "-n" direction
// where "n" is the normal of the triangle. Then, we use only the // where "n" is the normal of the triangle. Then, we use only the
// tetrahedron that contains the origin. // tetrahedron that contains the origin.
// Compute the normal of the triangle // Compute the normal of the triangle
vec3 v1 = points[1] - points[0]; vec3 v1 = points[1] - points[0];
vec3 v2 = points[2] - points[0]; vec3 v2 = points[2] - points[0];
vec3 n = v1.cross(v2); vec3 n = v1.cross(v2);
// Compute the two new vertices to obtain a hexahedron // Compute the two new vertices to obtain a hexahedron
suppPointsA[3] = shape1->getLocalSupportPointWithMargin(n, shape1CachedCollisionData); suppPointsA[3] = shape1->getLocalSupportPointWithMargin(n, shape1CachedCollisionData);
suppPointsB[3] = body2Tobody1 * suppPointsB[3] = body2Tobody1 *
@ -269,18 +213,15 @@ void EPAAlgorithm::computePenetrationDepthAndContactPoints(const Simplex& simple
suppPointsB[4] = body2Tobody1 * suppPointsB[4] = body2Tobody1 *
shape2->getLocalSupportPointWithMargin(rotateToBody2 * n, shape2CachedCollisionData); shape2->getLocalSupportPointWithMargin(rotateToBody2 * n, shape2CachedCollisionData);
points[4] = suppPointsA[4] - suppPointsB[4]; points[4] = suppPointsA[4] - suppPointsB[4];
TriangleEPA* face0 = NULL; TriangleEPA* face0 = NULL;
TriangleEPA* face1 = NULL; TriangleEPA* face1 = NULL;
TriangleEPA* face2 = NULL; TriangleEPA* face2 = NULL;
TriangleEPA* face3 = NULL; TriangleEPA* face3 = NULL;
// If the origin is in the first tetrahedron // If the origin is in the first tetrahedron
if (isOriginInTetrahedron(points[0], points[1], if (isOriginInTetrahedron(points[0], points[1],
points[2], points[3]) == 0) { points[2], points[3]) == 0) {
// The tetrahedron is a correct initial polytope for the EPA algorithm. // The tetrahedron is a correct initial polytope for the EPA algorithm.
// Therefore, we construct the tetrahedron. // Therefore, we construct the tetrahedron.
// Comstruct the 4 triangle faces of the tetrahedron // Comstruct the 4 triangle faces of the tetrahedron
face0 = triangleStore.newTriangle(points, 0, 1, 2); face0 = triangleStore.newTriangle(points, 0, 1, 2);
face1 = triangleStore.newTriangle(points, 0, 3, 1); face1 = triangleStore.newTriangle(points, 0, 3, 1);
@ -289,10 +230,8 @@ void EPAAlgorithm::computePenetrationDepthAndContactPoints(const Simplex& simple
} }
else if (isOriginInTetrahedron(points[0], points[1], else if (isOriginInTetrahedron(points[0], points[1],
points[2], points[4]) == 0) { points[2], points[4]) == 0) {
// The tetrahedron is a correct initial polytope for the EPA algorithm. // The tetrahedron is a correct initial polytope for the EPA algorithm.
// Therefore, we construct the tetrahedron. // Therefore, we construct the tetrahedron.
// Comstruct the 4 triangle faces of the tetrahedron // Comstruct the 4 triangle faces of the tetrahedron
face0 = triangleStore.newTriangle(points, 0, 1, 2); face0 = triangleStore.newTriangle(points, 0, 1, 2);
face1 = triangleStore.newTriangle(points, 0, 4, 1); face1 = triangleStore.newTriangle(points, 0, 4, 1);
@ -302,14 +241,12 @@ void EPAAlgorithm::computePenetrationDepthAndContactPoints(const Simplex& simple
else { else {
return; return;
} }
// If the constructed tetrahedron is not correct // If the constructed tetrahedron is not correct
if (!((face0 != NULL) && (face1 != NULL) && (face2 != NULL) && (face3 != NULL) if (!((face0 != NULL) && (face1 != NULL) && (face2 != NULL) && (face3 != NULL)
&& face0->getDistSquare() > 0.0 && face1->getDistSquare() > 0.0 && face0->getDistSquare() > 0.0 && face1->getDistSquare() > 0.0
&& face2->getDistSquare() > 0.0 && face3->getDistSquare() > 0.0)) { && face2->getDistSquare() > 0.0 && face3->getDistSquare() > 0.0)) {
return; return;
} }
// Associate the edges of neighbouring triangle faces // Associate the edges of neighbouring triangle faces
link(EdgeEPA(face0, 0), EdgeEPA(face1, 2)); link(EdgeEPA(face0, 0), EdgeEPA(face1, 2));
link(EdgeEPA(face0, 1), EdgeEPA(face3, 2)); link(EdgeEPA(face0, 1), EdgeEPA(face3, 2));
@ -317,36 +254,27 @@ void EPAAlgorithm::computePenetrationDepthAndContactPoints(const Simplex& simple
link(EdgeEPA(face1, 0), EdgeEPA(face2, 2)); link(EdgeEPA(face1, 0), EdgeEPA(face2, 2));
link(EdgeEPA(face1, 1), EdgeEPA(face3, 0)); link(EdgeEPA(face1, 1), EdgeEPA(face3, 0));
link(EdgeEPA(face2, 1), EdgeEPA(face3, 1)); link(EdgeEPA(face2, 1), EdgeEPA(face3, 1));
// Add the triangle faces in the candidate heap // Add the triangle faces in the candidate heap
addFaceCandidate(face0, triangleHeap, nbTriangles, DECIMAL_LARGEST); addFaceCandidate(face0, triangleHeap, nbTriangles, FLT_MAX);
addFaceCandidate(face1, triangleHeap, nbTriangles, DECIMAL_LARGEST); addFaceCandidate(face1, triangleHeap, nbTriangles, FLT_MAX);
addFaceCandidate(face2, triangleHeap, nbTriangles, DECIMAL_LARGEST); addFaceCandidate(face2, triangleHeap, nbTriangles, FLT_MAX);
addFaceCandidate(face3, triangleHeap, nbTriangles, DECIMAL_LARGEST); addFaceCandidate(face3, triangleHeap, nbTriangles, FLT_MAX);
nbVertices = 4; nbVertices = 4;
} }
break; break;
} }
// At this point, we have a polytope that contains the origin. Therefore, we // At this point, we have a polytope that contains the origin. Therefore, we
// can run the EPA algorithm. // can run the EPA algorithm.
if (nbTriangles == 0) { if (nbTriangles == 0) {
return; return;
} }
TriangleEPA* triangle = 0; TriangleEPA* triangle = 0;
float upperBoundSquarePenDepth = DECIMAL_LARGEST; float upperBoundSquarePenDepth = FLT_MAX;
do { do {
triangle = triangleHeap[0]; triangle = triangleHeap[0];
// Get the next candidate face (the face closest to the origin) // Get the next candidate face (the face closest to the origin)
std::pop_heap(&triangleHeap[0], &triangleHeap[nbTriangles], mTriangleComparison); std::pop_heap(&triangleHeap[0], &triangleHeap[nbTriangles], m_triangleComparison);
nbTriangles--; nbTriangles--;
// If the candidate face in the heap is not obsolete // If the candidate face in the heap is not obsolete
if (!triangle->getIsObsolete()) { if (!triangle->getIsObsolete()) {
// If we have reached the maximum number of support points // If we have reached the maximum number of support points
@ -354,7 +282,6 @@ void EPAAlgorithm::computePenetrationDepthAndContactPoints(const Simplex& simple
assert(false); assert(false);
break; break;
} }
// Compute the support point of the Minkowski // Compute the support point of the Minkowski
// difference (A-B) in the closest point direction // difference (A-B) in the closest point direction
suppPointsA[nbVertices] = shape1->getLocalSupportPointWithMargin( suppPointsA[nbVertices] = shape1->getLocalSupportPointWithMargin(
@ -363,10 +290,8 @@ void EPAAlgorithm::computePenetrationDepthAndContactPoints(const Simplex& simple
shape2->getLocalSupportPointWithMargin(rotateToBody2 * shape2->getLocalSupportPointWithMargin(rotateToBody2 *
(-triangle->getClosestPoint()), shape2CachedCollisionData); (-triangle->getClosestPoint()), shape2CachedCollisionData);
points[nbVertices] = suppPointsA[nbVertices] - suppPointsB[nbVertices]; points[nbVertices] = suppPointsA[nbVertices] - suppPointsB[nbVertices];
int32_t indexNewVertex = nbVertices; int32_t indexNewVertex = nbVertices;
nbVertices++; nbVertices++;
// Update the upper bound of the penetration depth // Update the upper bound of the penetration depth
float wDotv = points[indexNewVertex].dot(triangle->getClosestPoint()); float wDotv = points[indexNewVertex].dot(triangle->getClosestPoint());
assert(wDotv > 0.0); assert(wDotv > 0.0);
@ -374,7 +299,6 @@ void EPAAlgorithm::computePenetrationDepthAndContactPoints(const Simplex& simple
if (wDotVSquare < upperBoundSquarePenDepth) { if (wDotVSquare < upperBoundSquarePenDepth) {
upperBoundSquarePenDepth = wDotVSquare; upperBoundSquarePenDepth = wDotVSquare;
} }
// Compute the error // Compute the error
float error = wDotv - triangle->getDistSquare(); float error = wDotv - triangle->getDistSquare();
if (error <= etk::max(tolerance, REL_ERROR_SQUARE * wDotv) || if (error <= etk::max(tolerance, REL_ERROR_SQUARE * wDotv) ||
@ -383,7 +307,6 @@ void EPAAlgorithm::computePenetrationDepthAndContactPoints(const Simplex& simple
points[indexNewVertex] == points[(*triangle)[2]]) { points[indexNewVertex] == points[(*triangle)[2]]) {
break; break;
} }
// Now, we compute the silhouette cast by the new vertex. The current triangle // Now, we compute the silhouette cast by the new vertex. The current triangle
// face will not be in the convex hull. We start the local recursive silhouette // face will not be in the convex hull. We start the local recursive silhouette
// algorithm from the current triangle face. // algorithm from the current triangle face.
@ -391,7 +314,6 @@ void EPAAlgorithm::computePenetrationDepthAndContactPoints(const Simplex& simple
if (!triangle->computeSilhouette(points, indexNewVertex, triangleStore)) { if (!triangle->computeSilhouette(points, indexNewVertex, triangleStore)) {
break; break;
} }
// Add all the new triangle faces computed with the silhouette algorithm // Add all the new triangle faces computed with the silhouette algorithm
// to the candidates list of faces of the current polytope // to the candidates list of faces of the current polytope
while(i != triangleStore.getNbTriangles()) { while(i != triangleStore.getNbTriangles()) {
@ -401,7 +323,6 @@ void EPAAlgorithm::computePenetrationDepthAndContactPoints(const Simplex& simple
} }
} }
} while(nbTriangles > 0 && triangleHeap[0]->getDistSquare() <= upperBoundSquarePenDepth); } while(nbTriangles > 0 && triangleHeap[0]->getDistSquare() <= upperBoundSquarePenDepth);
// Compute the contact info // Compute the contact info
v = transform1.getOrientation() * triangle->getClosestPoint(); v = transform1.getOrientation() * triangle->getClosestPoint();
vec3 pALocal = triangle->computeClosestPointOfObject(suppPointsA); vec3 pALocal = triangle->computeClosestPointOfObject(suppPointsA);
@ -409,12 +330,10 @@ void EPAAlgorithm::computePenetrationDepthAndContactPoints(const Simplex& simple
vec3 normal = v.safeNormalized(); vec3 normal = v.safeNormalized();
float penetrationDepth = v.length(); float penetrationDepth = v.length();
assert(penetrationDepth > 0.0); assert(penetrationDepth > 0.0);
if (normal.length2() < FLT_EPSILON) {
if (normal.length2() < MACHINE_EPSILON) return; return;
}
// Create the contact info object // Create the contact info object
ContactPointInfo contactInfo(shape1Info.proxyShape, shape2Info.proxyShape, shape1Info.collisionShape, ContactPointInfo contactInfo(shape1Info.proxyShape, shape2Info.proxyShape, shape1Info.collisionShape, shape2Info.collisionShape, normal, penetrationDepth, pALocal, pBLocal);
shape2Info.collisionShape, normal, penetrationDepth, pALocal, pBLocal);
narrowPhaseCallback->notifyContact(shape1Info.overlappingPair, contactInfo); narrowPhaseCallback->notifyContact(shape1Info.overlappingPair, contactInfo);
} }

205
ephysics/collision/narrowphase/EPA/EPAAlgorithm.hpp
View File

@ -4,8 +4,6 @@
* @license BSD 3 clauses (see license file) * @license BSD 3 clauses (see license file)
*/ */
#pragma once #pragma once
// Libraries
#include <ephysics/collision/narrowphase/GJK/Simplex.hpp> #include <ephysics/collision/narrowphase/GJK/Simplex.hpp>
#include <ephysics/collision/shapes/CollisionShape.hpp> #include <ephysics/collision/shapes/CollisionShape.hpp>
#include <ephysics/collision/CollisionShapeInfo.hpp> #include <ephysics/collision/CollisionShapeInfo.hpp>
@ -16,123 +14,92 @@
#include <ephysics/memory/MemoryAllocator.hpp> #include <ephysics/memory/MemoryAllocator.hpp>
#include <algorithm> #include <algorithm>
/// ReactPhysics3D namespace
namespace ephysics { namespace ephysics {
/// Maximum number of support points of the polytope
// ---------- Constants ---------- // const uint32_t MAX_SUPPORT_POINTS = 100;
/// Maximum number of facets of the polytope
/// Maximum number of support points of the polytope const uint32_t MAX_FACETS = 200;
const uint32_t MAX_SUPPORT_POINTS = 100; /**
* @brief Class TriangleComparison
/// Maximum number of facets of the polytope * This class allows the comparison of two triangles in the heap
const uint32_t MAX_FACETS = 200; * The comparison between two triangles is made using their square distance to the closest
* point to the origin. The goal is that in the heap, the first triangle is the one with the
* smallest square distance.
// Class TriangleComparison */
/** class TriangleComparison {
* This class allows the comparison of two triangles in the heap public:
* The comparison between two triangles is made using their square distance to the closest /**
* point to the origin. The goal is that in the heap, the first triangle is the one with the * @brief Comparison operator
* smallest square distance. */
*/ bool operator()(const TriangleEPA* face1, const TriangleEPA* face2) {
class TriangleComparison { return (face1->getDistSquare() > face2->getDistSquare());
}
public: };
/**
/// Comparison operator * @brief Class EPAAlgorithm
bool operator()(const TriangleEPA* face1, const TriangleEPA* face2) { * This class is the implementation of the Expanding Polytope Algorithm (EPA).
return (face1->getDistSquare() > face2->getDistSquare()); * The EPA algorithm computes the penetration depth and contact points between
} * two enlarged objects (with margin) where the original objects (without margin)
}; * int32_tersect. The penetration depth of a pair of int32_tersecting objects A and B is
* the length of a point on the boundary of the Minkowski sum (A-B) closest to the
* origin. The goal of the EPA algorithm is to start with an initial simplex polytope
// Class EPAAlgorithm * that contains the origin and expend it in order to find the point on the boundary
/** * of (A-B) that is closest to the origin. An initial simplex that contains origin
* This class is the implementation of the Expanding Polytope Algorithm (EPA). * has been computed wit GJK algorithm. The EPA Algorithm will extend this simplex
* The EPA algorithm computes the penetration depth and contact points between * polytope to find the correct penetration depth. The implementation of the EPA
* two enlarged objects (with margin) where the original objects (without margin) * algorithm is based on the book "Collision Detection in 3D Environments".
* int32_tersect. The penetration depth of a pair of int32_tersecting objects A and B is */
* the length of a point on the boundary of the Minkowski sum (A-B) closest to the class EPAAlgorithm {
* origin. The goal of the EPA algorithm is to start with an initial simplex polytope private:
* that contains the origin and expend it in order to find the point on the boundary MemoryAllocator* m_memoryAllocator; //!< Reference to the memory allocator
* of (A-B) that is closest to the origin. An initial simplex that contains origin TriangleComparison m_triangleComparison; //!< Triangle comparison operator
* has been computed wit GJK algorithm. The EPA Algorithm will extend this simplex /// Private copy-constructor
* polytope to find the correct penetration depth. The implementation of the EPA EPAAlgorithm(const EPAAlgorithm& _algorithm);
* algorithm is based on the book "Collision Detection in 3D Environments". /// Private assignment operator
*/ EPAAlgorithm& operator=(const EPAAlgorithm& _algorithm);
class EPAAlgorithm { /// Add a triangle face in the candidate triangle heap
void addFaceCandidate(TriangleEPA* _triangle,
private: TriangleEPA** _heap,
uint32_t& _nbTriangles,
// -------------------- Attributes -------------------- // float _upperBoundSquarePenDepth) {
// If the closest point of the affine hull of triangle
/// Reference to the memory allocator // points is int32_ternal to the triangle and if the distance
MemoryAllocator* m_memoryAllocator; // of the closest point from the origin is at most the
// penetration depth upper bound
/// Triangle comparison operator if ( _triangle->isClosestPointInternalToTriangle()
TriangleComparison mTriangleComparison; && _triangle->getDistSquare() <= _upperBoundSquarePenDepth) {
// Add the triangle face to the list of candidates
// -------------------- Methods -------------------- // _heap[_nbTriangles] = _triangle;
_nbTriangles++;
/// Private copy-constructor std::push_heap(&_heap[0], &_heap[_nbTriangles], m_triangleComparison);
EPAAlgorithm(const EPAAlgorithm& algorithm); }
}
/// Private assignment operator // Decide if the origin is in the tetrahedron.
EPAAlgorithm& operator=(const EPAAlgorithm& algorithm); /// Return 0 if the origin is in the tetrahedron and return the number (1,2,3 or 4) of
/// the vertex that is wrong if the origin is not in the tetrahedron
/// Add a triangle face in the candidate triangle heap int32_t isOriginInTetrahedron(const vec3& _p1, const vec3& _p2, const vec3& _p3, const vec3& _p4) const;
void addFaceCandidate(TriangleEPA* triangle, TriangleEPA** heap, uint32_t& nbTriangles, public:
float upperBoundSquarePenDepth); /// Constructor
EPAAlgorithm();
/// Decide if the origin is in the tetrahedron. /// Destructor
int32_t isOriginInTetrahedron(const vec3& p1, const vec3& p2, ~EPAAlgorithm();
const vec3& p3, const vec3& p4) const; /// Initalize the algorithm
void init(MemoryAllocator* _memoryAllocator) {
public: m_memoryAllocator = _memoryAllocator;
}
// -------------------- Methods -------------------- // // Compute the penetration depth with the EPA algorithm.
/// This method computes the penetration depth and contact points between two
/// Constructor /// enlarged objects (with margin) where the original objects (without margin)
EPAAlgorithm(); /// int32_tersect. An initial simplex that contains origin has been computed with
/// GJK algorithm. The EPA Algorithm will extend this simplex polytope to find
/// Destructor /// the correct penetration depth
~EPAAlgorithm(); void computePenetrationDepthAndContactPoints(const Simplex& _simplex,
CollisionShapeInfo _shape1Info,
/// Initalize the algorithm const etk::Transform3D& _transform1,
void init(MemoryAllocator* memoryAllocator); CollisionShapeInfo _shape2Info,
const etk::Transform3D& _transform2,
/// Compute the penetration depth with EPA algorithm. vec3& _v,
void computePenetrationDepthAndContactPoints(const Simplex& simplex, NarrowPhaseCallback* _narrowPhaseCallback);
CollisionShapeInfo shape1Info, };
const etk::Transform3D& transform1,
CollisionShapeInfo shape2Info,
const etk::Transform3D& transform2,
vec3& v,
NarrowPhaseCallback* narrowPhaseCallback);
};
// Add a triangle face in the candidate triangle heap in the EPA algorithm
inline void EPAAlgorithm::addFaceCandidate(TriangleEPA* triangle, TriangleEPA** heap,
uint32_t& nbTriangles, float upperBoundSquarePenDepth) {
// If the closest point of the affine hull of triangle
// points is int32_ternal to the triangle and if the distance
// of the closest point from the origin is at most the
// penetration depth upper bound
if (triangle->isClosestPointInternalToTriangle() &&
triangle->getDistSquare() <= upperBoundSquarePenDepth) {
// Add the triangle face to the list of candidates
heap[nbTriangles] = triangle;
nbTriangles++;
std::push_heap(&heap[0], &heap[nbTriangles], mTriangleComparison);
}
}
// Initalize the algorithm
inline void EPAAlgorithm::init(MemoryAllocator* memoryAllocator) {
m_memoryAllocator = memoryAllocator;
}
} }

62
ephysics/collision/narrowphase/EPA/EdgeEPA.cpp
View File

@ -3,115 +3,89 @@
* @copyright 2010-2016 Daniel Chappuis * @copyright 2010-2016 Daniel Chappuis
* @license BSD 3 clauses (see license file) * @license BSD 3 clauses (see license file)
*/ */
// Libraries
#include <ephysics/collision/narrowphase/EPA/EdgeEPA.hpp> #include <ephysics/collision/narrowphase/EPA/EdgeEPA.hpp>
#include <ephysics/collision/narrowphase/EPA/TriangleEPA.hpp> #include <ephysics/collision/narrowphase/EPA/TriangleEPA.hpp>
#include <ephysics/collision/narrowphase/EPA/TrianglesStore.hpp> #include <ephysics/collision/narrowphase/EPA/TrianglesStore.hpp>
#include <cassert>
// We want to use the ReactPhysics3D namespace
using namespace ephysics; using namespace ephysics;
// Constructor
EdgeEPA::EdgeEPA() { EdgeEPA::EdgeEPA() {
} }
// Constructor
EdgeEPA::EdgeEPA(TriangleEPA* ownerTriangle, int32_t index) EdgeEPA::EdgeEPA(TriangleEPA* ownerTriangle, int32_t index)
: mOwnerTriangle(ownerTriangle), mIndex(index) { : m_ownerTriangle(ownerTriangle), m_index(index) {
assert(index >= 0 && index < 3); assert(index >= 0 && index < 3);
} }
// Copy-constructor
EdgeEPA::EdgeEPA(const EdgeEPA& edge) { EdgeEPA::EdgeEPA(const EdgeEPA& edge) {
mOwnerTriangle = edge.mOwnerTriangle; m_ownerTriangle = edge.m_ownerTriangle;
mIndex = edge.mIndex; m_index = edge.m_index;
} }
// Destructor
EdgeEPA::~EdgeEPA() { EdgeEPA::~EdgeEPA() {
} }
// Return the index of the source vertex of the edge (vertex starting the edge)
uint32_t EdgeEPA::getSourceVertexIndex() const { uint32_t EdgeEPA::getSourceVertexIndex() const {
return (*mOwnerTriangle)[mIndex]; return (*m_ownerTriangle)[m_index];
} }
// Return the index of the target vertex of the edge (vertex ending the edge)
uint32_t EdgeEPA::getTargetVertexIndex() const { uint32_t EdgeEPA::getTargetVertexIndex() const {
return (*mOwnerTriangle)[indexOfNextCounterClockwiseEdge(mIndex)]; return (*m_ownerTriangle)[indexOfNextCounterClockwiseEdge(m_index)];
} }
// Execute the recursive silhouette algorithm from this edge
bool EdgeEPA::computeSilhouette(const vec3* vertices, uint32_t indexNewVertex, bool EdgeEPA::computeSilhouette(const vec3* vertices, uint32_t indexNewVertex,
TrianglesStore& triangleStore) { TrianglesStore& triangleStore) {
// If the edge has not already been visited // If the edge has not already been visited
if (!mOwnerTriangle->getIsObsolete()) { if (!m_ownerTriangle->getIsObsolete()) {
// If the triangle of this edge is not visible from the given point // If the triangle of this edge is not visible from the given point
if (!mOwnerTriangle->isVisibleFromVertex(vertices, indexNewVertex)) { if (!m_ownerTriangle->isVisibleFromVertex(vertices, indexNewVertex)) {
TriangleEPA* triangle = triangleStore.newTriangle(vertices, indexNewVertex, TriangleEPA* triangle = triangleStore.newTriangle(vertices, indexNewVertex,
getTargetVertexIndex(), getTargetVertexIndex(),
getSourceVertexIndex()); getSourceVertexIndex());
// If the triangle has been created // If the triangle has been created
if (triangle != NULL) { if (triangle != nullptr) {
halfLink(EdgeEPA(triangle, 1), *this); halfLink(EdgeEPA(triangle, 1), *this);
return true; return true;
} }
return false; return false;
} } else {
else {
// The current triangle is visible and therefore obsolete // The current triangle is visible and therefore obsolete
mOwnerTriangle->setIsObsolete(true); m_ownerTriangle->setIsObsolete(true);
int32_t backup = triangleStore.getNbTriangles(); int32_t backup = triangleStore.getNbTriangles();
if(!m_ownerTriangle->getAdjacentEdge(indexOfNextCounterClockwiseEdge(
if(!mOwnerTriangle->getAdjacentEdge(indexOfNextCounterClockwiseEdge( this->m_index)).computeSilhouette(vertices,
this->mIndex)).computeSilhouette(vertices,
indexNewVertex, indexNewVertex,
triangleStore)) { triangleStore)) {
mOwnerTriangle->setIsObsolete(false); m_ownerTriangle->setIsObsolete(false);
TriangleEPA* triangle = triangleStore.newTriangle(vertices, indexNewVertex, TriangleEPA* triangle = triangleStore.newTriangle(vertices, indexNewVertex,
getTargetVertexIndex(), getTargetVertexIndex(),
getSourceVertexIndex()); getSourceVertexIndex());
// If the triangle has been created // If the triangle has been created
if (triangle != NULL) { if (triangle != nullptr) {
halfLink(EdgeEPA(triangle, 1), *this); halfLink(EdgeEPA(triangle, 1), *this);
return true; return true;
} }
return false; return false;
} } else if (!m_ownerTriangle->getAdjacentEdge(indexOfPreviousCounterClockwiseEdge(
else if (!mOwnerTriangle->getAdjacentEdge(indexOfPreviousCounterClockwiseEdge( this->m_index)).computeSilhouette(vertices,
this->mIndex)).computeSilhouette(vertices,
indexNewVertex, indexNewVertex,
triangleStore)) { triangleStore)) {
mOwnerTriangle->setIsObsolete(false); m_ownerTriangle->setIsObsolete(false);
triangleStore.setNbTriangles(backup); triangleStore.setNbTriangles(backup);
TriangleEPA* triangle = triangleStore.newTriangle(vertices, indexNewVertex, TriangleEPA* triangle = triangleStore.newTriangle(vertices, indexNewVertex,
getTargetVertexIndex(), getTargetVertexIndex(),
getSourceVertexIndex()); getSourceVertexIndex());
if (triangle != NULL) { if (triangle != NULL) {
halfLink(EdgeEPA(triangle, 1), *this); halfLink(EdgeEPA(triangle, 1), *this);
return true; return true;
} }
return false; return false;
} }
} }
} }
return true; return true;
} }

70
ephysics/collision/narrowphase/EPA/EdgeEPA.hpp
View File

@ -4,95 +4,61 @@
* @license BSD 3 clauses (see license file) * @license BSD 3 clauses (see license file)
*/ */
#pragma once #pragma once
// Libraries
#include <ephysics/mathematics/mathematics.hpp> #include <ephysics/mathematics/mathematics.hpp>
/// ReactPhysics3D namespace
namespace ephysics { namespace ephysics {
// Class declarations
class TriangleEPA; class TriangleEPA;
class TrianglesStore; class TrianglesStore;
// Class EdgeEPA
/** /**
* @brief Class EdgeEPA
* This class represents an edge of the current polytope in the EPA algorithm. * This class represents an edge of the current polytope in the EPA algorithm.
*/ */
class EdgeEPA { class EdgeEPA {
private: private:
// -------------------- Attributes -------------------- //
/// Pointer to the triangle that contains this edge /// Pointer to the triangle that contains this edge
TriangleEPA* mOwnerTriangle; TriangleEPA* m_ownerTriangle;
/// Index of the edge in the triangle (between 0 and 2). /// Index of the edge in the triangle (between 0 and 2).
/// The edge with index i connect triangle vertices i and (i+1 % 3) /// The edge with index i connect triangle vertices i and (i+1 % 3)
int32_t mIndex; int32_t m_index;
public: public:
// -------------------- Methods -------------------- //
/// Constructor /// Constructor
EdgeEPA(); EdgeEPA();
/// Constructor /// Constructor
EdgeEPA(TriangleEPA* ownerTriangle, int32_t index); EdgeEPA(TriangleEPA* ownerTriangle, int32_t index);
/// Copy-constructor /// Copy-constructor
EdgeEPA(const EdgeEPA& edge); EdgeEPA(const EdgeEPA& edge);
/// Destructor /// Destructor
~EdgeEPA(); ~EdgeEPA();
/// Return the pointer to the owner triangle /// Return the pointer to the owner triangle
TriangleEPA* getOwnerTriangle() const; TriangleEPA* getOwnerTriangle() const {
return m_ownerTriangle;
}
/// Return the index of the edge in the triangle /// Return the index of the edge in the triangle
int32_t getIndex() const; int32_t getIndex() const {
return m_index;
}
/// Return index of the source vertex of the edge /// Return index of the source vertex of the edge
uint32_t getSourceVertexIndex() const; uint32_t getSourceVertexIndex() const;
/// Return the index of the target vertex of the edge /// Return the index of the target vertex of the edge
uint32_t getTargetVertexIndex() const; uint32_t getTargetVertexIndex() const;
/// Execute the recursive silhouette algorithm from this edge /// Execute the recursive silhouette algorithm from this edge
bool computeSilhouette(const vec3* vertices, uint32_t index, TrianglesStore& triangleStore); bool computeSilhouette(const vec3* vertices, uint32_t index, TrianglesStore& triangleStore);
/// Assignment operator /// Assignment operator
EdgeEPA& operator=(const EdgeEPA& edge); EdgeEPA& operator=(const EdgeEPA& edge) {
m_ownerTriangle = edge.m_ownerTriangle;
m_index = edge.m_index;
return *this;
}
}; };
// Return the pointer to the owner triangle
inline TriangleEPA* EdgeEPA::getOwnerTriangle() const {
return mOwnerTriangle;
}
// Return the edge index
inline int32_t EdgeEPA::getIndex() const {
return mIndex;
}
// Assignment operator
inline EdgeEPA& EdgeEPA::operator=(const EdgeEPA& edge) {
mOwnerTriangle = edge.mOwnerTriangle;
mIndex = edge.mIndex;
return *this;
}
// Return the index of the next counter-clockwise edge of the ownver triangle // Return the index of the next counter-clockwise edge of the ownver triangle
inline int32_t indexOfNextCounterClockwiseEdge(int32_t i) { inline int32_t indexOfNextCounterClockwiseEdge(int32_t _iii) {
return (i + 1) % 3; return (_iii + 1) % 3;
} }
// Return the index of the previous counter-clockwise edge of the ownver triangle // Return the index of the previous counter-clockwise edge of the ownver triangle
inline int32_t indexOfPreviousCounterClockwiseEdge(int32_t i) { inline int32_t indexOfPreviousCounterClockwiseEdge(int32_t _iii) {
return (i + 2) % 3; return (_iii + 2) % 3;
} }
} }

121
ephysics/collision/narrowphase/EPA/TriangleEPA.cpp
View File

@ -4,132 +4,97 @@
* @license BSD 3 clauses (see license file) * @license BSD 3 clauses (see license file)
*/ */
// Libraries
#include <ephysics/collision/narrowphase/EPA/TriangleEPA.hpp> #include <ephysics/collision/narrowphase/EPA/TriangleEPA.hpp>
#include <ephysics/collision/narrowphase/EPA/EdgeEPA.hpp> #include <ephysics/collision/narrowphase/EPA/EdgeEPA.hpp>
#include <ephysics/collision/narrowphase/EPA/TrianglesStore.hpp> #include <ephysics/collision/narrowphase/EPA/TrianglesStore.hpp>
// We use the ReactPhysics3D namespace
using namespace ephysics; using namespace ephysics;
// Constructor
TriangleEPA::TriangleEPA() { TriangleEPA::TriangleEPA() {
} }
// Constructor TriangleEPA::TriangleEPA(uint32_t _indexVertex1, uint32_t _indexVertex2, uint32_t _indexVertex3):
TriangleEPA::TriangleEPA(uint32_t indexVertex1, uint32_t indexVertex2, uint32_t indexVertex3) m_isObsolete(false) {
: mIsObsolete(false) { m_indicesVertices[0] = _indexVertex1;
mIndicesVertices[0] = indexVertex1; m_indicesVertices[1] = _indexVertex2;
mIndicesVertices[1] = indexVertex2; m_indicesVertices[2] = _indexVertex3;
mIndicesVertices[2] = indexVertex3; }
void TriangleEPA::set(uint32_t _indexVertex1, uint32_t _indexVertex2, uint32_t _indexVertex3) {
m_isObsolete = false;
m_indicesVertices[0] = _indexVertex1;
m_indicesVertices[1] = _indexVertex2;
m_indicesVertices[2] = _indexVertex3;
} }
// Destructor
TriangleEPA::~TriangleEPA() { TriangleEPA::~TriangleEPA() {
} }
// Compute the point v closest to the origin of this triangle bool TriangleEPA::computeClosestPoint(const vec3* _vertices) {
bool TriangleEPA::computeClosestPoint(const vec3* vertices) { const vec3& p0 = _vertices[m_indicesVertices[0]];
const vec3& p0 = vertices[mIndicesVertices[0]]; vec3 v1 = _vertices[m_indicesVertices[1]] - p0;
vec3 v2 = _vertices[m_indicesVertices[2]] - p0;
vec3 v1 = vertices[mIndicesVertices[1]] - p0;
vec3 v2 = vertices[mIndicesVertices[2]] - p0;
float v1Dotv1 = v1.dot(v1); float v1Dotv1 = v1.dot(v1);
float v1Dotv2 = v1.dot(v2); float v1Dotv2 = v1.dot(v2);
float v2Dotv2 = v2.dot(v2); float v2Dotv2 = v2.dot(v2);
float p0Dotv1 = p0.dot(v1); float p0Dotv1 = p0.dot(v1);
float p0Dotv2 = p0.dot(v2); float p0Dotv2 = p0.dot(v2);
// Compute determinant // Compute determinant
mDet = v1Dotv1 * v2Dotv2 - v1Dotv2 * v1Dotv2; m_determinant = v1Dotv1 * v2Dotv2 - v1Dotv2 * v1Dotv2;
// Compute lambda values // Compute lambda values
mLambda1 = p0Dotv2 * v1Dotv2 - p0Dotv1 * v2Dotv2; m_lambda1 = p0Dotv2 * v1Dotv2 - p0Dotv1 * v2Dotv2;
mLambda2 = p0Dotv1 * v1Dotv2 - p0Dotv2 * v1Dotv1; m_lambda2 = p0Dotv1 * v1Dotv2 - p0Dotv2 * v1Dotv1;
// If the determinant is positive // If the determinant is positive
if (mDet > 0.0) { if (m_determinant > 0.0) {
// Compute the closest point v // Compute the closest point v
mClosestPoint = p0 + 1.0f / mDet * (mLambda1 * v1 + mLambda2 * v2); m_closestPoint = p0 + 1.0f / m_determinant * (m_lambda1 * v1 + m_lambda2 * v2);
// Compute the square distance of closest point to the origin // Compute the square distance of closest point to the origin
mDistSquare = mClosestPoint.dot(mClosestPoint); m_distSquare = m_closestPoint.dot(m_closestPoint);
return true; return true;
} }
return false; return false;
} }
/// Link an edge with another one. It means that the current edge of a triangle will bool ephysics::link(const EdgeEPA& _edge0, const EdgeEPA& _edge1) {
/// be associated with the edge of another triangle in order that both triangles if ( _edge0.getSourceVertexIndex() == _edge1.getTargetVertexIndex()
/// are neighbour along both edges). && _edge0.getTargetVertexIndex() == _edge1.getSourceVertexIndex() ) {
bool ephysics::link(const EdgeEPA& edge0, const EdgeEPA& edge1) { _edge0.getOwnerTriangle()->m_adjacentEdges[_edge0.getIndex()] = _edge1;
bool isPossible = (edge0.getSourceVertexIndex() == edge1.getTargetVertexIndex() && _edge1.getOwnerTriangle()->m_adjacentEdges[_edge1.getIndex()] = _edge0;
edge0.getTargetVertexIndex() == edge1.getSourceVertexIndex()); return true;
if (isPossible) {
edge0.getOwnerTriangle()->mAdjacentEdges[edge0.getIndex()] = edge1;
edge1.getOwnerTriangle()->mAdjacentEdges[edge1.getIndex()] = edge0;
} }
return false;
return isPossible;
} }
/// Make an half link of an edge with another one from another triangle. An half-link void ephysics::halfLink(const EdgeEPA& _edge0, const EdgeEPA& _edge1) {
/// between an edge "edge0" and an edge "edge1" represents the fact that "edge1" is an assert( _edge0.getSourceVertexIndex() == _edge1.getTargetVertexIndex()
/// adjacent edge of "edge0" but not the opposite. The opposite edge connection will && _edge0.getTargetVertexIndex() == _edge1.getSourceVertexIndex());
/// be made later. _edge0.getOwnerTriangle()->m_adjacentEdges[_edge0.getIndex()] = _edge1;
void ephysics::halfLink(const EdgeEPA& edge0, const EdgeEPA& edge1) {
assert(edge0.getSourceVertexIndex() == edge1.getTargetVertexIndex() &&
edge0.getTargetVertexIndex() == edge1.getSourceVertexIndex());
// Link
edge0.getOwnerTriangle()->mAdjacentEdges[edge0.getIndex()] = edge1;
} }
// Execute the recursive silhouette algorithm from this triangle face.
/// The parameter "vertices" is an array that contains the vertices of the current polytope and the
/// parameter "indexNewVertex" is the index of the new vertex in this array. The goal of the
/// silhouette algorithm is to add the new vertex in the polytope by keeping it convex. Therefore,
/// the triangle faces that are visible from the new vertex must be removed from the polytope and we
/// need to add triangle faces where each face contains the new vertex and an edge of the silhouette.
/// The silhouette is the connected set of edges that are part of the border between faces that
/// are seen and faces that are not seen from the new vertex. This method starts from the nearest
/// face from the new vertex, computes the silhouette and create the new faces from the new vertex in
/// order that we always have a convex polytope. The faces visible from the new vertex are set
/// obselete and will not be considered as being a candidate face in the future.
bool TriangleEPA::computeSilhouette(const vec3* vertices, uint32_t indexNewVertex,
TrianglesStore& triangleStore) {
uint32_t first = triangleStore.getNbTriangles();
bool TriangleEPA::computeSilhouette(const vec3* _vertices, uint32_t _indexNewVertex,
TrianglesStore& _triangleStore) {
uint32_t first = _triangleStore.getNbTriangles();
// Mark the current triangle as obsolete because it // Mark the current triangle as obsolete because it
setIsObsolete(true); setIsObsolete(true);
// Execute recursively the silhouette algorithm for the adjacent edges of neighboring // Execute recursively the silhouette algorithm for the adjacent edges of neighboring
// triangles of the current triangle // triangles of the current triangle
bool result = mAdjacentEdges[0].computeSilhouette(vertices, indexNewVertex, triangleStore) && bool result = m_adjacentEdges[0].computeSilhouette(_vertices, _indexNewVertex, _triangleStore) &&
mAdjacentEdges[1].computeSilhouette(vertices, indexNewVertex, triangleStore) && m_adjacentEdges[1].computeSilhouette(_vertices, _indexNewVertex, _triangleStore) &&
mAdjacentEdges[2].computeSilhouette(vertices, indexNewVertex, triangleStore); m_adjacentEdges[2].computeSilhouette(_vertices, _indexNewVertex, _triangleStore);
if (result) { if (result) {
int32_t i,j; int32_t i,j;
// For each triangle face that contains the new vertex and an edge of the silhouette // For each triangle face that contains the new vertex and an edge of the silhouette
for (i=first, j=triangleStore.getNbTriangles()-1; for (i=first, j=_triangleStore.getNbTriangles()-1;
i != triangleStore.getNbTriangles(); j = i++) { i != _triangleStore.getNbTriangles(); j = i++) {
TriangleEPA* triangle = &triangleStore[i]; TriangleEPA* triangle = &_triangleStore[i];
halfLink(triangle->getAdjacentEdge(1), EdgeEPA(triangle, 1)); halfLink(triangle->getAdjacentEdge(1), EdgeEPA(triangle, 1));
if (!link(EdgeEPA(triangle, 0), EdgeEPA(&_triangleStore[j], 2))) {
if (!link(EdgeEPA(triangle, 0), EdgeEPA(&triangleStore[j], 2))) {
return false; return false;
} }
} }
} }
return result; return result;
} }

263
ephysics/collision/narrowphase/EPA/TriangleEPA.hpp
View File

@ -4,173 +4,108 @@
* @license BSD 3 clauses (see license file) * @license BSD 3 clauses (see license file)
*/ */
#pragma once #pragma once
// Libraries
#include <ephysics/mathematics/mathematics.hpp> #include <ephysics/mathematics/mathematics.hpp>
#include <ephysics/configuration.hpp> #include <ephysics/configuration.hpp>
#include <ephysics/collision/narrowphase/EPA/EdgeEPA.hpp> #include <ephysics/collision/narrowphase/EPA/EdgeEPA.hpp>
#include <cassert>
/// ReactPhysics3D namespace
namespace ephysics { namespace ephysics {
bool link(const EdgeEPA& edge0, const EdgeEPA& edge1);
// Prototypes void halfLink(const EdgeEPA& edge0, const EdgeEPA& edge1);
bool link(const EdgeEPA& edge0, const EdgeEPA& edge1); /**
void halfLink(const EdgeEPA& edge0, const EdgeEPA& edge1); * @brief Class TriangleEPA
* This class represents a triangle face of the current polytope in the EPA algorithm.
*/
// Class TriangleEPA class TriangleEPA {
/** private:
* This class represents a triangle face of the current polytope in the EPA algorithm. uint32_t m_indicesVertices[3]; //!< Indices of the vertices y_i of the triangle
*/ EdgeEPA m_adjacentEdges[3]; //!< Three adjacent edges of the triangle (edges of other triangles)
class TriangleEPA { bool m_isObsolete; //!< True if the triangle face is visible from the new support point
float m_determinant; //!< Determinant
private: vec3 m_closestPoint; //!< Point v closest to the origin on the affine hull of the triangle
float m_lambda1; //!< Lambda1 value such that v = lambda0 * y_0 + lambda1 * y_1 + lambda2 * y_2
// -------------------- Attributes -------------------- // float m_lambda2; //!< Lambda1 value such that v = lambda0 * y_0 + lambda1 * y_1 + lambda2 * y_2
float m_distSquare; //!< Square distance of the point closest point v to the origin
/// Indices of the vertices y_i of the triangle /// Private copy-constructor
uint32_t mIndicesVertices[3]; TriangleEPA(const TriangleEPA& _triangle);
/// Private assignment operator
/// Three adjacent edges of the triangle (edges of other triangles) TriangleEPA& operator=(const TriangleEPA& _triangle);
EdgeEPA mAdjacentEdges[3]; public:
/// Constructor
/// True if the triangle face is visible from the new support point TriangleEPA();
bool mIsObsolete; /// Constructor
TriangleEPA(uint32_t _v1, uint32_t _v2, uint32_t _v3);
/// Determinant /// Constructor
float mDet; void set(uint32_t _v1, uint32_t _v2, uint32_t _v3);
/// Destructor
/// Point v closest to the origin on the affine hull of the triangle ~TriangleEPA();
vec3 mClosestPoint; /// Return an adjacent edge of the triangle
EdgeEPA& getAdjacentEdge(int32_t _index) {
/// Lambda1 value such that v = lambda0 * y_0 + lambda1 * y_1 + lambda2 * y_2 assert(_index >= 0 && _index < 3);
float mLambda1; return m_adjacentEdges[_index];
}
/// Lambda1 value such that v = lambda0 * y_0 + lambda1 * y_1 + lambda2 * y_2 /// Set an adjacent edge of the triangle
float mLambda2; void setAdjacentEdge(int32_t _index, EdgeEPA& _edge) {
assert(_index >=0 && _index < 3);
/// Square distance of the point closest point v to the origin m_adjacentEdges[_index] = _edge;
float mDistSquare; }
/// Return the square distance of the closest point to origin
// -------------------- Methods -------------------- // float getDistSquare() const {
return m_distSquare;
/// Private copy-constructor }
TriangleEPA(const TriangleEPA& triangle); /// Set the isObsolete value
void setIsObsolete(bool _isObsolete) {
/// Private assignment operator m_isObsolete = _isObsolete;
TriangleEPA& operator=(const TriangleEPA& triangle); }
/// Return true if the triangle face is obsolete
public: bool getIsObsolete() const {
return m_isObsolete;
// -------------------- Methods -------------------- // }
/// Return the point closest to the origin
/// Constructor const vec3& getClosestPoint() const {
TriangleEPA(); return m_closestPoint;
}
/// Constructor // Return true if the closest point on affine hull is inside the triangle
TriangleEPA(uint32_t v1, uint32_t v2, uint32_t v3); bool isClosestPointInternalToTriangle() const {
return (m_lambda1 >= 0.0 && m_lambda2 >= 0.0 && (m_lambda1 + m_lambda2) <= m_determinant);
/// Destructor }
~TriangleEPA(); /// Return true if the triangle is visible from a given vertex
bool isVisibleFromVertex(const vec3* _vertices, uint32_t _index) const {
/// Return an adjacent edge of the triangle vec3 closestToVert = _vertices[_index] - m_closestPoint;
EdgeEPA& getAdjacentEdge(int32_t index); return (m_closestPoint.dot(closestToVert) > 0.0);
}
/// Set an adjacent edge of the triangle /// Compute the point v closest to the origin of this triangle
void setAdjacentEdge(int32_t index, EdgeEPA& edge); bool computeClosestPoint(const vec3* _vertices);
/// Compute the point of an object closest to the origin
/// Return the square distance of the closest point to origin vec3 computeClosestPointOfObject(const vec3* _supportPointsOfObject) const{
float getDistSquare() const; const vec3& p0 = _supportPointsOfObject[m_indicesVertices[0]];
return p0 + 1.0f/m_determinant * (m_lambda1 * (_supportPointsOfObject[m_indicesVertices[1]] - p0) +
/// Set the isObsolete value m_lambda2 * (_supportPointsOfObject[m_indicesVertices[2]] - p0));
void setIsObsolete(bool isObsolete); }
// Execute the recursive silhouette algorithm from this triangle face.
/// Return true if the triangle face is obsolete /// The parameter "vertices" is an array that contains the vertices of the current polytope and the
bool getIsObsolete() const; /// parameter "indexNewVertex" is the index of the new vertex in this array. The goal of the
/// silhouette algorithm is to add the new vertex in the polytope by keeping it convex. Therefore,
/// Return the point closest to the origin /// the triangle faces that are visible from the new vertex must be removed from the polytope and we
const vec3& getClosestPoint() const; /// need to add triangle faces where each face contains the new vertex and an edge of the silhouette.
/// The silhouette is the connected set of edges that are part of the border between faces that
// Return true if the closest point on affine hull is inside the triangle /// are seen and faces that are not seen from the new vertex. This method starts from the nearest
bool isClosestPointInternalToTriangle() const; /// face from the new vertex, computes the silhouette and create the new faces from the new vertex in
/// order that we always have a convex polytope. The faces visible from the new vertex are set
/// Return true if the triangle is visible from a given vertex /// obselete and will not be considered as being a candidate face in the future.
bool isVisibleFromVertex(const vec3* vertices, uint32_t index) const; bool computeSilhouette(const vec3* _vertices, uint32_t _index, TrianglesStore& _triangleStore);
/// Access operator
/// Compute the point v closest to the origin of this triangle uint32_t operator[](int32_t _pos) const {
bool computeClosestPoint(const vec3* vertices); assert(_pos >= 0 && _pos <3);
return m_indicesVertices[_pos];
/// Compute the point of an object closest to the origin }
vec3 computeClosestPointOfObject(const vec3* supportPointsOfObject) const; /// Link an edge with another one. It means that the current edge of a triangle will
/// be associated with the edge of another triangle in order that both triangles
/// Execute the recursive silhouette algorithm from this triangle face. /// are neighbour along both edges).
bool computeSilhouette(const vec3* vertices, uint32_t index, TrianglesStore& triangleStore); friend bool link(const EdgeEPA& _edge0, const EdgeEPA& _edge1);
/// Make an half link of an edge with another one from another triangle. An half-link
/// Access operator /// between an edge "edge0" and an edge "edge1" represents the fact that "edge1" is an
uint32_t operator[](int32_t i) const; /// adjacent edge of "edge0" but not the opposite. The opposite edge connection will
/// be made later.
/// Associate two edges friend void halfLink(const EdgeEPA& _edge0, const EdgeEPA& _edge1);
friend bool link(const EdgeEPA& edge0, const EdgeEPA& edge1); };
/// Make a half-link between two edges
friend void halfLink(const EdgeEPA& edge0, const EdgeEPA& edge1);
};
// Return an edge of the triangle
inline EdgeEPA& TriangleEPA::getAdjacentEdge(int32_t index) {
assert(index >= 0 && index < 3);
return mAdjacentEdges[index];
}
// Set an adjacent edge of the triangle
inline void TriangleEPA::setAdjacentEdge(int32_t index, EdgeEPA& edge) {
assert(index >=0 && index < 3);
mAdjacentEdges[index] = edge;
}
// Return the square distance of the closest point to origin
inline float TriangleEPA::getDistSquare() const {
return mDistSquare;
}
// Set the isObsolete value
inline void TriangleEPA::setIsObsolete(bool isObsolete) {
mIsObsolete = isObsolete;
}
// Return true if the triangle face is obsolete
inline bool TriangleEPA::getIsObsolete() const {
return mIsObsolete;
}
// Return the point closest to the origin
inline const vec3& TriangleEPA::getClosestPoint() const {
return mClosestPoint;
}
// Return true if the closest point on affine hull is inside the triangle
inline bool TriangleEPA::isClosestPointInternalToTriangle() const {
return (mLambda1 >= 0.0 && mLambda2 >= 0.0 && (mLambda1 + mLambda2) <= mDet);
}
// Return true if the triangle is visible from a given vertex
inline bool TriangleEPA::isVisibleFromVertex(const vec3* vertices, uint32_t index) const {
vec3 closestToVert = vertices[index] - mClosestPoint;
return (mClosestPoint.dot(closestToVert) > 0.0);
}
// Compute the point of an object closest to the origin
inline vec3 TriangleEPA::computeClosestPointOfObject(const vec3* supportPointsOfObject) const{
const vec3& p0 = supportPointsOfObject[mIndicesVertices[0]];
return p0 + 1.0f/mDet * (mLambda1 * (supportPointsOfObject[mIndicesVertices[1]] - p0) +
mLambda2 * (supportPointsOfObject[mIndicesVertices[2]] - p0));
}
// Access operator
inline uint32_t TriangleEPA::operator[](int32_t i) const {
assert(i >= 0 && i <3);
return mIndicesVertices[i];
}
} }

10
ephysics/collision/narrowphase/EPA/TrianglesStore.cpp
View File

@ -4,18 +4,12 @@
* @license BSD 3 clauses (see license file) * @license BSD 3 clauses (see license file)
*/ */
// Libraries
#include <ephysics/collision/narrowphase/EPA/TrianglesStore.hpp> #include <ephysics/collision/narrowphase/EPA/TrianglesStore.hpp>
// We use the ReactPhysics3D namespace ephysics::TrianglesStore::TrianglesStore() : m_numberTriangles(0) {
using namespace ephysics;
// Constructor
TrianglesStore::TrianglesStore() : m_numberTriangles(0) {
} }
// Destructor ephysics::TrianglesStore::~TrianglesStore() {
TrianglesStore::~TrianglesStore() {
} }

163
ephysics/collision/narrowphase/EPA/TrianglesStore.hpp
View File

@ -4,116 +4,61 @@
* @license BSD 3 clauses (see license file) * @license BSD 3 clauses (see license file)
*/ */
#pragma once #pragma once
#include <ephysics/collision/narrowphase/EPA/TriangleEPA.hpp> #include <ephysics/collision/narrowphase/EPA/TriangleEPA.hpp>
// Libraries
#include <cassert>
/// ReactPhysics3D namespace
namespace ephysics { namespace ephysics {
const uint32_t MAX_TRIANGLES = 200; // Maximum number of triangles
// Constants /**
const uint32_t MAX_TRIANGLES = 200; // Maximum number of triangles * @brief This class stores several triangles of the polytope in the EPA algorithm.
*/
// Class TriangleStore class TrianglesStore {
/** private:
* This class stores several triangles of the polytope in the EPA algorithm. TriangleEPA m_triangles[MAX_TRIANGLES]; //!< Triangles
*/ int32_t m_numberTriangles; //!< Number of triangles
class TrianglesStore { /// Private copy-constructor
TrianglesStore(const TrianglesStore& triangleStore);
private: /// Private assignment operator
TrianglesStore& operator=(const TrianglesStore& triangleStore);
// -------------------- Attributes -------------------- // public:
/// Constructor
/// Triangles TrianglesStore();
TriangleEPA mTriangles[MAX_TRIANGLES]; /// Destructor
~TrianglesStore();
/// Number of triangles /// Clear all the storage
int32_t m_numberTriangles; void clear() {
m_numberTriangles = 0;
// -------------------- Methods -------------------- // }
/// Return the number of triangles
/// Private copy-constructor int32_t getNbTriangles() const {
TrianglesStore(const TrianglesStore& triangleStore); return m_numberTriangles;
}
/// Private assignment operator /// Set the number of triangles
TrianglesStore& operator=(const TrianglesStore& triangleStore); void setNbTriangles(int32_t _backup) {
m_numberTriangles = _backup;
public: }
/// Return the last triangle
// -------------------- Methods -------------------- // TriangleEPA& last() {
assert(m_numberTriangles > 0);
/// Constructor return m_triangles[m_numberTriangles - 1];
TrianglesStore(); }
/// Create a new triangle
/// Destructor TriangleEPA* newTriangle(const vec3* _vertices, uint32_t _v0, uint32_t _v1, uint32_t _v2) {
~TrianglesStore(); TriangleEPA* newTriangle = nullptr;
// If we have not reached the maximum number of triangles
/// Clear all the storage if (m_numberTriangles != MAX_TRIANGLES) {
void clear(); newTriangle = &m_triangles[m_numberTriangles++];
newTriangle->set(_v0, _v1, _v2);
/// Return the number of triangles if (!newTriangle->computeClosestPoint(_vertices)) {
int32_t getNbTriangles() const; m_numberTriangles--;
newTriangle = nullptr;
/// Set the number of triangles }
void setNbTriangles(int32_t backup); }
// Return the new triangle
/// Return the last triangle return newTriangle;
TriangleEPA& last(); }
/// Access operator
/// Create a new triangle TriangleEPA& operator[](int32_t _id) {
TriangleEPA* newTriangle(const vec3* vertices, uint32_t v0, uint32_t v1, uint32_t v2); return m_triangles[_id];
}
/// Access operator };
TriangleEPA& operator[](int32_t i);
};
// Clear all the storage
inline void TrianglesStore::clear() {
m_numberTriangles = 0;
}
// Return the number of triangles
inline int32_t TrianglesStore::getNbTriangles() const {
return m_numberTriangles;
}
inline void TrianglesStore::setNbTriangles(int32_t backup) {
m_numberTriangles = backup;
}
// Return the last triangle
inline TriangleEPA& TrianglesStore::last() {
assert(m_numberTriangles > 0);
return mTriangles[m_numberTriangles - 1];
}
// Create a new triangle
inline TriangleEPA* TrianglesStore::newTriangle(const vec3* vertices,
uint32_t v0,uint32_t v1, uint32_t v2) {
TriangleEPA* newTriangle = NULL;
// If we have not reached the maximum number of triangles
if (m_numberTriangles != MAX_TRIANGLES) {
newTriangle = &mTriangles[m_numberTriangles++];
new (newTriangle) TriangleEPA(v0, v1, v2);
if (!newTriangle->computeClosestPoint(vertices)) {
m_numberTriangles--;
newTriangle = NULL;
}
}
// Return the new triangle
return newTriangle;
}
// Access operator
inline TriangleEPA& TrianglesStore::operator[](int32_t i) {
return mTriangles[i];
}
} }

190
ephysics/collision/narrowphase/GJK/GJKAlgorithm.cpp
View File

@ -3,158 +3,107 @@
* @copyright 2010-2016 Daniel Chappuis * @copyright 2010-2016 Daniel Chappuis
* @license BSD 3 clauses (see license file) * @license BSD 3 clauses (see license file)
*/ */
// Libraries
#include <ephysics/collision/narrowphase/GJK/GJKAlgorithm.hpp> #include <ephysics/collision/narrowphase/GJK/GJKAlgorithm.hpp>
#include <ephysics/collision/narrowphase/GJK/Simplex.hpp> #include <ephysics/collision/narrowphase/GJK/Simplex.hpp>
#include <ephysics/constraint/ContactPoint.hpp> #include <ephysics/constraint/ContactPoint.hpp>
#include <ephysics/configuration.hpp> #include <ephysics/configuration.hpp>
#include <ephysics/engine/Profiler.hpp> #include <ephysics/engine/Profiler.hpp>
#include <algorithm>
#include <cmath>
#include <cfloat>
#include <cassert>
// We want to use the ReactPhysics3D namespace
using namespace ephysics; using namespace ephysics;
// Constructor
GJKAlgorithm::GJKAlgorithm() : NarrowPhaseAlgorithm() { GJKAlgorithm::GJKAlgorithm() : NarrowPhaseAlgorithm() {
} }
// Destructor
GJKAlgorithm::~GJKAlgorithm() { GJKAlgorithm::~GJKAlgorithm() {
} }
// Compute a contact info if the two collision shapes collide.
/// This method implements the Hybrid Technique for computing the penetration depth by
/// running the GJK algorithm on original objects (without margin). If the shapes int32_tersect
/// only in the margins, the method compute the penetration depth and contact points
/// (of enlarged objects). If the original objects (without margin) int32_tersect, we
/// call the computePenetrationDepthForEnlargedObjects() method that run the GJK
/// algorithm on the enlarged object to obtain a simplex polytope that contains the
/// origin, they we give that simplex polytope to the EPA algorithm which will compute
/// the correct penetration depth and contact points between the enlarged objects.
void GJKAlgorithm::testCollision(const CollisionShapeInfo& shape1Info, void GJKAlgorithm::testCollision(const CollisionShapeInfo& shape1Info,
const CollisionShapeInfo& shape2Info, const CollisionShapeInfo& shape2Info,
NarrowPhaseCallback* narrowPhaseCallback) { NarrowPhaseCallback* narrowPhaseCallback) {
PROFILE("GJKAlgorithm::testCollision()"); PROFILE("GJKAlgorithm::testCollision()");
vec3 suppA; // Support point of object A
vec3 suppA; // Support point of object A vec3 suppB; // Support point of object B
vec3 suppB; // Support point of object B vec3 w; // Support point of Minkowski difference A-B
vec3 w; // Support point of Minkowski difference A-B vec3 pA; // Closest point of object A
vec3 pA; // Closest point of object A vec3 pB; // Closest point of object B
vec3 pB; // Closest point of object B
float vDotw; float vDotw;
float prevDistSquare; float prevDistSquare;
assert(shape1Info.collisionShape->isConvex()); assert(shape1Info.collisionShape->isConvex());
assert(shape2Info.collisionShape->isConvex()); assert(shape2Info.collisionShape->isConvex());
const ConvexShape* shape1 = static_cast<const ConvexShape*>(shape1Info.collisionShape); const ConvexShape* shape1 = static_cast<const ConvexShape*>(shape1Info.collisionShape);
const ConvexShape* shape2 = static_cast<const ConvexShape*>(shape2Info.collisionShape); const ConvexShape* shape2 = static_cast<const ConvexShape*>(shape2Info.collisionShape);
void** shape1CachedCollisionData = shape1Info.cachedCollisionData; void** shape1CachedCollisionData = shape1Info.cachedCollisionData;
void** shape2CachedCollisionData = shape2Info.cachedCollisionData; void** shape2CachedCollisionData = shape2Info.cachedCollisionData;
// Get the local-space to world-space transforms // Get the local-space to world-space transforms
const etk::Transform3D transform1 = shape1Info.shapeToWorldTransform; const etk::Transform3D transform1 = shape1Info.shapeToWorldTransform;
const etk::Transform3D transform2 = shape2Info.shapeToWorldTransform; const etk::Transform3D transform2 = shape2Info.shapeToWorldTransform;
// etk::Transform3D a point from local space of body 2 to local // etk::Transform3D a point from local space of body 2 to local
// space of body 1 (the GJK algorithm is done in local space of body 1) // space of body 1 (the GJK algorithm is done in local space of body 1)
etk::Transform3D body2Tobody1 = transform1.getInverse() * transform2; etk::Transform3D body2Tobody1 = transform1.getInverse() * transform2;
// Matrix that transform a direction from local // Matrix that transform a direction from local
// space of body 1 int32_to local space of body 2 // space of body 1 int32_to local space of body 2
etk::Matrix3x3 rotateToBody2 = transform2.getOrientation().getMatrix().getTranspose() * etk::Matrix3x3 rotateToBody2 = transform2.getOrientation().getMatrix().getTranspose() *
transform1.getOrientation().getMatrix(); transform1.getOrientation().getMatrix();
// Initialize the margin (sum of margins of both objects) // Initialize the margin (sum of margins of both objects)
float margin = shape1->getMargin() + shape2->getMargin(); float margin = shape1->getMargin() + shape2->getMargin();
float marginSquare = margin * margin; float marginSquare = margin * margin;
assert(margin > 0.0); assert(margin > 0.0);
// Create a simplex set // Create a simplex set
Simplex simplex; Simplex simplex;
// Get the previous point V (last cached separating axis) // Get the previous point V (last cached separating axis)
vec3 v = m_currentOverlappingPair->getCachedSeparatingAxis(); vec3 v = m_currentOverlappingPair->getCachedSeparatingAxis();
// Initialize the upper bound for the square distance // Initialize the upper bound for the square distance
float distSquare = DECIMAL_LARGEST; float distSquare = FLT_MAX;
do { do {
// Compute the support points for original objects (without margins) A and B // Compute the support points for original objects (without margins) A and B
suppA = shape1->getLocalSupportPointWithoutMargin(-v, shape1CachedCollisionData); suppA = shape1->getLocalSupportPointWithoutMargin(-v, shape1CachedCollisionData);
suppB = body2Tobody1 * suppB = body2Tobody1 * shape2->getLocalSupportPointWithoutMargin(rotateToBody2 * v, shape2CachedCollisionData);
shape2->getLocalSupportPointWithoutMargin(rotateToBody2 * v, shape2CachedCollisionData);
// Compute the support point for the Minkowski difference A-B // Compute the support point for the Minkowski difference A-B
w = suppA - suppB; w = suppA - suppB;
vDotw = v.dot(w); vDotw = v.dot(w);
// If the enlarge objects (with margins) do not int32_tersect // If the enlarge objects (with margins) do not int32_tersect
if (vDotw > 0.0 && vDotw * vDotw > distSquare * marginSquare) { if (vDotw > 0.0 && vDotw * vDotw > distSquare * marginSquare) {
// Cache the current separating axis for frame coherence // Cache the current separating axis for frame coherence
m_currentOverlappingPair->setCachedSeparatingAxis(v); m_currentOverlappingPair->setCachedSeparatingAxis(v);
// No int32_tersection, we return // No int32_tersection, we return
return; return;
} }
// If the objects int32_tersect only in the margins // If the objects int32_tersect only in the margins
if (simplex.isPointInSimplex(w) || distSquare - vDotw <= distSquare * REL_ERROR_SQUARE) { if (simplex.isPointInSimplex(w) || distSquare - vDotw <= distSquare * REL_ERROR_SQUARE) {
// Compute the closet points of both objects (without the margins) // Compute the closet points of both objects (without the margins)
simplex.computeClosestPointsOfAandB(pA, pB); simplex.computeClosestPointsOfAandB(pA, pB);
// Project those two points on the margins to have the closest points of both // Project those two points on the margins to have the closest points of both
// object with the margins // object with the margins
float dist = sqrt(distSquare); float dist = sqrt(distSquare);
assert(dist > 0.0); assert(dist > 0.0);
pA = (pA - (shape1->getMargin() / dist) * v); pA = (pA - (shape1->getMargin() / dist) * v);
pB = body2Tobody1.getInverse() * (pB + (shape2->getMargin() / dist) * v); pB = body2Tobody1.getInverse() * (pB + (shape2->getMargin() / dist) * v);
// Compute the contact info // Compute the contact info
vec3 normal = transform1.getOrientation() * (-v.safeNormalized()); vec3 normal = transform1.getOrientation() * (-v.safeNormalized());
float penetrationDepth = margin - dist; float penetrationDepth = margin - dist;
// Reject the contact if the penetration depth is negative (due too numerical errors) // Reject the contact if the penetration depth is negative (due too numerical errors)
if (penetrationDepth <= 0.0) return; if (penetrationDepth <= 0.0) return;
// Create the contact info object // Create the contact info object
ContactPointInfo contactInfo(shape1Info.proxyShape, shape2Info.proxyShape, shape1Info.collisionShape, ContactPointInfo contactInfo(shape1Info.proxyShape, shape2Info.proxyShape, shape1Info.collisionShape,
shape2Info.collisionShape, normal, penetrationDepth, pA, pB); shape2Info.collisionShape, normal, penetrationDepth, pA, pB);
narrowPhaseCallback->notifyContact(shape1Info.overlappingPair, contactInfo); narrowPhaseCallback->notifyContact(shape1Info.overlappingPair, contactInfo);
// There is an int32_tersection, therefore we return // There is an int32_tersection, therefore we return
return; return;
} }
// Add the new support point to the simplex // Add the new support point to the simplex
simplex.addPoint(w, suppA, suppB); simplex.addPoint(w, suppA, suppB);
// If the simplex is affinely dependent // If the simplex is affinely dependent
if (simplex.isAffinelyDependent()) { if (simplex.isAffinelyDependent()) {
// Compute the closet points of both objects (without the margins) // Compute the closet points of both objects (without the margins)
simplex.computeClosestPointsOfAandB(pA, pB); simplex.computeClosestPointsOfAandB(pA, pB);
// Project those two points on the margins to have the closest points of both // Project those two points on the margins to have the closest points of both
// object with the margins // object with the margins
float dist = sqrt(distSquare); float dist = sqrt(distSquare);
assert(dist > 0.0); assert(dist > 0.0);
pA = (pA - (shape1->getMargin() / dist) * v); pA = (pA - (shape1->getMargin() / dist) * v);
pB = body2Tobody1.getInverse() * (pB + (shape2->getMargin() / dist) * v); pB = body2Tobody1.getInverse() * (pB + (shape2->getMargin() / dist) * v);
// Compute the contact info // Compute the contact info
vec3 normal = transform1.getOrientation() * (-v.safeNormalized()); vec3 normal = transform1.getOrientation() * (-v.safeNormalized());
float penetrationDepth = margin - dist; float penetrationDepth = margin - dist;
@ -165,27 +114,21 @@ void GJKAlgorithm::testCollision(const CollisionShapeInfo& shape1Info,
// Create the contact info object // Create the contact info object
ContactPointInfo contactInfo(shape1Info.proxyShape, shape2Info.proxyShape, shape1Info.collisionShape, ContactPointInfo contactInfo(shape1Info.proxyShape, shape2Info.proxyShape, shape1Info.collisionShape,
shape2Info.collisionShape, normal, penetrationDepth, pA, pB); shape2Info.collisionShape, normal, penetrationDepth, pA, pB);
narrowPhaseCallback->notifyContact(shape1Info.overlappingPair, contactInfo); narrowPhaseCallback->notifyContact(shape1Info.overlappingPair, contactInfo);
// There is an int32_tersection, therefore we return // There is an int32_tersection, therefore we return
return; return;
} }
// Compute the point of the simplex closest to the origin // Compute the point of the simplex closest to the origin
// If the computation of the closest point fail // If the computation of the closest point fail
if (!simplex.computeClosestPoint(v)) { if (!simplex.computeClosestPoint(v)) {
// Compute the closet points of both objects (without the margins) // Compute the closet points of both objects (without the margins)
simplex.computeClosestPointsOfAandB(pA, pB); simplex.computeClosestPointsOfAandB(pA, pB);
// Project those two points on the margins to have the closest points of both // Project those two points on the margins to have the closest points of both
// object with the margins // object with the margins
float dist = sqrt(distSquare); float dist = sqrt(distSquare);
assert(dist > 0.0); assert(dist > 0.0);
pA = (pA - (shape1->getMargin() / dist) * v); pA = (pA - (shape1->getMargin() / dist) * v);
pB = body2Tobody1.getInverse() * (pB + (shape2->getMargin() / dist) * v); pB = body2Tobody1.getInverse() * (pB + (shape2->getMargin() / dist) * v);
// Compute the contact info // Compute the contact info
vec3 normal = transform1.getOrientation() * (-v.safeNormalized()); vec3 normal = transform1.getOrientation() * (-v.safeNormalized());
float penetrationDepth = margin - dist; float penetrationDepth = margin - dist;
@ -196,34 +139,27 @@ void GJKAlgorithm::testCollision(const CollisionShapeInfo& shape1Info,
// Create the contact info object // Create the contact info object
ContactPointInfo contactInfo(shape1Info.proxyShape, shape2Info.proxyShape, shape1Info.collisionShape, ContactPointInfo contactInfo(shape1Info.proxyShape, shape2Info.proxyShape, shape1Info.collisionShape,
shape2Info.collisionShape, normal, penetrationDepth, pA, pB); shape2Info.collisionShape, normal, penetrationDepth, pA, pB);
narrowPhaseCallback->notifyContact(shape1Info.overlappingPair, contactInfo); narrowPhaseCallback->notifyContact(shape1Info.overlappingPair, contactInfo);
// There is an int32_tersection, therefore we return // There is an int32_tersection, therefore we return
return; return;
} }
// Store and update the squared distance of the closest point // Store and update the squared distance of the closest point
prevDistSquare = distSquare; prevDistSquare = distSquare;
distSquare = v.length2(); distSquare = v.length2();
// If the distance to the closest point doesn't improve a lot // If the distance to the closest point doesn't improve a lot
if (prevDistSquare - distSquare <= MACHINE_EPSILON * prevDistSquare) { if (prevDistSquare - distSquare <= FLT_EPSILON * prevDistSquare) {
simplex.backupClosestPointInSimplex(v); simplex.backupClosestPointInSimplex(v);
// Get the new squared distance // Get the new squared distance
distSquare = v.length2(); distSquare = v.length2();
// Compute the closet points of both objects (without the margins) // Compute the closet points of both objects (without the margins)
simplex.computeClosestPointsOfAandB(pA, pB); simplex.computeClosestPointsOfAandB(pA, pB);
// Project those two points on the margins to have the closest points of both // Project those two points on the margins to have the closest points of both
// object with the margins // object with the margins
float dist = sqrt(distSquare); float dist = sqrt(distSquare);
assert(dist > 0.0); assert(dist > 0.0);
pA = (pA - (shape1->getMargin() / dist) * v); pA = (pA - (shape1->getMargin() / dist) * v);
pB = body2Tobody1.getInverse() * (pB + (shape2->getMargin() / dist) * v); pB = body2Tobody1.getInverse() * (pB + (shape2->getMargin() / dist) * v);
// Compute the contact info // Compute the contact info
vec3 normal = transform1.getOrientation() * (-v.safeNormalized()); vec3 normal = transform1.getOrientation() * (-v.safeNormalized());
float penetrationDepth = margin - dist; float penetrationDepth = margin - dist;
@ -234,15 +170,12 @@ void GJKAlgorithm::testCollision(const CollisionShapeInfo& shape1Info,
// Create the contact info object // Create the contact info object
ContactPointInfo contactInfo(shape1Info.proxyShape, shape2Info.proxyShape, shape1Info.collisionShape, ContactPointInfo contactInfo(shape1Info.proxyShape, shape2Info.proxyShape, shape1Info.collisionShape,
shape2Info.collisionShape, normal, penetrationDepth, pA, pB); shape2Info.collisionShape, normal, penetrationDepth, pA, pB);
narrowPhaseCallback->notifyContact(shape1Info.overlappingPair, contactInfo); narrowPhaseCallback->notifyContact(shape1Info.overlappingPair, contactInfo);
// There is an int32_tersection, therefore we return // There is an int32_tersection, therefore we return
return; return;
} }
} while(!simplex.isFull() && distSquare > MACHINE_EPSILON * } while(!simplex.isFull() && distSquare > FLT_EPSILON *
simplex.getMaxLengthSquareOfAPoint()); simplex.getMaxLengthSquareOfAPoint());
// The objects (without margins) int32_tersect. Therefore, we run the GJK algorithm // The objects (without margins) int32_tersect. Therefore, we run the GJK algorithm
// again but on the enlarged objects to compute a simplex polytope that contains // again but on the enlarged objects to compute a simplex polytope that contains
// the origin. Then, we give that simplex polytope to the EPA algorithm to compute // the origin. Then, we give that simplex polytope to the EPA algorithm to compute
@ -251,11 +184,6 @@ void GJKAlgorithm::testCollision(const CollisionShapeInfo& shape1Info,
transform2, narrowPhaseCallback, v); transform2, narrowPhaseCallback, v);
} }
/// This method runs the GJK algorithm on the two enlarged objects (with margin)
/// to compute a simplex polytope that contains the origin. The two objects are
/// assumed to int32_tersect in the original objects (without margin). Therefore such
/// a polytope must exist. Then, we give that polytope to the EPA algorithm to
/// compute the correct penetration depth and contact points of the enlarged objects.
void GJKAlgorithm::computePenetrationDepthForEnlargedObjects(const CollisionShapeInfo& shape1Info, void GJKAlgorithm::computePenetrationDepthForEnlargedObjects(const CollisionShapeInfo& shape1Info,
const etk::Transform3D& transform1, const etk::Transform3D& transform1,
const CollisionShapeInfo& shape2Info, const CollisionShapeInfo& shape2Info,
@ -263,172 +191,120 @@ void GJKAlgorithm::computePenetrationDepthForEnlargedObjects(const CollisionShap
NarrowPhaseCallback* narrowPhaseCallback, NarrowPhaseCallback* narrowPhaseCallback,
vec3& v) { vec3& v) {
PROFILE("GJKAlgorithm::computePenetrationDepthForEnlargedObjects()"); PROFILE("GJKAlgorithm::computePenetrationDepthForEnlargedObjects()");
Simplex simplex; Simplex simplex;
vec3 suppA; vec3 suppA;
vec3 suppB; vec3 suppB;
vec3 w; vec3 w;
float vDotw; float vDotw;
float distSquare = DECIMAL_LARGEST; float distSquare = FLT_MAX;
float prevDistSquare; float prevDistSquare;
assert(shape1Info.collisionShape->isConvex()); assert(shape1Info.collisionShape->isConvex());
assert(shape2Info.collisionShape->isConvex()); assert(shape2Info.collisionShape->isConvex());
const ConvexShape* shape1 = static_cast<const ConvexShape*>(shape1Info.collisionShape); const ConvexShape* shape1 = static_cast<const ConvexShape*>(shape1Info.collisionShape);
const ConvexShape* shape2 = static_cast<const ConvexShape*>(shape2Info.collisionShape); const ConvexShape* shape2 = static_cast<const ConvexShape*>(shape2Info.collisionShape);
void** shape1CachedCollisionData = shape1Info.cachedCollisionData; void** shape1CachedCollisionData = shape1Info.cachedCollisionData;
void** shape2CachedCollisionData = shape2Info.cachedCollisionData; void** shape2CachedCollisionData = shape2Info.cachedCollisionData;
// etk::Transform3D a point from local space of body 2 to local space // etk::Transform3D a point from local space of body 2 to local space
// of body 1 (the GJK algorithm is done in local space of body 1) // of body 1 (the GJK algorithm is done in local space of body 1)
etk::Transform3D body2ToBody1 = transform1.getInverse() * transform2; etk::Transform3D body2ToBody1 = transform1.getInverse() * transform2;
// Matrix that transform a direction from local space of body 1 int32_to local space of body 2 // Matrix that transform a direction from local space of body 1 int32_to local space of body 2
etk::Matrix3x3 rotateToBody2 = transform2.getOrientation().getMatrix().getTranspose() * etk::Matrix3x3 rotateToBody2 = transform2.getOrientation().getMatrix().getTranspose() *
transform1.getOrientation().getMatrix(); transform1.getOrientation().getMatrix();
do { do {
// Compute the support points for the enlarged object A and B // Compute the support points for the enlarged object A and B
suppA = shape1->getLocalSupportPointWithMargin(-v, shape1CachedCollisionData); suppA = shape1->getLocalSupportPointWithMargin(-v, shape1CachedCollisionData);
suppB = body2ToBody1 * shape2->getLocalSupportPointWithMargin(rotateToBody2 * v, shape2CachedCollisionData); suppB = body2ToBody1 * shape2->getLocalSupportPointWithMargin(rotateToBody2 * v, shape2CachedCollisionData);
// Compute the support point for the Minkowski difference A-B // Compute the support point for the Minkowski difference A-B
w = suppA - suppB; w = suppA - suppB;
vDotw = v.dot(w); vDotw = v.dot(w);
// If the enlarge objects do not int32_tersect // If the enlarge objects do not int32_tersect
if (vDotw > 0.0) { if (vDotw > 0.0) {
// No int32_tersection, we return // No int32_tersection, we return
return; return;
} }
// Add the new support point to the simplex // Add the new support point to the simplex
simplex.addPoint(w, suppA, suppB); simplex.addPoint(w, suppA, suppB);
if (simplex.isAffinelyDependent()) { if (simplex.isAffinelyDependent()) {
return; return;
} }
if (!simplex.computeClosestPoint(v)) { if (!simplex.computeClosestPoint(v)) {
return; return;
} }
// Store and update the square distance // Store and update the square distance
prevDistSquare = distSquare; prevDistSquare = distSquare;
distSquare = v.length2(); distSquare = v.length2();
if (prevDistSquare - distSquare <= FLT_EPSILON * prevDistSquare) {
if (prevDistSquare - distSquare <= MACHINE_EPSILON * prevDistSquare) {
return; return;
} }
} while(!simplex.isFull() && distSquare > FLT_EPSILON *
} while(!simplex.isFull() && distSquare > MACHINE_EPSILON *
simplex.getMaxLengthSquareOfAPoint()); simplex.getMaxLengthSquareOfAPoint());
// Give the simplex computed with GJK algorithm to the EPA algorithm // Give the simplex computed with GJK algorithm to the EPA algorithm
// which will compute the correct penetration depth and contact points // which will compute the correct penetration depth and contact points
// between the two enlarged objects // between the two enlarged objects
return mAlgoEPA.computePenetrationDepthAndContactPoints(simplex, shape1Info, return m_algoEPA.computePenetrationDepthAndContactPoints(simplex, shape1Info,
transform1, shape2Info, transform2, transform1, shape2Info, transform2,
v, narrowPhaseCallback); v, narrowPhaseCallback);
} }
// Use the GJK Algorithm to find if a point is inside a convex collision shape
bool GJKAlgorithm::testPointInside(const vec3& localPoint, ProxyShape* proxyShape) { bool GJKAlgorithm::testPointInside(const vec3& localPoint, ProxyShape* proxyShape) {
vec3 suppA; // Support point of object A vec3 suppA; // Support point of object A
vec3 w; // Support point of Minkowski difference A-B vec3 w; // Support point of Minkowski difference A-B
float prevDistSquare; float prevDistSquare;
assert(proxyShape->getCollisionShape()->isConvex()); assert(proxyShape->getCollisionShape()->isConvex());
const ConvexShape* shape = static_cast<const ConvexShape*>(proxyShape->getCollisionShape()); const ConvexShape* shape = static_cast<const ConvexShape*>(proxyShape->getCollisionShape());
void** shapeCachedCollisionData = proxyShape->getCachedCollisionData(); void** shapeCachedCollisionData = proxyShape->getCachedCollisionData();
// Support point of object B (object B is a single point) // Support point of object B (object B is a single point)
const vec3 suppB(localPoint); const vec3 suppB(localPoint);
// Create a simplex set // Create a simplex set
Simplex simplex; Simplex simplex;
// Initial supporting direction // Initial supporting direction
vec3 v(1, 1, 1); vec3 v(1, 1, 1);
// Initialize the upper bound for the square distance // Initialize the upper bound for the square distance
float distSquare = DECIMAL_LARGEST; float distSquare = FLT_MAX;
do { do {
// Compute the support points for original objects (without margins) A and B // Compute the support points for original objects (without margins) A and B
suppA = shape->getLocalSupportPointWithoutMargin(-v, shapeCachedCollisionData); suppA = shape->getLocalSupportPointWithoutMargin(-v, shapeCachedCollisionData);
// Compute the support point for the Minkowski difference A-B // Compute the support point for the Minkowski difference A-B
w = suppA - suppB; w = suppA - suppB;
// Add the new support point to the simplex // Add the new support point to the simplex
simplex.addPoint(w, suppA, suppB); simplex.addPoint(w, suppA, suppB);
// If the simplex is affinely dependent // If the simplex is affinely dependent
if (simplex.isAffinelyDependent()) { if (simplex.isAffinelyDependent()) {
return false; return false;
} }
// Compute the point of the simplex closest to the origin // Compute the point of the simplex closest to the origin
// If the computation of the closest point fail // If the computation of the closest point fail
if (!simplex.computeClosestPoint(v)) { if (!simplex.computeClosestPoint(v)) {
return false; return false;
} }
// Store and update the squared distance of the closest point // Store and update the squared distance of the closest point
prevDistSquare = distSquare; prevDistSquare = distSquare;
distSquare = v.length2(); distSquare = v.length2();
// If the distance to the closest point doesn't improve a lot // If the distance to the closest point doesn't improve a lot
if (prevDistSquare - distSquare <= MACHINE_EPSILON * prevDistSquare) { if (prevDistSquare - distSquare <= FLT_EPSILON * prevDistSquare) {
return false; return false;
} }
} while(!simplex.isFull() && distSquare > MACHINE_EPSILON * } while( !simplex.isFull()
simplex.getMaxLengthSquareOfAPoint()); && distSquare > FLT_EPSILON * simplex.getMaxLengthSquareOfAPoint());
// The point is inside the collision shape // The point is inside the collision shape
return true; return true;
} }
// Ray casting algorithm agains a convex collision shape using the GJK Algorithm
/// This method implements the GJK ray casting algorithm described by Gino Van Den Bergen in
/// "Ray Casting against General Convex Objects with Application to Continuous Collision Detection".
bool GJKAlgorithm::raycast(const Ray& ray, ProxyShape* proxyShape, RaycastInfo& raycastInfo) { bool GJKAlgorithm::raycast(const Ray& ray, ProxyShape* proxyShape, RaycastInfo& raycastInfo) {
assert(proxyShape->getCollisionShape()->isConvex()); assert(proxyShape->getCollisionShape()->isConvex());
const ConvexShape* shape = static_cast<const ConvexShape*>(proxyShape->getCollisionShape()); const ConvexShape* shape = static_cast<const ConvexShape*>(proxyShape->getCollisionShape());
void** shapeCachedCollisionData = proxyShape->getCachedCollisionData(); void** shapeCachedCollisionData = proxyShape->getCachedCollisionData();
vec3 suppA; // Current lower bound point on the ray (starting at ray's origin) vec3 suppA; // Current lower bound point on the ray (starting at ray's origin)
vec3 suppB; // Support point on the collision shape vec3 suppB; // Support point on the collision shape
const float machineEpsilonSquare = MACHINE_EPSILON * MACHINE_EPSILON; const float machineEpsilonSquare = FLT_EPSILON * FLT_EPSILON;
const float epsilon = float(0.0001); const float epsilon = float(0.0001);
// Convert the ray origin and direction int32_to the local-space of the collision shape // Convert the ray origin and direction int32_to the local-space of the collision shape
vec3 rayDirection = ray.point2 - ray.point1; vec3 rayDirection = ray.point2 - ray.point1;
// If the points of the segment are two close, return no hit // If the points of the segment are two close, return no hit
if (rayDirection.length2() < machineEpsilonSquare) return false; if (rayDirection.length2() < machineEpsilonSquare) return false;
vec3 w; vec3 w;
// Create a simplex set // Create a simplex set
Simplex simplex; Simplex simplex;
vec3 n(0.0f, float(0.0), float(0.0)); vec3 n(0.0f, float(0.0), float(0.0));
float lambda = 0.0f; float lambda = 0.0f;
suppA = ray.point1; // Current lower bound point on the ray (starting at ray's origin) suppA = ray.point1; // Current lower bound point on the ray (starting at ray's origin)
@ -437,25 +313,17 @@ bool GJKAlgorithm::raycast(const Ray& ray, ProxyShape* proxyShape, RaycastInfo&
float vDotW, vDotR; float vDotW, vDotR;
float distSquare = v.length2(); float distSquare = v.length2();
int32_t nbIterations = 0; int32_t nbIterations = 0;
// GJK Algorithm loop // GJK Algorithm loop
while (distSquare > epsilon && nbIterations < MAX_ITERATIONS_GJK_RAYCAST) { while (distSquare > epsilon && nbIterations < MAX_ITERATIONS_GJK_RAYCAST) {
// Compute the support points // Compute the support points
suppB = shape->getLocalSupportPointWithoutMargin(v, shapeCachedCollisionData); suppB = shape->getLocalSupportPointWithoutMargin(v, shapeCachedCollisionData);
w = suppA - suppB; w = suppA - suppB;
vDotW = v.dot(w); vDotW = v.dot(w);
if (vDotW > float(0)) { if (vDotW > float(0)) {
vDotR = v.dot(rayDirection); vDotR = v.dot(rayDirection);
if (vDotR >= -machineEpsilonSquare) { if (vDotR >= -machineEpsilonSquare) {
return false; return false;
} } else {
else {
// We have found a better lower bound for the hit point along the ray // We have found a better lower bound for the hit point along the ray
lambda = lambda - vDotW / vDotR; lambda = lambda - vDotW / vDotR;
suppA = ray.point1 + lambda * rayDirection; suppA = ray.point1 + lambda * rayDirection;
@ -463,47 +331,37 @@ bool GJKAlgorithm::raycast(const Ray& ray, ProxyShape* proxyShape, RaycastInfo&
n = v; n = v;
} }
} }
// Add the new support point to the simplex // Add the new support point to the simplex
if (!simplex.isPointInSimplex(w)) { if (!simplex.isPointInSimplex(w)) {
simplex.addPoint(w, suppA, suppB); simplex.addPoint(w, suppA, suppB);
} }
// Compute the closest point // Compute the closest point
if (simplex.computeClosestPoint(v)) { if (simplex.computeClosestPoint(v)) {
distSquare = v.length2(); distSquare = v.length2();
} } else {
else {
distSquare = 0.0f; distSquare = 0.0f;
} }
// If the current lower bound distance is larger than the maximum raycasting distance // If the current lower bound distance is larger than the maximum raycasting distance
if (lambda > ray.maxFraction) return false; if (lambda > ray.maxFraction) return false;
nbIterations++; nbIterations++;
} }
// If the origin was inside the shape, we return no hit // If the origin was inside the shape, we return no hit
if (lambda < MACHINE_EPSILON) return false; if (lambda < FLT_EPSILON) {
return false;
}
// Compute the closet points of both objects (without the margins) // Compute the closet points of both objects (without the margins)
vec3 pointA; vec3 pointA;
vec3 pointB; vec3 pointB;
simplex.computeClosestPointsOfAandB(pointA, pointB); simplex.computeClosestPointsOfAandB(pointA, pointB);
// A raycast hit has been found, we fill in the raycast info // A raycast hit has been found, we fill in the raycast info
raycastInfo.hitFraction = lambda; raycastInfo.hitFraction = lambda;
raycastInfo.worldPoint = pointB; raycastInfo.worldPoint = pointB;
raycastInfo.body = proxyShape->getBody(); raycastInfo.body = proxyShape->getBody();
raycastInfo.proxyShape = proxyShape; raycastInfo.proxyShape = proxyShape;
if (n.length2() >= machineEpsilonSquare) { // The normal vector is valid if (n.length2() >= machineEpsilonSquare) { // The normal vector is valid
raycastInfo.worldNormal = n; raycastInfo.worldNormal = n;
} } else { // Degenerated normal vector, we return a zero normal vector
else { // Degenerated normal vector, we return a zero normal vector
raycastInfo.worldNormal = vec3(float(0), float(0), float(0)); raycastInfo.worldNormal = vec3(float(0), float(0), float(0));
} }
return true; return true;
} }

148
ephysics/collision/narrowphase/GJK/GJKAlgorithm.hpp
View File

@ -5,94 +5,76 @@
*/ */
#pragma once #pragma once
// Libraries
#include <ephysics/collision/narrowphase/NarrowPhaseAlgorithm.hpp> #include <ephysics/collision/narrowphase/NarrowPhaseAlgorithm.hpp>
#include <ephysics/constraint/ContactPoint.hpp> #include <ephysics/constraint/ContactPoint.hpp>
#include <ephysics/collision/shapes/ConvexShape.hpp> #include <ephysics/collision/shapes/ConvexShape.hpp>
#include <ephysics/collision/narrowphase/EPA/EPAAlgorithm.hpp> #include <ephysics/collision/narrowphase/EPA/EPAAlgorithm.hpp>
/// ReactPhysics3D namespace
namespace ephysics { namespace ephysics {
const float REL_ERROR = float(1.0e-3);
// Constants const float REL_ERROR_SQUARE = REL_ERROR * REL_ERROR;
const float REL_ERROR = float(1.0e-3); const int32_t MAX_ITERATIONS_GJK_RAYCAST = 32;
const float REL_ERROR_SQUARE = REL_ERROR * REL_ERROR; /**
const int32_t MAX_ITERATIONS_GJK_RAYCAST = 32; * @brief This class implements a narrow-phase collision detection algorithm. This
* algorithm uses the ISA-GJK algorithm and the EPA algorithm. This
// Class GJKAlgorithm * implementation is based on the implementation discussed in the book
/** * "Collision Detection in Interactive 3D Environments" by Gino van den Bergen.
* This class implements a narrow-phase collision detection algorithm. This * This method implements the Hybrid Technique for calculating the
* algorithm uses the ISA-GJK algorithm and the EPA algorithm. This * penetration depth. The two objects are enlarged with a small margin. If
* implementation is based on the implementation discussed in the book * the object int32_tersects in their margins, the penetration depth is quickly
* "Collision Detection in Interactive 3D Environments" by Gino van den Bergen. * computed using the GJK algorithm on the original objects (without margin).
* This method implements the Hybrid Technique for calculating the * If the original objects (without margin) int32_tersect, we run again the GJK
* penetration depth. The two objects are enlarged with a small margin. If * algorithm on the enlarged objects (with margin) to compute simplex
* the object int32_tersects in their margins, the penetration depth is quickly * polytope that contains the origin and give it to the EPA (Expanding
* computed using the GJK algorithm on the original objects (without margin). * Polytope Algorithm) to compute the correct penetration depth between the
* If the original objects (without margin) int32_tersect, we run again the GJK * enlarged objects.
* algorithm on the enlarged objects (with margin) to compute simplex */
* polytope that contains the origin and give it to the EPA (Expanding class GJKAlgorithm : public NarrowPhaseAlgorithm {
* Polytope Algorithm) to compute the correct penetration depth between the private :
* enlarged objects. EPAAlgorithm m_algoEPA; //!< EPA Algorithm
*/ /// Private copy-constructor
class GJKAlgorithm : public NarrowPhaseAlgorithm { GJKAlgorithm(const GJKAlgorithm& algorithm);
/// Private assignment operator
private : GJKAlgorithm& operator=(const GJKAlgorithm& algorithm);
/// This method runs the GJK algorithm on the two enlarged objects (with margin)
// -------------------- Attributes -------------------- // /// to compute a simplex polytope that contains the origin. The two objects are
/// assumed to int32_tersect in the original objects (without margin). Therefore such
/// EPA Algorithm /// a polytope must exist. Then, we give that polytope to the EPA algorithm to
EPAAlgorithm mAlgoEPA; /// compute the correct penetration depth and contact points of the enlarged objects.
void computePenetrationDepthForEnlargedObjects(const CollisionShapeInfo& shape1Info,
// -------------------- Methods -------------------- // const etk::Transform3D& transform1,
const CollisionShapeInfo& shape2Info,
/// Private copy-constructor const etk::Transform3D& transform2,
GJKAlgorithm(const GJKAlgorithm& algorithm); NarrowPhaseCallback* narrowPhaseCallback,
vec3& v);
/// Private assignment operator public :
GJKAlgorithm& operator=(const GJKAlgorithm& algorithm); /// Constructor
GJKAlgorithm();
/// Compute the penetration depth for enlarged objects. /// Destructor
void computePenetrationDepthForEnlargedObjects(const CollisionShapeInfo& shape1Info, ~GJKAlgorithm();
const etk::Transform3D& transform1, /// Initalize the algorithm
const CollisionShapeInfo& shape2Info, virtual void init(CollisionDetection* _collisionDetection, MemoryAllocator* _memoryAllocator) {
const etk::Transform3D& transform2, NarrowPhaseAlgorithm::init(_collisionDetection, _memoryAllocator);
NarrowPhaseCallback* narrowPhaseCallback, m_algoEPA.init(_memoryAllocator);
vec3& v); };
// Compute a contact info if the two collision shapes collide.
public : /// This method implements the Hybrid Technique for computing the penetration depth by
/// running the GJK algorithm on original objects (without margin). If the shapes int32_tersect
// -------------------- Methods -------------------- // /// only in the margins, the method compute the penetration depth and contact points
/// (of enlarged objects). If the original objects (without margin) int32_tersect, we
/// Constructor /// call the computePenetrationDepthForEnlargedObjects() method that run the GJK
GJKAlgorithm(); /// algorithm on the enlarged object to obtain a simplex polytope that contains the
/// origin, they we give that simplex polytope to the EPA algorithm which will compute
/// Destructor /// the correct penetration depth and contact points between the enlarged objects.
~GJKAlgorithm(); virtual void testCollision(const CollisionShapeInfo& shape1Info,
const CollisionShapeInfo& shape2Info,
/// Initalize the algorithm NarrowPhaseCallback* narrowPhaseCallback);
virtual void init(CollisionDetection* collisionDetection, /// Use the GJK Algorithm to find if a point is inside a convex collision shape
MemoryAllocator* memoryAllocator); bool testPointInside(const vec3& localPoint, ProxyShape* proxyShape);
/// Ray casting algorithm agains a convex collision shape using the GJK Algorithm
/// Compute a contact info if the two bounding volumes collide. /// This method implements the GJK ray casting algorithm described by Gino Van Den Bergen in
virtual void testCollision(const CollisionShapeInfo& shape1Info, /// "Ray Casting against General Convex Objects with Application to Continuous Collision Detection".
const CollisionShapeInfo& shape2Info, bool raycast(const Ray& ray, ProxyShape* proxyShape, RaycastInfo& raycastInfo);
NarrowPhaseCallback* narrowPhaseCallback); };
/// Use the GJK Algorithm to find if a point is inside a convex collision shape
bool testPointInside(const vec3& localPoint, ProxyShape* proxyShape);
/// Ray casting algorithm agains a convex collision shape using the GJK Algorithm
bool raycast(const Ray& ray, ProxyShape* proxyShape, RaycastInfo& raycastInfo);
};
// Initalize the algorithm
inline void GJKAlgorithm::init(CollisionDetection* collisionDetection,
MemoryAllocator* memoryAllocator) {
NarrowPhaseAlgorithm::init(collisionDetection, memoryAllocator);
mAlgoEPA.init(memoryAllocator);
}
} }

3
ephysics/collision/narrowphase/GJK/Simplex.cpp
View File

@ -6,7 +6,6 @@
// Libraries // Libraries
#include <ephysics/collision/narrowphase/GJK/Simplex.hpp> #include <ephysics/collision/narrowphase/GJK/Simplex.hpp>
#include <cfloat>
// We want to use the ReactPhysics3D namespace // We want to use the ReactPhysics3D namespace
using namespace ephysics; using namespace ephysics;
@ -327,7 +326,7 @@ bool Simplex::computeClosestPoint(vec3& v) {
// Backup the closest point // Backup the closest point
void Simplex::backupClosestPointInSimplex(vec3& v) { void Simplex::backupClosestPointInSimplex(vec3& v) {
float minDistSquare = DECIMAL_LARGEST; float minDistSquare = FLT_MAX;
Bits bit; Bits bit;
for (bit = mAllBits; bit != 0x0; bit--) { for (bit = mAllBits; bit != 0x0; bit--) {

2
ephysics/collision/narrowphase/SphereVsSphereAlgorithm.cpp
View File

@ -32,7 +32,7 @@ void ephysics::SphereVsSphereAlgorithm::testCollision(const ephysics::CollisionS
vec3 centerSphere1InBody2LocalSpace = transform2.getInverse() * transform1.getPosition(); vec3 centerSphere1InBody2LocalSpace = transform2.getInverse() * transform1.getPosition();
vec3 intersectionOnBody1 = sphereShape1->getRadius() * centerSphere2InBody1LocalSpace.safeNormalized(); vec3 intersectionOnBody1 = sphereShape1->getRadius() * centerSphere2InBody1LocalSpace.safeNormalized();
vec3 intersectionOnBody2 = sphereShape2->getRadius() * centerSphere1InBody2LocalSpace.safeNormalized(); vec3 intersectionOnBody2 = sphereShape2->getRadius() * centerSphere1InBody2LocalSpace.safeNormalized();
float penetrationDepth = sumRadius - std::sqrt(squaredDistanceBetweenCenters); float penetrationDepth = sumRadius - etk::sqrt(squaredDistanceBetweenCenters);
// Create the contact info object // Create the contact info object
ephysics::ContactPointInfo contactInfo(_shape1Info.proxyShape, ephysics::ContactPointInfo contactInfo(_shape1Info.proxyShape,

25
ephysics/collision/shapes/AABB.cpp
View File

@ -8,7 +8,6 @@
// Libraries // Libraries
#include <ephysics/collision/shapes/AABB.hpp> #include <ephysics/collision/shapes/AABB.hpp>
#include <ephysics/configuration.hpp> #include <ephysics/configuration.hpp>
#include <cassert>
using namespace ephysics; using namespace ephysics;
using namespace std; using namespace std;
@ -108,16 +107,16 @@ bool AABB::testRayIntersect(const Ray& _ray) const {
const vec3 d = point2 - _ray.point1; const vec3 d = point2 - _ray.point1;
const vec3 m = _ray.point1 + point2 - m_minCoordinates - m_maxCoordinates; const vec3 m = _ray.point1 + point2 - m_minCoordinates - m_maxCoordinates;
// Test if the AABB face normals are separating axis // Test if the AABB face normals are separating axis
float adx = std::abs(d.x()); float adx = etk::abs(d.x());
if (std::abs(m.x()) > e.x() + adx) { if (etk::abs(m.x()) > e.x() + adx) {
return false; return false;
} }
float ady = std::abs(d.y()); float ady = etk::abs(d.y());
if (std::abs(m.y()) > e.y() + ady) { if (etk::abs(m.y()) > e.y() + ady) {
return false; return false;
} }
float adz = std::abs(d.z()); float adz = etk::abs(d.z());
if (std::abs(m.z()) > e.z() + adz) { if (etk::abs(m.z()) > e.z() + adz) {
return false; return false;
} }
// Add in an epsilon term to counteract arithmetic errors when segment is // Add in an epsilon term to counteract arithmetic errors when segment is
@ -128,13 +127,13 @@ bool AABB::testRayIntersect(const Ray& _ray) const {
adz += epsilon; adz += epsilon;
// Test if the cross products between face normals and ray direction are // Test if the cross products between face normals and ray direction are
// separating axis // separating axis
if (std::abs(m.y() * d.z() - m.z() * d.y()) > e.y() * adz + e.z() * ady) { if (etk::abs(m.y() * d.z() - m.z() * d.y()) > e.y() * adz + e.z() * ady) {
return false; return false;
} }
if (std::abs(m.z() * d.x() - m.x() * d.z()) > e.x() * adz + e.z() * adx) { if (etk::abs(m.z() * d.x() - m.x() * d.z()) > e.x() * adz + e.z() * adx) {
return false; return false;
} }
if (std::abs(m.x() * d.y() - m.y() * d.x()) > e.x() * ady + e.y() * adx) { if (etk::abs(m.x() * d.y() - m.y() * d.x()) > e.x() * ady + e.y() * adx) {
return false; return false;
} }
// No separating axis has been found // No separating axis has been found
@ -188,9 +187,9 @@ bool AABB::testCollisionTriangleAABB(const vec3* _trianglePoints) const {
} }
bool AABB::contains(const vec3& _point) const { bool AABB::contains(const vec3& _point) const {
return _point.x() >= m_minCoordinates.x() - MACHINE_EPSILON && _point.x() <= m_maxCoordinates.x() + MACHINE_EPSILON return _point.x() >= m_minCoordinates.x() - FLT_EPSILON && _point.x() <= m_maxCoordinates.x() + FLT_EPSILON
&& _point.y() >= m_minCoordinates.y() - MACHINE_EPSILON && _point.y() <= m_maxCoordinates.y() + MACHINE_EPSILON && _point.y() >= m_minCoordinates.y() - FLT_EPSILON && _point.y() <= m_maxCoordinates.y() + FLT_EPSILON
&& _point.z() >= m_minCoordinates.z() - MACHINE_EPSILON && _point.z() <= m_maxCoordinates.z() + MACHINE_EPSILON; && _point.z() >= m_minCoordinates.z() - FLT_EPSILON && _point.z() <= m_maxCoordinates.z() + FLT_EPSILON;
} }
AABB& AABB::operator=(const AABB& _aabb) { AABB& AABB::operator=(const AABB& _aabb) {

9
ephysics/collision/shapes/BoxShape.cpp
View File

@ -9,7 +9,6 @@
#include <ephysics/collision/ProxyShape.hpp> #include <ephysics/collision/ProxyShape.hpp>
#include <ephysics/configuration.hpp> #include <ephysics/configuration.hpp>
#include <etk/Vector.hpp> #include <etk/Vector.hpp>
#include <cassert>
using namespace ephysics; using namespace ephysics;
@ -47,8 +46,8 @@ void BoxShape::computeLocalInertiaTensor(etk::Matrix3x3& tensor, float mass) con
bool BoxShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const { bool BoxShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const {
vec3 rayDirection = ray.point2 - ray.point1; vec3 rayDirection = ray.point2 - ray.point1;
float tMin = DECIMAL_SMALLEST; float tMin = FLT_MIN;
float tMax = DECIMAL_LARGEST; float tMax = FLT_MAX;
vec3 normalDirection(float(0), float(0), float(0)); vec3 normalDirection(float(0), float(0), float(0));
vec3 currentNormal; vec3 currentNormal;
@ -56,7 +55,7 @@ bool BoxShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* pro
for (int32_t i=0; i<3; i++) { for (int32_t i=0; i<3; i++) {
// If ray is parallel to the slab // If ray is parallel to the slab
if (std::abs(rayDirection[i]) < MACHINE_EPSILON) { if (etk::abs(rayDirection[i]) < FLT_EPSILON) {
// If the ray's origin is not inside the slab, there is no hit // If the ray's origin is not inside the slab, there is no hit
if (ray.point1[i] > m_extent[i] || ray.point1[i] < -m_extent[i]) return false; if (ray.point1[i] > m_extent[i] || ray.point1[i] < -m_extent[i]) return false;
@ -74,7 +73,7 @@ bool BoxShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* pro
// Swap t1 and t2 if need so that t1 is int32_tersection with near plane and // Swap t1 and t2 if need so that t1 is int32_tersection with near plane and
// t2 with far plane // t2 with far plane
if (t1 > t2) { if (t1 > t2) {
std::swap(t1, t2); etk::swap(t1, t2);
currentNormal = -currentNormal; currentNormal = -currentNormal;
} }

1
ephysics/collision/shapes/BoxShape.hpp
View File

@ -5,7 +5,6 @@
*/ */
#pragma once #pragma once
#include <cfloat>
#include <ephysics/collision/shapes/ConvexShape.hpp> #include <ephysics/collision/shapes/ConvexShape.hpp>
#include <ephysics/body/CollisionBody.hpp> #include <ephysics/body/CollisionBody.hpp>
#include <ephysics/mathematics/mathematics.hpp> #include <ephysics/mathematics/mathematics.hpp>

9
ephysics/collision/shapes/CapsuleShape.cpp
View File

@ -8,7 +8,6 @@
#include <ephysics/collision/shapes/CapsuleShape.hpp> #include <ephysics/collision/shapes/CapsuleShape.hpp>
#include <ephysics/collision/ProxyShape.hpp> #include <ephysics/collision/ProxyShape.hpp>
#include <ephysics/configuration.hpp> #include <ephysics/configuration.hpp>
#include <cassert>
using namespace ephysics; using namespace ephysics;
@ -101,7 +100,7 @@ bool CapsuleShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape*
float c = dDotD * k - mDotD * mDotD; float c = dDotD * k - mDotD * mDotD;
// If the ray is parallel to the capsule axis // If the ray is parallel to the capsule axis
if (std::abs(a) < epsilon) { if (etk::abs(a) < epsilon) {
// If the origin is outside the surface of the capusle's cylinder, we return no hit // If the origin is outside the surface of the capusle's cylinder, we return no hit
if (c > 0.0f) return false; if (c > 0.0f) return false;
@ -156,7 +155,7 @@ bool CapsuleShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape*
if (discriminant < 0.0f) return false; if (discriminant < 0.0f) return false;
// Compute the smallest root (first int32_tersection along the ray) // Compute the smallest root (first int32_tersection along the ray)
float t0 = t = (-b - std::sqrt(discriminant)) / a; float t0 = t = (-b - etk::sqrt(discriminant)) / a;
// If the int32_tersection is outside the finite cylinder of the capsule on "p" endcap side // If the int32_tersection is outside the finite cylinder of the capsule on "p" endcap side
float value = mDotD + t * nDotD; float value = mDotD + t * nDotD;
@ -241,10 +240,10 @@ bool CapsuleShape::raycastWithSphereEndCap(const vec3& point1, const vec3& point
float discriminant = b * b - raySquareLength * c; float discriminant = b * b - raySquareLength * c;
// If the discriminant is negative or the ray length is very small, there is no int32_tersection // If the discriminant is negative or the ray length is very small, there is no int32_tersection
if (discriminant < 0.0f || raySquareLength < MACHINE_EPSILON) return false; if (discriminant < 0.0f || raySquareLength < FLT_EPSILON) return false;
// Compute the solution "t" closest to the origin // Compute the solution "t" closest to the origin
float t = -b - std::sqrt(discriminant); float t = -b - etk::sqrt(discriminant);
assert(t >= 0.0f); assert(t >= 0.0f);

3
ephysics/collision/shapes/CollisionShape.hpp
View File

@ -5,8 +5,7 @@
*/ */
#pragma once #pragma once
#include <cassert> #include <etk/typeInfo.hpp>
#include <typeinfo>
#include <etk/math/Vector3D.hpp> #include <etk/math/Vector3D.hpp>
#include <etk/math/Matrix3x3.hpp> #include <etk/math/Matrix3x3.hpp>
#include <ephysics/mathematics/Ray.hpp> #include <ephysics/mathematics/Ray.hpp>

5
ephysics/collision/shapes/ConcaveMeshShape.cpp
View File

@ -6,7 +6,6 @@
#include <ephysics/collision/shapes/ConcaveMeshShape.hpp> #include <ephysics/collision/shapes/ConcaveMeshShape.hpp>
#include <ephysics/debug.hpp> #include <ephysics/debug.hpp>
#include <iostream>
using namespace ephysics; using namespace ephysics;
@ -45,7 +44,7 @@ void ConcaveMeshShape::getTriangleVerticesWithIndexPointer(int32_t _subPart, int
// Get the triangle vertex array of the current sub-part // Get the triangle vertex array of the current sub-part
TriangleVertexArray* triangleVertexArray = m_triangleMesh->getSubpart(_subPart); TriangleVertexArray* triangleVertexArray = m_triangleMesh->getSubpart(_subPart);
if (triangleVertexArray == nullptr) { if (triangleVertexArray == nullptr) {
std::cout << "get nullptr ..." << std::endl; EPHY_ERROR("get nullptr ...");
} }
ephysics::Triangle trianglePoints = triangleVertexArray->getTriangle(_triangleIndex); ephysics::Triangle trianglePoints = triangleVertexArray->getTriangle(_triangleIndex);
_outTriangleVertices[0] = trianglePoints[0] * m_scaling; _outTriangleVertices[0] = trianglePoints[0] * m_scaling;
@ -79,7 +78,7 @@ float ConcaveMeshRaycastCallback::raycastBroadPhaseShape(int32_t _nodeId, const
} }
void ConcaveMeshRaycastCallback::raycastTriangles() { void ConcaveMeshRaycastCallback::raycastTriangles() {
etk::Vector<int32_t>::const_iterator it; etk::Vector<int32_t>::Iterator it;
float smallestHitFraction = m_ray.maxFraction; float smallestHitFraction = m_ray.maxFraction;
for (it = m_hitAABBNodes.begin(); it != m_hitAABBNodes.end(); ++it) { for (it = m_hitAABBNodes.begin(); it != m_hitAABBNodes.end(); ++it) {
// Get the node data (triangle index and mesh subpart index) // Get the node data (triangle index and mesh subpart index)

15
ephysics/collision/shapes/ConeShape.cpp
View File

@ -5,7 +5,6 @@
*/ */
// Libraries // Libraries
#include <complex>
#include <ephysics/configuration.hpp> #include <ephysics/configuration.hpp>
#include <ephysics/collision/shapes/ConeShape.hpp> #include <ephysics/collision/shapes/ConeShape.hpp>
#include <ephysics/collision/ProxyShape.hpp> #include <ephysics/collision/ProxyShape.hpp>
@ -42,7 +41,7 @@ vec3 ConeShape::getLocalSupportPointWithoutMargin(const vec3& direction,
} }
else { else {
float projectedLength = sqrt(v.x() * v.x() + v.z() * v.z()); float projectedLength = sqrt(v.x() * v.x() + v.z() * v.z());
if (projectedLength > MACHINE_EPSILON) { if (projectedLength > FLT_EPSILON) {
float d = m_radius / projectedLength; float d = m_radius / projectedLength;
supportPoint = vec3(v.x() * d, -m_halfHeight, v.z() * d); supportPoint = vec3(v.x() * d, -m_halfHeight, v.z() * d);
} }
@ -79,7 +78,7 @@ bool ConeShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* pr
vec3 localNormal[3]; vec3 localNormal[3];
// If c2 is different from zero // If c2 is different from zero
if (std::abs(c2) > MACHINE_EPSILON) { if (etk::abs(c2) > FLT_EPSILON) {
float gamma = c1 * c1 - c0 * c2; float gamma = c1 * c1 - c0 * c2;
// If there is no real roots in the quadratic equation // If there is no real roots in the quadratic equation
@ -89,7 +88,7 @@ bool ConeShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* pr
else if (gamma > 0.0f) { // The equation has two real roots else if (gamma > 0.0f) { // The equation has two real roots
// Compute two int32_tersections // Compute two int32_tersections
float sqrRoot = std::sqrt(gamma); float sqrRoot = etk::sqrt(gamma);
tHit[0] = (-c1 - sqrRoot) / c2; tHit[0] = (-c1 - sqrRoot) / c2;
tHit[1] = (-c1 + sqrRoot) / c2; tHit[1] = (-c1 + sqrRoot) / c2;
} }
@ -102,7 +101,7 @@ bool ConeShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* pr
else { // If c2 == 0 else { // If c2 == 0
// If c2 = 0 and c1 != 0 // If c2 = 0 and c1 != 0
if (std::abs(c1) > MACHINE_EPSILON) { if (etk::abs(c1) > FLT_EPSILON) {
tHit[0] = -c0 / (float(2.0) * c1); tHit[0] = -c0 / (float(2.0) * c1);
} }
else { // If c2 = c1 = 0 else { // If c2 = c1 = 0
@ -155,7 +154,7 @@ bool ConeShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* pr
// Find the smallest positive t value // Find the smallest positive t value
int32_t hitIndex = -1; int32_t hitIndex = -1;
float t = DECIMAL_LARGEST; float t = FLT_MAX;
for (int32_t i=0; i<3; i++) { for (int32_t i=0; i<3; i++) {
if (tHit[i] < 0.0f) continue; if (tHit[i] < 0.0f) continue;
if (tHit[i] < t) { if (tHit[i] < t) {
@ -174,11 +173,11 @@ bool ConeShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* pr
localHitPoint[hitIndex].z() * localHitPoint[hitIndex].z()); localHitPoint[hitIndex].z() * localHitPoint[hitIndex].z());
float rOverH = m_radius / h; float rOverH = m_radius / h;
float value2 = 1.0f + rOverH * rOverH; float value2 = 1.0f + rOverH * rOverH;
float factor = 1.0f / std::sqrt(value1 * value2); float factor = 1.0f / etk::sqrt(value1 * value2);
float x = localHitPoint[hitIndex].x() * factor; float x = localHitPoint[hitIndex].x() * factor;
float z = localHitPoint[hitIndex].z() * factor; float z = localHitPoint[hitIndex].z() * factor;
localNormal[hitIndex].setX(x); localNormal[hitIndex].setX(x);
localNormal[hitIndex].setY(std::sqrt(x * x + z * z) * rOverH); localNormal[hitIndex].setY(etk::sqrt(x * x + z * z) * rOverH);
localNormal[hitIndex].setZ(z); localNormal[hitIndex].setZ(z);
} }

134
ephysics/collision/shapes/ConvexMeshShape.cpp
View File

@ -4,36 +4,23 @@
* @license BSD 3 clauses (see license file) * @license BSD 3 clauses (see license file)
*/ */
// Libraries
#include <complex>
#include <ephysics/configuration.hpp> #include <ephysics/configuration.hpp>
#include <ephysics/collision/shapes/ConvexMeshShape.hpp> #include <ephysics/collision/shapes/ConvexMeshShape.hpp>
using namespace ephysics; using namespace ephysics;
// Constructor to initialize with an array of 3D vertices.
/// This method creates an int32_ternal copy of the input vertices.
/**
* @param arrayVertices Array with the vertices of the convex mesh
* @param nbVertices Number of vertices in the convex mesh
* @param stride Stride between the beginning of two elements in the vertices array
* @param margin Collision margin (in meters) around the collision shape
*/
ConvexMeshShape::ConvexMeshShape(const float* arrayVertices, uint32_t nbVertices, int32_t stride, float margin) ConvexMeshShape::ConvexMeshShape(const float* arrayVertices, uint32_t nbVertices, int32_t stride, float margin)
: ConvexShape(CONVEX_MESH, margin), m_numberVertices(nbVertices), m_minBounds(0, 0, 0), : ConvexShape(CONVEX_MESH, margin), m_numberVertices(nbVertices), m_minBounds(0, 0, 0),
m_maxBounds(0, 0, 0), m_isEdgesInformationUsed(false) { m_maxBounds(0, 0, 0), m_isEdgesInformationUsed(false) {
assert(nbVertices > 0); assert(nbVertices > 0);
assert(stride > 0); assert(stride > 0);
const unsigned char* vertexPointer = (const unsigned char*) arrayVertices; const unsigned char* vertexPointer = (const unsigned char*) arrayVertices;
// Copy all the vertices int32_to the int32_ternal array // Copy all the vertices int32_to the int32_ternal array
for (uint32_t i=0; i<m_numberVertices; i++) { for (uint32_t i=0; i<m_numberVertices; i++) {
const float* newPoint = (const float*) vertexPointer; const float* newPoint = (const float*) vertexPointer;
m_vertices.pushBack(vec3(newPoint[0], newPoint[1], newPoint[2])); m_vertices.pushBack(vec3(newPoint[0], newPoint[1], newPoint[2]));
vertexPointer += stride; vertexPointer += stride;
} }
// Recalculate the bounds of the mesh // Recalculate the bounds of the mesh
recalculateBounds(); recalculateBounds();
} }
@ -76,50 +63,32 @@ ConvexMeshShape::ConvexMeshShape(float _margin):
m_isEdgesInformationUsed(false) { m_isEdgesInformationUsed(false) {
} }
// Return a local support point in a given direction without the object margin.
/// If the edges information is not used for collision detection, this method will go through
/// the whole vertices list and pick up the vertex with the largest dot product in the support
/// direction. This is an O(n) process with "n" being the number of vertices in the mesh.
/// However, if the edges information is used, we can cache the previous support vertex and use
/// it as a start in a hill-climbing (local search) process to find the new support vertex which
/// will be in most of the cases very close to the previous one. Using hill-climbing, this method
/// runs in almost constant time.
vec3 ConvexMeshShape::getLocalSupportPointWithoutMargin(const vec3& direction,
void** cachedCollisionData) const {
vec3 ConvexMeshShape::getLocalSupportPointWithoutMargin(const vec3& _direction, void** _cachedCollisionData) const {
assert(m_numberVertices == m_vertices.size()); assert(m_numberVertices == m_vertices.size());
assert(cachedCollisionData != NULL); assert(_cachedCollisionData != nullptr);
// Allocate memory for the cached collision data if not allocated yet // Allocate memory for the cached collision data if not allocated yet
if ((*cachedCollisionData) == NULL) { if ((*_cachedCollisionData) == nullptr) {
*cachedCollisionData = (int32_t*) malloc(sizeof(int32_t)); *_cachedCollisionData = (int32_t*) malloc(sizeof(int32_t));
*((int32_t*)(*cachedCollisionData)) = 0; *((int32_t*)(*_cachedCollisionData)) = 0;
} }
// If the edges information is used to speed up the collision detection // If the edges information is used to speed up the collision detection
if (m_isEdgesInformationUsed) { if (m_isEdgesInformationUsed) {
assert(m_edgesAdjacencyList.size() == m_numberVertices); assert(m_edgesAdjacencyList.size() == m_numberVertices);
uint32_t maxVertex = *((int32_t*)(*_cachedCollisionData));
uint32_t maxVertex = *((int32_t*)(*cachedCollisionData)); float maxDotProduct = _direction.dot(m_vertices[maxVertex]);
float maxDotProduct = direction.dot(m_vertices[maxVertex]);
bool isOptimal; bool isOptimal;
// Perform hill-climbing (local search) // Perform hill-climbing (local search)
do { do {
isOptimal = true; isOptimal = true;
assert(m_edgesAdjacencyList[maxVertex].size() > 0);
assert(m_edgesAdjacencyList.at(maxVertex).size() > 0);
// For all neighbors of the current vertex // For all neighbors of the current vertex
std::set<uint32_t>::const_iterator it; etk::Set<uint32_t>::Iterator it;
std::set<uint32_t>::const_iterator itBegin = m_edgesAdjacencyList.at(maxVertex).begin(); etk::Set<uint32_t>::Iterator itBegin = m_edgesAdjacencyList[maxVertex].begin();
std::set<uint32_t>::const_iterator itEnd = m_edgesAdjacencyList.at(maxVertex).end(); etk::Set<uint32_t>::Iterator itEnd = m_edgesAdjacencyList[maxVertex].end();
for (it = itBegin; it != itEnd; ++it) { for (it = itBegin; it != itEnd; ++it) {
// Compute the dot product // Compute the dot product
float dotProduct = direction.dot(m_vertices[*it]); float dotProduct = _direction.dot(m_vertices[*it]);
// If the current vertex is a better vertex (larger dot product) // If the current vertex is a better vertex (larger dot product)
if (dotProduct > maxDotProduct) { if (dotProduct > maxDotProduct) {
maxVertex = *it; maxVertex = *it;
@ -127,35 +96,26 @@ vec3 ConvexMeshShape::getLocalSupportPointWithoutMargin(const vec3& direction,
isOptimal = false; isOptimal = false;
} }
} }
} while(!isOptimal); } while(!isOptimal);
// Cache the support vertex // Cache the support vertex
*((int32_t*)(*cachedCollisionData)) = maxVertex; *((int32_t*)(*_cachedCollisionData)) = maxVertex;
// Return the support vertex // Return the support vertex
return m_vertices[maxVertex] * m_scaling; return m_vertices[maxVertex] * m_scaling;
} } else {
else { // If the edges information is not used // If the edges information is not used
double maxDotProduct = FLT_MIN;
double maxDotProduct = DECIMAL_SMALLEST;
uint32_t indexMaxDotProduct = 0; uint32_t indexMaxDotProduct = 0;
// For each vertex of the mesh // For each vertex of the mesh
for (uint32_t i=0; i<m_numberVertices; i++) { for (uint32_t i=0; i<m_numberVertices; i++) {
// Compute the dot product of the current vertex // Compute the dot product of the current vertex
double dotProduct = direction.dot(m_vertices[i]); double dotProduct = _direction.dot(m_vertices[i]);
// If the current dot product is larger than the maximum one // If the current dot product is larger than the maximum one
if (dotProduct > maxDotProduct) { if (dotProduct > maxDotProduct) {
indexMaxDotProduct = i; indexMaxDotProduct = i;
maxDotProduct = dotProduct; maxDotProduct = dotProduct;
} }
} }
assert(maxDotProduct >= 0.0f); assert(maxDotProduct >= 0.0f);
// Return the vertex with the largest dot product in the support direction // Return the vertex with the largest dot product in the support direction
return m_vertices[indexMaxDotProduct] * m_scaling; return m_vertices[indexMaxDotProduct] * m_scaling;
} }
@ -163,16 +123,12 @@ vec3 ConvexMeshShape::getLocalSupportPointWithoutMargin(const vec3& direction,
// Recompute the bounds of the mesh // Recompute the bounds of the mesh
void ConvexMeshShape::recalculateBounds() { void ConvexMeshShape::recalculateBounds() {
// TODO : Only works if the local origin is inside the mesh // TODO : Only works if the local origin is inside the mesh
// => Make it more robust (init with first vertex of mesh instead) // => Make it more robust (init with first vertex of mesh instead)
m_minBounds.setZero(); m_minBounds.setZero();
m_maxBounds.setZero(); m_maxBounds.setZero();
// For each vertex of the mesh // For each vertex of the mesh
for (uint32_t i=0; i<m_numberVertices; i++) { for (uint32_t i=0; i<m_numberVertices; i++) {
if (m_vertices[i].x() > m_maxBounds.x()) { if (m_vertices[i].x() > m_maxBounds.x()) {
m_maxBounds.setX(m_vertices[i].x()); m_maxBounds.setX(m_vertices[i].x());
} }
@ -185,7 +141,6 @@ void ConvexMeshShape::recalculateBounds() {
if (m_vertices[i].y() < m_minBounds.y()) { if (m_vertices[i].y() < m_minBounds.y()) {
m_minBounds.setY(m_vertices[i].y()); m_minBounds.setY(m_vertices[i].y());
} }
if (m_vertices[i].z() > m_maxBounds.z()) { if (m_vertices[i].z() > m_maxBounds.z()) {
m_maxBounds.setZ(m_vertices[i].z()); m_maxBounds.setZ(m_vertices[i].z());
} }
@ -193,50 +148,32 @@ void ConvexMeshShape::recalculateBounds() {
m_minBounds.setZ(m_vertices[i].z()); m_minBounds.setZ(m_vertices[i].z());
} }
} }
// Apply the local scaling factor // Apply the local scaling factor
m_maxBounds = m_maxBounds * m_scaling; m_maxBounds = m_maxBounds * m_scaling;
m_minBounds = m_minBounds * m_scaling; m_minBounds = m_minBounds * m_scaling;
// Add the object margin to the bounds // Add the object margin to the bounds
m_maxBounds += vec3(m_margin, m_margin, m_margin); m_maxBounds += vec3(m_margin, m_margin, m_margin);
m_minBounds -= vec3(m_margin, m_margin, m_margin); m_minBounds -= vec3(m_margin, m_margin, m_margin);
} }
// Raycast method with feedback information
bool ConvexMeshShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const { bool ConvexMeshShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const {
return proxyShape->m_body->m_world.m_collisionDetection.m_narrowPhaseGJKAlgorithm.raycast(ray, proxyShape, raycastInfo); return proxyShape->m_body->m_world.m_collisionDetection.m_narrowPhaseGJKAlgorithm.raycast(ray, proxyShape, raycastInfo);
} }
/// Set the scaling vector of the collision shape
void ConvexMeshShape::setLocalScaling(const vec3& scaling) { void ConvexMeshShape::setLocalScaling(const vec3& scaling) {
ConvexShape::setLocalScaling(scaling); ConvexShape::setLocalScaling(scaling);
recalculateBounds(); recalculateBounds();
} }
// Return the number of bytes used by the collision shape
size_t ConvexMeshShape::getSizeInBytes() const { size_t ConvexMeshShape::getSizeInBytes() const {
return sizeof(ConvexMeshShape); return sizeof(ConvexMeshShape);
} }
// Return the local bounds of the shape in x, y and z directions
/**
* @param min The minimum bounds of the shape in local-space coordinates
* @param max The maximum bounds of the shape in local-space coordinates
*/
void ConvexMeshShape::getLocalBounds(vec3& min, vec3& max) const { void ConvexMeshShape::getLocalBounds(vec3& min, vec3& max) const {
min = m_minBounds; min = m_minBounds;
max = m_maxBounds; max = m_maxBounds;
} }
// Return the local inertia tensor of the collision shape.
/// The local inertia tensor of the convex mesh is approximated using the inertia tensor
/// of its bounding box.
/**
* @param[out] tensor The 3x3 inertia tensor matrix of the shape in local-space
* coordinates
* @param mass Mass to use to compute the inertia tensor of the collision shape
*/
void ConvexMeshShape::computeLocalInertiaTensor(etk::Matrix3x3& tensor, float mass) const { void ConvexMeshShape::computeLocalInertiaTensor(etk::Matrix3x3& tensor, float mass) const {
float factor = (1.0f / float(3.0)) * mass; float factor = (1.0f / float(3.0)) * mass;
vec3 realExtent = 0.5f * (m_maxBounds - m_minBounds); vec3 realExtent = 0.5f * (m_maxBounds - m_minBounds);
@ -249,16 +186,10 @@ void ConvexMeshShape::computeLocalInertiaTensor(etk::Matrix3x3& tensor, float ma
0.0, 0.0, factor * (xSquare + ySquare)); 0.0, 0.0, factor * (xSquare + ySquare));
} }
// Add a vertex int32_to the convex mesh
/**
* @param vertex Vertex to be added
*/
void ConvexMeshShape::addVertex(const vec3& vertex) { void ConvexMeshShape::addVertex(const vec3& vertex) {
// Add the vertex in to vertices array // Add the vertex in to vertices array
m_vertices.pushBack(vertex); m_vertices.pushBack(vertex);
m_numberVertices++; m_numberVertices++;
// Update the bounds of the mesh // Update the bounds of the mesh
if (vertex.x() * m_scaling.x() > m_maxBounds.x()) { if (vertex.x() * m_scaling.x() > m_maxBounds.x()) {
m_maxBounds.setX(vertex.x() * m_scaling.x()); m_maxBounds.setX(vertex.x() * m_scaling.x());
@ -280,53 +211,30 @@ void ConvexMeshShape::addVertex(const vec3& vertex) {
} }
} }
// Add an edge int32_to the convex mesh by specifying the two vertex indices of the edge.
/// Note that the vertex indices start at zero and need to correspond to the order of
/// the vertices in the vertices array in the constructor or the order of the calls
/// of the addVertex() methods that you use to add vertices int32_to the convex mesh.
/**
* @param v1 Index of the first vertex of the edge to add
* @param v2 Index of the second vertex of the edge to add
*/
void ConvexMeshShape::addEdge(uint32_t v1, uint32_t v2) { void ConvexMeshShape::addEdge(uint32_t v1, uint32_t v2) {
// If the entry for vertex v1 does not exist in the adjacency list // If the entry for vertex v1 does not exist in the adjacency list
if (m_edgesAdjacencyList.count(v1) == 0) { if (m_edgesAdjacencyList.count(v1) == 0) {
m_edgesAdjacencyList.insert(etk::makePair(v1, std::set<uint32_t>())); m_edgesAdjacencyList.add(v1, etk::Set<uint32_t>());
} }
// If the entry for vertex v2 does not exist in the adjacency list // If the entry for vertex v2 does not exist in the adjacency list
if (m_edgesAdjacencyList.count(v2) == 0) { if (m_edgesAdjacencyList.count(v2) == 0) {
m_edgesAdjacencyList.insert(etk::makePair(v2, std::set<uint32_t>())); m_edgesAdjacencyList.add(v2, etk::Set<uint32_t>());
} }
// Add the edge in the adjacency list // Add the edge in the adjacency list
m_edgesAdjacencyList[v1].insert(v2); m_edgesAdjacencyList[v1].add(v2);
m_edgesAdjacencyList[v2].insert(v1); m_edgesAdjacencyList[v2].add(v1);
} }
// Return true if the edges information is used to speed up the collision detection
/**
* @return True if the edges information is used and false otherwise
*/
bool ConvexMeshShape::isEdgesInformationUsed() const { bool ConvexMeshShape::isEdgesInformationUsed() const {
return m_isEdgesInformationUsed; return m_isEdgesInformationUsed;
} }
// Set the variable to know if the edges information is used to speed up the
// collision detection
/**
* @param isEdgesUsed True if you want to use the edges information to speed up
* the collision detection with the convex mesh shape
*/
void ConvexMeshShape::setIsEdgesInformationUsed(bool isEdgesUsed) { void ConvexMeshShape::setIsEdgesInformationUsed(bool isEdgesUsed) {
m_isEdgesInformationUsed = isEdgesUsed; m_isEdgesInformationUsed = isEdgesUsed;
} }
// Return true if a point is inside the collision shape
bool ConvexMeshShape::testPointInside(const vec3& localPoint, bool ConvexMeshShape::testPointInside(const vec3& localPoint,
ProxyShape* proxyShape) const { ProxyShape* proxyShape) const {
// Use the GJK algorithm to test if the point is inside the convex mesh // Use the GJK algorithm to test if the point is inside the convex mesh
return proxyShape->m_body->m_world.m_collisionDetection. return proxyShape->m_body->m_world.m_collisionDetection.
m_narrowPhaseGJKAlgorithm.testPointInside(localPoint, proxyShape); m_narrowPhaseGJKAlgorithm.testPointInside(localPoint, proxyShape);

38
ephysics/collision/shapes/ConvexMeshShape.hpp
View File

@ -11,7 +11,6 @@
#include <ephysics/collision/TriangleMesh.hpp> #include <ephysics/collision/TriangleMesh.hpp>
#include <ephysics/collision/narrowphase/GJK/GJKAlgorithm.hpp> #include <ephysics/collision/narrowphase/GJK/GJKAlgorithm.hpp>
#include <etk/Vector.hpp> #include <etk/Vector.hpp>
#include <set>
#include <etk/Map.hpp> #include <etk/Map.hpp>
namespace ephysics { namespace ephysics {
@ -38,7 +37,7 @@ namespace ephysics {
vec3 m_minBounds; //!< Mesh minimum bounds in the three local x, y and z directions vec3 m_minBounds; //!< Mesh minimum bounds in the three local x, y and z directions
vec3 m_maxBounds; //!< Mesh maximum bounds in the three local x, y and z directions vec3 m_maxBounds; //!< Mesh maximum bounds in the three local x, y and z directions
bool m_isEdgesInformationUsed; //!< True if the shape contains the edges of the convex mesh in order to make the collision detection faster bool m_isEdgesInformationUsed; //!< True if the shape contains the edges of the convex mesh in order to make the collision detection faster
etk::Map<uint32_t, std::set<uint32_t> > m_edgesAdjacencyList; //!< Adjacency list representing the edges of the mesh etk::Map<uint32_t, etk::Set<uint32_t> > m_edgesAdjacencyList; //!< Adjacency list representing the edges of the mesh
/// Private copy-constructor /// Private copy-constructor
ConvexMeshShape(const ConvexMeshShape& _shape); ConvexMeshShape(const ConvexMeshShape& _shape);
/// Private assignment operator /// Private assignment operator
@ -51,7 +50,14 @@ namespace ephysics {
bool raycast(const Ray& _ray, RaycastInfo& _raycastInfo, ProxyShape* _proxyShape) const override; bool raycast(const Ray& _ray, RaycastInfo& _raycastInfo, ProxyShape* _proxyShape) const override;
size_t getSizeInBytes() const override; size_t getSizeInBytes() const override;
public : public :
/// Constructor to initialize with an array of 3D vertices. /**
* @brief Constructor to initialize with an array of 3D vertices.
* This method creates an int32_ternal copy of the input vertices.
* @param[in] _arrayVertices Array with the vertices of the convex mesh
* @param[in] _nbVertices Number of vertices in the convex mesh
* @param[in] _stride Stride between the beginning of two elements in the vertices array
* @param[in] _margin Collision margin (in meters) around the collision shape
*/
ConvexMeshShape(const float* _arrayVertices, ConvexMeshShape(const float* _arrayVertices,
uint32_t _nbVertices, uint32_t _nbVertices,
int32_t _stride, int32_t _stride,
@ -70,14 +76,30 @@ namespace ephysics {
ConvexMeshShape(float _margin = OBJECT_MARGIN); ConvexMeshShape(float _margin = OBJECT_MARGIN);
void getLocalBounds(vec3& _min, vec3& _max) const override; void getLocalBounds(vec3& _min, vec3& _max) const override;
void computeLocalInertiaTensor(etk::Matrix3x3& _tensor, float _mass) const override; void computeLocalInertiaTensor(etk::Matrix3x3& _tensor, float _mass) const override;
/// Add a vertex int32_to the convex mesh /**
* @brief Add a vertex int32_to the convex mesh
* @param vertex Vertex to be added
*/
void addVertex(const vec3& _vertex); void addVertex(const vec3& _vertex);
/// Add an edge int32_to the convex mesh by specifying the two vertex indices of the edge. /**
* @brief Add an edge int32_to the convex mesh by specifying the two vertex indices of the edge.
* Note that the vertex indices start at zero and need to correspond to the order of
* the vertices in the vertices array in the constructor or the order of the calls
* of the addVertex() methods that you use to add vertices int32_to the convex mesh.
* @param[in] _v1 Index of the first vertex of the edge to add
* @param[in] _v2 Index of the second vertex of the edge to add
*/
void addEdge(uint32_t _v1, uint32_t _v2); void addEdge(uint32_t _v1, uint32_t _v2);
/// Return true if the edges information is used to speed up the collision detection /**
* @brief Return true if the edges information is used to speed up the collision detection
* @return True if the edges information is used and false otherwise
*/
bool isEdgesInformationUsed() const; bool isEdgesInformationUsed() const;
/// Set the variable to know if the edges information is used to speed up the /**
/// collision detection * @brief Set the variable to know if the edges information is used to speed up the
* collision detection
* @param[in] isEdgesUsed True if you want to use the edges information to speed up the collision detection with the convex mesh shape
*/
void setIsEdgesInformationUsed(bool _isEdgesUsed); void setIsEdgesInformationUsed(bool _isEdgesUsed);
}; };
} }

2
ephysics/collision/shapes/ConvexShape.cpp
View File

@ -33,7 +33,7 @@ vec3 ConvexShape::getLocalSupportPointWithMargin(const vec3& direction,
// Add the margin to the support point // Add the margin to the support point
vec3 unitVec(0.0, -1.0, 0.0); vec3 unitVec(0.0, -1.0, 0.0);
if (direction.length2() > MACHINE_EPSILON * MACHINE_EPSILON) { if (direction.length2() > FLT_EPSILON * FLT_EPSILON) {
unitVec = direction.safeNormalized(); unitVec = direction.safeNormalized();
} }
supportPoint += unitVec * m_margin; supportPoint += unitVec * m_margin;

6
ephysics/collision/shapes/CylinderShape.cpp
View File

@ -29,7 +29,7 @@ vec3 CylinderShape::getLocalSupportPointWithoutMargin(const vec3& _direction, vo
float uDotv = _direction.y(); float uDotv = _direction.y();
vec3 w(_direction.x(), 0.0, _direction.z()); vec3 w(_direction.x(), 0.0, _direction.z());
float lengthW = sqrt(_direction.x() * _direction.x() + _direction.z() * _direction.z()); float lengthW = sqrt(_direction.x() * _direction.x() + _direction.z() * _direction.z());
if (lengthW > MACHINE_EPSILON) { if (lengthW > FLT_EPSILON) {
if (uDotv < 0.0) { if (uDotv < 0.0) {
supportPoint.setY(-m_halfHeight); supportPoint.setY(-m_halfHeight);
} else { } else {
@ -76,7 +76,7 @@ bool CylinderShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape
float c = dDotD * k - mDotD * mDotD; float c = dDotD * k - mDotD * mDotD;
// If the ray is parallel to the cylinder axis // If the ray is parallel to the cylinder axis
if (std::abs(a) < epsilon) { if (etk::abs(a) < epsilon) {
// If the origin is outside the surface of the cylinder, we return no hit // If the origin is outside the surface of the cylinder, we return no hit
if (c > 0.0f) return false; if (c > 0.0f) return false;
@ -133,7 +133,7 @@ bool CylinderShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape
if (discriminant < 0.0f) return false; if (discriminant < 0.0f) return false;
// Compute the smallest root (first int32_tersection along the ray) // Compute the smallest root (first int32_tersection along the ray)
float t0 = t = (-b - std::sqrt(discriminant)) / a; float t0 = t = (-b - etk::sqrt(discriminant)) / a;
// If the int32_tersection is outside the cylinder on "p" endcap side // If the int32_tersection is outside the cylinder on "p" endcap side
float value = mDotD + t * nDotD; float value = mDotD + t * nDotD;

5
ephysics/collision/shapes/SphereShape.cpp
View File

@ -8,7 +8,6 @@
#include <ephysics/collision/shapes/SphereShape.hpp> #include <ephysics/collision/shapes/SphereShape.hpp>
#include <ephysics/collision/ProxyShape.hpp> #include <ephysics/collision/ProxyShape.hpp>
#include <ephysics/configuration.hpp> #include <ephysics/configuration.hpp>
#include <cassert>
using namespace ephysics; using namespace ephysics;
@ -75,10 +74,10 @@ bool SphereShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape*
float discriminant = b * b - raySquareLength * c; float discriminant = b * b - raySquareLength * c;
// If the discriminant is negative or the ray length is very small, there is no int32_tersection // If the discriminant is negative or the ray length is very small, there is no int32_tersection
if (discriminant < 0.0f || raySquareLength < MACHINE_EPSILON) return false; if (discriminant < 0.0f || raySquareLength < FLT_EPSILON) return false;
// Compute the solution "t" closest to the origin // Compute the solution "t" closest to the origin
float t = -b - std::sqrt(discriminant); float t = -b - etk::sqrt(discriminant);
assert(t >= 0.0f); assert(t >= 0.0f);

1
ephysics/collision/shapes/TriangleShape.cpp
View File

@ -9,7 +9,6 @@
#include <ephysics/collision/ProxyShape.hpp> #include <ephysics/collision/ProxyShape.hpp>
#include <ephysics/engine/Profiler.hpp> #include <ephysics/engine/Profiler.hpp>
#include <ephysics/configuration.hpp> #include <ephysics/configuration.hpp>
#include <cassert>
using namespace ephysics; using namespace ephysics;

16
ephysics/configuration.hpp
View File

@ -6,10 +6,8 @@
#pragma once #pragma once
// Libraries // Libraries
#include <limits> #include <etk/types.hpp>
#include <cfloat> #include <etk/Pair.hpp>
#include <utility>
#include <cstdint>
/// Namespace ephysics /// Namespace ephysics
namespace ephysics { namespace ephysics {
@ -33,16 +31,6 @@ namespace ephysics {
enum ContactsPositionCorrectionTechnique {BAUMGARTE_CONTACTS, SPLIT_IMPULSES}; enum ContactsPositionCorrectionTechnique {BAUMGARTE_CONTACTS, SPLIT_IMPULSES};
// ------------------- Constants ------------------- // // ------------------- Constants ------------------- //
/// Smallest float value (negative)
const float DECIMAL_SMALLEST = - std::numeric_limits<float>::max();
/// Maximum float value
const float DECIMAL_LARGEST = std::numeric_limits<float>::max();
/// Machine epsilon
const float MACHINE_EPSILON = std::numeric_limits<float>::epsilon();
/// Pi constant /// Pi constant
const float PI = float(3.14159265); const float PI = float(3.14159265);

7
ephysics/constraint/ContactPoint.hpp
View File

@ -47,6 +47,13 @@ namespace ephysics {
localPoint2(_localPoint2) { localPoint2(_localPoint2) {
} }
ContactPointInfo():
shape1(nullptr),
shape2(nullptr),
collisionShape1(nullptr),
collisionShape2(nullptr) {
// TODO: add it for etk::Vector
}
}; };
/** /**

5
ephysics/constraint/HingeJoint.cpp
View File

@ -7,7 +7,6 @@
// Libraries // Libraries
#include <ephysics/constraint/HingeJoint.hpp> #include <ephysics/constraint/HingeJoint.hpp>
#include <ephysics/engine/ConstraintSolver.hpp> #include <ephysics/engine/ConstraintSolver.hpp>
#include <cmath>
using namespace ephysics; using namespace ephysics;
@ -770,10 +769,10 @@ float HingeJoint::computeCurrentHingeAngle(const etk::Quaternion& orientationBod
// If the relative rotation axis and the hinge axis are pointing the same direction // If the relative rotation axis and the hinge axis are pointing the same direction
if (dotProduct >= 0.0f) { if (dotProduct >= 0.0f) {
hingeAngle = float(2.0) * std::atan2(sinHalfAngleAbs, cosHalfAngle); hingeAngle = float(2.0) * etk::atan2(sinHalfAngleAbs, cosHalfAngle);
} }
else { else {
hingeAngle = float(2.0) * std::atan2(sinHalfAngleAbs, -cosHalfAngle); hingeAngle = float(2.0) * etk::atan2(sinHalfAngleAbs, -cosHalfAngle);
} }
// Convert the angle from range [-2*pi; 2*pi] int32_to the range [-pi; pi] // Convert the angle from range [-2*pi; 2*pi] int32_to the range [-pi; pi]

37
ephysics/engine/CollisionWorld.cpp
View File

@ -6,7 +6,6 @@
// Libraries // Libraries
#include <ephysics/engine/CollisionWorld.hpp> #include <ephysics/engine/CollisionWorld.hpp>
#include <algorithm>
// Namespaces // Namespaces
using namespace ephysics; using namespace ephysics;
@ -15,7 +14,7 @@ using namespace std;
// Constructor // Constructor
CollisionWorld::CollisionWorld() CollisionWorld::CollisionWorld()
: m_collisionDetection(this, m_memoryAllocator), m_currentBodyID(0), : m_collisionDetection(this, m_memoryAllocator), m_currentBodyID(0),
m_eventListener(NULL) { m_eventListener(nullptrptr) {
} }
@ -23,9 +22,9 @@ CollisionWorld::CollisionWorld()
CollisionWorld::~CollisionWorld() { CollisionWorld::~CollisionWorld() {
// Destroy all the collision bodies that have not been removed // Destroy all the collision bodies that have not been removed
std::set<CollisionBody*>::iterator itBodies; etk::Set<CollisionBody*>::Iterator itBodies;
for (itBodies = m_bodies.begin(); itBodies != m_bodies.end(); ) { for (itBodies = m_bodies.begin(); itBodies != m_bodies.end(); ) {
std::set<CollisionBody*>::iterator itToRemove = itBodies; etk::Set<CollisionBody*>::Iterator itToRemove = itBodies;
++itBodies; ++itBodies;
destroyCollisionBody(*itToRemove); destroyCollisionBody(*itToRemove);
} }
@ -44,13 +43,13 @@ CollisionBody* CollisionWorld::createCollisionBody(const etk::Transform3D& trans
bodyindex bodyID = computeNextAvailableBodyID(); bodyindex bodyID = computeNextAvailableBodyID();
// Largest index cannot be used (it is used for invalid index) // Largest index cannot be used (it is used for invalid index)
assert(bodyID < std::numeric_limits<ephysics::bodyindex>::max()); EPHYSIC_ASSERT(bodyID < std::numeric_limits<ephysics::bodyindex>::max(), "index too big");
// Create the collision body // Create the collision body
CollisionBody* collisionBody = new (m_memoryAllocator.allocate(sizeof(CollisionBody))) CollisionBody* collisionBody = new (m_memoryAllocator.allocate(sizeof(CollisionBody)))
CollisionBody(transform, *this, bodyID); CollisionBody(transform, *this, bodyID);
assert(collisionBody != NULL); EPHYSIC_ASSERT(collisionBody != nullptr, "empty Body collision");
// Add the collision body to the world // Add the collision body to the world
m_bodies.insert(collisionBody); m_bodies.insert(collisionBody);
@ -102,7 +101,7 @@ bodyindex CollisionWorld::computeNextAvailableBodyID() {
void CollisionWorld::resetContactManifoldListsOfBodies() { void CollisionWorld::resetContactManifoldListsOfBodies() {
// For each rigid body of the world // For each rigid body of the world
for (std::set<CollisionBody*>::iterator it = m_bodies.begin(); it != m_bodies.end(); ++it) { for (etk::Set<CollisionBody*>::Iterator it = m_bodies.begin(); it != m_bodies.end(); ++it) {
// Reset the contact manifold list of the body // Reset the contact manifold list of the body
(*it)->resetContactManifoldsList(); (*it)->resetContactManifoldsList();
@ -142,9 +141,9 @@ void CollisionWorld::testCollision(const ProxyShape* shape,
resetContactManifoldListsOfBodies(); resetContactManifoldListsOfBodies();
// Create the sets of shapes // Create the sets of shapes
std::set<uint32_t> shapes; etk::Set<uint32_t> shapes;
shapes.insert(shape->m_broadPhaseID); shapes.insert(shape->m_broadPhaseID);
std::set<uint32_t> emptySet; etk::Set<uint32_t> emptySet;
// Perform the collision detection and report contacts // Perform the collision detection and report contacts
m_collisionDetection.testCollisionBetweenShapes(callback, shapes, emptySet); m_collisionDetection.testCollisionBetweenShapes(callback, shapes, emptySet);
@ -164,9 +163,9 @@ void CollisionWorld::testCollision(const ProxyShape* shape1,
resetContactManifoldListsOfBodies(); resetContactManifoldListsOfBodies();
// Create the sets of shapes // Create the sets of shapes
std::set<uint32_t> shapes1; etk::Set<uint32_t> shapes1;
shapes1.insert(shape1->m_broadPhaseID); shapes1.insert(shape1->m_broadPhaseID);
std::set<uint32_t> shapes2; etk::Set<uint32_t> shapes2;
shapes2.insert(shape2->m_broadPhaseID); shapes2.insert(shape2->m_broadPhaseID);
// Perform the collision detection and report contacts // Perform the collision detection and report contacts
@ -186,15 +185,15 @@ void CollisionWorld::testCollision(const CollisionBody* body,
resetContactManifoldListsOfBodies(); resetContactManifoldListsOfBodies();
// Create the sets of shapes // Create the sets of shapes
std::set<uint32_t> shapes1; etk::Set<uint32_t> shapes1;
// For each shape of the body // For each shape of the body
for (const ProxyShape* shape=body->getProxyShapesList(); shape != NULL; for (const ProxyShape* shape=body->getProxyShapesList(); shape != nullptr;
shape = shape->getNext()) { shape = shape->getNext()) {
shapes1.insert(shape->m_broadPhaseID); shapes1.insert(shape->m_broadPhaseID);
} }
std::set<uint32_t> emptySet; etk::Set<uint32_t> emptySet;
// Perform the collision detection and report contacts // Perform the collision detection and report contacts
m_collisionDetection.testCollisionBetweenShapes(callback, shapes1, emptySet); m_collisionDetection.testCollisionBetweenShapes(callback, shapes1, emptySet);
@ -214,14 +213,14 @@ void CollisionWorld::testCollision(const CollisionBody* body1,
resetContactManifoldListsOfBodies(); resetContactManifoldListsOfBodies();
// Create the sets of shapes // Create the sets of shapes
std::set<uint32_t> shapes1; etk::Set<uint32_t> shapes1;
for (const ProxyShape* shape=body1->getProxyShapesList(); shape != NULL; for (const ProxyShape* shape=body1->getProxyShapesList(); shape != nullptr;
shape = shape->getNext()) { shape = shape->getNext()) {
shapes1.insert(shape->m_broadPhaseID); shapes1.insert(shape->m_broadPhaseID);
} }
std::set<uint32_t> shapes2; etk::Set<uint32_t> shapes2;
for (const ProxyShape* shape=body2->getProxyShapesList(); shape != NULL; for (const ProxyShape* shape=body2->getProxyShapesList(); shape != nullptr;
shape = shape->getNext()) { shape = shape->getNext()) {
shapes2.insert(shape->m_broadPhaseID); shapes2.insert(shape->m_broadPhaseID);
} }
@ -239,7 +238,7 @@ void CollisionWorld::testCollision(CollisionCallback* callback) {
// Reset all the contact manifolds lists of each body // Reset all the contact manifolds lists of each body
resetContactManifoldListsOfBodies(); resetContactManifoldListsOfBodies();
std::set<uint32_t> emptySet; etk::Set<uint32_t> emptySet;
// Perform the collision detection and report contacts // Perform the collision detection and report contacts
m_collisionDetection.testCollisionBetweenShapes(callback, emptySet, emptySet); m_collisionDetection.testCollisionBetweenShapes(callback, emptySet, emptySet);

9
ephysics/engine/CollisionWorld.hpp
View File

@ -6,9 +6,6 @@
#pragma once #pragma once
#include <etk/Vector.hpp> #include <etk/Vector.hpp>
#include <set>
#include <list>
#include <algorithm>
#include <ephysics/mathematics/mathematics.hpp> #include <ephysics/mathematics/mathematics.hpp>
#include <ephysics/engine/Profiler.hpp> #include <ephysics/engine/Profiler.hpp>
#include <ephysics/body/CollisionBody.hpp> #include <ephysics/body/CollisionBody.hpp>
@ -30,7 +27,7 @@ namespace ephysics {
class CollisionWorld { class CollisionWorld {
protected : protected :
CollisionDetection m_collisionDetection; //!< Reference to the collision detection CollisionDetection m_collisionDetection; //!< Reference to the collision detection
std::set<CollisionBody*> m_bodies; //!< All the bodies (rigid and soft) of the world etk::Set<CollisionBody*> m_bodies; //!< All the bodies (rigid and soft) of the world
bodyindex m_currentBodyID; //!< Current body ID bodyindex m_currentBodyID; //!< Current body ID
etk::Vector<uint64_t> m_freeBodiesIDs; //!< List of free ID for rigid bodies etk::Vector<uint64_t> m_freeBodiesIDs; //!< List of free ID for rigid bodies
MemoryAllocator m_memoryAllocator; //!< Memory allocator MemoryAllocator m_memoryAllocator; //!< Memory allocator
@ -52,14 +49,14 @@ namespace ephysics {
* @brief Get an iterator to the beginning of the bodies of the physics world * @brief Get an iterator to the beginning of the bodies of the physics world
* @return An starting iterator to the set of bodies of the world * @return An starting iterator to the set of bodies of the world
*/ */
std::set<CollisionBody*>::iterator getBodiesBeginIterator() { etk::Set<CollisionBody*>::Iterator getBodiesBeginIterator() {
return m_bodies.begin(); return m_bodies.begin();
} }
/** /**
* @brief Get an iterator to the end of the bodies of the physics world * @brief Get an iterator to the end of the bodies of the physics world
* @return An ending iterator to the set of bodies of the world * @return An ending iterator to the set of bodies of the world
*/ */
std::set<CollisionBody*>::iterator getBodiesEndIterator() { etk::Set<CollisionBody*>::Iterator getBodiesEndIterator() {
return m_bodies.end(); return m_bodies.end();
} }
/// Create a collision body /// Create a collision body

1
ephysics/engine/ConstraintSolver.hpp
View File

@ -10,7 +10,6 @@
#include <ephysics/constraint/Joint.hpp> #include <ephysics/constraint/Joint.hpp>
#include <ephysics/engine/Island.hpp> #include <ephysics/engine/Island.hpp>
#include <etk/Map.hpp> #include <etk/Map.hpp>
#include <set>
namespace ephysics { namespace ephysics {
/** /**

5
ephysics/engine/ContactSolver.cpp
View File

@ -9,7 +9,6 @@
#include <ephysics/engine/DynamicsWorld.hpp> #include <ephysics/engine/DynamicsWorld.hpp>
#include <ephysics/body/RigidBody.hpp> #include <ephysics/body/RigidBody.hpp>
#include <ephysics/engine/Profiler.hpp> #include <ephysics/engine/Profiler.hpp>
#include <limits>
using namespace ephysics; using namespace ephysics;
using namespace std; using namespace std;
@ -839,7 +838,7 @@ void ContactSolver::computeFrictionVectors(const vec3& deltaVelocity,
// If the velocty difference in the tangential plane is not zero // If the velocty difference in the tangential plane is not zero
float lengthTangenVelocity = tangentVelocity.length(); float lengthTangenVelocity = tangentVelocity.length();
if (lengthTangenVelocity > MACHINE_EPSILON) { if (lengthTangenVelocity > FLT_EPSILON) {
// Compute the first friction vector in the direction of the tangent // Compute the first friction vector in the direction of the tangent
// velocity difference // velocity difference
@ -869,7 +868,7 @@ void ContactSolver::computeFrictionVectors(const vec3& deltaVelocity,
// If the velocty difference in the tangential plane is not zero // If the velocty difference in the tangential plane is not zero
float lengthTangenVelocity = tangentVelocity.length(); float lengthTangenVelocity = tangentVelocity.length();
if (lengthTangenVelocity > MACHINE_EPSILON) { if (lengthTangenVelocity > FLT_EPSILON) {
// Compute the first friction vector in the direction of the tangent // Compute the first friction vector in the direction of the tangent
// velocity difference // velocity difference

1
ephysics/engine/ContactSolver.hpp
View File

@ -12,7 +12,6 @@
#include <ephysics/engine/Island.hpp> #include <ephysics/engine/Island.hpp>
#include <ephysics/engine/Impulse.hpp> #include <ephysics/engine/Impulse.hpp>
#include <etk/Map.hpp> #include <etk/Map.hpp>
#include <set>
namespace ephysics { namespace ephysics {
/** /**

51
ephysics/engine/DynamicsWorld.cpp
View File

@ -41,17 +41,17 @@ DynamicsWorld::DynamicsWorld(const vec3 &gravity)
DynamicsWorld::~DynamicsWorld() { DynamicsWorld::~DynamicsWorld() {
// Destroy all the joints that have not been removed // Destroy all the joints that have not been removed
std::set<Joint*>::iterator itJoints; etk::Set<Joint*>::Iterator itJoints;
for (itJoints = m_joints.begin(); itJoints != m_joints.end();) { for (itJoints = m_joints.begin(); itJoints != m_joints.end();) {
std::set<Joint*>::iterator itToRemove = itJoints; etk::Set<Joint*>::Iterator itToRemove = itJoints;
++itJoints; ++itJoints;
destroyJoint(*itToRemove); destroyJoint(*itToRemove);
} }
// Destroy all the rigid bodies that have not been removed // Destroy all the rigid bodies that have not been removed
std::set<RigidBody*>::iterator itRigidBodies; etk::Set<RigidBody*>::Iterator itRigidBodies;
for (itRigidBodies = m_rigidBodies.begin(); itRigidBodies != m_rigidBodies.end();) { for (itRigidBodies = m_rigidBodies.begin(); itRigidBodies != m_rigidBodies.end();) {
std::set<RigidBody*>::iterator itToRemove = itRigidBodies; etk::Set<RigidBody*>::Iterator itToRemove = itRigidBodies;
++itRigidBodies; ++itRigidBodies;
destroyRigidBody(*itToRemove); destroyRigidBody(*itToRemove);
} }
@ -83,9 +83,10 @@ DynamicsWorld::~DynamicsWorld() {
assert(m_rigidBodies.size() == 0); assert(m_rigidBodies.size() == 0);
#ifdef IS_PROFILING_ACTIVE #ifdef IS_PROFILING_ACTIVE
// Print32_t the profiling report // Print32_t the profiling report
Profiler::print32_tReport(std::cout); etk::Stream tmp;
Profiler::print32_tReport(tmp);
EPHYSIC_PRINT(tmp.str());
// Destroy the profiler (release the allocated memory) // Destroy the profiler (release the allocated memory)
Profiler::destroy(); Profiler::destroy();
@ -262,7 +263,7 @@ void DynamicsWorld::initVelocityArrays() {
// Initialize the map of body indexes in the velocity arrays // Initialize the map of body indexes in the velocity arrays
m_mapBodyToConstrainedVelocityIndex.clear(); m_mapBodyToConstrainedVelocityIndex.clear();
std::set<RigidBody*>::const_iterator it; etk::Set<RigidBody*>::Iterator it;
uint32_t indexBody = 0; uint32_t indexBody = 0;
for (it = m_rigidBodies.begin(); it != m_rigidBodies.end(); ++it) { for (it = m_rigidBodies.begin(); it != m_rigidBodies.end(); ++it) {
@ -641,11 +642,11 @@ void DynamicsWorld::computeIslands() {
int32_t nbContactManifolds = 0; int32_t nbContactManifolds = 0;
// Reset all the isAlreadyInIsland variables of bodies, joints and contact manifolds // Reset all the isAlreadyInIsland variables of bodies, joints and contact manifolds
for (std::set<RigidBody*>::iterator it = m_rigidBodies.begin(); it != m_rigidBodies.end(); ++it) { for (etk::Set<RigidBody*>::Iterator it = m_rigidBodies.begin(); it != m_rigidBodies.end(); ++it) {
int32_t nbBodyManifolds = (*it)->resetIsAlreadyInIslandAndCountManifolds(); int32_t nbBodyManifolds = (*it)->resetIsAlreadyInIslandAndCountManifolds();
nbContactManifolds += nbBodyManifolds; nbContactManifolds += nbBodyManifolds;
} }
for (std::set<Joint*>::iterator it = m_joints.begin(); it != m_joints.end(); ++it) { for (etk::Set<Joint*>::Iterator it = m_joints.begin(); it != m_joints.end(); ++it) {
(*it)->m_isAlreadyInIsland = false; (*it)->m_isAlreadyInIsland = false;
} }
@ -654,7 +655,7 @@ void DynamicsWorld::computeIslands() {
RigidBody** stackBodiesToVisit = (RigidBody**)m_memoryAllocator.allocate(nbBytesStack); RigidBody** stackBodiesToVisit = (RigidBody**)m_memoryAllocator.allocate(nbBytesStack);
// For each rigid body of the world // For each rigid body of the world
for (std::set<RigidBody*>::iterator it = m_rigidBodies.begin(); it != m_rigidBodies.end(); ++it) { for (etk::Set<RigidBody*>::Iterator it = m_rigidBodies.begin(); it != m_rigidBodies.end(); ++it) {
RigidBody* body = *it; RigidBody* body = *it;
@ -785,7 +786,7 @@ void DynamicsWorld::updateSleepingBodies() {
// For each island of the world // For each island of the world
for (uint32_t i=0; i<m_numberIslands; i++) { for (uint32_t i=0; i<m_numberIslands; i++) {
float minSleepTime = DECIMAL_LARGEST; float minSleepTime = FLT_MAX;
// For each body of the island // For each body of the island
RigidBody** bodies = m_islands[i]->getBodies(); RigidBody** bodies = m_islands[i]->getBodies();
@ -839,7 +840,7 @@ void DynamicsWorld::enableSleeping(bool isSleepingEnabled) {
if (!m_isSleepingEnabled) { if (!m_isSleepingEnabled) {
// For each body of the world // For each body of the world
std::set<RigidBody*>::iterator it; etk::Set<RigidBody*>::Iterator it;
for (it = m_rigidBodies.begin(); it != m_rigidBodies.end(); ++it) { for (it = m_rigidBodies.begin(); it != m_rigidBodies.end(); ++it) {
// Wake up the rigid body // Wake up the rigid body
@ -860,9 +861,9 @@ void DynamicsWorld::testCollision(const ProxyShape* shape,
CollisionCallback* callback) { CollisionCallback* callback) {
// Create the sets of shapes // Create the sets of shapes
std::set<uint32_t> shapes; etk::Set<uint32_t> shapes;
shapes.insert(shape->m_broadPhaseID); shapes.insert(shape->m_broadPhaseID);
std::set<uint32_t> emptySet; etk::Set<uint32_t> emptySet;
// Perform the collision detection and report contacts // Perform the collision detection and report contacts
m_collisionDetection.reportCollisionBetweenShapes(callback, shapes, emptySet); m_collisionDetection.reportCollisionBetweenShapes(callback, shapes, emptySet);
@ -881,9 +882,9 @@ void DynamicsWorld::testCollision(const ProxyShape* shape1,
CollisionCallback* callback) { CollisionCallback* callback) {
// Create the sets of shapes // Create the sets of shapes
std::set<uint32_t> shapes1; etk::Set<uint32_t> shapes1;
shapes1.insert(shape1->m_broadPhaseID); shapes1.insert(shape1->m_broadPhaseID);
std::set<uint32_t> shapes2; etk::Set<uint32_t> shapes2;
shapes2.insert(shape2->m_broadPhaseID); shapes2.insert(shape2->m_broadPhaseID);
// Perform the collision detection and report contacts // Perform the collision detection and report contacts
@ -902,7 +903,7 @@ void DynamicsWorld::testCollision(const CollisionBody* body,
CollisionCallback* callback) { CollisionCallback* callback) {
// Create the sets of shapes // Create the sets of shapes
std::set<uint32_t> shapes1; etk::Set<uint32_t> shapes1;
// For each shape of the body // For each shape of the body
for (const ProxyShape* shape=body->getProxyShapesList(); shape != nullptr; for (const ProxyShape* shape=body->getProxyShapesList(); shape != nullptr;
@ -910,7 +911,7 @@ void DynamicsWorld::testCollision(const CollisionBody* body,
shapes1.insert(shape->m_broadPhaseID); shapes1.insert(shape->m_broadPhaseID);
} }
std::set<uint32_t> emptySet; etk::Set<uint32_t> emptySet;
// Perform the collision detection and report contacts // Perform the collision detection and report contacts
m_collisionDetection.reportCollisionBetweenShapes(callback, shapes1, emptySet); m_collisionDetection.reportCollisionBetweenShapes(callback, shapes1, emptySet);
@ -929,13 +930,13 @@ void DynamicsWorld::testCollision(const CollisionBody* body1,
CollisionCallback* callback) { CollisionCallback* callback) {
// Create the sets of shapes // Create the sets of shapes
std::set<uint32_t> shapes1; etk::Set<uint32_t> shapes1;
for (const ProxyShape* shape=body1->getProxyShapesList(); shape != nullptr; for (const ProxyShape* shape=body1->getProxyShapesList(); shape != nullptr;
shape = shape->getNext()) { shape = shape->getNext()) {
shapes1.insert(shape->m_broadPhaseID); shapes1.insert(shape->m_broadPhaseID);
} }
std::set<uint32_t> shapes2; etk::Set<uint32_t> shapes2;
for (const ProxyShape* shape=body2->getProxyShapesList(); shape != nullptr; for (const ProxyShape* shape=body2->getProxyShapesList(); shape != nullptr;
shape = shape->getNext()) { shape = shape->getNext()) {
shapes2.insert(shape->m_broadPhaseID); shapes2.insert(shape->m_broadPhaseID);
@ -953,7 +954,7 @@ void DynamicsWorld::testCollision(const CollisionBody* body1,
*/ */
void DynamicsWorld::testCollision(CollisionCallback* callback) { void DynamicsWorld::testCollision(CollisionCallback* callback) {
std::set<uint32_t> emptySet; etk::Set<uint32_t> emptySet;
// Perform the collision detection and report contacts // Perform the collision detection and report contacts
m_collisionDetection.reportCollisionBetweenShapes(callback, emptySet, emptySet); m_collisionDetection.reportCollisionBetweenShapes(callback, emptySet, emptySet);
@ -965,7 +966,7 @@ etk::Vector<const ContactManifold*> DynamicsWorld::getContactsList() const {
etk::Vector<const ContactManifold*> contactManifolds; etk::Vector<const ContactManifold*> contactManifolds;
// For each currently overlapping pair of bodies // For each currently overlapping pair of bodies
etk::Map<overlappingpairid, OverlappingPair*>::const_iterator it; etk::Map<overlappingpairid, OverlappingPair*>::Iterator it;
for (it = m_collisionDetection.m_overlappingPairs.begin(); for (it = m_collisionDetection.m_overlappingPairs.begin();
it != m_collisionDetection.m_overlappingPairs.end(); ++it) { it != m_collisionDetection.m_overlappingPairs.end(); ++it) {
@ -990,7 +991,7 @@ etk::Vector<const ContactManifold*> DynamicsWorld::getContactsList() const {
void DynamicsWorld::resetBodiesForceAndTorque() { void DynamicsWorld::resetBodiesForceAndTorque() {
// For each body of the world // For each body of the world
std::set<RigidBody*>::iterator it; etk::Set<RigidBody*>::Iterator it;
for (it = m_rigidBodies.begin(); it != m_rigidBodies.end(); ++it) { for (it = m_rigidBodies.begin(); it != m_rigidBodies.end(); ++it) {
(*it)->m_externalForce.setZero(); (*it)->m_externalForce.setZero();
(*it)->m_externalTorque.setZero(); (*it)->m_externalTorque.setZero();
@ -1114,7 +1115,7 @@ uint32_t DynamicsWorld::getNbJoints() const {
/** /**
* @return Starting iterator of the set of rigid bodies * @return Starting iterator of the set of rigid bodies
*/ */
std::set<RigidBody*>::iterator DynamicsWorld::getRigidBodiesBeginIterator() { etk::Set<RigidBody*>::Iterator DynamicsWorld::getRigidBodiesBeginIterator() {
return m_rigidBodies.begin(); return m_rigidBodies.begin();
} }
@ -1122,7 +1123,7 @@ std::set<RigidBody*>::iterator DynamicsWorld::getRigidBodiesBeginIterator() {
/** /**
* @return Ending iterator of the set of rigid bodies * @return Ending iterator of the set of rigid bodies
*/ */
std::set<RigidBody*>::iterator DynamicsWorld::getRigidBodiesEndIterator() { etk::Set<RigidBody*>::Iterator DynamicsWorld::getRigidBodiesEndIterator() {
return m_rigidBodies.end(); return m_rigidBodies.end();
} }

8
ephysics/engine/DynamicsWorld.hpp
View File

@ -26,8 +26,8 @@ namespace ephysics {
uint32_t m_nbVelocitySolverIterations; //!< Number of iterations for the velocity solver of the Sequential Impulses technique uint32_t m_nbVelocitySolverIterations; //!< Number of iterations for the velocity solver of the Sequential Impulses technique
uint32_t m_nbPositionSolverIterations; //!< Number of iterations for the position solver of the Sequential Impulses technique uint32_t m_nbPositionSolverIterations; //!< Number of iterations for the position solver of the Sequential Impulses technique
bool m_isSleepingEnabled; //!< True if the spleeping technique for inactive bodies is enabled bool m_isSleepingEnabled; //!< True if the spleeping technique for inactive bodies is enabled
std::set<RigidBody*> m_rigidBodies; //!< All the rigid bodies of the physics world etk::Set<RigidBody*> m_rigidBodies; //!< All the rigid bodies of the physics world
std::set<Joint*> m_joints; //!< All the joints of the world etk::Set<Joint*> m_joints; //!< All the joints of the world
vec3 m_gravity; //!< Gravity vector of the world vec3 m_gravity; //!< Gravity vector of the world
float m_timeStep; //!< Current frame time step (in seconds) float m_timeStep; //!< Current frame time step (in seconds)
bool m_isGravityEnabled; //!< True if the gravity force is on bool m_isGravityEnabled; //!< True if the gravity force is on
@ -121,9 +121,9 @@ namespace ephysics {
/// Return the number of joints in the world /// Return the number of joints in the world
uint32_t getNbJoints() const; uint32_t getNbJoints() const;
/// Return an iterator to the beginning of the rigid bodies of the physics world /// Return an iterator to the beginning of the rigid bodies of the physics world
std::set<RigidBody*>::iterator getRigidBodiesBeginIterator(); etk::Set<RigidBody*>::Iterator getRigidBodiesBeginIterator();
/// Return an iterator to the end of the rigid bodies of the physics world /// Return an iterator to the end of the rigid bodies of the physics world
std::set<RigidBody*>::iterator getRigidBodiesEndIterator(); etk::Set<RigidBody*>::Iterator getRigidBodiesEndIterator();
/// Return true if the sleeping technique is enabled /// Return true if the sleeping technique is enabled
bool isSleepingEnabled() const; bool isSleepingEnabled() const;
/// Enable/Disable the sleeping technique /// Enable/Disable the sleeping technique

4
ephysics/engine/Material.hpp
View File

@ -4,7 +4,9 @@
* @license BSD 3 clauses (see license file) * @license BSD 3 clauses (see license file)
*/ */
#pragma once #pragma once
#include <cassert> extern "C" {
#include <assert.h>
}
#include <ephysics/configuration.hpp> #include <ephysics/configuration.hpp>
namespace ephysics { namespace ephysics {

62
ephysics/engine/Profiler.cpp
View File

@ -8,11 +8,12 @@
// Libraries // Libraries
#include <ephysics/Profiler.hpp> #include <ephysics/Profiler.hpp>
#include <ephysics/debug.hpp>
using namespace ephysics; using namespace ephysics;
// Initialization of static variables // Initialization of static variables
ProfileNode Profiler::m_rootNode("Root", NULL); ProfileNode Profiler::m_rootNode("Root", nullptr);
ProfileNode* Profiler::m_currentNode = &Profiler::m_rootNode; ProfileNode* Profiler::m_currentNode = &Profiler::m_rootNode;
long double Profiler::m_profilingStartTime = Timer::getCurrentSystemTime() * 1000.0; long double Profiler::m_profilingStartTime = Timer::getCurrentSystemTime() * 1000.0;
uint32_t Profiler::m_frameCounter = 0; uint32_t Profiler::m_frameCounter = 0;
@ -20,8 +21,8 @@ uint32_t Profiler::m_frameCounter = 0;
// Constructor // Constructor
ProfileNode::ProfileNode(const char* name, ProfileNode* parentNode) ProfileNode::ProfileNode(const char* name, ProfileNode* parentNode)
:m_name(name), m_numberTotalCalls(0), m_startTime(0), m_totalTime(0), :m_name(name), m_numberTotalCalls(0), m_startTime(0), m_totalTime(0),
m_recursionCounter(0), m_parentNode(parentNode), m_childNode(NULL), m_recursionCounter(0), m_parentNode(parentNode), m_childNode(nullptr),
m_siblingNode(NULL) { m_siblingNode(nullptr) {
reset(); reset();
} }
@ -37,7 +38,7 @@ ProfileNode* ProfileNode::findSubNode(const char* name) {
// Try to find the node among the child nodes // Try to find the node among the child nodes
ProfileNode* child = m_childNode; ProfileNode* child = m_childNode;
while (child != NULL) { while (child != nullptr) {
if (child->m_name == name) { if (child->m_name == name) {
return child; return child;
} }
@ -91,12 +92,12 @@ void ProfileNode::reset() {
m_totalTime = 0.0; m_totalTime = 0.0;
// Reset the child node // Reset the child node
if (m_childNode != NULL) { if (m_childNode != nullptr) {
m_childNode->reset(); m_childNode->reset();
} }
// Reset the sibling node // Reset the sibling node
if (m_siblingNode != NULL) { if (m_siblingNode != nullptr) {
m_siblingNode->reset(); m_siblingNode->reset();
} }
} }
@ -104,9 +105,9 @@ void ProfileNode::reset() {
// Destroy the node // Destroy the node
void ProfileNode::destroy() { void ProfileNode::destroy() {
delete m_childNode; delete m_childNode;
m_childNode = NULL; m_childNode = nullptr;
delete m_siblingNode; delete m_siblingNode;
m_siblingNode = NULL; m_siblingNode = nullptr;
} }
// Constructor // Constructor
@ -119,12 +120,12 @@ ProfileNodeIterator::ProfileNodeIterator(ProfileNode* startingNode)
// Enter a given child node // Enter a given child node
void ProfileNodeIterator::enterChild(int32_t index) { void ProfileNodeIterator::enterChild(int32_t index) {
m_currentChildNode = m_currentParentNode->getChildNode(); m_currentChildNode = m_currentParentNode->getChildNode();
while ((m_currentChildNode != NULL) && (index != 0)) { while ((m_currentChildNode != nullptr) && (index != 0)) {
index--; index--;
m_currentChildNode = m_currentChildNode->getSiblingNode(); m_currentChildNode = m_currentChildNode->getSiblingNode();
} }
if (m_currentChildNode != NULL) { if (m_currentChildNode != nullptr) {
m_currentParentNode = m_currentChildNode; m_currentParentNode = m_currentChildNode;
m_currentChildNode = m_currentParentNode->getChildNode(); m_currentChildNode = m_currentParentNode->getChildNode();
} }
@ -132,7 +133,7 @@ void ProfileNodeIterator::enterChild(int32_t index) {
// Enter a given parent node // Enter a given parent node
void ProfileNodeIterator::enterParent() { void ProfileNodeIterator::enterParent() {
if (m_currentParentNode->getParentNode() != NULL) { if (m_currentParentNode->getParentNode() != nullptr) {
m_currentParentNode = m_currentParentNode->getParentNode(); m_currentParentNode = m_currentParentNode->getParentNode();
} }
m_currentChildNode = m_currentParentNode->getChildNode(); m_currentChildNode = m_currentParentNode->getChildNode();
@ -171,11 +172,11 @@ void Profiler::reset() {
} }
// Print32_t the report of the profiler in a given output stream // Print32_t the report of the profiler in a given output stream
void Profiler::print32_tReport(etk::Stream& outputStream) { void Profiler::print32_tReport(etk::Stream& _stream) {
ProfileNodeIterator* iterator = Profiler::getIterator(); ProfileNodeIterator* iterator = Profiler::getIterator();
// Recursively print32_t the report of each node of the profiler tree // Recursively print32_t the report of each node of the profiler tree
print32_tRecursiveNodeReport(iterator, 0, outputStream); print32_tRecursiveNodeReport(iterator, 0, _stream);
// Destroy the iterator // Destroy the iterator
destroyIterator(iterator); destroyIterator(iterator);
@ -183,8 +184,8 @@ void Profiler::print32_tReport(etk::Stream& outputStream) {
// Recursively print32_t the report of a given node of the profiler tree // Recursively print32_t the report of a given node of the profiler tree
void Profiler::print32_tRecursiveNodeReport(ProfileNodeIterator* iterator, void Profiler::print32_tRecursiveNodeReport(ProfileNodeIterator* iterator,
int32_t spacing, int32_t spacing,
etk::Stream& outputStream) { etk::Stream& _stream) {
iterator->first(); iterator->first();
// If we are at the end of a branch in the profiler tree // If we are at the end of a branch in the profiler tree
@ -196,11 +197,13 @@ void Profiler::print32_tRecursiveNodeReport(ProfileNodeIterator* iterator,
iterator->getCurrentParentTotalTime(); iterator->getCurrentParentTotalTime();
long double accumulatedTime = 0.0; long double accumulatedTime = 0.0;
uint32_t nbFrames = Profiler::getNbFrames(); uint32_t nbFrames = Profiler::getNbFrames();
for (int32_t i=0; i<spacing; i++) outputStream << " "; for (int32_t i=0; i<spacing; i++) {
outputStream << "---------------" << std::endl; _stream << " ";
for (int32_t i=0; i<spacing; i++) outputStream << " "; }
outputStream << "| Profiling : " << iterator->getCurrentParentName() << _stream << "---------------\n";
" (total running time : " << parentTime << " ms) ---" << std::endl; for (int32_t i=0; i<spacing; i++) _stream << " ";
_stream << "| Profiling : " << iterator->getCurrentParentName() <<
" (total running time : " << parentTime << " ms) ---\n";
long double totalTime = 0.0; long double totalTime = 0.0;
// Recurse over the children of the current node // Recurse over the children of the current node
@ -209,28 +212,27 @@ void Profiler::print32_tRecursiveNodeReport(ProfileNodeIterator* iterator,
nbChildren++; nbChildren++;
long double currentTotalTime = iterator->getCurrentTotalTime(); long double currentTotalTime = iterator->getCurrentTotalTime();
accumulatedTime += currentTotalTime; accumulatedTime += currentTotalTime;
long double fraction = parentTime > std::numeric_limits<long double>::epsilon() ? long double fraction = parentTime > DLB_EPSILON ?
(currentTotalTime / parentTime) * 100.0 : 0.0; (currentTotalTime / parentTime) * 100.0 : 0.0;
for (int32_t j=0; j<spacing; j++) outputStream << " "; for (int32_t j=0; j<spacing; j++) _stream << " ";
outputStream << "| " << i << " -- " << iterator->getCurrentName() << " : " << _stream << "| " << i << " -- " << iterator->getCurrentName() << " : " <<
fraction << " % | " << (currentTotalTime / (long double) (nbFrames)) << fraction << " % | " << (currentTotalTime / (long double) (nbFrames)) <<
" ms/frame (" << iterator->getCurrentNbTotalCalls() << " calls)" << " ms/frame (" << iterator->getCurrentNbTotalCalls() << " calls)\n";
std::endl;
totalTime += currentTotalTime; totalTime += currentTotalTime;
} }
if (parentTime < accumulatedTime) { if (parentTime < accumulatedTime) {
outputStream << "Something is wrong !" << std::endl; _stream << "Something is wrong !\n";
} }
for (int32_t i=0; i<spacing; i++) outputStream << " "; for (int32_t i=0; i<spacing; i++) _stream << " ";
long double percentage = parentTime > std::numeric_limits<long double>::epsilon() ? long double percentage = parentTime > DLB_EPSILON ?
((parentTime - accumulatedTime) / parentTime) * 100.0 : 0.0; ((parentTime - accumulatedTime) / parentTime) * 100.0 : 0.0;
long double difference = parentTime - accumulatedTime; long double difference = parentTime - accumulatedTime;
outputStream << "| Unaccounted : " << difference << " ms (" << percentage << " %)" << std::endl; _stream << "| Unaccounted : " << difference << " ms (" << percentage << " %)\n";
for (int32_t i=0; i<nbChildren; i++){ for (int32_t i=0; i<nbChildren; i++){
iterator->enterChild(i); iterator->enterChild(i);
print32_tRecursiveNodeReport(iterator, spacing + 3, outputStream); print32_tRecursiveNodeReport(iterator, spacing + 3, _stream);
iterator->enterParent(); iterator->enterParent();
} }
} }

12
ephysics/engine/Timer.hpp
View File

@ -6,18 +6,8 @@
#pragma once #pragma once
// Libraries // Libraries
#include <stdexcept>
#include <iostream>
#include <ctime>
#include <cassert>
#include <ephysics/configuration.hpp> #include <ephysics/configuration.hpp>
#include <eechrono/echrono.hpp>
#if defined(__TARGET_OS__Windows)
#define NOMINMAX // This is used to avoid definition of max() and min() macros
#include <windows.h>
#else // For Mac OS or Linux platform
#include <sys/time.h>
#endif
namespace ephysics { namespace ephysics {

4
ephysics/mathematics/mathematics.hpp
View File

@ -16,6 +16,4 @@
#include <ephysics/configuration.hpp> #include <ephysics/configuration.hpp>
#include <ephysics/mathematics/mathematics_functions.hpp> #include <ephysics/mathematics/mathematics_functions.hpp>
#include <etk/Vector.hpp> #include <etk/Vector.hpp>
#include <cstdio>
#include <cassert>
#include <cmath>

8
ephysics/mathematics/mathematics_functions.hpp
View File

@ -9,10 +9,6 @@
#include <ephysics/configuration.hpp> #include <ephysics/configuration.hpp>
#include <etk/math/Vector3D.hpp> #include <etk/math/Vector3D.hpp>
#include <algorithm>
#include <cassert>
#include <cmath>
/// ReactPhysics3D namespace /// ReactPhysics3D namespace
namespace ephysics { namespace ephysics {
@ -20,8 +16,8 @@ namespace ephysics {
/// Function to test if two real numbers are (almost) equal /// Function to test if two real numbers are (almost) equal
/// We test if two numbers a and b are such that (a-b) are in [-EPSILON; EPSILON] /// We test if two numbers a and b are such that (a-b) are in [-EPSILON; EPSILON]
inline bool approxEqual(float a, float b, float epsilon = MACHINE_EPSILON) { inline bool approxEqual(float a, float b, float epsilon = FLT_EPSILON) {
return (std::fabs(a - b) < epsilon); return (etk::abs(a - b) < epsilon);
} }
/// Function that returns the result of the "value" clamped by /// Function that returns the result of the "value" clamped by

2
ephysics/memory/MemoryAllocator.cpp
View File

@ -6,8 +6,6 @@
// Libraries // Libraries
#include <ephysics/memory/MemoryAllocator.hpp> #include <ephysics/memory/MemoryAllocator.hpp>
#include <cstdlib>
#include <cassert>
using namespace ephysics; using namespace ephysics;

1
ephysics/memory/MemoryAllocator.hpp
View File

@ -6,7 +6,6 @@
#pragma once #pragma once
// Libraries // Libraries
#include <cstring>
#include <ephysics/configuration.hpp> #include <ephysics/configuration.hpp>
/// ReactPhysics3D namespace /// ReactPhysics3D namespace

2
lutin_ephysics-test.py
View File

@ -32,7 +32,7 @@ def configure(target, my_module):
]) ])
my_module.add_depend([ my_module.add_depend([
'ephysics', 'ephysics',
'gtest', 'etest',
'test-debug' 'test-debug'
]) ])
my_module.add_path(".") my_module.add_path(".")

2
test/Test.hpp
View File

@ -28,8 +28,6 @@
// Libraries // Libraries
#include <etk/String.hpp> #include <etk/String.hpp>
#include <iostream>
#include <cassert>
/// Reactphysics3D namespace /// Reactphysics3D namespace
namespace ephysics { namespace ephysics {

1
test/TestSuite.hpp
View File

@ -29,7 +29,6 @@
// Libraries // Libraries
#include <test/Test.hpp> #include <test/Test.hpp>
#include <etk/Vector.hpp> #include <etk/Vector.hpp>
#include <stdexcept>
/// Reactphysics3D namespace /// Reactphysics3D namespace
namespace ephysics { namespace ephysics {

6
test/tests/mathematics/TestQuaternion.h
View File

@ -95,7 +95,7 @@ class Testetk::Quaternion : public Test {
const float PI_OVER_2 = PI * 0.5f; const float PI_OVER_2 = PI * 0.5f;
const float PI_OVER_4 = PI_OVER_2 * 0.5f; const float PI_OVER_4 = PI_OVER_2 * 0.5f;
etk::Quaternion quaternion5(PI_OVER_2, 0, 0); etk::Quaternion quaternion5(PI_OVER_2, 0, 0);
etk::Quaternion quaternionTest5(std::sin(PI_OVER_4), 0, 0, std::cos(PI_OVER_4)); etk::Quaternion quaternionTest5(etk::sin(PI_OVER_4), 0, 0, etk::cos(PI_OVER_4));
quaternionTest5.normalize(); quaternionTest5.normalize();
test(approxEqual(quaternion5.x(), quaternionTest5.x)); test(approxEqual(quaternion5.x(), quaternionTest5.x));
test(approxEqual(quaternion5.y(), quaternionTest5.y)); test(approxEqual(quaternion5.y(), quaternionTest5.y));
@ -103,7 +103,7 @@ class Testetk::Quaternion : public Test {
test(approxEqual(quaternion5.w, quaternionTest5.w)); test(approxEqual(quaternion5.w, quaternionTest5.w));
etk::Quaternion quaternion6(0, PI_OVER_2, 0); etk::Quaternion quaternion6(0, PI_OVER_2, 0);
etk::Quaternion quaternionTest6(0, std::sin(PI_OVER_4), 0, std::cos(PI_OVER_4)); etk::Quaternion quaternionTest6(0, etk::sin(PI_OVER_4), 0, etk::cos(PI_OVER_4));
quaternionTest6.normalize(); quaternionTest6.normalize();
test(approxEqual(quaternion6.x(), quaternionTest6.x)); test(approxEqual(quaternion6.x(), quaternionTest6.x));
test(approxEqual(quaternion6.y(), quaternionTest6.y)); test(approxEqual(quaternion6.y(), quaternionTest6.y));
@ -111,7 +111,7 @@ class Testetk::Quaternion : public Test {
test(approxEqual(quaternion6.w, quaternionTest6.w)); test(approxEqual(quaternion6.w, quaternionTest6.w));
etk::Quaternion quaternion7(vec3(0, 0, PI_OVER_2)); etk::Quaternion quaternion7(vec3(0, 0, PI_OVER_2));
etk::Quaternion quaternionTest7(0, 0, std::sin(PI_OVER_4), std::cos(PI_OVER_4)); etk::Quaternion quaternionTest7(0, 0, etk::sin(PI_OVER_4), etk::cos(PI_OVER_4));
quaternionTest7.normalize(); quaternionTest7.normalize();
test(approxEqual(quaternion7.x(), quaternionTest7.x)); test(approxEqual(quaternion7.x(), quaternionTest7.x));
test(approxEqual(quaternion7.y(), quaternionTest7.y)); test(approxEqual(quaternion7.y(), quaternionTest7.y));

6
tools/testbed/scenes/raycast/RaycastScene.cpp
View File

@ -163,9 +163,9 @@ void RaycastScene::createLines() {
float phi = j * PI / float(nbRaysOneDimension); float phi = j * PI / float(nbRaysOneDimension);
// Generate a point on a sphere with spherical coordinates // Generate a point on a sphere with spherical coordinates
float x = RAY_LENGTH * std::sin(phi) * std::cos(theta); float x = RAY_LENGTH * etk::sin(phi) * etk::cos(theta);
float y = RAY_LENGTH * std::sin(phi) * std::sin(theta); float y = RAY_LENGTH * etk::sin(phi) * etk::sin(theta);
float z = RAY_LENGTH * std::cos(phi); float z = RAY_LENGTH * etk::cos(phi);
// Create a line from the point on the sphere to the center of // Create a line from the point on the sphere to the center of
// the scene // the scene