b2_world.cpp

Go to the documentation of this file.

// MIT License

// Copyright (c) 2019 Erin Catto

// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:

// The above copyright notice and this permission notice shall be included in all
// copies or substantial portions of the Software.

// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.

#include "b2_contact_solver.h"
#include "b2_island.h"

#include "box2d/b2_body.h"
#include "box2d/b2_broad_phase.h"
#include "box2d/b2_chain_shape.h"
#include "box2d/b2_circle_shape.h"
#include "box2d/b2_collision.h"
#include "box2d/b2_contact.h"
#include "box2d/b2_draw.h"
#include "box2d/b2_edge_shape.h"
#include "box2d/b2_fixture.h"
#include "box2d/b2_polygon_shape.h"
#include "box2d/b2_pulley_joint.h"
#include "box2d/b2_time_of_impact.h"
#include "box2d/b2_timer.h"
#include "box2d/b2_world.h"

#include <new>

b2World::b2World(const b2Vec2& gravity)
{
        m_destructionListener = nullptr;
        m_debugDraw = nullptr;

        m_bodyList = nullptr;
        m_jointList = nullptr;

        m_bodyCount = 0;
        m_jointCount = 0;

        m_warmStarting = true;
        m_continuousPhysics = true;
        m_subStepping = false;

        m_stepComplete = true;

        m_allowSleep = true;
        m_gravity = gravity;

        m_newContacts = false;
        m_locked = false;
        m_clearForces = true;

        m_inv_dt0 = 0.0f;

        m_contactManager.m_allocator = &m_blockAllocator;

        memset(&m_profile, 0, sizeof(b2Profile));
}

b2World::~b2World()
{
        // Some shapes allocate using b2Alloc.
        b2Body* b = m_bodyList;
        while (b)
        {
                b2Body* bNext = b->m_next;

                b2Fixture* f = b->m_fixtureList;
                while (f)
                {
                        b2Fixture* fNext = f->m_next;
                        f->m_proxyCount = 0;
                        f->Destroy(&m_blockAllocator);
                        f = fNext;
                }

                b = bNext;
        }
}

void b2World::SetDestructionListener(b2DestructionListener* listener)
{
        m_destructionListener = listener;
}

void b2World::SetContactFilter(b2ContactFilter* filter)
{
        m_contactManager.m_contactFilter = filter;
}

void b2World::SetContactListener(b2ContactListener* listener)
{
        m_contactManager.m_contactListener = listener;
}

void b2World::SetDebugDraw(b2Draw* debugDraw)
{
        m_debugDraw = debugDraw;
}

b2Body* b2World::CreateBody(const b2BodyDef* def)
{
        b2Assert(IsLocked() == false);
        if (IsLocked())
        {
                return nullptr;
        }

        void* mem = m_blockAllocator.Allocate(sizeof(b2Body));
        b2Body* b = new (mem) b2Body(def, this);

        // Add to world doubly linked list.
        b->m_prev = nullptr;
        b->m_next = m_bodyList;
        if (m_bodyList)
        {
                m_bodyList->m_prev = b;
        }
        m_bodyList = b;
        ++m_bodyCount;

        return b;
}

void b2World::DestroyBody(b2Body* b)
{
        b2Assert(m_bodyCount > 0);
        b2Assert(IsLocked() == false);
        if (IsLocked())
        {
                return;
        }

        // Delete the attached joints.
        b2JointEdge* je = b->m_jointList;
        while (je)
        {
                b2JointEdge* je0 = je;
                je = je->next;

                if (m_destructionListener)
                {
                        m_destructionListener->SayGoodbye(je0->joint);
                }

                DestroyJoint(je0->joint);

                b->m_jointList = je;
        }
        b->m_jointList = nullptr;

        // Delete the attached contacts.
        b2ContactEdge* ce = b->m_contactList;
        while (ce)
        {
                b2ContactEdge* ce0 = ce;
                ce = ce->next;
                m_contactManager.Destroy(ce0->contact);
        }
        b->m_contactList = nullptr;

        // Delete the attached fixtures. This destroys broad-phase proxies.
        b2Fixture* f = b->m_fixtureList;
        while (f)
        {
                b2Fixture* f0 = f;
                f = f->m_next;

                if (m_destructionListener)
                {
                        m_destructionListener->SayGoodbye(f0);
                }

                f0->DestroyProxies(&m_contactManager.m_broadPhase);
                f0->Destroy(&m_blockAllocator);
                f0->~b2Fixture();
                m_blockAllocator.Free(f0, sizeof(b2Fixture));

                b->m_fixtureList = f;
                b->m_fixtureCount -= 1;
        }
        b->m_fixtureList = nullptr;
        b->m_fixtureCount = 0;

        // Remove world body list.
        if (b->m_prev)
        {
                b->m_prev->m_next = b->m_next;
        }

        if (b->m_next)
        {
                b->m_next->m_prev = b->m_prev;
        }

        if (b == m_bodyList)
        {
                m_bodyList = b->m_next;
        }

        --m_bodyCount;
        b->~b2Body();
        m_blockAllocator.Free(b, sizeof(b2Body));
}

b2Joint* b2World::CreateJoint(const b2JointDef* def)
{
        b2Assert(IsLocked() == false);
        if (IsLocked())
        {
                return nullptr;
        }

        b2Joint* j = b2Joint::Create(def, &m_blockAllocator);

        // Connect to the world list.
        j->m_prev = nullptr;
        j->m_next = m_jointList;
        if (m_jointList)
        {
                m_jointList->m_prev = j;
        }
        m_jointList = j;
        ++m_jointCount;

        // Connect to the bodies' doubly linked lists.
        j->m_edgeA.joint = j;
        j->m_edgeA.other = j->m_bodyB;
        j->m_edgeA.prev = nullptr;
        j->m_edgeA.next = j->m_bodyA->m_jointList;
        if (j->m_bodyA->m_jointList) j->m_bodyA->m_jointList->prev = &j->m_edgeA;
        j->m_bodyA->m_jointList = &j->m_edgeA;

        j->m_edgeB.joint = j;
        j->m_edgeB.other = j->m_bodyA;
        j->m_edgeB.prev = nullptr;
        j->m_edgeB.next = j->m_bodyB->m_jointList;
        if (j->m_bodyB->m_jointList) j->m_bodyB->m_jointList->prev = &j->m_edgeB;
        j->m_bodyB->m_jointList = &j->m_edgeB;

        b2Body* bodyA = def->bodyA;
        b2Body* bodyB = def->bodyB;

        // If the joint prevents collisions, then flag any contacts for filtering.
        if (def->collideConnected == false)
        {
                b2ContactEdge* edge = bodyB->GetContactList();
                while (edge)
                {
                        if (edge->other == bodyA)
                        {
                                // Flag the contact for filtering at the next time step (where either
                                // body is awake).
                                edge->contact->FlagForFiltering();
                        }

                        edge = edge->next;
                }
        }

        // Note: creating a joint doesn't wake the bodies.

        return j;
}

void b2World::DestroyJoint(b2Joint* j)
{
        b2Assert(IsLocked() == false);
        if (IsLocked())
        {
                return;
        }

        bool collideConnected = j->m_collideConnected;

        // Remove from the doubly linked list.
        if (j->m_prev)
        {
                j->m_prev->m_next = j->m_next;
        }

        if (j->m_next)
        {
                j->m_next->m_prev = j->m_prev;
        }

        if (j == m_jointList)
        {
                m_jointList = j->m_next;
        }

        // Disconnect from island graph.
        b2Body* bodyA = j->m_bodyA;
        b2Body* bodyB = j->m_bodyB;

        // Wake up connected bodies.
        bodyA->SetAwake(true);
        bodyB->SetAwake(true);

        // Remove from body 1.
        if (j->m_edgeA.prev)
        {
                j->m_edgeA.prev->next = j->m_edgeA.next;
        }

        if (j->m_edgeA.next)
        {
                j->m_edgeA.next->prev = j->m_edgeA.prev;
        }

        if (&j->m_edgeA == bodyA->m_jointList)
        {
                bodyA->m_jointList = j->m_edgeA.next;
        }

        j->m_edgeA.prev = nullptr;
        j->m_edgeA.next = nullptr;

        // Remove from body 2
        if (j->m_edgeB.prev)
        {
                j->m_edgeB.prev->next = j->m_edgeB.next;
        }

        if (j->m_edgeB.next)
        {
                j->m_edgeB.next->prev = j->m_edgeB.prev;
        }

        if (&j->m_edgeB == bodyB->m_jointList)
        {
                bodyB->m_jointList = j->m_edgeB.next;
        }

        j->m_edgeB.prev = nullptr;
        j->m_edgeB.next = nullptr;

        b2Joint::Destroy(j, &m_blockAllocator);

        b2Assert(m_jointCount > 0);
        --m_jointCount;

        // If the joint prevents collisions, then flag any contacts for filtering.
        if (collideConnected == false)
        {
                b2ContactEdge* edge = bodyB->GetContactList();
                while (edge)
                {
                        if (edge->other == bodyA)
                        {
                                // Flag the contact for filtering at the next time step (where either
                                // body is awake).
                                edge->contact->FlagForFiltering();
                        }

                        edge = edge->next;
                }
        }
}

//
void b2World::SetAllowSleeping(bool flag)
{
        if (flag == m_allowSleep)
        {
                return;
        }

        m_allowSleep = flag;
        if (m_allowSleep == false)
        {
                for (b2Body* b = m_bodyList; b; b = b->m_next)
                {
                        b->SetAwake(true);
                }
        }
}

// Find islands, integrate and solve constraints, solve position constraints
void b2World::Solve(const b2TimeStep& step)
{
        m_profile.solveInit = 0.0f;
        m_profile.solveVelocity = 0.0f;
        m_profile.solvePosition = 0.0f;

        // Size the island for the worst case.
        b2Island island(m_bodyCount,
                                        m_contactManager.m_contactCount,
                                        m_jointCount,
                                        &m_stackAllocator,
                                        m_contactManager.m_contactListener);

        // Clear all the island flags.
        for (b2Body* b = m_bodyList; b; b = b->m_next)
        {
                b->m_flags &= ~b2Body::e_islandFlag;
        }
        for (b2Contact* c = m_contactManager.m_contactList; c; c = c->m_next)
        {
                c->m_flags &= ~b2Contact::e_islandFlag;
        }
        for (b2Joint* j = m_jointList; j; j = j->m_next)
        {
                j->m_islandFlag = false;
        }

        // Build and simulate all awake islands.
        int32 stackSize = m_bodyCount;
        b2Body** stack = (b2Body**)m_stackAllocator.Allocate(stackSize * sizeof(b2Body*));
        for (b2Body* seed = m_bodyList; seed; seed = seed->m_next)
        {
                if (seed->m_flags & b2Body::e_islandFlag)
                {
                        continue;
                }

                if (seed->IsAwake() == false || seed->IsEnabled() == false)
                {
                        continue;
                }

                // The seed can be dynamic or kinematic.
                if (seed->GetType() == b2_staticBody)
                {
                        continue;
                }

                // Reset island and stack.
                island.Clear();
                int32 stackCount = 0;
                stack[stackCount++] = seed;
                seed->m_flags |= b2Body::e_islandFlag;

                // Perform a depth first search (DFS) on the constraint graph.
                while (stackCount > 0)
                {
                        // Grab the next body off the stack and add it to the island.
                        b2Body* b = stack[--stackCount];
                        b2Assert(b->IsEnabled() == true);
                        island.Add(b);

                        // To keep islands as small as possible, we don't
                        // propagate islands across static bodies.
                        if (b->GetType() == b2_staticBody)
                        {
                                continue;
                        }

                        // Make sure the body is awake (without resetting sleep timer).
                        b->m_flags |= b2Body::e_awakeFlag;

                        // Search all contacts connected to this body.
                        for (b2ContactEdge* ce = b->m_contactList; ce; ce = ce->next)
                        {
                                b2Contact* contact = ce->contact;

                                // Has this contact already been added to an island?
                                if (contact->m_flags & b2Contact::e_islandFlag)
                                {
                                        continue;
                                }

                                // Is this contact solid and touching?
                                if (contact->IsEnabled() == false ||
                                        contact->IsTouching() == false)
                                {
                                        continue;
                                }

                                // Skip sensors.
                                bool sensorA = contact->m_fixtureA->m_isSensor;
                                bool sensorB = contact->m_fixtureB->m_isSensor;
                                if (sensorA || sensorB)
                                {
                                        continue;
                                }

                                island.Add(contact);
                                contact->m_flags |= b2Contact::e_islandFlag;

                                b2Body* other = ce->other;

                                // Was the other body already added to this island?
                                if (other->m_flags & b2Body::e_islandFlag)
                                {
                                        continue;
                                }

                                b2Assert(stackCount < stackSize);
                                stack[stackCount++] = other;
                                other->m_flags |= b2Body::e_islandFlag;
                        }

                        // Search all joints connect to this body.
                        for (b2JointEdge* je = b->m_jointList; je; je = je->next)
                        {
                                if (je->joint->m_islandFlag == true)
                                {
                                        continue;
                                }

                                b2Body* other = je->other;

                                // Don't simulate joints connected to diabled bodies.
                                if (other->IsEnabled() == false)
                                {
                                        continue;
                                }

                                island.Add(je->joint);
                                je->joint->m_islandFlag = true;

                                if (other->m_flags & b2Body::e_islandFlag)
                                {
                                        continue;
                                }

                                b2Assert(stackCount < stackSize);
                                stack[stackCount++] = other;
                                other->m_flags |= b2Body::e_islandFlag;
                        }
                }

                b2Profile profile;
                island.Solve(&profile, step, m_gravity, m_allowSleep);
                m_profile.solveInit += profile.solveInit;
                m_profile.solveVelocity += profile.solveVelocity;
                m_profile.solvePosition += profile.solvePosition;

                // Post solve cleanup.
                for (int32 i = 0; i < island.m_bodyCount; ++i)
                {
                        // Allow static bodies to participate in other islands.
                        b2Body* b = island.m_bodies[i];
                        if (b->GetType() == b2_staticBody)
                        {
                                b->m_flags &= ~b2Body::e_islandFlag;
                        }
                }
        }

        m_stackAllocator.Free(stack);

        {
                b2Timer timer;
                // Synchronize fixtures, check for out of range bodies.
                for (b2Body* b = m_bodyList; b; b = b->GetNext())
                {
                        // If a body was not in an island then it did not move.
                        if ((b->m_flags & b2Body::e_islandFlag) == 0)
                        {
                                continue;
                        }

                        if (b->GetType() == b2_staticBody)
                        {
                                continue;
                        }

                        // Update fixtures (for broad-phase).
                        b->SynchronizeFixtures();
                }

                // Look for new contacts.
                m_contactManager.FindNewContacts();
                m_profile.broadphase = timer.GetMilliseconds();
        }
}

// Find TOI contacts and solve them.
void b2World::SolveTOI(const b2TimeStep& step)
{
        b2Island island(2 * b2_maxTOIContacts, b2_maxTOIContacts, 0, &m_stackAllocator, m_contactManager.m_contactListener);

        if (m_stepComplete)
        {
                for (b2Body* b = m_bodyList; b; b = b->m_next)
                {
                        b->m_flags &= ~b2Body::e_islandFlag;
                        b->m_sweep.alpha0 = 0.0f;
                }

                for (b2Contact* c = m_contactManager.m_contactList; c; c = c->m_next)
                {
                        // Invalidate TOI
                        c->m_flags &= ~(b2Contact::e_toiFlag | b2Contact::e_islandFlag);
                        c->m_toiCount = 0;
                        c->m_toi = 1.0f;
                }
        }

        // Find TOI events and solve them.
        for (;;)
        {
                // Find the first TOI.
                b2Contact* minContact = nullptr;
                float minAlpha = 1.0f;

                for (b2Contact* c = m_contactManager.m_contactList; c; c = c->m_next)
                {
                        // Is this contact disabled?
                        if (c->IsEnabled() == false)
                        {
                                continue;
                        }

                        // Prevent excessive sub-stepping.
                        if (c->m_toiCount > b2_maxSubSteps)
                        {
                                continue;
                        }

                        float alpha = 1.0f;
                        if (c->m_flags & b2Contact::e_toiFlag)
                        {
                                // This contact has a valid cached TOI.
                                alpha = c->m_toi;
                        }
                        else
                        {
                                b2Fixture* fA = c->GetFixtureA();
                                b2Fixture* fB = c->GetFixtureB();

                                // Is there a sensor?
                                if (fA->IsSensor() || fB->IsSensor())
                                {
                                        continue;
                                }

                                b2Body* bA = fA->GetBody();
                                b2Body* bB = fB->GetBody();

                                b2BodyType typeA = bA->m_type;
                                b2BodyType typeB = bB->m_type;
                                b2Assert(typeA == b2_dynamicBody || typeB == b2_dynamicBody);

                                bool activeA = bA->IsAwake() && typeA != b2_staticBody;
                                bool activeB = bB->IsAwake() && typeB != b2_staticBody;

                                // Is at least one body active (awake and dynamic or kinematic)?
                                if (activeA == false && activeB == false)
                                {
                                        continue;
                                }

                                bool collideA = bA->IsBullet() || typeA != b2_dynamicBody;
                                bool collideB = bB->IsBullet() || typeB != b2_dynamicBody;

                                // Are these two non-bullet dynamic bodies?
                                if (collideA == false && collideB == false)
                                {
                                        continue;
                                }

                                // Compute the TOI for this contact.
                                // Put the sweeps onto the same time interval.
                                float alpha0 = bA->m_sweep.alpha0;

                                if (bA->m_sweep.alpha0 < bB->m_sweep.alpha0)
                                {
                                        alpha0 = bB->m_sweep.alpha0;
                                        bA->m_sweep.Advance(alpha0);
                                }
                                else if (bB->m_sweep.alpha0 < bA->m_sweep.alpha0)
                                {
                                        alpha0 = bA->m_sweep.alpha0;
                                        bB->m_sweep.Advance(alpha0);
                                }

                                b2Assert(alpha0 < 1.0f);

                                int32 indexA = c->GetChildIndexA();
                                int32 indexB = c->GetChildIndexB();

                                // Compute the time of impact in interval [0, minTOI]
                                b2TOIInput input;
                                input.proxyA.Set(fA->GetShape(), indexA);
                                input.proxyB.Set(fB->GetShape(), indexB);
                                input.sweepA = bA->m_sweep;
                                input.sweepB = bB->m_sweep;
                                input.tMax = 1.0f;

                                b2TOIOutput output;
                                b2TimeOfImpact(&output, &input);

                                // Beta is the fraction of the remaining portion of the .
                                float beta = output.t;
                                if (output.state == b2TOIOutput::e_touching)
                                {
                                        alpha = b2Min(alpha0 + (1.0f - alpha0) * beta, 1.0f);
                                }
                                else
                                {
                                        alpha = 1.0f;
                                }

                                c->m_toi = alpha;
                                c->m_flags |= b2Contact::e_toiFlag;
                        }

                        if (alpha < minAlpha)
                        {
                                // This is the minimum TOI found so far.
                                minContact = c;
                                minAlpha = alpha;
                        }
                }

                if (minContact == nullptr || 1.0f - 10.0f * b2_epsilon < minAlpha)
                {
                        // No more TOI events. Done!
                        m_stepComplete = true;
                        break;
                }

                // Advance the bodies to the TOI.
                b2Fixture* fA = minContact->GetFixtureA();
                b2Fixture* fB = minContact->GetFixtureB();
                b2Body* bA = fA->GetBody();
                b2Body* bB = fB->GetBody();

                b2Sweep backup1 = bA->m_sweep;
                b2Sweep backup2 = bB->m_sweep;

                bA->Advance(minAlpha);
                bB->Advance(minAlpha);

                // The TOI contact likely has some new contact points.
                minContact->Update(m_contactManager.m_contactListener);
                minContact->m_flags &= ~b2Contact::e_toiFlag;
                ++minContact->m_toiCount;

                // Is the contact solid?
                if (minContact->IsEnabled() == false || minContact->IsTouching() == false)
                {
                        // Restore the sweeps.
                        minContact->SetEnabled(false);
                        bA->m_sweep = backup1;
                        bB->m_sweep = backup2;
                        bA->SynchronizeTransform();
                        bB->SynchronizeTransform();
                        continue;
                }

                bA->SetAwake(true);
                bB->SetAwake(true);

                // Build the island
                island.Clear();
                island.Add(bA);
                island.Add(bB);
                island.Add(minContact);

                bA->m_flags |= b2Body::e_islandFlag;
                bB->m_flags |= b2Body::e_islandFlag;
                minContact->m_flags |= b2Contact::e_islandFlag;

                // Get contacts on bodyA and bodyB.
                b2Body* bodies[2] = {bA, bB};
                for (int32 i = 0; i < 2; ++i)
                {
                        b2Body* body = bodies[i];
                        if (body->m_type == b2_dynamicBody)
                        {
                                for (b2ContactEdge* ce = body->m_contactList; ce; ce = ce->next)
                                {
                                        if (island.m_bodyCount == island.m_bodyCapacity)
                                        {
                                                break;
                                        }

                                        if (island.m_contactCount == island.m_contactCapacity)
                                        {
                                                break;
                                        }

                                        b2Contact* contact = ce->contact;

                                        // Has this contact already been added to the island?
                                        if (contact->m_flags & b2Contact::e_islandFlag)
                                        {
                                                continue;
                                        }

                                        // Only add static, kinematic, or bullet bodies.
                                        b2Body* other = ce->other;
                                        if (other->m_type == b2_dynamicBody &&
                                                body->IsBullet() == false && other->IsBullet() == false)
                                        {
                                                continue;
                                        }

                                        // Skip sensors.
                                        bool sensorA = contact->m_fixtureA->m_isSensor;
                                        bool sensorB = contact->m_fixtureB->m_isSensor;
                                        if (sensorA || sensorB)
                                        {
                                                continue;
                                        }

                                        // Tentatively advance the body to the TOI.
                                        b2Sweep backup = other->m_sweep;
                                        if ((other->m_flags & b2Body::e_islandFlag) == 0)
                                        {
                                                other->Advance(minAlpha);
                                        }

                                        // Update the contact points
                                        contact->Update(m_contactManager.m_contactListener);

                                        // Was the contact disabled by the user?
                                        if (contact->IsEnabled() == false)
                                        {
                                                other->m_sweep = backup;
                                                other->SynchronizeTransform();
                                                continue;
                                        }

                                        // Are there contact points?
                                        if (contact->IsTouching() == false)
                                        {
                                                other->m_sweep = backup;
                                                other->SynchronizeTransform();
                                                continue;
                                        }

                                        // Add the contact to the island
                                        contact->m_flags |= b2Contact::e_islandFlag;
                                        island.Add(contact);

                                        // Has the other body already been added to the island?
                                        if (other->m_flags & b2Body::e_islandFlag)
                                        {
                                                continue;
                                        }
                                        
                                        // Add the other body to the island.
                                        other->m_flags |= b2Body::e_islandFlag;

                                        if (other->m_type != b2_staticBody)
                                        {
                                                other->SetAwake(true);
                                        }

                                        island.Add(other);
                                }
                        }
                }

                b2TimeStep subStep;
                subStep.dt = (1.0f - minAlpha) * step.dt;
                subStep.inv_dt = 1.0f / subStep.dt;
                subStep.dtRatio = 1.0f;
                subStep.positionIterations = 20;
                subStep.velocityIterations = step.velocityIterations;
                subStep.warmStarting = false;
                island.SolveTOI(subStep, bA->m_islandIndex, bB->m_islandIndex);

                // Reset island flags and synchronize broad-phase proxies.
                for (int32 i = 0; i < island.m_bodyCount; ++i)
                {
                        b2Body* body = island.m_bodies[i];
                        body->m_flags &= ~b2Body::e_islandFlag;

                        if (body->m_type != b2_dynamicBody)
                        {
                                continue;
                        }

                        body->SynchronizeFixtures();

                        // Invalidate all contact TOIs on this displaced body.
                        for (b2ContactEdge* ce = body->m_contactList; ce; ce = ce->next)
                        {
                                ce->contact->m_flags &= ~(b2Contact::e_toiFlag | b2Contact::e_islandFlag);
                        }
                }

                // Commit fixture proxy movements to the broad-phase so that new contacts are created.
                // Also, some contacts can be destroyed.
                m_contactManager.FindNewContacts();

                if (m_subStepping)
                {
                        m_stepComplete = false;
                        break;
                }
        }
}

void b2World::Step(float dt, int32 velocityIterations, int32 positionIterations)
{
        b2Timer stepTimer;

        // If new fixtures were added, we need to find the new contacts.
        if (m_newContacts)
        {
                m_contactManager.FindNewContacts();
                m_newContacts = false;
        }

        m_locked = true;

        b2TimeStep step;
        step.dt = dt;
        step.velocityIterations = velocityIterations;
        step.positionIterations = positionIterations;
        if (dt > 0.0f)
        {
                step.inv_dt = 1.0f / dt;
        }
        else
        {
                step.inv_dt = 0.0f;
        }

        step.dtRatio = m_inv_dt0 * dt;

        step.warmStarting = m_warmStarting;
        
        // Update contacts. This is where some contacts are destroyed.
        {
                b2Timer timer;
                m_contactManager.Collide();
                m_profile.collide = timer.GetMilliseconds();
        }

        // Integrate velocities, solve velocity constraints, and integrate positions.
        if (m_stepComplete && step.dt > 0.0f)
        {
                b2Timer timer;
                Solve(step);
                m_profile.solve = timer.GetMilliseconds();
        }

        // Handle TOI events.
        if (m_continuousPhysics && step.dt > 0.0f)
        {
                b2Timer timer;
                SolveTOI(step);
                m_profile.solveTOI = timer.GetMilliseconds();
        }

        if (step.dt > 0.0f)
        {
                m_inv_dt0 = step.inv_dt;
        }

        if (m_clearForces)
        {
                ClearForces();
        }

        m_locked = false;

        m_profile.step = stepTimer.GetMilliseconds();
}

void b2World::ClearForces()
{
        for (b2Body* body = m_bodyList; body; body = body->GetNext())
        {
                body->m_force.SetZero();
                body->m_torque = 0.0f;
        }
}

struct b2WorldQueryWrapper
{
        bool QueryCallback(int32 proxyId)
        {
                b2FixtureProxy* proxy = (b2FixtureProxy*)broadPhase->GetUserData(proxyId);
                return callback->ReportFixture(proxy->fixture);
        }

        const b2BroadPhase* broadPhase;
        b2QueryCallback* callback;
};

void b2World::QueryAABB(b2QueryCallback* callback, const b2AABB& aabb) const
{
        b2WorldQueryWrapper wrapper;
        wrapper.broadPhase = &m_contactManager.m_broadPhase;
        wrapper.callback = callback;
        m_contactManager.m_broadPhase.Query(&wrapper, aabb);
}

struct b2WorldRayCastWrapper
{
        float RayCastCallback(const b2RayCastInput& input, int32 proxyId)
        {
                void* userData = broadPhase->GetUserData(proxyId);
                b2FixtureProxy* proxy = (b2FixtureProxy*)userData;
                b2Fixture* fixture = proxy->fixture;
                int32 index = proxy->childIndex;
                b2RayCastOutput output;
                bool hit = fixture->RayCast(&output, input, index);

                if (hit)
                {
                        float fraction = output.fraction;
                        b2Vec2 point = (1.0f - fraction) * input.p1 + fraction * input.p2;
                        return callback->ReportFixture(fixture, point, output.normal, fraction);
                }

                return input.maxFraction;
        }

        const b2BroadPhase* broadPhase;
        b2RayCastCallback* callback;
};

void b2World::RayCast(b2RayCastCallback* callback, const b2Vec2& point1, const b2Vec2& point2) const
{
        b2WorldRayCastWrapper wrapper;
        wrapper.broadPhase = &m_contactManager.m_broadPhase;
        wrapper.callback = callback;
        b2RayCastInput input;
        input.maxFraction = 1.0f;
        input.p1 = point1;
        input.p2 = point2;
        m_contactManager.m_broadPhase.RayCast(&wrapper, input);
}

void b2World::DrawShape(b2Fixture* fixture, const b2Transform& xf, const b2Color& color)
{
        switch (fixture->GetType())
        {
        case b2Shape::e_circle:
                {
                        b2CircleShape* circle = (b2CircleShape*)fixture->GetShape();

                        b2Vec2 center = b2Mul(xf, circle->m_p);
                        float radius = circle->m_radius;
                        b2Vec2 axis = b2Mul(xf.q, b2Vec2(1.0f, 0.0f));

                        m_debugDraw->DrawSolidCircle(center, radius, axis, color);
                }
                break;

        case b2Shape::e_edge:
                {
                        b2EdgeShape* edge = (b2EdgeShape*)fixture->GetShape();
                        b2Vec2 v1 = b2Mul(xf, edge->m_vertex1);
                        b2Vec2 v2 = b2Mul(xf, edge->m_vertex2);
                        m_debugDraw->DrawSegment(v1, v2, color);

                        if (edge->m_oneSided == false)
                        {
                                m_debugDraw->DrawPoint(v1, 4.0f, color);
                                m_debugDraw->DrawPoint(v2, 4.0f, color);
                        }
                }
                break;

        case b2Shape::e_chain:
                {
                        b2ChainShape* chain = (b2ChainShape*)fixture->GetShape();
                        int32 count = chain->m_count;
                        const b2Vec2* vertices = chain->m_vertices;

                        b2Vec2 v1 = b2Mul(xf, vertices[0]);
                        for (int32 i = 1; i < count; ++i)
                        {
                                b2Vec2 v2 = b2Mul(xf, vertices[i]);
                                m_debugDraw->DrawSegment(v1, v2, color);
                                v1 = v2;
                        }
                }
                break;

        case b2Shape::e_polygon:
                {
                        b2PolygonShape* poly = (b2PolygonShape*)fixture->GetShape();
                        int32 vertexCount = poly->m_count;
                        b2Assert(vertexCount <= b2_maxPolygonVertices);
                        b2Vec2 vertices[b2_maxPolygonVertices];

                        for (int32 i = 0; i < vertexCount; ++i)
                        {
                                vertices[i] = b2Mul(xf, poly->m_vertices[i]);
                        }

                        m_debugDraw->DrawSolidPolygon(vertices, vertexCount, color);
                }
                break;

        default:
        break;
        }
}

void b2World::DebugDraw()
{
        if (m_debugDraw == nullptr)
        {
                return;
        }

        uint32 flags = m_debugDraw->GetFlags();

        if (flags & b2Draw::e_shapeBit)
        {
                for (b2Body* b = m_bodyList; b; b = b->GetNext())
                {
                        const b2Transform& xf = b->GetTransform();
                        for (b2Fixture* f = b->GetFixtureList(); f; f = f->GetNext())
                        {
                                if (b->GetType() == b2_dynamicBody && b->m_mass == 0.0f)
                                {
                                        // Bad body
                                        DrawShape(f, xf, b2Color(1.0f, 0.0f, 0.0f));
                                }
                                else if (b->IsEnabled() == false)
                                {
                                        DrawShape(f, xf, b2Color(0.5f, 0.5f, 0.3f));
                                }
                                else if (b->GetType() == b2_staticBody)
                                {
                                        DrawShape(f, xf, b2Color(0.5f, 0.9f, 0.5f));
                                }
                                else if (b->GetType() == b2_kinematicBody)
                                {
                                        DrawShape(f, xf, b2Color(0.5f, 0.5f, 0.9f));
                                }
                                else if (b->IsAwake() == false)
                                {
                                        DrawShape(f, xf, b2Color(0.6f, 0.6f, 0.6f));
                                }
                                else
                                {
                                        DrawShape(f, xf, b2Color(0.9f, 0.7f, 0.7f));
                                }
                        }
                }
        }

        if (flags & b2Draw::e_jointBit)
        {
                for (b2Joint* j = m_jointList; j; j = j->GetNext())
                {
                        j->Draw(m_debugDraw);
                }
        }

        if (flags & b2Draw::e_pairBit)
        {
                b2Color color(0.3f, 0.9f, 0.9f);
                for (b2Contact* c = m_contactManager.m_contactList; c; c = c->GetNext())
                {
                        b2Fixture* fixtureA = c->GetFixtureA();
                        b2Fixture* fixtureB = c->GetFixtureB();
                        int32 indexA = c->GetChildIndexA();
                        int32 indexB = c->GetChildIndexB();
                        b2Vec2 cA = fixtureA->GetAABB(indexA).GetCenter();
                        b2Vec2 cB = fixtureB->GetAABB(indexB).GetCenter();

                        m_debugDraw->DrawSegment(cA, cB, color);
                }
        }

        if (flags & b2Draw::e_aabbBit)
        {
                b2Color color(0.9f, 0.3f, 0.9f);
                b2BroadPhase* bp = &m_contactManager.m_broadPhase;

                for (b2Body* b = m_bodyList; b; b = b->GetNext())
                {
                        if (b->IsEnabled() == false)
                        {
                                continue;
                        }

                        for (b2Fixture* f = b->GetFixtureList(); f; f = f->GetNext())
                        {
                                for (int32 i = 0; i < f->m_proxyCount; ++i)
                                {
                                        b2FixtureProxy* proxy = f->m_proxies + i;
                                        b2AABB aabb = bp->GetFatAABB(proxy->proxyId);
                                        b2Vec2 vs[4];
                                        vs[0].Set(aabb.lowerBound.x, aabb.lowerBound.y);
                                        vs[1].Set(aabb.upperBound.x, aabb.lowerBound.y);
                                        vs[2].Set(aabb.upperBound.x, aabb.upperBound.y);
                                        vs[3].Set(aabb.lowerBound.x, aabb.upperBound.y);

                                        m_debugDraw->DrawPolygon(vs, 4, color);
                                }
                        }
                }
        }

        if (flags & b2Draw::e_centerOfMassBit)
        {
                for (b2Body* b = m_bodyList; b; b = b->GetNext())
                {
                        b2Transform xf = b->GetTransform();
                        xf.p = b->GetWorldCenter();
                        m_debugDraw->DrawTransform(xf);
                }
        }
}

int32 b2World::GetProxyCount() const
{
        return m_contactManager.m_broadPhase.GetProxyCount();
}

int32 b2World::GetTreeHeight() const
{
        return m_contactManager.m_broadPhase.GetTreeHeight();
}

int32 b2World::GetTreeBalance() const
{
        return m_contactManager.m_broadPhase.GetTreeBalance();
}

float b2World::GetTreeQuality() const
{
        return m_contactManager.m_broadPhase.GetTreeQuality();
}

void b2World::ShiftOrigin(const b2Vec2& newOrigin)
{
        b2Assert(m_locked == false);
        if (m_locked)
        {
                return;
        }

        for (b2Body* b = m_bodyList; b; b = b->m_next)
        {
                b->m_xf.p -= newOrigin;
                b->m_sweep.c0 -= newOrigin;
                b->m_sweep.c -= newOrigin;
        }

        for (b2Joint* j = m_jointList; j; j = j->m_next)
        {
                j->ShiftOrigin(newOrigin);
        }

        m_contactManager.m_broadPhase.ShiftOrigin(newOrigin);
}

void b2World::Dump()
{
        if (m_locked)
        {
                return;
        }

        b2OpenDump("box2d_dump.inl");

        b2Dump("b2Vec2 g(%.9g, %.9g);\n", m_gravity.x, m_gravity.y);
        b2Dump("m_world->SetGravity(g);\n");

        b2Dump("b2Body** bodies = (b2Body**)b2Alloc(%d * sizeof(b2Body*));\n", m_bodyCount);
        b2Dump("b2Joint** joints = (b2Joint**)b2Alloc(%d * sizeof(b2Joint*));\n", m_jointCount);

        int32 i = 0;
        for (b2Body* b = m_bodyList; b; b = b->m_next)
        {
                b->m_islandIndex = i;
                b->Dump();
                ++i;
        }

        i = 0;
        for (b2Joint* j = m_jointList; j; j = j->m_next)
        {
                j->m_index = i;
                ++i;
        }

        // First pass on joints, skip gear joints.
        for (b2Joint* j = m_jointList; j; j = j->m_next)
        {
                if (j->m_type == e_gearJoint)
                {
                        continue;
                }

                b2Dump("{\n");
                j->Dump();
                b2Dump("}\n");
        }

        // Second pass on joints, only gear joints.
        for (b2Joint* j = m_jointList; j; j = j->m_next)
        {
                if (j->m_type != e_gearJoint)
                {
                        continue;
                }

                b2Dump("{\n");
                j->Dump();
                b2Dump("}\n");
        }

        b2Dump("b2Free(joints);\n");
        b2Dump("b2Free(bodies);\n");
        b2Dump("joints = nullptr;\n");
        b2Dump("bodies = nullptr;\n");

        b2CloseDump();
}