diff --git a/ephysics/collision/broadphase/BroadPhaseAlgorithm.cpp b/ephysics/collision/broadphase/BroadPhaseAlgorithm.cpp index 87f884c..26176b5 100644 --- a/ephysics/collision/broadphase/BroadPhaseAlgorithm.cpp +++ b/ephysics/collision/broadphase/BroadPhaseAlgorithm.cpp @@ -13,25 +13,20 @@ using namespace ephysics; BroadPhaseAlgorithm::BroadPhaseAlgorithm(CollisionDetection& collisionDetection) :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) { // Allocate memory for the array of non-static proxy shapes IDs m_movedShapes = (int32_t*) malloc(m_numberAllocatedMovedShapes * sizeof(int32_t)); assert(m_movedShapes != NULL); - - // Allocate memory for the array of potential overlapping pairs - m_potentialPairs = (BroadPhasePair*) malloc(m_numberAllocatedPotentialPairs * sizeof(BroadPhasePair)); - assert(m_potentialPairs != NULL); + + m_potentialPairs.reserve(8); } BroadPhaseAlgorithm::~BroadPhaseAlgorithm() { // Release the memory for the array of non-static proxy shapes IDs free(m_movedShapes); - - // Release the memory for the array of potential overlapping pairs - free(m_potentialPairs); } void BroadPhaseAlgorithm::addMovedCollisionShape(int32_t broadPhaseID) { @@ -131,111 +126,72 @@ void BroadPhaseAlgorithm::updateProxyCollisionShape(ProxyShape* _proxyShape, } void BroadPhaseAlgorithm::computeOverlappingPairs() { - - // Reset the potential overlapping pairs - m_numberPotentialPairs = 0; - + m_potentialPairs.clear(); // For all collision shapes that have moved (or have been created) during the // last simulation step for (uint32_t i=0; i& _pair1, const etk::Pair& _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 // overlapping pairs - uint32_t i=0; - while (i < m_numberPotentialPairs) { - + uint32_t iii=0; + while (iii < m_potentialPairs.size()) { // Get a potential overlapping pair - BroadPhasePair* pair = m_potentialPairs + i; - i++; - - assert(pair->collisionShape1ID != pair->collisionShape2ID); - + const etk::Pair& pair = (m_potentialPairs[iii]); + ++iii; // Get the two collision shapes of the pair - ProxyShape* shape1 = static_cast(m_dynamicAABBTree.getNodeDataPointer(pair->collisionShape1ID)); - ProxyShape* shape2 = static_cast(m_dynamicAABBTree.getNodeDataPointer(pair->collisionShape2ID)); - + ProxyShape* shape1 = static_cast(m_dynamicAABBTree.getNodeDataPointer(pair.first)); + ProxyShape* shape2 = static_cast(m_dynamicAABBTree.getNodeDataPointer(pair.second)); // Notify the collision detection about the overlapping pair m_collisionDetection.broadPhaseNotifyOverlappingPair(shape1, shape2); - // Skip the duplicate overlapping pairs - while (i < m_numberPotentialPairs) { - + while (iii < m_potentialPairs.size()) { // Get the next pair - BroadPhasePair* nextPair = m_potentialPairs + i; - + const etk::Pair& nextPair = m_potentialPairs[iii]; // If the next pair is different from the previous one, we stop skipping pairs - if (nextPair->collisionShape1ID != pair->collisionShape1ID || - nextPair->collisionShape2ID != pair->collisionShape2ID) { + if ( nextPair.first != pair.first + || nextPair.second != pair.second) { 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) { - - // 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 BroadPhaseRaycastCallback::operator()(int32_t nodeId, const Ray& ray) { float hitFraction = float(-1.0); diff --git a/ephysics/collision/broadphase/BroadPhaseAlgorithm.hpp b/ephysics/collision/broadphase/BroadPhaseAlgorithm.hpp index 7c056de..c282140 100644 --- a/ephysics/collision/broadphase/BroadPhaseAlgorithm.hpp +++ b/ephysics/collision/broadphase/BroadPhaseAlgorithm.hpp @@ -16,49 +16,12 @@ namespace ephysics { class CollisionDetection; class BroadPhaseAlgorithm; - /** - * @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); - }; - + // TODO : remove this as callback ... DynamicAABBTreeOverlapCallback { /** * Callback called when the AABB of a leaf node is hit by a ray the * broad-phase Dynamic AABB Tree. */ - class BroadPhaseRaycastCallback : public DynamicAABBTreeRaycastCallback { + class BroadPhaseRaycastCallback { private : const DynamicAABBTree& m_dynamicAABBTree; unsigned short m_raycastWithCategoryMaskBits; @@ -74,7 +37,7 @@ namespace ephysics { } // 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_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. - BroadPhasePair* 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 + etk::Vector> m_potentialPairs; //!< Temporary array of potential overlapping pairs (with potential duplicates) CollisionDetection& m_collisionDetection; //!< Reference to the collision detection object /// Private copy-constructor 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 /// step and that need to be tested again for broad-phase overlapping. 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 void computeOverlappingPairs(); /// Return true if the two broad-phase collision shapes are overlapping diff --git a/ephysics/collision/broadphase/DynamicAABBTree.cpp b/ephysics/collision/broadphase/DynamicAABBTree.cpp index 5df6859..2a703b6 100644 --- a/ephysics/collision/broadphase/DynamicAABBTree.cpp +++ b/ephysics/collision/broadphase/DynamicAABBTree.cpp @@ -568,36 +568,31 @@ int32_t DynamicAABBTree::balanceSubTreeAtNode(int32_t _nodeID) { } /// Report all shapes overlapping with the AABB given in parameter. -void DynamicAABBTree::reportAllShapesOverlappingWithAABB(const AABB& aabb, - DynamicAABBTreeOverlapCallback& callback) const { - +void DynamicAABBTree::reportAllShapesOverlappingWithAABB(const AABB& _aabb, etk::Function _callback) const { + if (_callback == nullptr) { + EPHY_ERROR("call with nullptr callback"); + return; + } // Create a stack with the nodes to visit Stack stack; stack.push(m_rootNodeID); - // While there are still nodes to visit while(stack.getNbElements() > 0) { - // Get the next node ID to visit int32_t nodeIDToVisit = stack.pop(); - // 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 const TreeNode* nodeToVisit = m_nodes + nodeIDToVisit; - // 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 (nodeToVisit->isLeaf()) { - // Notify the broad-phase about a new potential overlapping pair - callback.notifyOverlappingNode(nodeIDToVisit); - } - else { // If the node is not a leaf - + _callback(nodeIDToVisit); + } else { // If the node is not a leaf // We need to visit its children stack.push(nodeToVisit->children[0]); stack.push(nodeToVisit->children[1]); @@ -607,60 +602,51 @@ void DynamicAABBTree::reportAllShapesOverlappingWithAABB(const AABB& aabb, } // Ray casting method -void DynamicAABBTree::raycast(const Ray& ray, DynamicAABBTreeRaycastCallback &callback) const { - +void DynamicAABBTree::raycast(const ephysics::Ray& _ray, etk::Function _callback) const { PROFILE("DynamicAABBTree::raycast()"); - - float maxFraction = ray.maxFraction; - + if (_callback == nullptr) { + EPHY_ERROR("call with nullptr callback"); + return; + } + float maxFraction = _ray.maxFraction; Stack stack; stack.push(m_rootNodeID); - // Walk through the tree from the root looking for proxy shapes // that overlap with the ray AABB while (stack.getNbElements() > 0) { - // Get the next node in the stack int32_t nodeID = stack.pop(); - // 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 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 - if (!node->aabb.testRayIntersect(rayTemp)) continue; - + if (node->aabb.testRayIntersect(rayTemp) == false) { + continue; + } // If the node is a leaf of the tree if (node->isLeaf()) { - // 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 // the raycasting should stop here if (hitFraction == 0.0f) { return; } - // If the user returned a positive fraction if (hitFraction > 0.0f) { - // We update the maxFraction value and the ray // AABB using the new maximum fraction if (hitFraction < maxFraction) { maxFraction = hitFraction; } } - // If the user returned a negative fraction, we continue // 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 stack.push(node->children[0]); stack.push(node->children[1]); diff --git a/ephysics/collision/broadphase/DynamicAABBTree.hpp b/ephysics/collision/broadphase/DynamicAABBTree.hpp index 1bf60bf..233e502 100644 --- a/ephysics/collision/broadphase/DynamicAABBTree.hpp +++ b/ephysics/collision/broadphase/DynamicAABBTree.hpp @@ -8,11 +8,11 @@ #include #include #include +#include namespace ephysics { class BroadPhaseAlgorithm; class BroadPhaseRaycastTestCallback; - class DynamicAABBTreeOverlapCallback; struct RaycastTest; /** * @brief It represents a node of the dynamic AABB tree. @@ -43,29 +43,6 @@ namespace ephysics { 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 * 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 void* getNodeDataPointer(int32_t _nodeID) const; /// Report all shapes overlapping with the AABB given in parameter. - void reportAllShapesOverlappingWithAABB(const AABB& _aabb, - DynamicAABBTreeOverlapCallback& _callback) const; + void reportAllShapesOverlappingWithAABB(const AABB& _aabb, etk::Function _callback) const; /// Ray casting method - void raycast(const Ray& _ray, DynamicAABBTreeRaycastCallback& _callback) const; + void raycast(const Ray& _ray, etk::Function _callback) const; /// Compute the height of the tree int32_t computeHeight(); /// Return the root AABB of the tree diff --git a/ephysics/collision/narrowphase/EPA/EPAAlgorithm.cpp b/ephysics/collision/narrowphase/EPA/EPAAlgorithm.cpp index 7992c51..ea56241 100644 --- a/ephysics/collision/narrowphase/EPA/EPAAlgorithm.cpp +++ b/ephysics/collision/narrowphase/EPA/EPAAlgorithm.cpp @@ -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 - vec3 normal1 = (p2-p1).cross(p3-p1); - if ((normal1.dot(p1) > 0.0) == (normal1.dot(p4) > 0.0)) { + 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)) { + 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)) { + 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)) { + 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; } -void EPAAlgorithm::computePenetrationDepthAndContactPoints(const Simplex& simplex, - CollisionShapeInfo shape1Info, - const etk::Transform3D& transform1, - CollisionShapeInfo shape2Info, - const etk::Transform3D& transform2, - vec3& v, - NarrowPhaseCallback* narrowPhaseCallback) { +void EPAAlgorithm::computePenetrationDepthAndContactPoints(const Simplex& _simplex, + CollisionShapeInfo _shape1Info, + const etk::Transform3D& _transform1, + CollisionShapeInfo _shape2Info, + const etk::Transform3D& _transform2, + vec3& _vector, + NarrowPhaseCallback* narrowPhaseCallback) { PROFILE("EPAAlgorithm::computePenetrationDepthAndContactPoints()"); - assert(shape1Info.collisionShape->isConvex()); - assert(shape2Info.collisionShape->isConvex()); - const ConvexShape* shape1 = static_cast(shape1Info.collisionShape); - const ConvexShape* shape2 = static_cast(shape2Info.collisionShape); - void** shape1CachedCollisionData = shape1Info.cachedCollisionData; - void** shape2CachedCollisionData = shape2Info.cachedCollisionData; + assert(_shape1Info.collisionShape->isConvex()); + assert(_shape2Info.collisionShape->isConvex()); + const ConvexShape* shape1 = static_cast(_shape1Info.collisionShape); + const ConvexShape* shape2 = static_cast(_shape2Info.collisionShape); + void** shape1CachedCollisionData = _shape1Info.cachedCollisionData; + void** shape2CachedCollisionData = _shape2Info.cachedCollisionData; vec3 suppPointsA[MAX_SUPPORT_POINTS]; // Support points of object A in local coordinates vec3 suppPointsB[MAX_SUPPORT_POINTS]; // Support points of object B in local coordinates vec3 points[MAX_SUPPORT_POINTS]; // Current points TrianglesStore triangleStore; // Store the triangles - TriangleEPA* triangleHeap[MAX_FACETS]; // Heap that contains the face - // candidate of the EPA algorithm + etk::Set triangleHeap; // list of face candidate of the EPA algorithm sorted lower square dist to upper square dist + 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 // 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 // space of body 1 int32_to local space of body 2 - etk::Quaternion rotateToBody2 = transform2.getOrientation().getInverse() * - transform1.getOrientation(); + 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); + uint32_t nbVertices = _simplex.getSimplex(suppPointsA, suppPointsB, points); // Compute the tolerance - float tolerance = FLT_EPSILON * simplex.getMaxLengthSquareOfAPoint(); - // Number of triangles in the polytope - uint32_t nbTriangles = 0; + float tolerance = FLT_EPSILON * _simplex.getMaxLengthSquareOfAPoint(); // Clear the storing of triangles triangleStore.clear(); // 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(face2, 1), EdgeEPA(face3, 1)); // Add the triangle faces in the candidate heap - addFaceCandidate(face0, triangleHeap, nbTriangles, FLT_MAX); - addFaceCandidate(face1, triangleHeap, nbTriangles, FLT_MAX); - addFaceCandidate(face2, triangleHeap, nbTriangles, FLT_MAX); - addFaceCandidate(face3, triangleHeap, nbTriangles, FLT_MAX); + addFaceCandidate(face0, triangleHeap, FLT_MAX); + addFaceCandidate(face1, triangleHeap, FLT_MAX); + addFaceCandidate(face2, triangleHeap, FLT_MAX); + addFaceCandidate(face3, triangleHeap, FLT_MAX); break; } // 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 * shape2->getLocalSupportPointWithMargin(rotateToBody2 * n, shape2CachedCollisionData); points[4] = suppPointsA[4] - suppPointsB[4]; - TriangleEPA* face0 = NULL; - TriangleEPA* face1 = NULL; - TriangleEPA* face2 = NULL; - TriangleEPA* face3 = NULL; + TriangleEPA* face0 = nullptr; + TriangleEPA* face1 = nullptr; + TriangleEPA* face2 = nullptr; + TriangleEPA* face3 = nullptr; // If the origin is in the first tetrahedron if (isOriginInTetrahedron(points[0], points[1], points[2], points[3]) == 0) { @@ -242,9 +241,14 @@ void EPAAlgorithm::computePenetrationDepthAndContactPoints(const Simplex& simple 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)) { + if (!( face0 != nullptr + && face1 != nullptr + && face2 != nullptr + && face3 != nullptr + && face0->getDistSquare() > 0.0 + && face1->getDistSquare() > 0.0 + && face2->getDistSquare() > 0.0 + && face3->getDistSquare() > 0.0) ) { return; } // 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(face2, 1), EdgeEPA(face3, 1)); // Add the triangle faces in the candidate heap - addFaceCandidate(face0, triangleHeap, nbTriangles, FLT_MAX); - addFaceCandidate(face1, triangleHeap, nbTriangles, FLT_MAX); - addFaceCandidate(face2, triangleHeap, nbTriangles, FLT_MAX); - addFaceCandidate(face3, triangleHeap, nbTriangles, FLT_MAX); + addFaceCandidate(face0, triangleHeap, FLT_MAX); + addFaceCandidate(face1, triangleHeap, FLT_MAX); + addFaceCandidate(face2, triangleHeap, FLT_MAX); + addFaceCandidate(face3, triangleHeap, 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) { + if (triangleHeap.size() == 0) { return; } TriangleEPA* triangle = 0; 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], m_triangleComparison); - nbTriangles--; + triangleHeap.popFront(); + EPHY_INFO("rm from heap:"); + for (size_t iii=0; iiigetDistSquare()); + } // If the candidate face in the heap is not obsolete if (!triangle->getIsObsolete()) { // 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 // difference (A-B) in the closest point direction - suppPointsA[nbVertices] = shape1->getLocalSupportPointWithMargin( - triangle->getClosestPoint(), shape1CachedCollisionData); - suppPointsB[nbVertices] = body2Tobody1 * - shape2->getLocalSupportPointWithMargin(rotateToBody2 * - (-triangle->getClosestPoint()), shape2CachedCollisionData); + suppPointsA[nbVertices] = shape1->getLocalSupportPointWithMargin(triangle->getClosestPoint(), shape1CachedCollisionData); + suppPointsB[nbVertices] = body2Tobody1 * 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); + EPHY_ASSERT(wDotv > 0.0, "depth penetration error"); float wDotVSquare = wDotv * wDotv / triangle->getDistSquare(); if (wDotVSquare < upperBoundSquarePenDepth) { 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 // face will not be in the convex hull. We start the local recursive silhouette // algorithm from the current triangle face. - int32_t i = triangleStore.getNbTriangles(); + size_t i = triangleStore.getNbTriangles(); if (!triangle->computeSilhouette(points, indexNewVertex, triangleStore)) { break; } @@ -318,22 +321,23 @@ void EPAAlgorithm::computePenetrationDepthAndContactPoints(const Simplex& simple // to the candidates list of faces of the current polytope while(i != triangleStore.getNbTriangles()) { TriangleEPA* newTriangle = &triangleStore[i]; - addFaceCandidate(newTriangle, triangleHeap, nbTriangles, upperBoundSquarePenDepth); + addFaceCandidate(newTriangle, triangleHeap, upperBoundSquarePenDepth); i++; } } - } while(nbTriangles > 0 && triangleHeap[0]->getDistSquare() <= upperBoundSquarePenDepth); + } while( triangleHeap.size() > 0 + && triangleHeap[0]->getDistSquare() <= upperBoundSquarePenDepth); // Compute the contact info - v = transform1.getOrientation() * triangle->getClosestPoint(); + _vector = _transform1.getOrientation() * triangle->getClosestPoint(); vec3 pALocal = triangle->computeClosestPointOfObject(suppPointsA); vec3 pBLocal = body2Tobody1.getInverse() * triangle->computeClosestPointOfObject(suppPointsB); - vec3 normal = v.safeNormalized(); - float penetrationDepth = v.length(); + vec3 normal = _vector.safeNormalized(); + float penetrationDepth = _vector.length(); assert(penetrationDepth > 0.0); 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); - narrowPhaseCallback->notifyContact(shape1Info.overlappingPair, contactInfo); + ContactPointInfo contactInfo(_shape1Info.proxyShape, _shape2Info.proxyShape, _shape1Info.collisionShape, _shape2Info.collisionShape, normal, penetrationDepth, pALocal, pBLocal); + narrowPhaseCallback->notifyContact(_shape1Info.overlappingPair, contactInfo); } diff --git a/ephysics/collision/narrowphase/EPA/EPAAlgorithm.hpp b/ephysics/collision/narrowphase/EPA/EPAAlgorithm.hpp index 5659a8d..7d77b18 100644 --- a/ephysics/collision/narrowphase/EPA/EPAAlgorithm.hpp +++ b/ephysics/collision/narrowphase/EPA/EPAAlgorithm.hpp @@ -11,29 +11,14 @@ #include #include #include -#include +#include +#include namespace ephysics { /// 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). @@ -50,15 +35,13 @@ namespace ephysics { */ class EPAAlgorithm { private: - 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, + etk::Set& _heap, float _upperBoundSquarePenDepth) { // If the closest point of the affine hull of triangle // points is int32_ternal to the triangle and if the distance @@ -67,9 +50,11 @@ namespace ephysics { 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); + _heap.add(_triangle); + EPHY_INFO("add in heap:"); + for (size_t iii=0; iii<_heap.size(); ++iii) { + EPHY_INFO(" [" << iii << "] " << _heap[iii]->getDistSquare()); + } } } // Decide if the origin is in the tetrahedron. diff --git a/ephysics/collision/narrowphase/EPA/EdgeEPA.cpp b/ephysics/collision/narrowphase/EPA/EdgeEPA.cpp index 70b5a12..1b87029 100644 --- a/ephysics/collision/narrowphase/EPA/EdgeEPA.cpp +++ b/ephysics/collision/narrowphase/EPA/EdgeEPA.cpp @@ -36,13 +36,13 @@ uint32_t EdgeEPA::getTargetVertexIndex() const { return (*m_ownerTriangle)[indexOfNextCounterClockwiseEdge(m_index)]; } -bool EdgeEPA::computeSilhouette(const vec3* vertices, uint32_t indexNewVertex, - TrianglesStore& triangleStore) { +bool EdgeEPA::computeSilhouette(const vec3* _vertices, uint32_t _indexNewVertex, + TrianglesStore& _triangleStore) { // If the edge has not already been visited if (!m_ownerTriangle->getIsObsolete()) { // If the triangle of this edge is not visible from the given point - if (!m_ownerTriangle->isVisibleFromVertex(vertices, indexNewVertex)) { - TriangleEPA* triangle = triangleStore.newTriangle(vertices, indexNewVertex, + if (!m_ownerTriangle->isVisibleFromVertex(_vertices, _indexNewVertex)) { + TriangleEPA* triangle = _triangleStore.newTriangle(_vertices, _indexNewVertex, getTargetVertexIndex(), getSourceVertexIndex()); // If the triangle has been created @@ -54,13 +54,12 @@ bool EdgeEPA::computeSilhouette(const vec3* vertices, uint32_t indexNewVertex, } else { // The current triangle is visible and therefore obsolete m_ownerTriangle->setIsObsolete(true); - int32_t backup = triangleStore.getNbTriangles(); - if(!m_ownerTriangle->getAdjacentEdge(indexOfNextCounterClockwiseEdge( - this->m_index)).computeSilhouette(vertices, - indexNewVertex, - triangleStore)) { + int32_t backup = _triangleStore.getNbTriangles(); + if(!m_ownerTriangle->getAdjacentEdge(indexOfNextCounterClockwiseEdge(this->m_index)).computeSilhouette(_vertices, + _indexNewVertex, + _triangleStore)) { m_ownerTriangle->setIsObsolete(false); - TriangleEPA* triangle = triangleStore.newTriangle(vertices, indexNewVertex, + TriangleEPA* triangle = _triangleStore.newTriangle(_vertices, _indexNewVertex, getTargetVertexIndex(), getSourceVertexIndex()); // If the triangle has been created @@ -69,13 +68,12 @@ bool EdgeEPA::computeSilhouette(const vec3* vertices, uint32_t indexNewVertex, return true; } return false; - } else if (!m_ownerTriangle->getAdjacentEdge(indexOfPreviousCounterClockwiseEdge( - this->m_index)).computeSilhouette(vertices, - indexNewVertex, - triangleStore)) { + } else if (!m_ownerTriangle->getAdjacentEdge(indexOfPreviousCounterClockwiseEdge(this->m_index)).computeSilhouette(_vertices, + _indexNewVertex, + _triangleStore)) { m_ownerTriangle->setIsObsolete(false); - triangleStore.setNbTriangles(backup); - TriangleEPA* triangle = triangleStore.newTriangle(vertices, indexNewVertex, + _triangleStore.resize(backup); + TriangleEPA* triangle = _triangleStore.newTriangle(_vertices, _indexNewVertex, getTargetVertexIndex(), getSourceVertexIndex()); if (triangle != NULL) { diff --git a/ephysics/collision/narrowphase/EPA/TriangleEPA.hpp b/ephysics/collision/narrowphase/EPA/TriangleEPA.hpp index 7267399..8b46abc 100644 --- a/ephysics/collision/narrowphase/EPA/TriangleEPA.hpp +++ b/ephysics/collision/narrowphase/EPA/TriangleEPA.hpp @@ -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_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: + /// 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 TriangleEPA(); /// Constructor diff --git a/ephysics/collision/narrowphase/EPA/TrianglesStore.cpp b/ephysics/collision/narrowphase/EPA/TrianglesStore.cpp index 28fb2ca..92ea806 100644 --- a/ephysics/collision/narrowphase/EPA/TrianglesStore.cpp +++ b/ephysics/collision/narrowphase/EPA/TrianglesStore.cpp @@ -6,10 +6,3 @@ #include -ephysics::TrianglesStore::TrianglesStore() : m_numberTriangles(0) { - -} - -ephysics::TrianglesStore::~TrianglesStore() { - -} diff --git a/ephysics/collision/narrowphase/EPA/TrianglesStore.hpp b/ephysics/collision/narrowphase/EPA/TrianglesStore.hpp index f12ae1d..b489c65 100644 --- a/ephysics/collision/narrowphase/EPA/TrianglesStore.hpp +++ b/ephysics/collision/narrowphase/EPA/TrianglesStore.hpp @@ -12,48 +12,43 @@ namespace ephysics { */ class TrianglesStore { private: - TriangleEPA m_triangles[MAX_TRIANGLES]; //!< Triangles - int32_t m_numberTriangles; //!< Number of triangles + etk::Vector m_triangles; //!< Triangles /// Private copy-constructor - TrianglesStore(const TrianglesStore& triangleStore); + TrianglesStore(const TrianglesStore& triangleStore) = delete; /// Private assignment operator - TrianglesStore& operator=(const TrianglesStore& triangleStore); + TrianglesStore& operator=(const TrianglesStore& triangleStore) = delete; public: /// Constructor - TrianglesStore(); - /// Destructor - ~TrianglesStore(); + TrianglesStore() = default; /// Clear all the storage void clear() { - m_numberTriangles = 0; + m_triangles.clear(); } /// Return the number of triangles - int32_t getNbTriangles() const { - return m_numberTriangles; + size_t getNbTriangles() const { + return m_triangles.size(); } /// Set the number of triangles - void setNbTriangles(int32_t _backup) { - m_numberTriangles = _backup; + void resize(int32_t _backup) { + m_triangles.resize(_backup); } /// Return the last triangle TriangleEPA& last() { - assert(m_numberTriangles > 0); - return m_triangles[m_numberTriangles - 1]; + return m_triangles.back(); } /// 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; + if (m_triangles.size() < MAX_TRIANGLES) { + TriangleEPA tmp(_v0, _v1, _v2); + if (!tmp.computeClosestPoint(_vertices)) { + return nullptr; } + m_triangles.pushBack(etk::move(tmp)); + return &m_triangles.back(); } - // Return the new triangle - return newTriangle; + // We are at the limit (internal) + return nullptr; } /// Access operator TriangleEPA& operator[](int32_t _id) { diff --git a/ephysics/collision/shapes/ConcaveMeshShape.cpp b/ephysics/collision/shapes/ConcaveMeshShape.cpp index 83341a0..aaaffb8 100644 --- a/ephysics/collision/shapes/ConcaveMeshShape.cpp +++ b/ephysics/collision/shapes/ConcaveMeshShape.cpp @@ -53,10 +53,17 @@ void ConcaveMeshShape::getTriangleVerticesWithIndexPointer(int32_t _subPart, int } 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 // 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 { @@ -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. // The raycastCallback object will then compute ray casting against the triangles // 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(); 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 m_hitAABBNodes.pushBack(_nodeId); return _ray.maxFraction; @@ -104,7 +111,7 @@ void ConcaveMeshRaycastCallback::raycastTriangles() { m_raycastInfo.meshSubpart = data[0]; m_raycastInfo.triangleIndex = data[1]; smallestHitFraction = raycastInfo.hitFraction; - mIsHit = true; + m_isHit = true; } } } @@ -136,17 +143,4 @@ void ConcaveMeshShape::computeLocalInertiaTensor(etk::Matrix3x3& _tensor, float 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); -} - diff --git a/ephysics/collision/shapes/ConcaveMeshShape.hpp b/ephysics/collision/shapes/ConcaveMeshShape.hpp index 996b888..93342e4 100644 --- a/ephysics/collision/shapes/ConcaveMeshShape.hpp +++ b/ephysics/collision/shapes/ConcaveMeshShape.hpp @@ -13,27 +13,7 @@ namespace ephysics { class ConcaveMeshShape; - class ConvexTriangleAABBOverlapCallback : public DynamicAABBTreeOverlapCallback { - 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 { + class ConcaveMeshRaycastCallback { private : etk::Vector m_hitAABBNodes; const DynamicAABBTree& m_dynamicAABBTree; @@ -41,7 +21,7 @@ namespace ephysics { ProxyShape* m_proxyShape; RaycastInfo& m_raycastInfo; const Ray& m_ray; - bool mIsHit; + bool m_isHit; public: // Constructor ConcaveMeshRaycastCallback(const DynamicAABBTree& _dynamicAABBTree, @@ -54,16 +34,16 @@ namespace ephysics { m_proxyShape(_proxyShape), m_raycastInfo(_raycastInfo), m_ray(_ray), - mIsHit(false) { + m_isHit(false) { } /// 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 void raycastTriangles(); /// Return true if a raycast hit has been found bool getIsHit() const { - return mIsHit; + return m_isHit; } }; /** diff --git a/ephysics/engine/CollisionWorld.cpp b/ephysics/engine/CollisionWorld.cpp index 6f3b965..16eb4b2 100644 --- a/ephysics/engine/CollisionWorld.cpp +++ b/ephysics/engine/CollisionWorld.cpp @@ -37,7 +37,7 @@ CollisionBody* CollisionWorld::createCollisionBody(const etk::Transform3D& trans // Get the next available body ID bodyindex bodyID = computeNextAvailableBodyID(); // Largest index cannot be used (it is used for invalid index) - EPHY_ASSERT(bodyID < std::numeric_limits::max(), "index too big"); + EPHY_ASSERT(bodyID < UINT64_MAX, "index too big"); // Create the collision body CollisionBody* collisionBody = new CollisionBody(transform, *this, bodyID); EPHY_ASSERT(collisionBody != nullptr, "empty Body collision"); diff --git a/ephysics/engine/ContactSolver.cpp b/ephysics/engine/ContactSolver.cpp index dabdfb0..783e687 100644 --- a/ephysics/engine/ContactSolver.cpp +++ b/ephysics/engine/ContactSolver.cpp @@ -521,7 +521,7 @@ void ContactSolver::solve() { float beta = m_isSplitImpulseActive ? BETA_SPLIT_IMPULSE : BETA; float biasPenetrationDepth = 0.0; 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; // Compute the Lagrange multiplier lambda diff --git a/ephysics/engine/DynamicsWorld.cpp b/ephysics/engine/DynamicsWorld.cpp index 25c2170..d441061 100644 --- a/ephysics/engine/DynamicsWorld.cpp +++ b/ephysics/engine/DynamicsWorld.cpp @@ -329,7 +329,7 @@ ephysics::RigidBody* ephysics::DynamicsWorld::createRigidBody(const etk::Transfo // Compute the body ID ephysics::bodyindex bodyID = computeNextAvailableBodyID(); // Largest index cannot be used (it is used for invalid index) - assert(bodyID < std::numeric_limits::max()); + assert(bodyID < UINT64_MAX); // Create the rigid body ephysics::RigidBody* rigidBody = new RigidBody(_transform, *this, bodyID); assert(rigidBody != nullptr); diff --git a/lutin_ephysics.py b/lutin_ephysics.py index 72df9ec..b02453d 100644 --- a/lutin_ephysics.py +++ b/lutin_ephysics.py @@ -150,7 +150,6 @@ def configure(target, my_module): my_module.compile_version("c++", 2011) # add dependency of the generic C++ library: my_module.add_depend([ - 'cxx', 'm', 'elog', 'etk', diff --git a/test/testDynamicAABBTree.cpp b/test/testDynamicAABBTree.cpp index 00d2704..d3cad7b 100644 --- a/test/testDynamicAABBTree.cpp +++ b/test/testDynamicAABBTree.cpp @@ -10,35 +10,35 @@ #include -class OverlapCallback : public ephysics::DynamicAABBTreeOverlapCallback { +class OverlapCallback { public : etk::Vector m_overlapNodes; // Called when a overlapping node has been found during the call to // DynamicAABBTree:reportAllShapesOverlappingWithAABB() - virtual void notifyOverlappingNode(int32_t nodeId) { - m_overlapNodes.pushBack(nodeId); + void operator()(int32_t _nodeId) { + m_overlapNodes.pushBack(_nodeId); } void reset() { m_overlapNodes.clear(); } - bool isOverlapping(int32_t nodeId) const { - return etk::isIn(nodeId, m_overlapNodes); + bool isOverlapping(int32_t _nodeId) const { + return etk::isIn(_nodeId, m_overlapNodes); } }; -class DynamicTreeRaycastCallback : public ephysics::DynamicAABBTreeRaycastCallback { +class DynamicTreeRaycastCallback { public: etk::Vector m_hitNodes; // Called when the AABB of a leaf node is hit by a ray - virtual float raycastBroadPhaseShape(int32_t nodeId, const ephysics::Ray& ray) { - m_hitNodes.pushBack(nodeId); + float operator()(int32_t _nodeId, const ephysics::Ray& _ray) { + m_hitNodes.pushBack(_nodeId); return 1.0; } void reset() { m_hitNodes.clear(); } - bool isHit(int32_t nodeId) const { - return etk::isIn(nodeId, m_hitNodes); + bool isHit(int32_t _nodeId) const { + return etk::isIn(_nodeId, m_hitNodes); } }; @@ -115,7 +115,7 @@ TEST(TestAABBTree, overlapping) { // AABB overlapping nothing 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(object2Id), false); EXPECT_EQ(overlapCallback.isOverlapping(object3Id), false); @@ -123,7 +123,7 @@ TEST(TestAABBTree, overlapping) { // AABB overlapping everything 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(object2Id), true); EXPECT_EQ(overlapCallback.isOverlapping(object3Id), true); @@ -131,7 +131,7 @@ TEST(TestAABBTree, overlapping) { // AABB overlapping object 1 and 3 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(object2Id), false); EXPECT_EQ(overlapCallback.isOverlapping(object3Id), true); @@ -139,7 +139,7 @@ TEST(TestAABBTree, overlapping) { // AABB overlapping object 3 and 4 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(object2Id), false); EXPECT_EQ(overlapCallback.isOverlapping(object3Id), true); @@ -147,7 +147,7 @@ TEST(TestAABBTree, overlapping) { // AABB overlapping object 2 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(object2Id), true); EXPECT_EQ(overlapCallback.isOverlapping(object3Id), false); @@ -162,7 +162,7 @@ TEST(TestAABBTree, overlapping) { // AABB overlapping nothing 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(object2Id), false); EXPECT_EQ(overlapCallback.isOverlapping(object3Id), false); @@ -170,7 +170,7 @@ TEST(TestAABBTree, overlapping) { // AABB overlapping everything 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(object2Id), true); EXPECT_EQ(overlapCallback.isOverlapping(object3Id), true); @@ -178,7 +178,7 @@ TEST(TestAABBTree, overlapping) { // AABB overlapping object 1 and 3 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(object2Id), false); EXPECT_EQ(overlapCallback.isOverlapping(object3Id), true); @@ -186,7 +186,7 @@ TEST(TestAABBTree, overlapping) { // AABB overlapping object 3 and 4 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(object2Id), false); EXPECT_EQ(overlapCallback.isOverlapping(object3Id), true); @@ -194,7 +194,7 @@ TEST(TestAABBTree, overlapping) { // AABB overlapping object 2 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(object2Id), true); EXPECT_EQ(overlapCallback.isOverlapping(object3Id), false); @@ -209,7 +209,7 @@ TEST(TestAABBTree, overlapping) { // AABB overlapping nothing 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(object2Id), false); EXPECT_EQ(overlapCallback.isOverlapping(object3Id), false); @@ -217,7 +217,7 @@ TEST(TestAABBTree, overlapping) { // AABB overlapping everything 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(object2Id), true); EXPECT_EQ(overlapCallback.isOverlapping(object3Id), true); @@ -225,7 +225,7 @@ TEST(TestAABBTree, overlapping) { // AABB overlapping object 1 and 3 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(object2Id), false); EXPECT_EQ(overlapCallback.isOverlapping(object3Id), true); @@ -233,7 +233,7 @@ TEST(TestAABBTree, overlapping) { // AABB overlapping object 3 and 4 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(object2Id), false); EXPECT_EQ(overlapCallback.isOverlapping(object3Id), true); @@ -241,7 +241,7 @@ TEST(TestAABBTree, overlapping) { // AABB overlapping object 2 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(object2Id), true); EXPECT_EQ(overlapCallback.isOverlapping(object3Id), false); @@ -257,7 +257,7 @@ TEST(TestAABBTree, overlapping) { // AABB overlapping object 3 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(object2Id), false); EXPECT_EQ(overlapCallback.isOverlapping(object3Id), true); @@ -265,7 +265,7 @@ TEST(TestAABBTree, overlapping) { // AABB overlapping objects 1, 2 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(object2Id), true); EXPECT_EQ(overlapCallback.isOverlapping(object3Id), false); @@ -303,7 +303,7 @@ TEST(TestAABBTree, raycast) { // Ray with no hits raycastCallback.reset(); 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(object2Id), false); EXPECT_EQ(raycastCallback.isHit(object3Id), false); @@ -312,7 +312,7 @@ TEST(TestAABBTree, raycast) { // Ray that hits object 1 raycastCallback.reset(); 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(object2Id), false); EXPECT_EQ(raycastCallback.isHit(object3Id), false); @@ -321,7 +321,7 @@ TEST(TestAABBTree, raycast) { // Ray that hits object 1 and 2 raycastCallback.reset(); 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(object2Id), true); EXPECT_EQ(raycastCallback.isHit(object3Id), false); @@ -330,7 +330,7 @@ TEST(TestAABBTree, raycast) { // Ray that hits object 3 raycastCallback.reset(); 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(object2Id), false); EXPECT_EQ(raycastCallback.isHit(object3Id), true); @@ -345,7 +345,7 @@ TEST(TestAABBTree, raycast) { // Ray with no hits 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(object2Id), false); EXPECT_EQ(raycastCallback.isHit(object3Id), false); @@ -353,7 +353,7 @@ TEST(TestAABBTree, raycast) { // Ray that hits object 1 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(object2Id), false); EXPECT_EQ(raycastCallback.isHit(object3Id), false); @@ -361,7 +361,7 @@ TEST(TestAABBTree, raycast) { // Ray that hits object 1 and 2 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(object2Id), true); EXPECT_EQ(raycastCallback.isHit(object3Id), false); @@ -369,7 +369,7 @@ TEST(TestAABBTree, raycast) { // Ray that hits object 3 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(object2Id), false); EXPECT_EQ(raycastCallback.isHit(object3Id), true); @@ -384,7 +384,7 @@ TEST(TestAABBTree, raycast) { // Ray with no hits 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(object2Id), false); EXPECT_EQ(raycastCallback.isHit(object3Id), false); @@ -392,7 +392,7 @@ TEST(TestAABBTree, raycast) { // Ray that hits object 1 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(object2Id), false); EXPECT_EQ(raycastCallback.isHit(object3Id), false); @@ -400,7 +400,7 @@ TEST(TestAABBTree, raycast) { // Ray that hits object 1 and 2 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(object2Id), true); EXPECT_EQ(raycastCallback.isHit(object3Id), false); @@ -408,7 +408,7 @@ TEST(TestAABBTree, raycast) { // Ray that hits object 3 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(object2Id), false); EXPECT_EQ(raycastCallback.isHit(object3Id), true); @@ -425,7 +425,7 @@ TEST(TestAABBTree, raycast) { // Ray that hits object 1, 2 ephysics::Ray ray5(vec3(-4, -5, 0), vec3(-4, 12, 0)); 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(object2Id), true); EXPECT_EQ(raycastCallback.isHit(object3Id), false); @@ -434,7 +434,7 @@ TEST(TestAABBTree, raycast) { // Ray that hits object 3 and 4 ephysics::Ray ray6(vec3(11, -3, 1), vec3(-2, -3, 1)); 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(object2Id), false); EXPECT_EQ(raycastCallback.isHit(object3Id), true);