b2World.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/Dynamics/b2World.h>
00020 #include <Box2D/Dynamics/b2Body.h>
00021 #include <Box2D/Dynamics/b2Fixture.h>
00022 #include <Box2D/Dynamics/b2Island.h>
00023 #include <Box2D/Dynamics/Joints/b2PulleyJoint.h>
00024 #include <Box2D/Dynamics/Contacts/b2Contact.h>
00025 #include <Box2D/Dynamics/Contacts/b2ContactSolver.h>
00026 #include <Box2D/Collision/b2Collision.h>
00027 #include <Box2D/Collision/b2BroadPhase.h>
00028 #include <Box2D/Collision/Shapes/b2CircleShape.h>
00029 #include <Box2D/Collision/Shapes/b2EdgeShape.h>
00030 #include <Box2D/Collision/Shapes/b2ChainShape.h>
00031 #include <Box2D/Collision/Shapes/b2PolygonShape.h>
00032 #include <Box2D/Collision/b2TimeOfImpact.h>
00033 #include <Box2D/Common/b2Draw.h>
00034 #include <Box2D/Common/b2Timer.h>
00035 #include <new>
00036 
00037 b2World::b2World(const b2Vec2& gravity)
00038 {
00039         m_destructionListener = NULL;
00040         g_debugDraw = NULL;
00041 
00042         m_bodyList = NULL;
00043         m_jointList = NULL;
00044 
00045         m_bodyCount = 0;
00046         m_jointCount = 0;
00047 
00048         m_warmStarting = true;
00049         m_continuousPhysics = true;
00050         m_subStepping = false;
00051 
00052         m_stepComplete = true;
00053 
00054         m_allowSleep = true;
00055         m_gravity = gravity;
00056 
00057         m_flags = e_clearForces;
00058 
00059         m_inv_dt0 = 0.0f;
00060 
00061         m_contactManager.m_allocator = &m_blockAllocator;
00062 
00063         memset(&m_profile, 0, sizeof(b2Profile));
00064 }
00065 
00066 b2World::~b2World()
00067 {
00068         // Some shapes allocate using b2Alloc.
00069         b2Body* b = m_bodyList;
00070         while (b)
00071         {
00072                 b2Body* bNext = b->m_next;
00073 
00074                 b2Fixture* f = b->m_fixtureList;
00075                 while (f)
00076                 {
00077                         b2Fixture* fNext = f->m_next;
00078                         f->m_proxyCount = 0;
00079                         f->Destroy(&m_blockAllocator);
00080                         f = fNext;
00081                 }
00082 
00083                 b = bNext;
00084         }
00085 }
00086 
00087 void b2World::SetDestructionListener(b2DestructionListener* listener)
00088 {
00089         m_destructionListener = listener;
00090 }
00091 
00092 void b2World::SetContactFilter(b2ContactFilter* filter)
00093 {
00094         m_contactManager.m_contactFilter = filter;
00095 }
00096 
00097 void b2World::SetContactListener(b2ContactListener* listener)
00098 {
00099         m_contactManager.m_contactListener = listener;
00100 }
00101 
00102 void b2World::SetDebugDraw(b2Draw* debugDraw)
00103 {
00104         g_debugDraw = debugDraw;
00105 }
00106 
00107 b2Body* b2World::CreateBody(const b2BodyDef* def)
00108 {
00109         b2Assert(IsLocked() == false);
00110         if (IsLocked())
00111         {
00112                 return NULL;
00113         }
00114 
00115         void* mem = m_blockAllocator.Allocate(sizeof(b2Body));
00116         b2Body* b = new (mem) b2Body(def, this);
00117 
00118         // Add to world doubly linked list.
00119         b->m_prev = NULL;
00120         b->m_next = m_bodyList;
00121         if (m_bodyList)
00122         {
00123                 m_bodyList->m_prev = b;
00124         }
00125         m_bodyList = b;
00126         ++m_bodyCount;
00127 
00128         return b;
00129 }
00130 
00131 void b2World::DestroyBody(b2Body* b)
00132 {
00133         b2Assert(m_bodyCount > 0);
00134         b2Assert(IsLocked() == false);
00135         if (IsLocked())
00136         {
00137                 return;
00138         }
00139 
00140         // Delete the attached joints.
00141         b2JointEdge* je = b->m_jointList;
00142         while (je)
00143         {
00144                 b2JointEdge* je0 = je;
00145                 je = je->next;
00146 
00147                 if (m_destructionListener)
00148                 {
00149                         m_destructionListener->SayGoodbye(je0->joint);
00150                 }
00151 
00152                 DestroyJoint(je0->joint);
00153 
00154                 b->m_jointList = je;
00155         }
00156         b->m_jointList = NULL;
00157 
00158         // Delete the attached contacts.
00159         b2ContactEdge* ce = b->m_contactList;
00160         while (ce)
00161         {
00162                 b2ContactEdge* ce0 = ce;
00163                 ce = ce->next;
00164                 m_contactManager.Destroy(ce0->contact);
00165         }
00166         b->m_contactList = NULL;
00167 
00168         // Delete the attached fixtures. This destroys broad-phase proxies.
00169         b2Fixture* f = b->m_fixtureList;
00170         while (f)
00171         {
00172                 b2Fixture* f0 = f;
00173                 f = f->m_next;
00174 
00175                 if (m_destructionListener)
00176                 {
00177                         m_destructionListener->SayGoodbye(f0);
00178                 }
00179 
00180                 f0->DestroyProxies(&m_contactManager.m_broadPhase);
00181                 f0->Destroy(&m_blockAllocator);
00182                 f0->~b2Fixture();
00183                 m_blockAllocator.Free(f0, sizeof(b2Fixture));
00184 
00185                 b->m_fixtureList = f;
00186                 b->m_fixtureCount -= 1;
00187         }
00188         b->m_fixtureList = NULL;
00189         b->m_fixtureCount = 0;
00190 
00191         // Remove world body list.
00192         if (b->m_prev)
00193         {
00194                 b->m_prev->m_next = b->m_next;
00195         }
00196 
00197         if (b->m_next)
00198         {
00199                 b->m_next->m_prev = b->m_prev;
00200         }
00201 
00202         if (b == m_bodyList)
00203         {
00204                 m_bodyList = b->m_next;
00205         }
00206 
00207         --m_bodyCount;
00208         b->~b2Body();
00209         m_blockAllocator.Free(b, sizeof(b2Body));
00210 }
00211 
00212 b2Joint* b2World::CreateJoint(const b2JointDef* def)
00213 {
00214         b2Assert(IsLocked() == false);
00215         if (IsLocked())
00216         {
00217                 return NULL;
00218         }
00219 
00220         b2Joint* j = b2Joint::Create(def, &m_blockAllocator);
00221 
00222         // Connect to the world list.
00223         j->m_prev = NULL;
00224         j->m_next = m_jointList;
00225         if (m_jointList)
00226         {
00227                 m_jointList->m_prev = j;
00228         }
00229         m_jointList = j;
00230         ++m_jointCount;
00231 
00232         // Connect to the bodies' doubly linked lists.
00233         j->m_edgeA.joint = j;
00234         j->m_edgeA.other = j->m_bodyB;
00235         j->m_edgeA.prev = NULL;
00236         j->m_edgeA.next = j->m_bodyA->m_jointList;
00237         if (j->m_bodyA->m_jointList) j->m_bodyA->m_jointList->prev = &j->m_edgeA;
00238         j->m_bodyA->m_jointList = &j->m_edgeA;
00239 
00240         j->m_edgeB.joint = j;
00241         j->m_edgeB.other = j->m_bodyA;
00242         j->m_edgeB.prev = NULL;
00243         j->m_edgeB.next = j->m_bodyB->m_jointList;
00244         if (j->m_bodyB->m_jointList) j->m_bodyB->m_jointList->prev = &j->m_edgeB;
00245         j->m_bodyB->m_jointList = &j->m_edgeB;
00246 
00247         b2Body* bodyA = def->bodyA;
00248         b2Body* bodyB = def->bodyB;
00249 
00250         // If the joint prevents collisions, then flag any contacts for filtering.
00251         if (def->collideConnected == false)
00252         {
00253                 b2ContactEdge* edge = bodyB->GetContactList();
00254                 while (edge)
00255                 {
00256                         if (edge->other == bodyA)
00257                         {
00258                                 // Flag the contact for filtering at the next time step (where either
00259                                 // body is awake).
00260                                 edge->contact->FlagForFiltering();
00261                         }
00262 
00263                         edge = edge->next;
00264                 }
00265         }
00266 
00267         // Note: creating a joint doesn't wake the bodies.
00268 
00269         return j;
00270 }
00271 
00272 void b2World::DestroyJoint(b2Joint* j)
00273 {
00274         b2Assert(IsLocked() == false);
00275         if (IsLocked())
00276         {
00277                 return;
00278         }
00279 
00280         bool collideConnected = j->m_collideConnected;
00281 
00282         // Remove from the doubly linked list.
00283         if (j->m_prev)
00284         {
00285                 j->m_prev->m_next = j->m_next;
00286         }
00287 
00288         if (j->m_next)
00289         {
00290                 j->m_next->m_prev = j->m_prev;
00291         }
00292 
00293         if (j == m_jointList)
00294         {
00295                 m_jointList = j->m_next;
00296         }
00297 
00298         // Disconnect from island graph.
00299         b2Body* bodyA = j->m_bodyA;
00300         b2Body* bodyB = j->m_bodyB;
00301 
00302         // Wake up connected bodies.
00303         bodyA->SetAwake(true);
00304         bodyB->SetAwake(true);
00305 
00306         // Remove from body 1.
00307         if (j->m_edgeA.prev)
00308         {
00309                 j->m_edgeA.prev->next = j->m_edgeA.next;
00310         }
00311 
00312         if (j->m_edgeA.next)
00313         {
00314                 j->m_edgeA.next->prev = j->m_edgeA.prev;
00315         }
00316 
00317         if (&j->m_edgeA == bodyA->m_jointList)
00318         {
00319                 bodyA->m_jointList = j->m_edgeA.next;
00320         }
00321 
00322         j->m_edgeA.prev = NULL;
00323         j->m_edgeA.next = NULL;
00324 
00325         // Remove from body 2
00326         if (j->m_edgeB.prev)
00327         {
00328                 j->m_edgeB.prev->next = j->m_edgeB.next;
00329         }
00330 
00331         if (j->m_edgeB.next)
00332         {
00333                 j->m_edgeB.next->prev = j->m_edgeB.prev;
00334         }
00335 
00336         if (&j->m_edgeB == bodyB->m_jointList)
00337         {
00338                 bodyB->m_jointList = j->m_edgeB.next;
00339         }
00340 
00341         j->m_edgeB.prev = NULL;
00342         j->m_edgeB.next = NULL;
00343 
00344         b2Joint::Destroy(j, &m_blockAllocator);
00345 
00346         b2Assert(m_jointCount > 0);
00347         --m_jointCount;
00348 
00349         // If the joint prevents collisions, then flag any contacts for filtering.
00350         if (collideConnected == false)
00351         {
00352                 b2ContactEdge* edge = bodyB->GetContactList();
00353                 while (edge)
00354                 {
00355                         if (edge->other == bodyA)
00356                         {
00357                                 // Flag the contact for filtering at the next time step (where either
00358                                 // body is awake).
00359                                 edge->contact->FlagForFiltering();
00360                         }
00361 
00362                         edge = edge->next;
00363                 }
00364         }
00365 }
00366 
00367 //
00368 void b2World::SetAllowSleeping(bool flag)
00369 {
00370         if (flag == m_allowSleep)
00371         {
00372                 return;
00373         }
00374 
00375         m_allowSleep = flag;
00376         if (m_allowSleep == false)
00377         {
00378                 for (b2Body* b = m_bodyList; b; b = b->m_next)
00379                 {
00380                         b->SetAwake(true);
00381                 }
00382         }
00383 }
00384 
00385 // Find islands, integrate and solve constraints, solve position constraints
00386 void b2World::Solve(const b2TimeStep& step)
00387 {
00388         m_profile.solveInit = 0.0f;
00389         m_profile.solveVelocity = 0.0f;
00390         m_profile.solvePosition = 0.0f;
00391 
00392         // Size the island for the worst case.
00393         b2Island island(m_bodyCount,
00394                                         m_contactManager.m_contactCount,
00395                                         m_jointCount,
00396                                         &m_stackAllocator,
00397                                         m_contactManager.m_contactListener);
00398 
00399         // Clear all the island flags.
00400         for (b2Body* b = m_bodyList; b; b = b->m_next)
00401         {
00402                 b->m_flags &= ~b2Body::e_islandFlag;
00403         }
00404         for (b2Contact* c = m_contactManager.m_contactList; c; c = c->m_next)
00405         {
00406                 c->m_flags &= ~b2Contact::e_islandFlag;
00407         }
00408         for (b2Joint* j = m_jointList; j; j = j->m_next)
00409         {
00410                 j->m_islandFlag = false;
00411         }
00412 
00413         // Build and simulate all awake islands.
00414         int32 stackSize = m_bodyCount;
00415         b2Body** stack = (b2Body**)m_stackAllocator.Allocate(stackSize * sizeof(b2Body*));
00416         for (b2Body* seed = m_bodyList; seed; seed = seed->m_next)
00417         {
00418                 if (seed->m_flags & b2Body::e_islandFlag)
00419                 {
00420                         continue;
00421                 }
00422 
00423                 if (seed->IsAwake() == false || seed->IsActive() == false)
00424                 {
00425                         continue;
00426                 }
00427 
00428                 // The seed can be dynamic or kinematic.
00429                 if (seed->GetType() == b2_staticBody)
00430                 {
00431                         continue;
00432                 }
00433 
00434                 // Reset island and stack.
00435                 island.Clear();
00436                 int32 stackCount = 0;
00437                 stack[stackCount++] = seed;
00438                 seed->m_flags |= b2Body::e_islandFlag;
00439 
00440                 // Perform a depth first search (DFS) on the constraint graph.
00441                 while (stackCount > 0)
00442                 {
00443                         // Grab the next body off the stack and add it to the island.
00444                         b2Body* b = stack[--stackCount];
00445                         b2Assert(b->IsActive() == true);
00446                         island.Add(b);
00447 
00448                         // Make sure the body is awake.
00449                         b->SetAwake(true);
00450 
00451                         // To keep islands as small as possible, we don't
00452                         // propagate islands across static bodies.
00453                         if (b->GetType() == b2_staticBody)
00454                         {
00455                                 continue;
00456                         }
00457 
00458                         // Search all contacts connected to this body.
00459                         for (b2ContactEdge* ce = b->m_contactList; ce; ce = ce->next)
00460                         {
00461                                 b2Contact* contact = ce->contact;
00462 
00463                                 // Has this contact already been added to an island?
00464                                 if (contact->m_flags & b2Contact::e_islandFlag)
00465                                 {
00466                                         continue;
00467                                 }
00468 
00469                                 // Is this contact solid and touching?
00470                                 if (contact->IsEnabled() == false ||
00471                                         contact->IsTouching() == false)
00472                                 {
00473                                         continue;
00474                                 }
00475 
00476                                 // Skip sensors.
00477                                 bool sensorA = contact->m_fixtureA->m_isSensor;
00478                                 bool sensorB = contact->m_fixtureB->m_isSensor;
00479                                 if (sensorA || sensorB)
00480                                 {
00481                                         continue;
00482                                 }
00483 
00484                                 island.Add(contact);
00485                                 contact->m_flags |= b2Contact::e_islandFlag;
00486 
00487                                 b2Body* other = ce->other;
00488 
00489                                 // Was the other body already added to this island?
00490                                 if (other->m_flags & b2Body::e_islandFlag)
00491                                 {
00492                                         continue;
00493                                 }
00494 
00495                                 b2Assert(stackCount < stackSize);
00496                                 stack[stackCount++] = other;
00497                                 other->m_flags |= b2Body::e_islandFlag;
00498                         }
00499 
00500                         // Search all joints connect to this body.
00501                         for (b2JointEdge* je = b->m_jointList; je; je = je->next)
00502                         {
00503                                 if (je->joint->m_islandFlag == true)
00504                                 {
00505                                         continue;
00506                                 }
00507 
00508                                 b2Body* other = je->other;
00509 
00510                                 // Don't simulate joints connected to inactive bodies.
00511                                 if (other->IsActive() == false)
00512                                 {
00513                                         continue;
00514                                 }
00515 
00516                                 island.Add(je->joint);
00517                                 je->joint->m_islandFlag = true;
00518 
00519                                 if (other->m_flags & b2Body::e_islandFlag)
00520                                 {
00521                                         continue;
00522                                 }
00523 
00524                                 b2Assert(stackCount < stackSize);
00525                                 stack[stackCount++] = other;
00526                                 other->m_flags |= b2Body::e_islandFlag;
00527                         }
00528                 }
00529 
00530                 b2Profile profile;
00531                 island.Solve(&profile, step, m_gravity, m_allowSleep);
00532                 m_profile.solveInit += profile.solveInit;
00533                 m_profile.solveVelocity += profile.solveVelocity;
00534                 m_profile.solvePosition += profile.solvePosition;
00535 
00536                 // Post solve cleanup.
00537                 for (int32 i = 0; i < island.m_bodyCount; ++i)
00538                 {
00539                         // Allow static bodies to participate in other islands.
00540                         b2Body* b = island.m_bodies[i];
00541                         if (b->GetType() == b2_staticBody)
00542                         {
00543                                 b->m_flags &= ~b2Body::e_islandFlag;
00544                         }
00545                 }
00546         }
00547 
00548         m_stackAllocator.Free(stack);
00549 
00550         {
00551                 b2Timer timer;
00552                 // Synchronize fixtures, check for out of range bodies.
00553                 for (b2Body* b = m_bodyList; b; b = b->GetNext())
00554                 {
00555                         // If a body was not in an island then it did not move.
00556                         if ((b->m_flags & b2Body::e_islandFlag) == 0)
00557                         {
00558                                 continue;
00559                         }
00560 
00561                         if (b->GetType() == b2_staticBody)
00562                         {
00563                                 continue;
00564                         }
00565 
00566                         // Update fixtures (for broad-phase).
00567                         b->SynchronizeFixtures();
00568                 }
00569 
00570                 // Look for new contacts.
00571                 m_contactManager.FindNewContacts();
00572                 m_profile.broadphase = timer.GetMilliseconds();
00573         }
00574 }
00575 
00576 // Find TOI contacts and solve them.
00577 void b2World::SolveTOI(const b2TimeStep& step)
00578 {
00579         b2Island island(2 * b2_maxTOIContacts, b2_maxTOIContacts, 0, &m_stackAllocator, m_contactManager.m_contactListener);
00580 
00581         if (m_stepComplete)
00582         {
00583                 for (b2Body* b = m_bodyList; b; b = b->m_next)
00584                 {
00585                         b->m_flags &= ~b2Body::e_islandFlag;
00586                         b->m_sweep.alpha0 = 0.0f;
00587                 }
00588 
00589                 for (b2Contact* c = m_contactManager.m_contactList; c; c = c->m_next)
00590                 {
00591                         // Invalidate TOI
00592                         c->m_flags &= ~(b2Contact::e_toiFlag | b2Contact::e_islandFlag);
00593                         c->m_toiCount = 0;
00594                         c->m_toi = 1.0f;
00595                 }
00596         }
00597 
00598         // Find TOI events and solve them.
00599         for (;;)
00600         {
00601                 // Find the first TOI.
00602                 b2Contact* minContact = NULL;
00603                 float32 minAlpha = 1.0f;
00604 
00605                 for (b2Contact* c = m_contactManager.m_contactList; c; c = c->m_next)
00606                 {
00607                         // Is this contact disabled?
00608                         if (c->IsEnabled() == false)
00609                         {
00610                                 continue;
00611                         }
00612 
00613                         // Prevent excessive sub-stepping.
00614                         if (c->m_toiCount > b2_maxSubSteps)
00615                         {
00616                                 continue;
00617                         }
00618 
00619                         float32 alpha = 1.0f;
00620                         if (c->m_flags & b2Contact::e_toiFlag)
00621                         {
00622                                 // This contact has a valid cached TOI.
00623                                 alpha = c->m_toi;
00624                         }
00625                         else
00626                         {
00627                                 b2Fixture* fA = c->GetFixtureA();
00628                                 b2Fixture* fB = c->GetFixtureB();
00629 
00630                                 // Is there a sensor?
00631                                 if (fA->IsSensor() || fB->IsSensor())
00632                                 {
00633                                         continue;
00634                                 }
00635 
00636                                 b2Body* bA = fA->GetBody();
00637                                 b2Body* bB = fB->GetBody();
00638 
00639                                 b2BodyType typeA = bA->m_type;
00640                                 b2BodyType typeB = bB->m_type;
00641                                 b2Assert(typeA == b2_dynamicBody || typeB == b2_dynamicBody);
00642 
00643                                 bool activeA = bA->IsAwake() && typeA != b2_staticBody;
00644                                 bool activeB = bB->IsAwake() && typeB != b2_staticBody;
00645 
00646                                 // Is at least one body active (awake and dynamic or kinematic)?
00647                                 if (activeA == false && activeB == false)
00648                                 {
00649                                         continue;
00650                                 }
00651 
00652                                 bool collideA = bA->IsBullet() || typeA != b2_dynamicBody;
00653                                 bool collideB = bB->IsBullet() || typeB != b2_dynamicBody;
00654 
00655                                 // Are these two non-bullet dynamic bodies?
00656                                 if (collideA == false && collideB == false)
00657                                 {
00658                                         continue;
00659                                 }
00660 
00661                                 // Compute the TOI for this contact.
00662                                 // Put the sweeps onto the same time interval.
00663                                 float32 alpha0 = bA->m_sweep.alpha0;
00664 
00665                                 if (bA->m_sweep.alpha0 < bB->m_sweep.alpha0)
00666                                 {
00667                                         alpha0 = bB->m_sweep.alpha0;
00668                                         bA->m_sweep.Advance(alpha0);
00669                                 }
00670                                 else if (bB->m_sweep.alpha0 < bA->m_sweep.alpha0)
00671                                 {
00672                                         alpha0 = bA->m_sweep.alpha0;
00673                                         bB->m_sweep.Advance(alpha0);
00674                                 }
00675 
00676                                 b2Assert(alpha0 < 1.0f);
00677 
00678                                 int32 indexA = c->GetChildIndexA();
00679                                 int32 indexB = c->GetChildIndexB();
00680 
00681                                 // Compute the time of impact in interval [0, minTOI]
00682                                 b2TOIInput input;
00683                                 input.proxyA.Set(fA->GetShape(), indexA);
00684                                 input.proxyB.Set(fB->GetShape(), indexB);
00685                                 input.sweepA = bA->m_sweep;
00686                                 input.sweepB = bB->m_sweep;
00687                                 input.tMax = 1.0f;
00688 
00689                                 b2TOIOutput output;
00690                                 b2TimeOfImpact(&output, &input);
00691 
00692                                 // Beta is the fraction of the remaining portion of the .
00693                                 float32 beta = output.t;
00694                                 if (output.state == b2TOIOutput::e_touching)
00695                                 {
00696                                         alpha = b2Min(alpha0 + (1.0f - alpha0) * beta, 1.0f);
00697                                 }
00698                                 else
00699                                 {
00700                                         alpha = 1.0f;
00701                                 }
00702 
00703                                 c->m_toi = alpha;
00704                                 c->m_flags |= b2Contact::e_toiFlag;
00705                         }
00706 
00707                         if (alpha < minAlpha)
00708                         {
00709                                 // This is the minimum TOI found so far.
00710                                 minContact = c;
00711                                 minAlpha = alpha;
00712                         }
00713                 }
00714 
00715                 if (minContact == NULL || 1.0f - 10.0f * b2_epsilon < minAlpha)
00716                 {
00717                         // No more TOI events. Done!
00718                         m_stepComplete = true;
00719                         break;
00720                 }
00721 
00722                 // Advance the bodies to the TOI.
00723                 b2Fixture* fA = minContact->GetFixtureA();
00724                 b2Fixture* fB = minContact->GetFixtureB();
00725                 b2Body* bA = fA->GetBody();
00726                 b2Body* bB = fB->GetBody();
00727 
00728                 b2Sweep backup1 = bA->m_sweep;
00729                 b2Sweep backup2 = bB->m_sweep;
00730 
00731                 bA->Advance(minAlpha);
00732                 bB->Advance(minAlpha);
00733 
00734                 // The TOI contact likely has some new contact points.
00735                 minContact->Update(m_contactManager.m_contactListener);
00736                 minContact->m_flags &= ~b2Contact::e_toiFlag;
00737                 ++minContact->m_toiCount;
00738 
00739                 // Is the contact solid?
00740                 if (minContact->IsEnabled() == false || minContact->IsTouching() == false)
00741                 {
00742                         // Restore the sweeps.
00743                         minContact->SetEnabled(false);
00744                         bA->m_sweep = backup1;
00745                         bB->m_sweep = backup2;
00746                         bA->SynchronizeTransform();
00747                         bB->SynchronizeTransform();
00748                         continue;
00749                 }
00750 
00751                 bA->SetAwake(true);
00752                 bB->SetAwake(true);
00753 
00754                 // Build the island
00755                 island.Clear();
00756                 island.Add(bA);
00757                 island.Add(bB);
00758                 island.Add(minContact);
00759 
00760                 bA->m_flags |= b2Body::e_islandFlag;
00761                 bB->m_flags |= b2Body::e_islandFlag;
00762                 minContact->m_flags |= b2Contact::e_islandFlag;
00763 
00764                 // Get contacts on bodyA and bodyB.
00765                 b2Body* bodies[2] = {bA, bB};
00766                 for (int32 i = 0; i < 2; ++i)
00767                 {
00768                         b2Body* body = bodies[i];
00769                         if (body->m_type == b2_dynamicBody)
00770                         {
00771                                 for (b2ContactEdge* ce = body->m_contactList; ce; ce = ce->next)
00772                                 {
00773                                         if (island.m_bodyCount == island.m_bodyCapacity)
00774                                         {
00775                                                 break;
00776                                         }
00777 
00778                                         if (island.m_contactCount == island.m_contactCapacity)
00779                                         {
00780                                                 break;
00781                                         }
00782 
00783                                         b2Contact* contact = ce->contact;
00784 
00785                                         // Has this contact already been added to the island?
00786                                         if (contact->m_flags & b2Contact::e_islandFlag)
00787                                         {
00788                                                 continue;
00789                                         }
00790 
00791                                         // Only add static, kinematic, or bullet bodies.
00792                                         b2Body* other = ce->other;
00793                                         if (other->m_type == b2_dynamicBody &&
00794                                                 body->IsBullet() == false && other->IsBullet() == false)
00795                                         {
00796                                                 continue;
00797                                         }
00798 
00799                                         // Skip sensors.
00800                                         bool sensorA = contact->m_fixtureA->m_isSensor;
00801                                         bool sensorB = contact->m_fixtureB->m_isSensor;
00802                                         if (sensorA || sensorB)
00803                                         {
00804                                                 continue;
00805                                         }
00806 
00807                                         // Tentatively advance the body to the TOI.
00808                                         b2Sweep backup = other->m_sweep;
00809                                         if ((other->m_flags & b2Body::e_islandFlag) == 0)
00810                                         {
00811                                                 other->Advance(minAlpha);
00812                                         }
00813 
00814                                         // Update the contact points
00815                                         contact->Update(m_contactManager.m_contactListener);
00816 
00817                                         // Was the contact disabled by the user?
00818                                         if (contact->IsEnabled() == false)
00819                                         {
00820                                                 other->m_sweep = backup;
00821                                                 other->SynchronizeTransform();
00822                                                 continue;
00823                                         }
00824 
00825                                         // Are there contact points?
00826                                         if (contact->IsTouching() == false)
00827                                         {
00828                                                 other->m_sweep = backup;
00829                                                 other->SynchronizeTransform();
00830                                                 continue;
00831                                         }
00832 
00833                                         // Add the contact to the island
00834                                         contact->m_flags |= b2Contact::e_islandFlag;
00835                                         island.Add(contact);
00836 
00837                                         // Has the other body already been added to the island?
00838                                         if (other->m_flags & b2Body::e_islandFlag)
00839                                         {
00840                                                 continue;
00841                                         }
00842                                         
00843                                         // Add the other body to the island.
00844                                         other->m_flags |= b2Body::e_islandFlag;
00845 
00846                                         if (other->m_type != b2_staticBody)
00847                                         {
00848                                                 other->SetAwake(true);
00849                                         }
00850 
00851                                         island.Add(other);
00852                                 }
00853                         }
00854                 }
00855 
00856                 b2TimeStep subStep;
00857                 subStep.dt = (1.0f - minAlpha) * step.dt;
00858                 subStep.inv_dt = 1.0f / subStep.dt;
00859                 subStep.dtRatio = 1.0f;
00860                 subStep.positionIterations = 20;
00861                 subStep.velocityIterations = step.velocityIterations;
00862                 subStep.warmStarting = false;
00863                 island.SolveTOI(subStep, bA->m_islandIndex, bB->m_islandIndex);
00864 
00865                 // Reset island flags and synchronize broad-phase proxies.
00866                 for (int32 i = 0; i < island.m_bodyCount; ++i)
00867                 {
00868                         b2Body* body = island.m_bodies[i];
00869                         body->m_flags &= ~b2Body::e_islandFlag;
00870 
00871                         if (body->m_type != b2_dynamicBody)
00872                         {
00873                                 continue;
00874                         }
00875 
00876                         body->SynchronizeFixtures();
00877 
00878                         // Invalidate all contact TOIs on this displaced body.
00879                         for (b2ContactEdge* ce = body->m_contactList; ce; ce = ce->next)
00880                         {
00881                                 ce->contact->m_flags &= ~(b2Contact::e_toiFlag | b2Contact::e_islandFlag);
00882                         }
00883                 }
00884 
00885                 // Commit fixture proxy movements to the broad-phase so that new contacts are created.
00886                 // Also, some contacts can be destroyed.
00887                 m_contactManager.FindNewContacts();
00888 
00889                 if (m_subStepping)
00890                 {
00891                         m_stepComplete = false;
00892                         break;
00893                 }
00894         }
00895 }
00896 
00897 void b2World::Step(float32 dt, int32 velocityIterations, int32 positionIterations)
00898 {
00899         b2Timer stepTimer;
00900 
00901         // If new fixtures were added, we need to find the new contacts.
00902         if (m_flags & e_newFixture)
00903         {
00904                 m_contactManager.FindNewContacts();
00905                 m_flags &= ~e_newFixture;
00906         }
00907 
00908         m_flags |= e_locked;
00909 
00910         b2TimeStep step;
00911         step.dt = dt;
00912         step.velocityIterations = velocityIterations;
00913         step.positionIterations = positionIterations;
00914         if (dt > 0.0f)
00915         {
00916                 step.inv_dt = 1.0f / dt;
00917         }
00918         else
00919         {
00920                 step.inv_dt = 0.0f;
00921         }
00922 
00923         step.dtRatio = m_inv_dt0 * dt;
00924 
00925         step.warmStarting = m_warmStarting;
00926         
00927         // Update contacts. This is where some contacts are destroyed.
00928         {
00929                 b2Timer timer;
00930                 m_contactManager.Collide();
00931                 m_profile.collide = timer.GetMilliseconds();
00932         }
00933 
00934         // Integrate velocities, solve velocity constraints, and integrate positions.
00935         if (m_stepComplete && step.dt > 0.0f)
00936         {
00937                 b2Timer timer;
00938                 Solve(step);
00939                 m_profile.solve = timer.GetMilliseconds();
00940         }
00941 
00942         // Handle TOI events.
00943         if (m_continuousPhysics && step.dt > 0.0f)
00944         {
00945                 b2Timer timer;
00946                 SolveTOI(step);
00947                 m_profile.solveTOI = timer.GetMilliseconds();
00948         }
00949 
00950         if (step.dt > 0.0f)
00951         {
00952                 m_inv_dt0 = step.inv_dt;
00953         }
00954 
00955         if (m_flags & e_clearForces)
00956         {
00957                 ClearForces();
00958         }
00959 
00960         m_flags &= ~e_locked;
00961 
00962         m_profile.step = stepTimer.GetMilliseconds();
00963 }
00964 
00965 void b2World::ClearForces()
00966 {
00967         for (b2Body* body = m_bodyList; body; body = body->GetNext())
00968         {
00969                 body->m_force.SetZero();
00970                 body->m_torque = 0.0f;
00971         }
00972 }
00973 
00974 struct b2WorldQueryWrapper
00975 {
00976         bool QueryCallback(int32 proxyId)
00977         {
00978                 b2FixtureProxy* proxy = (b2FixtureProxy*)broadPhase->GetUserData(proxyId);
00979                 return callback->ReportFixture(proxy->fixture);
00980         }
00981 
00982         const b2BroadPhase* broadPhase;
00983         b2QueryCallback* callback;
00984 };
00985 
00986 void b2World::QueryAABB(b2QueryCallback* callback, const b2AABB& aabb) const
00987 {
00988         b2WorldQueryWrapper wrapper;
00989         wrapper.broadPhase = &m_contactManager.m_broadPhase;
00990         wrapper.callback = callback;
00991         m_contactManager.m_broadPhase.Query(&wrapper, aabb);
00992 }
00993 
00994 struct b2WorldRayCastWrapper
00995 {
00996         float32 RayCastCallback(const b2RayCastInput& input, int32 proxyId)
00997         {
00998                 void* userData = broadPhase->GetUserData(proxyId);
00999                 b2FixtureProxy* proxy = (b2FixtureProxy*)userData;
01000                 b2Fixture* fixture = proxy->fixture;
01001                 int32 index = proxy->childIndex;
01002                 b2RayCastOutput output;
01003                 bool hit = fixture->RayCast(&output, input, index);
01004 
01005                 if (hit)
01006                 {
01007                         float32 fraction = output.fraction;
01008                         b2Vec2 point = (1.0f - fraction) * input.p1 + fraction * input.p2;
01009                         return callback->ReportFixture(fixture, point, output.normal, fraction);
01010                 }
01011 
01012                 return input.maxFraction;
01013         }
01014 
01015         const b2BroadPhase* broadPhase;
01016         b2RayCastCallback* callback;
01017 };
01018 
01019 void b2World::RayCast(b2RayCastCallback* callback, const b2Vec2& point1, const b2Vec2& point2) const
01020 {
01021         b2WorldRayCastWrapper wrapper;
01022         wrapper.broadPhase = &m_contactManager.m_broadPhase;
01023         wrapper.callback = callback;
01024         b2RayCastInput input;
01025         input.maxFraction = 1.0f;
01026         input.p1 = point1;
01027         input.p2 = point2;
01028         m_contactManager.m_broadPhase.RayCast(&wrapper, input);
01029 }
01030 
01031 void b2World::DrawShape(b2Fixture* fixture, const b2Transform& xf, const b2Color& color)
01032 {
01033         switch (fixture->GetType())
01034         {
01035         case b2Shape::e_circle:
01036                 {
01037                         b2CircleShape* circle = (b2CircleShape*)fixture->GetShape();
01038 
01039                         b2Vec2 center = b2Mul(xf, circle->m_p);
01040                         float32 radius = circle->m_radius;
01041                         b2Vec2 axis = b2Mul(xf.q, b2Vec2(1.0f, 0.0f));
01042 
01043                         g_debugDraw->DrawSolidCircle(center, radius, axis, color);
01044                 }
01045                 break;
01046 
01047         case b2Shape::e_edge:
01048                 {
01049                         b2EdgeShape* edge = (b2EdgeShape*)fixture->GetShape();
01050                         b2Vec2 v1 = b2Mul(xf, edge->m_vertex1);
01051                         b2Vec2 v2 = b2Mul(xf, edge->m_vertex2);
01052                         g_debugDraw->DrawSegment(v1, v2, color);
01053                 }
01054                 break;
01055 
01056         case b2Shape::e_chain:
01057                 {
01058                         b2ChainShape* chain = (b2ChainShape*)fixture->GetShape();
01059                         int32 count = chain->m_count;
01060                         const b2Vec2* vertices = chain->m_vertices;
01061 
01062                         b2Vec2 v1 = b2Mul(xf, vertices[0]);
01063                         for (int32 i = 1; i < count; ++i)
01064                         {
01065                                 b2Vec2 v2 = b2Mul(xf, vertices[i]);
01066                                 g_debugDraw->DrawSegment(v1, v2, color);
01067                                 g_debugDraw->DrawCircle(v1, 0.05f, color);
01068                                 v1 = v2;
01069                         }
01070                 }
01071                 break;
01072 
01073         case b2Shape::e_polygon:
01074                 {
01075                         b2PolygonShape* poly = (b2PolygonShape*)fixture->GetShape();
01076                         int32 vertexCount = poly->m_count;
01077                         b2Assert(vertexCount <= b2_maxPolygonVertices);
01078                         b2Vec2 vertices[b2_maxPolygonVertices];
01079 
01080                         for (int32 i = 0; i < vertexCount; ++i)
01081                         {
01082                                 vertices[i] = b2Mul(xf, poly->m_vertices[i]);
01083                         }
01084 
01085                         g_debugDraw->DrawSolidPolygon(vertices, vertexCount, color);
01086                 }
01087                 break;
01088             
01089     default:
01090         break;
01091         }
01092 }
01093 
01094 void b2World::DrawJoint(b2Joint* joint)
01095 {
01096         b2Body* bodyA = joint->GetBodyA();
01097         b2Body* bodyB = joint->GetBodyB();
01098         const b2Transform& xf1 = bodyA->GetTransform();
01099         const b2Transform& xf2 = bodyB->GetTransform();
01100         b2Vec2 x1 = xf1.p;
01101         b2Vec2 x2 = xf2.p;
01102         b2Vec2 p1 = joint->GetAnchorA();
01103         b2Vec2 p2 = joint->GetAnchorB();
01104 
01105         b2Color color(0.5f, 0.8f, 0.8f);
01106 
01107         switch (joint->GetType())
01108         {
01109         case e_distanceJoint:
01110                 g_debugDraw->DrawSegment(p1, p2, color);
01111                 break;
01112 
01113         case e_pulleyJoint:
01114                 {
01115                         b2PulleyJoint* pulley = (b2PulleyJoint*)joint;
01116                         b2Vec2 s1 = pulley->GetGroundAnchorA();
01117                         b2Vec2 s2 = pulley->GetGroundAnchorB();
01118                         g_debugDraw->DrawSegment(s1, p1, color);
01119                         g_debugDraw->DrawSegment(s2, p2, color);
01120                         g_debugDraw->DrawSegment(s1, s2, color);
01121                 }
01122                 break;
01123 
01124         case e_mouseJoint:
01125                 // don't draw this
01126                 break;
01127 
01128         default:
01129                 g_debugDraw->DrawSegment(x1, p1, color);
01130                 g_debugDraw->DrawSegment(p1, p2, color);
01131                 g_debugDraw->DrawSegment(x2, p2, color);
01132         }
01133 }
01134 
01135 void b2World::DrawDebugData()
01136 {
01137         if (g_debugDraw == NULL)
01138         {
01139                 return;
01140         }
01141 
01142         uint32 flags = g_debugDraw->GetFlags();
01143 
01144         if (flags & b2Draw::e_shapeBit)
01145         {
01146                 for (b2Body* b = m_bodyList; b; b = b->GetNext())
01147                 {
01148                         const b2Transform& xf = b->GetTransform();
01149                         for (b2Fixture* f = b->GetFixtureList(); f; f = f->GetNext())
01150                         {
01151                                 if (b->IsActive() == false)
01152                                 {
01153                                         DrawShape(f, xf, b2Color(0.5f, 0.5f, 0.3f));
01154                                 }
01155                                 else if (b->GetType() == b2_staticBody)
01156                                 {
01157                                         DrawShape(f, xf, b2Color(0.5f, 0.9f, 0.5f));
01158                                 }
01159                                 else if (b->GetType() == b2_kinematicBody)
01160                                 {
01161                                         DrawShape(f, xf, b2Color(0.5f, 0.5f, 0.9f));
01162                                 }
01163                                 else if (b->IsAwake() == false)
01164                                 {
01165                                         DrawShape(f, xf, b2Color(0.6f, 0.6f, 0.6f));
01166                                 }
01167                                 else
01168                                 {
01169                                         DrawShape(f, xf, b2Color(0.9f, 0.7f, 0.7f));
01170                                 }
01171                         }
01172                 }
01173         }
01174 
01175         if (flags & b2Draw::e_jointBit)
01176         {
01177                 for (b2Joint* j = m_jointList; j; j = j->GetNext())
01178                 {
01179                         DrawJoint(j);
01180                 }
01181         }
01182 
01183         if (flags & b2Draw::e_pairBit)
01184         {
01185                 b2Color color(0.3f, 0.9f, 0.9f);
01186                 for (b2Contact* c = m_contactManager.m_contactList; c; c = c->GetNext())
01187                 {
01188                         //b2Fixture* fixtureA = c->GetFixtureA();
01189                         //b2Fixture* fixtureB = c->GetFixtureB();
01190 
01191                         //b2Vec2 cA = fixtureA->GetAABB().GetCenter();
01192                         //b2Vec2 cB = fixtureB->GetAABB().GetCenter();
01193 
01194                         //g_debugDraw->DrawSegment(cA, cB, color);
01195                 }
01196         }
01197 
01198         if (flags & b2Draw::e_aabbBit)
01199         {
01200                 b2Color color(0.9f, 0.3f, 0.9f);
01201                 b2BroadPhase* bp = &m_contactManager.m_broadPhase;
01202 
01203                 for (b2Body* b = m_bodyList; b; b = b->GetNext())
01204                 {
01205                         if (b->IsActive() == false)
01206                         {
01207                                 continue;
01208                         }
01209 
01210                         for (b2Fixture* f = b->GetFixtureList(); f; f = f->GetNext())
01211                         {
01212                                 for (int32 i = 0; i < f->m_proxyCount; ++i)
01213                                 {
01214                                         b2FixtureProxy* proxy = f->m_proxies + i;
01215                                         b2AABB aabb = bp->GetFatAABB(proxy->proxyId);
01216                                         b2Vec2 vs[4];
01217                                         vs[0].Set(aabb.lowerBound.x, aabb.lowerBound.y);
01218                                         vs[1].Set(aabb.upperBound.x, aabb.lowerBound.y);
01219                                         vs[2].Set(aabb.upperBound.x, aabb.upperBound.y);
01220                                         vs[3].Set(aabb.lowerBound.x, aabb.upperBound.y);
01221 
01222                                         g_debugDraw->DrawPolygon(vs, 4, color);
01223                                 }
01224                         }
01225                 }
01226         }
01227 
01228         if (flags & b2Draw::e_centerOfMassBit)
01229         {
01230                 for (b2Body* b = m_bodyList; b; b = b->GetNext())
01231                 {
01232                         b2Transform xf = b->GetTransform();
01233                         xf.p = b->GetWorldCenter();
01234                         g_debugDraw->DrawTransform(xf);
01235                 }
01236         }
01237 }
01238 
01239 int32 b2World::GetProxyCount() const
01240 {
01241         return m_contactManager.m_broadPhase.GetProxyCount();
01242 }
01243 
01244 int32 b2World::GetTreeHeight() const
01245 {
01246         return m_contactManager.m_broadPhase.GetTreeHeight();
01247 }
01248 
01249 int32 b2World::GetTreeBalance() const
01250 {
01251         return m_contactManager.m_broadPhase.GetTreeBalance();
01252 }
01253 
01254 float32 b2World::GetTreeQuality() const
01255 {
01256         return m_contactManager.m_broadPhase.GetTreeQuality();
01257 }
01258 
01259 void b2World::ShiftOrigin(const b2Vec2& newOrigin)
01260 {
01261         b2Assert((m_flags & e_locked) == 0);
01262         if ((m_flags & e_locked) == e_locked)
01263         {
01264                 return;
01265         }
01266 
01267         for (b2Body* b = m_bodyList; b; b = b->m_next)
01268         {
01269                 b->m_xf.p -= newOrigin;
01270                 b->m_sweep.c0 -= newOrigin;
01271                 b->m_sweep.c -= newOrigin;
01272         }
01273 
01274         for (b2Joint* j = m_jointList; j; j = j->m_next)
01275         {
01276                 j->ShiftOrigin(newOrigin);
01277         }
01278 
01279         m_contactManager.m_broadPhase.ShiftOrigin(newOrigin);
01280 }
01281 
01282 void b2World::Dump()
01283 {
01284         if ((m_flags & e_locked) == e_locked)
01285         {
01286                 return;
01287         }
01288 
01289         b2Log("b2Vec2 g(%.15lef, %.15lef);\n", m_gravity.x, m_gravity.y);
01290         b2Log("m_world->SetGravity(g);\n");
01291 
01292         b2Log("b2Body** bodies = (b2Body**)b2Alloc(%d * sizeof(b2Body*));\n", m_bodyCount);
01293         b2Log("b2Joint** joints = (b2Joint**)b2Alloc(%d * sizeof(b2Joint*));\n", m_jointCount);
01294         int32 i = 0;
01295         for (b2Body* b = m_bodyList; b; b = b->m_next)
01296         {
01297                 b->m_islandIndex = i;
01298                 b->Dump();
01299                 ++i;
01300         }
01301 
01302         i = 0;
01303         for (b2Joint* j = m_jointList; j; j = j->m_next)
01304         {
01305                 j->m_index = i;
01306                 ++i;
01307         }
01308 
01309         // First pass on joints, skip gear joints.
01310         for (b2Joint* j = m_jointList; j; j = j->m_next)
01311         {
01312                 if (j->m_type == e_gearJoint)
01313                 {
01314                         continue;
01315                 }
01316 
01317                 b2Log("{\n");
01318                 j->Dump();
01319                 b2Log("}\n");
01320         }
01321 
01322         // Second pass on joints, only gear joints.
01323         for (b2Joint* j = m_jointList; j; j = j->m_next)
01324         {
01325                 if (j->m_type != e_gearJoint)
01326                 {
01327                         continue;
01328                 }
01329 
01330                 b2Log("{\n");
01331                 j->Dump();
01332                 b2Log("}\n");
01333         }
01334 
01335         b2Log("b2Free(joints);\n");
01336         b2Log("b2Free(bodies);\n");
01337         b2Log("joints = NULL;\n");
01338         b2Log("bodies = NULL;\n");
01339 }


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