1 /*
2 * Copyright (c) 2006-2011 Erin Catto http://www.box2d.org
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
19 #include <Box2D/Dynamics/Contacts/b2ContactSolver.h>
20
21 #include <Box2D/Dynamics/Contacts/b2Contact.h>
22 #include <Box2D/Dynamics/b2Body.h>
23 #include <Box2D/Dynamics/b2Fixture.h>
24 #include <Box2D/Dynamics/b2World.h>
25 #include <Box2D/Common/b2StackAllocator.h>
26
27 #define B2_DEBUG_SOLVER 0
28
29 bool g_blockSolve = true;
30 float b2_velocityThreshold = 1.0f;
31 struct b2ContactPositionConstraint
32 {
33 b2Vec2 localPoints[b2_maxManifoldPoints];
34 b2Vec2 localNormal;
35 b2Vec2 localPoint;
36 int32 indexA;
37 int32 indexB;
38 float32 invMassA, invMassB;
39 b2Vec2 localCenterA, localCenterB;
40 float32 invIA, invIB;
41 b2Manifold::Type type;
42 float32 radiusA, radiusB;
43 int32 pointCount;
44 };
45
b2ContactSolver(b2ContactSolverDef * def)46 b2ContactSolver::b2ContactSolver(b2ContactSolverDef* def)
47 {
48 m_step = def->step;
49 m_allocator = def->allocator;
50 m_count = def->count;
51 m_positionConstraints = (b2ContactPositionConstraint*)m_allocator->Allocate(m_count * sizeof(b2ContactPositionConstraint));
52 m_velocityConstraints = (b2ContactVelocityConstraint*)m_allocator->Allocate(m_count * sizeof(b2ContactVelocityConstraint));
53 m_positions = def->positions;
54 m_velocities = def->velocities;
55 m_contacts = def->contacts;
56
57 // Initialize position independent portions of the constraints.
58 for (int32 i = 0; i < m_count; ++i)
59 {
60 b2Contact* contact = m_contacts[i];
61
62 b2Fixture* fixtureA = contact->m_fixtureA;
63 b2Fixture* fixtureB = contact->m_fixtureB;
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
72 int32 pointCount = manifold->pointCount;
73 b2Assert(pointCount > 0);
74
75 b2ContactVelocityConstraint* vc = m_velocityConstraints + i;
76 vc->friction = contact->m_friction;
77 vc->restitution = contact->m_restitution;
78 vc->tangentSpeed = contact->m_tangentSpeed;
79 vc->indexA = bodyA->m_islandIndex;
80 vc->indexB = bodyB->m_islandIndex;
81 vc->invMassA = bodyA->m_invMass;
82 vc->invMassB = bodyB->m_invMass;
83 vc->invIA = bodyA->m_invI;
84 vc->invIB = bodyB->m_invI;
85 vc->contactIndex = i;
86 vc->pointCount = pointCount;
87 vc->K.SetZero();
88 vc->normalMass.SetZero();
89
90 b2ContactPositionConstraint* pc = m_positionConstraints + i;
91 pc->indexA = bodyA->m_islandIndex;
92 pc->indexB = bodyB->m_islandIndex;
93 pc->invMassA = bodyA->m_invMass;
94 pc->invMassB = bodyB->m_invMass;
95 pc->localCenterA = bodyA->m_sweep.localCenter;
96 pc->localCenterB = bodyB->m_sweep.localCenter;
97 pc->invIA = bodyA->m_invI;
98 pc->invIB = bodyB->m_invI;
99 pc->localNormal = manifold->localNormal;
100 pc->localPoint = manifold->localPoint;
101 pc->pointCount = pointCount;
102 pc->radiusA = radiusA;
103 pc->radiusB = radiusB;
104 pc->type = manifold->type;
105
106 for (int32 j = 0; j < pointCount; ++j)
107 {
108 b2ManifoldPoint* cp = manifold->points + j;
109 b2VelocityConstraintPoint* vcp = vc->points + j;
110
111 if (m_step.warmStarting)
112 {
113 vcp->normalImpulse = m_step.dtRatio * cp->normalImpulse;
114 vcp->tangentImpulse = m_step.dtRatio * cp->tangentImpulse;
115 }
116 else
117 {
118 vcp->normalImpulse = 0.0f;
119 vcp->tangentImpulse = 0.0f;
120 }
121
122 vcp->rA.SetZero();
123 vcp->rB.SetZero();
124 vcp->normalMass = 0.0f;
125 vcp->tangentMass = 0.0f;
126 vcp->velocityBias = 0.0f;
127
128 pc->localPoints[j] = cp->localPoint;
129 }
130 }
131 }
132
~b2ContactSolver()133 b2ContactSolver::~b2ContactSolver()
134 {
135 m_allocator->Free(m_velocityConstraints);
136 m_allocator->Free(m_positionConstraints);
137 }
138
139 // Initialize position dependent portions of the velocity constraints.
InitializeVelocityConstraints()140 void b2ContactSolver::InitializeVelocityConstraints()
141 {
142 for (int32 i = 0; i < m_count; ++i)
143 {
144 b2ContactVelocityConstraint* vc = m_velocityConstraints + i;
145 b2ContactPositionConstraint* pc = m_positionConstraints + i;
146
147 float32 radiusA = pc->radiusA;
148 float32 radiusB = pc->radiusB;
149 b2Manifold* manifold = m_contacts[vc->contactIndex]->GetManifold();
150
151 int32 indexA = vc->indexA;
152 int32 indexB = vc->indexB;
153
154 float32 mA = vc->invMassA;
155 float32 mB = vc->invMassB;
156 float32 iA = vc->invIA;
157 float32 iB = vc->invIB;
158 b2Vec2 localCenterA = pc->localCenterA;
159 b2Vec2 localCenterB = pc->localCenterB;
160
161 b2Vec2 cA = m_positions[indexA].c;
162 float32 aA = m_positions[indexA].a;
163 b2Vec2 vA = m_velocities[indexA].v;
164 float32 wA = m_velocities[indexA].w;
165
166 b2Vec2 cB = m_positions[indexB].c;
167 float32 aB = m_positions[indexB].a;
168 b2Vec2 vB = m_velocities[indexB].v;
169 float32 wB = m_velocities[indexB].w;
170
171 b2Assert(manifold->pointCount > 0);
172
173 b2Transform xfA, xfB;
174 xfA.q.Set(aA);
175 xfB.q.Set(aB);
176 xfA.p = cA - b2Mul(xfA.q, localCenterA);
177 xfB.p = cB - b2Mul(xfB.q, localCenterB);
178
179 b2WorldManifold worldManifold;
180 worldManifold.Initialize(manifold, xfA, radiusA, xfB, radiusB);
181
182 vc->normal = worldManifold.normal;
183
184 int32 pointCount = vc->pointCount;
185 for (int32 j = 0; j < pointCount; ++j)
186 {
187 b2VelocityConstraintPoint* vcp = vc->points + j;
188
189 vcp->rA = worldManifold.points[j] - cA;
190 vcp->rB = worldManifold.points[j] - cB;
191
192 float32 rnA = b2Cross(vcp->rA, vc->normal);
193 float32 rnB = b2Cross(vcp->rB, vc->normal);
194
195 float32 kNormal = mA + mB + iA * rnA * rnA + iB * rnB * rnB;
196
197 vcp->normalMass = kNormal > 0.0f ? 1.0f / kNormal : 0.0f;
198
199 b2Vec2 tangent = b2Cross(vc->normal, 1.0f);
200
201 float32 rtA = b2Cross(vcp->rA, tangent);
202 float32 rtB = b2Cross(vcp->rB, tangent);
203
204 float32 kTangent = mA + mB + iA * rtA * rtA + iB * rtB * rtB;
205
206 vcp->tangentMass = kTangent > 0.0f ? 1.0f / kTangent : 0.0f;
207
208 // Setup a velocity bias for restitution.
209 vcp->velocityBias = 0.0f;
210 float32 vRel = b2Dot(vc->normal, vB + b2Cross(wB, vcp->rB) - vA - b2Cross(wA, vcp->rA));
211 if (vRel < -b2_velocityThreshold)
212 {
213 vcp->velocityBias = -vc->restitution * vRel;
214 }
215 }
216
217 // If we have two points, then prepare the block solver.
218 if (vc->pointCount == 2 && g_blockSolve)
219 {
220 b2VelocityConstraintPoint* vcp1 = vc->points + 0;
221 b2VelocityConstraintPoint* vcp2 = vc->points + 1;
222
223 float32 rn1A = b2Cross(vcp1->rA, vc->normal);
224 float32 rn1B = b2Cross(vcp1->rB, vc->normal);
225 float32 rn2A = b2Cross(vcp2->rA, vc->normal);
226 float32 rn2B = b2Cross(vcp2->rB, vc->normal);
227
228 float32 k11 = mA + mB + iA * rn1A * rn1A + iB * rn1B * rn1B;
229 float32 k22 = mA + mB + iA * rn2A * rn2A + iB * rn2B * rn2B;
230 float32 k12 = mA + mB + iA * rn1A * rn2A + iB * rn1B * rn2B;
231
232 // Ensure a reasonable condition number.
233 const float32 k_maxConditionNumber = 1000.0f;
234 if (k11 * k11 < k_maxConditionNumber * (k11 * k22 - k12 * k12))
235 {
236 // K is safe to invert.
237 vc->K.ex.Set(k11, k12);
238 vc->K.ey.Set(k12, k22);
239 vc->normalMass = vc->K.GetInverse();
240 }
241 else
242 {
243 // The constraints are redundant, just use one.
244 // TODO_ERIN use deepest?
245 vc->pointCount = 1;
246 }
247 }
248 }
249 }
250
WarmStart()251 void b2ContactSolver::WarmStart()
252 {
253 // Warm start.
254 for (int32 i = 0; i < m_count; ++i)
255 {
256 b2ContactVelocityConstraint* vc = m_velocityConstraints + i;
257
258 int32 indexA = vc->indexA;
259 int32 indexB = vc->indexB;
260 float32 mA = vc->invMassA;
261 float32 iA = vc->invIA;
262 float32 mB = vc->invMassB;
263 float32 iB = vc->invIB;
264 int32 pointCount = vc->pointCount;
265
266 b2Vec2 vA = m_velocities[indexA].v;
267 float32 wA = m_velocities[indexA].w;
268 b2Vec2 vB = m_velocities[indexB].v;
269 float32 wB = m_velocities[indexB].w;
270
271 b2Vec2 normal = vc->normal;
272 b2Vec2 tangent = b2Cross(normal, 1.0f);
273
274 for (int32 j = 0; j < pointCount; ++j)
275 {
276 b2VelocityConstraintPoint* vcp = vc->points + j;
277 b2Vec2 P = vcp->normalImpulse * normal + vcp->tangentImpulse * tangent;
278 wA -= iA * b2Cross(vcp->rA, P);
279 vA -= mA * P;
280 wB += iB * b2Cross(vcp->rB, P);
281 vB += mB * P;
282 }
283
284 m_velocities[indexA].v = vA;
285 m_velocities[indexA].w = wA;
286 m_velocities[indexB].v = vB;
287 m_velocities[indexB].w = wB;
288 }
289 }
290
SolveVelocityConstraints()291 void b2ContactSolver::SolveVelocityConstraints()
292 {
293 for (int32 i = 0; i < m_count; ++i)
294 {
295 b2ContactVelocityConstraint* vc = m_velocityConstraints + i;
296
297 int32 indexA = vc->indexA;
298 int32 indexB = vc->indexB;
299 float32 mA = vc->invMassA;
300 float32 iA = vc->invIA;
301 float32 mB = vc->invMassB;
302 float32 iB = vc->invIB;
303 int32 pointCount = vc->pointCount;
304
305 b2Vec2 vA = m_velocities[indexA].v;
306 float32 wA = m_velocities[indexA].w;
307 b2Vec2 vB = m_velocities[indexB].v;
308 float32 wB = m_velocities[indexB].w;
309
310 b2Vec2 normal = vc->normal;
311 b2Vec2 tangent = b2Cross(normal, 1.0f);
312 float32 friction = vc->friction;
313
314 b2Assert(pointCount == 1 || pointCount == 2);
315
316 // Solve tangent constraints first because non-penetration is more important
317 // than friction.
318 for (int32 j = 0; j < pointCount; ++j)
319 {
320 b2VelocityConstraintPoint* vcp = vc->points + j;
321
322 // Relative velocity at contact
323 b2Vec2 dv = vB + b2Cross(wB, vcp->rB) - vA - b2Cross(wA, vcp->rA);
324
325 // Compute tangent force
326 float32 vt = b2Dot(dv, tangent) - vc->tangentSpeed;
327 float32 lambda = vcp->tangentMass * (-vt);
328
329 // b2Clamp the accumulated force
330 float32 maxFriction = friction * vcp->normalImpulse;
331 float32 newImpulse = b2Clamp(vcp->tangentImpulse + lambda, -maxFriction, maxFriction);
332 lambda = newImpulse - vcp->tangentImpulse;
333 vcp->tangentImpulse = newImpulse;
334
335 // Apply contact impulse
336 b2Vec2 P = lambda * tangent;
337
338 vA -= mA * P;
339 wA -= iA * b2Cross(vcp->rA, P);
340
341 vB += mB * P;
342 wB += iB * b2Cross(vcp->rB, P);
343 }
344
345 // Solve normal constraints
346 if (pointCount == 1 || g_blockSolve == false)
347 {
348 for (int32 i = 0; i < pointCount; ++i)
349 {
350 b2VelocityConstraintPoint* vcp = vc->points + i;
351
352 // Relative velocity at contact
353 b2Vec2 dv = vB + b2Cross(wB, vcp->rB) - vA - b2Cross(wA, vcp->rA);
354
355 // Compute normal impulse
356 float32 vn = b2Dot(dv, normal);
357 float32 lambda = -vcp->normalMass * (vn - vcp->velocityBias);
358
359 // b2Clamp the accumulated impulse
360 float32 newImpulse = b2Max(vcp->normalImpulse + lambda, 0.0f);
361 lambda = newImpulse - vcp->normalImpulse;
362 vcp->normalImpulse = newImpulse;
363
364 // Apply contact impulse
365 b2Vec2 P = lambda * normal;
366 vA -= mA * P;
367 wA -= iA * b2Cross(vcp->rA, P);
368
369 vB += mB * P;
370 wB += iB * b2Cross(vcp->rB, P);
371 }
372 }
373 else
374 {
375 // Block solver developed in collaboration with Dirk Gregorius (back in 01/07 on Box2D_Lite).
376 // Build the mini LCP for this contact patch
377 //
378 // vn = A * x + b, vn >= 0, , vn >= 0, x >= 0 and vn_i * x_i = 0 with i = 1..2
379 //
380 // A = J * W * JT and J = ( -n, -r1 x n, n, r2 x n )
381 // b = vn0 - velocityBias
382 //
383 // The system is solved using the "Total enumeration method" (s. Murty). The complementary constraint vn_i * x_i
384 // implies that we must have in any solution either vn_i = 0 or x_i = 0. So for the 2D contact problem the cases
385 // 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
386 // solution that satisfies the problem is chosen.
387 //
388 // In order to account of the accumulated impulse 'a' (because of the iterative nature of the solver which only requires
389 // that the accumulated impulse is clamped and not the incremental impulse) we change the impulse variable (x_i).
390 //
391 // Substitute:
392 //
393 // x = a + d
394 //
395 // a := old total impulse
396 // x := new total impulse
397 // d := incremental impulse
398 //
399 // For the current iteration we extend the formula for the incremental impulse
400 // to compute the new total impulse:
401 //
402 // vn = A * d + b
403 // = A * (x - a) + b
404 // = A * x + b - A * a
405 // = A * x + b'
406 // b' = b - A * a;
407
408 b2VelocityConstraintPoint* cp1 = vc->points + 0;
409 b2VelocityConstraintPoint* cp2 = vc->points + 1;
410
411 b2Vec2 a(cp1->normalImpulse, cp2->normalImpulse);
412 b2Assert(a.x >= 0.0f && a.y >= 0.0f);
413
414 // Relative velocity at contact
415 b2Vec2 dv1 = vB + b2Cross(wB, cp1->rB) - vA - b2Cross(wA, cp1->rA);
416 b2Vec2 dv2 = vB + b2Cross(wB, cp2->rB) - vA - b2Cross(wA, cp2->rA);
417
418 // Compute normal velocity
419 float32 vn1 = b2Dot(dv1, normal);
420 float32 vn2 = b2Dot(dv2, normal);
421
422 b2Vec2 b;
423 b.x = vn1 - cp1->velocityBias;
424 b.y = vn2 - cp2->velocityBias;
425
426 // Compute b'
427 b -= b2Mul(vc->K, a);
428
429 const float32 k_errorTol = 1e-3f;
430 B2_NOT_USED(k_errorTol);
431
432 for (;;)
433 {
434 //
435 // Case 1: vn = 0
436 //
437 // 0 = A * x + b'
438 //
439 // Solve for x:
440 //
441 // x = - inv(A) * b'
442 //
443 b2Vec2 x = - b2Mul(vc->normalMass, b);
444
445 if (x.x >= 0.0f && x.y >= 0.0f)
446 {
447 // Get the incremental impulse
448 b2Vec2 d = x - a;
449
450 // Apply incremental impulse
451 b2Vec2 P1 = d.x * normal;
452 b2Vec2 P2 = d.y * normal;
453 vA -= mA * (P1 + P2);
454 wA -= iA * (b2Cross(cp1->rA, P1) + b2Cross(cp2->rA, P2));
455
456 vB += mB * (P1 + P2);
457 wB += iB * (b2Cross(cp1->rB, P1) + b2Cross(cp2->rB, P2));
458
459 // Accumulate
460 cp1->normalImpulse = x.x;
461 cp2->normalImpulse = x.y;
462
463 #if B2_DEBUG_SOLVER == 1
464 // Postconditions
465 dv1 = vB + b2Cross(wB, cp1->rB) - vA - b2Cross(wA, cp1->rA);
466 dv2 = vB + b2Cross(wB, cp2->rB) - vA - b2Cross(wA, cp2->rA);
467
468 // Compute normal velocity
469 vn1 = b2Dot(dv1, normal);
470 vn2 = b2Dot(dv2, normal);
471
472 b2Assert(b2Abs(vn1 - cp1->velocityBias) < k_errorTol);
473 b2Assert(b2Abs(vn2 - cp2->velocityBias) < k_errorTol);
474 #endif
475 break;
476 }
477
478 //
479 // Case 2: vn1 = 0 and x2 = 0
480 //
481 // 0 = a11 * x1 + a12 * 0 + b1'
482 // vn2 = a21 * x1 + a22 * 0 + b2'
483 //
484 x.x = - cp1->normalMass * b.x;
485 x.y = 0.0f;
486 vn1 = 0.0f;
487 vn2 = vc->K.ex.y * x.x + b.y;
488
489 if (x.x >= 0.0f && vn2 >= 0.0f)
490 {
491 // Get the incremental impulse
492 b2Vec2 d = x - a;
493
494 // Apply incremental impulse
495 b2Vec2 P1 = d.x * normal;
496 b2Vec2 P2 = d.y * normal;
497 vA -= mA * (P1 + P2);
498 wA -= iA * (b2Cross(cp1->rA, P1) + b2Cross(cp2->rA, P2));
499
500 vB += mB * (P1 + P2);
501 wB += iB * (b2Cross(cp1->rB, P1) + b2Cross(cp2->rB, P2));
502
503 // Accumulate
504 cp1->normalImpulse = x.x;
505 cp2->normalImpulse = x.y;
506
507 #if B2_DEBUG_SOLVER == 1
508 // Postconditions
509 dv1 = vB + b2Cross(wB, cp1->rB) - vA - b2Cross(wA, cp1->rA);
510
511 // Compute normal velocity
512 vn1 = b2Dot(dv1, normal);
513
514 b2Assert(b2Abs(vn1 - cp1->velocityBias) < k_errorTol);
515 #endif
516 break;
517 }
518
519
520 //
521 // Case 3: vn2 = 0 and x1 = 0
522 //
523 // vn1 = a11 * 0 + a12 * x2 + b1'
524 // 0 = a21 * 0 + a22 * x2 + b2'
525 //
526 x.x = 0.0f;
527 x.y = - cp2->normalMass * b.y;
528 vn1 = vc->K.ey.x * x.y + b.x;
529 vn2 = 0.0f;
530
531 if (x.y >= 0.0f && vn1 >= 0.0f)
532 {
533 // Resubstitute for the incremental impulse
534 b2Vec2 d = x - a;
535
536 // Apply incremental impulse
537 b2Vec2 P1 = d.x * normal;
538 b2Vec2 P2 = d.y * normal;
539 vA -= mA * (P1 + P2);
540 wA -= iA * (b2Cross(cp1->rA, P1) + b2Cross(cp2->rA, P2));
541
542 vB += mB * (P1 + P2);
543 wB += iB * (b2Cross(cp1->rB, P1) + b2Cross(cp2->rB, P2));
544
545 // Accumulate
546 cp1->normalImpulse = x.x;
547 cp2->normalImpulse = x.y;
548
549 #if B2_DEBUG_SOLVER == 1
550 // Postconditions
551 dv2 = vB + b2Cross(wB, cp2->rB) - vA - b2Cross(wA, cp2->rA);
552
553 // Compute normal velocity
554 vn2 = b2Dot(dv2, normal);
555
556 b2Assert(b2Abs(vn2 - cp2->velocityBias) < k_errorTol);
557 #endif
558 break;
559 }
560
561 //
562 // Case 4: x1 = 0 and x2 = 0
563 //
564 // vn1 = b1
565 // vn2 = b2;
566 x.x = 0.0f;
567 x.y = 0.0f;
568 vn1 = b.x;
569 vn2 = b.y;
570
571 if (vn1 >= 0.0f && vn2 >= 0.0f )
572 {
573 // Resubstitute for the incremental impulse
574 b2Vec2 d = x - a;
575
576 // Apply incremental impulse
577 b2Vec2 P1 = d.x * normal;
578 b2Vec2 P2 = d.y * normal;
579 vA -= mA * (P1 + P2);
580 wA -= iA * (b2Cross(cp1->rA, P1) + b2Cross(cp2->rA, P2));
581
582 vB += mB * (P1 + P2);
583 wB += iB * (b2Cross(cp1->rB, P1) + b2Cross(cp2->rB, P2));
584
585 // Accumulate
586 cp1->normalImpulse = x.x;
587 cp2->normalImpulse = x.y;
588
589 break;
590 }
591
592 // No solution, give up. This is hit sometimes, but it doesn't seem to matter.
593 break;
594 }
595 }
596
597 m_velocities[indexA].v = vA;
598 m_velocities[indexA].w = wA;
599 m_velocities[indexB].v = vB;
600 m_velocities[indexB].w = wB;
601 }
602 }
603
StoreImpulses()604 void b2ContactSolver::StoreImpulses()
605 {
606 for (int32 i = 0; i < m_count; ++i)
607 {
608 b2ContactVelocityConstraint* vc = m_velocityConstraints + i;
609 b2Manifold* manifold = m_contacts[vc->contactIndex]->GetManifold();
610
611 for (int32 j = 0; j < vc->pointCount; ++j)
612 {
613 manifold->points[j].normalImpulse = vc->points[j].normalImpulse;
614 manifold->points[j].tangentImpulse = vc->points[j].tangentImpulse;
615 }
616 }
617 }
618
619 struct b2PositionSolverManifold
620 {
Initializeb2PositionSolverManifold621 void Initialize(b2ContactPositionConstraint* pc, const b2Transform& xfA, const b2Transform& xfB, int32 index)
622 {
623 b2Assert(pc->pointCount > 0);
624
625 switch (pc->type)
626 {
627 case b2Manifold::e_circles:
628 {
629 b2Vec2 pointA = b2Mul(xfA, pc->localPoint);
630 b2Vec2 pointB = b2Mul(xfB, pc->localPoints[0]);
631 normal = pointB - pointA;
632 normal.Normalize();
633 point = 0.5f * (pointA + pointB);
634 separation = b2Dot(pointB - pointA, normal) - pc->radiusA - pc->radiusB;
635 }
636 break;
637
638 case b2Manifold::e_faceA:
639 {
640 normal = b2Mul(xfA.q, pc->localNormal);
641 b2Vec2 planePoint = b2Mul(xfA, pc->localPoint);
642
643 b2Vec2 clipPoint = b2Mul(xfB, pc->localPoints[index]);
644 separation = b2Dot(clipPoint - planePoint, normal) - pc->radiusA - pc->radiusB;
645 point = clipPoint;
646 }
647 break;
648
649 case b2Manifold::e_faceB:
650 {
651 normal = b2Mul(xfB.q, pc->localNormal);
652 b2Vec2 planePoint = b2Mul(xfB, pc->localPoint);
653
654 b2Vec2 clipPoint = b2Mul(xfA, pc->localPoints[index]);
655 separation = b2Dot(clipPoint - planePoint, normal) - pc->radiusA - pc->radiusB;
656 point = clipPoint;
657
658 // Ensure normal points from A to B
659 normal = -normal;
660 }
661 break;
662 }
663 }
664
665 b2Vec2 normal;
666 b2Vec2 point;
667 float32 separation;
668 };
669
670 // Sequential solver.
SolvePositionConstraints()671 bool b2ContactSolver::SolvePositionConstraints()
672 {
673 float32 minSeparation = 0.0f;
674
675 for (int32 i = 0; i < m_count; ++i)
676 {
677 b2ContactPositionConstraint* pc = m_positionConstraints + i;
678
679 int32 indexA = pc->indexA;
680 int32 indexB = pc->indexB;
681 b2Vec2 localCenterA = pc->localCenterA;
682 float32 mA = pc->invMassA;
683 float32 iA = pc->invIA;
684 b2Vec2 localCenterB = pc->localCenterB;
685 float32 mB = pc->invMassB;
686 float32 iB = pc->invIB;
687 int32 pointCount = pc->pointCount;
688
689 b2Vec2 cA = m_positions[indexA].c;
690 float32 aA = m_positions[indexA].a;
691
692 b2Vec2 cB = m_positions[indexB].c;
693 float32 aB = m_positions[indexB].a;
694
695 // Solve normal constraints
696 for (int32 j = 0; j < pointCount; ++j)
697 {
698 b2Transform xfA, xfB;
699 xfA.q.Set(aA);
700 xfB.q.Set(aB);
701 xfA.p = cA - b2Mul(xfA.q, localCenterA);
702 xfB.p = cB - b2Mul(xfB.q, localCenterB);
703
704 b2PositionSolverManifold psm;
705 psm.Initialize(pc, xfA, xfB, j);
706 b2Vec2 normal = psm.normal;
707
708 b2Vec2 point = psm.point;
709 float32 separation = psm.separation;
710
711 b2Vec2 rA = point - cA;
712 b2Vec2 rB = point - cB;
713
714 // Track max constraint error.
715 minSeparation = b2Min(minSeparation, separation);
716
717 // Prevent large corrections and allow slop.
718 float32 C = b2Clamp(b2_baumgarte * (separation + b2_linearSlop), -b2_maxLinearCorrection, 0.0f);
719
720 // Compute the effective mass.
721 float32 rnA = b2Cross(rA, normal);
722 float32 rnB = b2Cross(rB, normal);
723 float32 K = mA + mB + iA * rnA * rnA + iB * rnB * rnB;
724
725 // Compute normal impulse
726 float32 impulse = K > 0.0f ? - C / K : 0.0f;
727
728 b2Vec2 P = impulse * normal;
729
730 cA -= mA * P;
731 aA -= iA * b2Cross(rA, P);
732
733 cB += mB * P;
734 aB += iB * b2Cross(rB, P);
735 }
736
737 m_positions[indexA].c = cA;
738 m_positions[indexA].a = aA;
739
740 m_positions[indexB].c = cB;
741 m_positions[indexB].a = aB;
742 }
743
744 // We can't expect minSpeparation >= -b2_linearSlop because we don't
745 // push the separation above -b2_linearSlop.
746 return minSeparation >= -3.0f * b2_linearSlop;
747 }
748
749 // Sequential position solver for position constraints.
SolveTOIPositionConstraints(int32 toiIndexA,int32 toiIndexB)750 bool b2ContactSolver::SolveTOIPositionConstraints(int32 toiIndexA, int32 toiIndexB)
751 {
752 float32 minSeparation = 0.0f;
753
754 for (int32 i = 0; i < m_count; ++i)
755 {
756 b2ContactPositionConstraint* pc = m_positionConstraints + i;
757
758 int32 indexA = pc->indexA;
759 int32 indexB = pc->indexB;
760 b2Vec2 localCenterA = pc->localCenterA;
761 b2Vec2 localCenterB = pc->localCenterB;
762 int32 pointCount = pc->pointCount;
763
764 float32 mA = 0.0f;
765 float32 iA = 0.0f;
766 if (indexA == toiIndexA || indexA == toiIndexB)
767 {
768 mA = pc->invMassA;
769 iA = pc->invIA;
770 }
771
772 float32 mB = 0.0f;
773 float32 iB = 0.;
774 if (indexB == toiIndexA || indexB == toiIndexB)
775 {
776 mB = pc->invMassB;
777 iB = pc->invIB;
778 }
779
780 b2Vec2 cA = m_positions[indexA].c;
781 float32 aA = m_positions[indexA].a;
782
783 b2Vec2 cB = m_positions[indexB].c;
784 float32 aB = m_positions[indexB].a;
785
786 // Solve normal constraints
787 for (int32 j = 0; j < pointCount; ++j)
788 {
789 b2Transform xfA, xfB;
790 xfA.q.Set(aA);
791 xfB.q.Set(aB);
792 xfA.p = cA - b2Mul(xfA.q, localCenterA);
793 xfB.p = cB - b2Mul(xfB.q, localCenterB);
794
795 b2PositionSolverManifold psm;
796 psm.Initialize(pc, xfA, xfB, j);
797 b2Vec2 normal = psm.normal;
798
799 b2Vec2 point = psm.point;
800 float32 separation = psm.separation;
801
802 b2Vec2 rA = point - cA;
803 b2Vec2 rB = point - cB;
804
805 // Track max constraint error.
806 minSeparation = b2Min(minSeparation, separation);
807
808 // Prevent large corrections and allow slop.
809 float32 C = b2Clamp(b2_toiBaugarte * (separation + b2_linearSlop), -b2_maxLinearCorrection, 0.0f);
810
811 // Compute the effective mass.
812 float32 rnA = b2Cross(rA, normal);
813 float32 rnB = b2Cross(rB, normal);
814 float32 K = mA + mB + iA * rnA * rnA + iB * rnB * rnB;
815
816 // Compute normal impulse
817 float32 impulse = K > 0.0f ? - C / K : 0.0f;
818
819 b2Vec2 P = impulse * normal;
820
821 cA -= mA * P;
822 aA -= iA * b2Cross(rA, P);
823
824 cB += mB * P;
825 aB += iB * b2Cross(rB, P);
826 }
827
828 m_positions[indexA].c = cA;
829 m_positions[indexA].a = aA;
830
831 m_positions[indexB].c = cB;
832 m_positions[indexB].a = aB;
833 }
834
835 // We can't expect minSpeparation >= -b2_linearSlop because we don't
836 // push the separation above -b2_linearSlop.
837 return minSeparation >= -1.5f * b2_linearSlop;
838 }
839