コード例 #1
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);
                    }
                }
            }
        }
コード例 #2
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);
        }
コード例 #3
0
ファイル: b2Island.cs プロジェクト: okebaybi/Box2DSharp
    public void Solve(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 = new b2Vec2(b.m_sweep.c);
            float  a = b.m_sweep.a;

            b2Vec2 v = new b2Vec2(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.b2_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
                // Pade approximation:
                // v2 = v1 * 1 / (1 + c * dt)
                v *= 1.0f / (1.0f + h * b.m_linearDamping);
                w *= 1.0f / (1.0f + h * b.m_angularDamping);
            }

            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 = new b2ContactSolverDef();


        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 (Utils.b2Dot(translation, translation) > (Settings.b2_maxTranslation * Settings.b2_maxTranslation))
            {
                float ratio = Settings.b2_maxTranslation / translation.Length();
                v *= ratio;
            }

            float rotation = h * w;
            if (rotation * rotation > Settings.b2_maxRotationSquared)
            {
                float ratio = (0.5f * Settings.b2_pi) / Utils.b2Abs(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_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)
        {
            b2Body 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 = float.MaxValue;

            const float linTolSqr = Settings.b2_linearSleepTolerance * Settings.b2_linearSleepTolerance;
            float       angTolSqr = Settings.b2_angularSlop * Settings.b2_angularSlop;

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

                if ((b.m_flags & b2Body.BodyFlags.e_autoSleepFlag) == 0 || b.m_angularVelocity * b.m_angularVelocity > angTolSqr || Utils.b2Dot(b.m_linearVelocity, b.m_linearVelocity) > linTolSqr)
                {
                    b.m_sleepTime = 0.0f;
                    minSleepTime  = 0.0f;
                }
                else
                {
                    b.m_sleepTime += h;
                    minSleepTime   = Utils.b2Min(minSleepTime, b.m_sleepTime);
                }
            }

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