Continue working on triangle mesh collision detection

This commit is contained in:
Daniel Chappuis 2015-11-13 21:23:34 +01:00
parent 9fd98f8efc
commit 568f03461f
9 changed files with 309 additions and 144 deletions

View File

@ -143,6 +143,8 @@ SET (REACTPHYSICS3D_SOURCES
"src/engine/OverlappingPair.cpp" "src/engine/OverlappingPair.cpp"
"src/engine/Profiler.h" "src/engine/Profiler.h"
"src/engine/Profiler.cpp" "src/engine/Profiler.cpp"
"src/engine/Timer.h"
"src/engine/Timer.cpp"
"src/mathematics/mathematics.h" "src/mathematics/mathematics.h"
"src/mathematics/mathematics_functions.h" "src/mathematics/mathematics_functions.h"
"src/mathematics/Matrix2x2.h" "src/mathematics/Matrix2x2.h"

View File

@ -118,7 +118,7 @@ void BroadPhaseAlgorithm::removeMovedCollisionShape(int broadPhaseID) {
void BroadPhaseAlgorithm::addProxyCollisionShape(ProxyShape* proxyShape, const AABB& aabb) { void BroadPhaseAlgorithm::addProxyCollisionShape(ProxyShape* proxyShape, const AABB& aabb) {
// Add the collision shape into the dynamic AABB tree and get its broad-phase ID // 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 // Set the broad-phase ID of the proxy shape
proxyShape->mBroadPhaseID = nodeId; proxyShape->mBroadPhaseID = nodeId;
@ -175,13 +175,15 @@ void BroadPhaseAlgorithm::computeOverlappingPairs() {
if (shapeID == -1) continue; 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); const AABB& shapeAABB = mDynamicAABBTree.getFatAABB(shapeID);
// Ask the dynamic AABB tree to report all collision shapes that overlap with // Ask the dynamic AABB tree to report all collision shapes that overlap with
// this AABB. The method BroadPhase::notifiyOverlappingPair() will be called // this AABB. The method BroadPhase::notifiyOverlappingPair() will be called
// by the dynamic AABB tree for each potential overlapping pair. // by the dynamic AABB tree for each potential overlapping pair.
mDynamicAABBTree.reportAllShapesOverlappingWith(shapeID, shapeAABB, *this); mDynamicAABBTree.reportAllShapesOverlappingWithAABB(shapeAABB, callback);
} }
// Reset the array of collision shapes that have move (or have been created) during the // Reset the array of collision shapes that have move (or have been created) during the
@ -200,9 +202,11 @@ void BroadPhaseAlgorithm::computeOverlappingPairs() {
BroadPhasePair* pair = mPotentialPairs + i; BroadPhasePair* pair = mPotentialPairs + i;
i++; i++;
assert(pair->collisionShape1ID != pair->collisionShape2ID);
// Get the two collision shapes of the pair // Get the two collision shapes of the pair
ProxyShape* shape1 = static_cast<ProxyShape*>(mDynamicAABBTree.getNodeData(pair->collisionShape1ID)); ProxyShape* shape1 = static_cast<ProxyShape*>(mDynamicAABBTree.getNodeDataPointer(pair->collisionShape1ID));
ProxyShape* shape2 = static_cast<ProxyShape*>(mDynamicAABBTree.getNodeData(pair->collisionShape2ID)); ProxyShape* shape2 = static_cast<ProxyShape*>(mDynamicAABBTree.getNodeDataPointer(pair->collisionShape2ID));
// Notify the collision detection about the overlapping pair // Notify the collision detection about the overlapping pair
mCollisionDetection.broadPhaseNotifyOverlappingPair(shape1, shape2); 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 // 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 both the nodes are the same, we do not create store the overlapping pair
if (node1ID == node2ID) return; 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 // 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, const Ray& ray,
unsigned short raycastWithCategoryMaskBits) const { unsigned short raycastWithCategoryMaskBits) const {
decimal hitFraction = decimal(-1.0); decimal hitFraction = decimal(-1.0);
// Get the proxy shape from the node // Get the proxy shape from the node
ProxyShape* proxyShape = static_cast<ProxyShape*>(nodeData); ProxyShape* proxyShape = static_cast<ProxyShape*>(mDynamicAABBTree.getNodeDataPointer(nodeId));
// Check if the raycast filtering mask allows raycast against this shape // Check if the raycast filtering mask allows raycast against this shape
if ((raycastWithCategoryMaskBits & proxyShape->getCollisionCategoryBits()) != 0) { if ((raycastWithCategoryMaskBits & proxyShape->getCollisionCategoryBits()) != 0) {
@ -280,4 +284,11 @@ decimal BroadPhaseAlgorithm::raycastBroadPhaseShape(void* nodeData, RaycastTest&
} }
return hitFraction; 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);
}

View File

@ -37,6 +37,7 @@ namespace reactphysics3d {
// Declarations // Declarations
class CollisionDetection; class CollisionDetection;
class BroadPhaseAlgorithm;
// Structure BroadPhasePair // Structure BroadPhasePair
/** /**
@ -59,17 +60,26 @@ struct BroadPhasePair {
static bool smallerThan(const BroadPhasePair& pair1, const BroadPhasePair& pair2); static bool smallerThan(const BroadPhasePair& pair1, const BroadPhasePair& pair2);
}; };
// Class BroadPhaseOverlapCallback // class AABBOverlapCallback
/** class AABBOverlapCallback : public DynamicAABBTreeOverlapCallback {
* Overlapping callback method that has to be used as parameter of the
* reportAllShapesOverlappingWith() method.
*/
class BroadPhaseOverlapCallback {
public : private:
// Called when two overlapping AABBs have been found BroadPhaseAlgorithm& mBroadPhaseAlgorithm;
virtual void notifyOverlappingAABBs(int broadPhaseId1, int broadPhaseId2)=0;
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: public:
// Called for a broad-phase shape that has to be tested for raycast // Called for a broad-phase shape that has to be tested for raycast
virtual decimal raycastBroadPhaseShape(void* nodeData, RaycastTest& raycastTest, virtual decimal raycastBroadPhaseShape(int32 nodeId, RaycastTest& raycastTest,
const Ray& ray, const Ray& ray,
unsigned short raycastWithCategoryMaskBits) const=0; unsigned short raycastWithCategoryMaskBits) const=0;
@ -96,7 +106,7 @@ class BroadPhaseRaycastTestCallback {
* later for collision during the narrow-phase collision detection. A dynamic AABB * later for collision during the narrow-phase collision detection. A dynamic AABB
* tree data structure is used for fast broad-phase collision detection. * tree data structure is used for fast broad-phase collision detection.
*/ */
class BroadPhaseAlgorithm : BroadPhaseRaycastTestCallback, BroadPhaseOverlapCallback { class BroadPhaseAlgorithm : BroadPhaseRaycastTestCallback {
protected : protected :
@ -171,10 +181,10 @@ class BroadPhaseAlgorithm : BroadPhaseRaycastTestCallback, BroadPhaseOverlapCall
void removeMovedCollisionShape(int broadPhaseID); void removeMovedCollisionShape(int broadPhaseID);
/// Notify the broad-phase about a potential overlapping pair in the dynamic AABB tree /// 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 /// 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, const Ray& ray,
unsigned short raycastWithCategoryMaskBits) const; unsigned short raycastWithCategoryMaskBits) const;

View File

@ -93,9 +93,6 @@ int DynamicAABBTree::allocateNode() {
int freeNodeID = mFreeNodeID; int freeNodeID = mFreeNodeID;
mFreeNodeID = mNodes[freeNodeID].nextNodeID; mFreeNodeID = mNodes[freeNodeID].nextNodeID;
mNodes[freeNodeID].parentID = TreeNode::NULL_TREE_NODE; 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; mNodes[freeNodeID].height = 0;
mNbNodes++; mNbNodes++;
@ -114,9 +111,8 @@ void DynamicAABBTree::releaseNode(int nodeID) {
mNbNodes--; mNbNodes--;
} }
// Add an object into the tree. This method creates a new leaf node in the tree and // Internally add an object into the tree
// returns the ID of the corresponding node. int DynamicAABBTree::addObjectInternal(const AABB& aabb) {
int DynamicAABBTree::addObject(void* nodeData, const AABB& aabb) {
// Get the next available node (or allocate new ones if necessary) // Get the next available node (or allocate new ones if necessary)
int nodeID = allocateNode(); 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.setMin(aabb.getMin() - gap);
mNodes[nodeID].aabb.setMax(aabb.getMax() + 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 // Set the height of the node in the tree
mNodes[nodeID].height = 0; mNodes[nodeID].height = 0;
@ -286,13 +279,13 @@ void DynamicAABBTree::insertLeafNode(int nodeID) {
int oldParentNode = mNodes[siblingNode].parentID; int oldParentNode = mNodes[siblingNode].parentID;
int newParentNode = allocateNode(); int newParentNode = allocateNode();
mNodes[newParentNode].parentID = oldParentNode; mNodes[newParentNode].parentID = oldParentNode;
mNodes[newParentNode].data = NULL;
mNodes[newParentNode].aabb.mergeTwoAABBs(mNodes[siblingNode].aabb, newNodeAABB); mNodes[newParentNode].aabb.mergeTwoAABBs(mNodes[siblingNode].aabb, newNodeAABB);
mNodes[newParentNode].height = mNodes[siblingNode].height + 1; mNodes[newParentNode].height = mNodes[siblingNode].height + 1;
assert(mNodes[newParentNode].height > 0); assert(mNodes[newParentNode].height > 0);
// If the sibling node was not the root node // If the sibling node was not the root node
if (oldParentNode != TreeNode::NULL_TREE_NODE) { if (oldParentNode != TreeNode::NULL_TREE_NODE) {
assert(!mNodes[oldParentNode].isLeaf());
if (mNodes[oldParentNode].children[0] == siblingNode) { if (mNodes[oldParentNode].children[0] == siblingNode) {
mNodes[oldParentNode].children[0] = newParentNode; 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 // Move up in the tree to change the AABBs that have changed
currentNodeID = mNodes[nodeID].parentID; currentNodeID = mNodes[nodeID].parentID;
assert(!mNodes[currentNodeID].isLeaf());
while (currentNodeID != TreeNode::NULL_TREE_NODE) { while (currentNodeID != TreeNode::NULL_TREE_NODE) {
// Balance the sub-tree of the current node if it is not balanced // Balance the sub-tree of the current node if it is not balanced
currentNodeID = balanceSubTreeAtNode(currentNodeID); currentNodeID = balanceSubTreeAtNode(currentNodeID);
assert(mNodes[nodeID].isLeaf()); assert(mNodes[nodeID].isLeaf());
assert(!mNodes[currentNodeID].isLeaf());
int leftChild = mNodes[currentNodeID].children[0]; int leftChild = mNodes[currentNodeID].children[0];
int rightChild = mNodes[currentNodeID].children[1]; int rightChild = mNodes[currentNodeID].children[1];
assert(leftChild != TreeNode::NULL_TREE_NODE); assert(leftChild != TreeNode::NULL_TREE_NODE);
@ -383,6 +378,8 @@ void DynamicAABBTree::removeLeafNode(int nodeID) {
// Balance the current sub-tree if necessary // Balance the current sub-tree if necessary
currentNodeID = balanceSubTreeAtNode(currentNodeID); currentNodeID = balanceSubTreeAtNode(currentNodeID);
assert(!mNodes[currentNodeID].isLeaf());
// Get the two children of the current node // Get the two children of the current node
int leftChildID = mNodes[currentNodeID].children[0]; int leftChildID = mNodes[currentNodeID].children[0];
int rightChildID = mNodes[currentNodeID].children[1]; 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 the right node C is 2 higher than left node B
if (balanceFactor > 1) { if (balanceFactor > 1) {
assert(!nodeC->isLeaf());
int nodeFID = nodeC->children[0]; int nodeFID = nodeC->children[0];
int nodeGID = nodeC->children[1]; int nodeGID = nodeC->children[1];
assert(nodeFID >= 0 && nodeFID < mNbAllocatedNodes); assert(nodeFID >= 0 && nodeFID < mNbAllocatedNodes);
@ -461,8 +460,12 @@ int DynamicAABBTree::balanceSubTreeAtNode(int nodeID) {
mRootNodeID = nodeCID; 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 the right node C was higher than left node B because of the F node
if (nodeF->height > nodeG->height) { if (nodeF->height > nodeG->height) {
nodeC->children[1] = nodeFID; nodeC->children[1] = nodeFID;
nodeA->children[1] = nodeGID; nodeA->children[1] = nodeGID;
nodeG->parentID = nodeID; 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 the left node B is 2 higher than right node C
if (balanceFactor < -1) { if (balanceFactor < -1) {
assert(!nodeB->isLeaf());
int nodeFID = nodeB->children[0]; int nodeFID = nodeB->children[0];
int nodeGID = nodeB->children[1]; int nodeGID = nodeB->children[1];
assert(nodeFID >= 0 && nodeFID < mNbAllocatedNodes); assert(nodeFID >= 0 && nodeFID < mNbAllocatedNodes);
@ -525,8 +530,12 @@ int DynamicAABBTree::balanceSubTreeAtNode(int nodeID) {
mRootNodeID = nodeBID; 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 the left node B was higher than right node C because of the F node
if (nodeF->height > nodeG->height) { if (nodeF->height > nodeG->height) {
nodeB->children[1] = nodeFID; nodeB->children[1] = nodeFID;
nodeA->children[0] = nodeGID; nodeA->children[0] = nodeGID;
nodeG->parentID = nodeID; nodeG->parentID = nodeID;
@ -565,12 +574,9 @@ int DynamicAABBTree::balanceSubTreeAtNode(int nodeID) {
return nodeID; return nodeID;
} }
// Report all shapes overlapping with the AABB given in parameter. /// Report all shapes overlapping with the AABB given in parameter.
/// For each overlapping shape with the AABB given in parameter, the void DynamicAABBTree::reportAllShapesOverlappingWithAABB(const AABB& aabb,
/// BroadPhase::notifyOverlappingPair() method is called to store a DynamicAABBTreeOverlapCallback& callback) const {
/// potential overlapping pair.
void DynamicAABBTree::reportAllShapesOverlappingWith(int nodeID, const AABB& aabb,
BroadPhaseOverlapCallback& callback) {
// Create a stack with the nodes to visit // Create a stack with the nodes to visit
Stack<int, 64> stack; Stack<int, 64> stack;
@ -595,7 +601,7 @@ void DynamicAABBTree::reportAllShapesOverlappingWith(int nodeID, const AABB& aab
if (nodeToVisit->isLeaf()) { if (nodeToVisit->isLeaf()) {
// Notify the broad-phase about a new potential overlapping pair // Notify the broad-phase about a new potential overlapping pair
callback.notifyOverlappingAABBs(nodeID, nodeIDToVisit); callback.notifyOverlappingNode(nodeIDToVisit);
} }
else { // If the node is not a leaf 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); Ray rayTemp(ray.point1, ray.point2, maxFraction);
// Call the callback that will raycast again the broad-phase shape // 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); raycastWithCategoryMaskBits);
// If the user returned a hitFraction of zero, it means that // 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 // Get the children nodes
TreeNode* pNode = mNodes + nodeID; TreeNode* pNode = mNodes + nodeID;
assert(!pNode->isLeaf());
int leftChild = pNode->children[0]; int leftChild = pNode->children[0];
int rightChild = pNode->children[1]; int rightChild = pNode->children[1];

View File

@ -37,7 +37,7 @@ namespace reactphysics3d {
// Declarations // Declarations
class BroadPhaseAlgorithm; class BroadPhaseAlgorithm;
class BroadPhaseRaycastTestCallback; class BroadPhaseRaycastTestCallback;
class BroadPhaseOverlapCallback; class DynamicAABBTreeOverlapCallback;
struct RaycastTest; struct RaycastTest;
// Structure TreeNode // Structure TreeNode
@ -70,8 +70,11 @@ struct TreeNode {
/// Left and right child of the node (children[0] = left child) /// Left and right child of the node (children[0] = left child)
int32 children[2]; int32 children[2];
/// Pointer to the data stored at that node (in case the node is a leaf) /// Two pieces of data stored at that node (in case the node is a leaf)
void* data; union {
int32 dataInt[2];
void* dataPointer;
};
}; };
/// Height of the node in the tree /// Height of the node in the tree
@ -86,6 +89,20 @@ struct TreeNode {
bool isLeaf() const; 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 // Class DynamicAABBTree
/** /**
* This class implements a dynamic AABB tree that is used for broad-phase * 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 /// Compute the height of a given node in the tree
int computeHeight(int nodeID); int computeHeight(int nodeID);
/// Internally add an object into the tree
int addObjectInternal(const AABB& aabb);
#ifndef NDEBUG #ifndef NDEBUG
/// Check if the tree structure is valid (for debugging purpose) /// Check if the tree structure is valid (for debugging purpose)
@ -159,8 +179,11 @@ class DynamicAABBTree {
/// Destructor /// Destructor
~DynamicAABBTree(); ~DynamicAABBTree();
/// Add an object into the tree /// Add an object into the tree (where node data are two integers)
int addObject(void* nodeData, const AABB& aabb); 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 /// Remove an object from the tree
void removeObject(int nodeID); void removeObject(int nodeID);
@ -171,12 +194,15 @@ class DynamicAABBTree {
/// Return the fat AABB corresponding to a given node ID /// Return the fat AABB corresponding to a given node ID
const AABB& getFatAABB(int nodeID) const; const AABB& getFatAABB(int nodeID) const;
/// Return the pointer to the data of a given leaf node of the tree /// Return the pointer to the data array of a given leaf node of the tree
void* getNodeData(int nodeID) const; 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. /// Report all shapes overlapping with the AABB given in parameter.
void reportAllShapesOverlappingWith(int nodeID, const AABB& aabb, void reportAllShapesOverlappingWithAABB(const AABB& aabb,
BroadPhaseOverlapCallback& callback); DynamicAABBTreeOverlapCallback& callback) const;
/// Ray casting method /// Ray casting method
void raycast(const Ray& ray, RaycastTest& raycastTest, void raycast(const Ray& ray, RaycastTest& raycastTest,
@ -185,6 +211,9 @@ class DynamicAABBTree {
/// Compute the height of the tree /// Compute the height of the tree
int computeHeight(); int computeHeight();
/// Return the root AABB of the tree
AABB getRootAABB() const;
}; };
// Return true if the node is a leaf of the tree // 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 mNodes[nodeID].aabb;
} }
// Return the pointer to the data of a given leaf node of the tree // Return the pointer to the data array of a given leaf node of the tree
inline void* DynamicAABBTree::getNodeData(int nodeID) const { inline int32* DynamicAABBTree::getNodeDataInt(int nodeID) const {
assert(nodeID >= 0 && nodeID < mNbAllocatedNodes); assert(nodeID >= 0 && nodeID < mNbAllocatedNodes);
assert(mNodes[nodeID].isLeaf()); 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;
} }
} }

View File

@ -89,3 +89,27 @@ bool AABB::contains(const AABB& aabb) {
return isInside; 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);
}

View File

@ -100,6 +100,9 @@ class AABB {
/// Return true if the AABB of a triangle intersects the AABB /// Return true if the AABB of a triangle intersects the AABB
bool testCollisionTriangleAABB(const Vector3* trianglePoints) const; bool testCollisionTriangleAABB(const Vector3* trianglePoints) const;
/// Create and return an AABB for a triangle
static AABB createAABBForTriangle(const Vector3* trianglePoints);
/// Assignment operator /// Assignment operator
AABB& operator=(const AABB& aabb); AABB& operator=(const AABB& aabb);

View File

@ -25,6 +25,7 @@
// Libraries // Libraries
#include "ConcaveMeshShape.h" #include "ConcaveMeshShape.h"
#include <iostream>
using namespace reactphysics3d; using namespace reactphysics3d;
@ -32,7 +33,8 @@ using namespace reactphysics3d;
ConcaveMeshShape::ConcaveMeshShape(TriangleMesh* triangleMesh) : ConcaveShape(CONCAVE_MESH) { ConcaveMeshShape::ConcaveMeshShape(TriangleMesh* triangleMesh) : ConcaveShape(CONCAVE_MESH) {
mTriangleMesh = triangleMesh; mTriangleMesh = triangleMesh;
recalculateBounds(); // Insert all the triangles into the dynamic AABB tree
initBVHTree();
} }
// Destructor // Destructor
@ -40,14 +42,16 @@ ConcaveMeshShape::~ConcaveMeshShape() {
} }
// Use a callback method on all triangles of the concave shape inside a given AABB // Insert all the triangles into the dynamic AABB tree
void ConcaveMeshShape::testAllTriangles(TriangleCallback& callback, const AABB& localAABB) const { 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 each sub-part of the mesh
for (int i=0; i<mTriangleMesh->getNbSubparts(); i++) { for (int subPart=0; subPart<mTriangleMesh->getNbSubparts(); subPart++) {
// Get the triangle vertex array of the current sub-part // 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::VertexDataType vertexType = triangleVertexArray->getVertexDataType();
TriangleVertexArray::IndexDataType indexType = triangleVertexArray->getIndexDataType(); TriangleVertexArray::IndexDataType indexType = triangleVertexArray->getIndexDataType();
@ -57,8 +61,9 @@ void ConcaveMeshShape::testAllTriangles(TriangleCallback& callback, const AABB&
int indexStride = triangleVertexArray->getIndicesStride(); int indexStride = triangleVertexArray->getIndicesStride();
// For each triangle of the concave mesh // For each triangle of the concave mesh
for (int j=0; j<triangleVertexArray->getNbTriangles(); j++) { for (int triangleIndex=0; triangleIndex<triangleVertexArray->getNbTriangles(); triangleIndex++) {
void* vertexIndexPointer = (indicesStart + triangleIndex * 3 * indexStride);
Vector3 trianglePoints[3]; Vector3 trianglePoints[3];
// For each vertex of the triangle // 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 // Get the index of the current vertex in the triangle
int vertexIndex; int vertexIndex;
if (indexType == TriangleVertexArray::INDEX_INTEGER_TYPE) { 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) { 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 // 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 // Create the AABB for the triangle
if (localAABB.testCollisionTriangleAABB(trianglePoints)) { AABB aabb = AABB::createAABBForTriangle(trianglePoints);
// Call the callback to report this triangle // Add the AABB with the index of the triangle into the dynamic AABB tree
callback.testTriangle(trianglePoints); 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 // Raycast method with feedback information
bool ConcaveMeshShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const { 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; 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; i<mTriangleMesh->getNbSubparts(); 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; j<triangleVertexArray->getNbTriangles(); 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;
}
}
}
}
}

View File

@ -28,10 +28,41 @@
// Libraries // Libraries
#include "ConcaveShape.h" #include "ConcaveShape.h"
#include "collision/broadphase/DynamicAABBTree.h"
#include "collision/TriangleMesh.h" #include "collision/TriangleMesh.h"
namespace reactphysics3d { 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 : Implement raycasting with this collision shape
// TODO : Make possible for the user to have a scaling factor on the mesh // TODO : Make possible for the user to have a scaling factor on the mesh
@ -51,11 +82,8 @@ class ConcaveMeshShape : public ConcaveShape {
/// Triangle mesh /// Triangle mesh
TriangleMesh* mTriangleMesh; TriangleMesh* mTriangleMesh;
/// Mesh minimum bounds in the three local x, y and z directions /// Dynamic AABB tree to accelerate collision with the triangles
Vector3 mMinBounds; DynamicAABBTree mDynamicAABBTree;
/// Mesh maximum bounds in the three local x, y and z directions
Vector3 mMaxBounds;
// -------------------- Methods -------------------- // // -------------------- Methods -------------------- //
@ -82,9 +110,13 @@ class ConcaveMeshShape : public ConcaveShape {
/// Return the number of bytes used by the collision shape /// Return the number of bytes used by the collision shape
virtual size_t getSizeInBytes() const; virtual size_t getSizeInBytes() const;
/// Recompute the bounds of the mesh /// Insert all the triangles into the dynamic AABB tree
// TODO : Check if we need this when AABB tree is used void initBVHTree();
void recalculateBounds();
/// 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: public:
@ -102,6 +134,10 @@ class ConcaveMeshShape : public ConcaveShape {
/// Use a callback method on all triangles of the concave shape inside a given AABB /// Use a callback method on all triangles of the concave shape inside a given AABB
virtual void testAllTriangles(TriangleCallback& callback, const AABB& localAABB) const; virtual void testAllTriangles(TriangleCallback& callback, const AABB& localAABB) const;
// ---------- Friendship ----------- //
friend class ConvexTriangleAABBOverlapCallback;
}; };
// Return the number of bytes used by the collision shape // 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 { inline void ConcaveMeshShape::getLocalBounds(Vector3& min, Vector3& max) const {
min = mMinBounds; // Get the AABB of the whole tree
max = mMaxBounds; AABB treeAABB = mDynamicAABBTree.getRootAABB();
min = treeAABB.getMin();
max = treeAABB.getMax();
} }
// Return the local inertia tensor of the sphere // Return the local inertia tensor of the sphere
@ -160,6 +199,21 @@ inline bool ConcaveMeshShape::testPointInside(const Vector3& localPoint, ProxySh
return false; 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 #endif