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

游戏引擎

开发平台:

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/b2Distance.h>
  19. #include <Box2D/Collision/Shapes/b2CircleShape.h>
  20. #include <Box2D/Collision/Shapes/b2PolygonShape.h>
  21. // GJK using Voronoi regions (Christer Ericson) and Barycentric coordinates.
  22. int32 b2_gjkCalls, b2_gjkIters, b2_gjkMaxIters;
  23. void b2DistanceProxy::Set(const b2Shape* shape)
  24. {
  25. switch (shape->GetType())
  26. {
  27. case b2Shape::e_circle:
  28. {
  29. const b2CircleShape* circle = (b2CircleShape*)shape;
  30. m_vertices = &circle->m_p;
  31. m_count = 1;
  32. m_radius = circle->m_radius;
  33. }
  34. break;
  35. case b2Shape::e_polygon:
  36. {
  37. const b2PolygonShape* polygon = (b2PolygonShape*)shape;
  38. m_vertices = polygon->m_vertices;
  39. m_count = polygon->m_vertexCount;
  40. m_radius = polygon->m_radius;
  41. }
  42. break;
  43. default:
  44. b2Assert(false);
  45. }
  46. }
  47. struct b2SimplexVertex
  48. {
  49. b2Vec2 wA; // support point in proxyA
  50. b2Vec2 wB; // support point in proxyB
  51. b2Vec2 w; // wB - wA
  52. float32 a; // barycentric coordinate for closest point
  53. int32 indexA; // wA index
  54. int32 indexB; // wB index
  55. };
  56. struct b2Simplex
  57. {
  58. void ReadCache( const b2SimplexCache* cache,
  59. const b2DistanceProxy* proxyA, const b2Transform& transformA,
  60. const b2DistanceProxy* proxyB, const b2Transform& transformB)
  61. {
  62. b2Assert(cache->count <= 3);
  63. // Copy data from cache.
  64. m_count = cache->count;
  65. b2SimplexVertex* vertices = &m_v1;
  66. for (int32 i = 0; i < m_count; ++i)
  67. {
  68. b2SimplexVertex* v = vertices + i;
  69. v->indexA = cache->indexA[i];
  70. v->indexB = cache->indexB[i];
  71. b2Vec2 wALocal = proxyA->GetVertex(v->indexA);
  72. b2Vec2 wBLocal = proxyB->GetVertex(v->indexB);
  73. v->wA = b2Mul(transformA, wALocal);
  74. v->wB = b2Mul(transformB, wBLocal);
  75. v->w = v->wB - v->wA;
  76. v->a = 0.0f;
  77. }
  78. // Compute the new simplex metric, if it is substantially different than
  79. // old metric then flush the simplex.
  80. if (m_count > 1)
  81. {
  82. float32 metric1 = cache->metric;
  83. float32 metric2 = GetMetric();
  84. if (metric2 < 0.5f * metric1 || 2.0f * metric1 < metric2 || metric2 < b2_epsilon)
  85. {
  86. // Reset the simplex.
  87. m_count = 0;
  88. }
  89. }
  90. // If the cache is empty or invalid ...
  91. if (m_count == 0)
  92. {
  93. b2SimplexVertex* v = vertices + 0;
  94. v->indexA = 0;
  95. v->indexB = 0;
  96. b2Vec2 wALocal = proxyA->GetVertex(0);
  97. b2Vec2 wBLocal = proxyB->GetVertex(0);
  98. v->wA = b2Mul(transformA, wALocal);
  99. v->wB = b2Mul(transformB, wBLocal);
  100. v->w = v->wB - v->wA;
  101. m_count = 1;
  102. }
  103. }
  104. void WriteCache(b2SimplexCache* cache) const
  105. {
  106. cache->metric = GetMetric();
  107. cache->count = uint16(m_count);
  108. const b2SimplexVertex* vertices = &m_v1;
  109. for (int32 i = 0; i < m_count; ++i)
  110. {
  111. cache->indexA[i] = uint8(vertices[i].indexA);
  112. cache->indexB[i] = uint8(vertices[i].indexB);
  113. }
  114. }
  115. b2Vec2 GetSearchDirection() const
  116. {
  117. switch (m_count)
  118. {
  119. case 1:
  120. return -m_v1.w;
  121. case 2:
  122. {
  123. b2Vec2 e12 = m_v2.w - m_v1.w;
  124. float32 sgn = b2Cross(e12, -m_v1.w);
  125. if (sgn > 0.0f)
  126. {
  127. // Origin is left of e12.
  128. return b2Cross(1.0f, e12);
  129. }
  130. else
  131. {
  132. // Origin is right of e12.
  133. return b2Cross(e12, 1.0f);
  134. }
  135. }
  136. default:
  137. b2Assert(false);
  138. return b2Vec2_zero;
  139. }
  140. }
  141. b2Vec2 GetClosestPoint() const
  142. {
  143. switch (m_count)
  144. {
  145. case 0:
  146. b2Assert(false);
  147. return b2Vec2_zero;
  148. case 1:
  149. return m_v1.w;
  150. case 2:
  151. return m_v1.a * m_v1.w + m_v2.a * m_v2.w;
  152. case 3:
  153. return b2Vec2_zero;
  154. default:
  155. b2Assert(false);
  156. return b2Vec2_zero;
  157. }
  158. }
  159. void GetWitnessPoints(b2Vec2* pA, b2Vec2* pB) const
  160. {
  161. switch (m_count)
  162. {
  163. case 0:
  164. b2Assert(false);
  165. break;
  166. case 1:
  167. *pA = m_v1.wA;
  168. *pB = m_v1.wB;
  169. break;
  170. case 2:
  171. *pA = m_v1.a * m_v1.wA + m_v2.a * m_v2.wA;
  172. *pB = m_v1.a * m_v1.wB + m_v2.a * m_v2.wB;
  173. break;
  174. case 3:
  175. *pA = m_v1.a * m_v1.wA + m_v2.a * m_v2.wA + m_v3.a * m_v3.wA;
  176. *pB = *pA;
  177. break;
  178. default:
  179. b2Assert(false);
  180. break;
  181. }
  182. }
  183. float32 GetMetric() const
  184. {
  185. switch (m_count)
  186. {
  187. case 0:
  188. b2Assert(false);
  189. return 0.0;
  190. case 1:
  191. return 0.0f;
  192. case 2:
  193. return b2Distance(m_v1.w, m_v2.w);
  194. case 3:
  195. return b2Cross(m_v2.w - m_v1.w, m_v3.w - m_v1.w);
  196. default:
  197. b2Assert(false);
  198. return 0.0f;
  199. }
  200. }
  201. void Solve2();
  202. void Solve3();
  203. b2SimplexVertex m_v1, m_v2, m_v3;
  204. int32 m_count;
  205. };
  206. // Solve a line segment using barycentric coordinates.
  207. //
  208. // p = a1 * w1 + a2 * w2
  209. // a1 + a2 = 1
  210. //
  211. // The vector from the origin to the closest point on the line is
  212. // perpendicular to the line.
  213. // e12 = w2 - w1
  214. // dot(p, e) = 0
  215. // a1 * dot(w1, e) + a2 * dot(w2, e) = 0
  216. //
  217. // 2-by-2 linear system
  218. // [1      1     ][a1] = [1]
  219. // [w1.e12 w2.e12][a2] = [0]
  220. //
  221. // Define
  222. // d12_1 =  dot(w2, e12)
  223. // d12_2 = -dot(w1, e12)
  224. // d12 = d12_1 + d12_2
  225. //
  226. // Solution
  227. // a1 = d12_1 / d12
  228. // a2 = d12_2 / d12
  229. void b2Simplex::Solve2()
  230. {
  231. b2Vec2 w1 = m_v1.w;
  232. b2Vec2 w2 = m_v2.w;
  233. b2Vec2 e12 = w2 - w1;
  234. // w1 region
  235. float32 d12_2 = -b2Dot(w1, e12);
  236. if (d12_2 <= 0.0f)
  237. {
  238. // a2 <= 0, so we clamp it to 0
  239. m_v1.a = 1.0f;
  240. m_count = 1;
  241. return;
  242. }
  243. // w2 region
  244. float32 d12_1 = b2Dot(w2, e12);
  245. if (d12_1 <= 0.0f)
  246. {
  247. // a1 <= 0, so we clamp it to 0
  248. m_v2.a = 1.0f;
  249. m_count = 1;
  250. m_v1 = m_v2;
  251. return;
  252. }
  253. // Must be in e12 region.
  254. float32 inv_d12 = 1.0f / (d12_1 + d12_2);
  255. m_v1.a = d12_1 * inv_d12;
  256. m_v2.a = d12_2 * inv_d12;
  257. m_count = 2;
  258. }
  259. // Possible regions:
  260. // - points[2]
  261. // - edge points[0]-points[2]
  262. // - edge points[1]-points[2]
  263. // - inside the triangle
  264. void b2Simplex::Solve3()
  265. {
  266. b2Vec2 w1 = m_v1.w;
  267. b2Vec2 w2 = m_v2.w;
  268. b2Vec2 w3 = m_v3.w;
  269. // Edge12
  270. // [1      1     ][a1] = [1]
  271. // [w1.e12 w2.e12][a2] = [0]
  272. // a3 = 0
  273. b2Vec2 e12 = w2 - w1;
  274. float32 w1e12 = b2Dot(w1, e12);
  275. float32 w2e12 = b2Dot(w2, e12);
  276. float32 d12_1 = w2e12;
  277. float32 d12_2 = -w1e12;
  278. // Edge13
  279. // [1      1     ][a1] = [1]
  280. // [w1.e13 w3.e13][a3] = [0]
  281. // a2 = 0
  282. b2Vec2 e13 = w3 - w1;
  283. float32 w1e13 = b2Dot(w1, e13);
  284. float32 w3e13 = b2Dot(w3, e13);
  285. float32 d13_1 = w3e13;
  286. float32 d13_2 = -w1e13;
  287. // Edge23
  288. // [1      1     ][a2] = [1]
  289. // [w2.e23 w3.e23][a3] = [0]
  290. // a1 = 0
  291. b2Vec2 e23 = w3 - w2;
  292. float32 w2e23 = b2Dot(w2, e23);
  293. float32 w3e23 = b2Dot(w3, e23);
  294. float32 d23_1 = w3e23;
  295. float32 d23_2 = -w2e23;
  296. // Triangle123
  297. float32 n123 = b2Cross(e12, e13);
  298. float32 d123_1 = n123 * b2Cross(w2, w3);
  299. float32 d123_2 = n123 * b2Cross(w3, w1);
  300. float32 d123_3 = n123 * b2Cross(w1, w2);
  301. // w1 region
  302. if (d12_2 <= 0.0f && d13_2 <= 0.0f)
  303. {
  304. m_v1.a = 1.0f;
  305. m_count = 1;
  306. return;
  307. }
  308. // e12
  309. if (d12_1 > 0.0f && d12_2 > 0.0f && d123_3 <= 0.0f)
  310. {
  311. float32 inv_d12 = 1.0f / (d12_1 + d12_2);
  312. m_v1.a = d12_1 * inv_d12;
  313. m_v2.a = d12_2 * inv_d12;
  314. m_count = 2;
  315. return;
  316. }
  317. // e13
  318. if (d13_1 > 0.0f && d13_2 > 0.0f && d123_2 <= 0.0f)
  319. {
  320. float32 inv_d13 = 1.0f / (d13_1 + d13_2);
  321. m_v1.a = d13_1 * inv_d13;
  322. m_v3.a = d13_2 * inv_d13;
  323. m_count = 2;
  324. m_v2 = m_v3;
  325. return;
  326. }
  327. // w2 region
  328. if (d12_1 <= 0.0f && d23_2 <= 0.0f)
  329. {
  330. m_v2.a = 1.0f;
  331. m_count = 1;
  332. m_v1 = m_v2;
  333. return;
  334. }
  335. // w3 region
  336. if (d13_1 <= 0.0f && d23_1 <= 0.0f)
  337. {
  338. m_v3.a = 1.0f;
  339. m_count = 1;
  340. m_v1 = m_v3;
  341. return;
  342. }
  343. // e23
  344. if (d23_1 > 0.0f && d23_2 > 0.0f && d123_1 <= 0.0f)
  345. {
  346. float32 inv_d23 = 1.0f / (d23_1 + d23_2);
  347. m_v2.a = d23_1 * inv_d23;
  348. m_v3.a = d23_2 * inv_d23;
  349. m_count = 2;
  350. m_v1 = m_v3;
  351. return;
  352. }
  353. // Must be in triangle123
  354. float32 inv_d123 = 1.0f / (d123_1 + d123_2 + d123_3);
  355. m_v1.a = d123_1 * inv_d123;
  356. m_v2.a = d123_2 * inv_d123;
  357. m_v3.a = d123_3 * inv_d123;
  358. m_count = 3;
  359. }
  360. void b2Distance(b2DistanceOutput* output,
  361. b2SimplexCache* cache,
  362. const b2DistanceInput* input)
  363. {
  364. ++b2_gjkCalls;
  365. const b2DistanceProxy* proxyA = &input->proxyA;
  366. const b2DistanceProxy* proxyB = &input->proxyB;
  367. b2Transform transformA = input->transformA;
  368. b2Transform transformB = input->transformB;
  369. // Initialize the simplex.
  370. b2Simplex simplex;
  371. simplex.ReadCache(cache, proxyA, transformA, proxyB, transformB);
  372. // Get simplex vertices as an array.
  373. b2SimplexVertex* vertices = &simplex.m_v1;
  374. const int32 k_maxIters = 20;
  375. // These store the vertices of the last simplex so that we
  376. // can check for duplicates and prevent cycling.
  377. int32 saveA[3], saveB[3];
  378. int32 saveCount = 0;
  379. b2Vec2 closestPoint = simplex.GetClosestPoint();
  380. float32 distanceSqr1 = closestPoint.LengthSquared();
  381. float32 distanceSqr2 = distanceSqr1;
  382. // Main iteration loop.
  383. int32 iter = 0;
  384. while (iter < k_maxIters)
  385. {
  386. // Copy simplex so we can identify duplicates.
  387. saveCount = simplex.m_count;
  388. for (int32 i = 0; i < saveCount; ++i)
  389. {
  390. saveA[i] = vertices[i].indexA;
  391. saveB[i] = vertices[i].indexB;
  392. }
  393. switch (simplex.m_count)
  394. {
  395. case 1:
  396. break;
  397. case 2:
  398. simplex.Solve2();
  399. break;
  400. case 3:
  401. simplex.Solve3();
  402. break;
  403. default:
  404. b2Assert(false);
  405. }
  406. // If we have 3 points, then the origin is in the corresponding triangle.
  407. if (simplex.m_count == 3)
  408. {
  409. break;
  410. }
  411. // Compute closest point.
  412. b2Vec2 p = simplex.GetClosestPoint();
  413. distanceSqr2 = p.LengthSquared();
  414. // Ensure progress
  415. if (distanceSqr2 >= distanceSqr1)
  416. {
  417. //break;
  418. }
  419. distanceSqr1 = distanceSqr2;
  420. // Get search direction.
  421. b2Vec2 d = simplex.GetSearchDirection();
  422. // Ensure the search direction is numerically fit.
  423. if (d.LengthSquared() < b2_epsilon * b2_epsilon)
  424. {
  425. // The origin is probably contained by a line segment
  426. // or triangle. Thus the shapes are overlapped.
  427. // We can't return zero here even though there may be overlap.
  428. // In case the simplex is a point, segment, or triangle it is difficult
  429. // to determine if the origin is contained in the CSO or very close to it.
  430. break;
  431. }
  432. // Compute a tentative new simplex vertex using support points.
  433. b2SimplexVertex* vertex = vertices + simplex.m_count;
  434. vertex->indexA = proxyA->GetSupport(b2MulT(transformA.R, -d));
  435. vertex->wA = b2Mul(transformA, proxyA->GetVertex(vertex->indexA));
  436. b2Vec2 wBLocal;
  437. vertex->indexB = proxyB->GetSupport(b2MulT(transformB.R, d));
  438. vertex->wB = b2Mul(transformB, proxyB->GetVertex(vertex->indexB));
  439. vertex->w = vertex->wB - vertex->wA;
  440. // Iteration count is equated to the number of support point calls.
  441. ++iter;
  442. ++b2_gjkIters;
  443. // Check for duplicate support points. This is the main termination criteria.
  444. bool duplicate = false;
  445. for (int32 i = 0; i < saveCount; ++i)
  446. {
  447. if (vertex->indexA == saveA[i] && vertex->indexB == saveB[i])
  448. {
  449. duplicate = true;
  450. break;
  451. }
  452. }
  453. // If we found a duplicate support point we must exit to avoid cycling.
  454. if (duplicate)
  455. {
  456. break;
  457. }
  458. // New vertex is ok and needed.
  459. ++simplex.m_count;
  460. }
  461. b2_gjkMaxIters = b2Max(b2_gjkMaxIters, iter);
  462. // Prepare output.
  463. simplex.GetWitnessPoints(&output->pointA, &output->pointB);
  464. output->distance = b2Distance(output->pointA, output->pointB);
  465. output->iterations = iter;
  466. // Cache the simplex.
  467. simplex.WriteCache(cache);
  468. // Apply radii if requested.
  469. if (input->useRadii)
  470. {
  471. float32 rA = proxyA->m_radius;
  472. float32 rB = proxyB->m_radius;
  473. if (output->distance > rA + rB && output->distance > b2_epsilon)
  474. {
  475. // Shapes are still no overlapped.
  476. // Move the witness points to the outer surface.
  477. output->distance -= rA + rB;
  478. b2Vec2 normal = output->pointB - output->pointA;
  479. normal.Normalize();
  480. output->pointA += rA * normal;
  481. output->pointB -= rB * normal;
  482. }
  483. else
  484. {
  485. // Shapes are overlapped when radii are considered.
  486. // Move the witness points to the middle.
  487. b2Vec2 p = 0.5f * (output->pointA + output->pointB);
  488. output->pointA = p;
  489. output->pointB = p;
  490. output->distance = 0.0f;
  491. }
  492. }
  493. }