1/*
2* Copyright (c) 2009 Erin Catto http://www.box2d.org
3*
4* This software is provided 'as-is', without any express or implied
5* warranty. In no event will the authors be held liable for any damages
6* arising from the use of this software.
7* Permission is granted to anyone to use this software for any purpose,
8* including commercial applications, and to alter it and redistribute it
9* freely, subject to the following restrictions:
10* 1. The origin of this software must not be misrepresented; you must not
11* claim that you wrote the original software. If you use this software
12* in a product, an acknowledgment in the product documentation would be
13* appreciated but is not required.
14* 2. Altered source versions must be plainly marked as such, and must not be
15* misrepresented as being the original software.
16* 3. This notice may not be removed or altered from any source distribution.
17*/
18
19#ifndef B2_DYNAMIC_TREE_H
20#define B2_DYNAMIC_TREE_H
21
22#include <Box2D/Collision/b2Collision.h>
23#include <Box2D/Common/b2GrowableStack.h>
24
25#define b2_nullNode (-1)
26
27/// A node in the dynamic tree. The client does not interact with this directly.
28struct b2TreeNode
29{
30 bool IsLeaf() const
31 {
32 return child1 == b2_nullNode;
33 }
34
35 /// Enlarged AABB
36 b2AABB aabb;
37
38 void* userData;
39
40 union
41 {
42 int32 parent;
43 int32 next;
44 };
45
46 int32 child1;
47 int32 child2;
48
49 // leaf = 0, free node = -1
50 int32 height;
51};
52
53/// A dynamic AABB tree broad-phase, inspired by Nathanael Presson's btDbvt.
54/// A dynamic tree arranges data in a binary tree to accelerate
55/// queries such as volume queries and ray casts. Leafs are proxies
56/// with an AABB. In the tree we expand the proxy AABB by b2_fatAABBFactor
57/// so that the proxy AABB is bigger than the client object. This allows the client
58/// object to move by small amounts without triggering a tree update.
59///
60/// Nodes are pooled and relocatable, so we use node indices rather than pointers.
61class b2DynamicTree
62{
63public:
64 /// Constructing the tree initializes the node pool.
65 b2DynamicTree();
66
67 /// Destroy the tree, freeing the node pool.
68 ~b2DynamicTree();
69
70 /// Create a proxy. Provide a tight fitting AABB and a userData pointer.
71 int32 CreateProxy(const b2AABB& aabb, void* userData);
72
73 /// Destroy a proxy. This asserts if the id is invalid.
74 void DestroyProxy(int32 proxyId);
75
76 /// Move a proxy with a swepted AABB. If the proxy has moved outside of its fattened AABB,
77 /// then the proxy is removed from the tree and re-inserted. Otherwise
78 /// the function returns immediately.
79 /// @return true if the proxy was re-inserted.
80 bool MoveProxy(int32 proxyId, const b2AABB& aabb1, const b2Vec2& displacement);
81
82 /// Get proxy user data.
83 /// @return the proxy user data or 0 if the id is invalid.
84 void* GetUserData(int32 proxyId) const;
85
86 /// Get the fat AABB for a proxy.
87 const b2AABB& GetFatAABB(int32 proxyId) const;
88
89 /// Query an AABB for overlapping proxies. The callback class
90 /// is called for each proxy that overlaps the supplied AABB.
91 template <typename T>
92 void Query(T* callback, const b2AABB& aabb) const;
93
94 /// Ray-cast against the proxies in the tree. This relies on the callback
95 /// to perform a exact ray-cast in the case were the proxy contains a shape.
96 /// The callback also performs the any collision filtering. This has performance
97 /// roughly equal to k * log(n), where k is the number of collisions and n is the
98 /// number of proxies in the tree.
99 /// @param input the ray-cast input data. The ray extends from p1 to p1 + maxFraction * (p2 - p1).
100 /// @param callback a callback class that is called for each proxy that is hit by the ray.
101 template <typename T>
102 void RayCast(T* callback, const b2RayCastInput& input) const;
103
104 /// Validate this tree. For testing.
105 void Validate() const;
106
107 /// Compute the height of the binary tree in O(N) time. Should not be
108 /// called often.
109 int32 GetHeight() const;
110
111 /// Get the maximum balance of an node in the tree. The balance is the difference
112 /// in height of the two children of a node.
113 int32 GetMaxBalance() const;
114
115 /// Get the ratio of the sum of the node areas to the root area.
116 float32 GetAreaRatio() const;
117
118 /// Build an optimal tree. Very expensive. For testing.
119 void RebuildBottomUp();
120
121 /// Shift the world origin. Useful for large worlds.
122 /// The shift formula is: position -= newOrigin
123 /// @param newOrigin the new origin with respect to the old origin
124 void ShiftOrigin(const b2Vec2& newOrigin);
125
126private:
127
128 int32 AllocateNode();
129 void FreeNode(int32 node);
130
131 void InsertLeaf(int32 node);
132 void RemoveLeaf(int32 node);
133
134 int32 Balance(int32 index);
135
136 int32 ComputeHeight() const;
137 int32 ComputeHeight(int32 nodeId) const;
138
139 void ValidateStructure(int32 index) const;
140 void ValidateMetrics(int32 index) const;
141
142 int32 m_root;
143
144 b2TreeNode* m_nodes;
145 int32 m_nodeCount;
146 int32 m_nodeCapacity;
147
148 int32 m_freeList;
149
150 /// This is used to incrementally traverse the tree for re-balancing.
151 uint32 m_path;
152
153 int32 m_insertionCount;
154};
155
156inline void* b2DynamicTree::GetUserData(int32 proxyId) const
157{
158 b2Assert(0 <= proxyId && proxyId < m_nodeCapacity);
159 return m_nodes[proxyId].userData;
160}
161
162inline const b2AABB& b2DynamicTree::GetFatAABB(int32 proxyId) const
163{
164 b2Assert(0 <= proxyId && proxyId < m_nodeCapacity);
165 return m_nodes[proxyId].aabb;
166}
167
168template <typename T>
169inline void b2DynamicTree::Query(T* callback, const b2AABB& aabb) const
170{
171 b2GrowableStack<int32, 256> stack;
172 stack.Push(m_root);
173
174 while (stack.GetCount() > 0)
175 {
176 int32 nodeId = stack.Pop();
177 if (nodeId == b2_nullNode)
178 {
179 continue;
180 }
181
182 const b2TreeNode* node = m_nodes + nodeId;
183
184 if (b2TestOverlap(node->aabb, aabb))
185 {
186 if (node->IsLeaf())
187 {
188 bool proceed = callback->QueryCallback(nodeId);
189 if (proceed == false)
190 {
191 return;
192 }
193 }
194 else
195 {
196 stack.Push(node->child1);
197 stack.Push(node->child2);
198 }
199 }
200 }
201}
202
203template <typename T>
204inline void b2DynamicTree::RayCast(T* callback, const b2RayCastInput& input) const
205{
206 b2Vec2 p1 = input.p1;
207 b2Vec2 p2 = input.p2;
208 b2Vec2 r = p2 - p1;
209 b2Assert(r.LengthSquared() > 0.0f);
210 r.Normalize();
211
212 // v is perpendicular to the segment.
213 b2Vec2 v = b2Cross(1.0f, r);
214 b2Vec2 abs_v = b2Abs(v);
215
216 // Separating axis for segment (Gino, p80).
217 // |dot(v, p1 - c)| > dot(|v|, h)
218
219 float32 maxFraction = input.maxFraction;
220
221 // Build a bounding box for the segment.
222 b2AABB segmentAABB;
223 {
224 b2Vec2 t = p1 + maxFraction * (p2 - p1);
225 segmentAABB.lowerBound = b2Min(p1, t);
226 segmentAABB.upperBound = b2Max(p1, t);
227 }
228
229 b2GrowableStack<int32, 256> stack;
230 stack.Push(m_root);
231
232 while (stack.GetCount() > 0)
233 {
234 int32 nodeId = stack.Pop();
235 if (nodeId == b2_nullNode)
236 {
237 continue;
238 }
239
240 const b2TreeNode* node = m_nodes + nodeId;
241
242 if (b2TestOverlap(node->aabb, segmentAABB) == false)
243 {
244 continue;
245 }
246
247 // Separating axis for segment (Gino, p80).
248 // |dot(v, p1 - c)| > dot(|v|, h)
249 b2Vec2 c = node->aabb.GetCenter();
250 b2Vec2 h = node->aabb.GetExtents();
251 float32 separation = b2Abs(b2Dot(v, p1 - c)) - b2Dot(abs_v, h);
252 if (separation > 0.0f)
253 {
254 continue;
255 }
256
257 if (node->IsLeaf())
258 {
259 b2RayCastInput subInput;
260 subInput.p1 = input.p1;
261 subInput.p2 = input.p2;
262 subInput.maxFraction = maxFraction;
263
264 float32 value = callback->RayCastCallback(subInput, nodeId);
265
266 if (value == 0.0f)
267 {
268 // The client has terminated the ray cast.
269 return;
270 }
271
272 if (value > 0.0f)
273 {
274 // Update segment bounding box.
275 maxFraction = value;
276 b2Vec2 t = p1 + maxFraction * (p2 - p1);
277 segmentAABB.lowerBound = b2Min(p1, t);
278 segmentAABB.upperBound = b2Max(p1, t);
279 }
280 }
281 else
282 {
283 stack.Push(node->child1);
284 stack.Push(node->child2);
285 }
286 }
287}
288
289#endif
290