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

游戏引擎

开发平台:

Visual C++

  1. /*
  2. * Copyright (c) 2006-2010 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/Dynamics/Contacts/b2TOISolver.h>
  19. #include <Box2D/Dynamics/Contacts/b2Contact.h>
  20. #include <Box2D/Dynamics/b2Body.h>
  21. #include <Box2D/Dynamics/b2Fixture.h>
  22. #include <Box2D/Common/b2StackAllocator.h>
  23. struct b2TOIConstraint
  24. {
  25. b2Vec2 localPoints[b2_maxManifoldPoints];
  26. b2Vec2 localNormal;
  27. b2Vec2 localPoint;
  28. b2Manifold::Type type;
  29. float32 radius;
  30. int32 pointCount;
  31. b2Body* bodyA;
  32. b2Body* bodyB;
  33. };
  34. b2TOISolver::b2TOISolver(b2StackAllocator* allocator)
  35. {
  36. m_allocator = allocator;
  37. m_constraints = NULL;
  38. m_count = NULL;
  39. m_toiBody = NULL;
  40. }
  41. b2TOISolver::~b2TOISolver()
  42. {
  43. Clear();
  44. }
  45. void b2TOISolver::Clear()
  46. {
  47. if (m_allocator && m_constraints)
  48. {
  49. m_allocator->Free(m_constraints);
  50. m_constraints = NULL;
  51. }
  52. }
  53. void b2TOISolver::Initialize(b2Contact** contacts, int32 count, b2Body* toiBody)
  54. {
  55. Clear();
  56. m_count = count;
  57. m_toiBody = toiBody;
  58. m_constraints = (b2TOIConstraint*) m_allocator->Allocate(m_count * sizeof(b2TOIConstraint));
  59. for (int32 i = 0; i < m_count; ++i)
  60. {
  61. b2Contact* contact = contacts[i];
  62. b2Fixture* fixtureA = contact->GetFixtureA();
  63. b2Fixture* fixtureB = contact->GetFixtureB();
  64. b2Shape* shapeA = fixtureA->GetShape();
  65. b2Shape* shapeB = fixtureB->GetShape();
  66. float32 radiusA = shapeA->m_radius;
  67. float32 radiusB = shapeB->m_radius;
  68. b2Body* bodyA = fixtureA->GetBody();
  69. b2Body* bodyB = fixtureB->GetBody();
  70. b2Manifold* manifold = contact->GetManifold();
  71. b2Assert(manifold->pointCount > 0);
  72. b2TOIConstraint* constraint = m_constraints + i;
  73. constraint->bodyA = bodyA;
  74. constraint->bodyB = bodyB;
  75. constraint->localNormal = manifold->localNormal;
  76. constraint->localPoint = manifold->localPoint;
  77. constraint->type = manifold->type;
  78. constraint->pointCount = manifold->pointCount;
  79. constraint->radius = radiusA + radiusB;
  80. for (int32 j = 0; j < constraint->pointCount; ++j)
  81. {
  82. b2ManifoldPoint* cp = manifold->points + j;
  83. constraint->localPoints[j] = cp->localPoint;
  84. }
  85. }
  86. }
  87. struct b2TOISolverManifold
  88. {
  89. void Initialize(b2TOIConstraint* cc, int32 index)
  90. {
  91. b2Assert(cc->pointCount > 0);
  92. switch (cc->type)
  93. {
  94. case b2Manifold::e_circles:
  95. {
  96. b2Vec2 pointA = cc->bodyA->GetWorldPoint(cc->localPoint);
  97. b2Vec2 pointB = cc->bodyB->GetWorldPoint(cc->localPoints[0]);
  98. if (b2DistanceSquared(pointA, pointB) > b2_epsilon * b2_epsilon)
  99. {
  100. normal = pointB - pointA;
  101. normal.Normalize();
  102. }
  103. else
  104. {
  105. normal.Set(1.0f, 0.0f);
  106. }
  107. point = 0.5f * (pointA + pointB);
  108. separation = b2Dot(pointB - pointA, normal) - cc->radius;
  109. }
  110. break;
  111. case b2Manifold::e_faceA:
  112. {
  113. normal = cc->bodyA->GetWorldVector(cc->localNormal);
  114. b2Vec2 planePoint = cc->bodyA->GetWorldPoint(cc->localPoint);
  115. b2Vec2 clipPoint = cc->bodyB->GetWorldPoint(cc->localPoints[index]);
  116. separation = b2Dot(clipPoint - planePoint, normal) - cc->radius;
  117. point = clipPoint;
  118. }
  119. break;
  120. case b2Manifold::e_faceB:
  121. {
  122. normal = cc->bodyB->GetWorldVector(cc->localNormal);
  123. b2Vec2 planePoint = cc->bodyB->GetWorldPoint(cc->localPoint);
  124. b2Vec2 clipPoint = cc->bodyA->GetWorldPoint(cc->localPoints[index]);
  125. separation = b2Dot(clipPoint - planePoint, normal) - cc->radius;
  126. point = clipPoint;
  127. // Ensure normal points from A to B
  128. normal = -normal;
  129. }
  130. break;
  131. }
  132. }
  133. b2Vec2 normal;
  134. b2Vec2 point;
  135. float32 separation;
  136. };
  137. // Push out the toi body to provide clearance for further simulation.
  138. bool b2TOISolver::Solve(float32 baumgarte)
  139. {
  140. float32 minSeparation = 0.0f;
  141. for (int32 i = 0; i < m_count; ++i)
  142. {
  143. b2TOIConstraint* c = m_constraints + i;
  144. b2Body* bodyA = c->bodyA;
  145. b2Body* bodyB = c->bodyB;
  146. float32 massA = bodyA->m_mass;
  147. float32 massB = bodyB->m_mass;
  148. // Only the TOI body should move.
  149. if (bodyA == m_toiBody)
  150. {
  151. massB = 0.0f;
  152. }
  153. else
  154. {
  155. massA = 0.0f;
  156. }
  157. float32 invMassA = massA * bodyA->m_invMass;
  158. float32 invIA = massA * bodyA->m_invI;
  159. float32 invMassB = massB * bodyB->m_invMass;
  160. float32 invIB = massB * bodyB->m_invI;
  161. // Solve normal constraints
  162. for (int32 j = 0; j < c->pointCount; ++j)
  163. {
  164. b2TOISolverManifold psm;
  165. psm.Initialize(c, j);
  166. b2Vec2 normal = psm.normal;
  167. b2Vec2 point = psm.point;
  168. float32 separation = psm.separation;
  169. b2Vec2 rA = point - bodyA->m_sweep.c;
  170. b2Vec2 rB = point - bodyB->m_sweep.c;
  171. // Track max constraint error.
  172. minSeparation = b2Min(minSeparation, separation);
  173. // Prevent large corrections and allow slop.
  174. float32 C = b2Clamp(baumgarte * (separation + b2_linearSlop), -b2_maxLinearCorrection, 0.0f);
  175. // Compute the effective mass.
  176. float32 rnA = b2Cross(rA, normal);
  177. float32 rnB = b2Cross(rB, normal);
  178. float32 K = invMassA + invMassB + invIA * rnA * rnA + invIB * rnB * rnB;
  179. // Compute normal impulse
  180. float32 impulse = K > 0.0f ? - C / K : 0.0f;
  181. b2Vec2 P = impulse * normal;
  182. bodyA->m_sweep.c -= invMassA * P;
  183. bodyA->m_sweep.a -= invIA * b2Cross(rA, P);
  184. bodyA->SynchronizeTransform();
  185. bodyB->m_sweep.c += invMassB * P;
  186. bodyB->m_sweep.a += invIB * b2Cross(rB, P);
  187. bodyB->SynchronizeTransform();
  188. }
  189. }
  190. // We can't expect minSpeparation >= -b2_linearSlop because we don't
  191. // push the separation above -b2_linearSlop.
  192. return minSeparation >= -1.5f * b2_linearSlop;
  193. }