b2Island.cpp
Go to the documentation of this file.
00001 /*
00002 * Copyright (c) 2006-2011 Erin Catto http://www.box2d.org
00003 *
00004 * This software is provided 'as-is', without any express or implied
00005 * warranty.  In no event will the authors be held liable for any damages
00006 * arising from the use of this software.
00007 * Permission is granted to anyone to use this software for any purpose,
00008 * including commercial applications, and to alter it and redistribute it
00009 * freely, subject to the following restrictions:
00010 * 1. The origin of this software must not be misrepresented; you must not
00011 * claim that you wrote the original software. If you use this software
00012 * in a product, an acknowledgment in the product documentation would be
00013 * appreciated but is not required.
00014 * 2. Altered source versions must be plainly marked as such, and must not be
00015 * misrepresented as being the original software.
00016 * 3. This notice may not be removed or altered from any source distribution.
00017 */
00018 
00019 #include <Box2D/Collision/b2Distance.h>
00020 #include <Box2D/Dynamics/b2Island.h>
00021 #include <Box2D/Dynamics/b2Body.h>
00022 #include <Box2D/Dynamics/b2Fixture.h>
00023 #include <Box2D/Dynamics/b2World.h>
00024 #include <Box2D/Dynamics/Contacts/b2Contact.h>
00025 #include <Box2D/Dynamics/Contacts/b2ContactSolver.h>
00026 #include <Box2D/Dynamics/Joints/b2Joint.h>
00027 #include <Box2D/Common/b2StackAllocator.h>
00028 #include <Box2D/Common/b2Timer.h>
00029 
00030 /*
00031 Position Correction Notes
00032 =========================
00033 I tried the several algorithms for position correction of the 2D revolute joint.
00034 I looked at these systems:
00035 - simple pendulum (1m diameter sphere on massless 5m stick) with initial angular velocity of 100 rad/s.
00036 - suspension bridge with 30 1m long planks of length 1m.
00037 - multi-link chain with 30 1m long links.
00038 
00039 Here are the algorithms:
00040 
00041 Baumgarte - A fraction of the position error is added to the velocity error. There is no
00042 separate position solver.
00043 
00044 Pseudo Velocities - After the velocity solver and position integration,
00045 the position error, Jacobian, and effective mass are recomputed. Then
00046 the velocity constraints are solved with pseudo velocities and a fraction
00047 of the position error is added to the pseudo velocity error. The pseudo
00048 velocities are initialized to zero and there is no warm-starting. After
00049 the position solver, the pseudo velocities are added to the positions.
00050 This is also called the First Order World method or the Position LCP method.
00051 
00052 Modified Nonlinear Gauss-Seidel (NGS) - Like Pseudo Velocities except the
00053 position error is re-computed for each constraint and the positions are updated
00054 after the constraint is solved. The radius vectors (aka Jacobians) are
00055 re-computed too (otherwise the algorithm has horrible instability). The pseudo
00056 velocity states are not needed because they are effectively zero at the beginning
00057 of each iteration. Since we have the current position error, we allow the
00058 iterations to terminate early if the error becomes smaller than b2_linearSlop.
00059 
00060 Full NGS or just NGS - Like Modified NGS except the effective mass are re-computed
00061 each time a constraint is solved.
00062 
00063 Here are the results:
00064 Baumgarte - this is the cheapest algorithm but it has some stability problems,
00065 especially with the bridge. The chain links separate easily close to the root
00066 and they jitter as they struggle to pull together. This is one of the most common
00067 methods in the field. The big drawback is that the position correction artificially
00068 affects the momentum, thus leading to instabilities and false bounce. I used a
00069 bias factor of 0.2. A larger bias factor makes the bridge less stable, a smaller
00070 factor makes joints and contacts more spongy.
00071 
00072 Pseudo Velocities - the is more stable than the Baumgarte method. The bridge is
00073 stable. However, joints still separate with large angular velocities. Drag the
00074 simple pendulum in a circle quickly and the joint will separate. The chain separates
00075 easily and does not recover. I used a bias factor of 0.2. A larger value lead to
00076 the bridge collapsing when a heavy cube drops on it.
00077 
00078 Modified NGS - this algorithm is better in some ways than Baumgarte and Pseudo
00079 Velocities, but in other ways it is worse. The bridge and chain are much more
00080 stable, but the simple pendulum goes unstable at high angular velocities.
00081 
00082 Full NGS - stable in all tests. The joints display good stiffness. The bridge
00083 still sags, but this is better than infinite forces.
00084 
00085 Recommendations
00086 Pseudo Velocities are not really worthwhile because the bridge and chain cannot
00087 recover from joint separation. In other cases the benefit over Baumgarte is small.
00088 
00089 Modified NGS is not a robust method for the revolute joint due to the violent
00090 instability seen in the simple pendulum. Perhaps it is viable with other constraint
00091 types, especially scalar constraints where the effective mass is a scalar.
00092 
00093 This leaves Baumgarte and Full NGS. Baumgarte has small, but manageable instabilities
00094 and is very fast. I don't think we can escape Baumgarte, especially in highly
00095 demanding cases where high constraint fidelity is not needed.
00096 
00097 Full NGS is robust and easy on the eyes. I recommend this as an option for
00098 higher fidelity simulation and certainly for suspension bridges and long chains.
00099 Full NGS might be a good choice for ragdolls, especially motorized ragdolls where
00100 joint separation can be problematic. The number of NGS iterations can be reduced
00101 for better performance without harming robustness much.
00102 
00103 Each joint in a can be handled differently in the position solver. So I recommend
00104 a system where the user can select the algorithm on a per joint basis. I would
00105 probably default to the slower Full NGS and let the user select the faster
00106 Baumgarte method in performance critical scenarios.
00107 */
00108 
00109 /*
00110 Cache Performance
00111 
00112 The Box2D solvers are dominated by cache misses. Data structures are designed
00113 to increase the number of cache hits. Much of misses are due to random access
00114 to body data. The constraint structures are iterated over linearly, which leads
00115 to few cache misses.
00116 
00117 The bodies are not accessed during iteration. Instead read only data, such as
00118 the mass values are stored with the constraints. The mutable data are the constraint
00119 impulses and the bodies velocities/positions. The impulses are held inside the
00120 constraint structures. The body velocities/positions are held in compact, temporary
00121 arrays to increase the number of cache hits. Linear and angular velocity are
00122 stored in a single array since multiple arrays lead to multiple misses.
00123 */
00124 
00125 /*
00126 2D Rotation
00127 
00128 R = [cos(theta) -sin(theta)]
00129     [sin(theta) cos(theta) ]
00130 
00131 thetaDot = omega
00132 
00133 Let q1 = cos(theta), q2 = sin(theta).
00134 R = [q1 -q2]
00135     [q2  q1]
00136 
00137 q1Dot = -thetaDot * q2
00138 q2Dot = thetaDot * q1
00139 
00140 q1_new = q1_old - dt * w * q2
00141 q2_new = q2_old + dt * w * q1
00142 then normalize.
00143 
00144 This might be faster than computing sin+cos.
00145 However, we can compute sin+cos of the same angle fast.
00146 */
00147 
00148 b2Island::b2Island(
00149         int32 bodyCapacity,
00150         int32 contactCapacity,
00151         int32 jointCapacity,
00152         b2StackAllocator* allocator,
00153         b2ContactListener* listener)
00154 {
00155         m_bodyCapacity = bodyCapacity;
00156         m_contactCapacity = contactCapacity;
00157         m_jointCapacity  = jointCapacity;
00158         m_bodyCount = 0;
00159         m_contactCount = 0;
00160         m_jointCount = 0;
00161 
00162         m_allocator = allocator;
00163         m_listener = listener;
00164 
00165         m_bodies = (b2Body**)m_allocator->Allocate(bodyCapacity * sizeof(b2Body*));
00166         m_contacts = (b2Contact**)m_allocator->Allocate(contactCapacity  * sizeof(b2Contact*));
00167         m_joints = (b2Joint**)m_allocator->Allocate(jointCapacity * sizeof(b2Joint*));
00168 
00169         m_velocities = (b2Velocity*)m_allocator->Allocate(m_bodyCapacity * sizeof(b2Velocity));
00170         m_positions = (b2Position*)m_allocator->Allocate(m_bodyCapacity * sizeof(b2Position));
00171 }
00172 
00173 b2Island::~b2Island()
00174 {
00175         // Warning: the order should reverse the constructor order.
00176         m_allocator->Free(m_positions);
00177         m_allocator->Free(m_velocities);
00178         m_allocator->Free(m_joints);
00179         m_allocator->Free(m_contacts);
00180         m_allocator->Free(m_bodies);
00181 }
00182 
00183 void b2Island::Solve(b2Profile* profile, const b2TimeStep& step, const b2Vec2& gravity, bool allowSleep)
00184 {
00185         b2Timer timer;
00186 
00187         float32 h = step.dt;
00188 
00189         // Integrate velocities and apply damping. Initialize the body state.
00190         for (int32 i = 0; i < m_bodyCount; ++i)
00191         {
00192                 b2Body* b = m_bodies[i];
00193 
00194                 b2Vec2 c = b->m_sweep.c;
00195                 float32 a = b->m_sweep.a;
00196                 b2Vec2 v = b->m_linearVelocity;
00197                 float32 w = b->m_angularVelocity;
00198 
00199                 // Store positions for continuous collision.
00200                 b->m_sweep.c0 = b->m_sweep.c;
00201                 b->m_sweep.a0 = b->m_sweep.a;
00202 
00203                 if (b->m_type == b2_dynamicBody)
00204                 {
00205                         // Integrate velocities.
00206                         v += h * (b->m_gravityScale * gravity + b->m_invMass * b->m_force);
00207                         w += h * b->m_invI * b->m_torque;
00208 
00209                         // Apply damping.
00210                         // ODE: dv/dt + c * v = 0
00211                         // Solution: v(t) = v0 * exp(-c * t)
00212                         // Time step: v(t + dt) = v0 * exp(-c * (t + dt)) = v0 * exp(-c * t) * exp(-c * dt) = v * exp(-c * dt)
00213                         // v2 = exp(-c * dt) * v1
00214                         // Pade approximation:
00215                         // v2 = v1 * 1 / (1 + c * dt)
00216                         v *= 1.0f / (1.0f + h * b->m_linearDamping);
00217                         w *= 1.0f / (1.0f + h * b->m_angularDamping);
00218                 }
00219 
00220                 m_positions[i].c = c;
00221                 m_positions[i].a = a;
00222                 m_velocities[i].v = v;
00223                 m_velocities[i].w = w;
00224         }
00225 
00226         timer.Reset();
00227 
00228         // Solver data
00229         b2SolverData solverData;
00230         solverData.step = step;
00231         solverData.positions = m_positions;
00232         solverData.velocities = m_velocities;
00233 
00234         // Initialize velocity constraints.
00235         b2ContactSolverDef contactSolverDef;
00236         contactSolverDef.step = step;
00237         contactSolverDef.contacts = m_contacts;
00238         contactSolverDef.count = m_contactCount;
00239         contactSolverDef.positions = m_positions;
00240         contactSolverDef.velocities = m_velocities;
00241         contactSolverDef.allocator = m_allocator;
00242 
00243         b2ContactSolver contactSolver(&contactSolverDef);
00244         contactSolver.InitializeVelocityConstraints();
00245 
00246         if (step.warmStarting)
00247         {
00248                 contactSolver.WarmStart();
00249         }
00250 
00251         for (int32 i = 0; i < m_jointCount; ++i)
00252         {
00253                 m_joints[i]->InitVelocityConstraints(solverData);
00254         }
00255 
00256         profile->solveInit = timer.GetMilliseconds();
00257 
00258         // Solve velocity constraints
00259         timer.Reset();
00260         for (int32 i = 0; i < step.velocityIterations; ++i)
00261         {
00262                 for (int32 j = 0; j < m_jointCount; ++j)
00263                 {
00264                         m_joints[j]->SolveVelocityConstraints(solverData);
00265                 }
00266 
00267                 contactSolver.SolveVelocityConstraints();
00268         }
00269 
00270         // Store impulses for warm starting
00271         contactSolver.StoreImpulses();
00272         profile->solveVelocity = timer.GetMilliseconds();
00273 
00274         // Integrate positions
00275         for (int32 i = 0; i < m_bodyCount; ++i)
00276         {
00277                 b2Vec2 c = m_positions[i].c;
00278                 float32 a = m_positions[i].a;
00279                 b2Vec2 v = m_velocities[i].v;
00280                 float32 w = m_velocities[i].w;
00281 
00282                 // Check for large velocities
00283                 b2Vec2 translation = h * v;
00284                 if (b2Dot(translation, translation) > b2_maxTranslationSquared)
00285                 {
00286                         float32 ratio = b2_maxTranslation / translation.Length();
00287                         v *= ratio;
00288                 }
00289 
00290                 float32 rotation = h * w;
00291                 if (rotation * rotation > b2_maxRotationSquared)
00292                 {
00293                         float32 ratio = b2_maxRotation / b2Abs(rotation);
00294                         w *= ratio;
00295                 }
00296 
00297                 // Integrate
00298                 c += h * v;
00299                 a += h * w;
00300 
00301                 m_positions[i].c = c;
00302                 m_positions[i].a = a;
00303                 m_velocities[i].v = v;
00304                 m_velocities[i].w = w;
00305         }
00306 
00307         // Solve position constraints
00308         timer.Reset();
00309         bool positionSolved = false;
00310         for (int32 i = 0; i < step.positionIterations; ++i)
00311         {
00312                 bool contactsOkay = contactSolver.SolvePositionConstraints();
00313 
00314                 bool jointsOkay = true;
00315                 for (int32 i = 0; i < m_jointCount; ++i)
00316                 {
00317                         bool jointOkay = m_joints[i]->SolvePositionConstraints(solverData);
00318                         jointsOkay = jointsOkay && jointOkay;
00319                 }
00320 
00321                 if (contactsOkay && jointsOkay)
00322                 {
00323                         // Exit early if the position errors are small.
00324                         positionSolved = true;
00325                         break;
00326                 }
00327         }
00328 
00329         // Copy state buffers back to the bodies
00330         for (int32 i = 0; i < m_bodyCount; ++i)
00331         {
00332                 b2Body* body = m_bodies[i];
00333                 body->m_sweep.c = m_positions[i].c;
00334                 body->m_sweep.a = m_positions[i].a;
00335                 body->m_linearVelocity = m_velocities[i].v;
00336                 body->m_angularVelocity = m_velocities[i].w;
00337                 body->SynchronizeTransform();
00338         }
00339 
00340         profile->solvePosition = timer.GetMilliseconds();
00341 
00342         Report(contactSolver.m_velocityConstraints);
00343 
00344         if (allowSleep)
00345         {
00346                 float32 minSleepTime = b2_maxFloat;
00347 
00348                 const float32 linTolSqr = b2_linearSleepTolerance * b2_linearSleepTolerance;
00349                 const float32 angTolSqr = b2_angularSleepTolerance * b2_angularSleepTolerance;
00350 
00351                 for (int32 i = 0; i < m_bodyCount; ++i)
00352                 {
00353                         b2Body* b = m_bodies[i];
00354                         if (b->GetType() == b2_staticBody)
00355                         {
00356                                 continue;
00357                         }
00358 
00359                         if ((b->m_flags & b2Body::e_autoSleepFlag) == 0 ||
00360                                 b->m_angularVelocity * b->m_angularVelocity > angTolSqr ||
00361                                 b2Dot(b->m_linearVelocity, b->m_linearVelocity) > linTolSqr)
00362                         {
00363                                 b->m_sleepTime = 0.0f;
00364                                 minSleepTime = 0.0f;
00365                         }
00366                         else
00367                         {
00368                                 b->m_sleepTime += h;
00369                                 minSleepTime = b2Min(minSleepTime, b->m_sleepTime);
00370                         }
00371                 }
00372 
00373                 if (minSleepTime >= b2_timeToSleep && positionSolved)
00374                 {
00375                         for (int32 i = 0; i < m_bodyCount; ++i)
00376                         {
00377                                 b2Body* b = m_bodies[i];
00378                                 b->SetAwake(false);
00379                         }
00380                 }
00381         }
00382 }
00383 
00384 void b2Island::SolveTOI(const b2TimeStep& subStep, int32 toiIndexA, int32 toiIndexB)
00385 {
00386         b2Assert(toiIndexA < m_bodyCount);
00387         b2Assert(toiIndexB < m_bodyCount);
00388 
00389         // Initialize the body state.
00390         for (int32 i = 0; i < m_bodyCount; ++i)
00391         {
00392                 b2Body* b = m_bodies[i];
00393                 m_positions[i].c = b->m_sweep.c;
00394                 m_positions[i].a = b->m_sweep.a;
00395                 m_velocities[i].v = b->m_linearVelocity;
00396                 m_velocities[i].w = b->m_angularVelocity;
00397         }
00398 
00399         b2ContactSolverDef contactSolverDef;
00400         contactSolverDef.contacts = m_contacts;
00401         contactSolverDef.count = m_contactCount;
00402         contactSolverDef.allocator = m_allocator;
00403         contactSolverDef.step = subStep;
00404         contactSolverDef.positions = m_positions;
00405         contactSolverDef.velocities = m_velocities;
00406         b2ContactSolver contactSolver(&contactSolverDef);
00407 
00408         // Solve position constraints.
00409         for (int32 i = 0; i < subStep.positionIterations; ++i)
00410         {
00411                 bool contactsOkay = contactSolver.SolveTOIPositionConstraints(toiIndexA, toiIndexB);
00412                 if (contactsOkay)
00413                 {
00414                         break;
00415                 }
00416         }
00417 
00418 #if 0
00419         // Is the new position really safe?
00420         for (int32 i = 0; i < m_contactCount; ++i)
00421         {
00422                 b2Contact* c = m_contacts[i];
00423                 b2Fixture* fA = c->GetFixtureA();
00424                 b2Fixture* fB = c->GetFixtureB();
00425 
00426                 b2Body* bA = fA->GetBody();
00427                 b2Body* bB = fB->GetBody();
00428 
00429                 int32 indexA = c->GetChildIndexA();
00430                 int32 indexB = c->GetChildIndexB();
00431 
00432                 b2DistanceInput input;
00433                 input.proxyA.Set(fA->GetShape(), indexA);
00434                 input.proxyB.Set(fB->GetShape(), indexB);
00435                 input.transformA = bA->GetTransform();
00436                 input.transformB = bB->GetTransform();
00437                 input.useRadii = false;
00438 
00439                 b2DistanceOutput output;
00440                 b2SimplexCache cache;
00441                 cache.count = 0;
00442                 b2Distance(&output, &cache, &input);
00443 
00444                 if (output.distance == 0 || cache.count == 3)
00445                 {
00446                         cache.count += 0;
00447                 }
00448         }
00449 #endif
00450 
00451         // Leap of faith to new safe state.
00452         m_bodies[toiIndexA]->m_sweep.c0 = m_positions[toiIndexA].c;
00453         m_bodies[toiIndexA]->m_sweep.a0 = m_positions[toiIndexA].a;
00454         m_bodies[toiIndexB]->m_sweep.c0 = m_positions[toiIndexB].c;
00455         m_bodies[toiIndexB]->m_sweep.a0 = m_positions[toiIndexB].a;
00456 
00457         // No warm starting is needed for TOI events because warm
00458         // starting impulses were applied in the discrete solver.
00459         contactSolver.InitializeVelocityConstraints();
00460 
00461         // Solve velocity constraints.
00462         for (int32 i = 0; i < subStep.velocityIterations; ++i)
00463         {
00464                 contactSolver.SolveVelocityConstraints();
00465         }
00466 
00467         // Don't store the TOI contact forces for warm starting
00468         // because they can be quite large.
00469 
00470         float32 h = subStep.dt;
00471 
00472         // Integrate positions
00473         for (int32 i = 0; i < m_bodyCount; ++i)
00474         {
00475                 b2Vec2 c = m_positions[i].c;
00476                 float32 a = m_positions[i].a;
00477                 b2Vec2 v = m_velocities[i].v;
00478                 float32 w = m_velocities[i].w;
00479 
00480                 // Check for large velocities
00481                 b2Vec2 translation = h * v;
00482                 if (b2Dot(translation, translation) > b2_maxTranslationSquared)
00483                 {
00484                         float32 ratio = b2_maxTranslation / translation.Length();
00485                         v *= ratio;
00486                 }
00487 
00488                 float32 rotation = h * w;
00489                 if (rotation * rotation > b2_maxRotationSquared)
00490                 {
00491                         float32 ratio = b2_maxRotation / b2Abs(rotation);
00492                         w *= ratio;
00493                 }
00494 
00495                 // Integrate
00496                 c += h * v;
00497                 a += h * w;
00498 
00499                 m_positions[i].c = c;
00500                 m_positions[i].a = a;
00501                 m_velocities[i].v = v;
00502                 m_velocities[i].w = w;
00503 
00504                 // Sync bodies
00505                 b2Body* body = m_bodies[i];
00506                 body->m_sweep.c = c;
00507                 body->m_sweep.a = a;
00508                 body->m_linearVelocity = v;
00509                 body->m_angularVelocity = w;
00510                 body->SynchronizeTransform();
00511         }
00512 
00513         Report(contactSolver.m_velocityConstraints);
00514 }
00515 
00516 void b2Island::Report(const b2ContactVelocityConstraint* constraints)
00517 {
00518         if (m_listener == NULL)
00519         {
00520                 return;
00521         }
00522 
00523         for (int32 i = 0; i < m_contactCount; ++i)
00524         {
00525                 b2Contact* c = m_contacts[i];
00526 
00527                 const b2ContactVelocityConstraint* vc = constraints + i;
00528 
00529                 b2ContactImpulse impulse;
00530                 impulse.count = vc->pointCount;
00531                 for (int32 j = 0; j < vc->pointCount; ++j)
00532                 {
00533                         impulse.normalImpulses[j] = vc->points[j].normalImpulse;
00534                         impulse.tangentImpulses[j] = vc->points[j].tangentImpulse;
00535                 }
00536 
00537                 m_listener->PostSolve(c, &impulse);
00538         }
00539 }


mvsim
Author(s):
autogenerated on Thu Jun 6 2019 22:08:34