FinalizeVelocityConstraints() public method

public FinalizeVelocityConstraints ( ) : void
return void
コード例 #1
0
ファイル: Island.cs プロジェクト: colgreen/box2dx
		public void Solve(TimeStep step, Vec2 gravity, bool allowSleep)
		{
			// Integrate velocities and apply damping.
			for (int i = 0; i < _bodyCount; ++i)
			{
				Body b = _bodies[i];

				if (b.IsStatic())
					continue;

				// Integrate velocities.
				b._linearVelocity += step.Dt * (gravity + b._invMass * b._force);
				b._angularVelocity += step.Dt * b._invI * b._torque;

				// Reset forces.
				b._force.Set(0.0f, 0.0f);
				b._torque = 0.0f;

				// 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
				b._linearVelocity *= Common.Math.Clamp(1.0f - step.Dt * b._linearDamping, 0.0f, 1.0f);
				b._angularVelocity *= Common.Math.Clamp(1.0f - step.Dt * b._angularDamping, 0.0f, 1.0f);

				// Check for large velocities.
#if TARGET_FLOAT32_IS_FIXED
				// Fixed point code written this way to prevent
				// overflows, float code is optimized for speed

				float vMagnitude = b._linearVelocity.Length();
				if(vMagnitude > Settings.MaxLinearVelocity)
				{
					b._linearVelocity *= Settings.MaxLinearVelocity/vMagnitude;
				}
				b._angularVelocity = Vector2.Clamp(b._angularVelocity, 
					-Settings.MaxAngularVelocity, Settings.MaxAngularVelocity);

#else
				if (Vec2.Dot(b._linearVelocity, b._linearVelocity) > Settings.MaxLinearVelocitySquared)
				{
					b._linearVelocity.Normalize();
					b._linearVelocity *= Settings.MaxLinearVelocity;
				}

				if (b._angularVelocity * b._angularVelocity > Settings.MaxAngularVelocitySquared)
				{
					if (b._angularVelocity < 0.0f)
					{
						b._angularVelocity = -Settings.MaxAngularVelocity;
					}
					else
					{
						b._angularVelocity = Settings.MaxAngularVelocity;
					}
				}
#endif
			}

			ContactSolver contactSolver = new ContactSolver(step, _contacts, _contactCount);

			// Initialize velocity constraints.
			contactSolver.InitVelocityConstraints(step);

			for (int i = 0; i < _jointCount; ++i)
			{
				_joints[i].InitVelocityConstraints(step);
			}

			// Solve velocity constraints.
			for (int i = 0; i < step.VelocityIterations; ++i)
			{
				for (int j = 0; j < _jointCount; ++j)
				{
					_joints[j].SolveVelocityConstraints(step);
				}
				contactSolver.SolveVelocityConstraints();
			}

			// Post-solve (store impulses for warm starting).
			contactSolver.FinalizeVelocityConstraints();

			// Integrate positions.
			for (int i = 0; i < _bodyCount; ++i)
			{
				Body b = _bodies[i];

				if (b.IsStatic())
					continue;

				// Store positions for continuous collision.
				b._sweep.C0 = b._sweep.C;
				b._sweep.A0 = b._sweep.A;

				// Integrate
				b._sweep.C += step.Dt * b._linearVelocity;
				b._sweep.A += step.Dt * b._angularVelocity;

				// Compute new transform
				b.SynchronizeTransform();

				// Note: shapes are synchronized later.
			}

			// Iterate over constraints.
			for (int ii = 0; ii < step.PositionIterations; ++ii)
			{
				bool contactsOkay = contactSolver.SolvePositionConstraints(Settings.ContactBaumgarte);
				bool jointsOkay = true;
				for (int i = 0; i < _jointCount; ++i)
				{
					bool jointOkay = _joints[i].SolvePositionConstraints(/*Settings.ContactBaumgarte*/);
					jointsOkay = jointsOkay && jointOkay;
				}

				if (contactsOkay && jointsOkay)
				{
					// Exit early if the position errors are small.
					break;
				}
			}

			Report(contactSolver._constraints);

			if (allowSleep)
			{
				float minSleepTime = Common.Settings.FLT_MAX;
#if !TARGET_FLOAT32_IS_FIXED
				float linTolSqr = Settings.LinearSleepTolerance * Settings.LinearSleepTolerance;
				float angTolSqr = Settings.AngularSleepTolerance * Settings.AngularSleepTolerance;
#endif

				for (int i = 0; i < _bodyCount; ++i)
				{
					Body b = _bodies[i];
					if (b._invMass == 0.0f)
					{
						continue;
					}

					if ((b._flags & Body.BodyFlags.AllowSleep) == 0)
					{
						b._sleepTime = 0.0f;
						minSleepTime = 0.0f;
					}

					if ((b._flags & Body.BodyFlags.AllowSleep) == 0 ||
#if TARGET_FLOAT32_IS_FIXED
						Common.Math.Abs(b._angularVelocity) > Settings.AngularSleepTolerance ||
						Common.Math.Abs(b._linearVelocity.X) > Settings.LinearSleepTolerance ||
						Common.Math.Abs(b._linearVelocity.Y) > Settings.LinearSleepTolerance)
#else
						b._angularVelocity * b._angularVelocity > angTolSqr ||
						Vec2.Dot(b._linearVelocity, b._linearVelocity) > linTolSqr)
#endif
					{
						b._sleepTime = 0.0f;
						minSleepTime = 0.0f;
					}
					else
					{
						b._sleepTime += step.Dt;
						minSleepTime = Common.Math.Min(minSleepTime, b._sleepTime);
					}
				}
コード例 #2
0
ファイル: Island.cs プロジェクト: levi1994/LitDev
        public void Solve(TimeStep step, Vec2 gravity, bool allowSleep)
        {
            for (int i = 0; i < this._bodyCount; i++)
            {
                Body body = this._bodies[i];
                if (!body.IsStatic())
                {
                    Body expr_27 = body;
                    //expr_27._linearVelocity += step.Dt * (gravity + body._invMass * body._force);
                    expr_27._linearVelocity += step.Dt * (body._useGravity ? body._gravity : gravity + body._invMass * body._force); //Steve body gravity
                    body._angularVelocity   += step.Dt * body._invI * body._torque;
                    body._force.Set(0f, 0f);
                    body._torque = 0f;
                    Body expr_9E = body;
                    expr_9E._linearVelocity *= Box2DX.Common.Math.Clamp(1f - step.Dt * body._linearDamping, 0f, 1f);
                    body._angularVelocity   *= Box2DX.Common.Math.Clamp(1f - step.Dt * body._angularDamping, 0f, 1f);
                    if (Vec2.Dot(body._linearVelocity, body._linearVelocity) > Settings.MaxLinearVelocitySquared)
                    {
                        body._linearVelocity.Normalize();
                        Body expr_130 = body;
                        expr_130._linearVelocity *= Settings.MaxLinearVelocity;
                    }
                    if (body._angularVelocity * body._angularVelocity > Settings.MaxAngularVelocitySquared)
                    {
                        if (body._angularVelocity < 0f)
                        {
                            body._angularVelocity = -Settings.MaxAngularVelocity;
                        }
                        else
                        {
                            body._angularVelocity = Settings.MaxAngularVelocity;
                        }
                    }
                }
            }
            ContactSolver contactSolver = new ContactSolver(step, this._contacts, this._contactCount);

            contactSolver.InitVelocityConstraints(step);
            for (int i = 0; i < this._jointCount; i++)
            {
                this._joints[i].InitVelocityConstraints(step);
            }
            for (int i = 0; i < step.VelocityIterations; i++)
            {
                for (int j = 0; j < this._jointCount; j++)
                {
                    this._joints[j].SolveVelocityConstraints(step);
                }
                contactSolver.SolveVelocityConstraints();
            }
            contactSolver.FinalizeVelocityConstraints();
            for (int i = 0; i < this._bodyCount; i++)
            {
                Body body = this._bodies[i];
                if (!body.IsStatic())
                {
                    body._sweep.C0 = body._sweep.C;
                    body._sweep.A0 = body._sweep.A;
                    Body expr_296_cp_0 = body;
                    expr_296_cp_0._sweep.C = expr_296_cp_0._sweep.C + step.Dt * body._linearVelocity;
                    Body expr_2BE_cp_0 = body;
                    expr_2BE_cp_0._sweep.A = expr_2BE_cp_0._sweep.A + step.Dt * body._angularVelocity;
                    body.SynchronizeTransform();
                }
            }
            for (int k = 0; k < step.PositionIterations; k++)
            {
                bool flag  = contactSolver.SolvePositionConstraints(Settings.ContactBaumgarte);
                bool flag2 = true;
                for (int i = 0; i < this._jointCount; i++)
                {
                    bool flag3 = this._joints[i].SolvePositionConstraints(Settings.ContactBaumgarte);
                    flag2 = (flag2 && flag3);
                }
                if (flag && flag2)
                {
                    break;
                }
            }
            this.Report(contactSolver._constraints);
            if (allowSleep)
            {
                float num  = Settings.FLT_MAX;
                float num2 = Settings.LinearSleepTolerance * Settings.LinearSleepTolerance;
                float num3 = Settings.AngularSleepTolerance * Settings.AngularSleepTolerance;
                for (int i = 0; i < this._bodyCount; i++)
                {
                    Body body = this._bodies[i];
                    if (body._invMass != 0f)
                    {
                        if ((body._flags & Body.BodyFlags.AllowSleep) == (Body.BodyFlags) 0)
                        {
                            body._sleepTime = 0f;
                            num             = 0f;
                        }
                        if ((body._flags & Body.BodyFlags.AllowSleep) == (Body.BodyFlags) 0 || body._angularVelocity * body._angularVelocity > num3 || Vec2.Dot(body._linearVelocity, body._linearVelocity) > num2)
                        {
                            body._sleepTime = 0f;
                            num             = 0f;
                        }
                        else
                        {
                            body._sleepTime += step.Dt;
                            num              = Box2DX.Common.Math.Min(num, body._sleepTime);
                        }
                    }
                }
                if (num >= Settings.TimeToSleep)
                {
                    for (int i = 0; i < this._bodyCount; i++)
                    {
                        Body body = this._bodies[i];
                        body._flags          |= Body.BodyFlags.Sleep;
                        body._linearVelocity  = Vec2.Zero;
                        body._angularVelocity = 0f;
                    }
                }
            }
        }
コード例 #3
0
ファイル: Island.cs プロジェクト: more-please/box2dx
        public void Solve(TimeStep step, Vec2 gravity, bool allowSleep)
        {
            // Integrate velocities and apply damping.
            for (int i = 0; i < _bodyCount; ++i)
            {
                Body b = _bodies[i];

                if (b.IsStatic())
                {
                    continue;
                }

                // Integrate velocities.
                b._linearVelocity  += step.Dt * (gravity + b._invMass * b._force);
                b._angularVelocity += step.Dt * b._invI * b._torque;

                // Reset forces.
                b._force.Set(0.0f, 0.0f);
                b._torque = 0.0f;

                // 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
                b._linearVelocity  *= Common.Math.Clamp(1.0f - step.Dt * b._linearDamping, 0.0f, 1.0f);
                b._angularVelocity *= Common.Math.Clamp(1.0f - step.Dt * b._angularDamping, 0.0f, 1.0f);

                // Check for large velocities.
#if TARGET_FLOAT32_IS_FIXED
                // Fixed point code written this way to prevent
                // overflows, float code is optimized for speed

                float vMagnitude = b._linearVelocity.Length();
                if (vMagnitude > Settings.MaxLinearVelocity)
                {
                    b._linearVelocity *= Settings.MaxLinearVelocity / vMagnitude;
                }
                b._angularVelocity = Vector2.Clamp(b._angularVelocity,
                                                   -Settings.MaxAngularVelocity, Settings.MaxAngularVelocity);
#else
                if (Vec2.Dot(b._linearVelocity, b._linearVelocity) > Settings.MaxLinearVelocitySquared)
                {
                    b._linearVelocity.Normalize();
                    b._linearVelocity *= Settings.MaxLinearVelocity;
                }

                if (b._angularVelocity * b._angularVelocity > Settings.MaxAngularVelocitySquared)
                {
                    if (b._angularVelocity < 0.0f)
                    {
                        b._angularVelocity = -Settings.MaxAngularVelocity;
                    }
                    else
                    {
                        b._angularVelocity = Settings.MaxAngularVelocity;
                    }
                }
#endif
            }

            ContactSolver contactSolver = new ContactSolver(step, _contacts, _contactCount);

            // Initialize velocity constraints.
            contactSolver.InitVelocityConstraints(step);

            for (int i = 0; i < _jointCount; ++i)
            {
                _joints[i].InitVelocityConstraints(step);
            }

            // Solve velocity constraints.
            for (int i = 0; i < step.VelocityIterations; ++i)
            {
                for (int j = 0; j < _jointCount; ++j)
                {
                    _joints[j].SolveVelocityConstraints(step);
                }
                contactSolver.SolveVelocityConstraints();
            }

            // Post-solve (store impulses for warm starting).
            contactSolver.FinalizeVelocityConstraints();

            // Integrate positions.
            for (int i = 0; i < _bodyCount; ++i)
            {
                Body b = _bodies[i];

                if (b.IsStatic())
                {
                    continue;
                }

                // Store positions for continuous collision.
                b._sweep.C0 = b._sweep.C;
                b._sweep.A0 = b._sweep.A;

                // Integrate
                b._sweep.C += step.Dt * b._linearVelocity;
                b._sweep.A += step.Dt * b._angularVelocity;

                // Compute new transform
                b.SynchronizeTransform();

                // Note: shapes are synchronized later.
            }

            // Iterate over constraints.
            for (int ii = 0; ii < step.PositionIterations; ++ii)
            {
                bool contactsOkay = contactSolver.SolvePositionConstraints(Settings.ContactBaumgarte);
                bool jointsOkay   = true;
                for (int i = 0; i < _jointCount; ++i)
                {
                    bool jointOkay = _joints[i].SolvePositionConstraints(/*Settings.ContactBaumgarte*/);
                    jointsOkay = jointsOkay && jointOkay;
                }

                if (contactsOkay && jointsOkay)
                {
                    // Exit early if the position errors are small.
                    break;
                }
            }

            Report(contactSolver._constraints);

            if (allowSleep)
            {
                float minSleepTime = Common.Settings.FLT_MAX;
#if !TARGET_FLOAT32_IS_FIXED
                float linTolSqr = Settings.LinearSleepTolerance * Settings.LinearSleepTolerance;
                float angTolSqr = Settings.AngularSleepTolerance * Settings.AngularSleepTolerance;
#endif

                for (int i = 0; i < _bodyCount; ++i)
                {
                    Body b = _bodies[i];
                    if (b._invMass == 0.0f)
                    {
                        continue;
                    }

                    if ((b._flags & Body.BodyFlags.AllowSleep) == 0)
                    {
                        b._sleepTime = 0.0f;
                        minSleepTime = 0.0f;
                    }

                    if ((b._flags & Body.BodyFlags.AllowSleep) == 0 ||
#if TARGET_FLOAT32_IS_FIXED
                        Common.Math.Abs(b._angularVelocity) > Settings.AngularSleepTolerance ||
                        Common.Math.Abs(b._linearVelocity.X) > Settings.LinearSleepTolerance ||
                        Common.Math.Abs(b._linearVelocity.Y) > Settings.LinearSleepTolerance)
#else
                        b._angularVelocity *b._angularVelocity > angTolSqr ||
                        Vec2.Dot(b._linearVelocity, b._linearVelocity) > linTolSqr)
#endif
                    {
                        b._sleepTime = 0.0f;
                        minSleepTime = 0.0f;
                    }
                    else
                    {
                        b._sleepTime += step.Dt;
                        minSleepTime  = Common.Math.Min(minSleepTime, b._sleepTime);
                    }
                }
コード例 #4
0
ファイル: Island.cs プロジェクト: hellogithubtesting/LGame
        public void Solve(TimeStep step, Vec2 gravity, bool allowSleep)
        {
            // Integrate velocities and apply damping.
            for (int i = 0; i < BodyCount; ++i)
            {
                Body b = Bodies[i];

                if (b.IsStatic())
                    continue;

                // Integrate velocities.
                b._linearVelocity += step.Dt * (gravity + b._invMass * b._force);
                b._angularVelocity += step.Dt * b._invI * b._torque;

                // Reset forces.
                b._force.Set(0.0f, 0.0f);
                b._torque = 0.0f;

                // 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
                b._linearVelocity *= Common.Math.Clamp(1.0f - step.Dt * b._linearDamping, 0.0f, 1.0f);
                b._angularVelocity *= Common.Math.Clamp(1.0f - step.Dt * b._angularDamping, 0.0f, 1.0f);
            }

            ContactSolver contactSolver = new ContactSolver(step, Contacts, ContactCount);

            // Initialize velocity constraints.
            contactSolver.InitVelocityConstraints(step);

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

            // Solve velocity constraints.
            for (int i = 0; i < step.VelocityIterations; ++i)
            {
                for (int j = 0; j < JointCount; ++j)
                {
                    Joints[j].SolveVelocityConstraints(step);
                }

                contactSolver.SolveVelocityConstraints();
            }

            // Post-solve (store impulses for warm starting).
            contactSolver.FinalizeVelocityConstraints();

            // Integrate positions.
            for (int i = 0; i < BodyCount; ++i)
            {
                Body b = Bodies[i];

                if (b.IsStatic())
                    continue;

                // Check for large velocities.
                Vec2 translation = step.Dt * b._linearVelocity;
                if (Vec2.Dot(translation, translation) > Settings.MaxTranslationSquared)
                {
                    translation.Normalize();
                    b._linearVelocity = (Settings.MaxTranslation * step.Inv_Dt) * translation;
                }

                float rotation = step.Dt * Bodies[i]._angularVelocity;
                if (rotation * rotation > Settings.MaxRotationSquared)
                {
                    if (rotation < 0.0)
                    {
                        b._angularVelocity = -step.Inv_Dt * Settings.MaxRotation;
                    }
                    else
                    {
                        b._angularVelocity = step.Inv_Dt * Settings.MaxRotation;
                    }
                }

                // Store positions for continuous collision.
                b._sweep.C0 = b._sweep.C;
                b._sweep.A0 = b._sweep.A;

                // Integrate
                b._sweep.C += step.Dt * b._linearVelocity;
                b._sweep.A += step.Dt * b._angularVelocity;

                // Compute new transform
                b.SynchronizeTransform();

                // Note: shapes are synchronized later.
            }

            // Iterate over constraints.
            for (int i = 0; i < step.PositionIterations; ++i)
            {
                bool contactsOkay = contactSolver.SolvePositionConstraints(Settings.ContactBaumgarte);

                bool jointsOkay = true;
                for (int j = 0; j < JointCount; ++j)
                {
                    bool jointOkay = Joints[j].SolvePositionConstraints(Settings.ContactBaumgarte);
                    jointsOkay = jointsOkay && jointOkay;
                }

                if (contactsOkay && jointsOkay)
                {
                    // Exit early if the position errors are small.
                    break;
                }
            }

            Report(contactSolver.Constraints);

            if (allowSleep)
            {
                float minSleepTime = Settings.FLT_MAX;

            #if !TARGET_FLOAT32_IS_FIXED
                float linTolSqr = Settings.LinearSleepTolerance * Settings.LinearSleepTolerance;
                float angTolSqr = Settings.AngularSleepTolerance * Settings.AngularSleepTolerance;
            #endif

                for (int i = 0; i < BodyCount; ++i)
                {
                    Body b = Bodies[i];

                    if (b._invMass == 0.0f)
                    {
                        continue;
                    }

                    if ((b._flags & Body.BodyFlags.AllowSleep) == 0)
                    {
                        b._sleepTime = 0.0f;
                        minSleepTime = 0.0f;
                    }

                    if ((b._flags & Body.BodyFlags.AllowSleep) == 0 ||
            #if TARGET_FLOAT32_IS_FIXED
                        Common.Math.Abs(b._angularVelocity) > Settings.AngularSleepTolerance ||
                        Common.Math.Abs(b._linearVelocity.X) > Settings.LinearSleepTolerance ||
                        Common.Math.Abs(b._linearVelocity.Y) > Settings.LinearSleepTolerance)
            #else
             b._angularVelocity * b._angularVelocity > angTolSqr ||
                        Vec2.Dot(b._linearVelocity, b._linearVelocity) > linTolSqr)
            #endif
                    {
                        b._sleepTime = 0.0f;
                        minSleepTime = 0.0f;
                    }
                    else
                    {
                        b._sleepTime += step.Dt;
                        minSleepTime = Common.Math.Min(minSleepTime, b._sleepTime);
                    }
                }
コード例 #5
0
ファイル: Island.cs プロジェクト: vb0067/LGame
        public void Solve(TimeStep step, Vec2 gravity, bool allowSleep)
        {
            // Integrate velocities and apply damping.
            for (int i = 0; i < BodyCount; ++i)
            {
                Body b = Bodies[i];

                if (b.IsStatic())
                {
                    continue;
                }

                // Integrate velocities.
                b._linearVelocity  += step.Dt * (gravity + b._invMass * b._force);
                b._angularVelocity += step.Dt * b._invI * b._torque;

                // Reset forces.
                b._force.Set(0.0f, 0.0f);
                b._torque = 0.0f;

                // 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
                b._linearVelocity  *= Common.Math.Clamp(1.0f - step.Dt * b._linearDamping, 0.0f, 1.0f);
                b._angularVelocity *= Common.Math.Clamp(1.0f - step.Dt * b._angularDamping, 0.0f, 1.0f);
            }

            ContactSolver contactSolver = new ContactSolver(step, Contacts, ContactCount);

            // Initialize velocity constraints.
            contactSolver.InitVelocityConstraints(step);

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

            // Solve velocity constraints.
            for (int i = 0; i < step.VelocityIterations; ++i)
            {
                for (int j = 0; j < JointCount; ++j)
                {
                    Joints[j].SolveVelocityConstraints(step);
                }

                contactSolver.SolveVelocityConstraints();
            }

            // Post-solve (store impulses for warm starting).
            contactSolver.FinalizeVelocityConstraints();

            // Integrate positions.
            for (int i = 0; i < BodyCount; ++i)
            {
                Body b = Bodies[i];

                if (b.IsStatic())
                {
                    continue;
                }

                // Check for large velocities.
                Vec2 translation = step.Dt * b._linearVelocity;
                if (Vec2.Dot(translation, translation) > Settings.MaxTranslationSquared)
                {
                    translation.Normalize();
                    b._linearVelocity = (Settings.MaxTranslation * step.Inv_Dt) * translation;
                }

                float rotation = step.Dt * Bodies[i]._angularVelocity;
                if (rotation * rotation > Settings.MaxRotationSquared)
                {
                    if (rotation < 0.0)
                    {
                        b._angularVelocity = -step.Inv_Dt * Settings.MaxRotation;
                    }
                    else
                    {
                        b._angularVelocity = step.Inv_Dt * Settings.MaxRotation;
                    }
                }

                // Store positions for continuous collision.
                b._sweep.C0 = b._sweep.C;
                b._sweep.A0 = b._sweep.A;

                // Integrate
                b._sweep.C += step.Dt * b._linearVelocity;
                b._sweep.A += step.Dt * b._angularVelocity;

                // Compute new transform
                b.SynchronizeTransform();

                // Note: shapes are synchronized later.
            }

            // Iterate over constraints.
            for (int i = 0; i < step.PositionIterations; ++i)
            {
                bool contactsOkay = contactSolver.SolvePositionConstraints(Settings.ContactBaumgarte);

                bool jointsOkay = true;
                for (int j = 0; j < JointCount; ++j)
                {
                    bool jointOkay = Joints[j].SolvePositionConstraints(Settings.ContactBaumgarte);
                    jointsOkay = jointsOkay && jointOkay;
                }

                if (contactsOkay && jointsOkay)
                {
                    // Exit early if the position errors are small.
                    break;
                }
            }

            Report(contactSolver.Constraints);

            if (allowSleep)
            {
                float minSleepTime = Settings.FLT_MAX;

#if !TARGET_FLOAT32_IS_FIXED
                float linTolSqr = Settings.LinearSleepTolerance * Settings.LinearSleepTolerance;
                float angTolSqr = Settings.AngularSleepTolerance * Settings.AngularSleepTolerance;
#endif

                for (int i = 0; i < BodyCount; ++i)
                {
                    Body b = Bodies[i];

                    if (b._invMass == 0.0f)
                    {
                        continue;
                    }

                    if ((b._flags & Body.BodyFlags.AllowSleep) == 0)
                    {
                        b._sleepTime = 0.0f;
                        minSleepTime = 0.0f;
                    }

                    if ((b._flags & Body.BodyFlags.AllowSleep) == 0 ||
#if TARGET_FLOAT32_IS_FIXED
                        Common.Math.Abs(b._angularVelocity) > Settings.AngularSleepTolerance ||
                        Common.Math.Abs(b._linearVelocity.X) > Settings.LinearSleepTolerance ||
                        Common.Math.Abs(b._linearVelocity.Y) > Settings.LinearSleepTolerance)
#else
                        b._angularVelocity *b._angularVelocity > angTolSqr ||
                        Vec2.Dot(b._linearVelocity, b._linearVelocity) > linTolSqr)
#endif
                    {
                        b._sleepTime = 0.0f;
                        minSleepTime = 0.0f;
                    }
                    else
                    {
                        b._sleepTime += step.Dt;
                        minSleepTime  = Common.Math.Min(minSleepTime, b._sleepTime);
                    }
                }
コード例 #6
0
ファイル: Island.cs プロジェクト: litdev1/LitDev
 public void Solve(TimeStep step, Vec2 gravity, bool allowSleep)
 {
     for (int i = 0; i < this._bodyCount; i++)
     {
         Body body = this._bodies[i];
         if (!body.IsStatic())
         {
             Body expr_27 = body;
             //expr_27._linearVelocity += step.Dt * (gravity + body._invMass * body._force);
             expr_27._linearVelocity += step.Dt * (body._useGravity ? body._gravity : gravity + body._invMass * body._force); //Steve body gravity
             body._angularVelocity += step.Dt * body._invI * body._torque;
             body._force.Set(0f, 0f);
             body._torque = 0f;
             Body expr_9E = body;
             expr_9E._linearVelocity *= Box2DX.Common.Math.Clamp(1f - step.Dt * body._linearDamping, 0f, 1f);
             body._angularVelocity *= Box2DX.Common.Math.Clamp(1f - step.Dt * body._angularDamping, 0f, 1f);
             if (Vec2.Dot(body._linearVelocity, body._linearVelocity) > Settings.MaxLinearVelocitySquared)
             {
                 body._linearVelocity.Normalize();
                 Body expr_130 = body;
                 expr_130._linearVelocity *= Settings.MaxLinearVelocity;
             }
             if (body._angularVelocity * body._angularVelocity > Settings.MaxAngularVelocitySquared)
             {
                 if (body._angularVelocity < 0f)
                 {
                     body._angularVelocity = -Settings.MaxAngularVelocity;
                 }
                 else
                 {
                     body._angularVelocity = Settings.MaxAngularVelocity;
                 }
             }
         }
     }
     ContactSolver contactSolver = new ContactSolver(step, this._contacts, this._contactCount);
     contactSolver.InitVelocityConstraints(step);
     for (int i = 0; i < this._jointCount; i++)
     {
         this._joints[i].InitVelocityConstraints(step);
     }
     for (int i = 0; i < step.VelocityIterations; i++)
     {
         for (int j = 0; j < this._jointCount; j++)
         {
             this._joints[j].SolveVelocityConstraints(step);
         }
         contactSolver.SolveVelocityConstraints();
     }
     contactSolver.FinalizeVelocityConstraints();
     for (int i = 0; i < this._bodyCount; i++)
     {
         Body body = this._bodies[i];
         if (!body.IsStatic())
         {
             body._sweep.C0 = body._sweep.C;
             body._sweep.A0 = body._sweep.A;
             Body expr_296_cp_0 = body;
             expr_296_cp_0._sweep.C = expr_296_cp_0._sweep.C + step.Dt * body._linearVelocity;
             Body expr_2BE_cp_0 = body;
             expr_2BE_cp_0._sweep.A = expr_2BE_cp_0._sweep.A + step.Dt * body._angularVelocity;
             body.SynchronizeTransform();
         }
     }
     for (int k = 0; k < step.PositionIterations; k++)
     {
         bool flag = contactSolver.SolvePositionConstraints(Settings.ContactBaumgarte);
         bool flag2 = true;
         for (int i = 0; i < this._jointCount; i++)
         {
             bool flag3 = this._joints[i].SolvePositionConstraints(Settings.ContactBaumgarte);
             flag2 = (flag2 && flag3);
         }
         if (flag && flag2)
         {
             break;
         }
     }
     this.Report(contactSolver._constraints);
     if (allowSleep)
     {
         float num = Settings.FLT_MAX;
         float num2 = Settings.LinearSleepTolerance * Settings.LinearSleepTolerance;
         float num3 = Settings.AngularSleepTolerance * Settings.AngularSleepTolerance;
         for (int i = 0; i < this._bodyCount; i++)
         {
             Body body = this._bodies[i];
             if (body._invMass != 0f)
             {
                 if ((body._flags & Body.BodyFlags.AllowSleep) == (Body.BodyFlags)0)
                 {
                     body._sleepTime = 0f;
                     num = 0f;
                 }
                 if ((body._flags & Body.BodyFlags.AllowSleep) == (Body.BodyFlags)0 || body._angularVelocity * body._angularVelocity > num3 || Vec2.Dot(body._linearVelocity, body._linearVelocity) > num2)
                 {
                     body._sleepTime = 0f;
                     num = 0f;
                 }
                 else
                 {
                     body._sleepTime += step.Dt;
                     num = Box2DX.Common.Math.Min(num, body._sleepTime);
                 }
             }
         }
         if (num >= Settings.TimeToSleep)
         {
             for (int i = 0; i < this._bodyCount; i++)
             {
                 Body body = this._bodies[i];
                 body._flags |= Body.BodyFlags.Sleep;
                 body._linearVelocity = Vec2.Zero;
                 body._angularVelocity = 0f;
             }
         }
     }
 }