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

游戏引擎

开发平台:

Visual C++

  1. /*
  2. * Copyright (c) 2007-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/b2Collision.h>
  19. #include <Box2D/Collision/b2Distance.h>
  20. #include <Box2D/Collision/b2TimeOfImpact.h>
  21. #include <Box2D/Collision/Shapes/b2CircleShape.h>
  22. #include <Box2D/Collision/Shapes/b2PolygonShape.h>
  23. #include <cstdio>
  24. int32 b2_toiCalls, b2_toiIters, b2_toiMaxIters;
  25. int32 b2_toiRootIters, b2_toiMaxRootIters;
  26. int32 b2_toiMaxOptIters;
  27. struct b2SeparationFunction
  28. {
  29. enum Type
  30. {
  31. e_points,
  32. e_faceA,
  33. e_faceB
  34. };
  35. // TODO_ERIN might not need to return the separation
  36. float32 Initialize(const b2SimplexCache* cache,
  37. const b2DistanceProxy* proxyA, const b2Sweep& sweepA,
  38. const b2DistanceProxy* proxyB, const b2Sweep& sweepB)
  39. {
  40. m_proxyA = proxyA;
  41. m_proxyB = proxyB;
  42. int32 count = cache->count;
  43. b2Assert(0 < count && count < 3);
  44. m_sweepA = sweepA;
  45. m_sweepB = sweepB;
  46. b2Transform xfA, xfB;
  47. m_sweepA.GetTransform(&xfA, 0.0f);
  48. m_sweepB.GetTransform(&xfB, 0.0f);
  49. if (count == 1)
  50. {
  51. m_type = e_points;
  52. b2Vec2 localPointA = m_proxyA->GetVertex(cache->indexA[0]);
  53. b2Vec2 localPointB = m_proxyB->GetVertex(cache->indexB[0]);
  54. b2Vec2 pointA = b2Mul(xfA, localPointA);
  55. b2Vec2 pointB = b2Mul(xfB, localPointB);
  56. m_axis = pointB - pointA;
  57. float32 s = m_axis.Normalize();
  58. return s;
  59. }
  60. else if (cache->indexA[0] == cache->indexA[1])
  61. {
  62. // Two points on B and one on A.
  63. m_type = e_faceB;
  64. b2Vec2 localPointB1 = proxyB->GetVertex(cache->indexB[0]);
  65. b2Vec2 localPointB2 = proxyB->GetVertex(cache->indexB[1]);
  66. m_axis = b2Cross(localPointB2 - localPointB1, 1.0f);
  67. m_axis.Normalize();
  68. b2Vec2 normal = b2Mul(xfB.R, m_axis);
  69. m_localPoint = 0.5f * (localPointB1 + localPointB2);
  70. b2Vec2 pointB = b2Mul(xfB, m_localPoint);
  71. b2Vec2 localPointA = proxyA->GetVertex(cache->indexA[0]);
  72. b2Vec2 pointA = b2Mul(xfA, localPointA);
  73. float32 s = b2Dot(pointA - pointB, normal);
  74. if (s < 0.0f)
  75. {
  76. m_axis = -m_axis;
  77. s = -s;
  78. }
  79. return s;
  80. }
  81. else
  82. {
  83. // Two points on A and one or two points on B.
  84. m_type = e_faceA;
  85. b2Vec2 localPointA1 = m_proxyA->GetVertex(cache->indexA[0]);
  86. b2Vec2 localPointA2 = m_proxyA->GetVertex(cache->indexA[1]);
  87. m_axis = b2Cross(localPointA2 - localPointA1, 1.0f);
  88. m_axis.Normalize();
  89. b2Vec2 normal = b2Mul(xfA.R, m_axis);
  90. m_localPoint = 0.5f * (localPointA1 + localPointA2);
  91. b2Vec2 pointA = b2Mul(xfA, m_localPoint);
  92. b2Vec2 localPointB = m_proxyB->GetVertex(cache->indexB[0]);
  93. b2Vec2 pointB = b2Mul(xfB, localPointB);
  94. float32 s = b2Dot(pointB - pointA, normal);
  95. if (s < 0.0f)
  96. {
  97. m_axis = -m_axis;
  98. s = -s;
  99. }
  100. return s;
  101. }
  102. }
  103. float32 FindMinSeparation(int32* indexA, int32* indexB, float32 t) const
  104. {
  105. b2Transform xfA, xfB;
  106. m_sweepA.GetTransform(&xfA, t);
  107. m_sweepB.GetTransform(&xfB, t);
  108. switch (m_type)
  109. {
  110. case e_points:
  111. {
  112. b2Vec2 axisA = b2MulT(xfA.R,  m_axis);
  113. b2Vec2 axisB = b2MulT(xfB.R, -m_axis);
  114. *indexA = m_proxyA->GetSupport(axisA);
  115. *indexB = m_proxyB->GetSupport(axisB);
  116. b2Vec2 localPointA = m_proxyA->GetVertex(*indexA);
  117. b2Vec2 localPointB = m_proxyB->GetVertex(*indexB);
  118. b2Vec2 pointA = b2Mul(xfA, localPointA);
  119. b2Vec2 pointB = b2Mul(xfB, localPointB);
  120. float32 separation = b2Dot(pointB - pointA, m_axis);
  121. return separation;
  122. }
  123. case e_faceA:
  124. {
  125. b2Vec2 normal = b2Mul(xfA.R, m_axis);
  126. b2Vec2 pointA = b2Mul(xfA, m_localPoint);
  127. b2Vec2 axisB = b2MulT(xfB.R, -normal);
  128. *indexA = -1;
  129. *indexB = m_proxyB->GetSupport(axisB);
  130. b2Vec2 localPointB = m_proxyB->GetVertex(*indexB);
  131. b2Vec2 pointB = b2Mul(xfB, localPointB);
  132. float32 separation = b2Dot(pointB - pointA, normal);
  133. return separation;
  134. }
  135. case e_faceB:
  136. {
  137. b2Vec2 normal = b2Mul(xfB.R, m_axis);
  138. b2Vec2 pointB = b2Mul(xfB, m_localPoint);
  139. b2Vec2 axisA = b2MulT(xfA.R, -normal);
  140. *indexB = -1;
  141. *indexA = m_proxyA->GetSupport(axisA);
  142. b2Vec2 localPointA = m_proxyA->GetVertex(*indexA);
  143. b2Vec2 pointA = b2Mul(xfA, localPointA);
  144. float32 separation = b2Dot(pointA - pointB, normal);
  145. return separation;
  146. }
  147. default:
  148. b2Assert(false);
  149. *indexA = -1;
  150. *indexB = -1;
  151. return 0.0f;
  152. }
  153. }
  154. float32 Evaluate(int32 indexA, int32 indexB, float32 t) const
  155. {
  156. b2Transform xfA, xfB;
  157. m_sweepA.GetTransform(&xfA, t);
  158. m_sweepB.GetTransform(&xfB, t);
  159. switch (m_type)
  160. {
  161. case e_points:
  162. {
  163. b2Vec2 axisA = b2MulT(xfA.R,  m_axis);
  164. b2Vec2 axisB = b2MulT(xfB.R, -m_axis);
  165. b2Vec2 localPointA = m_proxyA->GetVertex(indexA);
  166. b2Vec2 localPointB = m_proxyB->GetVertex(indexB);
  167. b2Vec2 pointA = b2Mul(xfA, localPointA);
  168. b2Vec2 pointB = b2Mul(xfB, localPointB);
  169. float32 separation = b2Dot(pointB - pointA, m_axis);
  170. return separation;
  171. }
  172. case e_faceA:
  173. {
  174. b2Vec2 normal = b2Mul(xfA.R, m_axis);
  175. b2Vec2 pointA = b2Mul(xfA, m_localPoint);
  176. b2Vec2 axisB = b2MulT(xfB.R, -normal);
  177. b2Vec2 localPointB = m_proxyB->GetVertex(indexB);
  178. b2Vec2 pointB = b2Mul(xfB, localPointB);
  179. float32 separation = b2Dot(pointB - pointA, normal);
  180. return separation;
  181. }
  182. case e_faceB:
  183. {
  184. b2Vec2 normal = b2Mul(xfB.R, m_axis);
  185. b2Vec2 pointB = b2Mul(xfB, m_localPoint);
  186. b2Vec2 axisA = b2MulT(xfA.R, -normal);
  187. b2Vec2 localPointA = m_proxyA->GetVertex(indexA);
  188. b2Vec2 pointA = b2Mul(xfA, localPointA);
  189. float32 separation = b2Dot(pointA - pointB, normal);
  190. return separation;
  191. }
  192. default:
  193. b2Assert(false);
  194. return 0.0f;
  195. }
  196. }
  197. const b2DistanceProxy* m_proxyA;
  198. const b2DistanceProxy* m_proxyB;
  199. b2Sweep m_sweepA, m_sweepB;
  200. Type m_type;
  201. b2Vec2 m_localPoint;
  202. b2Vec2 m_axis;
  203. };
  204. // CCD via the local separating axis method. This seeks progression
  205. // by computing the largest time at which separation is maintained.
  206. void b2TimeOfImpact(b2TOIOutput* output, const b2TOIInput* input)
  207. {
  208. ++b2_toiCalls;
  209. output->state = b2TOIOutput::e_unknown;
  210. output->t = input->tMax;
  211. const b2DistanceProxy* proxyA = &input->proxyA;
  212. const b2DistanceProxy* proxyB = &input->proxyB;
  213. b2Sweep sweepA = input->sweepA;
  214. b2Sweep sweepB = input->sweepB;
  215. // Large rotations can make the root finder fail, so we normalize the
  216. // sweep angles.
  217. sweepA.Normalize();
  218. sweepB.Normalize();
  219. float32 tMax = input->tMax;
  220. float32 totalRadius = proxyA->m_radius + proxyB->m_radius;
  221. float32 target = b2Max(b2_linearSlop, totalRadius - 3.0f * b2_linearSlop);
  222. float32 tolerance = 0.25f * b2_linearSlop;
  223. b2Assert(target > tolerance);
  224. float32 t1 = 0.0f;
  225. const int32 k_maxIterations = 20; // TODO_ERIN b2Settings
  226. int32 iter = 0;
  227. // Prepare input for distance query.
  228. b2SimplexCache cache;
  229. cache.count = 0;
  230. b2DistanceInput distanceInput;
  231. distanceInput.proxyA = input->proxyA;
  232. distanceInput.proxyB = input->proxyB;
  233. distanceInput.useRadii = false;
  234. // The outer loop progressively attempts to compute new separating axes.
  235. // This loop terminates when an axis is repeated (no progress is made).
  236. for(;;)
  237. {
  238. b2Transform xfA, xfB;
  239. sweepA.GetTransform(&xfA, t1);
  240. sweepB.GetTransform(&xfB, t1);
  241. // Get the distance between shapes. We can also use the results
  242. // to get a separating axis.
  243. distanceInput.transformA = xfA;
  244. distanceInput.transformB = xfB;
  245. b2DistanceOutput distanceOutput;
  246. b2Distance(&distanceOutput, &cache, &distanceInput);
  247. // If the shapes are overlapped, we give up on continuous collision.
  248. if (distanceOutput.distance <= 0.0f)
  249. {
  250. // Failure!
  251. output->state = b2TOIOutput::e_overlapped;
  252. output->t = 0.0f;
  253. break;
  254. }
  255. if (distanceOutput.distance < target + tolerance)
  256. {
  257. // Victory!
  258. output->state = b2TOIOutput::e_touching;
  259. output->t = t1;
  260. break;
  261. }
  262. // Initialize the separating axis.
  263. b2SeparationFunction fcn;
  264. fcn.Initialize(&cache, proxyA, sweepA, proxyB, sweepB);
  265. #if 0
  266. // Dump the curve seen by the root finder
  267. {
  268. const int32 N = 100;
  269. float32 dx = 1.0f / N;
  270. float32 xs[N+1];
  271. float32 fs[N+1];
  272. float32 x = 0.0f;
  273. for (int32 i = 0; i <= N; ++i)
  274. {
  275. sweepA.GetTransform(&xfA, x);
  276. sweepB.GetTransform(&xfB, x);
  277. float32 f = fcn.Evaluate(xfA, xfB) - target;
  278. printf("%g %gn", x, f);
  279. xs[i] = x;
  280. fs[i] = f;
  281. x += dx;
  282. }
  283. }
  284. #endif
  285. // Compute the TOI on the separating axis. We do this by successively
  286. // resolving the deepest point. This loop is bounded by the number of vertices.
  287. bool done = false;
  288. float32 t2 = tMax;
  289. int32 pushBackIter = 0;
  290. for (;;)
  291. {
  292. // Find the deepest point at t2. Store the witness point indices.
  293. int32 indexA, indexB;
  294. float32 s2 = fcn.FindMinSeparation(&indexA, &indexB, t2);
  295. // Is the final configuration separated?
  296. if (s2 > target + tolerance)
  297. {
  298. // Victory!
  299. output->state = b2TOIOutput::e_separated;
  300. output->t = tMax;
  301. done = true;
  302. break;
  303. }
  304. // Has the separation reached tolerance?
  305. if (s2 > target - tolerance)
  306. {
  307. // Advance the sweeps
  308. t1 = t2;
  309. break;
  310. }
  311. // Compute the initial separation of the witness points.
  312. float32 s1 = fcn.Evaluate(indexA, indexB, t1);
  313. // Check for initial overlap. This might happen if the root finder
  314. // runs out of iterations.
  315. if (s1 < target - tolerance)
  316. {
  317. output->state = b2TOIOutput::e_failed;
  318. output->t = t1;
  319. done = true;
  320. break;
  321. }
  322. // Check for touching
  323. if (s1 <= target + tolerance)
  324. {
  325. // Victory! t1 should hold the TOI (could be 0.0).
  326. output->state = b2TOIOutput::e_touching;
  327. output->t = t1;
  328. done = true;
  329. break;
  330. }
  331. // Compute 1D root of: f(x) - target = 0
  332. int32 rootIterCount = 0;
  333. float32 a1 = t1, a2 = t2;
  334. for (;;)
  335. {
  336. // Use a mix of the secant rule and bisection.
  337. float32 t;
  338. if (rootIterCount & 1)
  339. {
  340. // Secant rule to improve convergence.
  341. t = a1 + (target - s1) * (a2 - a1) / (s2 - s1);
  342. }
  343. else
  344. {
  345. // Bisection to guarantee progress.
  346. t = 0.5f * (a1 + a2);
  347. }
  348. float32 s = fcn.Evaluate(indexA, indexB, t);
  349. if (b2Abs(s - target) < tolerance)
  350. {
  351. // t2 holds a tentative value for t1
  352. t2 = t;
  353. break;
  354. }
  355. // Ensure we continue to bracket the root.
  356. if (s > target)
  357. {
  358. a1 = t;
  359. s1 = s;
  360. }
  361. else
  362. {
  363. a2 = t;
  364. s2 = s;
  365. }
  366. ++rootIterCount;
  367. ++b2_toiRootIters;
  368. if (rootIterCount == 50)
  369. {
  370. break;
  371. }
  372. }
  373. b2_toiMaxRootIters = b2Max(b2_toiMaxRootIters, rootIterCount);
  374. ++pushBackIter;
  375. if (pushBackIter == b2_maxPolygonVertices)
  376. {
  377. break;
  378. }
  379. }
  380. ++iter;
  381. ++b2_toiIters;
  382. if (done)
  383. {
  384. break;
  385. }
  386. if (iter == k_maxIterations)
  387. {
  388. // Root finder got stuck. Semi-victory.
  389. output->state = b2TOIOutput::e_failed;
  390. output->t = t1;
  391. break;
  392. }
  393. }
  394. b2_toiMaxIters = b2Max(b2_toiMaxIters, iter);
  395. }