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); }
public static void Prestep(ref Vector3Wide tangentX, ref Vector3Wide tangentY, ref Vector3Wide offsetA, ref BodyInertias inertiaA, out Projection projection) { ComputeJacobians(ref tangentX, ref tangentY, ref offsetA, out var jacobians); //Compute effective mass matrix contributions. Symmetric2x2Wide.SandwichScale(jacobians.LinearA, inertiaA.InverseMass, out var linearContributionA); Symmetric3x3Wide.MatrixSandwich(jacobians.AngularA, inertiaA.InverseInertiaTensor, out var angularContributionA); //No softening; this constraint is rigid by design. (It does support a maximum force, but that is distinct from a proper damping ratio/natural frequency.) Symmetric2x2Wide.Add(linearContributionA, angularContributionA, out var inverseEffectiveMass); Symmetric2x2Wide.InvertWithoutOverlap(inverseEffectiveMass, out projection.EffectiveMass); projection.OffsetA = offsetA; //Note that friction constraints have no bias velocity. They target zero velocity. }
static void Compare(ref Matrix2x2Wide m, ref Symmetric2x2Wide t) { var se12 = MeasureError(m.X.Y[0], m.Y.X[0]); if (se12 > epsilon) { throw new Exception("Matrix not symmetric; shouldn't compare against a symmetric triangular matrix."); } var e11 = MeasureError(m.X.X[0], t.XX[0]); var e21 = MeasureError(m.Y.X[0], t.YX[0]); var e22 = MeasureError(m.Y.Y[0], t.YY[0]); if (e11 > epsilon || e21 > epsilon || e22 > epsilon) { throw new Exception("Too much error in Matrix2x2Wide vs Triangular2x2Wide."); } }
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); }