b2_dynamic_tree.h

Go to the documentation of this file.

// MIT License
 
// Copyright (c) 2019 Erin Catto
 
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
 
// The above copyright notice and this permission notice shall be included in all
// copies or substantial portions of the Software.
 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
 
#ifndef B2_DYNAMIC_TREE_H
#define B2_DYNAMIC_TREE_H
 
#include "b2_api.h"
#include "b2_collision.h"
#include "b2_growable_stack.h"
 
#define b2_nullNode (-1)
 
struct B2_API b2TreeNode
{
        bool IsLeaf() const
        {
                return child1 == b2_nullNode;
        }
 
        b2AABB aabb;
 
        void* userData;
 
        union
        {
                int32 parent;
                int32 next;
        };
 
        int32 child1;
        int32 child2;
 
        // leaf = 0, free node = -1
        int32 height;
 
        bool moved;
};
 
class B2_API b2DynamicTree
{
public:
        b2DynamicTree();
 
        ~b2DynamicTree();
 
        int32 CreateProxy(const b2AABB& aabb, void* userData);
 
        void DestroyProxy(int32 proxyId);
 
        bool MoveProxy(int32 proxyId, const b2AABB& aabb1, const b2Vec2& displacement);
 
        void* GetUserData(int32 proxyId) const;
 
        bool WasMoved(int32 proxyId) const;
        void ClearMoved(int32 proxyId);
 
        const b2AABB& GetFatAABB(int32 proxyId) const;
 
        template <typename T>
        void Query(T* callback, const b2AABB& aabb) const;
 
        template <typename T>
        void RayCast(T* callback, const b2RayCastInput& input) const;
 
        void Validate() const;
 
        int32 GetHeight() const;
 
        int32 GetMaxBalance() const;
 
        float GetAreaRatio() const;
 
        void RebuildBottomUp();
 
        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;
 
        int32 m_insertionCount;
};
 
inline void* b2DynamicTree::GetUserData(int32 proxyId) const
{
        b2Assert(0 <= proxyId && proxyId < m_nodeCapacity);
        return m_nodes[proxyId].userData;
}
 
inline bool b2DynamicTree::WasMoved(int32 proxyId) const
{
        b2Assert(0 <= proxyId && proxyId < m_nodeCapacity);
        return m_nodes[proxyId].moved;
}
 
inline void b2DynamicTree::ClearMoved(int32 proxyId)
{
        b2Assert(0 <= proxyId && proxyId < m_nodeCapacity);
        m_nodes[proxyId].moved = false;
}
 
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)
 
        float 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();
                float 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;
 
                        float 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