b2DynamicTree.h
上传用户:gb3593
上传日期:2022-01-07
资源大小:3028k
文件大小:7k
源码类别:

游戏引擎

开发平台:

Visual C++

  1. /*
  2. * Copyright (c) 2009 Erin Catto http://www.gphysics.com
  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. #ifndef B2_DYNAMIC_TREE_H
  19. #define B2_DYNAMIC_TREE_H
  20. #include <Box2D/Collision/b2Collision.h>
  21. /// A dynamic AABB tree broad-phase, inspired by Nathanael Presson's btDbvt.
  22. #define b2_nullNode (-1)
  23. /// A node in the dynamic tree. The client does not interact with this directly.
  24. struct b2DynamicTreeNode
  25. {
  26. bool IsLeaf() const
  27. {
  28. return child1 == b2_nullNode;
  29. }
  30. /// This is the fattened AABB.
  31. b2AABB aabb;
  32. //int32 userData;
  33. void* userData;
  34. union
  35. {
  36. int32 parent;
  37. int32 next;
  38. };
  39. int32 child1;
  40. int32 child2;
  41. };
  42. /// A dynamic tree arranges data in a binary tree to accelerate
  43. /// queries such as volume queries and ray casts. Leafs are proxies
  44. /// with an AABB. In the tree we expand the proxy AABB by b2_fatAABBFactor
  45. /// so that the proxy AABB is bigger than the client object. This allows the client
  46. /// object to move by small amounts without triggering a tree update.
  47. ///
  48. /// Nodes are pooled and relocatable, so we use node indices rather than pointers.
  49. class b2DynamicTree
  50. {
  51. public:
  52. /// Constructing the tree initializes the node pool.
  53. b2DynamicTree();
  54. /// Destroy the tree, freeing the node pool.
  55. ~b2DynamicTree();
  56. /// Create a proxy. Provide a tight fitting AABB and a userData pointer.
  57. int32 CreateProxy(const b2AABB& aabb, void* userData);
  58. /// Destroy a proxy. This asserts if the id is invalid.
  59. void DestroyProxy(int32 proxyId);
  60. /// Move a proxy with a swepted AABB. If the proxy has moved outside of its fattened AABB,
  61. /// then the proxy is removed from the tree and re-inserted. Otherwise
  62. /// the function returns immediately.
  63. /// @return true if the proxy was re-inserted.
  64. bool MoveProxy(int32 proxyId, const b2AABB& aabb1, const b2Vec2& displacement);
  65. /// Perform some iterations to re-balance the tree.
  66. void Rebalance(int32 iterations);
  67. /// Get proxy user data.
  68. /// @return the proxy user data or 0 if the id is invalid.
  69. void* GetUserData(int32 proxyId) const;
  70. /// Get the fat AABB for a proxy.
  71. const b2AABB& GetFatAABB(int32 proxyId) const;
  72. /// Compute the height of the tree.
  73. int32 ComputeHeight() const;
  74. /// Query an AABB for overlapping proxies. The callback class
  75. /// is called for each proxy that overlaps the supplied AABB.
  76. template <typename T>
  77. void Query(T* callback, const b2AABB& aabb) const;
  78. /// Ray-cast against the proxies in the tree. This relies on the callback
  79. /// to perform a exact ray-cast in the case were the proxy contains a shape.
  80. /// The callback also performs the any collision filtering. This has performance
  81. /// roughly equal to k * log(n), where k is the number of collisions and n is the
  82. /// number of proxies in the tree.
  83. /// @param input the ray-cast input data. The ray extends from p1 to p1 + maxFraction * (p2 - p1).
  84. /// @param callback a callback class that is called for each proxy that is hit by the ray.
  85. template <typename T>
  86. void RayCast(T* callback, const b2RayCastInput& input) const;
  87. private:
  88. int32 AllocateNode();
  89. void FreeNode(int32 node);
  90. void InsertLeaf(int32 node);
  91. void RemoveLeaf(int32 node);
  92. int32 ComputeHeight(int32 nodeId) const;
  93. int32 m_root;
  94. b2DynamicTreeNode* m_nodes;
  95. int32 m_nodeCount;
  96. int32 m_nodeCapacity;
  97. int32 m_freeList;
  98. /// This is used incrementally traverse the tree for re-balancing.
  99. uint32 m_path;
  100. int32 m_insertionCount;
  101. };
  102. inline void* b2DynamicTree::GetUserData(int32 proxyId) const
  103. {
  104. b2Assert(0 <= proxyId && proxyId < m_nodeCapacity);
  105. return m_nodes[proxyId].userData;
  106. }
  107. inline const b2AABB& b2DynamicTree::GetFatAABB(int32 proxyId) const
  108. {
  109. b2Assert(0 <= proxyId && proxyId < m_nodeCapacity);
  110. return m_nodes[proxyId].aabb;
  111. }
  112. template <typename T>
  113. inline void b2DynamicTree::Query(T* callback, const b2AABB& aabb) const
  114. {
  115. const int32 k_stackSize = 128;
  116. int32 stack[k_stackSize];
  117. int32 count = 0;
  118. stack[count++] = m_root;
  119. while (count > 0)
  120. {
  121. int32 nodeId = stack[--count];
  122. if (nodeId == b2_nullNode)
  123. {
  124. continue;
  125. }
  126. const b2DynamicTreeNode* node = m_nodes + nodeId;
  127. if (b2TestOverlap(node->aabb, aabb))
  128. {
  129. if (node->IsLeaf())
  130. {
  131. bool proceed = callback->QueryCallback(nodeId);
  132. if (proceed == false)
  133. {
  134. return;
  135. }
  136. }
  137. else
  138. {
  139. if (count < k_stackSize)
  140. {
  141. stack[count++] = node->child1;
  142. }
  143. if (count < k_stackSize)
  144. {
  145. stack[count++] = node->child2;
  146. }
  147. }
  148. }
  149. }
  150. }
  151. template <typename T>
  152. inline void b2DynamicTree::RayCast(T* callback, const b2RayCastInput& input) const
  153. {
  154. b2Vec2 p1 = input.p1;
  155. b2Vec2 p2 = input.p2;
  156. b2Vec2 r = p2 - p1;
  157. b2Assert(r.LengthSquared() > 0.0f);
  158. r.Normalize();
  159. // v is perpendicular to the segment.
  160. b2Vec2 v = b2Cross(1.0f, r);
  161. b2Vec2 abs_v = b2Abs(v);
  162. // Separating axis for segment (Gino, p80).
  163. // |dot(v, p1 - c)| > dot(|v|, h)
  164. float32 maxFraction = input.maxFraction;
  165. // Build a bounding box for the segment.
  166. b2AABB segmentAABB;
  167. {
  168. b2Vec2 t = p1 + maxFraction * (p2 - p1);
  169. segmentAABB.lowerBound = b2Min(p1, t);
  170. segmentAABB.upperBound = b2Max(p1, t);
  171. }
  172. const int32 k_stackSize = 128;
  173. int32 stack[k_stackSize];
  174. int32 count = 0;
  175. stack[count++] = m_root;
  176. while (count > 0)
  177. {
  178. int32 nodeId = stack[--count];
  179. if (nodeId == b2_nullNode)
  180. {
  181. continue;
  182. }
  183. const b2DynamicTreeNode* node = m_nodes + nodeId;
  184. if (b2TestOverlap(node->aabb, segmentAABB) == false)
  185. {
  186. continue;
  187. }
  188. // Separating axis for segment (Gino, p80).
  189. // |dot(v, p1 - c)| > dot(|v|, h)
  190. b2Vec2 c = node->aabb.GetCenter();
  191. b2Vec2 h = node->aabb.GetExtents();
  192. float32 separation = b2Abs(b2Dot(v, p1 - c)) - b2Dot(abs_v, h);
  193. if (separation > 0.0f)
  194. {
  195. continue;
  196. }
  197. if (node->IsLeaf())
  198. {
  199. b2RayCastInput subInput;
  200. subInput.p1 = input.p1;
  201. subInput.p2 = input.p2;
  202. subInput.maxFraction = maxFraction;
  203. float32 value = callback->RayCastCallback(subInput, nodeId);
  204. if (value == 0.0f)
  205. {
  206. // The client has terminated the ray cast.
  207. return;
  208. }
  209. if (value > 0.0f)
  210. {
  211. // Update segment bounding box.
  212. maxFraction = value;
  213. b2Vec2 t = p1 + maxFraction * (p2 - p1);
  214. segmentAABB.lowerBound = b2Min(p1, t);
  215. segmentAABB.upperBound = b2Max(p1, t);
  216. }
  217. }
  218. else
  219. {
  220. if (count < k_stackSize)
  221. {
  222. stack[count++] = node->child1;
  223. }
  224. if (count < k_stackSize)
  225. {
  226. stack[count++] = node->child2;
  227. }
  228. }
  229. }
  230. }
  231. #endif