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);
}
