Exemple #1
0
        public void Solve(ref TimeStep step, ref TSVector2 gravity)
        {
            FP h = step.dt;

            // Integrate velocities and apply damping. Initialize the body state.
            for (int i = 0; i < BodyCount; ++i)
            {
                Body b = Bodies[i];

                TSVector2 c = b._sweep.C;
                FP        a = b._sweep.A;
                TSVector2 v = b._linearVelocity;
                FP        w = b._angularVelocity;

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

                if (b.BodyType == BodyType.Dynamic)
                {
                    // Integrate velocities.

                    // FPE: Only apply gravity if the body wants it.
                    if (b.IgnoreGravity)
                    {
                        v += h * (b._invMass * b._force);
                    }
                    else
                    {
                        v += h * (b.GravityScale * gravity + b._invMass * b._force);
                    }

                    w += h * b._invI * 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 *= MathUtils.Clamp(1.0f - h * b.LinearDamping, 0.0f, 1.0f);
                    //w *= MathUtils.Clamp(1.0f - h * b.AngularDamping, 0.0f, 1.0f);

                    v *= 1 / (1 + h * b.LinearDamping);
                    w *= 1 / (1 + h * b.AngularDamping);
                }

                _positions[i].c  = c;
                _positions[i].a  = a;
                _velocities[i].v = v;
                _velocities[i].w = w;
            }

            // Solver data
            SolverData solverData = new SolverData();

            solverData.step       = step;
            solverData.positions  = _positions;
            solverData.velocities = _velocities;

            _contactSolver.Reset(step, ContactCount, _contacts, _positions, _velocities);
            _contactSolver.InitializeVelocityConstraints();

            if (Settings.EnableWarmstarting)
            {
                _contactSolver.WarmStart();
            }

            for (int i = 0; i < JointCount; ++i)
            {
                if (_joints[i].Enabled)
                {
                    _joints[i].InitVelocityConstraints(ref solverData);
                }
            }

            // Solve velocity constraints.
            for (int i = 0; i < Settings.VelocityIterations; ++i)
            {
                for (int j = 0; j < JointCount; ++j)
                {
                    Joint2D joint = _joints[j];

                    if (!joint.Enabled)
                    {
                        continue;
                    }

                    joint.SolveVelocityConstraints(ref solverData);
                    joint.Validate(step.inv_dt);
                }

                _contactSolver.SolveVelocityConstraints();
            }

            // Store impulses for warm starting.
            _contactSolver.StoreImpulses();

            // Integrate positions
            for (int i = 0; i < BodyCount; ++i)
            {
                TSVector2 c = _positions[i].c;
                FP        a = _positions[i].a;
                TSVector2 v = _velocities[i].v;
                FP        w = _velocities[i].w;

                // Check for large velocities
                TSVector2 translation = h * v;
                if (TSVector2.Dot(translation, translation) > Settings.MaxTranslationSquared)
                {
                    FP ratio = Settings.MaxTranslation / translation.magnitude;
                    v *= ratio;
                }

                FP rotation = h * w;
                if (rotation * rotation > Settings.MaxRotationSquared)
                {
                    FP ratio = Settings.MaxRotation / FP.Abs(rotation);
                    w *= ratio;
                }

                // Integrate
                c += h * v;
                a += h * w;

                _positions[i].c  = c;
                _positions[i].a  = a;
                _velocities[i].v = v;
                _velocities[i].w = w;
            }


            // Solve position constraints
            bool positionSolved = false;

            for (int i = 0; i < Settings.PositionIterations; ++i)
            {
                bool contactsOkay = _contactSolver.SolvePositionConstraints();

                bool jointsOkay = true;
                for (int j = 0; j < JointCount; ++j)
                {
                    Joint2D joint = _joints[j];

                    if (!joint.Enabled)
                    {
                        continue;
                    }

                    bool jointOkay = joint.SolvePositionConstraints(ref 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 < BodyCount; ++i)
            {
                Body body = Bodies[i];
                body._sweep.C         = _positions[i].c;
                body._sweep.A         = _positions[i].a;
                body._linearVelocity  = _velocities[i].v;
                body._angularVelocity = _velocities[i].w;
                body.SynchronizeTransform();
            }

            Report(_contactSolver._velocityConstraints);

            if (Settings.AllowSleep)
            {
                FP minSleepTime = Settings.MaxFP;

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

                    if (b.BodyType == BodyType.Static)
                    {
                        continue;
                    }

                    if (!b.SleepingAllowed || b._angularVelocity * b._angularVelocity > AngTolSqr || TSVector2.Dot(b._linearVelocity, b._linearVelocity) > LinTolSqr)
                    {
                        b._sleepTime = 0.0f;
                        minSleepTime = 0.0f;
                    }
                    else
                    {
                        b._sleepTime += h;
                        minSleepTime  = SyncFrame.TSMath.Min(minSleepTime, b._sleepTime);
                    }
                }

                if (minSleepTime >= Settings.TimeToSleep && positionSolved)
                {
                    for (int i = 0; i < BodyCount; ++i)
                    {
                        Body b = Bodies[i];
                        b.Awake = false;
                    }
                }
            }
        }
        /// <summary>
        /// Requires two existing revolute or prismatic joints (any combination will work).
        /// The provided joints must attach a dynamic body to a static body.
        /// </summary>
        /// <param name="jointA">The first joint.</param>
        /// <param name="jointB">The second joint.</param>
        /// <param name="ratio">The ratio.</param>
        /// <param name="bodyA">The first body</param>
        /// <param name="bodyB">The second body</param>
        // TS - public GearJoint(Body bodyA, Body bodyB, Joint jointA, Joint jointB, FP ratio = 1f)
        public GearJoint(Body bodyA, Body bodyB, Joint2D jointA, Joint2D jointB, FP ratio)
        {
            JointType = JointType.Gear;
            BodyA     = bodyA;
            BodyB     = bodyB;
            JointA    = jointA;
            JointB    = jointB;
            Ratio     = ratio;

            _typeA = jointA.JointType;
            _typeB = jointB.JointType;

            Debug.Assert(_typeA == JointType.Revolute || _typeA == JointType.Prismatic || _typeA == JointType.FixedRevolute || _typeA == JointType.FixedPrismatic);
            Debug.Assert(_typeB == JointType.Revolute || _typeB == JointType.Prismatic || _typeB == JointType.FixedRevolute || _typeB == JointType.FixedPrismatic);

            FP coordinateA, coordinateB;

            // TODO_ERIN there might be some problem with the joint edges in b2Joint.

            _bodyC = JointA.BodyA;
            _bodyA = JointA.BodyB;

            // Get geometry of joint1
            Transform xfA = _bodyA._xf;
            FP        aA  = _bodyA._sweep.A;
            Transform xfC = _bodyC._xf;
            FP        aC  = _bodyC._sweep.A;

            if (_typeA == JointType.Revolute)
            {
                RevoluteJoint revolute = (RevoluteJoint)jointA;
                _localAnchorC    = revolute.LocalAnchorA;
                _localAnchorA    = revolute.LocalAnchorB;
                _referenceAngleA = revolute.ReferenceAngle;
                _localAxisC      = TSVector2.zero;

                coordinateA = aA - aC - _referenceAngleA;
            }
            else
            {
                PrismaticJoint prismatic = (PrismaticJoint)jointA;
                _localAnchorC    = prismatic.LocalAnchorA;
                _localAnchorA    = prismatic.LocalAnchorB;
                _referenceAngleA = prismatic.ReferenceAngle;
                _localAxisC      = prismatic.LocalXAxis;

                TSVector2 pC = _localAnchorC;
                TSVector2 pA = MathUtils.MulT(xfC.q, MathUtils.Mul(xfA.q, _localAnchorA) + (xfA.p - xfC.p));
                coordinateA = TSVector2.Dot(pA - pC, _localAxisC);
            }

            _bodyD = JointB.BodyA;
            _bodyB = JointB.BodyB;

            // Get geometry of joint2
            Transform xfB = _bodyB._xf;
            FP        aB  = _bodyB._sweep.A;
            Transform xfD = _bodyD._xf;
            FP        aD  = _bodyD._sweep.A;

            if (_typeB == JointType.Revolute)
            {
                RevoluteJoint revolute = (RevoluteJoint)jointB;
                _localAnchorD    = revolute.LocalAnchorA;
                _localAnchorB    = revolute.LocalAnchorB;
                _referenceAngleB = revolute.ReferenceAngle;
                _localAxisD      = TSVector2.zero;

                coordinateB = aB - aD - _referenceAngleB;
            }
            else
            {
                PrismaticJoint prismatic = (PrismaticJoint)jointB;
                _localAnchorD    = prismatic.LocalAnchorA;
                _localAnchorB    = prismatic.LocalAnchorB;
                _referenceAngleB = prismatic.ReferenceAngle;
                _localAxisD      = prismatic.LocalXAxis;

                TSVector2 pD = _localAnchorD;
                TSVector2 pB = MathUtils.MulT(xfD.q, MathUtils.Mul(xfB.q, _localAnchorB) + (xfB.p - xfD.p));
                coordinateB = TSVector2.Dot(pB - pD, _localAxisD);
            }

            _ratio    = ratio;
            _constant = coordinateA + _ratio * coordinateB;
            _impulse  = 0.0f;
        }
Exemple #3
0
 public void Add(Joint2D joint)
 {
     Debug.Assert(JointCount < JointCapacity);
     _joints[JointCount++] = joint;
 }