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 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 Test() { var random = new Random(4); var timer = new Stopwatch(); var symmetricVectorSandwichTime = 0.0; var symmetricWideVectorSandwichTime = 0.0; var triangularWideVectorSandwichTime = 0.0; var symmetricWide2x3SandwichTime = 0.0; var triangularWide2x3SandwichTime = 0.0; var symmetricSkewSandwichTime = 0.0; var symmetricWideSkewSandwichTime = 0.0; var triangularWideSkewSandwichTime = 0.0; var symmetricRotationSandwichTime = 0.0; var symmetricWideRotationSandwichTime = 0.0; var triangularWideRotationSandwichTime = 0.0; var symmetricInvertTime = 0.0; var symmetricWideInvertTime = 0.0; var triangularWideInvertTime = 0.0; for (int i = 0; i < 1000; ++i) { var axis = Vector3.Normalize(new Vector3((float)random.NextDouble() * 2 - 1, (float)random.NextDouble() * 2 - 1, (float)random.NextDouble() * 2 - 1)); Vector3Wide.Broadcast(axis, out var axisWide); var rotation = Matrix3x3.CreateFromAxisAngle(axis, (float)random.NextDouble()); Matrix3x3Wide rotationWide; Vector3Wide.Broadcast(rotation.X, out rotationWide.X); Vector3Wide.Broadcast(rotation.Y, out rotationWide.Y); Vector3Wide.Broadcast(rotation.Z, out rotationWide.Z); var m2x3Wide = new Matrix2x3Wide() { X = axisWide, Y = new Vector3Wide { X = -axisWide.Y, Y = axisWide.Z, Z = axisWide.X } }; var triangular = new Symmetric3x3 { XX = (float)random.NextDouble() * 2 + 1, YX = (float)random.NextDouble() * 1 + 1, YY = (float)random.NextDouble() * 2 + 1, ZX = (float)random.NextDouble() * 1 + 1, ZY = (float)random.NextDouble() * 1 + 1, ZZ = (float)random.NextDouble() * 2 + 1, }; Symmetric3x3Wide triangularWide; triangularWide.XX = new Vector <float>(triangular.XX); triangularWide.YX = new Vector <float>(triangular.YX); triangularWide.YY = new Vector <float>(triangular.YY); triangularWide.ZX = new Vector <float>(triangular.ZX); triangularWide.ZY = new Vector <float>(triangular.ZY); triangularWide.ZZ = new Vector <float>(triangular.ZZ); var symmetric = new Matrix3x3 { X = new Vector3(triangular.XX, triangular.YX, triangular.ZX), Y = new Vector3(triangular.YX, triangular.YY, triangular.ZY), Z = new Vector3(triangular.ZX, triangular.ZY, triangular.ZZ), }; Matrix3x3Wide symmetricWide; Vector3Wide.Broadcast(symmetric.X, out symmetricWide.X); Vector3Wide.Broadcast(symmetric.Y, out symmetricWide.Y); Vector3Wide.Broadcast(symmetric.Z, out symmetricWide.Z); var symmetricVectorSandwich = new SymmetricVectorSandwich() { v = axis, symmetric = symmetric }; var symmetricWideVectorSandwich = new SymmetricWideVectorSandwich() { v = axisWide, symmetric = symmetricWide }; var triangularWideVectorSandwich = new TriangularWideVectorSandwich() { v = axisWide, triangular = triangularWide }; var symmetricWide2x3Sandwich = new SymmetricWide2x3Sandwich() { m = m2x3Wide, symmetric = symmetricWide }; var triangularWide2x3Sandwich = new TriangularWide2x3Sandwich() { m = m2x3Wide, triangular = triangularWide }; var symmetricSkewSandwich = new SymmetricSkewSandwich() { v = axis, symmetric = symmetric }; var symmetricWideSkewSandwich = new SymmetricWideSkewSandwich() { v = axisWide, symmetric = symmetricWide }; var triangularWideSkewSandwich = new TriangularWideSkewSandwich() { v = axisWide, triangular = triangularWide }; var symmetricSandwich = new SymmetricRotationSandwich() { rotation = rotation, symmetric = symmetric }; var symmetricWideSandwich = new SymmetricRotationSandwichWide() { rotation = rotationWide, symmetric = symmetricWide }; var triangularWideSandwich = new TriangularRotationSandwichWide() { rotation = rotationWide, triangular = triangularWide }; var symmetricInvert = new SymmetricInvert() { symmetric = symmetric }; var symmetricWideInvert = new SymmetricInvertWide() { symmetric = symmetricWide }; var triangularWideInvert = new TriangularInvertWide() { triangular = triangularWide }; const int innerIterations = 100000; symmetricVectorSandwichTime += TimeTest(innerIterations, ref symmetricVectorSandwich); symmetricWideVectorSandwichTime += TimeTest(innerIterations, ref symmetricWideVectorSandwich); triangularWideVectorSandwichTime += TimeTest(innerIterations, ref triangularWideVectorSandwich); symmetricWide2x3SandwichTime += TimeTest(innerIterations, ref symmetricWide2x3Sandwich); triangularWide2x3SandwichTime += TimeTest(innerIterations, ref triangularWide2x3Sandwich); symmetricSkewSandwichTime += TimeTest(innerIterations, ref symmetricSkewSandwich); symmetricWideSkewSandwichTime += TimeTest(innerIterations, ref symmetricWideSkewSandwich); triangularWideSkewSandwichTime += TimeTest(innerIterations, ref triangularWideSkewSandwich); symmetricRotationSandwichTime += TimeTest(innerIterations, ref symmetricSandwich); symmetricWideRotationSandwichTime += TimeTest(innerIterations, ref symmetricWideSandwich); triangularWideRotationSandwichTime += TimeTest(innerIterations, ref triangularWideSandwich); symmetricInvertTime += TimeTest(innerIterations, ref symmetricInvert); symmetricWideInvertTime += TimeTest(innerIterations, ref symmetricWideInvert); triangularWideInvertTime += TimeTest(innerIterations, ref triangularWideInvert); Compare(symmetricVectorSandwich.result, ref symmetricWideVectorSandwich.result); Compare(symmetricVectorSandwich.result, ref triangularWideVectorSandwich.result); Compare(ref symmetricWide2x3Sandwich.result, ref triangularWide2x3Sandwich.result); Compare(ref symmetricSkewSandwich.result, ref symmetricWideSkewSandwich.result); Compare(ref symmetricSkewSandwich.result, ref triangularWideSkewSandwich.result); Compare(ref symmetricSandwich.result, ref symmetricWideSandwich.result); Compare(ref symmetricSandwich.result, ref triangularWideSandwich.result); Compare(ref symmetricInvert.result, ref symmetricWideInvert.result); Compare(ref symmetricInvert.result, ref triangularWideInvert.result); } Console.WriteLine($"Symmetric vector sandwich: {symmetricVectorSandwichTime}"); Console.WriteLine($"Symmetric wide vector sandwich: {symmetricWideVectorSandwichTime}"); Console.WriteLine($"Triangular wide vector sandwich: {triangularWideVectorSandwichTime}"); Console.WriteLine($"Symmetric wide 2x3 sandwich: {symmetricWide2x3SandwichTime}"); Console.WriteLine($"Triangular wide 2x3 sandwich: {triangularWide2x3SandwichTime}"); Console.WriteLine($"Symmetric skew sandwich: {symmetricSkewSandwichTime}"); Console.WriteLine($"Symmetric wide skew sandwich: {symmetricWideSkewSandwichTime}"); Console.WriteLine($"Triangular wide skew sandwich: {triangularWideSkewSandwichTime}"); Console.WriteLine($"Symmetric rotation sandwich: {symmetricRotationSandwichTime}"); Console.WriteLine($"Symmetric wide rotation sandwich: {symmetricWideRotationSandwichTime}"); Console.WriteLine($"Triangular wide rotation sandwich: {triangularWideRotationSandwichTime}"); Console.WriteLine($"Symmetric invert: {symmetricInvertTime}"); Console.WriteLine($"Symmetric wide invert: {symmetricWideInvertTime}"); Console.WriteLine($"Triangular wide invert: {triangularWideInvertTime}"); }
public void Do() { Matrix2x3Wide.MultiplyWithoutOverlap(m, symmetric, out var intermediate); Matrix2x3Wide.MultiplyByTransposeWithoutOverlap(intermediate, m, out result); }