diff --git a/BepuPhysics/Constraints/Contact/ContactConvexTypes.cs b/BepuPhysics/Constraints/Contact/ContactConvexTypes.cs index a858f6300..d8e777664 100644 --- a/BepuPhysics/Constraints/Contact/ContactConvexTypes.cs +++ b/BepuPhysics/Constraints/Contact/ContactConvexTypes.cs @@ -472,11 +472,10 @@ public static void Solve(in Vector3Wide positionA, in QuaternionWide orientation PenetrationLimitOneBody.Solve(inertiaA, prestep.Normal, prestep.Contact0.OffsetA, prestep.Contact0.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref accumulatedImpulses.Penetration0, ref wsvA); PenetrationLimitOneBody.Solve(inertiaA, prestep.Normal, prestep.Contact1.OffsetA, prestep.Contact1.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref accumulatedImpulses.Penetration1, ref wsvA); Helpers.BuildOrthonormalBasis(prestep.Normal, out var x, out var z); - var premultipliedFrictionCoefficient = new Vector(1f / 2f) * prestep.MaterialProperties.FrictionCoefficient; - var maximumTangentImpulse = premultipliedFrictionCoefficient * (accumulatedImpulses.Penetration0 + accumulatedImpulses.Penetration1); + var maximumTangentImpulse = prestep.MaterialProperties.FrictionCoefficient * (accumulatedImpulses.Penetration0 + accumulatedImpulses.Penetration1); FrictionHelpers.ComputeFrictionCenter(prestep.Contact0.OffsetA, prestep.Contact1.OffsetA, prestep.Contact0.Depth, prestep.Contact1.Depth, out var offsetToManifoldCenterA); TangentFrictionOneBody.Solve(x, z, offsetToManifoldCenterA, inertiaA, maximumTangentImpulse, ref accumulatedImpulses.Tangent, ref wsvA); - var maximumTwistImpulse = premultipliedFrictionCoefficient * ( + var maximumTwistImpulse = prestep.MaterialProperties.FrictionCoefficient * ( accumulatedImpulses.Penetration0 * Vector3Wide.Distance(offsetToManifoldCenterA, prestep.Contact0.OffsetA) + accumulatedImpulses.Penetration1 * Vector3Wide.Distance(offsetToManifoldCenterA, prestep.Contact1.OffsetA)); TwistFrictionOneBody.Solve(prestep.Normal, inertiaA, maximumTwistImpulse, ref accumulatedImpulses.Twist, ref wsvA); @@ -636,11 +635,10 @@ public static void Solve(in Vector3Wide positionA, in QuaternionWide orientation PenetrationLimitOneBody.Solve(inertiaA, prestep.Normal, prestep.Contact1.OffsetA, prestep.Contact1.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref accumulatedImpulses.Penetration1, ref wsvA); PenetrationLimitOneBody.Solve(inertiaA, prestep.Normal, prestep.Contact2.OffsetA, prestep.Contact2.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref accumulatedImpulses.Penetration2, ref wsvA); Helpers.BuildOrthonormalBasis(prestep.Normal, out var x, out var z); - var premultipliedFrictionCoefficient = new Vector(1f / 3f) * prestep.MaterialProperties.FrictionCoefficient; - var maximumTangentImpulse = premultipliedFrictionCoefficient * (accumulatedImpulses.Penetration0 + accumulatedImpulses.Penetration1 + accumulatedImpulses.Penetration2); + var maximumTangentImpulse = prestep.MaterialProperties.FrictionCoefficient * (accumulatedImpulses.Penetration0 + accumulatedImpulses.Penetration1 + accumulatedImpulses.Penetration2); FrictionHelpers.ComputeFrictionCenter(prestep.Contact0.OffsetA, prestep.Contact1.OffsetA, prestep.Contact2.OffsetA, prestep.Contact0.Depth, prestep.Contact1.Depth, prestep.Contact2.Depth, out var offsetToManifoldCenterA); TangentFrictionOneBody.Solve(x, z, offsetToManifoldCenterA, inertiaA, maximumTangentImpulse, ref accumulatedImpulses.Tangent, ref wsvA); - var maximumTwistImpulse = premultipliedFrictionCoefficient * ( + var maximumTwistImpulse = prestep.MaterialProperties.FrictionCoefficient * ( accumulatedImpulses.Penetration0 * Vector3Wide.Distance(offsetToManifoldCenterA, prestep.Contact0.OffsetA) + accumulatedImpulses.Penetration1 * Vector3Wide.Distance(offsetToManifoldCenterA, prestep.Contact1.OffsetA) + accumulatedImpulses.Penetration2 * Vector3Wide.Distance(offsetToManifoldCenterA, prestep.Contact2.OffsetA)); @@ -810,11 +808,10 @@ public static void Solve(in Vector3Wide positionA, in QuaternionWide orientation PenetrationLimitOneBody.Solve(inertiaA, prestep.Normal, prestep.Contact2.OffsetA, prestep.Contact2.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref accumulatedImpulses.Penetration2, ref wsvA); PenetrationLimitOneBody.Solve(inertiaA, prestep.Normal, prestep.Contact3.OffsetA, prestep.Contact3.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref accumulatedImpulses.Penetration3, ref wsvA); Helpers.BuildOrthonormalBasis(prestep.Normal, out var x, out var z); - var premultipliedFrictionCoefficient = new Vector(1f / 4f) * prestep.MaterialProperties.FrictionCoefficient; - var maximumTangentImpulse = premultipliedFrictionCoefficient * (accumulatedImpulses.Penetration0 + accumulatedImpulses.Penetration1 + accumulatedImpulses.Penetration2 + accumulatedImpulses.Penetration3); + var maximumTangentImpulse = prestep.MaterialProperties.FrictionCoefficient * (accumulatedImpulses.Penetration0 + accumulatedImpulses.Penetration1 + accumulatedImpulses.Penetration2 + accumulatedImpulses.Penetration3); FrictionHelpers.ComputeFrictionCenter(prestep.Contact0.OffsetA, prestep.Contact1.OffsetA, prestep.Contact2.OffsetA, prestep.Contact3.OffsetA, prestep.Contact0.Depth, prestep.Contact1.Depth, prestep.Contact2.Depth, prestep.Contact3.Depth, out var offsetToManifoldCenterA); TangentFrictionOneBody.Solve(x, z, offsetToManifoldCenterA, inertiaA, maximumTangentImpulse, ref accumulatedImpulses.Tangent, ref wsvA); - var maximumTwistImpulse = premultipliedFrictionCoefficient * ( + var maximumTwistImpulse = prestep.MaterialProperties.FrictionCoefficient * ( accumulatedImpulses.Penetration0 * Vector3Wide.Distance(offsetToManifoldCenterA, prestep.Contact0.OffsetA) + accumulatedImpulses.Penetration1 * Vector3Wide.Distance(offsetToManifoldCenterA, prestep.Contact1.OffsetA) + accumulatedImpulses.Penetration2 * Vector3Wide.Distance(offsetToManifoldCenterA, prestep.Contact2.OffsetA) + @@ -1135,12 +1132,11 @@ public static void Solve(in Vector3Wide positionA, in QuaternionWide orientation PenetrationLimit.Solve(inertiaA, inertiaB, prestep.Normal, prestep.Contact0.OffsetA, prestep.Contact0.OffsetA - prestep.OffsetB, prestep.Contact0.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref accumulatedImpulses.Penetration0, ref wsvA, ref wsvB); PenetrationLimit.Solve(inertiaA, inertiaB, prestep.Normal, prestep.Contact1.OffsetA, prestep.Contact1.OffsetA - prestep.OffsetB, prestep.Contact1.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref accumulatedImpulses.Penetration1, ref wsvA, ref wsvB); Helpers.BuildOrthonormalBasis(prestep.Normal, out var x, out var z); - var premultipliedFrictionCoefficient = new Vector(1f / 2f) * prestep.MaterialProperties.FrictionCoefficient; - var maximumTangentImpulse = premultipliedFrictionCoefficient * (accumulatedImpulses.Penetration0 + accumulatedImpulses.Penetration1); + var maximumTangentImpulse = prestep.MaterialProperties.FrictionCoefficient * (accumulatedImpulses.Penetration0 + accumulatedImpulses.Penetration1); FrictionHelpers.ComputeFrictionCenter(prestep.Contact0.OffsetA, prestep.Contact1.OffsetA, prestep.Contact0.Depth, prestep.Contact1.Depth, out var offsetToManifoldCenterA); Vector3Wide.Subtract(offsetToManifoldCenterA, prestep.OffsetB, out var offsetToManifoldCenterB); TangentFriction.Solve(x, z, offsetToManifoldCenterA, offsetToManifoldCenterB, inertiaA, inertiaB, maximumTangentImpulse, ref accumulatedImpulses.Tangent, ref wsvA, ref wsvB); - var maximumTwistImpulse = premultipliedFrictionCoefficient * ( + var maximumTwistImpulse = prestep.MaterialProperties.FrictionCoefficient * ( accumulatedImpulses.Penetration0 * Vector3Wide.Distance(offsetToManifoldCenterA, prestep.Contact0.OffsetA) + accumulatedImpulses.Penetration1 * Vector3Wide.Distance(offsetToManifoldCenterA, prestep.Contact1.OffsetA)); TwistFriction.Solve(prestep.Normal, inertiaA, inertiaB, maximumTwistImpulse, ref accumulatedImpulses.Twist, ref wsvA, ref wsvB); @@ -1312,12 +1308,11 @@ public static void Solve(in Vector3Wide positionA, in QuaternionWide orientation PenetrationLimit.Solve(inertiaA, inertiaB, prestep.Normal, prestep.Contact1.OffsetA, prestep.Contact1.OffsetA - prestep.OffsetB, prestep.Contact1.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref accumulatedImpulses.Penetration1, ref wsvA, ref wsvB); PenetrationLimit.Solve(inertiaA, inertiaB, prestep.Normal, prestep.Contact2.OffsetA, prestep.Contact2.OffsetA - prestep.OffsetB, prestep.Contact2.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref accumulatedImpulses.Penetration2, ref wsvA, ref wsvB); Helpers.BuildOrthonormalBasis(prestep.Normal, out var x, out var z); - var premultipliedFrictionCoefficient = new Vector(1f / 3f) * prestep.MaterialProperties.FrictionCoefficient; - var maximumTangentImpulse = premultipliedFrictionCoefficient * (accumulatedImpulses.Penetration0 + accumulatedImpulses.Penetration1 + accumulatedImpulses.Penetration2); + var maximumTangentImpulse = prestep.MaterialProperties.FrictionCoefficient * (accumulatedImpulses.Penetration0 + accumulatedImpulses.Penetration1 + accumulatedImpulses.Penetration2); FrictionHelpers.ComputeFrictionCenter(prestep.Contact0.OffsetA, prestep.Contact1.OffsetA, prestep.Contact2.OffsetA, prestep.Contact0.Depth, prestep.Contact1.Depth, prestep.Contact2.Depth, out var offsetToManifoldCenterA); Vector3Wide.Subtract(offsetToManifoldCenterA, prestep.OffsetB, out var offsetToManifoldCenterB); TangentFriction.Solve(x, z, offsetToManifoldCenterA, offsetToManifoldCenterB, inertiaA, inertiaB, maximumTangentImpulse, ref accumulatedImpulses.Tangent, ref wsvA, ref wsvB); - var maximumTwistImpulse = premultipliedFrictionCoefficient * ( + var maximumTwistImpulse = prestep.MaterialProperties.FrictionCoefficient * ( accumulatedImpulses.Penetration0 * Vector3Wide.Distance(offsetToManifoldCenterA, prestep.Contact0.OffsetA) + accumulatedImpulses.Penetration1 * Vector3Wide.Distance(offsetToManifoldCenterA, prestep.Contact1.OffsetA) + accumulatedImpulses.Penetration2 * Vector3Wide.Distance(offsetToManifoldCenterA, prestep.Contact2.OffsetA)); @@ -1499,12 +1494,11 @@ public static void Solve(in Vector3Wide positionA, in QuaternionWide orientation PenetrationLimit.Solve(inertiaA, inertiaB, prestep.Normal, prestep.Contact2.OffsetA, prestep.Contact2.OffsetA - prestep.OffsetB, prestep.Contact2.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref accumulatedImpulses.Penetration2, ref wsvA, ref wsvB); PenetrationLimit.Solve(inertiaA, inertiaB, prestep.Normal, prestep.Contact3.OffsetA, prestep.Contact3.OffsetA - prestep.OffsetB, prestep.Contact3.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref accumulatedImpulses.Penetration3, ref wsvA, ref wsvB); Helpers.BuildOrthonormalBasis(prestep.Normal, out var x, out var z); - var premultipliedFrictionCoefficient = new Vector(1f / 4f) * prestep.MaterialProperties.FrictionCoefficient; - var maximumTangentImpulse = premultipliedFrictionCoefficient * (accumulatedImpulses.Penetration0 + accumulatedImpulses.Penetration1 + accumulatedImpulses.Penetration2 + accumulatedImpulses.Penetration3); + var maximumTangentImpulse = prestep.MaterialProperties.FrictionCoefficient * (accumulatedImpulses.Penetration0 + accumulatedImpulses.Penetration1 + accumulatedImpulses.Penetration2 + accumulatedImpulses.Penetration3); FrictionHelpers.ComputeFrictionCenter(prestep.Contact0.OffsetA, prestep.Contact1.OffsetA, prestep.Contact2.OffsetA, prestep.Contact3.OffsetA, prestep.Contact0.Depth, prestep.Contact1.Depth, prestep.Contact2.Depth, prestep.Contact3.Depth, out var offsetToManifoldCenterA); Vector3Wide.Subtract(offsetToManifoldCenterA, prestep.OffsetB, out var offsetToManifoldCenterB); TangentFriction.Solve(x, z, offsetToManifoldCenterA, offsetToManifoldCenterB, inertiaA, inertiaB, maximumTangentImpulse, ref accumulatedImpulses.Tangent, ref wsvA, ref wsvB); - var maximumTwistImpulse = premultipliedFrictionCoefficient * ( + var maximumTwistImpulse = prestep.MaterialProperties.FrictionCoefficient * ( accumulatedImpulses.Penetration0 * Vector3Wide.Distance(offsetToManifoldCenterA, prestep.Contact0.OffsetA) + accumulatedImpulses.Penetration1 * Vector3Wide.Distance(offsetToManifoldCenterA, prestep.Contact1.OffsetA) + accumulatedImpulses.Penetration2 * Vector3Wide.Distance(offsetToManifoldCenterA, prestep.Contact2.OffsetA) + diff --git a/BepuPhysics/Constraints/Contact/ContactConvexTypes.tt b/BepuPhysics/Constraints/Contact/ContactConvexTypes.tt index 243e2215d..cf951b8f3 100644 --- a/BepuPhysics/Constraints/Contact/ContactConvexTypes.tt +++ b/BepuPhysics/Constraints/Contact/ContactConvexTypes.tt @@ -261,10 +261,7 @@ for (int i = 0; i < contactCount; ++i) PenetrationLimit<#=suffix#>.Solve(inertiaA<#=bodyCount == 2 ? ", inertiaB" : ""#>, prestep.Normal, prestep.Contact<#=i#>.OffsetA, <#if(bodyCount == 2) {#>prestep.Contact<#=i#>.OffsetA - prestep.OffsetB, <#}#>prestep.Contact<#=i#>.Depth, positionErrorToVelocity, effectiveMassCFMScale, prestep.MaterialProperties.MaximumRecoveryVelocity, inverseDtWide, softnessImpulseScale, ref accumulatedImpulses.Penetration<#=i#>, ref wsvA<#if (bodyCount == 2) {#>, ref wsvB<#}#>); <#}#> Helpers.BuildOrthonormalBasis(prestep.Normal, out var x, out var z); -<#if (contactCount > 1) {#> - var premultipliedFrictionCoefficient = new Vector(1f / <#=contactCount#>f) * prestep.MaterialProperties.FrictionCoefficient; -<#}#> - var maximumTangentImpulse = <#=contactCount > 1 ? "premultipliedFrictionCoefficient" : "prestep.MaterialProperties.FrictionCoefficient"#> * (<#for (int i = 0; i < contactCount; ++i) {#>accumulatedImpulses.Penetration<#=i#><#if(i < contactCount - 1){#> + <#}}#>); + var maximumTangentImpulse = prestep.MaterialProperties.FrictionCoefficient * (<#for (int i = 0; i < contactCount; ++i) {#>accumulatedImpulses.Penetration<#=i#><#if(i < contactCount - 1){#> + <#}}#>); <#if (contactCount > 1) {#> FrictionHelpers.ComputeFrictionCenter(<#for (int i = 0; i < contactCount; ++i) {#>prestep.Contact<#=i#>.OffsetA, <#}#><#for (int i = 0; i < contactCount; ++i) {#>prestep.Contact<#=i#>.Depth, <#}#>out var offsetToManifoldCenterA); <#if (bodyCount == 2) {#> @@ -277,9 +274,9 @@ for (int i = 0; i < contactCount; ++i) <#if (contactCount == 1) {#> //If there's only one contact, then the contact patch as determined by contact distance would be zero. //That can cause some subtle behavioral issues sometimes, so we approximate lever arm with the contact depth, assuming that the contact surface area will increase as the depth increases. - var maximumTwistImpulse = <#=contactCount > 1 ? "premultipliedFrictionCoefficient" : "prestep.MaterialProperties.FrictionCoefficient"#> * accumulatedImpulses.Penetration0 * Vector.Max(Vector.Zero, prestep.Contact0.Depth); + var maximumTwistImpulse = prestep.MaterialProperties.FrictionCoefficient * accumulatedImpulses.Penetration0 * Vector.Max(Vector.Zero, prestep.Contact0.Depth); <#} else {#> - var maximumTwistImpulse = <#=contactCount > 1 ? "premultipliedFrictionCoefficient" : "prestep.MaterialProperties.FrictionCoefficient"#> * ( + var maximumTwistImpulse = prestep.MaterialProperties.FrictionCoefficient * ( <#for (int i = 0; i < contactCount; ++i) {#> accumulatedImpulses.Penetration<#=i#> * Vector3Wide.Distance(offsetToManifoldCenterA, prestep.Contact<#=i#>.OffsetA)<#=i == contactCount - 1 ? ");" : " +"#> <#}#> diff --git a/Demos/Demos/Cars/CarDemo.cs b/Demos/Demos/Cars/CarDemo.cs index be03e43ad..49ec4b7de 100644 --- a/Demos/Demos/Cars/CarDemo.cs +++ b/Demos/Demos/Cars/CarDemo.cs @@ -62,7 +62,7 @@ public override void Initialize(ContentArchive content, Camera camera) const float wheelBaseWidth = x * 2; const float wheelBaseLength = frontZ - backZ; - playerController = new SimpleCarController(SimpleCar.Create(Simulation, properties, new Vector3(0, 10, 0), bodyShapeIndex, bodyInertia, 0.5f, wheelShapeIndex, wheelInertia, 2f, + playerController = new SimpleCarController(SimpleCar.Create(Simulation, properties, new Vector3(0, 10, 0), bodyShapeIndex, bodyInertia, 0.35f, wheelShapeIndex, wheelInertia, 1f, new Vector3(-x, y, frontZ), new Vector3(x, y, frontZ), new Vector3(-x, y, backZ), new Vector3(x, y, backZ), new Vector3(0, -1, 0), 0.25f, new SpringSettings(5f, 0.7f), QuaternionEx.CreateFromAxisAngle(Vector3.UnitZ, MathF.PI * 0.5f)), forwardSpeed: 75, forwardForce: 6, zoomMultiplier: 2, backwardSpeed: 30, backwardForce: 4, idleForce: 0.25f, brakeForce: 7, steeringSpeed: 1.5f, maximumSteeringAngle: MathF.PI * 0.23f, diff --git a/Demos/Demos/FrictionDemo.cs b/Demos/Demos/FrictionDemo.cs index b07cb19f6..c3d7dea46 100644 --- a/Demos/Demos/FrictionDemo.cs +++ b/Demos/Demos/FrictionDemo.cs @@ -95,7 +95,7 @@ public override void Initialize(ContentArchive content, Camera camera) var boxDescription = BodyDescription.CreateDynamic(RigidPose.Identity, new Vector3(20, 0, 0), shape.ComputeInertia(1), Simulation.Shapes.Add(shape), 1e-2f); int boxCount = 100; - float maximumFriction = 3; + float maximumFriction = 0.75f; for (int i = 0; i < 100; ++i) { //Drop the boxes in a line with varying friction. diff --git a/Demos/Demos/Tanks/TankDemo.cs b/Demos/Demos/Tanks/TankDemo.cs index 9be7778a8..0bfd7bfdc 100644 --- a/Demos/Demos/Tanks/TankDemo.cs +++ b/Demos/Demos/Tanks/TankDemo.cs @@ -93,7 +93,7 @@ public override void Initialize(ContentArchive content, Camera camera) SuspensionSettings = new SpringSettings(2.5f, 1.5f), WheelShape = wheelShapeIndex, WheelInertia = wheelInertia, - WheelFriction = 2f, + WheelFriction = 1f, TreadSpacing = 1f, WheelCountPerTread = 5, WheelOrientation = QuaternionEx.CreateFromAxisAngle(Vector3.UnitZ, MathF.PI * -0.5f), diff --git a/Demos/SpecializedTests/FrictionConsistencyTestDemo.cs b/Demos/SpecializedTests/FrictionConsistencyTestDemo.cs new file mode 100644 index 000000000..ac9d935f2 --- /dev/null +++ b/Demos/SpecializedTests/FrictionConsistencyTestDemo.cs @@ -0,0 +1,200 @@ +using System; +using System.Numerics; +using BepuPhysics; +using BepuPhysics.Collidables; +using BepuPhysics.CollisionDetection; +using BepuPhysics.Constraints; +using DemoContentLoader; +using DemoRenderer; +using DemoRenderer.UI; +using DemoUtilities; +using Demos.Demos; + +namespace Demos.SpecializedTests; + +/// +/// Launches identical rows of boxes across two surfaces with matched materials: a single static box (convex manifolds, one Contact4OneBody per pair) +/// and a densely tessellated flat mesh (MeshReduction merges several convex triangle manifolds into one nonconvex manifold, Contact4NonconvexOneBody). +/// If the convex and nonconvex constraint formulations treat the friction coefficient consistently, each row should stop at the same distance on both +/// surfaces, and both should land near the analytic v^2 / (2 * mu * g). +/// +public class FrictionConsistencyTestDemo : Demo +{ + const int rowCount = 10; + const float launchSpeed = 10; + const float spinSpeed = 10; + const float gravity = 10; + + public BodyHandle[] BoxSurfaceBoxes; + public BodyHandle[] MeshSurfaceBoxes; + public BodyHandle[] BoxSurfaceSpinners; + public BodyHandle[] MeshSurfaceSpinners; + public BodyHandle[] BoxSurfaceSpinSliders; + public BodyHandle[] MeshSurfaceSpinSliders; + public float[] BoxSurfaceSpinAngles; + public float[] MeshSurfaceSpinAngles; + public float[] BoxSurfaceSpinSliderAngles; + public float[] MeshSurfaceSpinSliderAngles; + public float[] SpinSliderStartZ; + public float[] FrictionCoefficients; + float startX; + + public override void Initialize(ContentArchive content, Camera camera) + { + camera.Position = new Vector3(10, 5, 120); + camera.Yaw = 0; + camera.Pitch = 0.4f; + + var collidableMaterials = new CollidableProperty(); + Simulation = Simulation.Create(BufferPool, new FrictionDemo.FrictionCallbacks { CollidableMaterials = collidableMaterials }, new DemoPoseIntegratorCallbacks(new Vector3(0, -gravity, 0)), new SolveDescription(4, 1)); + + var surfaceMaterial = new FrictionDemo.SimpleMaterial { FrictionCoefficient = 1, MaximumRecoveryVelocity = 2, SpringSettings = new SpringSettings(30, 1) }; + + //Convex surface: one big box. + collidableMaterials.Allocate(Simulation.Statics.Add(new StaticDescription(new Vector3(50, -0.5f, 14), Simulation.Shapes.Add(new Box(140, 1, 64))))) = surfaceMaterial; + + //Nonconvex surface: flat mesh with 0.5 unit cells, so a 2x2 box footprint spans several triangles and MeshReduction has multiple manifolds to merge. + var mesh = DemoMeshHelper.CreateDeformedPlane(281, 129, (x, y) => new Vector3(x, 0, y), new Vector3(0.5f, 1, 0.5f), BufferPool); + collidableMaterials.Allocate(Simulation.Statics.Add(new StaticDescription(new Vector3(-20, 0, 50), Simulation.Shapes.Add(mesh)))) = surfaceMaterial; + + var shape = new Box(2, 0.5f, 2); + var shapeIndex = Simulation.Shapes.Add(shape); + var inertia = shape.ComputeInertia(1); + + BoxSurfaceBoxes = new BodyHandle[rowCount]; + MeshSurfaceBoxes = new BodyHandle[rowCount]; + BoxSurfaceSpinners = new BodyHandle[rowCount]; + MeshSurfaceSpinners = new BodyHandle[rowCount]; + BoxSurfaceSpinSliders = new BodyHandle[rowCount]; + MeshSurfaceSpinSliders = new BodyHandle[rowCount]; + BoxSurfaceSpinAngles = new float[rowCount]; + MeshSurfaceSpinAngles = new float[rowCount]; + BoxSurfaceSpinSliderAngles = new float[rowCount]; + MeshSurfaceSpinSliderAngles = new float[rowCount]; + SpinSliderStartZ = new float[rowCount]; + FrictionCoefficients = new float[rowCount]; + startX = 0; + for (int i = 0; i < rowCount; ++i) + { + //The surface material's coefficient is 1 and the callbacks blend multiplicatively, so the pair coefficient equals the sliding box's coefficient. + FrictionCoefficients[i] = 0.25f * (i + 1); + var material = new FrictionDemo.SimpleMaterial { FrictionCoefficient = FrictionCoefficients[i], MaximumRecoveryVelocity = 2, SpringSettings = new SpringSettings(30, 1) }; + + var description = BodyDescription.CreateDynamic(new Vector3(startX, 0.251f, -15 + i * 3), new Vector3(launchSpeed, 0, 0), inertia, shapeIndex, -1); + BoxSurfaceBoxes[i] = Simulation.Bodies.Add(description); + collidableMaterials.Allocate(BoxSurfaceBoxes[i]) = material; + + description.Pose.Position.Z = 53 + i * 3; + MeshSurfaceBoxes[i] = Simulation.Bodies.Add(description); + collidableMaterials.Allocate(MeshSurfaceBoxes[i]) = material; + + //Twist friction comparison: boxes spinning in place about the vertical axis behind the launch line. + //The convex surface resists through the manifold's central twist constraint; the mesh's nonconvex manifold has no twist constraint, + //so its resistance emerges from the per-contact tangent friction constraints. + var spinnerDescription = BodyDescription.CreateDynamic(new Vector3(-12, 0.251f, -15 + i * 3), new BodyVelocity(default, new Vector3(0, spinSpeed, 0)), inertia, shapeIndex, -1); + BoxSurfaceSpinners[i] = Simulation.Bodies.Add(spinnerDescription); + collidableMaterials.Allocate(BoxSurfaceSpinners[i]) = material; + + spinnerDescription.Pose.Position.Z = 53 + i * 3; + MeshSurfaceSpinners[i] = Simulation.Bodies.Add(spinnerDescription); + collidableMaterials.Allocate(MeshSurfaceSpinners[i]) = material; + + //Simultaneous slide + spin couples the two friction modes: every contact's friction direction has to serve both the linear and angular motion, + //so matching trajectories require the tangent/twist budget split to agree between the convex and nonconvex formulations. + SpinSliderStartZ[i] = 16 + i * 3; + var spinSliderDescription = BodyDescription.CreateDynamic(new Vector3(startX, 0.251f, SpinSliderStartZ[i]), new BodyVelocity(new Vector3(launchSpeed, 0, 0), new Vector3(0, spinSpeed, 0)), inertia, shapeIndex, -1); + BoxSurfaceSpinSliders[i] = Simulation.Bodies.Add(spinSliderDescription); + collidableMaterials.Allocate(BoxSurfaceSpinSliders[i]) = material; + + spinSliderDescription.Pose.Position.Z = SpinSliderStartZ[i] + 68; + MeshSurfaceSpinSliders[i] = Simulation.Bodies.Add(spinSliderDescription); + collidableMaterials.Allocate(MeshSurfaceSpinSliders[i]) = material; + } + } + + public override void Update(Window window, Camera camera, Input input, float dt) + { + base.Update(window, camera, input, dt); + //Integrate spun angle so multi-revolution spins can be reported; orientation alone wraps. + for (int i = 0; i < rowCount; ++i) + { + BoxSurfaceSpinAngles[i] += Simulation.Bodies[BoxSurfaceSpinners[i]].Velocity.Angular.Y * TimestepDuration; + MeshSurfaceSpinAngles[i] += Simulation.Bodies[MeshSurfaceSpinners[i]].Velocity.Angular.Y * TimestepDuration; + BoxSurfaceSpinSliderAngles[i] += Simulation.Bodies[BoxSurfaceSpinSliders[i]].Velocity.Angular.Y * TimestepDuration; + MeshSurfaceSpinSliderAngles[i] += Simulation.Bodies[MeshSurfaceSpinSliders[i]].Velocity.Angular.Y * TimestepDuration; + } + } + + float GetDistance(BodyHandle handle) + { + return Simulation.Bodies[handle].Pose.Position.X - startX; + } + + public override void Render(Renderer renderer, Camera camera, Input input, TextBuilder text, Font font) + { + var resolution = renderer.Surface.Resolution; + var lineY = resolution.Y - 32 - 16 * (rowCount * 2 + 5); + renderer.TextBatcher.Write(text.Clear().Append("Identical boxes slide on a static box (convex manifolds, near strip) and a dense flat mesh (nonconvex manifolds, far strip)."), new Vector2(16, lineY), 16, Vector3.One, font); + lineY += 16; + renderer.TextBatcher.Write(text.Clear().Append("Expected stop distance is v^2 / (2 * mu * g); consistent friction handling would make all three slide columns match per row."), new Vector2(16, lineY), 16, Vector3.One, font); + lineY += 16; + renderer.TextBatcher.Write(text.Clear().Append("The spinner columns report revolutions spun in place; the convex twist constraint and the mesh's emergent per-contact resistance should roughly agree."), new Vector2(16, lineY), 16, Vector3.One, font); + lineY += 24; + for (int i = 0; i < rowCount; ++i) + { + text.Clear().Append("mu ").Append(FrictionCoefficients[i], 2) + .Append(": slide box ").Append(GetDistance(BoxSurfaceBoxes[i]), 2) + .Append(", mesh ").Append(GetDistance(MeshSurfaceBoxes[i]), 2) + .Append(", expected ").Append(launchSpeed * launchSpeed / (2 * FrictionCoefficients[i] * gravity), 2) + .Append(" | spin box ").Append(BoxSurfaceSpinAngles[i] / (2 * MathF.PI), 2) + .Append(" rev, mesh ").Append(MeshSurfaceSpinAngles[i] / (2 * MathF.PI), 2).Append(" rev"); + renderer.TextBatcher.Write(text, new Vector2(16, lineY), 16, Vector3.One, font); + lineY += 16; + } + lineY += 8; + renderer.TextBatcher.Write(text.Clear().Append("Spin-sliders launch with both velocities at once; consistent friction would give matching endpoints and drift on both surfaces."), new Vector2(16, lineY), 16, Vector3.One, font); + lineY += 24; + for (int i = 0; i < rowCount; ++i) + { + text.Clear().Append("mu ").Append(FrictionCoefficients[i], 2) + .Append(": spin-slide box (x ").Append(GetDistance(BoxSurfaceSpinSliders[i]), 2) + .Append(", z ").Append(Simulation.Bodies[BoxSurfaceSpinSliders[i]].Pose.Position.Z - SpinSliderStartZ[i], 2) + .Append(", ").Append(BoxSurfaceSpinSliderAngles[i] / (2 * MathF.PI), 2) + .Append(" rev), mesh (x ").Append(GetDistance(MeshSurfaceSpinSliders[i]), 2) + .Append(", z ").Append(Simulation.Bodies[MeshSurfaceSpinSliders[i]].Pose.Position.Z - (SpinSliderStartZ[i] + 68), 2) + .Append(", ").Append(MeshSurfaceSpinSliderAngles[i] / (2 * MathF.PI), 2).Append(" rev)"); + renderer.TextBatcher.Write(text, new Vector2(16, lineY), 16, Vector3.One, font); + lineY += 16; + } + + //Show which contact constraint types actually exist so there's no guessing about whether the mesh produced nonconvex manifolds. + text.Clear().Append("Constraints: "); + Span typeCounts = stackalloc int[PairCache.CollisionConstraintTypeCount]; + typeCounts.Clear(); + ref var activeSet = ref Simulation.Solver.ActiveSet; + for (int batchIndex = 0; batchIndex < activeSet.Batches.Count; ++batchIndex) + { + ref var batch = ref activeSet.Batches[batchIndex]; + for (int typeBatchIndex = 0; typeBatchIndex < batch.TypeBatches.Count; ++typeBatchIndex) + { + ref var typeBatch = ref batch.TypeBatches[typeBatchIndex]; + if (typeBatch.TypeId < typeCounts.Length) + typeCounts[typeBatch.TypeId] += typeBatch.ConstraintCount; + } + } + bool first = true; + for (int typeId = 0; typeId < typeCounts.Length; ++typeId) + { + if (typeCounts[typeId] > 0) + { + if (!first) + text.Append(", "); + first = false; + var name = Simulation.Solver.TypeProcessors[typeId].GetType().Name; + text.Append(name, 0, name.Length - "TypeProcessor".Length).Append(" x").Append(typeCounts[typeId]); + } + } + renderer.TextBatcher.Write(text, new Vector2(16, lineY + 8), 16, Vector3.One, font); + base.Render(renderer, camera, input, text, font); + } +}