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

游戏引擎

开发平台:

Visual C++

  1. /*
  2. * Copyright (c) 2006-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/Shapes/b2PolygonShape.h>
  20. // Find the separation between poly1 and poly2 for a give edge normal on poly1.
  21. static float32 b2EdgeSeparation(const b2PolygonShape* poly1, const b2Transform& xf1, int32 edge1,
  22.   const b2PolygonShape* poly2, const b2Transform& xf2)
  23. {
  24. int32 count1 = poly1->m_vertexCount;
  25. const b2Vec2* vertices1 = poly1->m_vertices;
  26. const b2Vec2* normals1 = poly1->m_normals;
  27. int32 count2 = poly2->m_vertexCount;
  28. const b2Vec2* vertices2 = poly2->m_vertices;
  29. b2Assert(0 <= edge1 && edge1 < count1);
  30. // Convert normal from poly1's frame into poly2's frame.
  31. b2Vec2 normal1World = b2Mul(xf1.R, normals1[edge1]);
  32. b2Vec2 normal1 = b2MulT(xf2.R, normal1World);
  33. // Find support vertex on poly2 for -normal.
  34. int32 index = 0;
  35. float32 minDot = b2_maxFloat;
  36. for (int32 i = 0; i < count2; ++i)
  37. {
  38. float32 dot = b2Dot(vertices2[i], normal1);
  39. if (dot < minDot)
  40. {
  41. minDot = dot;
  42. index = i;
  43. }
  44. }
  45. b2Vec2 v1 = b2Mul(xf1, vertices1[edge1]);
  46. b2Vec2 v2 = b2Mul(xf2, vertices2[index]);
  47. float32 separation = b2Dot(v2 - v1, normal1World);
  48. return separation;
  49. }
  50. // Find the max separation between poly1 and poly2 using edge normals from poly1.
  51. static float32 b2FindMaxSeparation(int32* edgeIndex,
  52.  const b2PolygonShape* poly1, const b2Transform& xf1,
  53.  const b2PolygonShape* poly2, const b2Transform& xf2)
  54. {
  55. int32 count1 = poly1->m_vertexCount;
  56. const b2Vec2* normals1 = poly1->m_normals;
  57. // Vector pointing from the centroid of poly1 to the centroid of poly2.
  58. b2Vec2 d = b2Mul(xf2, poly2->m_centroid) - b2Mul(xf1, poly1->m_centroid);
  59. b2Vec2 dLocal1 = b2MulT(xf1.R, d);
  60. // Find edge normal on poly1 that has the largest projection onto d.
  61. int32 edge = 0;
  62. float32 maxDot = -b2_maxFloat;
  63. for (int32 i = 0; i < count1; ++i)
  64. {
  65. float32 dot = b2Dot(normals1[i], dLocal1);
  66. if (dot > maxDot)
  67. {
  68. maxDot = dot;
  69. edge = i;
  70. }
  71. }
  72. // Get the separation for the edge normal.
  73. float32 s = b2EdgeSeparation(poly1, xf1, edge, poly2, xf2);
  74. // Check the separation for the previous edge normal.
  75. int32 prevEdge = edge - 1 >= 0 ? edge - 1 : count1 - 1;
  76. float32 sPrev = b2EdgeSeparation(poly1, xf1, prevEdge, poly2, xf2);
  77. // Check the separation for the next edge normal.
  78. int32 nextEdge = edge + 1 < count1 ? edge + 1 : 0;
  79. float32 sNext = b2EdgeSeparation(poly1, xf1, nextEdge, poly2, xf2);
  80. // Find the best edge and the search direction.
  81. int32 bestEdge;
  82. float32 bestSeparation;
  83. int32 increment;
  84. if (sPrev > s && sPrev > sNext)
  85. {
  86. increment = -1;
  87. bestEdge = prevEdge;
  88. bestSeparation = sPrev;
  89. }
  90. else if (sNext > s)
  91. {
  92. increment = 1;
  93. bestEdge = nextEdge;
  94. bestSeparation = sNext;
  95. }
  96. else
  97. {
  98. *edgeIndex = edge;
  99. return s;
  100. }
  101. // Perform a local search for the best edge normal.
  102. for ( ; ; )
  103. {
  104. if (increment == -1)
  105. edge = bestEdge - 1 >= 0 ? bestEdge - 1 : count1 - 1;
  106. else
  107. edge = bestEdge + 1 < count1 ? bestEdge + 1 : 0;
  108. s = b2EdgeSeparation(poly1, xf1, edge, poly2, xf2);
  109. if (s > bestSeparation)
  110. {
  111. bestEdge = edge;
  112. bestSeparation = s;
  113. }
  114. else
  115. {
  116. break;
  117. }
  118. }
  119. *edgeIndex = bestEdge;
  120. return bestSeparation;
  121. }
  122. static void b2FindIncidentEdge(b2ClipVertex c[2],
  123.  const b2PolygonShape* poly1, const b2Transform& xf1, int32 edge1,
  124.  const b2PolygonShape* poly2, const b2Transform& xf2)
  125. {
  126. int32 count1 = poly1->m_vertexCount;
  127. const b2Vec2* normals1 = poly1->m_normals;
  128. int32 count2 = poly2->m_vertexCount;
  129. const b2Vec2* vertices2 = poly2->m_vertices;
  130. const b2Vec2* normals2 = poly2->m_normals;
  131. b2Assert(0 <= edge1 && edge1 < count1);
  132. // Get the normal of the reference edge in poly2's frame.
  133. b2Vec2 normal1 = b2MulT(xf2.R, b2Mul(xf1.R, normals1[edge1]));
  134. // Find the incident edge on poly2.
  135. int32 index = 0;
  136. float32 minDot = b2_maxFloat;
  137. for (int32 i = 0; i < count2; ++i)
  138. {
  139. float32 dot = b2Dot(normal1, normals2[i]);
  140. if (dot < minDot)
  141. {
  142. minDot = dot;
  143. index = i;
  144. }
  145. }
  146. // Build the clip vertices for the incident edge.
  147. int32 i1 = index;
  148. int32 i2 = i1 + 1 < count2 ? i1 + 1 : 0;
  149. c[0].v = b2Mul(xf2, vertices2[i1]);
  150. c[0].id.features.referenceEdge = (uint8)edge1;
  151. c[0].id.features.incidentEdge = (uint8)i1;
  152. c[0].id.features.incidentVertex = 0;
  153. c[1].v = b2Mul(xf2, vertices2[i2]);
  154. c[1].id.features.referenceEdge = (uint8)edge1;
  155. c[1].id.features.incidentEdge = (uint8)i2;
  156. c[1].id.features.incidentVertex = 1;
  157. }
  158. // Find edge normal of max separation on A - return if separating axis is found
  159. // Find edge normal of max separation on B - return if separation axis is found
  160. // Choose reference edge as min(minA, minB)
  161. // Find incident edge
  162. // Clip
  163. // The normal points from 1 to 2
  164. void b2CollidePolygons(b2Manifold* manifold,
  165.   const b2PolygonShape* polyA, const b2Transform& xfA,
  166.   const b2PolygonShape* polyB, const b2Transform& xfB)
  167. {
  168. manifold->pointCount = 0;
  169. float32 totalRadius = polyA->m_radius + polyB->m_radius;
  170. int32 edgeA = 0;
  171. float32 separationA = b2FindMaxSeparation(&edgeA, polyA, xfA, polyB, xfB);
  172. if (separationA > totalRadius)
  173. return;
  174. int32 edgeB = 0;
  175. float32 separationB = b2FindMaxSeparation(&edgeB, polyB, xfB, polyA, xfA);
  176. if (separationB > totalRadius)
  177. return;
  178. const b2PolygonShape* poly1; // reference polygon
  179. const b2PolygonShape* poly2; // incident polygon
  180. b2Transform xf1, xf2;
  181. int32 edge1; // reference edge
  182. uint8 flip;
  183. const float32 k_relativeTol = 0.98f;
  184. const float32 k_absoluteTol = 0.001f;
  185. if (separationB > k_relativeTol * separationA + k_absoluteTol)
  186. {
  187. poly1 = polyB;
  188. poly2 = polyA;
  189. xf1 = xfB;
  190. xf2 = xfA;
  191. edge1 = edgeB;
  192. manifold->type = b2Manifold::e_faceB;
  193. flip = 1;
  194. }
  195. else
  196. {
  197. poly1 = polyA;
  198. poly2 = polyB;
  199. xf1 = xfA;
  200. xf2 = xfB;
  201. edge1 = edgeA;
  202. manifold->type = b2Manifold::e_faceA;
  203. flip = 0;
  204. }
  205. b2ClipVertex incidentEdge[2];
  206. b2FindIncidentEdge(incidentEdge, poly1, xf1, edge1, poly2, xf2);
  207. int32 count1 = poly1->m_vertexCount;
  208. const b2Vec2* vertices1 = poly1->m_vertices;
  209. b2Vec2 v11 = vertices1[edge1];
  210. b2Vec2 v12 = edge1 + 1 < count1 ? vertices1[edge1+1] : vertices1[0];
  211. b2Vec2 localTangent = v12 - v11;
  212. localTangent.Normalize();
  213. b2Vec2 localNormal = b2Cross(localTangent, 1.0f);
  214. b2Vec2 planePoint = 0.5f * (v11 + v12);
  215. b2Vec2 tangent = b2Mul(xf1.R, localTangent);
  216. b2Vec2 normal = b2Cross(tangent, 1.0f);
  217. v11 = b2Mul(xf1, v11);
  218. v12 = b2Mul(xf1, v12);
  219. // Face offset.
  220. float32 frontOffset = b2Dot(normal, v11);
  221. // Side offsets, extended by polytope skin thickness.
  222. float32 sideOffset1 = -b2Dot(tangent, v11) + totalRadius;
  223. float32 sideOffset2 = b2Dot(tangent, v12) + totalRadius;
  224. // Clip incident edge against extruded edge1 side edges.
  225. b2ClipVertex clipPoints1[2];
  226. b2ClipVertex clipPoints2[2];
  227. int np;
  228. // Clip to box side 1
  229. np = b2ClipSegmentToLine(clipPoints1, incidentEdge, -tangent, sideOffset1);
  230. if (np < 2)
  231. return;
  232. // Clip to negative box side 1
  233. np = b2ClipSegmentToLine(clipPoints2, clipPoints1,  tangent, sideOffset2);
  234. if (np < 2)
  235. {
  236. return;
  237. }
  238. // Now clipPoints2 contains the clipped points.
  239. manifold->localNormal = localNormal;
  240. manifold->localPoint = planePoint;
  241. int32 pointCount = 0;
  242. for (int32 i = 0; i < b2_maxManifoldPoints; ++i)
  243. {
  244. float32 separation = b2Dot(normal, clipPoints2[i].v) - frontOffset;
  245. if (separation <= totalRadius)
  246. {
  247. b2ManifoldPoint* cp = manifold->points + pointCount;
  248. cp->localPoint = b2MulT(xf2, clipPoints2[i].v);
  249. cp->id = clipPoints2[i].id;
  250. cp->id.features.flip = flip;
  251. ++pointCount;
  252. }
  253. }
  254. manifold->pointCount = pointCount;
  255. }