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

Make DynamicAABBTree more generic and use less memory per node

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

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

@ -33,7 +33,7 @@ using namespace reactphysics3d;
// Constructor
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<ProxyShape*>(mDynamicAABBTree.getNodeData(pair->collisionShape1ID));
ProxyShape* shape2 = static_cast<ProxyShape*>(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<ProxyShape*>(nodeData);
// Check if the raycast filtering mask allows raycast against this shape
if ((raycastWithCategoryMaskBits & proxyShape->getCollisionCategoryBits()) != 0) {
// Ask the collision detection to perform a ray cast test against
// the proxy shape of this node because the ray is overlapping
// with the shape in the broad-phase
hitFraction = raycastTest.raycastAgainstShape(proxyShape, ray);
}
return hitFraction;
}

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

@ -59,6 +59,35 @@ struct BroadPhasePair {
static bool smallerThan(const BroadPhasePair& pair1, const BroadPhasePair& pair2);
};
// 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);
}
}

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

@ -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<int, 64> 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);

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

@ -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;
}
}

5
src/configuration.h
View File

@ -51,6 +51,11 @@ typedef long unsigned int luint;
typedef luint bodyindex;
typedef std::pair<bodyindex, bodyindex> 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).