b2ContactSolver.cpp
Go to the documentation of this file.
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 
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 
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 
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.
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 
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 
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 
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 {
621  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.
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.
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 }
d
d
b2Dot
float32 b2Dot(const b2Vec2 &a, const b2Vec2 &b)
Perform the dot product on two vectors.
Definition: b2Math.h:406
b2Contact::m_fixtureB
b2Fixture * m_fixtureB
Definition: b2Contact.h:206
b2Body::m_invMass
float32 m_invMass
Definition: b2Body.h:455
b2Body::m_islandIndex
int32 m_islandIndex
Definition: b2Body.h:434
b2Mul
b2Vec2 b2Mul(const b2Mat22 &A, const b2Vec2 &v)
Definition: b2Math.h:433
b2Contact::m_friction
float32 m_friction
Definition: b2Contact.h:216
b2Transform::p
b2Vec2 p
Definition: b2Math.h:372
b2_linearSlop
#define b2_linearSlop
Definition: b2Settings.h:68
b2Contact::m_tangentSpeed
float32 m_tangentSpeed
Definition: b2Contact.h:219
b2Manifold::localNormal
b2Vec2 localNormal
not use for Type::e_points
Definition: b2Collision.h:103
b2Transform::q
b2Rot q
Definition: b2Math.h:373
b2WorldManifold::points
b2Vec2 points[b2_maxManifoldPoints]
world contact point (point of intersection)
Definition: b2Collision.h:121
f
f
b2ContactVelocityConstraint::points
b2VelocityConstraintPoint points[b2_maxManifoldPoints]
Definition: b2ContactSolver.h:44
b2Contact::m_fixtureA
b2Fixture * m_fixtureA
Definition: b2Contact.h:205
b2Mat22::SetZero
void SetZero()
Set this matrix to all zeros.
Definition: b2Math.h:217
b2ContactSolver::SolveTOIPositionConstraints
bool SolveTOIPositionConstraints(int32 toiIndexA, int32 toiIndexB)
Definition: b2ContactSolver.cpp:750
b2WorldManifold::normal
b2Vec2 normal
world vector pointing from A to B
Definition: b2Collision.h:120
B2_NOT_USED
#define B2_NOT_USED(x)
Definition: b2Settings.h:26
b2_maxManifoldPoints
#define b2_maxManifoldPoints
Definition: b2Settings.h:50
b2_toiBaugarte
#define b2_toiBaugarte
Definition: b2Settings.h:114
b2Max
T b2Max(T a, T b)
Definition: b2Math.h:643
b2ContactSolver::b2ContactSolver
b2ContactSolver(b2ContactSolverDef *def)
Definition: b2ContactSolver.cpp:46
b2Shape::m_radius
float32 m_radius
Definition: b2Shape.h:93
b2Vec2::SetZero
void SetZero()
Set this vector to all zeros.
Definition: b2Math.h:62
b2Vec2
A 2D column vector.
Definition: b2Math.h:53
b2Mat22::ey
b2Vec2 ey
Definition: b2Math.h:253
b2_maxLinearCorrection
#define b2_maxLinearCorrection
Definition: b2Settings.h:94
int32
signed int int32
Definition: b2Settings.h:31
b2Sweep::localCenter
b2Vec2 localCenter
local center of mass position
Definition: b2Math.h:393
b2Cross
float32 b2Cross(const b2Vec2 &a, const b2Vec2 &b)
Perform the cross product on two vectors. In 2D this produces a scalar.
Definition: b2Math.h:412
b2PositionSolverManifold::Initialize
void Initialize(b2ContactPositionConstraint *pc, const b2Transform &xfA, const b2Transform &xfB, int32 index)
Definition: b2ContactSolver.cpp:621
b2Body
A rigid body. These are created via b2World::CreateBody.
Definition: b2Body.h:126
b2Contact::GetManifold
b2Manifold * GetManifold()
Definition: b2Contact.h:222
b2Contact::m_restitution
float32 m_restitution
Definition: b2Contact.h:217
b2Body::m_invI
float32 m_invI
Definition: b2Body.h:458
b2Manifold::pointCount
int32 pointCount
the number of manifold points
Definition: b2Collision.h:106
x
TFSIMD_FORCE_INLINE const tfScalar & x() const
b2ManifoldPoint::localPoint
b2Vec2 localPoint
usage depends on manifold type
Definition: b2Collision.h:71
b2ContactSolver::SolveVelocityConstraints
void SolveVelocityConstraints()
Definition: b2ContactSolver.cpp:291
b2ContactSolver::SolvePositionConstraints
bool SolvePositionConstraints()
Definition: b2ContactSolver.cpp:671
b2ContactSolverDef::positions
b2Position * positions
Definition: b2ContactSolver.h:64
b2ManifoldPoint::tangentImpulse
float32 tangentImpulse
the friction impulse
Definition: b2Collision.h:73
b2Vec2::y
float32 y
Definition: b2Math.h:140
b2Manifold::localPoint
b2Vec2 localPoint
usage depends on manifold type
Definition: b2Collision.h:104
b2ContactSolverDef::contacts
b2Contact ** contacts
Definition: b2ContactSolver.h:62
b2_baumgarte
#define b2_baumgarte
Definition: b2Settings.h:113
b2Manifold::points
b2ManifoldPoint points[b2_maxManifoldPoints]
the points of contact
Definition: b2Collision.h:102
b2Assert
#define b2Assert(A)
Definition: b2Settings.h:27
b2Rot::Set
void Set(float32 angle)
Set using an angle in radians.
Definition: b2Math.h:312
b2Clamp
T b2Clamp(T a, T low, T high)
Definition: b2Math.h:654
b2WorldManifold::Initialize
void Initialize(const b2Manifold *manifold, const b2Transform &xfA, float32 radiusA, const b2Transform &xfB, float32 radiusB)
Definition: b2Collision.cpp:22
b2Mat22::ex
b2Vec2 ex
Definition: b2Math.h:253
g_blockSolve
bool g_blockSolve
Definition: b2ContactSolver.cpp:29
b2Abs
T b2Abs(T a)
Definition: b2Math.h:616
b2Min
T b2Min(T a, T b)
Definition: b2Math.h:632
b2ContactSolver::InitializeVelocityConstraints
void InitializeVelocityConstraints()
Definition: b2ContactSolver.cpp:140
b2ContactPositionConstraint::localPoints
b2Vec2 localPoints[b2_maxManifoldPoints]
Definition: b2ContactSolver.cpp:33
b2Vec2::x
float32 x
Definition: b2Math.h:140
b2Fixture::GetShape
b2Shape * GetShape()
Definition: b2Fixture.h:243
b2Vec2::Normalize
float32 Normalize()
Convert this vector into a unit vector. Returns the length.
Definition: b2Math.h:114
b2Vec2::Set
void Set(float32 x_, float32 y_)
Set this vector to some specified coordinates.
Definition: b2Math.h:65
b2_velocityThreshold
#define b2_velocityThreshold
Definition: b2Settings.h:90
b2ContactSolverDef::allocator
b2StackAllocator * allocator
Definition: b2ContactSolver.h:66
b2Mat22::GetInverse
b2Mat22 GetInverse() const
Definition: b2Math.h:223
b2Fixture::GetBody
b2Body * GetBody()
Definition: b2Fixture.h:273
b2ContactSolverDef::velocities
b2Velocity * velocities
Definition: b2ContactSolver.h:65
b2Body::m_sweep
b2Sweep m_sweep
Definition: b2Body.h:437
float32
float float32
Definition: b2Settings.h:35
b2WorldManifold
This is used to compute the current state of a contact manifold.
Definition: b2Collision.h:110
b2ManifoldPoint::normalImpulse
float32 normalImpulse
the non-penetration impulse
Definition: b2Collision.h:72

mvsim
Author(s):
autogenerated on Thu Jun 6 2019 19:36:39