Box2D  2.3.0
A 2D Physics Engine for Games
b2DynamicTree.h
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 
23 #include <Box2D/Common/b2GrowableStack.h>
24 
25 #define b2_nullNode (-1)
26 
28 struct b2TreeNode
29 {
30  bool IsLeaf() const
31  {
32  return child1 == b2_nullNode;
33  }
34 
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 
61 class b2DynamicTree
62 {
63 public:
65  b2DynamicTree();
66 
68  ~b2DynamicTree();
69 
71  int32 CreateProxy(const b2AABB& aabb, void* userData);
72 
74  void DestroyProxy(int32 proxyId);
75 
80  bool MoveProxy(int32 proxyId, const b2AABB& aabb1, const b2Vec2& displacement);
81 
84  void* GetUserData(int32 proxyId) const;
85 
87  const b2AABB& GetFatAABB(int32 proxyId) const;
88 
91  template <typename T>
92  void Query(T* callback, const b2AABB& aabb) const;
93 
101  template <typename T>
102  void RayCast(T* callback, const b2RayCastInput& input) const;
103 
105  void Validate() const;
106 
109  int32 GetHeight() const;
110 
113  int32 GetMaxBalance() const;
114 
116  float32 GetAreaRatio() const;
117 
119  void RebuildBottomUp();
120 
124  void ShiftOrigin(const b2Vec2& newOrigin);
125 
126 private:
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 
151  uint32 m_path;
152 
153  int32 m_insertionCount;
154 };
155 
156 inline void* b2DynamicTree::GetUserData(int32 proxyId) const
157 {
158  b2Assert(0 <= proxyId && proxyId < m_nodeCapacity);
159  return m_nodes[proxyId].userData;
160 }
161 
162 inline const b2AABB& b2DynamicTree::GetFatAABB(int32 proxyId) const
163 {
164  b2Assert(0 <= proxyId && proxyId < m_nodeCapacity);
165  return m_nodes[proxyId].aabb;
166 }
167 
168 template <typename T>
169 inline void b2DynamicTree::Query(T* callback, const b2AABB& aabb) const
170 {
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 
203 template <typename T>
204 inline 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 
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