public static void ComputeCorrectiveImpulse(ref BodyVelocities wsvA, ref BodyVelocities wsvB, ref TangentFriction.Projection data, ref Jacobians jacobians,
                                                    ref Vector <float> maximumImpulse, ref Vector2Wide accumulatedImpulse, out Vector2Wide correctiveCSI)
        {
            Matrix2x3Wide.TransformByTransposeWithoutOverlap(wsvA.Linear, jacobians.LinearA, out var csvaLinear);
            Matrix2x3Wide.TransformByTransposeWithoutOverlap(wsvA.Angular, jacobians.AngularA, out var csvaAngular);
            Matrix2x3Wide.TransformByTransposeWithoutOverlap(wsvB.Linear, jacobians.LinearA, out var csvbLinear);
            Matrix2x3Wide.TransformByTransposeWithoutOverlap(wsvB.Angular, jacobians.AngularB, out var csvbAngular);
            //Note that the velocity in constraint space is (csvaLinear - csvbLinear + csvaAngular + csvbAngular).
            //The subtraction there is due to sharing the linear jacobian between both bodies3D.
            //In the following, we need to compute the constraint space *violating* velocity- which is the negation of the above velocity in constraint space.
            //So, (csvbLinear - csvaLinear - (csvaAngular + csvbAngular)).
            Vector2Wide.Subtract(csvbLinear, csvaLinear, out var csvLinear);
            Vector2Wide.Add(csvaAngular, csvbAngular, out var csvAngular);
            Vector2Wide.Subtract(csvLinear, csvAngular, out var csv);

            Symmetric2x2Wide.TransformWithoutOverlap(csv, data.EffectiveMass, out var csi);

            var previousAccumulated = accumulatedImpulse;

            Vector2Wide.Add(accumulatedImpulse, csi, out accumulatedImpulse);
            //The maximum force of friction depends upon the normal impulse. The maximum is supplied per iteration.
            Vector2Wide.Length(accumulatedImpulse, out var accumulatedMagnitude);
            //Note division by zero guard.
            var scale = Vector.Min(Vector <float> .One, maximumImpulse / Vector.Max(new Vector <float>(1e-16f), accumulatedMagnitude));

            Vector2Wide.Scale(accumulatedImpulse, scale, out accumulatedImpulse);

            Vector2Wide.Subtract(accumulatedImpulse, previousAccumulated, out correctiveCSI);
        }
Beispiel #2
0
        public static void ComputeCorrectiveImpulse(ref BodyVelocities wsvA, ref Projection data, ref Jacobians jacobians,
                                                    ref Vector <float> maximumImpulse, ref Vector2Wide accumulatedImpulse, out Vector2Wide correctiveCSI)
        {
            Matrix2x3Wide.TransformByTransposeWithoutOverlap(wsvA.Linear, jacobians.LinearA, out var csvaLinear);
            Matrix2x3Wide.TransformByTransposeWithoutOverlap(wsvA.Angular, jacobians.AngularA, out var csvaAngular);
            Vector2Wide.Add(csvaLinear, csvaAngular, out var csv);
            //Required corrective velocity is the negation of the current constraint space velocity.
            Symmetric2x2Wide.TransformWithoutOverlap(csv, data.EffectiveMass, out var negativeCSI);

            var previousAccumulated = accumulatedImpulse;

            Vector2Wide.Subtract(accumulatedImpulse, negativeCSI, out accumulatedImpulse);
            //The maximum force of friction depends upon the normal impulse. The maximum is supplied per iteration.
            Vector2Wide.Length(accumulatedImpulse, out var accumulatedMagnitude);
            //Note division by zero guard.
            var scale = Vector.Min(Vector <float> .One, maximumImpulse / Vector.Max(new Vector <float>(1e-16f), accumulatedMagnitude));

            Vector2Wide.Scale(accumulatedImpulse, scale, out accumulatedImpulse);

            Vector2Wide.Subtract(accumulatedImpulse, previousAccumulated, out correctiveCSI);
        }