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

[DEV] update the 2 callback herited in function lanmba availlable

Browse Source
This commit is contained in:
Edouard DUPIN 2017-09-30 21:56:04 +02:00
parent cdd0d3e453
commit 275f32d377
17 changed files with 267 additions and 419 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

120
ephysics/collision/broadphase/BroadPhaseAlgorithm.cpp
View File

@ -13,25 +13,20 @@ using namespace ephysics;
BroadPhaseAlgorithm::BroadPhaseAlgorithm(CollisionDetection& collisionDetection) BroadPhaseAlgorithm::BroadPhaseAlgorithm(CollisionDetection& collisionDetection)
:m_dynamicAABBTree(DYNAMIC_TREE_AABB_GAP), m_numberMovedShapes(0), m_numberAllocatedMovedShapes(8), :m_dynamicAABBTree(DYNAMIC_TREE_AABB_GAP), m_numberMovedShapes(0), m_numberAllocatedMovedShapes(8),
m_numberNonUsedMovedShapes(0), m_numberPotentialPairs(0), m_numberAllocatedPotentialPairs(8), m_numberNonUsedMovedShapes(0),
m_collisionDetection(collisionDetection) { m_collisionDetection(collisionDetection) {
// Allocate memory for the array of non-static proxy shapes IDs // Allocate memory for the array of non-static proxy shapes IDs
m_movedShapes = (int32_t*) malloc(m_numberAllocatedMovedShapes * sizeof(int32_t)); m_movedShapes = (int32_t*) malloc(m_numberAllocatedMovedShapes * sizeof(int32_t));
assert(m_movedShapes != NULL); assert(m_movedShapes != NULL);
// Allocate memory for the array of potential overlapping pairs m_potentialPairs.reserve(8);
m_potentialPairs = (BroadPhasePair*) malloc(m_numberAllocatedPotentialPairs * sizeof(BroadPhasePair));
assert(m_potentialPairs != NULL);
} }
BroadPhaseAlgorithm::~BroadPhaseAlgorithm() { BroadPhaseAlgorithm::~BroadPhaseAlgorithm() {
// Release the memory for the array of non-static proxy shapes IDs // Release the memory for the array of non-static proxy shapes IDs
free(m_movedShapes); free(m_movedShapes);
// Release the memory for the array of potential overlapping pairs
free(m_potentialPairs);
} }
void BroadPhaseAlgorithm::addMovedCollisionShape(int32_t broadPhaseID) { void BroadPhaseAlgorithm::addMovedCollisionShape(int32_t broadPhaseID) {
@ -131,111 +126,72 @@ void BroadPhaseAlgorithm::updateProxyCollisionShape(ProxyShape* _proxyShape,
} }
void BroadPhaseAlgorithm::computeOverlappingPairs() { void BroadPhaseAlgorithm::computeOverlappingPairs() {
m_potentialPairs.clear();
// Reset the potential overlapping pairs
m_numberPotentialPairs = 0;
// For all collision shapes that have moved (or have been created) during the // For all collision shapes that have moved (or have been created) during the
// last simulation step // last simulation step
for (uint32_t i=0; i<m_numberMovedShapes; i++) { for (uint32_t i=0; i<m_numberMovedShapes; i++) {
int32_t shapeID = m_movedShapes[i]; int32_t shapeID = m_movedShapes[i];
if (shapeID == -1) {
if (shapeID == -1) continue; continue;
}
AABBOverlapCallback callback(*this, shapeID);
// Get the AABB of the shape // Get the AABB of the shape
const AABB& shapeAABB = m_dynamicAABBTree.getFatAABB(shapeID); const AABB& shapeAABB = m_dynamicAABBTree.getFatAABB(shapeID);
// Ask the dynamic AABB tree to report all collision shapes that overlap with // Ask the dynamic AABB tree to report all collision shapes that overlap with
// this AABB. The method BroadPhase::notifiyOverlappingPair() will be called // this AABB. The method BroadPhase::notifiyOverlappingPair() will be called
// by the dynamic AABB tree for each potential overlapping pair. // by the dynamic AABB tree for each potential overlapping pair.
m_dynamicAABBTree.reportAllShapesOverlappingWithAABB(shapeAABB, callback); m_dynamicAABBTree.reportAllShapesOverlappingWithAABB(shapeAABB, [&](int32_t nodeId) mutable {
// If both the nodes are the same, we do not create store the overlapping pair
if (shapeID == nodeId) {
return;
}
// Add the new potential pair int32_to the array of potential overlapping pairs
m_potentialPairs.pushBack(etk::makePair(etk::min(shapeID, nodeId), etk::max(shapeID, nodeId) ));
});
} }
// Reset the array of collision shapes that have move (or have been created) during the // Reset the array of collision shapes that have move (or have been created) during the
// last simulation step // last simulation step
m_numberMovedShapes = 0; m_numberMovedShapes = 0;
// Sort the array of potential overlapping pairs in order to remove duplicate pairs // Sort the array of potential overlapping pairs in order to remove duplicate pairs
std::sort(m_potentialPairs, m_potentialPairs + m_numberPotentialPairs, BroadPhasePair::smallerThan); m_potentialPairs.sort(0,
m_potentialPairs.size()-1,
[](const etk::Pair<int32_t,int32_t>& _pair1, const etk::Pair<int32_t,int32_t>& _pair2) {
if (_pair1.first < _pair2.first) {
return true;
}
if (_pair1.first == _pair2.first) {
return _pair1.second < _pair2.second;
}
return false;
});
// Check all the potential overlapping pairs avoiding duplicates to report unique // Check all the potential overlapping pairs avoiding duplicates to report unique
// overlapping pairs // overlapping pairs
uint32_t i=0; uint32_t iii=0;
while (i < m_numberPotentialPairs) { while (iii < m_potentialPairs.size()) {
// Get a potential overlapping pair // Get a potential overlapping pair
BroadPhasePair* pair = m_potentialPairs + i; const etk::Pair<int32_t,int32_t>& pair = (m_potentialPairs[iii]);
i++; ++iii;
assert(pair->collisionShape1ID != pair->collisionShape2ID);
// Get the two collision shapes of the pair // Get the two collision shapes of the pair
ProxyShape* shape1 = static_cast<ProxyShape*>(m_dynamicAABBTree.getNodeDataPointer(pair->collisionShape1ID)); ProxyShape* shape1 = static_cast<ProxyShape*>(m_dynamicAABBTree.getNodeDataPointer(pair.first));
ProxyShape* shape2 = static_cast<ProxyShape*>(m_dynamicAABBTree.getNodeDataPointer(pair->collisionShape2ID)); ProxyShape* shape2 = static_cast<ProxyShape*>(m_dynamicAABBTree.getNodeDataPointer(pair.second));
// Notify the collision detection about the overlapping pair // Notify the collision detection about the overlapping pair
m_collisionDetection.broadPhaseNotifyOverlappingPair(shape1, shape2); m_collisionDetection.broadPhaseNotifyOverlappingPair(shape1, shape2);
// Skip the duplicate overlapping pairs // Skip the duplicate overlapping pairs
while (i < m_numberPotentialPairs) { while (iii < m_potentialPairs.size()) {
// Get the next pair // Get the next pair
BroadPhasePair* nextPair = m_potentialPairs + i; const etk::Pair<int32_t,int32_t>& nextPair = m_potentialPairs[iii];
// If the next pair is different from the previous one, we stop skipping pairs // If the next pair is different from the previous one, we stop skipping pairs
if (nextPair->collisionShape1ID != pair->collisionShape1ID || if ( nextPair.first != pair.first
nextPair->collisionShape2ID != pair->collisionShape2ID) { || nextPair.second != pair.second) {
break; break;
} }
i++; ++iii;
} }
} }
// If the number of potential overlapping pairs is less than the quarter of allocated
// number of overlapping pairs
if (m_numberPotentialPairs < m_numberAllocatedPotentialPairs / 4 && m_numberPotentialPairs > 8) {
// Reduce the number of allocated potential overlapping pairs
BroadPhasePair* oldPairs = m_potentialPairs;
m_numberAllocatedPotentialPairs /= 2;
m_potentialPairs = (BroadPhasePair*) malloc(m_numberAllocatedPotentialPairs * sizeof(BroadPhasePair));
assert(m_potentialPairs);
memcpy(m_potentialPairs, oldPairs, m_numberPotentialPairs * sizeof(BroadPhasePair));
free(oldPairs);
}
} }
void BroadPhaseAlgorithm::notifyOverlappingNodes(int32_t node1ID, int32_t node2ID) { float BroadPhaseRaycastCallback::operator()(int32_t nodeId, const Ray& ray) {
// If both the nodes are the same, we do not create store the overlapping pair
if (node1ID == node2ID) return;
// If we need to allocate more memory for the array of potential overlapping pairs
if (m_numberPotentialPairs == m_numberAllocatedPotentialPairs) {
// Allocate more memory for the array of potential pairs
BroadPhasePair* oldPairs = m_potentialPairs;
m_numberAllocatedPotentialPairs *= 2;
m_potentialPairs = (BroadPhasePair*) malloc(m_numberAllocatedPotentialPairs * sizeof(BroadPhasePair));
assert(m_potentialPairs);
memcpy(m_potentialPairs, oldPairs, m_numberPotentialPairs * sizeof(BroadPhasePair));
free(oldPairs);
}
// Add the new potential pair int32_to the array of potential overlapping pairs
m_potentialPairs[m_numberPotentialPairs].collisionShape1ID = etk::min(node1ID, node2ID);
m_potentialPairs[m_numberPotentialPairs].collisionShape2ID = etk::max(node1ID, node2ID);
m_numberPotentialPairs++;
}
void AABBOverlapCallback::notifyOverlappingNode(int32_t nodeId) {
m_broadPhaseAlgorithm.notifyOverlappingNodes(m_referenceNodeId, nodeId);
}
float BroadPhaseRaycastCallback::raycastBroadPhaseShape(int32_t nodeId, const Ray& ray) {
float hitFraction = float(-1.0); float hitFraction = float(-1.0);

49
ephysics/collision/broadphase/BroadPhaseAlgorithm.hpp
View File

@ -16,49 +16,12 @@ namespace ephysics {
class CollisionDetection; class CollisionDetection;
class BroadPhaseAlgorithm; class BroadPhaseAlgorithm;
/** // TODO : remove this as callback ... DynamicAABBTreeOverlapCallback {
* @brief It represent a potential overlapping pair during the
* broad-phase collision detection.
*/
struct BroadPhasePair {
int32_t collisionShape1ID; //!< Broad-phase ID of the first collision shape
int32_t collisionShape2ID; //!< Broad-phase ID of the second collision shape
/**
* @brief Method used to compare two pairs for sorting algorithm
* @param[in] _pair1 first pair of element
* @param[in] _pair2 Second pair of element
* @return _pair1 is smaller than _pair2
*/
static bool smallerThan(const BroadPhasePair& _pair1, const BroadPhasePair& _pair2) {
if (_pair1.collisionShape1ID < _pair2.collisionShape1ID) return true;
if (_pair1.collisionShape1ID == _pair2.collisionShape1ID) {
return _pair1.collisionShape2ID < _pair2.collisionShape2ID;
}
return false;
}
};
class AABBOverlapCallback : public DynamicAABBTreeOverlapCallback {
private:
BroadPhaseAlgorithm& m_broadPhaseAlgorithm;
int32_t m_referenceNodeId;
public:
// Constructor
AABBOverlapCallback(BroadPhaseAlgorithm& _broadPhaseAlgo, int32_t _referenceNodeId):
m_broadPhaseAlgorithm(_broadPhaseAlgo),
m_referenceNodeId(_referenceNodeId) {
}
// Called when a overlapping node has been found during the call to
// DynamicAABBTree:reportAllShapesOverlappingWithAABB()
virtual void notifyOverlappingNode(int32_t nodeId);
};
/** /**
* Callback called when the AABB of a leaf node is hit by a ray the * Callback called when the AABB of a leaf node is hit by a ray the
* broad-phase Dynamic AABB Tree. * broad-phase Dynamic AABB Tree.
*/ */
class BroadPhaseRaycastCallback : public DynamicAABBTreeRaycastCallback { class BroadPhaseRaycastCallback {
private : private :
const DynamicAABBTree& m_dynamicAABBTree; const DynamicAABBTree& m_dynamicAABBTree;
unsigned short m_raycastWithCategoryMaskBits; unsigned short m_raycastWithCategoryMaskBits;
@ -74,7 +37,7 @@ namespace ephysics {
} }
// Called for a broad-phase shape that has to be tested for raycast // Called for a broad-phase shape that has to be tested for raycast
virtual float raycastBroadPhaseShape(int32_t _nodeId, const Ray& _ray); float operator()(int32_t _nodeId, const Ray& _ray);
}; };
/** /**
@ -91,9 +54,7 @@ namespace ephysics {
uint32_t m_numberMovedShapes; //!< Number of collision shapes in the array of shapes that have moved during the last simulation step. uint32_t m_numberMovedShapes; //!< Number of collision shapes in the array of shapes that have moved during the last simulation step.
uint32_t m_numberAllocatedMovedShapes; //!< Number of allocated elements for the array of shapes that have moved during the last simulation step. uint32_t m_numberAllocatedMovedShapes; //!< Number of allocated elements for the array of shapes that have moved during the last simulation step.
uint32_t m_numberNonUsedMovedShapes; //!< Number of non-used elements in the array of shapes that have moved during the last simulation step. uint32_t m_numberNonUsedMovedShapes; //!< Number of non-used elements in the array of shapes that have moved during the last simulation step.
BroadPhasePair* m_potentialPairs; //!< Temporary array of potential overlapping pairs (with potential duplicates) etk::Vector<etk::Pair<int32_t,int32_t>> m_potentialPairs; //!< Temporary array of potential overlapping pairs (with potential duplicates)
uint32_t m_numberPotentialPairs; //!< Number of potential overlapping pairs
uint32_t m_numberAllocatedPotentialPairs; //!< Number of allocated elements for the array of potential overlapping pairs
CollisionDetection& m_collisionDetection; //!< Reference to the collision detection object CollisionDetection& m_collisionDetection; //!< Reference to the collision detection object
/// Private copy-constructor /// Private copy-constructor
BroadPhaseAlgorithm(const BroadPhaseAlgorithm& algorithm); BroadPhaseAlgorithm(const BroadPhaseAlgorithm& algorithm);
@ -119,8 +80,6 @@ namespace ephysics {
/// Remove a collision shape from the array of shapes that have moved in the last simulation /// Remove a collision shape from the array of shapes that have moved in the last simulation
/// step and that need to be tested again for broad-phase overlapping. /// step and that need to be tested again for broad-phase overlapping.
void removeMovedCollisionShape(int32_t _broadPhaseID); void removeMovedCollisionShape(int32_t _broadPhaseID);
/// Notify the broad-phase about a potential overlapping pair in the dynamic AABB tree
void notifyOverlappingNodes(int32_t _broadPhaseId1, int32_t _broadPhaseId2);
/// Compute all the overlapping pairs of collision shapes /// Compute all the overlapping pairs of collision shapes
void computeOverlappingPairs(); void computeOverlappingPairs();
/// Return true if the two broad-phase collision shapes are overlapping /// Return true if the two broad-phase collision shapes are overlapping

66
ephysics/collision/broadphase/DynamicAABBTree.cpp
View File

@ -568,36 +568,31 @@ int32_t DynamicAABBTree::balanceSubTreeAtNode(int32_t _nodeID) {
} }
/// Report all shapes overlapping with the AABB given in parameter. /// Report all shapes overlapping with the AABB given in parameter.
void DynamicAABBTree::reportAllShapesOverlappingWithAABB(const AABB& aabb, void DynamicAABBTree::reportAllShapesOverlappingWithAABB(const AABB& _aabb, etk::Function<void(int32_t nodeId)> _callback) const {
DynamicAABBTreeOverlapCallback& callback) const { if (_callback == nullptr) {
EPHY_ERROR("call with nullptr callback");
return;
}
// Create a stack with the nodes to visit // Create a stack with the nodes to visit
Stack<int32_t, 64> stack; Stack<int32_t, 64> stack;
stack.push(m_rootNodeID); stack.push(m_rootNodeID);
// While there are still nodes to visit // While there are still nodes to visit
while(stack.getNbElements() > 0) { while(stack.getNbElements() > 0) {
// Get the next node ID to visit // Get the next node ID to visit
int32_t nodeIDToVisit = stack.pop(); int32_t nodeIDToVisit = stack.pop();
// Skip it if it is a null node // Skip it if it is a null node
if (nodeIDToVisit == TreeNode::NULL_TREE_NODE) continue; if (nodeIDToVisit == TreeNode::NULL_TREE_NODE) {
continue;
}
// Get the corresponding node // Get the corresponding node
const TreeNode* nodeToVisit = m_nodes + nodeIDToVisit; const TreeNode* nodeToVisit = m_nodes + nodeIDToVisit;
// If the AABB in parameter overlaps with the AABB of the node to visit // If the AABB in parameter overlaps with the AABB of the node to visit
if (aabb.testCollision(nodeToVisit->aabb)) { if (_aabb.testCollision(nodeToVisit->aabb)) {
// If the node is a leaf // If the node is a leaf
if (nodeToVisit->isLeaf()) { if (nodeToVisit->isLeaf()) {
// Notify the broad-phase about a new potential overlapping pair // Notify the broad-phase about a new potential overlapping pair
callback.notifyOverlappingNode(nodeIDToVisit); _callback(nodeIDToVisit);
} } else { // If the node is not a leaf
else { // If the node is not a leaf
// We need to visit its children // We need to visit its children
stack.push(nodeToVisit->children[0]); stack.push(nodeToVisit->children[0]);
stack.push(nodeToVisit->children[1]); stack.push(nodeToVisit->children[1]);
@ -607,60 +602,51 @@ void DynamicAABBTree::reportAllShapesOverlappingWithAABB(const AABB& aabb,
} }
// Ray casting method // Ray casting method
void DynamicAABBTree::raycast(const Ray& ray, DynamicAABBTreeRaycastCallback &callback) const { void DynamicAABBTree::raycast(const ephysics::Ray& _ray, etk::Function<float(int32_t _nodeId, const ephysics::Ray& _ray)> _callback) const {
PROFILE("DynamicAABBTree::raycast()"); PROFILE("DynamicAABBTree::raycast()");
if (_callback == nullptr) {
float maxFraction = ray.maxFraction; EPHY_ERROR("call with nullptr callback");
return;
}
float maxFraction = _ray.maxFraction;
Stack<int32_t, 128> stack; Stack<int32_t, 128> stack;
stack.push(m_rootNodeID); stack.push(m_rootNodeID);
// Walk through the tree from the root looking for proxy shapes // Walk through the tree from the root looking for proxy shapes
// that overlap with the ray AABB // that overlap with the ray AABB
while (stack.getNbElements() > 0) { while (stack.getNbElements() > 0) {
// Get the next node in the stack // Get the next node in the stack
int32_t nodeID = stack.pop(); int32_t nodeID = stack.pop();
// If it is a null node, skip it // If it is a null node, skip it
if (nodeID == TreeNode::NULL_TREE_NODE) continue; if (nodeID == TreeNode::NULL_TREE_NODE) {
continue;
}
// Get the corresponding node // Get the corresponding node
const TreeNode* node = m_nodes + nodeID; const TreeNode* node = m_nodes + nodeID;
Ray rayTemp(_ray.point1, _ray.point2, maxFraction);
Ray rayTemp(ray.point1, ray.point2, maxFraction);
// Test if the ray int32_tersects with the current node AABB // Test if the ray int32_tersects with the current node AABB
if (!node->aabb.testRayIntersect(rayTemp)) continue; if (node->aabb.testRayIntersect(rayTemp) == false) {
continue;
}
// If the node is a leaf of the tree // If the node is a leaf of the tree
if (node->isLeaf()) { if (node->isLeaf()) {
// Call the callback that will raycast again the broad-phase shape // Call the callback that will raycast again the broad-phase shape
float hitFraction = callback.raycastBroadPhaseShape(nodeID, rayTemp); float hitFraction = _callback(nodeID, rayTemp);
// If the user returned a hitFraction of zero, it means that // If the user returned a hitFraction of zero, it means that
// the raycasting should stop here // the raycasting should stop here
if (hitFraction == 0.0f) { if (hitFraction == 0.0f) {
return; return;
} }
// If the user returned a positive fraction // If the user returned a positive fraction
if (hitFraction > 0.0f) { if (hitFraction > 0.0f) {
// We update the maxFraction value and the ray // We update the maxFraction value and the ray
// AABB using the new maximum fraction // AABB using the new maximum fraction
if (hitFraction < maxFraction) { if (hitFraction < maxFraction) {
maxFraction = hitFraction; maxFraction = hitFraction;
} }
} }
// If the user returned a negative fraction, we continue // If the user returned a negative fraction, we continue
// the raycasting as if the proxy shape did not exist // the raycasting as if the proxy shape did not exist
} } else { // If the node has children
else { // If the node has children
// Push its children in the stack of nodes to explore // Push its children in the stack of nodes to explore
stack.push(node->children[0]); stack.push(node->children[0]);
stack.push(node->children[1]); stack.push(node->children[1]);

30
ephysics/collision/broadphase/DynamicAABBTree.hpp
View File

@ -8,11 +8,11 @@
#include <ephysics/configuration.hpp> #include <ephysics/configuration.hpp>
#include <ephysics/collision/shapes/AABB.hpp> #include <ephysics/collision/shapes/AABB.hpp>
#include <ephysics/body/CollisionBody.hpp> #include <ephysics/body/CollisionBody.hpp>
#include <etk/Function.hpp>
namespace ephysics { namespace ephysics {
class BroadPhaseAlgorithm; class BroadPhaseAlgorithm;
class BroadPhaseRaycastTestCallback; class BroadPhaseRaycastTestCallback;
class DynamicAABBTreeOverlapCallback;
struct RaycastTest; struct RaycastTest;
/** /**
* @brief It represents a node of the dynamic AABB tree. * @brief It represents a node of the dynamic AABB tree.
@ -43,29 +43,6 @@ namespace ephysics {
bool isLeaf() const; bool isLeaf() const;
}; };
/**
* @brief Overlapping callback method that has to be used as parameter of the
* reportAllShapesOverlappingWithNode() method.
*/
class DynamicAABBTreeOverlapCallback {
public :
virtual ~DynamicAABBTreeOverlapCallback() = default;
// Called when a overlapping node has been found during the call to
// DynamicAABBTree:reportAllShapesOverlappingWithAABB()
virtual void notifyOverlappingNode(int32_t nodeId)=0;
};
/**
* @brief Raycast callback in the Dynamic AABB Tree called when the AABB of a leaf
* node is hit by the ray.
*/
class DynamicAABBTreeRaycastCallback {
public:
virtual ~DynamicAABBTreeRaycastCallback() = default;
// Called when the AABB of a leaf node is hit by a ray
virtual float raycastBroadPhaseShape(int32_t nodeId, const Ray& ray)=0;
};
/** /**
* @brief It implements a dynamic AABB tree that is used for broad-phase * @brief It implements a dynamic AABB tree that is used for broad-phase
* collision detection. This data structure is inspired by Nathanael Presson's * collision detection. This data structure is inspired by Nathanael Presson's
@ -123,10 +100,9 @@ namespace ephysics {
/// Return the data pointer of a given leaf node of the tree /// Return the data pointer of a given leaf node of the tree
void* getNodeDataPointer(int32_t _nodeID) const; void* getNodeDataPointer(int32_t _nodeID) const;
/// Report all shapes overlapping with the AABB given in parameter. /// Report all shapes overlapping with the AABB given in parameter.
void reportAllShapesOverlappingWithAABB(const AABB& _aabb, void reportAllShapesOverlappingWithAABB(const AABB& _aabb, etk::Function<void(int32_t _nodeId)> _callback) const;
DynamicAABBTreeOverlapCallback& _callback) const;
/// Ray casting method /// Ray casting method
void raycast(const Ray& _ray, DynamicAABBTreeRaycastCallback& _callback) const; void raycast(const Ray& _ray, etk::Function<float(int32_t _nodeId, const ephysics::Ray& _ray)> _callback) const;
/// Compute the height of the tree /// Compute the height of the tree
int32_t computeHeight(); int32_t computeHeight();
/// Return the root AABB of the tree /// Return the root AABB of the tree

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

@ -18,64 +18,63 @@ EPAAlgorithm::~EPAAlgorithm() {
} }
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 // Check vertex 1
vec3 normal1 = (p2-p1).cross(p3-p1); vec3 normal1 = (_p2-_p1).cross(_p3-_p1);
if ((normal1.dot(p1) > 0.0) == (normal1.dot(p4) > 0.0)) { if ((normal1.dot(_p1) > 0.0) == (normal1.dot(_p4) > 0.0)) {
return 4; return 4;
} }
// Check vertex 2 // Check vertex 2
vec3 normal2 = (p4-p2).cross(p3-p2); vec3 normal2 = (_p4-_p2).cross(_p3-_p2);
if ((normal2.dot(p2) > 0.0) == (normal2.dot(p1) > 0.0)) { if ((normal2.dot(_p2) > 0.0) == (normal2.dot(_p1) > 0.0)) {
return 1; return 1;
} }
// Check vertex 3 // Check vertex 3
vec3 normal3 = (p4-p3).cross(p1-p3); vec3 normal3 = (_p4-_p3).cross(_p1-_p3);
if ((normal3.dot(p3) > 0.0) == (normal3.dot(p2) > 0.0)) { if ((normal3.dot(_p3) > 0.0) == (normal3.dot(_p2) > 0.0)) {
return 2; return 2;
} }
// Check vertex 4 // Check vertex 4
vec3 normal4 = (p2-p4).cross(p1-p4); vec3 normal4 = (_p2-_p4).cross(_p1-_p4);
if ((normal4.dot(p4) > 0.0) == (normal4.dot(p3) > 0.0)) { if ((normal4.dot(_p4) > 0.0) == (normal4.dot(_p3) > 0.0)) {
return 3; return 3;
} }
// The origin is in the tetrahedron, we return 0 // The origin is in the tetrahedron, we return 0
return 0; return 0;
} }
void EPAAlgorithm::computePenetrationDepthAndContactPoints(const Simplex& simplex, void EPAAlgorithm::computePenetrationDepthAndContactPoints(const Simplex& _simplex,
CollisionShapeInfo shape1Info, CollisionShapeInfo _shape1Info,
const etk::Transform3D& transform1, const etk::Transform3D& _transform1,
CollisionShapeInfo shape2Info, CollisionShapeInfo _shape2Info,
const etk::Transform3D& transform2, const etk::Transform3D& _transform2,
vec3& v, vec3& _vector,
NarrowPhaseCallback* narrowPhaseCallback) { NarrowPhaseCallback* narrowPhaseCallback) {
PROFILE("EPAAlgorithm::computePenetrationDepthAndContactPoints()"); PROFILE("EPAAlgorithm::computePenetrationDepthAndContactPoints()");
assert(shape1Info.collisionShape->isConvex()); assert(_shape1Info.collisionShape->isConvex());
assert(shape2Info.collisionShape->isConvex()); assert(_shape2Info.collisionShape->isConvex());
const ConvexShape* shape1 = static_cast<const ConvexShape*>(shape1Info.collisionShape); const ConvexShape* shape1 = static_cast<const ConvexShape*>(_shape1Info.collisionShape);
const ConvexShape* shape2 = static_cast<const ConvexShape*>(shape2Info.collisionShape); const ConvexShape* shape2 = static_cast<const ConvexShape*>(_shape2Info.collisionShape);
void** shape1CachedCollisionData = shape1Info.cachedCollisionData; void** shape1CachedCollisionData = _shape1Info.cachedCollisionData;
void** shape2CachedCollisionData = shape2Info.cachedCollisionData; void** shape2CachedCollisionData = _shape2Info.cachedCollisionData;
vec3 suppPointsA[MAX_SUPPORT_POINTS]; // Support points of object A in local coordinates vec3 suppPointsA[MAX_SUPPORT_POINTS]; // Support points of object A in local coordinates
vec3 suppPointsB[MAX_SUPPORT_POINTS]; // Support points of object B in local coordinates vec3 suppPointsB[MAX_SUPPORT_POINTS]; // Support points of object B in local coordinates
vec3 points[MAX_SUPPORT_POINTS]; // Current points vec3 points[MAX_SUPPORT_POINTS]; // Current points
TrianglesStore triangleStore; // Store the triangles TrianglesStore triangleStore; // Store the triangles
TriangleEPA* triangleHeap[MAX_FACETS]; // Heap that contains the face etk::Set<TriangleEPA*> triangleHeap; // list of face candidate of the EPA algorithm sorted lower square dist to upper square dist
// candidate of the EPA algorithm triangleHeap.setComparator([](TriangleEPA * const & _face1, TriangleEPA * const & _face2) {
return (_face1->getDistSquare() < _face2->getDistSquare());
});
// etk::Transform3D a point from local space of body 2 to local // etk::Transform3D a point from local space of body 2 to local
// space of body 1 (the GJK algorithm is done in local space of body 1) // space of body 1 (the GJK algorithm is done in local space of body 1)
etk::Transform3D body2Tobody1 = transform1.getInverse() * transform2; etk::Transform3D body2Tobody1 = _transform1.getInverse() * _transform2;
// Matrix that transform a direction from local // Matrix that transform a direction from local
// space of body 1 int32_to local space of body 2 // space of body 1 int32_to local space of body 2
etk::Quaternion rotateToBody2 = transform2.getOrientation().getInverse() * etk::Quaternion rotateToBody2 = _transform2.getOrientation().getInverse() * _transform1.getOrientation();
transform1.getOrientation();
// Get the simplex computed previously by the GJK algorithm // Get the simplex computed previously by the GJK algorithm
uint32_t nbVertices = simplex.getSimplex(suppPointsA, suppPointsB, points); uint32_t nbVertices = _simplex.getSimplex(suppPointsA, suppPointsB, points);
// Compute the tolerance // Compute the tolerance
float tolerance = FLT_EPSILON * simplex.getMaxLengthSquareOfAPoint(); float tolerance = FLT_EPSILON * _simplex.getMaxLengthSquareOfAPoint();
// Number of triangles in the polytope
uint32_t nbTriangles = 0;
// Clear the storing of triangles // Clear the storing of triangles
triangleStore.clear(); triangleStore.clear();
// Select an action according to the number of points in the simplex // Select an action according to the number of points in the simplex
@ -177,10 +176,10 @@ void EPAAlgorithm::computePenetrationDepthAndContactPoints(const Simplex& simple
link(EdgeEPA(face1, 1), EdgeEPA(face3, 0)); link(EdgeEPA(face1, 1), EdgeEPA(face3, 0));
link(EdgeEPA(face2, 1), EdgeEPA(face3, 1)); link(EdgeEPA(face2, 1), EdgeEPA(face3, 1));
// Add the triangle faces in the candidate heap // Add the triangle faces in the candidate heap
addFaceCandidate(face0, triangleHeap, nbTriangles, FLT_MAX); addFaceCandidate(face0, triangleHeap, FLT_MAX);
addFaceCandidate(face1, triangleHeap, nbTriangles, FLT_MAX); addFaceCandidate(face1, triangleHeap, FLT_MAX);
addFaceCandidate(face2, triangleHeap, nbTriangles, FLT_MAX); addFaceCandidate(face2, triangleHeap, FLT_MAX);
addFaceCandidate(face3, triangleHeap, nbTriangles, FLT_MAX); addFaceCandidate(face3, triangleHeap, FLT_MAX);
break; break;
} }
// The tetrahedron contains a wrong vertex (the origin is not inside the tetrahedron) // The tetrahedron contains a wrong vertex (the origin is not inside the tetrahedron)
@ -213,10 +212,10 @@ void EPAAlgorithm::computePenetrationDepthAndContactPoints(const Simplex& simple
suppPointsB[4] = body2Tobody1 * suppPointsB[4] = body2Tobody1 *
shape2->getLocalSupportPointWithMargin(rotateToBody2 * n, shape2CachedCollisionData); shape2->getLocalSupportPointWithMargin(rotateToBody2 * n, shape2CachedCollisionData);
points[4] = suppPointsA[4] - suppPointsB[4]; points[4] = suppPointsA[4] - suppPointsB[4];
TriangleEPA* face0 = NULL; TriangleEPA* face0 = nullptr;
TriangleEPA* face1 = NULL; TriangleEPA* face1 = nullptr;
TriangleEPA* face2 = NULL; TriangleEPA* face2 = nullptr;
TriangleEPA* face3 = NULL; TriangleEPA* face3 = nullptr;
// If the origin is in the first tetrahedron // If the origin is in the first tetrahedron
if (isOriginInTetrahedron(points[0], points[1], if (isOriginInTetrahedron(points[0], points[1],
points[2], points[3]) == 0) { points[2], points[3]) == 0) {
@ -242,9 +241,14 @@ void EPAAlgorithm::computePenetrationDepthAndContactPoints(const Simplex& simple
return; return;
} }
// If the constructed tetrahedron is not correct // If the constructed tetrahedron is not correct
if (!((face0 != NULL) && (face1 != NULL) && (face2 != NULL) && (face3 != NULL) if (!( face0 != nullptr
&& face0->getDistSquare() > 0.0 && face1->getDistSquare() > 0.0 && face1 != nullptr
&& face2->getDistSquare() > 0.0 && face3->getDistSquare() > 0.0)) { && face2 != nullptr
&& face3 != nullptr
&& face0->getDistSquare() > 0.0
&& face1->getDistSquare() > 0.0
&& face2->getDistSquare() > 0.0
&& face3->getDistSquare() > 0.0) ) {
return; return;
} }
// Associate the edges of neighbouring triangle faces // Associate the edges of neighbouring triangle faces
@ -255,26 +259,28 @@ void EPAAlgorithm::computePenetrationDepthAndContactPoints(const Simplex& simple
link(EdgeEPA(face1, 1), EdgeEPA(face3, 0)); link(EdgeEPA(face1, 1), EdgeEPA(face3, 0));
link(EdgeEPA(face2, 1), EdgeEPA(face3, 1)); link(EdgeEPA(face2, 1), EdgeEPA(face3, 1));
// Add the triangle faces in the candidate heap // Add the triangle faces in the candidate heap
addFaceCandidate(face0, triangleHeap, nbTriangles, FLT_MAX); addFaceCandidate(face0, triangleHeap, FLT_MAX);
addFaceCandidate(face1, triangleHeap, nbTriangles, FLT_MAX); addFaceCandidate(face1, triangleHeap, FLT_MAX);
addFaceCandidate(face2, triangleHeap, nbTriangles, FLT_MAX); addFaceCandidate(face2, triangleHeap, FLT_MAX);
addFaceCandidate(face3, triangleHeap, nbTriangles, FLT_MAX); addFaceCandidate(face3, triangleHeap, FLT_MAX);
nbVertices = 4; nbVertices = 4;
} }
break; break;
} }
// At this point, we have a polytope that contains the origin. Therefore, we // At this point, we have a polytope that contains the origin. Therefore, we
// can run the EPA algorithm. // can run the EPA algorithm.
if (nbTriangles == 0) { if (triangleHeap.size() == 0) {
return; return;
} }
TriangleEPA* triangle = 0; TriangleEPA* triangle = 0;
float upperBoundSquarePenDepth = FLT_MAX; float upperBoundSquarePenDepth = FLT_MAX;
do { do {
triangle = triangleHeap[0]; triangle = triangleHeap[0];
// Get the next candidate face (the face closest to the origin) triangleHeap.popFront();
std::pop_heap(&triangleHeap[0], &triangleHeap[nbTriangles], m_triangleComparison); EPHY_INFO("rm from heap:");
nbTriangles--; for (size_t iii=0; iii<triangleHeap.size(); ++iii) {
EPHY_INFO(" [" << iii << "] " << triangleHeap[iii]->getDistSquare());
}
// If the candidate face in the heap is not obsolete // If the candidate face in the heap is not obsolete
if (!triangle->getIsObsolete()) { if (!triangle->getIsObsolete()) {
// If we have reached the maximum number of support points // If we have reached the maximum number of support points
@ -284,17 +290,14 @@ void EPAAlgorithm::computePenetrationDepthAndContactPoints(const Simplex& simple
} }
// Compute the support point of the Minkowski // Compute the support point of the Minkowski
// difference (A-B) in the closest point direction // difference (A-B) in the closest point direction
suppPointsA[nbVertices] = shape1->getLocalSupportPointWithMargin( suppPointsA[nbVertices] = shape1->getLocalSupportPointWithMargin(triangle->getClosestPoint(), shape1CachedCollisionData);
triangle->getClosestPoint(), shape1CachedCollisionData); suppPointsB[nbVertices] = body2Tobody1 * shape2->getLocalSupportPointWithMargin(rotateToBody2 * (-triangle->getClosestPoint()), shape2CachedCollisionData);
suppPointsB[nbVertices] = body2Tobody1 *
shape2->getLocalSupportPointWithMargin(rotateToBody2 *
(-triangle->getClosestPoint()), shape2CachedCollisionData);
points[nbVertices] = suppPointsA[nbVertices] - suppPointsB[nbVertices]; points[nbVertices] = suppPointsA[nbVertices] - suppPointsB[nbVertices];
int32_t indexNewVertex = nbVertices; int32_t indexNewVertex = nbVertices;
nbVertices++; nbVertices++;
// Update the upper bound of the penetration depth // Update the upper bound of the penetration depth
float wDotv = points[indexNewVertex].dot(triangle->getClosestPoint()); float wDotv = points[indexNewVertex].dot(triangle->getClosestPoint());
assert(wDotv > 0.0); EPHY_ASSERT(wDotv > 0.0, "depth penetration error");
float wDotVSquare = wDotv * wDotv / triangle->getDistSquare(); float wDotVSquare = wDotv * wDotv / triangle->getDistSquare();
if (wDotVSquare < upperBoundSquarePenDepth) { if (wDotVSquare < upperBoundSquarePenDepth) {
upperBoundSquarePenDepth = wDotVSquare; upperBoundSquarePenDepth = wDotVSquare;
@ -310,7 +313,7 @@ void EPAAlgorithm::computePenetrationDepthAndContactPoints(const Simplex& simple
// Now, we compute the silhouette cast by the new vertex. The current triangle // Now, we compute the silhouette cast by the new vertex. The current triangle
// face will not be in the convex hull. We start the local recursive silhouette // face will not be in the convex hull. We start the local recursive silhouette
// algorithm from the current triangle face. // algorithm from the current triangle face.
int32_t i = triangleStore.getNbTriangles(); size_t i = triangleStore.getNbTriangles();
if (!triangle->computeSilhouette(points, indexNewVertex, triangleStore)) { if (!triangle->computeSilhouette(points, indexNewVertex, triangleStore)) {
break; break;
} }
@ -318,22 +321,23 @@ void EPAAlgorithm::computePenetrationDepthAndContactPoints(const Simplex& simple
// to the candidates list of faces of the current polytope // to the candidates list of faces of the current polytope
while(i != triangleStore.getNbTriangles()) { while(i != triangleStore.getNbTriangles()) {
TriangleEPA* newTriangle = &triangleStore[i]; TriangleEPA* newTriangle = &triangleStore[i];
addFaceCandidate(newTriangle, triangleHeap, nbTriangles, upperBoundSquarePenDepth); addFaceCandidate(newTriangle, triangleHeap, upperBoundSquarePenDepth);
i++; i++;
} }
} }
} while(nbTriangles > 0 && triangleHeap[0]->getDistSquare() <= upperBoundSquarePenDepth); } while( triangleHeap.size() > 0
&& triangleHeap[0]->getDistSquare() <= upperBoundSquarePenDepth);
// Compute the contact info // Compute the contact info
v = transform1.getOrientation() * triangle->getClosestPoint(); _vector = _transform1.getOrientation() * triangle->getClosestPoint();
vec3 pALocal = triangle->computeClosestPointOfObject(suppPointsA); vec3 pALocal = triangle->computeClosestPointOfObject(suppPointsA);
vec3 pBLocal = body2Tobody1.getInverse() * triangle->computeClosestPointOfObject(suppPointsB); vec3 pBLocal = body2Tobody1.getInverse() * triangle->computeClosestPointOfObject(suppPointsB);
vec3 normal = v.safeNormalized(); vec3 normal = _vector.safeNormalized();
float penetrationDepth = v.length(); float penetrationDepth = _vector.length();
assert(penetrationDepth > 0.0); assert(penetrationDepth > 0.0);
if (normal.length2() < FLT_EPSILON) { if (normal.length2() < FLT_EPSILON) {
return; return;
} }
// Create the contact info object // 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); narrowPhaseCallback->notifyContact(_shape1Info.overlappingPair, contactInfo);
} }

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

@ -11,29 +11,14 @@
#include <ephysics/collision/narrowphase/NarrowPhaseAlgorithm.hpp> #include <ephysics/collision/narrowphase/NarrowPhaseAlgorithm.hpp>
#include <ephysics/mathematics/mathematics.hpp> #include <ephysics/mathematics/mathematics.hpp>
#include <ephysics/collision/narrowphase/EPA/TriangleEPA.hpp> #include <ephysics/collision/narrowphase/EPA/TriangleEPA.hpp>
#include <algorithm> #include <ephysics/debug.hpp>
#include <etk/Set.hpp>
namespace ephysics { namespace ephysics {
/// Maximum number of support points of the polytope /// Maximum number of support points of the polytope
const uint32_t MAX_SUPPORT_POINTS = 100; const uint32_t MAX_SUPPORT_POINTS = 100;
/// Maximum number of facets of the polytope /// Maximum number of facets of the polytope
const uint32_t MAX_FACETS = 200; 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 * @brief Class EPAAlgorithm
* This class is the implementation of the Expanding Polytope Algorithm (EPA). * This class is the implementation of the Expanding Polytope Algorithm (EPA).
@ -50,15 +35,13 @@ namespace ephysics {
*/ */
class EPAAlgorithm { class EPAAlgorithm {
private: private:
TriangleComparison m_triangleComparison; //!< Triangle comparison operator
/// Private copy-constructor /// Private copy-constructor
EPAAlgorithm(const EPAAlgorithm& _algorithm); EPAAlgorithm(const EPAAlgorithm& _algorithm);
/// Private assignment operator /// Private assignment operator
EPAAlgorithm& operator=(const EPAAlgorithm& _algorithm); EPAAlgorithm& operator=(const EPAAlgorithm& _algorithm);
/// Add a triangle face in the candidate triangle heap /// Add a triangle face in the candidate triangle heap
void addFaceCandidate(TriangleEPA* _triangle, void addFaceCandidate(TriangleEPA* _triangle,
TriangleEPA** _heap, etk::Set<TriangleEPA*>& _heap,
uint32_t& _nbTriangles,
float _upperBoundSquarePenDepth) { float _upperBoundSquarePenDepth) {
// If the closest point of the affine hull of triangle // If the closest point of the affine hull of triangle
// points is int32_ternal to the triangle and if the distance // points is int32_ternal to the triangle and if the distance
@ -67,9 +50,11 @@ namespace ephysics {
if ( _triangle->isClosestPointInternalToTriangle() if ( _triangle->isClosestPointInternalToTriangle()
&& _triangle->getDistSquare() <= _upperBoundSquarePenDepth) { && _triangle->getDistSquare() <= _upperBoundSquarePenDepth) {
// Add the triangle face to the list of candidates // Add the triangle face to the list of candidates
_heap[_nbTriangles] = _triangle; _heap.add(_triangle);
_nbTriangles++; EPHY_INFO("add in heap:");
std::push_heap(&_heap[0], &_heap[_nbTriangles], m_triangleComparison); for (size_t iii=0; iii<_heap.size(); ++iii) {
EPHY_INFO(" [" << iii << "] " << _heap[iii]->getDistSquare());
}
} }
} }
// Decide if the origin is in the tetrahedron. // Decide if the origin is in the tetrahedron.

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

@ -36,13 +36,13 @@ uint32_t EdgeEPA::getTargetVertexIndex() const {
return (*m_ownerTriangle)[indexOfNextCounterClockwiseEdge(m_index)]; return (*m_ownerTriangle)[indexOfNextCounterClockwiseEdge(m_index)];
} }
bool EdgeEPA::computeSilhouette(const vec3* vertices, uint32_t indexNewVertex, bool EdgeEPA::computeSilhouette(const vec3* _vertices, uint32_t _indexNewVertex,
TrianglesStore& triangleStore) { TrianglesStore& _triangleStore) {
// If the edge has not already been visited // If the edge has not already been visited
if (!m_ownerTriangle->getIsObsolete()) { if (!m_ownerTriangle->getIsObsolete()) {
// If the triangle of this edge is not visible from the given point // If the triangle of this edge is not visible from the given point
if (!m_ownerTriangle->isVisibleFromVertex(vertices, indexNewVertex)) { if (!m_ownerTriangle->isVisibleFromVertex(_vertices, _indexNewVertex)) {
TriangleEPA* triangle = triangleStore.newTriangle(vertices, indexNewVertex, TriangleEPA* triangle = _triangleStore.newTriangle(_vertices, _indexNewVertex,
getTargetVertexIndex(), getTargetVertexIndex(),
getSourceVertexIndex()); getSourceVertexIndex());
// If the triangle has been created // If the triangle has been created
@ -54,13 +54,12 @@ bool EdgeEPA::computeSilhouette(const vec3* vertices, uint32_t indexNewVertex,
} else { } else {
// The current triangle is visible and therefore obsolete // The current triangle is visible and therefore obsolete
m_ownerTriangle->setIsObsolete(true); m_ownerTriangle->setIsObsolete(true);
int32_t backup = triangleStore.getNbTriangles(); int32_t backup = _triangleStore.getNbTriangles();
if(!m_ownerTriangle->getAdjacentEdge(indexOfNextCounterClockwiseEdge( if(!m_ownerTriangle->getAdjacentEdge(indexOfNextCounterClockwiseEdge(this->m_index)).computeSilhouette(_vertices,
this->m_index)).computeSilhouette(vertices, _indexNewVertex,
indexNewVertex, _triangleStore)) {
triangleStore)) {
m_ownerTriangle->setIsObsolete(false); m_ownerTriangle->setIsObsolete(false);
TriangleEPA* triangle = triangleStore.newTriangle(vertices, indexNewVertex, TriangleEPA* triangle = _triangleStore.newTriangle(_vertices, _indexNewVertex,
getTargetVertexIndex(), getTargetVertexIndex(),
getSourceVertexIndex()); getSourceVertexIndex());
// If the triangle has been created // If the triangle has been created
@ -69,13 +68,12 @@ bool EdgeEPA::computeSilhouette(const vec3* vertices, uint32_t indexNewVertex,
return true; return true;
} }
return false; return false;
} else if (!m_ownerTriangle->getAdjacentEdge(indexOfPreviousCounterClockwiseEdge( } else if (!m_ownerTriangle->getAdjacentEdge(indexOfPreviousCounterClockwiseEdge(this->m_index)).computeSilhouette(_vertices,
this->m_index)).computeSilhouette(vertices, _indexNewVertex,
indexNewVertex, _triangleStore)) {
triangleStore)) {
m_ownerTriangle->setIsObsolete(false); m_ownerTriangle->setIsObsolete(false);
triangleStore.setNbTriangles(backup); _triangleStore.resize(backup);
TriangleEPA* triangle = triangleStore.newTriangle(vertices, indexNewVertex, TriangleEPA* triangle = _triangleStore.newTriangle(_vertices, _indexNewVertex,
getTargetVertexIndex(), getTargetVertexIndex(),
getSourceVertexIndex()); getSourceVertexIndex());
if (triangle != NULL) { if (triangle != NULL) {

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

@ -24,11 +24,34 @@ namespace ephysics {
float m_lambda1; //!< Lambda1 value such that v = lambda0 * y_0 + lambda1 * y_1 + lambda2 * y_2 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_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 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: public:
/// Private copy-constructor
TriangleEPA(const TriangleEPA& _triangle) {
memcpy(m_indicesVertices, _triangle.m_indicesVertices, sizeof(m_indicesVertices));
m_adjacentEdges[0] = _triangle.m_adjacentEdges[0];
m_adjacentEdges[1] = _triangle.m_adjacentEdges[1];
m_adjacentEdges[2] = _triangle.m_adjacentEdges[2];
m_isObsolete = _triangle.m_isObsolete;
m_determinant = _triangle.m_determinant;
m_closestPoint = _triangle.m_closestPoint;
m_lambda1 = _triangle.m_lambda1;
m_lambda2 = _triangle.m_lambda2;
m_distSquare = _triangle.m_distSquare;
}
/// Private assignment operator
TriangleEPA& operator=(const TriangleEPA& _triangle) {
memcpy(m_indicesVertices, _triangle.m_indicesVertices, sizeof(m_indicesVertices));
m_adjacentEdges[0] = _triangle.m_adjacentEdges[0];
m_adjacentEdges[1] = _triangle.m_adjacentEdges[1];
m_adjacentEdges[2] = _triangle.m_adjacentEdges[2];
m_isObsolete = _triangle.m_isObsolete;
m_determinant = _triangle.m_determinant;
m_closestPoint = _triangle.m_closestPoint;
m_lambda1 = _triangle.m_lambda1;
m_lambda2 = _triangle.m_lambda2;
m_distSquare = _triangle.m_distSquare;
return *this;
}
/// Constructor /// Constructor
TriangleEPA(); TriangleEPA();
/// Constructor /// Constructor

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

@ -6,10 +6,3 @@
#include <ephysics/collision/narrowphase/EPA/TrianglesStore.hpp> #include <ephysics/collision/narrowphase/EPA/TrianglesStore.hpp>
ephysics::TrianglesStore::TrianglesStore() : m_numberTriangles(0) {
}
ephysics::TrianglesStore::~TrianglesStore() {
}

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

@ -12,48 +12,43 @@ namespace ephysics {
*/ */
class TrianglesStore { class TrianglesStore {
private: private:
TriangleEPA m_triangles[MAX_TRIANGLES]; //!< Triangles etk::Vector<TriangleEPA> m_triangles; //!< Triangles
int32_t m_numberTriangles; //!< Number of triangles
/// Private copy-constructor /// Private copy-constructor
TrianglesStore(const TrianglesStore& triangleStore); TrianglesStore(const TrianglesStore& triangleStore) = delete;
/// Private assignment operator /// Private assignment operator
TrianglesStore& operator=(const TrianglesStore& triangleStore); TrianglesStore& operator=(const TrianglesStore& triangleStore) = delete;
public: public:
/// Constructor /// Constructor
TrianglesStore(); TrianglesStore() = default;
/// Destructor
~TrianglesStore();
/// Clear all the storage /// Clear all the storage
void clear() { void clear() {
m_numberTriangles = 0; m_triangles.clear();
} }
/// Return the number of triangles /// Return the number of triangles
int32_t getNbTriangles() const { size_t getNbTriangles() const {
return m_numberTriangles; return m_triangles.size();
} }
/// Set the number of triangles /// Set the number of triangles
void setNbTriangles(int32_t _backup) { void resize(int32_t _backup) {
m_numberTriangles = _backup; m_triangles.resize(_backup);
} }
/// Return the last triangle /// Return the last triangle
TriangleEPA& last() { TriangleEPA& last() {
assert(m_numberTriangles > 0); return m_triangles.back();
return m_triangles[m_numberTriangles - 1];
} }
/// Create a new triangle /// Create a new triangle
TriangleEPA* newTriangle(const vec3* _vertices, uint32_t _v0, uint32_t _v1, uint32_t _v2) { 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 we have not reached the maximum number of triangles
if (m_numberTriangles != MAX_TRIANGLES) { if (m_triangles.size() < MAX_TRIANGLES) {
newTriangle = &m_triangles[m_numberTriangles++]; TriangleEPA tmp(_v0, _v1, _v2);
newTriangle->set(_v0, _v1, _v2); if (!tmp.computeClosestPoint(_vertices)) {
if (!newTriangle->computeClosestPoint(_vertices)) { return nullptr;
m_numberTriangles--;
newTriangle = nullptr;
} }
m_triangles.pushBack(etk::move(tmp));
return &m_triangles.back();
} }
// Return the new triangle // We are at the limit (internal)
return newTriangle; return nullptr;
} }
/// Access operator /// Access operator
TriangleEPA& operator[](int32_t _id) { TriangleEPA& operator[](int32_t _id) {

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

@ -53,10 +53,17 @@ void ConcaveMeshShape::getTriangleVerticesWithIndexPointer(int32_t _subPart, int
} }
void ConcaveMeshShape::testAllTriangles(TriangleCallback& _callback, const AABB& _localAABB) const { void ConcaveMeshShape::testAllTriangles(TriangleCallback& _callback, const AABB& _localAABB) const {
ConvexTriangleAABBOverlapCallback overlapCallback(_callback, *this, m_dynamicAABBTree);
// Ask the Dynamic AABB Tree to report all the triangles that are overlapping // Ask the Dynamic AABB Tree to report all the triangles that are overlapping
// with the AABB of the convex shape. // with the AABB of the convex shape.
m_dynamicAABBTree.reportAllShapesOverlappingWithAABB(_localAABB, overlapCallback); m_dynamicAABBTree.reportAllShapesOverlappingWithAABB(_localAABB, [&](int32_t _nodeId) {
// Get the node data (triangle index and mesh subpart index)
int32_t* data = m_dynamicAABBTree.getNodeDataInt(_nodeId);
// Get the triangle vertices for this node from the concave mesh shape
vec3 trianglePoints[3];
getTriangleVerticesWithIndexPointer(data[0], data[1], trianglePoints);
// Call the callback to test narrow-phase collision with this triangle
_callback.testTriangle(trianglePoints);
});
} }
bool ConcaveMeshShape::raycast(const Ray& _ray, RaycastInfo& _raycastInfo, ProxyShape* _proxyShape) const { bool ConcaveMeshShape::raycast(const Ray& _ray, RaycastInfo& _raycastInfo, ProxyShape* _proxyShape) const {
@ -66,12 +73,12 @@ bool ConcaveMeshShape::raycast(const Ray& _ray, RaycastInfo& _raycastInfo, Proxy
// Ask the Dynamic AABB Tree to report all AABB nodes that are hit by the ray. // Ask the Dynamic AABB Tree to report all AABB nodes that are hit by the ray.
// The raycastCallback object will then compute ray casting against the triangles // The raycastCallback object will then compute ray casting against the triangles
// in the hit AABBs. // in the hit AABBs.
m_dynamicAABBTree.raycast(_ray, raycastCallback); m_dynamicAABBTree.raycast(_ray, [&](int32_t _nodeId, const ephysics::Ray& _ray) mutable { return raycastCallback(_nodeId, _ray);});
raycastCallback.raycastTriangles(); raycastCallback.raycastTriangles();
return raycastCallback.getIsHit(); return raycastCallback.getIsHit();
} }
float ConcaveMeshRaycastCallback::raycastBroadPhaseShape(int32_t _nodeId, const Ray& _ray) { float ConcaveMeshRaycastCallback::operator()(int32_t _nodeId, const Ray& _ray) {
// Add the id of the hit AABB node int32_to // Add the id of the hit AABB node int32_to
m_hitAABBNodes.pushBack(_nodeId); m_hitAABBNodes.pushBack(_nodeId);
return _ray.maxFraction; return _ray.maxFraction;
@ -104,7 +111,7 @@ void ConcaveMeshRaycastCallback::raycastTriangles() {
m_raycastInfo.meshSubpart = data[0]; m_raycastInfo.meshSubpart = data[0];
m_raycastInfo.triangleIndex = data[1]; m_raycastInfo.triangleIndex = data[1];
smallestHitFraction = raycastInfo.hitFraction; smallestHitFraction = raycastInfo.hitFraction;
mIsHit = true; m_isHit = true;
} }
} }
} }
@ -136,17 +143,4 @@ void ConcaveMeshShape::computeLocalInertiaTensor(etk::Matrix3x3& _tensor, float
0, 0, _mass); 0, 0, _mass);
} }
void ConvexTriangleAABBOverlapCallback::notifyOverlappingNode(int32_t _nodeId) {
// Get the node data (triangle index and mesh subpart index)
int32_t* data = m_dynamicAABBTree.getNodeDataInt(_nodeId);
// Get the triangle vertices for this node from the concave mesh shape
vec3 trianglePoints[3];
m_concaveMeshShape.getTriangleVerticesWithIndexPointer(data[0], data[1], trianglePoints);
// Call the callback to test narrow-phase collision with this triangle
m_triangleTestCallback.testTriangle(trianglePoints);
}

30
ephysics/collision/shapes/ConcaveMeshShape.hpp
View File

@ -13,27 +13,7 @@
namespace ephysics { namespace ephysics {
class ConcaveMeshShape; class ConcaveMeshShape;
class ConvexTriangleAABBOverlapCallback : public DynamicAABBTreeOverlapCallback { class ConcaveMeshRaycastCallback {
private:
TriangleCallback& m_triangleTestCallback; //!<
const ConcaveMeshShape& m_concaveMeshShape; //!< Reference to the concave mesh shape
const DynamicAABBTree& m_dynamicAABBTree; //!< Reference to the Dynamic AABB tree
public:
// Constructor
ConvexTriangleAABBOverlapCallback(TriangleCallback& _triangleCallback,
const ConcaveMeshShape& _concaveShape,
const DynamicAABBTree& _dynamicAABBTree):
m_triangleTestCallback(_triangleCallback),
m_concaveMeshShape(_concaveShape),
m_dynamicAABBTree(_dynamicAABBTree) {
}
// Called when a overlapping node has been found during the call to
// DynamicAABBTree:reportAllShapesOverlappingWithAABB()
virtual void notifyOverlappingNode(int32_t _nodeId);
};
class ConcaveMeshRaycastCallback : public DynamicAABBTreeRaycastCallback {
private : private :
etk::Vector<int32_t> m_hitAABBNodes; etk::Vector<int32_t> m_hitAABBNodes;
const DynamicAABBTree& m_dynamicAABBTree; const DynamicAABBTree& m_dynamicAABBTree;
@ -41,7 +21,7 @@ namespace ephysics {
ProxyShape* m_proxyShape; ProxyShape* m_proxyShape;
RaycastInfo& m_raycastInfo; RaycastInfo& m_raycastInfo;
const Ray& m_ray; const Ray& m_ray;
bool mIsHit; bool m_isHit;
public: public:
// Constructor // Constructor
ConcaveMeshRaycastCallback(const DynamicAABBTree& _dynamicAABBTree, ConcaveMeshRaycastCallback(const DynamicAABBTree& _dynamicAABBTree,
@ -54,16 +34,16 @@ namespace ephysics {
m_proxyShape(_proxyShape), m_proxyShape(_proxyShape),
m_raycastInfo(_raycastInfo), m_raycastInfo(_raycastInfo),
m_ray(_ray), m_ray(_ray),
mIsHit(false) { m_isHit(false) {
} }
/// Collect all the AABB nodes that are hit by the ray in the Dynamic AABB Tree /// Collect all the AABB nodes that are hit by the ray in the Dynamic AABB Tree
virtual float raycastBroadPhaseShape(int32_t _nodeId, const Ray& _ray); float operator()(int32_t _nodeId, const ephysics::Ray& _ray);
/// Raycast all collision shapes that have been collected /// Raycast all collision shapes that have been collected
void raycastTriangles(); void raycastTriangles();
/// Return true if a raycast hit has been found /// Return true if a raycast hit has been found
bool getIsHit() const { bool getIsHit() const {
return mIsHit; return m_isHit;
} }
}; };
/** /**

2
ephysics/engine/CollisionWorld.cpp
View File

@ -37,7 +37,7 @@ CollisionBody* CollisionWorld::createCollisionBody(const etk::Transform3D& trans
// Get the next available body ID // Get the next available body ID
bodyindex bodyID = computeNextAvailableBodyID(); bodyindex bodyID = computeNextAvailableBodyID();
// Largest index cannot be used (it is used for invalid index) // Largest index cannot be used (it is used for invalid index)
EPHY_ASSERT(bodyID < std::numeric_limits<ephysics::bodyindex>::max(), "index too big"); EPHY_ASSERT(bodyID < UINT64_MAX, "index too big");
// Create the collision body // Create the collision body
CollisionBody* collisionBody = new CollisionBody(transform, *this, bodyID); CollisionBody* collisionBody = new CollisionBody(transform, *this, bodyID);
EPHY_ASSERT(collisionBody != nullptr, "empty Body collision"); EPHY_ASSERT(collisionBody != nullptr, "empty Body collision");

2
ephysics/engine/ContactSolver.cpp
View File

@ -521,7 +521,7 @@ void ContactSolver::solve() {
float beta = m_isSplitImpulseActive ? BETA_SPLIT_IMPULSE : BETA; float beta = m_isSplitImpulseActive ? BETA_SPLIT_IMPULSE : BETA;
float biasPenetrationDepth = 0.0; float biasPenetrationDepth = 0.0;
if (contactPoint.penetrationDepth > SLOP) biasPenetrationDepth = -(beta/m_timeStep) * if (contactPoint.penetrationDepth > SLOP) biasPenetrationDepth = -(beta/m_timeStep) *
max(0.0f, float(contactPoint.penetrationDepth - SLOP)); etk::max(0.0f, float(contactPoint.penetrationDepth - SLOP));
float b = biasPenetrationDepth + contactPoint.restitutionBias; float b = biasPenetrationDepth + contactPoint.restitutionBias;
// Compute the Lagrange multiplier lambda // Compute the Lagrange multiplier lambda

2
ephysics/engine/DynamicsWorld.cpp
View File

@ -329,7 +329,7 @@ ephysics::RigidBody* ephysics::DynamicsWorld::createRigidBody(const etk::Transfo
// Compute the body ID // Compute the body ID
ephysics::bodyindex bodyID = computeNextAvailableBodyID(); ephysics::bodyindex bodyID = computeNextAvailableBodyID();
// Largest index cannot be used (it is used for invalid index) // Largest index cannot be used (it is used for invalid index)
assert(bodyID < std::numeric_limits<ephysics::bodyindex>::max()); assert(bodyID < UINT64_MAX);
// Create the rigid body // Create the rigid body
ephysics::RigidBody* rigidBody = new RigidBody(_transform, *this, bodyID); ephysics::RigidBody* rigidBody = new RigidBody(_transform, *this, bodyID);
assert(rigidBody != nullptr); assert(rigidBody != nullptr);

1
lutin_ephysics.py
View File

@ -150,7 +150,6 @@ def configure(target, my_module):
my_module.compile_version("c++", 2011) my_module.compile_version("c++", 2011)
# add dependency of the generic C++ library: # add dependency of the generic C++ library:
my_module.add_depend([ my_module.add_depend([
'cxx',
'm', 'm',
'elog', 'elog',
'etk', 'etk',

82
test/testDynamicAABBTree.cpp
View File

@ -10,35 +10,35 @@
#include <etk/Vector.hpp> #include <etk/Vector.hpp>
class OverlapCallback : public ephysics::DynamicAABBTreeOverlapCallback { class OverlapCallback {
public : public :
etk::Vector<int32_t> m_overlapNodes; etk::Vector<int32_t> m_overlapNodes;
// Called when a overlapping node has been found during the call to // Called when a overlapping node has been found during the call to
// DynamicAABBTree:reportAllShapesOverlappingWithAABB() // DynamicAABBTree:reportAllShapesOverlappingWithAABB()
virtual void notifyOverlappingNode(int32_t nodeId) { void operator()(int32_t _nodeId) {
m_overlapNodes.pushBack(nodeId); m_overlapNodes.pushBack(_nodeId);
} }
void reset() { void reset() {
m_overlapNodes.clear(); m_overlapNodes.clear();
} }
bool isOverlapping(int32_t nodeId) const { bool isOverlapping(int32_t _nodeId) const {
return etk::isIn(nodeId, m_overlapNodes); return etk::isIn(_nodeId, m_overlapNodes);
} }
}; };
class DynamicTreeRaycastCallback : public ephysics::DynamicAABBTreeRaycastCallback { class DynamicTreeRaycastCallback {
public: public:
etk::Vector<int32_t> m_hitNodes; etk::Vector<int32_t> m_hitNodes;
// Called when the AABB of a leaf node is hit by a ray // Called when the AABB of a leaf node is hit by a ray
virtual float raycastBroadPhaseShape(int32_t nodeId, const ephysics::Ray& ray) { float operator()(int32_t _nodeId, const ephysics::Ray& _ray) {
m_hitNodes.pushBack(nodeId); m_hitNodes.pushBack(_nodeId);
return 1.0; return 1.0;
} }
void reset() { void reset() {
m_hitNodes.clear(); m_hitNodes.clear();
} }
bool isHit(int32_t nodeId) const { bool isHit(int32_t _nodeId) const {
return etk::isIn(nodeId, m_hitNodes); return etk::isIn(_nodeId, m_hitNodes);
} }
}; };
@ -115,7 +115,7 @@ TEST(TestAABBTree, overlapping) {
// AABB overlapping nothing // AABB overlapping nothing
overlapCallback.reset(); overlapCallback.reset();
tree.reportAllShapesOverlappingWithAABB(ephysics::AABB(vec3(-10, 12, -4), vec3(10, 50, 4)), overlapCallback); tree.reportAllShapesOverlappingWithAABB(ephysics::AABB(vec3(-10, 12, -4), vec3(10, 50, 4)), [&](int32_t _nodeId) mutable { overlapCallback(_nodeId);});
EXPECT_EQ(overlapCallback.isOverlapping(object1Id), false); EXPECT_EQ(overlapCallback.isOverlapping(object1Id), false);
EXPECT_EQ(overlapCallback.isOverlapping(object2Id), false); EXPECT_EQ(overlapCallback.isOverlapping(object2Id), false);
EXPECT_EQ(overlapCallback.isOverlapping(object3Id), false); EXPECT_EQ(overlapCallback.isOverlapping(object3Id), false);
@ -123,7 +123,7 @@ TEST(TestAABBTree, overlapping) {
// AABB overlapping everything // AABB overlapping everything
overlapCallback.reset(); overlapCallback.reset();
tree.reportAllShapesOverlappingWithAABB(ephysics::AABB(vec3(-15, -15, -4), vec3(15, 15, 4)), overlapCallback); tree.reportAllShapesOverlappingWithAABB(ephysics::AABB(vec3(-15, -15, -4), vec3(15, 15, 4)), [&](int32_t _nodeId) mutable { overlapCallback(_nodeId);});
EXPECT_EQ(overlapCallback.isOverlapping(object1Id), true); EXPECT_EQ(overlapCallback.isOverlapping(object1Id), true);
EXPECT_EQ(overlapCallback.isOverlapping(object2Id), true); EXPECT_EQ(overlapCallback.isOverlapping(object2Id), true);
EXPECT_EQ(overlapCallback.isOverlapping(object3Id), true); EXPECT_EQ(overlapCallback.isOverlapping(object3Id), true);
@ -131,7 +131,7 @@ TEST(TestAABBTree, overlapping) {
// AABB overlapping object 1 and 3 // AABB overlapping object 1 and 3
overlapCallback.reset(); overlapCallback.reset();
tree.reportAllShapesOverlappingWithAABB(ephysics::AABB(vec3(-4, 2, -4), vec3(-1, 7, 4)), overlapCallback); tree.reportAllShapesOverlappingWithAABB(ephysics::AABB(vec3(-4, 2, -4), vec3(-1, 7, 4)), [&](int32_t _nodeId) mutable { overlapCallback(_nodeId);});
EXPECT_EQ(overlapCallback.isOverlapping(object1Id), true); EXPECT_EQ(overlapCallback.isOverlapping(object1Id), true);
EXPECT_EQ(overlapCallback.isOverlapping(object2Id), false); EXPECT_EQ(overlapCallback.isOverlapping(object2Id), false);
EXPECT_EQ(overlapCallback.isOverlapping(object3Id), true); EXPECT_EQ(overlapCallback.isOverlapping(object3Id), true);
@ -139,7 +139,7 @@ TEST(TestAABBTree, overlapping) {
// AABB overlapping object 3 and 4 // AABB overlapping object 3 and 4
overlapCallback.reset(); overlapCallback.reset();
tree.reportAllShapesOverlappingWithAABB(ephysics::AABB(vec3(-6, -5, -2), vec3(2, 2, 0)), overlapCallback); tree.reportAllShapesOverlappingWithAABB(ephysics::AABB(vec3(-6, -5, -2), vec3(2, 2, 0)), [&](int32_t _nodeId) mutable { overlapCallback(_nodeId);});
EXPECT_EQ(overlapCallback.isOverlapping(object1Id), false); EXPECT_EQ(overlapCallback.isOverlapping(object1Id), false);
EXPECT_EQ(overlapCallback.isOverlapping(object2Id), false); EXPECT_EQ(overlapCallback.isOverlapping(object2Id), false);
EXPECT_EQ(overlapCallback.isOverlapping(object3Id), true); EXPECT_EQ(overlapCallback.isOverlapping(object3Id), true);
@ -147,7 +147,7 @@ TEST(TestAABBTree, overlapping) {
// AABB overlapping object 2 // AABB overlapping object 2
overlapCallback.reset(); overlapCallback.reset();
tree.reportAllShapesOverlappingWithAABB(ephysics::AABB(vec3(5, -10, -2), vec3(7, 10, 9)), overlapCallback); tree.reportAllShapesOverlappingWithAABB(ephysics::AABB(vec3(5, -10, -2), vec3(7, 10, 9)), [&](int32_t _nodeId) mutable { overlapCallback(_nodeId);});
EXPECT_EQ(overlapCallback.isOverlapping(object1Id), false); EXPECT_EQ(overlapCallback.isOverlapping(object1Id), false);
EXPECT_EQ(overlapCallback.isOverlapping(object2Id), true); EXPECT_EQ(overlapCallback.isOverlapping(object2Id), true);
EXPECT_EQ(overlapCallback.isOverlapping(object3Id), false); EXPECT_EQ(overlapCallback.isOverlapping(object3Id), false);
@ -162,7 +162,7 @@ TEST(TestAABBTree, overlapping) {
// AABB overlapping nothing // AABB overlapping nothing
overlapCallback.reset(); overlapCallback.reset();
tree.reportAllShapesOverlappingWithAABB(ephysics::AABB(vec3(-10, 12, -4), vec3(10, 50, 4)), overlapCallback); tree.reportAllShapesOverlappingWithAABB(ephysics::AABB(vec3(-10, 12, -4), vec3(10, 50, 4)), [&](int32_t _nodeId) mutable { overlapCallback(_nodeId);});
EXPECT_EQ(overlapCallback.isOverlapping(object1Id), false); EXPECT_EQ(overlapCallback.isOverlapping(object1Id), false);
EXPECT_EQ(overlapCallback.isOverlapping(object2Id), false); EXPECT_EQ(overlapCallback.isOverlapping(object2Id), false);
EXPECT_EQ(overlapCallback.isOverlapping(object3Id), false); EXPECT_EQ(overlapCallback.isOverlapping(object3Id), false);
@ -170,7 +170,7 @@ TEST(TestAABBTree, overlapping) {
// AABB overlapping everything // AABB overlapping everything
overlapCallback.reset(); overlapCallback.reset();
tree.reportAllShapesOverlappingWithAABB(ephysics::AABB(vec3(-15, -15, -4), vec3(15, 15, 4)), overlapCallback); tree.reportAllShapesOverlappingWithAABB(ephysics::AABB(vec3(-15, -15, -4), vec3(15, 15, 4)), [&](int32_t _nodeId) mutable { overlapCallback(_nodeId);});
EXPECT_EQ(overlapCallback.isOverlapping(object1Id), true); EXPECT_EQ(overlapCallback.isOverlapping(object1Id), true);
EXPECT_EQ(overlapCallback.isOverlapping(object2Id), true); EXPECT_EQ(overlapCallback.isOverlapping(object2Id), true);
EXPECT_EQ(overlapCallback.isOverlapping(object3Id), true); EXPECT_EQ(overlapCallback.isOverlapping(object3Id), true);
@ -178,7 +178,7 @@ TEST(TestAABBTree, overlapping) {
// AABB overlapping object 1 and 3 // AABB overlapping object 1 and 3
overlapCallback.reset(); overlapCallback.reset();
tree.reportAllShapesOverlappingWithAABB(ephysics::AABB(vec3(-4, 2, -4), vec3(-1, 7, 4)), overlapCallback); tree.reportAllShapesOverlappingWithAABB(ephysics::AABB(vec3(-4, 2, -4), vec3(-1, 7, 4)), [&](int32_t _nodeId) mutable { overlapCallback(_nodeId);});
EXPECT_EQ(overlapCallback.isOverlapping(object1Id), true); EXPECT_EQ(overlapCallback.isOverlapping(object1Id), true);
EXPECT_EQ(overlapCallback.isOverlapping(object2Id), false); EXPECT_EQ(overlapCallback.isOverlapping(object2Id), false);
EXPECT_EQ(overlapCallback.isOverlapping(object3Id), true); EXPECT_EQ(overlapCallback.isOverlapping(object3Id), true);
@ -186,7 +186,7 @@ TEST(TestAABBTree, overlapping) {
// AABB overlapping object 3 and 4 // AABB overlapping object 3 and 4
overlapCallback.reset(); overlapCallback.reset();
tree.reportAllShapesOverlappingWithAABB(ephysics::AABB(vec3(-6, -5, -2), vec3(2, 2, 0)), overlapCallback); tree.reportAllShapesOverlappingWithAABB(ephysics::AABB(vec3(-6, -5, -2), vec3(2, 2, 0)), [&](int32_t _nodeId) mutable { overlapCallback(_nodeId);});
EXPECT_EQ(overlapCallback.isOverlapping(object1Id), false); EXPECT_EQ(overlapCallback.isOverlapping(object1Id), false);
EXPECT_EQ(overlapCallback.isOverlapping(object2Id), false); EXPECT_EQ(overlapCallback.isOverlapping(object2Id), false);
EXPECT_EQ(overlapCallback.isOverlapping(object3Id), true); EXPECT_EQ(overlapCallback.isOverlapping(object3Id), true);
@ -194,7 +194,7 @@ TEST(TestAABBTree, overlapping) {
// AABB overlapping object 2 // AABB overlapping object 2
overlapCallback.reset(); overlapCallback.reset();
tree.reportAllShapesOverlappingWithAABB(ephysics::AABB(vec3(5, -10, -2), vec3(7, 10, 9)), overlapCallback); tree.reportAllShapesOverlappingWithAABB(ephysics::AABB(vec3(5, -10, -2), vec3(7, 10, 9)), [&](int32_t _nodeId) mutable { overlapCallback(_nodeId);});
EXPECT_EQ(overlapCallback.isOverlapping(object1Id), false); EXPECT_EQ(overlapCallback.isOverlapping(object1Id), false);
EXPECT_EQ(overlapCallback.isOverlapping(object2Id), true); EXPECT_EQ(overlapCallback.isOverlapping(object2Id), true);
EXPECT_EQ(overlapCallback.isOverlapping(object3Id), false); EXPECT_EQ(overlapCallback.isOverlapping(object3Id), false);
@ -209,7 +209,7 @@ TEST(TestAABBTree, overlapping) {
// AABB overlapping nothing // AABB overlapping nothing
overlapCallback.reset(); overlapCallback.reset();
tree.reportAllShapesOverlappingWithAABB(ephysics::AABB(vec3(-10, 12, -4), vec3(10, 50, 4)), overlapCallback); tree.reportAllShapesOverlappingWithAABB(ephysics::AABB(vec3(-10, 12, -4), vec3(10, 50, 4)), [&](int32_t _nodeId) mutable { overlapCallback(_nodeId);});
EXPECT_EQ(overlapCallback.isOverlapping(object1Id), false); EXPECT_EQ(overlapCallback.isOverlapping(object1Id), false);
EXPECT_EQ(overlapCallback.isOverlapping(object2Id), false); EXPECT_EQ(overlapCallback.isOverlapping(object2Id), false);
EXPECT_EQ(overlapCallback.isOverlapping(object3Id), false); EXPECT_EQ(overlapCallback.isOverlapping(object3Id), false);
@ -217,7 +217,7 @@ TEST(TestAABBTree, overlapping) {
// AABB overlapping everything // AABB overlapping everything
overlapCallback.reset(); overlapCallback.reset();
tree.reportAllShapesOverlappingWithAABB(ephysics::AABB(vec3(-15, -15, -4), vec3(15, 15, 4)), overlapCallback); tree.reportAllShapesOverlappingWithAABB(ephysics::AABB(vec3(-15, -15, -4), vec3(15, 15, 4)), [&](int32_t _nodeId) mutable { overlapCallback(_nodeId);});
EXPECT_EQ(overlapCallback.isOverlapping(object1Id), true); EXPECT_EQ(overlapCallback.isOverlapping(object1Id), true);
EXPECT_EQ(overlapCallback.isOverlapping(object2Id), true); EXPECT_EQ(overlapCallback.isOverlapping(object2Id), true);
EXPECT_EQ(overlapCallback.isOverlapping(object3Id), true); EXPECT_EQ(overlapCallback.isOverlapping(object3Id), true);
@ -225,7 +225,7 @@ TEST(TestAABBTree, overlapping) {
// AABB overlapping object 1 and 3 // AABB overlapping object 1 and 3
overlapCallback.reset(); overlapCallback.reset();
tree.reportAllShapesOverlappingWithAABB(ephysics::AABB(vec3(-4, 2, -4), vec3(-1, 7, 4)), overlapCallback); tree.reportAllShapesOverlappingWithAABB(ephysics::AABB(vec3(-4, 2, -4), vec3(-1, 7, 4)), [&](int32_t _nodeId) mutable { overlapCallback(_nodeId);});
EXPECT_EQ(overlapCallback.isOverlapping(object1Id), true); EXPECT_EQ(overlapCallback.isOverlapping(object1Id), true);
EXPECT_EQ(overlapCallback.isOverlapping(object2Id), false); EXPECT_EQ(overlapCallback.isOverlapping(object2Id), false);
EXPECT_EQ(overlapCallback.isOverlapping(object3Id), true); EXPECT_EQ(overlapCallback.isOverlapping(object3Id), true);
@ -233,7 +233,7 @@ TEST(TestAABBTree, overlapping) {
// AABB overlapping object 3 and 4 // AABB overlapping object 3 and 4
overlapCallback.reset(); overlapCallback.reset();
tree.reportAllShapesOverlappingWithAABB(ephysics::AABB(vec3(-6, -5, -2), vec3(2, 2, 0)), overlapCallback); tree.reportAllShapesOverlappingWithAABB(ephysics::AABB(vec3(-6, -5, -2), vec3(2, 2, 0)), [&](int32_t _nodeId) mutable { overlapCallback(_nodeId);});
EXPECT_EQ(overlapCallback.isOverlapping(object1Id), false); EXPECT_EQ(overlapCallback.isOverlapping(object1Id), false);
EXPECT_EQ(overlapCallback.isOverlapping(object2Id), false); EXPECT_EQ(overlapCallback.isOverlapping(object2Id), false);
EXPECT_EQ(overlapCallback.isOverlapping(object3Id), true); EXPECT_EQ(overlapCallback.isOverlapping(object3Id), true);
@ -241,7 +241,7 @@ TEST(TestAABBTree, overlapping) {
// AABB overlapping object 2 // AABB overlapping object 2
overlapCallback.reset(); overlapCallback.reset();
tree.reportAllShapesOverlappingWithAABB(ephysics::AABB(vec3(5, -10, -2), vec3(7, 10, 9)), overlapCallback); tree.reportAllShapesOverlappingWithAABB(ephysics::AABB(vec3(5, -10, -2), vec3(7, 10, 9)), [&](int32_t _nodeId) mutable { overlapCallback(_nodeId);});
EXPECT_EQ(overlapCallback.isOverlapping(object1Id), false); EXPECT_EQ(overlapCallback.isOverlapping(object1Id), false);
EXPECT_EQ(overlapCallback.isOverlapping(object2Id), true); EXPECT_EQ(overlapCallback.isOverlapping(object2Id), true);
EXPECT_EQ(overlapCallback.isOverlapping(object3Id), false); EXPECT_EQ(overlapCallback.isOverlapping(object3Id), false);
@ -257,7 +257,7 @@ TEST(TestAABBTree, overlapping) {
// AABB overlapping object 3 // AABB overlapping object 3
overlapCallback.reset(); overlapCallback.reset();
tree.reportAllShapesOverlappingWithAABB(ephysics::AABB(vec3(6, -10, -2), vec3(8, 5, 3)), overlapCallback); tree.reportAllShapesOverlappingWithAABB(ephysics::AABB(vec3(6, -10, -2), vec3(8, 5, 3)), [&](int32_t _nodeId) mutable { overlapCallback(_nodeId);});
EXPECT_EQ(overlapCallback.isOverlapping(object1Id), false); EXPECT_EQ(overlapCallback.isOverlapping(object1Id), false);
EXPECT_EQ(overlapCallback.isOverlapping(object2Id), false); EXPECT_EQ(overlapCallback.isOverlapping(object2Id), false);
EXPECT_EQ(overlapCallback.isOverlapping(object3Id), true); EXPECT_EQ(overlapCallback.isOverlapping(object3Id), true);
@ -265,7 +265,7 @@ TEST(TestAABBTree, overlapping) {
// AABB overlapping objects 1, 2 // AABB overlapping objects 1, 2
overlapCallback.reset(); overlapCallback.reset();
tree.reportAllShapesOverlappingWithAABB(ephysics::AABB(vec3(-8, 5, -3), vec3(-2, 11, 3)), overlapCallback); tree.reportAllShapesOverlappingWithAABB(ephysics::AABB(vec3(-8, 5, -3), vec3(-2, 11, 3)), [&](int32_t _nodeId) mutable { overlapCallback(_nodeId);});
EXPECT_EQ(overlapCallback.isOverlapping(object1Id), true); EXPECT_EQ(overlapCallback.isOverlapping(object1Id), true);
EXPECT_EQ(overlapCallback.isOverlapping(object2Id), true); EXPECT_EQ(overlapCallback.isOverlapping(object2Id), true);
EXPECT_EQ(overlapCallback.isOverlapping(object3Id), false); EXPECT_EQ(overlapCallback.isOverlapping(object3Id), false);
@ -303,7 +303,7 @@ TEST(TestAABBTree, raycast) {
// Ray with no hits // Ray with no hits
raycastCallback.reset(); raycastCallback.reset();
ephysics::Ray ray1(vec3(4.5, -10, -5), vec3(4.5, 10, -5)); ephysics::Ray ray1(vec3(4.5, -10, -5), vec3(4.5, 10, -5));
tree.raycast(ray1, raycastCallback); tree.raycast(ray1, [&](int32_t _nodeId, const ephysics::Ray& _ray) mutable { return raycastCallback(_nodeId, _ray);});
EXPECT_EQ(raycastCallback.isHit(object1Id), false); EXPECT_EQ(raycastCallback.isHit(object1Id), false);
EXPECT_EQ(raycastCallback.isHit(object2Id), false); EXPECT_EQ(raycastCallback.isHit(object2Id), false);
EXPECT_EQ(raycastCallback.isHit(object3Id), false); EXPECT_EQ(raycastCallback.isHit(object3Id), false);
@ -312,7 +312,7 @@ TEST(TestAABBTree, raycast) {
// Ray that hits object 1 // Ray that hits object 1
raycastCallback.reset(); raycastCallback.reset();
ephysics::Ray ray2(vec3(-1, -20, -2), vec3(-1, 20, -2)); ephysics::Ray ray2(vec3(-1, -20, -2), vec3(-1, 20, -2));
tree.raycast(ray2, raycastCallback); tree.raycast(ray2, [&](int32_t _nodeId, const ephysics::Ray& _ray) mutable { return raycastCallback(_nodeId, _ray);});
EXPECT_EQ(raycastCallback.isHit(object1Id), true); EXPECT_EQ(raycastCallback.isHit(object1Id), true);
EXPECT_EQ(raycastCallback.isHit(object2Id), false); EXPECT_EQ(raycastCallback.isHit(object2Id), false);
EXPECT_EQ(raycastCallback.isHit(object3Id), false); EXPECT_EQ(raycastCallback.isHit(object3Id), false);
@ -321,7 +321,7 @@ TEST(TestAABBTree, raycast) {
// Ray that hits object 1 and 2 // Ray that hits object 1 and 2
raycastCallback.reset(); raycastCallback.reset();
ephysics::Ray ray3(vec3(-7, 6, -2), vec3(8, 6, -2)); ephysics::Ray ray3(vec3(-7, 6, -2), vec3(8, 6, -2));
tree.raycast(ray3, raycastCallback); tree.raycast(ray3, [&](int32_t _nodeId, const ephysics::Ray& _ray) mutable { return raycastCallback(_nodeId, _ray);});
EXPECT_EQ(raycastCallback.isHit(object1Id), true); EXPECT_EQ(raycastCallback.isHit(object1Id), true);
EXPECT_EQ(raycastCallback.isHit(object2Id), true); EXPECT_EQ(raycastCallback.isHit(object2Id), true);
EXPECT_EQ(raycastCallback.isHit(object3Id), false); EXPECT_EQ(raycastCallback.isHit(object3Id), false);
@ -330,7 +330,7 @@ TEST(TestAABBTree, raycast) {
// Ray that hits object 3 // Ray that hits object 3
raycastCallback.reset(); raycastCallback.reset();
ephysics::Ray ray4(vec3(-7, 2, 0), vec3(-1, 2, 0)); ephysics::Ray ray4(vec3(-7, 2, 0), vec3(-1, 2, 0));
tree.raycast(ray4, raycastCallback); tree.raycast(ray4, [&](int32_t _nodeId, const ephysics::Ray& _ray) mutable { return raycastCallback(_nodeId, _ray);});
EXPECT_EQ(raycastCallback.isHit(object1Id), false); EXPECT_EQ(raycastCallback.isHit(object1Id), false);
EXPECT_EQ(raycastCallback.isHit(object2Id), false); EXPECT_EQ(raycastCallback.isHit(object2Id), false);
EXPECT_EQ(raycastCallback.isHit(object3Id), true); EXPECT_EQ(raycastCallback.isHit(object3Id), true);
@ -345,7 +345,7 @@ TEST(TestAABBTree, raycast) {
// Ray with no hits // Ray with no hits
raycastCallback.reset(); raycastCallback.reset();
tree.raycast(ray1, raycastCallback); tree.raycast(ray1, [&](int32_t _nodeId, const ephysics::Ray& _ray) mutable { return raycastCallback(_nodeId, _ray);});
EXPECT_EQ(raycastCallback.isHit(object1Id), false); EXPECT_EQ(raycastCallback.isHit(object1Id), false);
EXPECT_EQ(raycastCallback.isHit(object2Id), false); EXPECT_EQ(raycastCallback.isHit(object2Id), false);
EXPECT_EQ(raycastCallback.isHit(object3Id), false); EXPECT_EQ(raycastCallback.isHit(object3Id), false);
@ -353,7 +353,7 @@ TEST(TestAABBTree, raycast) {
// Ray that hits object 1 // Ray that hits object 1
raycastCallback.reset(); raycastCallback.reset();
tree.raycast(ray2, raycastCallback); tree.raycast(ray2, [&](int32_t _nodeId, const ephysics::Ray& _ray) mutable { return raycastCallback(_nodeId, _ray);});
EXPECT_EQ(raycastCallback.isHit(object1Id), true); EXPECT_EQ(raycastCallback.isHit(object1Id), true);
EXPECT_EQ(raycastCallback.isHit(object2Id), false); EXPECT_EQ(raycastCallback.isHit(object2Id), false);
EXPECT_EQ(raycastCallback.isHit(object3Id), false); EXPECT_EQ(raycastCallback.isHit(object3Id), false);
@ -361,7 +361,7 @@ TEST(TestAABBTree, raycast) {
// Ray that hits object 1 and 2 // Ray that hits object 1 and 2
raycastCallback.reset(); raycastCallback.reset();
tree.raycast(ray3, raycastCallback); tree.raycast(ray3, [&](int32_t _nodeId, const ephysics::Ray& _ray) mutable { return raycastCallback(_nodeId, _ray);});
EXPECT_EQ(raycastCallback.isHit(object1Id), true); EXPECT_EQ(raycastCallback.isHit(object1Id), true);
EXPECT_EQ(raycastCallback.isHit(object2Id), true); EXPECT_EQ(raycastCallback.isHit(object2Id), true);
EXPECT_EQ(raycastCallback.isHit(object3Id), false); EXPECT_EQ(raycastCallback.isHit(object3Id), false);
@ -369,7 +369,7 @@ TEST(TestAABBTree, raycast) {
// Ray that hits object 3 // Ray that hits object 3
raycastCallback.reset(); raycastCallback.reset();
tree.raycast(ray4, raycastCallback); tree.raycast(ray4, [&](int32_t _nodeId, const ephysics::Ray& _ray) mutable { return raycastCallback(_nodeId, _ray);});
EXPECT_EQ(raycastCallback.isHit(object1Id), false); EXPECT_EQ(raycastCallback.isHit(object1Id), false);
EXPECT_EQ(raycastCallback.isHit(object2Id), false); EXPECT_EQ(raycastCallback.isHit(object2Id), false);
EXPECT_EQ(raycastCallback.isHit(object3Id), true); EXPECT_EQ(raycastCallback.isHit(object3Id), true);
@ -384,7 +384,7 @@ TEST(TestAABBTree, raycast) {
// Ray with no hits // Ray with no hits
raycastCallback.reset(); raycastCallback.reset();
tree.raycast(ray1, raycastCallback); tree.raycast(ray1, [&](int32_t _nodeId, const ephysics::Ray& _ray) mutable { return raycastCallback(_nodeId, _ray);});
EXPECT_EQ(raycastCallback.isHit(object1Id), false); EXPECT_EQ(raycastCallback.isHit(object1Id), false);
EXPECT_EQ(raycastCallback.isHit(object2Id), false); EXPECT_EQ(raycastCallback.isHit(object2Id), false);
EXPECT_EQ(raycastCallback.isHit(object3Id), false); EXPECT_EQ(raycastCallback.isHit(object3Id), false);
@ -392,7 +392,7 @@ TEST(TestAABBTree, raycast) {
// Ray that hits object 1 // Ray that hits object 1
raycastCallback.reset(); raycastCallback.reset();
tree.raycast(ray2, raycastCallback); tree.raycast(ray2, [&](int32_t _nodeId, const ephysics::Ray& _ray) mutable { return raycastCallback(_nodeId, _ray);});
EXPECT_EQ(raycastCallback.isHit(object1Id), true); EXPECT_EQ(raycastCallback.isHit(object1Id), true);
EXPECT_EQ(raycastCallback.isHit(object2Id), false); EXPECT_EQ(raycastCallback.isHit(object2Id), false);
EXPECT_EQ(raycastCallback.isHit(object3Id), false); EXPECT_EQ(raycastCallback.isHit(object3Id), false);
@ -400,7 +400,7 @@ TEST(TestAABBTree, raycast) {
// Ray that hits object 1 and 2 // Ray that hits object 1 and 2
raycastCallback.reset(); raycastCallback.reset();
tree.raycast(ray3, raycastCallback); tree.raycast(ray3, [&](int32_t _nodeId, const ephysics::Ray& _ray) mutable { return raycastCallback(_nodeId, _ray);});
EXPECT_EQ(raycastCallback.isHit(object1Id), true); EXPECT_EQ(raycastCallback.isHit(object1Id), true);
EXPECT_EQ(raycastCallback.isHit(object2Id), true); EXPECT_EQ(raycastCallback.isHit(object2Id), true);
EXPECT_EQ(raycastCallback.isHit(object3Id), false); EXPECT_EQ(raycastCallback.isHit(object3Id), false);
@ -408,7 +408,7 @@ TEST(TestAABBTree, raycast) {
// Ray that hits object 3 // Ray that hits object 3
raycastCallback.reset(); raycastCallback.reset();
tree.raycast(ray4, raycastCallback); tree.raycast(ray4, [&](int32_t _nodeId, const ephysics::Ray& _ray) mutable { return raycastCallback(_nodeId, _ray);});
EXPECT_EQ(raycastCallback.isHit(object1Id), false); EXPECT_EQ(raycastCallback.isHit(object1Id), false);
EXPECT_EQ(raycastCallback.isHit(object2Id), false); EXPECT_EQ(raycastCallback.isHit(object2Id), false);
EXPECT_EQ(raycastCallback.isHit(object3Id), true); EXPECT_EQ(raycastCallback.isHit(object3Id), true);
@ -425,7 +425,7 @@ TEST(TestAABBTree, raycast) {
// Ray that hits object 1, 2 // Ray that hits object 1, 2
ephysics::Ray ray5(vec3(-4, -5, 0), vec3(-4, 12, 0)); ephysics::Ray ray5(vec3(-4, -5, 0), vec3(-4, 12, 0));
raycastCallback.reset(); raycastCallback.reset();
tree.raycast(ray5, raycastCallback); tree.raycast(ray5, [&](int32_t _nodeId, const ephysics::Ray& _ray) mutable { return raycastCallback(_nodeId, _ray);});
EXPECT_EQ(raycastCallback.isHit(object1Id), true); EXPECT_EQ(raycastCallback.isHit(object1Id), true);
EXPECT_EQ(raycastCallback.isHit(object2Id), true); EXPECT_EQ(raycastCallback.isHit(object2Id), true);
EXPECT_EQ(raycastCallback.isHit(object3Id), false); EXPECT_EQ(raycastCallback.isHit(object3Id), false);
@ -434,7 +434,7 @@ TEST(TestAABBTree, raycast) {
// Ray that hits object 3 and 4 // Ray that hits object 3 and 4
ephysics::Ray ray6(vec3(11, -3, 1), vec3(-2, -3, 1)); ephysics::Ray ray6(vec3(11, -3, 1), vec3(-2, -3, 1));
raycastCallback.reset(); raycastCallback.reset();
tree.raycast(ray6, raycastCallback); tree.raycast(ray6, [&](int32_t _nodeId, const ephysics::Ray& _ray) mutable { return raycastCallback(_nodeId, _ray);});
EXPECT_EQ(raycastCallback.isHit(object1Id), false); EXPECT_EQ(raycastCallback.isHit(object1Id), false);
EXPECT_EQ(raycastCallback.isHit(object2Id), false); EXPECT_EQ(raycastCallback.isHit(object2Id), false);
EXPECT_EQ(raycastCallback.isHit(object3Id), true); EXPECT_EQ(raycastCallback.isHit(object3Id), true);