Exemplo n.º 1
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);
                    }
                }
            }
        }
Exemplo n.º 2
0
        public Tiles()
        {
            m_fixtureCount = 0;
            b2Timer timer = new b2Timer();

            {
                float a = 0.5f;
                b2BodyDef bd  = new b2BodyDef();
                bd.position.y = -a;
                b2Body ground = m_world.CreateBody(bd);

#if true
                int N = 200;
                int M = 10;
                b2Vec2 position = new b2Vec2();
                position.y = 0.0f;
                for (int j = 0; j < M; ++j)
                {
                    position.x = -N * a;
                    for (int i = 0; i < N; ++i)
                    {
                        b2PolygonShape shape = new b2PolygonShape();
                        shape.SetAsBox(a, a, position, 0.0f);
                        ground.CreateFixture(shape, 0.0f);
                        ++m_fixtureCount;
                        position.x += 2.0f * a;
                    }
                    position.y -= 2.0f * a;
                }
#else
            int N = 200;
            int M = 10;
            b2Vec2 position = new b2Vec2();
            position.x = -N * a;
            for (int i = 0; i < N; ++i)
            {
                position.y = 0.0f;
                for (int j = 0; j < M; ++j)
                {
                    b2PolygonShape shape = new b2PolygonShape();
                    shape.SetAsBox(a, a, position, 0.0f);
                    ground.CreateFixture(shape, 0.0f);
                    position.y -= 2.0f * a;
                }
                position.x += 2.0f * a;
            }
#endif
            }

            {
                float a = 0.5f;
                b2PolygonShape shape = new b2PolygonShape();
                shape.SetAsBox(a, a);

                b2Vec2 x = new b2Vec2(-7.0f, 0.75f);
                b2Vec2 y;
                b2Vec2 deltaX = new b2Vec2(0.5625f, 1.25f);
                b2Vec2 deltaY = new b2Vec2(1.125f, 0.0f);

                for (int i = 0; i < e_count; ++i)
                {
                    y = x;

                    for (int j = i; j < e_count; ++j)
                    {
                        b2BodyDef bd  = new b2BodyDef();
                        bd.type = b2BodyType.b2_dynamicBody;
                        bd.position = y;

                        //if (i == 0 && j == 0)
                        //{
                        //    bd.allowSleep = false;
                        //}
                        //else
                        //{
                        //    bd.allowSleep = true;
                        //}

                        b2Body body = m_world.CreateBody(bd);
                        body.CreateFixture(shape, 5.0f);
                        ++m_fixtureCount;
                        y += deltaY;
                    }

                    x += deltaX;
                }
            }

            m_createTime = timer.GetMilliseconds();
        }