From 60b52c9a2ec5194e1dc45a2db8ea0ec5ab20a07c Mon Sep 17 00:00:00 2001 From: Nicolas Maman Date: Sat, 19 Sep 2026 23:14:01 -0300 Subject: [PATCH] mover: the character mover's plane solver, and the dynamics plan aephysics.mover, Box3D's mover.c in Aether: the plane solver (twenty Gauss-Seidel sweeps, the pushes accumulated and clamped to each plane's limit, the linear slop keeping the mover just off the surface, done when a sweep's total push falls under the slop) and the velocity clip against the planes that pushed. test_mover.ae: the reference's solver cases (two parallel planes, a game's pair with a deep target) and every mover collision that needs no world -- the sphere, capsule and hull separated, touching and in deep overlap, the mesh's back side and mirrored scale, the height field's back side, report and clockwise winding -- plus a corner, a soft plane and the clip. 56 checks. bench/mover.ae against bench/mover_box3d.c: equal sums, one iteration's difference in 1.7 million, 0.9x the reference's time. design.md lays out the dynamics: the reference's world is one mutually recursive body of C, so it becomes mesh_contact, broad_phase (with the pair filter and emission as visitors), dynamics (the world's state and bookkeeping without a step), contact_solver, joint_solver, solver and physics_world, in that order, each with the tests it can carry. --- README.md | 1 + aephysics/mover/module.ae | 85 +++++++++++++ aephysics/test_mover.ae | 245 ++++++++++++++++++++++++++++++++++++++ bench/RESULTS.md | 16 +++ bench/mover.ae | 49 ++++++++ bench/mover_box3d.c | 56 +++++++++ design.md | 72 +++++++++-- 7 files changed, 514 insertions(+), 10 deletions(-) create mode 100644 aephysics/mover/module.ae create mode 100644 aephysics/test_mover.ae create mode 100644 bench/mover.ae create mode 100644 bench/mover_box3d.c diff --git a/README.md b/README.md index d070cf2..8c723b0 100644 --- a/README.md +++ b/README.md @@ -39,6 +39,7 @@ so a test written against the reference reads the same here. | `aephysics.capsule` | the capsule: mass by cylinder and caps, bounds, the ray cast by the closest points of the lines with the near-parallel fallback, the shape cast, overlap, the mover's plane | done, in `test_shape.ae` | | `aephysics.compound` | the baked compound: capsules, hulls, meshes and spheres in one block under a static tree, hulls and meshes shared by content, materials by value; bounds, overlap, ray and shape casts, the box query, the mover's planes | done, `test_compound.ae` (159 checks); [same results as the reference, build 0.7x, queries 1.3-1.6x](bench/RESULTS.md#compound) | | `aephysics.shape` | the shape of any kind (sphere, capsule, hull, mesh, height field, compound) with the dispatch over every kind under a transform: bounds, swept and fat bounds, centroid, areas, mass, extent, ray and shape casts, overlap, the mover's planes, the proxy; the collision filters | done, `test_shape.ae` (440 checks); [same results as the reference, rays and masses at parity, radius casts 2.5x](bench/RESULTS.md#shape) | +| `aephysics.mover` | the character mover's plane solver: pushes accumulated and clamped over twenty sweeps, the velocity clip | done, `test_mover.ae` (56 checks); [same results as the reference, 0.9x its time](bench/RESULTS.md#mover) | | `aephysics.dynamics` | bodies, contacts, the constraint graph, islands, the Soft Step solver, joints (spherical, revolute, prismatic, distance, motor, weld, wheel), sensors, the character mover, the world | | | `aephysics` | the public API | | diff --git a/aephysics/mover/module.ae b/aephysics/mover/module.ae new file mode 100644 index 0000000..23bb67b --- /dev/null +++ b/aephysics/mover/module.ae @@ -0,0 +1,85 @@ +// aephysics.mover -- the character mover's plane solver: given the +// planes the mover's capsule touches (from the shapes' collide_mover +// functions, each with a push limit), the translation it wanted is +// pushed out of every plane by a Gauss-Seidel sweep with the pushes +// accumulated and clamped, and its velocity is clipped against the +// planes that pushed. +// +// Box3D's mover.c (Erin Catto, MIT), the reference this engine is +// measured against: twenty iterations, the linear slop added to each +// separation so the mover rests just off the surface without jitter, +// convergence when the sweep's total push falls under the slop. Names +// are the reference's without its prefix, in snake case: b3SolvePlanes +// is solve_planes. The mover's queries through a world (planes from +// every shape in a region, the mover's time of impact) come with the +// world. +import std.string +import aephysics.math + +exports ( CollisionPlane, PlaneSolverResult, collision_plane, solve_planes, clip_vector, MAX_PLANE_ITERATIONS ) + +const MAX_PLANE_ITERATIONS = 20 + +// A plane the mover is pushed out of. The push limit makes it soft +// (usually in metres; the largest float for rigid), the push is what the +// solver applied, and clip_velocity says whether clip_vector uses it. +struct CollisionPlane { + plane: Plane + push_limit: float + push: float + clip_velocity: bool +} + +struct PlaneSolverResult { + delta: Vec3 // the translation after the pushes + iteration_count: int // for diagnostics +} + +collision_plane(plane: Plane, push_limit: float, clip_velocity: bool) -> CollisionPlane { + return CollisionPlane { plane: plane, push_limit: push_limit, push: 0.0, clip_velocity: clip_velocity } +} + +// The translation pushed out of the planes, and each plane's push. +solve_planes(target_delta: Vec3, planes: CollisionPlane[], count: int) -> PlaneSolverResult { + i = 0 + while i < count { + planes[i].push = 0.0 + i = i + 1 + } + delta = target_delta + tolerance = math.LINEAR_SLOP + iteration = 0 + while iteration < MAX_PLANE_ITERATIONS { + total_push = 0.0 + plane_index = 0 + while plane_index < count { + // The slop keeps the mover just off the surface, without jitter. + separation = math.plane_separation(planes[plane_index].plane, delta) + math.LINEAR_SLOP + push = 0.0 - separation + // The accumulated push is clamped, not the increment. + accumulated = planes[plane_index].push + planes[plane_index].push = math.clamp_float(accumulated + push, 0.0, planes[plane_index].push_limit) + push = planes[plane_index].push - accumulated + delta = math.mul_add(delta, push, planes[plane_index].plane.normal) + total_push = total_push + math.abs_float(push) + plane_index = plane_index + 1 + } + if total_push < tolerance { break } + iteration = iteration + 1 + } + return PlaneSolverResult { delta: delta, iteration_count: iteration } +} + +// The vector with its component into every plane that pushed (and clips) removed. +clip_vector(vector: Vec3, planes: CollisionPlane[], count: int) -> Vec3 { + v = vector + plane_index = 0 + while plane_index < count { + if planes[plane_index].push != 0.0 && planes[plane_index].clip_velocity { + normal = planes[plane_index].plane.normal + v = math.mul_sub(v, math.min_float(0.0, math.dot(v, normal)), normal) + } + plane_index = plane_index + 1 + } + return v +} diff --git a/aephysics/test_mover.ae b/aephysics/test_mover.ae new file mode 100644 index 0000000..db55976 --- /dev/null +++ b/aephysics/test_mover.ae @@ -0,0 +1,245 @@ +// aephysics.mover against the reference's (Box3D's) test_mover.c: the +// plane solver on two parallel planes and on a game's pair with a deep +// target, and the shapes' mover collisions that need no world: the +// sphere, capsule and hull separated, touching and in deep overlap +// (never a zero normal), the mesh's back side and mirrored scale, the +// height field's back side, report and clockwise winding; and beyond +// it the clipping of a velocity, a soft plane's limit, and a corner. +// The mover through a world (materials, filters, the time of impact) +// comes with the world. + +import std.string +import aephysics.math +import aephysics.core +import aephysics.hull +import aephysics.distance +import aephysics.manifold +import aephysics.mesh +import aephysics.height_field +import aephysics.material +import aephysics.sphere +import aephysics.capsule +import aephysics.mover + +extern calloc(count: int, size: int) -> ptr +extern exit(code: int) +extern free(p: ptr) + +var failures = 0 +var checks = 0 + +ensure(name: string, ok: bool) { + checks = checks + 1 + if !ok { + println("mover: FAIL ${name}") + failures = failures + 1 + } +} + +small(name: string, value: float, tolerance: float) { + ensure("${name} (${value})", math.abs_float(value) < tolerance) +} + +rigid(normal: Vec3, offset: float) -> CollisionPlane { + return mover.collision_plane(Plane { normal: normal, offset: offset }, math.MAX_FLOAT, true) +} + +test_solver() { + planes_block = calloc(4, sizeof(CollisionPlane)) + planes = planes_block as CollisionPlane[] + // Two parallel planes: the target is pushed to the farther one in two sweeps. + planes[0] = rigid(math.vec3(0.0, 0.0, 1.0), 0.5) + planes[1] = rigid(math.vec3(0.0, 0.0, 1.0), 1.0) + result = mover.solve_planes(math.vec3_zero(), planes, 2) + ensure("parallel iterations (${result.iteration_count})", result.iteration_count == 2) + small("parallel delta", result.delta.z - 1.0, 0.0055) + small("parallel push 0", planes[0].push, 0.000001) + small("parallel push 1", planes[1].push - 0.995, 0.000001) + + // A game's pair with the target deep inside: every iteration is spent. + planes[0] = rigid(math.vec3(0.0, 0.0 - 0.23941046, 0.970918416), 0.390724182) + planes[1] = rigid(math.vec3(0.0, 0.0, 1.0), 1.49998093) + target = math.vec3(0.0 - 2.5390625, 0.0, 0.0 - 73.6880798) + planes[0].plane.offset = planes[0].plane.offset - math.dot(planes[0].plane.normal, target) + planes[1].plane.offset = planes[1].plane.offset - math.dot(planes[1].plane.normal, target) + result = mover.solve_planes(math.vec3_zero(), planes, 2) + ensure("game planes spend every iteration", result.iteration_count == mover.MAX_PLANE_ITERATIONS) + + // A corner of two walls: the target into both is pushed out of both, + // to rest a slop inside each (the slop that stops the jitter). + planes[0] = rigid(math.vec3(1.0, 0.0, 0.0), 0.0) + planes[1] = rigid(math.vec3(0.0, 1.0, 0.0), 0.0) + result = mover.solve_planes(math.vec3(0.0 - 1.0, 0.0 - 1.0, 0.0), planes, 2) + small("corner x", result.delta.x + math.LINEAR_SLOP, 0.000001) + small("corner y", result.delta.y + math.LINEAR_SLOP, 0.000001) + ensure("corner iterations", result.iteration_count == 1) + // Nothing to push: no iterations, the target unchanged. + result = mover.solve_planes(math.vec3(1.0, 1.0, 0.0), planes, 2) + ensure("clear target", result.iteration_count == 0 && result.delta.x == 1.0 && result.delta.y == 1.0) + // A soft plane pushes only to its limit. + planes[0] = mover.collision_plane(Plane { normal: math.vec3(0.0, 1.0, 0.0), offset: 0.0 }, 0.25, false) + result = mover.solve_planes(math.vec3(0.0, 0.0 - 1.0, 0.0), planes, 1) + small("soft plane limit", result.delta.y + 0.75, 0.000001) + small("soft plane push", planes[0].push - 0.25, 0.000001) + + // The velocity is clipped against the planes that pushed and clip; + // a soft plane and one that did not push are skipped. + planes[0] = rigid(math.vec3(0.0, 1.0, 0.0), 0.0) + planes[1] = rigid(math.vec3(1.0, 0.0, 0.0), 0.0) + planes[2] = mover.collision_plane(Plane { normal: math.vec3(0.0, 0.0, 1.0), offset: 0.0 }, 0.25, false) + result = mover.solve_planes(math.vec3(0.0 - 1.0, 0.0 - 1.0, 0.0 - 1.0), planes, 3) + v = mover.clip_vector(math.vec3(0.0 - 2.0, 0.0 - 3.0, 0.0 - 4.0), planes, 3) + small("clipped x", v.x, 0.000001) + small("clipped y", v.y, 0.000001) + small("soft plane does not clip", v.z + 4.0, 0.000001) + v = mover.clip_vector(math.vec3(2.0, 3.0, 4.0), planes, 3) + ensure("a velocity leaving the planes is kept", v.x == 2.0 && v.y == 3.0 && v.z == 4.0) + planes[0].push = 0.0 + v = mover.clip_vector(math.vec3(0.0 - 2.0, 0.0 - 3.0, 0.0), planes, 3) + small("a plane without push does not clip", v.y + 3.0, 0.000001) + free(planes_block) +} + +test_shape_movers() { + planes_block = calloc(4, sizeof(PlaneResult)) + planes = planes_block as PlaneResult[] + // The sphere: separated, touching (0.1 into the combined radius), and + // the mover's axis through the centre (the deep case: a perpendicular + // of the axis, the full combined radius). + s = manifold.sphere(math.vec3_zero(), 0.5) + ensure("sphere separated", sphere.collide_mover_and_sphere(planes, s, manifold.capsule(math.vec3(4.0, 3.0, 0.0), math.vec3(6.0, 3.0, 0.0), 0.2)) == 0) + count = sphere.collide_mover_and_sphere(planes, s, manifold.capsule(math.vec3(0.0 - 1.0, 0.6, 0.0), math.vec3(1.0, 0.6, 0.0), 0.2)) + ensure("sphere touching", count == 1) + ensure("sphere touching normalized", math.is_normalized(planes[0].plane.normal)) + ensure("sphere touching up", planes[0].plane.normal.y > 0.99) + small("sphere touching offset", planes[0].plane.offset - 0.1, 0.00001) + count = sphere.collide_mover_and_sphere(planes, s, manifold.capsule(math.vec3(0.0 - 1.0, 0.0, 0.0), math.vec3(1.0, 0.0, 0.0), 0.2)) + ensure("sphere deep", count == 1) + ensure("sphere deep normalized", math.is_normalized(planes[0].plane.normal)) + small("sphere deep perpendicular", planes[0].plane.normal.x, 0.00001) + small("sphere deep offset", planes[0].plane.offset - 0.7, 0.00001) + + // The capsule: separated, touching, crossing (perpendicular to both), coincident. + c = manifold.capsule(math.vec3(0.0 - 1.0, 0.0, 0.0), math.vec3(1.0, 0.0, 0.0), 0.3) + ensure("capsule separated", capsule.collide_mover_and_capsule(planes, c, manifold.capsule(math.vec3(0.0 - 1.0, 5.0, 0.0), math.vec3(1.0, 5.0, 0.0), 0.2)) == 0) + count = capsule.collide_mover_and_capsule(planes, c, manifold.capsule(math.vec3(0.0 - 1.0, 0.4, 0.0), math.vec3(1.0, 0.4, 0.0), 0.2)) + ensure("capsule touching", count == 1 && math.is_normalized(planes[0].plane.normal) && planes[0].plane.normal.y > 0.99) + small("capsule touching offset", planes[0].plane.offset - 0.1, 0.00001) + count = capsule.collide_mover_and_capsule(planes, c, manifold.capsule(math.vec3(0.0, 0.0, 0.0 - 1.0), math.vec3(0.0, 0.0, 1.0), 0.2)) + ensure("capsule crossing", count == 1 && math.is_normalized(planes[0].plane.normal)) + small("capsule crossing x", planes[0].plane.normal.x, 0.00001) + small("capsule crossing z", planes[0].plane.normal.z, 0.00001) + small("capsule crossing offset", planes[0].plane.offset - 0.5, 0.00001) + count = capsule.collide_mover_and_capsule(planes, c, manifold.capsule(math.vec3(0.0 - 1.0, 0.0, 0.0), math.vec3(1.0, 0.0, 0.0), 0.2)) + ensure("capsule coincident", count == 1 && math.is_normalized(planes[0].plane.normal)) + small("capsule coincident perpendicular", planes[0].plane.normal.x, 0.00001) + small("capsule coincident offset", planes[0].plane.offset - 0.5, 0.00001) + + // The hull: separated, touching, and the segment inside (the plane is dropped). + box = hull.make_box_hull(0.5, 0.5, 0.5) + ensure("hull separated", mesh.collide_mover_and_hull(planes, box, manifold.capsule(math.vec3(0.0 - 0.3, 5.0, 0.0), math.vec3(0.3, 5.0, 0.0), 0.2)) == 0) + count = mesh.collide_mover_and_hull(planes, box, manifold.capsule(math.vec3(0.0 - 0.3, 0.6, 0.0), math.vec3(0.3, 0.6, 0.0), 0.2)) + ensure("hull touching", count == 1 && math.is_normalized(planes[0].plane.normal) && planes[0].plane.normal.y > 0.99) + small("hull touching offset", planes[0].plane.offset - 0.1, 0.0001) + ensure("hull deep drops the plane", mesh.collide_mover_and_hull(planes, box, manifold.capsule(math.vec3(0.0 - 0.2, 0.0, 0.0), math.vec3(0.2, 0.0, 0.0), 0.1)) == 0) + hull.destroy_hull(box) + + // Two upward triangles on y = 0 with a material each. + vertices_block = calloc(6, sizeof(Vec3)) + vertices = vertices_block as Vec3[] + vertices[0] = math.vec3(0.0 - 3.0, 0.0, 0.0 - 1.0) + vertices[1] = math.vec3(0.0 - 2.0, 0.0, 1.0) + vertices[2] = math.vec3(0.0 - 1.0, 0.0, 0.0 - 1.0) + vertices[3] = math.vec3(1.0, 0.0, 0.0 - 1.0) + vertices[4] = math.vec3(2.0, 0.0, 1.0) + vertices[5] = math.vec3(3.0, 0.0, 0.0 - 1.0) + indices_block = calloc(6, 4) + indices = indices_block as int[] + i = 0 + while i < 6 { + indices[i] = i + i = i + 1 + } + material_indices_block = calloc(2, 4) + material_indices = material_indices_block as int[] + material_indices[1] = 1 + mdef = mesh.mesh_def() + mdef.vertices = vertices_block + mdef.indices = indices_block + mdef.material_indices = material_indices_block + mdef.vertex_count = 6 + mdef.triangle_count = 2 + md = mesh.create_mesh(&mdef, null, 0) + // On the front of the left triangle a plane comes back; from behind it is culled. + sh = mesh.mesh(md, math.vec3_one()) + count = mesh.collide_mover_and_mesh(planes, 4, sh, manifold.capsule(math.vec3(0.0 - 2.0, 0.15, 0.0), math.vec3(0.0 - 2.0, 0.35, 0.0), 0.2)) + ensure("mesh front", count == 1 && planes[0].plane.normal.y > 0.99 && planes[0].child_index == 0) + ensure("mesh front triangle", count == 1 && planes[0].triangle_index >= 0 && planes[0].triangle_index < md.triangle_count) + ensure("mesh back culled", mesh.collide_mover_and_mesh(planes, 4, sh, manifold.capsule(math.vec3(0.0 - 2.0, 0.0 - 0.35, 0.0), math.vec3(0.0 - 2.0, 0.0 - 0.15, 0.0), 0.2)) == 0) + // Mirrored in x: the winding flips and the collision flips it back; local x = -2 is the right triangle, material 1. + sh = mesh.mesh(md, math.vec3(0.0 - 1.0, 1.0, 1.0)) + count = mesh.collide_mover_and_mesh(planes, 4, sh, manifold.capsule(math.vec3(0.0 - 2.0, 0.15, 0.0), math.vec3(0.0 - 2.0, 0.35, 0.0), 0.2)) + ensure("mirrored mesh front", count == 1 && planes[0].plane.normal.y > 0.99) + ensure("mirrored mesh material", count == 1 && planes[0].material_index == 1) + mesh_materials = mesh.mesh_material_indices(md) + ensure("mirrored mesh triangle material", count == 1 && mesh_materials[planes[0].triangle_index] == 1) + ensure("mirrored mesh back culled", mesh.collide_mover_and_mesh(planes, 4, sh, manifold.capsule(math.vec3(0.0 - 2.0, 0.0 - 0.35, 0.0), math.vec3(0.0 - 2.0, 0.0 - 0.15, 0.0), 0.2)) == 0) + mesh.destroy_mesh(md) + free(material_indices_block) + free(indices_block) + free(vertices_block) + + // A flat 3 x 3 field: the front (upper) face pushes, the back is culled, + // the triangle and material are reported, and a clockwise field faces down. + heights = calloc(9, 8) + materials_block = calloc(4, 4) + materials = materials_block as int[] + def = height_field.height_field_def() + def.heights = heights + def.material_indices = materials_block + def.count_x = 3 + def.count_z = 3 + hf = height_field.create_height_field(&def) + count = height_field.collide_mover_and_height_field(planes, 4, hf, manifold.capsule(math.vec3(0.3, 0.15, 0.25), math.vec3(0.3, 0.35, 0.25), 0.2)) + ensure("field front", count == 1 && planes[0].plane.normal.y > 0.99) + small("field front offset", planes[0].plane.offset - 0.05, 0.0001) + ensure("field back culled", height_field.collide_mover_and_height_field(planes, 4, hf, manifold.capsule(math.vec3(0.3, 0.0 - 0.35, 0.25), math.vec3(0.3, 0.0 - 0.15, 0.25), 0.2)) == 0) + height_field.destroy_height_field(hf) + materials[0] = 1 + materials[1] = 2 + hf = height_field.create_height_field(&def) + count = height_field.collide_mover_and_height_field(planes, 4, hf, manifold.capsule(math.vec3(0.3, 0.15, 0.25), math.vec3(0.3, 0.35, 0.25), 0.2)) + ensure("field report first", count == 1 && planes[0].triangle_index == 0 && planes[0].child_index == 0 && planes[0].material_index == 1) + count = height_field.collide_mover_and_height_field(planes, 4, hf, manifold.capsule(math.vec3(1.3, 0.15, 0.3), math.vec3(1.3, 0.35, 0.3), 0.2)) + ensure("field report second", count == 1 && planes[0].triangle_index == 2 && planes[0].material_index == 2) + height_field.destroy_height_field(hf) + materials[0] = 0 + materials[1] = 0 + def.clockwise_winding = true + hf = height_field.create_height_field(&def) + count = height_field.collide_mover_and_height_field(planes, 4, hf, manifold.capsule(math.vec3(0.3, 0.0 - 0.35, 0.25), math.vec3(0.3, 0.0 - 0.15, 0.25), 0.2)) + ensure("clockwise field below", count == 1 && planes[0].plane.normal.y < 0.0 - 0.99) + small("clockwise field offset", planes[0].plane.offset - 0.05, 0.0001) + ensure("clockwise field triangle", count == 1 && (planes[0].triangle_index == 0 || planes[0].triangle_index == 1)) + ensure("clockwise field above culled", height_field.collide_mover_and_height_field(planes, 4, hf, manifold.capsule(math.vec3(0.3, 0.15, 0.25), math.vec3(0.3, 0.35, 0.25), 0.2)) == 0) + height_field.destroy_height_field(hf) + free(materials_block) + free(heights) + free(planes_block) +} + +main() { + before = core.alloc_count() + test_solver() + test_shape_movers() + // The scratch stays allocated: mesh's three blocks, the height field's three, sphere's length. + ensure("every other counted allocation was freed (${core.alloc_count() - before})", core.alloc_count() == before + 7) + + println("mover: ${checks} checks") + if failures == 0 { + println("mover: all checks passed") + } else { + println("mover: ${failures} failure(s)") + exit(1) + } +} diff --git a/bench/RESULTS.md b/bench/RESULTS.md index 05430fe..81cbc35 100644 --- a/bench/RESULTS.md +++ b/bench/RESULTS.md @@ -306,3 +306,19 @@ test against scalar doubles. The traversal is the place to profile (aephysics#11). The compound is 379 KB here against 231 KB there: the tree's nodes and proxies are our wider doubles and longs, and the block carries the traversal stack. + +## mover + +`bench/mover.ae` and `bench/mover_box3d.c`: 1,000,000 solves of a +target against six planes tilted around it (a floor, four walls leaning +in, a soft ceiling), each followed by a velocity clip. + +| phase | aephysics | Box3D | +|---|---|---| +| 1,000,000 plane solves and clips | **64.6 ms** | 73.2 | + +The same answers: equal sums of the deltas and of the clipped +velocities, 1,662,623 iterations here against 1,662,624 there (one +convergence test on the float's side of the slop). The solver runs +at 0.9x: the reference reads its planes through a pointer per pass +where the loop here indexes the array. diff --git a/bench/mover.ae b/bench/mover.ae new file mode 100644 index 0000000..21bcc02 --- /dev/null +++ b/bench/mover.ae @@ -0,0 +1,49 @@ +// The plane solver on the same scene as bench/mover_box3d.c: 1,000,000 +// solves of a target against six planes tilted around it (a floor, four +// walls leaning in, a soft ceiling), each followed by a velocity clip. +// Single thread, wall time, with the sums of the deltas and iteration +// counts as the checksum. +import std.string +import std.os +import aephysics.math +import aephysics.mover + +extern calloc(count: int, size: int) -> ptr +extern free(p: ptr) +extern sin(x: float) -> float +extern cos(x: float) -> float + +clock() -> long { return os.now_monotonic_ns() } +ms(ns: long) -> float { return (ns as float) / 1000000.0 } + +const SOLVES = 1000000 + +main() { + planes_block = calloc(6, sizeof(CollisionPlane)) + planes = planes_block as CollisionPlane[] + planes[0] = mover.collision_plane(Plane { normal: math.vec3(0.0, 1.0, 0.0), offset: 0.0 }, math.MAX_FLOAT, true) + planes[1] = mover.collision_plane(Plane { normal: math.normalize(math.vec3(1.0, 0.3, 0.0)), offset: 0.0 - 1.0 }, math.MAX_FLOAT, true) + planes[2] = mover.collision_plane(Plane { normal: math.normalize(math.vec3(0.0 - 1.0, 0.3, 0.0)), offset: 0.0 - 1.0 }, math.MAX_FLOAT, true) + planes[3] = mover.collision_plane(Plane { normal: math.normalize(math.vec3(0.0, 0.3, 1.0)), offset: 0.0 - 1.0 }, math.MAX_FLOAT, true) + planes[4] = mover.collision_plane(Plane { normal: math.normalize(math.vec3(0.0, 0.3, 0.0 - 1.0)), offset: 0.0 - 1.0 }, math.MAX_FLOAT, true) + planes[5] = mover.collision_plane(Plane { normal: math.vec3(0.0, 0.0 - 1.0, 0.0), offset: 0.0 - 2.0 }, 0.1, false) + + t0 = clock() + delta_sum = 0.0 + clip_sum = 0.0 + iterations = 0 + i = 0 + while i < SOLVES { + t = (i as float) / (SOLVES as float) + target = math.vec3(1.6 * sin(13.0 * t), 0.0 - 0.5 + 3.0 * cos(7.0 * t), 1.6 * sin(17.0 * t)) + result = mover.solve_planes(target, planes, 6) + delta_sum = delta_sum + result.delta.x + result.delta.y + result.delta.z + iterations = iterations + result.iteration_count + v = mover.clip_vector(math.vec3(2.0 * sin(5.0 * t), 0.0 - 3.0, 2.0 * cos(5.0 * t)), planes, 6) + clip_sum = clip_sum + v.x + v.y + v.z + i = i + 1 + } + t1 = clock() + println("aephysics mover: ${SOLVES} solves ${ms(t1 - t0)} ms (delta sum ${delta_sum}, ${iterations} iterations, clip sum ${clip_sum})") + free(planes_block) +} diff --git a/bench/mover_box3d.c b/bench/mover_box3d.c new file mode 100644 index 0000000..9e2bdd6 --- /dev/null +++ b/bench/mover_box3d.c @@ -0,0 +1,56 @@ +// The plane solver of the reference on the same scene as bench/mover.ae: +// 1,000,000 solves of a target against six planes tilted around it (a +// floor, four walls leaning in, a soft ceiling), each followed by a +// velocity clip. Single thread, wall time, with the sums of the deltas +// and iteration counts as the checksum. +#include "box3d/collision.h" +#include "box3d/math_functions.h" + +#include +#include +#include +#include + +static double now_ms( void ) +{ + struct timespec ts; + timespec_get( &ts, TIME_UTC ); + return ts.tv_sec * 1000.0 + ts.tv_nsec / 1.0e6; +} + +#define SOLVES 1000000 + +int main( void ) +{ + b3CollisionPlane planes[6] = { 0 }; + planes[0].plane = ( b3Plane ){ { 0.0f, 1.0f, 0.0f }, 0.0f }; + planes[1].plane = ( b3Plane ){ b3Normalize( ( b3Vec3 ){ 1.0f, 0.3f, 0.0f } ), -1.0f }; + planes[2].plane = ( b3Plane ){ b3Normalize( ( b3Vec3 ){ -1.0f, 0.3f, 0.0f } ), -1.0f }; + planes[3].plane = ( b3Plane ){ b3Normalize( ( b3Vec3 ){ 0.0f, 0.3f, 1.0f } ), -1.0f }; + planes[4].plane = ( b3Plane ){ b3Normalize( ( b3Vec3 ){ 0.0f, 0.3f, -1.0f } ), -1.0f }; + planes[5].plane = ( b3Plane ){ { 0.0f, -1.0f, 0.0f }, -2.0f }; + for ( int i = 0; i < 5; ++i ) + { + planes[i].pushLimit = FLT_MAX; + planes[i].clipVelocity = true; + } + planes[5].pushLimit = 0.1f; + planes[5].clipVelocity = false; + + double t0 = now_ms(); + double deltaSum = 0.0, clipSum = 0.0; + long iterations = 0; + for ( int i = 0; i < SOLVES; ++i ) + { + float t = (float)i / (float)SOLVES; + b3Vec3 target = { 1.6f * sinf( 13.0f * t ), -0.5f + 3.0f * cosf( 7.0f * t ), 1.6f * sinf( 17.0f * t ) }; + b3PlaneSolverResult result = b3SolvePlanes( target, planes, 6 ); + deltaSum += result.delta.x + result.delta.y + result.delta.z; + iterations += result.iterationCount; + b3Vec3 v = b3ClipVector( ( b3Vec3 ){ 2.0f * sinf( 5.0f * t ), -3.0f, 2.0f * cosf( 5.0f * t ) }, planes, 6 ); + clipSum += v.x + v.y + v.z; + } + double t1 = now_ms(); + printf( "box3d mover: %d solves %.2f ms (delta sum %.3f, %ld iterations, clip sum %.3f)\n", SOLVES, t1 - t0, deltaSum, iterations, clipSum ); + return 0; +} diff --git a/design.md b/design.md index e425f67..9148008 100644 --- a/design.md +++ b/design.md @@ -122,18 +122,70 @@ started until its tests pass. `aephysics.capsule`, the material `aephysics.material`, and the hull's mover moved beside the mesh's; `aephysics.shape` imports them all. `tree_validate` accepts a baked tree (no parents). -12. **dynamics**: `body`, `contact`, `constraint_graph` (graph colouring), - `solver_set`, `island`, `solver` (the Soft Step: sub-stepping, relax - iterations, restitution), `contact_solver` (scalar first; the wide SIMD - path second, measured), the joints (revolute, prismatic, distance, - motor, weld, wheel, spherical), `sensor`, `mover` (the character mover), - `physics_world`. Tests: `test_body`, `test_joint`, `test_world`, - `test_mover`, `test_determinism`, `test_large_world`. -13. **parallel**: `parallel_for` and the scheduler over Aether's actors; +12. **mover** (done): mover.c as `aephysics.mover`: the character + mover's plane solver (twenty Gauss-Seidel sweeps with the pushes + accumulated and clamped to each plane's limit, the slop keeping the + mover just off the surface) and the velocity clip. 56 checks: + test_mover.c's solver cases and every mover collision that needs no + world (sphere, capsule, hull, mesh, height field), plus a corner, a + soft plane and the clip. 0.9x the reference on a million solves with + one iteration's difference in 1.7 million. +13. **dynamics**: the reference's world is one mutually recursive body + of C (body.c, contact.c, joint.c, island.c, solver_set.c, + constraint_graph.c, sensor.c, broad_phase.c and half of + physics_world.c call into each other), so the cut into Aether's + acyclic modules is: + - `aephysics.mesh_contact`: mesh_contact.c's cluster reduction of a + mesh's or height field's triangle manifolds against a convex shape + (the point culling and the per-cluster reduction are pure; the + triangle cache it refreshes is the contact's, so the entry point + takes the cache as a struct). + - `aephysics.broad_phase`: broad_phase.c's trees per body type, + proxies keyed by type in the low bits, the moved-sibling gathering, + the self and cross pair walks and the pair set; the pair filter and + the pair emission are visitors, since the reference does its shape + filtering and contact creation inside. Own test: pairs found and + not found across moves, a compound's children, the pair set's + persistence. + - `aephysics.dynamics`: one module for the world's state and its + bookkeeping -- the World with its arrays (bodies, shapes, contacts, + joints, islands, solver sets), the ids with generations, the + body's sims and states, the shape's world half (creation on a + body, the fat bounds, the proxy, materials, events flags), the + contact (creation from a pair, the manifold update through the + manifold functions and the mesh contact), the joints' creation and + their bases, the constraint graph colouring, the solver sets + (awake, static, disabled, sleeping islands), the islands (union by + links, split on wake), the sensors. No stepping. Own tests: the + world's bookkeeping without a step (bodies and shapes created and + destroyed, contacts begun from pairs, islands linked and split, + sets moved on sleep and wake), and the parts of test_body.c and + test_world.c that need no step (mass data, extents, validity, + recycling). + - `aephysics.contact_solver`: contact_solver.c scalar (the prepare, + warm start, solve, restitution and store passes; the wide SIMD path + later, measured). + - `aephysics.joint_solver`: the seven joints' prepare, warm start, + solve and reaction (distance, motor, prismatic, revolute, spherical, + weld, wheel) with joint.c's dispatch. + - `aephysics.solver`: solver.c's stages (the Soft Step: integrate + velocities, warm start, solve, integrate positions, relax, + restitution, store impulses, per graph colour), continuous + collision, the sleep decision, the enlarged bounds and the broad + phase update. Single-threaded first; the stage/block structure kept + so the parallel layer only adds workers. + - `aephysics.physics_world`: the step (collide, solve, events), the + world queries (overlap, casts, the mover's planes and time of + impact through the broad phase), the events, the public setters. + Tests: test_body.c, test_joint.c, test_world.c, test_body_query.c, + the world parts of test_mover.c, test_determinism.c, + test_large_world.c; bench pairs on the reference's benchmark + scenes as each becomes possible. +14. **parallel**: `parallel_for` and the scheduler over Aether's actors; the benchmarks by thread count as the original records them. -14. **recording and replay**, `world_snapshot`: last, since they are the +15. **recording and replay**, `world_snapshot`: last, since they are the tooling and not the engine. -15. **benchmarks**: `reference/benchmark/main.c`'s nine scenes ported, run +16. **benchmarks**: `reference/benchmark/main.c`'s nine scenes ported, run against the C build on the same machine, recorded under `benchmark/`. ## Measures