public static void ApplyImpulse(ref Vector3Wide angularJacobianA, ref BodyInertias inertiaA,
ref Vector <float> correctiveImpulse, ref BodyVelocities wsvA)
{
Vector3Wide.Scale(angularJacobianA, correctiveImpulse, out var worldCorrectiveImpulseA);
Symmetric3x3Wide.TransformWithoutOverlap(worldCorrectiveImpulseA, inertiaA.InverseInertiaTensor, out var worldCorrectiveVelocityA);
Vector3Wide.Add(wsvA.Angular, worldCorrectiveVelocityA, out wsvA.Angular);
}
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 void WarmStart(ref BodyVelocities wsvA, ref ContactManifold1OneBodyProjection projection, ref Contact1AccumulatedImpulses accumulatedImpulses)
{
Helpers.BuildOrthnormalBasis(ref projection.Normal, out var x, out var z);
TangentFrictionOneBody.WarmStart(ref x, ref z, ref projection.Tangent, ref projection.InertiaA, ref accumulatedImpulses.Tangent, ref wsvA);
PenetrationLimit1OneBody.WarmStart(ref projection.Penetration, ref projection.InertiaA,
ref projection.Normal,
ref accumulatedImpulses.Penetration0, ref wsvA);
}
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 WarmStart(
ref Projection projection, ref BodyInertias inertiaA, ref BodyInertias inertiaB, ref Vector3Wide normal,
ref Vector <float> accumulatedImpulse0,
ref Vector <float> accumulatedImpulse1, ref BodyVelocities wsvA, ref BodyVelocities wsvB)
{
ApplyImpulse(ref projection.Penetration0, ref inertiaA, ref inertiaB, ref normal, ref accumulatedImpulse0, ref wsvA, ref wsvB);
ApplyImpulse(ref projection.Penetration1, ref inertiaA, ref inertiaB, ref normal, ref accumulatedImpulse1, ref wsvA, ref wsvB);
}
public static void Solve(ref BodyVelocities velocityA, ref BodyVelocities velocityB, ref Vector3Wide offsetA, ref Vector3Wide offsetB,
ref Vector3Wide biasVelocity, ref Symmetric3x3Wide effectiveMass, ref Vector <float> softnessImpulseScale, ref Vector3Wide accumulatedImpulse, ref BodyInertias inertiaA, ref BodyInertias inertiaB)
{
ComputeCorrectiveImpulse(ref velocityA, ref velocityB, ref offsetA, ref offsetB, ref biasVelocity, ref effectiveMass, ref softnessImpulseScale, ref accumulatedImpulse, out var correctiveImpulse);
//This function does not have a maximum impulse limit, so no clamping is required.
Vector3Wide.Add(accumulatedImpulse, correctiveImpulse, out accumulatedImpulse);
ApplyImpulse(ref velocityA, ref velocityB, ref offsetA, ref offsetB, ref inertiaA, ref inertiaB, ref correctiveImpulse);
}
public static void Solve(ref Projection projection, ref BodyInertias inertiaA, ref BodyInertias inertiaB, ref Vector3Wide normal,
ref Vector <float> accumulatedImpulse0,
ref Vector <float> accumulatedImpulse1, ref BodyVelocities wsvA, ref BodyVelocities wsvB)
{
ComputeCorrectiveImpulse(ref wsvA, ref wsvB, ref projection.Penetration0, ref normal, ref projection.SoftnessImpulseScale, ref accumulatedImpulse0, out var correctiveCSI0);
ApplyImpulse(ref projection.Penetration0, ref inertiaA, ref inertiaB, ref normal, ref correctiveCSI0, ref wsvA, ref wsvB);
ComputeCorrectiveImpulse(ref wsvA, ref wsvB, ref projection.Penetration1, ref normal, ref projection.SoftnessImpulseScale, ref accumulatedImpulse1, out var correctiveCSI1);
ApplyImpulse(ref projection.Penetration1, ref inertiaA, ref inertiaB, ref normal, ref correctiveCSI1, ref wsvA, ref wsvB);
}
public static void Solve(ref BodyVelocities velocityA, ref BodyVelocities velocityB, ref Vector3Wide offsetA, ref Vector3Wide offsetB,
ref Vector3Wide biasVelocity, ref Symmetric3x3Wide effectiveMass, ref Vector <float> softnessImpulseScale, ref Vector <float> maximumImpulse, ref Vector3Wide accumulatedImpulse, ref BodyInertias inertiaA, ref BodyInertias inertiaB)
{
ComputeCorrectiveImpulse(ref velocityA, ref velocityB, ref offsetA, ref offsetB, ref biasVelocity, ref effectiveMass, ref softnessImpulseScale, ref accumulatedImpulse, out var correctiveImpulse);
//This function DOES have a maximum impulse limit.
ServoSettingsWide.ClampImpulse(maximumImpulse, ref accumulatedImpulse, ref correctiveImpulse);
ApplyImpulse(ref velocityA, ref velocityB, ref offsetA, ref offsetB, ref inertiaA, ref inertiaB, ref correctiveImpulse);
}
public static void ApplyImpulse(ref Vector3Wide angularJacobianA, ref BodyInertias inertiaA, ref BodyInertias inertiaB,
ref Vector <float> correctiveImpulse, ref BodyVelocities wsvA, ref BodyVelocities wsvB)
{
Vector3Wide.Scale(ref angularJacobianA, ref correctiveImpulse, out var worldCorrectiveImpulseA);
Triangular3x3Wide.TransformBySymmetricWithoutOverlap(ref worldCorrectiveImpulseA, ref inertiaA.InverseInertiaTensor, out var worldCorrectiveVelocityA);
Triangular3x3Wide.TransformBySymmetricWithoutOverlap(ref worldCorrectiveImpulseA, ref inertiaB.InverseInertiaTensor, out var worldCorrectiveVelocityB);
Vector3Wide.Add(ref wsvA.AngularVelocity, ref worldCorrectiveVelocityA, out wsvA.AngularVelocity);
Vector3Wide.Subtract(ref wsvB.AngularVelocity, ref worldCorrectiveVelocityB, out wsvB.AngularVelocity);
}
static void TestScatter(Context context)
{
var a = new BodyVelocities();
var b = new BodyVelocities();
for (int i = 0; i < context.ConstraintBundleCount; ++i)
{
GatherScatter.ScatterVelocities(ref context.BodyVelocities, ref context.BodyReferences[i], Vector <float> .Count, ref a, ref b);
}
}
public void Solve(ref BodyVelocities wsvA, ref ContactManifold1OneBodyProjection projection, ref Contact1AccumulatedImpulses accumulatedImpulses)
{
Helpers.BuildOrthnormalBasis(ref projection.Normal, out var x, out var z);
var maximumTangentImpulse = projection.PremultipliedFrictionCoefficient * accumulatedImpulses.Penetration0;
TangentFrictionOneBody.Solve(ref x, ref z, ref projection.Tangent, ref projection.InertiaA, ref maximumTangentImpulse, ref accumulatedImpulses.Tangent, ref wsvA);
//Note that we solve the penetration constraints after the friction constraints.
//This makes the penetration constraints more authoritative at the cost of the first iteration of the first frame of an impact lacking friction influence.
//It's a pretty minor effect either way.
PenetrationLimit1OneBody.Solve(ref projection.Penetration, ref projection.InertiaA, ref projection.Normal,
ref accumulatedImpulses.Penetration0, 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 void WarmStart(ref BodyVelocities wsvA, ref BodyVelocities wsvB, ref Contact4Projection projection, ref Contact4AccumulatedImpulses accumulatedImpulses)
{
Helpers.BuildOrthnormalBasis(ref projection.Normal, out var x, out var z);
TangentFriction.WarmStart(ref x, ref z, ref projection.Tangent, ref projection.InertiaA, ref projection.InertiaB, ref accumulatedImpulses.Tangent, ref wsvA, ref wsvB);
PenetrationLimit4.WarmStart(ref projection.Penetration, ref projection.InertiaA, ref projection.InertiaB,
ref projection.Normal,
ref accumulatedImpulses.Penetration0,
ref accumulatedImpulses.Penetration1,
ref accumulatedImpulses.Penetration2,
ref accumulatedImpulses.Penetration3, ref wsvA, ref wsvB);
TwistFriction.WarmStart(ref projection.Normal, ref projection.InertiaA, ref projection.InertiaB, ref accumulatedImpulses.Twist, ref wsvA, ref wsvB);
}
public static void ApplyImpulse(ref PenetrationLimitOneBodyProjection projection, ref BodyInertias inertiaA, ref Vector3Wide normal,
ref Vector <float> correctiveImpulse,
ref BodyVelocities wsvA)
{
var linearVelocityChangeA = correctiveImpulse * inertiaA.InverseMass;
Vector3Wide.Scale(ref normal, ref linearVelocityChangeA, out var correctiveVelocityALinearVelocity);
Vector3Wide.Scale(ref projection.AngularA, ref correctiveImpulse, out var correctiveAngularImpulseA);
Triangular3x3Wide.TransformBySymmetricWithoutOverlap(ref correctiveAngularImpulseA, ref inertiaA.InverseInertiaTensor, out var correctiveVelocityAAngularVelocity);
Vector3Wide.Add(ref wsvA.LinearVelocity, ref correctiveVelocityALinearVelocity, out wsvA.LinearVelocity);
Vector3Wide.Add(ref wsvA.AngularVelocity, ref correctiveVelocityAAngularVelocity, out wsvA.AngularVelocity);
}
public static void ComputeCorrectiveImpulse(ref Vector3Wide angularJacobianA, ref TwistFrictionProjection projection,
ref BodyVelocities wsvA, ref Vector <float> maximumImpulse,
ref Vector <float> accumulatedImpulse, out Vector <float> correctiveCSI)
{
Vector3Wide.Dot(wsvA.Angular, angularJacobianA, out var csvA);
var negativeCSI = csvA * projection.EffectiveMass; //Since there is no bias or softness to give us the negative, we just do it when we apply to the accumulated impulse.
var previousAccumulated = accumulatedImpulse;
//The maximum force of friction depends upon the normal impulse.
accumulatedImpulse = Vector.Min(maximumImpulse, Vector.Max(-maximumImpulse, accumulatedImpulse - negativeCSI));
correctiveCSI = accumulatedImpulse - previousAccumulated;
}
public static void ComputeCorrectiveImpulse(ref Vector3Wide angularJacobianA, ref TwistFrictionProjection projection,
ref BodyVelocities wsvA, ref BodyVelocities wsvB, ref Vector <float> maximumImpulse,
ref Vector <float> accumulatedImpulse, out Vector <float> correctiveCSI)
{
Vector3Wide.Dot(ref wsvA.AngularVelocity, ref angularJacobianA, out var csvA);
Vector3Wide.Dot(ref wsvB.AngularVelocity, ref angularJacobianA, out var negatedCSVB);
var negatedCSI = (csvA - negatedCSVB) * projection.EffectiveMass; //Since there is no bias or softness to give us the negative, we just do it when we apply to the accumulated impulse.
var previousAccumulated = accumulatedImpulse;
accumulatedImpulse = Vector.Min(maximumImpulse, Vector.Max(-maximumImpulse, accumulatedImpulse - negatedCSI));
correctiveCSI = accumulatedImpulse - previousAccumulated;
}
public static void ComputeCorrectiveImpulse(ref BodyVelocities wsvA,
ref PenetrationLimitOneBodyProjection projection,
ref Vector3Wide normal, ref Vector <float> softnessImpulseScale,
ref Vector <float> accumulatedImpulse, out Vector <float> correctiveCSI)
{
//Note that we do NOT use pretransformed jacobians here; the linear jacobian sharing (normal) meant that we had the effective mass anyway.
Vector3Wide.Dot(ref wsvA.LinearVelocity, ref normal, out var csvaLinear);
Vector3Wide.Dot(ref wsvA.AngularVelocity, ref projection.AngularA, out var csvaAngular);
//Compute negated version to avoid the need for an explicit negate.
var negatedCSI = accumulatedImpulse * softnessImpulseScale + (csvaLinear + csvaAngular - projection.BiasVelocity) * projection.EffectiveMass;
var previousAccumulated = accumulatedImpulse;
accumulatedImpulse = Vector.Max(Vector <float> .Zero, accumulatedImpulse - negatedCSI);
correctiveCSI = accumulatedImpulse - previousAccumulated;
}
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 ApplyImpulse(ref BodyVelocities velocityA, ref BodyVelocities velocityB,
ref Vector3Wide offsetA, ref Vector3Wide offsetB, ref BodyInertias inertiaA, ref BodyInertias inertiaB, ref Vector3Wide constraintSpaceImpulse)
{
Vector3Wide.CrossWithoutOverlap(offsetA, constraintSpaceImpulse, out var wsi);
Symmetric3x3Wide.TransformWithoutOverlap(wsi, inertiaA.InverseInertiaTensor, out var change);
Vector3Wide.Add(velocityA.Angular, change, out velocityA.Angular);
Vector3Wide.Scale(constraintSpaceImpulse, inertiaA.InverseMass, out change);
Vector3Wide.Add(velocityA.Linear, change, out velocityA.Linear);
Vector3Wide.CrossWithoutOverlap(constraintSpaceImpulse, offsetB, out wsi); //note flip-negation
Symmetric3x3Wide.TransformWithoutOverlap(wsi, inertiaB.InverseInertiaTensor, out change);
Vector3Wide.Add(velocityB.Angular, change, out velocityB.Angular);
Vector3Wide.Scale(constraintSpaceImpulse, inertiaB.InverseMass, out change);
Vector3Wide.Subtract(velocityB.Linear, change, out velocityB.Linear); //note subtraction; the jacobian is -I
}
public static void ComputeCorrectiveImpulse(ref BodyVelocities velocityA, ref BodyVelocities velocityB, ref Vector3Wide offsetA, ref Vector3Wide offsetB,
ref Vector3Wide biasVelocity, ref Symmetric3x3Wide effectiveMass, ref Vector <float> softnessImpulseScale, ref Vector3Wide accumulatedImpulse, out Vector3Wide correctiveImpulse)
{
//csi = projection.BiasImpulse - accumulatedImpulse * projection.SoftnessImpulseScale - (csiaLinear + csiaAngular + csibLinear + csibAngular);
//Note subtraction; jLinearB = -I.
Vector3Wide.Subtract(velocityA.Linear, velocityB.Linear, out var csv);
Vector3Wide.CrossWithoutOverlap(velocityA.Angular, offsetA, out var angularCSV);
Vector3Wide.Add(csv, angularCSV, out csv);
//Note reversed cross order; matches the jacobian -CrossMatrix(offsetB).
Vector3Wide.CrossWithoutOverlap(offsetB, velocityB.Angular, out angularCSV);
Vector3Wide.Add(csv, angularCSV, out csv);
Vector3Wide.Subtract(biasVelocity, csv, out csv);
Symmetric3x3Wide.TransformWithoutOverlap(csv, effectiveMass, out correctiveImpulse);
Vector3Wide.Scale(accumulatedImpulse, softnessImpulseScale, out var softness);
Vector3Wide.Subtract(correctiveImpulse, softness, out correctiveImpulse);
}
public static void ApplyImpulse(ref Projection2Body1DOF data, ref Vector <float> correctiveImpulse,
ref BodyVelocities wsvA, ref BodyVelocities wsvB)
{
//Applying the impulse requires transforming the constraint space impulse into a world space velocity change.
//The first step is to transform into a world space impulse, which requires transforming by the transposed jacobian
//(transpose(jacobian) goes from world to constraint space, jacobian goes from constraint to world space).
//That world space impulse is then converted to a corrective velocity change by scaling the impulse by the inverse mass/inertia.
//As an optimization for constraints with smaller jacobians, the jacobian * (inertia or mass) transform is precomputed.
BodyVelocities correctiveVelocityA, correctiveVelocityB;
Vector3Wide.Scale(data.CSIToWSVLinearA, correctiveImpulse, out correctiveVelocityA.Linear);
Vector3Wide.Scale(data.CSIToWSVAngularA, correctiveImpulse, out correctiveVelocityA.Angular);
Vector3Wide.Scale(data.CSIToWSVLinearB, correctiveImpulse, out correctiveVelocityB.Linear);
Vector3Wide.Scale(data.CSIToWSVAngularB, correctiveImpulse, out correctiveVelocityB.Angular);
Vector3Wide.Add(correctiveVelocityA.Linear, wsvA.Linear, out wsvA.Linear);
Vector3Wide.Add(correctiveVelocityA.Angular, wsvA.Angular, out wsvA.Angular);
Vector3Wide.Add(correctiveVelocityB.Linear, wsvB.Linear, out wsvB.Linear);
Vector3Wide.Add(correctiveVelocityB.Angular, wsvB.Angular, out wsvB.Angular);
}
public static void ApplyImpulse(ref PenetrationLimitProjection projection, ref BodyInertias inertiaA, ref BodyInertias inertiaB, ref Vector3Wide normal,
ref Vector <float> correctiveImpulse,
ref BodyVelocities wsvA, ref BodyVelocities wsvB)
{
var linearVelocityChangeA = correctiveImpulse * inertiaA.InverseMass;
Vector3Wide.Scale(normal, linearVelocityChangeA, out var correctiveVelocityALinearVelocity);
Vector3Wide.Scale(projection.AngularA, correctiveImpulse, out var correctiveAngularImpulseA);
Symmetric3x3Wide.TransformWithoutOverlap(correctiveAngularImpulseA, inertiaA.InverseInertiaTensor, out var correctiveVelocityAAngularVelocity);
var linearVelocityChangeB = correctiveImpulse * inertiaB.InverseMass;
Vector3Wide.Scale(normal, linearVelocityChangeB, out var correctiveVelocityBLinearVelocity);
Vector3Wide.Scale(projection.AngularB, correctiveImpulse, out var correctiveAngularImpulseB);
Symmetric3x3Wide.TransformWithoutOverlap(correctiveAngularImpulseB, inertiaB.InverseInertiaTensor, out var correctiveVelocityBAngularVelocity);
Vector3Wide.Add(wsvA.Linear, correctiveVelocityALinearVelocity, out wsvA.Linear);
Vector3Wide.Add(wsvA.Angular, correctiveVelocityAAngularVelocity, out wsvA.Angular);
Vector3Wide.Subtract(wsvB.Linear, correctiveVelocityBLinearVelocity, out wsvB.Linear); //Note subtract; normal = -jacobianLinearB
Vector3Wide.Add(wsvB.Angular, correctiveVelocityBAngularVelocity, 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 void Solve(ref BodyVelocities wsvA, ref BodyVelocities wsvB, ref Contact4Projection projection, ref Contact4AccumulatedImpulses accumulatedImpulses)
{
Helpers.BuildOrthnormalBasis(ref projection.Normal, out var x, out var z);
var maximumTangentImpulse = projection.PremultipliedFrictionCoefficient *
(accumulatedImpulses.Penetration0 + accumulatedImpulses.Penetration1 + accumulatedImpulses.Penetration2 + accumulatedImpulses.Penetration3);
TangentFriction.Solve(ref x, ref z, ref projection.Tangent, ref projection.InertiaA, ref projection.InertiaB, ref maximumTangentImpulse, ref accumulatedImpulses.Tangent, ref wsvA, ref wsvB);
//Note that we solve the penetration constraints after the friction constraints.
//This makes the penetration constraints more authoritative at the cost of the first iteration of the first frame of an impact lacking friction influence.
//It's a pretty minor effect either way.
PenetrationLimit4.Solve(ref projection.Penetration, ref projection.InertiaA, ref projection.InertiaB, ref projection.Normal,
ref accumulatedImpulses.Penetration0,
ref accumulatedImpulses.Penetration1,
ref accumulatedImpulses.Penetration2,
ref accumulatedImpulses.Penetration3, ref wsvA, ref wsvB);
var maximumTwistImpulse = projection.PremultipliedFrictionCoefficient * (
accumulatedImpulses.Penetration0 * projection.LeverArm0 +
accumulatedImpulses.Penetration1 * projection.LeverArm1 +
accumulatedImpulses.Penetration2 * projection.LeverArm2 +
accumulatedImpulses.Penetration3 * projection.LeverArm3);
TwistFriction.Solve(ref projection.Normal, ref projection.InertiaA, ref projection.InertiaB, ref projection.Twist, ref maximumTwistImpulse, ref accumulatedImpulses.Twist, ref wsvA, ref wsvB);
}
public static void ComputeCorrectiveImpulse(ref BodyVelocities wsvA, ref BodyVelocities wsvB, ref Projection2Body1DOF projection, ref Vector <float> accumulatedImpulse,
out Vector <float> correctiveCSI)
{
//Take the world space velocity of each body into constraint space by transforming by the transpose(jacobian).
//(The jacobian is a row vector by convention, while we treat our velocity vectors as a 12x1 row vector for the purposes of constraint space velocity calculation.
//So we are multiplying v * JT.)
//Then, transform it into an impulse by applying the effective mass.
//Here, we combine the projection and impulse conversion into a precomputed value, i.e. v * (JT * softenedEffectiveMass).
Vector3Wide.Dot(wsvA.Linear, projection.WSVtoCSILinearA, out var csiaLinear);
Vector3Wide.Dot(wsvA.Angular, projection.WSVtoCSIAngularA, out var csiaAngular);
Vector3Wide.Dot(wsvB.Linear, projection.WSVtoCSILinearB, out var csibLinear);
Vector3Wide.Dot(wsvB.Angular, projection.WSVtoCSIAngularB, out var csibAngular);
//Combine it all together, following:
//constraint space impulse = (targetVelocity - currentVelocity) * softenedEffectiveMass
//constraint space impulse = (bias - accumulatedImpulse * softness - wsv * JT) * softenedEffectiveMass
//constraint space impulse = (bias * softenedEffectiveMass) - accumulatedImpulse * (softness * softenedEffectiveMass) - wsv * (JT * softenedEffectiveMass)
var csi = projection.BiasImpulse - accumulatedImpulse * projection.SoftnessImpulseScale - (csiaLinear + csiaAngular + csibLinear + csibAngular);
var previousAccumulated = accumulatedImpulse;
accumulatedImpulse = Vector.Max(Vector <float> .Zero, accumulatedImpulse + csi);
correctiveCSI = accumulatedImpulse - previousAccumulated;
}
public static void Solve(ref Vector3Wide angularJacobianA, ref BodyInertias inertiaA, ref TwistFrictionProjection projection,
ref Vector <float> maximumImpulse, ref Vector <float> accumulatedImpulse, ref BodyVelocities wsvA)
{
ComputeCorrectiveImpulse(ref angularJacobianA, ref projection, ref wsvA, ref maximumImpulse, ref accumulatedImpulse, out var correctiveCSI);
ApplyImpulse(ref angularJacobianA, ref inertiaA, ref correctiveCSI, ref wsvA);
}
public static void Solve(ref Projection2Body1DOF projection, ref Vector <float> accumulatedImpulse, ref BodyVelocities wsvA, ref BodyVelocities wsvB)
{
ComputeCorrectiveImpulse(ref wsvA, ref wsvB, ref projection, ref accumulatedImpulse, out var correctiveCSI);
ApplyImpulse(ref projection, ref correctiveCSI, ref wsvA, ref wsvB);
}
public static void WarmStart(ref Projection2Body1DOF data, ref Vector <float> accumulatedImpulse, ref BodyVelocities wsvA, ref BodyVelocities wsvB)
{
//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 data, ref accumulatedImpulse, ref wsvA, ref wsvB);
}
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);
}
public void GatherCollidableBundle(int collidablesStartIndex, int count, out Vector <int> shapeIndices, out Vector <float> maximumExpansion, out RigidPoses poses, out BodyVelocities velocities)
{
Bodies.GatherDataForBounds(ref BodyIndices[collidablesStartIndex], count, out poses, out velocities, out shapeIndices, out maximumExpansion);
}
