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 
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:
73 
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 
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 {
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 
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)