public static void ComputeJacobians(ref Vector3Wide tangentX, ref Vector3Wide tangentY, ref Vector3Wide offsetA, ref Vector3Wide offsetB,
out Jacobians jacobians)
{
//Two velocity constraints:
//dot(velocity(p, A), tangentX) = dot(velocity(p, B), tangentX)
//dot(velocity(p, A), tangentY) = dot(velocity(p, B), tangentY)
//where velocity(p, A) is the velocity of a point p attached to object A.
//velocity(p, A) = linearVelocityA + angularVelocityA x (p - positionA) = linearVelocityA + angularVelocityA x offsetA
//so:
//dot(velocity(p, A), tangentX) = dot(linearVelocityA, tangentX) + dot(angularVelocityA x offsetA, tangentX)
//dot(velocity(p, A), tangentX) = dot(linearVelocityA, tangentX) + dot(offsetA x tangentX, angularVelocityA)
//Restating the two constraints:
//dot(linearVelocityA, tangentX) + dot(offsetA x tangentX, angularVelocityA) = dot(linearVelocityB, tangentX) + dot(offsetB x tangentX, angularVelocityB)
//dot(linearVelocityA, tangentY) + dot(offsetA x tangentY, angularVelocityA) = dot(linearVelocityB, tangentY) + dot(offsetB x tangentY, angularVelocityB)
//dot(linearVelocityA, tangentX) + dot(offsetA x tangentX, angularVelocityA) - dot(linearVelocityB, tangentX) - dot(offsetB x tangentX, angularVelocityB) = 0
//dot(linearVelocityA, tangentY) + dot(offsetA x tangentY, angularVelocityA) - dot(linearVelocityB, tangentY) - dot(offsetB x tangentY, angularVelocityB) = 0
//Since there are two constraints (2DOFs), there are two rows in the jacobian, which based on the above is:
//jLinearA = [ tangentX ]
// [ tangentY ]
//jAngularA = [ offsetA x tangentX ]
// [ offsetA x tangentY ]
//jLinearB = [ -tangentX ]
// [ -tangentY ]
//jAngularB = [ -offsetB x tangentX ] = [ tangentX x offsetB ]
// [ -offsetB x tangentY ] [ tangentY x offsetB ]
//TODO: there would be a minor benefit in eliminating this copy manually, since it's very likely that the compiler won't. And it's probably also introducing more locals init.
jacobians.LinearA.X = tangentX;
jacobians.LinearA.Y = tangentY;
Vector3Wide.CrossWithoutOverlap(offsetA, tangentX, out jacobians.AngularA.X);
Vector3Wide.CrossWithoutOverlap(offsetA, tangentY, out jacobians.AngularA.Y);
Vector3Wide.CrossWithoutOverlap(tangentX, offsetB, out jacobians.AngularB.X);
Vector3Wide.CrossWithoutOverlap(tangentY, offsetB, out jacobians.AngularB.Y);
}
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 ComputeJacobians(ref Vector3Wide tangentX, ref Vector3Wide tangentY, ref Vector3Wide offsetA, out Jacobians jacobians)
{
//TODO: there would be a minor benefit in eliminating this copy manually, since it's very likely that the compiler won't. And it's probably also introducing more locals init.
jacobians.LinearA.X = tangentX;
jacobians.LinearA.Y = tangentY;
Vector3Wide.CrossWithoutOverlap(offsetA, tangentX, out jacobians.AngularA.X);
Vector3Wide.CrossWithoutOverlap(offsetA, tangentY, out jacobians.AngularA.Y);
}
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);
}
