SIMDPrototyping.Vector3Width4.Dot C# (CSharp) Method

        internal static void Dot(ref Vector3Width4 a, ref Vector3Width4 b, out Vector4 result)
        {
            result = a.X * b.X + a.Y * b.Y + a.Z * b.Z;
        }

        public void SolveIteration()
        {
            Vector3Width4 linearVelocityA  = new Vector3Width4(ref VelocitiesA0.LinearVelocity, ref VelocitiesA1.LinearVelocity, ref VelocitiesA2.LinearVelocity, ref VelocitiesA3.LinearVelocity);
            Vector3Width4 angularVelocityA = new Vector3Width4(ref VelocitiesA0.AngularVelocity, ref VelocitiesA1.AngularVelocity, ref VelocitiesA2.AngularVelocity, ref VelocitiesA3.AngularVelocity);
            Vector3Width4 linearVelocityB  = new Vector3Width4(ref VelocitiesB0.LinearVelocity, ref VelocitiesB1.LinearVelocity, ref VelocitiesB2.LinearVelocity, ref VelocitiesB3.LinearVelocity);
            Vector3Width4 angularVelocityB = new Vector3Width4(ref VelocitiesB0.AngularVelocity, ref VelocitiesB1.AngularVelocity, ref VelocitiesB2.AngularVelocity, ref VelocitiesB3.AngularVelocity);

            Vector4 linearA, angularA, linearB, angularB;

            Vector3Width4.Dot(ref LinearJacobianA, ref linearVelocityA, out linearA);
            Vector3Width4.Dot(ref AngularJacobianA, ref angularVelocityA, out angularA);
            Vector3Width4.Dot(ref LinearJacobianB, ref linearVelocityB, out linearB);
            Vector3Width4.Dot(ref AngularJacobianB, ref angularVelocityB, out angularB);
            var lambda = EffectiveMass * (linearA + angularA + linearB + angularB + PenetrationBias - AccumulatedImpulse * Softness);

            var previous = AccumulatedImpulse;

            AccumulatedImpulse = Vector4.Max(Vector4.Zero, AccumulatedImpulse + lambda);
            lambda             = AccumulatedImpulse - previous;

            ApplyImpulse(ref lambda, ref linearVelocityA, ref angularVelocityA, ref linearVelocityB, ref angularVelocityB);

            Vector3Width4.Transpose(ref linearVelocityA, out VelocitiesA0.LinearVelocity, out VelocitiesA1.LinearVelocity, out VelocitiesA2.LinearVelocity, out VelocitiesA3.LinearVelocity);
            Vector3Width4.Transpose(ref linearVelocityB, out VelocitiesB0.LinearVelocity, out VelocitiesB1.LinearVelocity, out VelocitiesB2.LinearVelocity, out VelocitiesB3.LinearVelocity);
            Vector3Width4.Transpose(ref angularVelocityA, out VelocitiesA0.AngularVelocity, out VelocitiesA1.AngularVelocity, out VelocitiesA2.AngularVelocity, out VelocitiesA3.AngularVelocity);
            Vector3Width4.Transpose(ref angularVelocityB, out VelocitiesB0.AngularVelocity, out VelocitiesB1.AngularVelocity, out VelocitiesB2.AngularVelocity, out VelocitiesB3.AngularVelocity);
        }