mrq/reactphysics3d
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Make DynamicAABBTree more generic and use less memory per node

Browse Source
This commit is contained in:
Daniel Chappuis 2015-11-06 17:29:42 +01:00
parent 63094dfd92
commit d9fe3ed97f
5 changed files with 203 additions and 134 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

37
src/collision/broadphase/BroadPhaseAlgorithm.cpp
View File

@ -33,7 +33,7 @@ using namespace reactphysics3d;
// Constructor // Constructor
BroadPhaseAlgorithm::BroadPhaseAlgorithm(CollisionDetection& collisionDetection) BroadPhaseAlgorithm::BroadPhaseAlgorithm(CollisionDetection& collisionDetection)
:mDynamicAABBTree(*this), mNbMovedShapes(0), mNbAllocatedMovedShapes(8), :mDynamicAABBTree(DYNAMIC_TREE_AABB_GAP), mNbMovedShapes(0), mNbAllocatedMovedShapes(8),
mNbNonUsedMovedShapes(0), mNbPotentialPairs(0), mNbAllocatedPotentialPairs(8), mNbNonUsedMovedShapes(0), mNbPotentialPairs(0), mNbAllocatedPotentialPairs(8),
mCollisionDetection(collisionDetection) { mCollisionDetection(collisionDetection) {
@ -118,7 +118,10 @@ 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
mDynamicAABBTree.addObject(proxyShape, aabb); int nodeId = mDynamicAABBTree.addObject(proxyShape, aabb);
// Set the broad-phase ID of the proxy shape
proxyShape->mBroadPhaseID = nodeId;
// Add the collision shape into the array of bodies that have moved (or have been created) // Add the collision shape into the array of bodies that have moved (or have been created)
// during the last simulation step // during the last simulation step
@ -178,7 +181,7 @@ void BroadPhaseAlgorithm::computeOverlappingPairs() {
// 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); mDynamicAABBTree.reportAllShapesOverlappingWith(shapeID, shapeAABB, *this);
} }
// 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
@ -198,8 +201,8 @@ void BroadPhaseAlgorithm::computeOverlappingPairs() {
i++; i++;
// Get the two collision shapes of the pair // Get the two collision shapes of the pair
ProxyShape* shape1 = mDynamicAABBTree.getCollisionShape(pair->collisionShape1ID); ProxyShape* shape1 = static_cast<ProxyShape*>(mDynamicAABBTree.getNodeData(pair->collisionShape1ID));
ProxyShape* shape2 = mDynamicAABBTree.getCollisionShape(pair->collisionShape2ID); ProxyShape* shape2 = static_cast<ProxyShape*>(mDynamicAABBTree.getNodeData(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);
@ -234,7 +237,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::notifyOverlappingPair(int node1ID, int node2ID) { void BroadPhaseAlgorithm::notifyOverlappingAABBs(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;
@ -256,3 +259,25 @@ void BroadPhaseAlgorithm::notifyOverlappingPair(int node1ID, int node2ID) {
mPotentialPairs[mNbPotentialPairs].collisionShape2ID = std::max(node1ID, node2ID); mPotentialPairs[mNbPotentialPairs].collisionShape2ID = std::max(node1ID, node2ID);
mNbPotentialPairs++; mNbPotentialPairs++;
} }
// Called for a broad-phase shape that has to be tested for raycast
decimal BroadPhaseAlgorithm::raycastBroadPhaseShape(void* nodeData, 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<ProxyShape*>(nodeData);
// Check if the raycast filtering mask allows raycast against this shape
if ((raycastWithCategoryMaskBits & proxyShape->getCollisionCategoryBits()) != 0) {
// Ask the collision detection to perform a ray cast test against
// the proxy shape of this node because the ray is overlapping
// with the shape in the broad-phase
hitFraction = raycastTest.raycastAgainstShape(proxyShape, ray);
}
return hitFraction;
}

40
src/collision/broadphase/BroadPhaseAlgorithm.h
View File

@ -59,6 +59,35 @@ struct BroadPhasePair {
static bool smallerThan(const BroadPhasePair& pair1, const BroadPhasePair& pair2); 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 {
public :
// Called when two overlapping AABBs have been found
virtual void notifyOverlappingAABBs(int broadPhaseId1, int broadPhaseId2)=0;
};
// Class BroadPhaseRaycastTestCallback
/**
* Callback method called to raycast against a given broad-phase shape
*/
class BroadPhaseRaycastTestCallback {
public:
// Called for a broad-phase shape that has to be tested for raycast
virtual decimal raycastBroadPhaseShape(void* nodeData, RaycastTest& raycastTest,
const Ray& ray,
unsigned short raycastWithCategoryMaskBits) const=0;
};
// Class BroadPhaseAlgorithm // Class BroadPhaseAlgorithm
/** /**
* This class represents the broad-phase collision detection. The * This class represents the broad-phase collision detection. The
@ -67,7 +96,7 @@ struct BroadPhasePair {
* 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 { class BroadPhaseAlgorithm : BroadPhaseRaycastTestCallback, BroadPhaseOverlapCallback {
protected : protected :
@ -142,7 +171,12 @@ class BroadPhaseAlgorithm {
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 notifyOverlappingPair(int node1ID, int node2ID); void notifyOverlappingAABBs(int broadPhaseId1, int broadPhaseId2);
/// Called for a broad-phase shape that has to be tested for raycast
virtual decimal raycastBroadPhaseShape(void* nodeData, RaycastTest& raycastTest,
const Ray& ray,
unsigned short raycastWithCategoryMaskBits) const;
/// Compute all the overlapping pairs of collision shapes /// Compute all the overlapping pairs of collision shapes
void computeOverlappingPairs(); void computeOverlappingPairs();
@ -179,7 +213,7 @@ inline bool BroadPhaseAlgorithm::testOverlappingShapes(const ProxyShape* shape1,
// Ray casting method // Ray casting method
inline void BroadPhaseAlgorithm::raycast(const Ray& ray, RaycastTest& raycastTest, inline void BroadPhaseAlgorithm::raycast(const Ray& ray, RaycastTest& raycastTest,
unsigned short raycastWithCategoryMaskBits) const { unsigned short raycastWithCategoryMaskBits) const {
mDynamicAABBTree.raycast(ray, raycastTest, raycastWithCategoryMaskBits); mDynamicAABBTree.raycast(ray, raycastTest, raycastWithCategoryMaskBits, *this);
} }
} }

190
src/collision/broadphase/DynamicAABBTree.cpp
View File

@ -35,7 +35,7 @@ using namespace reactphysics3d;
const int TreeNode::NULL_TREE_NODE = -1; const int TreeNode::NULL_TREE_NODE = -1;
// Constructor // Constructor
DynamicAABBTree::DynamicAABBTree(BroadPhaseAlgorithm& broadPhase) : mBroadPhase(broadPhase){ DynamicAABBTree::DynamicAABBTree(decimal extraAABBGap) : mExtraAABBGap(extraAABBGap) {
mRootNodeID = TreeNode::NULL_TREE_NODE; mRootNodeID = TreeNode::NULL_TREE_NODE;
mNbNodes = 0; mNbNodes = 0;
@ -93,9 +93,9 @@ 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].leftChildID = TreeNode::NULL_TREE_NODE; mNodes[freeNodeID].children[0] = TreeNode::NULL_TREE_NODE;
mNodes[freeNodeID].rightChildID = TreeNode::NULL_TREE_NODE; mNodes[freeNodeID].children[1] = TreeNode::NULL_TREE_NODE;
mNodes[freeNodeID].proxyShape = NULL; mNodes[freeNodeID].data = NULL;
mNodes[freeNodeID].height = 0; mNodes[freeNodeID].height = 0;
mNbNodes++; mNbNodes++;
@ -116,18 +116,18 @@ void DynamicAABBTree::releaseNode(int nodeID) {
// Add an object into the tree. This method creates a new leaf node in the tree and // Add an object into the tree. This method creates a new leaf node in the tree and
// returns the ID of the corresponding node. // returns the ID of the corresponding node.
void DynamicAABBTree::addObject(ProxyShape* proxyShape, 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();
// Create the fat aabb to use in the tree // Create the fat aabb to use in the tree
const Vector3 gap(DYNAMIC_TREE_AABB_GAP, DYNAMIC_TREE_AABB_GAP, DYNAMIC_TREE_AABB_GAP); const Vector3 gap(mExtraAABBGap, mExtraAABBGap, mExtraAABBGap);
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 collision shape // Set the node data
mNodes[nodeID].proxyShape = proxyShape; 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;
@ -136,9 +136,10 @@ void DynamicAABBTree::addObject(ProxyShape* proxyShape, const AABB& aabb) {
insertLeafNode(nodeID); insertLeafNode(nodeID);
assert(mNodes[nodeID].isLeaf()); assert(mNodes[nodeID].isLeaf());
// Set the broad-phase ID of the proxy shape
proxyShape->mBroadPhaseID = nodeID;
assert(nodeID >= 0); assert(nodeID >= 0);
// Return the Id of the node
return nodeID;
} }
// Remove an object from the tree // Remove an object from the tree
@ -176,7 +177,7 @@ bool DynamicAABBTree::updateObject(int nodeID, const AABB& newAABB, const Vector
// Compute the fat AABB by inflating the AABB with a constant gap // Compute the fat AABB by inflating the AABB with a constant gap
mNodes[nodeID].aabb = newAABB; mNodes[nodeID].aabb = newAABB;
const Vector3 gap(DYNAMIC_TREE_AABB_GAP, DYNAMIC_TREE_AABB_GAP, DYNAMIC_TREE_AABB_GAP); const Vector3 gap(mExtraAABBGap, mExtraAABBGap, mExtraAABBGap);
mNodes[nodeID].aabb.mMinCoordinates -= gap; mNodes[nodeID].aabb.mMinCoordinates -= gap;
mNodes[nodeID].aabb.mMaxCoordinates += gap; mNodes[nodeID].aabb.mMaxCoordinates += gap;
@ -227,8 +228,8 @@ void DynamicAABBTree::insertLeafNode(int nodeID) {
int currentNodeID = mRootNodeID; int currentNodeID = mRootNodeID;
while (!mNodes[currentNodeID].isLeaf()) { while (!mNodes[currentNodeID].isLeaf()) {
int leftChild = mNodes[currentNodeID].leftChildID; int leftChild = mNodes[currentNodeID].children[0];
int rightChild = mNodes[currentNodeID].rightChildID; int rightChild = mNodes[currentNodeID].children[1];
// Compute the merged AABB // Compute the merged AABB
decimal volumeAABB = mNodes[currentNodeID].aabb.getVolume(); decimal volumeAABB = mNodes[currentNodeID].aabb.getVolume();
@ -285,27 +286,27 @@ 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].proxyShape = NULL; 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) {
if (mNodes[oldParentNode].leftChildID == siblingNode) { if (mNodes[oldParentNode].children[0] == siblingNode) {
mNodes[oldParentNode].leftChildID = newParentNode; mNodes[oldParentNode].children[0] = newParentNode;
} }
else { else {
mNodes[oldParentNode].rightChildID = newParentNode; mNodes[oldParentNode].children[1] = newParentNode;
} }
mNodes[newParentNode].leftChildID = siblingNode; mNodes[newParentNode].children[0] = siblingNode;
mNodes[newParentNode].rightChildID = nodeID; mNodes[newParentNode].children[1] = nodeID;
mNodes[siblingNode].parentID = newParentNode; mNodes[siblingNode].parentID = newParentNode;
mNodes[nodeID].parentID = newParentNode; mNodes[nodeID].parentID = newParentNode;
} }
else { // If the sibling node was the root node else { // If the sibling node was the root node
mNodes[newParentNode].leftChildID = siblingNode; mNodes[newParentNode].children[0] = siblingNode;
mNodes[newParentNode].rightChildID = nodeID; mNodes[newParentNode].children[1] = nodeID;
mNodes[siblingNode].parentID = newParentNode; mNodes[siblingNode].parentID = newParentNode;
mNodes[nodeID].parentID = newParentNode; mNodes[nodeID].parentID = newParentNode;
mRootNodeID = newParentNode; mRootNodeID = newParentNode;
@ -319,8 +320,8 @@ void DynamicAABBTree::insertLeafNode(int nodeID) {
currentNodeID = balanceSubTreeAtNode(currentNodeID); currentNodeID = balanceSubTreeAtNode(currentNodeID);
assert(mNodes[nodeID].isLeaf()); assert(mNodes[nodeID].isLeaf());
int leftChild = mNodes[currentNodeID].leftChildID; int leftChild = mNodes[currentNodeID].children[0];
int rightChild = mNodes[currentNodeID].rightChildID; int rightChild = mNodes[currentNodeID].children[1];
assert(leftChild != TreeNode::NULL_TREE_NODE); assert(leftChild != TreeNode::NULL_TREE_NODE);
assert(rightChild != TreeNode::NULL_TREE_NODE); assert(rightChild != TreeNode::NULL_TREE_NODE);
@ -353,23 +354,23 @@ void DynamicAABBTree::removeLeafNode(int nodeID) {
int parentNodeID = mNodes[nodeID].parentID; int parentNodeID = mNodes[nodeID].parentID;
int grandParentNodeID = mNodes[parentNodeID].parentID; int grandParentNodeID = mNodes[parentNodeID].parentID;
int siblingNodeID; int siblingNodeID;
if (mNodes[parentNodeID].leftChildID == nodeID) { if (mNodes[parentNodeID].children[0] == nodeID) {
siblingNodeID = mNodes[parentNodeID].rightChildID; siblingNodeID = mNodes[parentNodeID].children[1];
} }
else { else {
siblingNodeID = mNodes[parentNodeID].leftChildID; siblingNodeID = mNodes[parentNodeID].children[0];
} }
// If the parent of the node to remove is not the root node // If the parent of the node to remove is not the root node
if (grandParentNodeID != TreeNode::NULL_TREE_NODE) { if (grandParentNodeID != TreeNode::NULL_TREE_NODE) {
// Destroy the parent node // Destroy the parent node
if (mNodes[grandParentNodeID].leftChildID == parentNodeID) { if (mNodes[grandParentNodeID].children[0] == parentNodeID) {
mNodes[grandParentNodeID].leftChildID = siblingNodeID; mNodes[grandParentNodeID].children[0] = siblingNodeID;
} }
else { else {
assert(mNodes[grandParentNodeID].rightChildID == parentNodeID); assert(mNodes[grandParentNodeID].children[1] == parentNodeID);
mNodes[grandParentNodeID].rightChildID = siblingNodeID; mNodes[grandParentNodeID].children[1] = siblingNodeID;
} }
mNodes[siblingNodeID].parentID = grandParentNodeID; mNodes[siblingNodeID].parentID = grandParentNodeID;
releaseNode(parentNodeID); releaseNode(parentNodeID);
@ -383,8 +384,8 @@ void DynamicAABBTree::removeLeafNode(int nodeID) {
currentNodeID = balanceSubTreeAtNode(currentNodeID); currentNodeID = balanceSubTreeAtNode(currentNodeID);
// Get the two children of the current node // Get the two children of the current node
int leftChildID = mNodes[currentNodeID].leftChildID; int leftChildID = mNodes[currentNodeID].children[0];
int rightChildID = mNodes[currentNodeID].rightChildID; int rightChildID = mNodes[currentNodeID].children[1];
// Recompute the AABB and the height of the current node // Recompute the AABB and the height of the current node
mNodes[currentNodeID].aabb.mergeTwoAABBs(mNodes[leftChildID].aabb, mNodes[currentNodeID].aabb.mergeTwoAABBs(mNodes[leftChildID].aabb,
@ -422,8 +423,8 @@ int DynamicAABBTree::balanceSubTreeAtNode(int nodeID) {
} }
// Get the two children nodes // Get the two children nodes
int nodeBID = nodeA->leftChildID; int nodeBID = nodeA->children[0];
int nodeCID = nodeA->rightChildID; int nodeCID = nodeA->children[1];
assert(nodeBID >= 0 && nodeBID < mNbAllocatedNodes); assert(nodeBID >= 0 && nodeBID < mNbAllocatedNodes);
assert(nodeCID >= 0 && nodeCID < mNbAllocatedNodes); assert(nodeCID >= 0 && nodeCID < mNbAllocatedNodes);
TreeNode* nodeB = mNodes + nodeBID; TreeNode* nodeB = mNodes + nodeBID;
@ -435,25 +436,25 @@ 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) {
int nodeFID = nodeC->leftChildID; int nodeFID = nodeC->children[0];
int nodeGID = nodeC->rightChildID; int nodeGID = nodeC->children[1];
assert(nodeFID >= 0 && nodeFID < mNbAllocatedNodes); assert(nodeFID >= 0 && nodeFID < mNbAllocatedNodes);
assert(nodeGID >= 0 && nodeGID < mNbAllocatedNodes); assert(nodeGID >= 0 && nodeGID < mNbAllocatedNodes);
TreeNode* nodeF = mNodes + nodeFID; TreeNode* nodeF = mNodes + nodeFID;
TreeNode* nodeG = mNodes + nodeGID; TreeNode* nodeG = mNodes + nodeGID;
nodeC->leftChildID = nodeID; nodeC->children[0] = nodeID;
nodeC->parentID = nodeA->parentID; nodeC->parentID = nodeA->parentID;
nodeA->parentID = nodeCID; nodeA->parentID = nodeCID;
if (nodeC->parentID != TreeNode::NULL_TREE_NODE) { if (nodeC->parentID != TreeNode::NULL_TREE_NODE) {
if (mNodes[nodeC->parentID].leftChildID == nodeID) { if (mNodes[nodeC->parentID].children[0] == nodeID) {
mNodes[nodeC->parentID].leftChildID = nodeCID; mNodes[nodeC->parentID].children[0] = nodeCID;
} }
else { else {
assert(mNodes[nodeC->parentID].rightChildID == nodeID); assert(mNodes[nodeC->parentID].children[1] == nodeID);
mNodes[nodeC->parentID].rightChildID = nodeCID; mNodes[nodeC->parentID].children[1] = nodeCID;
} }
} }
else { else {
@ -462,8 +463,8 @@ int DynamicAABBTree::balanceSubTreeAtNode(int nodeID) {
// 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->rightChildID = nodeFID; nodeC->children[1] = nodeFID;
nodeA->rightChildID = nodeGID; nodeA->children[1] = nodeGID;
nodeG->parentID = nodeID; nodeG->parentID = nodeID;
// Recompute the AABB of node A and C // Recompute the AABB of node A and C
@ -477,8 +478,8 @@ int DynamicAABBTree::balanceSubTreeAtNode(int nodeID) {
assert(nodeC->height > 0); assert(nodeC->height > 0);
} }
else { // If the right node C was higher than left node B because of node G else { // If the right node C was higher than left node B because of node G
nodeC->rightChildID = nodeGID; nodeC->children[1] = nodeGID;
nodeA->rightChildID = nodeFID; nodeA->children[1] = nodeFID;
nodeF->parentID = nodeID; nodeF->parentID = nodeID;
// Recompute the AABB of node A and C // Recompute the AABB of node A and C
@ -499,25 +500,25 @@ 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) {
int nodeFID = nodeB->leftChildID; int nodeFID = nodeB->children[0];
int nodeGID = nodeB->rightChildID; int nodeGID = nodeB->children[1];
assert(nodeFID >= 0 && nodeFID < mNbAllocatedNodes); assert(nodeFID >= 0 && nodeFID < mNbAllocatedNodes);
assert(nodeGID >= 0 && nodeGID < mNbAllocatedNodes); assert(nodeGID >= 0 && nodeGID < mNbAllocatedNodes);
TreeNode* nodeF = mNodes + nodeFID; TreeNode* nodeF = mNodes + nodeFID;
TreeNode* nodeG = mNodes + nodeGID; TreeNode* nodeG = mNodes + nodeGID;
nodeB->leftChildID = nodeID; nodeB->children[0] = nodeID;
nodeB->parentID = nodeA->parentID; nodeB->parentID = nodeA->parentID;
nodeA->parentID = nodeBID; nodeA->parentID = nodeBID;
if (nodeB->parentID != TreeNode::NULL_TREE_NODE) { if (nodeB->parentID != TreeNode::NULL_TREE_NODE) {
if (mNodes[nodeB->parentID].leftChildID == nodeID) { if (mNodes[nodeB->parentID].children[0] == nodeID) {
mNodes[nodeB->parentID].leftChildID = nodeBID; mNodes[nodeB->parentID].children[0] = nodeBID;
} }
else { else {
assert(mNodes[nodeB->parentID].rightChildID == nodeID); assert(mNodes[nodeB->parentID].children[1] == nodeID);
mNodes[nodeB->parentID].rightChildID = nodeBID; mNodes[nodeB->parentID].children[1] = nodeBID;
} }
} }
else { else {
@ -526,8 +527,8 @@ int DynamicAABBTree::balanceSubTreeAtNode(int nodeID) {
// 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->rightChildID = nodeFID; nodeB->children[1] = nodeFID;
nodeA->leftChildID = nodeGID; nodeA->children[0] = nodeGID;
nodeG->parentID = nodeID; nodeG->parentID = nodeID;
// Recompute the AABB of node A and B // Recompute the AABB of node A and B
@ -541,8 +542,8 @@ int DynamicAABBTree::balanceSubTreeAtNode(int nodeID) {
assert(nodeB->height > 0); assert(nodeB->height > 0);
} }
else { // If the left node B was higher than right node C because of node G else { // If the left node B was higher than right node C because of node G
nodeB->rightChildID = nodeGID; nodeB->children[1] = nodeGID;
nodeA->leftChildID = nodeFID; nodeA->children[0] = nodeFID;
nodeF->parentID = nodeID; nodeF->parentID = nodeID;
// Recompute the AABB of node A and B // Recompute the AABB of node A and B
@ -568,7 +569,8 @@ int DynamicAABBTree::balanceSubTreeAtNode(int nodeID) {
/// For each overlapping shape with the AABB given in parameter, the /// For each overlapping shape with the AABB given in parameter, the
/// BroadPhase::notifyOverlappingPair() method is called to store a /// BroadPhase::notifyOverlappingPair() method is called to store a
/// potential overlapping pair. /// potential overlapping pair.
void DynamicAABBTree::reportAllShapesOverlappingWith(int nodeID, const AABB& aabb) { 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;
@ -593,13 +595,13 @@ 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
mBroadPhase.notifyOverlappingPair(nodeID, nodeIDToVisit); callback.notifyOverlappingAABBs(nodeID, nodeIDToVisit);
} }
else { // If the node is not a leaf else { // If the node is not a leaf
// We need to visit its children // We need to visit its children
stack.push(nodeToVisit->leftChildID); stack.push(nodeToVisit->children[0]);
stack.push(nodeToVisit->rightChildID); stack.push(nodeToVisit->children[1]);
} }
} }
} }
@ -607,7 +609,8 @@ void DynamicAABBTree::reportAllShapesOverlappingWith(int nodeID, const AABB& aab
// Ray casting method // Ray casting method
void DynamicAABBTree::raycast(const Ray& ray, RaycastTest& raycastTest, void DynamicAABBTree::raycast(const Ray& ray, RaycastTest& raycastTest,
unsigned short raycastWithCategoryMaskBits) const { unsigned short raycastWithCategoryMaskBits,
const BroadPhaseRaycastTestCallback& callback) const {
decimal maxFraction = ray.maxFraction; decimal maxFraction = ray.maxFraction;
@ -639,46 +642,39 @@ void DynamicAABBTree::raycast(const Ray& ray, RaycastTest& raycastTest,
// If the node is a leaf of the tree // If the node is a leaf of the tree
if (node->isLeaf()) { if (node->isLeaf()) {
// Check if the raycast filtering mask allows raycast against this shape Ray rayTemp(ray.point1, ray.point2, maxFraction);
if ((raycastWithCategoryMaskBits & node->proxyShape->getCollisionCategoryBits()) != 0) {
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,
raycastWithCategoryMaskBits);
// Ask the collision detection to perform a ray cast test against // If the user returned a hitFraction of zero, it means that
// the proxy shape of this node because the ray is overlapping // the raycasting should stop here
// with the shape in the broad-phase if (hitFraction == decimal(0.0)) {
decimal hitFraction = raycastTest.raycastAgainstShape(node->proxyShape, return;
rayTemp);
// If the user returned a hitFraction of zero, it means that
// the raycasting should stop here
if (hitFraction == decimal(0.0)) {
return;
}
// If the user returned a positive fraction
if (hitFraction > decimal(0.0)) {
// We update the maxFraction value and the ray
// AABB using the new maximum fraction
if (hitFraction < maxFraction) {
maxFraction = hitFraction;
}
endPoint = ray.point1 + maxFraction * (ray.point2 - ray.point1);
rayAABB.mMinCoordinates = Vector3::min(ray.point1, endPoint);
rayAABB.mMaxCoordinates = Vector3::max(ray.point1, endPoint);
}
// If the user returned a negative fraction, we continue
// the raycasting as if the proxy shape did not exist
} }
// If the user returned a positive fraction
if (hitFraction > decimal(0.0)) {
// We update the maxFraction value and the ray
// AABB using the new maximum fraction
if (hitFraction < maxFraction) {
maxFraction = hitFraction;
}
endPoint = ray.point1 + maxFraction * (ray.point2 - ray.point1);
rayAABB.mMinCoordinates = Vector3::min(ray.point1, endPoint);
rayAABB.mMaxCoordinates = Vector3::max(ray.point1, endPoint);
}
// 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 // Push its children in the stack of nodes to explore
stack.push(node->leftChildID); stack.push(node->children[0]);
stack.push(node->rightChildID); stack.push(node->children[1]);
} }
} }
} }
@ -716,8 +712,8 @@ void DynamicAABBTree::checkNode(int nodeID) const {
// Get the children nodes // Get the children nodes
TreeNode* pNode = mNodes + nodeID; TreeNode* pNode = mNodes + nodeID;
int leftChild = pNode->leftChildID; int leftChild = pNode->children[0];
int rightChild = pNode->rightChildID; int rightChild = pNode->children[1];
assert(pNode->height >= 0); assert(pNode->height >= 0);
assert(pNode->aabb.getVolume() > 0); assert(pNode->aabb.getVolume() > 0);
@ -772,8 +768,8 @@ int DynamicAABBTree::computeHeight(int nodeID) {
} }
// Compute the height of the left and right sub-tree // Compute the height of the left and right sub-tree
int leftHeight = computeHeight(node->leftChildID); int leftHeight = computeHeight(node->children[0]);
int rightHeight = computeHeight(node->rightChildID); int rightHeight = computeHeight(node->children[1]);
// Return the height of the node // Return the height of the node
return 1 + std::max(leftHeight, rightHeight); return 1 + std::max(leftHeight, rightHeight);

65
src/collision/broadphase/DynamicAABBTree.h
View File

@ -36,13 +36,10 @@ namespace reactphysics3d {
// Declarations // Declarations
class BroadPhaseAlgorithm; class BroadPhaseAlgorithm;
class BroadPhaseRaycastTestCallback;
class BroadPhaseOverlapCallback;
struct RaycastTest; struct RaycastTest;
// Raycast callback method pointer type
typedef decimal (*RaycastTestCallback) (ProxyShape* shape,
RaycastCallback* userCallback,
const Ray& ray);
// Structure TreeNode // Structure TreeNode
/** /**
* This structure represents a node of the dynamic AABB tree. * This structure represents a node of the dynamic AABB tree.
@ -56,24 +53,33 @@ struct TreeNode {
// -------------------- Attributes -------------------- // // -------------------- Attributes -------------------- //
/// Parent node ID // A node is either in the tree (has a parent) or in the free nodes list
int parentID; // (has a next node)
union {
/// Left and right child of the node /// Parent node ID
int leftChildID, rightChildID; int32 parentID;
/// Next allocated node ID /// Next allocated node ID
int nextNodeID; int32 nextNodeID;
};
// A node is either a leaf (has data) or is an internal node (has children)
union {
/// 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;
};
/// Height of the node in the tree /// Height of the node in the tree
int height; int16 height;
/// Fat axis aligned bounding box (AABB) corresponding to the node /// Fat axis aligned bounding box (AABB) corresponding to the node
AABB aabb; AABB aabb;
/// Pointer to the corresponding collision shape (in case this node is a leaf)
ProxyShape* proxyShape;
// -------------------- Methods -------------------- // // -------------------- Methods -------------------- //
/// Return true if the node is a leaf of the tree /// Return true if the node is a leaf of the tree
@ -94,9 +100,6 @@ class DynamicAABBTree {
// -------------------- Attributes -------------------- // // -------------------- Attributes -------------------- //
/// Reference to the broad-phase
BroadPhaseAlgorithm& mBroadPhase;
/// Pointer to the memory location of the nodes of the tree /// Pointer to the memory location of the nodes of the tree
TreeNode* mNodes; TreeNode* mNodes;
@ -112,6 +115,10 @@ class DynamicAABBTree {
/// Number of nodes in the tree /// Number of nodes in the tree
int mNbNodes; int mNbNodes;
/// Extra AABB Gap used to allow the collision shape to move a little bit
/// without triggering a large modification of the tree which can be costly
decimal mExtraAABBGap;
// -------------------- Methods -------------------- // // -------------------- Methods -------------------- //
/// Allocate and return a node to use in the tree /// Allocate and return a node to use in the tree
@ -147,13 +154,13 @@ class DynamicAABBTree {
// -------------------- Methods -------------------- // // -------------------- Methods -------------------- //
/// Constructor /// Constructor
DynamicAABBTree(BroadPhaseAlgorithm& broadPhase); DynamicAABBTree(decimal extraAABBGap = decimal(0.0));
/// Destructor /// Destructor
~DynamicAABBTree(); ~DynamicAABBTree();
/// Add an object into the tree /// Add an object into the tree
void addObject(ProxyShape* proxyShape, const AABB& aabb); int addObject(void* nodeData, const AABB& aabb);
/// Remove an object from the tree /// Remove an object from the tree
void removeObject(int nodeID); void removeObject(int nodeID);
@ -164,15 +171,17 @@ 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 collision shape of a given leaf node of the tree /// Return the pointer to the data of a given leaf node of the tree
ProxyShape* getCollisionShape(int nodeID) const; void* getNodeData(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 reportAllShapesOverlappingWith(int nodeID, const AABB& aabb,
BroadPhaseOverlapCallback& callback);
/// Ray casting method /// Ray casting method
void raycast(const Ray& ray, RaycastTest& raycastTest, void raycast(const Ray& ray, RaycastTest& raycastTest,
unsigned short raycastWithCategoryMaskBits) const; unsigned short raycastWithCategoryMaskBits,
const BroadPhaseRaycastTestCallback& callback) const;
/// Compute the height of the tree /// Compute the height of the tree
int computeHeight(); int computeHeight();
@ -180,7 +189,7 @@ class DynamicAABBTree {
// Return true if the node is a leaf of the tree // Return true if the node is a leaf of the tree
inline bool TreeNode::isLeaf() const { inline bool TreeNode::isLeaf() const {
return leftChildID == NULL_TREE_NODE; return (height == 0);
} }
// Return the fat AABB corresponding to a given node ID // Return the fat AABB corresponding to a given node ID
@ -189,11 +198,11 @@ inline const AABB& DynamicAABBTree::getFatAABB(int nodeID) const {
return mNodes[nodeID].aabb; return mNodes[nodeID].aabb;
} }
// Return the collision shape of a given leaf node of the tree // Return the pointer to the data of a given leaf node of the tree
inline ProxyShape* DynamicAABBTree::getCollisionShape(int nodeID) const { inline void* DynamicAABBTree::getNodeData(int nodeID) const {
assert(nodeID >= 0 && nodeID < mNbAllocatedNodes); assert(nodeID >= 0 && nodeID < mNbAllocatedNodes);
assert(mNodes[nodeID].isLeaf()); assert(mNodes[nodeID].isLeaf());
return mNodes[nodeID].proxyShape; return mNodes[nodeID].data;
} }
} }

5
src/configuration.h
View File

@ -51,6 +51,11 @@ typedef long unsigned int luint;
typedef luint bodyindex; typedef luint bodyindex;
typedef std::pair<bodyindex, bodyindex> bodyindexpair; typedef std::pair<bodyindex, bodyindex> bodyindexpair;
typedef signed short int16;
typedef signed int int32;
typedef unsigned short uint16;
typedef unsigned int uint32;
// ------------------- Enumerations ------------------- // // ------------------- Enumerations ------------------- //
/// Position correction technique used in the constraint solver (for joints). /// Position correction technique used in the constraint solver (for joints).