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

[DEV] continue integration new ETK

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

5
ephysics/body/Body.hpp
View File

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

2
ephysics/body/CollisionBody.hpp
View File

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

1
ephysics/body/RigidBody.hpp
View File

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

315
ephysics/collision/CollisionDetection.cpp
View File

@ -11,106 +11,85 @@
#include <ephysics/collision/shapes/BoxShape.hpp>
#include <ephysics/body/RigidBody.hpp>
#include <ephysics/configuration.hpp>
#include <cassert>
#include <complex>
#include <set>
#include <utility>
#include <utility>
// 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<uint32_t>& shapes1,
const std::set<uint32_t>& shapes2) {
const etk::Set<uint32_t>& shapes1,
const etk::Set<uint32_t>& 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<uint32_t>& shapes1,
const std::set<uint32_t>& shapes2) {
const etk::Set<uint32_t>& shapes1,
const etk::Set<uint32_t>& shapes2) {
// For each possible collision pair of bodies
map<overlappingpairid, OverlappingPair*>::iterator it;
etk::Map<overlappingpairid, OverlappingPair*>::Iterator it;
for (it = m_overlappingPairs.begin(); it != m_overlappingPairs.end(); ++it) {
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; j<manifoldSet.getNbContactManifolds(); j++) {
const ContactManifold* manifold = manifoldSet.getContactManifold(j);
// For each contact manifold of the manifold set
for (uint32_t i=0; i<manifold->getNbContactPoints(); 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<overlappingpairid, OverlappingPair*>::iterator it;
etk::Map<overlappingpairid, OverlappingPair*>::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<overlappingpairid, OverlappingPair*>::iterator itToRemove = it;
etk::Map<overlappingpairid, OverlappingPair*>::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<uint32_t>& shapes1,
const std::set<uint32_t>& shapes2) {
const etk::Set<uint32_t>& shapes1,
const etk::Set<uint32_t>& shapes2) {
m_contactOverlappingPairs.clear();
// For each possible collision pair of bodies
map<overlappingpairid, OverlappingPair*>::iterator it;
etk::Map<overlappingpairid, OverlappingPair*>::Iterator it;
for (it = m_overlappingPairs.begin(); it != m_overlappingPairs.end(); ) {
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<overlappingpairid, OverlappingPair*>::iterator itToRemove = it;
etk::Map<overlappingpairid, OverlappingPair*>::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<map<overlappingpairid, OverlappingPair*>::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<overlappingpairid, OverlappingPair*>::iterator it;
etk::Map<overlappingpairid, OverlappingPair*>::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<overlappingpairid, OverlappingPair*>::iterator itToRemove = it;
etk::Map<overlappingpairid, OverlappingPair*>::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<overlappingpairid, OverlappingPair*>::iterator it;
etk::Map<overlappingpairid, OverlappingPair*>::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; i<manifoldSet.getNbContactManifolds(); i++) {
ContactManifold* contactManifold = manifoldSet.getContactManifold(i);
assert(contactManifold->getNbContactPoints() > 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<overlappingpairid, OverlappingPair*>::iterator it;
etk::Map<overlappingpairid, OverlappingPair*>::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; i<NB_COLLISION_SHAPE_TYPES; i++) {
for (int32_t j=0; j<NB_COLLISION_SHAPE_TYPES; j++) {
@ -496,99 +407,73 @@ void CollisionDetection::fillInCollisionMatrix() {
}
}
// Return the world event listener
EventListener* CollisionDetection::getWorldEventListener() {
return m_world->m_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;
}

22
ephysics/collision/CollisionDetection.hpp
View File

@ -14,8 +14,7 @@
#include <ephysics/constraint/ContactPoint.hpp>
#include <etk/Vector.hpp>
#include <etk/Map.hpp>
#include <set>
#include <utility>
#include <etk/Set.hpp>
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<bodyindexpair> m_noCollisionPairs; //!< Set of pair of bodies that cannot collide between each other
etk::Set<bodyindexpair> m_noCollisionPairs; //!< Set of pair of bodies that cannot collide between each other
bool m_isCollisionShapesAdded; //!< True if some collision shapes have been added previously
/// 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<uint32_t>& _shapes1,
const std::set<uint32_t>& _shapes2);
const etk::Set<uint32_t>& _shapes1,
const etk::Set<uint32_t>& _shapes2);
/// Report collision between two sets of shapes
void reportCollisionBetweenShapes(CollisionCallback* _callback,
const std::set<uint32_t>& _shapes1,
const std::set<uint32_t>& _shapes2) ;
const etk::Set<uint32_t>& _shapes1,
const etk::Set<uint32_t>& _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<uint32_t>& _shapes1,
const std::set<uint32_t>& _shapes2);
const etk::Set<uint32_t>& _shapes1,
const etk::Set<uint32_t>& _shapes2);
/// Return a pointer to the world
CollisionWorld* getWorld();
/// Return the world event listener

1
ephysics/collision/ContactManifold.cpp
View File

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

79
ephysics/collision/ContactManifoldSet.cpp
View File

@ -4,12 +4,10 @@
* @license BSD 3 clauses (see license file)
*/
// Libraries
#include <ephysics/collision/ContactManifoldSet.hpp>
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; i<m_nbManifolds; i++) {
assert(m_manifolds[i]->getNbContactPoints() > 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; i<m_nbManifolds; i++) {
assert(m_manifolds[i]->getNbContactPoints() > 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; i<m_nbManifolds; i++) {
assert(m_manifolds[i]->getNbContactPoints() > 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; i<m_nbManifolds; i++) {
if (normalDirectionId == m_manifolds[i]->getNormalDirectionId()) {
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<m_nbManifolds; i++) {

1
ephysics/collision/TriangleMesh.hpp
View File

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

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

@ -4,50 +4,42 @@
* @license BSD 3 clauses (see license file)
*/
// Libraries
#include <ephysics/collision/shapes/ConcaveShape.hpp>
#include <ephysics/collision/shapes/TriangleShape.hpp>
#include <ephysics/collision/narrowphase/ConcaveVsConvexAlgorithm.hpp>
#include <ephysics/collision/CollisionDetection.hpp>
#include <ephysics/engine/CollisionWorld.hpp>
#include <algorithm>
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<const ConvexShape*>(shape1Info.collisionShape);
concaveProxyShape = shape2Info.proxyShape;
concaveShape = static_cast<const ConcaveShape*>(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<const ConvexShape*>(shape2Info.collisionShape);
concaveProxyShape = shape1Info.proxyShape;
concaveShape = static_cast<const ConcaveShape*>(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<SmoothMeshContactInfo> 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<SmoothMeshContactInfo> contactPoints,
NarrowPhaseCallback* narrowPhaseCallback) {
// Set with the triangle vertices already processed to void further contacts with same triangle
std::unordered_multimap<int32_t, vec3> processTriangleVertices;
etk::Vector<etk::Pair<int32_t, vec3>> processTriangleVertices;
// Sort the list of narrow-phase contacts according to their penetration depth
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<SmoothMeshContactInfo>::const_iterator it;
etk::Vector<SmoothMeshContactInfo>::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<int32_t, vec3>& 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<etk::Pair<int32_t, vec3>>& _processTriangleVertices, const vec3& _vertex) const {
/* TODO : etk::Vector<etk::Pair<int32_t, vec3>> was an unordered map ... ==> stupid idee... I replace code because I do not have enouth time to do something good...
int32_t key = int32_t(_vertex.x() * _vertex.y() * _vertex.z());
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<const TriangleShape*>(contactInfo.collisionShape1);
if (_contactInfo.collisionShape1->getType() == TRIANGLE) {
assert(_contactInfo.collisionShape2->getType() != TRIANGLE);
const TriangleShape* triangleShape = static_cast<const TriangleShape*>(_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<const TriangleShape*>(contactInfo.collisionShape2);
} else { // If the collision shape 2 is the triangle
assert(_contactInfo.collisionShape2->getType() == TRIANGLE);
const TriangleShape* triangleShape = static_cast<const TriangleShape*>(_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);

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

@ -8,7 +8,6 @@
#include <ephysics/collision/narrowphase/NarrowPhaseAlgorithm.hpp>
#include <ephysics/collision/shapes/ConvexShape.hpp>
#include <ephysics/collision/shapes/ConcaveShape.hpp>
#include <unordered_map>
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<SmoothMeshContactInfo> _contactPoints,
NarrowPhaseCallback* _narrowPhaseCallback);
/// Add a triangle vertex int32_to the set of processed triangles
void addProcessedVertex(std::unordered_multimap<int32_t, vec3>& _processTriangleVertices, const vec3& _vertex) {
_processTriangleVertices.insert(etk::makePair(int32_t(_vertex.x() * _vertex.y() * _vertex.z()), _vertex));
void addProcessedVertex(etk::Vector<etk::Pair<int32_t, vec3>>& _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<int32_t, vec3>& _processTriangleVertices,
bool hasVertexBeenProcessed(const etk::Vector<etk::Pair<int32_t, vec3>>& _processTriangleVertices,
const vec3& _vertex) const;
public :
/// Constructor

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

@ -3,66 +3,46 @@
* @copyright 2010-2016 Daniel Chappuis
* @license BSD 3 clauses (see license file)
*/
// Libraries
#include <ephysics/collision/narrowphase/EPA/EPAAlgorithm.hpp>
#include <ephysics/engine/Profiler.hpp>
#include <ephysics/collision/narrowphase/GJK/GJKAlgorithm.hpp>
#include <ephysics/collision/narrowphase/EPA/TrianglesStore.hpp>
// 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<const ConvexShape*>(shape1Info.collisionShape);
const ConvexShape* shape2 = static_cast<const ConvexShape*>(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);
}

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

@ -4,8 +4,6 @@
* @license BSD 3 clauses (see license file)
*/
#pragma once
// Libraries
#include <ephysics/collision/narrowphase/GJK/Simplex.hpp>
#include <ephysics/collision/shapes/CollisionShape.hpp>
#include <ephysics/collision/CollisionShapeInfo.hpp>
@ -16,123 +14,92 @@
#include <ephysics/memory/MemoryAllocator.hpp>
#include <algorithm>
/// 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);
};
}

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

@ -3,115 +3,89 @@
* @copyright 2010-2016 Daniel Chappuis
* @license BSD 3 clauses (see license file)
*/
// Libraries
#include <ephysics/collision/narrowphase/EPA/EdgeEPA.hpp>
#include <ephysics/collision/narrowphase/EPA/TriangleEPA.hpp>
#include <ephysics/collision/narrowphase/EPA/TrianglesStore.hpp>
#include <cassert>
// 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;
}

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

@ -4,95 +4,61 @@
* @license BSD 3 clauses (see license file)
*/
#pragma once
// Libraries
#include <ephysics/mathematics/mathematics.hpp>
/// 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;
}
}

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

@ -4,132 +4,97 @@
* @license BSD 3 clauses (see license file)
*/
// Libraries
#include <ephysics/collision/narrowphase/EPA/TriangleEPA.hpp>
#include <ephysics/collision/narrowphase/EPA/EdgeEPA.hpp>
#include <ephysics/collision/narrowphase/EPA/TrianglesStore.hpp>
// 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;
}

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

@ -4,173 +4,108 @@
* @license BSD 3 clauses (see license file)
*/
#pragma once
// Libraries
#include <ephysics/mathematics/mathematics.hpp>
#include <ephysics/configuration.hpp>
#include <ephysics/collision/narrowphase/EPA/EdgeEPA.hpp>
#include <cassert>
/// 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);
};
}

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

@ -4,18 +4,12 @@
* @license BSD 3 clauses (see license file)
*/
// Libraries
#include <ephysics/collision/narrowphase/EPA/TrianglesStore.hpp>
// 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() {
}

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

@ -4,116 +4,61 @@
* @license BSD 3 clauses (see license file)
*/
#pragma once
#include <ephysics/collision/narrowphase/EPA/TriangleEPA.hpp>
// Libraries
#include <cassert>
/// 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];
}
};
}

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

@ -3,158 +3,107 @@
* @copyright 2010-2016 Daniel Chappuis
* @license BSD 3 clauses (see license file)
*/
// Libraries
#include <ephysics/collision/narrowphase/GJK/GJKAlgorithm.hpp>
#include <ephysics/collision/narrowphase/GJK/Simplex.hpp>
#include <ephysics/constraint/ContactPoint.hpp>
#include <ephysics/configuration.hpp>
#include <ephysics/engine/Profiler.hpp>
#include <algorithm>
#include <cmath>
#include <cfloat>
#include <cassert>
// 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<const ConvexShape*>(shape1Info.collisionShape);
const ConvexShape* shape2 = static_cast<const ConvexShape*>(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<const ConvexShape*>(shape1Info.collisionShape);
const ConvexShape* shape2 = static_cast<const ConvexShape*>(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<const ConvexShape*>(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<const ConvexShape*>(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;
}

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

@ -5,94 +5,76 @@
*/
#pragma once
// Libraries
#include <ephysics/collision/narrowphase/NarrowPhaseAlgorithm.hpp>
#include <ephysics/constraint/ContactPoint.hpp>
#include <ephysics/collision/shapes/ConvexShape.hpp>
#include <ephysics/collision/narrowphase/EPA/EPAAlgorithm.hpp>
/// 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);
};
}

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

@ -6,7 +6,6 @@
// Libraries
#include <ephysics/collision/narrowphase/GJK/Simplex.hpp>
#include <cfloat>
// 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--) {

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

@ -32,7 +32,7 @@ void ephysics::SphereVsSphereAlgorithm::testCollision(const ephysics::CollisionS
vec3 centerSphere1InBody2LocalSpace = transform2.getInverse() * transform1.getPosition();
vec3 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,

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

@ -8,7 +8,6 @@
// Libraries
#include <ephysics/collision/shapes/AABB.hpp>
#include <ephysics/configuration.hpp>
#include <cassert>
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) {

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

@ -9,7 +9,6 @@
#include <ephysics/collision/ProxyShape.hpp>
#include <ephysics/configuration.hpp>
#include <etk/Vector.hpp>
#include <cassert>
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;
}

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

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

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

@ -8,7 +8,6 @@
#include <ephysics/collision/shapes/CapsuleShape.hpp>
#include <ephysics/collision/ProxyShape.hpp>
#include <ephysics/configuration.hpp>
#include <cassert>
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);

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

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

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

@ -6,7 +6,6 @@
#include <ephysics/collision/shapes/ConcaveMeshShape.hpp>
#include <ephysics/debug.hpp>
#include <iostream>
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<int32_t>::const_iterator it;
etk::Vector<int32_t>::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)

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

@ -5,7 +5,6 @@
*/
// Libraries
#include <complex>
#include <ephysics/configuration.hpp>
#include <ephysics/collision/shapes/ConeShape.hpp>
#include <ephysics/collision/ProxyShape.hpp>
@ -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);
}

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

@ -4,36 +4,23 @@
* @license BSD 3 clauses (see license file)
*/
// Libraries
#include <complex>
#include <ephysics/configuration.hpp>
#include <ephysics/collision/shapes/ConvexMeshShape.hpp>
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<m_numberVertices; i++) {
const float* newPoint = (const float*) vertexPointer;
m_vertices.pushBack(vec3(newPoint[0], newPoint[1], newPoint[2]));
vertexPointer += stride;
}
// Recalculate the bounds of the mesh
recalculateBounds();
}
@ -74,52 +61,34 @@ ConvexMeshShape::ConvexMeshShape(float _margin):
m_minBounds(0, 0, 0),
m_maxBounds(0, 0, 0),
m_isEdgesInformationUsed(false) {
}
// Return a local support point in a given direction without the object margin.
/// If the edges information is not used for collision detection, this method will go through
/// the whole vertices list and pick up the vertex with the largest dot product in the support
/// direction. This is an O(n) process with "n" being the number of vertices in the mesh.
/// However, if the edges information is used, we can cache the previous support vertex and use
/// it as a start in a hill-climbing (local search) process to find the new support vertex which
/// will be in most of the cases very close to the previous one. Using hill-climbing, this method
/// runs in almost constant time.
vec3 ConvexMeshShape::getLocalSupportPointWithoutMargin(const vec3& direction,
void** cachedCollisionData) const {
vec3 ConvexMeshShape::getLocalSupportPointWithoutMargin(const vec3& _direction, void** _cachedCollisionData) const {
assert(m_numberVertices == m_vertices.size());
assert(cachedCollisionData != NULL);
assert(_cachedCollisionData != nullptr);
// Allocate memory for the cached collision data if not allocated yet
if ((*cachedCollisionData) == NULL) {
*cachedCollisionData = (int32_t*) malloc(sizeof(int32_t));
*((int32_t*)(*cachedCollisionData)) = 0;
if ((*_cachedCollisionData) == nullptr) {
*_cachedCollisionData = (int32_t*) malloc(sizeof(int32_t));
*((int32_t*)(*_cachedCollisionData)) = 0;
}
// If the edges information is used to speed up the collision detection
if (m_isEdgesInformationUsed) {
assert(m_edgesAdjacencyList.size() == m_numberVertices);
uint32_t maxVertex = *((int32_t*)(*cachedCollisionData));
float maxDotProduct = direction.dot(m_vertices[maxVertex]);
uint32_t maxVertex = *((int32_t*)(*_cachedCollisionData));
float maxDotProduct = _direction.dot(m_vertices[maxVertex]);
bool isOptimal;
// Perform hill-climbing (local search)
do {
isOptimal = true;
assert(m_edgesAdjacencyList.at(maxVertex).size() > 0);
assert(m_edgesAdjacencyList[maxVertex].size() > 0);
// For all neighbors of the current vertex
std::set<uint32_t>::const_iterator it;
std::set<uint32_t>::const_iterator itBegin = m_edgesAdjacencyList.at(maxVertex).begin();
std::set<uint32_t>::const_iterator itEnd = m_edgesAdjacencyList.at(maxVertex).end();
etk::Set<uint32_t>::Iterator it;
etk::Set<uint32_t>::Iterator itBegin = m_edgesAdjacencyList[maxVertex].begin();
etk::Set<uint32_t>::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<m_numberVertices; i++) {
// Compute the dot product of the current vertex
double dotProduct = direction.dot(m_vertices[i]);
double dotProduct = _direction.dot(m_vertices[i]);
// If the current dot product is larger than the maximum one
if (dotProduct > 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_numberVertices; i++) {
if (m_vertices[i].x() > 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<uint32_t>()));
m_edgesAdjacencyList.add(v1, etk::Set<uint32_t>());
}
// 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<uint32_t>()));
m_edgesAdjacencyList.add(v2, etk::Set<uint32_t>());
}
// 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);

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

@ -11,7 +11,6 @@
#include <ephysics/collision/TriangleMesh.hpp>
#include <ephysics/collision/narrowphase/GJK/GJKAlgorithm.hpp>
#include <etk/Vector.hpp>
#include <set>
#include <etk/Map.hpp>
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<uint32_t, std::set<uint32_t> > m_edgesAdjacencyList; //!< Adjacency list representing the edges of the mesh
etk::Map<uint32_t, etk::Set<uint32_t> > m_edgesAdjacencyList; //!< Adjacency list representing the edges of the mesh
/// Private copy-constructor
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);
};
}

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

@ -33,7 +33,7 @@ vec3 ConvexShape::getLocalSupportPointWithMargin(const vec3& direction,
// Add the margin to the support point
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;

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

@ -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;

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

@ -8,7 +8,6 @@
#include <ephysics/collision/shapes/SphereShape.hpp>
#include <ephysics/collision/ProxyShape.hpp>
#include <ephysics/configuration.hpp>
#include <cassert>
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);

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

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

16
ephysics/configuration.hpp
View File

@ -6,10 +6,8 @@
#pragma once
// Libraries
#include <limits>
#include <cfloat>
#include <utility>
#include <cstdint>
#include <etk/types.hpp>
#include <etk/Pair.hpp>
/// 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<float>::max();
/// Maximum float value
const float DECIMAL_LARGEST = std::numeric_limits<float>::max();
/// Machine epsilon
const float MACHINE_EPSILON = std::numeric_limits<float>::epsilon();
/// Pi constant
const float PI = float(3.14159265);

7
ephysics/constraint/ContactPoint.hpp
View File

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

5
ephysics/constraint/HingeJoint.cpp
View File

@ -7,7 +7,6 @@
// Libraries
#include <ephysics/constraint/HingeJoint.hpp>
#include <ephysics/engine/ConstraintSolver.hpp>
#include <cmath>
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]

37
ephysics/engine/CollisionWorld.cpp
View File

@ -6,7 +6,6 @@
// Libraries
#include <ephysics/engine/CollisionWorld.hpp>
#include <algorithm>
// 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<CollisionBody*>::iterator itBodies;
etk::Set<CollisionBody*>::Iterator itBodies;
for (itBodies = m_bodies.begin(); itBodies != m_bodies.end(); ) {
std::set<CollisionBody*>::iterator itToRemove = itBodies;
etk::Set<CollisionBody*>::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<ephysics::bodyindex>::max());
EPHYSIC_ASSERT(bodyID < std::numeric_limits<ephysics::bodyindex>::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<CollisionBody*>::iterator it = m_bodies.begin(); it != m_bodies.end(); ++it) {
for (etk::Set<CollisionBody*>::Iterator it = m_bodies.begin(); it != m_bodies.end(); ++it) {
// Reset the contact manifold list of the body
(*it)->resetContactManifoldsList();
@ -142,9 +141,9 @@ void CollisionWorld::testCollision(const ProxyShape* shape,
resetContactManifoldListsOfBodies();
// Create the sets of shapes
std::set<uint32_t> shapes;
etk::Set<uint32_t> shapes;
shapes.insert(shape->m_broadPhaseID);
std::set<uint32_t> emptySet;
etk::Set<uint32_t> 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<uint32_t> shapes1;
etk::Set<uint32_t> shapes1;
shapes1.insert(shape1->m_broadPhaseID);
std::set<uint32_t> shapes2;
etk::Set<uint32_t> 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<uint32_t> shapes1;
etk::Set<uint32_t> 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<uint32_t> emptySet;
etk::Set<uint32_t> 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<uint32_t> shapes1;
for (const ProxyShape* shape=body1->getProxyShapesList(); shape != NULL;
etk::Set<uint32_t> shapes1;
for (const ProxyShape* shape=body1->getProxyShapesList(); shape != nullptr;
shape = shape->getNext()) {
shapes1.insert(shape->m_broadPhaseID);
}
std::set<uint32_t> shapes2;
for (const ProxyShape* shape=body2->getProxyShapesList(); shape != NULL;
etk::Set<uint32_t> 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<uint32_t> emptySet;
etk::Set<uint32_t> emptySet;
// Perform the collision detection and report contacts
m_collisionDetection.testCollisionBetweenShapes(callback, emptySet, emptySet);

9
ephysics/engine/CollisionWorld.hpp
View File

@ -6,9 +6,6 @@
#pragma once
#include <etk/Vector.hpp>
#include <set>
#include <list>
#include <algorithm>
#include <ephysics/mathematics/mathematics.hpp>
#include <ephysics/engine/Profiler.hpp>
#include <ephysics/body/CollisionBody.hpp>
@ -30,7 +27,7 @@ namespace ephysics {
class CollisionWorld {
protected :
CollisionDetection m_collisionDetection; //!< Reference to the collision detection
std::set<CollisionBody*> m_bodies; //!< All the bodies (rigid and soft) of the world
etk::Set<CollisionBody*> m_bodies; //!< All the bodies (rigid and soft) of the world
bodyindex m_currentBodyID; //!< Current body ID
etk::Vector<uint64_t> 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<CollisionBody*>::iterator getBodiesBeginIterator() {
etk::Set<CollisionBody*>::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<CollisionBody*>::iterator getBodiesEndIterator() {
etk::Set<CollisionBody*>::Iterator getBodiesEndIterator() {
return m_bodies.end();
}
/// Create a collision body

1
ephysics/engine/ConstraintSolver.hpp
View File

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

5
ephysics/engine/ContactSolver.cpp
View File

@ -9,7 +9,6 @@
#include <ephysics/engine/DynamicsWorld.hpp>
#include <ephysics/body/RigidBody.hpp>
#include <ephysics/engine/Profiler.hpp>
#include <limits>
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

1
ephysics/engine/ContactSolver.hpp
View File

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

51
ephysics/engine/DynamicsWorld.cpp
View File

@ -41,17 +41,17 @@ DynamicsWorld::DynamicsWorld(const vec3 &gravity)
DynamicsWorld::~DynamicsWorld() {
// Destroy all the joints that have not been removed
std::set<Joint*>::iterator itJoints;
etk::Set<Joint*>::Iterator itJoints;
for (itJoints = m_joints.begin(); itJoints != m_joints.end();) {
std::set<Joint*>::iterator itToRemove = itJoints;
etk::Set<Joint*>::Iterator itToRemove = itJoints;
++itJoints;
destroyJoint(*itToRemove);
}
// Destroy all the rigid bodies that have not been removed
std::set<RigidBody*>::iterator itRigidBodies;
etk::Set<RigidBody*>::Iterator itRigidBodies;
for (itRigidBodies = m_rigidBodies.begin(); itRigidBodies != m_rigidBodies.end();) {
std::set<RigidBody*>::iterator itToRemove = itRigidBodies;
etk::Set<RigidBody*>::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<RigidBody*>::const_iterator it;
etk::Set<RigidBody*>::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<RigidBody*>::iterator it = m_rigidBodies.begin(); it != m_rigidBodies.end(); ++it) {
for (etk::Set<RigidBody*>::Iterator it = m_rigidBodies.begin(); it != m_rigidBodies.end(); ++it) {
int32_t nbBodyManifolds = (*it)->resetIsAlreadyInIslandAndCountManifolds();
nbContactManifolds += nbBodyManifolds;
}
for (std::set<Joint*>::iterator it = m_joints.begin(); it != m_joints.end(); ++it) {
for (etk::Set<Joint*>::Iterator it = m_joints.begin(); it != m_joints.end(); ++it) {
(*it)->m_isAlreadyInIsland = false;
}
@ -654,7 +655,7 @@ void DynamicsWorld::computeIslands() {
RigidBody** stackBodiesToVisit = (RigidBody**)m_memoryAllocator.allocate(nbBytesStack);
// For each rigid body of the world
for (std::set<RigidBody*>::iterator it = m_rigidBodies.begin(); it != m_rigidBodies.end(); ++it) {
for (etk::Set<RigidBody*>::Iterator it = m_rigidBodies.begin(); it != m_rigidBodies.end(); ++it) {
RigidBody* body = *it;
@ -785,7 +786,7 @@ void DynamicsWorld::updateSleepingBodies() {
// For each island of the world
for (uint32_t i=0; i<m_numberIslands; i++) {
float minSleepTime = DECIMAL_LARGEST;
float minSleepTime = FLT_MAX;
// For each body of the island
RigidBody** bodies = m_islands[i]->getBodies();
@ -839,7 +840,7 @@ void DynamicsWorld::enableSleeping(bool isSleepingEnabled) {
if (!m_isSleepingEnabled) {
// For each body of the world
std::set<RigidBody*>::iterator it;
etk::Set<RigidBody*>::Iterator it;
for (it = m_rigidBodies.begin(); it != m_rigidBodies.end(); ++it) {
// Wake up the rigid body
@ -860,9 +861,9 @@ void DynamicsWorld::testCollision(const ProxyShape* shape,
CollisionCallback* callback) {
// Create the sets of shapes
std::set<uint32_t> shapes;
etk::Set<uint32_t> shapes;
shapes.insert(shape->m_broadPhaseID);
std::set<uint32_t> emptySet;
etk::Set<uint32_t> 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<uint32_t> shapes1;
etk::Set<uint32_t> shapes1;
shapes1.insert(shape1->m_broadPhaseID);
std::set<uint32_t> shapes2;
etk::Set<uint32_t> 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<uint32_t> shapes1;
etk::Set<uint32_t> 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<uint32_t> emptySet;
etk::Set<uint32_t> 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<uint32_t> shapes1;
etk::Set<uint32_t> shapes1;
for (const ProxyShape* shape=body1->getProxyShapesList(); shape != nullptr;
shape = shape->getNext()) {
shapes1.insert(shape->m_broadPhaseID);
}
std::set<uint32_t> shapes2;
etk::Set<uint32_t> 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<uint32_t> emptySet;
etk::Set<uint32_t> emptySet;
// Perform the collision detection and report contacts
m_collisionDetection.reportCollisionBetweenShapes(callback, emptySet, emptySet);
@ -965,7 +966,7 @@ etk::Vector<const ContactManifold*> DynamicsWorld::getContactsList() const {
etk::Vector<const ContactManifold*> contactManifolds;
// For each currently overlapping pair of bodies
etk::Map<overlappingpairid, OverlappingPair*>::const_iterator it;
etk::Map<overlappingpairid, OverlappingPair*>::Iterator it;
for (it = m_collisionDetection.m_overlappingPairs.begin();
it != m_collisionDetection.m_overlappingPairs.end(); ++it) {
@ -990,7 +991,7 @@ etk::Vector<const ContactManifold*> DynamicsWorld::getContactsList() const {
void DynamicsWorld::resetBodiesForceAndTorque() {
// For each body of the world
std::set<RigidBody*>::iterator it;
etk::Set<RigidBody*>::Iterator it;
for (it = m_rigidBodies.begin(); it != m_rigidBodies.end(); ++it) {
(*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<RigidBody*>::iterator DynamicsWorld::getRigidBodiesBeginIterator() {
etk::Set<RigidBody*>::Iterator DynamicsWorld::getRigidBodiesBeginIterator() {
return m_rigidBodies.begin();
}
@ -1122,7 +1123,7 @@ std::set<RigidBody*>::iterator DynamicsWorld::getRigidBodiesBeginIterator() {
/**
* @return Ending iterator of the set of rigid bodies
*/
std::set<RigidBody*>::iterator DynamicsWorld::getRigidBodiesEndIterator() {
etk::Set<RigidBody*>::Iterator DynamicsWorld::getRigidBodiesEndIterator() {
return m_rigidBodies.end();
}

8
ephysics/engine/DynamicsWorld.hpp
View File

@ -26,8 +26,8 @@ namespace ephysics {
uint32_t m_nbVelocitySolverIterations; //!< Number of iterations for the velocity solver of the Sequential Impulses technique
uint32_t m_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<RigidBody*> m_rigidBodies; //!< All the rigid bodies of the physics world
std::set<Joint*> m_joints; //!< All the joints of the world
etk::Set<RigidBody*> m_rigidBodies; //!< All the rigid bodies of the physics world
etk::Set<Joint*> 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<RigidBody*>::iterator getRigidBodiesBeginIterator();
etk::Set<RigidBody*>::Iterator getRigidBodiesBeginIterator();
/// Return an iterator to the end of the rigid bodies of the physics world
std::set<RigidBody*>::iterator getRigidBodiesEndIterator();
etk::Set<RigidBody*>::Iterator getRigidBodiesEndIterator();
/// Return true if the sleeping technique is enabled
bool isSleepingEnabled() const;
/// Enable/Disable the sleeping technique

4
ephysics/engine/Material.hpp
View File

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

62
ephysics/engine/Profiler.cpp
View File

@ -8,11 +8,12 @@
// Libraries
#include <ephysics/Profiler.hpp>
#include <ephysics/debug.hpp>
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; i<spacing; i++) outputStream << " ";
outputStream << "---------------" << std::endl;
for (int32_t i=0; i<spacing; i++) outputStream << " ";
outputStream << "| Profiling : " << iterator->getCurrentParentName() <<
" (total running time : " << parentTime << " ms) ---" << std::endl;
for (int32_t i=0; i<spacing; i++) {
_stream << " ";
}
_stream << "---------------\n";
for (int32_t i=0; i<spacing; i++) _stream << " ";
_stream << "| Profiling : " << iterator->getCurrentParentName() <<
" (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<long double>::epsilon() ?
long double fraction = parentTime > DLB_EPSILON ?
(currentTotalTime / parentTime) * 100.0 : 0.0;
for (int32_t j=0; j<spacing; j++) outputStream << " ";
outputStream << "| " << i << " -- " << iterator->getCurrentName() << " : " <<
for (int32_t j=0; j<spacing; j++) _stream << " ";
_stream << "| " << i << " -- " << iterator->getCurrentName() << " : " <<
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<spacing; i++) outputStream << " ";
long double percentage = parentTime > std::numeric_limits<long double>::epsilon() ?
for (int32_t i=0; i<spacing; i++) _stream << " ";
long double percentage = parentTime > 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; i<nbChildren; i++){
iterator->enterChild(i);
print32_tRecursiveNodeReport(iterator, spacing + 3, outputStream);
print32_tRecursiveNodeReport(iterator, spacing + 3, _stream);
iterator->enterParent();
}
}

12
ephysics/engine/Timer.hpp
View File

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

4
ephysics/mathematics/mathematics.hpp
View File

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

8
ephysics/mathematics/mathematics_functions.hpp
View File

@ -9,10 +9,6 @@
#include <ephysics/configuration.hpp>
#include <etk/math/Vector3D.hpp>
#include <algorithm>
#include <cassert>
#include <cmath>
/// 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

2
ephysics/memory/MemoryAllocator.cpp
View File

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

1
ephysics/memory/MemoryAllocator.hpp
View File

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

2
lutin_ephysics-test.py
View File

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

2
test/Test.hpp
View File

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

1
test/TestSuite.hpp
View File

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

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

@ -95,7 +95,7 @@ class Testetk::Quaternion : public Test {
const float PI_OVER_2 = PI * 0.5f;
const float PI_OVER_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));

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

@ -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