Box2D.Dynamics.Joints.PrismaticJoint.SolvePositionConstraints C# (CSharp) Метод

        public override bool SolvePositionConstraints(SolverData data)
        {
            Rot qA = Pool.PopRot();
            Rot qB = Pool.PopRot();
            Vec2 rA = Pool.PopVec2();
            Vec2 rB = Pool.PopVec2();
            Vec2 d = Pool.PopVec2();
            Vec2 axis = Pool.PopVec2();
            Vec2 perp = Pool.PopVec2();
            Vec2 temp = Pool.PopVec2();
            Vec2 C1 = Pool.PopVec2();

            Vec3 impulse = Pool.PopVec3();

            Vec2 cA = data.Positions[IndexA].C;
            float aA = data.Positions[IndexA].A;
            Vec2 cB = data.Positions[IndexB].C;
            float aB = data.Positions[IndexB].A;

            qA.Set(aA);
            qB.Set(aB);

            float mA = InvMassA, mB = InvMassB;
            float iA = InvIA, iB = InvIB;

            // Compute fresh Jacobians
            Rot.MulToOutUnsafe(qA, temp.Set(LocalAnchorA).SubLocal(LocalCenterA), rA);
            Rot.MulToOutUnsafe(qB, temp.Set(LocalAnchorB).SubLocal(LocalCenterB), rB);
            d.Set(cB).AddLocal(rB).SubLocal(cA).SubLocal(rA);

            Rot.MulToOutUnsafe(qA, LocalXAxisA, axis);
            float a1 = Vec2.Cross(temp.Set(d).AddLocal(rA), axis);
            float a2 = Vec2.Cross(rB, axis);
            Rot.MulToOutUnsafe(qA, LocalYAxisA, perp);

            float s1 = Vec2.Cross(temp.Set(d).AddLocal(rA), perp);
            float s2 = Vec2.Cross(rB, perp);

            C1.X = Vec2.Dot(perp, d);
            C1.Y = aB - aA - ReferenceAngle;

            float linearError = MathUtils.Abs(C1.X);
            float angularError = MathUtils.Abs(C1.Y);

            bool active = false;
            float C2 = 0.0f;
            if (m_limitEnabled)
            {
                float translation = Vec2.Dot(axis, d);
                if (MathUtils.Abs(UpperTranslation - LowerTranslation) < 2.0f * Settings.LINEAR_SLOP)
                {
                    // Prevent large angular corrections
                    C2 = MathUtils.Clamp(translation, -Settings.MAX_LINEAR_CORRECTION, Settings.MAX_LINEAR_CORRECTION);
                    linearError = MathUtils.Max(linearError, MathUtils.Abs(translation));
                    active = true;
                }
                else if (translation <= LowerTranslation)
                {
                    // Prevent large linear corrections and allow some slop.
                    C2 = MathUtils.Clamp(translation - LowerTranslation + Settings.LINEAR_SLOP, -Settings.MAX_LINEAR_CORRECTION, 0.0f);
                    linearError = MathUtils.Max(linearError, LowerTranslation - translation);
                    active = true;
                }
                else if (translation >= UpperTranslation)
                {
                    // Prevent large linear corrections and allow some slop.
                    C2 = MathUtils.Clamp(translation - UpperTranslation - Settings.LINEAR_SLOP, 0.0f, Settings.MAX_LINEAR_CORRECTION);
                    linearError = MathUtils.Max(linearError, translation - UpperTranslation);
                    active = true;
                }
            }

            if (active)
            {
                float k11 = mA + mB + iA * s1 * s1 + iB * s2 * s2;
                float k12 = iA * s1 + iB * s2;
                float k13 = iA * s1 * a1 + iB * s2 * a2;
                float k22 = iA + iB;
                if (k22 == 0.0f)
                {
                    // For fixed rotation
                    k22 = 1.0f;
                }
                float k23 = iA * a1 + iB * a2;
                float k33 = mA + mB + iA * a1 * a1 + iB * a2 * a2;

                Mat33 K = Pool.PopMat33();
                K.Ex.Set(k11, k12, k13);
                K.Ey.Set(k12, k22, k23);
                K.Ez.Set(k13, k23, k33);

                Vec3 C = Pool.PopVec3();
                C.X = C1.X;
                C.Y = C1.Y;
                C.Z = C2;

                K.Solve33ToOut(C.NegateLocal(), impulse);
                Pool.PushVec3(1);
                Pool.PushMat33(1);
            }
            else
            {
                float k11 = mA + mB + iA * s1 * s1 + iB * s2 * s2;
                float k12 = iA * s1 + iB * s2;
                float k22 = iA + iB;
                if (k22 == 0.0f)
                {
                    k22 = 1.0f;
                }

                Mat22 K = Pool.PopMat22();
                K.Ex.Set(k11, k12);
                K.Ey.Set(k12, k22);

                // temp is impulse1
                K.SolveToOut(C1.NegateLocal(), temp);
                C1.NegateLocal();

                impulse.X = temp.X;
                impulse.Y = temp.Y;
                impulse.Z = 0.0f;

                Pool.PushMat22(1);
            }

            float Px = impulse.X * perp.X + impulse.Z * axis.X;
            float Py = impulse.X * perp.Y + impulse.Z * axis.Y;
            float LA = impulse.X * s1 + impulse.Y + impulse.Z * a1;
            float LB = impulse.X * s2 + impulse.Y + impulse.Z * a2;

            cA.X -= mA * Px;
            cA.Y -= mA * Py;
            aA -= iA * LA;
            cB.X += mB * Px;
            cB.Y += mB * Py;
            aB += iB * LB;

            data.Positions[IndexA].C.Set(cA);
            data.Positions[IndexA].A = aA;
            data.Positions[IndexB].C.Set(cB);
            data.Positions[IndexB].A = aB;

            Pool.PushVec2(7);
            Pool.PushVec3(1);
            Pool.PushRot(2);

            return linearError <= Settings.LINEAR_SLOP && angularError <= Settings.ANGULAR_SLOP;
        }