diff --git a/ephysics/body/Body.hpp b/ephysics/body/Body.hpp index eb62160..b344920 100644 --- a/ephysics/body/Body.hpp +++ b/ephysics/body/Body.hpp @@ -5,8 +5,9 @@ */ #pragma once -#include -#include +extern "C" { + #include +} #include namespace ephysics { diff --git a/ephysics/body/CollisionBody.hpp b/ephysics/body/CollisionBody.hpp index 0a845c9..a636b65 100644 --- a/ephysics/body/CollisionBody.hpp +++ b/ephysics/body/CollisionBody.hpp @@ -6,8 +6,6 @@ #pragma once // Libraries -#include -#include #include #include #include diff --git a/ephysics/body/RigidBody.hpp b/ephysics/body/RigidBody.hpp index d97c6e1..248435d 100644 --- a/ephysics/body/RigidBody.hpp +++ b/ephysics/body/RigidBody.hpp @@ -5,7 +5,6 @@ */ #pragma once -#include #include #include #include diff --git a/ephysics/collision/CollisionDetection.cpp b/ephysics/collision/CollisionDetection.cpp index 6cb0058..0e45002 100644 --- a/ephysics/collision/CollisionDetection.cpp +++ b/ephysics/collision/CollisionDetection.cpp @@ -11,106 +11,85 @@ #include #include #include -#include -#include -#include -#include -#include // We want to use the ReactPhysics3D namespace using namespace ephysics; using namespace std; // Constructor -CollisionDetection::CollisionDetection(CollisionWorld* world, MemoryAllocator& memoryAllocator) - : m_memoryAllocator(memoryAllocator), - m_world(world), m_broadPhaseAlgorithm(*this), - m_isCollisionShapesAdded(false) { - +CollisionDetection::CollisionDetection(CollisionWorld* _world, MemoryAllocator& _memoryAllocator): + m_memoryAllocator(_memoryAllocator), + m_world(_world), + m_broadPhaseAlgorithm(*this), + m_isCollisionShapesAdded(false) { // Set the default collision dispatch configuration setCollisionDispatch(&m_defaultCollisionDispatch); - // Fill-in the collision detection matrix with algorithms fillInCollisionMatrix(); } -// Destructor CollisionDetection::~CollisionDetection() { - + } -// Compute the collision detection void CollisionDetection::computeCollisionDetection() { - PROFILE("CollisionDetection::computeCollisionDetection()"); - // Compute the broad-phase collision detection computeBroadPhase(); - // Compute the narrow-phase collision detection computeNarrowPhase(); } -// Compute the collision detection void CollisionDetection::testCollisionBetweenShapes(CollisionCallback* callback, - const std::set& shapes1, - const std::set& shapes2) { - + const etk::Set& shapes1, + const etk::Set& shapes2) { // Compute the broad-phase collision detection computeBroadPhase(); - // Delete all the contact points in the currently overlapping pairs clearContactPoints(); - // Compute the narrow-phase collision detection among given sets of shapes computeNarrowPhaseBetweenShapes(callback, shapes1, shapes2); } -// Report collision between two sets of shapes void CollisionDetection::reportCollisionBetweenShapes(CollisionCallback* callback, - const std::set& shapes1, - const std::set& shapes2) { - + const etk::Set& shapes1, + const etk::Set& shapes2) { // For each possible collision pair of bodies - map::iterator it; + etk::Map::Iterator it; for (it = m_overlappingPairs.begin(); it != m_overlappingPairs.end(); ++it) { - OverlappingPair* pair = it->second; - const ProxyShape* shape1 = pair->getShape1(); const ProxyShape* shape2 = pair->getShape2(); - assert(shape1->m_broadPhaseID != shape2->m_broadPhaseID); - // If both shapes1 and shapes2 sets are non-empty, we check that // shape1 is among on set and shape2 is among the other one - if (!shapes1.empty() && !shapes2.empty() && - (shapes1.count(shape1->m_broadPhaseID) == 0 || shapes2.count(shape2->m_broadPhaseID) == 0) && - (shapes1.count(shape2->m_broadPhaseID) == 0 || shapes2.count(shape1->m_broadPhaseID) == 0)) { + if ( !shapes1.empty() + && !shapes2.empty() + && ( 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; } - if (!shapes1.empty() && shapes2.empty() && - shapes1.count(shape1->m_broadPhaseID) == 0 && shapes1.count(shape2->m_broadPhaseID) == 0) - { + if ( !shapes1.empty() + && shapes2.empty() + && shapes1.count(shape1->m_broadPhaseID) == 0 + && shapes1.count(shape2->m_broadPhaseID) == 0) { continue; } - if (!shapes2.empty() && shapes1.empty() && - shapes2.count(shape1->m_broadPhaseID) == 0 && shapes2.count(shape2->m_broadPhaseID) == 0) - { + if ( !shapes2.empty() + && shapes1.empty() + && shapes2.count(shape1->m_broadPhaseID) == 0 + && shapes2.count(shape2->m_broadPhaseID) == 0) { continue; } - // For each contact manifold set of the overlapping pair const ContactManifoldSet& manifoldSet = pair->getContactManifoldSet(); for (int32_t j=0; jgetNbContactPoints(); i++) { - ContactPoint* contactPoint = manifold->getContactPoint(i); - // Create the contact info object for the contact ContactPointInfo contactInfo(manifold->getShape1(), manifold->getShape2(), manifold->getShape1()->getCollisionShape(), @@ -119,22 +98,19 @@ void CollisionDetection::reportCollisionBetweenShapes(CollisionCallback* callbac contactPoint->getPenetrationDepth(), contactPoint->getLocalPointOnBody1(), contactPoint->getLocalPointOnBody2()); - // 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() { - PROFILE("CollisionDetection::computeBroadPhase()"); - // If new collision shapes have been added to bodies if (m_isCollisionShapesAdded) { - // Ask the broad-phase to recompute the overlapping pairs of collision // shapes. This call can only add new overlapping pairs in the collision // detection. @@ -142,73 +118,59 @@ void CollisionDetection::computeBroadPhase() { } } -// Compute the narrow-phase collision detection void CollisionDetection::computeNarrowPhase() { - PROFILE("CollisionDetection::computeNarrowPhase()"); - // Clear the set of overlapping pairs in narrow-phase contact m_contactOverlappingPairs.clear(); - // For each possible collision pair of bodies - map::iterator it; + etk::Map::Iterator it; for (it = m_overlappingPairs.begin(); it != m_overlappingPairs.end(); ) { - OverlappingPair* pair = it->second; - ProxyShape* shape1 = pair->getShape1(); ProxyShape* shape2 = pair->getShape2(); - assert(shape1->m_broadPhaseID != shape2->m_broadPhaseID); - // Check if the collision filtering allows collision between the two shapes and // that the two shapes are still overlapping. Otherwise, we destroy the // overlapping pair if (((shape1->getCollideWithMaskBits() & shape2->getCollisionCategoryBits()) == 0 || (shape1->getCollisionCategoryBits() & shape2->getCollideWithMaskBits()) == 0) || !m_broadPhaseAlgorithm.testOverlappingShapes(shape1, shape2)) { - - etk::Map::iterator itToRemove = it; + etk::Map::Iterator itToRemove = it; ++it; - // TODO : Remove all the contact manifold of the overlapping pair from the contact manifolds list of the two bodies involved - // Destroy the overlapping pair itToRemove->second->~OverlappingPair(); m_world->m_memoryAllocator.release(itToRemove->second, sizeof(OverlappingPair)); m_overlappingPairs.erase(itToRemove); continue; - } - else { + } else { ++it; } - CollisionBody* const body1 = shape1->getBody(); CollisionBody* const body2 = shape2->getBody(); - // Update the contact cache of the overlapping pair pair->update(); - // Check that at least one body is awake and not static bool isBody1Active = !body1->isSleeping() && body1->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 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 const CollisionShapeType shape1Type = shape1->getCollisionShape()->getType(); const CollisionShapeType shape2Type = shape2->getCollisionShape()->getType(); NarrowPhaseAlgorithm* narrowPhaseAlgorithm = m_collisionMatrix[shape1Type][shape2Type]; - // 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 narrowPhaseAlgorithm->setCurrentOverlappingPair(pair); - // Create the CollisionShapeInfo objects CollisionShapeInfo shape1Info(shape1, shape1->getCollisionShape(), shape1->getLocalToWorldTransform(), pair, shape1->getCachedCollisionData()); @@ -220,166 +182,140 @@ void CollisionDetection::computeNarrowPhase() { // notifyContact() callback method will be called. narrowPhaseAlgorithm->testCollision(shape1Info, shape2Info, this); } - // Add all the contact manifolds (between colliding bodies) to the bodies addAllContactManifoldsToBodies(); } -// Compute the narrow-phase collision detection void CollisionDetection::computeNarrowPhaseBetweenShapes(CollisionCallback* callback, - const std::set& shapes1, - const std::set& shapes2) { - + const etk::Set& shapes1, + const etk::Set& shapes2) { m_contactOverlappingPairs.clear(); - // For each possible collision pair of bodies - map::iterator it; + etk::Map::Iterator it; for (it = m_overlappingPairs.begin(); it != m_overlappingPairs.end(); ) { - OverlappingPair* pair = it->second; - ProxyShape* shape1 = pair->getShape1(); ProxyShape* shape2 = pair->getShape2(); - assert(shape1->m_broadPhaseID != shape2->m_broadPhaseID); - // If both shapes1 and shapes2 sets are non-empty, we check that // shape1 is among on set and shape2 is among the other one - if (!shapes1.empty() && !shapes2.empty() && - (shapes1.count(shape1->m_broadPhaseID) == 0 || shapes2.count(shape2->m_broadPhaseID) == 0) && - (shapes1.count(shape2->m_broadPhaseID) == 0 || shapes2.count(shape1->m_broadPhaseID) == 0)) { + if ( !shapes1.empty() + && !shapes2.empty() + && ( 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; continue; } - if (!shapes1.empty() && shapes2.empty() && - shapes1.count(shape1->m_broadPhaseID) == 0 && shapes1.count(shape2->m_broadPhaseID) == 0) - { + if ( !shapes1.empty() + && shapes2.empty() + && shapes1.count(shape1->m_broadPhaseID) == 0 + && shapes1.count(shape2->m_broadPhaseID) == 0) { ++it; continue; } - if (!shapes2.empty() && shapes1.empty() && - shapes2.count(shape1->m_broadPhaseID) == 0 && shapes2.count(shape2->m_broadPhaseID) == 0) - { + if ( !shapes2.empty() + && shapes1.empty() + && shapes2.count(shape1->m_broadPhaseID) == 0 + && shapes2.count(shape2->m_broadPhaseID) == 0) { ++it; continue; } - // Check if the collision filtering allows collision between the two shapes and // that the two shapes are still overlapping. Otherwise, we destroy the // overlapping pair if (((shape1->getCollideWithMaskBits() & shape2->getCollisionCategoryBits()) == 0 || (shape1->getCollisionCategoryBits() & shape2->getCollideWithMaskBits()) == 0) || !m_broadPhaseAlgorithm.testOverlappingShapes(shape1, shape2)) { - - etk::Map::iterator itToRemove = it; + etk::Map::Iterator itToRemove = it; ++it; - // TODO : Remove all the contact manifold of the overlapping pair from the contact manifolds list of the two bodies involved - // Destroy the overlapping pair itToRemove->second->~OverlappingPair(); m_world->m_memoryAllocator.release(itToRemove->second, sizeof(OverlappingPair)); m_overlappingPairs.erase(itToRemove); continue; - } - else { + } else { ++it; } - CollisionBody* const body1 = shape1->getBody(); CollisionBody* const body2 = shape2->getBody(); - // Update the contact cache of the overlapping pair pair->update(); - // 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); - 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 - 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 const CollisionShapeType shape1Type = shape1->getCollisionShape()->getType(); const CollisionShapeType shape2Type = shape2->getCollisionShape()->getType(); NarrowPhaseAlgorithm* narrowPhaseAlgorithm = m_collisionMatrix[shape1Type][shape2Type]; - // 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 narrowPhaseAlgorithm->setCurrentOverlappingPair(pair); - // Create the CollisionShapeInfo objects CollisionShapeInfo shape1Info(shape1, shape1->getCollisionShape(), shape1->getLocalToWorldTransform(), pair, shape1->getCachedCollisionData()); CollisionShapeInfo shape2Info(shape2, shape2->getCollisionShape(), shape2->getLocalToWorldTransform(), pair, shape2->getCachedCollisionData()); - TestCollisionBetweenShapesCallback narrowPhaseCallback(callback); - // Use the narrow-phase collision detection algorithm to check // if there really is a collision narrowPhaseAlgorithm->testCollision(shape1Info, shape2Info, &narrowPhaseCallback); } - // Add all the contact manifolds (between colliding bodies) to the bodies 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) { - assert(shape1->m_broadPhaseID != shape2->m_broadPhaseID); - // 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 - if ((shape1->getCollideWithMaskBits() & shape2->getCollisionCategoryBits()) == 0 || - (shape1->getCollisionCategoryBits() & shape2->getCollideWithMaskBits()) == 0) return; - + if ( (shape1->getCollideWithMaskBits() & shape2->getCollisionCategoryBits()) == 0 + || (shape1->getCollisionCategoryBits() & shape2->getCollideWithMaskBits()) == 0) { + return; + } // Compute the overlapping pair ID overlappingpairid pairID = OverlappingPair::computeID(shape1, shape2); - // Check if the overlapping pair already exists if (m_overlappingPairs.find(pairID) != m_overlappingPairs.end()) return; - // Compute the maximum number of contact manifolds for this pair int32_t nbMaxManifolds = CollisionShape::computeNbMaxContactManifolds(shape1->getCollisionShape()->getType(), shape2->getCollisionShape()->getType()); - // 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(shape1, shape2, nbMaxManifolds, m_world->m_memoryAllocator); - assert(newPair != NULL); - -#ifndef NDEBUG - etk::Pair::iterator, bool> check = -#endif - m_overlappingPairs.insert(make_pair(pairID, newPair)); - assert(check.second); - + assert(newPair != nullptr); + m_overlappingPairs.set(pairID, newPair); // Wake up the two bodies shape1->getBody()->setIsSleeping(false); shape2->getBody()->setIsSleeping(false); } -// Remove a body from the collision detection void CollisionDetection::removeProxyCollisionShape(ProxyShape* proxyShape) { - // Remove all the overlapping pairs involving this proxy shape - etk::Map::iterator it; + etk::Map::Iterator it; for (it = m_overlappingPairs.begin(); it != m_overlappingPairs.end(); ) { if (it->second->getShape1()->m_broadPhaseID == proxyShape->m_broadPhaseID|| it->second->getShape2()->m_broadPhaseID == proxyShape->m_broadPhaseID) { - etk::Map::iterator itToRemove = it; + etk::Map::Iterator itToRemove = it; ++it; - // TODO : Remove all the contact manifold of the overlapping pair from the contact manifolds list of the two bodies involved - // Destroy the overlapping pair itToRemove->second->~OverlappingPair(); m_world->m_memoryAllocator.release(itToRemove->second, sizeof(OverlappingPair)); @@ -389,39 +325,28 @@ void CollisionDetection::removeProxyCollisionShape(ProxyShape* proxyShape) { ++it; } } - // Remove the body from the broad-phase 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) { - // If it is the first contact since the pairs are overlapping if (overlappingPair->getNbContactPoints() == 0) { - // Trigger a callback event if (m_world->m_eventListener != NULL) m_world->m_eventListener->beginContact(contactInfo); } - // Create a new contact createContact(overlappingPair, contactInfo); - // Trigger a callback event for the new contact 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 ContactPoint* contact = new (m_world->m_memoryAllocator.allocate(sizeof(ContactPoint))) ContactPoint(contactInfo); - // Add the contact to the contact manifold set of the corresponding overlapping pair overlappingPair->addContact(contact); - // Add the overlapping pair int32_to the set of pairs in contact during narrow-phase overlappingpairid pairId = OverlappingPair::computeID(overlappingPair->getShape1(), overlappingPair->getShape2()); @@ -429,34 +354,24 @@ void CollisionDetection::createContact(OverlappingPair* overlappingPair, } void CollisionDetection::addAllContactManifoldsToBodies() { - // For each overlapping pairs in contact during the narrow-phase - etk::Map::iterator it; + etk::Map::Iterator 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 // of the two bodies involved in the contact 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) { - - assert(pair != NULL); - + assert(pair != nullptr); CollisionBody* body1 = pair->getShape1()->getBody(); CollisionBody* body2 = pair->getShape2()->getBody(); const ContactManifoldSet& manifoldSet = pair->getContactManifoldSet(); - // For each contact manifold in the set of manifolds in the pair for (int32_t i=0; igetNbContactPoints() > 0); - // Add the contact manifold at the beginning of the linked // list of contact manifolds of the first body void* allocatedMemory1 = m_world->m_memoryAllocator.allocate(sizeof(ContactManifoldListElement)); @@ -464,7 +379,6 @@ void CollisionDetection::addContactManifoldToBody(OverlappingPair* pair) { ContactManifoldListElement(contactManifold, body1->m_contactManifoldsList); body1->m_contactManifoldsList = listElement1; - // Add the contact manifold at the beginning of the linked // list of the contact manifolds of the second body 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() { // For each overlapping pair - etk::Map::iterator it; + etk::Map::Iterator it; for (it = m_overlappingPairs.begin(); it != m_overlappingPairs.end(); ++it) { it->second->clearContactPoints(); } } -// Fill-in the collision detection matrix void CollisionDetection::fillInCollisionMatrix() { - // For each possible type of collision shape for (int32_t i=0; im_eventListener; } -/// Return a reference to the world memory allocator MemoryAllocator& CollisionDetection::getWorldMemoryAllocator() { return m_world->m_memoryAllocator; } -// Called by a narrow-phase collision algorithm when a new contact has been found -void TestCollisionBetweenShapesCallback::notifyContact(OverlappingPair* overlappingPair, - const ContactPointInfo& contactInfo) { - m_collisionCallback->notifyContact(contactInfo); +void TestCollisionBetweenShapesCallback::notifyContact(OverlappingPair* _overlappingPair, + const ContactPointInfo& _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 { - return m_collisionMatrix[shape1Type][shape2Type]; +NarrowPhaseAlgorithm* CollisionDetection::getCollisionAlgorithm(CollisionShapeType _shape1Type, CollisionShapeType _shape2Type) const { + return m_collisionMatrix[_shape1Type][_shape2Type]; } -// Set the collision dispatch configuration -void CollisionDetection::setCollisionDispatch(CollisionDispatch* collisionDispatch) { - m_collisionDispatch = collisionDispatch; - +void CollisionDetection::setCollisionDispatch(CollisionDispatch* _collisionDispatch) { + m_collisionDispatch = _collisionDispatch; m_collisionDispatch->init(this, &m_memoryAllocator); - // Fill-in the collision matrix with the new algorithms to use 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 - m_broadPhaseAlgorithm.addProxyCollisionShape(proxyShape, aabb); - + m_broadPhaseAlgorithm.addProxyCollisionShape(_proxyShape, _aabb); m_isCollisionShapesAdded = true; -} - -// Add a pair of bodies that cannot collide with each other -void CollisionDetection::addNoCollisionPair(CollisionBody* body1, - CollisionBody* body2) { - m_noCollisionPairs.insert(OverlappingPair::computeBodiesIndexPair(body1, body2)); } -// Remove a pair of bodies that cannot collide with each other +void CollisionDetection::addNoCollisionPair(CollisionBody* body1, CollisionBody* body2) { + m_noCollisionPairs.set(OverlappingPair::computeBodiesIndexPair(body1, body2)); +} + void CollisionDetection::removeNoCollisionPair(CollisionBody* body1, 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) { m_broadPhaseAlgorithm.addMovedCollisionShape(shape->m_broadPhaseID); } -// Update a proxy collision shape (that has moved for instance) void CollisionDetection::updateProxyCollisionShape(ProxyShape* shape, const AABB& aabb, const vec3& displacement, bool forceReinsert) { m_broadPhaseAlgorithm.updateProxyCollisionShape(shape, aabb, displacement); } -// Ray casting method void CollisionDetection::raycast(RaycastCallback* raycastCallback, const Ray& ray, unsigned short raycastWithCategoryMaskBits) const { - PROFILE("CollisionDetection::raycast()"); - RaycastTest rayCastTest(raycastCallback); - // Ask the broad-phase algorithm to call the testRaycastAgainstShape() // callback method for each proxy shape hit by the ray in the broad-phase m_broadPhaseAlgorithm.raycast(ray, rayCastTest, raycastWithCategoryMaskBits); } -// Test if the AABBs of two proxy shapes overlap bool CollisionDetection::testAABBOverlap(const ProxyShape* shape1, const ProxyShape* shape2) const { - // If one of the shape's body is not active, we return no overlap if (!shape1->getBody()->isActive() || !shape2->getBody()->isActive()) { return false; } - return m_broadPhaseAlgorithm.testOverlappingShapes(shape1, shape2); } -// Return a pointer to the world CollisionWorld* CollisionDetection::getWorld() { return m_world; } diff --git a/ephysics/collision/CollisionDetection.hpp b/ephysics/collision/CollisionDetection.hpp index 8922a68..852e6f9 100644 --- a/ephysics/collision/CollisionDetection.hpp +++ b/ephysics/collision/CollisionDetection.hpp @@ -14,8 +14,7 @@ #include #include #include -#include -#include +#include namespace ephysics { @@ -56,7 +55,7 @@ namespace ephysics { BroadPhaseAlgorithm m_broadPhaseAlgorithm; //!< Broad-phase algorithm // TODO : Delete this GJKAlgorithm m_narrowPhaseGJKAlgorithm; //!< Narrow-phase GJK algorithm - std::set m_noCollisionPairs; //!< Set of pair of bodies that cannot collide between each other + etk::Set 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 /// Private copy-constructor CollisionDetection(const CollisionDetection& _collisionDetection); @@ -98,18 +97,20 @@ namespace ephysics { void addNoCollisionPair(CollisionBody* _body1, CollisionBody* _body2); /// Remove a pair of bodies that cannot collide with each other 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); /// Compute the collision detection void computeCollisionDetection(); /// Compute the collision detection void testCollisionBetweenShapes(CollisionCallback* _callback, - const std::set& _shapes1, - const std::set& _shapes2); + const etk::Set& _shapes1, + const etk::Set& _shapes2); /// Report collision between two sets of shapes void reportCollisionBetweenShapes(CollisionCallback* _callback, - const std::set& _shapes1, - const std::set& _shapes2) ; + const etk::Set& _shapes1, + const etk::Set& _shapes2) ; /// Ray casting method void raycast(RaycastCallback* _raycastCallback, const Ray& _ray, @@ -121,11 +122,12 @@ namespace ephysics { bool testAABBOverlap(const ProxyShape* _shape1, const ProxyShape* _shape2) const; /// 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); /// Compute the narrow-phase collision detection void computeNarrowPhaseBetweenShapes(CollisionCallback* _callback, - const std::set& _shapes1, - const std::set& _shapes2); + const etk::Set& _shapes1, + const etk::Set& _shapes2); /// Return a pointer to the world CollisionWorld* getWorld(); /// Return the world event listener diff --git a/ephysics/collision/ContactManifold.cpp b/ephysics/collision/ContactManifold.cpp index d609f9b..30c7e0b 100644 --- a/ephysics/collision/ContactManifold.cpp +++ b/ephysics/collision/ContactManifold.cpp @@ -6,7 +6,6 @@ // Libraries -#include #include using namespace ephysics; diff --git a/ephysics/collision/ContactManifoldSet.cpp b/ephysics/collision/ContactManifoldSet.cpp index a435785..4fc8431 100644 --- a/ephysics/collision/ContactManifoldSet.cpp +++ b/ephysics/collision/ContactManifoldSet.cpp @@ -4,12 +4,10 @@ * @license BSD 3 clauses (see license file) */ -// Libraries #include using namespace ephysics; -// Constructor ContactManifoldSet::ContactManifoldSet(ProxyShape* shape1, ProxyShape* shape2, MemoryAllocator& memoryAllocator, int32_t nbMaxManifolds) : m_nbMaxManifolds(nbMaxManifolds), m_nbManifolds(0), m_shape1(shape1), @@ -17,61 +15,45 @@ ContactManifoldSet::ContactManifoldSet(ProxyShape* shape1, ProxyShape* shape2, assert(nbMaxManifolds >= 1); } -// Destructor ContactManifoldSet::~ContactManifoldSet() { - - // Clear all the contact manifolds clear(); } -// Add a contact point to the manifold set void ContactManifoldSet::addContactPoint(ContactPoint* contact) { - // Compute an Id corresponding to the normal direction (using a cubemap) int16_t normalDirectionId = computeCubemapNormalId(contact->getNormal()); - // If there is no contact manifold yet if (m_nbManifolds == 0) { - createManifold(normalDirectionId); m_manifolds[0]->addContactPoint(contact); assert(m_manifolds[m_nbManifolds-1]->getNbContactPoints() > 0); for (int32_t i=0; igetNbContactPoints() > 0); } - return; } - // Select the manifold with the most similar normal (if exists) int32_t similarManifoldIndex = 0; if (m_nbMaxManifolds > 1) { similarManifoldIndex = selectManifoldWithSimilarNormal(normalDirectionId); } - // If a similar manifold has been found if (similarManifoldIndex != -1) { - // Add the contact point to that similar manifold m_manifolds[similarManifoldIndex]->addContactPoint(contact); assert(m_manifolds[similarManifoldIndex]->getNbContactPoints() > 0); - return; } - // If the maximum number of manifold has not been reached yet if (m_nbManifolds < m_nbMaxManifolds) { - // Create a new manifold for the contact point createManifold(normalDirectionId); m_manifolds[m_nbManifolds-1]->addContactPoint(contact); for (int32_t i=0; igetNbContactPoints() > 0); } - return; } - // 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 // with the largest contact depth among their points @@ -85,20 +67,15 @@ void ContactManifoldSet::addContactPoint(ContactPoint* contact) { smallestDepthIndex = i; } } - // If we do not want to keep to new manifold (not created yet) with the // new contact point if (smallestDepthIndex == -1) { - // Delete the new contact contact->~ContactPoint(); m_memoryAllocator.release(contact, sizeof(ContactPoint)); - return; } - assert(smallestDepthIndex >= 0 && smallestDepthIndex < m_nbManifolds); - // Here we need to replace an existing manifold with a new one (that contains // the new contact point) removeManifold(smallestDepthIndex); @@ -108,70 +85,55 @@ void ContactManifoldSet::addContactPoint(ContactPoint* contact) { for (int32_t i=0; igetNbContactPoints() > 0); } - 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 { - // Return the Id of the manifold with the same normal direction id (if exists) for (int32_t i=0; igetNormalDirectionId()) { return i; } } - 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 { - - assert(normal.length2() > MACHINE_EPSILON); - + assert(normal.length2() > FLT_EPSILON); int32_t faceNo; float u, v; float max = max3(fabs(normal.x()), fabs(normal.y()), fabs(normal.z())); vec3 normalScaled = normal / max; - if (normalScaled.x() >= normalScaled.y() && normalScaled.x() >= normalScaled.z()) { faceNo = normalScaled.x() > 0 ? 0 : 1; u = normalScaled.y(); 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; u = normalScaled.x(); v = normalScaled.z(); - } - else { + } else { faceNo = normalScaled.z() > 0 ? 4 : 5; u = normalScaled.x(); v = normalScaled.y(); } - int32_t indexU = floor(((u + 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 (indexV == CONTACT_CUBEMAP_FACE_NB_SUBDIVISIONS) indexV--; - + if (indexU == CONTACT_CUBEMAP_FACE_NB_SUBDIVISIONS) { + indexU--; + } + if (indexV == CONTACT_CUBEMAP_FACE_NB_SUBDIVISIONS) { + indexV--; + } const int32_t nbSubDivInFace = CONTACT_CUBEMAP_FACE_NB_SUBDIVISIONS * CONTACT_CUBEMAP_FACE_NB_SUBDIVISIONS; return faceNo * 200 + indexU * nbSubDivInFace + indexV; } -// Update the contact manifolds void ContactManifoldSet::update() { - for (int32_t i=m_nbManifolds-1; i>=0; i--) { - // Update the contact manifold m_manifolds[i]->update(m_shape1->getBody()->getTransform() * m_shape1->getLocalToBodyTransform(), m_shape2->getBody()->getTransform() * m_shape2->getLocalToBodyTransform()); - // Remove the contact manifold if has no contact points anymore if (m_manifolds[i]->getNbContactPoints() == 0) { removeManifold(i); @@ -179,65 +141,48 @@ void ContactManifoldSet::update() { } } -// Clear the contact manifold set void ContactManifoldSet::clear() { - - // Destroy all the contact manifolds for (int32_t i=m_nbManifolds-1; i>=0; i--) { removeManifold(i); } - assert(m_nbManifolds == 0); } -// Create a new contact manifold and add it to the set void ContactManifoldSet::createManifold(int16_t normalDirectionId) { assert(m_nbManifolds < m_nbMaxManifolds); - - m_manifolds[m_nbManifolds] = new (m_memoryAllocator.allocate(sizeof(ContactManifold))) - ContactManifold(m_shape1, m_shape2, m_memoryAllocator, normalDirectionId); + m_manifolds[m_nbManifolds] = new ContactManifold(m_shape1, m_shape2, m_memoryAllocator, normalDirectionId); m_nbManifolds++; } -// Remove a contact manifold from the set void ContactManifoldSet::removeManifold(int32_t index) { - assert(m_nbManifolds > 0); assert(index >= 0 && index < m_nbManifolds); - // Delete the new contact - m_manifolds[index]->~ContactManifold(); - m_memoryAllocator.release(m_manifolds[index], sizeof(ContactManifold)); - + delete m_manifolds[index]; + m_manifolds[index] = nullptr; for (int32_t i=index; (i+1) < m_nbManifolds; i++) { m_manifolds[i] = m_manifolds[i+1]; } - m_nbManifolds--; } -// Return the first proxy shape ProxyShape* ContactManifoldSet::getShape1() const { return m_shape1; } -// Return the second proxy shape ProxyShape* ContactManifoldSet::getShape2() const { return m_shape2; } -// Return the number of manifolds in the set int32_t ContactManifoldSet::getNbContactManifolds() const { return m_nbManifolds; } -// Return a given contact manifold ContactManifold* ContactManifoldSet::getContactManifold(int32_t index) const { assert(index >= 0 && index < m_nbManifolds); return m_manifolds[index]; } -// Return the total number of contact points in the set of manifolds int32_t ContactManifoldSet::getTotalNbContactPoints() const { int32_t nbPoints = 0; for (int32_t i=0; i -#include #include namespace ephysics { diff --git a/ephysics/collision/narrowphase/ConcaveVsConvexAlgorithm.cpp b/ephysics/collision/narrowphase/ConcaveVsConvexAlgorithm.cpp index 886a0e3..74e4207 100644 --- a/ephysics/collision/narrowphase/ConcaveVsConvexAlgorithm.cpp +++ b/ephysics/collision/narrowphase/ConcaveVsConvexAlgorithm.cpp @@ -4,50 +4,42 @@ * @license BSD 3 clauses (see license file) */ -// Libraries #include #include #include #include #include -#include using namespace ephysics; -// Constructor ConcaveVsConvexAlgorithm::ConcaveVsConvexAlgorithm() { - + } -// Destructor ConcaveVsConvexAlgorithm::~ConcaveVsConvexAlgorithm() { - + } -// Return true and compute a contact info if the two bounding volumes collide void ConcaveVsConvexAlgorithm::testCollision(const CollisionShapeInfo& shape1Info, const CollisionShapeInfo& shape2Info, NarrowPhaseCallback* narrowPhaseCallback) { - ProxyShape* convexProxyShape; ProxyShape* concaveProxyShape; const ConvexShape* convexShape; const ConcaveShape* concaveShape; - // Collision shape 1 is convex, collision shape 2 is concave if (shape1Info.collisionShape->isConvex()) { convexProxyShape = shape1Info.proxyShape; convexShape = static_cast(shape1Info.collisionShape); concaveProxyShape = shape2Info.proxyShape; concaveShape = static_cast(shape2Info.collisionShape); - } - else { // Collision shape 2 is convex, collision shape 1 is concave + } else { + // Collision shape 2 is convex, collision shape 1 is concave convexProxyShape = shape2Info.proxyShape; convexShape = static_cast(shape2Info.collisionShape); concaveProxyShape = shape1Info.proxyShape; concaveShape = static_cast(shape1Info.collisionShape); } - // Set the parameters of the callback object ConvexVsTriangleCallback convexVsTriangleCallback; convexVsTriangleCallback.setCollisionDetection(m_collisionDetection); @@ -55,83 +47,64 @@ void ConcaveVsConvexAlgorithm::testCollision(const CollisionShapeInfo& shape1Inf convexVsTriangleCallback.setConcaveShape(concaveShape); convexVsTriangleCallback.setProxyShapes(convexProxyShape, concaveProxyShape); convexVsTriangleCallback.setOverlappingPair(shape1Info.overlappingPair); - // Compute the convex shape AABB in the local-space of the convex shape AABB aabb; convexShape->computeAABB(aabb, convexProxyShape->getLocalToWorldTransform()); - // If smooth mesh collision is enabled for the concave mesh if (concaveShape->getIsSmoothMeshCollisionEnabled()) { - etk::Vector contactPoints; - SmoothCollisionNarrowPhaseCallback smoothNarrowPhaseCallback(contactPoints); - convexVsTriangleCallback.setNarrowPhaseCallback(&smoothNarrowPhaseCallback); - // Call the convex vs triangle callback for each triangle of the concave shape concaveShape->testAllTriangles(convexVsTriangleCallback, aabb); - // Run the smooth mesh collision algorithm processSmoothMeshCollision(shape1Info.overlappingPair, contactPoints, narrowPhaseCallback); - } - else { - + } else { convexVsTriangleCallback.setNarrowPhaseCallback(narrowPhaseCallback); - // Call the convex vs triangle callback for each triangle of the concave shape concaveShape->testAllTriangles(convexVsTriangleCallback, aabb); } } -// Test collision between a triangle and the convex mesh shape void ConvexVsTriangleCallback::testTriangle(const vec3* trianglePoints) { - // Create a triangle collision shape float margin = m_concaveShape->getTriangleMargin(); TriangleShape triangleShape(trianglePoints[0], trianglePoints[1], trianglePoints[2], margin); - // Select the collision algorithm to use between the triangle and the convex shape NarrowPhaseAlgorithm* algo = m_collisionDetection->getCollisionAlgorithm(triangleShape.getType(), m_convexShape->getType()); - // 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 algo->setCurrentOverlappingPair(m_overlappingPair); - // Create the CollisionShapeInfo objects CollisionShapeInfo shapeConvexInfo(m_convexProxyShape, m_convexShape, m_convexProxyShape->getLocalToWorldTransform(), m_overlappingPair, m_convexProxyShape->getCachedCollisionData()); CollisionShapeInfo shapeConcaveInfo(m_concaveProxyShape, &triangleShape, m_concaveProxyShape->getLocalToWorldTransform(), m_overlappingPair, m_concaveProxyShape->getCachedCollisionData()); - // Use the collision algorithm to test collision between the triangle and the other convex shape 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, etk::Vector contactPoints, NarrowPhaseCallback* narrowPhaseCallback) { - // Set with the triangle vertices already processed to void further contacts with same triangle - std::unordered_multimap processTriangleVertices; - + etk::Vector> processTriangleVertices; // 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) - etk::Vector::const_iterator it; + etk::Vector::Iterator it; for (it = contactPoints.begin(); it != contactPoints.end(); ++it) { - const SmoothMeshContactInfo info = *it; const vec3& contactPoint = info.isFirstShapeTriangle ? info.contactInfo.localPoint1 : info.contactInfo.localPoint2; - // Compute the barycentric coordinates of the point in the triangle float u, v, w; computeBarycentricCoordinatesInTriangle(info.triangleVertices[0], @@ -142,51 +115,45 @@ void ConcaveVsConvexAlgorithm::processSmoothMeshCollision(OverlappingPair* overl bool isUZero = approxEqual(u, 0, 0.0001); bool isVZero = approxEqual(v, 0, 0.0001); bool isWZero = approxEqual(w, 0, 0.0001); - if (isUZero) nbZeros++; - if (isVZero) nbZeros++; - if (isWZero) nbZeros++; - + if (isUZero) { + nbZeros++; + } + if (isVZero) { + nbZeros++; + } + if (isWZero) { + nbZeros++; + } // If it is a vertex contact if (nbZeros == 2) { - vec3 contactVertex = !isUZero ? info.triangleVertices[0] : (!isVZero ? info.triangleVertices[1] : info.triangleVertices[2]); - // Check that this triangle vertex has not been processed yet if (!hasVertexBeenProcessed(processTriangleVertices, contactVertex)) { - // Keep the contact as it is and report it narrowPhaseCallback->notifyContact(overlappingPair, info.contactInfo); } - } - else if (nbZeros == 1) { // If it is an edge contact - + } else if (nbZeros == 1) { + // If it is an edge contact 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]); - // Check that this triangle edge has not been processed yet if (!hasVertexBeenProcessed(processTriangleVertices, contactVertex1) && !hasVertexBeenProcessed(processTriangleVertices, contactVertex2)) { - // Keep the contact as it is and report it narrowPhaseCallback->notifyContact(overlappingPair, info.contactInfo); } - - } - else { // If it is a face contact - + } else { + // If it is a face contact ContactPointInfo newContactInfo(info.contactInfo); - ProxyShape* firstShape; ProxyShape* secondShape; if (info.isFirstShapeTriangle) { firstShape = overlappingPair->getShape1(); secondShape = overlappingPair->getShape2(); - } - else { + } else { firstShape = overlappingPair->getShape2(); secondShape = overlappingPair->getShape1(); } - // We use the triangle normal as the contact normal vec3 a = info.triangleVertices[1] - 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) { newContactInfo.normal = -newContactInfo.normal; } - // 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 // Dirk Gregorius) to avoid adding torque @@ -206,16 +172,13 @@ void ConcaveVsConvexAlgorithm::processSmoothMeshCollision(OverlappingPair* overl if (info.isFirstShapeTriangle) { vec3 newSecondWorldPoint = firstWorldPoint + newContactInfo.normal; newContactInfo.localPoint2 = worldToLocalSecondPoint * newSecondWorldPoint; - } - else { + } else { vec3 newSecondWorldPoint = firstWorldPoint - newContactInfo.normal; newContactInfo.localPoint1 = worldToLocalSecondPoint * newSecondWorldPoint; } - // Report the contact narrowPhaseCallback->notifyContact(overlappingPair, newContactInfo); } - // Add the three vertices of the triangle to the set of processed // triangle vertices 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 std::unordered_multimap& processTriangleVertices, const vec3& vertex) const { - - int32_t key = int32_t(vertex.x() * vertex.y() * vertex.z()); - - auto range = processTriangleVertices.equal_range(key); +bool ConcaveVsConvexAlgorithm::hasVertexBeenProcessed(const etk::Vector>& _processTriangleVertices, const vec3& _vertex) const { + /* TODO : etk::Vector> 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()); + auto range = _processTriangleVertices.equal_range(key); 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; } -// Called by a narrow-phase collision algorithm when a new contact has been found -void SmoothCollisionNarrowPhaseCallback::notifyContact(OverlappingPair* overlappingPair, - const ContactPointInfo& contactInfo) { +void SmoothCollisionNarrowPhaseCallback::notifyContact(OverlappingPair* _overlappingPair, + const ContactPointInfo& _contactInfo) { vec3 triangleVertices[3]; bool isFirstShapeTriangle; - // If the collision shape 1 is the triangle - if (contactInfo.collisionShape1->getType() == TRIANGLE) { - assert(contactInfo.collisionShape2->getType() != TRIANGLE); - - const TriangleShape* triangleShape = static_cast(contactInfo.collisionShape1); + if (_contactInfo.collisionShape1->getType() == TRIANGLE) { + assert(_contactInfo.collisionShape2->getType() != TRIANGLE); + const TriangleShape* triangleShape = static_cast(_contactInfo.collisionShape1); triangleVertices[0] = triangleShape->getVertex(0); triangleVertices[1] = triangleShape->getVertex(1); triangleVertices[2] = triangleShape->getVertex(2); - isFirstShapeTriangle = true; - } - else { // If the collision shape 2 is the triangle - assert(contactInfo.collisionShape2->getType() == TRIANGLE); - - const TriangleShape* triangleShape = static_cast(contactInfo.collisionShape2); + } else { // If the collision shape 2 is the triangle + assert(_contactInfo.collisionShape2->getType() == TRIANGLE); + const TriangleShape* triangleShape = static_cast(_contactInfo.collisionShape2); triangleVertices[0] = triangleShape->getVertex(0); triangleVertices[1] = triangleShape->getVertex(1); triangleVertices[2] = triangleShape->getVertex(2); - 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 // smooth mesh collision m_contactPoints.pushBack(smoothContactInfo); diff --git a/ephysics/collision/narrowphase/ConcaveVsConvexAlgorithm.hpp b/ephysics/collision/narrowphase/ConcaveVsConvexAlgorithm.hpp index 338b87e..4cc7a67 100644 --- a/ephysics/collision/narrowphase/ConcaveVsConvexAlgorithm.hpp +++ b/ephysics/collision/narrowphase/ConcaveVsConvexAlgorithm.hpp @@ -8,7 +8,6 @@ #include #include #include -#include namespace ephysics { @@ -79,13 +78,18 @@ namespace ephysics { triangleVertices[1] = _trianglePoint2; triangleVertices[2] = _trianglePoint3; } + SmoothMeshContactInfo() { + // TODO: add it for etk::Vector + } }; + /* struct ContactsDepthCompare { bool operator()(const SmoothMeshContactInfo& _contact1, const SmoothMeshContactInfo& _contact2) { return _contact1.contactInfo.penetrationDepth < _contact2.contactInfo.penetrationDepth; } }; + */ /** * @brief This class is used as a narrow-phase callback to get narrow-phase contacts @@ -122,11 +126,11 @@ namespace ephysics { etk::Vector _contactPoints, NarrowPhaseCallback* _narrowPhaseCallback); /// Add a triangle vertex int32_to the set of processed triangles - void addProcessedVertex(std::unordered_multimap& _processTriangleVertices, const vec3& _vertex) { - _processTriangleVertices.insert(etk::makePair(int32_t(_vertex.x() * _vertex.y() * _vertex.z()), _vertex)); + void addProcessedVertex(etk::Vector>& _processTriangleVertices, const vec3& _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 - bool hasVertexBeenProcessed(const std::unordered_multimap& _processTriangleVertices, + bool hasVertexBeenProcessed(const etk::Vector>& _processTriangleVertices, const vec3& _vertex) const; public : /// Constructor diff --git a/ephysics/collision/narrowphase/EPA/EPAAlgorithm.cpp b/ephysics/collision/narrowphase/EPA/EPAAlgorithm.cpp index 754cc65..7992c51 100644 --- a/ephysics/collision/narrowphase/EPA/EPAAlgorithm.cpp +++ b/ephysics/collision/narrowphase/EPA/EPAAlgorithm.cpp @@ -3,66 +3,46 @@ * @copyright 2010-2016 Daniel Chappuis * @license BSD 3 clauses (see license file) */ - -// Libraries #include #include #include #include -// We want to use the ReactPhysics3D namespace using namespace ephysics; -// Constructor EPAAlgorithm::EPAAlgorithm() { - + } -// Destructor EPAAlgorithm::~EPAAlgorithm() { } -// Decide if the origin is in the tetrahedron. -/// 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 { - +int32_t EPAAlgorithm::isOriginInTetrahedron(const vec3& p1, const vec3& p2, const vec3& p3, const vec3& p4) const { // Check vertex 1 vec3 normal1 = (p2-p1).cross(p3-p1); if ((normal1.dot(p1) > 0.0) == (normal1.dot(p4) > 0.0)) { return 4; } - // Check vertex 2 vec3 normal2 = (p4-p2).cross(p3-p2); if ((normal2.dot(p2) > 0.0) == (normal2.dot(p1) > 0.0)) { return 1; } - // Check vertex 3 vec3 normal3 = (p4-p3).cross(p1-p3); if ((normal3.dot(p3) > 0.0) == (normal3.dot(p2) > 0.0)) { return 2; } - // Check vertex 4 vec3 normal4 = (p2-p4).cross(p1-p4); if ((normal4.dot(p4) > 0.0) == (normal4.dot(p3) > 0.0)) { return 3; } - // The origin is in the tetrahedron, we 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, CollisionShapeInfo shape1Info, const etk::Transform3D& transform1, @@ -70,46 +50,34 @@ void EPAAlgorithm::computePenetrationDepthAndContactPoints(const Simplex& simple const etk::Transform3D& transform2, vec3& v, NarrowPhaseCallback* narrowPhaseCallback) { - PROFILE("EPAAlgorithm::computePenetrationDepthAndContactPoints()"); - assert(shape1Info.collisionShape->isConvex()); assert(shape2Info.collisionShape->isConvex()); - const ConvexShape* shape1 = static_cast(shape1Info.collisionShape); const ConvexShape* shape2 = static_cast(shape2Info.collisionShape); - void** shape1CachedCollisionData = shape1Info.cachedCollisionData; void** shape2CachedCollisionData = shape2Info.cachedCollisionData; - 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 points[MAX_SUPPORT_POINTS]; // Current points TrianglesStore triangleStore; // Store the triangles TriangleEPA* triangleHeap[MAX_FACETS]; // Heap that contains the face // candidate of the EPA algorithm - // 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) etk::Transform3D body2Tobody1 = transform1.getInverse() * transform2; - // Matrix that transform a direction from local // space of body 1 int32_to local space of body 2 etk::Quaternion rotateToBody2 = transform2.getOrientation().getInverse() * transform1.getOrientation(); - // Get the simplex computed previously by the GJK algorithm uint32_t nbVertices = simplex.getSimplex(suppPointsA, suppPointsB, points); - // Compute the tolerance - float tolerance = MACHINE_EPSILON * simplex.getMaxLengthSquareOfAPoint(); - + float tolerance = FLT_EPSILON * simplex.getMaxLengthSquareOfAPoint(); // Number of triangles in the polytope uint32_t nbTriangles = 0; - // Clear the storing of triangles triangleStore.clear(); - // Select an action according to the number of points in the simplex // computed with GJK algorithm in order to obtain an initial polytope for // The EPA algorithm. @@ -119,7 +87,6 @@ void EPAAlgorithm::computePenetrationDepthAndContactPoints(const Simplex& simple // We have a touching contact with zero penetration depth. // We drop that kind of contact. Therefore, we return false return; - case 2: { // 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 @@ -128,47 +95,37 @@ void EPAAlgorithm::computePenetrationDepthAndContactPoints(const Simplex& simple // 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 // v1, v2 and v3. - // Direction of the segment vec3 d = (points[1] - points[0]).safeNormalized(); - // Choose the coordinate axis from the minimal absolute component of the vector d int32_t minAxis = d.absolute().getMinAxis(); - // Compute sin(60) const float sin60 = float(sqrt(3.0)) * 0.5f; - // Create a rotation quaternion to rotate the vector v1 to get the vectors // v2 and v3 etk::Quaternion rotationQuat(d.x() * sin60, d.y() * sin60, d.z() * sin60, 0.5); - // Compute the vector v1, v2, v3 vec3 v1 = d.cross(vec3(minAxis == 0, minAxis == 1, minAxis == 2)); vec3 v2 = rotationQuat * v1; vec3 v3 = rotationQuat * v2; - // Compute the support point in the direction of v1 suppPointsA[2] = shape1->getLocalSupportPointWithMargin(v1, shape1CachedCollisionData); suppPointsB[2] = body2Tobody1 * shape2->getLocalSupportPointWithMargin(rotateToBody2 * (-v1), shape2CachedCollisionData); points[2] = suppPointsA[2] - suppPointsB[2]; - // Compute the support point in the direction of v2 suppPointsA[3] = shape1->getLocalSupportPointWithMargin(v2, shape1CachedCollisionData); suppPointsB[3] = body2Tobody1 * shape2->getLocalSupportPointWithMargin(rotateToBody2 * (-v2), shape2CachedCollisionData); points[3] = suppPointsA[3] - suppPointsB[3]; - // Compute the support point in the direction of v3 suppPointsA[4] = shape1->getLocalSupportPointWithMargin(v3, shape1CachedCollisionData); suppPointsB[4] = body2Tobody1 * shape2->getLocalSupportPointWithMargin(rotateToBody2 * (-v3), shape2CachedCollisionData); points[4] = suppPointsA[4] - suppPointsB[4]; - // 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 // 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 (isOriginInTetrahedron(points[0], points[2], points[3], points[4]) == 0) { // 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 return; } - // The polytope contains now 4 vertices 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 // otherwise we remove the wrong vertex of the tetrahedron and go in the case // where the GJK algorithm compute a simplex of three vertices. - // 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]); - // If the origin is in the tetrahedron if (badVertex == 0) { // The tetrahedron is a correct initial polytope for the EPA algorithm. // Therefore, we construct the tetrahedron. - // Comstruct the 4 triangle faces of the tetrahedron TriangleEPA* face0 = triangleStore.newTriangle(points, 0, 1, 2); TriangleEPA* face1 = triangleStore.newTriangle(points, 0, 3, 1); TriangleEPA* face2 = triangleStore.newTriangle(points, 0, 2, 3); TriangleEPA* face3 = triangleStore.newTriangle(points, 1, 3, 2); - // If the constructed tetrahedron is not correct if (!((face0 != NULL) && (face1 != NULL) && (face2 != NULL) && (face3 != NULL) && face0->getDistSquare() > 0.0 && face1->getDistSquare() > 0.0 && face2->getDistSquare() > 0.0 && face3->getDistSquare() > 0.0)) { return; } - // Associate the edges of neighbouring triangle faces link(EdgeEPA(face0, 0), EdgeEPA(face1, 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, 1), EdgeEPA(face3, 0)); link(EdgeEPA(face2, 1), EdgeEPA(face3, 1)); - // Add the triangle faces in the candidate heap - addFaceCandidate(face0, triangleHeap, nbTriangles, DECIMAL_LARGEST); - addFaceCandidate(face1, triangleHeap, nbTriangles, DECIMAL_LARGEST); - addFaceCandidate(face2, triangleHeap, nbTriangles, DECIMAL_LARGEST); - addFaceCandidate(face3, triangleHeap, nbTriangles, DECIMAL_LARGEST); - + addFaceCandidate(face0, triangleHeap, nbTriangles, FLT_MAX); + addFaceCandidate(face1, triangleHeap, nbTriangles, FLT_MAX); + addFaceCandidate(face2, triangleHeap, nbTriangles, FLT_MAX); + addFaceCandidate(face3, triangleHeap, nbTriangles, FLT_MAX); break; } - // 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 // three remaining vertices if (badVertex < 4) { - suppPointsA[badVertex-1] = suppPointsA[3]; suppPointsB[badVertex-1] = suppPointsB[3]; points[badVertex-1] = points[3]; } - // We have removed the wrong vertex nbVertices = 3; } @@ -254,12 +200,10 @@ void EPAAlgorithm::computePenetrationDepthAndContactPoints(const Simplex& simple // vertices are the support points in the "n" and "-n" direction // where "n" is the normal of the triangle. Then, we use only the // tetrahedron that contains the origin. - // Compute the normal of the triangle vec3 v1 = points[1] - points[0]; vec3 v2 = points[2] - points[0]; vec3 n = v1.cross(v2); - // Compute the two new vertices to obtain a hexahedron suppPointsA[3] = shape1->getLocalSupportPointWithMargin(n, shape1CachedCollisionData); suppPointsB[3] = body2Tobody1 * @@ -269,18 +213,15 @@ void EPAAlgorithm::computePenetrationDepthAndContactPoints(const Simplex& simple suppPointsB[4] = body2Tobody1 * shape2->getLocalSupportPointWithMargin(rotateToBody2 * n, shape2CachedCollisionData); points[4] = suppPointsA[4] - suppPointsB[4]; - TriangleEPA* face0 = NULL; TriangleEPA* face1 = NULL; TriangleEPA* face2 = NULL; TriangleEPA* face3 = NULL; - // If the origin is in the first tetrahedron if (isOriginInTetrahedron(points[0], points[1], points[2], points[3]) == 0) { // The tetrahedron is a correct initial polytope for the EPA algorithm. // Therefore, we construct the tetrahedron. - // Comstruct the 4 triangle faces of the tetrahedron face0 = triangleStore.newTriangle(points, 0, 1, 2); face1 = triangleStore.newTriangle(points, 0, 3, 1); @@ -289,10 +230,8 @@ void EPAAlgorithm::computePenetrationDepthAndContactPoints(const Simplex& simple } else if (isOriginInTetrahedron(points[0], points[1], points[2], points[4]) == 0) { - // The tetrahedron is a correct initial polytope for the EPA algorithm. // Therefore, we construct the tetrahedron. - // Comstruct the 4 triangle faces of the tetrahedron face0 = triangleStore.newTriangle(points, 0, 1, 2); face1 = triangleStore.newTriangle(points, 0, 4, 1); @@ -302,14 +241,12 @@ void EPAAlgorithm::computePenetrationDepthAndContactPoints(const Simplex& simple else { return; } - // If the constructed tetrahedron is not correct if (!((face0 != NULL) && (face1 != NULL) && (face2 != NULL) && (face3 != NULL) && face0->getDistSquare() > 0.0 && face1->getDistSquare() > 0.0 && face2->getDistSquare() > 0.0 && face3->getDistSquare() > 0.0)) { return; } - // Associate the edges of neighbouring triangle faces link(EdgeEPA(face0, 0), EdgeEPA(face1, 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, 1), EdgeEPA(face3, 0)); link(EdgeEPA(face2, 1), EdgeEPA(face3, 1)); - // Add the triangle faces in the candidate heap - addFaceCandidate(face0, triangleHeap, nbTriangles, DECIMAL_LARGEST); - addFaceCandidate(face1, triangleHeap, nbTriangles, DECIMAL_LARGEST); - addFaceCandidate(face2, triangleHeap, nbTriangles, DECIMAL_LARGEST); - addFaceCandidate(face3, triangleHeap, nbTriangles, DECIMAL_LARGEST); - + addFaceCandidate(face0, triangleHeap, nbTriangles, FLT_MAX); + addFaceCandidate(face1, triangleHeap, nbTriangles, FLT_MAX); + addFaceCandidate(face2, triangleHeap, nbTriangles, FLT_MAX); + addFaceCandidate(face3, triangleHeap, nbTriangles, FLT_MAX); nbVertices = 4; - } break; } - // At this point, we have a polytope that contains the origin. Therefore, we // can run the EPA algorithm. - if (nbTriangles == 0) { return; } - TriangleEPA* triangle = 0; - float upperBoundSquarePenDepth = DECIMAL_LARGEST; - + float upperBoundSquarePenDepth = FLT_MAX; do { triangle = triangleHeap[0]; - // 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--; - // If the candidate face in the heap is not obsolete if (!triangle->getIsObsolete()) { // If we have reached the maximum number of support points @@ -354,7 +282,6 @@ void EPAAlgorithm::computePenetrationDepthAndContactPoints(const Simplex& simple assert(false); break; } - // Compute the support point of the Minkowski // difference (A-B) in the closest point direction suppPointsA[nbVertices] = shape1->getLocalSupportPointWithMargin( @@ -363,10 +290,8 @@ void EPAAlgorithm::computePenetrationDepthAndContactPoints(const Simplex& simple shape2->getLocalSupportPointWithMargin(rotateToBody2 * (-triangle->getClosestPoint()), shape2CachedCollisionData); points[nbVertices] = suppPointsA[nbVertices] - suppPointsB[nbVertices]; - int32_t indexNewVertex = nbVertices; nbVertices++; - // Update the upper bound of the penetration depth float wDotv = points[indexNewVertex].dot(triangle->getClosestPoint()); assert(wDotv > 0.0); @@ -374,7 +299,6 @@ void EPAAlgorithm::computePenetrationDepthAndContactPoints(const Simplex& simple if (wDotVSquare < upperBoundSquarePenDepth) { upperBoundSquarePenDepth = wDotVSquare; } - // Compute the error float error = wDotv - triangle->getDistSquare(); if (error <= etk::max(tolerance, REL_ERROR_SQUARE * wDotv) || @@ -383,7 +307,6 @@ void EPAAlgorithm::computePenetrationDepthAndContactPoints(const Simplex& simple points[indexNewVertex] == points[(*triangle)[2]]) { break; } - // 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 // algorithm from the current triangle face. @@ -391,7 +314,6 @@ void EPAAlgorithm::computePenetrationDepthAndContactPoints(const Simplex& simple if (!triangle->computeSilhouette(points, indexNewVertex, triangleStore)) { break; } - // Add all the new triangle faces computed with the silhouette algorithm // to the candidates list of faces of the current polytope while(i != triangleStore.getNbTriangles()) { @@ -401,7 +323,6 @@ void EPAAlgorithm::computePenetrationDepthAndContactPoints(const Simplex& simple } } } while(nbTriangles > 0 && triangleHeap[0]->getDistSquare() <= upperBoundSquarePenDepth); - // Compute the contact info v = transform1.getOrientation() * triangle->getClosestPoint(); vec3 pALocal = triangle->computeClosestPointOfObject(suppPointsA); @@ -409,12 +330,10 @@ void EPAAlgorithm::computePenetrationDepthAndContactPoints(const Simplex& simple vec3 normal = v.safeNormalized(); float penetrationDepth = v.length(); assert(penetrationDepth > 0.0); - - if (normal.length2() < MACHINE_EPSILON) return; - + if (normal.length2() < FLT_EPSILON) { + return; + } // Create the contact info object - ContactPointInfo contactInfo(shape1Info.proxyShape, shape2Info.proxyShape, shape1Info.collisionShape, - shape2Info.collisionShape, normal, penetrationDepth, pALocal, pBLocal); - + ContactPointInfo contactInfo(shape1Info.proxyShape, shape2Info.proxyShape, shape1Info.collisionShape, shape2Info.collisionShape, normal, penetrationDepth, pALocal, pBLocal); narrowPhaseCallback->notifyContact(shape1Info.overlappingPair, contactInfo); } diff --git a/ephysics/collision/narrowphase/EPA/EPAAlgorithm.hpp b/ephysics/collision/narrowphase/EPA/EPAAlgorithm.hpp index 96ebbff..db2cef6 100644 --- a/ephysics/collision/narrowphase/EPA/EPAAlgorithm.hpp +++ b/ephysics/collision/narrowphase/EPA/EPAAlgorithm.hpp @@ -4,8 +4,6 @@ * @license BSD 3 clauses (see license file) */ #pragma once - -// Libraries #include #include #include @@ -16,123 +14,92 @@ #include #include -/// ReactPhysics3D namespace namespace ephysics { - -// ---------- Constants ---------- // - -/// Maximum number of support points of the polytope -const uint32_t MAX_SUPPORT_POINTS = 100; - -/// Maximum number of facets of the polytope -const uint32_t MAX_FACETS = 200; - - -// Class TriangleComparison -/** - * This class allows the comparison of two triangles in the heap - * 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 { - - public: - - /// Comparison operator - bool operator()(const TriangleEPA* face1, const TriangleEPA* face2) { - return (face1->getDistSquare() > face2->getDistSquare()); - } -}; - - -// Class EPAAlgorithm -/** - * This class is the implementation of the Expanding Polytope Algorithm (EPA). - * 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 - * 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 - * has been computed wit GJK algorithm. The EPA Algorithm will extend this simplex - * polytope to find the correct penetration depth. The implementation of the EPA - * algorithm is based on the book "Collision Detection in 3D Environments". - */ -class EPAAlgorithm { - - private: - - // -------------------- Attributes -------------------- // - - /// Reference to the memory allocator - MemoryAllocator* m_memoryAllocator; - - /// Triangle comparison operator - TriangleComparison mTriangleComparison; - - // -------------------- Methods -------------------- // - - /// Private copy-constructor - EPAAlgorithm(const EPAAlgorithm& algorithm); - - /// Private assignment operator - EPAAlgorithm& operator=(const EPAAlgorithm& algorithm); - - /// Add a triangle face in the candidate triangle heap - void addFaceCandidate(TriangleEPA* triangle, TriangleEPA** heap, uint32_t& nbTriangles, - float upperBoundSquarePenDepth); - - /// Decide if the origin is in the tetrahedron. - int32_t isOriginInTetrahedron(const vec3& p1, const vec3& p2, - const vec3& p3, const vec3& p4) const; - - public: - - // -------------------- Methods -------------------- // - - /// Constructor - EPAAlgorithm(); - - /// Destructor - ~EPAAlgorithm(); - - /// Initalize the algorithm - void init(MemoryAllocator* memoryAllocator); - - /// Compute the penetration depth with EPA algorithm. - void computePenetrationDepthAndContactPoints(const Simplex& simplex, - 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; -} - + /// Maximum number of support points of the polytope + const uint32_t MAX_SUPPORT_POINTS = 100; + /// Maximum number of facets of the polytope + const uint32_t MAX_FACETS = 200; + /** + * @brief Class TriangleComparison + * This class allows the comparison of two triangles in the heap + * 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 { + public: + /** + * @brief Comparison operator + */ + bool operator()(const TriangleEPA* face1, const TriangleEPA* face2) { + return (face1->getDistSquare() > face2->getDistSquare()); + } + }; + /** + * @brief Class EPAAlgorithm + * This class is the implementation of the Expanding Polytope Algorithm (EPA). + * 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 + * 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 + * has been computed wit GJK algorithm. The EPA Algorithm will extend this simplex + * polytope to find the correct penetration depth. The implementation of the EPA + * algorithm is based on the book "Collision Detection in 3D Environments". + */ + class EPAAlgorithm { + private: + MemoryAllocator* m_memoryAllocator; //!< Reference to the memory allocator + TriangleComparison m_triangleComparison; //!< Triangle comparison operator + /// Private copy-constructor + EPAAlgorithm(const EPAAlgorithm& _algorithm); + /// Private assignment operator + EPAAlgorithm& operator=(const EPAAlgorithm& _algorithm); + /// Add a triangle face in the candidate triangle heap + void 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], m_triangleComparison); + } + } + // Decide if the origin is in the tetrahedron. + /// 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 isOriginInTetrahedron(const vec3& _p1, const vec3& _p2, const vec3& _p3, const vec3& _p4) const; + public: + /// Constructor + EPAAlgorithm(); + /// Destructor + ~EPAAlgorithm(); + /// Initalize the algorithm + void init(MemoryAllocator* _memoryAllocator) { + m_memoryAllocator = _memoryAllocator; + } + // 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 computePenetrationDepthAndContactPoints(const Simplex& _simplex, + CollisionShapeInfo _shape1Info, + const etk::Transform3D& _transform1, + CollisionShapeInfo _shape2Info, + const etk::Transform3D& _transform2, + vec3& _v, + NarrowPhaseCallback* _narrowPhaseCallback); + }; } diff --git a/ephysics/collision/narrowphase/EPA/EdgeEPA.cpp b/ephysics/collision/narrowphase/EPA/EdgeEPA.cpp index 4ad7495..70b5a12 100644 --- a/ephysics/collision/narrowphase/EPA/EdgeEPA.cpp +++ b/ephysics/collision/narrowphase/EPA/EdgeEPA.cpp @@ -3,115 +3,89 @@ * @copyright 2010-2016 Daniel Chappuis * @license BSD 3 clauses (see license file) */ - -// Libraries #include #include #include -#include -// We want to use the ReactPhysics3D namespace using namespace ephysics; -// Constructor EdgeEPA::EdgeEPA() { } -// Constructor EdgeEPA::EdgeEPA(TriangleEPA* ownerTriangle, int32_t index) - : mOwnerTriangle(ownerTriangle), mIndex(index) { + : m_ownerTriangle(ownerTriangle), m_index(index) { assert(index >= 0 && index < 3); } -// Copy-constructor EdgeEPA::EdgeEPA(const EdgeEPA& edge) { - mOwnerTriangle = edge.mOwnerTriangle; - mIndex = edge.mIndex; + m_ownerTriangle = edge.m_ownerTriangle; + m_index = edge.m_index; } -// Destructor EdgeEPA::~EdgeEPA() { - + } -// Return the index of the source vertex of the edge (vertex starting the edge) 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 { - 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, TrianglesStore& triangleStore) { // 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 (!mOwnerTriangle->isVisibleFromVertex(vertices, indexNewVertex)) { + if (!m_ownerTriangle->isVisibleFromVertex(vertices, indexNewVertex)) { TriangleEPA* triangle = triangleStore.newTriangle(vertices, indexNewVertex, getTargetVertexIndex(), getSourceVertexIndex()); - // If the triangle has been created - if (triangle != NULL) { + if (triangle != nullptr) { halfLink(EdgeEPA(triangle, 1), *this); return true; } - return false; - } - else { - + } else { // The current triangle is visible and therefore obsolete - mOwnerTriangle->setIsObsolete(true); - + m_ownerTriangle->setIsObsolete(true); int32_t backup = triangleStore.getNbTriangles(); - - if(!mOwnerTriangle->getAdjacentEdge(indexOfNextCounterClockwiseEdge( - this->mIndex)).computeSilhouette(vertices, + if(!m_ownerTriangle->getAdjacentEdge(indexOfNextCounterClockwiseEdge( + this->m_index)).computeSilhouette(vertices, indexNewVertex, triangleStore)) { - mOwnerTriangle->setIsObsolete(false); - + m_ownerTriangle->setIsObsolete(false); TriangleEPA* triangle = triangleStore.newTriangle(vertices, indexNewVertex, getTargetVertexIndex(), getSourceVertexIndex()); - // If the triangle has been created - if (triangle != NULL) { + if (triangle != nullptr) { halfLink(EdgeEPA(triangle, 1), *this); return true; } - return false; - } - else if (!mOwnerTriangle->getAdjacentEdge(indexOfPreviousCounterClockwiseEdge( - this->mIndex)).computeSilhouette(vertices, + } else if (!m_ownerTriangle->getAdjacentEdge(indexOfPreviousCounterClockwiseEdge( + this->m_index)).computeSilhouette(vertices, indexNewVertex, triangleStore)) { - mOwnerTriangle->setIsObsolete(false); - + m_ownerTriangle->setIsObsolete(false); triangleStore.setNbTriangles(backup); - TriangleEPA* triangle = triangleStore.newTriangle(vertices, indexNewVertex, getTargetVertexIndex(), getSourceVertexIndex()); - if (triangle != NULL) { halfLink(EdgeEPA(triangle, 1), *this); return true; } - return false; } } } - return true; } + diff --git a/ephysics/collision/narrowphase/EPA/EdgeEPA.hpp b/ephysics/collision/narrowphase/EPA/EdgeEPA.hpp index 1d1daec..f0373f0 100644 --- a/ephysics/collision/narrowphase/EPA/EdgeEPA.hpp +++ b/ephysics/collision/narrowphase/EPA/EdgeEPA.hpp @@ -4,95 +4,61 @@ * @license BSD 3 clauses (see license file) */ #pragma once - - -// Libraries #include -/// ReactPhysics3D namespace namespace ephysics { - -// Class declarations class TriangleEPA; class TrianglesStore; - -// Class EdgeEPA -/** +/** + * @brief Class EdgeEPA * This class represents an edge of the current polytope in the EPA algorithm. */ class EdgeEPA { - private: - - // -------------------- Attributes -------------------- // - /// Pointer to the triangle that contains this edge - TriangleEPA* mOwnerTriangle; - + TriangleEPA* m_ownerTriangle; /// Index of the edge in the triangle (between 0 and 2). /// The edge with index i connect triangle vertices i and (i+1 % 3) - int32_t mIndex; - + int32_t m_index; public: - - // -------------------- Methods -------------------- // - /// Constructor EdgeEPA(); - /// Constructor EdgeEPA(TriangleEPA* ownerTriangle, int32_t index); - /// Copy-constructor EdgeEPA(const EdgeEPA& edge); - /// Destructor ~EdgeEPA(); - /// 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 - int32_t getIndex() const; - + int32_t getIndex() const { + return m_index; + } /// Return index of the source vertex of the edge uint32_t getSourceVertexIndex() const; - /// Return the index of the target vertex of the edge uint32_t getTargetVertexIndex() const; - /// Execute the recursive silhouette algorithm from this edge bool computeSilhouette(const vec3* vertices, uint32_t index, TrianglesStore& triangleStore); - /// 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 -inline int32_t indexOfNextCounterClockwiseEdge(int32_t i) { - return (i + 1) % 3; +inline int32_t indexOfNextCounterClockwiseEdge(int32_t _iii) { + return (_iii + 1) % 3; } // Return the index of the previous counter-clockwise edge of the ownver triangle -inline int32_t indexOfPreviousCounterClockwiseEdge(int32_t i) { - return (i + 2) % 3; +inline int32_t indexOfPreviousCounterClockwiseEdge(int32_t _iii) { + return (_iii + 2) % 3; } } diff --git a/ephysics/collision/narrowphase/EPA/TriangleEPA.cpp b/ephysics/collision/narrowphase/EPA/TriangleEPA.cpp index 2b1c441..632038d 100644 --- a/ephysics/collision/narrowphase/EPA/TriangleEPA.cpp +++ b/ephysics/collision/narrowphase/EPA/TriangleEPA.cpp @@ -4,132 +4,97 @@ * @license BSD 3 clauses (see license file) */ -// Libraries #include #include #include -// We use the ReactPhysics3D namespace using namespace ephysics; -// Constructor TriangleEPA::TriangleEPA() { } -// Constructor -TriangleEPA::TriangleEPA(uint32_t indexVertex1, uint32_t indexVertex2, uint32_t indexVertex3) - : mIsObsolete(false) { - mIndicesVertices[0] = indexVertex1; - mIndicesVertices[1] = indexVertex2; - mIndicesVertices[2] = indexVertex3; +TriangleEPA::TriangleEPA(uint32_t _indexVertex1, uint32_t _indexVertex2, uint32_t _indexVertex3): + m_isObsolete(false) { + m_indicesVertices[0] = _indexVertex1; + m_indicesVertices[1] = _indexVertex2; + m_indicesVertices[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() { - + } -// Compute the point v closest to the origin of this triangle -bool TriangleEPA::computeClosestPoint(const vec3* vertices) { - const vec3& p0 = vertices[mIndicesVertices[0]]; - - vec3 v1 = vertices[mIndicesVertices[1]] - p0; - vec3 v2 = vertices[mIndicesVertices[2]] - p0; +bool TriangleEPA::computeClosestPoint(const vec3* _vertices) { + const vec3& p0 = _vertices[m_indicesVertices[0]]; + vec3 v1 = _vertices[m_indicesVertices[1]] - p0; + vec3 v2 = _vertices[m_indicesVertices[2]] - p0; float v1Dotv1 = v1.dot(v1); float v1Dotv2 = v1.dot(v2); float v2Dotv2 = v2.dot(v2); float p0Dotv1 = p0.dot(v1); float p0Dotv2 = p0.dot(v2); - // Compute determinant - mDet = v1Dotv1 * v2Dotv2 - v1Dotv2 * v1Dotv2; - + m_determinant = v1Dotv1 * v2Dotv2 - v1Dotv2 * v1Dotv2; // Compute lambda values - mLambda1 = p0Dotv2 * v1Dotv2 - p0Dotv1 * v2Dotv2; - mLambda2 = p0Dotv1 * v1Dotv2 - p0Dotv2 * v1Dotv1; - + m_lambda1 = p0Dotv2 * v1Dotv2 - p0Dotv1 * v2Dotv2; + m_lambda2 = p0Dotv1 * v1Dotv2 - p0Dotv2 * v1Dotv1; // If the determinant is positive - if (mDet > 0.0) { + if (m_determinant > 0.0) { // 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 - mDistSquare = mClosestPoint.dot(mClosestPoint); - + m_distSquare = m_closestPoint.dot(m_closestPoint); return true; } - return false; } -/// 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 -/// are neighbour along both edges). -bool ephysics::link(const EdgeEPA& edge0, const EdgeEPA& edge1) { - bool isPossible = (edge0.getSourceVertexIndex() == edge1.getTargetVertexIndex() && - edge0.getTargetVertexIndex() == edge1.getSourceVertexIndex()); - - if (isPossible) { - edge0.getOwnerTriangle()->mAdjacentEdges[edge0.getIndex()] = edge1; - edge1.getOwnerTriangle()->mAdjacentEdges[edge1.getIndex()] = edge0; +bool ephysics::link(const EdgeEPA& _edge0, const EdgeEPA& _edge1) { + if ( _edge0.getSourceVertexIndex() == _edge1.getTargetVertexIndex() + && _edge0.getTargetVertexIndex() == _edge1.getSourceVertexIndex() ) { + _edge0.getOwnerTriangle()->m_adjacentEdges[_edge0.getIndex()] = _edge1; + _edge1.getOwnerTriangle()->m_adjacentEdges[_edge1.getIndex()] = _edge0; + return true; } - - return isPossible; + return false; } -/// Make an half link of an edge with another one from another triangle. An half-link -/// between an edge "edge0" and an edge "edge1" represents the fact that "edge1" is an -/// adjacent edge of "edge0" but not the opposite. The opposite edge connection will -/// be made later. -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; +void ephysics::halfLink(const EdgeEPA& _edge0, const EdgeEPA& _edge1) { + assert( _edge0.getSourceVertexIndex() == _edge1.getTargetVertexIndex() + && _edge0.getTargetVertexIndex() == _edge1.getSourceVertexIndex()); + _edge0.getOwnerTriangle()->m_adjacentEdges[_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 setIsObsolete(true); - // Execute recursively the silhouette algorithm for the adjacent edges of neighboring // triangles of the current triangle - bool result = mAdjacentEdges[0].computeSilhouette(vertices, indexNewVertex, triangleStore) && - mAdjacentEdges[1].computeSilhouette(vertices, indexNewVertex, triangleStore) && - mAdjacentEdges[2].computeSilhouette(vertices, indexNewVertex, triangleStore); - + bool result = m_adjacentEdges[0].computeSilhouette(_vertices, _indexNewVertex, _triangleStore) && + m_adjacentEdges[1].computeSilhouette(_vertices, _indexNewVertex, _triangleStore) && + m_adjacentEdges[2].computeSilhouette(_vertices, _indexNewVertex, _triangleStore); if (result) { int32_t i,j; - // For each triangle face that contains the new vertex and an edge of the silhouette - for (i=first, j=triangleStore.getNbTriangles()-1; - i != triangleStore.getNbTriangles(); j = i++) { - TriangleEPA* triangle = &triangleStore[i]; + for (i=first, j=_triangleStore.getNbTriangles()-1; + i != _triangleStore.getNbTriangles(); j = i++) { + TriangleEPA* triangle = &_triangleStore[i]; 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 result; } diff --git a/ephysics/collision/narrowphase/EPA/TriangleEPA.hpp b/ephysics/collision/narrowphase/EPA/TriangleEPA.hpp index 9cc155d..7267399 100644 --- a/ephysics/collision/narrowphase/EPA/TriangleEPA.hpp +++ b/ephysics/collision/narrowphase/EPA/TriangleEPA.hpp @@ -4,173 +4,108 @@ * @license BSD 3 clauses (see license file) */ #pragma once - -// Libraries #include #include #include -#include - -/// ReactPhysics3D namespace namespace ephysics { - -// Prototypes -bool link(const EdgeEPA& edge0, const EdgeEPA& edge1); -void halfLink(const EdgeEPA& edge0, const EdgeEPA& edge1); - - -// Class TriangleEPA -/** - * This class represents a triangle face of the current polytope in the EPA algorithm. - */ -class TriangleEPA { - - private: - - // -------------------- Attributes -------------------- // - - /// Indices of the vertices y_i of the triangle - uint32_t mIndicesVertices[3]; - - /// Three adjacent edges of the triangle (edges of other triangles) - EdgeEPA mAdjacentEdges[3]; - - /// True if the triangle face is visible from the new support point - bool mIsObsolete; - - /// Determinant - float mDet; - - /// Point v closest to the origin on the affine hull of the triangle - vec3 mClosestPoint; - - /// Lambda1 value such that v = lambda0 * y_0 + lambda1 * y_1 + lambda2 * y_2 - float mLambda1; - - /// Lambda1 value such that v = lambda0 * y_0 + lambda1 * y_1 + lambda2 * y_2 - float mLambda2; - - /// Square distance of the point closest point v to the origin - float mDistSquare; - - // -------------------- Methods -------------------- // - - /// Private copy-constructor - TriangleEPA(const TriangleEPA& triangle); - - /// Private assignment operator - TriangleEPA& operator=(const TriangleEPA& triangle); - - public: - - // -------------------- Methods -------------------- // - - /// Constructor - TriangleEPA(); - - /// Constructor - TriangleEPA(uint32_t v1, uint32_t v2, uint32_t v3); - - /// Destructor - ~TriangleEPA(); - - /// Return an adjacent edge of the triangle - EdgeEPA& getAdjacentEdge(int32_t index); - - /// Set an adjacent edge of the triangle - void setAdjacentEdge(int32_t index, EdgeEPA& edge); - - /// Return the square distance of the closest point to origin - float getDistSquare() const; - - /// Set the isObsolete value - void setIsObsolete(bool isObsolete); - - /// Return true if the triangle face is obsolete - bool getIsObsolete() const; - - /// Return the point closest to the origin - const vec3& getClosestPoint() const; - - // Return true if the closest point on affine hull is inside the triangle - bool isClosestPointInternalToTriangle() const; - - /// Return true if the triangle is visible from a given vertex - bool isVisibleFromVertex(const vec3* vertices, uint32_t index) const; - - /// Compute the point v closest to the origin of this triangle - bool computeClosestPoint(const vec3* vertices); - - /// Compute the point of an object closest to the origin - vec3 computeClosestPointOfObject(const vec3* supportPointsOfObject) const; - - /// Execute the recursive silhouette algorithm from this triangle face. - bool computeSilhouette(const vec3* vertices, uint32_t index, TrianglesStore& triangleStore); - - /// Access operator - uint32_t operator[](int32_t i) const; - - /// Associate two edges - 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]; -} + 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 { + private: + 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) + bool m_isObsolete; //!< True if the triangle face is visible from the new support point + float m_determinant; //!< Determinant + 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 + 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 + /// Private copy-constructor + TriangleEPA(const TriangleEPA& _triangle); + /// Private assignment operator + TriangleEPA& operator=(const TriangleEPA& _triangle); + public: + /// Constructor + TriangleEPA(); + /// Constructor + TriangleEPA(uint32_t _v1, uint32_t _v2, uint32_t _v3); + /// Constructor + void set(uint32_t _v1, uint32_t _v2, uint32_t _v3); + /// Destructor + ~TriangleEPA(); + /// Return an adjacent edge of the triangle + EdgeEPA& getAdjacentEdge(int32_t _index) { + assert(_index >= 0 && _index < 3); + return m_adjacentEdges[_index]; + } + /// Set an adjacent edge of the triangle + void setAdjacentEdge(int32_t _index, EdgeEPA& _edge) { + assert(_index >=0 && _index < 3); + m_adjacentEdges[_index] = _edge; + } + /// Return the square distance of the closest point to origin + float getDistSquare() const { + return m_distSquare; + } + /// Set the isObsolete value + void setIsObsolete(bool _isObsolete) { + m_isObsolete = _isObsolete; + } + /// Return true if the triangle face is obsolete + bool getIsObsolete() const { + return m_isObsolete; + } + /// Return the point closest to the origin + const vec3& getClosestPoint() const { + return m_closestPoint; + } + // Return true if the closest point on affine hull is inside the triangle + bool isClosestPointInternalToTriangle() const { + return (m_lambda1 >= 0.0 && m_lambda2 >= 0.0 && (m_lambda1 + m_lambda2) <= m_determinant); + } + /// Return true if the triangle is visible from a given vertex + bool isVisibleFromVertex(const vec3* _vertices, uint32_t _index) const { + vec3 closestToVert = _vertices[_index] - m_closestPoint; + return (m_closestPoint.dot(closestToVert) > 0.0); + } + /// Compute the point v closest to the origin of this triangle + bool computeClosestPoint(const vec3* _vertices); + /// Compute the point of an object closest to the origin + vec3 computeClosestPointOfObject(const vec3* _supportPointsOfObject) const{ + const vec3& p0 = _supportPointsOfObject[m_indicesVertices[0]]; + return p0 + 1.0f/m_determinant * (m_lambda1 * (_supportPointsOfObject[m_indicesVertices[1]] - p0) + + m_lambda2 * (_supportPointsOfObject[m_indicesVertices[2]] - p0)); + } + // 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 computeSilhouette(const vec3* _vertices, uint32_t _index, TrianglesStore& _triangleStore); + /// Access operator + uint32_t operator[](int32_t _pos) const { + assert(_pos >= 0 && _pos <3); + return m_indicesVertices[_pos]; + } + /// 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 + /// are neighbour along both edges). + 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 + /// between an edge "edge0" and an edge "edge1" represents the fact that "edge1" is an + /// adjacent edge of "edge0" but not the opposite. The opposite edge connection will + /// be made later. + friend void halfLink(const EdgeEPA& _edge0, const EdgeEPA& _edge1); + }; } diff --git a/ephysics/collision/narrowphase/EPA/TrianglesStore.cpp b/ephysics/collision/narrowphase/EPA/TrianglesStore.cpp index d57b106..28fb2ca 100644 --- a/ephysics/collision/narrowphase/EPA/TrianglesStore.cpp +++ b/ephysics/collision/narrowphase/EPA/TrianglesStore.cpp @@ -4,18 +4,12 @@ * @license BSD 3 clauses (see license file) */ -// Libraries #include -// We use the ReactPhysics3D namespace -using namespace ephysics; - -// Constructor -TrianglesStore::TrianglesStore() : m_numberTriangles(0) { +ephysics::TrianglesStore::TrianglesStore() : m_numberTriangles(0) { } -// Destructor -TrianglesStore::~TrianglesStore() { - +ephysics::TrianglesStore::~TrianglesStore() { + } diff --git a/ephysics/collision/narrowphase/EPA/TrianglesStore.hpp b/ephysics/collision/narrowphase/EPA/TrianglesStore.hpp index fe50c2c..f12ae1d 100644 --- a/ephysics/collision/narrowphase/EPA/TrianglesStore.hpp +++ b/ephysics/collision/narrowphase/EPA/TrianglesStore.hpp @@ -4,116 +4,61 @@ * @license BSD 3 clauses (see license file) */ #pragma once - #include - - -// Libraries -#include - -/// ReactPhysics3D namespace namespace ephysics { - -// Constants -const uint32_t MAX_TRIANGLES = 200; // Maximum number of triangles - -// Class TriangleStore -/** - * This class stores several triangles of the polytope in the EPA algorithm. - */ -class TrianglesStore { - - private: - - // -------------------- Attributes -------------------- // - - /// Triangles - TriangleEPA mTriangles[MAX_TRIANGLES]; - - /// Number of triangles - int32_t m_numberTriangles; - - // -------------------- Methods -------------------- // - - /// Private copy-constructor - TrianglesStore(const TrianglesStore& triangleStore); - - /// Private assignment operator - TrianglesStore& operator=(const TrianglesStore& triangleStore); - - public: - - // -------------------- Methods -------------------- // - - /// Constructor - TrianglesStore(); - - /// Destructor - ~TrianglesStore(); - - /// Clear all the storage - void clear(); - - /// Return the number of triangles - int32_t getNbTriangles() const; - - /// Set the number of triangles - void setNbTriangles(int32_t backup); - - /// Return the last triangle - TriangleEPA& last(); - - /// Create a new triangle - TriangleEPA* newTriangle(const vec3* vertices, uint32_t v0, uint32_t v1, uint32_t v2); - - /// 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]; -} - + const uint32_t MAX_TRIANGLES = 200; // Maximum number of triangles + /** + * @brief This class stores several triangles of the polytope in the EPA algorithm. + */ + class TrianglesStore { + private: + TriangleEPA m_triangles[MAX_TRIANGLES]; //!< Triangles + int32_t m_numberTriangles; //!< Number of triangles + /// Private copy-constructor + TrianglesStore(const TrianglesStore& triangleStore); + /// Private assignment operator + TrianglesStore& operator=(const TrianglesStore& triangleStore); + public: + /// Constructor + TrianglesStore(); + /// Destructor + ~TrianglesStore(); + /// Clear all the storage + void clear() { + m_numberTriangles = 0; + } + /// Return the number of triangles + int32_t getNbTriangles() const { + return m_numberTriangles; + } + /// Set the number of triangles + void setNbTriangles(int32_t _backup) { + m_numberTriangles = _backup; + } + /// Return the last triangle + TriangleEPA& last() { + assert(m_numberTriangles > 0); + return m_triangles[m_numberTriangles - 1]; + } + /// Create a new triangle + TriangleEPA* newTriangle(const vec3* _vertices, uint32_t _v0, uint32_t _v1, uint32_t _v2) { + TriangleEPA* newTriangle = nullptr; + // If we have not reached the maximum number of triangles + if (m_numberTriangles != MAX_TRIANGLES) { + newTriangle = &m_triangles[m_numberTriangles++]; + newTriangle->set(_v0, _v1, _v2); + if (!newTriangle->computeClosestPoint(_vertices)) { + m_numberTriangles--; + newTriangle = nullptr; + } + } + // Return the new triangle + return newTriangle; + } + /// Access operator + TriangleEPA& operator[](int32_t _id) { + return m_triangles[_id]; + } + }; } diff --git a/ephysics/collision/narrowphase/GJK/GJKAlgorithm.cpp b/ephysics/collision/narrowphase/GJK/GJKAlgorithm.cpp index 3b3200d..fd4f771 100644 --- a/ephysics/collision/narrowphase/GJK/GJKAlgorithm.cpp +++ b/ephysics/collision/narrowphase/GJK/GJKAlgorithm.cpp @@ -3,158 +3,107 @@ * @copyright 2010-2016 Daniel Chappuis * @license BSD 3 clauses (see license file) */ - -// Libraries #include #include #include #include #include -#include -#include -#include -#include -// We want to use the ReactPhysics3D namespace using namespace ephysics; -// Constructor GJKAlgorithm::GJKAlgorithm() : NarrowPhaseAlgorithm() { - + } -// Destructor 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, const CollisionShapeInfo& shape2Info, NarrowPhaseCallback* narrowPhaseCallback) { - PROFILE("GJKAlgorithm::testCollision()"); - - vec3 suppA; // Support point of object A - vec3 suppB; // Support point of object B - vec3 w; // Support point of Minkowski difference A-B - vec3 pA; // Closest point of object A - vec3 pB; // Closest point of object B + vec3 suppA; // Support point of object A + vec3 suppB; // Support point of object B + vec3 w; // Support point of Minkowski difference A-B + vec3 pA; // Closest point of object A + vec3 pB; // Closest point of object B float vDotw; float prevDistSquare; - assert(shape1Info.collisionShape->isConvex()); assert(shape2Info.collisionShape->isConvex()); - const ConvexShape* shape1 = static_cast(shape1Info.collisionShape); const ConvexShape* shape2 = static_cast(shape2Info.collisionShape); - void** shape1CachedCollisionData = shape1Info.cachedCollisionData; void** shape2CachedCollisionData = shape2Info.cachedCollisionData; - // Get the local-space to world-space transforms const etk::Transform3D transform1 = shape1Info.shapeToWorldTransform; const etk::Transform3D transform2 = shape2Info.shapeToWorldTransform; - // 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) etk::Transform3D body2Tobody1 = transform1.getInverse() * transform2; - // 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() * transform1.getOrientation().getMatrix(); - // Initialize the margin (sum of margins of both objects) float margin = shape1->getMargin() + shape2->getMargin(); float marginSquare = margin * margin; assert(margin > 0.0); - // Create a simplex set Simplex simplex; - // Get the previous point V (last cached separating axis) vec3 v = m_currentOverlappingPair->getCachedSeparatingAxis(); - // Initialize the upper bound for the square distance - float distSquare = DECIMAL_LARGEST; - + float distSquare = FLT_MAX; do { - // Compute the support points for original objects (without margins) A and B suppA = shape1->getLocalSupportPointWithoutMargin(-v, shape1CachedCollisionData); - suppB = body2Tobody1 * - shape2->getLocalSupportPointWithoutMargin(rotateToBody2 * v, shape2CachedCollisionData); - + suppB = body2Tobody1 * shape2->getLocalSupportPointWithoutMargin(rotateToBody2 * v, shape2CachedCollisionData); // Compute the support point for the Minkowski difference A-B w = suppA - suppB; - vDotw = v.dot(w); - // If the enlarge objects (with margins) do not int32_tersect if (vDotw > 0.0 && vDotw * vDotw > distSquare * marginSquare) { - // Cache the current separating axis for frame coherence m_currentOverlappingPair->setCachedSeparatingAxis(v); - // No int32_tersection, we return return; } - // If the objects int32_tersect only in the margins if (simplex.isPointInSimplex(w) || distSquare - vDotw <= distSquare * REL_ERROR_SQUARE) { - // Compute the closet points of both objects (without the margins) simplex.computeClosestPointsOfAandB(pA, pB); - // Project those two points on the margins to have the closest points of both // object with the margins float dist = sqrt(distSquare); assert(dist > 0.0); pA = (pA - (shape1->getMargin() / dist) * v); pB = body2Tobody1.getInverse() * (pB + (shape2->getMargin() / dist) * v); - // Compute the contact info vec3 normal = transform1.getOrientation() * (-v.safeNormalized()); float penetrationDepth = margin - dist; - // Reject the contact if the penetration depth is negative (due too numerical errors) if (penetrationDepth <= 0.0) return; - // Create the contact info object ContactPointInfo contactInfo(shape1Info.proxyShape, shape2Info.proxyShape, shape1Info.collisionShape, shape2Info.collisionShape, normal, penetrationDepth, pA, pB); - narrowPhaseCallback->notifyContact(shape1Info.overlappingPair, contactInfo); - // There is an int32_tersection, therefore we return return; } - // Add the new support point to the simplex simplex.addPoint(w, suppA, suppB); - // If the simplex is affinely dependent if (simplex.isAffinelyDependent()) { - // Compute the closet points of both objects (without the margins) simplex.computeClosestPointsOfAandB(pA, pB); - // Project those two points on the margins to have the closest points of both // object with the margins float dist = sqrt(distSquare); assert(dist > 0.0); pA = (pA - (shape1->getMargin() / dist) * v); pB = body2Tobody1.getInverse() * (pB + (shape2->getMargin() / dist) * v); - // Compute the contact info vec3 normal = transform1.getOrientation() * (-v.safeNormalized()); float penetrationDepth = margin - dist; @@ -165,27 +114,21 @@ void GJKAlgorithm::testCollision(const CollisionShapeInfo& shape1Info, // Create the contact info object ContactPointInfo contactInfo(shape1Info.proxyShape, shape2Info.proxyShape, shape1Info.collisionShape, shape2Info.collisionShape, normal, penetrationDepth, pA, pB); - narrowPhaseCallback->notifyContact(shape1Info.overlappingPair, contactInfo); - // There is an int32_tersection, therefore we return return; } - // Compute the point of the simplex closest to the origin // If the computation of the closest point fail if (!simplex.computeClosestPoint(v)) { - // Compute the closet points of both objects (without the margins) simplex.computeClosestPointsOfAandB(pA, pB); - // Project those two points on the margins to have the closest points of both // object with the margins float dist = sqrt(distSquare); assert(dist > 0.0); pA = (pA - (shape1->getMargin() / dist) * v); pB = body2Tobody1.getInverse() * (pB + (shape2->getMargin() / dist) * v); - // Compute the contact info vec3 normal = transform1.getOrientation() * (-v.safeNormalized()); float penetrationDepth = margin - dist; @@ -196,34 +139,27 @@ void GJKAlgorithm::testCollision(const CollisionShapeInfo& shape1Info, // Create the contact info object ContactPointInfo contactInfo(shape1Info.proxyShape, shape2Info.proxyShape, shape1Info.collisionShape, shape2Info.collisionShape, normal, penetrationDepth, pA, pB); - narrowPhaseCallback->notifyContact(shape1Info.overlappingPair, contactInfo); - // There is an int32_tersection, therefore we return return; } - // Store and update the squared distance of the closest point prevDistSquare = distSquare; distSquare = v.length2(); - // 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); // Get the new squared distance distSquare = v.length2(); - // Compute the closet points of both objects (without the margins) simplex.computeClosestPointsOfAandB(pA, pB); - // Project those two points on the margins to have the closest points of both // object with the margins float dist = sqrt(distSquare); assert(dist > 0.0); pA = (pA - (shape1->getMargin() / dist) * v); pB = body2Tobody1.getInverse() * (pB + (shape2->getMargin() / dist) * v); - // Compute the contact info vec3 normal = transform1.getOrientation() * (-v.safeNormalized()); float penetrationDepth = margin - dist; @@ -234,15 +170,12 @@ void GJKAlgorithm::testCollision(const CollisionShapeInfo& shape1Info, // Create the contact info object ContactPointInfo contactInfo(shape1Info.proxyShape, shape2Info.proxyShape, shape1Info.collisionShape, shape2Info.collisionShape, normal, penetrationDepth, pA, pB); - narrowPhaseCallback->notifyContact(shape1Info.overlappingPair, contactInfo); - // There is an int32_tersection, therefore we return return; } - } while(!simplex.isFull() && distSquare > MACHINE_EPSILON * + } while(!simplex.isFull() && distSquare > FLT_EPSILON * simplex.getMaxLengthSquareOfAPoint()); - // 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 // 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); } -/// 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, const etk::Transform3D& transform1, const CollisionShapeInfo& shape2Info, @@ -263,172 +191,120 @@ void GJKAlgorithm::computePenetrationDepthForEnlargedObjects(const CollisionShap NarrowPhaseCallback* narrowPhaseCallback, vec3& v) { PROFILE("GJKAlgorithm::computePenetrationDepthForEnlargedObjects()"); - Simplex simplex; vec3 suppA; vec3 suppB; vec3 w; float vDotw; - float distSquare = DECIMAL_LARGEST; + float distSquare = FLT_MAX; float prevDistSquare; - assert(shape1Info.collisionShape->isConvex()); assert(shape2Info.collisionShape->isConvex()); - const ConvexShape* shape1 = static_cast(shape1Info.collisionShape); const ConvexShape* shape2 = static_cast(shape2Info.collisionShape); - void** shape1CachedCollisionData = shape1Info.cachedCollisionData; void** shape2CachedCollisionData = shape2Info.cachedCollisionData; - // 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) etk::Transform3D body2ToBody1 = transform1.getInverse() * transform2; - // 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() * transform1.getOrientation().getMatrix(); - do { // Compute the support points for the enlarged object A and B suppA = shape1->getLocalSupportPointWithMargin(-v, shape1CachedCollisionData); suppB = body2ToBody1 * shape2->getLocalSupportPointWithMargin(rotateToBody2 * v, shape2CachedCollisionData); - // Compute the support point for the Minkowski difference A-B w = suppA - suppB; - vDotw = v.dot(w); - // If the enlarge objects do not int32_tersect if (vDotw > 0.0) { - // No int32_tersection, we return return; } - // Add the new support point to the simplex simplex.addPoint(w, suppA, suppB); - if (simplex.isAffinelyDependent()) { return; } - if (!simplex.computeClosestPoint(v)) { return; } - // Store and update the square distance prevDistSquare = distSquare; distSquare = v.length2(); - - if (prevDistSquare - distSquare <= MACHINE_EPSILON * prevDistSquare) { + if (prevDistSquare - distSquare <= FLT_EPSILON * prevDistSquare) { return; } - - } while(!simplex.isFull() && distSquare > MACHINE_EPSILON * + } while(!simplex.isFull() && distSquare > FLT_EPSILON * simplex.getMaxLengthSquareOfAPoint()); - // Give the simplex computed with GJK algorithm to the EPA algorithm // which will compute the correct penetration depth and contact points // between the two enlarged objects - return mAlgoEPA.computePenetrationDepthAndContactPoints(simplex, shape1Info, + return m_algoEPA.computePenetrationDepthAndContactPoints(simplex, shape1Info, transform1, shape2Info, transform2, 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) { - vec3 suppA; // Support point of object A vec3 w; // Support point of Minkowski difference A-B float prevDistSquare; - assert(proxyShape->getCollisionShape()->isConvex()); - const ConvexShape* shape = static_cast(proxyShape->getCollisionShape()); - void** shapeCachedCollisionData = proxyShape->getCachedCollisionData(); - // Support point of object B (object B is a single point) const vec3 suppB(localPoint); - // Create a simplex set Simplex simplex; - // Initial supporting direction vec3 v(1, 1, 1); - // Initialize the upper bound for the square distance - float distSquare = DECIMAL_LARGEST; - + float distSquare = FLT_MAX; do { - // Compute the support points for original objects (without margins) A and B suppA = shape->getLocalSupportPointWithoutMargin(-v, shapeCachedCollisionData); - // Compute the support point for the Minkowski difference A-B w = suppA - suppB; - // Add the new support point to the simplex simplex.addPoint(w, suppA, suppB); - // If the simplex is affinely dependent if (simplex.isAffinelyDependent()) { - return false; } - // Compute the point of the simplex closest to the origin // If the computation of the closest point fail if (!simplex.computeClosestPoint(v)) { - return false; } - // Store and update the squared distance of the closest point prevDistSquare = distSquare; distSquare = v.length2(); - // 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; } - } while(!simplex.isFull() && distSquare > MACHINE_EPSILON * - simplex.getMaxLengthSquareOfAPoint()); - + } while( !simplex.isFull() + && distSquare > FLT_EPSILON * simplex.getMaxLengthSquareOfAPoint()); // The point is inside the collision shape 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) { - assert(proxyShape->getCollisionShape()->isConvex()); - const ConvexShape* shape = static_cast(proxyShape->getCollisionShape()); - void** shapeCachedCollisionData = proxyShape->getCachedCollisionData(); - vec3 suppA; // Current lower bound point on the ray (starting at ray's origin) 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); - // Convert the ray origin and direction int32_to the local-space of the collision shape vec3 rayDirection = ray.point2 - ray.point1; - // If the points of the segment are two close, return no hit if (rayDirection.length2() < machineEpsilonSquare) return false; - vec3 w; - // Create a simplex set Simplex simplex; - vec3 n(0.0f, float(0.0), float(0.0)); float lambda = 0.0f; 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 distSquare = v.length2(); int32_t nbIterations = 0; - // GJK Algorithm loop while (distSquare > epsilon && nbIterations < MAX_ITERATIONS_GJK_RAYCAST) { - // Compute the support points suppB = shape->getLocalSupportPointWithoutMargin(v, shapeCachedCollisionData); w = suppA - suppB; - vDotW = v.dot(w); - if (vDotW > float(0)) { - vDotR = v.dot(rayDirection); - if (vDotR >= -machineEpsilonSquare) { return false; - } - else { - + } else { // We have found a better lower bound for the hit point along the ray lambda = lambda - vDotW / vDotR; suppA = ray.point1 + lambda * rayDirection; @@ -463,47 +331,37 @@ bool GJKAlgorithm::raycast(const Ray& ray, ProxyShape* proxyShape, RaycastInfo& n = v; } } - // Add the new support point to the simplex if (!simplex.isPointInSimplex(w)) { simplex.addPoint(w, suppA, suppB); } - // Compute the closest point if (simplex.computeClosestPoint(v)) { - distSquare = v.length2(); - } - else { + } else { distSquare = 0.0f; } - // If the current lower bound distance is larger than the maximum raycasting distance if (lambda > ray.maxFraction) return false; - nbIterations++; } - // 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) vec3 pointA; vec3 pointB; simplex.computeClosestPointsOfAandB(pointA, pointB); - // A raycast hit has been found, we fill in the raycast info raycastInfo.hitFraction = lambda; raycastInfo.worldPoint = pointB; raycastInfo.body = proxyShape->getBody(); raycastInfo.proxyShape = proxyShape; - if (n.length2() >= machineEpsilonSquare) { // The normal vector is valid 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)); } - return true; } diff --git a/ephysics/collision/narrowphase/GJK/GJKAlgorithm.hpp b/ephysics/collision/narrowphase/GJK/GJKAlgorithm.hpp index bbcf93c..4a0da56 100644 --- a/ephysics/collision/narrowphase/GJK/GJKAlgorithm.hpp +++ b/ephysics/collision/narrowphase/GJK/GJKAlgorithm.hpp @@ -5,94 +5,76 @@ */ #pragma once -// Libraries #include #include #include #include - -/// ReactPhysics3D namespace namespace ephysics { - -// Constants -const float REL_ERROR = float(1.0e-3); -const float REL_ERROR_SQUARE = REL_ERROR * REL_ERROR; -const int32_t MAX_ITERATIONS_GJK_RAYCAST = 32; - -// Class GJKAlgorithm -/** - * This class implements a narrow-phase collision detection algorithm. This - * algorithm uses the ISA-GJK algorithm and the EPA algorithm. This - * implementation is based on the implementation discussed in the book - * "Collision Detection in Interactive 3D Environments" by Gino van den Bergen. - * This method implements the Hybrid Technique for calculating the - * penetration depth. The two objects are enlarged with a small margin. If - * the object int32_tersects in their margins, the penetration depth is quickly - * computed using the GJK algorithm on the original objects (without margin). - * If the original objects (without margin) int32_tersect, we run again the GJK - * algorithm on the enlarged objects (with margin) to compute simplex - * polytope that contains the origin and give it to the EPA (Expanding - * Polytope Algorithm) to compute the correct penetration depth between the - * enlarged objects. - */ -class GJKAlgorithm : public NarrowPhaseAlgorithm { - - private : - - // -------------------- Attributes -------------------- // - - /// EPA Algorithm - EPAAlgorithm mAlgoEPA; - - // -------------------- Methods -------------------- // - - /// Private copy-constructor - GJKAlgorithm(const GJKAlgorithm& algorithm); - - /// Private assignment operator - GJKAlgorithm& operator=(const GJKAlgorithm& algorithm); - - /// Compute the penetration depth for enlarged objects. - void computePenetrationDepthForEnlargedObjects(const CollisionShapeInfo& shape1Info, - const etk::Transform3D& transform1, - const CollisionShapeInfo& shape2Info, - const etk::Transform3D& transform2, - NarrowPhaseCallback* narrowPhaseCallback, - vec3& v); - - public : - - // -------------------- Methods -------------------- // - - /// Constructor - GJKAlgorithm(); - - /// Destructor - ~GJKAlgorithm(); - - /// Initalize the algorithm - virtual void init(CollisionDetection* collisionDetection, - MemoryAllocator* memoryAllocator); - - /// Compute a contact info if the two bounding volumes collide. - virtual void testCollision(const CollisionShapeInfo& shape1Info, - const CollisionShapeInfo& shape2Info, - 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); -} - + const float REL_ERROR = float(1.0e-3); + 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 + * implementation is based on the implementation discussed in the book + * "Collision Detection in Interactive 3D Environments" by Gino van den Bergen. + * This method implements the Hybrid Technique for calculating the + * penetration depth. The two objects are enlarged with a small margin. If + * the object int32_tersects in their margins, the penetration depth is quickly + * computed using the GJK algorithm on the original objects (without margin). + * If the original objects (without margin) int32_tersect, we run again the GJK + * algorithm on the enlarged objects (with margin) to compute simplex + * polytope that contains the origin and give it to the EPA (Expanding + * Polytope Algorithm) to compute the correct penetration depth between the + * enlarged objects. + */ + class GJKAlgorithm : public NarrowPhaseAlgorithm { + private : + EPAAlgorithm m_algoEPA; //!< EPA Algorithm + /// Private copy-constructor + GJKAlgorithm(const GJKAlgorithm& algorithm); + /// Private assignment operator + GJKAlgorithm& operator=(const GJKAlgorithm& algorithm); + /// 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 computePenetrationDepthForEnlargedObjects(const CollisionShapeInfo& shape1Info, + const etk::Transform3D& transform1, + const CollisionShapeInfo& shape2Info, + const etk::Transform3D& transform2, + NarrowPhaseCallback* narrowPhaseCallback, + vec3& v); + public : + /// Constructor + GJKAlgorithm(); + /// Destructor + ~GJKAlgorithm(); + /// Initalize the algorithm + virtual void init(CollisionDetection* _collisionDetection, MemoryAllocator* _memoryAllocator) { + NarrowPhaseAlgorithm::init(_collisionDetection, _memoryAllocator); + m_algoEPA.init(_memoryAllocator); + }; + // 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. + virtual void testCollision(const CollisionShapeInfo& shape1Info, + const CollisionShapeInfo& shape2Info, + 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 + /// 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 raycast(const Ray& ray, ProxyShape* proxyShape, RaycastInfo& raycastInfo); + }; } diff --git a/ephysics/collision/narrowphase/GJK/Simplex.cpp b/ephysics/collision/narrowphase/GJK/Simplex.cpp index 956fa8d..a35164f 100644 --- a/ephysics/collision/narrowphase/GJK/Simplex.cpp +++ b/ephysics/collision/narrowphase/GJK/Simplex.cpp @@ -6,7 +6,6 @@ // Libraries #include -#include // We want to use the ReactPhysics3D namespace using namespace ephysics; @@ -327,7 +326,7 @@ bool Simplex::computeClosestPoint(vec3& v) { // Backup the closest point void Simplex::backupClosestPointInSimplex(vec3& v) { - float minDistSquare = DECIMAL_LARGEST; + float minDistSquare = FLT_MAX; Bits bit; for (bit = mAllBits; bit != 0x0; bit--) { diff --git a/ephysics/collision/narrowphase/SphereVsSphereAlgorithm.cpp b/ephysics/collision/narrowphase/SphereVsSphereAlgorithm.cpp index 639b9e2..6ca3e6d 100644 --- a/ephysics/collision/narrowphase/SphereVsSphereAlgorithm.cpp +++ b/ephysics/collision/narrowphase/SphereVsSphereAlgorithm.cpp @@ -32,7 +32,7 @@ void ephysics::SphereVsSphereAlgorithm::testCollision(const ephysics::CollisionS vec3 centerSphere1InBody2LocalSpace = transform2.getInverse() * transform1.getPosition(); vec3 intersectionOnBody1 = sphereShape1->getRadius() * centerSphere2InBody1LocalSpace.safeNormalized(); vec3 intersectionOnBody2 = sphereShape2->getRadius() * centerSphere1InBody2LocalSpace.safeNormalized(); - float penetrationDepth = sumRadius - std::sqrt(squaredDistanceBetweenCenters); + float penetrationDepth = sumRadius - etk::sqrt(squaredDistanceBetweenCenters); // Create the contact info object ephysics::ContactPointInfo contactInfo(_shape1Info.proxyShape, diff --git a/ephysics/collision/shapes/AABB.cpp b/ephysics/collision/shapes/AABB.cpp index 280cfba..d3d5107 100644 --- a/ephysics/collision/shapes/AABB.cpp +++ b/ephysics/collision/shapes/AABB.cpp @@ -8,7 +8,6 @@ // Libraries #include #include -#include using namespace ephysics; using namespace std; @@ -108,16 +107,16 @@ bool AABB::testRayIntersect(const Ray& _ray) const { const vec3 d = point2 - _ray.point1; const vec3 m = _ray.point1 + point2 - m_minCoordinates - m_maxCoordinates; // Test if the AABB face normals are separating axis - float adx = std::abs(d.x()); - if (std::abs(m.x()) > e.x() + adx) { + float adx = etk::abs(d.x()); + if (etk::abs(m.x()) > e.x() + adx) { return false; } - float ady = std::abs(d.y()); - if (std::abs(m.y()) > e.y() + ady) { + float ady = etk::abs(d.y()); + if (etk::abs(m.y()) > e.y() + ady) { return false; } - float adz = std::abs(d.z()); - if (std::abs(m.z()) > e.z() + adz) { + float adz = etk::abs(d.z()); + if (etk::abs(m.z()) > e.z() + adz) { return false; } // 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; // Test if the cross products between face normals and ray direction are // 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; } - 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; } - 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; } // No separating axis has been found @@ -188,9 +187,9 @@ bool AABB::testCollisionTriangleAABB(const vec3* _trianglePoints) const { } bool AABB::contains(const vec3& _point) const { - return _point.x() >= m_minCoordinates.x() - MACHINE_EPSILON && _point.x() <= m_maxCoordinates.x() + MACHINE_EPSILON - && _point.y() >= m_minCoordinates.y() - MACHINE_EPSILON && _point.y() <= m_maxCoordinates.y() + MACHINE_EPSILON - && _point.z() >= m_minCoordinates.z() - MACHINE_EPSILON && _point.z() <= m_maxCoordinates.z() + MACHINE_EPSILON; + return _point.x() >= m_minCoordinates.x() - FLT_EPSILON && _point.x() <= m_maxCoordinates.x() + FLT_EPSILON + && _point.y() >= m_minCoordinates.y() - FLT_EPSILON && _point.y() <= m_maxCoordinates.y() + FLT_EPSILON + && _point.z() >= m_minCoordinates.z() - FLT_EPSILON && _point.z() <= m_maxCoordinates.z() + FLT_EPSILON; } AABB& AABB::operator=(const AABB& _aabb) { diff --git a/ephysics/collision/shapes/BoxShape.cpp b/ephysics/collision/shapes/BoxShape.cpp index cd78311..dffff34 100644 --- a/ephysics/collision/shapes/BoxShape.cpp +++ b/ephysics/collision/shapes/BoxShape.cpp @@ -9,7 +9,6 @@ #include #include #include -#include 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 { vec3 rayDirection = ray.point2 - ray.point1; - float tMin = DECIMAL_SMALLEST; - float tMax = DECIMAL_LARGEST; + float tMin = FLT_MIN; + float tMax = FLT_MAX; vec3 normalDirection(float(0), float(0), float(0)); vec3 currentNormal; @@ -56,7 +55,7 @@ bool BoxShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* pro for (int32_t i=0; i<3; i++) { // 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 (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 // t2 with far plane if (t1 > t2) { - std::swap(t1, t2); + etk::swap(t1, t2); currentNormal = -currentNormal; } diff --git a/ephysics/collision/shapes/BoxShape.hpp b/ephysics/collision/shapes/BoxShape.hpp index cba2327..585a1a9 100644 --- a/ephysics/collision/shapes/BoxShape.hpp +++ b/ephysics/collision/shapes/BoxShape.hpp @@ -5,7 +5,6 @@ */ #pragma once -#include #include #include #include diff --git a/ephysics/collision/shapes/CapsuleShape.cpp b/ephysics/collision/shapes/CapsuleShape.cpp index 7f462f6..f258fe1 100644 --- a/ephysics/collision/shapes/CapsuleShape.cpp +++ b/ephysics/collision/shapes/CapsuleShape.cpp @@ -8,7 +8,6 @@ #include #include #include -#include using namespace ephysics; @@ -101,7 +100,7 @@ bool CapsuleShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* float c = dDotD * k - mDotD * mDotD; // 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 (c > 0.0f) return false; @@ -156,7 +155,7 @@ bool CapsuleShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* if (discriminant < 0.0f) return false; // 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 float value = mDotD + t * nDotD; @@ -241,10 +240,10 @@ bool CapsuleShape::raycastWithSphereEndCap(const vec3& point1, const vec3& point float discriminant = b * b - raySquareLength * c; // 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 - float t = -b - std::sqrt(discriminant); + float t = -b - etk::sqrt(discriminant); assert(t >= 0.0f); diff --git a/ephysics/collision/shapes/CollisionShape.hpp b/ephysics/collision/shapes/CollisionShape.hpp index 8f0c479..a933b22 100644 --- a/ephysics/collision/shapes/CollisionShape.hpp +++ b/ephysics/collision/shapes/CollisionShape.hpp @@ -5,8 +5,7 @@ */ #pragma once -#include -#include +#include #include #include #include diff --git a/ephysics/collision/shapes/ConcaveMeshShape.cpp b/ephysics/collision/shapes/ConcaveMeshShape.cpp index f55c933..83341a0 100644 --- a/ephysics/collision/shapes/ConcaveMeshShape.cpp +++ b/ephysics/collision/shapes/ConcaveMeshShape.cpp @@ -6,7 +6,6 @@ #include #include -#include using namespace ephysics; @@ -45,7 +44,7 @@ void ConcaveMeshShape::getTriangleVerticesWithIndexPointer(int32_t _subPart, int // Get the triangle vertex array of the current sub-part TriangleVertexArray* triangleVertexArray = m_triangleMesh->getSubpart(_subPart); if (triangleVertexArray == nullptr) { - std::cout << "get nullptr ..." << std::endl; + EPHY_ERROR("get nullptr ..."); } ephysics::Triangle trianglePoints = triangleVertexArray->getTriangle(_triangleIndex); _outTriangleVertices[0] = trianglePoints[0] * m_scaling; @@ -79,7 +78,7 @@ float ConcaveMeshRaycastCallback::raycastBroadPhaseShape(int32_t _nodeId, const } void ConcaveMeshRaycastCallback::raycastTriangles() { - etk::Vector::const_iterator it; + etk::Vector::Iterator it; float smallestHitFraction = m_ray.maxFraction; for (it = m_hitAABBNodes.begin(); it != m_hitAABBNodes.end(); ++it) { // Get the node data (triangle index and mesh subpart index) diff --git a/ephysics/collision/shapes/ConeShape.cpp b/ephysics/collision/shapes/ConeShape.cpp index 557d497..11f436a 100644 --- a/ephysics/collision/shapes/ConeShape.cpp +++ b/ephysics/collision/shapes/ConeShape.cpp @@ -5,7 +5,6 @@ */ // Libraries -#include #include #include #include @@ -42,7 +41,7 @@ vec3 ConeShape::getLocalSupportPointWithoutMargin(const vec3& direction, } else { float projectedLength = sqrt(v.x() * v.x() + v.z() * v.z()); - if (projectedLength > MACHINE_EPSILON) { + if (projectedLength > FLT_EPSILON) { float d = m_radius / projectedLength; 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]; // If c2 is different from zero - if (std::abs(c2) > MACHINE_EPSILON) { + if (etk::abs(c2) > FLT_EPSILON) { float gamma = c1 * c1 - c0 * c2; // 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 // Compute two int32_tersections - float sqrRoot = std::sqrt(gamma); + float sqrRoot = etk::sqrt(gamma); tHit[0] = (-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 // 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); } 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 int32_t hitIndex = -1; - float t = DECIMAL_LARGEST; + float t = FLT_MAX; for (int32_t i=0; i<3; i++) { if (tHit[i] < 0.0f) continue; if (tHit[i] < t) { @@ -174,11 +173,11 @@ bool ConeShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* pr localHitPoint[hitIndex].z() * localHitPoint[hitIndex].z()); float rOverH = m_radius / h; 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 z = localHitPoint[hitIndex].z() * factor; 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); } diff --git a/ephysics/collision/shapes/ConvexMeshShape.cpp b/ephysics/collision/shapes/ConvexMeshShape.cpp index a669306..7896178 100644 --- a/ephysics/collision/shapes/ConvexMeshShape.cpp +++ b/ephysics/collision/shapes/ConvexMeshShape.cpp @@ -4,36 +4,23 @@ * @license BSD 3 clauses (see license file) */ -// Libraries -#include #include #include 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) : ConvexShape(CONVEX_MESH, margin), m_numberVertices(nbVertices), m_minBounds(0, 0, 0), m_maxBounds(0, 0, 0), m_isEdgesInformationUsed(false) { assert(nbVertices > 0); assert(stride > 0); - const unsigned char* vertexPointer = (const unsigned char*) arrayVertices; - // Copy all the vertices int32_to the int32_ternal array for (uint32_t i=0; i 0); - + assert(m_edgesAdjacencyList[maxVertex].size() > 0); // For all neighbors of the current vertex - std::set::const_iterator it; - std::set::const_iterator itBegin = m_edgesAdjacencyList.at(maxVertex).begin(); - std::set::const_iterator itEnd = m_edgesAdjacencyList.at(maxVertex).end(); + etk::Set::Iterator it; + etk::Set::Iterator itBegin = m_edgesAdjacencyList[maxVertex].begin(); + etk::Set::Iterator itEnd = m_edgesAdjacencyList[maxVertex].end(); for (it = itBegin; it != itEnd; ++it) { - // 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 (dotProduct > maxDotProduct) { maxVertex = *it; @@ -127,35 +96,26 @@ vec3 ConvexMeshShape::getLocalSupportPointWithoutMargin(const vec3& direction, isOptimal = false; } } - } while(!isOptimal); - // Cache the support vertex - *((int32_t*)(*cachedCollisionData)) = maxVertex; - + *((int32_t*)(*_cachedCollisionData)) = maxVertex; // Return the support vertex return m_vertices[maxVertex] * m_scaling; - } - else { // If the edges information is not used - - double maxDotProduct = DECIMAL_SMALLEST; + } else { + // If the edges information is not used + double maxDotProduct = FLT_MIN; uint32_t indexMaxDotProduct = 0; - // For each vertex of the mesh for (uint32_t i=0; i maxDotProduct) { indexMaxDotProduct = i; maxDotProduct = dotProduct; } } - assert(maxDotProduct >= 0.0f); - // Return the vertex with the largest dot product in the support direction return m_vertices[indexMaxDotProduct] * m_scaling; } @@ -163,16 +123,12 @@ vec3 ConvexMeshShape::getLocalSupportPointWithoutMargin(const vec3& direction, // Recompute the bounds of the mesh void ConvexMeshShape::recalculateBounds() { - // TODO : Only works if the local origin is inside the mesh // => Make it more robust (init with first vertex of mesh instead) - m_minBounds.setZero(); m_maxBounds.setZero(); - // For each vertex of the mesh for (uint32_t i=0; i m_maxBounds.x()) { m_maxBounds.setX(m_vertices[i].x()); } @@ -185,7 +141,6 @@ void ConvexMeshShape::recalculateBounds() { if (m_vertices[i].y() < m_minBounds.y()) { m_minBounds.setY(m_vertices[i].y()); } - if (m_vertices[i].z() > m_maxBounds.z()) { m_maxBounds.setZ(m_vertices[i].z()); } @@ -193,50 +148,32 @@ void ConvexMeshShape::recalculateBounds() { m_minBounds.setZ(m_vertices[i].z()); } } - // Apply the local scaling factor m_maxBounds = m_maxBounds * m_scaling; m_minBounds = m_minBounds * m_scaling; - // Add the object margin to the bounds m_maxBounds += 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 { 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) { ConvexShape::setLocalScaling(scaling); recalculateBounds(); } -// Return the number of bytes used by the collision shape size_t ConvexMeshShape::getSizeInBytes() const { 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 { min = m_minBounds; 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 { float factor = (1.0f / float(3.0)) * mass; 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)); } -// Add a vertex int32_to the convex mesh -/** - * @param vertex Vertex to be added - */ void ConvexMeshShape::addVertex(const vec3& vertex) { - // Add the vertex in to vertices array m_vertices.pushBack(vertex); m_numberVertices++; - // Update the bounds of the mesh if (vertex.x() * m_scaling.x() > m_maxBounds.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) { - // If the entry for vertex v1 does not exist in the adjacency list if (m_edgesAdjacencyList.count(v1) == 0) { - m_edgesAdjacencyList.insert(etk::makePair(v1, std::set())); + m_edgesAdjacencyList.add(v1, etk::Set()); } - // If the entry for vertex v2 does not exist in the adjacency list if (m_edgesAdjacencyList.count(v2) == 0) { - m_edgesAdjacencyList.insert(etk::makePair(v2, std::set())); + m_edgesAdjacencyList.add(v2, etk::Set()); } - // Add the edge in the adjacency list - m_edgesAdjacencyList[v1].insert(v2); - m_edgesAdjacencyList[v2].insert(v1); + m_edgesAdjacencyList[v1].add(v2); + 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 { 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) { m_isEdgesInformationUsed = isEdgesUsed; } -// Return true if a point is inside the collision shape bool ConvexMeshShape::testPointInside(const vec3& localPoint, ProxyShape* proxyShape) const { - // Use the GJK algorithm to test if the point is inside the convex mesh return proxyShape->m_body->m_world.m_collisionDetection. m_narrowPhaseGJKAlgorithm.testPointInside(localPoint, proxyShape); diff --git a/ephysics/collision/shapes/ConvexMeshShape.hpp b/ephysics/collision/shapes/ConvexMeshShape.hpp index 2a663f1..d8e927a 100644 --- a/ephysics/collision/shapes/ConvexMeshShape.hpp +++ b/ephysics/collision/shapes/ConvexMeshShape.hpp @@ -11,7 +11,6 @@ #include #include #include -#include #include 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_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 - etk::Map > m_edgesAdjacencyList; //!< Adjacency list representing the edges of the mesh + etk::Map > m_edgesAdjacencyList; //!< Adjacency list representing the edges of the mesh /// Private copy-constructor ConvexMeshShape(const ConvexMeshShape& _shape); /// Private assignment operator @@ -51,7 +50,14 @@ namespace ephysics { bool raycast(const Ray& _ray, RaycastInfo& _raycastInfo, ProxyShape* _proxyShape) const override; size_t getSizeInBytes() const override; 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, uint32_t _nbVertices, int32_t _stride, @@ -70,14 +76,30 @@ namespace ephysics { ConvexMeshShape(float _margin = OBJECT_MARGIN); void getLocalBounds(vec3& _min, vec3& _max) 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); - /// 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); - /// 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; - /// 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); }; } diff --git a/ephysics/collision/shapes/ConvexShape.cpp b/ephysics/collision/shapes/ConvexShape.cpp index 731f5b3..0bdc6ef 100644 --- a/ephysics/collision/shapes/ConvexShape.cpp +++ b/ephysics/collision/shapes/ConvexShape.cpp @@ -33,7 +33,7 @@ vec3 ConvexShape::getLocalSupportPointWithMargin(const vec3& direction, // Add the margin to the support point 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(); } supportPoint += unitVec * m_margin; diff --git a/ephysics/collision/shapes/CylinderShape.cpp b/ephysics/collision/shapes/CylinderShape.cpp index 11e30cb..94b8e34 100644 --- a/ephysics/collision/shapes/CylinderShape.cpp +++ b/ephysics/collision/shapes/CylinderShape.cpp @@ -29,7 +29,7 @@ vec3 CylinderShape::getLocalSupportPointWithoutMargin(const vec3& _direction, vo float uDotv = _direction.y(); vec3 w(_direction.x(), 0.0, _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) { supportPoint.setY(-m_halfHeight); } else { @@ -76,7 +76,7 @@ bool CylinderShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape float c = dDotD * k - mDotD * mDotD; // 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 (c > 0.0f) return false; @@ -133,7 +133,7 @@ bool CylinderShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape if (discriminant < 0.0f) return false; // 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 float value = mDotD + t * nDotD; diff --git a/ephysics/collision/shapes/SphereShape.cpp b/ephysics/collision/shapes/SphereShape.cpp index 519f6ee..0c699c0 100644 --- a/ephysics/collision/shapes/SphereShape.cpp +++ b/ephysics/collision/shapes/SphereShape.cpp @@ -8,7 +8,6 @@ #include #include #include -#include using namespace ephysics; @@ -75,10 +74,10 @@ bool SphereShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* float discriminant = b * b - raySquareLength * c; // 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 - float t = -b - std::sqrt(discriminant); + float t = -b - etk::sqrt(discriminant); assert(t >= 0.0f); diff --git a/ephysics/collision/shapes/TriangleShape.cpp b/ephysics/collision/shapes/TriangleShape.cpp index dd3de02..035040c 100644 --- a/ephysics/collision/shapes/TriangleShape.cpp +++ b/ephysics/collision/shapes/TriangleShape.cpp @@ -9,7 +9,6 @@ #include #include #include -#include using namespace ephysics; diff --git a/ephysics/configuration.hpp b/ephysics/configuration.hpp index c68b624..b38f9cf 100644 --- a/ephysics/configuration.hpp +++ b/ephysics/configuration.hpp @@ -6,10 +6,8 @@ #pragma once // Libraries -#include -#include -#include -#include +#include +#include /// Namespace ephysics namespace ephysics { @@ -33,16 +31,6 @@ namespace ephysics { enum ContactsPositionCorrectionTechnique {BAUMGARTE_CONTACTS, SPLIT_IMPULSES}; // ------------------- Constants ------------------- // - - /// Smallest float value (negative) - const float DECIMAL_SMALLEST = - std::numeric_limits::max(); - - /// Maximum float value - const float DECIMAL_LARGEST = std::numeric_limits::max(); - - /// Machine epsilon - const float MACHINE_EPSILON = std::numeric_limits::epsilon(); - /// Pi constant const float PI = float(3.14159265); diff --git a/ephysics/constraint/ContactPoint.hpp b/ephysics/constraint/ContactPoint.hpp index 8141073..aba90a2 100644 --- a/ephysics/constraint/ContactPoint.hpp +++ b/ephysics/constraint/ContactPoint.hpp @@ -47,6 +47,13 @@ namespace ephysics { localPoint2(_localPoint2) { } + ContactPointInfo(): + shape1(nullptr), + shape2(nullptr), + collisionShape1(nullptr), + collisionShape2(nullptr) { + // TODO: add it for etk::Vector + } }; /** diff --git a/ephysics/constraint/HingeJoint.cpp b/ephysics/constraint/HingeJoint.cpp index 10d1386..d0f9f8b 100644 --- a/ephysics/constraint/HingeJoint.cpp +++ b/ephysics/constraint/HingeJoint.cpp @@ -7,7 +7,6 @@ // Libraries #include #include -#include 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 (dotProduct >= 0.0f) { - hingeAngle = float(2.0) * std::atan2(sinHalfAngleAbs, cosHalfAngle); + hingeAngle = float(2.0) * etk::atan2(sinHalfAngleAbs, cosHalfAngle); } 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] diff --git a/ephysics/engine/CollisionWorld.cpp b/ephysics/engine/CollisionWorld.cpp index e58d9c4..d479f73 100644 --- a/ephysics/engine/CollisionWorld.cpp +++ b/ephysics/engine/CollisionWorld.cpp @@ -6,7 +6,6 @@ // Libraries #include -#include // Namespaces using namespace ephysics; @@ -15,7 +14,7 @@ using namespace std; // Constructor CollisionWorld::CollisionWorld() : m_collisionDetection(this, m_memoryAllocator), m_currentBodyID(0), - m_eventListener(NULL) { + m_eventListener(nullptrptr) { } @@ -23,9 +22,9 @@ CollisionWorld::CollisionWorld() CollisionWorld::~CollisionWorld() { // Destroy all the collision bodies that have not been removed - std::set::iterator itBodies; + etk::Set::Iterator itBodies; for (itBodies = m_bodies.begin(); itBodies != m_bodies.end(); ) { - std::set::iterator itToRemove = itBodies; + etk::Set::Iterator itToRemove = itBodies; ++itBodies; destroyCollisionBody(*itToRemove); } @@ -44,13 +43,13 @@ CollisionBody* CollisionWorld::createCollisionBody(const etk::Transform3D& trans bodyindex bodyID = computeNextAvailableBodyID(); // Largest index cannot be used (it is used for invalid index) - assert(bodyID < std::numeric_limits::max()); + EPHYSIC_ASSERT(bodyID < std::numeric_limits::max(), "index too big"); // Create the collision body CollisionBody* collisionBody = new (m_memoryAllocator.allocate(sizeof(CollisionBody))) CollisionBody(transform, *this, bodyID); - assert(collisionBody != NULL); + EPHYSIC_ASSERT(collisionBody != nullptr, "empty Body collision"); // Add the collision body to the world m_bodies.insert(collisionBody); @@ -102,7 +101,7 @@ bodyindex CollisionWorld::computeNextAvailableBodyID() { void CollisionWorld::resetContactManifoldListsOfBodies() { // For each rigid body of the world - for (std::set::iterator it = m_bodies.begin(); it != m_bodies.end(); ++it) { + for (etk::Set::Iterator it = m_bodies.begin(); it != m_bodies.end(); ++it) { // Reset the contact manifold list of the body (*it)->resetContactManifoldsList(); @@ -142,9 +141,9 @@ void CollisionWorld::testCollision(const ProxyShape* shape, resetContactManifoldListsOfBodies(); // Create the sets of shapes - std::set shapes; + etk::Set shapes; shapes.insert(shape->m_broadPhaseID); - std::set emptySet; + etk::Set emptySet; // Perform the collision detection and report contacts m_collisionDetection.testCollisionBetweenShapes(callback, shapes, emptySet); @@ -164,9 +163,9 @@ void CollisionWorld::testCollision(const ProxyShape* shape1, resetContactManifoldListsOfBodies(); // Create the sets of shapes - std::set shapes1; + etk::Set shapes1; shapes1.insert(shape1->m_broadPhaseID); - std::set shapes2; + etk::Set shapes2; shapes2.insert(shape2->m_broadPhaseID); // Perform the collision detection and report contacts @@ -186,15 +185,15 @@ void CollisionWorld::testCollision(const CollisionBody* body, resetContactManifoldListsOfBodies(); // Create the sets of shapes - std::set shapes1; + etk::Set shapes1; // 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()) { shapes1.insert(shape->m_broadPhaseID); } - std::set emptySet; + etk::Set emptySet; // Perform the collision detection and report contacts m_collisionDetection.testCollisionBetweenShapes(callback, shapes1, emptySet); @@ -214,14 +213,14 @@ void CollisionWorld::testCollision(const CollisionBody* body1, resetContactManifoldListsOfBodies(); // Create the sets of shapes - std::set shapes1; - for (const ProxyShape* shape=body1->getProxyShapesList(); shape != NULL; + etk::Set shapes1; + for (const ProxyShape* shape=body1->getProxyShapesList(); shape != nullptr; shape = shape->getNext()) { shapes1.insert(shape->m_broadPhaseID); } - std::set shapes2; - for (const ProxyShape* shape=body2->getProxyShapesList(); shape != NULL; + etk::Set shapes2; + for (const ProxyShape* shape=body2->getProxyShapesList(); shape != nullptr; shape = shape->getNext()) { shapes2.insert(shape->m_broadPhaseID); } @@ -239,7 +238,7 @@ void CollisionWorld::testCollision(CollisionCallback* callback) { // Reset all the contact manifolds lists of each body resetContactManifoldListsOfBodies(); - std::set emptySet; + etk::Set emptySet; // Perform the collision detection and report contacts m_collisionDetection.testCollisionBetweenShapes(callback, emptySet, emptySet); diff --git a/ephysics/engine/CollisionWorld.hpp b/ephysics/engine/CollisionWorld.hpp index ca86205..08683ec 100644 --- a/ephysics/engine/CollisionWorld.hpp +++ b/ephysics/engine/CollisionWorld.hpp @@ -6,9 +6,6 @@ #pragma once #include -#include -#include -#include #include #include #include @@ -30,7 +27,7 @@ namespace ephysics { class CollisionWorld { protected : CollisionDetection m_collisionDetection; //!< Reference to the collision detection - std::set m_bodies; //!< All the bodies (rigid and soft) of the world + etk::Set m_bodies; //!< All the bodies (rigid and soft) of the world bodyindex m_currentBodyID; //!< Current body ID etk::Vector m_freeBodiesIDs; //!< List of free ID for rigid bodies 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 * @return An starting iterator to the set of bodies of the world */ - std::set::iterator getBodiesBeginIterator() { + etk::Set::Iterator getBodiesBeginIterator() { return m_bodies.begin(); } /** * @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 */ - std::set::iterator getBodiesEndIterator() { + etk::Set::Iterator getBodiesEndIterator() { return m_bodies.end(); } /// Create a collision body diff --git a/ephysics/engine/ConstraintSolver.hpp b/ephysics/engine/ConstraintSolver.hpp index df74c08..6fc4e8f 100644 --- a/ephysics/engine/ConstraintSolver.hpp +++ b/ephysics/engine/ConstraintSolver.hpp @@ -10,7 +10,6 @@ #include #include #include -#include namespace ephysics { /** diff --git a/ephysics/engine/ContactSolver.cpp b/ephysics/engine/ContactSolver.cpp index 1199ea3..dabdfb0 100644 --- a/ephysics/engine/ContactSolver.cpp +++ b/ephysics/engine/ContactSolver.cpp @@ -9,7 +9,6 @@ #include #include #include -#include using namespace ephysics; using namespace std; @@ -839,7 +838,7 @@ void ContactSolver::computeFrictionVectors(const vec3& deltaVelocity, // If the velocty difference in the tangential plane is not zero float lengthTangenVelocity = tangentVelocity.length(); - if (lengthTangenVelocity > MACHINE_EPSILON) { + if (lengthTangenVelocity > FLT_EPSILON) { // Compute the first friction vector in the direction of the tangent // velocity difference @@ -869,7 +868,7 @@ void ContactSolver::computeFrictionVectors(const vec3& deltaVelocity, // If the velocty difference in the tangential plane is not zero float lengthTangenVelocity = tangentVelocity.length(); - if (lengthTangenVelocity > MACHINE_EPSILON) { + if (lengthTangenVelocity > FLT_EPSILON) { // Compute the first friction vector in the direction of the tangent // velocity difference diff --git a/ephysics/engine/ContactSolver.hpp b/ephysics/engine/ContactSolver.hpp index d785db8..a833c75 100644 --- a/ephysics/engine/ContactSolver.hpp +++ b/ephysics/engine/ContactSolver.hpp @@ -12,7 +12,6 @@ #include #include #include -#include namespace ephysics { /** diff --git a/ephysics/engine/DynamicsWorld.cpp b/ephysics/engine/DynamicsWorld.cpp index 3a78683..f887cfc 100644 --- a/ephysics/engine/DynamicsWorld.cpp +++ b/ephysics/engine/DynamicsWorld.cpp @@ -41,17 +41,17 @@ DynamicsWorld::DynamicsWorld(const vec3 &gravity) DynamicsWorld::~DynamicsWorld() { // Destroy all the joints that have not been removed - std::set::iterator itJoints; + etk::Set::Iterator itJoints; for (itJoints = m_joints.begin(); itJoints != m_joints.end();) { - std::set::iterator itToRemove = itJoints; + etk::Set::Iterator itToRemove = itJoints; ++itJoints; destroyJoint(*itToRemove); } // Destroy all the rigid bodies that have not been removed - std::set::iterator itRigidBodies; + etk::Set::Iterator itRigidBodies; for (itRigidBodies = m_rigidBodies.begin(); itRigidBodies != m_rigidBodies.end();) { - std::set::iterator itToRemove = itRigidBodies; + etk::Set::Iterator itToRemove = itRigidBodies; ++itRigidBodies; destroyRigidBody(*itToRemove); } @@ -83,9 +83,10 @@ DynamicsWorld::~DynamicsWorld() { assert(m_rigidBodies.size() == 0); #ifdef IS_PROFILING_ACTIVE - // 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) Profiler::destroy(); @@ -262,7 +263,7 @@ void DynamicsWorld::initVelocityArrays() { // Initialize the map of body indexes in the velocity arrays m_mapBodyToConstrainedVelocityIndex.clear(); - std::set::const_iterator it; + etk::Set::Iterator it; uint32_t indexBody = 0; for (it = m_rigidBodies.begin(); it != m_rigidBodies.end(); ++it) { @@ -641,11 +642,11 @@ void DynamicsWorld::computeIslands() { int32_t nbContactManifolds = 0; // Reset all the isAlreadyInIsland variables of bodies, joints and contact manifolds - for (std::set::iterator it = m_rigidBodies.begin(); it != m_rigidBodies.end(); ++it) { + for (etk::Set::Iterator it = m_rigidBodies.begin(); it != m_rigidBodies.end(); ++it) { int32_t nbBodyManifolds = (*it)->resetIsAlreadyInIslandAndCountManifolds(); nbContactManifolds += nbBodyManifolds; } - for (std::set::iterator it = m_joints.begin(); it != m_joints.end(); ++it) { + for (etk::Set::Iterator it = m_joints.begin(); it != m_joints.end(); ++it) { (*it)->m_isAlreadyInIsland = false; } @@ -654,7 +655,7 @@ void DynamicsWorld::computeIslands() { RigidBody** stackBodiesToVisit = (RigidBody**)m_memoryAllocator.allocate(nbBytesStack); // For each rigid body of the world - for (std::set::iterator it = m_rigidBodies.begin(); it != m_rigidBodies.end(); ++it) { + for (etk::Set::Iterator it = m_rigidBodies.begin(); it != m_rigidBodies.end(); ++it) { RigidBody* body = *it; @@ -785,7 +786,7 @@ void DynamicsWorld::updateSleepingBodies() { // For each island of the world for (uint32_t i=0; igetBodies(); @@ -839,7 +840,7 @@ void DynamicsWorld::enableSleeping(bool isSleepingEnabled) { if (!m_isSleepingEnabled) { // For each body of the world - std::set::iterator it; + etk::Set::Iterator it; for (it = m_rigidBodies.begin(); it != m_rigidBodies.end(); ++it) { // Wake up the rigid body @@ -860,9 +861,9 @@ void DynamicsWorld::testCollision(const ProxyShape* shape, CollisionCallback* callback) { // Create the sets of shapes - std::set shapes; + etk::Set shapes; shapes.insert(shape->m_broadPhaseID); - std::set emptySet; + etk::Set emptySet; // Perform the collision detection and report contacts m_collisionDetection.reportCollisionBetweenShapes(callback, shapes, emptySet); @@ -881,9 +882,9 @@ void DynamicsWorld::testCollision(const ProxyShape* shape1, CollisionCallback* callback) { // Create the sets of shapes - std::set shapes1; + etk::Set shapes1; shapes1.insert(shape1->m_broadPhaseID); - std::set shapes2; + etk::Set shapes2; shapes2.insert(shape2->m_broadPhaseID); // Perform the collision detection and report contacts @@ -902,7 +903,7 @@ void DynamicsWorld::testCollision(const CollisionBody* body, CollisionCallback* callback) { // Create the sets of shapes - std::set shapes1; + etk::Set shapes1; // For each shape of the body for (const ProxyShape* shape=body->getProxyShapesList(); shape != nullptr; @@ -910,7 +911,7 @@ void DynamicsWorld::testCollision(const CollisionBody* body, shapes1.insert(shape->m_broadPhaseID); } - std::set emptySet; + etk::Set emptySet; // Perform the collision detection and report contacts m_collisionDetection.reportCollisionBetweenShapes(callback, shapes1, emptySet); @@ -929,13 +930,13 @@ void DynamicsWorld::testCollision(const CollisionBody* body1, CollisionCallback* callback) { // Create the sets of shapes - std::set shapes1; + etk::Set shapes1; for (const ProxyShape* shape=body1->getProxyShapesList(); shape != nullptr; shape = shape->getNext()) { shapes1.insert(shape->m_broadPhaseID); } - std::set shapes2; + etk::Set shapes2; for (const ProxyShape* shape=body2->getProxyShapesList(); shape != nullptr; shape = shape->getNext()) { shapes2.insert(shape->m_broadPhaseID); @@ -953,7 +954,7 @@ void DynamicsWorld::testCollision(const CollisionBody* body1, */ void DynamicsWorld::testCollision(CollisionCallback* callback) { - std::set emptySet; + etk::Set emptySet; // Perform the collision detection and report contacts m_collisionDetection.reportCollisionBetweenShapes(callback, emptySet, emptySet); @@ -965,7 +966,7 @@ etk::Vector DynamicsWorld::getContactsList() const { etk::Vector contactManifolds; // For each currently overlapping pair of bodies - etk::Map::const_iterator it; + etk::Map::Iterator it; for (it = m_collisionDetection.m_overlappingPairs.begin(); it != m_collisionDetection.m_overlappingPairs.end(); ++it) { @@ -990,7 +991,7 @@ etk::Vector DynamicsWorld::getContactsList() const { void DynamicsWorld::resetBodiesForceAndTorque() { // For each body of the world - std::set::iterator it; + etk::Set::Iterator it; for (it = m_rigidBodies.begin(); it != m_rigidBodies.end(); ++it) { (*it)->m_externalForce.setZero(); (*it)->m_externalTorque.setZero(); @@ -1114,7 +1115,7 @@ uint32_t DynamicsWorld::getNbJoints() const { /** * @return Starting iterator of the set of rigid bodies */ -std::set::iterator DynamicsWorld::getRigidBodiesBeginIterator() { +etk::Set::Iterator DynamicsWorld::getRigidBodiesBeginIterator() { return m_rigidBodies.begin(); } @@ -1122,7 +1123,7 @@ std::set::iterator DynamicsWorld::getRigidBodiesBeginIterator() { /** * @return Ending iterator of the set of rigid bodies */ -std::set::iterator DynamicsWorld::getRigidBodiesEndIterator() { +etk::Set::Iterator DynamicsWorld::getRigidBodiesEndIterator() { return m_rigidBodies.end(); } diff --git a/ephysics/engine/DynamicsWorld.hpp b/ephysics/engine/DynamicsWorld.hpp index eba16e2..4b3a4dd 100644 --- a/ephysics/engine/DynamicsWorld.hpp +++ b/ephysics/engine/DynamicsWorld.hpp @@ -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_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 - std::set m_rigidBodies; //!< All the rigid bodies of the physics world - std::set m_joints; //!< All the joints of the world + etk::Set m_rigidBodies; //!< All the rigid bodies of the physics world + etk::Set m_joints; //!< All the joints of the world vec3 m_gravity; //!< Gravity vector of the world float m_timeStep; //!< Current frame time step (in seconds) bool m_isGravityEnabled; //!< True if the gravity force is on @@ -121,9 +121,9 @@ namespace ephysics { /// Return the number of joints in the world uint32_t getNbJoints() const; /// Return an iterator to the beginning of the rigid bodies of the physics world - std::set::iterator getRigidBodiesBeginIterator(); + etk::Set::Iterator getRigidBodiesBeginIterator(); /// Return an iterator to the end of the rigid bodies of the physics world - std::set::iterator getRigidBodiesEndIterator(); + etk::Set::Iterator getRigidBodiesEndIterator(); /// Return true if the sleeping technique is enabled bool isSleepingEnabled() const; /// Enable/Disable the sleeping technique diff --git a/ephysics/engine/Material.hpp b/ephysics/engine/Material.hpp index 176c19e..5566bd0 100644 --- a/ephysics/engine/Material.hpp +++ b/ephysics/engine/Material.hpp @@ -4,7 +4,9 @@ * @license BSD 3 clauses (see license file) */ #pragma once -#include +extern "C" { + #include +} #include namespace ephysics { diff --git a/ephysics/engine/Profiler.cpp b/ephysics/engine/Profiler.cpp index 4f1495e..5e0ac18 100644 --- a/ephysics/engine/Profiler.cpp +++ b/ephysics/engine/Profiler.cpp @@ -8,11 +8,12 @@ // Libraries #include +#include using namespace ephysics; // Initialization of static variables -ProfileNode Profiler::m_rootNode("Root", NULL); +ProfileNode Profiler::m_rootNode("Root", nullptr); ProfileNode* Profiler::m_currentNode = &Profiler::m_rootNode; long double Profiler::m_profilingStartTime = Timer::getCurrentSystemTime() * 1000.0; uint32_t Profiler::m_frameCounter = 0; @@ -20,8 +21,8 @@ uint32_t Profiler::m_frameCounter = 0; // Constructor ProfileNode::ProfileNode(const char* name, ProfileNode* parentNode) :m_name(name), m_numberTotalCalls(0), m_startTime(0), m_totalTime(0), - m_recursionCounter(0), m_parentNode(parentNode), m_childNode(NULL), - m_siblingNode(NULL) { + m_recursionCounter(0), m_parentNode(parentNode), m_childNode(nullptr), + m_siblingNode(nullptr) { reset(); } @@ -37,7 +38,7 @@ ProfileNode* ProfileNode::findSubNode(const char* name) { // Try to find the node among the child nodes ProfileNode* child = m_childNode; - while (child != NULL) { + while (child != nullptr) { if (child->m_name == name) { return child; } @@ -91,12 +92,12 @@ void ProfileNode::reset() { m_totalTime = 0.0; // Reset the child node - if (m_childNode != NULL) { + if (m_childNode != nullptr) { m_childNode->reset(); } // Reset the sibling node - if (m_siblingNode != NULL) { + if (m_siblingNode != nullptr) { m_siblingNode->reset(); } } @@ -104,9 +105,9 @@ void ProfileNode::reset() { // Destroy the node void ProfileNode::destroy() { delete m_childNode; - m_childNode = NULL; + m_childNode = nullptr; delete m_siblingNode; - m_siblingNode = NULL; + m_siblingNode = nullptr; } // Constructor @@ -119,12 +120,12 @@ ProfileNodeIterator::ProfileNodeIterator(ProfileNode* startingNode) // Enter a given child node void ProfileNodeIterator::enterChild(int32_t index) { m_currentChildNode = m_currentParentNode->getChildNode(); - while ((m_currentChildNode != NULL) && (index != 0)) { + while ((m_currentChildNode != nullptr) && (index != 0)) { index--; m_currentChildNode = m_currentChildNode->getSiblingNode(); } - if (m_currentChildNode != NULL) { + if (m_currentChildNode != nullptr) { m_currentParentNode = m_currentChildNode; m_currentChildNode = m_currentParentNode->getChildNode(); } @@ -132,7 +133,7 @@ void ProfileNodeIterator::enterChild(int32_t index) { // Enter a given parent node void ProfileNodeIterator::enterParent() { - if (m_currentParentNode->getParentNode() != NULL) { + if (m_currentParentNode->getParentNode() != nullptr) { m_currentParentNode = m_currentParentNode->getParentNode(); } m_currentChildNode = m_currentParentNode->getChildNode(); @@ -171,11 +172,11 @@ void Profiler::reset() { } // 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(); // 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 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 void Profiler::print32_tRecursiveNodeReport(ProfileNodeIterator* iterator, - int32_t spacing, - etk::Stream& outputStream) { + int32_t spacing, + etk::Stream& _stream) { iterator->first(); // 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(); long double accumulatedTime = 0.0; uint32_t nbFrames = Profiler::getNbFrames(); - for (int32_t i=0; igetCurrentParentName() << - " (total running time : " << parentTime << " ms) ---" << std::endl; + for (int32_t i=0; igetCurrentParentName() << + " (total running time : " << parentTime << " ms) ---\n"; long double totalTime = 0.0; // Recurse over the children of the current node @@ -209,28 +212,27 @@ void Profiler::print32_tRecursiveNodeReport(ProfileNodeIterator* iterator, nbChildren++; long double currentTotalTime = iterator->getCurrentTotalTime(); accumulatedTime += currentTotalTime; - long double fraction = parentTime > std::numeric_limits::epsilon() ? + long double fraction = parentTime > DLB_EPSILON ? (currentTotalTime / parentTime) * 100.0 : 0.0; - for (int32_t j=0; jgetCurrentName() << " : " << + for (int32_t j=0; jgetCurrentName() << " : " << fraction << " % | " << (currentTotalTime / (long double) (nbFrames)) << - " ms/frame (" << iterator->getCurrentNbTotalCalls() << " calls)" << - std::endl; + " ms/frame (" << iterator->getCurrentNbTotalCalls() << " calls)\n"; totalTime += currentTotalTime; } if (parentTime < accumulatedTime) { - outputStream << "Something is wrong !" << std::endl; + _stream << "Something is wrong !\n"; } - for (int32_t i=0; i std::numeric_limits::epsilon() ? + for (int32_t i=0; i DLB_EPSILON ? ((parentTime - accumulatedTime) / parentTime) * 100.0 : 0.0; long double difference = parentTime - accumulatedTime; - outputStream << "| Unaccounted : " << difference << " ms (" << percentage << " %)" << std::endl; + _stream << "| Unaccounted : " << difference << " ms (" << percentage << " %)\n"; for (int32_t i=0; ienterChild(i); - print32_tRecursiveNodeReport(iterator, spacing + 3, outputStream); + print32_tRecursiveNodeReport(iterator, spacing + 3, _stream); iterator->enterParent(); } } diff --git a/ephysics/engine/Timer.hpp b/ephysics/engine/Timer.hpp index 18eb3db..8a3b70d 100644 --- a/ephysics/engine/Timer.hpp +++ b/ephysics/engine/Timer.hpp @@ -6,18 +6,8 @@ #pragma once // Libraries -#include -#include -#include -#include #include - -#if defined(__TARGET_OS__Windows) - #define NOMINMAX // This is used to avoid definition of max() and min() macros - #include -#else // For Mac OS or Linux platform - #include -#endif +#include namespace ephysics { diff --git a/ephysics/mathematics/mathematics.hpp b/ephysics/mathematics/mathematics.hpp index 25df364..366db0b 100644 --- a/ephysics/mathematics/mathematics.hpp +++ b/ephysics/mathematics/mathematics.hpp @@ -16,6 +16,4 @@ #include #include #include -#include -#include -#include + diff --git a/ephysics/mathematics/mathematics_functions.hpp b/ephysics/mathematics/mathematics_functions.hpp index 77891fe..33029c6 100644 --- a/ephysics/mathematics/mathematics_functions.hpp +++ b/ephysics/mathematics/mathematics_functions.hpp @@ -9,10 +9,6 @@ #include #include -#include -#include -#include - /// ReactPhysics3D namespace namespace ephysics { @@ -20,8 +16,8 @@ namespace ephysics { /// 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] -inline bool approxEqual(float a, float b, float epsilon = MACHINE_EPSILON) { - return (std::fabs(a - b) < epsilon); +inline bool approxEqual(float a, float b, float epsilon = FLT_EPSILON) { + return (etk::abs(a - b) < epsilon); } /// Function that returns the result of the "value" clamped by diff --git a/ephysics/memory/MemoryAllocator.cpp b/ephysics/memory/MemoryAllocator.cpp index 3aa539d..a1d149e 100644 --- a/ephysics/memory/MemoryAllocator.cpp +++ b/ephysics/memory/MemoryAllocator.cpp @@ -6,8 +6,6 @@ // Libraries #include -#include -#include using namespace ephysics; diff --git a/ephysics/memory/MemoryAllocator.hpp b/ephysics/memory/MemoryAllocator.hpp index 5fc6e54..b65646b 100644 --- a/ephysics/memory/MemoryAllocator.hpp +++ b/ephysics/memory/MemoryAllocator.hpp @@ -6,7 +6,6 @@ #pragma once // Libraries -#include #include /// ReactPhysics3D namespace diff --git a/lutin_ephysics-test.py b/lutin_ephysics-test.py index 4019294..837f139 100644 --- a/lutin_ephysics-test.py +++ b/lutin_ephysics-test.py @@ -32,7 +32,7 @@ def configure(target, my_module): ]) my_module.add_depend([ 'ephysics', - 'gtest', + 'etest', 'test-debug' ]) my_module.add_path(".") diff --git a/test/Test.hpp b/test/Test.hpp index 9cdcd09..2947f91 100644 --- a/test/Test.hpp +++ b/test/Test.hpp @@ -28,8 +28,6 @@ // Libraries #include -#include -#include /// Reactphysics3D namespace namespace ephysics { diff --git a/test/TestSuite.hpp b/test/TestSuite.hpp index 25dcf98..5e117d6 100644 --- a/test/TestSuite.hpp +++ b/test/TestSuite.hpp @@ -29,7 +29,6 @@ // Libraries #include #include -#include /// Reactphysics3D namespace namespace ephysics { diff --git a/test/tests/mathematics/TestQuaternion.h b/test/tests/mathematics/TestQuaternion.h index 365f50c..a388fac 100644 --- a/test/tests/mathematics/TestQuaternion.h +++ b/test/tests/mathematics/TestQuaternion.h @@ -95,7 +95,7 @@ class Testetk::Quaternion : public Test { const float PI_OVER_2 = PI * 0.5f; const float PI_OVER_4 = PI_OVER_2 * 0.5f; 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(); test(approxEqual(quaternion5.x(), quaternionTest5.x)); test(approxEqual(quaternion5.y(), quaternionTest5.y)); @@ -103,7 +103,7 @@ class Testetk::Quaternion : public Test { test(approxEqual(quaternion5.w, quaternionTest5.w)); 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(); test(approxEqual(quaternion6.x(), quaternionTest6.x)); test(approxEqual(quaternion6.y(), quaternionTest6.y)); @@ -111,7 +111,7 @@ class Testetk::Quaternion : public Test { test(approxEqual(quaternion6.w, quaternionTest6.w)); 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(); test(approxEqual(quaternion7.x(), quaternionTest7.x)); test(approxEqual(quaternion7.y(), quaternionTest7.y)); diff --git a/tools/testbed/scenes/raycast/RaycastScene.cpp b/tools/testbed/scenes/raycast/RaycastScene.cpp index 3dbebd0..1cb5555 100644 --- a/tools/testbed/scenes/raycast/RaycastScene.cpp +++ b/tools/testbed/scenes/raycast/RaycastScene.cpp @@ -163,9 +163,9 @@ void RaycastScene::createLines() { float phi = j * PI / float(nbRaysOneDimension); // Generate a point on a sphere with spherical coordinates - float x = RAY_LENGTH * std::sin(phi) * std::cos(theta); - float y = RAY_LENGTH * std::sin(phi) * std::sin(theta); - float z = RAY_LENGTH * std::cos(phi); + float x = RAY_LENGTH * etk::sin(phi) * etk::cos(theta); + float y = RAY_LENGTH * etk::sin(phi) * etk::sin(theta); + float z = RAY_LENGTH * etk::cos(phi); // Create a line from the point on the sphere to the center of // the scene