Warning, file /education/gcompris/external/qml-box2d/Box2D/Dynamics/b2Island.cpp was not indexed or was modified since last indexation (in which case cross-reference links may be missing, inaccurate or erroneous).
0001 /* 0002 * Copyright (c) 2006-2011 Erin Catto http://www.box2d.org 0003 * 0004 * This software is provided 'as-is', without any express or implied 0005 * warranty. In no event will the authors be held liable for any damages 0006 * arising from the use of this software. 0007 * Permission is granted to anyone to use this software for any purpose, 0008 * including commercial applications, and to alter it and redistribute it 0009 * freely, subject to the following restrictions: 0010 * 1. The origin of this software must not be misrepresented; you must not 0011 * claim that you wrote the original software. If you use this software 0012 * in a product, an acknowledgment in the product documentation would be 0013 * appreciated but is not required. 0014 * 2. Altered source versions must be plainly marked as such, and must not be 0015 * misrepresented as being the original software. 0016 * 3. This notice may not be removed or altered from any source distribution. 0017 */ 0018 0019 #include <Box2D/Collision/b2Distance.h> 0020 #include <Box2D/Dynamics/b2Island.h> 0021 #include <Box2D/Dynamics/b2Body.h> 0022 #include <Box2D/Dynamics/b2Fixture.h> 0023 #include <Box2D/Dynamics/b2World.h> 0024 #include <Box2D/Dynamics/Contacts/b2Contact.h> 0025 #include <Box2D/Dynamics/Contacts/b2ContactSolver.h> 0026 #include <Box2D/Dynamics/Joints/b2Joint.h> 0027 #include <Box2D/Common/b2StackAllocator.h> 0028 #include <Box2D/Common/b2Timer.h> 0029 0030 /* 0031 Position Correction Notes 0032 ========================= 0033 I tried the several algorithms for position correction of the 2D revolute joint. 0034 I looked at these systems: 0035 - simple pendulum (1m diameter sphere on massless 5m stick) with initial angular velocity of 100 rad/s. 0036 - suspension bridge with 30 1m long planks of length 1m. 0037 - multi-link chain with 30 1m long links. 0038 0039 Here are the algorithms: 0040 0041 Baumgarte - A fraction of the position error is added to the velocity error. There is no 0042 separate position solver. 0043 0044 Pseudo Velocities - After the velocity solver and position integration, 0045 the position error, Jacobian, and effective mass are recomputed. Then 0046 the velocity constraints are solved with pseudo velocities and a fraction 0047 of the position error is added to the pseudo velocity error. The pseudo 0048 velocities are initialized to zero and there is no warm-starting. After 0049 the position solver, the pseudo velocities are added to the positions. 0050 This is also called the First Order World method or the Position LCP method. 0051 0052 Modified Nonlinear Gauss-Seidel (NGS) - Like Pseudo Velocities except the 0053 position error is re-computed for each constraint and the positions are updated 0054 after the constraint is solved. The radius vectors (aka Jacobians) are 0055 re-computed too (otherwise the algorithm has horrible instability). The pseudo 0056 velocity states are not needed because they are effectively zero at the beginning 0057 of each iteration. Since we have the current position error, we allow the 0058 iterations to terminate early if the error becomes smaller than b2_linearSlop. 0059 0060 Full NGS or just NGS - Like Modified NGS except the effective mass are re-computed 0061 each time a constraint is solved. 0062 0063 Here are the results: 0064 Baumgarte - this is the cheapest algorithm but it has some stability problems, 0065 especially with the bridge. The chain links separate easily close to the root 0066 and they jitter as they struggle to pull together. This is one of the most common 0067 methods in the field. The big drawback is that the position correction artificially 0068 affects the momentum, thus leading to instabilities and false bounce. I used a 0069 bias factor of 0.2. A larger bias factor makes the bridge less stable, a smaller 0070 factor makes joints and contacts more spongy. 0071 0072 Pseudo Velocities - the is more stable than the Baumgarte method. The bridge is 0073 stable. However, joints still separate with large angular velocities. Drag the 0074 simple pendulum in a circle quickly and the joint will separate. The chain separates 0075 easily and does not recover. I used a bias factor of 0.2. A larger value lead to 0076 the bridge collapsing when a heavy cube drops on it. 0077 0078 Modified NGS - this algorithm is better in some ways than Baumgarte and Pseudo 0079 Velocities, but in other ways it is worse. The bridge and chain are much more 0080 stable, but the simple pendulum goes unstable at high angular velocities. 0081 0082 Full NGS - stable in all tests. The joints display good stiffness. The bridge 0083 still sags, but this is better than infinite forces. 0084 0085 Recommendations 0086 Pseudo Velocities are not really worthwhile because the bridge and chain cannot 0087 recover from joint separation. In other cases the benefit over Baumgarte is small. 0088 0089 Modified NGS is not a robust method for the revolute joint due to the violent 0090 instability seen in the simple pendulum. Perhaps it is viable with other constraint 0091 types, especially scalar constraints where the effective mass is a scalar. 0092 0093 This leaves Baumgarte and Full NGS. Baumgarte has small, but manageable instabilities 0094 and is very fast. I don't think we can escape Baumgarte, especially in highly 0095 demanding cases where high constraint fidelity is not needed. 0096 0097 Full NGS is robust and easy on the eyes. I recommend this as an option for 0098 higher fidelity simulation and certainly for suspension bridges and long chains. 0099 Full NGS might be a good choice for ragdolls, especially motorized ragdolls where 0100 joint separation can be problematic. The number of NGS iterations can be reduced 0101 for better performance without harming robustness much. 0102 0103 Each joint in a can be handled differently in the position solver. So I recommend 0104 a system where the user can select the algorithm on a per joint basis. I would 0105 probably default to the slower Full NGS and let the user select the faster 0106 Baumgarte method in performance critical scenarios. 0107 */ 0108 0109 /* 0110 Cache Performance 0111 0112 The Box2D solvers are dominated by cache misses. Data structures are designed 0113 to increase the number of cache hits. Much of misses are due to random access 0114 to body data. The constraint structures are iterated over linearly, which leads 0115 to few cache misses. 0116 0117 The bodies are not accessed during iteration. Instead read only data, such as 0118 the mass values are stored with the constraints. The mutable data are the constraint 0119 impulses and the bodies velocities/positions. The impulses are held inside the 0120 constraint structures. The body velocities/positions are held in compact, temporary 0121 arrays to increase the number of cache hits. Linear and angular velocity are 0122 stored in a single array since multiple arrays lead to multiple misses. 0123 */ 0124 0125 /* 0126 2D Rotation 0127 0128 R = [cos(theta) -sin(theta)] 0129 [sin(theta) cos(theta) ] 0130 0131 thetaDot = omega 0132 0133 Let q1 = cos(theta), q2 = sin(theta). 0134 R = [q1 -q2] 0135 [q2 q1] 0136 0137 q1Dot = -thetaDot * q2 0138 q2Dot = thetaDot * q1 0139 0140 q1_new = q1_old - dt * w * q2 0141 q2_new = q2_old + dt * w * q1 0142 then normalize. 0143 0144 This might be faster than computing sin+cos. 0145 However, we can compute sin+cos of the same angle fast. 0146 */ 0147 0148 b2Island::b2Island( 0149 int32 bodyCapacity, 0150 int32 contactCapacity, 0151 int32 jointCapacity, 0152 b2StackAllocator* allocator, 0153 b2ContactListener* listener) 0154 { 0155 m_bodyCapacity = bodyCapacity; 0156 m_contactCapacity = contactCapacity; 0157 m_jointCapacity = jointCapacity; 0158 m_bodyCount = 0; 0159 m_contactCount = 0; 0160 m_jointCount = 0; 0161 0162 m_allocator = allocator; 0163 m_listener = listener; 0164 0165 m_bodies = (b2Body**)m_allocator->Allocate(bodyCapacity * sizeof(b2Body*)); 0166 m_contacts = (b2Contact**)m_allocator->Allocate(contactCapacity * sizeof(b2Contact*)); 0167 m_joints = (b2Joint**)m_allocator->Allocate(jointCapacity * sizeof(b2Joint*)); 0168 0169 m_velocities = (b2Velocity*)m_allocator->Allocate(m_bodyCapacity * sizeof(b2Velocity)); 0170 m_positions = (b2Position*)m_allocator->Allocate(m_bodyCapacity * sizeof(b2Position)); 0171 } 0172 0173 b2Island::~b2Island() 0174 { 0175 // Warning: the order should reverse the constructor order. 0176 m_allocator->Free(m_positions); 0177 m_allocator->Free(m_velocities); 0178 m_allocator->Free(m_joints); 0179 m_allocator->Free(m_contacts); 0180 m_allocator->Free(m_bodies); 0181 } 0182 0183 void b2Island::Solve(b2Profile* profile, const b2TimeStep& step, const b2Vec2& gravity, bool allowSleep) 0184 { 0185 b2Timer timer; 0186 0187 float32 h = step.dt; 0188 0189 // Integrate velocities and apply damping. Initialize the body state. 0190 for (int32 i = 0; i < m_bodyCount; ++i) 0191 { 0192 b2Body* b = m_bodies[i]; 0193 0194 b2Vec2 c = b->m_sweep.c; 0195 float32 a = b->m_sweep.a; 0196 b2Vec2 v = b->m_linearVelocity; 0197 float32 w = b->m_angularVelocity; 0198 0199 // Store positions for continuous collision. 0200 b->m_sweep.c0 = b->m_sweep.c; 0201 b->m_sweep.a0 = b->m_sweep.a; 0202 0203 if (b->m_type == b2_dynamicBody) 0204 { 0205 // Integrate velocities. 0206 v += h * (b->m_gravityScale * gravity + b->m_invMass * b->m_force); 0207 w += h * b->m_invI * b->m_torque; 0208 0209 // Apply damping. 0210 // ODE: dv/dt + c * v = 0 0211 // Solution: v(t) = v0 * exp(-c * t) 0212 // Time step: v(t + dt) = v0 * exp(-c * (t + dt)) = v0 * exp(-c * t) * exp(-c * dt) = v * exp(-c * dt) 0213 // v2 = exp(-c * dt) * v1 0214 // Pade approximation: 0215 // v2 = v1 * 1 / (1 + c * dt) 0216 v *= 1.0f / (1.0f + h * b->m_linearDamping); 0217 w *= 1.0f / (1.0f + h * b->m_angularDamping); 0218 } 0219 0220 m_positions[i].c = c; 0221 m_positions[i].a = a; 0222 m_velocities[i].v = v; 0223 m_velocities[i].w = w; 0224 } 0225 0226 timer.Reset(); 0227 0228 // Solver data 0229 b2SolverData solverData; 0230 solverData.step = step; 0231 solverData.positions = m_positions; 0232 solverData.velocities = m_velocities; 0233 0234 // Initialize velocity constraints. 0235 b2ContactSolverDef contactSolverDef; 0236 contactSolverDef.step = step; 0237 contactSolverDef.contacts = m_contacts; 0238 contactSolverDef.count = m_contactCount; 0239 contactSolverDef.positions = m_positions; 0240 contactSolverDef.velocities = m_velocities; 0241 contactSolverDef.allocator = m_allocator; 0242 0243 b2ContactSolver contactSolver(&contactSolverDef); 0244 contactSolver.InitializeVelocityConstraints(); 0245 0246 if (step.warmStarting) 0247 { 0248 contactSolver.WarmStart(); 0249 } 0250 0251 for (int32 i = 0; i < m_jointCount; ++i) 0252 { 0253 m_joints[i]->InitVelocityConstraints(solverData); 0254 } 0255 0256 profile->solveInit = timer.GetMilliseconds(); 0257 0258 // Solve velocity constraints 0259 timer.Reset(); 0260 for (int32 i = 0; i < step.velocityIterations; ++i) 0261 { 0262 for (int32 j = 0; j < m_jointCount; ++j) 0263 { 0264 m_joints[j]->SolveVelocityConstraints(solverData); 0265 } 0266 0267 contactSolver.SolveVelocityConstraints(); 0268 } 0269 0270 // Store impulses for warm starting 0271 contactSolver.StoreImpulses(); 0272 profile->solveVelocity = timer.GetMilliseconds(); 0273 0274 // Integrate positions 0275 for (int32 i = 0; i < m_bodyCount; ++i) 0276 { 0277 b2Vec2 c = m_positions[i].c; 0278 float32 a = m_positions[i].a; 0279 b2Vec2 v = m_velocities[i].v; 0280 float32 w = m_velocities[i].w; 0281 0282 // Check for large velocities 0283 b2Vec2 translation = h * v; 0284 if (b2Dot(translation, translation) > b2_maxTranslationSquared) 0285 { 0286 float32 ratio = b2_maxTranslation / translation.Length(); 0287 v *= ratio; 0288 } 0289 0290 float32 rotation = h * w; 0291 if (rotation * rotation > b2_maxRotationSquared) 0292 { 0293 float32 ratio = b2_maxRotation / b2Abs(rotation); 0294 w *= ratio; 0295 } 0296 0297 // Integrate 0298 c += h * v; 0299 a += h * w; 0300 0301 m_positions[i].c = c; 0302 m_positions[i].a = a; 0303 m_velocities[i].v = v; 0304 m_velocities[i].w = w; 0305 } 0306 0307 // Solve position constraints 0308 timer.Reset(); 0309 bool positionSolved = false; 0310 for (int32 i = 0; i < step.positionIterations; ++i) 0311 { 0312 bool contactsOkay = contactSolver.SolvePositionConstraints(); 0313 0314 bool jointsOkay = true; 0315 for (int32 i = 0; i < m_jointCount; ++i) 0316 { 0317 bool jointOkay = m_joints[i]->SolvePositionConstraints(solverData); 0318 jointsOkay = jointsOkay && jointOkay; 0319 } 0320 0321 if (contactsOkay && jointsOkay) 0322 { 0323 // Exit early if the position errors are small. 0324 positionSolved = true; 0325 break; 0326 } 0327 } 0328 0329 // Copy state buffers back to the bodies 0330 for (int32 i = 0; i < m_bodyCount; ++i) 0331 { 0332 b2Body* body = m_bodies[i]; 0333 body->m_sweep.c = m_positions[i].c; 0334 body->m_sweep.a = m_positions[i].a; 0335 body->m_linearVelocity = m_velocities[i].v; 0336 body->m_angularVelocity = m_velocities[i].w; 0337 body->SynchronizeTransform(); 0338 } 0339 0340 profile->solvePosition = timer.GetMilliseconds(); 0341 0342 Report(contactSolver.m_velocityConstraints); 0343 0344 if (allowSleep) 0345 { 0346 float32 minSleepTime = b2_maxFloat; 0347 0348 const float32 linTolSqr = b2_linearSleepTolerance * b2_linearSleepTolerance; 0349 const float32 angTolSqr = b2_angularSleepTolerance * b2_angularSleepTolerance; 0350 0351 for (int32 i = 0; i < m_bodyCount; ++i) 0352 { 0353 b2Body* b = m_bodies[i]; 0354 if (b->GetType() == b2_staticBody) 0355 { 0356 continue; 0357 } 0358 0359 if ((b->m_flags & b2Body::e_autoSleepFlag) == 0 || 0360 b->m_angularVelocity * b->m_angularVelocity > angTolSqr || 0361 b2Dot(b->m_linearVelocity, b->m_linearVelocity) > linTolSqr) 0362 { 0363 b->m_sleepTime = 0.0f; 0364 minSleepTime = 0.0f; 0365 } 0366 else 0367 { 0368 b->m_sleepTime += h; 0369 minSleepTime = b2Min(minSleepTime, b->m_sleepTime); 0370 } 0371 } 0372 0373 if (minSleepTime >= b2_timeToSleep && positionSolved) 0374 { 0375 for (int32 i = 0; i < m_bodyCount; ++i) 0376 { 0377 b2Body* b = m_bodies[i]; 0378 b->SetAwake(false); 0379 } 0380 } 0381 } 0382 } 0383 0384 void b2Island::SolveTOI(const b2TimeStep& subStep, int32 toiIndexA, int32 toiIndexB) 0385 { 0386 b2Assert(toiIndexA < m_bodyCount); 0387 b2Assert(toiIndexB < m_bodyCount); 0388 0389 // Initialize the body state. 0390 for (int32 i = 0; i < m_bodyCount; ++i) 0391 { 0392 b2Body* b = m_bodies[i]; 0393 m_positions[i].c = b->m_sweep.c; 0394 m_positions[i].a = b->m_sweep.a; 0395 m_velocities[i].v = b->m_linearVelocity; 0396 m_velocities[i].w = b->m_angularVelocity; 0397 } 0398 0399 b2ContactSolverDef contactSolverDef; 0400 contactSolverDef.contacts = m_contacts; 0401 contactSolverDef.count = m_contactCount; 0402 contactSolverDef.allocator = m_allocator; 0403 contactSolverDef.step = subStep; 0404 contactSolverDef.positions = m_positions; 0405 contactSolverDef.velocities = m_velocities; 0406 b2ContactSolver contactSolver(&contactSolverDef); 0407 0408 // Solve position constraints. 0409 for (int32 i = 0; i < subStep.positionIterations; ++i) 0410 { 0411 bool contactsOkay = contactSolver.SolveTOIPositionConstraints(toiIndexA, toiIndexB); 0412 if (contactsOkay) 0413 { 0414 break; 0415 } 0416 } 0417 0418 #if 0 0419 // Is the new position really safe? 0420 for (int32 i = 0; i < m_contactCount; ++i) 0421 { 0422 b2Contact* c = m_contacts[i]; 0423 b2Fixture* fA = c->GetFixtureA(); 0424 b2Fixture* fB = c->GetFixtureB(); 0425 0426 b2Body* bA = fA->GetBody(); 0427 b2Body* bB = fB->GetBody(); 0428 0429 int32 indexA = c->GetChildIndexA(); 0430 int32 indexB = c->GetChildIndexB(); 0431 0432 b2DistanceInput input; 0433 input.proxyA.Set(fA->GetShape(), indexA); 0434 input.proxyB.Set(fB->GetShape(), indexB); 0435 input.transformA = bA->GetTransform(); 0436 input.transformB = bB->GetTransform(); 0437 input.useRadii = false; 0438 0439 b2DistanceOutput output; 0440 b2SimplexCache cache; 0441 cache.count = 0; 0442 b2Distance(&output, &cache, &input); 0443 0444 if (output.distance == 0 || cache.count == 3) 0445 { 0446 cache.count += 0; 0447 } 0448 } 0449 #endif 0450 0451 // Leap of faith to new safe state. 0452 m_bodies[toiIndexA]->m_sweep.c0 = m_positions[toiIndexA].c; 0453 m_bodies[toiIndexA]->m_sweep.a0 = m_positions[toiIndexA].a; 0454 m_bodies[toiIndexB]->m_sweep.c0 = m_positions[toiIndexB].c; 0455 m_bodies[toiIndexB]->m_sweep.a0 = m_positions[toiIndexB].a; 0456 0457 // No warm starting is needed for TOI events because warm 0458 // starting impulses were applied in the discrete solver. 0459 contactSolver.InitializeVelocityConstraints(); 0460 0461 // Solve velocity constraints. 0462 for (int32 i = 0; i < subStep.velocityIterations; ++i) 0463 { 0464 contactSolver.SolveVelocityConstraints(); 0465 } 0466 0467 // Don't store the TOI contact forces for warm starting 0468 // because they can be quite large. 0469 0470 float32 h = subStep.dt; 0471 0472 // Integrate positions 0473 for (int32 i = 0; i < m_bodyCount; ++i) 0474 { 0475 b2Vec2 c = m_positions[i].c; 0476 float32 a = m_positions[i].a; 0477 b2Vec2 v = m_velocities[i].v; 0478 float32 w = m_velocities[i].w; 0479 0480 // Check for large velocities 0481 b2Vec2 translation = h * v; 0482 if (b2Dot(translation, translation) > b2_maxTranslationSquared) 0483 { 0484 float32 ratio = b2_maxTranslation / translation.Length(); 0485 v *= ratio; 0486 } 0487 0488 float32 rotation = h * w; 0489 if (rotation * rotation > b2_maxRotationSquared) 0490 { 0491 float32 ratio = b2_maxRotation / b2Abs(rotation); 0492 w *= ratio; 0493 } 0494 0495 // Integrate 0496 c += h * v; 0497 a += h * w; 0498 0499 m_positions[i].c = c; 0500 m_positions[i].a = a; 0501 m_velocities[i].v = v; 0502 m_velocities[i].w = w; 0503 0504 // Sync bodies 0505 b2Body* body = m_bodies[i]; 0506 body->m_sweep.c = c; 0507 body->m_sweep.a = a; 0508 body->m_linearVelocity = v; 0509 body->m_angularVelocity = w; 0510 body->SynchronizeTransform(); 0511 } 0512 0513 Report(contactSolver.m_velocityConstraints); 0514 } 0515 0516 void b2Island::Report(const b2ContactVelocityConstraint* constraints) 0517 { 0518 if (m_listener == NULL) 0519 { 0520 return; 0521 } 0522 0523 for (int32 i = 0; i < m_contactCount; ++i) 0524 { 0525 b2Contact* c = m_contacts[i]; 0526 0527 const b2ContactVelocityConstraint* vc = constraints + i; 0528 0529 b2ContactImpulse impulse; 0530 impulse.count = vc->pointCount; 0531 for (int32 j = 0; j < vc->pointCount; ++j) 0532 { 0533 impulse.normalImpulses[j] = vc->points[j].normalImpulse; 0534 impulse.tangentImpulses[j] = vc->points[j].tangentImpulse; 0535 } 0536 0537 m_listener->PostSolve(c, &impulse); 0538 } 0539 }