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

mvsim
Author(s):
autogenerated on Tue Jul 4 2023 03:08:19