예제 #1
0
        // Find islands, integrate and solveraints, solve positionraints
        public void Solve(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 = new b2Island(m_bodyCount,
                                           m_contactManager.ContactCount,
                                           m_jointCount,
                                           m_contactManager.ContactListener);

            // Clear all the island flags.
            for (b2Body b = m_bodyList; b != null; b = b.Next)
            {
                b.BodyFlags &= ~b2BodyFlags.e_islandFlag;
            }
            for (b2Contact c = m_contactManager.ContactList; c != null; c = c.Next)
            {
                c.ContactFlags &= ~b2ContactFlags.e_islandFlag;
            }
            for (b2Joint j = m_jointList; j; j = j.Next)
            {
                j.m_islandFlag = false;
            }

            // Build and simulate all awake islands.
            int stackSize = m_bodyCount;

            b2Body[] stack = new b2Body[stackSize];
            for (b2Body seed = m_bodyList; seed != null; seed = seed.Next)
            {
                if (seed.BodyFlags & b2BodyFlags.e_islandFlag)
                {
                    continue;
                }

                if (seed.IsAwake() == false || seed.IsActive() == false)
                {
                    continue;
                }

                // The seed can be dynamic or kinematic.
                if (seed.BodyType == b2BodyType.b2_staticBody)
                {
                    continue;
                }

                // Reset island and stack.
                island.Clear();
                int stackCount = 0;
                stack[stackCount++] = seed;
                seed.BodyFlags     |= b2BodyFlags.e_islandFlag;

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

                    // Make sure the body is awake.
                    b.SetAwake(true);

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

                    // Search all contacts connected to this body.
                    for (b2ContactEdge ce = b.ContactList; ce != null; ce = ce.next)
                    {
                        b2Contact contact = ce.contact;

                        // Has this contact already been added to an island?
                        if (contact.ContactFlags & b2ContactFlags.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.ContactFlags |= b2ContactType.e_islandFlag;

                        b2Body other = ce.other;

                        // Was the other body already added to this island?
                        if ((other.BodyFlags & b2BodyFlags.e_islandFlag) > 0)
                        {
                            continue;
                        }

                        stack[stackCount++] = other;
                        other.BodyFlags    |= b2BodyFlags.e_islandFlag;
                    }

                    // Search all joints connect to this body.
                    for (b2JointEdge je = b.JointList; je; je = je.next)
                    {
                        if (je.joint.IslandFlag == true)
                        {
                            continue;
                        }

                        b2Body other = je.other;

                        // Don't simulate joints connected to inactive bodies.
                        if (other.IsActive() == false)
                        {
                            continue;
                        }

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

                        if ((other.BodyFlags & b2BodyFlags.e_islandFlag) > 0)
                        {
                            continue;
                        }

                        stack[stackCount++] = other;
                        other.BodyFlags    |= b2BodyFlags.e_islandFlag;
                    }
                }

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

                // Post solve cleanup.
                for (int i = 0; i < island.m_bodyCount; ++i)
                {
                    // Allow static bodies to participate in other islands.
                    b2Body b = island.m_bodies[i];
                    if (b.BodyType == b2BodyType.b2_staticBody)
                    {
                        b.BodyFlags &= ~b2BodyFlags.e_islandFlag;
                    }
                }
            }

            {
                b2Timer timer;
                // Synchronize fixtures, check for out of range bodies.
                for (b2Body b = m_bodyList; b != null; b = b.Next)
                {
                    // If a body was not in an island then it did not move.
                    if ((b.BodyFlags & b2BodyType.e_islandFlag) == 0)
                    {
                        continue;
                    }

                    if (b.GetBodyType() == b2BodyType.b2_staticBody)
                    {
                        continue;
                    }

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

                // Look for new contacts.
                m_contactManager.FindNewContacts();
                m_profile.broadphase = timer.GetMilliseconds();
            }
        }
예제 #2
0
        // Find TOI contacts and solve them.
        public void SolveTOI(b2TimeStep step)
        {
            b2Island island = new b2Island(2 * b2Settings.b2_maxTOIContacts, b2Settings.b2_maxTOIContacts, 0, m_contactManager.ContactListener);

            if (m_stepComplete)
            {
                for (b2Body b = m_bodyList; b; b = b.Next)
                {
                    b.BodyFlags     &= ~b2Body.e_islandFlag;
                    b.m_sweep.alpha0 = 0.0f;
                }

                for (b2Contact c = m_contactManager.ContactList; c; c = c.Next)
                {
                    // Invalidate TOI
                    c.ContactFlags &= ~(b2ContactType.e_toiFlag | b2ContactType.e_islandFlag);
                    c.m_toiCount    = 0;
                    c.m_toi         = 1.0f;
                }
            }

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

                for (b2Contact c = m_contactManager.ContactList; c != null; c = c.Next)
                {
                    // Is this contact disabled?
                    if (c.IsEnabled() == false)
                    {
                        continue;
                    }

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

                    float alpha = 1.0f;
                    if (c.ContactFlags.HasFlag(b2ContactFlags.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.Body;
                        b2Body bB = fB.Body;

                        b2BodyType typeA = bA.BodyType;
                        b2BodyType typeB = bB.BodyType;

                        bool activeA = bA.IsAwake() && typeA != b2BodyType.b2_staticBody;
                        bool activeB = bB.IsAwake() && typeB != b2BodyType.b2_staticBody;

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

                        bool collideA = bA.IsBullet() || typeA != b2BodyType.b2_dynamicBody;
                        bool collideB = bB.IsBullet() || typeB != b2BodyType.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.Sweep.alpha0;

                        if (bA.Sweep.alpha0 < bB.Sweep.alpha0)
                        {
                            alpha0 = bB.Sweep.alpha0;
                            bA.Sweep.Advance(alpha0);
                        }
                        else if (bB.Sweep.alpha0 < bA.Sweep.alpha0)
                        {
                            alpha0 = bA.Sweep.alpha0;
                            bB.Sweep.Advance(alpha0);
                        }

                        int indexA = c.GetChildIndexA();
                        int indexB = c.GetChildIndexB();

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

                        b2TOIOutput output = b2TimeOfImpact(input);

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

                        c.m_toi         = alpha;
                        c.ContactFlags |= b2ContactFlags.e_toiFlag;
                    }

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

                if (minContact == null || 1.0f - 10.0f * b2Settings.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.Body;
                    b2Body    bB = fB.Body;

                    b2Sweep backup1 = bA.Sweep;
                    b2Sweep backup2 = bB.Sweep;

                    bA.Advance(minAlpha);
                    bB.Advance(minAlpha);

                    // The TOI contact likely has some new contact points.
                    minContact.Update(m_contactManager.ContactListener);
                    minContact.ContactFlags &= ~b2ContactFlags.e_toiFlag;
                    ++minContact.m_toiCount;

                    // Is the contact solid?
                    if (minContact.IsEnabled() == false || minContact.IsTouching() == false)
                    {
                        // Restore the sweeps.
                        minContact.SetEnabled(false);
                        bA.Sweep = backup1;
                        bB.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.BodyFlags           |= b2BodyFlags.e_islandFlag;
                    bB.BodyFlags           |= b2BodyFlags.e_islandFlag;
                    minContact.ContentType |= b2ContactFlags.e_islandFlag;

                    // Get contacts on bodyA and bodyB.
                    b2Body[] bodies = new b2Body[] { bA, bB };
                    for (int i = 0; i < 2; ++i)
                    {
                        b2Body body = bodies[i];
                        if (body.BodyType == b2BodyType.b2_dynamicBody)
                        {
                            for (b2ContactEdge ce = body.ContactList; ce != null; ce = ce.next)
                            {
                                if (island.BodyCount == island.BodyCapacity)
                                {
                                    break;
                                }

                                if (island.ContactCount == island.ContactCapacity)
                                {
                                    break;
                                }

                                b2Contact contact = ce.contact;

                                // Has this contact already been added to the island?
                                if (contact.ContactType & b2ContactType.e_islandFlag)
                                {
                                    continue;
                                }

                                // Only add static, kinematic, or bullet bodies.
                                b2Body other = ce.other;
                                if (other.BodyType == b2BodyType.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.Sweep;
                                if (other.BodyFlags.HasFlag(b2BodyFlags.e_islandFlag))
                                {
                                    other.Advance(minAlpha);
                                }

                                // Update the contact points
                                contact.Update(m_contactManager.ContactListener);

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

                                // Are there contact points?
                                if (contact.IsTouching() == false)
                                {
                                    other.Sweep = backup;
                                    other.SynchronizeTransform();
                                    continue;
                                }

                                // Add the contact to the island
                                contact.ContactFlags |= b2ContactFlags.e_islandFlag;
                                island.Add(contact);

                                // Has the other body already been added to the island?
                                if (other.BodyFlags.HasFlag(b2BodyFlags.e_islandFlag))
                                {
                                    continue;
                                }

                                // Add the other body to the island.
                                other.BodyFlags |= b2BodyFlags.e_islandFlag;

                                if (other.BodyType != b2BodyType.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 (int i = 0; i < island.m_bodyCount; ++i)
                    {
                        b2Body body = island.m_bodies[i];
                        body.BodyFlags &= ~b2BodyFlags.e_islandFlag;

                        if (body.BodyType != b2BodyType.b2_dynamicBody)
                        {
                            continue;
                        }

                        body.SynchronizeFixtures();

                        // Invalidate all contact TOIs on this displaced body.
                        for (b2ContactEdge ce = body.ContactList; ce != null; ce = ce.next)
                        {
                            ce.Contact.ContactFlags &= ~(b2ContactFlags.e_toiFlag | b2ContactFlags.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;
                    }
                }
            }
        }
예제 #3
0
        public void SolveTOI(b2TimeStep subStep, int toiIndexA, int toiIndexB)
        {
            Debug.Assert(toiIndexA < m_bodyCount);
            Debug.Assert(toiIndexB < m_bodyCount);

            // Initialize the body state.
            for (int i = 0; i < m_bodyCount; ++i)
            {
                b2Body b = m_bodies[i];
                m_positions[i].c = b.Sweep.c;
                m_positions[i].a = b.Sweep.a;
                m_velocities[i].v = b.LinearVelocity;
                m_velocities[i].w = b.AngularVelocity;
            }

            b2ContactSolverDef contactSolverDef;
            contactSolverDef.contacts = m_contacts;
            contactSolverDef.count = m_contactCount;
            contactSolverDef.step = subStep;
            contactSolverDef.positions = m_positions;
            contactSolverDef.velocities = m_velocities;
            b2ContactSolver contactSolver = new b2ContactSolver(contactSolverDef);

            // Solve position constraints.
            for (int i = 0; i < subStep.positionIterations; ++i)
            {
                bool contactsOkay = contactSolver.SolveTOIPositionConstraints(toiIndexA, toiIndexB);
                if (contactsOkay)
                {
                    break;
                }
            }

            #if false
            // Is the new position really safe?
            for (int i = 0; i < m_contactCount; ++i)
            {
            b2Contact c = m_contacts[i];
            b2Fixture fA = c.GetFixtureA();
            b2Fixture fB = c.GetFixtureB();

            b2Body bA = fA.Body;
            b2Body bB = fB.Body;

            int indexA = c.GetChildIndexA();
            int indexB = c.GetChildIndexB();

            b2DistanceInput input = new b2DistanceInput();
            input.proxyA.Set(fA.Shape, indexA);
            input.proxyB.Set(fB.Shape, indexB);
            input.transformA = bA.Transform;
            input.transformB = bB.Transform;
            input.useRadii = false;

            b2DistanceOutput output;
            b2SimplexCache cache = new b2SimplexCache();
            cache.count = 0;
            output = b2Distance(cache, input);

            if (output.distance == 0 || cache.count == 3)
            {
            cache.count += 0;
            }
            }
            #endif

            // Leap of faith to new safe state.
            m_bodies[toiIndexA].Sweep.c0 = m_positions[toiIndexA].c;
            m_bodies[toiIndexA].Sweep.a0 = m_positions[toiIndexA].a;
            m_bodies[toiIndexB].Sweep.c0 = m_positions[toiIndexB].c;
            m_bodies[toiIndexB].Sweep.a0 = m_positions[toiIndexB].a;

            // No warm starting is needed for TOI events because warm
            // starting impulses were applied in the discrete solver.
            contactSolver.InitializeVelocityConstraints();

            // Solve velocity constraints.
            for (int i = 0; i < subStep.velocityIterations; ++i)
            {
                contactSolver.SolveVelocityConstraints();
            }

            // Don't store the TOI contact forces for warm starting
            // because they can be quite large.

            float h = subStep.dt;

            // Integrate positions
            for (int i = 0; i < m_bodyCount; ++i)
            {
                b2Vec2 c = m_positions[i].c;
                float a = m_positions[i].a;
                b2Vec2 v = m_velocities[i].v;
                float w = m_velocities[i].w;

                // Check for large velocities
                b2Vec2 translation = h * v;
                if (b2Math.b2Dot(translation, translation) > b2Settings.b2_maxTranslationSquared)
                {
                    float ratio = b2Settings.b2_maxTranslation / translation.Length();
                    v *= ratio;
                }

                float rotation = h * w;
                if (rotation * rotation > b2Settings.b2_maxRotationSquared)
                {
                    float ratio = b2Settings.b2_maxRotation / Math.Abs(rotation);
                    w *= ratio;
                }

                // Integrate
                c += h * v;
                a += h * w;

                m_positions[i].c = c;
                m_positions[i].a = a;
                m_velocities[i].v = v;
                m_velocities[i].w = w;

                // Sync bodies
                b2Body body = m_bodies[i];
                body.Sweep.c = c;
                body.Sweep.a = a;
                body.LinearVelocity = v;
                body.AngularVelocity = w;
                body.SynchronizeTransform();
            }

            Report(contactSolver.m_velocityConstraints);
        }
예제 #4
0
 private void SolveTOI(b2TimeStep step);
예제 #5
0
        public void SolveTOI(b2TimeStep subStep)
        {
            int i;
            int j;

            m_contactSolver.Initialize(subStep, m_contacts, m_contactCount, m_allocator);
            b2ContactSolver contactSolver = m_contactSolver;

            // No warm starting is needed for TOI events because warm
            // starting impulses were applied in the discrete solver.

            // Warm starting for joints is off for now, but we need to
            // call this function to compute Jacobians.
            for (i = 0; i < m_jointCount; ++i)
            {
                m_joints[i].InitVelocityConstraints(subStep);
            }


            // Solve velocity constraints.
            for (i = 0; i < subStep.velocityIterations; ++i)
            {
                contactSolver.SolveVelocityConstraints();
                for (j = 0; j < m_jointCount; ++j)
                {
                    m_joints[j].SolveVelocityConstraints(subStep);
                }
            }

            // Don't store the TOI contact forces for warm starting
            // because they can be quite large.

            // Integrate positions.
            for (i = 0; i < m_bodyCount; ++i)
            {
                b2Body b = m_bodies[i];

                if (b.GetType() == b2Body.b2_staticBody)
                {
                    continue;
                }

                // Check for large velocities.
                // b2Vec2 translation = subStep.dt * b.m_linearVelocity;
                float translationX = subStep.dt * b.m_linearVelocity.x;
                float translationY = subStep.dt * b.m_linearVelocity.y;
                //if (b2Dot(translation, translation) > b2_maxTranslationSquared)
                if ((translationX * translationX + translationY * translationY) > b2Settings.b2_maxTranslationSquared)
                {
                    b.m_linearVelocity.Normalize();
                    b.m_linearVelocity.x *= b2Settings.b2_maxTranslation * subStep.inv_dt;
                    b.m_linearVelocity.y *= b2Settings.b2_maxTranslation * subStep.inv_dt;
                }

                float rotation = subStep.dt * b.m_angularVelocity;
                if (rotation * rotation > b2Settings.b2_maxRotationSquared)
                {
                    if (b.m_angularVelocity < 0.0f)
                    {
                        b.m_angularVelocity = -b2Settings.b2_maxRotation * subStep.inv_dt;
                    }
                    else
                    {
                        b.m_angularVelocity = b2Settings.b2_maxRotation * subStep.inv_dt;
                    }
                }

                // Store positions for continuous collision.
                b.m_sweep.c0.SetV(b.m_sweep.c);
                b.m_sweep.a0 = b.m_sweep.a;

                // Integrate
                b.m_sweep.c.x += subStep.dt * b.m_linearVelocity.x;
                b.m_sweep.c.y += subStep.dt * b.m_linearVelocity.y;
                b.m_sweep.a   += subStep.dt * b.m_angularVelocity;

                // Compute new transform
                b.SynchronizeTransform();

                // Note: shapes are synchronized later.
            }

            // Solve position constraints.
            float k_toiBaumgarte = 0.75f;

            for (i = 0; i < subStep.positionIterations; ++i)
            {
                bool contactsOkay = contactSolver.SolvePositionConstraints(k_toiBaumgarte);
                bool jointsOkay   = true;
                for (j = 0; j < m_jointCount; ++j)
                {
                    bool jointOkay = m_joints[j].SolvePositionConstraints(b2Settings.b2_contactBaumgarte);
                    jointsOkay = jointsOkay && jointOkay;
                }

                if (contactsOkay && jointsOkay)
                {
                    break;
                }
            }
            Report(contactSolver.m_constraints);
        }
예제 #6
0
        public void Solve(ref b2Profile profile, b2TimeStep step, b2Vec2 gravity, bool allowSleep)
        {
            b2Timer timer = new b2Timer();

            float h = step.dt;

            // Integrate velocities and apply damping. Initialize the body state.
            for (int i = 0; i < m_bodyCount; ++i)
            {
                b2Body b = m_bodies[i];

                b2Vec2 c = b.Sweep.c;
                float a = b.Sweep.a;
                b2Vec2 v = b.LinearVelocity;
                float w = b.AngularVelocity;

                // Store positions for continuous collision.
                b.Sweep.c0 = b.Sweep.c;
                b.Sweep.a0 = b.Sweep.a;

                if (b.BodyType == b2BodyType.b2_dynamicBody)
                {
                    // Integrate velocities.
                    v += h * (b.GravityScale * gravity + b.InvertedMass * b.Force);
                    w += h * b.InvertedI * b.Torque;

                    // Apply damping.
                    // ODE: dv/dt + c * v = 0
                    // Solution: v(t) = v0 * exp(-c * t)
                    // Time step: v(t + dt) = v0 * exp(-c * (t + dt)) = v0 * exp(-c * t) * exp(-c * dt) = v * exp(-c * dt)
                    // v2 = exp(-c * dt) * v1
                    // Taylor expansion:
                    // v2 = (1.0f - c * dt) * v1
                    v *= b2Math.b2Clamp(1.0f - h * b.LinearDamping, 0.0f, 1.0f);
                    w *= b2Math.b2Clamp(1.0f - h * b.AngularDamping, 0.0f, 1.0f);
                }

                m_positions[i].c = c;
                m_positions[i].a = a;
                m_velocities[i].v = v;
                m_velocities[i].w = w;
            }

            timer.Reset();

            // Solver data
            b2SolverData solverData = new b2SolverData();
            solverData.step = step;
            solverData.positions = m_positions;
            solverData.velocities = m_velocities;

            // Initialize velocity constraints.
            b2ContactSolverDef contactSolverDef;
            contactSolverDef.step = step;
            contactSolverDef.contacts = m_contacts;
            contactSolverDef.count = m_contactCount;
            contactSolverDef.positions = m_positions;
            contactSolverDef.velocities = m_velocities;

            b2ContactSolver contactSolver = new b2ContactSolver(contactSolverDef);
            contactSolver.InitializeVelocityConstraints();

            if (step.warmStarting)
            {
                contactSolver.WarmStart();
            }

            for (int i = 0; i < m_jointCount; ++i)
            {
                m_joints[i].InitVelocityConstraints(solverData);
            }

            profile.solveInit = timer.GetMilliseconds();

            // Solve velocity constraints
            timer.Reset();
            for (int i = 0; i < step.velocityIterations; ++i)
            {
                for (int j = 0; j < m_jointCount; ++j)
                {
                    m_joints[j].SolveVelocityConstraints(solverData);
                }

                contactSolver.SolveVelocityConstraints();
            }

            // Store impulses for warm starting
            contactSolver.StoreImpulses();
            profile.solveVelocity = timer.GetMilliseconds();

            // Integrate positions
            for (int i = 0; i < m_bodyCount; ++i)
            {
                b2Vec2 c = m_positions[i].c;
                float a = m_positions[i].a;
                b2Vec2 v = m_velocities[i].v;
                float w = m_velocities[i].w;

                // Check for large velocities
                b2Vec2 translation = h * v;
                if (b2Math.b2Dot(translation, translation) > b2Settings.b2_maxTranslationSquared)
                {
                    float ratio = b2Settings.b2_maxTranslation / translation.Length();
                    v *= ratio;
                }

                float rotation = h * w;
                if (rotation * rotation > b2Settings.b2_maxRotationSquared)
                {
                    float ratio = b2Settings.b2_maxRotation / Math.Abs(rotation);
                    w *= ratio;
                }

                // Integrate
                c += h * v;
                a += h * w;

                m_positions[i].c = c;
                m_positions[i].a = a;
                m_velocities[i].v = v;
                m_velocities[i].w = w;
            }

            // Solve position constraints
            timer.Reset();
            bool positionSolved = false;
            for (int i = 0; i < step.positionIterations; ++i)
            {
                bool contactsOkay = contactSolver.SolvePositionConstraints();

                bool jointsOkay = true;
                for (int i2 = 0; i2 < m_jointCount; ++i2)
                {
                    bool jointOkay = m_joints[i2].SolvePositionConstraints(solverData);
                    jointsOkay = jointsOkay && jointOkay;
                }

                if (contactsOkay && jointsOkay)
                {
                    // Exit early if the position errors are small.
                    positionSolved = true;
                    break;
                }
            }

            // Copy state buffers back to the bodies
            for (int i = 0; i < m_bodyCount; ++i)
            {
                b2Body body = m_bodies[i];
                body.Sweep.c = m_positions[i].c;
                body.Sweep.a = m_positions[i].a;
                body.LinearVelocity = m_velocities[i].v;
                body.AngularVelocity = m_velocities[i].w;
                body.SynchronizeTransform();
            }

            profile.solvePosition = timer.GetMilliseconds();

            Report(contactSolver.m_velocityConstraints);

            if (allowSleep)
            {
                float minSleepTime = b2Settings.b2_maxFloat;

                float linTolSqr = b2Settings.b2_linearSleepTolerance * b2Settings.b2_linearSleepTolerance;
                float angTolSqr = b2Settings.b2_angularSleepTolerance * b2Settings.b2_angularSleepTolerance;

                for (int i = 0; i < m_bodyCount; ++i)
                {
                    b2Body b = m_bodies[i];
                    if (b.BodyType == b2BodyType.b2_staticBody)
                    {
                        continue;
                    }

                    if (!(b.BodyFlags.HasFlag(b2BodyFlags.e_autoSleepFlag)) ||
                        b.AngularVelocity * b.AngularVelocity > angTolSqr ||
                        b2Math.b2Dot(b.LinearVelocity, b.LinearVelocity) > linTolSqr)
                    {
                        b.SleepTime = 0.0f;
                        minSleepTime = 0.0f;
                    }
                    else
                    {
                        b.SleepTime += h;
                        minSleepTime = Math.Min(minSleepTime, b.SleepTime);
                    }
                }

                if (minSleepTime >= b2Settings.b2_timeToSleep && positionSolved)
                {
                    for (int i = 0; i < m_bodyCount; ++i)
                    {
                        b2Body b = m_bodies[i];
                        b.SetAwake(false);
                    }
                }
            }
        }
예제 #7
0
        public void SolveTOI(b2TimeStep subStep, int toiIndexA, int toiIndexB)
        {
            Debug.Assert(toiIndexA < m_bodyCount);
            Debug.Assert(toiIndexB < m_bodyCount);

            // Initialize the body state.
            for (int i = 0; i < m_bodyCount; ++i)
            {
                b2Body b = m_bodies[i];
                m_positions[i].c  = b.Sweep.c;
                m_positions[i].a  = b.Sweep.a;
                m_velocities[i].v = b.LinearVelocity;
                m_velocities[i].w = b.AngularVelocity;
            }

            b2ContactSolverDef contactSolverDef;

            contactSolverDef.contacts   = m_contacts;
            contactSolverDef.count      = m_contactCount;
            contactSolverDef.step       = subStep;
            contactSolverDef.positions  = m_positions;
            contactSolverDef.velocities = m_velocities;
            b2ContactSolver contactSolver = new b2ContactSolver(contactSolverDef);

            // Solve position constraints.
            for (int i = 0; i < subStep.positionIterations; ++i)
            {
                bool contactsOkay = contactSolver.SolveTOIPositionConstraints(toiIndexA, toiIndexB);
                if (contactsOkay)
                {
                    break;
                }
            }

#if false
            // Is the new position really safe?
            for (int i = 0; i < m_contactCount; ++i)
            {
                b2Contact c  = m_contacts[i];
                b2Fixture fA = c.GetFixtureA();
                b2Fixture fB = c.GetFixtureB();

                b2Body bA = fA.Body;
                b2Body bB = fB.Body;

                int indexA = c.GetChildIndexA();
                int indexB = c.GetChildIndexB();

                b2DistanceInput input = new b2DistanceInput();
                input.proxyA.Set(fA.Shape, indexA);
                input.proxyB.Set(fB.Shape, indexB);
                input.transformA = bA.Transform;
                input.transformB = bB.Transform;
                input.useRadii   = false;

                b2DistanceOutput output;
                b2SimplexCache   cache = new b2SimplexCache();
                cache.count = 0;
                output      = b2Distance(cache, input);

                if (output.distance == 0 || cache.count == 3)
                {
                    cache.count += 0;
                }
            }
#endif

            // Leap of faith to new safe state.
            m_bodies[toiIndexA].Sweep.c0 = m_positions[toiIndexA].c;
            m_bodies[toiIndexA].Sweep.a0 = m_positions[toiIndexA].a;
            m_bodies[toiIndexB].Sweep.c0 = m_positions[toiIndexB].c;
            m_bodies[toiIndexB].Sweep.a0 = m_positions[toiIndexB].a;

            // No warm starting is needed for TOI events because warm
            // starting impulses were applied in the discrete solver.
            contactSolver.InitializeVelocityConstraints();

            // Solve velocity constraints.
            for (int i = 0; i < subStep.velocityIterations; ++i)
            {
                contactSolver.SolveVelocityConstraints();
            }

            // Don't store the TOI contact forces for warm starting
            // because they can be quite large.

            float h = subStep.dt;

            // Integrate positions
            for (int i = 0; i < m_bodyCount; ++i)
            {
                b2Vec2 c = m_positions[i].c;
                float  a = m_positions[i].a;
                b2Vec2 v = m_velocities[i].v;
                float  w = m_velocities[i].w;

                // Check for large velocities
                b2Vec2 translation = h * v;
                if (b2Math.b2Dot(translation, translation) > b2Settings.b2_maxTranslationSquared)
                {
                    float ratio = b2Settings.b2_maxTranslation / translation.Length;
                    v *= ratio;
                }

                float rotation = h * w;
                if (rotation * rotation > b2Settings.b2_maxRotationSquared)
                {
                    float ratio = b2Settings.b2_maxRotation / Math.Abs(rotation);
                    w *= ratio;
                }

                // Integrate
                c += h * v;
                a += h * w;

                m_positions[i].c  = c;
                m_positions[i].a  = a;
                m_velocities[i].v = v;
                m_velocities[i].w = w;

                // Sync bodies
                b2Body body = m_bodies[i];
                body.Sweep.c         = c;
                body.Sweep.a         = a;
                body.LinearVelocity  = v;
                body.AngularVelocity = w;
                body.SynchronizeTransform();
            }

            Report(contactSolver.m_velocityConstraints);
        }
예제 #8
0
        public void Solve(b2TimeStep step, b2Vec2 gravity, bool allowSleep)
        {
            int     i;
            int     j;
            b2Body  b;
            b2Joint joint;

            // Integrate velocities and apply damping.
            for (i = 0; i < m_bodyCount; ++i)
            {
                b = m_bodies[i];

                if (b.GetType() != b2Body.b2_dynamicBody)
                {
                    continue;
                }
                //=====================add by kingBook 2015/10/26 17:25==============
                b2Vec2 l_gravity;
                if (b.m_customGravity != null)
                {
                    l_gravity = b.m_customGravity;
                }
                else
                {
                    l_gravity = gravity;
                }
                //==============================added================================
                // Integrate velocities.
                //b.m_linearVelocity += step.dt * (gravity + b.m_invMass * b.m_force);
                b.m_linearVelocity.x += step.dt * (l_gravity.x + b.m_invMass * b.m_force.x);
                b.m_linearVelocity.y += step.dt * (l_gravity.y + b.m_invMass * b.m_force.y);
                b.m_angularVelocity  += step.dt * b.m_invI * b.m_torque;

                // Apply damping.
                // ODE: dv/dt + c * v = 0
                // Solution: v(t) = v0 * exp(-c * t)
                // Time step: v(t + dt) = v0 * exp(-c * (t + dt)) = v0 * exp(-c * t) * exp(-c * dt) = v * exp(-c * dt)
                // v2 = exp(-c * dt) * v1
                // Taylor expansion:
                // v2 = (1.0f - c * dt) * v1
                b.m_linearVelocity.Multiply(b2Math.Clamp(1.0f - step.dt * b.m_linearDamping, 0.0f, 1.0f));
                b.m_angularVelocity *= b2Math.Clamp(1.0f - step.dt * b.m_angularDamping, 0.0f, 1.0f);
            }

            m_contactSolver.Initialize(step, m_contacts, m_contactCount, m_allocator);
            b2ContactSolver contactSolver = m_contactSolver;

            // Initialize velocity constraints.
            contactSolver.InitVelocityConstraints(step);

            for (i = 0; i < m_jointCount; ++i)
            {
                joint = m_joints[i];
                joint.InitVelocityConstraints(step);
            }

            // Solve velocity constraints.
            for (i = 0; i < step.velocityIterations; ++i)
            {
                for (j = 0; j < m_jointCount; ++j)
                {
                    joint = m_joints[j];
                    joint.SolveVelocityConstraints(step);
                }

                contactSolver.SolveVelocityConstraints();
            }

            // Post-solve (store impulses for warm starting).
            for (i = 0; i < m_jointCount; ++i)
            {
                joint = m_joints[i];
                joint.FinalizeVelocityConstraints();
            }
            contactSolver.FinalizeVelocityConstraints();

            // Integrate positions.
            for (i = 0; i < m_bodyCount; ++i)
            {
                b = m_bodies[i];

                if (b.GetType() == b2Body.b2_staticBody)
                {
                    continue;
                }

                // Check for large velocities.
                // b2Vec2 translation = step.dt * b.m_linearVelocity;
                float translationX = step.dt * b.m_linearVelocity.x;
                float translationY = step.dt * b.m_linearVelocity.y;
                //if (b2Dot(translation, translation) > b2_maxTranslationSquared)
                if ((translationX * translationX + translationY * translationY) > b2Settings.b2_maxTranslationSquared)
                {
                    b.m_linearVelocity.Normalize();
                    b.m_linearVelocity.x *= b2Settings.b2_maxTranslation * step.inv_dt;
                    b.m_linearVelocity.y *= b2Settings.b2_maxTranslation * step.inv_dt;
                }
                float rotation = step.dt * b.m_angularVelocity;
                if (rotation * rotation > b2Settings.b2_maxRotationSquared)
                {
                    if (b.m_angularVelocity < 0.0f)
                    {
                        b.m_angularVelocity = -b2Settings.b2_maxRotation * step.inv_dt;
                    }
                    else
                    {
                        b.m_angularVelocity = b2Settings.b2_maxRotation * step.inv_dt;
                    }
                }

                // Store positions for continuous collision.
                b.m_sweep.c0.SetV(b.m_sweep.c);
                b.m_sweep.a0 = b.m_sweep.a;

                // Integrate
                //b.m_sweep.c += step.dt * b.m_linearVelocity;

                //--------------修改start kingBook---------------
                if (b.m_allowMovement)
                {
                    b.m_sweep.c.x += step.dt * b.m_linearVelocity.x;
                    b.m_sweep.c.y += step.dt * b.m_linearVelocity.y;
                }
                //--------------修改 end-------------------------

                b.m_sweep.a += step.dt * b.m_angularVelocity;
                // Compute new transform
                b.SynchronizeTransform();

                // Note: shapes are synchronized later.
            }

            // Iterate over constraints.
            for (i = 0; i < step.positionIterations; ++i)
            {
                bool contactsOkay = contactSolver.SolvePositionConstraints(b2Settings.b2_contactBaumgarte);

                bool jointsOkay = true;
                for (j = 0; j < m_jointCount; ++j)
                {
                    joint = m_joints[j];
                    bool jointOkay = joint.SolvePositionConstraints(b2Settings.b2_contactBaumgarte);
                    jointsOkay = jointsOkay && jointOkay;
                }

                if (contactsOkay && jointsOkay)
                {
                    break;
                }
            }

            Report(contactSolver.m_constraints);

            if (allowSleep)
            {
                float minSleepTime = float.MaxValue;

                float linTolSqr = b2Settings.b2_linearSleepTolerance * b2Settings.b2_linearSleepTolerance;
                float angTolSqr = b2Settings.b2_angularSleepTolerance * b2Settings.b2_angularSleepTolerance;

                for (i = 0; i < m_bodyCount; ++i)
                {
                    b = m_bodies[i];
                    if (b.GetType() == b2Body.b2_staticBody)
                    {
                        continue;
                    }

                    if ((b.m_flags & b2Body.e_allowSleepFlag) == 0)
                    {
                        b.m_sleepTime = 0.0f;
                        minSleepTime  = 0.0f;
                    }

                    if ((b.m_flags & b2Body.e_allowSleepFlag) == 0 ||
                        b.m_angularVelocity * b.m_angularVelocity > angTolSqr ||
                        b2Math.Dot(b.m_linearVelocity, b.m_linearVelocity) > linTolSqr)
                    {
                        b.m_sleepTime = 0.0f;
                        minSleepTime  = 0.0f;
                    }
                    else
                    {
                        b.m_sleepTime += step.dt;
                        minSleepTime   = b2Math.Min(minSleepTime, b.m_sleepTime);
                    }
                }

                if (minSleepTime >= b2Settings.b2_timeToSleep)
                {
                    for (i = 0; i < m_bodyCount; ++i)
                    {
                        b = m_bodies[i];
                        b.SetAwake(false);
                    }
                }
            }
        }
예제 #9
0
        public void Solve(b2TimeStep step, b2Vec2 gravity, bool allowSleep)
#endif
        {
#if PROFILING
            b2Timer timer = new b2Timer();
#endif
            float h = step.dt;

            // Integrate velocities and apply damping. Initialize the body state.
            for (int i = 0; i < m_bodyCount; ++i)
            {
                b2Body b = m_bodies[i];

                b2Vec2 c = b.Sweep.c;
                float  a = b.Sweep.a;
                b2Vec2 v = b.LinearVelocity;
                float  w = b.AngularVelocity;

                // Store positions for continuous collision.
                b.Sweep.c0 = b.Sweep.c;
                b.Sweep.a0 = b.Sweep.a;

                if (b.BodyType == b2BodyType.b2_dynamicBody)
                {
                    // Integrate velocities.
                    v += h * (b.GravityScale * gravity + b.InvertedMass * b.Force);
                    w += h * b.InvertedI * b.Torque;

                    // Apply damping.
                    // ODE: dv/dt + c * v = 0
                    // Solution: v(t) = v0 * exp(-c * t)
                    // Time step: v(t + dt) = v0 * exp(-c * (t + dt)) = v0 * exp(-c * t) * exp(-c * dt) = v * exp(-c * dt)
                    // v2 = exp(-c * dt) * v1
                    // Taylor expansion:
                    // v2 = (1.0f - c * dt) * v1
                    v *= b2Math.b2Clamp(1.0f - h * b.LinearDamping, 0.0f, 1.0f);
                    w *= b2Math.b2Clamp(1.0f - h * b.AngularDamping, 0.0f, 1.0f);
                }

                m_positions[i].c  = c;
                m_positions[i].a  = a;
                m_velocities[i].v = v;
                m_velocities[i].w = w;
            }

#if PROFILING
            timer.Reset();
#endif
            // Solver data
            b2SolverData solverData = new b2SolverData();
            solverData.step       = step;
            solverData.positions  = m_positions;
            solverData.velocities = m_velocities;

            // Initialize velocity constraints.
            b2ContactSolverDef contactSolverDef;
            contactSolverDef.step       = step;
            contactSolverDef.contacts   = m_contacts;
            contactSolverDef.count      = m_contactCount;
            contactSolverDef.positions  = m_positions;
            contactSolverDef.velocities = m_velocities;

            b2ContactSolver contactSolver = new b2ContactSolver(contactSolverDef);
            contactSolver.InitializeVelocityConstraints();

            if (step.warmStarting)
            {
                contactSolver.WarmStart();
            }

            for (int i = 0; i < m_jointCount; ++i)
            {
                m_joints[i].InitVelocityConstraints(solverData);
            }
#if PROFILING
            profile.solveInit = timer.GetMilliseconds();
#endif
            // Solve velocity constraints
#if PROFILING
            timer.Reset();
#endif
            for (int i = 0; i < step.velocityIterations; ++i)
            {
                for (int j = 0; j < m_jointCount; ++j)
                {
                    m_joints[j].SolveVelocityConstraints(solverData);
                }

                contactSolver.SolveVelocityConstraints();
            }

            // Store impulses for warm starting
            contactSolver.StoreImpulses();
#if PROFILING
            profile.solveVelocity = timer.GetMilliseconds();
#endif
            // Integrate positions
            for (int i = 0; i < m_bodyCount; ++i)
            {
                b2Vec2 c = m_positions[i].c;
                float  a = m_positions[i].a;
                b2Vec2 v = m_velocities[i].v;
                float  w = m_velocities[i].w;

                // Check for large velocities
                b2Vec2 translation = h * v;
                if (translation.LengthSquared /* b2Math.b2Dot(translation, translation)*/ > b2Settings.b2_maxTranslationSquared)
                {
                    float ratio = b2Settings.b2_maxTranslation / translation.Length;
                    v *= ratio;
                }

                float rotation = h * w;
                if (rotation * rotation > b2Settings.b2_maxRotationSquared)
                {
                    float ratio = b2Settings.b2_maxRotation / Math.Abs(rotation);
                    w *= ratio;
                }

                // Integrate
                c += h * v;
                a += h * w;

                m_positions[i].c  = c;
                m_positions[i].a  = a;
                m_velocities[i].v = v;
                m_velocities[i].w = w;
            }

            // Solve position constraints
#if PROFILING
            timer.Reset();
#endif
            bool positionSolved = false;
            for (int i = 0; i < step.positionIterations; ++i)
            {
                bool contactsOkay = contactSolver.SolvePositionConstraints();

                bool jointsOkay = true;
                for (int i2 = 0; i2 < m_jointCount; ++i2)
                {
                    bool jointOkay = m_joints[i2].SolvePositionConstraints(solverData);
                    jointsOkay = jointsOkay && jointOkay;
                }

                if (contactsOkay && jointsOkay)
                {
                    // Exit early if the position errors are small.
                    positionSolved = true;
                    break;
                }
            }

            // Copy state buffers back to the bodies
            for (int i = 0; i < m_bodyCount; ++i)
            {
                b2Body body = m_bodies[i];
                body.Sweep.c         = m_positions[i].c;
                body.Sweep.a         = m_positions[i].a;
                body.LinearVelocity  = m_velocities[i].v;
                body.AngularVelocity = m_velocities[i].w;
                body.SynchronizeTransform();
            }
#if PROFILING
            profile.solvePosition = timer.GetMilliseconds();
#endif
            Report(contactSolver.m_velocityConstraints);

            if (allowSleep)
            {
                float minSleepTime = b2Settings.b2_maxFloat;

                float linTolSqr = b2Settings.b2_linearSleepTolerance * b2Settings.b2_linearSleepTolerance;
                float angTolSqr = b2Settings.b2_angularSleepTolerance * b2Settings.b2_angularSleepTolerance;

                for (int i = 0; i < m_bodyCount; ++i)
                {
                    b2Body b = m_bodies[i];
                    if (b.BodyType == b2BodyType.b2_staticBody)
                    {
                        continue;
                    }

                    if (!(b.BodyFlags.HasFlag(b2BodyFlags.e_autoSleepFlag)) ||
                        b.AngularVelocity * b.AngularVelocity > angTolSqr ||
                        b2Math.b2Dot(b.LinearVelocity, b.LinearVelocity) > linTolSqr)
                    {
                        b.SleepTime  = 0.0f;
                        minSleepTime = 0.0f;
                    }
                    else
                    {
                        b.SleepTime += h;
                        minSleepTime = Math.Min(minSleepTime, b.SleepTime);
                    }
                }

                if (minSleepTime >= b2Settings.b2_timeToSleep && positionSolved)
                {
                    for (int i = 0; i < m_bodyCount; ++i)
                    {
                        b2Body b = m_bodies[i];
                        b.SetAwake(false);
                    }
                }
            }
        }
예제 #10
0
 public void Solve(ref b2Profile profile, b2TimeStep step, b2Vec2 gravity, bool allowSleep)
예제 #11
0
 public void Solve(ref b2Profile profile, b2TimeStep step, b2Vec2 gravity, bool allowSleep)
예제 #12
0
        // Find islands, integrate and solveraints, solve positionraints
        public void Solve(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 = new b2Island(m_bodyCount,
                    m_contactManager.ContactCount,
                    m_jointCount,
                    m_contactManager.ContactListener);

    // Clear all the island flags.
    for (b2Body b = m_bodyList; b != null; b = b.Next)
    {
        b.BodyFlags &= ~b2BodyFlags.e_islandFlag;
    }
    for (b2Contact c = m_contactManager.ContactList; c != null; c = c.Next)
    {
        c.ContactFlags &= ~b2ContactFlags.e_islandFlag;
    }
    for (b2Joint j = m_jointList; j; j = j.Next)
    {
        j.m_islandFlag = false;
    }

    // Build and simulate all awake islands.
    int stackSize = m_bodyCount;
    b2Body[] stack = new b2Body[stackSize];
    for (b2Body seed = m_bodyList; seed != null; seed = seed.Next)
    {
        if (seed.BodyFlags & b2BodyFlags.e_islandFlag)
        {
            continue;
        }

        if (seed.IsAwake() == false || seed.IsActive() == false)
        {
            continue;
        }

        // The seed can be dynamic or kinematic.
        if (seed.BodyType == b2BodyType.b2_staticBody)
        {
            continue;
        }

        // Reset island and stack.
        island.Clear();
        int stackCount = 0;
        stack[stackCount++] = seed;
        seed.BodyFlags |= b2BodyFlags.e_islandFlag;

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

            // Make sure the body is awake.
            b.SetAwake(true);

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

            // Search all contacts connected to this body.
            for (b2ContactEdge ce = b.ContactList; ce != null; ce = ce.next)
            {
                b2Contact contact = ce.contact;

                // Has this contact already been added to an island?
                if (contact.ContactFlags & b2ContactFlags.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.ContactFlags |= b2ContactType.e_islandFlag;

                b2Body other = ce.other;

                // Was the other body already added to this island?
                if ((other.BodyFlags & b2BodyFlags.e_islandFlag) > 0)
                {
                    continue;
                }

                stack[stackCount++] = other;
                other.BodyFlags |= b2BodyFlags.e_islandFlag;
            }

            // Search all joints connect to this body.
            for (b2JointEdge je = b.JointList; je; je = je.next)
            {
                if (je.joint.IslandFlag == true)
                {
                    continue;
                }

                b2Body other = je.other;

                // Don't simulate joints connected to inactive bodies.
                if (other.IsActive() == false)
                {
                    continue;
                }

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

                if ((other.BodyFlags & b2BodyFlags.e_islandFlag) > 0)
                {
                    continue;
                }

                stack[stackCount++] = other;
                other.BodyFlags |= b2BodyFlags.e_islandFlag;
            }
        }

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

        // Post solve cleanup.
        for (int i = 0; i < island.m_bodyCount; ++i)
        {
            // Allow static bodies to participate in other islands.
            b2Body b = island.m_bodies[i];
            if (b.BodyType == b2BodyType.b2_staticBody)
            {
                b.BodyFlags &= ~b2BodyFlags.e_islandFlag;
            }
        }
    }

    {
        b2Timer timer;
        // Synchronize fixtures, check for out of range bodies.
        for (b2Body b = m_bodyList; b != null; b = b.Next)
        {
            // If a body was not in an island then it did not move.
            if ((b.BodyFlags & b2BodyType.e_islandFlag) == 0)
            {
                continue;
            }

            if (b.GetBodyType() == b2BodyType.b2_staticBody)
            {
                continue;
            }

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

        // Look for new contacts.
        m_contactManager.FindNewContacts();
        m_profile.broadphase = timer.GetMilliseconds();
    }
}
예제 #13
0
 private void SolveTOI(b2TimeStep step);