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

游戏引擎

开发平台:

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. void b2WorldManifold::Initialize(const b2Manifold* manifold,
  21.   const b2Transform& xfA, float32 radiusA,
  22.   const b2Transform& xfB, float32 radiusB)
  23. {
  24. if (manifold->pointCount == 0)
  25. {
  26. return;
  27. }
  28. switch (manifold->type)
  29. {
  30. case b2Manifold::e_circles:
  31. {
  32. normal.Set(1.0f, 0.0f);
  33. b2Vec2 pointA = b2Mul(xfA, manifold->localPoint);
  34. b2Vec2 pointB = b2Mul(xfB, manifold->points[0].localPoint);
  35. if (b2DistanceSquared(pointA, pointB) > b2_epsilon * b2_epsilon)
  36. {
  37. normal = pointB - pointA;
  38. normal.Normalize();
  39. }
  40. b2Vec2 cA = pointA + radiusA * normal;
  41. b2Vec2 cB = pointB - radiusB * normal;
  42. points[0] = 0.5f * (cA + cB);
  43. }
  44. break;
  45. case b2Manifold::e_faceA:
  46. {
  47. normal = b2Mul(xfA.R, manifold->localNormal);
  48. b2Vec2 planePoint = b2Mul(xfA, manifold->localPoint);
  49. for (int32 i = 0; i < manifold->pointCount; ++i)
  50. {
  51. b2Vec2 clipPoint = b2Mul(xfB, manifold->points[i].localPoint);
  52. b2Vec2 cA = clipPoint + (radiusA - b2Dot(clipPoint - planePoint, normal)) * normal;
  53. b2Vec2 cB = clipPoint - radiusB * normal;
  54. points[i] = 0.5f * (cA + cB);
  55. }
  56. }
  57. break;
  58. case b2Manifold::e_faceB:
  59. {
  60. normal = b2Mul(xfB.R, manifold->localNormal);
  61. b2Vec2 planePoint = b2Mul(xfB, manifold->localPoint);
  62. for (int32 i = 0; i < manifold->pointCount; ++i)
  63. {
  64. b2Vec2 clipPoint = b2Mul(xfA, manifold->points[i].localPoint);
  65. b2Vec2 cB = clipPoint + (radiusB - b2Dot(clipPoint - planePoint, normal)) * normal;
  66. b2Vec2 cA = clipPoint - radiusA * normal;
  67. points[i] = 0.5f * (cA + cB);
  68. }
  69. // Ensure normal points from A to B.
  70. normal = -normal;
  71. }
  72. break;
  73. }
  74. }
  75. void b2GetPointStates(b2PointState state1[b2_maxManifoldPoints], b2PointState state2[b2_maxManifoldPoints],
  76.   const b2Manifold* manifold1, const b2Manifold* manifold2)
  77. {
  78. for (int32 i = 0; i < b2_maxManifoldPoints; ++i)
  79. {
  80. state1[i] = b2_nullState;
  81. state2[i] = b2_nullState;
  82. }
  83. // Detect persists and removes.
  84. for (int32 i = 0; i < manifold1->pointCount; ++i)
  85. {
  86. b2ContactID id = manifold1->points[i].id;
  87. state1[i] = b2_removeState;
  88. for (int32 j = 0; j < manifold2->pointCount; ++j)
  89. {
  90. if (manifold2->points[j].id.key == id.key)
  91. {
  92. state1[i] = b2_persistState;
  93. break;
  94. }
  95. }
  96. }
  97. // Detect persists and adds.
  98. for (int32 i = 0; i < manifold2->pointCount; ++i)
  99. {
  100. b2ContactID id = manifold2->points[i].id;
  101. state2[i] = b2_addState;
  102. for (int32 j = 0; j < manifold1->pointCount; ++j)
  103. {
  104. if (manifold1->points[j].id.key == id.key)
  105. {
  106. state2[i] = b2_persistState;
  107. break;
  108. }
  109. }
  110. }
  111. }
  112. // From Real-time Collision Detection, p179.
  113. bool b2AABB::RayCast(b2RayCastOutput* output, const b2RayCastInput& input) const
  114. {
  115. float32 tmin = -b2_maxFloat;
  116. float32 tmax = b2_maxFloat;
  117. b2Vec2 p = input.p1;
  118. b2Vec2 d = input.p2 - input.p1;
  119. b2Vec2 absD = b2Abs(d);
  120. b2Vec2 normal;
  121. for (int32 i = 0; i < 2; ++i)
  122. {
  123. if (absD(i) < b2_epsilon)
  124. {
  125. // Parallel.
  126. if (p(i) < lowerBound(i) || upperBound(i) < p(i))
  127. {
  128. return false;
  129. }
  130. }
  131. else
  132. {
  133. float32 inv_d = 1.0f / d(i);
  134. float32 t1 = (lowerBound(i) - p(i)) * inv_d;
  135. float32 t2 = (upperBound(i) - p(i)) * inv_d;
  136. // Sign of the normal vector.
  137. float32 s = -1.0f;
  138. if (t1 > t2)
  139. {
  140. b2Swap(t1, t2);
  141. s = 1.0f;
  142. }
  143. // Push the min up
  144. if (t1 > tmin)
  145. {
  146. normal.SetZero();
  147. normal(i) = s;
  148. tmin = t1;
  149. }
  150. // Pull the max down
  151. tmax = b2Min(tmax, t2);
  152. if (tmin > tmax)
  153. {
  154. return false;
  155. }
  156. }
  157. }
  158. // Does the ray start inside the box?
  159. // Does the ray intersect beyond the max fraction?
  160. if (tmin < 0.0f || input.maxFraction < tmin)
  161. {
  162. return false;
  163. }
  164. // Intersection.
  165. output->fraction = tmin;
  166. output->normal = normal;
  167. return true;
  168. }
  169. // Sutherland-Hodgman clipping.
  170. int32 b2ClipSegmentToLine(b2ClipVertex vOut[2], const b2ClipVertex vIn[2],
  171. const b2Vec2& normal, float32 offset)
  172. {
  173. // Start with no output points
  174. int32 numOut = 0;
  175. // Calculate the distance of end points to the line
  176. float32 distance0 = b2Dot(normal, vIn[0].v) - offset;
  177. float32 distance1 = b2Dot(normal, vIn[1].v) - offset;
  178. // If the points are behind the plane
  179. if (distance0 <= 0.0f) vOut[numOut++] = vIn[0];
  180. if (distance1 <= 0.0f) vOut[numOut++] = vIn[1];
  181. // If the points are on different sides of the plane
  182. if (distance0 * distance1 < 0.0f)
  183. {
  184. // Find intersection point of edge and plane
  185. float32 interp = distance0 / (distance0 - distance1);
  186. vOut[numOut].v = vIn[0].v + interp * (vIn[1].v - vIn[0].v);
  187. if (distance0 > 0.0f)
  188. {
  189. vOut[numOut].id = vIn[0].id;
  190. }
  191. else
  192. {
  193. vOut[numOut].id = vIn[1].id;
  194. }
  195. ++numOut;
  196. }
  197. return numOut;
  198. }
  199. bool b2TestOverlap(const b2Shape* shapeA, const b2Shape* shapeB,
  200.    const b2Transform& xfA, const b2Transform& xfB)
  201. {
  202. b2DistanceInput input;
  203. input.proxyA.Set(shapeA);
  204. input.proxyB.Set(shapeB);
  205. input.transformA = xfA;
  206. input.transformB = xfB;
  207. input.useRadii = true;
  208. b2SimplexCache cache;
  209. cache.count = 0;
  210. b2DistanceOutput output;
  211. b2Distance(&output, &cache, &input);
  212. return output.distance < 10.0f * b2_epsilon;
  213. }