/** @file * @author Daniel Chappuis * @copyright 2010-2016 Daniel Chappuis * @license BSD 3 clauses (see license file) */ #include #include #include #include #include using namespace ephysics; const int32_t TreeNode::NULL_TREE_NODE = -1; DynamicAABBTree::DynamicAABBTree(float extraAABBGap) : m_extraAABBGap(extraAABBGap) { init(); } DynamicAABBTree::~DynamicAABBTree() { free(m_nodes); } // Initialize the tree void DynamicAABBTree::init() { m_rootNodeID = TreeNode::NULL_TREE_NODE; m_numberNodes = 0; m_numberAllocatedNodes = 8; // Allocate memory for the nodes of the tree m_nodes = (TreeNode*) malloc(m_numberAllocatedNodes * sizeof(TreeNode)); assert(m_nodes); memset(m_nodes, 0, m_numberAllocatedNodes * sizeof(TreeNode)); // Initialize the allocated nodes for (int32_t i=0; i 0); assert(_nodeID >= 0 && _nodeID < m_numberAllocatedNodes); assert(m_nodes[_nodeID].height >= 0); m_nodes[_nodeID].nextNodeID = m_freeNodeID; m_nodes[_nodeID].height = -1; m_freeNodeID = _nodeID; m_numberNodes--; } // Internally add an object int32_to the tree int32_t DynamicAABBTree::addObjectInternal(const AABB& aabb) { // Get the next available node (or allocate new ones if necessary) int32_t _nodeID = allocateNode(); // Create the fat aabb to use in the tree const vec3 gap(m_extraAABBGap, m_extraAABBGap, m_extraAABBGap); m_nodes[_nodeID].aabb.setMin(aabb.getMin() - gap); m_nodes[_nodeID].aabb.setMax(aabb.getMax() + gap); // Set the height of the node in the tree m_nodes[_nodeID].height = 0; // Insert the new leaf node in the tree insertLeafNode(_nodeID); assert(m_nodes[_nodeID].isLeaf()); assert(_nodeID >= 0); // Return the Id of the node return _nodeID; } // Remove an object from the tree void DynamicAABBTree::removeObject(int32_t _nodeID) { assert(_nodeID >= 0 && _nodeID < m_numberAllocatedNodes); assert(m_nodes[_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 int32_to the tree. /// The method returns true if the object has been reinserted int32_to the tree. The "displacement" /// argument is the linear velocity of the AABB multiplied by the elapsed time between two /// frames. If the "forceReinsert" parameter is true, we force a removal and reinsertion of the node /// (this can be useful if the shape AABB has become much smaller than the previous one for instance). bool DynamicAABBTree::updateObject(int32_t _nodeID, const AABB& _newAABB, const vec3& _displacement, bool _forceReinsert) { PROFILE("DynamicAABBTree::updateObject()"); assert(_nodeID >= 0 && _nodeID < m_numberAllocatedNodes); assert(m_nodes[_nodeID].isLeaf()); assert(m_nodes[_nodeID].height >= 0); EPHY_INFO(" compare : " << m_nodes[_nodeID].aabb.m_minCoordinates << " " << m_nodes[_nodeID].aabb.m_maxCoordinates); EPHY_INFO(" : " << _newAABB.m_minCoordinates << " " << _newAABB.m_maxCoordinates); // If the new AABB is still inside the fat AABB of the node if ( _forceReinsert == false && m_nodes[_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 a constant gap m_nodes[_nodeID].aabb = _newAABB; const vec3 gap(m_extraAABBGap, m_extraAABBGap, m_extraAABBGap); m_nodes[_nodeID].aabb.m_minCoordinates -= gap; m_nodes[_nodeID].aabb.m_maxCoordinates += gap; // Inflate the fat AABB in direction of the linear motion of the AABB if (_displacement.x() < 0.0f) { m_nodes[_nodeID].aabb.m_minCoordinates.setX(m_nodes[_nodeID].aabb.m_minCoordinates.x() + DYNAMIC_TREE_AABB_LIN_GAP_MULTIPLIER *_displacement.x()); } else { m_nodes[_nodeID].aabb.m_maxCoordinates.setX(m_nodes[_nodeID].aabb.m_maxCoordinates.x() + DYNAMIC_TREE_AABB_LIN_GAP_MULTIPLIER *_displacement.x()); } if (_displacement.y() < 0.0f) { m_nodes[_nodeID].aabb.m_minCoordinates.setY(m_nodes[_nodeID].aabb.m_minCoordinates.y() + DYNAMIC_TREE_AABB_LIN_GAP_MULTIPLIER *_displacement.y()); } else { m_nodes[_nodeID].aabb.m_maxCoordinates.setY(m_nodes[_nodeID].aabb.m_maxCoordinates.y() + DYNAMIC_TREE_AABB_LIN_GAP_MULTIPLIER *_displacement.y()); } if (_displacement.z() < 0.0f) { m_nodes[_nodeID].aabb.m_minCoordinates.setZ(m_nodes[_nodeID].aabb.m_minCoordinates.z() + DYNAMIC_TREE_AABB_LIN_GAP_MULTIPLIER *_displacement.z()); } else { m_nodes[_nodeID].aabb.m_maxCoordinates.setZ(m_nodes[_nodeID].aabb.m_maxCoordinates.z() + DYNAMIC_TREE_AABB_LIN_GAP_MULTIPLIER *_displacement.z()); } EPHY_ERROR(" compare : " << m_nodes[_nodeID].aabb.m_minCoordinates << " " << m_nodes[_nodeID].aabb.m_maxCoordinates); EPHY_ERROR(" : " << _newAABB.m_minCoordinates << " " << _newAABB.m_maxCoordinates); if (m_nodes[_nodeID].aabb.contains(_newAABB) == false) { //EPHY_CRITICAL("ERROR"); } assert(m_nodes[_nodeID].aabb.contains(_newAABB)); // Reinsert the node int32_to 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(int32_t _nodeID) { // If the tree is empty if (m_rootNodeID == TreeNode::NULL_TREE_NODE) { m_rootNodeID = _nodeID; m_nodes[m_rootNodeID].parentID = TreeNode::NULL_TREE_NODE; return; } assert(m_rootNodeID != TreeNode::NULL_TREE_NODE); // Find the best sibling node for the new node AABB newNodeAABB = m_nodes[_nodeID].aabb; int32_t currentNodeID = m_rootNodeID; while (!m_nodes[currentNodeID].isLeaf()) { int32_t leftChild = m_nodes[currentNodeID].children[0]; int32_t rightChild = m_nodes[currentNodeID].children[1]; // Compute the merged AABB float volumeAABB = m_nodes[currentNodeID].aabb.getVolume(); AABB mergedAABBs; mergedAABBs.mergeTwoAABBs(m_nodes[currentNodeID].aabb, newNodeAABB); float mergedVolume = mergedAABBs.getVolume(); // Compute the cost of making the current node the sibbling of the new node float costS = float(2.0) * mergedVolume; // Compute the minimum cost of pushing the new node further down the tree (inheritance cost) float costI = float(2.0) * (mergedVolume - volumeAABB); // Compute the cost of descending int32_to the left child float costLeft; AABB currentAndLeftAABB; currentAndLeftAABB.mergeTwoAABBs(newNodeAABB, m_nodes[leftChild].aabb); if (m_nodes[leftChild].isLeaf()) { // If the left child is a leaf costLeft = currentAndLeftAABB.getVolume() + costI; } else { float leftChildVolume = m_nodes[leftChild].aabb.getVolume(); costLeft = costI + currentAndLeftAABB.getVolume() - leftChildVolume; } // Compute the cost of descending int32_to the right child float costRight; AABB currentAndRightAABB; currentAndRightAABB.mergeTwoAABBs(newNodeAABB, m_nodes[rightChild].aabb); if (m_nodes[rightChild].isLeaf()) { // If the right child is a leaf costRight = currentAndRightAABB.getVolume() + costI; } else { float rightChildVolume = m_nodes[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 int32_to the left or right child if (costS < costLeft && costS < costRight) break; // It is cheaper to go down int32_to a child of the current node, choose the best child if (costLeft < costRight) { currentNodeID = leftChild; } else { currentNodeID = rightChild; } } int32_t siblingNode = currentNodeID; // Create a new parent for the new node and the sibling node int32_t oldParentNode = m_nodes[siblingNode].parentID; int32_t newParentNode = allocateNode(); m_nodes[newParentNode].parentID = oldParentNode; m_nodes[newParentNode].aabb.mergeTwoAABBs(m_nodes[siblingNode].aabb, newNodeAABB); m_nodes[newParentNode].height = m_nodes[siblingNode].height + 1; assert(m_nodes[newParentNode].height > 0); // If the sibling node was not the root node if (oldParentNode != TreeNode::NULL_TREE_NODE) { assert(!m_nodes[oldParentNode].isLeaf()); if (m_nodes[oldParentNode].children[0] == siblingNode) { m_nodes[oldParentNode].children[0] = newParentNode; } else { m_nodes[oldParentNode].children[1] = newParentNode; } m_nodes[newParentNode].children[0] = siblingNode; m_nodes[newParentNode].children[1] = _nodeID; m_nodes[siblingNode].parentID = newParentNode; m_nodes[_nodeID].parentID = newParentNode; } else { // If the sibling node was the root node m_nodes[newParentNode].children[0] = siblingNode; m_nodes[newParentNode].children[1] = _nodeID; m_nodes[siblingNode].parentID = newParentNode; m_nodes[_nodeID].parentID = newParentNode; m_rootNodeID = newParentNode; } // Move up in the tree to change the AABBs that have changed currentNodeID = m_nodes[_nodeID].parentID; assert(!m_nodes[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(m_nodes[_nodeID].isLeaf()); assert(!m_nodes[currentNodeID].isLeaf()); int32_t leftChild = m_nodes[currentNodeID].children[0]; int32_t rightChild = m_nodes[currentNodeID].children[1]; assert(leftChild != TreeNode::NULL_TREE_NODE); assert(rightChild != TreeNode::NULL_TREE_NODE); // Recompute the height of the node in the tree m_nodes[currentNodeID].height = std::max(m_nodes[leftChild].height, m_nodes[rightChild].height) + 1; assert(m_nodes[currentNodeID].height > 0); // Recompute the AABB of the node m_nodes[currentNodeID].aabb.mergeTwoAABBs(m_nodes[leftChild].aabb, m_nodes[rightChild].aabb); currentNodeID = m_nodes[currentNodeID].parentID; } assert(m_nodes[_nodeID].isLeaf()); } // Remove a leaf node from the tree void DynamicAABBTree::removeLeafNode(int32_t _nodeID) { assert(_nodeID >= 0 && _nodeID < m_numberAllocatedNodes); assert(m_nodes[_nodeID].isLeaf()); // If we are removing the root node (root node is a leaf in this case) if (m_rootNodeID == _nodeID) { m_rootNodeID = TreeNode::NULL_TREE_NODE; return; } int32_t parentNodeID = m_nodes[_nodeID].parentID; int32_t grandParentNodeID = m_nodes[parentNodeID].parentID; int32_t siblingNodeID; if (m_nodes[parentNodeID].children[0] == _nodeID) { siblingNodeID = m_nodes[parentNodeID].children[1]; } else { siblingNodeID = m_nodes[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 (m_nodes[grandParentNodeID].children[0] == parentNodeID) { m_nodes[grandParentNodeID].children[0] = siblingNodeID; } else { assert(m_nodes[grandParentNodeID].children[1] == parentNodeID); m_nodes[grandParentNodeID].children[1] = siblingNodeID; } m_nodes[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 int32_t currentNodeID = grandParentNodeID; while(currentNodeID != TreeNode::NULL_TREE_NODE) { // Balance the current sub-tree if necessary currentNodeID = balanceSubTreeAtNode(currentNodeID); assert(!m_nodes[currentNodeID].isLeaf()); // Get the two children of the current node int32_t leftChildID = m_nodes[currentNodeID].children[0]; int32_t rightChildID = m_nodes[currentNodeID].children[1]; // Recompute the AABB and the height of the current node m_nodes[currentNodeID].aabb.mergeTwoAABBs(m_nodes[leftChildID].aabb, m_nodes[rightChildID].aabb); m_nodes[currentNodeID].height = std::max(m_nodes[leftChildID].height, m_nodes[rightChildID].height) + 1; assert(m_nodes[currentNodeID].height > 0); currentNodeID = m_nodes[currentNodeID].parentID; } } else { // If the parent of the node to remove is the root node // The sibling node becomes the new root node m_rootNodeID = siblingNodeID; m_nodes[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_t DynamicAABBTree::balanceSubTreeAtNode(int32_t _nodeID) { assert(_nodeID != TreeNode::NULL_TREE_NODE); TreeNode* nodeA = m_nodes + _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 int32_t nodeBID = nodeA->children[0]; int32_t nodeCID = nodeA->children[1]; assert(nodeBID >= 0 && nodeBID < m_numberAllocatedNodes); assert(nodeCID >= 0 && nodeCID < m_numberAllocatedNodes); TreeNode* nodeB = m_nodes + nodeBID; TreeNode* nodeC = m_nodes + nodeCID; // Compute the factor of the left and right sub-trees int32_t balanceFactor = nodeC->height - nodeB->height; // If the right node C is 2 higher than left node B if (balanceFactor > 1) { assert(!nodeC->isLeaf()); int32_t nodeFID = nodeC->children[0]; int32_t nodeGID = nodeC->children[1]; assert(nodeFID >= 0 && nodeFID < m_numberAllocatedNodes); assert(nodeGID >= 0 && nodeGID < m_numberAllocatedNodes); TreeNode* nodeF = m_nodes + nodeFID; TreeNode* nodeG = m_nodes + nodeGID; nodeC->children[0] = _nodeID; nodeC->parentID = nodeA->parentID; nodeA->parentID = nodeCID; if (nodeC->parentID != TreeNode::NULL_TREE_NODE) { if (m_nodes[nodeC->parentID].children[0] == _nodeID) { m_nodes[nodeC->parentID].children[0] = nodeCID; } else { assert(m_nodes[nodeC->parentID].children[1] == _nodeID); m_nodes[nodeC->parentID].children[1] = nodeCID; } } else { m_rootNodeID = 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()); int32_t nodeFID = nodeB->children[0]; int32_t nodeGID = nodeB->children[1]; assert(nodeFID >= 0 && nodeFID < m_numberAllocatedNodes); assert(nodeGID >= 0 && nodeGID < m_numberAllocatedNodes); TreeNode* nodeF = m_nodes + nodeFID; TreeNode* nodeG = m_nodes + nodeGID; nodeB->children[0] = _nodeID; nodeB->parentID = nodeA->parentID; nodeA->parentID = nodeBID; if (nodeB->parentID != TreeNode::NULL_TREE_NODE) { if (m_nodes[nodeB->parentID].children[0] == _nodeID) { m_nodes[nodeB->parentID].children[0] = nodeBID; } else { assert(m_nodes[nodeB->parentID].children[1] == _nodeID); m_nodes[nodeB->parentID].children[1] = nodeBID; } } else { m_rootNodeID = 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; } /// Report all shapes overlapping with the AABB given in parameter. void DynamicAABBTree::reportAllShapesOverlappingWithAABB(const AABB& aabb, DynamicAABBTreeOverlapCallback& callback) const { // Create a stack with the nodes to visit Stack stack; stack.push(m_rootNodeID); // While there are still nodes to visit while(stack.getNbElements() > 0) { // Get the next node ID to visit int32_t 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 = m_nodes + 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 callback.notifyOverlappingNode(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 { PROFILE("DynamicAABBTree::raycast()"); float maxFraction = ray.maxFraction; Stack stack; stack.push(m_rootNodeID); // Walk through the tree from the root looking for proxy shapes // that overlap with the ray AABB while (stack.getNbElements() > 0) { // Get the next node in the stack int32_t 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 = m_nodes + nodeID; Ray rayTemp(ray.point1, ray.point2, maxFraction); // Test if the ray int32_tersects with the current node AABB if (!node->aabb.testRayIntersect(rayTemp)) continue; // If the node is a leaf of the tree if (node->isLeaf()) { // Call the callback that will raycast again the broad-phase shape float hitFraction = callback.raycastBroadPhaseShape(nodeID, rayTemp); // If the user returned a hitFraction of zero, it means that // the raycasting should stop here if (hitFraction == 0.0f) { return; } // If the user returned a positive fraction if (hitFraction > 0.0f) { // 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 proxy shape 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]); } } } // Return true if the node is a leaf of the tree bool TreeNode::isLeaf() const { return (height == 0); } // Return the fat AABB corresponding to a given node ID const AABB& DynamicAABBTree::getFatAABB(int32_t nodeID) const { assert(nodeID >= 0 && nodeID < m_numberAllocatedNodes); return m_nodes[nodeID].aabb; } // Return the pointer to the data array of a given leaf node of the tree int32_t* DynamicAABBTree::getNodeDataInt(int32_t nodeID) const { assert(nodeID >= 0 && nodeID < m_numberAllocatedNodes); assert(m_nodes[nodeID].isLeaf()); return m_nodes[nodeID].dataInt; } // Return the pointer to the data pointer of a given leaf node of the tree void* DynamicAABBTree::getNodeDataPointer(int32_t nodeID) const { assert(nodeID >= 0 && nodeID < m_numberAllocatedNodes); assert(m_nodes[nodeID].isLeaf()); return m_nodes[nodeID].dataPointer; } // Return the root AABB of the tree AABB DynamicAABBTree::getRootAABB() const { return getFatAABB(m_rootNodeID); } // Add an object int32_to the tree. This method creates a new leaf node in the tree and // returns the ID of the corresponding node. int32_t DynamicAABBTree::addObject(const AABB& aabb, int32_t data1, int32_t data2) { int32_t nodeId = addObjectInternal(aabb); m_nodes[nodeId].dataInt[0] = data1; m_nodes[nodeId].dataInt[1] = data2; return nodeId; } // Add an object int32_to the tree. This method creates a new leaf node in the tree and // returns the ID of the corresponding node. int32_t DynamicAABBTree::addObject(const AABB& aabb, void* data) { int32_t nodeId = addObjectInternal(aabb); m_nodes[nodeId].dataPointer = data; return nodeId; } #ifndef NDEBUG // Check if the tree structure is valid (for debugging purpose) void DynamicAABBTree::check() const { // Recursively check each node checkNode(m_rootNodeID); int32_t nbFreeNodes = 0; int32_t freeNodeID = m_freeNodeID; // Check the free nodes while(freeNodeID != TreeNode::NULL_TREE_NODE) { assert(0 <= freeNodeID && freeNodeID < m_numberAllocatedNodes); freeNodeID = m_nodes[freeNodeID].nextNodeID; nbFreeNodes++; } assert(m_numberNodes + nbFreeNodes == m_numberAllocatedNodes); } // Check if the node structure is valid (for debugging purpose) void DynamicAABBTree::checkNode(int32_t _nodeID) const { if (_nodeID == TreeNode::NULL_TREE_NODE) return; // If it is the root if (_nodeID == m_rootNodeID) { assert(m_nodes[_nodeID].parentID == TreeNode::NULL_TREE_NODE); } // Get the children nodes TreeNode* pNode = m_nodes + _nodeID; assert(!pNode->isLeaf()); int32_t leftChild = pNode->children[0]; int32_t 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 < m_numberAllocatedNodes); assert(0 <= rightChild && rightChild < m_numberAllocatedNodes); // Check that the children nodes have the correct parent node assert(m_nodes[leftChild].parentID == _nodeID); assert(m_nodes[rightChild].parentID == _nodeID); // Check the height of node int32_t height = 1 + std::max(m_nodes[leftChild].height, m_nodes[rightChild].height); assert(m_nodes[_nodeID].height == height); // Check the AABB of the node AABB aabb; aabb.mergeTwoAABBs(m_nodes[leftChild].aabb, m_nodes[rightChild].aabb); assert(aabb.getMin() == m_nodes[_nodeID].aabb.getMin()); assert(aabb.getMax() == m_nodes[_nodeID].aabb.getMax()); // Recursively check the children nodes checkNode(leftChild); checkNode(rightChild); } } // Compute the height of the tree int32_t DynamicAABBTree::computeHeight() { return computeHeight(m_rootNodeID); } // Compute the height of a given node in the tree int32_t DynamicAABBTree::computeHeight(int32_t _nodeID) { assert(_nodeID >= 0 && _nodeID < m_numberAllocatedNodes); TreeNode* node = m_nodes + _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 int32_t leftHeight = computeHeight(node->children[0]); int32_t rightHeight = computeHeight(node->children[1]); // Return the height of the node return 1 + std::max(leftHeight, rightHeight); } #endif