00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
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
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
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
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
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
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
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
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
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
00251 if (def->collideConnected == false)
00252 {
00253 b2ContactEdge* edge = bodyB->GetContactList();
00254 while (edge)
00255 {
00256 if (edge->other == bodyA)
00257 {
00258
00259
00260 edge->contact->FlagForFiltering();
00261 }
00262
00263 edge = edge->next;
00264 }
00265 }
00266
00267
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
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
00299 b2Body* bodyA = j->m_bodyA;
00300 b2Body* bodyB = j->m_bodyB;
00301
00302
00303 bodyA->SetAwake(true);
00304 bodyB->SetAwake(true);
00305
00306
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
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
00350 if (collideConnected == false)
00351 {
00352 b2ContactEdge* edge = bodyB->GetContactList();
00353 while (edge)
00354 {
00355 if (edge->other == bodyA)
00356 {
00357
00358
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
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
00393 b2Island island(m_bodyCount,
00394 m_contactManager.m_contactCount,
00395 m_jointCount,
00396 &m_stackAllocator,
00397 m_contactManager.m_contactListener);
00398
00399
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
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
00429 if (seed->GetType() == b2_staticBody)
00430 {
00431 continue;
00432 }
00433
00434
00435 island.Clear();
00436 int32 stackCount = 0;
00437 stack[stackCount++] = seed;
00438 seed->m_flags |= b2Body::e_islandFlag;
00439
00440
00441 while (stackCount > 0)
00442 {
00443
00444 b2Body* b = stack[--stackCount];
00445 b2Assert(b->IsActive() == true);
00446 island.Add(b);
00447
00448
00449 b->SetAwake(true);
00450
00451
00452
00453 if (b->GetType() == b2_staticBody)
00454 {
00455 continue;
00456 }
00457
00458
00459 for (b2ContactEdge* ce = b->m_contactList; ce; ce = ce->next)
00460 {
00461 b2Contact* contact = ce->contact;
00462
00463
00464 if (contact->m_flags & b2Contact::e_islandFlag)
00465 {
00466 continue;
00467 }
00468
00469
00470 if (contact->IsEnabled() == false ||
00471 contact->IsTouching() == false)
00472 {
00473 continue;
00474 }
00475
00476
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
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
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
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
00537 for (int32 i = 0; i < island.m_bodyCount; ++i)
00538 {
00539
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
00553 for (b2Body* b = m_bodyList; b; b = b->GetNext())
00554 {
00555
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
00567 b->SynchronizeFixtures();
00568 }
00569
00570
00571 m_contactManager.FindNewContacts();
00572 m_profile.broadphase = timer.GetMilliseconds();
00573 }
00574 }
00575
00576
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
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
00599 for (;;)
00600 {
00601
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
00608 if (c->IsEnabled() == false)
00609 {
00610 continue;
00611 }
00612
00613
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
00623 alpha = c->m_toi;
00624 }
00625 else
00626 {
00627 b2Fixture* fA = c->GetFixtureA();
00628 b2Fixture* fB = c->GetFixtureB();
00629
00630
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
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
00656 if (collideA == false && collideB == false)
00657 {
00658 continue;
00659 }
00660
00661
00662
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
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
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
00710 minContact = c;
00711 minAlpha = alpha;
00712 }
00713 }
00714
00715 if (minContact == NULL || 1.0f - 10.0f * b2_epsilon < minAlpha)
00716 {
00717
00718 m_stepComplete = true;
00719 break;
00720 }
00721
00722
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
00735 minContact->Update(m_contactManager.m_contactListener);
00736 minContact->m_flags &= ~b2Contact::e_toiFlag;
00737 ++minContact->m_toiCount;
00738
00739
00740 if (minContact->IsEnabled() == false || minContact->IsTouching() == false)
00741 {
00742
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
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
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
00786 if (contact->m_flags & b2Contact::e_islandFlag)
00787 {
00788 continue;
00789 }
00790
00791
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
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
00808 b2Sweep backup = other->m_sweep;
00809 if ((other->m_flags & b2Body::e_islandFlag) == 0)
00810 {
00811 other->Advance(minAlpha);
00812 }
00813
00814
00815 contact->Update(m_contactManager.m_contactListener);
00816
00817
00818 if (contact->IsEnabled() == false)
00819 {
00820 other->m_sweep = backup;
00821 other->SynchronizeTransform();
00822 continue;
00823 }
00824
00825
00826 if (contact->IsTouching() == false)
00827 {
00828 other->m_sweep = backup;
00829 other->SynchronizeTransform();
00830 continue;
00831 }
00832
00833
00834 contact->m_flags |= b2Contact::e_islandFlag;
00835 island.Add(contact);
00836
00837
00838 if (other->m_flags & b2Body::e_islandFlag)
00839 {
00840 continue;
00841 }
00842
00843
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
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
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
00886
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
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
00928 {
00929 b2Timer timer;
00930 m_contactManager.Collide();
00931 m_profile.collide = timer.GetMilliseconds();
00932 }
00933
00934
00935 if (m_stepComplete && step.dt > 0.0f)
00936 {
00937 b2Timer timer;
00938 Solve(step);
00939 m_profile.solve = timer.GetMilliseconds();
00940 }
00941
00942
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
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
01189
01190
01191
01192
01193
01194
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
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
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 }