SIMDPrototyping.PenetrationConstraint.Prestep C# (CSharp) Method

        public void Prestep(float inverseDt)
        {
            //Single constraint version.
            //C = dot(Pa - Pb, N) > 0
            //Jacobians:
            //LinearA: N
            //AngularA: cross(OffsetPa, N)
            //LinearB: -N
            //AngularB: -cross(OffsetPb, N)

            LinearJacobianA = ContactNormal;
            LinearJacobianB = -ContactNormal;
            var offsetA = ContactPosition - ConnectionA.Position;
            var offsetB = ContactPosition - ConnectionB.Position;

            //Note scalar implementation; JIT doesn't do shuffles yet.
            Vector3Ex.Cross(ref offsetA, ref ContactNormal, out AngularJacobianA);
            Vector3Ex.Cross(ref ContactNormal, ref offsetB, out AngularJacobianB); //note negation->parameter reverse

            //Allow velocity that closes a gap, and apply penetration correction against positive depth.
            //Bounciness not yet included.
            PenetrationBias = ContactPenetration * inverseDt;
            PenetrationBias = -Math.Min(Math.Min(PenetrationBias, PenetrationBias * 0.2f), 0.2f);

            LinearJacobianITA = LinearJacobianA * ConnectionA.InverseMass;
            LinearJacobianITB = LinearJacobianB * ConnectionB.InverseMass;
            Matrix3x3.Transform(ref AngularJacobianA, ref ConnectionA.InertiaTensorInverse, out AngularJacobianITA);
            Matrix3x3.Transform(ref AngularJacobianB, ref ConnectionB.InertiaTensorInverse, out AngularJacobianITB);
            float inverseEffectiveMass = ConnectionA.InverseMass + ConnectionB.InverseMass + Vector3.Dot(AngularJacobianITA, AngularJacobianITA) + Vector3.Dot(AngularJacobianITB, AngularJacobianITB);

            const float CollisionSoftness = 5;
            Softness = CollisionSoftness * inverseEffectiveMass * inverseDt;
            EffectiveMass = 1f / (Softness + inverseEffectiveMass);
        }

        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}");
        }