1/*
2* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org
3*
4* This software is provided 'as-is', without any express or implied
5* warranty. In no event will the authors be held liable for any damages
6* arising from the use of this software.
7* Permission is granted to anyone to use this software for any purpose,
8* including commercial applications, and to alter it and redistribute it
9* freely, subject to the following restrictions:
10* 1. The origin of this software must not be misrepresented; you must not
11* claim that you wrote the original software. If you use this software
12* in a product, an acknowledgment in the product documentation would be
13* appreciated but is not required.
14* 2. Altered source versions must be plainly marked as such, and must not be
15* misrepresented as being the original software.
16* 3. This notice may not be removed or altered from any source distribution.
17*/
18
19#include <Box2D/Dynamics/b2World.h>
20#include <Box2D/Dynamics/b2Body.h>
21#include <Box2D/Dynamics/b2Fixture.h>
22#include <Box2D/Dynamics/b2Island.h>
23#include <Box2D/Dynamics/Joints/b2PulleyJoint.h>
24#include <Box2D/Dynamics/Contacts/b2Contact.h>
25#include <Box2D/Dynamics/Contacts/b2ContactSolver.h>
26#include <Box2D/Collision/b2Collision.h>
27#include <Box2D/Collision/b2BroadPhase.h>
28#include <Box2D/Collision/Shapes/b2CircleShape.h>
29#include <Box2D/Collision/Shapes/b2EdgeShape.h>
30#include <Box2D/Collision/Shapes/b2ChainShape.h>
31#include <Box2D/Collision/Shapes/b2PolygonShape.h>
32#include <Box2D/Collision/b2TimeOfImpact.h>
33#include <Box2D/Common/b2Draw.h>
34#include <Box2D/Common/b2Timer.h>
35#include <new>
36
37b2World::b2World(const b2Vec2& gravity)
38{
39 m_destructionListener = NULL;
40 g_debugDraw = NULL;
41
42 m_bodyList = NULL;
43 m_jointList = NULL;
44
45 m_bodyCount = 0;
46 m_jointCount = 0;
47
48 m_warmStarting = true;
49 m_continuousPhysics = true;
50 m_subStepping = false;
51
52 m_stepComplete = true;
53
54 m_allowSleep = true;
55 m_gravity = gravity;
56
57 m_flags = e_clearForces;
58
59 m_inv_dt0 = 0.0f;
60
61 m_contactManager.m_allocator = &m_blockAllocator;
62
63 memset(&m_profile, 0, sizeof(b2Profile));
64}
65
66b2World::~b2World()
67{
68 // Some shapes allocate using b2Alloc.
69 b2Body* b = m_bodyList;
70 while (b)
71 {
72 b2Body* bNext = b->m_next;
73
74 b2Fixture* f = b->m_fixtureList;
75 while (f)
76 {
77 b2Fixture* fNext = f->m_next;
78 f->m_proxyCount = 0;
79 f->Destroy(&m_blockAllocator);
80 f = fNext;
81 }
82
83 b = bNext;
84 }
85}
86
87void b2World::SetDestructionListener(b2DestructionListener* listener)
88{
89 m_destructionListener = listener;
90}
91
92void b2World::SetContactFilter(b2ContactFilter* filter)
93{
94 m_contactManager.m_contactFilter = filter;
95}
96
97void b2World::SetContactListener(b2ContactListener* listener)
98{
99 m_contactManager.m_contactListener = listener;
100}
101
102void b2World::SetDebugDraw(b2Draw* debugDraw)
103{
104 g_debugDraw = debugDraw;
105}
106
107b2Body* b2World::CreateBody(const b2BodyDef* def)
108{
109 b2Assert(IsLocked() == false);
110 if (IsLocked())
111 {
112 return NULL;
113 }
114
115 void* mem = m_blockAllocator.Allocate(sizeof(b2Body));
116 b2Body* b = new (mem) b2Body(def, this);
117
118 // Add to world doubly linked list.
119 b->m_prev = NULL;
120 b->m_next = m_bodyList;
121 if (m_bodyList)
122 {
123 m_bodyList->m_prev = b;
124 }
125 m_bodyList = b;
126 ++m_bodyCount;
127
128 return b;
129}
130
131void b2World::DestroyBody(b2Body* b)
132{
133 b2Assert(m_bodyCount > 0);
134 b2Assert(IsLocked() == false);
135 if (IsLocked())
136 {
137 return;
138 }
139
140 // Delete the attached joints.
141 b2JointEdge* je = b->m_jointList;
142 while (je)
143 {
144 b2JointEdge* je0 = je;
145 je = je->next;
146
147 if (m_destructionListener)
148 {
149 m_destructionListener->SayGoodbye(je0->joint);
150 }
151
152 DestroyJoint(je0->joint);
153
154 b->m_jointList = je;
155 }
156 b->m_jointList = NULL;
157
158 // Delete the attached contacts.
159 b2ContactEdge* ce = b->m_contactList;
160 while (ce)
161 {
162 b2ContactEdge* ce0 = ce;
163 ce = ce->next;
164 m_contactManager.Destroy(ce0->contact);
165 }
166 b->m_contactList = NULL;
167
168 // Delete the attached fixtures. This destroys broad-phase proxies.
169 b2Fixture* f = b->m_fixtureList;
170 while (f)
171 {
172 b2Fixture* f0 = f;
173 f = f->m_next;
174
175 if (m_destructionListener)
176 {
177 m_destructionListener->SayGoodbye(f0);
178 }
179
180 f0->DestroyProxies(&m_contactManager.m_broadPhase);
181 f0->Destroy(&m_blockAllocator);
182 f0->~b2Fixture();
183 m_blockAllocator.Free(f0, sizeof(b2Fixture));
184
185 b->m_fixtureList = f;
186 b->m_fixtureCount -= 1;
187 }
188 b->m_fixtureList = NULL;
189 b->m_fixtureCount = 0;
190
191 // Remove world body list.
192 if (b->m_prev)
193 {
194 b->m_prev->m_next = b->m_next;
195 }
196
197 if (b->m_next)
198 {
199 b->m_next->m_prev = b->m_prev;
200 }
201
202 if (b == m_bodyList)
203 {
204 m_bodyList = b->m_next;
205 }
206
207 --m_bodyCount;
208 b->~b2Body();
209 m_blockAllocator.Free(b, sizeof(b2Body));
210}
211
212b2Joint* b2World::CreateJoint(const b2JointDef* def)
213{
214 b2Assert(IsLocked() == false);
215 if (IsLocked())
216 {
217 return NULL;
218 }
219
220 b2Joint* j = b2Joint::Create(def, &m_blockAllocator);
221
222 // Connect to the world list.
223 j->m_prev = NULL;
224 j->m_next = m_jointList;
225 if (m_jointList)
226 {
227 m_jointList->m_prev = j;
228 }
229 m_jointList = j;
230 ++m_jointCount;
231
232 // Connect to the bodies' doubly linked lists.
233 j->m_edgeA.joint = j;
234 j->m_edgeA.other = j->m_bodyB;
235 j->m_edgeA.prev = NULL;
236 j->m_edgeA.next = j->m_bodyA->m_jointList;
237 if (j->m_bodyA->m_jointList) j->m_bodyA->m_jointList->prev = &j->m_edgeA;
238 j->m_bodyA->m_jointList = &j->m_edgeA;
239
240 j->m_edgeB.joint = j;
241 j->m_edgeB.other = j->m_bodyA;
242 j->m_edgeB.prev = NULL;
243 j->m_edgeB.next = j->m_bodyB->m_jointList;
244 if (j->m_bodyB->m_jointList) j->m_bodyB->m_jointList->prev = &j->m_edgeB;
245 j->m_bodyB->m_jointList = &j->m_edgeB;
246
247 b2Body* bodyA = def->bodyA;
248 b2Body* bodyB = def->bodyB;
249
250 // If the joint prevents collisions, then flag any contacts for filtering.
251 if (def->collideConnected == false)
252 {
253 b2ContactEdge* edge = bodyB->GetContactList();
254 while (edge)
255 {
256 if (edge->other == bodyA)
257 {
258 // Flag the contact for filtering at the next time step (where either
259 // body is awake).
260 edge->contact->FlagForFiltering();
261 }
262
263 edge = edge->next;
264 }
265 }
266
267 // Note: creating a joint doesn't wake the bodies.
268
269 return j;
270}
271
272void b2World::DestroyJoint(b2Joint* j)
273{
274 b2Assert(IsLocked() == false);
275 if (IsLocked())
276 {
277 return;
278 }
279
280 bool collideConnected = j->m_collideConnected;
281
282 // Remove from the doubly linked list.
283 if (j->m_prev)
284 {
285 j->m_prev->m_next = j->m_next;
286 }
287
288 if (j->m_next)
289 {
290 j->m_next->m_prev = j->m_prev;
291 }
292
293 if (j == m_jointList)
294 {
295 m_jointList = j->m_next;
296 }
297
298 // Disconnect from island graph.
299 b2Body* bodyA = j->m_bodyA;
300 b2Body* bodyB = j->m_bodyB;
301
302 // Wake up connected bodies.
303 bodyA->SetAwake(true);
304 bodyB->SetAwake(true);
305
306 // Remove from body 1.
307 if (j->m_edgeA.prev)
308 {
309 j->m_edgeA.prev->next = j->m_edgeA.next;
310 }
311
312 if (j->m_edgeA.next)
313 {
314 j->m_edgeA.next->prev = j->m_edgeA.prev;
315 }
316
317 if (&j->m_edgeA == bodyA->m_jointList)
318 {
319 bodyA->m_jointList = j->m_edgeA.next;
320 }
321
322 j->m_edgeA.prev = NULL;
323 j->m_edgeA.next = NULL;
324
325 // Remove from body 2
326 if (j->m_edgeB.prev)
327 {
328 j->m_edgeB.prev->next = j->m_edgeB.next;
329 }
330
331 if (j->m_edgeB.next)
332 {
333 j->m_edgeB.next->prev = j->m_edgeB.prev;
334 }
335
336 if (&j->m_edgeB == bodyB->m_jointList)
337 {
338 bodyB->m_jointList = j->m_edgeB.next;
339 }
340
341 j->m_edgeB.prev = NULL;
342 j->m_edgeB.next = NULL;
343
344 b2Joint::Destroy(j, &m_blockAllocator);
345
346 b2Assert(m_jointCount > 0);
347 --m_jointCount;
348
349 // If the joint prevents collisions, then flag any contacts for filtering.
350 if (collideConnected == false)
351 {
352 b2ContactEdge* edge = bodyB->GetContactList();
353 while (edge)
354 {
355 if (edge->other == bodyA)
356 {
357 // Flag the contact for filtering at the next time step (where either
358 // body is awake).
359 edge->contact->FlagForFiltering();
360 }
361
362 edge = edge->next;
363 }
364 }
365}
366
367//
368void b2World::SetAllowSleeping(bool flag)
369{
370 if (flag == m_allowSleep)
371 {
372 return;
373 }
374
375 m_allowSleep = flag;
376 if (m_allowSleep == false)
377 {
378 for (b2Body* b = m_bodyList; b; b = b->m_next)
379 {
380 b->SetAwake(true);
381 }
382 }
383}
384
385// Find islands, integrate and solve constraints, solve position constraints
386void b2World::Solve(const b2TimeStep& step)
387{
388 m_profile.solveInit = 0.0f;
389 m_profile.solveVelocity = 0.0f;
390 m_profile.solvePosition = 0.0f;
391
392 // Size the island for the worst case.
393 b2Island island(m_bodyCount,
394 m_contactManager.m_contactCount,
395 m_jointCount,
396 &m_stackAllocator,
397 m_contactManager.m_contactListener);
398
399 // Clear all the island flags.
400 for (b2Body* b = m_bodyList; b; b = b->m_next)
401 {
402 b->m_flags &= ~b2Body::e_islandFlag;
403 }
404 for (b2Contact* c = m_contactManager.m_contactList; c; c = c->m_next)
405 {
406 c->m_flags &= ~b2Contact::e_islandFlag;
407 }
408 for (b2Joint* j = m_jointList; j; j = j->m_next)
409 {
410 j->m_islandFlag = false;
411 }
412
413 // Build and simulate all awake islands.
414 int32 stackSize = m_bodyCount;
415 b2Body** stack = (b2Body**)m_stackAllocator.Allocate(stackSize * sizeof(b2Body*));
416 for (b2Body* seed = m_bodyList; seed; seed = seed->m_next)
417 {
418 if (seed->m_flags & b2Body::e_islandFlag)
419 {
420 continue;
421 }
422
423 if (seed->IsAwake() == false || seed->IsActive() == false)
424 {
425 continue;
426 }
427
428 // The seed can be dynamic or kinematic.
429 if (seed->GetType() == b2_staticBody)
430 {
431 continue;
432 }
433
434 // Reset island and stack.
435 island.Clear();
436 int32 stackCount = 0;
437 stack[stackCount++] = seed;
438 seed->m_flags |= b2Body::e_islandFlag;
439
440 // Perform a depth first search (DFS) on the constraint graph.
441 while (stackCount > 0)
442 {
443 // Grab the next body off the stack and add it to the island.
444 b2Body* b = stack[--stackCount];
445 b2Assert(b->IsActive() == true);
446 island.Add(b);
447
448 // Make sure the body is awake.
449 b->SetAwake(true);
450
451 // To keep islands as small as possible, we don't
452 // propagate islands across static bodies.
453 if (b->GetType() == b2_staticBody)
454 {
455 continue;
456 }
457
458 // Search all contacts connected to this body.
459 for (b2ContactEdge* ce = b->m_contactList; ce; ce = ce->next)
460 {
461 b2Contact* contact = ce->contact;
462
463 // Has this contact already been added to an island?
464 if (contact->m_flags & b2Contact::e_islandFlag)
465 {
466 continue;
467 }
468
469 // Is this contact solid and touching?
470 if (contact->IsEnabled() == false ||
471 contact->IsTouching() == false)
472 {
473 continue;
474 }
475
476 // Skip sensors.
477 bool sensorA = contact->m_fixtureA->m_isSensor;
478 bool sensorB = contact->m_fixtureB->m_isSensor;
479 if (sensorA || sensorB)
480 {
481 continue;
482 }
483
484 island.Add(contact);
485 contact->m_flags |= b2Contact::e_islandFlag;
486
487 b2Body* other = ce->other;
488
489 // Was the other body already added to this island?
490 if (other->m_flags & b2Body::e_islandFlag)
491 {
492 continue;
493 }
494
495 b2Assert(stackCount < stackSize);
496 stack[stackCount++] = other;
497 other->m_flags |= b2Body::e_islandFlag;
498 }
499
500 // Search all joints connect to this body.
501 for (b2JointEdge* je = b->m_jointList; je; je = je->next)
502 {
503 if (je->joint->m_islandFlag == true)
504 {
505 continue;
506 }
507
508 b2Body* other = je->other;
509
510 // Don't simulate joints connected to inactive bodies.
511 if (other->IsActive() == false)
512 {
513 continue;
514 }
515
516 island.Add(je->joint);
517 je->joint->m_islandFlag = true;
518
519 if (other->m_flags & b2Body::e_islandFlag)
520 {
521 continue;
522 }
523
524 b2Assert(stackCount < stackSize);
525 stack[stackCount++] = other;
526 other->m_flags |= b2Body::e_islandFlag;
527 }
528 }
529
530 b2Profile profile;
531 island.Solve(&profile, step, m_gravity, m_allowSleep);
532 m_profile.solveInit += profile.solveInit;
533 m_profile.solveVelocity += profile.solveVelocity;
534 m_profile.solvePosition += profile.solvePosition;
535
536 // Post solve cleanup.
537 for (int32 i = 0; i < island.m_bodyCount; ++i)
538 {
539 // Allow static bodies to participate in other islands.
540 b2Body* b = island.m_bodies[i];
541 if (b->GetType() == b2_staticBody)
542 {
543 b->m_flags &= ~b2Body::e_islandFlag;
544 }
545 }
546 }
547
548 m_stackAllocator.Free(stack);
549
550 {
551 b2Timer timer;
552 // Synchronize fixtures, check for out of range bodies.
553 for (b2Body* b = m_bodyList; b; b = b->GetNext())
554 {
555 // If a body was not in an island then it did not move.
556 if ((b->m_flags & b2Body::e_islandFlag) == 0)
557 {
558 continue;
559 }
560
561 if (b->GetType() == b2_staticBody)
562 {
563 continue;
564 }
565
566 // Update fixtures (for broad-phase).
567 b->SynchronizeFixtures();
568 }
569
570 // Look for new contacts.
571 m_contactManager.FindNewContacts();
572 m_profile.broadphase = timer.GetMilliseconds();
573 }
574}
575
576// Find TOI contacts and solve them.
577void b2World::SolveTOI(const b2TimeStep& step)
578{
579 b2Island island(2 * b2_maxTOIContacts, b2_maxTOIContacts, 0, &m_stackAllocator, m_contactManager.m_contactListener);
580
581 if (m_stepComplete)
582 {
583 for (b2Body* b = m_bodyList; b; b = b->m_next)
584 {
585 b->m_flags &= ~b2Body::e_islandFlag;
586 b->m_sweep.alpha0 = 0.0f;
587 }
588
589 for (b2Contact* c = m_contactManager.m_contactList; c; c = c->m_next)
590 {
591 // Invalidate TOI
592 c->m_flags &= ~(b2Contact::e_toiFlag | b2Contact::e_islandFlag);
593 c->m_toiCount = 0;
594 c->m_toi = 1.0f;
595 }
596 }
597
598 // Find TOI events and solve them.
599 for (;;)
600 {
601 // Find the first TOI.
602 b2Contact* minContact = NULL;
603 float32 minAlpha = 1.0f;
604
605 for (b2Contact* c = m_contactManager.m_contactList; c; c = c->m_next)
606 {
607 // Is this contact disabled?
608 if (c->IsEnabled() == false)
609 {
610 continue;
611 }
612
613 // Prevent excessive sub-stepping.
614 if (c->m_toiCount > b2_maxSubSteps)
615 {
616 continue;
617 }
618
619 float32 alpha = 1.0f;
620 if (c->m_flags & b2Contact::e_toiFlag)
621 {
622 // This contact has a valid cached TOI.
623 alpha = c->m_toi;
624 }
625 else
626 {
627 b2Fixture* fA = c->GetFixtureA();
628 b2Fixture* fB = c->GetFixtureB();
629
630 // Is there a sensor?
631 if (fA->IsSensor() || fB->IsSensor())
632 {
633 continue;
634 }
635
636 b2Body* bA = fA->GetBody();
637 b2Body* bB = fB->GetBody();
638
639 b2BodyType typeA = bA->m_type;
640 b2BodyType typeB = bB->m_type;
641 b2Assert(typeA == b2_dynamicBody || typeB == b2_dynamicBody);
642
643 bool activeA = bA->IsAwake() && typeA != b2_staticBody;
644 bool activeB = bB->IsAwake() && typeB != b2_staticBody;
645
646 // Is at least one body active (awake and dynamic or kinematic)?
647 if (activeA == false && activeB == false)
648 {
649 continue;
650 }
651
652 bool collideA = bA->IsBullet() || typeA != b2_dynamicBody;
653 bool collideB = bB->IsBullet() || typeB != b2_dynamicBody;
654
655 // Are these two non-bullet dynamic bodies?
656 if (collideA == false && collideB == false)
657 {
658 continue;
659 }
660
661 // Compute the TOI for this contact.
662 // Put the sweeps onto the same time interval.
663 float32 alpha0 = bA->m_sweep.alpha0;
664
665 if (bA->m_sweep.alpha0 < bB->m_sweep.alpha0)
666 {
667 alpha0 = bB->m_sweep.alpha0;
668 bA->m_sweep.Advance(alpha0);
669 }
670 else if (bB->m_sweep.alpha0 < bA->m_sweep.alpha0)
671 {
672 alpha0 = bA->m_sweep.alpha0;
673 bB->m_sweep.Advance(alpha0);
674 }
675
676 b2Assert(alpha0 < 1.0f);
677
678 int32 indexA = c->GetChildIndexA();
679 int32 indexB = c->GetChildIndexB();
680
681 // Compute the time of impact in interval [0, minTOI]
682 b2TOIInput input;
683 input.proxyA.Set(fA->GetShape(), indexA);
684 input.proxyB.Set(fB->GetShape(), indexB);
685 input.sweepA = bA->m_sweep;
686 input.sweepB = bB->m_sweep;
687 input.tMax = 1.0f;
688
689 b2TOIOutput output;
690 b2TimeOfImpact(&output, &input);
691
692 // Beta is the fraction of the remaining portion of the .
693 float32 beta = output.t;
694 if (output.state == b2TOIOutput::e_touching)
695 {
696 alpha = b2Min(alpha0 + (1.0f - alpha0) * beta, 1.0f);
697 }
698 else
699 {
700 alpha = 1.0f;
701 }
702
703 c->m_toi = alpha;
704 c->m_flags |= b2Contact::e_toiFlag;
705 }
706
707 if (alpha < minAlpha)
708 {
709 // This is the minimum TOI found so far.
710 minContact = c;
711 minAlpha = alpha;
712 }
713 }
714
715 if (minContact == NULL || 1.0f - 10.0f * b2_epsilon < minAlpha)
716 {
717 // No more TOI events. Done!
718 m_stepComplete = true;
719 break;
720 }
721
722 // Advance the bodies to the TOI.
723 b2Fixture* fA = minContact->GetFixtureA();
724 b2Fixture* fB = minContact->GetFixtureB();
725 b2Body* bA = fA->GetBody();
726 b2Body* bB = fB->GetBody();
727
728 b2Sweep backup1 = bA->m_sweep;
729 b2Sweep backup2 = bB->m_sweep;
730
731 bA->Advance(minAlpha);
732 bB->Advance(minAlpha);
733
734 // The TOI contact likely has some new contact points.
735 minContact->Update(m_contactManager.m_contactListener);
736 minContact->m_flags &= ~b2Contact::e_toiFlag;
737 ++minContact->m_toiCount;
738
739 // Is the contact solid?
740 if (minContact->IsEnabled() == false || minContact->IsTouching() == false)
741 {
742 // Restore the sweeps.
743 minContact->SetEnabled(false);
744 bA->m_sweep = backup1;
745 bB->m_sweep = backup2;
746 bA->SynchronizeTransform();
747 bB->SynchronizeTransform();
748 continue;
749 }
750
751 bA->SetAwake(true);
752 bB->SetAwake(true);
753
754 // Build the island
755 island.Clear();
756 island.Add(bA);
757 island.Add(bB);
758 island.Add(minContact);
759
760 bA->m_flags |= b2Body::e_islandFlag;
761 bB->m_flags |= b2Body::e_islandFlag;
762 minContact->m_flags |= b2Contact::e_islandFlag;
763
764 // Get contacts on bodyA and bodyB.
765 b2Body* bodies[2] = {bA, bB};
766 for (int32 i = 0; i < 2; ++i)
767 {
768 b2Body* body = bodies[i];
769 if (body->m_type == b2_dynamicBody)
770 {
771 for (b2ContactEdge* ce = body->m_contactList; ce; ce = ce->next)
772 {
773 if (island.m_bodyCount == island.m_bodyCapacity)
774 {
775 break;
776 }
777
778 if (island.m_contactCount == island.m_contactCapacity)
779 {
780 break;
781 }
782
783 b2Contact* contact = ce->contact;
784
785 // Has this contact already been added to the island?
786 if (contact->m_flags & b2Contact::e_islandFlag)
787 {
788 continue;
789 }
790
791 // Only add static, kinematic, or bullet bodies.
792 b2Body* other = ce->other;
793 if (other->m_type == b2_dynamicBody &&
794 body->IsBullet() == false && other->IsBullet() == false)
795 {
796 continue;
797 }
798
799 // Skip sensors.
800 bool sensorA = contact->m_fixtureA->m_isSensor;
801 bool sensorB = contact->m_fixtureB->m_isSensor;
802 if (sensorA || sensorB)
803 {
804 continue;
805 }
806
807 // Tentatively advance the body to the TOI.
808 b2Sweep backup = other->m_sweep;
809 if ((other->m_flags & b2Body::e_islandFlag) == 0)
810 {
811 other->Advance(minAlpha);
812 }
813
814 // Update the contact points
815 contact->Update(m_contactManager.m_contactListener);
816
817 // Was the contact disabled by the user?
818 if (contact->IsEnabled() == false)
819 {
820 other->m_sweep = backup;
821 other->SynchronizeTransform();
822 continue;
823 }
824
825 // Are there contact points?
826 if (contact->IsTouching() == false)
827 {
828 other->m_sweep = backup;
829 other->SynchronizeTransform();
830 continue;
831 }
832
833 // Add the contact to the island
834 contact->m_flags |= b2Contact::e_islandFlag;
835 island.Add(contact);
836
837 // Has the other body already been added to the island?
838 if (other->m_flags & b2Body::e_islandFlag)
839 {
840 continue;
841 }
842
843 // Add the other body to the island.
844 other->m_flags |= b2Body::e_islandFlag;
845
846 if (other->m_type != b2_staticBody)
847 {
848 other->SetAwake(true);
849 }
850
851 island.Add(other);
852 }
853 }
854 }
855
856 b2TimeStep subStep;
857 subStep.dt = (1.0f - minAlpha) * step.dt;
858 subStep.inv_dt = 1.0f / subStep.dt;
859 subStep.dtRatio = 1.0f;
860 subStep.positionIterations = 20;
861 subStep.velocityIterations = step.velocityIterations;
862 subStep.warmStarting = false;
863 island.SolveTOI(subStep, bA->m_islandIndex, bB->m_islandIndex);
864
865 // Reset island flags and synchronize broad-phase proxies.
866 for (int32 i = 0; i < island.m_bodyCount; ++i)
867 {
868 b2Body* body = island.m_bodies[i];
869 body->m_flags &= ~b2Body::e_islandFlag;
870
871 if (body->m_type != b2_dynamicBody)
872 {
873 continue;
874 }
875
876 body->SynchronizeFixtures();
877
878 // Invalidate all contact TOIs on this displaced body.
879 for (b2ContactEdge* ce = body->m_contactList; ce; ce = ce->next)
880 {
881 ce->contact->m_flags &= ~(b2Contact::e_toiFlag | b2Contact::e_islandFlag);
882 }
883 }
884
885 // Commit fixture proxy movements to the broad-phase so that new contacts are created.
886 // Also, some contacts can be destroyed.
887 m_contactManager.FindNewContacts();
888
889 if (m_subStepping)
890 {
891 m_stepComplete = false;
892 break;
893 }
894 }
895}
896
897void b2World::Step(float32 dt, int32 velocityIterations, int32 positionIterations)
898{
899 b2Timer stepTimer;
900
901 // If new fixtures were added, we need to find the new contacts.
902 if (m_flags & e_newFixture)
903 {
904 m_contactManager.FindNewContacts();
905 m_flags &= ~e_newFixture;
906 }
907
908 m_flags |= e_locked;
909
910 b2TimeStep step;
911 step.dt = dt;
912 step.velocityIterations = velocityIterations;
913 step.positionIterations = positionIterations;
914 if (dt > 0.0f)
915 {
916 step.inv_dt = 1.0f / dt;
917 }
918 else
919 {
920 step.inv_dt = 0.0f;
921 }
922
923 step.dtRatio = m_inv_dt0 * dt;
924
925 step.warmStarting = m_warmStarting;
926
927 // Update contacts. This is where some contacts are destroyed.
928 {
929 b2Timer timer;
930 m_contactManager.Collide();
931 m_profile.collide = timer.GetMilliseconds();
932 }
933
934 // Integrate velocities, solve velocity constraints, and integrate positions.
935 if (m_stepComplete && step.dt > 0.0f)
936 {
937 b2Timer timer;
938 Solve(step);
939 m_profile.solve = timer.GetMilliseconds();
940 }
941
942 // Handle TOI events.
943 if (m_continuousPhysics && step.dt > 0.0f)
944 {
945 b2Timer timer;
946 SolveTOI(step);
947 m_profile.solveTOI = timer.GetMilliseconds();
948 }
949
950 if (step.dt > 0.0f)
951 {
952 m_inv_dt0 = step.inv_dt;
953 }
954
955 if (m_flags & e_clearForces)
956 {
957 ClearForces();
958 }
959
960 m_flags &= ~e_locked;
961
962 m_profile.step = stepTimer.GetMilliseconds();
963}
964
965void b2World::ClearForces()
966{
967 for (b2Body* body = m_bodyList; body; body = body->GetNext())
968 {
969 body->m_force.SetZero();
970 body->m_torque = 0.0f;
971 }
972}
973
974struct b2WorldQueryWrapper
975{
976 bool QueryCallback(int32 proxyId)
977 {
978 b2FixtureProxy* proxy = (b2FixtureProxy*)broadPhase->GetUserData(proxyId);
979 return callback->ReportFixture(proxy->fixture);
980 }
981
982 const b2BroadPhase* broadPhase;
983 b2QueryCallback* callback;
984};
985
986void b2World::QueryAABB(b2QueryCallback* callback, const b2AABB& aabb) const
987{
988 b2WorldQueryWrapper wrapper;
989 wrapper.broadPhase = &m_contactManager.m_broadPhase;
990 wrapper.callback = callback;
991 m_contactManager.m_broadPhase.Query(&wrapper, aabb);
992}
993
994struct b2WorldRayCastWrapper
995{
996 float32 RayCastCallback(const b2RayCastInput& input, int32 proxyId)
997 {
998 void* userData = broadPhase->GetUserData(proxyId);
999 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
1019void 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
1031void 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
1094void 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
1135void 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
1239int32 b2World::GetProxyCount() const
1240{
1241 return m_contactManager.m_broadPhase.GetProxyCount();
1242}
1243
1244int32 b2World::GetTreeHeight() const
1245{
1246 return m_contactManager.m_broadPhase.GetTreeHeight();
1247}
1248
1249int32 b2World::GetTreeBalance() const
1250{
1251 return m_contactManager.m_broadPhase.GetTreeBalance();
1252}
1253
1254float32 b2World::GetTreeQuality() const
1255{
1256 return m_contactManager.m_broadPhase.GetTreeQuality();
1257}
1258
1259void 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
1282void 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}
1340