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

游戏引擎

开发平台:

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/Shapes/b2CircleShape.h>
  20. #include <Box2D/Collision/Shapes/b2PolygonShape.h>
  21. void b2CollideCircles(
  22. b2Manifold* manifold,
  23. const b2CircleShape* circleA, const b2Transform& xfA,
  24. const b2CircleShape* circleB, const b2Transform& xfB)
  25. {
  26. manifold->pointCount = 0;
  27. b2Vec2 pA = b2Mul(xfA, circleA->m_p);
  28. b2Vec2 pB = b2Mul(xfB, circleB->m_p);
  29. b2Vec2 d = pB - pA;
  30. float32 distSqr = b2Dot(d, d);
  31. float32 rA = circleA->m_radius, rB = circleB->m_radius;
  32. float32 radius = rA + rB;
  33. if (distSqr > radius * radius)
  34. {
  35. return;
  36. }
  37. manifold->type = b2Manifold::e_circles;
  38. manifold->localPoint = circleA->m_p;
  39. manifold->localNormal.SetZero();
  40. manifold->pointCount = 1;
  41. manifold->points[0].localPoint = circleB->m_p;
  42. manifold->points[0].id.key = 0;
  43. }
  44. void b2CollidePolygonAndCircle(
  45. b2Manifold* manifold,
  46. const b2PolygonShape* polygonA, const b2Transform& xfA,
  47. const b2CircleShape* circleB, const b2Transform& xfB)
  48. {
  49. manifold->pointCount = 0;
  50. // Compute circle position in the frame of the polygon.
  51. b2Vec2 c = b2Mul(xfB, circleB->m_p);
  52. b2Vec2 cLocal = b2MulT(xfA, c);
  53. // Find the min separating edge.
  54. int32 normalIndex = 0;
  55. float32 separation = -b2_maxFloat;
  56. float32 radius = polygonA->m_radius + circleB->m_radius;
  57. int32 vertexCount = polygonA->m_vertexCount;
  58. const b2Vec2* vertices = polygonA->m_vertices;
  59. const b2Vec2* normals = polygonA->m_normals;
  60. for (int32 i = 0; i < vertexCount; ++i)
  61. {
  62. float32 s = b2Dot(normals[i], cLocal - vertices[i]);
  63. if (s > radius)
  64. {
  65. // Early out.
  66. return;
  67. }
  68. if (s > separation)
  69. {
  70. separation = s;
  71. normalIndex = i;
  72. }
  73. }
  74. // Vertices that subtend the incident face.
  75. int32 vertIndex1 = normalIndex;
  76. int32 vertIndex2 = vertIndex1 + 1 < vertexCount ? vertIndex1 + 1 : 0;
  77. b2Vec2 v1 = vertices[vertIndex1];
  78. b2Vec2 v2 = vertices[vertIndex2];
  79. // If the center is inside the polygon ...
  80. if (separation < b2_epsilon)
  81. {
  82. manifold->pointCount = 1;
  83. manifold->type = b2Manifold::e_faceA;
  84. manifold->localNormal = normals[normalIndex];
  85. manifold->localPoint = 0.5f * (v1 + v2);
  86. manifold->points[0].localPoint = circleB->m_p;
  87. manifold->points[0].id.key = 0;
  88. return;
  89. }
  90. // Compute barycentric coordinates
  91. float32 u1 = b2Dot(cLocal - v1, v2 - v1);
  92. float32 u2 = b2Dot(cLocal - v2, v1 - v2);
  93. if (u1 <= 0.0f)
  94. {
  95. if (b2DistanceSquared(cLocal, v1) > radius * radius)
  96. {
  97. return;
  98. }
  99. manifold->pointCount = 1;
  100. manifold->type = b2Manifold::e_faceA;
  101. manifold->localNormal = cLocal - v1;
  102. manifold->localNormal.Normalize();
  103. manifold->localPoint = v1;
  104. manifold->points[0].localPoint = circleB->m_p;
  105. manifold->points[0].id.key = 0;
  106. }
  107. else if (u2 <= 0.0f)
  108. {
  109. if (b2DistanceSquared(cLocal, v2) > radius * radius)
  110. {
  111. return;
  112. }
  113. manifold->pointCount = 1;
  114. manifold->type = b2Manifold::e_faceA;
  115. manifold->localNormal = cLocal - v2;
  116. manifold->localNormal.Normalize();
  117. manifold->localPoint = v2;
  118. manifold->points[0].localPoint = circleB->m_p;
  119. manifold->points[0].id.key = 0;
  120. }
  121. else
  122. {
  123. b2Vec2 faceCenter = 0.5f * (v1 + v2);
  124. float32 separation = b2Dot(cLocal - faceCenter, normals[vertIndex1]);
  125. if (separation > radius)
  126. {
  127. return;
  128. }
  129. manifold->pointCount = 1;
  130. manifold->type = b2Manifold::e_faceA;
  131. manifold->localNormal = normals[vertIndex1];
  132. manifold->localPoint = faceCenter;
  133. manifold->points[0].localPoint = circleB->m_p;
  134. manifold->points[0].id.key = 0;
  135. }
  136. }