示例#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);
        }