Box2D: /build/box2d/src/box2d-adaedf1772ede7420bc9ee569c8cba7952db91a0/include/box2d/b2_dynamic_tree.h Source File

| Box2D 2.4.0 A 2D physics engine for games

b2_dynamic_tree.h
1 // MIT License
2
3 // Copyright (c) 2019 Erin Catto
4
5 // Permission is hereby granted, free of charge, to any person obtaining a copy
6 // of this software and associated documentation files (the "Software"), to deal
7 // in the Software without restriction, including without limitation the rights
8 // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
9 // copies of the Software, and to permit persons to whom the Software is
10 // furnished to do so, subject to the following conditions:
11
12 // The above copyright notice and this permission notice shall be included in all
13 // copies or substantial portions of the Software.
14
15 // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
16 // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
17 // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
18 // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
19 // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
20 // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
21 // SOFTWARE.
22
23 #ifndef B2_DYNAMIC_TREE_H
24 #define B2_DYNAMIC_TREE_H
25
26 #include "b2_api.h"
27 #include "b2_collision.h"
28 #include "b2_growable_stack.h"
29
30 #define b2_nullNode (-1)
31
33 struct B2_API b2TreeNode
34 {
35 bool IsLeaf() const
36 {
37 return child1 == b2_nullNode;
38 }
39
41 b2AABB aabb;
42
43 void* userData;
44
45 union
46 {
47 int32 parent;
48 int32 next;
49 };
50
51 int32 child1;
52 int32 child2;
53
54 // leaf = 0, free node = -1
55 int32 height;
56
57 bool moved;
58 };
59
68 class B2_API b2DynamicTree
69 {
70 public:
72 b2DynamicTree();
73
75 ~b2DynamicTree();
76
78 int32 CreateProxy(const b2AABB& aabb, void* userData);
79
81 void DestroyProxy(int32 proxyId);
82
87 bool MoveProxy(int32 proxyId, const b2AABB& aabb1, const b2Vec2& displacement);
88
91 void* GetUserData(int32 proxyId) const;
92
93 bool WasMoved(int32 proxyId) const;
94 void ClearMoved(int32 proxyId);
95
97 const b2AABB& GetFatAABB(int32 proxyId) const;
98
101 template <typename T>
102 void Query(T* callback, const b2AABB& aabb) const;
103
111 template <typename T>
112 void RayCast(T* callback, const b2RayCastInput& input) const;
113
115 void Validate() const;
116
119 int32 GetHeight() const;
120
123 int32 GetMaxBalance() const;
124
126 float GetAreaRatio() const;
127
129 void RebuildBottomUp();
130
134 void ShiftOrigin(const b2Vec2& newOrigin);
135
136 private:
137
138 int32 AllocateNode();
139 void FreeNode(int32 node);
140
141 void InsertLeaf(int32 node);
142 void RemoveLeaf(int32 node);
143
144 int32 Balance(int32 index);
145
146 int32 ComputeHeight() const;
147 int32 ComputeHeight(int32 nodeId) const;
148
149 void ValidateStructure(int32 index) const;
150 void ValidateMetrics(int32 index) const;
151
152 int32 m_root;
153
154 b2TreeNode* m_nodes;
155 int32 m_nodeCount;
156 int32 m_nodeCapacity;
157
158 int32 m_freeList;
159
161 uint32 m_path;
162
163 int32 m_insertionCount;
164 };
165
166 inline void* b2DynamicTree::GetUserData(int32 proxyId) const
167 {
168 b2Assert(0 <= proxyId && proxyId < m_nodeCapacity);
169 return m_nodes[proxyId].userData;
170 }
171
172 inline bool b2DynamicTree::WasMoved(int32 proxyId) const
173 {
174 b2Assert(0 <= proxyId && proxyId < m_nodeCapacity);
175 return m_nodes[proxyId].moved;
176 }
177
178 inline void b2DynamicTree::ClearMoved(int32 proxyId)
179 {
180 b2Assert(0 <= proxyId && proxyId < m_nodeCapacity);
181 m_nodes[proxyId].moved = false;
182 }
183
184 inline const b2AABB& b2DynamicTree::GetFatAABB(int32 proxyId) const
185 {
186 b2Assert(0 <= proxyId && proxyId < m_nodeCapacity);
187 return m_nodes[proxyId].aabb;
188 }
189
190 template <typename T>
191 inline void b2DynamicTree::Query(T* callback, const b2AABB& aabb) const
192 {
193 b2GrowableStack<int32, 256> stack;
194 stack.Push(m_root);
195
196 while (stack.GetCount() > 0)
197 {
198 int32 nodeId = stack.Pop();
199 if (nodeId == b2_nullNode)
200 {
201 continue;
202 }
203
204 const b2TreeNode* node = m_nodes + nodeId;
205
206 if (b2TestOverlap(node->aabb, aabb))
207 {
208 if (node->IsLeaf())
209 {
210 bool proceed = callback->QueryCallback(nodeId);
211 if (proceed == false)
212 {
213 return;
214 }
215 }
216 else
217 {
218 stack.Push(node->child1);
219 stack.Push(node->child2);
220 }
221 }
222 }
223 }
224
225 template <typename T>
226 inline void b2DynamicTree::RayCast(T* callback, const b2RayCastInput& input) const
227 {
228 b2Vec2 p1 = input.p1;
229 b2Vec2 p2 = input.p2;
230 b2Vec2 r = p2 - p1;
231 b2Assert(r.LengthSquared() > 0.0f);
232 r.Normalize();
233
234 // v is perpendicular to the segment.
235 b2Vec2 v = b2Cross(1.0f, r);
236 b2Vec2 abs_v = b2Abs(v);
237
238 // Separating axis for segment (Gino, p80).
239 // |dot(v, p1 - c)| > dot(|v|, h)
240
241 float maxFraction = input.maxFraction;
242
243 // Build a bounding box for the segment.
244 b2AABB segmentAABB;
245 {
246 b2Vec2 t = p1 + maxFraction * (p2 - p1);
247 segmentAABB.lowerBound = b2Min(p1, t);
248 segmentAABB.upperBound = b2Max(p1, t);
249 }
250
251 b2GrowableStack<int32, 256> stack;
252 stack.Push(m_root);
253
254 while (stack.GetCount() > 0)
255 {
256 int32 nodeId = stack.Pop();
257 if (nodeId == b2_nullNode)
258 {
259 continue;
260 }
261
262 const b2TreeNode* node = m_nodes + nodeId;
263
264 if (b2TestOverlap(node->aabb, segmentAABB) == false)
265 {
266 continue;
267 }
268
269 // Separating axis for segment (Gino, p80).
270 // |dot(v, p1 - c)| > dot(|v|, h)
271 b2Vec2 c = node->aabb.GetCenter();
272 b2Vec2 h = node->aabb.GetExtents();
273 float separation = b2Abs(b2Dot(v, p1 - c)) - b2Dot(abs_v, h);
274 if (separation > 0.0f)
275 {
276 continue;
277 }
278
279 if (node->IsLeaf())
280 {
281 b2RayCastInput subInput;
282 subInput.p1 = input.p1;
283 subInput.p2 = input.p2;
284 subInput.maxFraction = maxFraction;
285
286 float value = callback->RayCastCallback(subInput, nodeId);
287
288 if (value == 0.0f)
289 {
290 // The client has terminated the ray cast.
291 return;
292 }
293
294 if (value > 0.0f)
295 {
296 // Update segment bounding box.
297 maxFraction = value;
298 b2Vec2 t = p1 + maxFraction * (p2 - p1);
299 segmentAABB.lowerBound = b2Min(p1, t);
300 segmentAABB.upperBound = b2Max(p1, t);
301 }
302 }
303 else
304 {
305 stack.Push(node->child1);
306 stack.Push(node->child2);
307 }
308 }
309 }
310
311 #endif
b2Vec2
A 2D column vector.
Definition: b2_math.h:42
b2DynamicTree
Definition: b2_dynamic_tree.h:69
b2AABB::GetExtents
b2Vec2 GetExtents() const
Get the extents of the AABB (half-widths).
Definition: b2_collision.h:180
b2DynamicTree::GetFatAABB
const b2AABB & GetFatAABB(int32 proxyId) const
Get the fat AABB for a proxy.
Definition: b2_dynamic_tree.h:184
b2TreeNode
A node in the dynamic tree. The client does not interact with this directly.
Definition: b2_dynamic_tree.h:34
b2AABB::lowerBound
b2Vec2 lowerBound
the lower vertex
Definition: b2_collision.h:220
b2RayCastInput
Ray-cast input data. The ray extends from p1 to p1 + maxFraction * (p2 - p1).
Definition: b2_collision.h:154
b2DynamicTree::GetAreaRatio
float GetAreaRatio() const
Get the ratio of the sum of the node areas to the root area.
b2GrowableStack
Definition: b2_growable_stack.h:35
b2AABB::GetCenter
b2Vec2 GetCenter() const
Get the center of the AABB.
Definition: b2_collision.h:174
b2DynamicTree::RebuildBottomUp
void RebuildBottomUp()
Build an optimal tree. Very expensive. For testing.
b2DynamicTree::~b2DynamicTree
~b2DynamicTree()
Destroy the tree, freeing the node pool.
b2DynamicTree::MoveProxy
bool MoveProxy(int32 proxyId, const b2AABB &aabb1, const b2Vec2 &displacement)
b2DynamicTree::GetUserData
void * GetUserData(int32 proxyId) const
Definition: b2_dynamic_tree.h:166
b2DynamicTree::Validate
void Validate() const
Validate this tree. For testing.
b2Vec2::Normalize
float Normalize()
Convert this vector into a unit vector. Returns the length.
Definition: b2_math.h:102
b2DynamicTree::DestroyProxy
void DestroyProxy(int32 proxyId)
Destroy a proxy. This asserts if the id is invalid.
b2DynamicTree::RayCast
void RayCast(T *callback, const b2RayCastInput &input) const
Definition: b2_dynamic_tree.h:226
b2AABB
An axis aligned bounding box.
Definition: b2_collision.h:169
b2DynamicTree::b2DynamicTree
b2DynamicTree()
Constructing the tree initializes the node pool.
b2TestOverlap
B2_API bool b2TestOverlap(const b2Shape *shapeA, int32 indexA, const b2Shape *shapeB, int32 indexB, const b2Transform &xfA, const b2Transform &xfB)
Determine if two generic shapes overlap.
b2_collision.h
b2AABB::upperBound
b2Vec2 upperBound
the upper vertex
Definition: b2_collision.h:221
b2DynamicTree::CreateProxy
int32 CreateProxy(const b2AABB &aabb, void *userData)
Create a proxy. Provide a tight fitting AABB and a userData pointer.
b2TreeNode::aabb
b2AABB aabb
Enlarged AABB.
Definition: b2_dynamic_tree.h:41
b2Vec2::LengthSquared
float LengthSquared() const
Definition: b2_math.h:96
b2DynamicTree::Query
void Query(T *callback, const b2AABB &aabb) const
Definition: b2_dynamic_tree.h:191
b2DynamicTree::GetHeight
int32 GetHeight() const
b2DynamicTree::GetMaxBalance
int32 GetMaxBalance() const
b2DynamicTree::ShiftOrigin
void ShiftOrigin(const b2Vec2 &newOrigin)