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