1 /*
2 * Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
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 "b2World.h"
20 #include "b2Body.h"
21 #include "b2Island.h"
22 #include "Joints/b2PulleyJoint.h"
23 #include "Contacts/b2Contact.h"
24 #include "Contacts/b2ContactSolver.h"
25 #include "../Collision/b2Collision.h"
26 #include "../Collision/Shapes/b2CircleShape.h"
27 #include "../Collision/Shapes/b2PolygonShape.h"
28 #include <new>
29
b2World(const b2AABB & worldAABB,const b2Vec2 & gravity,bool doSleep)30 b2World::b2World(const b2AABB& worldAABB, const b2Vec2& gravity, bool doSleep)
31 {
32 m_destructionListener = NULL;
33 m_boundaryListener = NULL;
34 m_contactFilter = &b2_defaultFilter;
35 m_contactListener = NULL;
36 m_debugDraw = NULL;
37
38 m_bodyList = NULL;
39 m_contactList = NULL;
40 m_jointList = NULL;
41
42 m_bodyCount = 0;
43 m_contactCount = 0;
44 m_jointCount = 0;
45
46 m_positionCorrection = true;
47 m_warmStarting = true;
48 m_continuousPhysics = true;
49
50 m_allowSleep = doSleep;
51 m_gravity = gravity;
52
53 m_lock = false;
54
55 m_inv_dt0 = 0.0f;
56
57 m_contactManager.m_world = this;
58 void* mem = b2Alloc(sizeof(b2BroadPhase));
59 m_broadPhase = new (mem) b2BroadPhase(worldAABB, &m_contactManager);
60
61 b2BodyDef bd;
62 m_groundBody = CreateBody(&bd);
63 }
64
~b2World()65 b2World::~b2World()
66 {
67 DestroyBody(m_groundBody);
68 m_broadPhase->~b2BroadPhase();
69 b2Free(m_broadPhase);
70 }
71
SetDestructionListener(b2DestructionListener * listener)72 void b2World::SetDestructionListener(b2DestructionListener* listener)
73 {
74 m_destructionListener = listener;
75 }
76
SetBoundaryListener(b2BoundaryListener * listener)77 void b2World::SetBoundaryListener(b2BoundaryListener* listener)
78 {
79 m_boundaryListener = listener;
80 }
81
SetContactFilter(b2ContactFilter * filter)82 void b2World::SetContactFilter(b2ContactFilter* filter)
83 {
84 m_contactFilter = filter;
85 }
86
SetContactListener(b2ContactListener * listener)87 void b2World::SetContactListener(b2ContactListener* listener)
88 {
89 m_contactListener = listener;
90 }
91
SetDebugDraw(b2DebugDraw * debugDraw)92 void b2World::SetDebugDraw(b2DebugDraw* debugDraw)
93 {
94 m_debugDraw = debugDraw;
95 }
96
CreateBody(const b2BodyDef * def)97 b2Body* b2World::CreateBody(const b2BodyDef* def)
98 {
99 b2Assert(m_lock == false);
100 if (m_lock == true)
101 {
102 return NULL;
103 }
104
105 void* mem = m_blockAllocator.Allocate(sizeof(b2Body));
106 b2Body* b = new (mem) b2Body(def, this);
107
108 // Add to world doubly linked list.
109 b->m_prev = NULL;
110 b->m_next = m_bodyList;
111 if (m_bodyList)
112 {
113 m_bodyList->m_prev = b;
114 }
115 m_bodyList = b;
116 ++m_bodyCount;
117
118 return b;
119 }
120
DestroyBody(b2Body * b)121 void b2World::DestroyBody(b2Body* b)
122 {
123 b2Assert(m_bodyCount > 0);
124 b2Assert(m_lock == false);
125 if (m_lock == true)
126 {
127 return;
128 }
129
130 // Delete the attached joints.
131 b2JointEdge* jn = b->m_jointList;
132 while (jn)
133 {
134 b2JointEdge* jn0 = jn;
135 jn = jn->next;
136
137 if (m_destructionListener)
138 {
139 m_destructionListener->SayGoodbye(jn0->joint);
140 }
141
142 DestroyJoint(jn0->joint);
143 }
144
145 // Delete the attached shapes. This destroys broad-phase
146 // proxies and pairs, leading to the destruction of contacts.
147 b2Shape* s = b->m_shapeList;
148 while (s)
149 {
150 b2Shape* s0 = s;
151 s = s->m_next;
152
153 if (m_destructionListener)
154 {
155 m_destructionListener->SayGoodbye(s0);
156 }
157
158 s0->DestroyProxy(m_broadPhase);
159 b2Shape::Destroy(s0, &m_blockAllocator);
160 }
161
162 // Remove world body list.
163 if (b->m_prev)
164 {
165 b->m_prev->m_next = b->m_next;
166 }
167
168 if (b->m_next)
169 {
170 b->m_next->m_prev = b->m_prev;
171 }
172
173 if (b == m_bodyList)
174 {
175 m_bodyList = b->m_next;
176 }
177
178 --m_bodyCount;
179 b->~b2Body();
180 m_blockAllocator.Free(b, sizeof(b2Body));
181 }
182
CreateJoint(const b2JointDef * def)183 b2Joint* b2World::CreateJoint(const b2JointDef* def)
184 {
185 b2Assert(m_lock == false);
186
187 b2Joint* j = b2Joint::Create(def, &m_blockAllocator);
188
189 // Connect to the world list.
190 j->m_prev = NULL;
191 j->m_next = m_jointList;
192 if (m_jointList)
193 {
194 m_jointList->m_prev = j;
195 }
196 m_jointList = j;
197 ++m_jointCount;
198
199 // Connect to the bodies' doubly linked lists.
200 j->m_node1.joint = j;
201 j->m_node1.other = j->m_body2;
202 j->m_node1.prev = NULL;
203 j->m_node1.next = j->m_body1->m_jointList;
204 if (j->m_body1->m_jointList) j->m_body1->m_jointList->prev = &j->m_node1;
205 j->m_body1->m_jointList = &j->m_node1;
206
207 j->m_node2.joint = j;
208 j->m_node2.other = j->m_body1;
209 j->m_node2.prev = NULL;
210 j->m_node2.next = j->m_body2->m_jointList;
211 if (j->m_body2->m_jointList) j->m_body2->m_jointList->prev = &j->m_node2;
212 j->m_body2->m_jointList = &j->m_node2;
213
214 // If the joint prevents collisions, then reset collision filtering.
215 if (def->collideConnected == false)
216 {
217 // Reset the proxies on the body with the minimum number of shapes.
218 b2Body* b = def->body1->m_shapeCount < def->body2->m_shapeCount ? def->body1 : def->body2;
219 for (b2Shape* s = b->m_shapeList; s; s = s->m_next)
220 {
221 s->RefilterProxy(m_broadPhase, b->GetXForm());
222 }
223 }
224
225 return j;
226 }
227
DestroyJoint(b2Joint * j)228 void b2World::DestroyJoint(b2Joint* j)
229 {
230 b2Assert(m_lock == false);
231
232 bool collideConnected = j->m_collideConnected;
233
234 // Remove from the doubly linked list.
235 if (j->m_prev)
236 {
237 j->m_prev->m_next = j->m_next;
238 }
239
240 if (j->m_next)
241 {
242 j->m_next->m_prev = j->m_prev;
243 }
244
245 if (j == m_jointList)
246 {
247 m_jointList = j->m_next;
248 }
249
250 // Disconnect from island graph.
251 b2Body* body1 = j->m_body1;
252 b2Body* body2 = j->m_body2;
253
254 // Wake up connected bodies.
255 body1->WakeUp();
256 body2->WakeUp();
257
258 // Remove from body 1.
259 if (j->m_node1.prev)
260 {
261 j->m_node1.prev->next = j->m_node1.next;
262 }
263
264 if (j->m_node1.next)
265 {
266 j->m_node1.next->prev = j->m_node1.prev;
267 }
268
269 if (&j->m_node1 == body1->m_jointList)
270 {
271 body1->m_jointList = j->m_node1.next;
272 }
273
274 j->m_node1.prev = NULL;
275 j->m_node1.next = NULL;
276
277 // Remove from body 2
278 if (j->m_node2.prev)
279 {
280 j->m_node2.prev->next = j->m_node2.next;
281 }
282
283 if (j->m_node2.next)
284 {
285 j->m_node2.next->prev = j->m_node2.prev;
286 }
287
288 if (&j->m_node2 == body2->m_jointList)
289 {
290 body2->m_jointList = j->m_node2.next;
291 }
292
293 j->m_node2.prev = NULL;
294 j->m_node2.next = NULL;
295
296 b2Joint::Destroy(j, &m_blockAllocator);
297
298 b2Assert(m_jointCount > 0);
299 --m_jointCount;
300
301 // If the joint prevents collisions, then reset collision filtering.
302 if (collideConnected == false)
303 {
304 // Reset the proxies on the body with the minimum number of shapes.
305 b2Body* b = body1->m_shapeCount < body2->m_shapeCount ? body1 : body2;
306 for (b2Shape* s = b->m_shapeList; s; s = s->m_next)
307 {
308 s->RefilterProxy(m_broadPhase, b->GetXForm());
309 }
310 }
311 }
312
Refilter(b2Shape * shape)313 void b2World::Refilter(b2Shape* shape)
314 {
315 shape->RefilterProxy(m_broadPhase, shape->GetBody()->GetXForm());
316 }
317
318 // Find islands, integrate and solve constraints, solve position constraints
Solve(const b2TimeStep & step)319 void b2World::Solve(const b2TimeStep& step)
320 {
321 m_positionIterationCount = 0;
322
323 // Size the island for the worst case.
324 b2Island island(m_bodyCount, m_contactCount, m_jointCount, &m_stackAllocator, m_contactListener);
325
326 // Clear all the island flags.
327 for (b2Body* b = m_bodyList; b; b = b->m_next)
328 {
329 b->m_flags &= ~b2Body::e_islandFlag;
330 }
331 for (b2Contact* c = m_contactList; c; c = c->m_next)
332 {
333 c->m_flags &= ~b2Contact::e_islandFlag;
334 }
335 for (b2Joint* j = m_jointList; j; j = j->m_next)
336 {
337 j->m_islandFlag = false;
338 }
339
340 // Build and simulate all awake islands.
341 int32 stackSize = m_bodyCount;
342 b2Body** stack = (b2Body**)m_stackAllocator.Allocate(stackSize * sizeof(b2Body*));
343 for (b2Body* seed = m_bodyList; seed; seed = seed->m_next)
344 {
345 if (seed->m_flags & (b2Body::e_islandFlag | b2Body::e_sleepFlag | b2Body::e_frozenFlag))
346 {
347 continue;
348 }
349
350 if (seed->IsStatic())
351 {
352 continue;
353 }
354
355 // Reset island and stack.
356 island.Clear();
357 int32 stackCount = 0;
358 stack[stackCount++] = seed;
359 seed->m_flags |= b2Body::e_islandFlag;
360
361 // Perform a depth first search (DFS) on the constraint graph.
362 while (stackCount > 0)
363 {
364 // Grab the next body off the stack and add it to the island.
365 b2Body* b = stack[--stackCount];
366 island.Add(b);
367
368 // Make sure the body is awake.
369 b->m_flags &= ~b2Body::e_sleepFlag;
370
371 // To keep islands as small as possible, we don't
372 // propagate islands across static bodies.
373 if (b->IsStatic())
374 {
375 continue;
376 }
377
378 // Search all contacts connected to this body.
379 for (b2ContactEdge* cn = b->m_contactList; cn; cn = cn->next)
380 {
381 // Has this contact already been added to an island?
382 if (cn->contact->m_flags & (b2Contact::e_islandFlag | b2Contact::e_nonSolidFlag))
383 {
384 continue;
385 }
386
387 // Is this contact touching?
388 if (cn->contact->GetManifoldCount() == 0)
389 {
390 continue;
391 }
392
393 island.Add(cn->contact);
394 cn->contact->m_flags |= b2Contact::e_islandFlag;
395
396 b2Body* other = cn->other;
397
398 // Was the other body already added to this island?
399 if (other->m_flags & b2Body::e_islandFlag)
400 {
401 continue;
402 }
403
404 b2Assert(stackCount < stackSize);
405 stack[stackCount++] = other;
406 other->m_flags |= b2Body::e_islandFlag;
407 }
408
409 // Search all joints connect to this body.
410 for (b2JointEdge* jn = b->m_jointList; jn; jn = jn->next)
411 {
412 if (jn->joint->m_islandFlag == true)
413 {
414 continue;
415 }
416
417 island.Add(jn->joint);
418 jn->joint->m_islandFlag = true;
419
420 b2Body* other = jn->other;
421 if (other->m_flags & b2Body::e_islandFlag)
422 {
423 continue;
424 }
425
426 b2Assert(stackCount < stackSize);
427 stack[stackCount++] = other;
428 other->m_flags |= b2Body::e_islandFlag;
429 }
430 }
431
432 island.Solve(step, m_gravity, m_positionCorrection, m_allowSleep);
433 m_positionIterationCount = b2Max(m_positionIterationCount, island.m_positionIterationCount);
434
435 // Post solve cleanup.
436 for (int32 i = 0; i < island.m_bodyCount; ++i)
437 {
438 // Allow static bodies to participate in other islands.
439 b2Body* b = island.m_bodies[i];
440 if (b->IsStatic())
441 {
442 b->m_flags &= ~b2Body::e_islandFlag;
443 }
444 }
445 }
446
447 m_stackAllocator.Free(stack);
448
449 // Synchronize shapes, check for out of range bodies.
450 for (b2Body* b = m_bodyList; b; b = b->GetNext())
451 {
452 if (b->m_flags & (b2Body::e_sleepFlag | b2Body::e_frozenFlag))
453 {
454 continue;
455 }
456
457 if (b->IsStatic())
458 {
459 continue;
460 }
461
462 // Update shapes (for broad-phase). If the shapes go out of
463 // the world AABB then shapes and contacts may be destroyed,
464 // including contacts that are
465 bool inRange = b->SynchronizeShapes();
466
467 // Did the body's shapes leave the world?
468 if (inRange == false && m_boundaryListener != NULL)
469 {
470 m_boundaryListener->Violation(b);
471 }
472 }
473
474 // Commit shape proxy movements to the broad-phase so that new contacts are created.
475 // Also, some contacts can be destroyed.
476 m_broadPhase->Commit();
477 }
478
479 // Find TOI contacts and solve them.
SolveTOI(const b2TimeStep & step)480 void b2World::SolveTOI(const b2TimeStep& step)
481 {
482 // Reserve an island and a stack for TOI island solution.
483 b2Island island(m_bodyCount, b2_maxTOIContactsPerIsland, 0, &m_stackAllocator, m_contactListener);
484 int32 stackSize = m_bodyCount;
485 b2Body** stack = (b2Body**)m_stackAllocator.Allocate(stackSize * sizeof(b2Body*));
486
487 for (b2Body* b = m_bodyList; b; b = b->m_next)
488 {
489 b->m_flags &= ~b2Body::e_islandFlag;
490 b->m_sweep.t0 = 0.0f;
491 }
492
493 for (b2Contact* c = m_contactList; c; c = c->m_next)
494 {
495 // Invalidate TOI
496 c->m_flags &= ~(b2Contact::e_toiFlag | b2Contact::e_islandFlag);
497 }
498
499 // Find TOI events and solve them.
500 for (;;)
501 {
502 // Find the first TOI.
503 b2Contact* minContact = NULL;
504 float32 minTOI = 1.0f;
505
506 for (b2Contact* c = m_contactList; c; c = c->m_next)
507 {
508 if (c->m_flags & (b2Contact::e_slowFlag | b2Contact::e_nonSolidFlag))
509 {
510 continue;
511 }
512
513 // TODO_ERIN keep a counter on the contact, only respond to M TOIs per contact.
514
515 float32 toi = 1.0f;
516 if (c->m_flags & b2Contact::e_toiFlag)
517 {
518 // This contact has a valid cached TOI.
519 toi = c->m_toi;
520 }
521 else
522 {
523 // Compute the TOI for this contact.
524 b2Shape* s1 = c->GetShape1();
525 b2Shape* s2 = c->GetShape2();
526 b2Body* b1 = s1->GetBody();
527 b2Body* b2 = s2->GetBody();
528
529 if ((b1->IsStatic() || b1->IsSleeping()) && (b2->IsStatic() || b2->IsSleeping()))
530 {
531 continue;
532 }
533
534 // Put the sweeps onto the same time interval.
535 float32 t0 = b1->m_sweep.t0;
536
537 if (b1->m_sweep.t0 < b2->m_sweep.t0)
538 {
539 t0 = b2->m_sweep.t0;
540 b1->m_sweep.Advance(t0);
541 }
542 else if (b2->m_sweep.t0 < b1->m_sweep.t0)
543 {
544 t0 = b1->m_sweep.t0;
545 b2->m_sweep.Advance(t0);
546 }
547
548 b2Assert(t0 < 1.0f);
549
550 // Compute the time of impact.
551 toi = b2TimeOfImpact(c->m_shape1, b1->m_sweep, c->m_shape2, b2->m_sweep);
552
553 b2Assert(0.0f <= toi && toi <= 1.0f);
554
555 if (toi > 0.0f && toi < 1.0f)
556 {
557 toi = b2Min((1.0f - toi) * t0 + toi, 1.0f);
558 }
559
560
561 c->m_toi = toi;
562 c->m_flags |= b2Contact::e_toiFlag;
563 }
564
565 if (B2_FLT_EPSILON < toi && toi < minTOI)
566 {
567 // This is the minimum TOI found so far.
568 minContact = c;
569 minTOI = toi;
570 }
571 }
572
573 if (minContact == NULL || 1.0f - 100.0f * B2_FLT_EPSILON < minTOI)
574 {
575 // No more TOI events. Done!
576 break;
577 }
578
579 // Advance the bodies to the TOI.
580 b2Shape* s1 = minContact->GetShape1();
581 b2Shape* s2 = minContact->GetShape2();
582 b2Body* b1 = s1->GetBody();
583 b2Body* b2 = s2->GetBody();
584 b1->Advance(minTOI);
585 b2->Advance(minTOI);
586
587 // The TOI contact likely has some new contact points.
588 minContact->Update(m_contactListener);
589 minContact->m_flags &= ~b2Contact::e_toiFlag;
590
591 if (minContact->GetManifoldCount() == 0)
592 {
593 // This shouldn't happen. Numerical error?
594 //b2Assert(false);
595 continue;
596 }
597
598 // Build the TOI island. We need a dynamic seed.
599 b2Body* seed = b1;
600 if (seed->IsStatic())
601 {
602 seed = b2;
603 }
604
605 // Reset island and stack.
606 island.Clear();
607 int32 stackCount = 0;
608 stack[stackCount++] = seed;
609 seed->m_flags |= b2Body::e_islandFlag;
610
611 // Perform a depth first search (DFS) on the contact graph.
612 while (stackCount > 0)
613 {
614 // Grab the next body off the stack and add it to the island.
615 b2Body* b = stack[--stackCount];
616 island.Add(b);
617
618 // Make sure the body is awake.
619 b->m_flags &= ~b2Body::e_sleepFlag;
620
621 // To keep islands as small as possible, we don't
622 // propagate islands across static bodies.
623 if (b->IsStatic())
624 {
625 continue;
626 }
627
628 // Search all contacts connected to this body.
629 for (b2ContactEdge* cn = b->m_contactList; cn; cn = cn->next)
630 {
631 // Does the TOI island still have space for contacts?
632 if (island.m_contactCount == island.m_contactCapacity)
633 {
634 continue;
635 }
636
637 // Has this contact already been added to an island? Skip slow or non-solid contacts.
638 if (cn->contact->m_flags & (b2Contact::e_islandFlag | b2Contact::e_slowFlag | b2Contact::e_nonSolidFlag))
639 {
640 continue;
641 }
642
643 // Is this contact touching? For performance we are not updating this contact.
644 if (cn->contact->GetManifoldCount() == 0)
645 {
646 continue;
647 }
648
649 island.Add(cn->contact);
650 cn->contact->m_flags |= b2Contact::e_islandFlag;
651
652 // Update other body.
653 b2Body* other = cn->other;
654
655 // Was the other body already added to this island?
656 if (other->m_flags & b2Body::e_islandFlag)
657 {
658 continue;
659 }
660
661 // March forward, this can do no harm since this is the min TOI.
662 if (other->IsStatic() == false)
663 {
664 other->Advance(minTOI);
665 other->WakeUp();
666 }
667
668 b2Assert(stackCount < stackSize);
669 stack[stackCount++] = other;
670 other->m_flags |= b2Body::e_islandFlag;
671 }
672 }
673
674 b2TimeStep subStep;
675 subStep.dt = (1.0f - minTOI) * step.dt;
676 b2Assert(subStep.dt > B2_FLT_EPSILON);
677 subStep.inv_dt = 1.0f / subStep.dt;
678 subStep.maxIterations = step.maxIterations;
679
680 island.SolveTOI(subStep);
681
682 // Post solve cleanup.
683 for (int32 i = 0; i < island.m_bodyCount; ++i)
684 {
685 // Allow bodies to participate in future TOI islands.
686 b2Body* b = island.m_bodies[i];
687 b->m_flags &= ~b2Body::e_islandFlag;
688
689 if (b->m_flags & (b2Body::e_sleepFlag | b2Body::e_frozenFlag))
690 {
691 continue;
692 }
693
694 if (b->IsStatic())
695 {
696 continue;
697 }
698
699 // Update shapes (for broad-phase). If the shapes go out of
700 // the world AABB then shapes and contacts may be destroyed,
701 // including contacts that are
702 bool inRange = b->SynchronizeShapes();
703
704 // Did the body's shapes leave the world?
705 if (inRange == false && m_boundaryListener != NULL)
706 {
707 m_boundaryListener->Violation(b);
708 }
709
710 // Invalidate all contact TOIs associated with this body. Some of these
711 // may not be in the island because they were not touching.
712 for (b2ContactEdge* cn = b->m_contactList; cn; cn = cn->next)
713 {
714 cn->contact->m_flags &= ~b2Contact::e_toiFlag;
715 }
716 }
717
718 for (int32 i = 0; i < island.m_contactCount; ++i)
719 {
720 // Allow contacts to participate in future TOI islands.
721 b2Contact* c = island.m_contacts[i];
722 c->m_flags &= ~(b2Contact::e_toiFlag | b2Contact::e_islandFlag);
723 }
724
725 // Commit shape proxy movements to the broad-phase so that new contacts are created.
726 // Also, some contacts can be destroyed.
727 m_broadPhase->Commit();
728 }
729
730 m_stackAllocator.Free(stack);
731 }
732
Step(float32 dt,int32 iterations)733 void b2World::Step(float32 dt, int32 iterations)
734 {
735 m_lock = true;
736
737 b2TimeStep step;
738 step.dt = dt;
739 step.maxIterations = iterations;
740 if (dt > 0.0f)
741 {
742 step.inv_dt = 1.0f / dt;
743 }
744 else
745 {
746 step.inv_dt = 0.0f;
747 }
748
749 step.dtRatio = m_inv_dt0 * dt;
750
751 step.positionCorrection = m_positionCorrection;
752 step.warmStarting = m_warmStarting;
753
754 // Update contacts.
755 m_contactManager.Collide();
756
757 // Integrate velocities, solve velocity constraints, and integrate positions.
758 if (step.dt > 0.0f)
759 {
760 Solve(step);
761 }
762
763 // Handle TOI events.
764 if (m_continuousPhysics && step.dt > 0.0f)
765 {
766 SolveTOI(step);
767 }
768
769 // Draw debug information.
770 DrawDebugData();
771
772 m_inv_dt0 = step.inv_dt;
773 m_lock = false;
774 }
775
Query(const b2AABB & aabb,b2Shape ** shapes,int32 maxCount)776 int32 b2World::Query(const b2AABB& aabb, b2Shape** shapes, int32 maxCount)
777 {
778 void** results = (void**)m_stackAllocator.Allocate(maxCount * sizeof(void*));
779
780 int32 count = m_broadPhase->Query(aabb, results, maxCount);
781
782 for (int32 i = 0; i < count; ++i)
783 {
784 shapes[i] = (b2Shape*)results[i];
785 }
786
787 m_stackAllocator.Free(results);
788 return count;
789 }
790
DrawShape(b2Shape * shape,const b2XForm & xf,const b2Color & color,bool core)791 void b2World::DrawShape(b2Shape* shape, const b2XForm& xf, const b2Color& color, bool core)
792 {
793 b2Color coreColor(0.9f, 0.6f, 0.6f);
794
795 switch (shape->GetType())
796 {
797 case e_circleShape:
798 {
799 b2CircleShape* circle = (b2CircleShape*)shape;
800
801 b2Vec2 center = b2Mul(xf, circle->GetLocalPosition());
802 float32 radius = circle->GetRadius();
803 b2Vec2 axis = xf.R.col1;
804
805 m_debugDraw->DrawSolidCircle(center, radius, axis, color);
806
807 if (core)
808 {
809 m_debugDraw->DrawCircle(center, radius - b2_toiSlop, coreColor);
810 }
811 }
812 break;
813
814 case e_polygonShape:
815 {
816 b2PolygonShape* poly = (b2PolygonShape*)shape;
817 int32 vertexCount = poly->GetVertexCount();
818 const b2Vec2* localVertices = poly->GetVertices();
819
820 b2Assert(vertexCount <= b2_maxPolygonVertices);
821 b2Vec2 vertices[b2_maxPolygonVertices];
822
823 for (int32 i = 0; i < vertexCount; ++i)
824 {
825 vertices[i] = b2Mul(xf, localVertices[i]);
826 }
827
828 m_debugDraw->DrawSolidPolygon(vertices, vertexCount, color);
829
830 if (core)
831 {
832 const b2Vec2* localCoreVertices = poly->GetCoreVertices();
833 for (int32 i = 0; i < vertexCount; ++i)
834 {
835 vertices[i] = b2Mul(xf, localCoreVertices[i]);
836 }
837 m_debugDraw->DrawPolygon(vertices, vertexCount, coreColor);
838 }
839 }
840 break;
841 }
842 }
843
DrawJoint(b2Joint * joint)844 void b2World::DrawJoint(b2Joint* joint)
845 {
846 b2Body* b1 = joint->GetBody1();
847 b2Body* b2 = joint->GetBody2();
848 const b2XForm& xf1 = b1->GetXForm();
849 const b2XForm& xf2 = b2->GetXForm();
850 b2Vec2 x1 = xf1.position;
851 b2Vec2 x2 = xf2.position;
852 b2Vec2 p1 = joint->GetAnchor1();
853 b2Vec2 p2 = joint->GetAnchor2();
854
855 b2Color color(0.5f, 0.8f, 0.8f);
856
857 switch (joint->GetType())
858 {
859 case e_distanceJoint:
860 m_debugDraw->DrawSegment(p1, p2, color);
861 break;
862
863 case e_pulleyJoint:
864 {
865 b2PulleyJoint* pulley = (b2PulleyJoint*)joint;
866 b2Vec2 s1 = pulley->GetGroundAnchor1();
867 b2Vec2 s2 = pulley->GetGroundAnchor2();
868 m_debugDraw->DrawSegment(s1, p1, color);
869 m_debugDraw->DrawSegment(s2, p2, color);
870 m_debugDraw->DrawSegment(s1, s2, color);
871 }
872 break;
873
874 case e_mouseJoint:
875 // don't draw this
876 break;
877
878 default:
879 m_debugDraw->DrawSegment(x1, p1, color);
880 m_debugDraw->DrawSegment(p1, p2, color);
881 m_debugDraw->DrawSegment(x2, p2, color);
882 }
883 }
884
DrawDebugData()885 void b2World::DrawDebugData()
886 {
887 if (m_debugDraw == NULL)
888 {
889 return;
890 }
891
892 uint32 flags = m_debugDraw->GetFlags();
893
894 if (flags & b2DebugDraw::e_shapeBit)
895 {
896 bool core = (flags & b2DebugDraw::e_coreShapeBit) == b2DebugDraw::e_coreShapeBit;
897
898 for (b2Body* b = m_bodyList; b; b = b->GetNext())
899 {
900 const b2XForm& xf = b->GetXForm();
901 for (b2Shape* s = b->GetShapeList(); s; s = s->GetNext())
902 {
903 if (b->IsStatic())
904 {
905 DrawShape(s, xf, b2Color(0.5f, 0.9f, 0.5f), core);
906 }
907 else if (b->IsSleeping())
908 {
909 DrawShape(s, xf, b2Color(0.5f, 0.5f, 0.9f), core);
910 }
911 else
912 {
913 DrawShape(s, xf, b2Color(0.9f, 0.9f, 0.9f), core);
914 }
915 }
916 }
917 }
918
919 if (flags & b2DebugDraw::e_jointBit)
920 {
921 for (b2Joint* j = m_jointList; j; j = j->GetNext())
922 {
923 if (j->GetType() != e_mouseJoint)
924 {
925 DrawJoint(j);
926 }
927 }
928 }
929
930 if (flags & b2DebugDraw::e_pairBit)
931 {
932 b2BroadPhase* bp = m_broadPhase;
933 b2Vec2 invQ;
934 invQ.Set(1.0f / bp->m_quantizationFactor.x, 1.0f / bp->m_quantizationFactor.y);
935 b2Color color(0.9f, 0.9f, 0.3f);
936
937 for (int32 i = 0; i < b2_tableCapacity; ++i)
938 {
939 uint16 index = bp->m_pairManager.m_hashTable[i];
940 while (index != b2_nullPair)
941 {
942 b2Pair* pair = bp->m_pairManager.m_pairs + index;
943 b2Proxy* p1 = bp->m_proxyPool + pair->proxyId1;
944 b2Proxy* p2 = bp->m_proxyPool + pair->proxyId2;
945
946 b2AABB b1, b2;
947 b1.lowerBound.x = bp->m_worldAABB.lowerBound.x + invQ.x * bp->m_bounds[0][p1->lowerBounds[0]].value;
948 b1.lowerBound.y = bp->m_worldAABB.lowerBound.y + invQ.y * bp->m_bounds[1][p1->lowerBounds[1]].value;
949 b1.upperBound.x = bp->m_worldAABB.lowerBound.x + invQ.x * bp->m_bounds[0][p1->upperBounds[0]].value;
950 b1.upperBound.y = bp->m_worldAABB.lowerBound.y + invQ.y * bp->m_bounds[1][p1->upperBounds[1]].value;
951 b2.lowerBound.x = bp->m_worldAABB.lowerBound.x + invQ.x * bp->m_bounds[0][p2->lowerBounds[0]].value;
952 b2.lowerBound.y = bp->m_worldAABB.lowerBound.y + invQ.y * bp->m_bounds[1][p2->lowerBounds[1]].value;
953 b2.upperBound.x = bp->m_worldAABB.lowerBound.x + invQ.x * bp->m_bounds[0][p2->upperBounds[0]].value;
954 b2.upperBound.y = bp->m_worldAABB.lowerBound.y + invQ.y * bp->m_bounds[1][p2->upperBounds[1]].value;
955
956 b2Vec2 x1 = 0.5f * (b1.lowerBound + b1.upperBound);
957 b2Vec2 x2 = 0.5f * (b2.lowerBound + b2.upperBound);
958
959 m_debugDraw->DrawSegment(x1, x2, color);
960
961 index = pair->next;
962 }
963 }
964 }
965
966 if (flags & b2DebugDraw::e_aabbBit)
967 {
968 b2BroadPhase* bp = m_broadPhase;
969 b2Vec2 worldLower = bp->m_worldAABB.lowerBound;
970 b2Vec2 worldUpper = bp->m_worldAABB.upperBound;
971
972 b2Vec2 invQ;
973 invQ.Set(1.0f / bp->m_quantizationFactor.x, 1.0f / bp->m_quantizationFactor.y);
974 b2Color color(0.9f, 0.3f, 0.9f);
975 for (int32 i = 0; i < b2_maxProxies; ++i)
976 {
977 b2Proxy* p = bp->m_proxyPool + i;
978 if (p->IsValid() == false)
979 {
980 continue;
981 }
982
983 b2AABB b;
984 b.lowerBound.x = worldLower.x + invQ.x * bp->m_bounds[0][p->lowerBounds[0]].value;
985 b.lowerBound.y = worldLower.y + invQ.y * bp->m_bounds[1][p->lowerBounds[1]].value;
986 b.upperBound.x = worldLower.x + invQ.x * bp->m_bounds[0][p->upperBounds[0]].value;
987 b.upperBound.y = worldLower.y + invQ.y * bp->m_bounds[1][p->upperBounds[1]].value;
988
989 b2Vec2 vs[4];
990 vs[0].Set(b.lowerBound.x, b.lowerBound.y);
991 vs[1].Set(b.upperBound.x, b.lowerBound.y);
992 vs[2].Set(b.upperBound.x, b.upperBound.y);
993 vs[3].Set(b.lowerBound.x, b.upperBound.y);
994
995 m_debugDraw->DrawPolygon(vs, 4, color);
996 }
997
998 b2Vec2 vs[4];
999 vs[0].Set(worldLower.x, worldLower.y);
1000 vs[1].Set(worldUpper.x, worldLower.y);
1001 vs[2].Set(worldUpper.x, worldUpper.y);
1002 vs[3].Set(worldLower.x, worldUpper.y);
1003 m_debugDraw->DrawPolygon(vs, 4, b2Color(0.3f, 0.9f, 0.9f));
1004 }
1005
1006 if (flags & b2DebugDraw::e_obbBit)
1007 {
1008 b2Color color(0.5f, 0.3f, 0.5f);
1009
1010 for (b2Body* b = m_bodyList; b; b = b->GetNext())
1011 {
1012 const b2XForm& xf = b->GetXForm();
1013 for (b2Shape* s = b->GetShapeList(); s; s = s->GetNext())
1014 {
1015 if (s->GetType() != e_polygonShape)
1016 {
1017 continue;
1018 }
1019
1020 b2PolygonShape* poly = (b2PolygonShape*)s;
1021 const b2OBB& obb = poly->GetOBB();
1022 b2Vec2 h = obb.extents;
1023 b2Vec2 vs[4];
1024 vs[0].Set(-h.x, -h.y);
1025 vs[1].Set( h.x, -h.y);
1026 vs[2].Set( h.x, h.y);
1027 vs[3].Set(-h.x, h.y);
1028
1029 for (int32 i = 0; i < 4; ++i)
1030 {
1031 vs[i] = obb.center + b2Mul(obb.R, vs[i]);
1032 vs[i] = b2Mul(xf, vs[i]);
1033 }
1034
1035 m_debugDraw->DrawPolygon(vs, 4, color);
1036 }
1037 }
1038 }
1039
1040 if (flags & b2DebugDraw::e_centerOfMassBit)
1041 {
1042 for (b2Body* b = m_bodyList; b; b = b->GetNext())
1043 {
1044 b2XForm xf = b->GetXForm();
1045 xf.position = b->GetWorldCenter();
1046 m_debugDraw->DrawXForm(xf);
1047 }
1048 }
1049 }
1050
Validate()1051 void b2World::Validate()
1052 {
1053 m_broadPhase->Validate();
1054 }
1055
GetProxyCount() const1056 int32 b2World::GetProxyCount() const
1057 {
1058 return m_broadPhase->m_proxyCount;
1059 }
1060
GetPairCount() const1061 int32 b2World::GetPairCount() const
1062 {
1063 return m_broadPhase->m_pairManager.m_pairCount;
1064 }
1065