Initialize() public method

public Initialize ( ContactPositionConstraint pc, Transform xfA, Transform xfB, int index ) : void
pc ContactPositionConstraint
xfA Box2D.Common.Transform
xfB Box2D.Common.Transform
index int
return void
PositionSolverManifold Class Documentation
Example #1
0
        // Sequential position solver for position constraints.
        public bool SolveTOIPositionConstraints(int toiIndexA, int toiIndexB)
        {
            float minSeparation = 0.0f;

            for (int i = 0; i < Count; ++i)
            {
                ContactPositionConstraint pc = PositionConstraints[i];

                int  indexA       = pc.IndexA;
                int  indexB       = pc.IndexB;
                Vec2 localCenterA = pc.LocalCenterA;
                Vec2 localCenterB = pc.LocalCenterB;
                int  pointCount   = pc.PointCount;

                float mA = 0.0f;
                float iA = 0.0f;
                if (indexA == toiIndexA || indexA == toiIndexB)
                {
                    mA = pc.InvMassA;
                    iA = pc.InvIA;
                }

                float mB = pc.InvMassB;
                float iB = pc.InvIB;
                if (indexB == toiIndexA || indexB == toiIndexB)
                {
                    mB = pc.InvMassB;
                    iB = pc.InvIB;
                }

                Vec2  cA = Positions[indexA].C;
                float aA = Positions[indexA].A;

                Vec2  cB = Positions[indexB].C;
                float aB = Positions[indexB].A;

                // Solve normal constraints
                for (int j = 0; j < pointCount; ++j)
                {
                    xfA.Q.Set(aA);
                    xfB.Q.Set(aB);
                    Rot.MulToOutUnsafe(xfA.Q, localCenterA, xfA.P);
                    xfA.P.NegateLocal().AddLocal(cA);
                    Rot.MulToOutUnsafe(xfB.Q, localCenterB, xfB.P);
                    xfB.P.NegateLocal().AddLocal(cB);

                    PositionSolverManifold psm = psolver;
                    psm.Initialize(pc, xfA, xfB, j);
                    Vec2 normal = psm.Normal;

                    Vec2  point      = psm.Point;
                    float separation = psm.Separation;

                    rA.Set(point).SubLocal(cA);
                    rB.Set(point).SubLocal(cB);

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

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

                    // Compute the effective mass.
                    float rnA = Vec2.Cross(rA, normal);
                    float rnB = Vec2.Cross(rB, normal);
                    float K   = mA + mB + iA * rnA * rnA + iB * rnB * rnB;

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

                    P.Set(normal).MulLocal(impulse);

                    cA.SubLocal(temp.Set(P).MulLocal(mA));
                    aA -= iA * Vec2.Cross(rA, P);

                    cB.AddLocal(temp.Set(P).MulLocal(mB));
                    aB += iB * Vec2.Cross(rB, P);
                }

                Positions[indexA].C.Set(cA);
                Positions[indexA].A = aA;

                Positions[indexB].C.Set(cB);
                Positions[indexB].A = aB;
            }

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