axmol/Box2D/Collision/b2DynamicTree.h

/*
* 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();

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