diff options
Diffstat (limited to 'Client/ThirdParty/Box2D/include/box2d/b2_dynamic_tree.h')
| -rw-r--r-- | Client/ThirdParty/Box2D/include/box2d/b2_dynamic_tree.h | 308 |
1 files changed, 308 insertions, 0 deletions
diff --git a/Client/ThirdParty/Box2D/include/box2d/b2_dynamic_tree.h b/Client/ThirdParty/Box2D/include/box2d/b2_dynamic_tree.h new file mode 100644 index 0000000..b854919 --- /dev/null +++ b/Client/ThirdParty/Box2D/include/box2d/b2_dynamic_tree.h @@ -0,0 +1,308 @@ +// 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) + +/// A node in the dynamic tree. The client does not interact with this directly. +struct B2_API 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; + + bool moved; +}; + +/// 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 B2_API 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; + + bool WasMoved(int32 proxyId) const; + void ClearMoved(int32 proxyId); + + /// 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. + float 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; + + 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 |