diff --git a/src/collision/broadphase/BroadPhaseAlgorithm.cpp b/src/collision/broadphase/BroadPhaseAlgorithm.cpp index 00ed62f5..d91ce848 100644 --- a/src/collision/broadphase/BroadPhaseAlgorithm.cpp +++ b/src/collision/broadphase/BroadPhaseAlgorithm.cpp @@ -33,7 +33,7 @@ using namespace reactphysics3d; // Constructor 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), mCollisionDetection(collisionDetection) { @@ -118,7 +118,10 @@ 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 - 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) // 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 // this AABB. The method BroadPhase::notifiyOverlappingPair() will be called // 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 @@ -198,8 +201,8 @@ void BroadPhaseAlgorithm::computeOverlappingPairs() { i++; // Get the two collision shapes of the pair - ProxyShape* shape1 = mDynamicAABBTree.getCollisionShape(pair->collisionShape1ID); - ProxyShape* shape2 = mDynamicAABBTree.getCollisionShape(pair->collisionShape2ID); + ProxyShape* shape1 = static_cast(mDynamicAABBTree.getNodeData(pair->collisionShape1ID)); + ProxyShape* shape2 = static_cast(mDynamicAABBTree.getNodeData(pair->collisionShape2ID)); // Notify the collision detection about the overlapping pair 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 -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 (node1ID == node2ID) return; @@ -256,3 +259,25 @@ void BroadPhaseAlgorithm::notifyOverlappingPair(int node1ID, int node2ID) { mPotentialPairs[mNbPotentialPairs].collisionShape2ID = std::max(node1ID, node2ID); 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(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; + } diff --git a/src/collision/broadphase/BroadPhaseAlgorithm.h b/src/collision/broadphase/BroadPhaseAlgorithm.h index 06b5579a..121fe529 100644 --- a/src/collision/broadphase/BroadPhaseAlgorithm.h +++ b/src/collision/broadphase/BroadPhaseAlgorithm.h @@ -59,6 +59,35 @@ 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 { + + 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 /** * 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 * tree data structure is used for fast broad-phase collision detection. */ -class BroadPhaseAlgorithm { +class BroadPhaseAlgorithm : BroadPhaseRaycastTestCallback, BroadPhaseOverlapCallback { protected : @@ -142,7 +171,12 @@ class BroadPhaseAlgorithm { void removeMovedCollisionShape(int broadPhaseID); /// 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 void computeOverlappingPairs(); @@ -179,7 +213,7 @@ inline bool BroadPhaseAlgorithm::testOverlappingShapes(const ProxyShape* shape1, // Ray casting method inline void BroadPhaseAlgorithm::raycast(const Ray& ray, RaycastTest& raycastTest, unsigned short raycastWithCategoryMaskBits) const { - mDynamicAABBTree.raycast(ray, raycastTest, raycastWithCategoryMaskBits); + mDynamicAABBTree.raycast(ray, raycastTest, raycastWithCategoryMaskBits, *this); } } diff --git a/src/collision/broadphase/DynamicAABBTree.cpp b/src/collision/broadphase/DynamicAABBTree.cpp index 51651c5c..f82ac450 100644 --- a/src/collision/broadphase/DynamicAABBTree.cpp +++ b/src/collision/broadphase/DynamicAABBTree.cpp @@ -35,7 +35,7 @@ using namespace reactphysics3d; const int TreeNode::NULL_TREE_NODE = -1; // Constructor -DynamicAABBTree::DynamicAABBTree(BroadPhaseAlgorithm& broadPhase) : mBroadPhase(broadPhase){ +DynamicAABBTree::DynamicAABBTree(decimal extraAABBGap) : mExtraAABBGap(extraAABBGap) { mRootNodeID = TreeNode::NULL_TREE_NODE; mNbNodes = 0; @@ -93,9 +93,9 @@ int DynamicAABBTree::allocateNode() { int freeNodeID = mFreeNodeID; mFreeNodeID = mNodes[freeNodeID].nextNodeID; mNodes[freeNodeID].parentID = TreeNode::NULL_TREE_NODE; - mNodes[freeNodeID].leftChildID = TreeNode::NULL_TREE_NODE; - mNodes[freeNodeID].rightChildID = TreeNode::NULL_TREE_NODE; - mNodes[freeNodeID].proxyShape = NULL; + 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++; @@ -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 // 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) int nodeID = allocateNode(); // 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.setMax(aabb.getMax() + gap); - // Set the collision shape - mNodes[nodeID].proxyShape = proxyShape; + // Set the node data + mNodes[nodeID].data = nodeData; // Set the height of the node in the tree mNodes[nodeID].height = 0; @@ -136,9 +136,10 @@ void DynamicAABBTree::addObject(ProxyShape* proxyShape, const AABB& aabb) { insertLeafNode(nodeID); assert(mNodes[nodeID].isLeaf()); - // Set the broad-phase ID of the proxy shape - proxyShape->mBroadPhaseID = nodeID; assert(nodeID >= 0); + + // Return the Id of the node + return nodeID; } // 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 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.mMaxCoordinates += gap; @@ -227,8 +228,8 @@ void DynamicAABBTree::insertLeafNode(int nodeID) { int currentNodeID = mRootNodeID; while (!mNodes[currentNodeID].isLeaf()) { - int leftChild = mNodes[currentNodeID].leftChildID; - int rightChild = mNodes[currentNodeID].rightChildID; + int leftChild = mNodes[currentNodeID].children[0]; + int rightChild = mNodes[currentNodeID].children[1]; // Compute the merged AABB decimal volumeAABB = mNodes[currentNodeID].aabb.getVolume(); @@ -285,27 +286,27 @@ void DynamicAABBTree::insertLeafNode(int nodeID) { int oldParentNode = mNodes[siblingNode].parentID; int newParentNode = allocateNode(); mNodes[newParentNode].parentID = oldParentNode; - mNodes[newParentNode].proxyShape = NULL; + 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) { - if (mNodes[oldParentNode].leftChildID == siblingNode) { - mNodes[oldParentNode].leftChildID = newParentNode; + if (mNodes[oldParentNode].children[0] == siblingNode) { + mNodes[oldParentNode].children[0] = newParentNode; } else { - mNodes[oldParentNode].rightChildID = newParentNode; + mNodes[oldParentNode].children[1] = newParentNode; } - mNodes[newParentNode].leftChildID = siblingNode; - mNodes[newParentNode].rightChildID = nodeID; + mNodes[newParentNode].children[0] = siblingNode; + mNodes[newParentNode].children[1] = nodeID; mNodes[siblingNode].parentID = newParentNode; mNodes[nodeID].parentID = newParentNode; } else { // If the sibling node was the root node - mNodes[newParentNode].leftChildID = siblingNode; - mNodes[newParentNode].rightChildID = nodeID; + mNodes[newParentNode].children[0] = siblingNode; + mNodes[newParentNode].children[1] = nodeID; mNodes[siblingNode].parentID = newParentNode; mNodes[nodeID].parentID = newParentNode; mRootNodeID = newParentNode; @@ -319,8 +320,8 @@ void DynamicAABBTree::insertLeafNode(int nodeID) { currentNodeID = balanceSubTreeAtNode(currentNodeID); assert(mNodes[nodeID].isLeaf()); - int leftChild = mNodes[currentNodeID].leftChildID; - int rightChild = mNodes[currentNodeID].rightChildID; + int leftChild = mNodes[currentNodeID].children[0]; + int rightChild = mNodes[currentNodeID].children[1]; assert(leftChild != 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 grandParentNodeID = mNodes[parentNodeID].parentID; int siblingNodeID; - if (mNodes[parentNodeID].leftChildID == nodeID) { - siblingNodeID = mNodes[parentNodeID].rightChildID; + if (mNodes[parentNodeID].children[0] == nodeID) { + siblingNodeID = mNodes[parentNodeID].children[1]; } else { - siblingNodeID = mNodes[parentNodeID].leftChildID; + siblingNodeID = mNodes[parentNodeID].children[0]; } // If the parent of the node to remove is not the root node if (grandParentNodeID != TreeNode::NULL_TREE_NODE) { // Destroy the parent node - if (mNodes[grandParentNodeID].leftChildID == parentNodeID) { - mNodes[grandParentNodeID].leftChildID = siblingNodeID; + if (mNodes[grandParentNodeID].children[0] == parentNodeID) { + mNodes[grandParentNodeID].children[0] = siblingNodeID; } else { - assert(mNodes[grandParentNodeID].rightChildID == parentNodeID); - mNodes[grandParentNodeID].rightChildID = siblingNodeID; + assert(mNodes[grandParentNodeID].children[1] == parentNodeID); + mNodes[grandParentNodeID].children[1] = siblingNodeID; } mNodes[siblingNodeID].parentID = grandParentNodeID; releaseNode(parentNodeID); @@ -383,8 +384,8 @@ void DynamicAABBTree::removeLeafNode(int nodeID) { currentNodeID = balanceSubTreeAtNode(currentNodeID); // Get the two children of the current node - int leftChildID = mNodes[currentNodeID].leftChildID; - int rightChildID = mNodes[currentNodeID].rightChildID; + int leftChildID = mNodes[currentNodeID].children[0]; + int rightChildID = mNodes[currentNodeID].children[1]; // Recompute the AABB and the height of the current node mNodes[currentNodeID].aabb.mergeTwoAABBs(mNodes[leftChildID].aabb, @@ -422,8 +423,8 @@ int DynamicAABBTree::balanceSubTreeAtNode(int nodeID) { } // Get the two children nodes - int nodeBID = nodeA->leftChildID; - int nodeCID = nodeA->rightChildID; + int nodeBID = nodeA->children[0]; + int nodeCID = nodeA->children[1]; assert(nodeBID >= 0 && nodeBID < mNbAllocatedNodes); assert(nodeCID >= 0 && nodeCID < mNbAllocatedNodes); 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 (balanceFactor > 1) { - int nodeFID = nodeC->leftChildID; - int nodeGID = nodeC->rightChildID; + int nodeFID = nodeC->children[0]; + int nodeGID = nodeC->children[1]; assert(nodeFID >= 0 && nodeFID < mNbAllocatedNodes); assert(nodeGID >= 0 && nodeGID < mNbAllocatedNodes); TreeNode* nodeF = mNodes + nodeFID; TreeNode* nodeG = mNodes + nodeGID; - nodeC->leftChildID = nodeID; + nodeC->children[0] = nodeID; nodeC->parentID = nodeA->parentID; nodeA->parentID = nodeCID; if (nodeC->parentID != TreeNode::NULL_TREE_NODE) { - if (mNodes[nodeC->parentID].leftChildID == nodeID) { - mNodes[nodeC->parentID].leftChildID = nodeCID; + if (mNodes[nodeC->parentID].children[0] == nodeID) { + mNodes[nodeC->parentID].children[0] = nodeCID; } else { - assert(mNodes[nodeC->parentID].rightChildID == nodeID); - mNodes[nodeC->parentID].rightChildID = nodeCID; + assert(mNodes[nodeC->parentID].children[1] == nodeID); + mNodes[nodeC->parentID].children[1] = nodeCID; } } 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 (nodeF->height > nodeG->height) { - nodeC->rightChildID = nodeFID; - nodeA->rightChildID = nodeGID; + nodeC->children[1] = nodeFID; + nodeA->children[1] = nodeGID; nodeG->parentID = nodeID; // Recompute the AABB of node A and C @@ -477,8 +478,8 @@ int DynamicAABBTree::balanceSubTreeAtNode(int nodeID) { assert(nodeC->height > 0); } else { // If the right node C was higher than left node B because of node G - nodeC->rightChildID = nodeGID; - nodeA->rightChildID = nodeFID; + nodeC->children[1] = nodeGID; + nodeA->children[1] = nodeFID; nodeF->parentID = nodeID; // 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 (balanceFactor < -1) { - int nodeFID = nodeB->leftChildID; - int nodeGID = nodeB->rightChildID; + int nodeFID = nodeB->children[0]; + int nodeGID = nodeB->children[1]; assert(nodeFID >= 0 && nodeFID < mNbAllocatedNodes); assert(nodeGID >= 0 && nodeGID < mNbAllocatedNodes); TreeNode* nodeF = mNodes + nodeFID; TreeNode* nodeG = mNodes + nodeGID; - nodeB->leftChildID = nodeID; + nodeB->children[0] = nodeID; nodeB->parentID = nodeA->parentID; nodeA->parentID = nodeBID; if (nodeB->parentID != TreeNode::NULL_TREE_NODE) { - if (mNodes[nodeB->parentID].leftChildID == nodeID) { - mNodes[nodeB->parentID].leftChildID = nodeBID; + if (mNodes[nodeB->parentID].children[0] == nodeID) { + mNodes[nodeB->parentID].children[0] = nodeBID; } else { - assert(mNodes[nodeB->parentID].rightChildID == nodeID); - mNodes[nodeB->parentID].rightChildID = nodeBID; + assert(mNodes[nodeB->parentID].children[1] == nodeID); + mNodes[nodeB->parentID].children[1] = nodeBID; } } 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 (nodeF->height > nodeG->height) { - nodeB->rightChildID = nodeFID; - nodeA->leftChildID = nodeGID; + nodeB->children[1] = nodeFID; + nodeA->children[0] = nodeGID; nodeG->parentID = nodeID; // Recompute the AABB of node A and B @@ -541,8 +542,8 @@ int DynamicAABBTree::balanceSubTreeAtNode(int nodeID) { assert(nodeB->height > 0); } else { // If the left node B was higher than right node C because of node G - nodeB->rightChildID = nodeGID; - nodeA->leftChildID = nodeFID; + nodeB->children[1] = nodeGID; + nodeA->children[0] = nodeFID; nodeF->parentID = nodeID; // 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 /// BroadPhase::notifyOverlappingPair() method is called to store a /// 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 Stack stack; @@ -593,13 +595,13 @@ void DynamicAABBTree::reportAllShapesOverlappingWith(int nodeID, const AABB& aab if (nodeToVisit->isLeaf()) { // 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 // We need to visit its children - stack.push(nodeToVisit->leftChildID); - stack.push(nodeToVisit->rightChildID); + stack.push(nodeToVisit->children[0]); + stack.push(nodeToVisit->children[1]); } } } @@ -607,7 +609,8 @@ void DynamicAABBTree::reportAllShapesOverlappingWith(int nodeID, const AABB& aab // Ray casting method void DynamicAABBTree::raycast(const Ray& ray, RaycastTest& raycastTest, - unsigned short raycastWithCategoryMaskBits) const { + unsigned short raycastWithCategoryMaskBits, + const BroadPhaseRaycastTestCallback& callback) const { 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 (node->isLeaf()) { - // Check if the raycast filtering mask allows raycast against this shape - if ((raycastWithCategoryMaskBits & node->proxyShape->getCollisionCategoryBits()) != 0) { + 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 + decimal hitFraction = callback.raycastBroadPhaseShape(node->data, raycastTest, rayTemp, + raycastWithCategoryMaskBits); - // 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 - decimal hitFraction = raycastTest.raycastAgainstShape(node->proxyShape, - 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 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 } else { // If the node has children // Push its children in the stack of nodes to explore - stack.push(node->leftChildID); - stack.push(node->rightChildID); + stack.push(node->children[0]); + stack.push(node->children[1]); } } } @@ -716,8 +712,8 @@ void DynamicAABBTree::checkNode(int nodeID) const { // Get the children nodes TreeNode* pNode = mNodes + nodeID; - int leftChild = pNode->leftChildID; - int rightChild = pNode->rightChildID; + int leftChild = pNode->children[0]; + int rightChild = pNode->children[1]; assert(pNode->height >= 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 - int leftHeight = computeHeight(node->leftChildID); - int rightHeight = computeHeight(node->rightChildID); + int leftHeight = computeHeight(node->children[0]); + int rightHeight = computeHeight(node->children[1]); // Return the height of the node return 1 + std::max(leftHeight, rightHeight); diff --git a/src/collision/broadphase/DynamicAABBTree.h b/src/collision/broadphase/DynamicAABBTree.h index a6f782e4..5a6d3cc6 100644 --- a/src/collision/broadphase/DynamicAABBTree.h +++ b/src/collision/broadphase/DynamicAABBTree.h @@ -36,13 +36,10 @@ namespace reactphysics3d { // Declarations class BroadPhaseAlgorithm; +class BroadPhaseRaycastTestCallback; +class BroadPhaseOverlapCallback; struct RaycastTest; -// Raycast callback method pointer type -typedef decimal (*RaycastTestCallback) (ProxyShape* shape, - RaycastCallback* userCallback, - const Ray& ray); - // Structure TreeNode /** * This structure represents a node of the dynamic AABB tree. @@ -56,24 +53,33 @@ struct TreeNode { // -------------------- Attributes -------------------- // - /// Parent node ID - int parentID; + // A node is either in the tree (has a parent) or in the free nodes list + // (has a next node) + union { - /// Left and right child of the node - int leftChildID, rightChildID; + /// Parent node ID + int32 parentID; - /// Next allocated node ID - int nextNodeID; + /// Next allocated node ID + 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 - int height; + int16 height; /// Fat axis aligned bounding box (AABB) corresponding to the node AABB aabb; - /// Pointer to the corresponding collision shape (in case this node is a leaf) - ProxyShape* proxyShape; - // -------------------- Methods -------------------- // /// Return true if the node is a leaf of the tree @@ -94,9 +100,6 @@ class DynamicAABBTree { // -------------------- Attributes -------------------- // - /// Reference to the broad-phase - BroadPhaseAlgorithm& mBroadPhase; - /// Pointer to the memory location of the nodes of the tree TreeNode* mNodes; @@ -112,6 +115,10 @@ class DynamicAABBTree { /// Number of nodes in the tree 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 -------------------- // /// Allocate and return a node to use in the tree @@ -147,13 +154,13 @@ class DynamicAABBTree { // -------------------- Methods -------------------- // /// Constructor - DynamicAABBTree(BroadPhaseAlgorithm& broadPhase); + DynamicAABBTree(decimal extraAABBGap = decimal(0.0)); /// Destructor ~DynamicAABBTree(); /// 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 void removeObject(int nodeID); @@ -164,15 +171,17 @@ class DynamicAABBTree { /// Return the fat AABB corresponding to a given node ID const AABB& getFatAABB(int nodeID) const; - /// Return the collision shape of a given leaf node of the tree - ProxyShape* getCollisionShape(int nodeID) const; + /// Return the pointer to the data of a given leaf node of the tree + void* getNodeData(int nodeID) const; /// 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 void raycast(const Ray& ray, RaycastTest& raycastTest, - unsigned short raycastWithCategoryMaskBits) const; + unsigned short raycastWithCategoryMaskBits, + const BroadPhaseRaycastTestCallback& callback) const; /// Compute the height of the tree int computeHeight(); @@ -180,7 +189,7 @@ class DynamicAABBTree { // Return true if the node is a leaf of the tree inline bool TreeNode::isLeaf() const { - return leftChildID == NULL_TREE_NODE; + return (height == 0); } // 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 the collision shape of a given leaf node of the tree -inline ProxyShape* DynamicAABBTree::getCollisionShape(int nodeID) const { +// Return the pointer to the data of a given leaf node of the tree +inline void* DynamicAABBTree::getNodeData(int nodeID) const { assert(nodeID >= 0 && nodeID < mNbAllocatedNodes); assert(mNodes[nodeID].isLeaf()); - return mNodes[nodeID].proxyShape; + return mNodes[nodeID].data; } } diff --git a/src/configuration.h b/src/configuration.h index 156a1d84..88bc83e5 100644 --- a/src/configuration.h +++ b/src/configuration.h @@ -51,6 +51,11 @@ typedef long unsigned int luint; typedef luint bodyindex; typedef std::pair bodyindexpair; +typedef signed short int16; +typedef signed int int32; +typedef unsigned short uint16; +typedef unsigned int uint32; + // ------------------- Enumerations ------------------- // /// Position correction technique used in the constraint solver (for joints).