diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..cd42ee3 --- /dev/null +++ b/.gitignore @@ -0,0 +1,2 @@ +bin/ +obj/ diff --git a/Directory.Build.props b/Directory.Build.props new file mode 100644 index 0000000..cbbf6bd --- /dev/null +++ b/Directory.Build.props @@ -0,0 +1,9 @@ + + + + $(YMIR_ROOT) + $(MSBuildThisFileDirectory)..\Ymir + enable + disable + + diff --git a/Lodur.slnx b/Lodur.slnx new file mode 100644 index 0000000..b755d72 --- /dev/null +++ b/Lodur.slnx @@ -0,0 +1,5 @@ + + + + + diff --git a/README.md b/README.md index 06ef5a0..b079601 100644 --- a/README.md +++ b/README.md @@ -4,8 +4,16 @@ A physics character middleware for characters that **feel and react**: they pull back from something hot, jump when poked, and, if they want their objective badly enough, crawl toward it after losing a leg. -**Status: design only. No code exists yet.** The design is -[`docs/lodur-target.md`](docs/lodur-target.md); read it first. +**Status: design plus a first slice of layer 5.** The design is +[`docs/lodur-target.md`](docs/lodur-target.md); read it first. What runs today is a +PD-muscle active ragdoll on Ymir's Box3D SoA world: [`docs/first-slice.md`](docs/first-slice.md) +(measured and unmeasured). Layers 1-4 (perception, reflex, drive, behaviour) are not +coded. + +Entry points: `src/Lodur.Core/PdMuscle.cs` (engine-free PD law, strength per muscle), +`src/Lodur.Body.Ymir/HumanoidRagdoll.cs` (the ragdoll and `Drive`), `tests/Lodur.Tests` +(stand, fall, rest, same-seed-same-hash). Needs a sibling Ymir checkout at +`main` (the SoA joints merge, 2026-11-10, or later); `dotnet test tests/Lodur.Tests` builds its native library with cmake. ## The problem diff --git a/docs/first-slice.md b/docs/first-slice.md new file mode 100644 index 0000000..361e474 --- /dev/null +++ b/docs/first-slice.md @@ -0,0 +1,43 @@ +# First slice: a PD-muscle ragdoll on Ymir + +Bad Day Simulator ladder rung 1: a body that stands, falls and rests deterministically, read through +Ymir's structure-of-arrays world. Nothing above layer 5 exists yet. + +## What is built + +- `Lodur.Core` (`netstandard2.1`, no references): `Quat`, `MuscleGains`, `PdMuscle.Torque`. A PD law on one + joint: the child's rotation relative to the parent and their relative angular velocity in, a torque out. + Strength is a multiplier on every gain; zero is limp and returns exactly zero. Swing (joint frame x, y) and + twist (z) have separate gains. +- `Lodur.Body.Ymir` (`net8.0`, references Ymir.Box3D): `HumanoidRagdoll` builds 13 bodies and 12 joints in a + caller-owned `Box3DSoaWorld`: capsule limbs, box pelvis, torso and feet, a sphere head, ball joints at + spine, neck, shoulders and hips, hinges at elbows, knees and ankles. `Drive(strengths, targets)` writes + each muscle's torque onto the world's torque lanes (equal and opposite on child and parent). The strength + array and the target array belong to the caller; nothing is copied or retained, and a tick allocates + nothing (tested). +- Tests (`tests/Lodur.Tests`): the PD law's behaviour; stand, fall, rest; per-muscle strength; torque + conservation; fractional strength scaling; no allocation; same seed, same state hash. + +## Measured (Linux x64, Debug native build, one machine) + +- At 120 Hz, with full strength and a random one-step shove from the seed (up to 30 N m per axis on pelvis and + torso), the ragdoll holds head height 1.73 m (initial 1.74) through 1.5 s for seeds 1, 2, 3 (tested) and + through about 2 s in the exploratory runs. It then topples: there is no balance controller, so a + pose-holding PD ragdoll is an inverted pendulum with soft ankles. +- With zero strength it falls (head under 0.6 m within 1 s) and its trunk comes to rest within 6 s for seeds + 1, 2, 3 (trunk under 0.05 m/s and 0.2 rad/s). Extremities can keep rocking: a head resting on an arm + chattered at up to about 3.5 rad/s for 10 s in one seed. The test bounds it (under 0.6 m/s, 6 rad/s); it + does not claim every body is still. +- Same seed gives the same FNV-1a hash of every body and joint lane at 14 checkpoints, in one process. A + different seed, or one strength element changed, gives a different hash. + +## Not measured, or limits found + +- Cross-platform bit-identity: unclaimed. Windows build and Unity consumption: unrun. One machine, Debug + native build only; no Release run, no run in a second process. +- Explicit PD through torque lanes limits stiffness and damping: damping times the step must stay under the + inertia it acts on. Hips and spine went unstable at damping 15 and 20 and are set to 3 and 4; the gains + are tuned by running, not derived. Stiff ankles (what balance needs) need joint springs or motors inside + the solver, which Ymir's SoA world does not expose yet. +- A bare sphere rolls on forever, so the head takes a rolling resistance (0.1). +- Hands, a rig licence question, impairment, contact facts (pain input), body removal: not built. diff --git a/src/Lodur.Body.Ymir/AssemblyInfo.cs b/src/Lodur.Body.Ymir/AssemblyInfo.cs new file mode 100644 index 0000000..4b4cdac --- /dev/null +++ b/src/Lodur.Body.Ymir/AssemblyInfo.cs @@ -0,0 +1 @@ +[assembly: System.Runtime.CompilerServices.InternalsVisibleTo("Lodur.Tests")] diff --git a/src/Lodur.Body.Ymir/HumanoidRagdoll.cs b/src/Lodur.Body.Ymir/HumanoidRagdoll.cs new file mode 100644 index 0000000..eaa28f0 --- /dev/null +++ b/src/Lodur.Body.Ymir/HumanoidRagdoll.cs @@ -0,0 +1,139 @@ +using System; +using Ymir.Box3D; + +namespace Lodur.Body.Ymir +{ + /// The ragdoll's joints, in strength-array and target-array order. + internal enum Muscle + { + Spine, Neck, ShoulderL, ShoulderR, ElbowL, ElbowR, HipL, HipR, KneeL, KneeR, AnkleL, AnkleR + } + + /// The ragdoll's bodies, also the limb label written into the world's limb lane. + internal enum Limb + { + Pelvis, Torso, Head, UpperArmL, UpperArmR, LowerArmL, LowerArmR, ThighL, ThighR, ShinL, ShinR, FootL, FootR + } + + /// + /// A 13-body humanoid built in a caller-owned Ymir SoA world (invariant one-physics-world): capsule limbs, + /// box pelvis, torso and feet, a sphere head, spherical joints at spine, neck, shoulders and hips, hinges at + /// elbows, knees and ankles. Facing +z, left is +x, standing on y = 0 at the creation pose, which is also + /// every joint's zero. is the muscle layer: it turns caller-owned strengths and target + /// rotations into torques on the world's torque lanes and does nothing else to the physics. + /// + internal sealed class HumanoidRagdoll + { + public const int MuscleCount = 12; + public const int LimbCount = 13; + + private const float Density = 1000f; + private const float Friction = 0.8f; + private const float HeadRolling = 0.1f; // a bare sphere on the ground rolls on, so the fallen head never settles + + private readonly Box3DSoaWorld _world; + private readonly int[] _bodyLane = new int[LimbCount]; + private readonly int[] _jointLane = new int[MuscleCount]; + private readonly MuscleGains[] _gains = new MuscleGains[MuscleCount]; + + private HumanoidRagdoll(Box3DSoaWorld world) => _world = world; + + /// World lane index of a limb's body. + public int BodyLane(Limb limb) => _bodyLane[(int)limb]; + + /// World lane index of a muscle's joint. + public int JointLane(Muscle muscle) => _jointLane[(int)muscle]; + + /// Body and joint capacity a world needs to hold one ragdoll (plus whatever else the caller adds). + public static (int Bodies, int Joints) Needs => (LimbCount, MuscleCount); + + /// Builds the ragdoll standing with its feet on y = 0 at (x, z used). Stable ids are firstStableId.. + 12. + public static HumanoidRagdoll Build(Box3DSoaWorld world, (float X, float Z) origin, ulong firstStableId) + { + var r = new HumanoidRagdoll(world); + var ox = origin.X; + var oz = origin.Z; + ulong id = firstStableId; + + int Box(Limb limb, int parent, float x, float y, float z, float hx, float hy, float hz) => + r._bodyLane[(int)limb] = world.AddBox(id++, (ox + x, y, oz + z), (hx, hy, hz), Density, + friction: Friction, parent: parent, limb: (int)limb); + int Capsule(Limb limb, int parent, float x, float y, float radius, float half) => + r._bodyLane[(int)limb] = world.AddCapsule(id++, (ox + x, y, oz), radius, half, Density, + friction: Friction, parent: parent, limb: (int)limb); + + var pelvis = Box(Limb.Pelvis, -1, 0f, 1.11f, 0f, 0.15f, 0.08f, 0.09f); + var torso = Box(Limb.Torso, pelvis, 0f, 1.41f, 0f, 0.15f, 0.22f, 0.09f); + var head = r._bodyLane[(int)Limb.Head] = world.AddSphere(id++, (ox, 1.74f, oz), 0.11f, Density, + friction: Friction, parent: torso, limb: (int)Limb.Head, rollingResistance: HeadRolling); + foreach (var (side, sx) in new[] { (0, 1f), (1, -1f) }) + { + var upperArm = Capsule(Limb.UpperArmL + side, torso, sx * 0.21f, 1.395f, 0.045f, 0.11f); + var lowerArm = Capsule(Limb.LowerArmL + side, upperArm, sx * 0.21f, 1.10f, 0.04f, 0.10f); + var thigh = Capsule(Limb.ThighL + side, pelvis, sx * 0.09f, 0.79f, 0.07f, 0.18f); + var shin = Capsule(Limb.ShinL + side, thigh, sx * 0.09f, 0.31f, 0.06f, 0.17f); + Box(Limb.FootL + side, shin, sx * 0.09f, 0.04f, 0.05f, 0.05f, 0.04f, 0.12f); + } + + void Joint(Muscle m, Box3DSoaJointKind kind, Limb parent, Limb child, float x, float y, float z, + (float, float, float) axis, MuscleGains gains, float lower = 0f, float upper = 0f, float cone = 0f) + { + r._jointLane[(int)m] = world.AddJoint(kind, r._bodyLane[(int)parent], r._bodyLane[(int)child], + (ox + x, y, oz + z), axis, limit: true, lower: lower, upper: upper, coneAngle: cone); + r._gains[(int)m] = gains; + } + + var up = (0f, 1f, 0f); + var across = (1f, 0f, 0f); + // Gains are tuned against measured stand/fall runs at 120 Hz (see the tests), not derived: explicit PD on + // light limbs is stable only while damping * dt stays under the inertia it acts on. + Joint(Muscle.Spine, Box3DSoaJointKind.Spherical, Limb.Pelvis, Limb.Torso, 0f, 1.19f, 0f, up, new MuscleGains(500f, 4f, 200f, 8f), -0.4f, 0.4f, 0.5f); + Joint(Muscle.Neck, Box3DSoaJointKind.Spherical, Limb.Torso, Limb.Head, 0f, 1.63f, 0f, up, new MuscleGains(30f, 0.8f, 20f, 0.6f), -0.6f, 0.6f, 0.6f); + for (var side = 0; side < 2; side++) + { + var sx = side == 0 ? 1f : -1f; + Joint(Muscle.ShoulderL + side, Box3DSoaJointKind.Spherical, Limb.Torso, Limb.UpperArmL + side, sx * 0.21f, 1.55f, 0f, up, new MuscleGains(20f, 1f, 1f, 0.02f), -1f, 1f, 1.5f); + Joint(Muscle.ElbowL + side, Box3DSoaJointKind.Revolute, Limb.UpperArmL + side, Limb.LowerArmL + side, sx * 0.21f, 1.24f, 0f, across, new MuscleGains(0f, 0f, 12f, 0.6f), -2.4f, 0f); + Joint(Muscle.HipL + side, Box3DSoaJointKind.Spherical, Limb.Pelvis, Limb.ThighL + side, sx * 0.09f, 1.04f, 0f, up, new MuscleGains(600f, 3f, 10f, 0.15f), -0.5f, 0.5f, 1.2f); + Joint(Muscle.KneeL + side, Box3DSoaJointKind.Revolute, Limb.ThighL + side, Limb.ShinL + side, sx * 0.09f, 0.54f, 0f, across, new MuscleGains(0f, 0f, 300f, 8f), 0f, 2.4f); + Joint(Muscle.AnkleL + side, Box3DSoaJointKind.Revolute, Limb.ShinL + side, Limb.FootL + side, sx * 0.09f, 0.08f, 0f, across, new MuscleGains(0f, 0f, 100f, 0.5f), -0.6f, 0.6f); + } + return r; + } + + /// + /// Writes this tick's muscle torques onto the world's torque lanes: for each joint a PD torque toward + /// [joint] (the child's rotation relative to the parent in the joint frame, whose z axis is a hinge's axis; a hinge has no swing gains, so it takes torque about that axis only) scaled by [joint], applied to the child + /// and, equal and opposite, to the parent. Both arrays are the caller's, one entry per ; + /// nothing is copied or retained. Allocates nothing. Call once per step, before world.Step; the + /// world clears the lanes when it applies them. + /// + public void Drive(ReadOnlySpan strengths, ReadOnlySpan targets) + { + if (strengths.Length != MuscleCount || targets.Length != MuscleCount) + { + throw new ArgumentException($"strengths and targets need {MuscleCount} entries, one per muscle."); + } + + var v = _world.View; + for (var m = 0; m < MuscleCount; m++) + { + if (!(strengths[m] > 0f)) + { + continue; // limp (or NaN): no torque + } + + var j = _jointLane[m]; + var rotation = new Quat(v.JointRotationX[j], v.JointRotationY[j], v.JointRotationZ[j], v.JointRotationW[j]); + PdMuscle.Torque(rotation, v.JointAngularVelocityX[j], v.JointAngularVelocityY[j], v.JointAngularVelocityZ[j], + targets[m], _gains[m], strengths[m], out var tx, out var ty, out var tz); + new Quat(v.JointFrameX[j], v.JointFrameY[j], v.JointFrameZ[j], v.JointFrameW[j]) + .Rotate(tx, ty, tz, out var wx, out var wy, out var wz); + var child = v.JointBodyB[j]; + var parent = v.JointBodyA[j]; + v.TorqueX[child] += wx; v.TorqueY[child] += wy; v.TorqueZ[child] += wz; + v.TorqueX[parent] -= wx; v.TorqueY[parent] -= wy; v.TorqueZ[parent] -= wz; + } + } + } +} diff --git a/src/Lodur.Body.Ymir/Lodur.Body.Ymir.csproj b/src/Lodur.Body.Ymir/Lodur.Body.Ymir.csproj new file mode 100644 index 0000000..ddd418c --- /dev/null +++ b/src/Lodur.Body.Ymir/Lodur.Body.Ymir.csproj @@ -0,0 +1,15 @@ + + + + + net8.0 + Lodur.Body.Ymir + true + + + + + + + + diff --git a/src/Lodur.Core/Lodur.Core.csproj b/src/Lodur.Core/Lodur.Core.csproj new file mode 100644 index 0000000..9588aab --- /dev/null +++ b/src/Lodur.Core/Lodur.Core.csproj @@ -0,0 +1,10 @@ + + + + + netstandard2.1 + 12.0 + Lodur + + + diff --git a/src/Lodur.Core/PdMuscle.cs b/src/Lodur.Core/PdMuscle.cs new file mode 100644 index 0000000..05c0cb1 --- /dev/null +++ b/src/Lodur.Core/PdMuscle.cs @@ -0,0 +1,102 @@ +using System; + +namespace Lodur +{ + /// A unit quaternion, x y z w. + public readonly struct Quat + { + public readonly float X, Y, Z, W; + + public Quat(float x, float y, float z, float w) + { + X = x; Y = y; Z = z; W = w; + } + + public static Quat Identity => new Quat(0f, 0f, 0f, 1f); + + public Quat Conjugate() => new Quat(-X, -Y, -Z, W); + + public static Quat operator *(Quat a, Quat b) => new Quat( + a.W * b.X + a.X * b.W + a.Y * b.Z - a.Z * b.Y, + a.W * b.Y - a.X * b.Z + a.Y * b.W + a.Z * b.X, + a.W * b.Z + a.X * b.Y - a.Y * b.X + a.Z * b.W, + a.W * b.W - a.X * b.X - a.Y * b.Y - a.Z * b.Z); + + /// Rotation about ,, (a unit axis) by . + public static Quat FromAxisAngle(float x, float y, float z, float radians) + { + var half = 0.5f * radians; + var s = MathF.Sin(half); + return new Quat(x * s, y * s, z * s, MathF.Cos(half)); + } + + /// Rotates the vector v by this quaternion. + public void Rotate(float vx, float vy, float vz, out float rx, out float ry, out float rz) + { + // v' = v + 2w(q x v) + 2 q x (q x v) + var tx = 2f * (Y * vz - Z * vy); + var ty = 2f * (Z * vx - X * vz); + var tz = 2f * (X * vy - Y * vx); + rx = vx + W * tx + (Y * tz - Z * ty); + ry = vy + W * ty + (Z * tx - X * tz); + rz = vz + W * tz + (X * ty - Y * tx); + } + } + + /// + /// Gains of one muscle. Swing acts on the joint frame's x and y axes, twist on its z axis (the hinge axis + /// of a hinge, the limb's own axis for a ball joint). They differ because a limb turns about its own axis + /// with a fraction of the inertia it has swinging about its end, and an explicit controller is unstable + /// when damping times the step exceeds that inertia. + /// + public readonly struct MuscleGains + { + public readonly float SwingStiffness, SwingDamping, TwistStiffness, TwistDamping; + + public MuscleGains(float swingStiffness, float swingDamping, float twistStiffness, float twistDamping) + { + SwingStiffness = swingStiffness; SwingDamping = swingDamping; + TwistStiffness = twistStiffness; TwistDamping = twistDamping; + } + } + + /// + /// The body layer's muscle: a proportional-derivative law on one joint. It sees only the joint's + /// relative rotation and angular velocity, a target rotation and a strength, and returns a torque; + /// it knows nothing of the physics engine that produced those numbers (invariant body-one-interface). + /// + public static class PdMuscle + { + /// + /// Torque on the child, in the parent joint frame, that turns (child frame + /// relative to parent frame) toward and damps + /// (child minus parent, in the parent frame). Strength scales every gain together, so the + /// damping ratio does not change with it; strength 0 is a limp joint and returns exactly zero. + /// The rotation error takes the shorter way round. + /// + public static void Torque( + in Quat rotation, float wx, float wy, float wz, in Quat target, in MuscleGains gains, float strength, + out float tx, out float ty, out float tz) + { + if (strength <= 0f) + { + tx = ty = tz = 0f; + return; + } + + // Error rotation in the parent frame that carries the current rotation onto the target. + var e = target * rotation.Conjugate(); + if (e.W < 0f) + { + e = new Quat(-e.X, -e.Y, -e.Z, -e.W); + } + + var sin = MathF.Sqrt(e.X * e.X + e.Y * e.Y + e.Z * e.Z); + // Rotation vector of the error: angle * axis, with the small-angle limit of 2 * vector part. + var scale = sin < 1e-6f ? 2f : 2f * MathF.Atan2(sin, e.W) / sin; + tx = strength * (gains.SwingStiffness * scale * e.X - gains.SwingDamping * wx); + ty = strength * (gains.SwingStiffness * scale * e.Y - gains.SwingDamping * wy); + tz = strength * (gains.TwistStiffness * scale * e.Z - gains.TwistDamping * wz); + } + } +} diff --git a/tests/Lodur.Tests/Lodur.Tests.csproj b/tests/Lodur.Tests/Lodur.Tests.csproj new file mode 100644 index 0000000..86df1b6 --- /dev/null +++ b/tests/Lodur.Tests/Lodur.Tests.csproj @@ -0,0 +1,24 @@ + + + + net10.0 + enable + false + + + + + + + + + + + + + + + + + + diff --git a/tests/Lodur.Tests/PdMuscleTests.cs b/tests/Lodur.Tests/PdMuscleTests.cs new file mode 100644 index 0000000..6a1eeb6 --- /dev/null +++ b/tests/Lodur.Tests/PdMuscleTests.cs @@ -0,0 +1,86 @@ +using Lodur; + +namespace Lodur.Tests; + +public sealed class PdMuscleTests +{ + private static (float X, float Y, float Z) Torque(Quat rotation, (float X, float Y, float Z) w, Quat target, float kp, float kd, float strength, float? twistKp = null, float? twistKd = null) + { + PdMuscle.Torque(rotation, w.X, w.Y, w.Z, target, new MuscleGains(kp, kd, twistKp ?? kp, twistKd ?? kd), strength, out var x, out var y, out var z); + return (x, y, z); + } + + [Fact] + public void At_the_target_and_at_rest_there_is_no_torque() + { + var q = Quat.FromAxisAngle(0f, 1f, 0f, 0.7f); + var t = Torque(q, (0f, 0f, 0f), q, 100f, 10f, 1f); + Assert.Equal((0f, 0f, 0f), (MathF.Abs(t.X) < 1e-4f ? 0f : t.X, MathF.Abs(t.Y) < 1e-4f ? 0f : t.Y, MathF.Abs(t.Z) < 1e-4f ? 0f : t.Z)); + } + + [Fact] + public void Torque_turns_the_child_toward_the_target_in_proportion_to_the_error() + { + var near = Torque(Quat.Identity, default, Quat.FromAxisAngle(0f, 0f, 1f, 0.1f), 100f, 0f, 1f); + var far = Torque(Quat.Identity, default, Quat.FromAxisAngle(0f, 0f, 1f, 0.2f), 100f, 0f, 1f); + Assert.Equal(10f, near.Z, 1e-2f); // kp * angle about +z + Assert.Equal(20f, far.Z, 1e-2f); + Assert.Equal(0f, near.X, 1e-4f); + Assert.Equal(0f, near.Y, 1e-4f); + var back = Torque(Quat.FromAxisAngle(0f, 0f, 1f, 0.3f), default, Quat.Identity, 100f, 0f, 1f); + Assert.Equal(-30f, back.Z, 1e-2f); // the error is a rotation, so it flips sign with direction + } + + [Fact] + public void Damping_opposes_the_relative_angular_velocity() + { + var t = Torque(Quat.Identity, (1f, -2f, 3f), Quat.Identity, 100f, 5f, 1f); + Assert.Equal((-5f, 10f, -15f), (t.X, t.Y, t.Z)); + } + + [Fact] + public void Strength_scales_the_whole_law_and_zero_is_limp() + { + var target = Quat.FromAxisAngle(1f, 0f, 0f, 0.4f); + var full = Torque(Quat.Identity, (0.5f, 0f, 0f), target, 100f, 10f, 1f); + var half = Torque(Quat.Identity, (0.5f, 0f, 0f), target, 100f, 10f, 0.5f); + Assert.Equal(full.X * 0.5f, half.X, 1e-4f); + var limp = Torque(Quat.Identity, (0.5f, 0f, 0f), target, 100f, 10f, 0f); + Assert.Equal((0f, 0f, 0f), limp); + Assert.Equal((0f, 0f, 0f), Torque(Quat.Identity, (0.5f, 0f, 0f), target, 100f, 10f, -1f)); + } + + [Fact] + public void The_error_takes_the_shorter_way_round_even_when_the_quaternions_are_opposite_signs() + { + // 350 degrees about z is 10 degrees the other way. + var rotation = Quat.FromAxisAngle(0f, 0f, 1f, 350f * MathF.PI / 180f); + var t = Torque(rotation, default, Quat.Identity, 100f, 0f, 1f); + Assert.Equal(100f * 10f * MathF.PI / 180f, t.Z, 1e-2f); + // The same rotation written with every sign flipped gives the same torque. + var flipped = new Quat(-rotation.X, -rotation.Y, -rotation.Z, -rotation.W); + var t2 = Torque(flipped, default, Quat.Identity, 100f, 0f, 1f); + Assert.Equal(t.Z, t2.Z, 1e-3f); + } + + [Fact] + public void Twist_gains_act_on_z_only_and_swing_gains_on_x_and_y() + { + var w = (1f, 1f, 1f); + var t = Torque(Quat.Identity, w, Quat.Identity, 100f, 10f, 1f, twistKp: 100f, twistKd: 1f); + Assert.Equal((-10f, -10f, -1f), (t.X, t.Y, t.Z)); + var e = Torque(Quat.Identity, default, Quat.FromAxisAngle(1f, 0f, 0f, 0.1f), 100f, 0f, 1f, twistKp: 5f, twistKd: 0f); + Assert.Equal(10f, e.X, 1e-2f); + var z = Torque(Quat.Identity, default, Quat.FromAxisAngle(0f, 0f, 1f, 0.1f), 100f, 0f, 1f, twistKp: 5f, twistKd: 0f); + Assert.Equal(0.5f, z.Z, 1e-3f); + } + + [Fact] + public void Quaternion_rotate_matches_the_axis_angle() + { + Quat.FromAxisAngle(0f, 0f, 1f, MathF.PI / 2).Rotate(1f, 0f, 0f, out var x, out var y, out var z); + Assert.Equal(0f, x, 1e-5f); + Assert.Equal(1f, y, 1e-5f); + Assert.Equal(0f, z, 1e-5f); + } +} diff --git a/tests/Lodur.Tests/RagdollScenario.cs b/tests/Lodur.Tests/RagdollScenario.cs new file mode 100644 index 0000000..aebfbcc --- /dev/null +++ b/tests/Lodur.Tests/RagdollScenario.cs @@ -0,0 +1,118 @@ +using Lodur; +using Lodur.Body.Ymir; +using Ymir.Box3D; + +namespace Lodur.Tests; + +/// One ragdoll on a ground slab, stepped at 120 Hz, with caller-owned strength and target arrays. +internal sealed class RagdollScenario : IDisposable +{ + public const float Dt = 1f / 120f; + public readonly Box3DSoaWorld World; + public readonly HumanoidRagdoll Ragdoll; + public readonly float[] Strength = new float[HumanoidRagdoll.MuscleCount]; + public readonly Quat[] Target = new Quat[HumanoidRagdoll.MuscleCount]; + private int _steps; + + public RagdollScenario(ulong seed) + { + var (bodies, joints) = HumanoidRagdoll.Needs; + World = new Box3DSoaWorld(capacity: bodies + 1, jointCapacity: joints); + World.AddStaticBox(1, (0f, -0.5f, 0f), (20f, 0.5f, 20f), friction: 0.8f); + Ragdoll = HumanoidRagdoll.Build(World, (0f, 0f), firstStableId: 100); + Array.Fill(Target, Quat.Identity); + Array.Fill(Strength, 1f); + Shove(seed); + } + + /// The seed's only effect: one step of random torque on pelvis and torso, up to 30 N m per axis. + private void Shove(ulong seed) + { + var rng = new SplitMix64(seed); + var v = World.View; + foreach (var limb in new[] { Limb.Pelvis, Limb.Torso }) + { + var i = Ragdoll.BodyLane(limb); + v.TorqueX[i] = 30f * rng.NextSigned(); + v.TorqueY[i] = 30f * rng.NextSigned(); + v.TorqueZ[i] = 30f * rng.NextSigned(); + } + } + + public void Run(float seconds) + { + var n = (int)MathF.Round(seconds / Dt); + for (var i = 0; i < n; i++) + { + Ragdoll.Drive(Strength, Target); + World.Step(Dt, 4); + _steps++; + } + } + + public float HeadHeight => World.View.PositionY[Ragdoll.BodyLane(Limb.Head)]; + public float PelvisHeight => World.View.PositionY[Ragdoll.BodyLane(Limb.Pelvis)]; + + /// Linear and angular speed of one limb's body. + public (float Linear, float Angular) Speeds(Limb limb) + { + var v = World.View; + var i = Ragdoll.BodyLane(limb); + return (MathF.Sqrt(v.VelocityX[i] * v.VelocityX[i] + v.VelocityY[i] * v.VelocityY[i] + v.VelocityZ[i] * v.VelocityZ[i]), + MathF.Sqrt(v.AngularVelocityX[i] * v.AngularVelocityX[i] + v.AngularVelocityY[i] * v.AngularVelocityY[i] + v.AngularVelocityZ[i] * v.AngularVelocityZ[i])); + } + + /// The fastest linear and angular speed of any ragdoll body. + public (float Linear, float Angular) MaxSpeeds() + { + var v = World.View; + float lin = 0f, ang = 0f; + for (var i = 1; i < v.Count; i++) + { + lin = MathF.Max(lin, MathF.Sqrt(v.VelocityX[i] * v.VelocityX[i] + v.VelocityY[i] * v.VelocityY[i] + v.VelocityZ[i] * v.VelocityZ[i])); + ang = MathF.Max(ang, MathF.Sqrt(v.AngularVelocityX[i] * v.AngularVelocityX[i] + v.AngularVelocityY[i] * v.AngularVelocityY[i] + v.AngularVelocityZ[i] * v.AngularVelocityZ[i])); + } + return (lin, ang); + } + + /// FNV-1a 64 over every body and joint state lane, byte for byte, in lane order. + public ulong StateHash() + { + var v = World.View; + var h = 14695981039346656037ul; + void Bytes(ReadOnlySpan b) + { + foreach (var x in b) { h ^= x; h *= 1099511628211ul; } + } + void F(ReadOnlySpan lane) => Bytes(System.Runtime.InteropServices.MemoryMarshal.AsBytes(lane)); + Bytes(System.Runtime.InteropServices.MemoryMarshal.AsBytes(BitConverter.GetBytes(World.StepCount).AsSpan())); + F(v.PositionX); F(v.PositionY); F(v.PositionZ); + F(v.RotationX); F(v.RotationY); F(v.RotationZ); F(v.RotationW); + F(v.VelocityX); F(v.VelocityY); F(v.VelocityZ); + F(v.AngularVelocityX); F(v.AngularVelocityY); F(v.AngularVelocityZ); + F(v.JointAngle); + F(v.JointRotationX); F(v.JointRotationY); F(v.JointRotationZ); F(v.JointRotationW); + F(v.JointAngularVelocityX); F(v.JointAngularVelocityY); F(v.JointAngularVelocityZ); + return h; + } + + public void Dispose() => World.Dispose(); +} + +/// SplitMix64: a seedable generator whose sequence is the same on every platform. +internal struct SplitMix64 +{ + private ulong _s; + public SplitMix64(ulong seed) => _s = seed; + + public ulong Next() + { + var z = _s += 0x9E3779B97F4A7C15ul; + z = (z ^ (z >> 30)) * 0xBF58476D1CE4E5B9ul; + z = (z ^ (z >> 27)) * 0x94D049BB133111EBul; + return z ^ (z >> 31); + } + + /// Uniform in [-1, 1). + public float NextSigned() => (Next() >> 40) / (float)(1 << 23) - 1f; +} diff --git a/tests/Lodur.Tests/RagdollTests.cs b/tests/Lodur.Tests/RagdollTests.cs new file mode 100644 index 0000000..0590a97 --- /dev/null +++ b/tests/Lodur.Tests/RagdollTests.cs @@ -0,0 +1,214 @@ +using Lodur; +using Lodur.Body.Ymir; +using Ymir.Box3D; + +namespace Lodur.Tests; + +public sealed class RagdollTests +{ + private const float StandingHead = 1.74f; + + [Theory] + [InlineData(1ul)] + [InlineData(2ul)] + [InlineData(3ul)] + public void A_ragdoll_with_full_strength_stands_through_a_shove(ulong seed) + { + using var s = new RagdollScenario(seed); + s.Run(1.5f); + Assert.InRange(s.HeadHeight, StandingHead - 0.1f, StandingHead + 0.05f); + Assert.True(s.PelvisHeight > 1.05f); + var (linear, angular) = s.MaxSpeeds(); + Assert.True(linear < 0.6f && angular < 1.0f, $"settled upright: {linear} m/s, {angular} rad/s"); + } + + /// The trunk is at rest; extremities may still rock a little (a head resting on an arm chatters at a few rad/s for seconds). + private static void AssertResting(RagdollScenario s, string label) + { + foreach (var limb in new[] { Limb.Pelvis, Limb.Torso }) + { + var (linear, angular) = s.Speeds(limb); + Assert.True(linear < 0.05f && angular < 0.2f, $"{label}: {limb} at {linear} m/s, {angular} rad/s"); + } + var (maxLinear, maxAngular) = s.MaxSpeeds(); + Assert.True(maxLinear < 0.6f && maxAngular < 6f, $"{label}: an extremity still moves at {maxLinear} m/s, {maxAngular} rad/s"); + Assert.True(s.HeadHeight < 0.4f && s.PelvisHeight < 0.3f, $"{label}: lying down"); + var v = s.World.View; + for (var i = 1; i < v.Count; i++) + { + Assert.True(v.PositionY[i] > 0f && float.IsFinite(v.PositionY[i]), $"{label}: body {i} above the ground"); + } + } + + [Theory] + [InlineData(1ul)] + [InlineData(2ul)] + [InlineData(3ul)] + public void Without_strength_the_same_ragdoll_falls_and_comes_to_rest(ulong seed) + { + using var s = new RagdollScenario(seed); + Array.Clear(s.Strength); + s.Run(1.0f); + Assert.True(s.HeadHeight < 0.6f, $"limp joints cannot hold it up: head at {s.HeadHeight}"); + s.Run(5.0f); + AssertResting(s, $"seed {seed}"); + } + + [Fact] + public void Stand_then_go_limp_falls_from_height_and_rests() + { + using var s = new RagdollScenario(seed: 7); + s.Run(1.5f); + Assert.True(s.HeadHeight > 1.6f); + Array.Clear(s.Strength); // will gives out: the caller's array is the only thing that changed + s.Run(1.0f); + Assert.True(s.HeadHeight < 1.0f, $"falling after strength drops: head at {s.HeadHeight}"); + s.Run(5.0f); + AssertResting(s, "after standing"); + } + + [Fact] + public void Strength_is_per_muscle_zeroing_one_knee_changes_that_knee_and_not_the_other_arrays_entries() + { + static (float Left, float Right) Knees(float leftStrength) + { + using var s = new RagdollScenario(seed: 1); + // Both knees are told to flex 0.5 rad about their hinge (the joint frame's z axis). + s.Target[(int)Muscle.KneeL] = Quat.FromAxisAngle(0f, 0f, 1f, 0.5f); + s.Target[(int)Muscle.KneeR] = Quat.FromAxisAngle(0f, 0f, 1f, 0.5f); + s.Strength[(int)Muscle.KneeL] = leftStrength; // a caller-owned element, written in place + s.Run(0.5f); + var v = s.World.View; + return (v.JointAngle[s.Ragdoll.JointLane(Muscle.KneeL)], v.JointAngle[s.Ragdoll.JointLane(Muscle.KneeR)]); + } + + var (strongL, strongR) = Knees(1f); + var (limpL, limpR) = Knees(0f); + Assert.InRange(strongL, 0.35f, 1.2f); // both follow the target, plus the sag of the body's weight + Assert.True(MathF.Abs(strongL - strongR) < 0.15f, $"symmetric when both are strong: {strongL} {strongR}"); + Assert.True(MathF.Abs(limpL - limpR) > 0.3f, $"the limp knee no longer tracks: left {limpL}, right {limpR}"); + } + + [Fact] + public void Same_seed_gives_the_same_state_hash_at_every_checkpoint_and_other_seeds_differ() + { + static List Hashes(ulong seed, int strengthElementToFlip = -1) + { + using var s = new RagdollScenario(seed); + if (strengthElementToFlip >= 0) s.Strength[strengthElementToFlip] = 0.5f; + var hashes = new List(); + for (var i = 0; i < 6; i++) { s.Run(0.5f); hashes.Add(s.StateHash()); } + Array.Clear(s.Strength); // stand, then fall + for (var i = 0; i < 8; i++) { s.Run(0.5f); hashes.Add(s.StateHash()); } + return hashes; + } + + var a = Hashes(42); + Assert.Equal(a, Hashes(42)); + Assert.Equal(a.Count, a.Distinct().Count()); // the hash moves every checkpoint: the ragdoll is not frozen + Assert.NotEqual(a[0], Hashes(43)[0]); // the seed matters + Assert.NotEqual(a[0], Hashes(42, (int)Muscle.HipL)[0]); // and so does one strength element + } + + [Fact] + public void Muscle_torques_are_equal_and_opposite_on_parent_and_child() + { + using var s = new RagdollScenario(seed: 1); + var v = s.World.View; + v.TorqueX.Clear(); v.TorqueY.Clear(); v.TorqueZ.Clear(); // drop the shove + Array.Clear(s.Strength); + s.Strength[(int)Muscle.ElbowL] = 1f; + s.Strength[(int)Muscle.HipR] = 1f; + s.Target[(int)Muscle.ElbowL] = Quat.FromAxisAngle(0f, 0f, 1f, -0.5f); + s.Target[(int)Muscle.HipR] = Quat.FromAxisAngle(1f, 0f, 0f, 0.4f); + s.Ragdoll.Drive(s.Strength, s.Target); + + float sx = 0f, sy = 0f, sz = 0f; + for (var i = 0; i < v.Count; i++) { sx += v.TorqueX[i]; sy += v.TorqueY[i]; sz += v.TorqueZ[i]; } + Assert.Equal(0f, sx, 1e-3f); Assert.Equal(0f, sy, 1e-3f); Assert.Equal(0f, sz, 1e-3f); + + // Only the four bodies of those two joints were touched, and the elbow child got the hinge torque. + var touched = Enumerable.Range(0, v.Count).Where(i => v.TorqueX[i] != 0f || v.TorqueY[i] != 0f || v.TorqueZ[i] != 0f).ToHashSet(); + Assert.Equal(new HashSet + { + s.Ragdoll.BodyLane(Limb.UpperArmL), s.Ragdoll.BodyLane(Limb.LowerArmL), + s.Ragdoll.BodyLane(Limb.Pelvis), s.Ragdoll.BodyLane(Limb.ThighR), + }, touched); + Assert.NotEqual(0f, v.TorqueX[s.Ragdoll.BodyLane(Limb.LowerArmL)]); + } + + private static float TorqueNorm(RagdollScenario s, Limb limb) + { + var v = s.World.View; + var i = s.Ragdoll.BodyLane(limb); + return MathF.Sqrt(v.TorqueX[i] * v.TorqueX[i] + v.TorqueY[i] * v.TorqueY[i] + v.TorqueZ[i] * v.TorqueZ[i]); + } + + private static void ClearTorques(RagdollScenario s) + { + var v = s.World.View; + v.TorqueX.Clear(); v.TorqueY.Clear(); v.TorqueZ.Clear(); + } + + [Fact] + public void A_fractional_strength_scales_the_written_torque_linearly() + { + using var s = new RagdollScenario(seed: 1); + Array.Clear(s.Strength); + s.Target[(int)Muscle.HipL] = Quat.FromAxisAngle(1f, 0f, 0f, 0.3f); + ClearTorques(s); + s.Strength[(int)Muscle.HipL] = 1f; + s.Ragdoll.Drive(s.Strength, s.Target); + var full = TorqueNorm(s, Limb.ThighL); + ClearTorques(s); + s.Strength[(int)Muscle.HipL] = 0.25f; + s.Ragdoll.Drive(s.Strength, s.Target); + Assert.True(full > 1f); + Assert.Equal(full * 0.25f, TorqueNorm(s, Limb.ThighL), full * 1e-3f); + } + + [Fact] + public void A_hinge_takes_torque_about_its_own_axis_only_and_a_ball_joint_takes_any() + { + using var s = new RagdollScenario(seed: 1); + Array.Clear(s.Strength); + var offAxis = Quat.FromAxisAngle(1f, 0f, 0f, 0.3f); // about the joint frame's x: not the hinge axis (z) + s.Target[(int)Muscle.ElbowL] = offAxis; + s.Target[(int)Muscle.HipL] = offAxis; + s.Strength[(int)Muscle.ElbowL] = 1f; + s.Strength[(int)Muscle.HipL] = 1f; + ClearTorques(s); + s.Ragdoll.Drive(s.Strength, s.Target); + Assert.Equal(0f, TorqueNorm(s, Limb.LowerArmL)); // the hinge ignores what it cannot turn + Assert.True(TorqueNorm(s, Limb.ThighL) > 1f); // the ball joint does not + } + + [Fact] + public void Driving_a_tick_allocates_nothing() + { + using var s = new RagdollScenario(seed: 1); + s.Ragdoll.Drive(s.Strength, s.Target); // warm + var before = GC.GetAllocatedBytesForCurrentThread(); + for (var i = 0; i < 1000; i++) s.Ragdoll.Drive(s.Strength, s.Target); + Assert.Equal(0L, GC.GetAllocatedBytesForCurrentThread() - before); + } + + [Fact] + public void The_world_carries_the_limb_tree_and_the_arrays_must_have_one_entry_per_muscle() + { + using var s = new RagdollScenario(seed: 1); + var v = s.World.View; + var r = s.Ragdoll; + Assert.Equal(-1, v.Parent[r.BodyLane(Limb.Pelvis)]); + Assert.Equal(r.BodyLane(Limb.Pelvis), v.Parent[r.BodyLane(Limb.Torso)]); + Assert.Equal(r.BodyLane(Limb.ThighL), v.Parent[r.BodyLane(Limb.ShinL)]); + Assert.Equal(r.BodyLane(Limb.ShinR), v.Parent[r.BodyLane(Limb.FootR)]); + Assert.Equal((int)Limb.LowerArmR, v.Limb[r.BodyLane(Limb.LowerArmR)]); + Assert.Equal((uint)Box3DSoaKind.Capsule, v.Kind[r.BodyLane(Limb.ThighL)]); + Assert.Equal((uint)Box3DSoaKind.Box, v.Kind[r.BodyLane(Limb.Torso)]); + Assert.Equal((uint)Box3DSoaKind.Sphere, v.Kind[r.BodyLane(Limb.Head)]); + Assert.Equal(HumanoidRagdoll.MuscleCount, v.JointCount); + Assert.Throws(() => r.Drive(new float[3], s.Target)); + Assert.Throws(() => r.Drive(s.Strength, new Quat[3])); + } +}