diff --git a/README.md b/README.md index 4b95068..2c02c10 100644 --- a/README.md +++ b/README.md @@ -49,7 +49,7 @@ so a test written against the reference reads the same here. | `aephysics.contact_solver_wide` | a colour's convex contacts eight at a time: the constraint of eight contacts as one single-precision record in lanes (`f32x8`, std.lanes -- the reference's SIMD path; one AVX2 register with `AETHER_AEPHYSICS_CFLAGS=-mavx2`, two four-lane halves without), the same prepare, warm start, solve, restitution and store, the bodies gathered into lanes and scattered back | `test_contact_solver_wide.ae` (one scene two ways -- the stack, the bounce with hit events, rolling resistance, a conveyor, a slide -- the lanes within a millimetre of the scalar solve, and the same lanes twice bit for bit) | [the falling grid: scalar 241 ms, lanes 142, the reference 155](bench/RESULTS.md#solver) | | `aephysics.joint_solver` | the seven joints solved: each kind's prepare (frames relative to the centres of mass, effective masses, spring softness), warm start and solve (rigid or soft constraints, speculative limits, springs, motors), the kinds' accessors (limits, springs, motors, current angles and translations, forces and torques), joint.c's dispatch with the constraint hertz clamp; beyond the reference, a spherical joint's cone may be centred off frame A's z and may be an ellipse of two half-angles (the circle stays the default, bit for bit) | `test_joint_solver.ae` (181 checks: test_joint.c's accessors on every kind, each kind solved by hand) | [ours alone](bench/RESULTS.md#joint_solver) | | `aephysics.solver` | the Soft Step over the workers, the reference's stages: the work in blocks (the bodies', each colour's joints, wide and scalar constraints) that workers claim by compare-and-swap, the stages published as sync bits by worker 0 -- the constraints prepared, per sub-step the velocities integrated (gravity, damping, the gyroscopic torque), warm start, solve, positions, relax, colour by colour with the overflow first on worker 0; restitution, the impulses stored; the bodies finalised in parallel (sleep velocities, move events, fast bodies swept for the time of impact, bounds and proxies), the joint and hit events, the trees refit, bullets, the sensors' hits, islands put to sleep | `test_solver.ae` (45 checks: free fall against the closed form, resting and sleeping, a stack, a bounce and its hit event, a joint event, a pendulum, a fast sphere stopped by a thin wall, a bullet, a sensor swept, two worlds bit for bit alike) | [1.2-1.3x the reference's step, stage by stage](bench/RESULTS.md#stage-by-stage) | -| `aephysics.physics_world` | the world's face: the step (pairs, narrow phase, solve, sensors, events), the events read back, the settings and counters, queries over every shape (overlap of a box or a proxy, the mover's planes, ray, shape and mover casts, the closest ray hit) and against one body at a transform of the caller's, explosions | `test_physics_world.ae` (118 checks: test_world.c's HelloWorld, contact, hit, move and sensor events, the explosion near and far; test_body_query.c's casts, overlaps, mover planes and time of impact; the world queries; a wave pile stepping alike twice) | [the reference's own benchmark scenes with the same checksums: 1.25x its AVX2 build on the large pyramid, 1.2x on the many pyramids, 1.05x on the joint grid, ours at eight lanes in AVX2 with float records (#121)](bench/RESULTS.md#body-sims-in-float-2026-10-07-121-37) | +| `aephysics.physics_world` | the world's face: the step (pairs, narrow phase, solve, sensors, events), the events read back, the settings and counters, queries over every shape (overlap of a box or a proxy, the mover's planes, ray, shape and mover casts, the closest ray hit) and against one body at a transform of the caller's, explosions | `test_physics_world.ae` (118 checks: test_world.c's HelloWorld, contact, hit, move and sensor events, the explosion near and far; test_body_query.c's casts, overlaps, mover planes and time of impact; the world queries; a wave pile stepping alike twice) | [the reference's own benchmark scenes with the same checksums: 1.2x its AVX2 build on the large pyramid, 1.15x on the many pyramids, 1.05x on the joint grid, ours at eight lanes in AVX2 with float records (#121)](bench/RESULTS.md#the-collide-pass-as-the-reference-keeps-it-2026-10-09-121) | | `aephysics.human` | the ragdoll: twelve capsule bones on spherical joints with cone and twist limits and revolute joints with angle limits -- a person's ranges by default (shoulders that reach overhead on a cone centred off the rest, hips on an elliptical cone through 125 degrees ahead, 25 back, 45 out and 30 across, knees and elbows that never pass straight), the reference's tight ones on request -- soles that grip, a spring on every joint toward the reference pose, a motor whose torque limit is joint friction, filter joints for the limbs that clash; the align spring, kinematic anchors through motor or parallel joints and their targets driven over a step (the pose drive of an active ragdoll), velocity, kicks, bullets | `test_human.ae` (96 checks: the figure's shape, a fall to rest in one piece, the setters, a kick, standing under the align spring, the pose held on anchors, the anchors driven two metres and the figure walked behind them, the same drop twice bit for bit, a fitted shape, each arm reaching flexion 170, abduction 160 and extension 45 degrees standing and prone, the hips' cone through a person's four ranges and short of a split, the hinges stopping at straight and folding to their range, the soles' grip) | [the reference's rain benchmark, 300 ragdolls over 400 steps, at 0.9x](bench/RESULTS.md#human) | | `aephysics` | the public API | | @@ -170,7 +170,9 @@ geometry that cannot collide, a heavy chain, an unlimited motor, a hinge whose limits meet, a hinge's motor pressing past its limit settling where the reference's does, an elliptical cone stopping a swing at each half-angle, a kinematic body's contacts made stiff on request, and a -hull rocking about a vertex on the floor coming to rest; 128 checks). +hull rocking about a vertex on the floor coming to rest, and the +contacts' sim indices kept current through every move of a sim; 142 +checks). A joint's limits are soft constraints, as the reference's are: driven past one by a motor or held under a steady load, it settles past itself diff --git a/aephysics/contact_solver/module.ae b/aephysics/contact_solver/module.ae index 1d86efc..d0b3d63 100644 --- a/aephysics/contact_solver/module.ae +++ b/aephysics/contact_solver/module.ae @@ -197,8 +197,8 @@ prepare_contact(ctx: *StepContext, c: *Contact, cc: *ContactConstraint, manifold warm_start_scale = 0.0 if ctx.enable_warm_starting { warm_start_scale = 1.0 } inv_tau = 1.0 / dynamics.SPECULATIVE_DISTANCE - index_a = c.body_sim_index_a - index_b = c.body_sim_index_b + index_a = dynamics.decode_awake_index(c.encoded_body_sim_a) + index_b = dynamics.decode_awake_index(c.encoded_body_sim_b) m_a = 0.0 i_a = math.mat3_zero() v_a = math.vec3_zero() diff --git a/aephysics/contact_solver_wide/module.ae b/aephysics/contact_solver_wide/module.ae index 16f339b..68c5b74 100644 --- a/aephysics/contact_solver_wide/module.ae +++ b/aephysics/contact_solver_wide/module.ae @@ -479,8 +479,8 @@ prepare_wide_slot(ctx: *StepContext, color: *GraphColor, slot: int, end: int) { m5 = gather_manifold(c.manifolds.l5 as *Manifold) m6 = gather_manifold(c.manifolds.l6 as *Manifold) m7 = gather_manifold(c.manifolds.l7 as *Manifold) - c.index_a = IntW { l0: lane_index1(k0, k0.body_sim_index_a), l1: lane_index1(k1, k1.body_sim_index_a), l2: lane_index1(k2, k2.body_sim_index_a), l3: lane_index1(k3, k3.body_sim_index_a), l4: lane_index1(k4, k4.body_sim_index_a), l5: lane_index1(k5, k5.body_sim_index_a), l6: lane_index1(k6, k6.body_sim_index_a), l7: lane_index1(k7, k7.body_sim_index_a) } - c.index_b = IntW { l0: lane_index1(k0, k0.body_sim_index_b), l1: lane_index1(k1, k1.body_sim_index_b), l2: lane_index1(k2, k2.body_sim_index_b), l3: lane_index1(k3, k3.body_sim_index_b), l4: lane_index1(k4, k4.body_sim_index_b), l5: lane_index1(k5, k5.body_sim_index_b), l6: lane_index1(k6, k6.body_sim_index_b), l7: lane_index1(k7, k7.body_sim_index_b) } + c.index_a = IntW { l0: lane_index1(k0, dynamics.decode_awake_index(k0.encoded_body_sim_a)), l1: lane_index1(k1, dynamics.decode_awake_index(k1.encoded_body_sim_a)), l2: lane_index1(k2, dynamics.decode_awake_index(k2.encoded_body_sim_a)), l3: lane_index1(k3, dynamics.decode_awake_index(k3.encoded_body_sim_a)), l4: lane_index1(k4, dynamics.decode_awake_index(k4.encoded_body_sim_a)), l5: lane_index1(k5, dynamics.decode_awake_index(k5.encoded_body_sim_a)), l6: lane_index1(k6, dynamics.decode_awake_index(k6.encoded_body_sim_a)), l7: lane_index1(k7, dynamics.decode_awake_index(k7.encoded_body_sim_a)) } + c.index_b = IntW { l0: lane_index1(k0, dynamics.decode_awake_index(k0.encoded_body_sim_b)), l1: lane_index1(k1, dynamics.decode_awake_index(k1.encoded_body_sim_b)), l2: lane_index1(k2, dynamics.decode_awake_index(k2.encoded_body_sim_b)), l3: lane_index1(k3, dynamics.decode_awake_index(k3.encoded_body_sim_b)), l4: lane_index1(k4, dynamics.decode_awake_index(k4.encoded_body_sim_b)), l5: lane_index1(k5, dynamics.decode_awake_index(k5.encoded_body_sim_b)), l6: lane_index1(k6, dynamics.decode_awake_index(k6.encoded_body_sim_b)), l7: lane_index1(k7, dynamics.decode_awake_index(k7.encoded_body_sim_b)) } c.point_counts = IntW { l0: m0.point_count, l1: m1.point_count, l2: m2.point_count, l3: m3.point_count, l4: m4.point_count, l5: m5.point_count, l6: m6.point_count, l7: m7.point_count } // The store reads this rather than go back out to the contacts. c.hit_lanes = hit_bit(k0, 0) | hit_bit(k1, 1) | hit_bit(k2, 2) | hit_bit(k3, 3) | hit_bit(k4, 4) | hit_bit(k5, 5) | hit_bit(k6, 6) | hit_bit(k7, 7) diff --git a/aephysics/dynamics/module.ae b/aephysics/dynamics/module.ae index c241601..201aabc 100644 --- a/aephysics/dynamics/module.ae +++ b/aephysics/dynamics/module.ae @@ -102,6 +102,7 @@ exports ( create_island, destroy_island, validate_island, create_solver_set, destroy_solver_set, wake_solver_set, validate_solver_sets, get_solver_set, get_island, world_island_count, world_solver_set_count, body_sim_at, body_state_at, island_sim_at, world_shape_at, island_at, solver_set_at, body_at, + encode_body_sim_index, decode_awake_index, is_static_sim_index, shape_fat_aabb, set_shape_fat_aabb, Manifold, ManifoldPoint, ContactSpec, ContactId, ContactBeginTouchEvent, ContactEndTouchEvent, JointSim, GraphColor, ConstraintGraph, GRAPH_COLOR_COUNT, OVERFLOW_INDEX, DYNAMIC_COLOR_COUNT, MAX_MANIFOLD_POINTS, CONTACT_TOUCHING, CONTACT_ENABLE_CONTACT_EVENTS, CONTACT_STATIC, CONTACT_RECYCLE, SIM_TOUCHING, SIM_MESH_CONTACT, @@ -434,13 +435,12 @@ struct BodySim { // A shape in the world: its geometry (the shape module's Shape, with // its density) and its place among its body's shapes and in the broad phase. -// What the collide pass reads of every contact's two shapes comes first, -// in one cache line: the id, the body and the fat box; then what finalize -// reads to refit it; the geometry and the rest after. +// Its fat box, the bounds the broad phase holds, is kept apart in the +// world's fat_aabbs (the collide pass reads only that of a shape); what +// finalize reads to refit it comes first, the geometry and the rest after. struct WorldShape { id: int // NULL_INDEX when the slot is free body_id: int - fat_aabb: AABB aabb: AABB aabb_margin: float proxy_key: int @@ -477,8 +477,11 @@ struct Contact { flags: int manifold_count: int manifolds: ptr // Manifold[manifold_count], a heap block of the contact's - body_sim_index_a: int - body_sim_index_b: int + // The bodies' sims as encode_body_sim_index gives them (the reference's + // encodedBodySimA/B), kept current as the bodies move between sets, so + // the collide pass and the prepares reach a sim without the body. + encoded_body_sim_a: int + encoded_body_sim_b: int // For the recycling, single precision as the reference's: the bodies' // rotations and B's pose relative to A at the last narrow phase (a // relative pose of touching bodies is small; the world positions stay @@ -1209,6 +1212,7 @@ struct World { islands: Buffer // Island shape_id_pool: IdPool shapes: Buffer // WorldShape + fat_aabbs: Buffer // AABB by shape id: the fat boxes, dense for the collide pass (the reference's fatAABBs) sensors: Buffer // Sensor sensor_begin_events: Buffer // SensorBeginTouchEvent sensor_end_events0: Buffer // SensorEndTouchEvent, double buffered with the contacts' index @@ -1287,6 +1291,14 @@ world_at(index: int) -> *World { return (g_worlds + index * sizeof(World)) as *W body_at(w: *World, id: int) -> *Body { return (w.bodies.data + id * sizeof(Body)) as *Body } world_shape_at(w: *World, id: int) -> *WorldShape { return (w.shapes.data + id * sizeof(WorldShape)) as *WorldShape } +shape_fat_aabb(w: *World, id: int) -> AABB { + boxes = w.fat_aabbs.data as AABB[] + return boxes[id] +} +set_shape_fat_aabb(w: *World, id: int, box: AABB) { + boxes = w.fat_aabbs.data as AABB[] + boxes[id] = box +} island_at(w: *World, id: int) -> *Island { return (w.islands.data + id * sizeof(Island)) as *Island } solver_set_at(w: *World, index: int) -> *SolverSet { return (w.solver_sets.data + index * sizeof(SolverSet)) as *SolverSet } body_sim_at(set: *SolverSet, index: int) -> *BodySim { return (set.body_sims.data + index * sizeof(BodySim)) as *BodySim } @@ -1438,6 +1450,7 @@ create_world(def: *WorldDef) -> WorldId { basics.buffer_reserve(&solver_set_at(w, awake_set).body_states, math.max_int(16, def.dynamic_body_count)) w.shape_id_pool = basics.idpool_create() w.shapes = basics.buffer_create(sizeof(WorldShape), math.max_int(16, def.static_shape_count + def.dynamic_shape_count)) + w.fat_aabbs = basics.buffer_create(sizeof(AABB), math.max_int(16, def.static_shape_count + def.dynamic_shape_count)) w.sensors = basics.buffer_create(sizeof(Sensor), 0) w.sensor_begin_events = basics.buffer_create(sizeof(SensorBeginTouchEvent), 4) w.sensor_end_events0 = basics.buffer_create(sizeof(SensorEndTouchEvent), 4) @@ -1567,6 +1580,7 @@ destroy_world(id: WorldId) { basics.buffer_destroy(&w.bodies) basics.idpool_destroy(&w.body_id_pool) basics.buffer_destroy(&w.shapes) + basics.buffer_destroy(&w.fat_aabbs) basics.idpool_destroy(&w.shape_id_pool) basics.buffer_destroy(&w.contacts) basics.idpool_destroy(&w.contact_id_pool) @@ -1709,6 +1723,46 @@ get_body_full_id(w: *World, id: BodyId) -> *Body { return body_at(w, id.index1 - get_body(w: *World, body_id: int) -> *Body { return body_at(w, body_id) } get_body_sim(w: *World, body: *Body) -> *BodySim { return body_sim_at(solver_set_at(w, body.set_index), body.local_index) } +// A body's sim as a contact keeps it (b3EncodeBodySimIndex): its index for +// an awake body, -(index + 2) for a static one, NULL_INDEX for any other +// (asleep or disabled), whose sim is found through the body. +encode_body_sim_index(body: *Body) -> int { + if body.set_index == AWAKE_SET { return body.local_index } + if body.set_index == STATIC_SET { return 0 - (body.local_index + 2) } + return NULL_INDEX +} +is_static_sim_index(encoded: int) -> bool { return encoded < NULL_INDEX } +// The awake index, or NULL_INDEX for a static or sleeping body. +decode_awake_index(encoded: int) -> int { + if encoded >= 0 { return encoded } + return NULL_INDEX +} +// What an encoding becomes when its body falls asleep: a static body's stays. +sleep_body_sim_index(encoded: int) -> int { + if encoded >= 0 { return NULL_INDEX } + return encoded +} + +// A contact's body sim from its encoding, through the body when the body +// is neither awake nor static (b3ResolveContactBodySim). +resolve_contact_body_sim(w: *World, awake_sims: ptr, static_sims: ptr, encoded: int, body_id: int) -> *BodySim { + if encoded >= 0 { return (awake_sims + encoded * sizeof(BodySim)) as *BodySim } + if encoded < NULL_INDEX { return (static_sims + (0 - encoded - 2) * sizeof(BodySim)) as *BodySim } + return get_body_sim(w, body_at(w, body_id)) +} + +// Every contact of a body given the body's encoding again, after its sim +// moved (b3RefreshBodyContactIndices). +refresh_body_contact_indices(w: *World, body: *Body) { + encoded = encode_body_sim_index(body) + key = body.head_contact_key + while key != NULL_INDEX { + c = contact_at(w, key >> 1) + if (key & 1) == 0 { c.encoded_body_sim_a = encoded } else { c.encoded_body_sim_b = encoded } + key = contact_edge_next(w, key) + } +} + // The state exists for the awake set only; null otherwise. get_body_state(w: *World, body: *Body) -> *BodyState { if body.set_index == AWAKE_SET { return body_state_at(solver_set_at(w, AWAKE_SET), body.local_index) } @@ -1891,6 +1945,7 @@ destroy_body(id: BodyId) { native.name_free(s.name) s.name = null basics.idpool_free(&w.shape_id_pool, shape_id) + set_shape_fat_aabb(w, shape_id, AABB { lower: math.vec3_zero(), upper: math.vec3_zero() }) s.id = NULL_INDEX shape_id = s.next_shape_id } @@ -1901,7 +1956,9 @@ destroy_body(id: BodyId) { moved_index = basics.buffer_remove_swap(&set.body_sims, body.local_index) if moved_index != NULL_INDEX && moved_index != body.local_index { moved_sim = body_sim_at(set, body.local_index) - body_at(w, moved_sim.body_id).local_index = body.local_index + moved_body = body_at(w, moved_sim.body_id) + moved_body.local_index = body.local_index + refresh_body_contact_indices(w, moved_body) } if body.set_index == AWAKE_SET { basics.buffer_remove_swap(&set.body_states, body.local_index) @@ -1941,17 +1998,17 @@ compute_shape_margin(g: *Shape) -> float { // The shape's bounds with the speculative margin, and the fat bounds the // broad phase holds (a static body's grow only by the speculative distance). -update_shape_aabbs(s: *WorldShape, t: Transform, proxy_kind: int) { +update_shape_aabbs(w: *World, s: *WorldShape, t: Transform, proxy_kind: int) { box = shape.compute_fat_shape_aabb(&s.geometry, t, SPECULATIVE_DISTANCE) s.aabb = box margin = s.aabb_margin if proxy_kind == BODY_STATIC { margin = SPECULATIVE_DISTANCE } - s.fat_aabb = AABB { lower: math.sub(box.lower, math.vec3(margin, margin, margin)), upper: math.add(box.upper, math.vec3(margin, margin, margin)) } + set_shape_fat_aabb(w, s.id, AABB { lower: math.sub(box.lower, math.vec3(margin, margin, margin)), upper: math.add(box.upper, math.vec3(margin, margin, margin)) }) } create_shape_proxy(w: *World, s: *WorldShape, kind: int, t: Transform, force_pair_creation: bool) { - update_shape_aabbs(s, t, kind) - s.proxy_key = broad_phase.broad_phase_create_proxy(&w.broad_phase, kind, s.fat_aabb, s.filter.category_bits, s.id, force_pair_creation) + update_shape_aabbs(w, s, t, kind) + s.proxy_key = broad_phase.broad_phase_create_proxy(&w.broad_phase, kind, shape_fat_aabb(w, s.id), s.filter.category_bits, s.id, force_pair_creation) } // A shape on a body: its slot, its geometry, its materials, its proxy in @@ -1988,6 +2045,7 @@ create_shape(body_id: BodyId, def: *ShapeDef, geometry: Shape) -> ShapeId { index = basics.buffer_push(&w.shapes) fresh = world_shape_at(w, index) fresh.generation = 0 + basics.buffer_push(&w.fat_aabbs) } s = world_shape_at(w, shape_id) s.geometry = geometry @@ -2009,7 +2067,7 @@ create_shape(body_id: BodyId, def: *ShapeDef, geometry: Shape) -> ShapeId { s.local_centroid = shape.get_shape_centroid(&s.geometry) s.aabb_margin = compute_shape_margin(&s.geometry) s.aabb = AABB { lower: math.vec3_zero(), upper: math.vec3_zero() } - s.fat_aabb = s.aabb + set_shape_fat_aabb(w, shape_id, s.aabb) s.generation = s.generation + 1 s.sensor_index = NULL_INDEX s.name = null @@ -2141,6 +2199,7 @@ destroy_shape(id: ShapeId, update_body_mass: bool) { native.name_free(s.name) s.name = null basics.idpool_free(&w.shape_id_pool, s.id) + set_shape_fat_aabb(w, s.id, AABB { lower: math.vec3_zero(), upper: math.vec3_zero() }) s.id = NULL_INDEX if update_body_mass { update_body_mass_data(w, body) } w.locked = false @@ -2192,16 +2251,16 @@ reset_shape_proxy(w: *World, s: *WorldShape, wake_bodies: bool, remake: bool) { t = get_body_transform(w, s.body_id) if s.proxy_key != NULL_INDEX { kind = broad_phase.proxy_type(s.proxy_key) - update_shape_aabbs(s, t, kind) + update_shape_aabbs(w, s, t, kind) if remake { broad_phase.broad_phase_destroy_proxy(&w.broad_phase, s.proxy_key) - s.proxy_key = broad_phase.broad_phase_create_proxy(&w.broad_phase, kind, s.fat_aabb, s.filter.category_bits, s.id, true) + s.proxy_key = broad_phase.broad_phase_create_proxy(&w.broad_phase, kind, shape_fat_aabb(w, s.id), s.filter.category_bits, s.id, true) } else { - broad_phase.broad_phase_move_proxy(&w.broad_phase, s.proxy_key, s.fat_aabb) + broad_phase.broad_phase_move_proxy(&w.broad_phase, s.proxy_key, shape_fat_aabb(w, s.id)) } } else { // A disabled body's shape: its bounds kept current for when it returns. - update_shape_aabbs(s, t, body.kind) + update_shape_aabbs(w, s, t, body.kind) } w.locked = false } @@ -2620,12 +2679,12 @@ body_set_transform(id: BodyId, position: Vec3, rotation: Quat) { // with it. The proxy moves only when the new bounds leave the old // fat ones, as the reference's does: a body moved a little every // frame is not taken out of the tree and put back each time. - old_fat = s.fat_aabb - update_shape_aabbs(s, sim.transform, body.kind) + old_fat = shape_fat_aabb(w, s.id) + update_shape_aabbs(w, s, sim.transform, body.kind) if math.aabb_contains(old_fat, s.aabb) { - s.fat_aabb = old_fat + set_shape_fat_aabb(w, s.id, old_fat) } else if s.proxy_key != NULL_INDEX { - broad_phase.broad_phase_move_proxy(&w.broad_phase, s.proxy_key, s.fat_aabb) + broad_phase.broad_phase_move_proxy(&w.broad_phase, s.proxy_key, shape_fat_aabb(w, s.id)) } shape_id = s.next_shape_id } @@ -3081,6 +3140,16 @@ body_compute_aabb(id: BodyId) -> AABB { // awake set and none elsewhere, every set's sims point back at live // bodies, every island is consistent, and the shape lists close. validate_solver_sets(w: *World) -> bool { + // Every contact's encodings are its bodies' (b3ValidateSolverSets). + i = 0 + while i < w.contacts.count { + c = contact_at(w, i) + if c.contact_id != NULL_INDEX { + if c.encoded_body_sim_a != encode_body_sim_index(body_at(w, c.edge_a.body_id)) { return false } + if c.encoded_body_sim_b != encode_body_sim_index(body_at(w, c.edge_b.body_id)) { return false } + } + i = i + 1 + } body_count = 0 i = 0 while i < w.bodies.count { @@ -3264,10 +3333,8 @@ add_contact_to_graph(w: *World, c: *Contact) { is_scalar = (c.flags & SIM_MESH_CONTACT) != 0 || color_index == OVERFLOW_INDEX color = graph_color(w, color_index) c.color_index = color_index - c.body_sim_index_a = body_a.local_index - if body_a.kind == BODY_STATIC { c.body_sim_index_a = NULL_INDEX } - c.body_sim_index_b = body_b.local_index - if body_b.kind == BODY_STATIC { c.body_sim_index_b = NULL_INDEX } + c.encoded_body_sim_a = encode_body_sim_index(body_a) + c.encoded_body_sim_b = encode_body_sim_index(body_b) if is_scalar { c.local_index = color.contacts.count index = basics.buffer_push(&color.contacts) @@ -3348,8 +3415,8 @@ create_contact(w: *World, shape_a: *WorldShape, shape_b: *WorldShape, child_inde c.local_index = set.contact_indices.count c.island_id = NULL_INDEX c.island_index = NULL_INDEX - c.body_sim_index_a = NULL_INDEX - c.body_sim_index_b = NULL_INDEX + c.encoded_body_sim_a = encode_body_sim_index(body_a) + c.encoded_body_sim_b = encode_body_sim_index(body_b) c.shape_id_a = shape_a.id c.shape_id_b = shape_b.id c.child_index = child_index @@ -4295,11 +4362,17 @@ wake_solver_set(w: *World, set_index: int) { body.set_index = AWAKE_SET body.local_index = target body.sleep_time = 0.0 - // The body's non-touching contacts, from the disabled set to the awake one. + // The body's non-touching contacts, from the disabled set to the + // awake one, and its awake ones, with its index; the touching ones + // take theirs entering the graph. key = body.head_contact_key while key != NULL_INDEX { c = contact_at(w, key >> 1) + edge_index = key & 1 key = contact_edge_next(w, key) + if c.set_index == AWAKE_SET || c.set_index == DISABLED_SET { + if edge_index == 0 { c.encoded_body_sim_a = target } else { c.encoded_body_sim_b = target } + } if c.set_index == DISABLED_SET { local_index = c.local_index c.set_index = AWAKE_SET @@ -4388,7 +4461,10 @@ try_sleep_island(w: *World, island_id: int) -> bool { memcpy(sleep_set.body_sims.data + sleep_index * sizeof(BodySim), body_sim_at(awake, awake_index) as ptr, sizeof(BodySim)) moved = basics.buffer_remove_swap(&awake.body_sims, awake_index) if moved != NULL_INDEX && moved != awake_index { - body_at(w, body_sim_at(awake, awake_index).body_id).local_index = awake_index + moved_body = body_at(w, body_sim_at(awake, awake_index).body_id) + moved_body.local_index = awake_index + // One of this island's bodies is re-encoded below as it sleeps. + if moved_body.island_id != island_id { refresh_body_contact_indices(w, moved_body) } } basics.buffer_remove_swap(&awake.body_states, awake_index) body.set_index = sleep_set_id @@ -4399,14 +4475,16 @@ try_sleep_island(w: *World, island_id: int) -> bool { body_move_event_at(w, body.body_move_index).fell_asleep = true } body.body_move_index = NULL_INDEX - // Its non-touching contacts whose other body is not awake go to the disabled set. + // Its non-touching contacts lose its index; those whose other body + // is not awake go to the disabled set. key = body.head_contact_key while key != NULL_INDEX { c = contact_at(w, key >> 1) edge_index = key & 1 key = contact_edge_next(w, key) - if c.set_index == DISABLED_SET { continue } if c.color_index != NULL_INDEX { continue } + if edge_index == 0 { c.encoded_body_sim_a = NULL_INDEX } else { c.encoded_body_sim_b = NULL_INDEX } + if c.set_index == DISABLED_SET { continue } other_id = c.edge_b.body_id if edge_index == 1 { other_id = c.edge_a.body_id } if body_at(w, other_id).set_index == AWAKE_SET { continue } @@ -4447,6 +4525,8 @@ try_sleep_island(w: *World, island_id: int) -> bool { c.set_index = sleep_set_id c.color_index = NULL_INDEX c.local_index = sleep_contact_index + c.encoded_body_sim_a = sleep_body_sim_index(c.encoded_body_sim_a) + c.encoded_body_sim_b = sleep_body_sim_index(c.encoded_body_sim_b) i = i + 1 } i = 0 @@ -4588,23 +4668,21 @@ collide_contact(w: *World, contact_id: int) { c = contact_at(w, contact_id) shape_a = world_shape_at(w, c.shape_id_a) shape_b = world_shape_at(w, c.shape_id_b) - if math.aabb_overlaps(shape_a.fat_aabb, shape_b.fat_aabb) == false { + if math.aabb_overlaps(shape_fat_aabb(w, c.shape_id_a), shape_fat_aabb(w, c.shape_id_b)) == false { c.flags = (c.flags | SIM_DISJOINT) & (0 - 1 - SIM_TOUCHING) basics.bitset_set(&task_context(w).contact_state, contact_id) return } - body_a = body_at(w, shape_a.body_id) - body_b = body_at(w, shape_b.body_id) - static_a = body_a.kind == BODY_STATIC - static_b = body_b.kind == BODY_STATIC + // The sims straight from the contact's encodings, the bodies only for + // one asleep (a non-touching contact with a sleeping body). + encoded_a = c.encoded_body_sim_a + encoded_b = c.encoded_body_sim_b + sim_a = resolve_contact_body_sim(w, solver_set_at(w, AWAKE_SET).body_sims.data, solver_set_at(w, STATIC_SET).body_sims.data, encoded_a, c.edge_a.body_id) + sim_b = resolve_contact_body_sim(w, solver_set_at(w, AWAKE_SET).body_sims.data, solver_set_at(w, STATIC_SET).body_sims.data, encoded_b, c.edge_b.body_id) + static_a = is_static_sim_index(encoded_a) + static_b = is_static_sim_index(encoded_b) was_touching = (c.flags & SIM_TOUCHING) != 0 - sim_a = get_body_sim(w, body_a) - sim_b = get_body_sim(w, body_b) - is_fast = (sim_a.flags & IS_FAST) != 0 || (sim_b.flags & IS_FAST) != 0 - c.body_sim_index_a = body_a.local_index - if static_a { c.body_sim_index_a = NULL_INDEX } - c.body_sim_index_b = body_b.local_index - if static_b { c.body_sim_index_b = NULL_INDEX } + is_fast = ((sim_a.flags | sim_b.flags) & IS_FAST) != 0 recycle_distance = w.contact_recycle_distance tolerance = recycle_distance if was_touching == false { tolerance = math.min_float(recycle_distance, SPECULATIVE_DISTANCE) } @@ -4645,8 +4723,6 @@ add_non_touching_contact(w: *World, c: *Contact) { set = solver_set_at(w, AWAKE_SET) c.color_index = NULL_INDEX c.local_index = set.contact_indices.count - c.body_sim_index_a = NULL_INDEX - c.body_sim_index_b = NULL_INDEX basics.ints_push(&set.contact_indices, c.contact_id) } @@ -4670,14 +4746,11 @@ struct CollideTask { indices: ptr // int[], the contacts of the pass } -// The lines of a shape and of a body the collide pass reads: a shape's -// body id and its bounds, a body's set and index and its kind. -prefetch_shape(s: *WorldShape) { - native.prefetch(s as ptr) -} - -prefetch_body(b: *Body) { - native.prefetch(b as ptr) +// An awake or static body's sim, by a contact's encoding: its transform +// and centre lead it. +prefetch_sim(w: *World, encoded: int) { + if encoded >= 0 { native.prefetch(body_sim_at(solver_set_at(w, AWAKE_SET), encoded) as ptr) } + else if encoded < NULL_INDEX { native.prefetch(body_sim_at(solver_set_at(w, STATIC_SET), 0 - encoded - 2) as ptr) } } collide_range(start: int, end: int, worker: int, context: ptr) { @@ -4689,20 +4762,15 @@ collide_range(start: int, end: int, worker: int, context: ptr) { // The contacts to come and their manifolds fetched ahead, as the // reference's narrow phase does: each pass's first touch of a // contact and of its separately allocated manifold missed the cache. - // Further in, what a contact leads to: its shapes (their bounds - // are what the pass reads first) four ahead, the contact fetched - // by then, and their bodies two ahead, the shapes fetched by then. + // Further in, what a contact leads to: its bodies' sims four ahead, + // the contact fetched by then (the fat boxes are dense, and the + // shapes are read only when the manifolds are made again). if i + 8 < end { native.prefetch(contact_at(w, indices[i + 8]) as ptr) } if i + 4 < end { ahead = contact_at(w, indices[i + 4]) native.prefetch(ahead.manifolds) - prefetch_shape(world_shape_at(w, ahead.shape_id_a)) - prefetch_shape(world_shape_at(w, ahead.shape_id_b)) - } - if i + 2 < end { - near = contact_at(w, indices[i + 2]) - prefetch_body(body_at(w, world_shape_at(w, near.shape_id_a).body_id)) - prefetch_body(body_at(w, world_shape_at(w, near.shape_id_b).body_id)) + prefetch_sim(w, ahead.encoded_body_sim_a) + prefetch_sim(w, ahead.encoded_body_sim_b) } collide_contact(w, indices[i]) i = i + 1 @@ -5918,7 +5986,9 @@ transfer_body(w: *World, target: *SolverSet, source: *SolverSet, body: *Body) { target_sim.flags = target_sim.flags & (0 - 1 - (IS_FAST | IS_SPEED_CAPPED | HAD_TIME_OF_IMPACT)) moved = basics.buffer_remove_swap(&source.body_sims, source_index) if moved != NULL_INDEX && moved != source_index { - body_at(w, body_sim_at(source, source_index).body_id).local_index = source_index + moved_body = body_at(w, body_sim_at(source, source_index).body_id) + moved_body.local_index = source_index + refresh_body_contact_indices(w, moved_body) } if source.set_index == AWAKE_SET { basics.buffer_remove_swap(&source.body_states, source_index) diff --git a/aephysics/solver/module.ae b/aephysics/solver/module.ae index 1ba0668..d636e5b 100644 --- a/aephysics/solver/module.ae +++ b/aephysics/solver/module.ae @@ -314,12 +314,13 @@ continuous_query_visitor(proxy_id: int, user_data: long, context: ptr) -> bool { // A shape's bounds after the sweep, and the fat bounds and proxy grown when they no longer hold it. grow_bounds(w: *World, s: *WorldShape, sim: *BodySim, is_bullet: bool) { - if math.aabb_contains(s.fat_aabb, s.aabb) { return } + if math.aabb_contains(dynamics.shape_fat_aabb(w, s.id), s.aabb) { return } margin = math.vec3(s.aabb_margin, s.aabb_margin, s.aabb_margin) - s.fat_aabb = AABB { lower: math.sub(s.aabb.lower, margin), upper: math.add(s.aabb.upper, margin) } + fat = AABB { lower: math.sub(s.aabb.lower, margin), upper: math.add(s.aabb.upper, margin) } + dynamics.set_shape_fat_aabb(w, s.id, fat) native.atomic_or((&sim.flags) as ptr, dynamics.ENLARGE_BOUNDS) // A bullet's proxies are enlarged later, serially. - if is_bullet == false { broad_phase.broad_phase_mark_proxy_moved(&w.broad_phase, s.proxy_key, s.fat_aabb) } + if is_bullet == false { broad_phase.broad_phase_mark_proxy_moved(&w.broad_phase, s.proxy_key, fat) } } // A fast body swept against the static world (a bullet against the @@ -519,10 +520,11 @@ finalize_range(start: int, end: int, worker: int, context: ptr) { while shape_id != NULL_INDEX { s = dynamics.world_shape_at(w, shape_id) s.aabb = shape.compute_fat_shape_aabb(&s.geometry, t, dynamics.SPECULATIVE_DISTANCE) - if math.aabb_contains(s.fat_aabb, s.aabb) == false { + if math.aabb_contains(dynamics.shape_fat_aabb(w, shape_id), s.aabb) == false { margin = math.vec3(s.aabb_margin, s.aabb_margin, s.aabb_margin) - s.fat_aabb = AABB { lower: math.sub(s.aabb.lower, margin), upper: math.add(s.aabb.upper, margin) } - broad_phase.broad_phase_mark_proxy_moved(&w.broad_phase, s.proxy_key, s.fat_aabb) + fat = AABB { lower: math.sub(s.aabb.lower, margin), upper: math.add(s.aabb.upper, margin) } + dynamics.set_shape_fat_aabb(w, shape_id, fat) + broad_phase.broad_phase_mark_proxy_moved(&w.broad_phase, s.proxy_key, fat) } shape_id = s.next_shape_id } @@ -1315,8 +1317,9 @@ solve_step(w: *World, ctx: *StepContext) { while shape_id != NULL_INDEX { s = dynamics.world_shape_at(w, shape_id) proxy_id = broad_phase.proxy_id(s.proxy_key) - if math.aabb_contains(dynamic_tree.tree_get_aabb(tree, proxy_id), s.fat_aabb) == false { - dynamic_tree.tree_enlarge_proxy(tree, proxy_id, s.fat_aabb) + fat = dynamics.shape_fat_aabb(w, shape_id) + if math.aabb_contains(dynamic_tree.tree_get_aabb(tree, proxy_id), fat) == false { + dynamic_tree.tree_enlarge_proxy(tree, proxy_id, fat) } shape_id = s.next_shape_id } diff --git a/bench/RESULTS.md b/bench/RESULTS.md index 888fe05..c39871b 100644 --- a/bench/RESULTS.md +++ b/bench/RESULTS.md @@ -800,6 +800,55 @@ reference's algorithm in double). After this morning's #124, #126, #127 and this, the reference's three scenes stand at 1.25x, 1.2x and 1.1x its AVX2 build, from 1.8x, 1.8x and 1.2x. +### The collide pass as the reference keeps it (2026-10-09, #121) + +Two pieces of `e77352c`'s collide pass were not ported until now, and +both are about what a contact reaches without going through anything +else: + +- A contact keeps its bodies' sims as indices (the reference's + `encodedBodySimA/B`): the awake set's index for an awake body, + -(index + 2) for a static one, `NULL_INDEX` for any other. They are + kept current wherever a sim moves: a contact made or put into the + graph, a body woken or put to sleep, a sim swapped into the place of a + destroyed or transferred body's. The collide pass and the contact + prepares read them where they used to go from the shape to the body, + its solver set and its sim, and rewrite the indices every step. A + sleeping body's sim, which a non-touching contact may still need, is + found through the body, as the reference's `b3ResolveContactBodySim` + does. `validate_solver_sets` checks every contact's indices against its + bodies (the reference's `b3ValidateSolverSets`), and + `test_encoded_sims` in `tests/test_edge_cases.ae` runs every path that + moves a sim with that check after each step: leaving out the refresh + on a destroy, or on a wake, fails it. +- The shapes' fat boxes live in the world's `fat_aabbs`, by shape id (the + reference's `fatAABBs`), not in the shapes: the first test of every + contact, whether its proxies still overlap, reads two entries of a + dense array instead of two shapes, and a recycled contact never + touches its shapes at all. + +Nothing is computed differently: the golden trace and every checksum are +the same. A quiet machine, pinned to one core, 5 runs each rotated, ms per +step (range and median): + +| scene | main | this change | Box3D `e77352c`, AVX2 | +|---|---|---|---| +| large pyramid | 6.50-7.29 (6.81) | 6.27-8.85 (6.47) | 5.28-5.80 (5.39) | +| collide | 1.00-1.08 | 0.81-0.85 | 0.81-0.94 | +| many pyramids | 14.66-17.10 (14.86) | 13.43-15.02 (14.34) | 11.73-13.83 (12.40) | +| collide | 2.49-2.97 | 1.81-1.97 | 1.97-2.36 | +| joint grid | 10.62-10.91 | 10.76-11.00 | 10.25-10.39 | + +The collide pass is level with the reference's on the large pyramid and +ahead of it on the many pyramids. The mixed pile (`bench/soak.ae`, awake, +6 runs): 2.58-2.64 ms per step to 2.52-2.55 against the reference's +1.99-2.02, its collide 0.59-0.60 to 0.52-0.53 against 0.42-0.43; more of +that pile's contacts compute their manifolds afresh every step, and the +narrow phase of its mixed pairings is in double. The scenes now stand at +1.2x, 1.15x and 1.05x the reference, the mixed pile at 1.26x. What is left +is in the lanes (warm start, solve, relax: the transpose of aether#2550), +the prepare, and the pairs (the tree in double, kept for large worlds). + ### The joint grid's warm start (2026-10-08, #121) The joint grid's solve and relax were already ahead of the reference's; diff --git a/tests/test_edge_cases.ae b/tests/test_edge_cases.ae index ff01ae0..dd6ec92 100644 --- a/tests/test_edge_cases.ae +++ b/tests/test_edge_cases.ae @@ -1090,6 +1090,84 @@ test_rocking_hull() { hull.destroy_hull(rock) } +// The contacts keep their bodies' sims as indices (the reference's +// encodedBodySimA/B), which every move of a sim between and within the sets +// must keep current: a sleeping island's, a woken one's, a sim swapped into +// a destroyed body's place in the static and the awake set, a body changed +// to kinematic, disabled and enabled, and a non-touching contact between an +// awake body and a sleeping one (which the collide pass reaches through the +// body). validate_solver_sets checks every contact's indices after each +// step and change. +valid_world(world_id: WorldId) -> bool { + w = dynamics.get_world(world_id.index1 - 1) + return dynamics.validate_solver_sets(w) && dynamics.validate_contacts(w) +} + +checked_steps(world_id: WorldId, count: int) -> bool { + i = 0 + while i < count { + physics_world.step(world_id, 1.0 / 60.0, 4) + if valid_world(world_id) == false { return false } + i = i + 1 + } + return true +} + +slab_at(world_id: WorldId, x: float, h: ptr) -> BodyId { + bdef = dynamics.default_body_def() + bdef.position = math.vec3(x, 0.0 - 0.5, 0.0) + body = dynamics.create_body(world_id, &bdef) + sdef = dynamics.default_shape_def() + dynamics.create_hull_shape(body, &sdef, h as *HullData) + return body +} + +test_encoded_sims() { + fixtures_ready() + slab = hull.make_box_hull(2.5, 0.5, 2.5) + world_id = new_world() + s0 = slab_at(world_id, 0.0 - 6.0, slab as ptr) + s1 = slab_at(world_id, 0.0, slab as ptr) + s2 = slab_at(world_id, 6.0, slab as ptr) + b0 = box_at(world_id, math.vec3(0.0 - 6.0, 0.5, 0.0), g_box, 1.0) + b1 = box_at(world_id, math.vec3(0.0, 0.5, 0.0), g_box, 1.0) + // Beside b1, 5 cm off: the fat boxes overlap, the cubes don't touch. + b1x = box_at(world_id, math.vec3(1.05, 0.5, 0.0), g_box, 1.0) + b2 = box_at(world_id, math.vec3(6.0, 0.5, 0.0), g_box, 1.0) + ensure("encoded sims: valid while settling and falling asleep", checked_steps(world_id, 120)) + ensure("encoded sims: all asleep", dynamics.body_is_awake(b0) == false && dynamics.body_is_awake(b1) == false && + dynamics.body_is_awake(b1x) == false && dynamics.body_is_awake(b2) == false) + // b1 woken alone: its contact with the sleeping b1x leaves the disabled set. + dynamics.body_set_awake(b1, true) + ensure("encoded sims: one island woken, valid", valid_world(world_id) && dynamics.body_is_awake(b1x) == false) + ensure("encoded sims: an awake body beside a sleeping one", checked_steps(world_id, 5) && dynamics.body_is_awake(b1x) == false) + // The first static body destroyed: the last one's sim takes its place. + dynamics.destroy_body(s0) + ensure("encoded sims: a static sim moved, valid", valid_world(world_id)) + ensure("encoded sims: valid after, b0 falling", checked_steps(world_id, 10)) + // Everything awake, then a body not last in the awake set destroyed. + dynamics.body_set_awake(b1x, true) + dynamics.body_set_awake(b2, true) + dynamics.destroy_body(b0) + ensure("encoded sims: an awake sim moved, valid", valid_world(world_id)) + ensure("encoded sims: valid after", checked_steps(world_id, 5)) + dynamics.body_set_type(b2, dynamics.BODY_KINEMATIC) + ensure("encoded sims: kinematic, valid", valid_world(world_id) && checked_steps(world_id, 5)) + dynamics.body_set_type(b2, dynamics.BODY_DYNAMIC) + dynamics.body_disable(b1) + ensure("encoded sims: disabled, valid", valid_world(world_id) && checked_steps(world_id, 5)) + dynamics.body_enable(b1) + ensure("encoded sims: enabled, valid", valid_world(world_id) && checked_steps(world_id, 5)) + ensure("encoded sims: asleep again, valid throughout", checked_steps(world_id, 120) && dynamics.body_is_awake(b1) == false) + ensure("encoded sims: resting where they were", finite_body(b1) && finite_body(b1x) && finite_body(b2) && + math.abs_float(dynamics.body_get_position(b1).y - 0.5) < 0.02 && math.abs_float(dynamics.body_get_position(b2).y - 0.5) < 0.02) + dynamics.destroy_body(s2) + dynamics.destroy_body(s1) + ensure("encoded sims: the grounds gone, valid", valid_world(world_id) && checked_steps(world_id, 5)) + dynamics.destroy_world(world_id) + hull.destroy_hull(slab) +} + test_motor_and_limits() { fixtures_ready() wdef = dynamics.default_world_def() @@ -1170,6 +1248,7 @@ main() { test_elliptical_cone() test_kinematic_softness() test_rocking_hull() + test_encoded_sims() println("edge_cases: ${checks} checks") if failures == 0 { println("edge_cases: all checks passed")