835 lines
30 KiB
C++
835 lines
30 KiB
C++
/********************************************************************************
|
|
* ReactPhysics3D physics library, http://www.reactphysics3d.com *
|
|
* Copyright (c) 2010-2020 Daniel Chappuis *
|
|
*********************************************************************************
|
|
* *
|
|
* This software is provided 'as-is', without any express or implied warranty. *
|
|
* In no event will the authors be held liable for any damages arising from the *
|
|
* use of this software. *
|
|
* *
|
|
* Permission is granted to anyone to use this software for any purpose, *
|
|
* including commercial applications, and to alter it and redistribute it *
|
|
* freely, subject to the following restrictions: *
|
|
* *
|
|
* 1. The origin of this software must not be misrepresented; you must not claim *
|
|
* that you wrote the original software. If you use this software in a *
|
|
* product, an acknowledgment in the product documentation would be *
|
|
* appreciated but is not required. *
|
|
* *
|
|
* 2. Altered source versions must be plainly marked as such, and must not be *
|
|
* misrepresented as being the original software. *
|
|
* *
|
|
* 3. This notice may not be removed or altered from any source distribution. *
|
|
* *
|
|
********************************************************************************/
|
|
|
|
// Libraries
|
|
#include <reactphysics3d/collision/broadphase/DynamicAABBTree.h>
|
|
#include <reactphysics3d/systems/BroadPhaseSystem.h>
|
|
#include <reactphysics3d/containers/Stack.h>
|
|
#include <reactphysics3d/utils/Profiler.h>
|
|
|
|
using namespace reactphysics3d;
|
|
|
|
// Initialization of static variables
|
|
const int32 TreeNode::NULL_TREE_NODE = -1;
|
|
|
|
// Constructor
|
|
DynamicAABBTree::DynamicAABBTree(MemoryAllocator& allocator, decimal fatAABBInflatePercentage)
|
|
: mAllocator(allocator), mFatAABBInflatePercentage(fatAABBInflatePercentage) {
|
|
|
|
init();
|
|
}
|
|
|
|
// Destructor
|
|
DynamicAABBTree::~DynamicAABBTree() {
|
|
|
|
// Free the allocated memory for the nodes
|
|
mAllocator.release(mNodes, static_cast<size_t>(mNbAllocatedNodes) * sizeof(TreeNode));
|
|
}
|
|
|
|
// Initialize the tree
|
|
void DynamicAABBTree::init() {
|
|
|
|
mRootNodeID = TreeNode::NULL_TREE_NODE;
|
|
mNbNodes = 0;
|
|
mNbAllocatedNodes = 8;
|
|
|
|
// Allocate memory for the nodes of the tree
|
|
mNodes = static_cast<TreeNode*>(mAllocator.allocate(static_cast<size_t>(mNbAllocatedNodes) * sizeof(TreeNode)));
|
|
assert(mNodes);
|
|
std::memset(mNodes, 0, static_cast<size_t>(mNbAllocatedNodes) * sizeof(TreeNode));
|
|
|
|
// Initialize the allocated nodes
|
|
for (int32 i=0; i<mNbAllocatedNodes - 1; i++) {
|
|
mNodes[i].nextNodeID = i + 1;
|
|
mNodes[i].height = -1;
|
|
}
|
|
mNodes[mNbAllocatedNodes - 1].nextNodeID = TreeNode::NULL_TREE_NODE;
|
|
mNodes[mNbAllocatedNodes - 1].height = -1;
|
|
mFreeNodeID = 0;
|
|
}
|
|
|
|
// Clear all the nodes and reset the tree
|
|
void DynamicAABBTree::reset() {
|
|
|
|
// Free the allocated memory for the nodes
|
|
mAllocator.release(mNodes, static_cast<size_t>(mNbAllocatedNodes) * sizeof(TreeNode));
|
|
|
|
// Initialize the tree
|
|
init();
|
|
}
|
|
|
|
// Allocate and return a new node in the tree
|
|
int32 DynamicAABBTree::allocateNode() {
|
|
|
|
// If there is no more allocated node to use
|
|
if (mFreeNodeID == TreeNode::NULL_TREE_NODE) {
|
|
|
|
assert(mNbNodes == mNbAllocatedNodes);
|
|
|
|
// Allocate more nodes in the tree
|
|
int32 oldNbAllocatedNodes = mNbAllocatedNodes;
|
|
mNbAllocatedNodes *= 2;
|
|
TreeNode* oldNodes = mNodes;
|
|
mNodes = static_cast<TreeNode*>(mAllocator.allocate(static_cast<size_t>(mNbAllocatedNodes) * sizeof(TreeNode)));
|
|
assert(mNodes);
|
|
memcpy(mNodes, oldNodes, static_cast<size_t>(mNbNodes) * sizeof(TreeNode));
|
|
mAllocator.release(oldNodes, static_cast<size_t>(oldNbAllocatedNodes) * sizeof(TreeNode));
|
|
|
|
// Initialize the allocated nodes
|
|
for (int32 i=mNbNodes; i<mNbAllocatedNodes - 1; i++) {
|
|
mNodes[i].nextNodeID = i + 1;
|
|
mNodes[i].height = -1;
|
|
}
|
|
mNodes[mNbAllocatedNodes - 1].nextNodeID = TreeNode::NULL_TREE_NODE;
|
|
mNodes[mNbAllocatedNodes - 1].height = -1;
|
|
mFreeNodeID = mNbNodes;
|
|
}
|
|
|
|
// Get the next free node
|
|
int32 freeNodeID = mFreeNodeID;
|
|
mFreeNodeID = mNodes[freeNodeID].nextNodeID;
|
|
mNodes[freeNodeID].parentID = TreeNode::NULL_TREE_NODE;
|
|
mNodes[freeNodeID].height = 0;
|
|
mNbNodes++;
|
|
|
|
return freeNodeID;
|
|
}
|
|
|
|
// Release a node
|
|
void DynamicAABBTree::releaseNode(int nodeID) {
|
|
|
|
assert(mNbNodes > 0);
|
|
assert(nodeID >= 0 && nodeID < mNbAllocatedNodes);
|
|
assert(mNodes[nodeID].height >= 0);
|
|
mNodes[nodeID].nextNodeID = mFreeNodeID;
|
|
mNodes[nodeID].height = -1;
|
|
mFreeNodeID = nodeID;
|
|
mNbNodes--;
|
|
}
|
|
|
|
// Internally add an object into the tree
|
|
int32 DynamicAABBTree::addObjectInternal(const AABB& aabb) {
|
|
|
|
// Get the next available node (or allocate new ones if necessary)
|
|
int32 nodeID = allocateNode();
|
|
|
|
// Create the fat aabb to use in the tree (inflate the aabb by a constant percentage of its size)
|
|
const Vector3 gap(aabb.getExtent() * mFatAABBInflatePercentage * decimal(0.5f));
|
|
mNodes[nodeID].aabb.setMin(aabb.getMin() - gap);
|
|
mNodes[nodeID].aabb.setMax(aabb.getMax() + gap);
|
|
|
|
// Set the height of the node in the tree
|
|
mNodes[nodeID].height = 0;
|
|
|
|
// Insert the new leaf node in the tree
|
|
insertLeafNode(nodeID);
|
|
assert(mNodes[nodeID].isLeaf());
|
|
|
|
assert(nodeID >= 0);
|
|
|
|
// Return the Id of the node
|
|
return nodeID;
|
|
}
|
|
|
|
// Remove an object from the tree
|
|
void DynamicAABBTree::removeObject(int32 nodeID) {
|
|
|
|
assert(nodeID >= 0 && nodeID < mNbAllocatedNodes);
|
|
assert(mNodes[nodeID].isLeaf());
|
|
|
|
// Remove the node from the tree
|
|
removeLeafNode(nodeID);
|
|
releaseNode(nodeID);
|
|
}
|
|
|
|
// Update the dynamic tree after an object has moved.
|
|
/// If the new AABB of the object that has moved is still inside its fat AABB, then
|
|
/// nothing is done. Otherwise, the corresponding node is removed and reinserted into the tree.
|
|
/// The method returns true if the object has been reinserted into the tree.
|
|
/// If the "forceReInsert" parameter is true, we force the existing AABB to take the size
|
|
/// of the "newAABB" parameter even if it is larger than "newAABB". This can be used to shrink the
|
|
/// AABB in the tree for instance if the corresponding collision shape has been shrunk.
|
|
bool DynamicAABBTree::updateObject(int32 nodeID, const AABB& newAABB, bool forceReinsert) {
|
|
|
|
RP3D_PROFILE("DynamicAABBTree::updateObject()", mProfiler);
|
|
|
|
assert(nodeID >= 0 && nodeID < mNbAllocatedNodes);
|
|
assert(mNodes[nodeID].isLeaf());
|
|
assert(mNodes[nodeID].height >= 0);
|
|
|
|
// If the new AABB is still inside the fat AABB of the node
|
|
if (!forceReinsert && mNodes[nodeID].aabb.contains(newAABB)) {
|
|
return false;
|
|
}
|
|
|
|
// If the new AABB is outside the fat AABB, we remove the corresponding node
|
|
removeLeafNode(nodeID);
|
|
|
|
// Compute the fat AABB by inflating the AABB with by a constant percentage of the size of the AABB
|
|
mNodes[nodeID].aabb = newAABB;
|
|
const Vector3 gap(newAABB.getExtent() * mFatAABBInflatePercentage * decimal(0.5f));
|
|
mNodes[nodeID].aabb.mMinCoordinates -= gap;
|
|
mNodes[nodeID].aabb.mMaxCoordinates += gap;
|
|
|
|
assert(mNodes[nodeID].aabb.contains(newAABB));
|
|
|
|
// Reinsert the node into the tree
|
|
insertLeafNode(nodeID);
|
|
|
|
return true;
|
|
}
|
|
|
|
// Insert a leaf node in the tree. The process of inserting a new leaf node
|
|
// in the dynamic tree is described in the book "Introduction to Game Physics
|
|
// with Box2D" by Ian Parberry.
|
|
void DynamicAABBTree::insertLeafNode(int nodeID) {
|
|
|
|
// If the tree is empty
|
|
if (mRootNodeID == TreeNode::NULL_TREE_NODE) {
|
|
mRootNodeID = nodeID;
|
|
mNodes[mRootNodeID].parentID = TreeNode::NULL_TREE_NODE;
|
|
return;
|
|
}
|
|
|
|
assert(mRootNodeID != TreeNode::NULL_TREE_NODE);
|
|
|
|
// Find the best sibling node for the new node
|
|
AABB newNodeAABB = mNodes[nodeID].aabb;
|
|
int currentNodeID = mRootNodeID;
|
|
while (!mNodes[currentNodeID].isLeaf()) {
|
|
|
|
int leftChild = mNodes[currentNodeID].children[0];
|
|
int rightChild = mNodes[currentNodeID].children[1];
|
|
|
|
// Compute the merged AABB
|
|
decimal volumeAABB = mNodes[currentNodeID].aabb.getVolume();
|
|
AABB mergedAABBs;
|
|
mergedAABBs.mergeTwoAABBs(mNodes[currentNodeID].aabb, newNodeAABB);
|
|
decimal mergedVolume = mergedAABBs.getVolume();
|
|
|
|
// Compute the cost of making the current node the sibbling of the new node
|
|
decimal costS = decimal(2.0) * mergedVolume;
|
|
|
|
// Compute the minimum cost of pushing the new node further down the tree (inheritance cost)
|
|
decimal costI = decimal(2.0) * (mergedVolume - volumeAABB);
|
|
|
|
// Compute the cost of descending into the left child
|
|
decimal costLeft;
|
|
AABB currentAndLeftAABB;
|
|
currentAndLeftAABB.mergeTwoAABBs(newNodeAABB, mNodes[leftChild].aabb);
|
|
if (mNodes[leftChild].isLeaf()) { // If the left child is a leaf
|
|
costLeft = currentAndLeftAABB.getVolume() + costI;
|
|
}
|
|
else {
|
|
decimal leftChildVolume = mNodes[leftChild].aabb.getVolume();
|
|
costLeft = costI + currentAndLeftAABB.getVolume() - leftChildVolume;
|
|
}
|
|
|
|
// Compute the cost of descending into the right child
|
|
decimal costRight;
|
|
AABB currentAndRightAABB;
|
|
currentAndRightAABB.mergeTwoAABBs(newNodeAABB, mNodes[rightChild].aabb);
|
|
if (mNodes[rightChild].isLeaf()) { // If the right child is a leaf
|
|
costRight = currentAndRightAABB.getVolume() + costI;
|
|
}
|
|
else {
|
|
decimal rightChildVolume = mNodes[rightChild].aabb.getVolume();
|
|
costRight = costI + currentAndRightAABB.getVolume() - rightChildVolume;
|
|
}
|
|
|
|
// If the cost of making the current node a sibbling of the new node is smaller than
|
|
// the cost of going down into the left or right child
|
|
if (costS < costLeft && costS < costRight) break;
|
|
|
|
// It is cheaper to go down into a child of the current node, choose the best child
|
|
if (costLeft < costRight) {
|
|
currentNodeID = leftChild;
|
|
}
|
|
else {
|
|
currentNodeID = rightChild;
|
|
}
|
|
}
|
|
|
|
int siblingNode = currentNodeID;
|
|
|
|
// Create a new parent for the new node and the sibling node
|
|
int oldParentNode = mNodes[siblingNode].parentID;
|
|
int newParentNode = allocateNode();
|
|
mNodes[newParentNode].parentID = oldParentNode;
|
|
mNodes[newParentNode].aabb.mergeTwoAABBs(mNodes[siblingNode].aabb, newNodeAABB);
|
|
mNodes[newParentNode].height = mNodes[siblingNode].height + 1;
|
|
assert(mNodes[newParentNode].height > 0);
|
|
|
|
// If the sibling node was not the root node
|
|
if (oldParentNode != TreeNode::NULL_TREE_NODE) {
|
|
assert(!mNodes[oldParentNode].isLeaf());
|
|
if (mNodes[oldParentNode].children[0] == siblingNode) {
|
|
mNodes[oldParentNode].children[0] = newParentNode;
|
|
}
|
|
else {
|
|
mNodes[oldParentNode].children[1] = newParentNode;
|
|
}
|
|
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].children[0] = siblingNode;
|
|
mNodes[newParentNode].children[1] = nodeID;
|
|
mNodes[siblingNode].parentID = newParentNode;
|
|
mNodes[nodeID].parentID = newParentNode;
|
|
mRootNodeID = newParentNode;
|
|
}
|
|
|
|
// Move up in the tree to change the AABBs that have changed
|
|
currentNodeID = mNodes[nodeID].parentID;
|
|
assert(!mNodes[currentNodeID].isLeaf());
|
|
while (currentNodeID != TreeNode::NULL_TREE_NODE) {
|
|
|
|
// Balance the sub-tree of the current node if it is not balanced
|
|
currentNodeID = balanceSubTreeAtNode(currentNodeID);
|
|
assert(mNodes[nodeID].isLeaf());
|
|
|
|
assert(!mNodes[currentNodeID].isLeaf());
|
|
int leftChild = mNodes[currentNodeID].children[0];
|
|
int rightChild = mNodes[currentNodeID].children[1];
|
|
assert(leftChild != TreeNode::NULL_TREE_NODE);
|
|
assert(rightChild != TreeNode::NULL_TREE_NODE);
|
|
|
|
// Recompute the height of the node in the tree
|
|
mNodes[currentNodeID].height = std::max(mNodes[leftChild].height,
|
|
mNodes[rightChild].height) + 1;
|
|
assert(mNodes[currentNodeID].height > 0);
|
|
|
|
// Recompute the AABB of the node
|
|
mNodes[currentNodeID].aabb.mergeTwoAABBs(mNodes[leftChild].aabb, mNodes[rightChild].aabb);
|
|
|
|
currentNodeID = mNodes[currentNodeID].parentID;
|
|
}
|
|
|
|
assert(mNodes[nodeID].isLeaf());
|
|
}
|
|
|
|
// Remove a leaf node from the tree
|
|
void DynamicAABBTree::removeLeafNode(int nodeID) {
|
|
|
|
assert(nodeID >= 0 && nodeID < mNbAllocatedNodes);
|
|
assert(mNodes[nodeID].isLeaf());
|
|
|
|
// If we are removing the root node (root node is a leaf in this case)
|
|
if (mRootNodeID == nodeID) {
|
|
mRootNodeID = TreeNode::NULL_TREE_NODE;
|
|
return;
|
|
}
|
|
|
|
int parentNodeID = mNodes[nodeID].parentID;
|
|
int grandParentNodeID = mNodes[parentNodeID].parentID;
|
|
int siblingNodeID;
|
|
if (mNodes[parentNodeID].children[0] == nodeID) {
|
|
siblingNodeID = mNodes[parentNodeID].children[1];
|
|
}
|
|
else {
|
|
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].children[0] == parentNodeID) {
|
|
mNodes[grandParentNodeID].children[0] = siblingNodeID;
|
|
}
|
|
else {
|
|
assert(mNodes[grandParentNodeID].children[1] == parentNodeID);
|
|
mNodes[grandParentNodeID].children[1] = siblingNodeID;
|
|
}
|
|
mNodes[siblingNodeID].parentID = grandParentNodeID;
|
|
releaseNode(parentNodeID);
|
|
|
|
// Now, we need to recompute the AABBs of the node on the path back to the root
|
|
// and make sure that the tree is still balanced
|
|
int currentNodeID = grandParentNodeID;
|
|
while(currentNodeID != TreeNode::NULL_TREE_NODE) {
|
|
|
|
// Balance the current sub-tree if necessary
|
|
currentNodeID = balanceSubTreeAtNode(currentNodeID);
|
|
|
|
assert(!mNodes[currentNodeID].isLeaf());
|
|
|
|
// Get the two children of the current node
|
|
int leftChildID = mNodes[currentNodeID].children[0];
|
|
int rightChildID = mNodes[currentNodeID].children[1];
|
|
|
|
// Recompute the AABB and the height of the current node
|
|
mNodes[currentNodeID].aabb.mergeTwoAABBs(mNodes[leftChildID].aabb,
|
|
mNodes[rightChildID].aabb);
|
|
mNodes[currentNodeID].height = std::max(mNodes[leftChildID].height,
|
|
mNodes[rightChildID].height) + 1;
|
|
assert(mNodes[currentNodeID].height > 0);
|
|
|
|
currentNodeID = mNodes[currentNodeID].parentID;
|
|
}
|
|
}
|
|
else { // If the parent of the node to remove is the root node
|
|
|
|
// The sibling node becomes the new root node
|
|
mRootNodeID = siblingNodeID;
|
|
mNodes[siblingNodeID].parentID = TreeNode::NULL_TREE_NODE;
|
|
releaseNode(parentNodeID);
|
|
}
|
|
}
|
|
|
|
// Balance the sub-tree of a given node using left or right rotations.
|
|
/// The rotation schemes are described in the book "Introduction to Game Physics
|
|
/// with Box2D" by Ian Parberry. This method returns the new root node ID.
|
|
int32 DynamicAABBTree::balanceSubTreeAtNode(int32 nodeID) {
|
|
|
|
assert(nodeID != TreeNode::NULL_TREE_NODE);
|
|
|
|
TreeNode* nodeA = mNodes + nodeID;
|
|
|
|
// If the node is a leaf or the height of A's sub-tree is less than 2
|
|
if (nodeA->isLeaf() || nodeA->height < 2) {
|
|
|
|
// Do not perform any rotation
|
|
return nodeID;
|
|
}
|
|
|
|
// Get the two children nodes
|
|
int nodeBID = nodeA->children[0];
|
|
int nodeCID = nodeA->children[1];
|
|
assert(nodeBID >= 0 && nodeBID < mNbAllocatedNodes);
|
|
assert(nodeCID >= 0 && nodeCID < mNbAllocatedNodes);
|
|
TreeNode* nodeB = mNodes + nodeBID;
|
|
TreeNode* nodeC = mNodes + nodeCID;
|
|
|
|
// Compute the factor of the left and right sub-trees
|
|
int balanceFactor = nodeC->height - nodeB->height;
|
|
|
|
// If the right node C is 2 higher than left node B
|
|
if (balanceFactor > 1) {
|
|
|
|
assert(!nodeC->isLeaf());
|
|
|
|
int nodeFID = nodeC->children[0];
|
|
int nodeGID = nodeC->children[1];
|
|
assert(nodeFID >= 0 && nodeFID < mNbAllocatedNodes);
|
|
assert(nodeGID >= 0 && nodeGID < mNbAllocatedNodes);
|
|
TreeNode* nodeF = mNodes + nodeFID;
|
|
TreeNode* nodeG = mNodes + nodeGID;
|
|
|
|
nodeC->children[0] = nodeID;
|
|
nodeC->parentID = nodeA->parentID;
|
|
nodeA->parentID = nodeCID;
|
|
|
|
if (nodeC->parentID != TreeNode::NULL_TREE_NODE) {
|
|
|
|
if (mNodes[nodeC->parentID].children[0] == nodeID) {
|
|
mNodes[nodeC->parentID].children[0] = nodeCID;
|
|
}
|
|
else {
|
|
assert(mNodes[nodeC->parentID].children[1] == nodeID);
|
|
mNodes[nodeC->parentID].children[1] = nodeCID;
|
|
}
|
|
}
|
|
else {
|
|
mRootNodeID = nodeCID;
|
|
}
|
|
|
|
assert(!nodeC->isLeaf());
|
|
assert(!nodeA->isLeaf());
|
|
|
|
// If the right node C was higher than left node B because of the F node
|
|
if (nodeF->height > nodeG->height) {
|
|
|
|
nodeC->children[1] = nodeFID;
|
|
nodeA->children[1] = nodeGID;
|
|
nodeG->parentID = nodeID;
|
|
|
|
// Recompute the AABB of node A and C
|
|
nodeA->aabb.mergeTwoAABBs(nodeB->aabb, nodeG->aabb);
|
|
nodeC->aabb.mergeTwoAABBs(nodeA->aabb, nodeF->aabb);
|
|
|
|
// Recompute the height of node A and C
|
|
nodeA->height = std::max(nodeB->height, nodeG->height) + 1;
|
|
nodeC->height = std::max(nodeA->height, nodeF->height) + 1;
|
|
assert(nodeA->height > 0);
|
|
assert(nodeC->height > 0);
|
|
}
|
|
else { // If the right node C was higher than left node B because of node G
|
|
nodeC->children[1] = nodeGID;
|
|
nodeA->children[1] = nodeFID;
|
|
nodeF->parentID = nodeID;
|
|
|
|
// Recompute the AABB of node A and C
|
|
nodeA->aabb.mergeTwoAABBs(nodeB->aabb, nodeF->aabb);
|
|
nodeC->aabb.mergeTwoAABBs(nodeA->aabb, nodeG->aabb);
|
|
|
|
// Recompute the height of node A and C
|
|
nodeA->height = std::max(nodeB->height, nodeF->height) + 1;
|
|
nodeC->height = std::max(nodeA->height, nodeG->height) + 1;
|
|
assert(nodeA->height > 0);
|
|
assert(nodeC->height > 0);
|
|
}
|
|
|
|
// Return the new root of the sub-tree
|
|
return nodeCID;
|
|
}
|
|
|
|
// If the left node B is 2 higher than right node C
|
|
if (balanceFactor < -1) {
|
|
|
|
assert(!nodeB->isLeaf());
|
|
|
|
int nodeFID = nodeB->children[0];
|
|
int nodeGID = nodeB->children[1];
|
|
assert(nodeFID >= 0 && nodeFID < mNbAllocatedNodes);
|
|
assert(nodeGID >= 0 && nodeGID < mNbAllocatedNodes);
|
|
TreeNode* nodeF = mNodes + nodeFID;
|
|
TreeNode* nodeG = mNodes + nodeGID;
|
|
|
|
nodeB->children[0] = nodeID;
|
|
nodeB->parentID = nodeA->parentID;
|
|
nodeA->parentID = nodeBID;
|
|
|
|
if (nodeB->parentID != TreeNode::NULL_TREE_NODE) {
|
|
|
|
if (mNodes[nodeB->parentID].children[0] == nodeID) {
|
|
mNodes[nodeB->parentID].children[0] = nodeBID;
|
|
}
|
|
else {
|
|
assert(mNodes[nodeB->parentID].children[1] == nodeID);
|
|
mNodes[nodeB->parentID].children[1] = nodeBID;
|
|
}
|
|
}
|
|
else {
|
|
mRootNodeID = nodeBID;
|
|
}
|
|
|
|
assert(!nodeB->isLeaf());
|
|
assert(!nodeA->isLeaf());
|
|
|
|
// If the left node B was higher than right node C because of the F node
|
|
if (nodeF->height > nodeG->height) {
|
|
|
|
nodeB->children[1] = nodeFID;
|
|
nodeA->children[0] = nodeGID;
|
|
nodeG->parentID = nodeID;
|
|
|
|
// Recompute the AABB of node A and B
|
|
nodeA->aabb.mergeTwoAABBs(nodeC->aabb, nodeG->aabb);
|
|
nodeB->aabb.mergeTwoAABBs(nodeA->aabb, nodeF->aabb);
|
|
|
|
// Recompute the height of node A and B
|
|
nodeA->height = std::max(nodeC->height, nodeG->height) + 1;
|
|
nodeB->height = std::max(nodeA->height, nodeF->height) + 1;
|
|
assert(nodeA->height > 0);
|
|
assert(nodeB->height > 0);
|
|
}
|
|
else { // If the left node B was higher than right node C because of node G
|
|
nodeB->children[1] = nodeGID;
|
|
nodeA->children[0] = nodeFID;
|
|
nodeF->parentID = nodeID;
|
|
|
|
// Recompute the AABB of node A and B
|
|
nodeA->aabb.mergeTwoAABBs(nodeC->aabb, nodeF->aabb);
|
|
nodeB->aabb.mergeTwoAABBs(nodeA->aabb, nodeG->aabb);
|
|
|
|
// Recompute the height of node A and B
|
|
nodeA->height = std::max(nodeC->height, nodeF->height) + 1;
|
|
nodeB->height = std::max(nodeA->height, nodeG->height) + 1;
|
|
assert(nodeA->height > 0);
|
|
assert(nodeB->height > 0);
|
|
}
|
|
|
|
// Return the new root of the sub-tree
|
|
return nodeBID;
|
|
}
|
|
|
|
// If the sub-tree is balanced, return the current root node
|
|
return nodeID;
|
|
}
|
|
|
|
/// Take an array of shapes to be tested for broad-phase overlap and return an array of pair of overlapping shapes
|
|
void DynamicAABBTree::reportAllShapesOverlappingWithShapes(const Array<int32>& nodesToTest, size_t startIndex,
|
|
size_t endIndex, Array<Pair<int32, int32>>& outOverlappingNodes) const {
|
|
|
|
RP3D_PROFILE("DynamicAABBTree::reportAllShapesOverlappingWithAABB()", mProfiler);
|
|
|
|
// Create a stack with the nodes to visit
|
|
Stack<int32> stack(mAllocator, 64);
|
|
|
|
// For each shape to be tested for overlap
|
|
for (uint i=startIndex; i < endIndex; i++) {
|
|
|
|
assert(nodesToTest[i] != -1);
|
|
|
|
stack.push(mRootNodeID);
|
|
|
|
const AABB& shapeAABB = getFatAABB(nodesToTest[i]);
|
|
|
|
// While there are still nodes to visit
|
|
while(stack.size() > 0) {
|
|
|
|
// Get the next node ID to visit
|
|
const int32 nodeIDToVisit = stack.pop();
|
|
|
|
// Skip it if it is a null node
|
|
if (nodeIDToVisit == TreeNode::NULL_TREE_NODE) continue;
|
|
|
|
// Get the corresponding node
|
|
const TreeNode* nodeToVisit = mNodes + nodeIDToVisit;
|
|
|
|
// If the AABB in parameter overlaps with the AABB of the node to visit
|
|
if (shapeAABB.testCollision(nodeToVisit->aabb)) {
|
|
|
|
// If the node is a leaf
|
|
if (nodeToVisit->isLeaf()) {
|
|
|
|
// Add the node in the array of overlapping nodes
|
|
outOverlappingNodes.add(Pair<int32, int32>(nodesToTest[i], nodeIDToVisit));
|
|
}
|
|
else { // If the node is not a leaf
|
|
|
|
// We need to visit its children
|
|
stack.push(nodeToVisit->children[0]);
|
|
stack.push(nodeToVisit->children[1]);
|
|
}
|
|
}
|
|
}
|
|
|
|
stack.clear();
|
|
}
|
|
}
|
|
|
|
// Report all shapes overlapping with the AABB given in parameter.
|
|
void DynamicAABBTree::reportAllShapesOverlappingWithAABB(const AABB& aabb, Array<int32>& overlappingNodes) const {
|
|
|
|
RP3D_PROFILE("DynamicAABBTree::reportAllShapesOverlappingWithAABB()", mProfiler);
|
|
|
|
// Create a stack with the nodes to visit
|
|
Stack<int32> stack(mAllocator, 64);
|
|
stack.push(mRootNodeID);
|
|
|
|
// While there are still nodes to visit
|
|
while(stack.size() > 0) {
|
|
|
|
// Get the next node ID to visit
|
|
const int32 nodeIDToVisit = stack.pop();
|
|
|
|
assert(nodeIDToVisit >= 0);
|
|
assert(nodeIDToVisit < mNbAllocatedNodes);
|
|
|
|
// Skip it if it is a null node
|
|
if (nodeIDToVisit == TreeNode::NULL_TREE_NODE) continue;
|
|
|
|
// Get the corresponding node
|
|
const TreeNode* nodeToVisit = mNodes + nodeIDToVisit;
|
|
|
|
// If the AABB in parameter overlaps with the AABB of the node to visit
|
|
if (aabb.testCollision(nodeToVisit->aabb)) {
|
|
|
|
// If the node is a leaf
|
|
if (nodeToVisit->isLeaf()) {
|
|
|
|
// Notify the broad-phase about a new potential overlapping pair
|
|
overlappingNodes.add(nodeIDToVisit);
|
|
}
|
|
else { // If the node is not a leaf
|
|
|
|
// We need to visit its children
|
|
stack.push(nodeToVisit->children[0]);
|
|
stack.push(nodeToVisit->children[1]);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
// Ray casting method
|
|
void DynamicAABBTree::raycast(const Ray& ray, DynamicAABBTreeRaycastCallback& callback) const {
|
|
|
|
RP3D_PROFILE("DynamicAABBTree::raycast()", mProfiler);
|
|
|
|
decimal maxFraction = ray.maxFraction;
|
|
|
|
// Compute the inverse ray direction
|
|
const Vector3 rayDirection = ray.point2 - ray.point1;
|
|
const Vector3 rayDirectionInverse(decimal(1.0) / rayDirection.x, decimal(1.0) / rayDirection.y, decimal(1.0) / rayDirection.z);
|
|
|
|
Stack<int32> stack(mAllocator, 128);
|
|
stack.push(mRootNodeID);
|
|
|
|
// Walk through the tree from the root looking for colliders
|
|
// that overlap with the ray AABB
|
|
while (stack.size() > 0) {
|
|
|
|
// Get the next node in the stack
|
|
int32 nodeID = stack.pop();
|
|
|
|
// If it is a null node, skip it
|
|
if (nodeID == TreeNode::NULL_TREE_NODE) continue;
|
|
|
|
// Get the corresponding node
|
|
const TreeNode* node = mNodes + nodeID;
|
|
|
|
// Test if the ray intersects with the current node AABB
|
|
if (!node->aabb.testRayIntersect(ray.point1, rayDirectionInverse, maxFraction)) continue;
|
|
|
|
// If the node is a leaf of the tree
|
|
if (node->isLeaf()) {
|
|
|
|
Ray rayTemp(ray.point1, ray.point2, maxFraction);
|
|
|
|
// Call the callback that will raycast again the broad-phase shape
|
|
decimal hitFraction = callback.raycastBroadPhaseShape(nodeID, 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;
|
|
}
|
|
}
|
|
|
|
// If the user returned a negative fraction, we continue
|
|
// the raycasting as if the collider did not exist
|
|
}
|
|
else { // If the node has children
|
|
|
|
// Push its children in the stack of nodes to explore
|
|
stack.push(node->children[0]);
|
|
stack.push(node->children[1]);
|
|
}
|
|
}
|
|
}
|
|
|
|
#ifndef NDEBUG
|
|
|
|
// Check if the tree structure is valid (for debugging purpose)
|
|
void DynamicAABBTree::check() const {
|
|
|
|
// Recursively check each node
|
|
checkNode(mRootNodeID);
|
|
|
|
int32 nbFreeNodes = 0;
|
|
int32 freeNodeID = mFreeNodeID;
|
|
|
|
// Check the free nodes
|
|
while(freeNodeID != TreeNode::NULL_TREE_NODE) {
|
|
assert(0 <= freeNodeID && freeNodeID < mNbAllocatedNodes);
|
|
freeNodeID = mNodes[freeNodeID].nextNodeID;
|
|
nbFreeNodes++;
|
|
}
|
|
|
|
assert(mNbNodes + nbFreeNodes == mNbAllocatedNodes);
|
|
}
|
|
|
|
// Check if the node structure is valid (for debugging purpose)
|
|
void DynamicAABBTree::checkNode(int32 nodeID) const {
|
|
|
|
if (nodeID == TreeNode::NULL_TREE_NODE) return;
|
|
|
|
// If it is the root
|
|
if (nodeID == mRootNodeID) {
|
|
assert(mNodes[nodeID].parentID == TreeNode::NULL_TREE_NODE);
|
|
}
|
|
|
|
// Get the children nodes
|
|
TreeNode* pNode = mNodes + nodeID;
|
|
assert(!pNode->isLeaf());
|
|
int32 leftChild = pNode->children[0];
|
|
int32 rightChild = pNode->children[1];
|
|
|
|
assert(pNode->height >= 0);
|
|
assert(pNode->aabb.getVolume() > 0);
|
|
|
|
// If the current node is a leaf
|
|
if (pNode->isLeaf()) {
|
|
|
|
// Check that there are no children
|
|
assert(leftChild == TreeNode::NULL_TREE_NODE);
|
|
assert(rightChild == TreeNode::NULL_TREE_NODE);
|
|
assert(pNode->height == 0);
|
|
}
|
|
else {
|
|
|
|
// Check that the children node IDs are valid
|
|
assert(0 <= leftChild && leftChild < mNbAllocatedNodes);
|
|
assert(0 <= rightChild && rightChild < mNbAllocatedNodes);
|
|
|
|
// Check that the children nodes have the correct parent node
|
|
assert(mNodes[leftChild].parentID == nodeID);
|
|
assert(mNodes[rightChild].parentID == nodeID);
|
|
|
|
// Check the height of node
|
|
int height = 1 + std::max(mNodes[leftChild].height, mNodes[rightChild].height);
|
|
assert(mNodes[nodeID].height == height);
|
|
|
|
// Check the AABB of the node
|
|
AABB aabb;
|
|
aabb.mergeTwoAABBs(mNodes[leftChild].aabb, mNodes[rightChild].aabb);
|
|
assert(aabb.getMin() == mNodes[nodeID].aabb.getMin());
|
|
assert(aabb.getMax() == mNodes[nodeID].aabb.getMax());
|
|
|
|
// Recursively check the children nodes
|
|
checkNode(leftChild);
|
|
checkNode(rightChild);
|
|
}
|
|
}
|
|
|
|
// Compute the height of the tree
|
|
int DynamicAABBTree::computeHeight() {
|
|
return computeHeight(mRootNodeID);
|
|
}
|
|
|
|
// Compute the height of a given node in the tree
|
|
int DynamicAABBTree::computeHeight(int32 nodeID) {
|
|
assert(nodeID >= 0 && nodeID < mNbAllocatedNodes);
|
|
TreeNode* node = mNodes + nodeID;
|
|
|
|
// If the node is a leaf, its height is zero
|
|
if (node->isLeaf()) {
|
|
return 0;
|
|
}
|
|
|
|
// Compute the height of the left and right sub-tree
|
|
int leftHeight = computeHeight(node->children[0]);
|
|
int rightHeight = computeHeight(node->children[1]);
|
|
|
|
// Return the height of the node
|
|
return 1 + std::max(leftHeight, rightHeight);
|
|
}
|
|
|
|
#endif
|