Ejemplo n.º 1
0
        internal override bool SolvePositionConstraints(ref SolverData data)
        {
            if (Frequency > 0.0f)
            {
                // There is no position correction for soft distance constraints.
                return(true);
            }

            FSBody b1 = BodyA;
            FSBody b2 = BodyB;

            Transform xf1, xf2;

            b1.GetTransform(out xf1);
            b2.GetTransform(out xf2);

            FVector2 r1 = MathUtils.Mul(ref xf1.q, LocalAnchorA - b1.LocalCenter);
            FVector2 r2 = MathUtils.Mul(ref xf2.q, LocalAnchorB - b2.LocalCenter);

            FVector2 d = b2.Sweep.C + r2 - b1.Sweep.C - r1;

            float length = d.Length();

            if (length < MaxLength && length > MinLength)
            {
                return(true);
            }

            if (length == 0.0f)
            {
                return(true);
            }

            d /= length;
            float C = length - MaxLength;

            C = MathUtils.Clamp(C, -FSSettings.MaxLinearCorrection, FSSettings.MaxLinearCorrection);

            float impulse = -_mass * C;

            _u = d;
            FVector2 P = impulse * _u;

            b1.Sweep.C -= b1.InvMass * P;
            b1.Sweep.A -= b1.InvI * MathUtils.Cross(r1, P);
            b2.Sweep.C += b2.InvMass * P;
            b2.Sweep.A += b2.InvI * MathUtils.Cross(r2, P);

            b1.SynchronizeTransform();
            b2.SynchronizeTransform();

            return(Math.Abs(C) < FSSettings.LinearSlop);
        }
Ejemplo n.º 2
0
        internal override void SolveVelocityConstraints(ref SolverData data)
        {
            FSBody b1 = BodyA;
            FSBody b2 = BodyB;

            Transform xf1, xf2;

            b1.GetTransform(out xf1);
            b2.GetTransform(out xf2);

            FVector2 r1 = MathUtils.Mul(ref xf1.q, LocalAnchorA - b1.LocalCenter);
            FVector2 r2 = MathUtils.Mul(ref xf2.q, LocalAnchorB - b2.LocalCenter);

            FVector2 d = b2.Sweep.C + r2 - b1.Sweep.C - r1;

            float length = d.Length();

            if (length < MaxLength && length > MinLength)
            {
                return;
            }

            // Cdot = dot(u, v + cross(w, r))
            FVector2 v1   = b1.LinearVelocityInternal + MathUtils.Cross(b1.AngularVelocityInternal, r1);
            FVector2 v2   = b2.LinearVelocityInternal + MathUtils.Cross(b2.AngularVelocityInternal, r2);
            float    Cdot = FVector2.Dot(_u, v2 - v1);

            float impulse = -_mass * (Cdot + _bias + _gamma * _impulse);

            _impulse += impulse;

            FVector2 P = impulse * _u;

            b1.LinearVelocityInternal  -= b1.InvMass * P;
            b1.AngularVelocityInternal -= b1.InvI * MathUtils.Cross(r1, P);
            b2.LinearVelocityInternal  += b2.InvMass * P;
            b2.AngularVelocityInternal += b2.InvI * MathUtils.Cross(r2, P);
        }
Ejemplo n.º 3
0
        private void DrawJoint(FarseerJoint joint)
        {
            if (!joint.Enabled)
            {
                return;
            }

            FSBody    b1 = joint.BodyA;
            FSBody    b2 = joint.BodyB;
            Transform xf1, xf2;

            b1.GetTransform(out xf1);

            FVector2 x2 = FVector2.Zero;

            // WIP David
            if (!joint.IsFixedType())
            {
                b2.GetTransform(out xf2);
                x2 = xf2.p;
            }

            FVector2 p1 = joint.WorldAnchorA;
            FVector2 p2 = joint.WorldAnchorB;
            FVector2 x1 = xf1.p;

            Color color = new Color(0.5f, 0.8f, 0.8f);

            switch (joint.JointType)
            {
            case JointType.Distance:
                DrawSegment(p1, p2, color);
                break;

            case JointType.Pulley:
                FSPulleyJoint pulley = (FSPulleyJoint)joint;
                FVector2      s1     = pulley.GroundAnchorA;
                FVector2      s2     = pulley.GroundAnchorB;
                DrawSegment(s1, p1, color);
                DrawSegment(s2, p2, color);
                DrawSegment(s1, s2, color);
                break;

            case JointType.FixedMouse:
                DrawPoint(p1, 0.5f, new Color(0.0f, 1.0f, 0.0f));
                DrawSegment(p1, p2, new Color(0.8f, 0.8f, 0.8f));
                break;

            case JointType.Revolute:
                //DrawSegment(x2, p1, color);
                DrawSegment(p2, p1, color);
                DrawSolidCircle(p2, 0.1f, FVector2.Zero, Color.red);
                DrawSolidCircle(p1, 0.1f, FVector2.Zero, Color.blue);
                break;

            case JointType.FixedAngle:
                //Should not draw anything.
                break;

            case JointType.FixedRevolute:
                DrawSegment(x1, p1, color);
                DrawSolidCircle(p1, 0.1f, FVector2.Zero, new Color(1f, 0.5f, 1f));
                break;

            case JointType.FixedLine:
                DrawSegment(x1, p1, color);
                DrawSegment(p1, p2, color);
                break;

            case JointType.FixedDistance:
                DrawSegment(x1, p1, color);
                DrawSegment(p1, p2, color);
                break;

            case JointType.FixedPrismatic:
                DrawSegment(x1, p1, color);
                DrawSegment(p1, p2, color);
                break;

            case JointType.Gear:
                DrawSegment(x1, x2, color);
                break;

            //case JointType.Weld:
            //    break;
            default:
                DrawSegment(x1, p1, color);
                DrawSegment(p1, p2, color);
                DrawSegment(x2, p2, color);
                break;
            }
        }
Ejemplo n.º 4
0
        internal override void InitVelocityConstraints(ref SolverData data)
        {
            FSBody b1 = BodyA;
            FSBody b2 = BodyB;

            Transform xf1, xf2;

            b1.GetTransform(out xf1);
            b2.GetTransform(out xf2);

            // Compute the effective mass matrix.
            FVector2 r1 = MathUtils.Mul(ref xf1.q, LocalAnchorA - b1.LocalCenter);
            FVector2 r2 = MathUtils.Mul(ref xf2.q, LocalAnchorB - b2.LocalCenter);

            _u = b2.Sweep.C + r2 - b1.Sweep.C - r1;

            // Handle singularity.
            float length = _u.Length();

            if (length < MaxLength && length > MinLength)
            {
                return;
            }

            if (length > FSSettings.LinearSlop)
            {
                _u *= 1.0f / length;
            }
            else
            {
                _u = FVector2.Zero;
            }

            float cr1u    = MathUtils.Cross(r1, _u);
            float cr2u    = MathUtils.Cross(r2, _u);
            float invMass = b1.InvMass + b1.InvI * cr1u * cr1u + b2.InvMass + b2.InvI * cr2u * cr2u;

            Debug.Assert(invMass > FSSettings.Epsilon);
            _mass = invMass != 0.0f ? 1.0f / invMass : 0.0f;

            if (Frequency > 0.0f)
            {
                float C = length - MaxLength;

                // Frequency
                float omega = 2.0f * FSSettings.Pi * Frequency;

                // Damping coefficient
                float d = 2.0f * _mass * DampingRatio * omega;

                // Spring stiffness
                float k = _mass * omega * omega;

                // magic formulas
                _gamma = data.step.dt * (d + data.step.dt * k);
                _gamma = _gamma != 0.0f ? 1.0f / _gamma : 0.0f;
                _bias  = C * data.step.dt * k * _gamma;

                _mass = invMass + _gamma;
                _mass = _mass != 0.0f ? 1.0f / _mass : 0.0f;
            }

            if (FSSettings.EnableWarmstarting)
            {
                // Scale the impulse to support a variable time step.
                _impulse *= data.step.dtRatio;

                FVector2 P = _impulse * _u;
                b1.LinearVelocityInternal  -= b1.InvMass * P;
                b1.AngularVelocityInternal -= b1.InvI * MathUtils.Cross(r1, P);
                b2.LinearVelocityInternal  += b2.InvMass * P;
                b2.AngularVelocityInternal += b2.InvI * MathUtils.Cross(r2, P);
            }
            else
            {
                _impulse = 0.0f;
            }
        }