Пример #1
0
        /// <summary>
        /// Perform one solver iteration. Returns true if converged.
        /// </summary>
        /// <param name="baumgarte">The baumgarte value.</param>
        /// <returns></returns>
        public bool Solve(float baumgarte)
        {
            float minSeparation = 0.0f;

            for (int i = 0; i < _count; ++i)
            {
                TOIConstraint c     = _constraints[i];
                Body          bodyA = c.BodyA;
                Body          bodyB = c.BodyB;

                float massA = bodyA.Mass;
                float massB = bodyB.Mass;

                // Only the TOI body should move.
                if (bodyA == _toiBody)
                {
                    massB = 0.0f;
                }
                else
                {
                    massA = 0.0f;
                }

                float invMassA = massA * bodyA.InvMass;
                float invIA    = massA * bodyA.InvI;
                float invMassB = massB * bodyB.InvMass;
                float invIB    = massB * bodyB.InvI;

                // Solve normal constraints
                for (int j = 0; j < c.PointCount; ++j)
                {
                    TOISolverManifold psm = new TOISolverManifold(ref c, j);

                    Vector2 normal     = psm.Normal;
                    Vector2 point      = psm.Point;
                    float   separation = psm.Separation;

                    Vector2 rA = point - bodyA.Sweep.c;
                    Vector2 rB = point - bodyB.Sweep.c;

                    // Track max constraint error.
                    minSeparation = Math.Min(minSeparation, separation);

                    // Prevent large corrections and allow slop.
                    float C = MathUtils.Clamp(baumgarte * (separation + Settings.LinearSlop),
                                              -Settings.MaxLinearCorrection, 0.0f);

                    // Compute the effective mass.
                    float rnA = MathUtils.Cross(rA, normal);
                    float rnB = MathUtils.Cross(rB, normal);
                    float K   = invMassA + invMassB + invIA * rnA * rnA + invIB * rnB * rnB;

                    // Compute normal impulse
                    float impulse = K > 0.0f ? -C / K : 0.0f;

                    Vector2 P = impulse * normal;

                    bodyA.Sweep.c -= invMassA * P;
                    bodyA.Sweep.a -= invIA * MathUtils.Cross(rA, P);
                    bodyA.SynchronizeTransform();

                    bodyB.Sweep.c += invMassB * P;
                    bodyB.Sweep.a += invIB * MathUtils.Cross(rB, P);
                    bodyB.SynchronizeTransform();
                }
            }

            // We can't expect minSpeparation >= -b2_linearSlop because we don't
            // push the separation above -b2_linearSlop.
            return(minSeparation >= -1.5f * Settings.LinearSlop);
        }
Пример #2
0
        /// <summary>
        /// Perform one solver iteration. Returns true if converged.
        /// </summary>
        /// <param name="baumgarte">The baumgarte value.</param>
        /// <returns></returns>
        public bool Solve(float baumgarte)
        {
            float minSeparation = 0.0f;

            for (int i = 0; i < _count; ++i)
            {
                TOIConstraint c = _constraints[i];
                Body bodyA = c.BodyA;
                Body bodyB = c.BodyB;

                float massA = bodyA.Mass;
                float massB = bodyB.Mass;

                // Only the TOI body should move.
                if (bodyA == _toiBody)
                {
                    massB = 0.0f;
                }
                else
                {
                    massA = 0.0f;
                }

                float invMassA = massA * bodyA.InvMass;
                float invIA = massA * bodyA.InvI;
                float invMassB = massB * bodyB.InvMass;
                float invIB = massB * bodyB.InvI;

                // Solve normal constraints
                for (int j = 0; j < c.PointCount; ++j)
                {
                    TOISolverManifold psm = new TOISolverManifold(ref c, j);

                    Vector2 normal = psm.Normal;
                    Vector2 point = psm.Point;
                    float separation = psm.Separation;

                    Vector2 rA = point - bodyA.Sweep.c;
                    Vector2 rB = point - bodyB.Sweep.c;

                    // Track max constraint error.
                    minSeparation = Math.Min(minSeparation, separation);

                    // Prevent large corrections and allow slop.
                    float C = MathUtils.Clamp(baumgarte * (separation + Settings.LinearSlop),
                                              -Settings.MaxLinearCorrection, 0.0f);

                    // Compute the effective mass.
                    float rnA = MathUtils.Cross(rA, normal);
                    float rnB = MathUtils.Cross(rB, normal);
                    float K = invMassA + invMassB + invIA * rnA * rnA + invIB * rnB * rnB;

                    // Compute normal impulse
                    float impulse = K > 0.0f ? -C / K : 0.0f;

                    Vector2 P = impulse * normal;

                    bodyA.Sweep.c -= invMassA * P;
                    bodyA.Sweep.a -= invIA * MathUtils.Cross(rA, P);
                    bodyA.SynchronizeTransform();

                    bodyB.Sweep.c += invMassB * P;
                    bodyB.Sweep.a += invIB * MathUtils.Cross(rB, P);
                    bodyB.SynchronizeTransform();
                }
            }

            // We can't expect minSpeparation >= -b2_linearSlop because we don't
            // push the separation above -b2_linearSlop.
            return minSeparation >= -1.5f * Settings.LinearSlop;
        }