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

游戏引擎

开发平台:

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. #include <Box2D/Collision/b2DynamicTree.h>
  19. #include <cstring>
  20. #include <cfloat>
  21. b2DynamicTree::b2DynamicTree()
  22. {
  23. m_root = b2_nullNode;
  24. m_nodeCapacity = 16;
  25. m_nodeCount = 0;
  26. m_nodes = (b2DynamicTreeNode*)b2Alloc(m_nodeCapacity * sizeof(b2DynamicTreeNode));
  27. memset(m_nodes, 0, m_nodeCapacity * sizeof(b2DynamicTreeNode));
  28. // Build a linked list for the free list.
  29. for (int32 i = 0; i < m_nodeCapacity - 1; ++i)
  30. {
  31. m_nodes[i].next = i + 1;
  32. }
  33. m_nodes[m_nodeCapacity-1].next = b2_nullNode;
  34. m_freeList = 0;
  35. m_path = 0;
  36. m_insertionCount = 0;
  37. }
  38. b2DynamicTree::~b2DynamicTree()
  39. {
  40. // This frees the entire tree in one shot.
  41. b2Free(m_nodes);
  42. }
  43. // Allocate a node from the pool. Grow the pool if necessary.
  44. int32 b2DynamicTree::AllocateNode()
  45. {
  46. // Expand the node pool as needed.
  47. if (m_freeList == b2_nullNode)
  48. {
  49. b2Assert(m_nodeCount == m_nodeCapacity);
  50. // The free list is empty. Rebuild a bigger pool.
  51. b2DynamicTreeNode* oldNodes = m_nodes;
  52. m_nodeCapacity *= 2;
  53. m_nodes = (b2DynamicTreeNode*)b2Alloc(m_nodeCapacity * sizeof(b2DynamicTreeNode));
  54. memcpy(m_nodes, oldNodes, m_nodeCount * sizeof(b2DynamicTreeNode));
  55. b2Free(oldNodes);
  56. // Build a linked list for the free list. The parent
  57. // pointer becomes the "next" pointer.
  58. for (int32 i = m_nodeCount; i < m_nodeCapacity - 1; ++i)
  59. {
  60. m_nodes[i].next = i + 1;
  61. }
  62. m_nodes[m_nodeCapacity-1].next = b2_nullNode;
  63. m_freeList = m_nodeCount;
  64. }
  65. // Peel a node off the free list.
  66. int32 nodeId = m_freeList;
  67. m_freeList = m_nodes[nodeId].next;
  68. m_nodes[nodeId].parent = b2_nullNode;
  69. m_nodes[nodeId].child1 = b2_nullNode;
  70. m_nodes[nodeId].child2 = b2_nullNode;
  71. ++m_nodeCount;
  72. return nodeId;
  73. }
  74. // Return a node to the pool.
  75. void b2DynamicTree::FreeNode(int32 nodeId)
  76. {
  77. b2Assert(0 <= nodeId && nodeId < m_nodeCapacity);
  78. b2Assert(0 < m_nodeCount);
  79. m_nodes[nodeId].next = m_freeList;
  80. m_freeList = nodeId;
  81. --m_nodeCount;
  82. }
  83. // Create a proxy in the tree as a leaf node. We return the index
  84. // of the node instead of a pointer so that we can grow
  85. // the node pool.
  86. int32 b2DynamicTree::CreateProxy(const b2AABB& aabb, void* userData)
  87. {
  88. int32 proxyId = AllocateNode();
  89. // Fatten the aabb.
  90. b2Vec2 r(b2_aabbExtension, b2_aabbExtension);
  91. m_nodes[proxyId].aabb.lowerBound = aabb.lowerBound - r;
  92. m_nodes[proxyId].aabb.upperBound = aabb.upperBound + r;
  93. m_nodes[proxyId].userData = userData;
  94. InsertLeaf(proxyId);
  95. // Rebalance if necessary.
  96. int32 iterationCount = m_nodeCount >> 4;
  97. int32 tryCount = 0;
  98. int32 height = ComputeHeight();
  99. while (height > 64 && tryCount < 10)
  100. {
  101. Rebalance(iterationCount);
  102. height = ComputeHeight();
  103. ++tryCount;
  104. }
  105. return proxyId;
  106. }
  107. void b2DynamicTree::DestroyProxy(int32 proxyId)
  108. {
  109. b2Assert(0 <= proxyId && proxyId < m_nodeCapacity);
  110. b2Assert(m_nodes[proxyId].IsLeaf());
  111. RemoveLeaf(proxyId);
  112. FreeNode(proxyId);
  113. }
  114. bool b2DynamicTree::MoveProxy(int32 proxyId, const b2AABB& aabb, const b2Vec2& displacement)
  115. {
  116. b2Assert(0 <= proxyId && proxyId < m_nodeCapacity);
  117. b2Assert(m_nodes[proxyId].IsLeaf());
  118. if (m_nodes[proxyId].aabb.Contains(aabb))
  119. {
  120. return false;
  121. }
  122. RemoveLeaf(proxyId);
  123. // Extend AABB.
  124. b2AABB b = aabb;
  125. b2Vec2 r(b2_aabbExtension, b2_aabbExtension);
  126. b.lowerBound = b.lowerBound - r;
  127. b.upperBound = b.upperBound + r;
  128. // Predict AABB displacement.
  129. b2Vec2 d = b2_aabbMultiplier * displacement;
  130. if (d.x < 0.0f)
  131. {
  132. b.lowerBound.x += d.x;
  133. }
  134. else
  135. {
  136. b.upperBound.x += d.x;
  137. }
  138. if (d.y < 0.0f)
  139. {
  140. b.lowerBound.y += d.y;
  141. }
  142. else
  143. {
  144. b.upperBound.y += d.y;
  145. }
  146. m_nodes[proxyId].aabb = b;
  147. InsertLeaf(proxyId);
  148. return true;
  149. }
  150. void b2DynamicTree::InsertLeaf(int32 leaf)
  151. {
  152. ++m_insertionCount;
  153. if (m_root == b2_nullNode)
  154. {
  155. m_root = leaf;
  156. m_nodes[m_root].parent = b2_nullNode;
  157. return;
  158. }
  159. // Find the best sibling for this node.
  160. b2Vec2 center = m_nodes[leaf].aabb.GetCenter();
  161. int32 sibling = m_root;
  162. if (m_nodes[sibling].IsLeaf() == false)
  163. {
  164. do 
  165. {
  166. int32 child1 = m_nodes[sibling].child1;
  167. int32 child2 = m_nodes[sibling].child2;
  168. b2Vec2 delta1 = b2Abs(m_nodes[child1].aabb.GetCenter() - center);
  169. b2Vec2 delta2 = b2Abs(m_nodes[child2].aabb.GetCenter() - center);
  170. float32 norm1 = delta1.x + delta1.y;
  171. float32 norm2 = delta2.x + delta2.y;
  172. if (norm1 < norm2)
  173. {
  174. sibling = child1;
  175. }
  176. else
  177. {
  178. sibling = child2;
  179. }
  180. }
  181. while(m_nodes[sibling].IsLeaf() == false);
  182. }
  183. // Create a parent for the siblings.
  184. int32 node1 = m_nodes[sibling].parent;
  185. int32 node2 = AllocateNode();
  186. m_nodes[node2].parent = node1;
  187. m_nodes[node2].userData = NULL;
  188. m_nodes[node2].aabb.Combine(m_nodes[leaf].aabb, m_nodes[sibling].aabb);
  189. if (node1 != b2_nullNode)
  190. {
  191. if (m_nodes[m_nodes[sibling].parent].child1 == sibling)
  192. {
  193. m_nodes[node1].child1 = node2;
  194. }
  195. else
  196. {
  197. m_nodes[node1].child2 = node2;
  198. }
  199. m_nodes[node2].child1 = sibling;
  200. m_nodes[node2].child2 = leaf;
  201. m_nodes[sibling].parent = node2;
  202. m_nodes[leaf].parent = node2;
  203. do 
  204. {
  205. if (m_nodes[node1].aabb.Contains(m_nodes[node2].aabb))
  206. {
  207. break;
  208. }
  209. m_nodes[node1].aabb.Combine(m_nodes[m_nodes[node1].child1].aabb, m_nodes[m_nodes[node1].child2].aabb);
  210. node2 = node1;
  211. node1 = m_nodes[node1].parent;
  212. }
  213. while(node1 != b2_nullNode);
  214. }
  215. else
  216. {
  217. m_nodes[node2].child1 = sibling;
  218. m_nodes[node2].child2 = leaf;
  219. m_nodes[sibling].parent = node2;
  220. m_nodes[leaf].parent = node2;
  221. m_root = node2;
  222. }
  223. }
  224. void b2DynamicTree::RemoveLeaf(int32 leaf)
  225. {
  226. if (leaf == m_root)
  227. {
  228. m_root = b2_nullNode;
  229. return;
  230. }
  231. int32 node2 = m_nodes[leaf].parent;
  232. int32 node1 = m_nodes[node2].parent;
  233. int32 sibling;
  234. if (m_nodes[node2].child1 == leaf)
  235. {
  236. sibling = m_nodes[node2].child2;
  237. }
  238. else
  239. {
  240. sibling = m_nodes[node2].child1;
  241. }
  242. if (node1 != b2_nullNode)
  243. {
  244. // Destroy node2 and connect node1 to sibling.
  245. if (m_nodes[node1].child1 == node2)
  246. {
  247. m_nodes[node1].child1 = sibling;
  248. }
  249. else
  250. {
  251. m_nodes[node1].child2 = sibling;
  252. }
  253. m_nodes[sibling].parent = node1;
  254. FreeNode(node2);
  255. // Adjust ancestor bounds.
  256. while (node1 != b2_nullNode)
  257. {
  258. b2AABB oldAABB = m_nodes[node1].aabb;
  259. m_nodes[node1].aabb.Combine(m_nodes[m_nodes[node1].child1].aabb, m_nodes[m_nodes[node1].child2].aabb);
  260. if (oldAABB.Contains(m_nodes[node1].aabb))
  261. {
  262. break;
  263. }
  264. node1 = m_nodes[node1].parent;
  265. }
  266. }
  267. else
  268. {
  269. m_root = sibling;
  270. m_nodes[sibling].parent = b2_nullNode;
  271. FreeNode(node2);
  272. }
  273. }
  274. void b2DynamicTree::Rebalance(int32 iterations)
  275. {
  276. if (m_root == b2_nullNode)
  277. {
  278. return;
  279. }
  280. for (int32 i = 0; i < iterations; ++i)
  281. {
  282. int32 node = m_root;
  283. uint32 bit = 0;
  284. while (m_nodes[node].IsLeaf() == false)
  285. {
  286. int32* children = &m_nodes[node].child1;
  287. node = children[(m_path >> bit) & 1];
  288. bit = (bit + 1) & (8* sizeof(uint32) - 1);
  289. }
  290. ++m_path;
  291. RemoveLeaf(node);
  292. InsertLeaf(node);
  293. }
  294. }
  295. // Compute the height of a sub-tree.
  296. int32 b2DynamicTree::ComputeHeight(int32 nodeId) const
  297. {
  298. if (nodeId == b2_nullNode)
  299. {
  300. return 0;
  301. }
  302. b2Assert(0 <= nodeId && nodeId < m_nodeCapacity);
  303. b2DynamicTreeNode* node = m_nodes + nodeId;
  304. int32 height1 = ComputeHeight(node->child1);
  305. int32 height2 = ComputeHeight(node->child2);
  306. return 1 + b2Max(height1, height2);
  307. }
  308. int32 b2DynamicTree::ComputeHeight() const
  309. {
  310. return ComputeHeight(m_root);
  311. }