Add the DynamicAABBTree class
This commit is contained in:
Daniel Chappuis
parent
6b8180b620
commit
76cb11a74f
|
|
@ -49,6 +49,8 @@ SET (REACTPHYSICS3D_SOURCES
|
|||
"src/collision/broadphase/PairManager.cpp"
|
||||
"src/collision/broadphase/SweepAndPruneAlgorithm.h"
|
||||
"src/collision/broadphase/SweepAndPruneAlgorithm.cpp"
|
||||
"src/collision/broadphase/DynamicAABBTree.h"
|
||||
"src/collision/broadphase/DynamicAABBTree.cpp"
|
||||
"src/collision/narrowphase/EPA/EdgeEPA.h"
|
||||
"src/collision/narrowphase/EPA/EdgeEPA.cpp"
|
||||
"src/collision/narrowphase/EPA/EPAAlgorithm.h"
|
||||
|
|
|
|||
|
|
@ -30,6 +30,7 @@
|
|||
#include <vector>
|
||||
#include "../../body/CollisionBody.h"
|
||||
#include "PairManager.h"
|
||||
#include "DynamicAABBTree.h"
|
||||
|
||||
/// Namespace ReactPhysics3D
|
||||
namespace reactphysics3d {
|
||||
|
|
@ -39,9 +40,8 @@ class CollisionDetection;
|
|||
|
||||
// Class BroadPhaseAlgorithm
|
||||
/**
|
||||
* This class is an abstract class that represents an algorithm
|
||||
* used to perform the broad-phase of a collision detection. The
|
||||
* goal of the broad-phase algorithm is to compute the pair of bodies
|
||||
* This class represents an algorithm the broad-phase collision detection. The
|
||||
* goal of the broad-phase collision detection is to compute the pair of bodies
|
||||
* that can collide. But it's important to understand that the
|
||||
* broad-phase doesn't compute only body pairs that can collide but
|
||||
* could also pairs of body that doesn't collide but are very close.
|
||||
|
|
@ -55,6 +55,9 @@ class BroadPhaseAlgorithm {
|
|||
|
||||
// -------------------- Attributes -------------------- //
|
||||
|
||||
/// Dynamic AABB tree
|
||||
DynamicAABBTree mDynamicAABBTree;
|
||||
|
||||
/// Pair manager containing the overlapping pairs
|
||||
PairManager mPairManager;
|
||||
|
||||
|
|
|
|||
554
src/collision/broadphase/DynamicAABBTree.cpp
Normal file
554
src/collision/broadphase/DynamicAABBTree.cpp
Normal file
|
|
@ -0,0 +1,554 @@
|
|||
/********************************************************************************
|
||||
* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ *
|
||||
* Copyright (c) 2010-2013 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 "DynamicAABBTree.h"
|
||||
|
||||
using namespace reactphysics3d;
|
||||
|
||||
// Initialization of static variables
|
||||
const int TreeNode::NULL_TREE_NODE = -1;
|
||||
|
||||
// Constructor
|
||||
DynamicAABBTree::DynamicAABBTree() {
|
||||
|
||||
mRootNodeID = TreeNode::NULL_TREE_NODE;
|
||||
mNbNodes = 0;
|
||||
mNbAllocatedNodes = 8;
|
||||
|
||||
// Allocate memory for the nodes of the tree
|
||||
mNodes = (TreeNode*) malloc(mNbAllocatedNodes * sizeof(TreeNode));
|
||||
assert(mNodes);
|
||||
memset(mNodes, 0, mNbAllocatedNodes * sizeof(TreeNode));
|
||||
|
||||
// Initialize the allocated nodes
|
||||
for (int 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;
|
||||
}
|
||||
|
||||
// Destructor
|
||||
DynamicAABBTree::~DynamicAABBTree() {
|
||||
|
||||
// Free the allocated memory for the nodes
|
||||
free(mNodes);
|
||||
}
|
||||
|
||||
// Allocate and return a new node in the tree
|
||||
int 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
|
||||
mNbAllocatedNodes *= 2;
|
||||
TreeNode* oldNodes = mNodes;
|
||||
mNodes = (TreeNode*) malloc(mNbAllocatedNodes * sizeof(TreeNode));
|
||||
assert(mNodes);
|
||||
memcpy(mNodes, oldNodes, mNbNodes * sizeof(TreeNode));
|
||||
free(oldNodes);
|
||||
|
||||
// Initialize the allocated nodes
|
||||
for (int 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
|
||||
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].collisionShape = NULL;
|
||||
mNodes[freeNodeID].height = 0;
|
||||
mNbNodes++;
|
||||
|
||||
return freeNodeID;
|
||||
}
|
||||
|
||||
// Release a node
|
||||
void DynamicAABBTree::releaseNode(int nodeID) {
|
||||
|
||||
assert(mNbNodes > 0);
|
||||
assert(nodeID >= 0 && nodeID < mNbAllocatedNodes);
|
||||
mNodes[nodeID].nextNodeID = mFreeNodeID;
|
||||
mNodes[nodeID].height = -1;
|
||||
mFreeNodeID = nodeID;
|
||||
mNbNodes--;
|
||||
|
||||
// Deallocate nodes memory here if the number of allocated nodes is large
|
||||
// compared to the number of nodes in the tree
|
||||
if ((mNbNodes < mNbAllocatedNodes / 4) && mNbNodes > 8) {
|
||||
|
||||
// Allocate less nodes in the tree
|
||||
mNbAllocatedNodes /= 2;
|
||||
TreeNode* oldNodes = mNodes;
|
||||
mNodes = (TreeNode*) malloc(mNbAllocatedNodes * sizeof(TreeNode));
|
||||
assert(mNodes);
|
||||
memcpy(mNodes, oldNodes, mNbNodes * sizeof(TreeNode));
|
||||
free(oldNodes);
|
||||
|
||||
// Initialize the allocated nodes
|
||||
for (int 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;
|
||||
}
|
||||
}
|
||||
|
||||
// Add an object into the tree. This method creates a new leaf node in the tree and
|
||||
// returns the ID of the corresponding node.
|
||||
int DynamicAABBTree::addObject(CollisionShape* collisionShape, 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);
|
||||
mNodes[nodeID].aabb.setMin(mNodes[nodeID].aabb.getMin() - gap);
|
||||
mNodes[nodeID].aabb.setMax(mNodes[nodeID].aabb.getMax() + gap);
|
||||
|
||||
// Set the collision shape
|
||||
mNodes[nodeID].collisionShape = collisionShape;
|
||||
|
||||
// Set the height of the node in the tree
|
||||
mNodes[nodeID].height = 0;
|
||||
|
||||
// Insert the new leaf node in the tree
|
||||
insertLeafNode(nodeID);
|
||||
|
||||
// Return the node ID
|
||||
return nodeID;
|
||||
}
|
||||
|
||||
// Remove an object from the tree
|
||||
void DynamicAABBTree::removeObject(int 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.
|
||||
bool DynamicAABBTree::updateObject(int nodeID, const AABB& newAABB, const Vector3& displacement) {
|
||||
|
||||
assert(nodeID >= 0 && nodeID < mNbAllocatedNodes);
|
||||
assert(mNodes[nodeID].isLeaf());
|
||||
|
||||
// If the new AABB is still inside the fat AABB of the node
|
||||
if (mNodes[nodeID].aabb.contains(newAABB)) {
|
||||
return false;
|
||||
}
|
||||
|
||||
// If the new AABB is outside the fat AABB, we remove the corresponding node
|
||||
removeLeafNode(nodeID);
|
||||
|
||||
// Compute a new fat AABB for the new AABB by taking the object displacement into account
|
||||
AABB fatAABB = newAABB;
|
||||
const Vector3 gap(DYNAMIC_TREE_AABB_GAP, DYNAMIC_TREE_AABB_GAP, DYNAMIC_TREE_AABB_GAP);
|
||||
fatAABB.mMinCoordinates -= gap;
|
||||
fatAABB.mMaxCoordinates += gap;
|
||||
const Vector3 displacementGap = AABB_DISPLACEMENT_MULTIPLIER * displacement;
|
||||
if (displacementGap.x < decimal(0.0)) {
|
||||
fatAABB.mMinCoordinates.x += displacementGap.x;
|
||||
}
|
||||
else {
|
||||
fatAABB.mMaxCoordinates.x += displacementGap.x;
|
||||
}
|
||||
if (displacementGap.y < decimal(0.0)) {
|
||||
fatAABB.mMinCoordinates.y += displacementGap.y;
|
||||
}
|
||||
else {
|
||||
fatAABB.mMaxCoordinates.y += displacementGap.y;
|
||||
}
|
||||
if (displacementGap.z < decimal(0.0)) {
|
||||
fatAABB.mMinCoordinates.z += displacementGap.z;
|
||||
}
|
||||
else {
|
||||
fatAABB.mMaxCoordinates.z += displacementGap.z;
|
||||
}
|
||||
mNodes[nodeID].aabb = fatAABB;
|
||||
|
||||
// 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;
|
||||
}
|
||||
|
||||
// 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].leftChildID;
|
||||
int rightChild = mNodes[currentNodeID].rightChildID;
|
||||
|
||||
// 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].collisionShape = NULL;
|
||||
mNodes[newParentNode].aabb.mergeTwoAABBs(mNodes[siblingNode].aabb, newNodeAABB);
|
||||
mNodes[newParentNode].height = mNodes[siblingNode].height + 1;
|
||||
|
||||
// If the sibling node was not the root node
|
||||
if (oldParentNode != TreeNode::NULL_TREE_NODE) {
|
||||
if (mNodes[oldParentNode].leftChildID == siblingNode) {
|
||||
mNodes[oldParentNode].leftChildID = newParentNode;
|
||||
}
|
||||
else {
|
||||
mNodes[oldParentNode].rightChildID = newParentNode;
|
||||
}
|
||||
}
|
||||
else { // If the sibling node was the root node
|
||||
mNodes[newParentNode].leftChildID = siblingNode;
|
||||
mNodes[newParentNode].rightChildID = 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;
|
||||
while (currentNodeID != TreeNode::NULL_TREE_NODE) {
|
||||
|
||||
// Balance the sub-tree of the current node if it is not balanced
|
||||
currentNodeID = balanceSubTreeAtNode(currentNodeID);
|
||||
|
||||
int leftChild = mNodes[currentNodeID].leftChildID;
|
||||
int rightChild = mNodes[currentNodeID].rightChildID;
|
||||
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;
|
||||
|
||||
// Recompute the AABB of the node
|
||||
mNodes[currentNodeID].aabb.mergeTwoAABBs(mNodes[leftChild].aabb, mNodes[rightChild].aabb);
|
||||
|
||||
currentNodeID = mNodes[currentNodeID].parentID;
|
||||
}
|
||||
}
|
||||
|
||||
// Remove a leaf node from the tree
|
||||
void DynamicAABBTree::removeLeafNode(int nodeID) {
|
||||
|
||||
assert(nodeID >= 0 && nodeID < mNbAllocatedNodes);
|
||||
|
||||
// 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].leftChildID == nodeID) {
|
||||
siblingNodeID = mNodes[parentNodeID].rightChildID;
|
||||
}
|
||||
else {
|
||||
siblingNodeID = mNodes[parentNodeID].leftChildID;
|
||||
}
|
||||
|
||||
// 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;
|
||||
}
|
||||
else {
|
||||
assert(mNodes[grandParentNodeID].rightChildID == parentNodeID);
|
||||
mNodes[grandParentNodeID].rightChildID = 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);
|
||||
|
||||
// Get the two children of the current node
|
||||
int leftChildID = mNodes[currentNodeID].leftChildID;
|
||||
int rightChildID = mNodes[currentNodeID].rightChildID;
|
||||
|
||||
// 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;
|
||||
|
||||
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(nodeID);
|
||||
}
|
||||
}
|
||||
|
||||
// Balance the sub-tree of a given node using left or right rotations.
|
||||
/// The rotation schemes are described in in the book "Introduction to Game Physics
|
||||
/// with Box2D" by Ian Parberry. This method returns the new root node ID.
|
||||
int DynamicAABBTree::balanceSubTreeAtNode(int 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->leftChildID;
|
||||
int nodeCID = nodeA->rightChildID;
|
||||
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) {
|
||||
|
||||
int nodeFID = nodeC->leftChildID;
|
||||
int nodeGID = nodeC->rightChildID;
|
||||
assert(nodeFID >= 0 && nodeFID < mNbAllocatedNodes);
|
||||
assert(nodeGID >= 0 && nodeGID < mNbAllocatedNodes);
|
||||
TreeNode* nodeF = mNodes + nodeFID;
|
||||
TreeNode* nodeG = mNodes + nodeGID;
|
||||
|
||||
nodeC->leftChildID = 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;
|
||||
}
|
||||
else {
|
||||
assert(mNodes[nodeC->parentID].rightChildID == nodeID);
|
||||
mNodes[nodeC->parentID].rightChildID = nodeCID;
|
||||
}
|
||||
}
|
||||
else {
|
||||
mRootNodeID = nodeCID;
|
||||
}
|
||||
|
||||
// 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;
|
||||
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;
|
||||
}
|
||||
else { // If the right node C was higher than left node B because of node G
|
||||
nodeC->rightChildID = nodeGID;
|
||||
nodeA->rightChildID = 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;
|
||||
}
|
||||
|
||||
// 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) {
|
||||
|
||||
int nodeFID = nodeB->leftChildID;
|
||||
int nodeGID = nodeB->rightChildID;
|
||||
assert(nodeFID >= 0 && nodeFID < mNbAllocatedNodes);
|
||||
assert(nodeGID >= 0 && nodeGID < mNbAllocatedNodes);
|
||||
TreeNode* nodeF = mNodes + nodeFID;
|
||||
TreeNode* nodeG = mNodes + nodeGID;
|
||||
|
||||
nodeB->leftChildID = 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;
|
||||
}
|
||||
else {
|
||||
assert(mNodes[nodeB->parentID].rightChildID == nodeID);
|
||||
mNodes[nodeB->parentID].rightChildID = nodeBID;
|
||||
}
|
||||
}
|
||||
else {
|
||||
mRootNodeID = nodeBID;
|
||||
}
|
||||
|
||||
// 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;
|
||||
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;
|
||||
}
|
||||
else { // If the left node B was higher than right node C because of node G
|
||||
nodeB->rightChildID = nodeGID;
|
||||
nodeA->leftChildID = 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;
|
||||
}
|
||||
|
||||
// Return the new root of the sub-tree
|
||||
return nodeBID;
|
||||
}
|
||||
|
||||
// If the sub-tree is balanced, return the current root node
|
||||
return nodeID;
|
||||
}
|
||||
147
src/collision/broadphase/DynamicAABBTree.h
Normal file
147
src/collision/broadphase/DynamicAABBTree.h
Normal file
|
|
@ -0,0 +1,147 @@
|
|||
/********************************************************************************
|
||||
* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ *
|
||||
* Copyright (c) 2010-2014 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. *
|
||||
* *
|
||||
********************************************************************************/
|
||||
|
||||
#ifndef REACTPHYSICS3D_DYNAMIC_AABB_TREE_H
|
||||
#define REACTPHYSICS3D_DYNAMIC_AABB_TREE_H
|
||||
|
||||
// Libraries
|
||||
#include "../../configuration.h"
|
||||
#include "../shapes/AABB.h"
|
||||
#include "../shapes/CollisionShape.h"
|
||||
|
||||
/// Namespace ReactPhysics3D
|
||||
namespace reactphysics3d {
|
||||
|
||||
// Structure TreeNode
|
||||
/**
|
||||
* This structure represents a node of the dynamic AABB tree.
|
||||
*/
|
||||
struct TreeNode {
|
||||
|
||||
// -------------------- Constants -------------------- //
|
||||
|
||||
/// Null tree node constant
|
||||
const static int NULL_TREE_NODE;
|
||||
|
||||
// -------------------- Attributes -------------------- //
|
||||
|
||||
/// Parent node ID
|
||||
int parentID;
|
||||
|
||||
/// Left and right child of the node
|
||||
int leftChildID, rightChildID;
|
||||
|
||||
/// Next allocated node ID
|
||||
int nextNodeID;
|
||||
|
||||
/// Height of the node in the tree
|
||||
int 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)
|
||||
CollisionShape* collisionShape;
|
||||
|
||||
// -------------------- Methods -------------------- //
|
||||
|
||||
/// Return true if the node is a leaf of the tree
|
||||
bool isLeaf() const;
|
||||
};
|
||||
|
||||
// Class DynamicAABBTree
|
||||
/**
|
||||
* This class implements a dynamic AABB tree that is used for broad-phase
|
||||
* collision detection. This data structure is inspired by Nathanael Presson's
|
||||
* dynamic tree implementation in BulletPhysics. The following implementation is
|
||||
* based on the one from Erin Catto in Box2D as described in the book
|
||||
* "Introduction to Game Physics with Box2D" by Ian Parberry.
|
||||
*/
|
||||
class DynamicAABBTree {
|
||||
|
||||
private:
|
||||
|
||||
// -------------------- Attributes -------------------- //
|
||||
|
||||
/// Pointer to the memory location of the nodes of the tree
|
||||
TreeNode* mNodes;
|
||||
|
||||
/// ID of the root node of the tree
|
||||
int mRootNodeID;
|
||||
|
||||
/// ID of the first node of the list of free (allocated) nodes in the tree that we can use
|
||||
int mFreeNodeID;
|
||||
|
||||
/// Number of allocated nodes in the tree
|
||||
int mNbAllocatedNodes;
|
||||
|
||||
/// Number of nodes in the tree
|
||||
int mNbNodes;
|
||||
|
||||
// -------------------- Methods -------------------- //
|
||||
|
||||
/// Allocate and return a node to use in the tree
|
||||
int allocateNode();
|
||||
|
||||
/// Release a node
|
||||
void releaseNode(int nodeID);
|
||||
|
||||
/// Insert a leaf node in the tree
|
||||
void insertLeafNode(int nodeID);
|
||||
|
||||
/// Remove a leaf node from the tree
|
||||
void removeLeafNode(int nodeID);
|
||||
|
||||
/// Balance the sub-tree of a given node using left or right rotations.
|
||||
int balanceSubTreeAtNode(int nodeID);
|
||||
|
||||
public:
|
||||
|
||||
// -------------------- Methods -------------------- //
|
||||
|
||||
/// Constructor
|
||||
DynamicAABBTree();
|
||||
|
||||
/// Destructor
|
||||
~DynamicAABBTree();
|
||||
|
||||
/// Add an object into the tree
|
||||
int addObject(CollisionShape* collisionShape, const AABB& aabb);
|
||||
|
||||
/// Remove an object from the tree
|
||||
void removeObject(int nodeID);
|
||||
|
||||
/// Update the dynamic tree after an object has moved.
|
||||
bool updateObject(int nodeID, const AABB &newAABB, const Vector3 &displacement);
|
||||
};
|
||||
|
||||
// Return true if the node is a leaf of the tree
|
||||
inline bool TreeNode::isLeaf() const {
|
||||
return leftChildID == NULL_TREE_NODE;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
#endif
|
||||
|
|
@ -42,8 +42,39 @@ AABB::AABB(const Vector3& minCoordinates, const Vector3& maxCoordinates)
|
|||
|
||||
}
|
||||
|
||||
// Copy-constructor
|
||||
AABB::AABB(const AABB& aabb)
|
||||
: mMinCoordinates(aabb.mMinCoordinates), mMaxCoordinates(aabb.mMaxCoordinates) {
|
||||
|
||||
}
|
||||
|
||||
// Destructor
|
||||
AABB::~AABB() {
|
||||
|
||||
}
|
||||
|
||||
// Replace the current AABB with a new AABB that is the union of two AABBs in parameters
|
||||
void AABB::mergeTwoAABBs(const AABB& aabb1, const AABB& aabb2) {
|
||||
mMinCoordinates.x = std::min(aabb1.mMinCoordinates.x, aabb2.mMinCoordinates.x);
|
||||
mMinCoordinates.y = std::min(aabb1.mMinCoordinates.y, aabb2.mMinCoordinates.y);
|
||||
mMinCoordinates.z = std::min(aabb1.mMinCoordinates.z, aabb2.mMinCoordinates.z);
|
||||
|
||||
mMaxCoordinates.x = std::max(aabb1.mMaxCoordinates.x, aabb2.mMaxCoordinates.x);
|
||||
mMaxCoordinates.y = std::max(aabb1.mMaxCoordinates.y, aabb2.mMaxCoordinates.y);
|
||||
mMaxCoordinates.z = std::max(aabb1.mMaxCoordinates.z, aabb2.mMaxCoordinates.z);
|
||||
}
|
||||
|
||||
// Return true if the current AABB contains the AABB given in parameter
|
||||
bool AABB::contains(const AABB& aabb) {
|
||||
|
||||
bool isInside = true;
|
||||
isInside = isInside && mMinCoordinates.x <= aabb.mMinCoordinates.x;
|
||||
isInside = isInside && mMinCoordinates.y <= aabb.mMinCoordinates.y;
|
||||
isInside = isInside && mMinCoordinates.z <= aabb.mMinCoordinates.z;
|
||||
|
||||
isInside = isInside && mMaxCoordinates.x >= aabb.mMaxCoordinates.x;
|
||||
isInside = isInside && mMaxCoordinates.y >= aabb.mMaxCoordinates.y;
|
||||
isInside = isInside && mMaxCoordinates.z >= aabb.mMaxCoordinates.z;
|
||||
return isInside;
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -31,9 +31,6 @@
|
|||
|
||||
/// ReactPhysics3D namespace
|
||||
namespace reactphysics3d {
|
||||
|
||||
// Declaration
|
||||
class Body;
|
||||
|
||||
// Class AABB
|
||||
/**
|
||||
|
|
@ -54,17 +51,6 @@ class AABB {
|
|||
/// Maximum world coordinates of the AABB on the x,y and z axis
|
||||
Vector3 mMaxCoordinates;
|
||||
|
||||
// -------------------- Methods -------------------- //
|
||||
|
||||
/// Private copy-constructor
|
||||
AABB(const AABB& aabb);
|
||||
|
||||
/// Private assignment operator
|
||||
AABB& operator=(const AABB& aabb);
|
||||
|
||||
/// Constructor
|
||||
AABB(const Transform& transform, const Vector3& extents);
|
||||
|
||||
public :
|
||||
|
||||
// -------------------- Methods -------------------- //
|
||||
|
|
@ -75,10 +61,11 @@ class AABB {
|
|||
/// Constructor
|
||||
AABB(const Vector3& minCoordinates, const Vector3& maxCoordinates);
|
||||
|
||||
|
||||
/// Copy-constructor
|
||||
AABB(const AABB& aabb);
|
||||
|
||||
/// Destructor
|
||||
virtual ~AABB();
|
||||
~AABB();
|
||||
|
||||
/// Return the center point
|
||||
Vector3 getCenter() const;
|
||||
|
|
@ -97,11 +84,27 @@ class AABB {
|
|||
|
||||
/// Return true if the current AABB is overlapping with the AABB in argument
|
||||
bool testCollision(const AABB& aabb) const;
|
||||
|
||||
/// Return the volume of the AABB
|
||||
decimal getVolume() const;
|
||||
|
||||
/// Replace the current AABB with a new AABB that is the union of two AABBs in parameters
|
||||
void mergeTwoAABBs(const AABB& aabb1, const AABB& aabb2);
|
||||
|
||||
/// Return true if the current AABB contains the AABB given in parameter
|
||||
bool contains(const AABB& aabb);
|
||||
|
||||
/// Assignment operator
|
||||
AABB& operator=(const AABB& aabb);
|
||||
|
||||
// -------------------- Friendship -------------------- //
|
||||
|
||||
friend class DynamicAABBTree;
|
||||
};
|
||||
|
||||
// Return the center point of the AABB in world coordinates
|
||||
inline Vector3 AABB::getCenter() const {
|
||||
return (mMinCoordinates + mMaxCoordinates) * 0.5;
|
||||
return (mMinCoordinates + mMaxCoordinates) * decimal(0.5);
|
||||
}
|
||||
|
||||
// Return the minimum coordinates of the AABB
|
||||
|
|
@ -119,7 +122,7 @@ inline const Vector3& AABB::getMax() const {
|
|||
return mMaxCoordinates;
|
||||
}
|
||||
|
||||
/// Set the maximum coordinates of the AABB
|
||||
// Set the maximum coordinates of the AABB
|
||||
inline void AABB::setMax(const Vector3& max) {
|
||||
mMaxCoordinates = max;
|
||||
}
|
||||
|
|
@ -136,6 +139,21 @@ inline bool AABB::testCollision(const AABB& aabb) const {
|
|||
return true;
|
||||
}
|
||||
|
||||
// Return the volume of the AABB
|
||||
inline decimal AABB::getVolume() const {
|
||||
const Vector3 diff = mMaxCoordinates - mMinCoordinates;
|
||||
return (diff.x * diff.y * diff.z);
|
||||
}
|
||||
|
||||
// Assignment operator
|
||||
inline AABB& AABB::operator=(const AABB& aabb) {
|
||||
if (this != &aabb) {
|
||||
mMinCoordinates = aabb.mMinCoordinates;
|
||||
mMaxCoordinates = aabb.mMaxCoordinates;
|
||||
}
|
||||
return *this;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -121,6 +121,15 @@ const decimal DEFAULT_SLEEP_LINEAR_VELOCITY = decimal(0.02);
|
|||
/// might enter sleeping mode
|
||||
const decimal DEFAULT_SLEEP_ANGULAR_VELOCITY = decimal(3.0 * (PI / 180.0));
|
||||
|
||||
/// In the broad-phase collision detection (dynamic AABB tree), the AABBs are
|
||||
/// fatten to allow the collision shape to move a little bit without triggering
|
||||
/// a large modification of the tree which can be costly
|
||||
const decimal DYNAMIC_TREE_AABB_GAP = decimal(0.1);
|
||||
|
||||
/// In the dynamic AABB tree, we multiply this factor by the displacement of
|
||||
/// an object that has moved to recompute a new fat AABB
|
||||
const decimal AABB_DISPLACEMENT_MULTIPLIER = decimal(2.0);
|
||||
|
||||
}
|
||||
|
||||
#endif
|
||||
|
|
|
|||
Loading…
Reference in New Issue
Block a user