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

游戏引擎

开发平台:

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/Dynamics/Contacts/b2ContactSolver.h>
  19. #include <Box2D/Dynamics/Contacts/b2Contact.h>
  20. #include <Box2D/Dynamics/b2Body.h>
  21. #include <Box2D/Dynamics/b2Fixture.h>
  22. #include <Box2D/Dynamics/b2World.h>
  23. #include <Box2D/Common/b2StackAllocator.h>
  24. #define B2_DEBUG_SOLVER 0
  25. b2ContactSolver::b2ContactSolver(b2Contact** contacts, int32 contactCount,
  26. b2StackAllocator* allocator, float32 impulseRatio)
  27. {
  28. m_allocator = allocator;
  29. m_constraintCount = contactCount;
  30. m_constraints = (b2ContactConstraint*)m_allocator->Allocate(m_constraintCount * sizeof(b2ContactConstraint));
  31. for (int32 i = 0; i < m_constraintCount; ++i)
  32. {
  33. b2Contact* contact = contacts[i];
  34. b2Fixture* fixtureA = contact->m_fixtureA;
  35. b2Fixture* fixtureB = contact->m_fixtureB;
  36. b2Shape* shapeA = fixtureA->GetShape();
  37. b2Shape* shapeB = fixtureB->GetShape();
  38. float32 radiusA = shapeA->m_radius;
  39. float32 radiusB = shapeB->m_radius;
  40. b2Body* bodyA = fixtureA->GetBody();
  41. b2Body* bodyB = fixtureB->GetBody();
  42. b2Manifold* manifold = contact->GetManifold();
  43. float32 friction = b2MixFriction(fixtureA->GetFriction(), fixtureB->GetFriction());
  44. float32 restitution = b2MixRestitution(fixtureA->GetRestitution(), fixtureB->GetRestitution());
  45. b2Vec2 vA = bodyA->m_linearVelocity;
  46. b2Vec2 vB = bodyB->m_linearVelocity;
  47. float32 wA = bodyA->m_angularVelocity;
  48. float32 wB = bodyB->m_angularVelocity;
  49. b2Assert(manifold->pointCount > 0);
  50. b2WorldManifold worldManifold;
  51. worldManifold.Initialize(manifold, bodyA->m_xf, radiusA, bodyB->m_xf, radiusB);
  52. b2ContactConstraint* cc = m_constraints + i;
  53. cc->bodyA = bodyA;
  54. cc->bodyB = bodyB;
  55. cc->manifold = manifold;
  56. cc->normal = worldManifold.normal;
  57. cc->pointCount = manifold->pointCount;
  58. cc->friction = friction;
  59. cc->localNormal = manifold->localNormal;
  60. cc->localPoint = manifold->localPoint;
  61. cc->radius = radiusA + radiusB;
  62. cc->type = manifold->type;
  63. for (int32 j = 0; j < cc->pointCount; ++j)
  64. {
  65. b2ManifoldPoint* cp = manifold->points + j;
  66. b2ContactConstraintPoint* ccp = cc->points + j;
  67. ccp->normalImpulse = impulseRatio * cp->normalImpulse;
  68. ccp->tangentImpulse = impulseRatio * cp->tangentImpulse;
  69. ccp->localPoint = cp->localPoint;
  70. ccp->rA = worldManifold.points[j] - bodyA->m_sweep.c;
  71. ccp->rB = worldManifold.points[j] - bodyB->m_sweep.c;
  72. float32 rnA = b2Cross(ccp->rA, cc->normal);
  73. float32 rnB = b2Cross(ccp->rB, cc->normal);
  74. rnA *= rnA;
  75. rnB *= rnB;
  76. float32 kNormal = bodyA->m_invMass + bodyB->m_invMass + bodyA->m_invI * rnA + bodyB->m_invI * rnB;
  77. b2Assert(kNormal > b2_epsilon);
  78. ccp->normalMass = 1.0f / kNormal;
  79. b2Vec2 tangent = b2Cross(cc->normal, 1.0f);
  80. float32 rtA = b2Cross(ccp->rA, tangent);
  81. float32 rtB = b2Cross(ccp->rB, tangent);
  82. rtA *= rtA;
  83. rtB *= rtB;
  84. float32 kTangent = bodyA->m_invMass + bodyB->m_invMass + bodyA->m_invI * rtA + bodyB->m_invI * rtB;
  85. b2Assert(kTangent > b2_epsilon);
  86. ccp->tangentMass = 1.0f /  kTangent;
  87. // Setup a velocity bias for restitution.
  88. ccp->velocityBias = 0.0f;
  89. float32 vRel = b2Dot(cc->normal, vB + b2Cross(wB, ccp->rB) - vA - b2Cross(wA, ccp->rA));
  90. if (vRel < -b2_velocityThreshold)
  91. {
  92. ccp->velocityBias = -restitution * vRel;
  93. }
  94. }
  95. // If we have two points, then prepare the block solver.
  96. if (cc->pointCount == 2)
  97. {
  98. b2ContactConstraintPoint* ccp1 = cc->points + 0;
  99. b2ContactConstraintPoint* ccp2 = cc->points + 1;
  100. float32 invMassA = bodyA->m_invMass;
  101. float32 invIA = bodyA->m_invI;
  102. float32 invMassB = bodyB->m_invMass;
  103. float32 invIB = bodyB->m_invI;
  104. float32 rn1A = b2Cross(ccp1->rA, cc->normal);
  105. float32 rn1B = b2Cross(ccp1->rB, cc->normal);
  106. float32 rn2A = b2Cross(ccp2->rA, cc->normal);
  107. float32 rn2B = b2Cross(ccp2->rB, cc->normal);
  108. float32 k11 = invMassA + invMassB + invIA * rn1A * rn1A + invIB * rn1B * rn1B;
  109. float32 k22 = invMassA + invMassB + invIA * rn2A * rn2A + invIB * rn2B * rn2B;
  110. float32 k12 = invMassA + invMassB + invIA * rn1A * rn2A + invIB * rn1B * rn2B;
  111. // Ensure a reasonable condition number.
  112. const float32 k_maxConditionNumber = 100.0f;
  113. if (k11 * k11 < k_maxConditionNumber * (k11 * k22 - k12 * k12))
  114. {
  115. // K is safe to invert.
  116. cc->K.col1.Set(k11, k12);
  117. cc->K.col2.Set(k12, k22);
  118. cc->normalMass = cc->K.GetInverse();
  119. }
  120. else
  121. {
  122. // The constraints are redundant, just use one.
  123. // TODO_ERIN use deepest?
  124. cc->pointCount = 1;
  125. }
  126. }
  127. }
  128. }
  129. b2ContactSolver::~b2ContactSolver()
  130. {
  131. m_allocator->Free(m_constraints);
  132. }
  133. void b2ContactSolver::WarmStart()
  134. {
  135. // Warm start.
  136. for (int32 i = 0; i < m_constraintCount; ++i)
  137. {
  138. b2ContactConstraint* c = m_constraints + i;
  139. b2Body* bodyA = c->bodyA;
  140. b2Body* bodyB = c->bodyB;
  141. float32 invMassA = bodyA->m_invMass;
  142. float32 invIA = bodyA->m_invI;
  143. float32 invMassB = bodyB->m_invMass;
  144. float32 invIB = bodyB->m_invI;
  145. b2Vec2 normal = c->normal;
  146. b2Vec2 tangent = b2Cross(normal, 1.0f);
  147. for (int32 j = 0; j < c->pointCount; ++j)
  148. {
  149. b2ContactConstraintPoint* ccp = c->points + j;
  150. b2Vec2 P = ccp->normalImpulse * normal + ccp->tangentImpulse * tangent;
  151. bodyA->m_angularVelocity -= invIA * b2Cross(ccp->rA, P);
  152. bodyA->m_linearVelocity -= invMassA * P;
  153. bodyB->m_angularVelocity += invIB * b2Cross(ccp->rB, P);
  154. bodyB->m_linearVelocity += invMassB * P;
  155. }
  156. }
  157. }
  158. void b2ContactSolver::SolveVelocityConstraints()
  159. {
  160. for (int32 i = 0; i < m_constraintCount; ++i)
  161. {
  162. b2ContactConstraint* c = m_constraints + i;
  163. b2Body* bodyA = c->bodyA;
  164. b2Body* bodyB = c->bodyB;
  165. float32 wA = bodyA->m_angularVelocity;
  166. float32 wB = bodyB->m_angularVelocity;
  167. b2Vec2 vA = bodyA->m_linearVelocity;
  168. b2Vec2 vB = bodyB->m_linearVelocity;
  169. float32 invMassA = bodyA->m_invMass;
  170. float32 invIA = bodyA->m_invI;
  171. float32 invMassB = bodyB->m_invMass;
  172. float32 invIB = bodyB->m_invI;
  173. b2Vec2 normal = c->normal;
  174. b2Vec2 tangent = b2Cross(normal, 1.0f);
  175. float32 friction = c->friction;
  176. b2Assert(c->pointCount == 1 || c->pointCount == 2);
  177. // Solve tangent constraints
  178. for (int32 j = 0; j < c->pointCount; ++j)
  179. {
  180. b2ContactConstraintPoint* ccp = c->points + j;
  181. // Relative velocity at contact
  182. b2Vec2 dv = vB + b2Cross(wB, ccp->rB) - vA - b2Cross(wA, ccp->rA);
  183. // Compute tangent force
  184. float32 vt = b2Dot(dv, tangent);
  185. float32 lambda = ccp->tangentMass * (-vt);
  186. // b2Clamp the accumulated force
  187. float32 maxFriction = friction * ccp->normalImpulse;
  188. float32 newImpulse = b2Clamp(ccp->tangentImpulse + lambda, -maxFriction, maxFriction);
  189. lambda = newImpulse - ccp->tangentImpulse;
  190. // Apply contact impulse
  191. b2Vec2 P = lambda * tangent;
  192. vA -= invMassA * P;
  193. wA -= invIA * b2Cross(ccp->rA, P);
  194. vB += invMassB * P;
  195. wB += invIB * b2Cross(ccp->rB, P);
  196. ccp->tangentImpulse = newImpulse;
  197. }
  198. // Solve normal constraints
  199. if (c->pointCount == 1)
  200. {
  201. b2ContactConstraintPoint* ccp = c->points + 0;
  202. // Relative velocity at contact
  203. b2Vec2 dv = vB + b2Cross(wB, ccp->rB) - vA - b2Cross(wA, ccp->rA);
  204. // Compute normal impulse
  205. float32 vn = b2Dot(dv, normal);
  206. float32 lambda = -ccp->normalMass * (vn - ccp->velocityBias);
  207. // b2Clamp the accumulated impulse
  208. float32 newImpulse = b2Max(ccp->normalImpulse + lambda, 0.0f);
  209. lambda = newImpulse - ccp->normalImpulse;
  210. // Apply contact impulse
  211. b2Vec2 P = lambda * normal;
  212. vA -= invMassA * P;
  213. wA -= invIA * b2Cross(ccp->rA, P);
  214. vB += invMassB * P;
  215. wB += invIB * b2Cross(ccp->rB, P);
  216. ccp->normalImpulse = newImpulse;
  217. }
  218. else
  219. {
  220. // Block solver developed in collaboration with Dirk Gregorius (back in 01/07 on Box2D_Lite).
  221. // Build the mini LCP for this contact patch
  222. //
  223. // vn = A * x + b, vn >= 0, , vn >= 0, x >= 0 and vn_i * x_i = 0 with i = 1..2
  224. //
  225. // A = J * W * JT and J = ( -n, -r1 x n, n, r2 x n )
  226. // b = vn_0 - velocityBias
  227. //
  228. // The system is solved using the "Total enumeration method" (s. Murty). The complementary constraint vn_i * x_i
  229. // implies that we must have in any solution either vn_i = 0 or x_i = 0. So for the 2D contact problem the cases
  230. // vn1 = 0 and vn2 = 0, x1 = 0 and x2 = 0, x1 = 0 and vn2 = 0, x2 = 0 and vn1 = 0 need to be tested. The first valid
  231. // solution that satisfies the problem is chosen.
  232. // 
  233. // In order to account of the accumulated impulse 'a' (because of the iterative nature of the solver which only requires
  234. // that the accumulated impulse is clamped and not the incremental impulse) we change the impulse variable (x_i).
  235. //
  236. // Substitute:
  237. // 
  238. // x = x' - a
  239. // 
  240. // Plug into above equation:
  241. //
  242. // vn = A * x + b
  243. //    = A * (x' - a) + b
  244. //    = A * x' + b - A * a
  245. //    = A * x' + b'
  246. // b' = b - A * a;
  247. b2ContactConstraintPoint* cp1 = c->points + 0;
  248. b2ContactConstraintPoint* cp2 = c->points + 1;
  249. b2Vec2 a(cp1->normalImpulse, cp2->normalImpulse);
  250. b2Assert(a.x >= 0.0f && a.y >= 0.0f);
  251. // Relative velocity at contact
  252. b2Vec2 dv1 = vB + b2Cross(wB, cp1->rB) - vA - b2Cross(wA, cp1->rA);
  253. b2Vec2 dv2 = vB + b2Cross(wB, cp2->rB) - vA - b2Cross(wA, cp2->rA);
  254. // Compute normal velocity
  255. float32 vn1 = b2Dot(dv1, normal);
  256. float32 vn2 = b2Dot(dv2, normal);
  257. b2Vec2 b;
  258. b.x = vn1 - cp1->velocityBias;
  259. b.y = vn2 - cp2->velocityBias;
  260. b -= b2Mul(c->K, a);
  261. const float32 k_errorTol = 1e-3f;
  262. B2_NOT_USED(k_errorTol);
  263. for (;;)
  264. {
  265. //
  266. // Case 1: vn = 0
  267. //
  268. // 0 = A * x' + b'
  269. //
  270. // Solve for x':
  271. //
  272. // x' = - inv(A) * b'
  273. //
  274. b2Vec2 x = - b2Mul(c->normalMass, b);
  275. if (x.x >= 0.0f && x.y >= 0.0f)
  276. {
  277. // Resubstitute for the incremental impulse
  278. b2Vec2 d = x - a;
  279. // Apply incremental impulse
  280. b2Vec2 P1 = d.x * normal;
  281. b2Vec2 P2 = d.y * normal;
  282. vA -= invMassA * (P1 + P2);
  283. wA -= invIA * (b2Cross(cp1->rA, P1) + b2Cross(cp2->rA, P2));
  284. vB += invMassB * (P1 + P2);
  285. wB += invIB * (b2Cross(cp1->rB, P1) + b2Cross(cp2->rB, P2));
  286. // Accumulate
  287. cp1->normalImpulse = x.x;
  288. cp2->normalImpulse = x.y;
  289. #if B2_DEBUG_SOLVER == 1
  290. // Postconditions
  291. dv1 = vB + b2Cross(wB, cp1->rB) - vA - b2Cross(wA, cp1->rA);
  292. dv2 = vB + b2Cross(wB, cp2->rB) - vA - b2Cross(wA, cp2->rA);
  293. // Compute normal velocity
  294. vn1 = b2Dot(dv1, normal);
  295. vn2 = b2Dot(dv2, normal);
  296. b2Assert(b2Abs(vn1 - cp1->velocityBias) < k_errorTol);
  297. b2Assert(b2Abs(vn2 - cp2->velocityBias) < k_errorTol);
  298. #endif
  299. break;
  300. }
  301. //
  302. // Case 2: vn1 = 0 and x2 = 0
  303. //
  304. //   0 = a11 * x1' + a12 * 0 + b1' 
  305. // vn2 = a21 * x1' + a22 * 0 + b2'
  306. //
  307. x.x = - cp1->normalMass * b.x;
  308. x.y = 0.0f;
  309. vn1 = 0.0f;
  310. vn2 = c->K.col1.y * x.x + b.y;
  311. if (x.x >= 0.0f && vn2 >= 0.0f)
  312. {
  313. // Resubstitute for the incremental impulse
  314. b2Vec2 d = x - a;
  315. // Apply incremental impulse
  316. b2Vec2 P1 = d.x * normal;
  317. b2Vec2 P2 = d.y * normal;
  318. vA -= invMassA * (P1 + P2);
  319. wA -= invIA * (b2Cross(cp1->rA, P1) + b2Cross(cp2->rA, P2));
  320. vB += invMassB * (P1 + P2);
  321. wB += invIB * (b2Cross(cp1->rB, P1) + b2Cross(cp2->rB, P2));
  322. // Accumulate
  323. cp1->normalImpulse = x.x;
  324. cp2->normalImpulse = x.y;
  325. #if B2_DEBUG_SOLVER == 1
  326. // Postconditions
  327. dv1 = vB + b2Cross(wB, cp1->rB) - vA - b2Cross(wA, cp1->rA);
  328. // Compute normal velocity
  329. vn1 = b2Dot(dv1, normal);
  330. b2Assert(b2Abs(vn1 - cp1->velocityBias) < k_errorTol);
  331. #endif
  332. break;
  333. }
  334. //
  335. // Case 3: vn2 = 0 and x1 = 0
  336. //
  337. // vn1 = a11 * 0 + a12 * x2' + b1' 
  338. //   0 = a21 * 0 + a22 * x2' + b2'
  339. //
  340. x.x = 0.0f;
  341. x.y = - cp2->normalMass * b.y;
  342. vn1 = c->K.col2.x * x.y + b.x;
  343. vn2 = 0.0f;
  344. if (x.y >= 0.0f && vn1 >= 0.0f)
  345. {
  346. // Resubstitute for the incremental impulse
  347. b2Vec2 d = x - a;
  348. // Apply incremental impulse
  349. b2Vec2 P1 = d.x * normal;
  350. b2Vec2 P2 = d.y * normal;
  351. vA -= invMassA * (P1 + P2);
  352. wA -= invIA * (b2Cross(cp1->rA, P1) + b2Cross(cp2->rA, P2));
  353. vB += invMassB * (P1 + P2);
  354. wB += invIB * (b2Cross(cp1->rB, P1) + b2Cross(cp2->rB, P2));
  355. // Accumulate
  356. cp1->normalImpulse = x.x;
  357. cp2->normalImpulse = x.y;
  358. #if B2_DEBUG_SOLVER == 1
  359. // Postconditions
  360. dv2 = vB + b2Cross(wB, cp2->rB) - vA - b2Cross(wA, cp2->rA);
  361. // Compute normal velocity
  362. vn2 = b2Dot(dv2, normal);
  363. b2Assert(b2Abs(vn2 - cp2->velocityBias) < k_errorTol);
  364. #endif
  365. break;
  366. }
  367. //
  368. // Case 4: x1 = 0 and x2 = 0
  369. // 
  370. // vn1 = b1
  371. // vn2 = b2;
  372. x.x = 0.0f;
  373. x.y = 0.0f;
  374. vn1 = b.x;
  375. vn2 = b.y;
  376. if (vn1 >= 0.0f && vn2 >= 0.0f )
  377. {
  378. // Resubstitute for the incremental impulse
  379. b2Vec2 d = x - a;
  380. // Apply incremental impulse
  381. b2Vec2 P1 = d.x * normal;
  382. b2Vec2 P2 = d.y * normal;
  383. vA -= invMassA * (P1 + P2);
  384. wA -= invIA * (b2Cross(cp1->rA, P1) + b2Cross(cp2->rA, P2));
  385. vB += invMassB * (P1 + P2);
  386. wB += invIB * (b2Cross(cp1->rB, P1) + b2Cross(cp2->rB, P2));
  387. // Accumulate
  388. cp1->normalImpulse = x.x;
  389. cp2->normalImpulse = x.y;
  390. break;
  391. }
  392. // No solution, give up. This is hit sometimes, but it doesn't seem to matter.
  393. break;
  394. }
  395. }
  396. bodyA->m_linearVelocity = vA;
  397. bodyA->m_angularVelocity = wA;
  398. bodyB->m_linearVelocity = vB;
  399. bodyB->m_angularVelocity = wB;
  400. }
  401. }
  402. void b2ContactSolver::StoreImpulses()
  403. {
  404. for (int32 i = 0; i < m_constraintCount; ++i)
  405. {
  406. b2ContactConstraint* c = m_constraints + i;
  407. b2Manifold* m = c->manifold;
  408. for (int32 j = 0; j < c->pointCount; ++j)
  409. {
  410. m->points[j].normalImpulse = c->points[j].normalImpulse;
  411. m->points[j].tangentImpulse = c->points[j].tangentImpulse;
  412. }
  413. }
  414. }
  415. struct b2PositionSolverManifold
  416. {
  417. void Initialize(b2ContactConstraint* cc, int32 index)
  418. {
  419. b2Assert(cc->pointCount > 0);
  420. switch (cc->type)
  421. {
  422. case b2Manifold::e_circles:
  423. {
  424. b2Vec2 pointA = cc->bodyA->GetWorldPoint(cc->localPoint);
  425. b2Vec2 pointB = cc->bodyB->GetWorldPoint(cc->points[0].localPoint);
  426. if (b2DistanceSquared(pointA, pointB) > b2_epsilon * b2_epsilon)
  427. {
  428. normal = pointB - pointA;
  429. normal.Normalize();
  430. }
  431. else
  432. {
  433. normal.Set(1.0f, 0.0f);
  434. }
  435. point = 0.5f * (pointA + pointB);
  436. separation = b2Dot(pointB - pointA, normal) - cc->radius;
  437. }
  438. break;
  439. case b2Manifold::e_faceA:
  440. {
  441. normal = cc->bodyA->GetWorldVector(cc->localNormal);
  442. b2Vec2 planePoint = cc->bodyA->GetWorldPoint(cc->localPoint);
  443. b2Vec2 clipPoint = cc->bodyB->GetWorldPoint(cc->points[index].localPoint);
  444. separation = b2Dot(clipPoint - planePoint, normal) - cc->radius;
  445. point = clipPoint;
  446. }
  447. break;
  448. case b2Manifold::e_faceB:
  449. {
  450. normal = cc->bodyB->GetWorldVector(cc->localNormal);
  451. b2Vec2 planePoint = cc->bodyB->GetWorldPoint(cc->localPoint);
  452. b2Vec2 clipPoint = cc->bodyA->GetWorldPoint(cc->points[index].localPoint);
  453. separation = b2Dot(clipPoint - planePoint, normal) - cc->radius;
  454. point = clipPoint;
  455. // Ensure normal points from A to B
  456. normal = -normal;
  457. }
  458. break;
  459. }
  460. }
  461. b2Vec2 normal;
  462. b2Vec2 point;
  463. float32 separation;
  464. };
  465. // Sequential solver.
  466. bool b2ContactSolver::SolvePositionConstraints(float32 baumgarte)
  467. {
  468. float32 minSeparation = 0.0f;
  469. for (int32 i = 0; i < m_constraintCount; ++i)
  470. {
  471. b2ContactConstraint* c = m_constraints + i;
  472. b2Body* bodyA = c->bodyA;
  473. b2Body* bodyB = c->bodyB;
  474. float32 invMassA = bodyA->m_mass * bodyA->m_invMass;
  475. float32 invIA = bodyA->m_mass * bodyA->m_invI;
  476. float32 invMassB = bodyB->m_mass * bodyB->m_invMass;
  477. float32 invIB = bodyB->m_mass * bodyB->m_invI;
  478. // Solve normal constraints
  479. for (int32 j = 0; j < c->pointCount; ++j)
  480. {
  481. b2PositionSolverManifold psm;
  482. psm.Initialize(c, j);
  483. b2Vec2 normal = psm.normal;
  484. b2Vec2 point = psm.point;
  485. float32 separation = psm.separation;
  486. b2Vec2 rA = point - bodyA->m_sweep.c;
  487. b2Vec2 rB = point - bodyB->m_sweep.c;
  488. // Track max constraint error.
  489. minSeparation = b2Min(minSeparation, separation);
  490. // Prevent large corrections and allow slop.
  491. float32 C = b2Clamp(baumgarte * (separation + b2_linearSlop), -b2_maxLinearCorrection, 0.0f);
  492. // Compute the effective mass.
  493. float32 rnA = b2Cross(rA, normal);
  494. float32 rnB = b2Cross(rB, normal);
  495. float32 K = invMassA + invMassB + invIA * rnA * rnA + invIB * rnB * rnB;
  496. // Compute normal impulse
  497. float32 impulse = K > 0.0f ? - C / K : 0.0f;
  498. b2Vec2 P = impulse * normal;
  499. bodyA->m_sweep.c -= invMassA * P;
  500. bodyA->m_sweep.a -= invIA * b2Cross(rA, P);
  501. bodyA->SynchronizeTransform();
  502. bodyB->m_sweep.c += invMassB * P;
  503. bodyB->m_sweep.a += invIB * b2Cross(rB, P);
  504. bodyB->SynchronizeTransform();
  505. }
  506. }
  507. // We can't expect minSpeparation >= -b2_linearSlop because we don't
  508. // push the separation above -b2_linearSlop.
  509. return minSeparation >= -1.5f * b2_linearSlop;
  510. }