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 WarmStart(ref Vector3Wide tangentX, ref Vector3Wide tangentY, ref Projection projection, ref BodyInertias inertiaA, ref Vector2Wide accumulatedImpulse, ref BodyVelocities wsvA) { ComputeJacobians(ref tangentX, ref tangentY, ref projection.OffsetA, out var jacobians); //TODO: If the previous frame and current frame are associated with different time steps, the previous frame's solution won't be a good solution anymore. //To compensate for this, the accumulated impulse should be scaled if dt changes. ApplyImpulse(ref jacobians, ref inertiaA, ref accumulatedImpulse, ref wsvA); }
public static void ApplyImpulse(ref Jacobians jacobians, ref BodyInertias inertiaA, ref Vector2Wide correctiveImpulse, ref BodyVelocities wsvA) { Matrix2x3Wide.Transform(correctiveImpulse, jacobians.LinearA, out var linearImpulseA); Matrix2x3Wide.Transform(correctiveImpulse, jacobians.AngularA, out var angularImpulseA); BodyVelocities correctiveVelocityA; Vector3Wide.Scale(linearImpulseA, inertiaA.InverseMass, out correctiveVelocityA.Linear); Symmetric3x3Wide.TransformWithoutOverlap(angularImpulseA, inertiaA.InverseInertiaTensor, out correctiveVelocityA.Angular); Vector3Wide.Add(wsvA.Linear, correctiveVelocityA.Linear, out wsvA.Linear); Vector3Wide.Add(wsvA.Angular, correctiveVelocityA.Angular, out wsvA.Angular); }
public static void ApplyImpulse(ref Jacobians jacobians, ref BodyInertias inertiaA, ref BodyInertias inertiaB, ref Vector2Wide correctiveImpulse, ref BodyVelocities wsvA, ref BodyVelocities wsvB) { Matrix2x3Wide.Transform(correctiveImpulse, jacobians.LinearA, out var linearImpulseA); Matrix2x3Wide.Transform(correctiveImpulse, jacobians.AngularA, out var angularImpulseA); Matrix2x3Wide.Transform(correctiveImpulse, jacobians.AngularB, out var angularImpulseB); BodyVelocities correctiveVelocityA, correctiveVelocityB; Vector3Wide.Scale(linearImpulseA, inertiaA.InverseMass, out correctiveVelocityA.Linear); Symmetric3x3Wide.TransformWithoutOverlap(angularImpulseA, inertiaA.InverseInertiaTensor, out correctiveVelocityA.Angular); Vector3Wide.Scale(linearImpulseA, inertiaB.InverseMass, out correctiveVelocityB.Linear); Symmetric3x3Wide.TransformWithoutOverlap(angularImpulseB, inertiaB.InverseInertiaTensor, out correctiveVelocityB.Angular); Vector3Wide.Add(wsvA.Linear, correctiveVelocityA.Linear, out wsvA.Linear); Vector3Wide.Add(wsvA.Angular, correctiveVelocityA.Angular, out wsvA.Angular); Vector3Wide.Subtract(wsvB.Linear, correctiveVelocityB.Linear, out wsvB.Linear); //note subtract- we based it on the LinearA jacobian. Vector3Wide.Add(wsvB.Angular, correctiveVelocityB.Angular, out wsvB.Angular); }
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); }
public static void Solve(ref Vector3Wide tangentX, ref Vector3Wide tangentY, ref Projection projection, ref BodyInertias inertiaA, ref Vector <float> maximumImpulse, ref Vector2Wide accumulatedImpulse, ref BodyVelocities wsvA) { ComputeJacobians(ref tangentX, ref tangentY, ref projection.OffsetA, out var jacobians); ComputeCorrectiveImpulse(ref wsvA, ref projection, ref jacobians, ref maximumImpulse, ref accumulatedImpulse, out var correctiveCSI); ApplyImpulse(ref jacobians, ref inertiaA, ref correctiveCSI, ref wsvA); }