Ejemplo n.º 1
0
        public void Solve(Profile profile, TimeStep step, Vec2 gravity, bool allowSleep)
        {
            Timer timer = new Timer();

            float h = step.dt;

            // Integrate velocities and apply damping. Initialize the body state.
            for (int i = 0; i < m_bodies.Count(); 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 = b.m_sweep.c;
                b.m_sweep.a0 = b.m_sweep.a;

                if (b.m_type == BodyType._dynamicBody)
                {
                    // Integrate velocities.
                    v += h * (b.m_gravityScale * gravity + b.m_invMass * b.m_force);
                    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 *= Utilities.Clamp(1.0f - h * b.m_linearDamping, 0.0f, 1.0f);
                    w *= Utilities.Clamp(1.0f - h * b.m_angularDamping, 0.0f, 1.0f);
                }

                Position pos = new Position();
                pos.c = c;
                pos.a = a;
                m_positions.Add(pos);

                Velocity vel = new Velocity();
                vel.v = v;
                vel.w = w;
                m_velocities.Add(vel);
            }

            timer.Reset();

            // Solver data
            SolverData solverData;

            solverData.step       = step;
            solverData.positions  = m_positions;
            solverData.velocities = m_velocities;

            // Initialize velocity constraints.
            ContactSolverDef contactSolverDef;

            contactSolverDef.step       = step;
            contactSolverDef.contacts   = m_contacts;
            contactSolverDef.positions  = m_positions;
            contactSolverDef.velocities = m_velocities;

            ContactSolver contactSolver = new ContactSolver(contactSolverDef);

            contactSolver.InitializeVelocityConstraints();

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

            for (int i = 0; i < m_joints.Count(); ++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_joints.Count(); ++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_bodies.Count(); ++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
                Vec2 translation = h * v;
                if (Utilities.Dot(translation, translation) > Settings._maxTranslationSquared)
                {
                    float ratio = Settings._maxTranslation / translation.Length();
                    v *= ratio;
                }

                float rotation = h * w;
                if (rotation * rotation > Settings._maxRotationSquared)
                {
                    float ratio = Settings._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 j = 0; j < m_joints.Count; ++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_bodies.Count(); ++i)
            {
                Body body = m_bodies[i];
                body.m_sweep.c         = m_positions[i].c;
                body.m_sweep.a         = m_positions[i].a;
                body.m_linearVelocity  = m_velocities[i].v;
                body.m_angularVelocity = m_velocities[i].w;
                body.SynchronizeTransform();
            }

            profile.solvePosition = timer.GetMilliseconds();

            Report(contactSolver.m_velocityConstraints);

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

                const float linTolSqr = Settings._linearSleepTolerance * Settings._linearSleepTolerance;
                const float angTolSqr = Settings._angularSleepTolerance * Settings._angularSleepTolerance;

                for (int i = 0; i < m_bodies.Count(); ++i)
                {
                    Body b = m_bodies[i];
                    if (b.GetBodyType() == BodyType._staticBody)
                    {
                        continue;
                    }

                    if ((b.m_flags & Body.BodyFlags.e_autoSleepFlag) == 0 ||
                        b.m_angularVelocity * b.m_angularVelocity > angTolSqr ||
                        Utilities.Dot(b.m_linearVelocity, b.m_linearVelocity) > linTolSqr)
                    {
                        b.m_sleepTime = 0.0f;
                        minSleepTime  = 0.0f;
                    }
                    else
                    {
                        b.m_sleepTime += h;
                        minSleepTime   = Math.Min(minSleepTime, b.m_sleepTime);
                    }
                }

                if (minSleepTime >= Settings._timeToSleep && positionSolved)
                {
                    for (int i = 0; i < m_bodies.Count(); ++i)
                    {
                        Body b = m_bodies[i];
                        b.SetAwake(false);
                    }
                }
            }
        }
Ejemplo n.º 2
0
        private void Solve(TimeStep step)
        {
            m_profile.solveInit     = 0.0f;
            m_profile.solveVelocity = 0.0f;
            m_profile.solvePosition = 0.0f;

            // Size the island for the worst case.
            Island island = new Island(m_contactManager.m_contactListener);

            // Clear all the island flags.
            foreach (Body b in m_bodyList)
            {
                b.m_flags &= ~Body.BodyFlags.e_islandFlag;
            }
            foreach (Contact c in m_contactManager.m_contactList)
            {
                c.m_flags &= ~ContactFlags.e_islandFlag;
            }
            foreach (Joint j in m_jointList)
            {
                j.m_islandFlag = false;
            }

            // Build and simulate all awake islands.
            List <Body> stack = new List <Body>(m_bodyList.Count());

            foreach (Body seed in m_bodyList)
            {
                if (seed.m_flags.HasFlag(Body.BodyFlags.e_islandFlag))
                {
                    continue;
                }

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

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

                // Reset island and stack.
                island.Clear();
                int stackCount = 0;
                stack.Add(seed); stackCount++;
                seed.m_flags |= Body.BodyFlags.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];
                    Utilities.Assert(b.IsActive() == true);
                    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.GetBodyType() == BodyType._staticBody)
                    {
                        continue;
                    }

                    // Search all contacts connected to this body.
                    foreach (ContactEdge ce in b.m_contactList)
                    {
                        Contact contact = ce.contact;

                        // Has this contact already been added to an island?
                        if (contact.m_flags.HasFlag(ContactFlags.e_islandFlag))
                        {
                            continue;
                        }

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

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

                        island.Add(contact);
                        contact.m_flags |= ContactFlags.e_islandFlag;

                        Body other = ce.other;

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

                        Utilities.Assert(stackCount < m_bodyList.Count());
                        stack.Add(other); stackCount++;
                        other.m_flags |= Body.BodyFlags.e_islandFlag;
                    }

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

                        Body 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.m_flags.HasFlag(Body.BodyFlags.e_islandFlag))
                        {
                            continue;
                        }

                        stack.Add(other); stackCount++;
                        other.m_flags |= Body.BodyFlags.e_islandFlag;
                    }
                }

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

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

            {
                Timer timer = new Timer();
                // Synchronize fixtures, check for out of range bodies.
                foreach (Body b in m_bodyList)
                {
                    // If a body was not in an island then it did not move.
                    if ((b.m_flags & Body.BodyFlags.e_islandFlag) == 0)
                    {
                        continue;
                    }

                    if (b.GetBodyType() == BodyType._staticBody)
                    {
                        continue;
                    }

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

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