DynamicTree.hpp

Go to the documentation of this file.

/*
 * Original work Copyright (c) 2009 Erin Catto http://www.box2d.org
 * Modified work Copyright (c) 2017 Louis Langholtz https://github.com/louis-langholtz/PlayRho
 *
 * 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 PLAYRHO_COLLISION_DYNAMICTREE_HPP
#define PLAYRHO_COLLISION_DYNAMICTREE_HPP
 
 
#include <PlayRho/Collision/AABB.hpp>
#include <PlayRho/Common/Settings.hpp>
 
#include <functional>
#include <type_traits>
#include <utility>
 
namespace playrho {
namespace d2 {
 
class Fixture;
class Body;
 
class DynamicTree
{
public:
    using Size = ContactCounter;
    
    class TreeNode;
    struct UnusedData;
    struct BranchData;
    struct LeafData;
    union VariantData;
    
    static PLAYRHO_CONSTEXPR inline Size GetInvalidSize() noexcept
    {
        return static_cast<Size>(-1);
    }
    
    using Height = ContactCounter;
    
    static PLAYRHO_CONSTEXPR const auto InvalidHeight = static_cast<Height>(-1);
 
    static PLAYRHO_CONSTEXPR inline Height GetInvalidHeight() noexcept
    {
        return InvalidHeight;
    }
    
    static PLAYRHO_CONSTEXPR inline bool IsUnused(Height value) noexcept
    {
        return value == GetInvalidHeight();
    }
    
    static PLAYRHO_CONSTEXPR inline bool IsLeaf(Height value) noexcept
    {
        return value == 0;
    }
    
    static PLAYRHO_CONSTEXPR inline bool IsBranch(Height value) noexcept
    {
        return !IsUnused(value) && !IsLeaf(value);
    }
 
    static PLAYRHO_CONSTEXPR inline Size GetDefaultInitialNodeCapacity() noexcept;
    
    DynamicTree() noexcept;
    
    explicit DynamicTree(Size nodeCapacity);
 
    ~DynamicTree() noexcept;
 
    DynamicTree(const DynamicTree& other);
    
    DynamicTree(DynamicTree&& other) noexcept;
 
    DynamicTree& operator= (DynamicTree other) noexcept;
    
    Size CreateLeaf(const AABB& aabb, const LeafData& data);
 
    void DestroyLeaf(Size index) noexcept;
 
    void UpdateLeaf(Size index, const AABB& aabb);
 
    LeafData GetLeafData(Size index) const noexcept;
 
    void SetLeafData(Size index, LeafData value) noexcept;
 
    AABB GetAABB(Size index) const noexcept;
 
    Height GetHeight(Size index) const noexcept;
    
    Size GetOther(Size index) const noexcept;
    
    BranchData GetBranchData(Size index) const noexcept;
 
    Size GetRootIndex() const noexcept;
 
    Size GetFreeIndex() const noexcept;
 
    void RebuildBottomUp();
 
    void ShiftOrigin(Length2 newOrigin);
 
    Size GetNodeCapacity() const noexcept;
 
    Size GetNodeCount() const noexcept;
    
    Size GetLeafCount() const noexcept;
    
    Size FindReference(Size index) const noexcept;
 
    friend void swap(DynamicTree& lhs, DynamicTree& rhs) noexcept;
    
private:
    
    void SetNodeCapacity(Size value) noexcept;
 
    Size AllocateNode() noexcept;
 
    Size AllocateNode(const LeafData& data, AABB aabb) noexcept;
 
    Size AllocateNode(const BranchData& data, AABB aabb, Height height,
                      Size parent = GetInvalidSize()) noexcept;
 
    void FreeNode(Size index) noexcept;
    
    TreeNode* m_nodes{nullptr}; 
    Size m_rootIndex{GetInvalidSize()}; 
    Size m_freeIndex{GetInvalidSize()}; 
    Size m_nodeCount{0u}; 
    Size m_nodeCapacity{0u}; 
    Size m_leafCount{0u}; 
};
 
struct DynamicTree::UnusedData
{
    // Intentionally empty.
};
 
struct DynamicTree::BranchData
{
    Size child1; 
    Size child2; 
};
 
struct DynamicTree::LeafData
{
    // In terms of what needs to be in this structure, it minimally needs to have enough
    // information in it to identify the child shape for which the node's AABB represents,
    // and its associated body. A pointer to the fixture and the index of the child in
    // its shape could suffice for this. Meanwhile, a Contact is defined to be the
    // recognition of an overlap between two child shapes having different bodies making
    // the caching of the bodies a potential speed-up opportunity.
 
    Body* body;
    
    Fixture* fixture;
 
    ChildCounter childIndex;
};
 
PLAYRHO_CONSTEXPR inline bool operator== (const DynamicTree::LeafData& lhs,
                                          const DynamicTree::LeafData& rhs) noexcept
{
    return lhs.fixture == rhs.fixture && lhs.childIndex == rhs.childIndex;
}
 
PLAYRHO_CONSTEXPR inline bool operator!= (const DynamicTree::LeafData& lhs,
                                          const DynamicTree::LeafData& rhs) noexcept
{
    return !(lhs == rhs);
}
 
union DynamicTree::VariantData
{
    UnusedData unused;
    
    LeafData leaf;
    
    BranchData branch;
    
    VariantData() noexcept = default;
    
    PLAYRHO_CONSTEXPR inline VariantData(UnusedData value) noexcept: unused{value} {}
    
    PLAYRHO_CONSTEXPR inline VariantData(LeafData value) noexcept: leaf{value} {}
    
    PLAYRHO_CONSTEXPR inline VariantData(BranchData value) noexcept: branch{value} {}
};
 
PLAYRHO_CONSTEXPR inline bool IsUnused(const DynamicTree::TreeNode& node) noexcept;
 
PLAYRHO_CONSTEXPR inline bool IsLeaf(const DynamicTree::TreeNode& node) noexcept;
 
PLAYRHO_CONSTEXPR inline bool IsBranch(const DynamicTree::TreeNode& node) noexcept;
 
class DynamicTree::TreeNode
{
public:
    ~TreeNode() = default;
    
    PLAYRHO_CONSTEXPR inline TreeNode(const TreeNode& other) = default;
 
    PLAYRHO_CONSTEXPR inline TreeNode(TreeNode&& other) = default;
 
    PLAYRHO_CONSTEXPR inline explicit TreeNode(Size other = DynamicTree::GetInvalidSize()) noexcept:
        m_other{other}
    {
        assert(IsUnused(m_height));
    }
 
    PLAYRHO_CONSTEXPR inline TreeNode(const LeafData& value, AABB aabb,
                                      Size other = DynamicTree::GetInvalidSize()) noexcept:
        m_height{0}, m_other{other}, m_aabb{aabb}, m_variant{value}
    {
        // Intentionally empty.
    }
    
    PLAYRHO_CONSTEXPR inline TreeNode(const BranchData& value, AABB aabb, Height height,
                       Size other = DynamicTree::GetInvalidSize()) noexcept:
        m_height{height}, m_other{other}, m_aabb{aabb}, m_variant{value}
    {
        assert(IsBranch(height));
        assert(value.child1 != GetInvalidSize());
        assert(value.child2 != GetInvalidSize());
    }
    
    TreeNode& operator= (const TreeNode& other) = default;
    
    PLAYRHO_CONSTEXPR inline Height GetHeight() const noexcept
    {
        return m_height;
    }
    
    PLAYRHO_CONSTEXPR inline Size GetOther() const noexcept
    {
        return m_other;
    }
                
    PLAYRHO_CONSTEXPR inline void SetOther(Size other) noexcept
    {
        m_other = other;
    }
 
    PLAYRHO_CONSTEXPR inline AABB GetAABB() const noexcept
    {
        assert(!IsUnused(m_height));
        return m_aabb;
    }
 
    PLAYRHO_CONSTEXPR inline void SetAABB(AABB value) noexcept
    {
        assert(!IsUnused(m_height));
        m_aabb = value;
    }
    
    PLAYRHO_CONSTEXPR inline UnusedData AsUnused() const noexcept
    {
        assert(IsUnused(m_height));
        return m_variant.unused;
    }
    
    PLAYRHO_CONSTEXPR inline LeafData AsLeaf() const noexcept
    {
        assert(IsLeaf(m_height));
        return m_variant.leaf;
    }
    
    PLAYRHO_CONSTEXPR inline BranchData AsBranch() const noexcept
    {
        assert(IsBranch(m_height));
        return m_variant.branch;
    }
 
    PLAYRHO_CONSTEXPR inline void Assign(const UnusedData& v) noexcept
    {
        m_variant.unused = v;
        m_height = static_cast<Height>(-1);
    }
    
    PLAYRHO_CONSTEXPR inline void Assign(const LeafData& v) noexcept
    {
        m_variant.leaf = v;
        m_height = 0;
    }
    
    PLAYRHO_CONSTEXPR inline void Assign(const BranchData& v, const AABB& bb, Height h) noexcept
    {
        assert(v.child1 != GetInvalidSize());
        assert(v.child2 != GetInvalidSize());
        assert(IsBranch(h));
        m_variant.branch = v;
        m_aabb = bb;
        m_height = h;
    }
 
private:
    Height m_height = GetInvalidHeight();
 
    Size m_other = DynamicTree::GetInvalidSize(); 
    
    AABB m_aabb;
    
    VariantData m_variant{UnusedData{}};
};
 
PLAYRHO_CONSTEXPR inline DynamicTree::Size DynamicTree::GetDefaultInitialNodeCapacity() noexcept
{
    return Size{64};
}
 
inline DynamicTree::Size DynamicTree::GetRootIndex() const noexcept
{
    assert((m_rootIndex == GetInvalidSize() && (m_leafCount == 0)) ||
           ((m_rootIndex < m_nodeCapacity) && (m_leafCount > 0) && (GetOther(m_rootIndex) == GetInvalidSize())));
    return m_rootIndex;
}
 
inline DynamicTree::Size DynamicTree::GetFreeIndex() const noexcept
{
    return m_freeIndex;
}
 
inline DynamicTree::Size DynamicTree::GetNodeCapacity() const noexcept
{
    return m_nodeCapacity;
}
 
inline DynamicTree::Size DynamicTree::GetNodeCount() const noexcept
{
    return m_nodeCount;
}
 
inline DynamicTree::Size DynamicTree::GetLeafCount() const noexcept
{
    assert(((m_leafCount == 0) && (m_rootIndex == GetInvalidSize())) ||
           ((m_leafCount > 0) && (m_rootIndex != GetInvalidSize()) && (GetOther(m_rootIndex) == GetInvalidSize())));
    return m_leafCount;
}
 
inline DynamicTree::Height DynamicTree::GetHeight(Size index) const noexcept
{
    assert(index != GetInvalidSize());
    assert(index < m_nodeCapacity);
    return m_nodes[index].GetHeight();
}
 
inline DynamicTree::Size DynamicTree::GetOther(Size index) const noexcept
{
    assert(index != GetInvalidSize());
    assert(index < m_nodeCapacity);
    return m_nodes[index].GetOther();
}
 
inline AABB DynamicTree::GetAABB(Size index) const noexcept
{
    assert(index != GetInvalidSize());
    assert(index < m_nodeCapacity);
    assert(!IsUnused(m_nodes[index].GetHeight()));
    return m_nodes[index].GetAABB();
}
 
inline DynamicTree::BranchData DynamicTree::GetBranchData(Size index) const noexcept
{
    assert(index != GetInvalidSize());
    assert(index < m_nodeCapacity);
    assert(IsBranch(m_nodes[index].GetHeight()));
    return m_nodes[index].AsBranch();
}
 
inline DynamicTree::LeafData DynamicTree::GetLeafData(Size index) const noexcept
{
    assert(index != GetInvalidSize());
    assert(index < m_nodeCapacity);
    assert(IsLeaf(m_nodes[index].GetHeight()));
    return m_nodes[index].AsLeaf();
}
 
inline void DynamicTree::SetLeafData(Size index, LeafData value) noexcept
{
    assert(index != GetInvalidSize());
    assert(index < m_nodeCapacity);
    assert(IsLeaf(m_nodes[index].GetHeight()));
    m_nodes[index].AsLeaf() = value;
}
 
// Free functions...
 
PLAYRHO_CONSTEXPR inline DynamicTree::BranchData
ReplaceChild(DynamicTree::BranchData bd, DynamicTree::Size oldChild, DynamicTree::Size newChild)
{
    assert(bd.child1 == oldChild || bd.child2 == oldChild);
    return (bd.child1 == oldChild)?
        DynamicTree::BranchData{newChild, bd.child2}: DynamicTree::BranchData{bd.child1, newChild};
}
 
PLAYRHO_CONSTEXPR inline bool IsUnused(const DynamicTree::TreeNode& node) noexcept
{
    return DynamicTree::IsUnused(node.GetHeight());
}
 
PLAYRHO_CONSTEXPR inline bool IsLeaf(const DynamicTree::TreeNode& node) noexcept
{
    return DynamicTree::IsLeaf(node.GetHeight());
}
 
PLAYRHO_CONSTEXPR inline bool IsBranch(const DynamicTree::TreeNode& node) noexcept
{
    return DynamicTree::IsBranch(node.GetHeight());
}
 
PLAYRHO_CONSTEXPR inline AABB GetAABB(const DynamicTree::TreeNode& node) noexcept
{
    assert(!IsUnused(node));
    return node.GetAABB();
}
 
PLAYRHO_CONSTEXPR inline DynamicTree::Size GetNext(const DynamicTree::TreeNode& node) noexcept
{
    assert(IsUnused(node));
    return node.GetOther();
}
 
inline DynamicTree::Height GetHeight(const DynamicTree& tree) noexcept
{
    const auto index = tree.GetRootIndex();
    return (index != DynamicTree::GetInvalidSize())? tree.GetHeight(index): DynamicTree::Height{0};
}
 
inline AABB GetAABB(const DynamicTree& tree) noexcept
{
    const auto index = tree.GetRootIndex();
    return (index != DynamicTree::GetInvalidSize())? tree.GetAABB(index): AABB{};
}
 
inline bool TestOverlap(const DynamicTree& tree,
                        DynamicTree::Size leafIdA, DynamicTree::Size leafIdB) noexcept
{
    return TestOverlap(tree.GetAABB(leafIdA), tree.GetAABB(leafIdB));
}
 
Length ComputeTotalPerimeter(const DynamicTree& tree) noexcept;
 
Real ComputePerimeterRatio(const DynamicTree& tree) noexcept;
 
DynamicTree::Height ComputeHeight(const DynamicTree& tree, DynamicTree::Size index) noexcept;
 
inline DynamicTree::Height ComputeHeight(const DynamicTree& tree) noexcept
{
    return ComputeHeight(tree, tree.GetRootIndex());
}
 
bool ValidateStructure(const DynamicTree& tree, DynamicTree::Size index) noexcept;
 
bool ValidateMetrics(const DynamicTree& tree, DynamicTree::Size index) noexcept;
 
DynamicTree::Height GetMaxImbalance(const DynamicTree& tree) noexcept;
 
enum class DynamicTreeOpcode
{
    End,
    Continue,
};
 
using DynamicTreeSizeCB = std::function<DynamicTreeOpcode(DynamicTree::Size)>;
 
void Query(const DynamicTree& tree, const AABB& aabb,
           const DynamicTreeSizeCB& callback);
 
using QueryFixtureCallback = std::function<bool(Fixture* fixture, ChildCounter child)>;
 
void Query(const DynamicTree& tree, const AABB& aabb, QueryFixtureCallback callback);
 
inline std::size_t size(const DynamicTree& tree) noexcept
{
    return tree.GetLeafCount();
}
 
} // namespace d2
} // namespace playrho
 
#endif // PLAYRHO_COLLISION_DYNAMICTREE_HPP