/// Constructing the tree initializes the node pool. public DynamicTree() { _root = NullNode; _nodeCapacity = 16; _nodeCount = 0; _nodes = new DynamicTreeNode[_nodeCapacity]; //Fill the array with nodes: for (int i = 0; i < _nodeCapacity; ++i) { _nodes[i] = new DynamicTreeNode(); } // Build a linked list for the free list. The parent // pointer becomes the "next" pointer. for (int i = 0; i < _nodeCapacity - 1; ++i) { _nodes[i].Next = i + 1; } _nodes[_nodeCapacity - 1].Next = NullNode; _freeList = 0; _insertionCount = 0; }
/// Constructing the tree initializes the node pool. public DynamicTree() { _root = NullNode; _nodeCapacity = 16; _nodeCount = 0; _nodes = new DynamicTreeNode[_nodeCapacity]; //Fill the array with nodes: for (int i = 0; i < _nodeCapacity; ++i) { _nodes[i] = new DynamicTreeNode(); } // Build a linked list for the free list. The parent // pointer becomes the "next" pointer. for (int i = 0; i < _nodeCapacity - 1; ++i) { _nodes[i].Next = i + 1; } _nodes[_nodeCapacity - 1].Next = NullNode; _freeList = 0; _insertionCount = 0; }
//TODO: uncomment //public void Rebalance(int iterations) //{ // if (_root == NullNode) // { // return; // } // for (int i = 0; i < iterations; ++i) // { // int node = _root; // uint bit = 0; // while (_nodes[node].IsLeaf() == false) // { // int children = _nodes[node].Child1; // node = children[(_path >> bit) & 1]; // bit = (bit + 1) & (8 * sizeof(uint) - 1); // } // ++_path; // RemoveLeaf(node); // InsertLeaf(node); // } //} // Compute the height of a sub-tree. public int ComputeHeight(int nodeId) { if (nodeId == NullNode) { return(0); } Box2DXDebug.Assert(0 <= nodeId && nodeId < _nodeCapacity); DynamicTreeNode node = _nodes[nodeId]; int height1 = ComputeHeight(node.Child1); int height2 = ComputeHeight(node.Child2); return(1 + Math.Max(height1, height2)); }
/// Query an AABB for overlapping proxies. The callback class /// is called for each proxy that overlaps the supplied AABB. public void Query(IQueryEnabled callback, AABB aabb) { const int k_stackSize = 128; int[] stack = new int[k_stackSize]; int count = 0; stack[count++] = _root; while (count > 0) { int nodeId = stack[--count]; if (nodeId == NullNode) { continue; } DynamicTreeNode node = _nodes[nodeId]; if (Collision.TestOverlap(node.Aabb, aabb)) { if (node.IsLeaf()) { bool proceed = callback.QueryCallback(nodeId); if (proceed == false) { return; } } else { Box2DXDebug.Assert(count + 1 < k_stackSize); stack[count++] = node.Child1; stack[count++] = node.Child2; } } } }
private int AllocateNode() { // Expand the node pool as needed. if (_freeList == NullNode) { Box2DXDebug.Assert(_nodeCount == _nodeCapacity); // The free list is empty. Rebuild a bigger pool. _nodeCapacity *= 2; Array.Resize(ref _nodes, _nodeCapacity); //Fill array with nodes for (int i = _nodeCount; i < _nodeCapacity; i++) { _nodes[i] = new DynamicTreeNode(); } // Build a linked list for the free list. The parent // pointer becomes the "next" pointer. for (int i = _nodeCount; i < _nodeCapacity - 1; ++i) { _nodes[i].Next = i + 1; } _nodes[_nodeCapacity - 1].Next = NullNode; _freeList = _nodeCount; } // Peel a node off the free list. int nodeId = _freeList; _freeList = _nodes[nodeId].Next; _nodes[nodeId].Parent = NullNode; _nodes[nodeId].Child1 = NullNode; _nodes[nodeId].Child2 = NullNode; ++_nodeCount; return(nodeId); }
/// Ray-cast against the proxies in the tree. This relies on the callback /// to perform a exact ray-cast in the case were the proxy contains a shape. /// The callback also performs the any collision filtering. This has performance /// roughly equal to k * log(n), where k is the number of collisions and n is the /// number of proxies in the tree. /// @param input the ray-cast input data. The ray extends from p1 to p1 + maxFraction * (p2 - p1). /// @param callback a callback class that is called for each proxy that is hit by the ray. public void RayCast(IRayCastEnabled callback, RayCastInput input) { Vec2 p1 = input.P1; Vec2 p2 = input.P2; Vec2 r = p2 - p1; Box2DXDebug.Assert(r.LengthSquared() > 0.0f); r.Normalize(); // v is perpendicular to the segment. Vec2 v = Vec2.Cross(1.0f, r); Vec2 abs_v = Math.Abs(v); // Separating axis for segment (Gino, p80). // |dot(v, p1 - c)| > dot(|v|, h) float maxFraction = input.MaxFraction; // Build a bounding box for the segment. AABB segmentAABB = new AABB(); { Vec2 t = p1 + maxFraction * (p2 - p1); segmentAABB.LowerBound = Math.Min(p1, t); segmentAABB.UpperBound = Math.Max(p1, t); } const int k_stackSize = 128; int[] stack = new int[k_stackSize]; int count = 0; stack[count++] = _root; while (count > 0) { int nodeId = stack[--count]; if (nodeId == NullNode) { continue; } DynamicTreeNode node = _nodes[nodeId]; if (Collision.TestOverlap(node.Aabb, segmentAABB) == false) { continue; } // Separating axis for segment (Gino, p80). // |dot(v, p1 - c)| > dot(|v|, h) Vec2 c = node.Aabb.GetCenter(); Vec2 h = node.Aabb.GetExtents(); float separation = Math.Abs(Vec2.Dot(v, p1 - c)) - Vec2.Dot(abs_v, h); if (separation > 0.0f) { continue; } if (node.IsLeaf()) { RayCastInput subInput = new RayCastInput(); subInput.P1 = input.P1; subInput.P2 = input.P2; subInput.MaxFraction = maxFraction; maxFraction = callback.RayCastCallback(subInput, nodeId); if (maxFraction == 0.0f) { return; } // Update segment bounding box. { Vec2 t = p1 + maxFraction * (p2 - p1); segmentAABB.LowerBound = Math.Min(p1, t); segmentAABB.UpperBound = Math.Max(p1, t); } } else { Box2DXDebug.Assert(count + 1 < k_stackSize); stack[count++] = node.Child1; stack[count++] = node.Child2; } } }
private int AllocateNode() { // Expand the node pool as needed. if (_freeList == NullNode) { Box2DXDebug.Assert(_nodeCount == _nodeCapacity); // The free list is empty. Rebuild a bigger pool. _nodeCapacity *= 2; Array.Resize(ref _nodes, _nodeCapacity); //Fill array with nodes for (int i = _nodeCount; i < _nodeCapacity; i++) { _nodes[i] = new DynamicTreeNode(); } // Build a linked list for the free list. The parent // pointer becomes the "next" pointer. for (int i = _nodeCount; i < _nodeCapacity - 1; ++i) { _nodes[i].Next = i + 1; } _nodes[_nodeCapacity - 1].Next = NullNode; _freeList = _nodeCount; } // Peel a node off the free list. int nodeId = _freeList; _freeList = _nodes[nodeId].Next; _nodes[nodeId].Parent = NullNode; _nodes[nodeId].Child1 = NullNode; _nodes[nodeId].Child2 = NullNode; ++_nodeCount; return nodeId; }