File indexing completed on 2024-05-19 14:56:22

 /*
 * Copyright (c) 2009 Erin Catto http://www.box2d.org
 *
 * 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 B2_DYNAMIC_TREE_H
 #define B2_DYNAMIC_TREE_H
 
 #include <Box2D/Collision/b2Collision.h>
 #include <Box2D/Common/b2GrowableStack.h>
 
 #define b2_nullNode (-1)
 
 /// A node in the dynamic tree. The client does not interact with this directly.
 struct b2TreeNode
 {
     bool IsLeaf() const
     {
         return child1 == b2_nullNode;
     }
 
     /// Enlarged AABB
     b2AABB aabb;
 
     void* userData;
 
     union
     {
         int32 parent;
         int32 next;
     };
 
     int32 child1;
     int32 child2;
 
     // leaf = 0, free node = -1
     int32 height;
 };
 
 /// A dynamic AABB tree broad-phase, inspired by Nathanael Presson's btDbvt.
 /// A dynamic tree arranges data in a binary tree to accelerate
 /// queries such as volume queries and ray casts. Leafs are proxies
 /// with an AABB. In the tree we expand the proxy AABB by b2_fatAABBFactor
 /// so that the proxy AABB is bigger than the client object. This allows the client
 /// object to move by small amounts without triggering a tree update.
 ///
 /// Nodes are pooled and relocatable, so we use node indices rather than pointers.
 class b2DynamicTree
 {
 public:
     /// Constructing the tree initializes the node pool.
     b2DynamicTree();
 
     /// Destroy the tree, freeing the node pool.
     ~b2DynamicTree();
 
     /// Create a proxy. Provide a tight fitting AABB and a userData pointer.
     int32 CreateProxy(const b2AABB& aabb, void* userData);
 
     /// Destroy a proxy. This asserts if the id is invalid.
     void DestroyProxy(int32 proxyId);
 
     /// Move a proxy with a swepted AABB. If the proxy has moved outside of its fattened AABB,
     /// then the proxy is removed from the tree and re-inserted. Otherwise
     /// the function returns immediately.
     /// @return true if the proxy was re-inserted.
     bool MoveProxy(int32 proxyId, const b2AABB& aabb1, const b2Vec2& displacement);
 
     /// Get proxy user data.
     /// @return the proxy user data or 0 if the id is invalid.
     void* GetUserData(int32 proxyId) const;
 
     /// Get the fat AABB for a proxy.
     const b2AABB& GetFatAABB(int32 proxyId) const;
 
     /// Query an AABB for overlapping proxies. The callback class
     /// is called for each proxy that overlaps the supplied AABB.
     template <typename T>
     void Query(T* callback, const b2AABB& aabb) const;
 
     /// 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.
     template <typename T>
     void RayCast(T* callback, const b2RayCastInput& input) const;
 
     /// Validate this tree. For testing.
     void Validate() const;
 
     /// Compute the height of the binary tree in O(N) time. Should not be
     /// called often.
     int32 GetHeight() const;
 
     /// Get the maximum balance of an node in the tree. The balance is the difference
     /// in height of the two children of a node.
     int32 GetMaxBalance() const;
 
     /// Get the ratio of the sum of the node areas to the root area.
     float32 GetAreaRatio() const;
 
     /// Build an optimal tree. Very expensive. For testing.
     void RebuildBottomUp();
 
     /// Shift the world origin. Useful for large worlds.
     /// The shift formula is: position -= newOrigin
     /// @param newOrigin the new origin with respect to the old origin
     void ShiftOrigin(const b2Vec2& newOrigin);
 
 private:
 
     int32 AllocateNode();
     void FreeNode(int32 node);
 
     void InsertLeaf(int32 node);
     void RemoveLeaf(int32 node);
 
     int32 Balance(int32 index);
 
     int32 ComputeHeight() const;
     int32 ComputeHeight(int32 nodeId) const;
 
     void ValidateStructure(int32 index) const;
     void ValidateMetrics(int32 index) const;
 
     int32 m_root;
 
     b2TreeNode* m_nodes;
     int32 m_nodeCount;
     int32 m_nodeCapacity;
 
     int32 m_freeList;
 
     /// This is used to incrementally traverse the tree for re-balancing.
     uint32 m_path;
 
     int32 m_insertionCount;
 };
 
 inline void* b2DynamicTree::GetUserData(int32 proxyId) const
 {
     b2Assert(0 <= proxyId && proxyId < m_nodeCapacity);
     return m_nodes[proxyId].userData;
 }
 
 inline const b2AABB& b2DynamicTree::GetFatAABB(int32 proxyId) const
 {
     b2Assert(0 <= proxyId && proxyId < m_nodeCapacity);
     return m_nodes[proxyId].aabb;
 }
 
 template <typename T>
 inline void b2DynamicTree::Query(T* callback, const b2AABB& aabb) const
 {
     b2GrowableStack<int32, 256> stack;
     stack.Push(m_root);
 
     while (stack.GetCount() > 0)
     {
         int32 nodeId = stack.Pop();
         if (nodeId == b2_nullNode)
         {
             continue;
         }
 
         const b2TreeNode* node = m_nodes + nodeId;
 
         if (b2TestOverlap(node->aabb, aabb))
         {
             if (node->IsLeaf())
             {
                 bool proceed = callback->QueryCallback(nodeId);
                 if (proceed == false)
                 {
                     return;
                 }
             }
             else
             {
                 stack.Push(node->child1);
                 stack.Push(node->child2);
             }
         }
     }
 }
 
 template <typename T>
 inline void b2DynamicTree::RayCast(T* callback, const b2RayCastInput& input) const
 {
     b2Vec2 p1 = input.p1;
     b2Vec2 p2 = input.p2;
     b2Vec2 r = p2 - p1;
     b2Assert(r.LengthSquared() > 0.0f);
     r.Normalize();
 
     // v is perpendicular to the segment.
     b2Vec2 v = b2Cross(1.0f, r);
     b2Vec2 abs_v = b2Abs(v);
 
     // Separating axis for segment (Gino, p80).
     // |dot(v, p1 - c)| > dot(|v|, h)
 
     float32 maxFraction = input.maxFraction;
 
     // Build a bounding box for the segment.
     b2AABB segmentAABB;
     {
         b2Vec2 t = p1 + maxFraction * (p2 - p1);
         segmentAABB.lowerBound = b2Min(p1, t);
         segmentAABB.upperBound = b2Max(p1, t);
     }
 
     b2GrowableStack<int32, 256> stack;
     stack.Push(m_root);
 
     while (stack.GetCount() > 0)
     {
         int32 nodeId = stack.Pop();
         if (nodeId == b2_nullNode)
         {
             continue;
         }
 
         const b2TreeNode* node = m_nodes + nodeId;
 
         if (b2TestOverlap(node->aabb, segmentAABB) == false)
         {
             continue;
         }
 
         // Separating axis for segment (Gino, p80).
         // |dot(v, p1 - c)| > dot(|v|, h)
         b2Vec2 c = node->aabb.GetCenter();
         b2Vec2 h = node->aabb.GetExtents();
         float32 separation = b2Abs(b2Dot(v, p1 - c)) - b2Dot(abs_v, h);
         if (separation > 0.0f)
         {
             continue;
         }
 
         if (node->IsLeaf())
         {
             b2RayCastInput subInput;
             subInput.p1 = input.p1;
             subInput.p2 = input.p2;
             subInput.maxFraction = maxFraction;
 
             float32 value = callback->RayCastCallback(subInput, nodeId);
 
             if (value == 0.0f)
             {
                 // The client has terminated the ray cast.
                 return;
             }
 
             if (value > 0.0f)
             {
                 // Update segment bounding box.
                 maxFraction = value;
                 b2Vec2 t = p1 + maxFraction * (p2 - p1);
                 segmentAABB.lowerBound = b2Min(p1, t);
                 segmentAABB.upperBound = b2Max(p1, t);
             }
         }
         else
         {
             stack.Push(node->child1);
             stack.Push(node->child2);
         }
     }
 }
 
 #endif