コード例 #1
0
ファイル: Island.cs プロジェクト: Nomad1/sharpbox2d
        public void solve(Profile profile, TimeStep step, Vec2 gravity, bool allowSleep)
        {
            // System.ref.println("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];
                Sweep bm_sweep = b.m_sweep;
                Vec2 c = bm_sweep.c;
                float a = bm_sweep.a;
                Vec2 v = b.m_linearVelocity;
                float w = b.m_angularVelocity;

                // Store positions for continuous collision.
                bm_sweep.c0.set(bm_sweep.c);
                bm_sweep.a0 = bm_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
                    // Pade approximation:
                    // v2 = v1 * 1 / (1 + c * dt)
                    v.x *= 1.0f/(1.0f + h*b.m_linearDamping);
                    v.y *= 1.0f/(1.0f + h*b.m_linearDamping);
                    w *= 1.0f/(1.0f + h*b.m_angularDamping);
                }

                m_positions[i].c.x = c.x;
                m_positions[i].c.y = c.y;
                m_positions[i].a = a;
                m_velocities[i].v.x = v.x;
                m_velocities[i].v.y = v.y;
                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);
            // System.ref.println("island init vel");
            contactSolver.initializeVelocityConstraints();

            if (step.warmStarting)
            {
                // System.ref.println("island warm start");
                contactSolver.warmStart();
            }

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

            profile.solveInit.accum(timer.getMilliseconds());

            // Solve velocity constraints
            timer.reset();
            // System.ref.println("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.accum(timer.getMilliseconds());

            // 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
                float translationx = v.x*h;
                float translationy = v.y*h;

                if (translationx*translationx + translationy*translationy > Settings.maxTranslationSquared)
                {
                    float ratio = Settings.maxTranslation
                                  /MathUtils.sqrt(translationx*translationx + translationy*translationy);
                    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.x = m_positions[i].c.x;
                body.m_sweep.c.y = m_positions[i].c.y;
                body.m_sweep.a = m_positions[i].a;
                body.m_linearVelocity.x = m_velocities[i].v.x;
                body.m_linearVelocity.y = m_velocities[i].v.y;
                body.m_angularVelocity = m_velocities[i].w;
                body.synchronizeTransform();
            }

            profile.solvePosition.accum(timer.getMilliseconds());

            report(contactSolver.m_velocityConstraints);

            if (allowSleep)
            {
                float minSleepTime = float.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.getType() == BodyType.STATIC)
                    {
                        continue;
                    }

                    if ((b.m_flags & BodyFlags.AutoSleep) == 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.setAwake(false);
                    }
                }
            }
        }
コード例 #2
0
ファイル: World.cs プロジェクト: Nomad1/sharpbox2d
        public World(Vec2 gravity, IWorldPool pool, BroadPhase broadPhase)
        {
            this.pool = pool;
            m_destructionListener = null;
            m_debugDraw = null;

            m_bodyList = null;
            m_jointList = null;

            m_bodyCount = 0;
            m_jointCount = 0;

            m_warmStarting = true;
            m_continuousPhysics = true;
            m_subStepping = false;
            m_stepComplete = true;

            m_allowSleep = true;
            m_gravity.set(gravity);

            m_flags = CLEAR_FORCES;

            m_inv_dt0 = 0f;

            m_contactManager = new ContactManager(this, broadPhase);
            m_profile = new Profile();

            m_particleSystem = new ParticleSystem(this);

            for (int i = 0; i < contactStacks.Length; i++)
            {
                contactStacks[i] = new ContactRegister[Enum.GetValues(typeof (ShapeType)).Length];
            }

            initializeRegisters();
        }