This is an internal structure.
Beispiel #1
0
        private void solveTOI(TimeStep step)
        {
            Island island = toiIsland;
            island.init(2 * Settings.maxTOIContacts, Settings.maxTOIContacts, 0, m_contactManager.m_contactListener);
            if (m_stepComplete)
            {
                for (Body b = m_bodyList; b != null; b = b.m_next)
                {
                    b.m_flags &= ~Body.e_islandFlag;
                    b.m_sweep.alpha0 = 0.0f;
                }

                for (Contact c = m_contactManager.m_contactList; c != null; c = c.m_next)
                {
                    // Invalidate TOI
                    c.m_flags &= ~(Contact.TOI_FLAG | Contact.ISLAND_FLAG);
                    c.m_toiCount = 0;
                    c.m_toi = 1.0f;
                }
            }

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

                for (Contact c = m_contactManager.m_contactList; c != null; c = c.m_next)
                {
                    // Is this contact disabled?
                    if (c.Enabled == false)
                    {
                        continue;
                    }

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

                    float alpha = 1.0f;
                    if ((c.m_flags & Contact.TOI_FLAG) != 0)
                    {
                        // This contact has a valid cached TOI.
                        alpha = c.m_toi;
                    }
                    else
                    {
                        Fixture fA = c.FixtureA;
                        Fixture fB = c.FixtureB;

                        // Is there a sensor?
                        if (fA.Sensor || fB.Sensor)
                        {
                            continue;
                        }

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

                        BodyType typeA = bA.m_type;
                        BodyType typeB = bB.m_type;
                        Debug.Assert(typeA == BodyType.DYNAMIC || typeB == BodyType.DYNAMIC);

                        bool activeA = bA.Awake && typeA != BodyType.STATIC;
                        bool activeB = bB.Awake && typeB != BodyType.STATIC;

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

                        bool collideA = bA.Bullet || typeA != BodyType.DYNAMIC;
                        bool collideB = bB.Bullet || typeB != BodyType.DYNAMIC;

                        // 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);
                        }

                        Debug.Assert(alpha0 < 1.0f);

                        int indexA = c.ChildIndexA;
                        int indexB = c.ChildIndexB;

                        // Compute the time of impact in interval [0, minTOI]
                        TimeOfImpact.TOIInput input = toiInput;
                        input.proxyA.set_Renamed(fA.Shape, indexA);
                        input.proxyB.set_Renamed(fB.Shape, indexB);
                        input.sweepA.set_Renamed(bA.m_sweep);
                        input.sweepB.set_Renamed(bB.m_sweep);
                        input.tMax = 1.0f;

                        pool.getTimeOfImpact().timeOfImpact(toiOutput, input);

                        // Beta is the fraction of the remaining portion of the .
                        float beta = toiOutput.t;
                        if (toiOutput.state == TimeOfImpact.TOIOutputState.TOUCHING)
                        {
                            alpha = MathUtils.min(alpha0 + (1.0f - alpha0) * beta, 1.0f);
                        }
                        else
                        {
                            alpha = 1.0f;
                        }

                        c.m_toi = alpha;
                        c.m_flags |= Contact.TOI_FLAG;
                    }

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

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

                // Advance the bodies to the TOI.
                Fixture fA2 = minContact.FixtureA;
                Fixture fB2 = minContact.FixtureB;
                Body bA2 = fA2.Body;
                Body bB2 = fB2.Body;

                backup1.set_Renamed(bA2.m_sweep);
                backup2.set_Renamed(bB2.m_sweep);

                bA2.advance(minAlpha);
                bB2.advance(minAlpha);

                // The TOI contact likely has some new contact points.
                minContact.update(m_contactManager.m_contactListener);
                minContact.m_flags &= ~Contact.TOI_FLAG;
                ++minContact.m_toiCount;

                // Is the contact solid?
                if (minContact.Enabled == false || minContact.Touching == false)
                {
                    // Restore the sweeps.
                    minContact.Enabled = false;
                    bA2.m_sweep.set_Renamed(backup1);
                    bB2.m_sweep.set_Renamed(backup2);
                    bA2.synchronizeTransform();
                    bB2.synchronizeTransform();
                    continue;
                }

                bA2.Awake = true;
                bB2.Awake = true;

                // Build the island
                island.clear();
                island.add(bA2);
                island.add(bB2);
                island.add(minContact);

                bA2.m_flags |= Body.e_islandFlag;
                bB2.m_flags |= Body.e_islandFlag;
                minContact.m_flags |= Contact.ISLAND_FLAG;

                // Get contacts on bodyA and bodyB.
                tempBodies[0] = bA2;
                tempBodies[1] = bB2;
                for (int i = 0; i < 2; ++i)
                {
                    Body body = tempBodies[i];
                    if (body.m_type == BodyType.DYNAMIC)
                    {
                        for (ContactEdge ce = body.m_contactList; ce != null; ce = ce.next)
                        {
                            if (island.m_bodyCount == island.m_bodyCapacity)
                            {
                                break;
                            }

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

                            Contact contact = ce.contact;

                            // Has this contact already been added to the island?
                            if ((contact.m_flags & Contact.ISLAND_FLAG) != 0)
                            {
                                continue;
                            }

                            // Only add static, kinematic, or bullet bodies.
                            Body other = ce.other;
                            if (other.m_type == BodyType.DYNAMIC && body.Bullet == false && other.Bullet == 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.
                            backup1.set_Renamed(other.m_sweep);
                            if ((other.m_flags & Body.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.Enabled == false)
                            {
                                other.m_sweep.set_Renamed(backup1);
                                other.synchronizeTransform();
                                continue;
                            }

                            // Are there contact points?
                            if (contact.Touching == false)
                            {
                                other.m_sweep.set_Renamed(backup1);
                                other.synchronizeTransform();
                                continue;
                            }

                            // Add the contact to the island
                            contact.m_flags |= Contact.ISLAND_FLAG;
                            island.add(contact);

                            // Has the other body already been added to the island?
                            if ((other.m_flags & Body.e_islandFlag) != 0)
                            {
                                continue;
                            }

                            // Add the other body to the island.
                            other.m_flags |= Body.e_islandFlag;

                            if (other.m_type != BodyType.STATIC)
                            {
                                other.Awake = true;
                            }

                            island.add(other);
                        }
                    }
                }

                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, bA2.m_islandIndex, bB2.m_islandIndex);

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

                    if (body.m_type != BodyType.DYNAMIC)
                    {
                        continue;
                    }

                    body.synchronizeFixtures();

                    // Invalidate all contact TOIs on this displaced body.
                    for (ContactEdge ce = body.m_contactList; ce != null; ce = ce.next)
                    {
                        ce.contact.m_flags &= ~(Contact.TOI_FLAG | Contact.ISLAND_FLAG);
                    }
                }

                // 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;
                }
            }
        }
Beispiel #2
0
        public void SolveToi(TimeStep subStep, int toiIndexA, int toiIndexB)
        {
            Debug.Assert(toiIndexA < BodyCount);
            Debug.Assert(toiIndexB < BodyCount);

            // Initialize the body state.
            for (int i = 0; i < BodyCount; ++i)
            {
                Body b = Bodies[i];
                Positions[i].C.Set(b.Sweep.C);
                Positions[i].A = b.Sweep.A;
                Velocities[i].V.Set(b.LinearVelocity);
                Velocities[i].W = b.AngularVelocity;
            }

            toiSolverDef.Contacts = Contacts;
            toiSolverDef.Count = ContactCount;
            toiSolverDef.Step = subStep;
            toiSolverDef.Positions = Positions;
            toiSolverDef.Velocities = Velocities;
            toiContactSolver.Init(toiSolverDef);

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

            // #if 0
            // // Is the new position really safe?
            // for (int i = 0; i < m_contactCount; ++i)
            // {
            // Contact* c = m_contacts[i];
            // Fixture* fA = c.GetFixtureA();
            // Fixture* fB = c.GetFixtureB();
            //
            // Body bA = fA.GetBody();
            // Body bB = fB.GetBody();
            //
            // int indexA = c.GetChildIndexA();
            // int indexB = c.GetChildIndexB();
            //
            // DistanceInput input;
            // input.proxyA.Set(fA.GetShape(), indexA);
            // input.proxyB.Set(fB.GetShape(), indexB);
            // input.transformA = bA.GetTransform();
            // input.transformB = bB.GetTransform();
            // input.useRadii = false;
            //
            // DistanceOutput output;
            // SimplexCache cache;
            // cache.count = 0;
            // Distance(&output, &cache, &input);
            //
            // if (output.distance == 0 || cache.count == 3)
            // {
            // cache.count += 0;
            // }
            // }
            // #endif

            // Leap of faith to new safe state.
            Bodies[toiIndexA].Sweep.C0.Set(Positions[toiIndexA].C);
            Bodies[toiIndexA].Sweep.A0 = Positions[toiIndexA].A;
            Bodies[toiIndexB].Sweep.C0.Set(Positions[toiIndexB].C);
            Bodies[toiIndexB].Sweep.A0 = Positions[toiIndexB].A;

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

            // Solve velocity constraints.
            for (int i = 0; i < subStep.VelocityIterations; ++i)
            {
                toiContactSolver.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 < BodyCount; ++i)
            {
                Vec2 c = Positions[i].C;
                float a = Positions[i].A;
                Vec2 v = Velocities[i].V;
                float w = Velocities[i].W;

                // Check for large velocities
                translation.Set(v).MulLocal(h);
                if (Vec2.Dot(translation, translation) > Settings.MAX_TRANSLATION_SQUARED)
                {
                    float ratio = Settings.MAX_TRANSLATION / translation.Length();
                    v.MulLocal(ratio);
                }

                float rotation = h * w;
                if (rotation * rotation > Settings.MaxRotationSquared)
                {
                    float ratio = Settings.MAX_ROTATION / MathUtils.Abs(rotation);
                    w *= ratio;
                }

                // Integrate
                c.X += v.X * h;
                c.Y += v.Y * h;
                a += h * w;

                Positions[i].C.Set(c);
                Positions[i].A = a;
                Velocities[i].V.Set(v);
                Velocities[i].W = w;

                // Sync bodies
                Body body = Bodies[i];
                body.Sweep.C.Set(c);
                body.Sweep.A = a;
                body.LinearVelocity.Set(v);
                body.AngularVelocity = w;
                body.SynchronizeTransform();
            }

            Report(toiContactSolver.VelocityConstraints);
        }
Beispiel #3
0
        private void solve(TimeStep step)
        {
            m_profile.solveInit = 0;
            m_profile.solveVelocity = 0;
            m_profile.solvePosition = 0;

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

            // Clear all the island flags.
            for (Body b = m_bodyList; b != null; b = b.m_next)
            {
                b.m_flags &= ~Body.e_islandFlag;
            }
            for (Contact c = m_contactManager.m_contactList; c != null; c = c.m_next)
            {
                c.m_flags &= ~Contact.ISLAND_FLAG;
            }
            for (Joint j = m_jointList; j != null; j = j.m_next)
            {
                j.m_islandFlag = false;
            }

            // Build and simulate all awake islands.
            int stackSize = m_bodyCount;
            if (stack.Length < stackSize)
            {
                stack = new Body[stackSize];
            }
            for (Body seed = m_bodyList; seed != null; seed = seed.m_next)
            {
                if ((seed.m_flags & Body.e_islandFlag) == Body.e_islandFlag)
                {
                    continue;
                }

                if (seed.Awake == false || seed.Active == false)
                {
                    continue;
                }

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

                // Reset island and stack.
                island.clear();
                int stackCount = 0;
                stack[stackCount++] = seed;
                seed.m_flags |= Body.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.
                    Body b = stack[--stackCount];
                    Debug.Assert(b.Active == true);
                    island.add(b);

                    // Make sure the body is awake.
                    b.Awake = true;

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

                    // Search all contacts connected to this body.
                    for (ContactEdge ce = b.m_contactList; ce != null; ce = ce.next)
                    {
                        Contact contact = ce.contact;

                        // Has this contact already been added to an island?
                        if ((contact.m_flags & Contact.ISLAND_FLAG) == Contact.ISLAND_FLAG)
                        {
                            continue;
                        }

                        // Is this contact solid and touching?
                        if (contact.Enabled == false || contact.Touching == 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 |= Contact.ISLAND_FLAG;

                        Body other = ce.other;

                        // Was the other body already added to this island?
                        if ((other.m_flags & Body.e_islandFlag) == Body.e_islandFlag)
                        {
                            continue;
                        }

                        Debug.Assert(stackCount < stackSize);
                        stack[stackCount++] = other;
                        other.m_flags |= Body.e_islandFlag;
                    }

                    // Search all joints connect to this body.
                    for (JointEdge je = b.m_jointList; je != null; je = je.next)
                    {
                        if (je.joint.m_islandFlag == true)
                        {
                            continue;
                        }

                        Body other = je.other;

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

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

                        if ((other.m_flags & Body.e_islandFlag) == Body.e_islandFlag)
                        {
                            continue;
                        }

                        Debug.Assert(stackCount < stackSize);
                        stack[stackCount++] = other;
                        other.m_flags |= Body.e_islandFlag;
                    }
                }
                island.solve(islandProfile, step, m_gravity, m_allowSleep);
                m_profile.solveInit += islandProfile.solveInit;
                m_profile.solveVelocity += islandProfile.solveVelocity;
                m_profile.solvePosition += islandProfile.solvePosition;

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

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

                if (b.Type == BodyType.STATIC)
                {
                    continue;
                }

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

            // Look for new contacts.
            m_contactManager.findNewContacts();
            m_profile.broadphase = broadphaseTimer.Milliseconds;
        }
Beispiel #4
0
        public void Solve(Profile profile, TimeStep step, Vec2 gravity, bool allowSleep)
        {
            // Console.WriteLine("Solving Island");

            float h = step.Dt;

            // Integrate velocities and apply damping. Initialize the body state.
            for (int i = 0; i < BodyCount; ++i)
            {
                Body b = Bodies[i];
                Vec2 c = b.Sweep.C;
                float a = b.Sweep.A;
                Vec2 v = b.LinearVelocity;
                float w = b.AngularVelocity;

                // Store positions for continuous collision.
                b.Sweep.C0.Set(b.Sweep.C);
                b.Sweep.A0 = b.Sweep.A;

                if (b.Type == BodyType.Dynamic)
                {
                    // Integrate velocities.
                    // v += h * (b.m_gravityScale * gravity + b.m_invMass * b.m_force);
                    v.X += h * (b.GravityScale * gravity.X + b.InvMass * b.Force.X);
                    v.Y += h * (b.GravityScale * gravity.Y + b.InvMass * b.Force.Y);
                    w += h * b.InvI * 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.MulLocal(MathUtils.Clamp(1.0f - h * b.LinearDamping, 0.0f, 1.0f));
                    w *= MathUtils.Clamp(1.0f - h * b.AngularDamping, 0.0f, 1.0f);
                }
                //Debug.Assert (v.x == 0);

                Positions[i].C.Set(c);
                Positions[i].A = a;
                Velocities[i].V.Set(v);
                Velocities[i].W = w;
            }

            timer.Reset();

            // Solver data
            solverData.Step = step;
            solverData.Positions = Positions;
            solverData.Velocities = Velocities;

            // Initialize velocity constraints.
            solverDef.Step = step;
            solverDef.Contacts = Contacts;
            solverDef.Count = ContactCount;
            solverDef.Positions = Positions;
            solverDef.Velocities = Velocities;

            contactSolver.Init(solverDef);
            //Console.WriteLine("island init vel");
            contactSolver.InitializeVelocityConstraints();

            if (step.WarmStarting)
            {
                //Console.WriteLine("island warm start");
                contactSolver.WarmStart();
            }

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

            profile.SolveInit = timer.Milliseconds;

            // Solve velocity constraints
            timer.Reset();
            //Console.WriteLine("island solving velocities");
            for (int i = 0; i < step.VelocityIterations; ++i)
            {
                for (int j = 0; j < JointCount; ++j)
                {
                    Joints[j].SolveVelocityConstraints(solverData);
                }

                contactSolver.SolveVelocityConstraints();
            }

            // Store impulses for warm starting
            contactSolver.StoreImpulses();
            profile.SolveVelocity = timer.Milliseconds;

            // Integrate positions
            for (int i = 0; i < BodyCount; ++i)
            {
                Vec2 c = Positions[i].C;
                float a = Positions[i].A;
                Vec2 v = Velocities[i].V;
                float w = Velocities[i].W;

                // Check for large velocities
                translation.X = v.X * h;
                translation.Y = v.Y * h;

                if (Vec2.Dot(translation, translation) > Settings.MAX_TRANSLATION_SQUARED)
                {
                    float ratio = Settings.MAX_TRANSLATION / translation.Length();
                    v.X *= ratio;
                    v.Y *= ratio;
                }

                float rotation = h * w;
                if (rotation * rotation > Settings.MaxRotationSquared)
                {
                    float ratio = Settings.MAX_ROTATION / MathUtils.Abs(rotation);
                    w *= ratio;
                }

                // Integrate
                c.X += h * v.X;
                c.Y += h * v.Y;
                a += h * w;

                Positions[i].A = a;
                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 j = 0; j < JointCount; ++j)
                {
                    bool jointOkay = Joints[j].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 < BodyCount; ++i)
            {
                Body body = Bodies[i];
                body.Sweep.C.Set(Positions[i].C);
                body.Sweep.A = Positions[i].A;
                body.LinearVelocity.Set(Velocities[i].V);
                body.AngularVelocity = Velocities[i].W;
                body.SynchronizeTransform();
            }

            profile.SolvePosition = timer.Milliseconds;

            Report(contactSolver.VelocityConstraints);

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

                const float linTolSqr = Settings.LINEAR_SLEEP_TOLERANCE * Settings.LINEAR_SLEEP_TOLERANCE;
                float angTolSqr = Settings.ANGULAR_SLEEP_TOLERANCE * Settings.ANGULAR_SLEEP_TOLERANCE;

                for (int i = 0; i < BodyCount; ++i)
                {
                    Body b = Bodies[i];
                    if (b.Type == BodyType.Static)
                    {
                        continue;
                    }

                    if ((b.Flags & Body.TypeFlags.AutoSleep) == 0 || b.AngularVelocity * b.AngularVelocity > angTolSqr || Vec2.Dot(b.LinearVelocity, b.LinearVelocity) > linTolSqr)
                    {
                        b.SleepTime = 0.0f;
                        minSleepTime = 0.0f;
                    }
                    else
                    {
                        b.SleepTime += h;
                        minSleepTime = MathUtils.Min(minSleepTime, b.SleepTime);
                    }
                }

                if (minSleepTime >= Settings.TIME_TO_SLEEP && positionSolved)
                {
                    for (int i = 0; i < BodyCount; ++i)
                    {
                        Body b = Bodies[i];
                        b.Awake = false;
                    }
                }
            }
        }
Beispiel #5
0
        public void SolveToi(TimeStep subStep, int toiIndexA, int toiIndexB)
        {
            Debug.Assert(toiIndexA < BodyCount);
            Debug.Assert(toiIndexB < BodyCount);

            // Initialize the body state.
            for (int i = 0; i < BodyCount; ++i)
            {
                Body b = Bodies[i];
                Positions[i].C.Set(b.Sweep.C);
                Positions[i].A = b.Sweep.A;
                Velocities[i].V.Set(b.LinearVelocity);
                Velocities[i].W = b.AngularVelocity;
            }

            toiSolverDef.Contacts   = Contacts;
            toiSolverDef.Count      = ContactCount;
            toiSolverDef.Step       = subStep;
            toiSolverDef.Positions  = Positions;
            toiSolverDef.Velocities = Velocities;
            toiContactSolver.Init(toiSolverDef);

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

            // #if 0
            // // Is the new position really safe?
            // for (int i = 0; i < m_contactCount; ++i)
            // {
            // Contact* c = m_contacts[i];
            // Fixture* fA = c.GetFixtureA();
            // Fixture* fB = c.GetFixtureB();
            //
            // Body bA = fA.GetBody();
            // Body bB = fB.GetBody();
            //
            // int indexA = c.GetChildIndexA();
            // int indexB = c.GetChildIndexB();
            //
            // DistanceInput input;
            // input.proxyA.Set(fA.GetShape(), indexA);
            // input.proxyB.Set(fB.GetShape(), indexB);
            // input.transformA = bA.GetTransform();
            // input.transformB = bB.GetTransform();
            // input.useRadii = false;
            //
            // DistanceOutput output;
            // SimplexCache cache;
            // cache.count = 0;
            // Distance(&output, &cache, &input);
            //
            // if (output.distance == 0 || cache.count == 3)
            // {
            // cache.count += 0;
            // }
            // }
            // #endif

            // Leap of faith to new safe state.
            Bodies[toiIndexA].Sweep.C0.Set(Positions[toiIndexA].C);
            Bodies[toiIndexA].Sweep.A0 = Positions[toiIndexA].A;
            Bodies[toiIndexB].Sweep.C0.Set(Positions[toiIndexB].C);
            Bodies[toiIndexB].Sweep.A0 = Positions[toiIndexB].A;

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

            // Solve velocity constraints.
            for (int i = 0; i < subStep.VelocityIterations; ++i)
            {
                toiContactSolver.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 < BodyCount; ++i)
            {
                Vec2  c = Positions[i].C;
                float a = Positions[i].A;
                Vec2  v = Velocities[i].V;
                float w = Velocities[i].W;

                // Check for large velocities
                translation.Set(v).MulLocal(h);
                if (Vec2.Dot(translation, translation) > Settings.MAX_TRANSLATION_SQUARED)
                {
                    float ratio = Settings.MAX_TRANSLATION / translation.Length();
                    v.MulLocal(ratio);
                }

                float rotation = h * w;
                if (rotation * rotation > Settings.MaxRotationSquared)
                {
                    float ratio = Settings.MAX_ROTATION / MathUtils.Abs(rotation);
                    w *= ratio;
                }

                // Integrate
                c.X += v.X * h;
                c.Y += v.Y * h;
                a   += h * w;

                Positions[i].C.Set(c);
                Positions[i].A = a;
                Velocities[i].V.Set(v);
                Velocities[i].W = w;

                // Sync bodies
                Body body = Bodies[i];
                body.Sweep.C.Set(c);
                body.Sweep.A = a;
                body.LinearVelocity.Set(v);
                body.AngularVelocity = w;
                body.SynchronizeTransform();
            }

            Report(toiContactSolver.VelocityConstraints);
        }
Beispiel #6
0
        public void Solve(Profile profile, TimeStep step, Vec2 gravity, bool allowSleep)
        {
            // Console.WriteLine("Solving Island");

            float h = step.Dt;

            // Integrate velocities and apply damping. Initialize the body state.
            for (int i = 0; i < BodyCount; ++i)
            {
                Body  b = Bodies[i];
                Vec2  c = b.Sweep.C;
                float a = b.Sweep.A;
                Vec2  v = b.LinearVelocity;
                float w = b.AngularVelocity;

                // Store positions for continuous collision.
                b.Sweep.C0.Set(b.Sweep.C);
                b.Sweep.A0 = b.Sweep.A;

                if (b.Type == BodyType.Dynamic)
                {
                    // Integrate velocities.
                    // v += h * (b.m_gravityScale * gravity + b.m_invMass * b.m_force);
                    v.X += h * (b.GravityScale * gravity.X + b.InvMass * b.Force.X);
                    v.Y += h * (b.GravityScale * gravity.Y + b.InvMass * b.Force.Y);
                    w   += h * b.InvI * 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.MulLocal(MathUtils.Clamp(1.0f - h * b.LinearDamping, 0.0f, 1.0f));
                    w *= MathUtils.Clamp(1.0f - h * b.AngularDamping, 0.0f, 1.0f);
                }
                //Debug.Assert (v.x == 0);

                Positions[i].C.Set(c);
                Positions[i].A = a;
                Velocities[i].V.Set(v);
                Velocities[i].W = w;
            }

            timer.Reset();

            // Solver data
            solverData.Step       = step;
            solverData.Positions  = Positions;
            solverData.Velocities = Velocities;

            // Initialize velocity constraints.
            solverDef.Step       = step;
            solverDef.Contacts   = Contacts;
            solverDef.Count      = ContactCount;
            solverDef.Positions  = Positions;
            solverDef.Velocities = Velocities;

            contactSolver.Init(solverDef);
            //Console.WriteLine("island init vel");
            contactSolver.InitializeVelocityConstraints();

            if (step.WarmStarting)
            {
                //Console.WriteLine("island warm start");
                contactSolver.WarmStart();
            }

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

            profile.SolveInit = timer.Milliseconds;

            // Solve velocity constraints
            timer.Reset();
            //Console.WriteLine("island solving velocities");
            for (int i = 0; i < step.VelocityIterations; ++i)
            {
                for (int j = 0; j < JointCount; ++j)
                {
                    Joints[j].SolveVelocityConstraints(solverData);
                }

                contactSolver.SolveVelocityConstraints();
            }

            // Store impulses for warm starting
            contactSolver.StoreImpulses();
            profile.SolveVelocity = timer.Milliseconds;

            // Integrate positions
            for (int i = 0; i < BodyCount; ++i)
            {
                Vec2  c = Positions[i].C;
                float a = Positions[i].A;
                Vec2  v = Velocities[i].V;
                float w = Velocities[i].W;

                // Check for large velocities
                translation.X = v.X * h;
                translation.Y = v.Y * h;

                if (Vec2.Dot(translation, translation) > Settings.MAX_TRANSLATION_SQUARED)
                {
                    float ratio = Settings.MAX_TRANSLATION / translation.Length();
                    v.X *= ratio;
                    v.Y *= ratio;
                }

                float rotation = h * w;
                if (rotation * rotation > Settings.MaxRotationSquared)
                {
                    float ratio = Settings.MAX_ROTATION / MathUtils.Abs(rotation);
                    w *= ratio;
                }

                // Integrate
                c.X += h * v.X;
                c.Y += h * v.Y;
                a   += h * w;

                Positions[i].A  = a;
                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 j = 0; j < JointCount; ++j)
                {
                    bool jointOkay = Joints[j].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 < BodyCount; ++i)
            {
                Body body = Bodies[i];
                body.Sweep.C.Set(Positions[i].C);
                body.Sweep.A = Positions[i].A;
                body.LinearVelocity.Set(Velocities[i].V);
                body.AngularVelocity = Velocities[i].W;
                body.SynchronizeTransform();
            }

            profile.SolvePosition = timer.Milliseconds;

            Report(contactSolver.VelocityConstraints);

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

                const float linTolSqr = Settings.LINEAR_SLEEP_TOLERANCE * Settings.LINEAR_SLEEP_TOLERANCE;
                float       angTolSqr = Settings.ANGULAR_SLEEP_TOLERANCE * Settings.ANGULAR_SLEEP_TOLERANCE;

                for (int i = 0; i < BodyCount; ++i)
                {
                    Body b = Bodies[i];
                    if (b.Type == BodyType.Static)
                    {
                        continue;
                    }

                    if ((b.Flags & Body.TypeFlags.AutoSleep) == 0 || b.AngularVelocity * b.AngularVelocity > angTolSqr || Vec2.Dot(b.LinearVelocity, b.LinearVelocity) > linTolSqr)
                    {
                        b.SleepTime  = 0.0f;
                        minSleepTime = 0.0f;
                    }
                    else
                    {
                        b.SleepTime += h;
                        minSleepTime = MathUtils.Min(minSleepTime, b.SleepTime);
                    }
                }

                if (minSleepTime >= Settings.TIME_TO_SLEEP && positionSolved)
                {
                    for (int i = 0; i < BodyCount; ++i)
                    {
                        Body b = Bodies[i];
                        b.Awake = false;
                    }
                }
            }
        }
Beispiel #7
0
        public virtual void solveTOI(TimeStep 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)
            {
                Body b = m_bodies[i];
                m_positions[i].c.set_Renamed(b.m_sweep.c);
                m_positions[i].a = b.m_sweep.a;
                m_velocities[i].v.set_Renamed(b.m_linearVelocity);
                m_velocities[i].w = b.m_angularVelocity;
            }

            toiSolverDef.contacts = m_contacts;
            toiSolverDef.count = m_contactCount;
            toiSolverDef.step = subStep;
            toiSolverDef.positions = m_positions;
            toiSolverDef.velocities = m_velocities;
            toiContactSolver.init(toiSolverDef);

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

            // #if 0
            // // Is the new position really safe?
            // for (int i = 0; i < m_contactCount; ++i)
            // {
            // Contact* c = m_contacts[i];
            // Fixture* fA = c.GetFixtureA();
            // Fixture* fB = c.GetFixtureB();
            //
            // Body bA = fA.GetBody();
            // Body bB = fB.GetBody();
            //
            // int indexA = c.GetChildIndexA();
            // int indexB = c.GetChildIndexB();
            //
            // DistanceInput input;
            // input.proxyA.Set(fA.GetShape(), indexA);
            // input.proxyB.Set(fB.GetShape(), indexB);
            // input.transformA = bA.GetTransform();
            // input.transformB = bB.GetTransform();
            // input.useRadii = false;
            //
            // DistanceOutput output;
            // SimplexCache cache;
            // cache.count = 0;
            // Distance(&output, &cache, &input);
            //
            // if (output.distance == 0 || cache.count == 3)
            // {
            // cache.count += 0;
            // }
            // }
            // #endif

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

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

            // Solve velocity constraints.
            for (int i = 0; i < subStep.velocityIterations; ++i)
            {
                toiContactSolver.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)
            {
                Vec2 c = m_positions[i].c;
                float a = m_positions[i].a;
                Vec2 v = m_velocities[i].v;
                float w = m_velocities[i].w;

                // Check for large velocities
                translation.set_Renamed(v).mulLocal(h);
                if (Vec2.dot(translation, translation) > Settings.maxTranslationSquared)
                {
                    float ratio = Settings.maxTranslation / translation.length();
                    v.mulLocal(ratio);
                }

                float rotation = h * w;
                if (rotation * rotation > Settings.maxRotationSquared)
                {
                    float ratio = Settings.maxRotation / MathUtils.abs(rotation);
                    w *= ratio;
                }

                // Integrate
                c.x += v.x * h;
                c.y += v.y * h;
                a += h * w;

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

                // Sync bodies
                Body body = m_bodies[i];
                body.m_sweep.c.set_Renamed(c);
                body.m_sweep.a = a;
                body.m_linearVelocity.set_Renamed(v);
                body.m_angularVelocity = w;
                body.synchronizeTransform();
            }

            report(toiContactSolver.m_velocityConstraints);
        }
Beispiel #8
0
        public virtual void solve(Profile profile, TimeStep step, Vec2 gravity, bool allowSleep)
        {
            // Console.WriteLine("Solving Island");

            float h = step.dt;

            // Integrate velocities and apply damping. Initialize the body state.
            for (int i = 0; i < m_bodyCount; ++i)
            {
                Body b = m_bodies[i];
                Vec2 c = b.m_sweep.c;
                float a = b.m_sweep.a;
                Vec2 v = b.m_linearVelocity;
                float w = b.m_angularVelocity;

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

                if (b.m_type == BodyType.DYNAMIC)
                {
                    // Integrate velocities.
                    // v += h * (b.m_gravityScale * gravity + b.m_invMass * b.m_force);
                    v.x += h * (b.m_gravityScale * gravity.x + b.m_invMass * b.m_force.x);
                    v.y += h * (b.m_gravityScale * gravity.y + b.m_invMass * b.m_force.y);
                    w += h * 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
                    v.mulLocal(MathUtils.clamp(1.0f - h * b.m_linearDamping, 0.0f, 1.0f));
                    w *= MathUtils.clamp(1.0f - h * b.m_angularDamping, 0.0f, 1.0f);
                }
                //Debug.Assert (v.x == 0);

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

            timer.reset();

            // Solver data
            solverData.step = step;
            solverData.positions = m_positions;
            solverData.velocities = m_velocities;

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

            contactSolver.init(solverDef);
            //Console.WriteLine("island init vel");
            contactSolver.initializeVelocityConstraints();

            if (step.warmStarting)
            {
                //Console.WriteLine("island warm start");
                contactSolver.warmStart();
            }

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

            profile.solveInit = timer.Milliseconds;

            // Solve velocity constraints
            timer.reset();
            //Console.WriteLine("island solving velocities");
            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.Milliseconds;

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

                // Check for large velocities
                translation.x = v.x * h;
                translation.y = v.y * h;

                if (Vec2.dot(translation, translation) > Settings.maxTranslationSquared)
                {
                    float ratio = Settings.maxTranslation / translation.length();
                    v.x *= ratio;
                    v.y *= ratio;
                }

                float rotation = h * w;
                if (rotation * rotation > Settings.maxRotationSquared)
                {
                    float ratio = Settings.maxRotation / MathUtils.abs(rotation);
                    w *= ratio;
                }

                // Integrate
                c.x += h * v.x;
                c.y += h * v.y;
                a += h * w;

                m_positions[i].a = a;
                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 j = 0; j < m_jointCount; ++j)
                {
                    bool jointOkay = m_joints[j].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)
            {
                Body body = m_bodies[i];
                body.m_sweep.c.set_Renamed(m_positions[i].c);
                body.m_sweep.a = m_positions[i].a;
                body.m_linearVelocity.set_Renamed(m_velocities[i].v);
                body.m_angularVelocity = m_velocities[i].w;
                body.synchronizeTransform();
            }

            profile.solvePosition = timer.Milliseconds;

            report(contactSolver.m_velocityConstraints);

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

                float linTolSqr = Settings.linearSleepTolerance * Settings.linearSleepTolerance;
                float angTolSqr = Settings.angularSleepTolerance * Settings.angularSleepTolerance;

                for (int i = 0; i < m_bodyCount; ++i)
                {
                    Body b = m_bodies[i];
                    if (b.Type == BodyType.STATIC)
                    {
                        continue;
                    }

                    if ((b.m_flags & Body.e_autoSleepFlag) == 0 || b.m_angularVelocity * b.m_angularVelocity > angTolSqr || Vec2.dot(b.m_linearVelocity, b.m_linearVelocity) > linTolSqr)
                    {
                        b.m_sleepTime = 0.0f;
                        minSleepTime = 0.0f;
                    }
                    else
                    {
                        b.m_sleepTime += h;
                        minSleepTime = MathUtils.min(minSleepTime, b.m_sleepTime);
                    }
                }

                if (minSleepTime >= Settings.timeToSleep && positionSolved)
                {
                    for (int i = 0; i < m_bodyCount; ++i)
                    {
                        Body b = m_bodies[i];
                        b.Awake = false;
                    }
                }
            }
        }