From 1fbd4f9a3a153c2d3da2efcc161e44f8ffaee2bf Mon Sep 17 00:00:00 2001 From: Ketmar Dark Date: Fri, 28 Oct 2016 01:38:58 +0300 Subject: [PATCH] cosmetix --- b2dlite.d | 832 +++++++++++++++++++++++++++++--------------------------------- xmain.d | 16 +- 2 files changed, 400 insertions(+), 448 deletions(-) diff --git a/b2dlite.d b/b2dlite.d index 771bb46..3b49c66 100644 --- a/b2dlite.d +++ b/b2dlite.d @@ -18,6 +18,7 @@ private: public import iv.vmath; version = b2dlite_use_bvh; +version = b2dlite_bvh_many_asserts; // ////////////////////////////////////////////////////////////////////////// // @@ -148,7 +149,7 @@ public: public: void drawBVH (scope void delegate (Vec2 min, Vec2 max) dg) { version(b2dlite_use_bvh) { - bvh.forEachLeaf((int nodeid, in ref AABB aabb) { + bvh.forEachLeaf((int nodeId, in ref AABB aabb) { dg(aabb.mMin, aabb.mMax); }); } @@ -163,7 +164,7 @@ public: void add (BodyBase bbody) { if (bbody !is null) { bodies ~= bbody; - version(b2dlite_use_bvh) bbody.nodeid = bvh.addObject(bbody); + version(b2dlite_use_bvh) bbody.nodeId = bvh.addObject(bbody); } } @@ -213,7 +214,7 @@ public: b.rotation += b.angularVelocity*dt; b.force.set(VFloatNum!0, VFloatNum!0); b.torque = VFloatNum!0; - version(b2dlite_use_bvh) bvh.updateObject(b.nodeid, disp); + version(b2dlite_use_bvh) bvh.updateObject(b.nodeId, disp); } } @@ -221,8 +222,8 @@ public: version(b2dlite_use_bvh) { foreach (immutable idx, BodyBase bi; bodies) { auto aabb = bi.getAABB(); - bvh.reportAllShapesOverlappingWithAABB(aabb, (int nodeid) { - auto bj = bvh.getNodeBody(nodeid); + bvh.reportAllShapesOverlappingWithAABB(aabb, (int nodeId) { + auto bj = bvh.getNodeBody(nodeId); if (bj == bi) return; if (bi.invMass == VFloatNum!0 && bj.invMass == VFloatNum!0) return; auto ak = ArbiterKey(bi, bj); @@ -259,7 +260,7 @@ private: private: uint midx; Mat22 rmattmp; // undefined most of the time; used only in coldet for now - int nodeid; // in bvh + int nodeId; // in bvh public: Vec2 position; @@ -1093,83 +1094,109 @@ private void polygon2Polygon (ref ContactInfo m, PolyBody A, PolyBody B) { * * 3. This notice may not be removed or altered from any source distribution. */ -private struct AABB { +private struct AABBBase(VT) if (Vec2.isVector!VT) { private: - Vec2 mMin, mMax; + VT mMin, mMax; public: - /*const ref*/ Vec2 getMin () const pure nothrow @safe @nogc { pragma(inline, true); return mMin; } - /*const ref*/ Vec2 getMax () const pure nothrow @safe @nogc { pragma(inline, true); return mMax; } + alias VType = VT; - void setMin (Vec2 v) pure nothrow @safe @nogc { pragma(inline, true); mMin = v; } - void setMax (Vec2 v) pure nothrow @safe @nogc { pragma(inline, true); mMax = v; } +public pure nothrow @safe @nogc: + /*const ref*/ VT getMin () const { pragma(inline, true); return mMin; } + /*const ref*/ VT getMax () const { pragma(inline, true); return mMax; } - void setMin (in ref Vec2 v) pure nothrow @safe @nogc { pragma(inline, true); mMin = v; } - void setMax (in ref Vec2 v) pure nothrow @safe @nogc { pragma(inline, true); mMax = v; } + void setMin (VT v) { pragma(inline, true); mMin = v; } + void setMax (VT v) { pragma(inline, true); mMax = v; } - // Return the volume of the AABB - VFloat getVolume() const { - const Vec2 diff = mMax-mMin; - return (diff.x*diff.y/**diff.z*/); + void setMin (in ref VT v) { pragma(inline, true); mMin = v; } + void setMax (in ref VT v) { pragma(inline, true); mMax = v; } + + // return the volume of the AABB + @property VFloat volume () const { + immutable diff = mMax-mMin; + static if (VT.isVector3!VT) { + return diff.x*diff.y*diff.z; + } else { + return diff.x*diff.y; + } } - void mergeTwoAABBs (in ref AABB aabb1, in ref AABB aabb2) { + static auto mergeAABBs (in ref AABB aabb1, in ref AABB aabb2) { + import std.algorithm : max, min; + typeof(this) res; + res.mMin.x = min(aabb1.mMin.x, aabb2.mMin.x); + res.mMin.y = min(aabb1.mMin.y, aabb2.mMin.y); + res.mMax.x = max(aabb1.mMax.x, aabb2.mMax.x); + res.mMax.y = max(aabb1.mMax.y, aabb2.mMax.y); + static if (VT.isVector3!VT) { + res.mMin.z = min(aabb1.mMin.z, aabb2.mMin.z); + res.mMax.z = max(aabb1.mMax.z, aabb2.mMax.z); + } + return res; + } + + void merge (in ref AABB aabb1, in ref AABB aabb2) { import std.algorithm : max, min; mMin.x = min(aabb1.mMin.x, aabb2.mMin.x); mMin.y = min(aabb1.mMin.y, aabb2.mMin.y); - //mMin.z = min(aabb1.mMin.z, aabb2.mMin.z); mMax.x = max(aabb1.mMax.x, aabb2.mMax.x); mMax.y = max(aabb1.mMax.y, aabb2.mMax.y); - //mMax.z = max(aabb1.mMax.z, aabb2.mMax.z); + static if (VT.isVector3!VT) { + mMin.z = min(aabb1.mMin.z, aabb2.mMin.z); + mMax.z = max(aabb1.mMax.z, aabb2.mMax.z); + } } // return true if the current AABB contains the AABB given in parameter - bool contains (const ref AABB aabb) const { + bool contains (in ref AABB aabb) const { bool isInside = true; isInside = isInside && mMin.x <= aabb.mMin.x; isInside = isInside && mMin.y <= aabb.mMin.y; - //isInside = isInside && mMin.z <= aabb.mMin.z; isInside = isInside && mMax.x >= aabb.mMax.x; isInside = isInside && mMax.y >= aabb.mMax.y; - //isInside = isInside && mMax.z >= aabb.mMax.z; + static if (VT.isVector3!VT) { + isInside = isInside && mMin.z <= aabb.mMin.z; + isInside = isInside && mMax.z >= aabb.mMax.z; + } return isInside; } - // Return true if the current AABB is overlapping with the AABB in argument. - // Two AABBs overlap if they overlap in the three x, y and z axis at the same time + // return true if the current AABB is overlapping with the AABB in argument + // two AABBs overlap if they overlap in the three x, y (and z) axes at the same time bool testCollision (in ref AABB aabb) const { if (mMax.x < aabb.mMin.x || aabb.mMax.x < mMin.x) return false; if (mMax.y < aabb.mMin.y || aabb.mMax.y < mMin.y) return false; - //if (mMax.z < aabb.mMin.z || aabb.mMax.z < mMin.z) return false; + static if (VT.isVector3!VT) { + if (mMax.z < aabb.mMin.z || aabb.mMax.z < mMin.z) return false; + } return true; } } +alias AABB = AABBBase!Vec2; -// ////////////////////////////////////////////////////////////////////////// // -private struct TreeNode { - enum int NullTreeNode = -1; +// ////////////////////////////////////////////////////////////////////////// // +private align(1) struct TreeNode { +align(1): + enum NullTreeNode = -1; + enum { Left = 0, Right = 1 } // a node is either in the tree (has a parent) or in the free nodes list (has a next node) union { - int parentID; - int nextNodeID; + int parentId; + int nextNodeId; } - // a node is either a leaf (has data) or is an internal node (has children) union { int[2] children; /// left and right child of the node (children[0] = left child) BodyBase body; } - - // Height of the node in the tree + // height of the node in the tree short height; - // fat axis aligned bounding box (AABB) corresponding to the node AABB aabb; - - // Return true if the node is a leaf of the tree - @property bool isLeaf () const pure nothrow @safe @nogc { pragma(inline, true); return (height == 0); } + // return true if the node is a leaf of the tree + @property bool leaf () const pure nothrow @safe @nogc { pragma(inline, true); return (height == 0); } } @@ -1186,90 +1213,89 @@ private final class DynamicAABBTree { // in the broad-phase collision detection (dynamic AABB tree), the AABBs are // also inflated in direction of the linear motion of the body by mutliplying the - // followin constant with the linear velocity and the elapsed time between two frames. + // followin constant with the linear velocity and the elapsed time between two frames enum VFloat DynamicBVHLinGapMult = VFloatNum!(1.7); public: // called when a overlapping node has been found during the call to reportAllShapesOverlappingWithAABB() - alias DynamicAABBTreeOverlapCallback = void delegate (int nodeId); + alias OverlapCallback = void delegate (int nodeId); private: - TreeNode* mNodes; // Pointer to the memory location of the nodes of the tree - int mRootNodeID; // ID of the root node of the tree - int mFreeNodeID; // ID of the first node of the list of free (allocated) nodes in the tree that we can use - int mNbAllocatedNodes; // Number of allocated nodes in the tree - int mNbNodes; // Number of nodes in the tree + TreeNode* mNodes; // pointer to the memory location of the nodes of the tree + int mRootNodeId; // id of the root node of the tree + int mFreeNodeId; // id of the first node of the list of free (allocated) nodes in the tree that we can use + int mAllocCount; // number of allocated nodes in the tree + int mNodeCount; // number of nodes in the tree // 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 - VFloat mExtraAABBGap; + VFloat mExtraGap; - /// Allocate and return a node to use in the tree + // allocate and return a node to use in the tree int allocateNode () { // if there is no more allocated node to use - if (mFreeNodeID == TreeNode.NullTreeNode) { + if (mFreeNodeId == TreeNode.NullTreeNode) { import core.stdc.stdlib : realloc; - assert(mNbNodes == mNbAllocatedNodes); + version(b2dlite_bvh_many_asserts) assert(mNodeCount == mAllocCount); // allocate more nodes in the tree - auto newsz = mNbAllocatedNodes*2; + auto newsz = mAllocCount*2; + if (newsz-mAllocCount > 4096) newsz = mAllocCount+4096; TreeNode* nn = cast(TreeNode*)realloc(mNodes, newsz*TreeNode.sizeof); if (nn is null) assert(0, "out of memory"); - mNbAllocatedNodes = newsz; + mAllocCount = newsz; mNodes = nn; // initialize the allocated nodes - for (int i = mNbNodes; i < mNbAllocatedNodes-1; ++i) { - mNodes[i].nextNodeID = i+1; + foreach (int i; mNodeCount..mAllocCount-1) { + mNodes[i].nextNodeId = i+1; mNodes[i].height = -1; } - mNodes[mNbAllocatedNodes-1].nextNodeID = TreeNode.NullTreeNode; - mNodes[mNbAllocatedNodes-1].height = -1; - mFreeNodeID = mNbNodes; + mNodes[mAllocCount-1].nextNodeId = TreeNode.NullTreeNode; + mNodes[mAllocCount-1].height = -1; + mFreeNodeId = mNodeCount; } // get the next free node - int freeNodeID = mFreeNodeID; - mFreeNodeID = mNodes[freeNodeID].nextNodeID; - mNodes[freeNodeID].parentID = TreeNode.NullTreeNode; - mNodes[freeNodeID].height = 0; - ++mNbNodes; - return freeNodeID; + int freeNodeId = mFreeNodeId; + mFreeNodeId = mNodes[freeNodeId].nextNodeId; + mNodes[freeNodeId].parentId = TreeNode.NullTreeNode; + mNodes[freeNodeId].height = 0; + ++mNodeCount; + return freeNodeId; } // release a node - void 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; - } - - // 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 insertLeafNode (int nodeID) { + void releaseNode (int nodeId) { + version(b2dlite_bvh_many_asserts) assert(mNodeCount > 0); + version(b2dlite_bvh_many_asserts) assert(nodeId >= 0 && nodeId < mAllocCount); + version(b2dlite_bvh_many_asserts) assert(mNodes[nodeId].height >= 0); + mNodes[nodeId].nextNodeId = mFreeNodeId; + mNodes[nodeId].height = -1; + mFreeNodeId = nodeId; + --mNodeCount; + } + + // 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 insertLeafNode (int nodeId) { // if the tree is empty - if (mRootNodeID == TreeNode.NullTreeNode) { - mRootNodeID = nodeID; - mNodes[mRootNodeID].parentID = TreeNode.NullTreeNode; + if (mRootNodeId == TreeNode.NullTreeNode) { + mRootNodeId = nodeId; + mNodes[mRootNodeId].parentId = TreeNode.NullTreeNode; return; } - assert(mRootNodeID != TreeNode.NullTreeNode); + version(b2dlite_bvh_many_asserts) assert(mRootNodeId != TreeNode.NullTreeNode); // 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]; + AABB newNodeAABB = mNodes[nodeId].aabb; + int currentNodeId = mRootNodeId; + while (!mNodes[currentNodeId].leaf) { + int leftChild = mNodes[currentNodeId].children.ptr[TreeNode.Left]; + int rightChild = mNodes[currentNodeId].children.ptr[TreeNode.Right]; // compute the merged AABB - VFloat volumeAABB = mNodes[currentNodeID].aabb.getVolume(); - AABB mergedAABBs; - mergedAABBs.mergeTwoAABBs(mNodes[currentNodeID].aabb, newNodeAABB); - VFloat mergedVolume = mergedAABBs.getVolume(); + VFloat volumeAABB = mNodes[currentNodeId].aabb.volume; + AABB mergedAABBs = AABB.mergeAABBs(mNodes[currentNodeId].aabb, newNodeAABB); + VFloat mergedVolume = mergedAABBs.volume; // compute the cost of making the current node the sibbling of the new node VFloat costS = VFloatNum!(2.0)*mergedVolume; @@ -1279,350 +1305,339 @@ private: // compute the cost of descending into the left child VFloat costLeft; - AABB currentAndLeftAABB; - currentAndLeftAABB.mergeTwoAABBs(newNodeAABB, mNodes[leftChild].aabb); - if (mNodes[leftChild].isLeaf) { - // if the left child is a leaf - costLeft = currentAndLeftAABB.getVolume()+costI; + AABB currentAndLeftAABB = AABB.mergeAABBs(newNodeAABB, mNodes[leftChild].aabb); + if (mNodes[leftChild].leaf) { + costLeft = currentAndLeftAABB.volume+costI; } else { - VFloat leftChildVolume = mNodes[leftChild].aabb.getVolume(); - costLeft = costI+currentAndLeftAABB.getVolume()-leftChildVolume; + VFloat leftChildVolume = mNodes[leftChild].aabb.volume; + costLeft = costI+currentAndLeftAABB.volume-leftChildVolume; } // compute the cost of descending into the right child VFloat costRight; - AABB currentAndRightAABB; - currentAndRightAABB.mergeTwoAABBs(newNodeAABB, mNodes[rightChild].aabb); - if (mNodes[rightChild].isLeaf) { - // if the right child is a leaf - costRight = currentAndRightAABB.getVolume()+costI; + AABB currentAndRightAABB = AABB.mergeAABBs(newNodeAABB, mNodes[rightChild].aabb); + if (mNodes[rightChild].leaf) { + costRight = currentAndRightAABB.volume+costI; } else { - VFloat rightChildVolume = mNodes[rightChild].aabb.getVolume(); - costRight = costI+currentAndRightAABB.getVolume()-rightChildVolume; + VFloat rightChildVolume = mNodes[rightChild].aabb.volume; + costRight = costI+currentAndRightAABB.volume-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; - } + currentNodeId = (costLeft < costRight ? leftChild : rightChild); } - int siblingNode = currentNodeID; + int siblingNode = currentNodeId; - // Create a new parent for the new node and the sibling node - int oldParentNode = mNodes[siblingNode].parentID; + // 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].parentId = oldParentNode; + mNodes[newParentNode].aabb.merge(mNodes[siblingNode].aabb, newNodeAABB); mNodes[newParentNode].height = cast(short)(mNodes[siblingNode].height+1); - assert(mNodes[newParentNode].height > 0); + version(b2dlite_bvh_many_asserts) assert(mNodes[newParentNode].height > 0); // If the sibling node was not the root node if (oldParentNode != TreeNode.NullTreeNode) { - assert(!mNodes[oldParentNode].isLeaf); - if (mNodes[oldParentNode].children[0] == siblingNode) { - mNodes[oldParentNode].children[0] = newParentNode; + version(b2dlite_bvh_many_asserts) assert(!mNodes[oldParentNode].leaf); + if (mNodes[oldParentNode].children.ptr[TreeNode.Left] == siblingNode) { + mNodes[oldParentNode].children.ptr[TreeNode.Left] = newParentNode; } else { - mNodes[oldParentNode].children[1] = newParentNode; + mNodes[oldParentNode].children.ptr[TreeNode.Right] = newParentNode; } - mNodes[newParentNode].children[0] = siblingNode; - mNodes[newParentNode].children[1] = nodeID; - mNodes[siblingNode].parentID = newParentNode; - mNodes[nodeID].parentID = newParentNode; + mNodes[newParentNode].children.ptr[TreeNode.Left] = siblingNode; + mNodes[newParentNode].children.ptr[TreeNode.Right] = 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; + mNodes[newParentNode].children.ptr[TreeNode.Left] = siblingNode; + mNodes[newParentNode].children.ptr[TreeNode.Right] = 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.NullTreeNode) { + currentNodeId = mNodes[nodeId].parentId; + version(b2dlite_bvh_many_asserts) assert(!mNodes[currentNodeId].leaf); + while (currentNodeId != TreeNode.NullTreeNode) { // balance the sub-tree of the current node if it is not balanced - currentNodeID = balanceSubTreeAtNode(currentNodeID); - assert(mNodes[nodeID].isLeaf); + currentNodeId = balanceSubTreeAtNode(currentNodeId); + version(b2dlite_bvh_many_asserts) assert(mNodes[nodeId].leaf); - assert(!mNodes[currentNodeID].isLeaf); - int leftChild = mNodes[currentNodeID].children[0]; - int rightChild = mNodes[currentNodeID].children[1]; - assert(leftChild != TreeNode.NullTreeNode); - assert(rightChild != TreeNode.NullTreeNode); + version(b2dlite_bvh_many_asserts) assert(!mNodes[currentNodeId].leaf); + int leftChild = mNodes[currentNodeId].children.ptr[TreeNode.Left]; + int rightChild = mNodes[currentNodeId].children.ptr[TreeNode.Right]; + version(b2dlite_bvh_many_asserts) assert(leftChild != TreeNode.NullTreeNode); + version(b2dlite_bvh_many_asserts) assert(rightChild != TreeNode.NullTreeNode); // recompute the height of the node in the tree - mNodes[currentNodeID].height = cast(short)(max(mNodes[leftChild].height, mNodes[rightChild].height)+1); - assert(mNodes[currentNodeID].height > 0); + mNodes[currentNodeId].height = cast(short)(max(mNodes[leftChild].height, mNodes[rightChild].height)+1); + version(b2dlite_bvh_many_asserts) assert(mNodes[currentNodeId].height > 0); // recompute the AABB of the node - mNodes[currentNodeID].aabb.mergeTwoAABBs(mNodes[leftChild].aabb, mNodes[rightChild].aabb); + mNodes[currentNodeId].aabb.merge(mNodes[leftChild].aabb, mNodes[rightChild].aabb); - currentNodeID = mNodes[currentNodeID].parentID; + currentNodeId = mNodes[currentNodeId].parentId; } - assert(mNodes[nodeID].isLeaf); + version(b2dlite_bvh_many_asserts) assert(mNodes[nodeId].leaf); } // remove a leaf node from the tree - void removeLeafNode (int nodeID) { - assert(nodeID >= 0 && nodeID < mNbAllocatedNodes); - assert(mNodes[nodeID].isLeaf); + void removeLeafNode (int nodeId) { + version(b2dlite_bvh_many_asserts) assert(nodeId >= 0 && nodeId < mAllocCount); + version(b2dlite_bvh_many_asserts) assert(mNodes[nodeId].leaf); // If we are removing the root node (root node is a leaf in this case) - if (mRootNodeID == nodeID) { - mRootNodeID = TreeNode.NullTreeNode; - return; - } + if (mRootNodeId == nodeId) { mRootNodeId = TreeNode.NullTreeNode; return; } - int parentNodeID = mNodes[nodeID].parentID; - int grandParentNodeID = mNodes[parentNodeID].parentID; - int siblingNodeID; - if (mNodes[parentNodeID].children[0] == nodeID) { - siblingNodeID = mNodes[parentNodeID].children[1]; + int parentNodeId = mNodes[nodeId].parentId; + int grandParentNodeId = mNodes[parentNodeId].parentId; + int siblingNodeId; + + if (mNodes[parentNodeId].children.ptr[TreeNode.Left] == nodeId) { + siblingNodeId = mNodes[parentNodeId].children.ptr[TreeNode.Right]; } else { - siblingNodeID = mNodes[parentNodeID].children[0]; + siblingNodeId = mNodes[parentNodeId].children.ptr[TreeNode.Left]; } // if the parent of the node to remove is not the root node - if (grandParentNodeID != TreeNode.NullTreeNode) { + if (grandParentNodeId != TreeNode.NullTreeNode) { // destroy the parent node - if (mNodes[grandParentNodeID].children[0] == parentNodeID) { - mNodes[grandParentNodeID].children[0] = siblingNodeID; + if (mNodes[grandParentNodeId].children.ptr[TreeNode.Left] == parentNodeId) { + mNodes[grandParentNodeId].children.ptr[TreeNode.Left] = siblingNodeId; } else { - assert(mNodes[grandParentNodeID].children[1] == parentNodeID); - mNodes[grandParentNodeID].children[1] = siblingNodeID; + version(b2dlite_bvh_many_asserts) assert(mNodes[grandParentNodeId].children.ptr[TreeNode.Right] == parentNodeId); + mNodes[grandParentNodeId].children.ptr[TreeNode.Right] = siblingNodeId; } - mNodes[siblingNodeID].parentID = grandParentNodeID; - releaseNode(parentNodeID); + 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.NullTreeNode) { + int currentNodeId = grandParentNodeId; + while (currentNodeId != TreeNode.NullTreeNode) { // balance the current sub-tree if necessary - currentNodeID = balanceSubTreeAtNode(currentNodeID); + currentNodeId = balanceSubTreeAtNode(currentNodeId); - assert(!mNodes[currentNodeID].isLeaf); + version(b2dlite_bvh_many_asserts) assert(!mNodes[currentNodeId].leaf); - // get the two children of the current node - int leftChildID = mNodes[currentNodeID].children[0]; - int rightChildID = mNodes[currentNodeID].children[1]; + // get the two children.ptr of the current node + int leftChildId = mNodes[currentNodeId].children.ptr[TreeNode.Left]; + int rightChildId = mNodes[currentNodeId].children.ptr[TreeNode.Right]; // recompute the AABB and the height of the current node - mNodes[currentNodeID].aabb.mergeTwoAABBs(mNodes[leftChildID].aabb, mNodes[rightChildID].aabb); - mNodes[currentNodeID].height = cast(short)(max(mNodes[leftChildID].height, mNodes[rightChildID].height)+1); - assert(mNodes[currentNodeID].height > 0); + mNodes[currentNodeId].aabb.merge(mNodes[leftChildId].aabb, mNodes[rightChildId].aabb); + mNodes[currentNodeId].height = cast(short)(max(mNodes[leftChildId].height, mNodes[rightChildId].height)+1); + version(b2dlite_bvh_many_asserts) assert(mNodes[currentNodeId].height > 0); - currentNodeID = mNodes[currentNodeID].parentID; + 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.NullTreeNode; - releaseNode(parentNodeID); + // 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.NullTreeNode; + 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. - int balanceSubTreeAtNode (int nodeID) { - assert(nodeID != TreeNode.NullTreeNode); + // 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 + int balanceSubTreeAtNode (int nodeId) { + version(b2dlite_bvh_many_asserts) assert(nodeId != TreeNode.NullTreeNode); - TreeNode* nodeA = mNodes+nodeID; + 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) return nodeID; // do not perform any rotation + if (nodeA.leaf || nodeA.height < 2) return nodeId; // do not perform any rotation // 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; + int nodeBId = nodeA.children.ptr[TreeNode.Left]; + int nodeCId = nodeA.children.ptr[TreeNode.Right]; + version(b2dlite_bvh_many_asserts) assert(nodeBId >= 0 && nodeBId < mAllocCount); + version(b2dlite_bvh_many_asserts) assert(nodeCId >= 0 && nodeCId < mAllocCount); + 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.NullTreeNode) { - if (mNodes[nodeC.parentID].children[0] == nodeID) { - mNodes[nodeC.parentID].children[0] = nodeCID; + version(b2dlite_bvh_many_asserts) assert(!nodeC.leaf); + + int nodeFId = nodeC.children.ptr[TreeNode.Left]; + int nodeGId = nodeC.children.ptr[TreeNode.Right]; + version(b2dlite_bvh_many_asserts) assert(nodeFId >= 0 && nodeFId < mAllocCount); + version(b2dlite_bvh_many_asserts) assert(nodeGId >= 0 && nodeGId < mAllocCount); + TreeNode* nodeF = mNodes+nodeFId; + TreeNode* nodeG = mNodes+nodeGId; + + nodeC.children.ptr[TreeNode.Left] = nodeId; + nodeC.parentId = nodeA.parentId; + nodeA.parentId = nodeCId; + + if (nodeC.parentId != TreeNode.NullTreeNode) { + if (mNodes[nodeC.parentId].children.ptr[TreeNode.Left] == nodeId) { + mNodes[nodeC.parentId].children.ptr[TreeNode.Left] = nodeCId; } else { - assert(mNodes[nodeC.parentID].children[1] == nodeID); - mNodes[nodeC.parentID].children[1] = nodeCID; + version(b2dlite_bvh_many_asserts) assert(mNodes[nodeC.parentId].children.ptr[TreeNode.Right] == nodeId); + mNodes[nodeC.parentId].children.ptr[TreeNode.Right] = nodeCId; } } else { - mRootNodeID = nodeCID; + mRootNodeId = nodeCId; } - assert(!nodeC.isLeaf); - assert(!nodeA.isLeaf); + version(b2dlite_bvh_many_asserts) assert(!nodeC.leaf); + version(b2dlite_bvh_many_asserts) assert(!nodeA.leaf); // 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; + nodeC.children.ptr[TreeNode.Right] = nodeFId; + nodeA.children.ptr[TreeNode.Right] = 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); + nodeA.aabb.merge(nodeB.aabb, nodeG.aabb); + nodeC.aabb.merge(nodeA.aabb, nodeF.aabb); // recompute the height of node A and C nodeA.height = cast(short)(max(nodeB.height, nodeG.height)+1); nodeC.height = cast(short)(max(nodeA.height, nodeF.height)+1); - assert(nodeA.height > 0); - assert(nodeC.height > 0); + version(b2dlite_bvh_many_asserts) assert(nodeA.height > 0); + version(b2dlite_bvh_many_asserts) 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; + nodeC.children.ptr[TreeNode.Right] = nodeGId; + nodeA.children.ptr[TreeNode.Right] = 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); + nodeA.aabb.merge(nodeB.aabb, nodeF.aabb); + nodeC.aabb.merge(nodeA.aabb, nodeG.aabb); // recompute the height of node A and C nodeA.height = cast(short)(max(nodeB.height, nodeF.height)+1); nodeC.height = cast(short)(max(nodeA.height, nodeG.height)+1); - assert(nodeA.height > 0); - assert(nodeC.height > 0); + version(b2dlite_bvh_many_asserts) assert(nodeA.height > 0); + version(b2dlite_bvh_many_asserts) assert(nodeC.height > 0); } // return the new root of the sub-tree - return nodeCID; + 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.NullTreeNode) { - if (mNodes[nodeB.parentID].children[0] == nodeID) { - mNodes[nodeB.parentID].children[0] = nodeBID; + version(b2dlite_bvh_many_asserts) assert(!nodeB.leaf); + + int nodeFId = nodeB.children.ptr[TreeNode.Left]; + int nodeGId = nodeB.children.ptr[TreeNode.Right]; + version(b2dlite_bvh_many_asserts) assert(nodeFId >= 0 && nodeFId < mAllocCount); + version(b2dlite_bvh_many_asserts) assert(nodeGId >= 0 && nodeGId < mAllocCount); + TreeNode* nodeF = mNodes+nodeFId; + TreeNode* nodeG = mNodes+nodeGId; + + nodeB.children.ptr[TreeNode.Left] = nodeId; + nodeB.parentId = nodeA.parentId; + nodeA.parentId = nodeBId; + + if (nodeB.parentId != TreeNode.NullTreeNode) { + if (mNodes[nodeB.parentId].children.ptr[TreeNode.Left] == nodeId) { + mNodes[nodeB.parentId].children.ptr[TreeNode.Left] = nodeBId; } else { - assert(mNodes[nodeB.parentID].children[1] == nodeID); - mNodes[nodeB.parentID].children[1] = nodeBID; + version(b2dlite_bvh_many_asserts) assert(mNodes[nodeB.parentId].children.ptr[TreeNode.Right] == nodeId); + mNodes[nodeB.parentId].children.ptr[TreeNode.Right] = nodeBId; } } else { - mRootNodeID = nodeBID; + mRootNodeId = nodeBId; } - assert(!nodeB.isLeaf); - assert(!nodeA.isLeaf); + version(b2dlite_bvh_many_asserts) assert(!nodeB.leaf); + version(b2dlite_bvh_many_asserts) assert(!nodeA.leaf); // 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; + nodeB.children.ptr[TreeNode.Right] = nodeFId; + nodeA.children.ptr[TreeNode.Left] = 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); + nodeA.aabb.merge(nodeC.aabb, nodeG.aabb); + nodeB.aabb.merge(nodeA.aabb, nodeF.aabb); // recompute the height of node A and B nodeA.height = cast(short)(max(nodeC.height, nodeG.height)+1); nodeB.height = cast(short)(max(nodeA.height, nodeF.height)+1); - assert(nodeA.height > 0); - assert(nodeB.height > 0); + version(b2dlite_bvh_many_asserts) assert(nodeA.height > 0); + version(b2dlite_bvh_many_asserts) 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; + nodeB.children.ptr[TreeNode.Right] = nodeGId; + nodeA.children.ptr[TreeNode.Left] = 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); + nodeA.aabb.merge(nodeC.aabb, nodeF.aabb); + nodeB.aabb.merge(nodeA.aabb, nodeG.aabb); // recompute the height of node A and B nodeA.height = cast(short)(max(nodeC.height, nodeF.height)+1); nodeB.height = cast(short)(max(nodeA.height, nodeG.height)+1); - assert(nodeA.height > 0); - assert(nodeB.height > 0); + version(b2dlite_bvh_many_asserts) assert(nodeA.height > 0); + version(b2dlite_bvh_many_asserts) assert(nodeB.height > 0); } // return the new root of the sub-tree - return nodeBID; + return nodeBId; } // if the sub-tree is balanced, return the current root node - return nodeID; + return nodeId; } - // Compute the height of a given node in the tree - int computeHeight (int nodeID) { - assert(nodeID >= 0 && nodeID < mNbAllocatedNodes); - TreeNode* node = mNodes+nodeID; + // compute the height of a given node in the tree + int computeHeight (int nodeId) { + version(b2dlite_bvh_many_asserts) assert(nodeId >= 0 && nodeId < mAllocCount); + TreeNode* node = mNodes+nodeId; // If the node is a leaf, its height is zero - if (node.isLeaf) return 0; + if (node.leaf) return 0; // Compute the height of the left and right sub-tree - int leftHeight = computeHeight(node.children[0]); - int rightHeight = computeHeight(node.children[1]); + int leftHeight = computeHeight(node.children.ptr[TreeNode.Left]); + int rightHeight = computeHeight(node.children.ptr[TreeNode.Right]); // Return the height of the node return 1+max(leftHeight, rightHeight); } - /// Internally add an object into the tree + // internally add an object into the tree int addObjectInternal (in ref AABB aabb) { // get the next available node (or allocate new ones if necessary) - int nodeID = allocateNode(); + int nodeId = allocateNode(); // create the fat aabb to use in the tree - immutable gap = Vec2(mExtraAABBGap, mExtraAABBGap, mExtraAABBGap); - mNodes[nodeID].aabb.setMin(aabb.getMin()-gap); - mNodes[nodeID].aabb.setMax(aabb.getMax()+gap); + immutable gap = AABB.VType(mExtraGap, mExtraGap, mExtraGap); + 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; + mNodes[nodeId].height = 0; // insert the new leaf node in the tree - insertLeafNode(nodeID); - assert(mNodes[nodeID].isLeaf); + insertLeafNode(nodeId); + version(b2dlite_bvh_many_asserts) assert(mNodes[nodeId].leaf); - assert(nodeID >= 0); + version(b2dlite_bvh_many_asserts) assert(nodeId >= 0); // return the Id of the node - return nodeID; + return nodeId; } // initialize the tree @@ -1630,135 +1645,68 @@ private: import core.stdc.stdlib : malloc; import core.stdc.string : memset; - mRootNodeID = TreeNode.NullTreeNode; - mNbNodes = 0; - mNbAllocatedNodes = 8; + mRootNodeId = TreeNode.NullTreeNode; + mNodeCount = 0; + mAllocCount = 64; - mNodes = cast(TreeNode*)malloc(mNbAllocatedNodes*TreeNode.sizeof); + mNodes = cast(TreeNode*)malloc(mAllocCount*TreeNode.sizeof); if (mNodes is null) assert(0, "out of memory"); - memset(mNodes, 0, mNbAllocatedNodes*TreeNode.sizeof); + memset(mNodes, 0, mAllocCount*TreeNode.sizeof); // initialize the allocated nodes - for (int i = 0; i < mNbAllocatedNodes-1; ++i) { - mNodes[i].nextNodeID = i+1; + foreach (int i; 0..mAllocCount-1) { + mNodes[i].nextNodeId = i+1; mNodes[i].height = -1; } - mNodes[mNbAllocatedNodes-1].nextNodeID = TreeNode.NullTreeNode; - mNodes[mNbAllocatedNodes-1].height = -1; - mFreeNodeID = 0; + mNodes[mAllocCount-1].nextNodeId = TreeNode.NullTreeNode; + mNodes[mAllocCount-1].height = -1; + mFreeNodeId = 0; } - // check if the tree structure is valid (for debugging purpose) - void check () { - // recursively check each node - checkNode(mRootNodeID); - int nbFreeNodes = 0; - int freeNodeID = mFreeNodeID; - // check the free nodes - while (freeNodeID != TreeNode.NullTreeNode) { - 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 checkNode (int nodeID) { - if (nodeID == TreeNode.NullTreeNode) return; - - // if it is the root - if (nodeID == mRootNodeID) { - assert(mNodes[nodeID].parentID == TreeNode.NullTreeNode); - } - - // get the children nodes - TreeNode* pNode = mNodes+nodeID; - - 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(pNode.height == 0); - } else { - int leftChild = pNode.children[0]; - int rightChild = pNode.children[1]; - // 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+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); - } - } - - // check if the tree structure is valid (for debugging purpose) - void forEachLeaf (scope void delegate (int nodeid, in ref AABB aabb) dg) { - void forEachNode (int nodeID) { - if (nodeID == TreeNode.NullTreeNode) return; + // also, checks if the tree structure is valid (for debugging purpose) + void forEachLeaf (scope void delegate (int nodeId, in ref AABB aabb) dg) { + void forEachNode (int nodeId) { + if (nodeId == TreeNode.NullTreeNode) return; // if it is the root - if (nodeID == mRootNodeID) { - assert(mNodes[nodeID].parentID == TreeNode.NullTreeNode); + if (nodeId == mRootNodeId) { + assert(mNodes[nodeId].parentId == TreeNode.NullTreeNode); } // get the children nodes - TreeNode* pNode = mNodes+nodeID; - //if (pNode.isLeaf) { import core.stdc.stdio; printf("node %d is leaf (root=%d)\n", nodeID, mRootNodeID); } - //assert(!pNode.isLeaf); + TreeNode* pNode = mNodes+nodeId; assert(pNode.height >= 0); - assert(pNode.aabb.getVolume() > 0); + assert(pNode.aabb.volume > 0); // if the current node is a leaf - if (pNode.isLeaf) { - // check that there are no children - //assert(leftChild == TreeNode.NullTreeNode); - //assert(rightChild == TreeNode.NullTreeNode); + if (pNode.leaf) { assert(pNode.height == 0); - dg(nodeID, pNode.aabb); + if (dg !is null) dg(nodeId, pNode.aabb); } else { - int leftChild = pNode.children[0]; - int rightChild = pNode.children[1]; - // check that the children node IDs are valid - assert(0 <= leftChild && leftChild < mNbAllocatedNodes); - assert(0 <= rightChild && rightChild < mNbAllocatedNodes); + int leftChild = pNode.children.ptr[TreeNode.Left]; + int rightChild = pNode.children.ptr[TreeNode.Right]; + // check that the children node Ids are valid + assert(0 <= leftChild && leftChild < mAllocCount); + assert(0 <= rightChild && rightChild < mAllocCount); // check that the children nodes have the correct parent node - assert(mNodes[leftChild].parentID == nodeID); - assert(mNodes[rightChild].parentID == nodeID); + assert(mNodes[leftChild].parentId == nodeId); + assert(mNodes[rightChild].parentId == nodeId); // check the height of node int height = 1+max(mNodes[leftChild].height, mNodes[rightChild].height); - assert(mNodes[nodeID].height == 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()); + AABB aabb = AABB.mergeAABBs(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 forEachNode(leftChild); forEachNode(rightChild); } } // recursively check each node - forEachNode(mRootNodeID); + forEachNode(mRootNodeId); } public: - this (VFloat extraAABBGap=VFloatNum!(0.0)) { - mExtraAABBGap = extraAABBGap; + this (VFloat extraAABBGap=VFloatNum!0) { + mExtraGap = extraAABBGap; setup(); } @@ -1767,24 +1715,26 @@ public: free(mNodes); } - // return the fat AABB corresponding to a given node ID - /*const ref*/ AABB getFatAABB (int nodeID) const { - assert(nodeID >= 0 && nodeID < mNbAllocatedNodes); - return mNodes[nodeID].aabb; + // return the fat AABB corresponding to a given node Id + /*const ref*/ AABB getFatAABB (int nodeId) const { + pragma(inline, true); + version(b2dlite_bvh_many_asserts) assert(nodeId >= 0 && nodeId < mAllocCount); + return mNodes[nodeId].aabb; } // return the pointer to the data array of a given leaf node of the tree - BodyBase getNodeBody (int nodeID) { - assert(nodeID >= 0 && nodeID < mNbAllocatedNodes); - assert(mNodes[nodeID].isLeaf); - return mNodes[nodeID].body; + BodyBase getNodeBody (int nodeId) { + pragma(inline, true); + version(b2dlite_bvh_many_asserts) assert(nodeId >= 0 && nodeId < mAllocCount); + version(b2dlite_bvh_many_asserts) assert(mNodes[nodeId].leaf); + return mNodes[nodeId].body; } // return the root AABB of the tree - AABB getRootAABB () { return getFatAABB(mRootNodeID); } + AABB getRootAABB () { pragma(inline, true); return getFatAABB(mRootNodeId); } // add an object into the tree. - // this method creates a new leaf node in the tree and returns the ID of the corresponding node + // this method creates a new leaf node in the tree and returns the Id of the corresponding node int addObject (/*in ref AABB aabb,*/ BodyBase body) { auto aabb = body.getAABB(); int nodeId = addObjectInternal(aabb); @@ -1793,70 +1743,70 @@ public: } // remove an object from the tree - void removeObject (int nodeID) { - assert(nodeID >= 0 && nodeID < mNbAllocatedNodes); - assert(mNodes[nodeID].isLeaf); + void removeObject (int nodeId) { + version(b2dlite_bvh_many_asserts) assert(nodeId >= 0 && nodeId < mAllocCount); + version(b2dlite_bvh_many_asserts) assert(mNodes[nodeId].leaf); // remove the node from the tree - removeLeafNode(nodeID); - releaseNode(nodeID); + 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. 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 + // 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. + // 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 updateObject (int nodeID/*, in ref AABB newAABB*/, in ref Vec2 displacement, bool forceReinsert=false) { - assert(nodeID >= 0 && nodeID < mNbAllocatedNodes); - assert(mNodes[nodeID].isLeaf); - assert(mNodes[nodeID].height >= 0); + bool updateObject (int nodeId/*, in ref AABB newAABB*/, in ref AABB.VType displacement, bool forceReinsert=false) { + version(b2dlite_bvh_many_asserts) assert(nodeId >= 0 && nodeId < mAllocCount); + version(b2dlite_bvh_many_asserts) assert(mNodes[nodeId].leaf); + version(b2dlite_bvh_many_asserts) assert(mNodes[nodeId].height >= 0); - auto newAABB = mNodes[nodeID].body.getAABB(); + auto newAABB = mNodes[nodeId].body.getAABB(); // If the new AABB is still inside the fat AABB of the node - if (!forceReinsert && mNodes[nodeID].aabb.contains(newAABB)) return false; + 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); + removeLeafNode(nodeId); // compute the fat AABB by inflating the AABB with a constant gap - mNodes[nodeID].aabb = newAABB; - immutable gap = Vec2(mExtraAABBGap, mExtraAABBGap, mExtraAABBGap); - mNodes[nodeID].aabb.mMin -= gap; - mNodes[nodeID].aabb.mMax += gap; + mNodes[nodeId].aabb = newAABB; + immutable gap = AABB.VType(mExtraGap, mExtraGap, mExtraGap); + mNodes[nodeId].aabb.mMin -= gap; + mNodes[nodeId].aabb.mMax += gap; // inflate the fat AABB in direction of the linear motion of the AABB - if (displacement.x < VFloatNum!(0.0)) { - mNodes[nodeID].aabb.mMin.x += DynamicBVHLinGapMult*displacement.x; + if (displacement.x < VFloatNum!0) { + mNodes[nodeId].aabb.mMin.x += DynamicBVHLinGapMult*displacement.x; } else { - mNodes[nodeID].aabb.mMax.x += DynamicBVHLinGapMult*displacement.x; + mNodes[nodeId].aabb.mMax.x += DynamicBVHLinGapMult*displacement.x; } - if (displacement.y < VFloatNum!(0.0)) { - mNodes[nodeID].aabb.mMin.y += DynamicBVHLinGapMult*displacement.y; + if (displacement.y < VFloatNum!0) { + mNodes[nodeId].aabb.mMin.y += DynamicBVHLinGapMult*displacement.y; } else { - mNodes[nodeID].aabb.mMax.y += DynamicBVHLinGapMult*displacement.y; + mNodes[nodeId].aabb.mMax.y += DynamicBVHLinGapMult*displacement.y; } - /* - if (displacement.z < VFloatNum!(0.0)) { - mNodes[nodeID].aabb.mMin.z += DynamicBVHLinGapMult *displacement.z; - } else { - mNodes[nodeID].aabb.mMax.z += DynamicBVHLinGapMult *displacement.z; + static if (AABB.VType.isVector3!(AABB.VType)) { + if (displacement.z < VFloatNum!0) { + mNodes[nodeId].aabb.mMin.z += DynamicBVHLinGapMult *displacement.z; + } else { + mNodes[nodeId].aabb.mMax.z += DynamicBVHLinGapMult *displacement.z; + } } - */ - assert(mNodes[nodeID].aabb.contains(newAABB)); + version(b2dlite_bvh_many_asserts) assert(mNodes[nodeId].aabb.contains(newAABB)); // reinsert the node into the tree - insertLeafNode(nodeID); + insertLeafNode(nodeId); return true; } // report all shapes overlapping with the AABB given in parameter - void reportAllShapesOverlappingWithAABB (in ref AABB aabb, scope DynamicAABBTreeOverlapCallback callback) { - int[256] stack; // stack with the nodes to visit + void reportAllShapesOverlappingWithAABB (in ref AABB aabb, scope OverlapCallback callback) { + int[256] stack = void; // stack with the nodes to visit int sp = 0; void spush (int id) { @@ -1869,34 +1819,34 @@ public: return stack.ptr[--sp]; } - spush(mRootNodeID); + spush(mRootNodeId); // while there are still nodes to visit while (sp > 0) { - // get the next node ID to visit - int nodeIDToVisit = spop(); + // get the next node id to visit + int nodeIdToVisit = spop(); // skip it if it is a null node - if (nodeIDToVisit == TreeNode.NullTreeNode) continue; + if (nodeIdToVisit == TreeNode.NullTreeNode) continue; // get the corresponding node - const(TreeNode)* nodeToVisit = mNodes+nodeIDToVisit; + 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) { + if (nodeToVisit.leaf) { // notify the broad-phase about a new potential overlapping pair - callback(nodeIDToVisit); + callback(nodeIdToVisit); } else { // if the node is not a leaf // we need to visit its children - spush(nodeToVisit.children[0]); - spush(nodeToVisit.children[1]); + spush(nodeToVisit.children.ptr[TreeNode.Left]); + spush(nodeToVisit.children.ptr[TreeNode.Right]); } } } } // compute the height of the tree - int computeHeight () { return computeHeight(mRootNodeID); } + int computeHeight () { pragma(inline, true); return computeHeight(mRootNodeId); } // clear all the nodes and reset the tree void reset() { diff --git a/xmain.d b/xmain.d index 2226948..01bd67f 100644 --- a/xmain.d +++ b/xmain.d @@ -21,7 +21,8 @@ import iv.vmath; import b2dlite; -enum show_times = false; +bool optShowTimes = false; +bool optDrawBVH = false; // ////////////////////////////////////////////////////////////////////////// // @@ -517,7 +518,7 @@ void drawWorld () { glPointSize(VFloatNum!1); } - version(none) { + if (optDrawBVH) { // draw BVH world.drawBVH((Vec2 min, Vec2 max) { glColor3f(VFloatNum!1, VFloatNum!1, VFloatNum!0); @@ -563,12 +564,11 @@ void simulationStep (SimpleWindow sdwin) { contacts.length = 0; contacts.assumeSafeAppend; - static if (show_times) { - import core.time; - auto stt = MonoTime.currTime; - } + import core.time; + MonoTime stt; + if (optShowTimes) stt = MonoTime.currTime; world.step(timeStep); - static if (show_times) { + if (optShowTimes) { auto tm = (MonoTime.currTime-stt).total!"msecs"; { import core.stdc.stdio; printf("step: %d msecs\n", cast(int)tm); } } @@ -667,6 +667,8 @@ void main () { if (ch == '+') { setupDemo(demoIndex+1, sdwin); return; } if (ch == '-') { if (demoIndex > 0) setupDemo(demoIndex-1, sdwin); return; } if (ch == 'z') { doOneStep = true; return; } + if (ch == 'T') { optShowTimes = !optShowTimes; return; } + if (ch == 'A') { optDrawBVH = !optDrawBVH; return; } }, ); } -- 2.11.4.GIT