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 "box2d/b2_body.h"
24 #include "box2d/b2_contact.h"
25 #include "box2d/b2_distance.h"
26 #include "box2d/b2_fixture.h"
27 #include "box2d/b2_joint.h"
28 #include "box2d/b2_stack_allocator.h"
29 #include "box2d/b2_timer.h"
30 #include "box2d/b2_world.h"
31
32 #include "b2_island.h"
33 #include "dynamics/b2_contact_solver.h"
34
35 /*
36 Position Correction Notes
37 =========================
38 I tried the several algorithms for position correction of the 2D revolute joint.
39 I looked at these systems:
40 - simple pendulum (1m diameter sphere on massless 5m stick) with initial angular velocity of 100 rad/s.
41 - suspension bridge with 30 1m long planks of length 1m.
42 - multi-link chain with 30 1m long links.
43
44 Here are the algorithms:
45
46 Baumgarte - A fraction of the position error is added to the velocity error. There is no
47 separate position solver.
48
49 Pseudo Velocities - After the velocity solver and position integration,
50 the position error, Jacobian, and effective mass are recomputed. Then
51 the velocity constraints are solved with pseudo velocities and a fraction
52 of the position error is added to the pseudo velocity error. The pseudo
53 velocities are initialized to zero and there is no warm-starting. After
54 the position solver, the pseudo velocities are added to the positions.
55 This is also called the First Order World method or the Position LCP method.
56
57 Modified Nonlinear Gauss-Seidel (NGS) - Like Pseudo Velocities except the
58 position error is re-computed for each constraint and the positions are updated
59 after the constraint is solved. The radius vectors (aka Jacobians) are
60 re-computed too (otherwise the algorithm has horrible instability). The pseudo
61 velocity states are not needed because they are effectively zero at the beginning
62 of each iteration. Since we have the current position error, we allow the
63 iterations to terminate early if the error becomes smaller than b2_linearSlop.
64
65 Full NGS or just NGS - Like Modified NGS except the effective mass are re-computed
66 each time a constraint is solved.
67
68 Here are the results:
69 Baumgarte - this is the cheapest algorithm but it has some stability problems,
70 especially with the bridge. The chain links separate easily close to the root
71 and they jitter as they struggle to pull together. This is one of the most common
72 methods in the field. The big drawback is that the position correction artificially
73 affects the momentum, thus leading to instabilities and false bounce. I used a
74 bias factor of 0.2. A larger bias factor makes the bridge less stable, a smaller
75 factor makes joints and contacts more spongy.
76
77 Pseudo Velocities - the is more stable than the Baumgarte method. The bridge is
78 stable. However, joints still separate with large angular velocities. Drag the
79 simple pendulum in a circle quickly and the joint will separate. The chain separates
80 easily and does not recover. I used a bias factor of 0.2. A larger value lead to
81 the bridge collapsing when a heavy cube drops on it.
82
83 Modified NGS - this algorithm is better in some ways than Baumgarte and Pseudo
84 Velocities, but in other ways it is worse. The bridge and chain are much more
85 stable, but the simple pendulum goes unstable at high angular velocities.
86
87 Full NGS - stable in all tests. The joints display good stiffness. The bridge
88 still sags, but this is better than infinite forces.
89
90 Recommendations
91 Pseudo Velocities are not really worthwhile because the bridge and chain cannot
92 recover from joint separation. In other cases the benefit over Baumgarte is small.
93
94 Modified NGS is not a robust method for the revolute joint due to the violent
95 instability seen in the simple pendulum. Perhaps it is viable with other constraint
96 types, especially scalar constraints where the effective mass is a scalar.
97
98 This leaves Baumgarte and Full NGS. Baumgarte has small, but manageable instabilities
99 and is very fast. I don't think we can escape Baumgarte, especially in highly
100 demanding cases where high constraint fidelity is not needed.
101
102 Full NGS is robust and easy on the eyes. I recommend this as an option for
103 higher fidelity simulation and certainly for suspension bridges and long chains.
104 Full NGS might be a good choice for ragdolls, especially motorized ragdolls where
105 joint separation can be problematic. The number of NGS iterations can be reduced
106 for better performance without harming robustness much.
107
108 Each joint in a can be handled differently in the position solver. So I recommend
109 a system where the user can select the algorithm on a per joint basis. I would
110 probably default to the slower Full NGS and let the user select the faster
111 Baumgarte method in performance critical scenarios.
112 */
113
114 /*
115 Cache Performance
116
117 The Box2D solvers are dominated by cache misses. Data structures are designed
118 to increase the number of cache hits. Much of misses are due to random access
119 to body data. The constraint structures are iterated over linearly, which leads
120 to few cache misses.
121
122 The bodies are not accessed during iteration. Instead read only data, such as
123 the mass values are stored with the constraints. The mutable data are the constraint
124 impulses and the bodies velocities/positions. The impulses are held inside the
125 constraint structures. The body velocities/positions are held in compact, temporary
126 arrays to increase the number of cache hits. Linear and angular velocity are
127 stored in a single array since multiple arrays lead to multiple misses.
128 */
129
130 /*
131 2D Rotation
132
133 R = [cos(theta) -sin(theta)]
134 [sin(theta) cos(theta) ]
135
136 thetaDot = omega
137
138 Let q1 = cos(theta), q2 = sin(theta).
139 R = [q1 -q2]
140 [q2 q1]
141
142 q1Dot = -thetaDot * q2
143 q2Dot = thetaDot * q1
144
145 q1_new = q1_old - dt * w * q2
146 q2_new = q2_old + dt * w * q1
147 then normalize.
148
149 This might be faster than computing sin+cos.
150 However, we can compute sin+cos of the same angle fast.
151 */
152
b2Island(int32 bodyCapacity,int32 contactCapacity,int32 jointCapacity,b2StackAllocator * allocator,b2ContactListener * listener)153 b2Island::b2Island(
154 int32 bodyCapacity,
155 int32 contactCapacity,
156 int32 jointCapacity,
157 b2StackAllocator* allocator,
158 b2ContactListener* listener)
159 {
160 m_bodyCapacity = bodyCapacity;
161 m_contactCapacity = contactCapacity;
162 m_jointCapacity = jointCapacity;
163 m_bodyCount = 0;
164 m_contactCount = 0;
165 m_jointCount = 0;
166
167 m_allocator = allocator;
168 m_listener = listener;
169
170 m_bodies = (b2Body**)m_allocator->Allocate(bodyCapacity * sizeof(b2Body*));
171 m_contacts = (b2Contact**)m_allocator->Allocate(contactCapacity * sizeof(b2Contact*));
172 m_joints = (b2Joint**)m_allocator->Allocate(jointCapacity * sizeof(b2Joint*));
173
174 m_velocities = (b2Velocity*)m_allocator->Allocate(m_bodyCapacity * sizeof(b2Velocity));
175 m_positions = (b2Position*)m_allocator->Allocate(m_bodyCapacity * sizeof(b2Position));
176 }
177
~b2Island()178 b2Island::~b2Island()
179 {
180 // Warning: the order should reverse the constructor order.
181 m_allocator->Free(m_positions);
182 m_allocator->Free(m_velocities);
183 m_allocator->Free(m_joints);
184 m_allocator->Free(m_contacts);
185 m_allocator->Free(m_bodies);
186 }
187
Solve(b2Profile * profile,const b2TimeStep & step,const b2Vec2 & gravity,bool allowSleep)188 void b2Island::Solve(b2Profile* profile, const b2TimeStep& step, const b2Vec2& gravity, bool allowSleep)
189 {
190 b2Timer timer;
191
192 float h = step.dt;
193
194 // Integrate velocities and apply damping. Initialize the body state.
195 for (int32 i = 0; i < m_bodyCount; ++i)
196 {
197 b2Body* b = m_bodies[i];
198
199 b2Vec2 c = b->m_sweep.c;
200 float a = b->m_sweep.a;
201 b2Vec2 v = b->m_linearVelocity;
202 float w = b->m_angularVelocity;
203
204 // Store positions for continuous collision.
205 b->m_sweep.c0 = b->m_sweep.c;
206 b->m_sweep.a0 = b->m_sweep.a;
207
208 if (b->m_type == b2_dynamicBody)
209 {
210 // Integrate velocities.
211 v += h * b->m_invMass * (b->m_gravityScale * b->m_mass * gravity + b->m_force);
212 w += h * b->m_invI * b->m_torque;
213
214 // Apply damping.
215 // ODE: dv/dt + c * v = 0
216 // Solution: v(t) = v0 * exp(-c * t)
217 // Time step: v(t + dt) = v0 * exp(-c * (t + dt)) = v0 * exp(-c * t) * exp(-c * dt) = v * exp(-c * dt)
218 // v2 = exp(-c * dt) * v1
219 // Pade approximation:
220 // v2 = v1 * 1 / (1 + c * dt)
221 v *= 1.0f / (1.0f + h * b->m_linearDamping);
222 w *= 1.0f / (1.0f + h * b->m_angularDamping);
223 }
224
225 m_positions[i].c = c;
226 m_positions[i].a = a;
227 m_velocities[i].v = v;
228 m_velocities[i].w = w;
229 }
230
231 timer.Reset();
232
233 // Solver data
234 b2SolverData solverData;
235 solverData.step = step;
236 solverData.positions = m_positions;
237 solverData.velocities = m_velocities;
238
239 // Initialize velocity constraints.
240 b2ContactSolverDef contactSolverDef;
241 contactSolverDef.step = step;
242 contactSolverDef.contacts = m_contacts;
243 contactSolverDef.count = m_contactCount;
244 contactSolverDef.positions = m_positions;
245 contactSolverDef.velocities = m_velocities;
246 contactSolverDef.allocator = m_allocator;
247
248 b2ContactSolver contactSolver(&contactSolverDef);
249 contactSolver.InitializeVelocityConstraints();
250
251 if (step.warmStarting)
252 {
253 contactSolver.WarmStart();
254 }
255
256 for (int32 i = 0; i < m_jointCount; ++i)
257 {
258 m_joints[i]->InitVelocityConstraints(solverData);
259 }
260
261 profile->solveInit = timer.GetMilliseconds();
262
263 // Solve velocity constraints
264 timer.Reset();
265 for (int32 i = 0; i < step.velocityIterations; ++i)
266 {
267 for (int32 j = 0; j < m_jointCount; ++j)
268 {
269 m_joints[j]->SolveVelocityConstraints(solverData);
270 }
271
272 contactSolver.SolveVelocityConstraints();
273 }
274
275 // Store impulses for warm starting
276 contactSolver.StoreImpulses();
277 profile->solveVelocity = timer.GetMilliseconds();
278
279 // Integrate positions
280 for (int32 i = 0; i < m_bodyCount; ++i)
281 {
282 b2Vec2 c = m_positions[i].c;
283 float a = m_positions[i].a;
284 b2Vec2 v = m_velocities[i].v;
285 float w = m_velocities[i].w;
286
287 // Check for large velocities
288 b2Vec2 translation = h * v;
289 if (b2Dot(translation, translation) > b2_maxTranslationSquared)
290 {
291 float ratio = b2_maxTranslation / translation.Length();
292 v *= ratio;
293 }
294
295 float rotation = h * w;
296 if (rotation * rotation > b2_maxRotationSquared)
297 {
298 float ratio = b2_maxRotation / b2Abs(rotation);
299 w *= ratio;
300 }
301
302 // Integrate
303 c += h * v;
304 a += h * w;
305
306 m_positions[i].c = c;
307 m_positions[i].a = a;
308 m_velocities[i].v = v;
309 m_velocities[i].w = w;
310 }
311
312 // Solve position constraints
313 timer.Reset();
314 bool positionSolved = false;
315 for (int32 i = 0; i < step.positionIterations; ++i)
316 {
317 bool contactsOkay = contactSolver.SolvePositionConstraints();
318
319 bool jointsOkay = true;
320 for (int32 j = 0; j < m_jointCount; ++j)
321 {
322 bool jointOkay = m_joints[j]->SolvePositionConstraints(solverData);
323 jointsOkay = jointsOkay && jointOkay;
324 }
325
326 if (contactsOkay && jointsOkay)
327 {
328 // Exit early if the position errors are small.
329 positionSolved = true;
330 break;
331 }
332 }
333
334 // Copy state buffers back to the bodies
335 for (int32 i = 0; i < m_bodyCount; ++i)
336 {
337 b2Body* body = m_bodies[i];
338 body->m_sweep.c = m_positions[i].c;
339 body->m_sweep.a = m_positions[i].a;
340 body->m_linearVelocity = m_velocities[i].v;
341 body->m_angularVelocity = m_velocities[i].w;
342 body->SynchronizeTransform();
343 }
344
345 profile->solvePosition = timer.GetMilliseconds();
346
347 Report(contactSolver.m_velocityConstraints);
348
349 if (allowSleep)
350 {
351 float minSleepTime = b2_maxFloat;
352
353 const float linTolSqr = b2_linearSleepTolerance * b2_linearSleepTolerance;
354 const float angTolSqr = b2_angularSleepTolerance * b2_angularSleepTolerance;
355
356 for (int32 i = 0; i < m_bodyCount; ++i)
357 {
358 b2Body* b = m_bodies[i];
359 if (b->GetType() == b2_staticBody)
360 {
361 continue;
362 }
363
364 if ((b->m_flags & b2Body::e_autoSleepFlag) == 0 ||
365 b->m_angularVelocity * b->m_angularVelocity > angTolSqr ||
366 b2Dot(b->m_linearVelocity, b->m_linearVelocity) > linTolSqr)
367 {
368 b->m_sleepTime = 0.0f;
369 minSleepTime = 0.0f;
370 }
371 else
372 {
373 b->m_sleepTime += h;
374 minSleepTime = b2Min(minSleepTime, b->m_sleepTime);
375 }
376 }
377
378 if (minSleepTime >= b2_timeToSleep && positionSolved)
379 {
380 for (int32 i = 0; i < m_bodyCount; ++i)
381 {
382 b2Body* b = m_bodies[i];
383 b->SetAwake(false);
384 }
385 }
386 }
387 }
388
SolveTOI(const b2TimeStep & subStep,int32 toiIndexA,int32 toiIndexB)389 void b2Island::SolveTOI(const b2TimeStep& subStep, int32 toiIndexA, int32 toiIndexB)
390 {
391 b2Assert(toiIndexA < m_bodyCount);
392 b2Assert(toiIndexB < m_bodyCount);
393
394 // Initialize the body state.
395 for (int32 i = 0; i < m_bodyCount; ++i)
396 {
397 b2Body* b = m_bodies[i];
398 m_positions[i].c = b->m_sweep.c;
399 m_positions[i].a = b->m_sweep.a;
400 m_velocities[i].v = b->m_linearVelocity;
401 m_velocities[i].w = b->m_angularVelocity;
402 }
403
404 b2ContactSolverDef contactSolverDef;
405 contactSolverDef.contacts = m_contacts;
406 contactSolverDef.count = m_contactCount;
407 contactSolverDef.allocator = m_allocator;
408 contactSolverDef.step = subStep;
409 contactSolverDef.positions = m_positions;
410 contactSolverDef.velocities = m_velocities;
411 b2ContactSolver contactSolver(&contactSolverDef);
412
413 // Solve position constraints.
414 for (int32 i = 0; i < subStep.positionIterations; ++i)
415 {
416 bool contactsOkay = contactSolver.SolveTOIPositionConstraints(toiIndexA, toiIndexB);
417 if (contactsOkay)
418 {
419 break;
420 }
421 }
422
423 #if 0
424 // Is the new position really safe?
425 for (int32 i = 0; i < m_contactCount; ++i)
426 {
427 b2Contact* c = m_contacts[i];
428 b2Fixture* fA = c->GetFixtureA();
429 b2Fixture* fB = c->GetFixtureB();
430
431 b2Body* bA = fA->GetBody();
432 b2Body* bB = fB->GetBody();
433
434 int32 indexA = c->GetChildIndexA();
435 int32 indexB = c->GetChildIndexB();
436
437 b2DistanceInput input;
438 input.proxyA.Set(fA->GetShape(), indexA);
439 input.proxyB.Set(fB->GetShape(), indexB);
440 input.transformA = bA->GetTransform();
441 input.transformB = bB->GetTransform();
442 input.useRadii = false;
443
444 b2DistanceOutput output;
445 b2SimplexCache cache;
446 cache.count = 0;
447 b2Distance(&output, &cache, &input);
448
449 if (output.distance == 0 || cache.count == 3)
450 {
451 cache.count += 0;
452 }
453 }
454 #endif
455
456 // Leap of faith to new safe state.
457 m_bodies[toiIndexA]->m_sweep.c0 = m_positions[toiIndexA].c;
458 m_bodies[toiIndexA]->m_sweep.a0 = m_positions[toiIndexA].a;
459 m_bodies[toiIndexB]->m_sweep.c0 = m_positions[toiIndexB].c;
460 m_bodies[toiIndexB]->m_sweep.a0 = m_positions[toiIndexB].a;
461
462 // No warm starting is needed for TOI events because warm
463 // starting impulses were applied in the discrete solver.
464 contactSolver.InitializeVelocityConstraints();
465
466 // Solve velocity constraints.
467 for (int32 i = 0; i < subStep.velocityIterations; ++i)
468 {
469 contactSolver.SolveVelocityConstraints();
470 }
471
472 // Don't store the TOI contact forces for warm starting
473 // because they can be quite large.
474
475 float h = subStep.dt;
476
477 // Integrate positions
478 for (int32 i = 0; i < m_bodyCount; ++i)
479 {
480 b2Vec2 c = m_positions[i].c;
481 float a = m_positions[i].a;
482 b2Vec2 v = m_velocities[i].v;
483 float w = m_velocities[i].w;
484
485 // Check for large velocities
486 b2Vec2 translation = h * v;
487 if (b2Dot(translation, translation) > b2_maxTranslationSquared)
488 {
489 float ratio = b2_maxTranslation / translation.Length();
490 v *= ratio;
491 }
492
493 float rotation = h * w;
494 if (rotation * rotation > b2_maxRotationSquared)
495 {
496 float ratio = b2_maxRotation / b2Abs(rotation);
497 w *= ratio;
498 }
499
500 // Integrate
501 c += h * v;
502 a += h * w;
503
504 m_positions[i].c = c;
505 m_positions[i].a = a;
506 m_velocities[i].v = v;
507 m_velocities[i].w = w;
508
509 // Sync bodies
510 b2Body* body = m_bodies[i];
511 body->m_sweep.c = c;
512 body->m_sweep.a = a;
513 body->m_linearVelocity = v;
514 body->m_angularVelocity = w;
515 body->SynchronizeTransform();
516 }
517
518 Report(contactSolver.m_velocityConstraints);
519 }
520
Report(const b2ContactVelocityConstraint * constraints)521 void b2Island::Report(const b2ContactVelocityConstraint* constraints)
522 {
523 if (m_listener == nullptr)
524 {
525 return;
526 }
527
528 for (int32 i = 0; i < m_contactCount; ++i)
529 {
530 b2Contact* c = m_contacts[i];
531
532 const b2ContactVelocityConstraint* vc = constraints + i;
533
534 b2ContactImpulse impulse;
535 impulse.count = vc->pointCount;
536 for (int32 j = 0; j < vc->pointCount; ++j)
537 {
538 impulse.normalImpulses[j] = vc->points[j].normalImpulse;
539 impulse.tangentImpulses[j] = vc->points[j].tangentImpulse;
540 }
541
542 m_listener->PostSolve(c, &impulse);
543 }
544 }
545