From 568f03461feba1c47f1aa07a8da2bbdd41bb71a6 Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Fri, 13 Nov 2015 21:23:34 +0100 Subject: [PATCH] Continue working on triangle mesh collision detection --- CMakeLists.txt | 2 + .../broadphase/BroadPhaseAlgorithm.cpp | 29 +++- .../broadphase/BroadPhaseAlgorithm.h | 36 ++-- src/collision/broadphase/DynamicAABBTree.cpp | 43 +++-- src/collision/broadphase/DynamicAABBTree.h | 88 ++++++++-- src/collision/shapes/AABB.cpp | 24 +++ src/collision/shapes/AABB.h | 3 + src/collision/shapes/ConcaveMeshShape.cpp | 154 ++++++++---------- src/collision/shapes/ConcaveMeshShape.h | 74 +++++++-- 9 files changed, 309 insertions(+), 144 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index b71e60b6..272c1186 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -143,6 +143,8 @@ SET (REACTPHYSICS3D_SOURCES "src/engine/OverlappingPair.cpp" "src/engine/Profiler.h" "src/engine/Profiler.cpp" + "src/engine/Timer.h" + "src/engine/Timer.cpp" "src/mathematics/mathematics.h" "src/mathematics/mathematics_functions.h" "src/mathematics/Matrix2x2.h" diff --git a/src/collision/broadphase/BroadPhaseAlgorithm.cpp b/src/collision/broadphase/BroadPhaseAlgorithm.cpp index d91ce848..aa1d6ba8 100644 --- a/src/collision/broadphase/BroadPhaseAlgorithm.cpp +++ b/src/collision/broadphase/BroadPhaseAlgorithm.cpp @@ -118,7 +118,7 @@ void BroadPhaseAlgorithm::removeMovedCollisionShape(int broadPhaseID) { void BroadPhaseAlgorithm::addProxyCollisionShape(ProxyShape* proxyShape, const AABB& aabb) { // Add the collision shape into the dynamic AABB tree and get its broad-phase ID - int nodeId = mDynamicAABBTree.addObject(proxyShape, aabb); + int nodeId = mDynamicAABBTree.addObject(aabb, proxyShape); // Set the broad-phase ID of the proxy shape proxyShape->mBroadPhaseID = nodeId; @@ -175,13 +175,15 @@ void BroadPhaseAlgorithm::computeOverlappingPairs() { if (shapeID == -1) continue; - // Get the fat AABB of the collision shape from the dynamic AABB tree + AABBOverlapCallback callback(*this, shapeID); + + // Get the AABB of the shape const AABB& shapeAABB = mDynamicAABBTree.getFatAABB(shapeID); // Ask the dynamic AABB tree to report all collision shapes that overlap with // this AABB. The method BroadPhase::notifiyOverlappingPair() will be called // by the dynamic AABB tree for each potential overlapping pair. - mDynamicAABBTree.reportAllShapesOverlappingWith(shapeID, shapeAABB, *this); + mDynamicAABBTree.reportAllShapesOverlappingWithAABB(shapeAABB, callback); } // Reset the array of collision shapes that have move (or have been created) during the @@ -200,9 +202,11 @@ void BroadPhaseAlgorithm::computeOverlappingPairs() { BroadPhasePair* pair = mPotentialPairs + i; i++; + assert(pair->collisionShape1ID != pair->collisionShape2ID); + // Get the two collision shapes of the pair - ProxyShape* shape1 = static_cast(mDynamicAABBTree.getNodeData(pair->collisionShape1ID)); - ProxyShape* shape2 = static_cast(mDynamicAABBTree.getNodeData(pair->collisionShape2ID)); + ProxyShape* shape1 = static_cast(mDynamicAABBTree.getNodeDataPointer(pair->collisionShape1ID)); + ProxyShape* shape2 = static_cast(mDynamicAABBTree.getNodeDataPointer(pair->collisionShape2ID)); // Notify the collision detection about the overlapping pair mCollisionDetection.broadPhaseNotifyOverlappingPair(shape1, shape2); @@ -237,7 +241,7 @@ void BroadPhaseAlgorithm::computeOverlappingPairs() { } // Notify the broad-phase about a potential overlapping pair in the dynamic AABB tree -void BroadPhaseAlgorithm::notifyOverlappingAABBs(int node1ID, int node2ID) { +void BroadPhaseAlgorithm::notifyOverlappingNodes(int node1ID, int node2ID) { // If both the nodes are the same, we do not create store the overlapping pair if (node1ID == node2ID) return; @@ -261,14 +265,14 @@ void BroadPhaseAlgorithm::notifyOverlappingAABBs(int node1ID, int node2ID) { } // Called for a broad-phase shape that has to be tested for raycast -decimal BroadPhaseAlgorithm::raycastBroadPhaseShape(void* nodeData, RaycastTest& raycastTest, +decimal BroadPhaseAlgorithm::raycastBroadPhaseShape(int32 nodeId, RaycastTest& raycastTest, const Ray& ray, unsigned short raycastWithCategoryMaskBits) const { decimal hitFraction = decimal(-1.0); // Get the proxy shape from the node - ProxyShape* proxyShape = static_cast(nodeData); + ProxyShape* proxyShape = static_cast(mDynamicAABBTree.getNodeDataPointer(nodeId)); // Check if the raycast filtering mask allows raycast against this shape if ((raycastWithCategoryMaskBits & proxyShape->getCollisionCategoryBits()) != 0) { @@ -280,4 +284,11 @@ decimal BroadPhaseAlgorithm::raycastBroadPhaseShape(void* nodeData, RaycastTest& } return hitFraction; - } +} + +// Called when a overlapping node has been found during the call to +// DynamicAABBTree:reportAllShapesOverlappingWithAABB() +void AABBOverlapCallback::notifyOverlappingNode(int nodeId) { + + mBroadPhaseAlgorithm.notifyOverlappingNodes(mReferenceNodeId, nodeId); +} diff --git a/src/collision/broadphase/BroadPhaseAlgorithm.h b/src/collision/broadphase/BroadPhaseAlgorithm.h index 121fe529..9c3080ce 100644 --- a/src/collision/broadphase/BroadPhaseAlgorithm.h +++ b/src/collision/broadphase/BroadPhaseAlgorithm.h @@ -37,6 +37,7 @@ namespace reactphysics3d { // Declarations class CollisionDetection; +class BroadPhaseAlgorithm; // Structure BroadPhasePair /** @@ -59,17 +60,26 @@ struct BroadPhasePair { static bool smallerThan(const BroadPhasePair& pair1, const BroadPhasePair& pair2); }; -// Class BroadPhaseOverlapCallback -/** - * Overlapping callback method that has to be used as parameter of the - * reportAllShapesOverlappingWith() method. - */ -class BroadPhaseOverlapCallback { +// class AABBOverlapCallback +class AABBOverlapCallback : public DynamicAABBTreeOverlapCallback { - public : + private: - // Called when two overlapping AABBs have been found - virtual void notifyOverlappingAABBs(int broadPhaseId1, int broadPhaseId2)=0; + BroadPhaseAlgorithm& mBroadPhaseAlgorithm; + + int mReferenceNodeId; + + public: + + // Constructor + AABBOverlapCallback(BroadPhaseAlgorithm& broadPhaseAlgo, int referenceNodeId) + : mBroadPhaseAlgorithm(broadPhaseAlgo), mReferenceNodeId(referenceNodeId) { + + } + + // Called when a overlapping node has been found during the call to + // DynamicAABBTree:reportAllShapesOverlappingWithAABB() + virtual void notifyOverlappingNode(int nodeId); }; @@ -82,7 +92,7 @@ class BroadPhaseRaycastTestCallback { public: // Called for a broad-phase shape that has to be tested for raycast - virtual decimal raycastBroadPhaseShape(void* nodeData, RaycastTest& raycastTest, + virtual decimal raycastBroadPhaseShape(int32 nodeId, RaycastTest& raycastTest, const Ray& ray, unsigned short raycastWithCategoryMaskBits) const=0; @@ -96,7 +106,7 @@ class BroadPhaseRaycastTestCallback { * later for collision during the narrow-phase collision detection. A dynamic AABB * tree data structure is used for fast broad-phase collision detection. */ -class BroadPhaseAlgorithm : BroadPhaseRaycastTestCallback, BroadPhaseOverlapCallback { +class BroadPhaseAlgorithm : BroadPhaseRaycastTestCallback { protected : @@ -171,10 +181,10 @@ class BroadPhaseAlgorithm : BroadPhaseRaycastTestCallback, BroadPhaseOverlapCall void removeMovedCollisionShape(int broadPhaseID); /// Notify the broad-phase about a potential overlapping pair in the dynamic AABB tree - void notifyOverlappingAABBs(int broadPhaseId1, int broadPhaseId2); + void notifyOverlappingNodes(int broadPhaseId1, int broadPhaseId2); /// Called for a broad-phase shape that has to be tested for raycast - virtual decimal raycastBroadPhaseShape(void* nodeData, RaycastTest& raycastTest, + virtual decimal raycastBroadPhaseShape(int32 nodeId, RaycastTest& raycastTest, const Ray& ray, unsigned short raycastWithCategoryMaskBits) const; diff --git a/src/collision/broadphase/DynamicAABBTree.cpp b/src/collision/broadphase/DynamicAABBTree.cpp index f82ac450..dd2446cf 100644 --- a/src/collision/broadphase/DynamicAABBTree.cpp +++ b/src/collision/broadphase/DynamicAABBTree.cpp @@ -93,9 +93,6 @@ int DynamicAABBTree::allocateNode() { int freeNodeID = mFreeNodeID; mFreeNodeID = mNodes[freeNodeID].nextNodeID; mNodes[freeNodeID].parentID = TreeNode::NULL_TREE_NODE; - mNodes[freeNodeID].children[0] = TreeNode::NULL_TREE_NODE; - mNodes[freeNodeID].children[1] = TreeNode::NULL_TREE_NODE; - mNodes[freeNodeID].data = NULL; mNodes[freeNodeID].height = 0; mNbNodes++; @@ -114,9 +111,8 @@ void DynamicAABBTree::releaseNode(int nodeID) { mNbNodes--; } -// Add an object into the tree. This method creates a new leaf node in the tree and -// returns the ID of the corresponding node. -int DynamicAABBTree::addObject(void* nodeData, const AABB& aabb) { +// Internally add an object into the tree +int DynamicAABBTree::addObjectInternal(const AABB& aabb) { // Get the next available node (or allocate new ones if necessary) int nodeID = allocateNode(); @@ -126,9 +122,6 @@ int DynamicAABBTree::addObject(void* nodeData, const AABB& aabb) { mNodes[nodeID].aabb.setMin(aabb.getMin() - gap); mNodes[nodeID].aabb.setMax(aabb.getMax() + gap); - // Set the node data - mNodes[nodeID].data = nodeData; - // Set the height of the node in the tree mNodes[nodeID].height = 0; @@ -286,13 +279,13 @@ void DynamicAABBTree::insertLeafNode(int nodeID) { int oldParentNode = mNodes[siblingNode].parentID; int newParentNode = allocateNode(); mNodes[newParentNode].parentID = oldParentNode; - mNodes[newParentNode].data = NULL; mNodes[newParentNode].aabb.mergeTwoAABBs(mNodes[siblingNode].aabb, newNodeAABB); mNodes[newParentNode].height = mNodes[siblingNode].height + 1; assert(mNodes[newParentNode].height > 0); // If the sibling node was not the root node if (oldParentNode != TreeNode::NULL_TREE_NODE) { + assert(!mNodes[oldParentNode].isLeaf()); if (mNodes[oldParentNode].children[0] == siblingNode) { mNodes[oldParentNode].children[0] = newParentNode; } @@ -314,12 +307,14 @@ void DynamicAABBTree::insertLeafNode(int nodeID) { // Move up in the tree to change the AABBs that have changed currentNodeID = mNodes[nodeID].parentID; + assert(!mNodes[currentNodeID].isLeaf()); while (currentNodeID != TreeNode::NULL_TREE_NODE) { // Balance the sub-tree of the current node if it is not balanced currentNodeID = balanceSubTreeAtNode(currentNodeID); assert(mNodes[nodeID].isLeaf()); + assert(!mNodes[currentNodeID].isLeaf()); int leftChild = mNodes[currentNodeID].children[0]; int rightChild = mNodes[currentNodeID].children[1]; assert(leftChild != TreeNode::NULL_TREE_NODE); @@ -383,6 +378,8 @@ void DynamicAABBTree::removeLeafNode(int nodeID) { // Balance the current sub-tree if necessary currentNodeID = balanceSubTreeAtNode(currentNodeID); + assert(!mNodes[currentNodeID].isLeaf()); + // Get the two children of the current node int leftChildID = mNodes[currentNodeID].children[0]; int rightChildID = mNodes[currentNodeID].children[1]; @@ -436,6 +433,8 @@ int DynamicAABBTree::balanceSubTreeAtNode(int nodeID) { // If the right node C is 2 higher than left node B if (balanceFactor > 1) { + assert(!nodeC->isLeaf()); + int nodeFID = nodeC->children[0]; int nodeGID = nodeC->children[1]; assert(nodeFID >= 0 && nodeFID < mNbAllocatedNodes); @@ -461,8 +460,12 @@ int DynamicAABBTree::balanceSubTreeAtNode(int nodeID) { mRootNodeID = nodeCID; } + assert(!nodeC->isLeaf()); + assert(!nodeA->isLeaf()); + // If the right node C was higher than left node B because of the F node if (nodeF->height > nodeG->height) { + nodeC->children[1] = nodeFID; nodeA->children[1] = nodeGID; nodeG->parentID = nodeID; @@ -500,6 +503,8 @@ int DynamicAABBTree::balanceSubTreeAtNode(int nodeID) { // If the left node B is 2 higher than right node C if (balanceFactor < -1) { + assert(!nodeB->isLeaf()); + int nodeFID = nodeB->children[0]; int nodeGID = nodeB->children[1]; assert(nodeFID >= 0 && nodeFID < mNbAllocatedNodes); @@ -525,8 +530,12 @@ int DynamicAABBTree::balanceSubTreeAtNode(int nodeID) { mRootNodeID = nodeBID; } + assert(!nodeB->isLeaf()); + assert(!nodeA->isLeaf()); + // If the left node B was higher than right node C because of the F node if (nodeF->height > nodeG->height) { + nodeB->children[1] = nodeFID; nodeA->children[0] = nodeGID; nodeG->parentID = nodeID; @@ -565,12 +574,9 @@ int DynamicAABBTree::balanceSubTreeAtNode(int nodeID) { return nodeID; } -// Report all shapes overlapping with the AABB given in parameter. -/// For each overlapping shape with the AABB given in parameter, the -/// BroadPhase::notifyOverlappingPair() method is called to store a -/// potential overlapping pair. -void DynamicAABBTree::reportAllShapesOverlappingWith(int nodeID, const AABB& aabb, - BroadPhaseOverlapCallback& callback) { +/// Report all shapes overlapping with the AABB given in parameter. +void DynamicAABBTree::reportAllShapesOverlappingWithAABB(const AABB& aabb, + DynamicAABBTreeOverlapCallback& callback) const { // Create a stack with the nodes to visit Stack stack; @@ -595,7 +601,7 @@ void DynamicAABBTree::reportAllShapesOverlappingWith(int nodeID, const AABB& aab if (nodeToVisit->isLeaf()) { // Notify the broad-phase about a new potential overlapping pair - callback.notifyOverlappingAABBs(nodeID, nodeIDToVisit); + callback.notifyOverlappingNode(nodeIDToVisit); } else { // If the node is not a leaf @@ -645,7 +651,7 @@ void DynamicAABBTree::raycast(const Ray& ray, RaycastTest& raycastTest, Ray rayTemp(ray.point1, ray.point2, maxFraction); // Call the callback that will raycast again the broad-phase shape - decimal hitFraction = callback.raycastBroadPhaseShape(node->data, raycastTest, rayTemp, + decimal hitFraction = callback.raycastBroadPhaseShape(nodeID, raycastTest, rayTemp, raycastWithCategoryMaskBits); // If the user returned a hitFraction of zero, it means that @@ -712,6 +718,7 @@ void DynamicAABBTree::checkNode(int nodeID) const { // Get the children nodes TreeNode* pNode = mNodes + nodeID; + assert(!pNode->isLeaf()); int leftChild = pNode->children[0]; int rightChild = pNode->children[1]; diff --git a/src/collision/broadphase/DynamicAABBTree.h b/src/collision/broadphase/DynamicAABBTree.h index 5a6d3cc6..ec530e94 100644 --- a/src/collision/broadphase/DynamicAABBTree.h +++ b/src/collision/broadphase/DynamicAABBTree.h @@ -37,7 +37,7 @@ namespace reactphysics3d { // Declarations class BroadPhaseAlgorithm; class BroadPhaseRaycastTestCallback; -class BroadPhaseOverlapCallback; +class DynamicAABBTreeOverlapCallback; struct RaycastTest; // Structure TreeNode @@ -70,8 +70,11 @@ struct TreeNode { /// Left and right child of the node (children[0] = left child) int32 children[2]; - /// Pointer to the data stored at that node (in case the node is a leaf) - void* data; + /// Two pieces of data stored at that node (in case the node is a leaf) + union { + int32 dataInt[2]; + void* dataPointer; + }; }; /// Height of the node in the tree @@ -86,6 +89,20 @@ struct TreeNode { bool isLeaf() const; }; +// Class DynamicAABBTreeOverlapCallback +/** + * Overlapping callback method that has to be used as parameter of the + * reportAllShapesOverlappingWithNode() method. + */ +class DynamicAABBTreeOverlapCallback { + + public : + + // Called when a overlapping node has been found during the call to + // DynamicAABBTree:reportAllShapesOverlappingWithAABB() + virtual void notifyOverlappingNode(int nodeId)=0; +}; + // Class DynamicAABBTree /** * This class implements a dynamic AABB tree that is used for broad-phase @@ -139,6 +156,9 @@ class DynamicAABBTree { /// Compute the height of a given node in the tree int computeHeight(int nodeID); + /// Internally add an object into the tree + int addObjectInternal(const AABB& aabb); + #ifndef NDEBUG /// Check if the tree structure is valid (for debugging purpose) @@ -159,8 +179,11 @@ class DynamicAABBTree { /// Destructor ~DynamicAABBTree(); - /// Add an object into the tree - int addObject(void* nodeData, const AABB& aabb); + /// Add an object into the tree (where node data are two integers) + int addObject(const AABB& aabb, int32 data1, int32 data2); + + /// Add an object into the tree (where node data is a pointer) + int addObject(const AABB& aabb, void* data); /// Remove an object from the tree void removeObject(int nodeID); @@ -171,12 +194,15 @@ class DynamicAABBTree { /// Return the fat AABB corresponding to a given node ID const AABB& getFatAABB(int nodeID) const; - /// Return the pointer to the data of a given leaf node of the tree - void* getNodeData(int nodeID) const; + /// Return the pointer to the data array of a given leaf node of the tree + int32* getNodeDataInt(int nodeID) const; + + /// Return the data pointer of a given leaf node of the tree + void* getNodeDataPointer(int nodeID) const; /// Report all shapes overlapping with the AABB given in parameter. - void reportAllShapesOverlappingWith(int nodeID, const AABB& aabb, - BroadPhaseOverlapCallback& callback); + void reportAllShapesOverlappingWithAABB(const AABB& aabb, + DynamicAABBTreeOverlapCallback& callback) const; /// Ray casting method void raycast(const Ray& ray, RaycastTest& raycastTest, @@ -185,6 +211,9 @@ class DynamicAABBTree { /// Compute the height of the tree int computeHeight(); + + /// Return the root AABB of the tree + AABB getRootAABB() const; }; // Return true if the node is a leaf of the tree @@ -198,11 +227,46 @@ inline const AABB& DynamicAABBTree::getFatAABB(int nodeID) const { return mNodes[nodeID].aabb; } -// Return the pointer to the data of a given leaf node of the tree -inline void* DynamicAABBTree::getNodeData(int nodeID) const { +// Return the pointer to the data array of a given leaf node of the tree +inline int32* DynamicAABBTree::getNodeDataInt(int nodeID) const { assert(nodeID >= 0 && nodeID < mNbAllocatedNodes); assert(mNodes[nodeID].isLeaf()); - return mNodes[nodeID].data; + return mNodes[nodeID].dataInt; +} + +// Return the pointer to the data pointer of a given leaf node of the tree +inline void* DynamicAABBTree::getNodeDataPointer(int nodeID) const { + assert(nodeID >= 0 && nodeID < mNbAllocatedNodes); + assert(mNodes[nodeID].isLeaf()); + return mNodes[nodeID].dataPointer; +} + +// Return the root AABB of the tree +inline AABB DynamicAABBTree::getRootAABB() const { + return getFatAABB(mRootNodeID); +} + +// Add an object into the tree. This method creates a new leaf node in the tree and +// returns the ID of the corresponding node. +inline int DynamicAABBTree::addObject(const AABB& aabb, int32 data1, int32 data2) { + + int nodeId = addObjectInternal(aabb); + + mNodes[nodeId].dataInt[0] = data1; + mNodes[nodeId].dataInt[1] = data2; + + return nodeId; +} + +// Add an object into the tree. This method creates a new leaf node in the tree and +// returns the ID of the corresponding node. +inline int DynamicAABBTree::addObject(const AABB& aabb, void* data) { + + int nodeId = addObjectInternal(aabb); + + mNodes[nodeId].dataPointer = data; + + return nodeId; } } diff --git a/src/collision/shapes/AABB.cpp b/src/collision/shapes/AABB.cpp index 7b4d95a0..c69a6aba 100644 --- a/src/collision/shapes/AABB.cpp +++ b/src/collision/shapes/AABB.cpp @@ -89,3 +89,27 @@ bool AABB::contains(const AABB& aabb) { return isInside; } +// Create and return an AABB for a triangle +AABB AABB::createAABBForTriangle(const Vector3* trianglePoints) { + + Vector3 minCoords(trianglePoints[0].x, trianglePoints[0].y, trianglePoints[0].z); + Vector3 maxCoords(trianglePoints[0].x, trianglePoints[0].y, trianglePoints[0].z); + + if (trianglePoints[1].x < minCoords.x) minCoords.x = trianglePoints[1].x; + if (trianglePoints[1].y < minCoords.y) minCoords.y = trianglePoints[1].y; + if (trianglePoints[1].z < minCoords.z) minCoords.z = trianglePoints[1].z; + + if (trianglePoints[2].x < minCoords.x) minCoords.x = trianglePoints[2].x; + if (trianglePoints[2].y < minCoords.y) minCoords.y = trianglePoints[2].y; + if (trianglePoints[2].z < minCoords.z) minCoords.z = trianglePoints[2].z; + + if (trianglePoints[1].x > maxCoords.x) maxCoords.x = trianglePoints[1].x; + if (trianglePoints[1].y > maxCoords.y) maxCoords.y = trianglePoints[1].y; + if (trianglePoints[1].z > maxCoords.z) maxCoords.z = trianglePoints[1].z; + + if (trianglePoints[2].x > maxCoords.x) maxCoords.x = trianglePoints[2].x; + if (trianglePoints[2].y > maxCoords.y) maxCoords.y = trianglePoints[2].y; + if (trianglePoints[2].z > maxCoords.z) maxCoords.z = trianglePoints[2].z; + + return AABB(minCoords, maxCoords); +} diff --git a/src/collision/shapes/AABB.h b/src/collision/shapes/AABB.h index f17ca300..d019f655 100644 --- a/src/collision/shapes/AABB.h +++ b/src/collision/shapes/AABB.h @@ -100,6 +100,9 @@ class AABB { /// Return true if the AABB of a triangle intersects the AABB bool testCollisionTriangleAABB(const Vector3* trianglePoints) const; + /// Create and return an AABB for a triangle + static AABB createAABBForTriangle(const Vector3* trianglePoints); + /// Assignment operator AABB& operator=(const AABB& aabb); diff --git a/src/collision/shapes/ConcaveMeshShape.cpp b/src/collision/shapes/ConcaveMeshShape.cpp index e3f8977e..81268307 100644 --- a/src/collision/shapes/ConcaveMeshShape.cpp +++ b/src/collision/shapes/ConcaveMeshShape.cpp @@ -25,6 +25,7 @@ // Libraries #include "ConcaveMeshShape.h" +#include using namespace reactphysics3d; @@ -32,7 +33,8 @@ using namespace reactphysics3d; ConcaveMeshShape::ConcaveMeshShape(TriangleMesh* triangleMesh) : ConcaveShape(CONCAVE_MESH) { mTriangleMesh = triangleMesh; - recalculateBounds(); + // Insert all the triangles into the dynamic AABB tree + initBVHTree(); } // Destructor @@ -40,14 +42,16 @@ ConcaveMeshShape::~ConcaveMeshShape() { } -// Use a callback method on all triangles of the concave shape inside a given AABB -void ConcaveMeshShape::testAllTriangles(TriangleCallback& callback, const AABB& localAABB) const { +// Insert all the triangles into the dynamic AABB tree +void ConcaveMeshShape::initBVHTree() { + + // TODO : Try to randomly add the triangles into the tree to obtain a better tree // For each sub-part of the mesh - for (int i=0; igetNbSubparts(); i++) { + for (int subPart=0; subPartgetNbSubparts(); subPart++) { // Get the triangle vertex array of the current sub-part - TriangleVertexArray* triangleVertexArray = mTriangleMesh->getSubpart(i); + TriangleVertexArray* triangleVertexArray = mTriangleMesh->getSubpart(subPart); TriangleVertexArray::VertexDataType vertexType = triangleVertexArray->getVertexDataType(); TriangleVertexArray::IndexDataType indexType = triangleVertexArray->getIndexDataType(); @@ -57,8 +61,9 @@ void ConcaveMeshShape::testAllTriangles(TriangleCallback& callback, const AABB& int indexStride = triangleVertexArray->getIndicesStride(); // For each triangle of the concave mesh - for (int j=0; jgetNbTriangles(); j++) { + for (int triangleIndex=0; triangleIndexgetNbTriangles(); triangleIndex++) { + void* vertexIndexPointer = (indicesStart + triangleIndex * 3 * indexStride); Vector3 trianglePoints[3]; // For each vertex of the triangle @@ -67,10 +72,10 @@ void ConcaveMeshShape::testAllTriangles(TriangleCallback& callback, const AABB& // Get the index of the current vertex in the triangle int vertexIndex; if (indexType == TriangleVertexArray::INDEX_INTEGER_TYPE) { - vertexIndex = ((unsigned int*)(indicesStart + j * 3 * indexStride))[k]; + vertexIndex = ((uint*)vertexIndexPointer)[k]; } else if (indexType == TriangleVertexArray::INDEX_SHORT_TYPE) { - vertexIndex = ((unsigned short*)(indicesStart + j * 3 * indexStride))[k]; + vertexIndex = ((unsigned short*)vertexIndexPointer)[k]; } // Get the vertices components of the triangle @@ -88,16 +93,70 @@ void ConcaveMeshShape::testAllTriangles(TriangleCallback& callback, const AABB& } } - // If the triangle AABB intersects with the convex shape AABB - if (localAABB.testCollisionTriangleAABB(trianglePoints)) { + // Create the AABB for the triangle + AABB aabb = AABB::createAABBForTriangle(trianglePoints); - // Call the callback to report this triangle - callback.testTriangle(trianglePoints); - } + // Add the AABB with the index of the triangle into the dynamic AABB tree + mDynamicAABBTree.addObject(aabb, subPart, triangleIndex); } } } +// Return the three vertices coordinates (in the array outTriangleVertices) of a triangle +// given the start vertex index pointer of the triangle +void ConcaveMeshShape::getTriangleVerticesWithIndexPointer(int32 subPart, int32 triangleIndex, + Vector3* outTriangleVertices) const { + + // Get the triangle vertex array of the current sub-part + TriangleVertexArray* triangleVertexArray = mTriangleMesh->getSubpart(subPart); + + TriangleVertexArray::VertexDataType vertexType = triangleVertexArray->getVertexDataType(); + TriangleVertexArray::IndexDataType indexType = triangleVertexArray->getIndexDataType(); + unsigned char* verticesStart = triangleVertexArray->getVerticesStart(); + unsigned char* indicesStart = triangleVertexArray->getIndicesStart(); + int vertexStride = triangleVertexArray->getVerticesStride(); + int indexStride = triangleVertexArray->getIndicesStride(); + + void* vertexIndexPointer = (indicesStart + triangleIndex * 3 * indexStride); + + // For each vertex of the triangle + for (int k=0; k < 3; k++) { + + // Get the index of the current vertex in the triangle + int vertexIndex; + if (indexType == TriangleVertexArray::INDEX_INTEGER_TYPE) { + vertexIndex = ((uint*)vertexIndexPointer)[k]; + } + else if (indexType == TriangleVertexArray::INDEX_SHORT_TYPE) { + vertexIndex = ((unsigned short*)vertexIndexPointer)[k]; + } + + // Get the vertices components of the triangle + if (vertexType == TriangleVertexArray::VERTEX_FLOAT_TYPE) { + const float* vertices = (float*)(verticesStart + vertexIndex * vertexStride); + outTriangleVertices[k][0] = decimal(vertices[0]); + outTriangleVertices[k][1] = decimal(vertices[1]); + outTriangleVertices[k][2] = decimal(vertices[2]); + } + else if (vertexType == TriangleVertexArray::VERTEX_DOUBLE_TYPE) { + const double* vertices = (double*)(verticesStart + vertexIndex * vertexStride); + outTriangleVertices[k][0] = decimal(vertices[0]); + outTriangleVertices[k][1] = decimal(vertices[1]); + outTriangleVertices[k][2] = decimal(vertices[2]); + } + } +} + +// Use a callback method on all triangles of the concave shape inside a given AABB +void ConcaveMeshShape::testAllTriangles(TriangleCallback& callback, const AABB& localAABB) const { + + ConvexTriangleAABBOverlapCallback overlapCallback(callback, *this, mDynamicAABBTree); + + // Ask the Dynamic AABB Tree to report all the triangles that are overlapping + // with the AABB of the convex shape. + mDynamicAABBTree.reportAllShapesOverlappingWithAABB(localAABB, overlapCallback); +} + // Raycast method with feedback information bool ConcaveMeshShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const { @@ -106,72 +165,3 @@ bool ConcaveMeshShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxySh return false; } - -// Recompute the bounds of the mesh -void ConcaveMeshShape::recalculateBounds() { - - bool isFirstVertex = true; - - // For each sub-part of the mesh - for (int i=0; igetNbSubparts(); i++) { - - // Get the triangle vertex array of the current sub-part - TriangleVertexArray* triangleVertexArray = mTriangleMesh->getSubpart(i); - - TriangleVertexArray::VertexDataType vertexType = triangleVertexArray->getVertexDataType(); - TriangleVertexArray::IndexDataType indexType = triangleVertexArray->getIndexDataType(); - unsigned char* verticesStart = triangleVertexArray->getVerticesStart(); - unsigned char* indicesStart = triangleVertexArray->getIndicesStart(); - int vertexStride = triangleVertexArray->getVerticesStride(); - int indexStride = triangleVertexArray->getIndicesStride(); - - // For each triangle of the concave mesh - for (int j=0; jgetNbTriangles(); j++) { - - // For each vertex of the triangle - for (int k=0; k < 3; k++) { - - // Get the index of the current vertex in the triangle - int vertexIndex; - if (indexType == TriangleVertexArray::INDEX_INTEGER_TYPE) { - vertexIndex = ((unsigned int*)(indicesStart + j * 3 * indexStride))[k]; - } - else if (indexType == TriangleVertexArray::INDEX_SHORT_TYPE) { - vertexIndex = ((unsigned short*)(indicesStart + j * 3 * indexStride))[k]; - } - - Vector3 vertex; - - // Get the vertices components of the triangle - if (vertexType == TriangleVertexArray::VERTEX_FLOAT_TYPE) { - const float* vertices = (float*)(verticesStart + vertexIndex * vertexStride); - vertex[0] = decimal(vertices[0]); - vertex[1] = decimal(vertices[1]); - vertex[2] = decimal(vertices[2]); - } - else if (vertexType == TriangleVertexArray::VERTEX_DOUBLE_TYPE) { - const double* vertices = (double*)(verticesStart + vertexIndex * vertexStride); - vertex[0] = decimal(vertices[0]); - vertex[1] = decimal(vertices[1]); - vertex[2] = decimal(vertices[2]); - } - - if (isFirstVertex) { - mMinBounds.setAllValues(vertex.x, vertex.y, vertex.z); - mMaxBounds.setAllValues(vertex.x, vertex.y, vertex.z); - isFirstVertex = false; - } - else { - if (vertex.x > mMaxBounds.x) mMaxBounds.x = vertex.x; - if (vertex.x < mMinBounds.x) mMinBounds.x = vertex.x; - - if (vertex.y > mMaxBounds.y) mMaxBounds.y = vertex.y; - if (vertex.y < mMinBounds.y) mMinBounds.y = vertex.y; - - if (vertex.z > mMaxBounds.z) mMaxBounds.z = vertex.z; - if (vertex.z < mMinBounds.z) mMinBounds.z = vertex.z; - } - } - } - } -} diff --git a/src/collision/shapes/ConcaveMeshShape.h b/src/collision/shapes/ConcaveMeshShape.h index 385e69b8..07699d98 100644 --- a/src/collision/shapes/ConcaveMeshShape.h +++ b/src/collision/shapes/ConcaveMeshShape.h @@ -28,10 +28,41 @@ // Libraries #include "ConcaveShape.h" +#include "collision/broadphase/DynamicAABBTree.h" #include "collision/TriangleMesh.h" namespace reactphysics3d { +class ConcaveMeshShape; + +// class ConvexTriangleAABBOverlapCallback +class ConvexTriangleAABBOverlapCallback : public DynamicAABBTreeOverlapCallback { + + private: + + TriangleCallback& mTriangleTestCallback; + + // Reference to the concave mesh shape + const ConcaveMeshShape& mConcaveMeshShape; + + // Reference to the Dynamic AABB tree + const DynamicAABBTree& mDynamicAABBTree; + + public: + + // Constructor + ConvexTriangleAABBOverlapCallback(TriangleCallback& triangleCallback, const ConcaveMeshShape& concaveShape, + const DynamicAABBTree& dynamicAABBTree) + : mTriangleTestCallback(triangleCallback), mConcaveMeshShape(concaveShape), mDynamicAABBTree(dynamicAABBTree) { + + } + + // Called when a overlapping node has been found during the call to + // DynamicAABBTree:reportAllShapesOverlappingWithAABB() + virtual void notifyOverlappingNode(int nodeId); + +}; + // TODO : Implement raycasting with this collision shape // TODO : Make possible for the user to have a scaling factor on the mesh @@ -51,11 +82,8 @@ class ConcaveMeshShape : public ConcaveShape { /// Triangle mesh TriangleMesh* mTriangleMesh; - /// Mesh minimum bounds in the three local x, y and z directions - Vector3 mMinBounds; - - /// Mesh maximum bounds in the three local x, y and z directions - Vector3 mMaxBounds; + /// Dynamic AABB tree to accelerate collision with the triangles + DynamicAABBTree mDynamicAABBTree; // -------------------- Methods -------------------- // @@ -82,9 +110,13 @@ class ConcaveMeshShape : public ConcaveShape { /// Return the number of bytes used by the collision shape virtual size_t getSizeInBytes() const; - /// Recompute the bounds of the mesh - // TODO : Check if we need this when AABB tree is used - void recalculateBounds(); + /// Insert all the triangles into the dynamic AABB tree + void initBVHTree(); + + /// Return the three vertices coordinates (in the array outTriangleVertices) of a triangle + /// given the start vertex index pointer of the triangle. + void getTriangleVerticesWithIndexPointer(int32 subPart, int32 triangleIndex, + Vector3* outTriangleVertices) const; public: @@ -102,6 +134,10 @@ class ConcaveMeshShape : public ConcaveShape { /// Use a callback method on all triangles of the concave shape inside a given AABB virtual void testAllTriangles(TriangleCallback& callback, const AABB& localAABB) const; + + // ---------- Friendship ----------- // + + friend class ConvexTriangleAABBOverlapCallback; }; // Return the number of bytes used by the collision shape @@ -132,8 +168,11 @@ inline Vector3 ConcaveMeshShape::getLocalSupportPointWithoutMargin(const Vector3 */ inline void ConcaveMeshShape::getLocalBounds(Vector3& min, Vector3& max) const { - min = mMinBounds; - max = mMaxBounds; + // Get the AABB of the whole tree + AABB treeAABB = mDynamicAABBTree.getRootAABB(); + + min = treeAABB.getMin(); + max = treeAABB.getMax(); } // Return the local inertia tensor of the sphere @@ -160,6 +199,21 @@ inline bool ConcaveMeshShape::testPointInside(const Vector3& localPoint, ProxySh return false; } +// Called when a overlapping node has been found during the call to +// DynamicAABBTree:reportAllShapesOverlappingWithAABB() +inline void ConvexTriangleAABBOverlapCallback::notifyOverlappingNode(int nodeId) { + + // Get the node data (triangle index and mesh subpart index) + int32* data = mDynamicAABBTree.getNodeDataInt(nodeId); + + // Get the triangle vertices for this node from the concave mesh shape + Vector3 trianglePoints[3]; + mConcaveMeshShape.getTriangleVerticesWithIndexPointer(data[0], data[1], trianglePoints); + + // Call the callback to test narrow-phase collision with this triangle + mTriangleTestCallback.testTriangle(trianglePoints); +} + } #endif