SIMDPrototyping.PenetrationConstraint.SolveIteration C# (CSharp) Method

        public void SolveIteration()
        {
            var lambda = EffectiveMass * (
                Vector3.Dot(LinearJacobianA, ConnectionA.LinearVelocity) +
                Vector3.Dot(AngularJacobianA, ConnectionA.AngularVelocity) +
                Vector3.Dot(LinearJacobianB, ConnectionB.LinearVelocity) +
                Vector3.Dot(AngularJacobianB, ConnectionB.AngularVelocity) +
                PenetrationBias - AccumulatedImpulse * Softness);

            var previous = AccumulatedImpulse;
            AccumulatedImpulse = Math.Max(0, AccumulatedImpulse + lambda);
            lambda = AccumulatedImpulse - previous;

            ApplyImpulse(lambda);
        }

        public unsafe static void Test()
        {
            var identityMatrix = new Matrix3x3 {
                X = new Vector3(1, 0, 0), Y = new Vector3(0, 1, 0), Z = new Vector3(0, 0, 1)
            };
            var a = new RigidBody
            {
                Position             = new Vector3(0, 0, 0),
                Orientation          = identityMatrix,
                InertiaTensorInverse = identityMatrix,
                InverseMass          = 1,
                LinearVelocity       = new Vector3(0, 0, 0)
            };
            var b = new RigidBody
            {
                Position             = new Vector3(0, 1, 0),
                Orientation          = identityMatrix,
                InertiaTensorInverse = identityMatrix,
                InverseMass          = 1,
                LinearVelocity       = new Vector3(0, 0, 0)
            };
            PenetrationConstraint constraint = new PenetrationConstraint
            {
                ConnectionA        = a,
                ConnectionB        = b,
                ContactPosition    = new Vector3(),
                ContactNormal      = new Vector3(0, 1, 0),
                ContactPenetration = 0
            };
            float dt        = 1 / 60f;
            float inverseDt = 1 / dt;

            constraint.Prestep(inverseDt);
            constraint.WarmStart();
            constraint.SolveIteration();

            const int testCount      = VectorizedConstraintTest.TestCount * 4;
            const int iterationCount = VectorizedConstraintTest.IterationCount;

            var startTime = Stopwatch.GetTimestamp() / (double)Stopwatch.Frequency;

            for (int i = 0; i < testCount; ++i)
            {
                constraint.Prestep(inverseDt);
                constraint.WarmStart();

                for (int iterationIndex = 0; iterationIndex < iterationCount; ++iterationIndex)
                {
                    constraint.SolveIteration();
                }
            }


            var endtime = Stopwatch.GetTimestamp() / (double)Stopwatch.Frequency;

            Console.WriteLine($"Scalar New: {endtime - startTime}, acc: {constraint.AccumulatedImpulse}");
        }