File indexing completed on 2024-05-19 14:56:33
0001 /* 0002 * Copyright (c) 2006-2011 Erin Catto http://www.box2d.org 0003 * 0004 * This software is provided 'as-is', without any express or implied 0005 * warranty. In no event will the authors be held liable for any damages 0006 * arising from the use of this software. 0007 * Permission is granted to anyone to use this software for any purpose, 0008 * including commercial applications, and to alter it and redistribute it 0009 * freely, subject to the following restrictions: 0010 * 1. The origin of this software must not be misrepresented; you must not 0011 * claim that you wrote the original software. If you use this software 0012 * in a product, an acknowledgment in the product documentation would be 0013 * appreciated but is not required. 0014 * 2. Altered source versions must be plainly marked as such, and must not be 0015 * misrepresented as being the original software. 0016 * 3. This notice may not be removed or altered from any source distribution. 0017 */ 0018 0019 #include <Box2D/Dynamics/b2World.h> 0020 #include <Box2D/Dynamics/b2Body.h> 0021 #include <Box2D/Dynamics/b2Fixture.h> 0022 #include <Box2D/Dynamics/b2Island.h> 0023 #include <Box2D/Dynamics/Joints/b2PulleyJoint.h> 0024 #include <Box2D/Dynamics/Contacts/b2Contact.h> 0025 #include <Box2D/Dynamics/Contacts/b2ContactSolver.h> 0026 #include <Box2D/Collision/b2Collision.h> 0027 #include <Box2D/Collision/b2BroadPhase.h> 0028 #include <Box2D/Collision/Shapes/b2CircleShape.h> 0029 #include <Box2D/Collision/Shapes/b2EdgeShape.h> 0030 #include <Box2D/Collision/Shapes/b2ChainShape.h> 0031 #include <Box2D/Collision/Shapes/b2PolygonShape.h> 0032 #include <Box2D/Collision/b2TimeOfImpact.h> 0033 #include <Box2D/Common/b2Draw.h> 0034 #include <Box2D/Common/b2Timer.h> 0035 #include <new> 0036 0037 b2World::b2World(const b2Vec2& gravity) 0038 { 0039 m_destructionListener = NULL; 0040 g_debugDraw = NULL; 0041 0042 m_bodyList = NULL; 0043 m_jointList = NULL; 0044 0045 m_bodyCount = 0; 0046 m_jointCount = 0; 0047 0048 m_warmStarting = true; 0049 m_continuousPhysics = true; 0050 m_subStepping = false; 0051 0052 m_stepComplete = true; 0053 0054 m_allowSleep = true; 0055 m_gravity = gravity; 0056 0057 m_flags = e_clearForces; 0058 0059 m_inv_dt0 = 0.0f; 0060 0061 m_contactManager.m_allocator = &m_blockAllocator; 0062 0063 memset(&m_profile, 0, sizeof(b2Profile)); 0064 } 0065 0066 b2World::~b2World() 0067 { 0068 // Some shapes allocate using b2Alloc. 0069 b2Body* b = m_bodyList; 0070 while (b) 0071 { 0072 b2Body* bNext = b->m_next; 0073 0074 b2Fixture* f = b->m_fixtureList; 0075 while (f) 0076 { 0077 b2Fixture* fNext = f->m_next; 0078 f->m_proxyCount = 0; 0079 f->Destroy(&m_blockAllocator); 0080 f = fNext; 0081 } 0082 0083 b = bNext; 0084 } 0085 } 0086 0087 void b2World::SetDestructionListener(b2DestructionListener* listener) 0088 { 0089 m_destructionListener = listener; 0090 } 0091 0092 void b2World::SetContactFilter(b2ContactFilter* filter) 0093 { 0094 m_contactManager.m_contactFilter = filter; 0095 } 0096 0097 void b2World::SetContactListener(b2ContactListener* listener) 0098 { 0099 m_contactManager.m_contactListener = listener; 0100 } 0101 0102 void b2World::SetDebugDraw(b2Draw* debugDraw) 0103 { 0104 g_debugDraw = debugDraw; 0105 } 0106 0107 b2Body* b2World::CreateBody(const b2BodyDef* def) 0108 { 0109 b2Assert(IsLocked() == false); 0110 if (IsLocked()) 0111 { 0112 return NULL; 0113 } 0114 0115 void* mem = m_blockAllocator.Allocate(sizeof(b2Body)); 0116 b2Body* b = new (mem) b2Body(def, this); 0117 0118 // Add to world doubly linked list. 0119 b->m_prev = NULL; 0120 b->m_next = m_bodyList; 0121 if (m_bodyList) 0122 { 0123 m_bodyList->m_prev = b; 0124 } 0125 m_bodyList = b; 0126 ++m_bodyCount; 0127 0128 return b; 0129 } 0130 0131 void b2World::DestroyBody(b2Body* b) 0132 { 0133 b2Assert(m_bodyCount > 0); 0134 b2Assert(IsLocked() == false); 0135 if (IsLocked()) 0136 { 0137 return; 0138 } 0139 0140 // Delete the attached joints. 0141 b2JointEdge* je = b->m_jointList; 0142 while (je) 0143 { 0144 b2JointEdge* je0 = je; 0145 je = je->next; 0146 0147 if (m_destructionListener) 0148 { 0149 m_destructionListener->SayGoodbye(je0->joint); 0150 } 0151 0152 DestroyJoint(je0->joint); 0153 0154 b->m_jointList = je; 0155 } 0156 b->m_jointList = NULL; 0157 0158 // Delete the attached contacts. 0159 b2ContactEdge* ce = b->m_contactList; 0160 while (ce) 0161 { 0162 b2ContactEdge* ce0 = ce; 0163 ce = ce->next; 0164 m_contactManager.Destroy(ce0->contact); 0165 } 0166 b->m_contactList = NULL; 0167 0168 // Delete the attached fixtures. This destroys broad-phase proxies. 0169 b2Fixture* f = b->m_fixtureList; 0170 while (f) 0171 { 0172 b2Fixture* f0 = f; 0173 f = f->m_next; 0174 0175 if (m_destructionListener) 0176 { 0177 m_destructionListener->SayGoodbye(f0); 0178 } 0179 0180 f0->DestroyProxies(&m_contactManager.m_broadPhase); 0181 f0->Destroy(&m_blockAllocator); 0182 f0->~b2Fixture(); 0183 m_blockAllocator.Free(f0, sizeof(b2Fixture)); 0184 0185 b->m_fixtureList = f; 0186 b->m_fixtureCount -= 1; 0187 } 0188 b->m_fixtureList = NULL; 0189 b->m_fixtureCount = 0; 0190 0191 // Remove world body list. 0192 if (b->m_prev) 0193 { 0194 b->m_prev->m_next = b->m_next; 0195 } 0196 0197 if (b->m_next) 0198 { 0199 b->m_next->m_prev = b->m_prev; 0200 } 0201 0202 if (b == m_bodyList) 0203 { 0204 m_bodyList = b->m_next; 0205 } 0206 0207 --m_bodyCount; 0208 b->~b2Body(); 0209 m_blockAllocator.Free(b, sizeof(b2Body)); 0210 } 0211 0212 b2Joint* b2World::CreateJoint(const b2JointDef* def) 0213 { 0214 b2Assert(IsLocked() == false); 0215 if (IsLocked()) 0216 { 0217 return NULL; 0218 } 0219 0220 b2Joint* j = b2Joint::Create(def, &m_blockAllocator); 0221 0222 // Connect to the world list. 0223 j->m_prev = NULL; 0224 j->m_next = m_jointList; 0225 if (m_jointList) 0226 { 0227 m_jointList->m_prev = j; 0228 } 0229 m_jointList = j; 0230 ++m_jointCount; 0231 0232 // Connect to the bodies' doubly linked lists. 0233 j->m_edgeA.joint = j; 0234 j->m_edgeA.other = j->m_bodyB; 0235 j->m_edgeA.prev = NULL; 0236 j->m_edgeA.next = j->m_bodyA->m_jointList; 0237 if (j->m_bodyA->m_jointList) j->m_bodyA->m_jointList->prev = &j->m_edgeA; 0238 j->m_bodyA->m_jointList = &j->m_edgeA; 0239 0240 j->m_edgeB.joint = j; 0241 j->m_edgeB.other = j->m_bodyA; 0242 j->m_edgeB.prev = NULL; 0243 j->m_edgeB.next = j->m_bodyB->m_jointList; 0244 if (j->m_bodyB->m_jointList) j->m_bodyB->m_jointList->prev = &j->m_edgeB; 0245 j->m_bodyB->m_jointList = &j->m_edgeB; 0246 0247 b2Body* bodyA = def->bodyA; 0248 b2Body* bodyB = def->bodyB; 0249 0250 // If the joint prevents collisions, then flag any contacts for filtering. 0251 if (def->collideConnected == false) 0252 { 0253 b2ContactEdge* edge = bodyB->GetContactList(); 0254 while (edge) 0255 { 0256 if (edge->other == bodyA) 0257 { 0258 // Flag the contact for filtering at the next time step (where either 0259 // body is awake). 0260 edge->contact->FlagForFiltering(); 0261 } 0262 0263 edge = edge->next; 0264 } 0265 } 0266 0267 // Note: creating a joint doesn't wake the bodies. 0268 0269 return j; 0270 } 0271 0272 void b2World::DestroyJoint(b2Joint* j) 0273 { 0274 b2Assert(IsLocked() == false); 0275 if (IsLocked()) 0276 { 0277 return; 0278 } 0279 0280 bool collideConnected = j->m_collideConnected; 0281 0282 // Remove from the doubly linked list. 0283 if (j->m_prev) 0284 { 0285 j->m_prev->m_next = j->m_next; 0286 } 0287 0288 if (j->m_next) 0289 { 0290 j->m_next->m_prev = j->m_prev; 0291 } 0292 0293 if (j == m_jointList) 0294 { 0295 m_jointList = j->m_next; 0296 } 0297 0298 // Disconnect from island graph. 0299 b2Body* bodyA = j->m_bodyA; 0300 b2Body* bodyB = j->m_bodyB; 0301 0302 // Wake up connected bodies. 0303 bodyA->SetAwake(true); 0304 bodyB->SetAwake(true); 0305 0306 // Remove from body 1. 0307 if (j->m_edgeA.prev) 0308 { 0309 j->m_edgeA.prev->next = j->m_edgeA.next; 0310 } 0311 0312 if (j->m_edgeA.next) 0313 { 0314 j->m_edgeA.next->prev = j->m_edgeA.prev; 0315 } 0316 0317 if (&j->m_edgeA == bodyA->m_jointList) 0318 { 0319 bodyA->m_jointList = j->m_edgeA.next; 0320 } 0321 0322 j->m_edgeA.prev = NULL; 0323 j->m_edgeA.next = NULL; 0324 0325 // Remove from body 2 0326 if (j->m_edgeB.prev) 0327 { 0328 j->m_edgeB.prev->next = j->m_edgeB.next; 0329 } 0330 0331 if (j->m_edgeB.next) 0332 { 0333 j->m_edgeB.next->prev = j->m_edgeB.prev; 0334 } 0335 0336 if (&j->m_edgeB == bodyB->m_jointList) 0337 { 0338 bodyB->m_jointList = j->m_edgeB.next; 0339 } 0340 0341 j->m_edgeB.prev = NULL; 0342 j->m_edgeB.next = NULL; 0343 0344 b2Joint::Destroy(j, &m_blockAllocator); 0345 0346 b2Assert(m_jointCount > 0); 0347 --m_jointCount; 0348 0349 // If the joint prevents collisions, then flag any contacts for filtering. 0350 if (collideConnected == false) 0351 { 0352 b2ContactEdge* edge = bodyB->GetContactList(); 0353 while (edge) 0354 { 0355 if (edge->other == bodyA) 0356 { 0357 // Flag the contact for filtering at the next time step (where either 0358 // body is awake). 0359 edge->contact->FlagForFiltering(); 0360 } 0361 0362 edge = edge->next; 0363 } 0364 } 0365 } 0366 0367 // 0368 void b2World::SetAllowSleeping(bool flag) 0369 { 0370 if (flag == m_allowSleep) 0371 { 0372 return; 0373 } 0374 0375 m_allowSleep = flag; 0376 if (m_allowSleep == false) 0377 { 0378 for (b2Body* b = m_bodyList; b; b = b->m_next) 0379 { 0380 b->SetAwake(true); 0381 } 0382 } 0383 } 0384 0385 // Find islands, integrate and solve constraints, solve position constraints 0386 void b2World::Solve(const b2TimeStep& step) 0387 { 0388 m_profile.solveInit = 0.0f; 0389 m_profile.solveVelocity = 0.0f; 0390 m_profile.solvePosition = 0.0f; 0391 0392 // Size the island for the worst case. 0393 b2Island island(m_bodyCount, 0394 m_contactManager.m_contactCount, 0395 m_jointCount, 0396 &m_stackAllocator, 0397 m_contactManager.m_contactListener); 0398 0399 // Clear all the island flags. 0400 for (b2Body* b = m_bodyList; b; b = b->m_next) 0401 { 0402 b->m_flags &= ~b2Body::e_islandFlag; 0403 } 0404 for (b2Contact* c = m_contactManager.m_contactList; c; c = c->m_next) 0405 { 0406 c->m_flags &= ~b2Contact::e_islandFlag; 0407 } 0408 for (b2Joint* j = m_jointList; j; j = j->m_next) 0409 { 0410 j->m_islandFlag = false; 0411 } 0412 0413 // Build and simulate all awake islands. 0414 int32 stackSize = m_bodyCount; 0415 b2Body** stack = (b2Body**)m_stackAllocator.Allocate(stackSize * sizeof(b2Body*)); 0416 for (b2Body* seed = m_bodyList; seed; seed = seed->m_next) 0417 { 0418 if (seed->m_flags & b2Body::e_islandFlag) 0419 { 0420 continue; 0421 } 0422 0423 if (seed->IsAwake() == false || seed->IsActive() == false) 0424 { 0425 continue; 0426 } 0427 0428 // The seed can be dynamic or kinematic. 0429 if (seed->GetType() == b2_staticBody) 0430 { 0431 continue; 0432 } 0433 0434 // Reset island and stack. 0435 island.Clear(); 0436 int32 stackCount = 0; 0437 stack[stackCount++] = seed; 0438 seed->m_flags |= b2Body::e_islandFlag; 0439 0440 // Perform a depth first search (DFS) on the constraint graph. 0441 while (stackCount > 0) 0442 { 0443 // Grab the next body off the stack and add it to the island. 0444 b2Body* b = stack[--stackCount]; 0445 b2Assert(b->IsActive() == true); 0446 island.Add(b); 0447 0448 // Make sure the body is awake. 0449 b->SetAwake(true); 0450 0451 // To keep islands as small as possible, we don't 0452 // propagate islands across static bodies. 0453 if (b->GetType() == b2_staticBody) 0454 { 0455 continue; 0456 } 0457 0458 // Search all contacts connected to this body. 0459 for (b2ContactEdge* ce = b->m_contactList; ce; ce = ce->next) 0460 { 0461 b2Contact* contact = ce->contact; 0462 0463 // Has this contact already been added to an island? 0464 if (contact->m_flags & b2Contact::e_islandFlag) 0465 { 0466 continue; 0467 } 0468 0469 // Is this contact solid and touching? 0470 if (contact->IsEnabled() == false || 0471 contact->IsTouching() == false) 0472 { 0473 continue; 0474 } 0475 0476 // Skip sensors. 0477 bool sensorA = contact->m_fixtureA->m_isSensor; 0478 bool sensorB = contact->m_fixtureB->m_isSensor; 0479 if (sensorA || sensorB) 0480 { 0481 continue; 0482 } 0483 0484 island.Add(contact); 0485 contact->m_flags |= b2Contact::e_islandFlag; 0486 0487 b2Body* other = ce->other; 0488 0489 // Was the other body already added to this island? 0490 if (other->m_flags & b2Body::e_islandFlag) 0491 { 0492 continue; 0493 } 0494 0495 b2Assert(stackCount < stackSize); 0496 stack[stackCount++] = other; 0497 other->m_flags |= b2Body::e_islandFlag; 0498 } 0499 0500 // Search all joints connect to this body. 0501 for (b2JointEdge* je = b->m_jointList; je; je = je->next) 0502 { 0503 if (je->joint->m_islandFlag == true) 0504 { 0505 continue; 0506 } 0507 0508 b2Body* other = je->other; 0509 0510 // Don't simulate joints connected to inactive bodies. 0511 if (other->IsActive() == false) 0512 { 0513 continue; 0514 } 0515 0516 island.Add(je->joint); 0517 je->joint->m_islandFlag = true; 0518 0519 if (other->m_flags & b2Body::e_islandFlag) 0520 { 0521 continue; 0522 } 0523 0524 b2Assert(stackCount < stackSize); 0525 stack[stackCount++] = other; 0526 other->m_flags |= b2Body::e_islandFlag; 0527 } 0528 } 0529 0530 b2Profile profile; 0531 island.Solve(&profile, step, m_gravity, m_allowSleep); 0532 m_profile.solveInit += profile.solveInit; 0533 m_profile.solveVelocity += profile.solveVelocity; 0534 m_profile.solvePosition += profile.solvePosition; 0535 0536 // Post solve cleanup. 0537 for (int32 i = 0; i < island.m_bodyCount; ++i) 0538 { 0539 // Allow static bodies to participate in other islands. 0540 b2Body* b = island.m_bodies[i]; 0541 if (b->GetType() == b2_staticBody) 0542 { 0543 b->m_flags &= ~b2Body::e_islandFlag; 0544 } 0545 } 0546 } 0547 0548 m_stackAllocator.Free(stack); 0549 0550 { 0551 b2Timer timer; 0552 // Synchronize fixtures, check for out of range bodies. 0553 for (b2Body* b = m_bodyList; b; b = b->GetNext()) 0554 { 0555 // If a body was not in an island then it did not move. 0556 if ((b->m_flags & b2Body::e_islandFlag) == 0) 0557 { 0558 continue; 0559 } 0560 0561 if (b->GetType() == b2_staticBody) 0562 { 0563 continue; 0564 } 0565 0566 // Update fixtures (for broad-phase). 0567 b->SynchronizeFixtures(); 0568 } 0569 0570 // Look for new contacts. 0571 m_contactManager.FindNewContacts(); 0572 m_profile.broadphase = timer.GetMilliseconds(); 0573 } 0574 } 0575 0576 // Find TOI contacts and solve them. 0577 void b2World::SolveTOI(const b2TimeStep& step) 0578 { 0579 b2Island island(2 * b2_maxTOIContacts, b2_maxTOIContacts, 0, &m_stackAllocator, m_contactManager.m_contactListener); 0580 0581 if (m_stepComplete) 0582 { 0583 for (b2Body* b = m_bodyList; b; b = b->m_next) 0584 { 0585 b->m_flags &= ~b2Body::e_islandFlag; 0586 b->m_sweep.alpha0 = 0.0f; 0587 } 0588 0589 for (b2Contact* c = m_contactManager.m_contactList; c; c = c->m_next) 0590 { 0591 // Invalidate TOI 0592 c->m_flags &= ~(b2Contact::e_toiFlag | b2Contact::e_islandFlag); 0593 c->m_toiCount = 0; 0594 c->m_toi = 1.0f; 0595 } 0596 } 0597 0598 // Find TOI events and solve them. 0599 for (;;) 0600 { 0601 // Find the first TOI. 0602 b2Contact* minContact = NULL; 0603 float32 minAlpha = 1.0f; 0604 0605 for (b2Contact* c = m_contactManager.m_contactList; c; c = c->m_next) 0606 { 0607 // Is this contact disabled? 0608 if (c->IsEnabled() == false) 0609 { 0610 continue; 0611 } 0612 0613 // Prevent excessive sub-stepping. 0614 if (c->m_toiCount > b2_maxSubSteps) 0615 { 0616 continue; 0617 } 0618 0619 float32 alpha = 1.0f; 0620 if (c->m_flags & b2Contact::e_toiFlag) 0621 { 0622 // This contact has a valid cached TOI. 0623 alpha = c->m_toi; 0624 } 0625 else 0626 { 0627 b2Fixture* fA = c->GetFixtureA(); 0628 b2Fixture* fB = c->GetFixtureB(); 0629 0630 // Is there a sensor? 0631 if (fA->IsSensor() || fB->IsSensor()) 0632 { 0633 continue; 0634 } 0635 0636 b2Body* bA = fA->GetBody(); 0637 b2Body* bB = fB->GetBody(); 0638 0639 b2BodyType typeA = bA->m_type; 0640 b2BodyType typeB = bB->m_type; 0641 b2Assert(typeA == b2_dynamicBody || typeB == b2_dynamicBody); 0642 0643 bool activeA = bA->IsAwake() && typeA != b2_staticBody; 0644 bool activeB = bB->IsAwake() && typeB != b2_staticBody; 0645 0646 // Is at least one body active (awake and dynamic or kinematic)? 0647 if (activeA == false && activeB == false) 0648 { 0649 continue; 0650 } 0651 0652 bool collideA = bA->IsBullet() || typeA != b2_dynamicBody; 0653 bool collideB = bB->IsBullet() || typeB != b2_dynamicBody; 0654 0655 // Are these two non-bullet dynamic bodies? 0656 if (collideA == false && collideB == false) 0657 { 0658 continue; 0659 } 0660 0661 // Compute the TOI for this contact. 0662 // Put the sweeps onto the same time interval. 0663 float32 alpha0 = bA->m_sweep.alpha0; 0664 0665 if (bA->m_sweep.alpha0 < bB->m_sweep.alpha0) 0666 { 0667 alpha0 = bB->m_sweep.alpha0; 0668 bA->m_sweep.Advance(alpha0); 0669 } 0670 else if (bB->m_sweep.alpha0 < bA->m_sweep.alpha0) 0671 { 0672 alpha0 = bA->m_sweep.alpha0; 0673 bB->m_sweep.Advance(alpha0); 0674 } 0675 0676 b2Assert(alpha0 < 1.0f); 0677 0678 int32 indexA = c->GetChildIndexA(); 0679 int32 indexB = c->GetChildIndexB(); 0680 0681 // Compute the time of impact in interval [0, minTOI] 0682 b2TOIInput input; 0683 input.proxyA.Set(fA->GetShape(), indexA); 0684 input.proxyB.Set(fB->GetShape(), indexB); 0685 input.sweepA = bA->m_sweep; 0686 input.sweepB = bB->m_sweep; 0687 input.tMax = 1.0f; 0688 0689 b2TOIOutput output; 0690 b2TimeOfImpact(&output, &input); 0691 0692 // Beta is the fraction of the remaining portion of the . 0693 float32 beta = output.t; 0694 if (output.state == b2TOIOutput::e_touching) 0695 { 0696 alpha = b2Min(alpha0 + (1.0f - alpha0) * beta, 1.0f); 0697 } 0698 else 0699 { 0700 alpha = 1.0f; 0701 } 0702 0703 c->m_toi = alpha; 0704 c->m_flags |= b2Contact::e_toiFlag; 0705 } 0706 0707 if (alpha < minAlpha) 0708 { 0709 // This is the minimum TOI found so far. 0710 minContact = c; 0711 minAlpha = alpha; 0712 } 0713 } 0714 0715 if (minContact == NULL || 1.0f - 10.0f * b2_epsilon < minAlpha) 0716 { 0717 // No more TOI events. Done! 0718 m_stepComplete = true; 0719 break; 0720 } 0721 0722 // Advance the bodies to the TOI. 0723 b2Fixture* fA = minContact->GetFixtureA(); 0724 b2Fixture* fB = minContact->GetFixtureB(); 0725 b2Body* bA = fA->GetBody(); 0726 b2Body* bB = fB->GetBody(); 0727 0728 b2Sweep backup1 = bA->m_sweep; 0729 b2Sweep backup2 = bB->m_sweep; 0730 0731 bA->Advance(minAlpha); 0732 bB->Advance(minAlpha); 0733 0734 // The TOI contact likely has some new contact points. 0735 minContact->Update(m_contactManager.m_contactListener); 0736 minContact->m_flags &= ~b2Contact::e_toiFlag; 0737 ++minContact->m_toiCount; 0738 0739 // Is the contact solid? 0740 if (minContact->IsEnabled() == false || minContact->IsTouching() == false) 0741 { 0742 // Restore the sweeps. 0743 minContact->SetEnabled(false); 0744 bA->m_sweep = backup1; 0745 bB->m_sweep = backup2; 0746 bA->SynchronizeTransform(); 0747 bB->SynchronizeTransform(); 0748 continue; 0749 } 0750 0751 bA->SetAwake(true); 0752 bB->SetAwake(true); 0753 0754 // Build the island 0755 island.Clear(); 0756 island.Add(bA); 0757 island.Add(bB); 0758 island.Add(minContact); 0759 0760 bA->m_flags |= b2Body::e_islandFlag; 0761 bB->m_flags |= b2Body::e_islandFlag; 0762 minContact->m_flags |= b2Contact::e_islandFlag; 0763 0764 // Get contacts on bodyA and bodyB. 0765 b2Body* bodies[2] = {bA, bB}; 0766 for (int32 i = 0; i < 2; ++i) 0767 { 0768 b2Body* body = bodies[i]; 0769 if (body->m_type == b2_dynamicBody) 0770 { 0771 for (b2ContactEdge* ce = body->m_contactList; ce; ce = ce->next) 0772 { 0773 if (island.m_bodyCount == island.m_bodyCapacity) 0774 { 0775 break; 0776 } 0777 0778 if (island.m_contactCount == island.m_contactCapacity) 0779 { 0780 break; 0781 } 0782 0783 b2Contact* contact = ce->contact; 0784 0785 // Has this contact already been added to the island? 0786 if (contact->m_flags & b2Contact::e_islandFlag) 0787 { 0788 continue; 0789 } 0790 0791 // Only add static, kinematic, or bullet bodies. 0792 b2Body* other = ce->other; 0793 if (other->m_type == b2_dynamicBody && 0794 body->IsBullet() == false && other->IsBullet() == false) 0795 { 0796 continue; 0797 } 0798 0799 // Skip sensors. 0800 bool sensorA = contact->m_fixtureA->m_isSensor; 0801 bool sensorB = contact->m_fixtureB->m_isSensor; 0802 if (sensorA || sensorB) 0803 { 0804 continue; 0805 } 0806 0807 // Tentatively advance the body to the TOI. 0808 b2Sweep backup = other->m_sweep; 0809 if ((other->m_flags & b2Body::e_islandFlag) == 0) 0810 { 0811 other->Advance(minAlpha); 0812 } 0813 0814 // Update the contact points 0815 contact->Update(m_contactManager.m_contactListener); 0816 0817 // Was the contact disabled by the user? 0818 if (contact->IsEnabled() == false) 0819 { 0820 other->m_sweep = backup; 0821 other->SynchronizeTransform(); 0822 continue; 0823 } 0824 0825 // Are there contact points? 0826 if (contact->IsTouching() == false) 0827 { 0828 other->m_sweep = backup; 0829 other->SynchronizeTransform(); 0830 continue; 0831 } 0832 0833 // Add the contact to the island 0834 contact->m_flags |= b2Contact::e_islandFlag; 0835 island.Add(contact); 0836 0837 // Has the other body already been added to the island? 0838 if (other->m_flags & b2Body::e_islandFlag) 0839 { 0840 continue; 0841 } 0842 0843 // Add the other body to the island. 0844 other->m_flags |= b2Body::e_islandFlag; 0845 0846 if (other->m_type != b2_staticBody) 0847 { 0848 other->SetAwake(true); 0849 } 0850 0851 island.Add(other); 0852 } 0853 } 0854 } 0855 0856 b2TimeStep subStep; 0857 subStep.dt = (1.0f - minAlpha) * step.dt; 0858 subStep.inv_dt = 1.0f / subStep.dt; 0859 subStep.dtRatio = 1.0f; 0860 subStep.positionIterations = 20; 0861 subStep.velocityIterations = step.velocityIterations; 0862 subStep.warmStarting = false; 0863 island.SolveTOI(subStep, bA->m_islandIndex, bB->m_islandIndex); 0864 0865 // Reset island flags and synchronize broad-phase proxies. 0866 for (int32 i = 0; i < island.m_bodyCount; ++i) 0867 { 0868 b2Body* body = island.m_bodies[i]; 0869 body->m_flags &= ~b2Body::e_islandFlag; 0870 0871 if (body->m_type != b2_dynamicBody) 0872 { 0873 continue; 0874 } 0875 0876 body->SynchronizeFixtures(); 0877 0878 // Invalidate all contact TOIs on this displaced body. 0879 for (b2ContactEdge* ce = body->m_contactList; ce; ce = ce->next) 0880 { 0881 ce->contact->m_flags &= ~(b2Contact::e_toiFlag | b2Contact::e_islandFlag); 0882 } 0883 } 0884 0885 // Commit fixture proxy movements to the broad-phase so that new contacts are created. 0886 // Also, some contacts can be destroyed. 0887 m_contactManager.FindNewContacts(); 0888 0889 if (m_subStepping) 0890 { 0891 m_stepComplete = false; 0892 break; 0893 } 0894 } 0895 } 0896 0897 void b2World::Step(float32 dt, int32 velocityIterations, int32 positionIterations) 0898 { 0899 b2Timer stepTimer; 0900 0901 // If new fixtures were added, we need to find the new contacts. 0902 if (m_flags & e_newFixture) 0903 { 0904 m_contactManager.FindNewContacts(); 0905 m_flags &= ~e_newFixture; 0906 } 0907 0908 m_flags |= e_locked; 0909 0910 b2TimeStep step; 0911 step.dt = dt; 0912 step.velocityIterations = velocityIterations; 0913 step.positionIterations = positionIterations; 0914 if (dt > 0.0f) 0915 { 0916 step.inv_dt = 1.0f / dt; 0917 } 0918 else 0919 { 0920 step.inv_dt = 0.0f; 0921 } 0922 0923 step.dtRatio = m_inv_dt0 * dt; 0924 0925 step.warmStarting = m_warmStarting; 0926 0927 // Update contacts. This is where some contacts are destroyed. 0928 { 0929 b2Timer timer; 0930 m_contactManager.Collide(); 0931 m_profile.collide = timer.GetMilliseconds(); 0932 } 0933 0934 // Integrate velocities, solve velocity constraints, and integrate positions. 0935 if (m_stepComplete && step.dt > 0.0f) 0936 { 0937 b2Timer timer; 0938 Solve(step); 0939 m_profile.solve = timer.GetMilliseconds(); 0940 } 0941 0942 // Handle TOI events. 0943 if (m_continuousPhysics && step.dt > 0.0f) 0944 { 0945 b2Timer timer; 0946 SolveTOI(step); 0947 m_profile.solveTOI = timer.GetMilliseconds(); 0948 } 0949 0950 if (step.dt > 0.0f) 0951 { 0952 m_inv_dt0 = step.inv_dt; 0953 } 0954 0955 if (m_flags & e_clearForces) 0956 { 0957 ClearForces(); 0958 } 0959 0960 m_flags &= ~e_locked; 0961 0962 m_profile.step = stepTimer.GetMilliseconds(); 0963 } 0964 0965 void b2World::ClearForces() 0966 { 0967 for (b2Body* body = m_bodyList; body; body = body->GetNext()) 0968 { 0969 body->m_force.SetZero(); 0970 body->m_torque = 0.0f; 0971 } 0972 } 0973 0974 struct b2WorldQueryWrapper 0975 { 0976 bool QueryCallback(int32 proxyId) 0977 { 0978 b2FixtureProxy* proxy = (b2FixtureProxy*)broadPhase->GetUserData(proxyId); 0979 return callback->ReportFixture(proxy->fixture); 0980 } 0981 0982 const b2BroadPhase* broadPhase; 0983 b2QueryCallback* callback; 0984 }; 0985 0986 void b2World::QueryAABB(b2QueryCallback* callback, const b2AABB& aabb) const 0987 { 0988 b2WorldQueryWrapper wrapper; 0989 wrapper.broadPhase = &m_contactManager.m_broadPhase; 0990 wrapper.callback = callback; 0991 m_contactManager.m_broadPhase.Query(&wrapper, aabb); 0992 } 0993 0994 struct b2WorldRayCastWrapper 0995 { 0996 float32 RayCastCallback(const b2RayCastInput& input, int32 proxyId) 0997 { 0998 void* userData = broadPhase->GetUserData(proxyId); 0999 b2FixtureProxy* proxy = (b2FixtureProxy*)userData; 1000 b2Fixture* fixture = proxy->fixture; 1001 int32 index = proxy->childIndex; 1002 b2RayCastOutput output; 1003 bool hit = fixture->RayCast(&output, input, index); 1004 1005 if (hit) 1006 { 1007 float32 fraction = output.fraction; 1008 b2Vec2 point = (1.0f - fraction) * input.p1 + fraction * input.p2; 1009 return callback->ReportFixture(fixture, point, output.normal, fraction); 1010 } 1011 1012 return input.maxFraction; 1013 } 1014 1015 const b2BroadPhase* broadPhase; 1016 b2RayCastCallback* callback; 1017 }; 1018 1019 void b2World::RayCast(b2RayCastCallback* callback, const b2Vec2& point1, const b2Vec2& point2) const 1020 { 1021 b2WorldRayCastWrapper wrapper; 1022 wrapper.broadPhase = &m_contactManager.m_broadPhase; 1023 wrapper.callback = callback; 1024 b2RayCastInput input; 1025 input.maxFraction = 1.0f; 1026 input.p1 = point1; 1027 input.p2 = point2; 1028 m_contactManager.m_broadPhase.RayCast(&wrapper, input); 1029 } 1030 1031 void b2World::DrawShape(b2Fixture* fixture, const b2Transform& xf, const b2Color& color) 1032 { 1033 switch (fixture->GetType()) 1034 { 1035 case b2Shape::e_circle: 1036 { 1037 b2CircleShape* circle = (b2CircleShape*)fixture->GetShape(); 1038 1039 b2Vec2 center = b2Mul(xf, circle->m_p); 1040 float32 radius = circle->m_radius; 1041 b2Vec2 axis = b2Mul(xf.q, b2Vec2(1.0f, 0.0f)); 1042 1043 g_debugDraw->DrawSolidCircle(center, radius, axis, color); 1044 } 1045 break; 1046 1047 case b2Shape::e_edge: 1048 { 1049 b2EdgeShape* edge = (b2EdgeShape*)fixture->GetShape(); 1050 b2Vec2 v1 = b2Mul(xf, edge->m_vertex1); 1051 b2Vec2 v2 = b2Mul(xf, edge->m_vertex2); 1052 g_debugDraw->DrawSegment(v1, v2, color); 1053 } 1054 break; 1055 1056 case b2Shape::e_chain: 1057 { 1058 b2ChainShape* chain = (b2ChainShape*)fixture->GetShape(); 1059 int32 count = chain->m_count; 1060 const b2Vec2* vertices = chain->m_vertices; 1061 1062 b2Vec2 v1 = b2Mul(xf, vertices[0]); 1063 for (int32 i = 1; i < count; ++i) 1064 { 1065 b2Vec2 v2 = b2Mul(xf, vertices[i]); 1066 g_debugDraw->DrawSegment(v1, v2, color); 1067 g_debugDraw->DrawCircle(v1, 0.05f, color); 1068 v1 = v2; 1069 } 1070 } 1071 break; 1072 1073 case b2Shape::e_polygon: 1074 { 1075 b2PolygonShape* poly = (b2PolygonShape*)fixture->GetShape(); 1076 int32 vertexCount = poly->m_count; 1077 b2Assert(vertexCount <= b2_maxPolygonVertices); 1078 b2Vec2 vertices[b2_maxPolygonVertices]; 1079 1080 for (int32 i = 0; i < vertexCount; ++i) 1081 { 1082 vertices[i] = b2Mul(xf, poly->m_vertices[i]); 1083 } 1084 1085 g_debugDraw->DrawSolidPolygon(vertices, vertexCount, color); 1086 } 1087 break; 1088 1089 default: 1090 break; 1091 } 1092 } 1093 1094 void b2World::DrawJoint(b2Joint* joint) 1095 { 1096 b2Body* bodyA = joint->GetBodyA(); 1097 b2Body* bodyB = joint->GetBodyB(); 1098 const b2Transform& xf1 = bodyA->GetTransform(); 1099 const b2Transform& xf2 = bodyB->GetTransform(); 1100 b2Vec2 x1 = xf1.p; 1101 b2Vec2 x2 = xf2.p; 1102 b2Vec2 p1 = joint->GetAnchorA(); 1103 b2Vec2 p2 = joint->GetAnchorB(); 1104 1105 b2Color color(0.5f, 0.8f, 0.8f); 1106 1107 switch (joint->GetType()) 1108 { 1109 case e_distanceJoint: 1110 g_debugDraw->DrawSegment(p1, p2, color); 1111 break; 1112 1113 case e_pulleyJoint: 1114 { 1115 b2PulleyJoint* pulley = (b2PulleyJoint*)joint; 1116 b2Vec2 s1 = pulley->GetGroundAnchorA(); 1117 b2Vec2 s2 = pulley->GetGroundAnchorB(); 1118 g_debugDraw->DrawSegment(s1, p1, color); 1119 g_debugDraw->DrawSegment(s2, p2, color); 1120 g_debugDraw->DrawSegment(s1, s2, color); 1121 } 1122 break; 1123 1124 case e_mouseJoint: 1125 // don't draw this 1126 break; 1127 1128 default: 1129 g_debugDraw->DrawSegment(x1, p1, color); 1130 g_debugDraw->DrawSegment(p1, p2, color); 1131 g_debugDraw->DrawSegment(x2, p2, color); 1132 } 1133 } 1134 1135 void b2World::DrawDebugData() 1136 { 1137 if (g_debugDraw == NULL) 1138 { 1139 return; 1140 } 1141 1142 uint32 flags = g_debugDraw->GetFlags(); 1143 1144 if (flags & b2Draw::e_shapeBit) 1145 { 1146 for (b2Body* b = m_bodyList; b; b = b->GetNext()) 1147 { 1148 const b2Transform& xf = b->GetTransform(); 1149 for (b2Fixture* f = b->GetFixtureList(); f; f = f->GetNext()) 1150 { 1151 if (b->IsActive() == false) 1152 { 1153 DrawShape(f, xf, b2Color(0.5f, 0.5f, 0.3f)); 1154 } 1155 else if (b->GetType() == b2_staticBody) 1156 { 1157 DrawShape(f, xf, b2Color(0.5f, 0.9f, 0.5f)); 1158 } 1159 else if (b->GetType() == b2_kinematicBody) 1160 { 1161 DrawShape(f, xf, b2Color(0.5f, 0.5f, 0.9f)); 1162 } 1163 else if (b->IsAwake() == false) 1164 { 1165 DrawShape(f, xf, b2Color(0.6f, 0.6f, 0.6f)); 1166 } 1167 else 1168 { 1169 DrawShape(f, xf, b2Color(0.9f, 0.7f, 0.7f)); 1170 } 1171 } 1172 } 1173 } 1174 1175 if (flags & b2Draw::e_jointBit) 1176 { 1177 for (b2Joint* j = m_jointList; j; j = j->GetNext()) 1178 { 1179 DrawJoint(j); 1180 } 1181 } 1182 1183 if (flags & b2Draw::e_pairBit) 1184 { 1185 b2Color color(0.3f, 0.9f, 0.9f); 1186 for (b2Contact* c = m_contactManager.m_contactList; c; c = c->GetNext()) 1187 { 1188 //b2Fixture* fixtureA = c->GetFixtureA(); 1189 //b2Fixture* fixtureB = c->GetFixtureB(); 1190 1191 //b2Vec2 cA = fixtureA->GetAABB().GetCenter(); 1192 //b2Vec2 cB = fixtureB->GetAABB().GetCenter(); 1193 1194 //g_debugDraw->DrawSegment(cA, cB, color); 1195 } 1196 } 1197 1198 if (flags & b2Draw::e_aabbBit) 1199 { 1200 b2Color color(0.9f, 0.3f, 0.9f); 1201 b2BroadPhase* bp = &m_contactManager.m_broadPhase; 1202 1203 for (b2Body* b = m_bodyList; b; b = b->GetNext()) 1204 { 1205 if (b->IsActive() == false) 1206 { 1207 continue; 1208 } 1209 1210 for (b2Fixture* f = b->GetFixtureList(); f; f = f->GetNext()) 1211 { 1212 for (int32 i = 0; i < f->m_proxyCount; ++i) 1213 { 1214 b2FixtureProxy* proxy = f->m_proxies + i; 1215 b2AABB aabb = bp->GetFatAABB(proxy->proxyId); 1216 b2Vec2 vs[4]; 1217 vs[0].Set(aabb.lowerBound.x, aabb.lowerBound.y); 1218 vs[1].Set(aabb.upperBound.x, aabb.lowerBound.y); 1219 vs[2].Set(aabb.upperBound.x, aabb.upperBound.y); 1220 vs[3].Set(aabb.lowerBound.x, aabb.upperBound.y); 1221 1222 g_debugDraw->DrawPolygon(vs, 4, color); 1223 } 1224 } 1225 } 1226 } 1227 1228 if (flags & b2Draw::e_centerOfMassBit) 1229 { 1230 for (b2Body* b = m_bodyList; b; b = b->GetNext()) 1231 { 1232 b2Transform xf = b->GetTransform(); 1233 xf.p = b->GetWorldCenter(); 1234 g_debugDraw->DrawTransform(xf); 1235 } 1236 } 1237 } 1238 1239 int32 b2World::GetProxyCount() const 1240 { 1241 return m_contactManager.m_broadPhase.GetProxyCount(); 1242 } 1243 1244 int32 b2World::GetTreeHeight() const 1245 { 1246 return m_contactManager.m_broadPhase.GetTreeHeight(); 1247 } 1248 1249 int32 b2World::GetTreeBalance() const 1250 { 1251 return m_contactManager.m_broadPhase.GetTreeBalance(); 1252 } 1253 1254 float32 b2World::GetTreeQuality() const 1255 { 1256 return m_contactManager.m_broadPhase.GetTreeQuality(); 1257 } 1258 1259 void b2World::ShiftOrigin(const b2Vec2& newOrigin) 1260 { 1261 b2Assert((m_flags & e_locked) == 0); 1262 if ((m_flags & e_locked) == e_locked) 1263 { 1264 return; 1265 } 1266 1267 for (b2Body* b = m_bodyList; b; b = b->m_next) 1268 { 1269 b->m_xf.p -= newOrigin; 1270 b->m_sweep.c0 -= newOrigin; 1271 b->m_sweep.c -= newOrigin; 1272 } 1273 1274 for (b2Joint* j = m_jointList; j; j = j->m_next) 1275 { 1276 j->ShiftOrigin(newOrigin); 1277 } 1278 1279 m_contactManager.m_broadPhase.ShiftOrigin(newOrigin); 1280 } 1281 1282 void b2World::Dump() 1283 { 1284 if ((m_flags & e_locked) == e_locked) 1285 { 1286 return; 1287 } 1288 1289 b2Log("b2Vec2 g(%.15lef, %.15lef);\n", m_gravity.x, m_gravity.y); 1290 b2Log("m_world->SetGravity(g);\n"); 1291 1292 b2Log("b2Body** bodies = (b2Body**)b2Alloc(%d * sizeof(b2Body*));\n", m_bodyCount); 1293 b2Log("b2Joint** joints = (b2Joint**)b2Alloc(%d * sizeof(b2Joint*));\n", m_jointCount); 1294 int32 i = 0; 1295 for (b2Body* b = m_bodyList; b; b = b->m_next) 1296 { 1297 b->m_islandIndex = i; 1298 b->Dump(); 1299 ++i; 1300 } 1301 1302 i = 0; 1303 for (b2Joint* j = m_jointList; j; j = j->m_next) 1304 { 1305 j->m_index = i; 1306 ++i; 1307 } 1308 1309 // First pass on joints, skip gear joints. 1310 for (b2Joint* j = m_jointList; j; j = j->m_next) 1311 { 1312 if (j->m_type == e_gearJoint) 1313 { 1314 continue; 1315 } 1316 1317 b2Log("{\n"); 1318 j->Dump(); 1319 b2Log("}\n"); 1320 } 1321 1322 // Second pass on joints, only gear joints. 1323 for (b2Joint* j = m_jointList; j; j = j->m_next) 1324 { 1325 if (j->m_type != e_gearJoint) 1326 { 1327 continue; 1328 } 1329 1330 b2Log("{\n"); 1331 j->Dump(); 1332 b2Log("}\n"); 1333 } 1334 1335 b2Log("b2Free(joints);\n"); 1336 b2Log("b2Free(bodies);\n"); 1337 b2Log("joints = NULL;\n"); 1338 b2Log("bodies = NULL;\n"); 1339 }