diff --git a/BepuPhysics/Constraints/AreaConstraint.cs b/BepuPhysics/Constraints/AreaConstraint.cs index 9cb68c1c..12e4aab0 100644 --- a/BepuPhysics/Constraints/AreaConstraint.cs +++ b/BepuPhysics/Constraints/AreaConstraint.cs @@ -89,7 +89,11 @@ private static void ApplyImpulse(in Vector inverseMassA, in Vector } [MethodImpl(MethodImplOptions.AggressiveInlining)] - static void ComputeJacobian(in Vector3Wide positionA, in Vector3Wide positionB, in Vector3Wide positionC, out Vector normalLength, out Vector3Wide negatedJacobianA, out Vector3Wide jacobianB, out Vector3Wide jacobianC) + static void ComputeJacobian(in Vector3Wide positionA, in Vector3Wide positionB, in Vector3Wide positionC, + out Vector normalLength, + out Vector3Wide negatedJacobianA, out Vector3Wide jacobianB, out Vector3Wide jacobianC, + out Vector contributionA, out Vector contributionB, out Vector contributionC, + out Vector inverseJacobianLength) { //Area of a triangle with vertices a, b, and c is: //||ab x ac|| * 0.5 @@ -118,52 +122,69 @@ static void ComputeJacobian(in Vector3Wide positionA, in Vector3Wide positionB, Vector3Wide.CrossWithoutOverlap(normal, ab, out jacobianC); //Similar to the volume constraint, we could create a similar expression for jacobianA, but it's cheap to just do a couple of adds. Vector3Wide.Add(jacobianB, jacobianC, out negatedJacobianA); + //Normalize the jacobian to unit length. The jacobians are cross products of edges with the unit normal, + //giving magnitudes proportional to edge lengths (~L). Without normalization, the inverse effective mass + //scales with L², causing the accumulated impulse scale to vary with triangle size and making warm starting unstable. + //Normalizing gives a unit-length effective jacobian J_eff = inverseJacobianLength * J_raw. + //The inverse effective mass becomes a weighted average of inverse masses (always bounded), + //and the physical impulse is identical because the scaling factors cancel in the solve. + Vector3Wide.Dot(negatedJacobianA, negatedJacobianA, out contributionA); + Vector3Wide.Dot(jacobianB, jacobianB, out contributionB); + Vector3Wide.Dot(jacobianC, jacobianC, out contributionC); + var jacobianLengthSquared = contributionA + contributionB + contributionC; + //Guard against the degenerate case where edges are parallel/antiparallel (triangle collapses to a line). + jacobianLengthSquared = Vector.Max(new Vector(1e-14f), jacobianLengthSquared); + inverseJacobianLength = MathHelper.FastReciprocalSquareRoot(jacobianLengthSquared); } public static void WarmStart( in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, - in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, - in Vector3Wide positionC, in QuaternionWide orientationC, in BodyInertiaWide inertiaC, + in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, + in Vector3Wide positionC, in QuaternionWide orientationC, in BodyInertiaWide inertiaC, ref AreaConstraintPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB, ref BodyVelocityWide wsvC) { - ComputeJacobian(positionA, positionB, positionC, out _, out var negatedJacobianA, out var jacobianB, out var jacobianC); - ApplyImpulse(inertiaA.InverseMass, inertiaB.InverseMass, inertiaC.InverseMass, negatedJacobianA, jacobianB, jacobianC, accumulatedImpulses, ref wsvA, ref wsvB, ref wsvC); + ComputeJacobian(positionA, positionB, positionC, out _, out var negatedJacobianA, out var jacobianB, out var jacobianC, out _, out _, out _, out var inverseJacobianLength); + //The accumulated impulse is in unit-jacobian space. Replay through J_eff = inverseJacobianLength * J_raw. + //Since |J_eff| = 1, the warm start magnitude is bounded by |accumulated| * max(invMass), same as a distance constraint. + ApplyImpulse(inertiaA.InverseMass, inertiaB.InverseMass, inertiaC.InverseMass, negatedJacobianA, jacobianB, jacobianC, inverseJacobianLength * accumulatedImpulses, ref wsvA, ref wsvB, ref wsvC); } public static void Solve(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in Vector3Wide positionC, in QuaternionWide orientationC, in BodyInertiaWide inertiaC, float dt, float inverseDt, ref AreaConstraintPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB, ref BodyVelocityWide wsvC) { - ComputeJacobian(positionA, positionB, positionC, out var normalLength, out var negatedJacobianA, out var jacobianB, out var jacobianC); - - Vector3Wide.Dot(negatedJacobianA, negatedJacobianA, out var contributionA); - Vector3Wide.Dot(jacobianB, jacobianB, out var contributionB); - Vector3Wide.Dot(jacobianC, jacobianC, out var contributionC); - - //Protect against singularity by padding the jacobian contributions. This is very much a hack, but it's a pretty simple hack. - //Less sensitive to tuning than attempting to guard the inverseEffectiveMass itself, since that is sensitive to both scale AND mass. - - //Choose an epsilon based on the target area. Note that area ~= width^2 and our jacobian contributions are things like (ac x N) * (ac x N). - //Given that N is perpendicular to AC, ||(ac x N)|| == ||ac||, so the contribution is just ||ac||^2. Given the square, it's proportional to area and the area is a decent epsilon source. - var epsilon = 5e-4f * prestep.TargetScaledArea; - contributionA = Vector.Max(epsilon, contributionA); - contributionB = Vector.Max(epsilon, contributionB); - contributionC = Vector.Max(epsilon, contributionC); - var inverseEffectiveMass = contributionA * inertiaA.InverseMass + contributionB * inertiaB.InverseMass + contributionC * inertiaC.InverseMass; + //The area jacobians (ac x normal, normal x ab) have magnitude ~L (edge lengths). + //Without normalization, the inverse effective mass scales with L², making the accumulated impulse + //scale vary with triangle size and causing warm start instability. + //We normalize to a unit-length effective jacobian: J_eff = J_raw * inverseJacobianLength, where inverseJacobianLength = 1/|J_raw|. + //The inverse effective mass becomes a weighted average of inverse masses (always bounded), + //keeping the accumulated impulse well-scaled across substeps. + // + //The position error is scaled to match: error = (targetArea - area) * inverseJacobianLength. + //The physical impulse (inverseJacobianLength * csi applied through J_raw) is identical to the raw formulation + //because the inverseJacobianLength factors cancel. + ComputeJacobian(positionA, positionB, positionC, out var normalLength, out var negatedJacobianA, out var jacobianB, out var jacobianC, out var contributionA, out var contributionB, out var contributionC, out var inverseJacobianLength); + var inverseJacobianLengthSquared = inverseJacobianLength * inverseJacobianLength; + + //With the unit-length jacobian, the inverse effective mass is a weighted average of inverse masses, always bounded. + //Guard against degenerate configurations (e.g. triangle collapsed to a line) where all jacobian contributions are zero, + //which would cause a division by zero when computing the effective mass. + var inverseEffectiveMass = Vector.Max(new Vector(1e-14f), + inverseJacobianLengthSquared * (contributionA * inertiaA.InverseMass + contributionB * inertiaB.InverseMass + contributionC * inertiaC.InverseMass)); SpringSettingsWide.ComputeSpringiness(prestep.SpringSettings, dt, out var positionErrorToVelocity, out var effectiveMassCFMScale, out var softnessImpulseScale); var effectiveMass = effectiveMassCFMScale / inverseEffectiveMass; //Compute the position error and bias velocities. Note the order of subtraction when calculating error- we want the bias velocity to counteract the separation. - var biasVelocity = (prestep.TargetScaledArea - normalLength) * positionErrorToVelocity; + var biasVelocity = (prestep.TargetScaledArea - normalLength) * inverseJacobianLength * positionErrorToVelocity; //csi = projection.BiasImpulse - accumulatedImpulse * projection.SoftnessImpulseScale - (csiaLinear + csiaAngular + csibLinear + csibAngular); Vector3Wide.Dot(negatedJacobianA, wsvA.Linear, out var negatedVelocityContributionA); Vector3Wide.Dot(jacobianB, wsvB.Linear, out var velocityContributionB); Vector3Wide.Dot(jacobianC, wsvC.Linear, out var velocityContributionC); - var csv = velocityContributionB + velocityContributionC - negatedVelocityContributionA; + var csv = inverseJacobianLength * (velocityContributionB + velocityContributionC - negatedVelocityContributionA); var csi = (biasVelocity - csv) * effectiveMass - accumulatedImpulses * softnessImpulseScale; accumulatedImpulses += csi; - ApplyImpulse(inertiaA.InverseMass, inertiaB.InverseMass, inertiaC.InverseMass, negatedJacobianA, jacobianB, jacobianC, csi, ref wsvA, ref wsvB, ref wsvC); + ApplyImpulse(inertiaA.InverseMass, inertiaB.InverseMass, inertiaC.InverseMass, negatedJacobianA, jacobianB, jacobianC, inverseJacobianLength * csi, ref wsvA, ref wsvB, ref wsvC); } public static bool RequiresIncrementalSubstepUpdates => false; diff --git a/BepuPhysics/Constraints/VolumeConstraint.cs b/BepuPhysics/Constraints/VolumeConstraint.cs index 0ff92bd6..7e843bb6 100644 --- a/BepuPhysics/Constraints/VolumeConstraint.cs +++ b/BepuPhysics/Constraints/VolumeConstraint.cs @@ -1,4 +1,4 @@ -using BepuUtilities; +using BepuUtilities; using BepuUtilities.Memory; using System; using System.Diagnostics; @@ -8,7 +8,7 @@ namespace BepuPhysics.Constraints { /// - /// Constrains the volume of a tetrahedron connecting the centers of four bodies to match a goal volume. + /// Constrains the volume of a tetrahedron connecting the centers of four bodies to match a goal volume. /// Scaled volume computed from (ab x ac) * ad; the volume may be negative depending on the winding of the tetrahedron. /// public struct VolumeConstraint : IFourBodyConstraintDescription @@ -94,7 +94,9 @@ private static void ApplyImpulse( [MethodImpl(MethodImplOptions.AggressiveInlining)] static void ComputeJacobian(in Vector3Wide positionA, in Vector3Wide positionB, in Vector3Wide positionC, in Vector3Wide positionD, out Vector3Wide ad, - out Vector3Wide negatedJA, out Vector3Wide jacobianB, out Vector3Wide jacobianC, out Vector3Wide jacobianD) + out Vector3Wide negatedJA, out Vector3Wide jacobianB, out Vector3Wide jacobianC, out Vector3Wide jacobianD, + out Vector contributionA, out Vector contributionB, out Vector contributionC, out Vector contributionD, + out Vector inverseJacobianLength) { var ab = positionB - positionA; var ac = positionC - positionA; @@ -104,72 +106,74 @@ static void ComputeJacobian(in Vector3Wide positionA, in Vector3Wide positionB, Vector3Wide.CrossWithoutOverlap(ab, ac, out jacobianD); Vector3Wide.Add(jacobianB, jacobianC, out negatedJA); Vector3Wide.Add(jacobianD, negatedJA, out negatedJA); + //Normalize the jacobian to unit length. The raw jacobians are cross products of edges (face area vectors) with magnitude ~L². + //Normalizing gives a unit-length effective jacobian J_eff = inverseJacobianLength * J_raw where inverseJacobianLength = 1/|J_raw|. + //This keeps the inverse effective mass bounded (it becomes a weighted average of inverse masses), + //which bounds the accumulated impulse and makes warm starting stable regardless of configuration. + //The physical impulse (inverseJacobianLength cancels in the solve) is identical to the raw volume formulation. + Vector3Wide.Dot(negatedJA, negatedJA, out contributionA); + Vector3Wide.Dot(jacobianB, jacobianB, out contributionB); + Vector3Wide.Dot(jacobianC, jacobianC, out contributionC); + Vector3Wide.Dot(jacobianD, jacobianD, out contributionD); + var jacobianLengthSquared = contributionA + contributionB + contributionC + contributionD; + //Guard against the collinear degeneracy (all cross products vanish). This is far more extreme than coplanar; + //for generic coplanar configurations the cross products remain nonzero. + jacobianLengthSquared = Vector.Max(new Vector(1e-14f), jacobianLengthSquared); + inverseJacobianLength = MathHelper.FastReciprocalSquareRoot(jacobianLengthSquared); } + public static void WarmStart(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in Vector3Wide positionC, in QuaternionWide orientationC, in BodyInertiaWide inertiaC, in Vector3Wide positionD, in QuaternionWide orientationD, in BodyInertiaWide inertiaD, ref VolumeConstraintPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB, ref BodyVelocityWide wsvC, ref BodyVelocityWide wsvD) { - ComputeJacobian(positionA, positionB, positionC, positionD, out var ad, out var negatedJA, out var jacobianB, out var jacobianC, out var jacobianD); - //Vector3Wide.Dot(jacobianD, ad, out var unscaledVolume); - ApplyImpulse(inertiaA.InverseMass, inertiaB.InverseMass, inertiaC.InverseMass, inertiaD.InverseMass, negatedJA, jacobianB, jacobianC, jacobianD, accumulatedImpulses, ref wsvA, ref wsvB, ref wsvC, ref wsvD); + ComputeJacobian(positionA, positionB, positionC, positionD, out _, out var negatedJA, out var jacobianB, out var jacobianC, out var jacobianD, out _, out _, out _, out _, out var inverseJacobianLength); + //The accumulated impulse is in unit-jacobian space. Replay through J_eff = inverseJacobianLength * J_raw. + //Since |J_eff| = 1, the warm start magnitude is bounded by |accumulated| * max(invMass), same as a distance constraint. + ApplyImpulse(inertiaA.InverseMass, inertiaB.InverseMass, inertiaC.InverseMass, inertiaD.InverseMass, negatedJA, jacobianB, jacobianC, jacobianD, inverseJacobianLength * accumulatedImpulses, ref wsvA, ref wsvB, ref wsvC, ref wsvD); } public static void Solve(in Vector3Wide positionA, in QuaternionWide orientationA, in BodyInertiaWide inertiaA, in Vector3Wide positionB, in QuaternionWide orientationB, in BodyInertiaWide inertiaB, in Vector3Wide positionC, in QuaternionWide orientationC, in BodyInertiaWide inertiaC, in Vector3Wide positionD, in QuaternionWide orientationD, in BodyInertiaWide inertiaD, float dt, float inverseDt, ref VolumeConstraintPrestepData prestep, ref Vector accumulatedImpulses, ref BodyVelocityWide wsvA, ref BodyVelocityWide wsvB, ref BodyVelocityWide wsvC, ref BodyVelocityWide wsvD) { - //Volume of parallelepiped with vertices a, b, c, d is: - //(ab x ac) * ad - //A tetrahedron with the same edges will have one sixth of this volume. As a constant factor, it's not relevant. So the constraint is just: - //OriginalVolume * 6 = (ab x ac) * ad - //Taking the derivative to get the velocity constraint: - //0 = d/dt(ab x ac) * ad + (ab x ac) * d/dt(ad) - //0 = d/dt(ab x ac) * ad + (ab x ac) * (d/dt(d) - d/dt(a)) - //0 = (d/dt(ab) x ac + ab x d/dt(ac)) * ad + (ab x ac) * (d/dt(d) - d/dt(a)) - //0 = ((d/dt(ab) x ac) * ad + (ab x d/dt(ac)) * ad + (ab x ac) * (d/dt(d) - d/dt(a)) - //0 = (ac x ad) * (d/dt(b) - d/dt(a)) + (ad x ab) * (d/dt(c) - d/dt(a)) + (ab x ac) * (d/dt(d) - d/dt(a)) - //Giving the linear jacobians: - //JA: -ac x ad - ad x ab - ab x ac == bd x bc - //JB: ac x ad - //JC: ad x ab - //JD: ab x ac - //We're not blending the jacobians into the effective mass or inverse mass either- even though that would save ALU time, the goal here is to minimize memory bandwidth since that - //tends to be the bottleneck for any multithreaded simulation. (Despite being a 1DOF constraint, this doesn't need to output inverse inertia tensors, so premultiplying isn't a win.) - ComputeJacobian(positionA, positionB, positionC, positionD, out var ad, out var negatedJA, out var jacobianB, out var jacobianC, out var jacobianD); - - Vector3Wide.Dot(negatedJA, negatedJA, out var contributionA); - Vector3Wide.Dot(jacobianB, jacobianB, out var contributionB); - Vector3Wide.Dot(jacobianC, jacobianC, out var contributionC); - Vector3Wide.Dot(jacobianD, jacobianD, out var contributionD); - - //Protect against singularity by padding the jacobian contributions. This is very much a hack, but it's a pretty simple hack. - //Less sensitive to tuning than attempting to guard the inverseEffectiveMass itself, since that is sensitive to both scale AND mass. - - //Choose an epsilon based on the target volume. Note that volume ~= width^3, whereas our jacobian contributions are things like (ac x ad) * (ac x ad), which is proportional - //to the area of the triangle acd squared. In other words, the contribution is ~ width^4. - //Scaling the volume by a constant factor will not match the growth rate of the jacobian contributions. - //We're going to ignore this until proven to be a noticeable problem because Vector does not expose exp or pow and this is cheap. - //Could still implement it, but it's not super high value. - var epsilon = 5e-4f * prestep.TargetScaledVolume; - contributionA = Vector.Max(epsilon, contributionA); - contributionB = Vector.Max(epsilon, contributionB); - contributionC = Vector.Max(epsilon, contributionC); - contributionD = Vector.Max(epsilon, contributionD); - var inverseEffectiveMass = contributionA * inertiaA.InverseMass + contributionB * inertiaB.InverseMass + contributionC * inertiaC.InverseMass + contributionD * inertiaD.InverseMass; + //Volume of parallelepiped with vertices a, b, c, d is V = (ab x ac) * ad. + //The raw volume jacobians (dV/dq) are cross products of edges: + //JA_raw: -(ac x ad) - (ad x ab) - (ab x ac) + //JB_raw: ac x ad + //JC_raw: ad x ab + //JD_raw: ab x ac + // + //These have magnitude ~L² (face areas), which varies with configuration and causes warm start instability. + //We normalize to a unit-length effective jacobian: J_eff = J_raw * inverseJacobianLength, where inverseJacobianLength = 1/|J_raw|. + //The inverse effective mass becomes a weighted average of inverse masses (always bounded), + //keeping the accumulated impulse well-scaled across substeps. + // + //The position error is the linearized signed distance to the constraint surface V = target: + // error = (target_V - V) / |J_raw| = (target_V - V) * inverseJacobianLength + //The physical impulse (inverseJacobianLength * csi applied through J_raw) is identical to the raw volume formulation + //because the inverseJacobianLength factors cancel. + ComputeJacobian(positionA, positionB, positionC, positionD, out var ad, out var negatedJA, out var jacobianB, out var jacobianC, out var jacobianD, out var contributionA, out var contributionB, out var contributionC, out var contributionD, out var inverseJacobianLength); + var inverseJacobianLengthSquared = inverseJacobianLength * inverseJacobianLength; + + //With the unit-length jacobian, the inverse effective mass is sum(fraction_i * invMass_i) — a weighted average of inverse masses, always bounded. + //Guard against degenerate configurations (e.g. all points collinear) where all jacobian contributions are zero, + //which would cause a division by zero when computing the effective mass. + var inverseEffectiveMass = Vector.Max(new Vector(1e-14f), + inverseJacobianLengthSquared * (contributionA * inertiaA.InverseMass + contributionB * inertiaB.InverseMass + contributionC * inertiaC.InverseMass + contributionD * inertiaD.InverseMass)); SpringSettingsWide.ComputeSpringiness(prestep.SpringSettings, dt, out var positionErrorToVelocity, out var effectiveMassCFMScale, out var softnessImpulseScale); var effectiveMass = effectiveMassCFMScale / inverseEffectiveMass; //Compute the position error and bias velocities. Note the order of subtraction when calculating error- we want the bias velocity to counteract the separation. - Vector3Wide.Dot(jacobianD, ad, out var unscaledVolume); - var biasVelocity = (prestep.TargetScaledVolume - unscaledVolume) * positionErrorToVelocity; + Vector3Wide.Dot(jacobianD, ad, out var volume); + var biasVelocity = (prestep.TargetScaledVolume - volume) * inverseJacobianLength * positionErrorToVelocity; //csi = projection.BiasImpulse - accumulatedImpulse * projection.SoftnessImpulseScale - (csiaLinear + csiaAngular + csibLinear + csibAngular); Vector3Wide.Dot(negatedJA, wsvA.Linear, out var negatedVelocityContributionA); Vector3Wide.Dot(jacobianB, wsvB.Linear, out var velocityContributionB); Vector3Wide.Dot(jacobianC, wsvC.Linear, out var velocityContributionC); Vector3Wide.Dot(jacobianD, wsvD.Linear, out var velocityContributionD); - var csv = velocityContributionB + velocityContributionC + velocityContributionD - negatedVelocityContributionA; + var csv = inverseJacobianLength * (velocityContributionB + velocityContributionC + velocityContributionD - negatedVelocityContributionA); var csi = (biasVelocity - csv) * effectiveMass - accumulatedImpulses * softnessImpulseScale; accumulatedImpulses += csi; - ApplyImpulse(inertiaA.InverseMass, inertiaB.InverseMass, inertiaC.InverseMass, inertiaD.InverseMass, negatedJA, jacobianB, jacobianC, jacobianD, csi, ref wsvA, ref wsvB, ref wsvC, ref wsvD); + ApplyImpulse(inertiaA.InverseMass, inertiaB.InverseMass, inertiaC.InverseMass, inertiaD.InverseMass, negatedJA, jacobianB, jacobianC, jacobianD, inverseJacobianLength * csi, ref wsvA, ref wsvB, ref wsvC, ref wsvD); } public static bool RequiresIncrementalSubstepUpdates => false;