diff --git a/README.md b/README.md index 3a07e29..7f4aa8f 100644 --- a/README.md +++ b/README.md @@ -44,12 +44,12 @@ so a test written against the reference reads the same here. | `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.broad_phase` | the broad phase: a tree per body type, proxies keyed by type, the pair update through the moved siblings and cross-tree seeds with the filter and compound lookups as visitors, the pair set, the keys sorted | done, `test_broad_phase.ae` (36 checks against a brute force); [10,000 moving boxes at 4.6 ms a step](bench/RESULTS.md#broad_phase) | | `aephysics.mesh_contact` | a convex shape against a mesh or height field: the triangle cache with per-triangle warm starts, the manifolds per triangle, the seam rules against ghost collisions, clusters by normal, the four-point cull | done, `test_mesh_contact.ae` (64 checks); [a box on a wave at 3.2 us a step](bench/RESULTS.md#mesh_contact) | -| `aephysics.dynamics` | the world's state and bookkeeping: worlds, bodies (creation, mass from shapes or by hand, transforms, velocities, extents, locks, sleep and enable flags, the type changed, disabled and enabled) in solver sets, shapes of every kind on bodies with their broad-phase proxies, islands linked by contacts and joints and split by union-find, sleeping sets, waking and merging, the constraint graph's colouring, contacts from the broad phase's pairs with the narrow phase (convex, mesh, height field, compound children) and the state changes with their events, contact recycling, joints of every kind (definitions, creation with the edges, sets and islands, collide-connected in the pair filter, destruction, separations, reactions), sensors with their begin and end events | `test_dynamics.ae` (635 checks: test_body.c, the step-free test_world.c and test_joint.c, test_shape.c's flags, contacts, islands, sleep, joints, sensors); [creation 0.9x, the first collide at parity, recycling 1.1-1.2x, joints 0.6x, sensors 0.75-1.0x](bench/RESULTS.md#dynamics-bookkeeping-contacts-joints-sensors) | +| `aephysics.dynamics` | the world's state and bookkeeping: worlds, bodies (creation, mass from shapes or by hand, transforms, velocities, extents, locks, sleep and enable flags, the type changed, disabled and enabled) in solver sets, shapes of every kind on bodies with their broad-phase proxies, islands linked by contacts and joints and split by union-find, sleeping sets, waking and merging, the constraint graph's colouring, contacts from the broad phase's pairs with the narrow phase (convex, mesh, height field, compound children) and the state changes with their events, contact recycling, joints of every kind (definitions, creation with the edges, sets and islands, collide-connected in the pair filter, destruction, separations, reactions), sensors with their begin and end events | `test_dynamics.ae` (641 checks: test_body.c, the step-free test_world.c and test_joint.c, test_shape.c's names and flags, contacts, islands, sleep, joints, sensors); [creation 0.9x, the first collide at parity, recycling 1.1-1.2x, joints 0.6x, sensors 0.75-1.0x](bench/RESULTS.md#dynamics-bookkeeping-contacts-joints-sensors) | | `aephysics.contact_solver` | the contact constraints of a step, scalar: prepared from the manifolds (anchors, base separations, normal masses, the friction centre and its tangent mass, twist and rolling masses), warm started, solved per sub-step (soft normal constraints with speculative bias, then central friction, twist friction and rolling resistance in the relax pass), the restitution pass, the impulses stored with the hit events; the step context | `test_contact_solver.ae` (50 checks: the passes by hand on a cube on a slab) | [ours alone](bench/RESULTS.md#contact_solver) | | `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` (159 checks: test_world.c's HelloWorld, empty and recycled worlds, every setting, contact, hit, move and sensor events, the explosion near and far, the bullet and sleep flags, the compound count, an enlarged proxy destroyed; test_body_query.c's casts, overlaps, mover planes and time of impact; test_broad_phase.c's static rebuild; 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); [the rest of its suite, nine scenes and the SAT runs, at 1.04-1.21x](bench/RESULTS.md#suite-the-references-benchmark-suite) | +| `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` (238 checks: test_world.c whole but the hull database, among it empty and recycled worlds, every setting, the worker count set between steps, contact, hit, move and sensor events, a compound child's hit events and materials, the overflow colour pile, the explosion near and far, the bullet and sleep flags, the compound count, an enlarged proxy destroyed; test_body_query.c whole: casts, overlaps of hull proxies and turned compounds, mover planes, and the mover's time of impact against 2,000 random sweeps; test_broad_phase.c's static rebuild and its stress test, a swarm through every proxy change with the pair set checked by brute force every step; test_id.c; 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); [the rest of its suite, nine scenes and the SAT runs, at 1.04-1.21x](bench/RESULTS.md#suite-the-references-benchmark-suite) | | `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 | | @@ -131,7 +131,10 @@ never makes a thread. A host with a job system of its own lends it instead (`WorldDef.scheduler`, an `aephysics.parallel` scheduler the host made and keeps; its worker count then rules), as the reference takes a game's task callbacks, so an engine runs its crowd, its -weather and its physics on one pool. [What the workers buy, against +weather and its physics on one pool. `physics_world.set_worker_count` +changes a world's own count between steps, clamped to the cap as the +reference's `b3World_SetWorkerCount` clamps it, and the steps after give +the same bits. [What the workers buy, against the reference's own scaling](bench/RESULTS.md#parallel). A world's queries may be called from any thread at once, as the @@ -155,7 +158,10 @@ shapes; 40 checks), `test_determinism.ae` (the reference's wave pile, query spawn and mesh drop with its own random numbers, and the falling ragdolls, each run to sleep twice and over four workers and compared bit for bit; the query spawn sleeps a step after the -reference's with its 59 query hits, the mesh drop two; 20 checks) and `test_large_world.ae` (a stack, a bullet and the +reference's with its 59 query hits, the mesh drop two; and its rolling +mix of spheres, capsules and boxes with rolling resistance, whose trace +folds the manifolds' impulses in, the scene of its SIMD width test: CI +runs the golden trace on the four-lane and the AVX2 builds; 22 checks) and `test_large_world.ae` (a stack, a bullet and the origin-relative queries at x = 0 and at x = 1e7 agree, the whole engine being in doubles; 43 checks), `test_collision.ae` (the reference's test_collision.c: boxes valid and not, a ray against a box in its twelve @@ -175,9 +181,10 @@ 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, and the -contacts' sim indices kept current through every move of a sim, and +contacts' sim indices kept current through every move of a sim (and +test_world.c's five locator scenes, at one worker and four alike), and a weld and a hinge between static and kinematic bodies surviving a -disable and an enable; 145 checks). +disable and an enable; 158 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/dynamics/module.ae b/aephysics/dynamics/module.ae index 201aabc..0ab436f 100644 --- a/aephysics/dynamics/module.ae +++ b/aephysics/dynamics/module.ae @@ -103,6 +103,8 @@ exports ( 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, + get_max_manifold_points, store_world_id, load_world_id, store_body_id, load_body_id, store_shape_id, load_shape_id, + store_joint_id, load_joint_id, set_worker_count, 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, @@ -285,6 +287,7 @@ struct BodyDef { gravity_scale: float sleep_threshold: float safety_factor: float + name: string // copied for debugging; null for none user_data: ptr motion_locks: MotionLocks enable_sleep: bool @@ -296,6 +299,7 @@ struct BodyDef { } struct ShapeDef { + name: string // copied for debugging; null for none user_data: ptr materials: ptr // SurfaceMaterial[], per triangle material of a mesh, or null material_count: int @@ -348,18 +352,39 @@ default_world_def() -> WorldDef { default_body_def() -> BodyDef { return BodyDef { kind: BODY_STATIC, position: math.vec3_zero(), rotation: math.quat_identity(), linear_velocity: math.vec3_zero(), angular_velocity: math.vec3_zero(), linear_damping: 0.0, angular_damping: 0.0, gravity_scale: 1.0, - sleep_threshold: 0.05, safety_factor: 0.5, user_data: null, + sleep_threshold: 0.05, safety_factor: 0.5, name: null, user_data: null, motion_locks: MotionLocks { linear_x: false, linear_y: false, linear_z: false, angular_x: false, angular_y: false, angular_z: false }, enable_sleep: true, is_awake: true, is_bullet: false, is_enabled: true, allow_fast_rotation: false, enable_contact_recycling: true } } default_shape_def() -> ShapeDef { - return ShapeDef { user_data: null, materials: null, material_count: 0, base_material: material.default_surface_material(), + return ShapeDef { name: null, user_data: null, materials: null, material_count: 0, base_material: material.default_surface_material(), density: 1000.0, explosion_scale: 1.0, filter: shape.default_filter(), enable_custom_filtering: false, is_sensor: false, enable_sensor_events: false, enable_contact_events: false, enable_hit_events: false, enable_pre_solve_events: false, invoke_contact_creation: true, update_body_mass: true, enable_speculative_contact: true } } +// The most points a manifold holds (b3GetMaxManifoldPoints). +get_max_manifold_points() -> int { return MAX_MANIFOLD_POINTS } + +// Ids into 64-bit values and back (b3StoreBodyId and its siblings, id.h): +// the index in the high half, the world and the generation 16 bits each +// below it; a world id into 32 bits. +store_world_id(id: WorldId) -> int { return ((id.index1 & 65535) << 16) | (id.generation & 65535) } +load_world_id(x: int) -> WorldId { return WorldId { index1: (x >> 16) & 65535, generation: x & 65535 } } +store_id(index1: int, world0: int, generation: int) -> long { + return ((index1 as long) << 32) | (((world0 & 65535) as long) << 16) | ((generation & 65535) as long) +} +stored_index(x: long) -> int { return ((x >> 32) & (4294967295 as long)) as int } +stored_world(x: long) -> int { return ((x >> 16) & (65535 as long)) as int } +stored_generation(x: long) -> int { return (x & (65535 as long)) as int } +store_body_id(id: BodyId) -> long { return store_id(id.index1, id.world0, id.generation) } +load_body_id(x: long) -> BodyId { return BodyId { index1: stored_index(x), world0: stored_world(x), generation: stored_generation(x) } } +store_shape_id(id: ShapeId) -> long { return store_id(id.index1, id.world0, id.generation) } +load_shape_id(x: long) -> ShapeId { return ShapeId { index1: stored_index(x), world0: stored_world(x), generation: stored_generation(x) } } +store_joint_id(id: JointId) -> long { return store_id(id.index1, id.world0, id.generation) } +load_joint_id(x: long) -> JointId { return JointId { index1: stored_index(x), world0: stored_world(x), generation: stored_generation(x) } } + null_body_id() -> BodyId { return BodyId { index1: 0, world0: 0, generation: 0 } } null_shape_id() -> ShapeId { return ShapeId { index1: 0, world0: 0, generation: 0 } } @@ -1397,40 +1422,8 @@ create_world(def: *WorldDef) -> WorldId { w.stack = basics.stack_create(2048) w.manifold_allocators = basics.buffer_create(sizeof(BlockAllocator), 0) w.manifold_lock = 0 - // The threads: the host's scheduler when it lends one (the reference's - // task callbacks: a game runs its physics on its own job system), else - // the world's own, destroyed with it. - if def.scheduler != null { - w.scheduler = def.scheduler - w.owns_scheduler = false - w.worker_count = parallel.scheduler_worker_count(def.scheduler as *Scheduler) - } else { - w.worker_count = def.worker_count - if w.worker_count < 1 { w.worker_count = 1 } - if w.worker_count > native.MAX_WORKERS { w.worker_count = native.MAX_WORKERS } - w.scheduler = parallel.create_scheduler(w.worker_count) as ptr - w.owns_scheduler = true - } reserve_scratch() - // The world's steps run as worker slots of its own, so worlds stepped - // at once on different threads use different scratch; with none free, - // slot 0 on, shared, as before (such worlds step one at a time). - w.worker_base = claim_worker_slots(w.worker_count) - w.owns_worker_slots = w.worker_base >= 0 - if w.worker_base < 0 { w.worker_base = 0 } - w.task_contexts = basics.alloc(w.worker_count * sizeof(TaskContext)) - i = 0 - while i < w.worker_count { - tc = task_context_at(w, i) - tc.arena = basics.arena_create(1 << 20) - tc.contact_state = basics.bitset_create(64) - tc.joint_state_bits = basics.bitset_create(64) - tc.hit_event_bits = basics.bitset_create(64) - tc.awake_island_bits = basics.bitset_create(64) - tc.split_island_id = NULL_INDEX - tc.sensor_hits = basics.buffer_create(sizeof(SensorHit), 4) - i = i + 1 - } + create_workers(w, def.scheduler, def.worker_count) w.contact_state = basics.bitset_create(64) w.contact_begin_events = basics.buffer_create(sizeof(ContactBeginTouchEvent), 4) w.contact_end_events0 = basics.buffer_create(sizeof(ContactEndTouchEvent), 4) @@ -1590,20 +1583,7 @@ destroy_world(id: WorldId) { basics.idpool_destroy(&w.island_id_pool) broad_phase.destroy_broad_phase(&w.broad_phase) destroy_graph(&w.constraint_graph) - i = 0 - while i < w.worker_count { - tc = task_context_at(w, i) - basics.arena_destroy(&tc.arena) - basics.bitset_destroy(&tc.contact_state) - basics.bitset_destroy(&tc.joint_state_bits) - basics.bitset_destroy(&tc.hit_event_bits) - basics.bitset_destroy(&tc.awake_island_bits) - basics.buffer_destroy(&tc.sensor_hits) - i = i + 1 - } - basics.free_bytes(w.task_contexts, w.worker_count * sizeof(TaskContext)) - if w.owns_worker_slots { release_worker_slots(w.worker_base, w.worker_count) } - if w.owns_scheduler { parallel.destroy_scheduler(w.scheduler as *Scheduler) } + destroy_workers(w) basics.bitset_destroy(&w.contact_state) basics.buffer_destroy(&w.contact_begin_events) basics.buffer_destroy(&w.contact_end_events0) @@ -1895,7 +1875,7 @@ create_body(world_id: WorldId, def: *BodyDef) -> BodyId { body.inertia = math.mat3_zero() body.kind = def.kind body.flags = flags - body.name = null + body.name = native.name_copy(def.name) if set_id >= AWAKE_SET { create_island_for_body(w, set_id, body) } w.locked = false return BodyId { index1: body_id + 1, world0: w.world_id, generation: body.generation } @@ -2070,7 +2050,7 @@ create_shape(body_id: BodyId, def: *ShapeDef, geometry: Shape) -> ShapeId { set_shape_fat_aabb(w, shape_id, s.aabb) s.generation = s.generation + 1 s.sensor_index = NULL_INDEX - s.name = null + s.name = native.name_copy(def.name) if kind == KIND_COMPOUND { w.compound_shape_count = w.compound_shape_count + 1 // A copy of the compound's materials, so every shape frees its array the same way. @@ -4792,6 +4772,71 @@ task_context(w: *World) -> *TaskContext { // theirs from (aephysics.native): a world's workers and the program's // other threads never share scratch, whichever threads they run on. claim_worker_slots(count: int) -> int { return native.claim_slots(count) } + +// A world's workers: the threads (the host's scheduler when it lends one, +// the reference's task callbacks: a game runs its physics on its own job +// system; else the world's own, destroyed with it), the worker slots its +// steps run as, and a task context each. +create_workers(w: *World, scheduler: ptr, count: int) { + if scheduler != null { + w.scheduler = scheduler + w.owns_scheduler = false + w.worker_count = parallel.scheduler_worker_count(scheduler as *Scheduler) + } else { + w.worker_count = math.clamp_int(count, 1, native.MAX_WORKERS) + w.scheduler = parallel.create_scheduler(w.worker_count) as ptr + w.owns_scheduler = true + } + // The world's steps run as worker slots of its own, so worlds stepped + // at once on different threads use different scratch; with none free, + // slot 0 on, shared, as before (such worlds step one at a time). + w.worker_base = claim_worker_slots(w.worker_count) + w.owns_worker_slots = w.worker_base >= 0 + if w.worker_base < 0 { w.worker_base = 0 } + w.task_contexts = basics.alloc(w.worker_count * sizeof(TaskContext)) + i = 0 + while i < w.worker_count { + tc = task_context_at(w, i) + tc.arena = basics.arena_create(1 << 20) + tc.contact_state = basics.bitset_create(64) + tc.joint_state_bits = basics.bitset_create(64) + tc.hit_event_bits = basics.bitset_create(64) + tc.awake_island_bits = basics.bitset_create(64) + tc.split_island_id = NULL_INDEX + tc.sensor_hits = basics.buffer_create(sizeof(SensorHit), 4) + i = i + 1 + } +} + +destroy_workers(w: *World) { + i = 0 + while i < w.worker_count { + tc = task_context_at(w, i) + basics.arena_destroy(&tc.arena) + basics.bitset_destroy(&tc.contact_state) + basics.bitset_destroy(&tc.joint_state_bits) + basics.bitset_destroy(&tc.hit_event_bits) + basics.bitset_destroy(&tc.awake_island_bits) + basics.buffer_destroy(&tc.sensor_hits) + i = i + 1 + } + basics.free_bytes(w.task_contexts, w.worker_count * sizeof(TaskContext)) + if w.owns_worker_slots { release_worker_slots(w.worker_base, w.worker_count) } + if w.owns_scheduler { parallel.destroy_scheduler(w.scheduler as *Scheduler) } + w.task_contexts = null +} + +// The worker count between steps (b3World_SetWorkerCount), clamped to +// [1, MAX_WORKERS]: the world's own threads, slots and task contexts made +// again for it. A world on a host's scheduler steps on that scheduler's +// workers, so its count is the host's to set, and this leaves it. +set_worker_count(w: *World, count: int) { + if w.locked || w.owns_scheduler == false { return } + clamped = math.clamp_int(count, 1, native.MAX_WORKERS) + if clamped == w.worker_count { return } + destroy_workers(w) + create_workers(w, null, clamped) +} release_worker_slots(start: int, count: int) { native.release_slots(start, count) } // The calling thread made the world's first worker for a pass over it (a diff --git a/aephysics/physics_world/module.ae b/aephysics/physics_world/module.ae index ac97a84..55ca1ed 100644 --- a/aephysics/physics_world/module.ae +++ b/aephysics/physics_world/module.ae @@ -42,6 +42,7 @@ exports ( set_contact_tuning, set_contact_recycle_distance, get_contact_recycle_distance, set_maximum_linear_speed, get_maximum_linear_speed, set_gravity, get_gravity, set_user_data, get_user_data, set_friction_callback, set_restitution_callback, set_custom_filter_callback, set_pre_solve_callback, get_awake_body_count, get_counters, get_bounds, rebuild_static_tree, + set_worker_count, get_worker_count, overlap_aabb, overlap_shape, collide_mover, cast_ray, cast_ray_closest, cast_shape, cast_mover, body_cast_ray, body_cast_shape, body_overlap_shape, body_collide_mover, body_time_of_impact_mover, shape_ray_cast, default_explosion_def, explode @@ -108,8 +109,9 @@ struct Counters { tree_height: int sat_call_count: int sat_cache_hit_count: int - awake_contact_count: int + awake_contact_count: int // the contacts the collide pass touches: the graph's and the awake non-touching recycled_contact_count: int + color_counts: int[24] // each colour's contacts and joints, the overflow last } // The world of an id, or null when the id is stale or the world is locked. @@ -386,11 +388,38 @@ get_counters(id: WorldId) -> Counters { stack_used = basics.stack_max_allocation(&w.stack) static_height = dynamic_tree.tree_get_height(broad_phase.broad_phase_tree(&w.broad_phase, dynamics.BODY_STATIC)) dynamic_height = dynamic_tree.tree_get_height(broad_phase.broad_phase_tree(&w.broad_phase, dynamics.BODY_DYNAMIC)) + // As the reference counts them (b3World_GetCounters): every colour's + // convex and scalar contacts and joints, and the awake contacts are + // those plus the awake set's non-touching ones. + c = Counters { body_count: bodies, shape_count: shapes, contact_count: contacts, joint_count: joints, island_count: islands, + stack_used: stack_used, static_tree_height: static_height, tree_height: dynamic_height, + sat_call_count: w.sat_call_count, sat_cache_hit_count: w.sat_cache_hit_count, + awake_contact_count: 0, recycled_contact_count: w.recycled_contact_count } awake_contacts = dynamics.get_solver_set(w, dynamics.AWAKE_SET).contact_indices.count - return Counters { body_count: bodies, shape_count: shapes, contact_count: contacts, joint_count: joints, island_count: islands, - stack_used: stack_used, static_tree_height: static_height, tree_height: dynamic_height, - sat_call_count: w.sat_call_count, sat_cache_hit_count: w.sat_cache_hit_count, - awake_contact_count: awake_contacts, recycled_contact_count: w.recycled_contact_count } + i = 0 + while i < dynamics.GRAPH_COLOR_COUNT { + color = dynamics.graph_color(w, i) + color_contacts = color.convex_contacts.count + color.contacts.count + c.color_counts[i] = color_contacts + color.joint_sims.count + awake_contacts = awake_contacts + color_contacts + i = i + 1 + } + c.awake_contact_count = awake_contacts + return c +} + +// The worker count, set between steps and clamped to [1, MAX_WORKERS] +// (b3World_SetWorkerCount); a world on a host's scheduler keeps the +// scheduler's count. +set_worker_count(id: WorldId, count: int) { + w = world_of(id) + if w == null { return } + dynamics.set_worker_count(w, count) +} + +get_worker_count(id: WorldId) -> int { + if dynamics.world_is_valid(id) == false { return 0 } + return dynamics.get_world(id.index1 - 1).worker_count } // The bounds of everything in the trees. diff --git a/tests/golden/determinism_trace.txt b/tests/golden/determinism_trace.txt index 082a7dc..c005f3c 100644 --- a/tests/golden/determinism_trace.txt +++ b/tests/golden/determinism_trace.txt @@ -1134,3 +1134,123 @@ mesh_drop 216 -4539169550538835365 mesh_drop 217 -4539169550538964501 mesh_drop 218 -4539169550539050707 mesh_drop 219 -4539169550539108495 +rolling_mix 1 4593987550764978457 +rolling_mix 2 4620748529162417439 +rolling_mix 3 4626252668930552788 +rolling_mix 4 4630162832849529543 +rolling_mix 5 4633802372592044754 +rolling_mix 6 4634177517553126579 +rolling_mix 7 4634993398468506909 +rolling_mix 8 4634019532556714724 +rolling_mix 9 4631657793956071754 +rolling_mix 10 4628349211542146430 +rolling_mix 11 4618440733910833956 +rolling_mix 12 -4598071551809769733 +rolling_mix 13 -4592282649318198681 +rolling_mix 14 -4590958510055062503 +rolling_mix 15 -4590403026210960489 +rolling_mix 16 -4590706185950741027 +rolling_mix 17 -4590963961206383895 +rolling_mix 18 -4591740625800997580 +rolling_mix 19 -4591604488511184629 +rolling_mix 20 -4592081779335607981 +rolling_mix 21 -4593136788615043964 +rolling_mix 22 -4592120403677661663 +rolling_mix 23 -4591705702738608082 +rolling_mix 24 -4591512217010568979 +rolling_mix 25 -4591252691845366159 +rolling_mix 26 -4591059956167658501 +rolling_mix 27 -4590775923773625138 +rolling_mix 28 -4590647469510538739 +rolling_mix 29 -4590428046808400626 +rolling_mix 30 -4590273255385241823 +rolling_mix 31 -4590116120199933040 +rolling_mix 32 -4589969477472721671 +rolling_mix 33 -4589676793063018472 +rolling_mix 34 -4589508875085570877 +rolling_mix 35 -4589247453902244064 +rolling_mix 36 -4589104777842180988 +rolling_mix 37 -4588997966963920498 +rolling_mix 38 -4588919688551813036 +rolling_mix 39 -4588792302450338067 +rolling_mix 40 -4588738982756698560 +rolling_mix 41 -4588636744591568685 +rolling_mix 42 -4588563164210573825 +rolling_mix 43 -4588486593592553022 +rolling_mix 44 -4588438814254337978 +rolling_mix 45 -4588342374567079156 +rolling_mix 46 -4588320765623635192 +rolling_mix 47 -4588250601125850063 +rolling_mix 48 -4588209502347603397 +rolling_mix 49 -4588166037762541702 +rolling_mix 50 -4588151700013768136 +rolling_mix 51 -4588089080661714602 +rolling_mix 52 -4588101933996323340 +rolling_mix 53 -4588066555861028213 +rolling_mix 54 -4588060662644522087 +rolling_mix 55 -4588052886208427018 +rolling_mix 56 -4588074636791045519 +rolling_mix 57 -4588048576276609619 +rolling_mix 58 -4588098258646817790 +rolling_mix 59 -4588100129648351270 +rolling_mix 60 -4588131897894365615 +rolling_mix 61 -4588162107476980005 +rolling_mix 62 -4588222208916824217 +rolling_mix 63 -4588234792683730967 +rolling_mix 64 -4588323583369541062 +rolling_mix 65 -4588364834953961835 +rolling_mix 66 -4588436240644486935 +rolling_mix 67 -4588506552085017533 +rolling_mix 68 -4588606959249782631 +rolling_mix 69 -4588660076893910784 +rolling_mix 70 -4588789844309407990 +rolling_mix 71 -4588872248575664027 +rolling_mix 72 -4588985020159280046 +rolling_mix 73 -4589097065610535005 +rolling_mix 74 -4589310727383001039 +rolling_mix 75 -4589501225725628068 +rolling_mix 76 -4589845532823589373 +rolling_mix 77 -4590095472011076950 +rolling_mix 78 -4590406519507683577 +rolling_mix 79 -4590716596452779260 +rolling_mix 80 -4591087487638347813 +rolling_mix 81 -4591364430313528018 +rolling_mix 82 -4591795888492411277 +rolling_mix 83 -4592133053476226288 +rolling_mix 84 -4592531500833617081 +rolling_mix 85 -4592929471860095232 +rolling_mix 86 -4593388347168542380 +rolling_mix 87 -4593835525231013133 +rolling_mix 88 -4594875100667937064 +rolling_mix 89 -4595726520803331558 +rolling_mix 90 -4596700762306194940 +rolling_mix 91 -4597674489712915518 +rolling_mix 92 -4599364880904475289 +rolling_mix 93 -4601180929553315165 +rolling_mix 94 -4604555795394156905 +rolling_mix 95 -4610165968670917889 +rolling_mix 96 4609068871342182694 +rolling_mix 97 4617892055387377208 +rolling_mix 98 4621840040314421298 +rolling_mix 99 4624012582280405851 +rolling_mix 100 4626000623632746281 +rolling_mix 101 4627207958817772123 +rolling_mix 102 4628537991592869779 +rolling_mix 103 4629783800115092048 +rolling_mix 104 4630279120116386159 +rolling_mix 105 -4599650851729195271 +rolling_mix 106 -4588912192956286807 +rolling_mix 107 -4584221084214209033 +rolling_mix 108 -4581482393331992217 +rolling_mix 109 -4579404928790366591 +rolling_mix 110 -4578007056210988386 +rolling_mix 111 -4576588437572334823 +rolling_mix 112 -4575418282403068821 +rolling_mix 113 -4574707361003479550 +rolling_mix 114 -4573994301662714735 +rolling_mix 115 -4573280968697602334 +rolling_mix 116 -4572565545202092866 +rolling_mix 117 -4571843894114714458 +rolling_mix 118 -4571139936517946285 +rolling_mix 119 -4570777995870511516 +rolling_mix 120 -4570415062153654170 diff --git a/tests/test_determinism.ae b/tests/test_determinism.ae index b9984da..6ffb7e8 100644 --- a/tests/test_determinism.ae +++ b/tests/test_determinism.ae @@ -558,6 +558,95 @@ test_concurrent_worlds() { hull.destroy_hull(box) hull.destroy_hull(slab) } +// test_determinism.c's rolling mix (SingleRollingMixTest, the scene of its +// SIMDWidthTest): 64 spheres, capsules and boxes thrown spinning onto a +// slab, a few with rolling resistance, 120 steps; every step's checksum +// folds each body's transform and its manifolds' rolling, friction, twist +// and normal impulses, the state the wide solver keeps. The reference runs +// it at width 4 and 8 in one process; here the width is the build's, and +// CI checks this trace bit for bit on the four-lane build and the AVX2 one. +rolling_mix(workers: int) -> Outcome { + wdef = dynamics.default_world_def() + wdef.worker_count = workers + world_id = dynamics.create_world(&wdef) + gdef = dynamics.default_body_def() + ground = dynamics.create_body(world_id, &gdef) + slab = hull.make_box_hull(40.0, 1.0, 40.0) + gsdef = dynamics.default_shape_def() + dynamics.create_hull_shape(ground, &gsdef, slab) + box = hull.make_box_hull(0.4, 0.4, 0.4) + ball = manifold.sphere(math.vec3_zero(), 0.4) + rod = manifold.capsule(math.vec3(0.0 - 0.3, 0.0, 0.0), math.vec3(0.3, 0.0, 0.0), 0.25) + count = 64 + bodies_block = calloc(count, sizeof(BodyId)) + bodies = bodies_block as BodyId[] + seed = 271828 as long + i = 0 + while i < count { + row = i / 8 + column = i % 8 + bdef = dynamics.default_body_def() + bdef.kind = dynamics.BODY_DYNAMIC + bdef.position = math.vec3(2.0 * (column as float) - 7.0, 1.45, 2.0 * (row as float) - 7.0) + bodies[i] = dynamics.create_body(world_id, &bdef) + seed = (seed * (1664525 as long) + (1013904223 as long)) & (4294967295 as long) + sdef = dynamics.default_shape_def() + sdef.density = 1.0 + sdef.base_material.rolling_resistance = 0.0 + if (seed >> 28) < (5 as long) { sdef.base_material.rolling_resistance = 0.2 } + kind = i % 3 + if kind == 0 { dynamics.create_sphere_shape(bodies[i], &sdef, ball) } + else if kind == 1 { dynamics.create_capsule_shape(bodies[i], &sdef, rod) } + else { dynamics.create_hull_shape(bodies[i], &sdef, box) } + sign = 1.0 + if (i & 1) != 0 { sign = 0.0 - 1.0 } + dynamics.body_set_linear_velocity(bodies[i], math.vec3(0.0, 0.0 - 0.5, sign * 0.5)) + dynamics.body_set_angular_velocity(bodies[i], math.vec3(sign * 3.0, 0.0, 0.0 - 2.0 * sign)) + i = i + 1 + } + data_block = calloc(4, sizeof(ContactData)) + data = data_block as ContactData[] + total = 0.0 + step = 0 + while step < 120 { + physics_world.step(world_id, 1.0 / 60.0, 4) + hash = 0.0 + i = 0 + while i < count { + hash = fold_transform(hash, dynamics.body_get_transform(bodies[i])) + contacts = dynamics.body_get_contact_data(bodies[i], data, 4) + c = 0 + while c < contacts { + m = 0 + while m < data[c].manifold_count { + mf = ((data[c].manifolds + m * sizeof(Manifold)) as ptr) as *Manifold + hash = hash + (mf.rolling_impulse.x as float) * 3.0 + (mf.rolling_impulse.y as float) * 0.3 + (mf.rolling_impulse.z as float) * 30.0 + hash = hash + (mf.friction_impulse.x as float) * 5.0 + (mf.friction_impulse.y as float) * 0.5 + (mf.friction_impulse.z as float) * 50.0 + hash = hash + (mf.twist_impulse as float) * 11.0 + p = 0 + while p < mf.point_count { + mp = dynamics.manifold_point_at(mf, p) + hash = hash + (mp.normal_impulse as float) * 13.0 + (mp.total_normal_impulse as float) * 0.013 + p = p + 1 + } + m = m + 1 + } + c = c + 1 + } + i = i + 1 + } + step = step + 1 + if g_trace && g_trace_run { println("trace rolling_mix ${step} ${hash} ${bits_of(hash)}") } + total = total + hash + } + free(data_block) + free(bodies_block) + dynamics.destroy_world(world_id) + hull.destroy_hull(slab) + hull.destroy_hull(box) + return Outcome { sleep_step: 120, hash: total, query_hits: 0, query_hash: 0.0 } +} + main() { test_concurrent_worlds() value = os.getenv("AEPHYSICS_TRACE") @@ -609,6 +698,15 @@ main() { ensure("mesh drop: the same scene twice, bit for bit", same_outcome(first, second)) ensure("mesh drop: the same over four workers, bit for bit", same_outcome(first, four)) + g_trace_run = true + first = rolling_mix(1) + g_trace_run = false + second = rolling_mix(1) + four = rolling_mix(4) + println("determinism: rolling mix checksum ${first.hash}") + ensure("rolling mix: the same scene twice, bit for bit", same_outcome(first, second)) + ensure("rolling mix: the same over four workers, bit for bit", same_outcome(first, four)) + println("determinism: ${checks} checks") if failures == 0 { println("determinism: all checks passed") diff --git a/tests/test_dynamics.ae b/tests/test_dynamics.ae index f8db184..7986701 100644 --- a/tests/test_dynamics.ae +++ b/tests/test_dynamics.ae @@ -1248,6 +1248,32 @@ test_shape_flags() { dynamics.destroy_world(world_id) } +// test_shape.c's ShapeNameTest: no name from a default def, the def's name +// carried, the setter overwriting, a long name kept whole (the reference's +// name cache keeps it so), null clearing it; a body's name from its def. +test_shape_names() { + world_id = new_world() + bdef = dynamic_body_def() + bdef.name = "carrier" + body = dynamics.create_body(world_id, &bdef) + ensure("names: the body def's name", string.equals(dynamics.body_get_name(body), "carrier") == 1) + sdef = dynamics.default_shape_def() + sdef.density = 1.0 + ball = manifold.sphere(math.vec3_zero(), 0.5) + plain = dynamics.create_sphere_shape(body, &sdef, ball) + ensure("names: none from a default def", string.length(dynamics.shape_get_name(plain)) == 0) + sdef.name = "box" + named = dynamics.create_sphere_shape(body, &sdef, ball) + ensure("names: the def's name", string.equals(dynamics.shape_get_name(named), "box") == 1) + dynamics.shape_set_name(plain, "wheel") + ensure("names: the setter", string.equals(dynamics.shape_get_name(plain), "wheel") == 1) + dynamics.shape_set_name(plain, "abcdefghijklmnopqrstuvwxyz") + ensure("names: a long name whole", string.equals(dynamics.shape_get_name(plain), "abcdefghijklmnopqrstuvwxyz") == 1) + dynamics.shape_set_name(plain, null) + ensure("names: null clears it", string.length(dynamics.shape_get_name(plain)) == 0) + dynamics.destroy_world(world_id) +} + test_shape_api() { world_id = slab_world() w = dynamics.get_world(world_id.index1 - 1) @@ -1730,6 +1756,7 @@ main() { test_shape_filter() test_shape_api() test_shape_flags() + test_shape_names() test_sensors() hull.destroy_hull(g_cube as *HullData) hull.destroy_hull(g_slab as *HullData) diff --git a/tests/test_edge_cases.ae b/tests/test_edge_cases.ae index 714e7e9..9d33539 100644 --- a/tests/test_edge_cases.ae +++ b/tests/test_edge_cases.ae @@ -1170,6 +1170,252 @@ test_non_dynamic_joint_disable() { dynamics.destroy_world(world_id) } +// test_world.c's BodySimLocatorTest: the reference's five scenes that +// drive each place a contact's sim indices can go stale, every step +// validated, each run at one worker and at four with the final states +// compared bit for bit; and LocatorFastFlagTest. +var g_locator_failed = false +var g_locator_boxes: ptr = null // the box hulls a scene's bodies hold, freed with it +var g_locator_box_count = 0 + +locator_world(workers: int) -> WorldId { + wdef = dynamics.default_world_def() + wdef.worker_count = workers + g_locator_box_count = 0 + return dynamics.create_world(&wdef) +} + +locator_body(world_id: WorldId, kind: int, p: Vec3, hx: float, hy: float, hz: float) -> BodyId { + bdef = dynamics.default_body_def() + bdef.kind = kind + bdef.position = p + body = dynamics.create_body(world_id, &bdef) + box = hull.make_box_hull(hx, hy, hz) + boxes = g_locator_boxes as ptr[] + boxes[g_locator_box_count] = box as ptr + g_locator_box_count = g_locator_box_count + 1 + sdef = dynamics.default_shape_def() + dynamics.create_hull_shape(body, &sdef, box) + return body +} + +locator_steps(world_id: WorldId, count: int) { + if checked_steps(world_id, count) == false { g_locator_failed = true } +} + +// Steps until the body sleeps or the budget runs out; the steps taken. +until_asleep(world_id: WorldId, body: BodyId, budget: int) -> int { + i = 0 + while i < budget { + locator_steps(world_id, 1) + if dynamics.body_is_awake(body) == false { return i + 1 } + i = i + 1 + } + return budget +} + +// A world's state as one number: every live body's transform and +// velocities, weighted by its id, so equal runs give equal sums. +locator_hash(world_id: WorldId) -> float { + w = dynamics.get_world(world_id.index1 - 1) + sum = 0.0 + i = 0 + while i < w.bodies.count { + body = dynamics.body_at(w, i) + if body.set_index != dynamics.NULL_INDEX { + t = dynamics.get_body_sim(w, body).transform + weight = (i + 1) as float + sum = sum + weight * (t.p.x + 3.0 * t.p.y + 5.0 * t.p.z + 7.0 * t.q.x + 11.0 * t.q.y + 13.0 * t.q.z + 17.0 * t.q.w) + } + i = i + 1 + } + return sum +} + +locator_end(world_id: WorldId) -> float { + h = locator_hash(world_id) + dynamics.destroy_world(world_id) + boxes = g_locator_boxes as ptr[] + i = 0 + while i < g_locator_box_count { + hull.destroy_hull(boxes[i] as *HullData) + i = i + 1 + } + g_locator_box_count = 0 + return h +} + +// Sleep and wake: the plate beside the stack holds a non-touching contact +// through the cycle, the one the wake path must fix up itself. +locator_sleep_wake(workers: int) -> float { + world_id = locator_world(workers) + locator_body(world_id, dynamics.BODY_STATIC, math.vec3(0.0, 0.0 - 1.0, 0.0), 5.0, 1.0, 5.0) + stack0 = locator_body(world_id, dynamics.BODY_DYNAMIC, math.vec3(0.0, 0.5, 0.0), 0.5, 0.5, 0.5) + locator_body(world_id, dynamics.BODY_DYNAMIC, math.vec3(0.0, 1.5, 0.0), 0.5, 0.5, 0.5) + stack2 = locator_body(world_id, dynamics.BODY_DYNAMIC, math.vec3(0.0, 2.5, 0.0), 0.5, 0.5, 0.5) + plate = locator_body(world_id, dynamics.BODY_KINEMATIC, math.vec3(1.04, 2.5, 0.0), 0.5, 0.5, 0.5) + dynamics.body_enable_sleep(plate, false) + if until_asleep(world_id, stack0, 300) == 300 || dynamics.body_is_awake(stack2) { g_locator_failed = true } + dynamics.body_set_awake(stack0, true) + locator_steps(world_id, 5) + if dynamics.body_is_awake(stack2) == false { g_locator_failed = true } + if until_asleep(world_id, stack0, 300) == 300 { g_locator_failed = true } + locator_body(world_id, dynamics.BODY_DYNAMIC, math.vec3(0.0, 8.0, 0.0), 0.5, 0.5, 0.5) + woke = false + i = 0 + while i < 180 { + locator_steps(world_id, 1) + if dynamics.body_is_awake(stack0) { woke = true } + i = i + 1 + } + if woke == false { g_locator_failed = true } + return locator_end(world_id) +} + +// An island falling asleep and a body leaving the awake set swap the last +// awake sim into the hole. +locator_awake_neighbor(workers: int) -> float { + world_id = locator_world(workers) + locator_body(world_id, dynamics.BODY_STATIC, math.vec3(0.0, 0.0 - 1.0, 0.0), 30.0, 1.0, 5.0) + sleeper0 = locator_body(world_id, dynamics.BODY_DYNAMIC, math.vec3(0.0 - 10.0, 0.5, 0.0), 0.5, 0.5, 0.5) + locator_body(world_id, dynamics.BODY_DYNAMIC, math.vec3(0.0 - 10.0, 1.5, 0.0), 0.5, 0.5, 0.5) + spare = locator_body(world_id, dynamics.BODY_DYNAMIC, math.vec3(20.0, 0.5, 0.0), 0.5, 0.5, 0.5) + dynamics.body_enable_sleep(spare, false) + awake0 = locator_body(world_id, dynamics.BODY_DYNAMIC, math.vec3(10.0, 0.5, 0.0), 0.5, 0.5, 0.5) + dynamics.body_enable_sleep(awake0, false) + i = 1 + while i < 3 { + a = locator_body(world_id, dynamics.BODY_DYNAMIC, math.vec3(10.0, 0.5 + (i as float), 0.0), 0.5, 0.5, 0.5) + dynamics.body_enable_sleep(a, false) + i = i + 1 + } + if until_asleep(world_id, sleeper0, 300) == 300 || dynamics.body_is_awake(awake0) == false { g_locator_failed = true } + locator_steps(world_id, 10) + dynamics.body_disable(spare) + locator_steps(world_id, 10) + dynamics.body_enable(spare) + locator_steps(world_id, 10) + return locator_end(world_id) +} + +// Destroying a body swaps the last sim of its set into the hole, awake and static. +locator_destroy(workers: int) -> float { + world_id = locator_world(workers) + locator_body(world_id, dynamics.BODY_STATIC, math.vec3(0.0, 0.0 - 1.0, 0.0), 5.0, 1.0, 5.0) + first = locator_body(world_id, dynamics.BODY_DYNAMIC, math.vec3(0.0, 0.5, 0.0), 0.5, 0.5, 0.5) + i = 1 + while i < 4 { + locator_body(world_id, dynamics.BODY_DYNAMIC, math.vec3(0.0, 0.5 + (i as float), 0.0), 0.5, 0.5, 0.5) + i = i + 1 + } + locator_steps(world_id, 30) + dynamics.destroy_body(first) + locator_steps(world_id, 30) + first_static = locator_body(world_id, dynamics.BODY_STATIC, math.vec3(10.0, 0.0 - 1.0, 0.0), 1.0, 1.0, 1.0) + locator_body(world_id, dynamics.BODY_DYNAMIC, math.vec3(10.0, 0.5, 0.0), 0.5, 0.5, 0.5) + i = 1 + while i < 3 { + x = 10.0 + 4.0 * (i as float) + locator_body(world_id, dynamics.BODY_STATIC, math.vec3(x, 0.0 - 1.0, 0.0), 1.0, 1.0, 1.0) + locator_body(world_id, dynamics.BODY_DYNAMIC, math.vec3(x, 0.5, 0.0), 0.5, 0.5, 0.5) + i = i + 1 + } + locator_steps(world_id, 30) + dynamics.destroy_body(first_static) + locator_steps(world_id, 30) + return locator_end(world_id) +} + +// A kinematic plate parked a hair above a box keeps a non-touching contact +// to it as it sleeps, the one case found through the body; driven down, +// it wakes the box. +locator_kinematic(workers: int) -> float { + world_id = locator_world(workers) + locator_body(world_id, dynamics.BODY_STATIC, math.vec3(0.0, 0.0 - 1.0, 0.0), 5.0, 1.0, 5.0) + box = locator_body(world_id, dynamics.BODY_DYNAMIC, math.vec3(0.0, 0.5, 0.0), 0.5, 0.5, 0.5) + plate = locator_body(world_id, dynamics.BODY_KINEMATIC, math.vec3(0.0, 1.52, 0.0), 1.0, 0.5, 1.0) + if until_asleep(world_id, box, 300) == 300 { g_locator_failed = true } + dynamics.body_set_linear_velocity(plate, math.vec3(0.0, 0.0 - 1.0, 0.0)) + locator_steps(world_id, 60) + if dynamics.body_is_awake(box) == false { g_locator_failed = true } + return locator_end(world_id) +} + +// The type changed and the body disabled move it between sets; a joint +// across two sleeping sets merges them. +locator_transfer(workers: int) -> float { + world_id = locator_world(workers) + locator_body(world_id, dynamics.BODY_STATIC, math.vec3(0.0, 0.0 - 1.0, 0.0), 20.0, 1.0, 5.0) + box = locator_body(world_id, dynamics.BODY_DYNAMIC, math.vec3(0.0, 0.5, 0.0), 0.5, 0.5, 0.5) + locator_steps(world_id, 30) + dynamics.body_set_type(box, dynamics.BODY_STATIC) + locator_steps(world_id, 5) + dynamics.body_set_type(box, dynamics.BODY_DYNAMIC) + locator_steps(world_id, 5) + dynamics.body_disable(box) + locator_steps(world_id, 5) + dynamics.body_enable(box) + locator_steps(world_id, 30) + sleeper0 = locator_body(world_id, dynamics.BODY_DYNAMIC, math.vec3(0.0 - 12.0, 0.5, 0.0), 0.5, 0.5, 0.5) + sleeper1 = locator_body(world_id, dynamics.BODY_DYNAMIC, math.vec3(12.0, 0.5, 0.0), 0.5, 0.5, 0.5) + if until_asleep(world_id, sleeper0, 300) == 300 || dynamics.body_is_awake(sleeper1) { g_locator_failed = true } + jdef = dynamics.default_spherical_joint_def() + jdef.base.body_id_a = sleeper0 + jdef.base.body_id_b = sleeper1 + dynamics.create_spherical_joint(world_id, &jdef) + locator_steps(world_id, 30) + return locator_end(world_id) +} + +locator_scene(index: int, workers: int) -> float { + if index == 0 { return locator_sleep_wake(workers) } + if index == 1 { return locator_awake_neighbor(workers) } + if index == 2 { return locator_destroy(workers) } + if index == 3 { return locator_kinematic(workers) } + return locator_transfer(workers) +} + +test_body_sim_locators() { + g_locator_boxes = calloc(64, 8) + names = [ "sleep and wake", "awake neighbour", "destroy", "kinematic", "transfer" ] + i = 0 + while i < 5 { + g_locator_failed = false + one = locator_scene(i, 1) + four = locator_scene(i, 4) + ensure("locators, ${names[i]}: valid every step, each event as the scene expects", g_locator_failed == false) + ensure("locators, ${names[i]}: one worker and four alike (${one} against ${four})", one == four) + i = i + 1 + } + free(g_locator_boxes) + // LocatorFastFlagTest: the fast flag a real fast step set survives a + // setter that syncs the body's flags to its sim. + wdef = dynamics.default_world_def() + world_id = dynamics.create_world(&wdef) + g_locator_boxes = calloc(4, 8) + g_locator_box_count = 0 + locator_body(world_id, dynamics.BODY_STATIC, math.vec3(0.0, 0.0 - 1.0, 0.0), 5.0, 1.0, 5.0) + bdef = dynamics.default_body_def() + bdef.kind = dynamics.BODY_DYNAMIC + bdef.position = math.vec3(0.0 - 20.0, 2.0, 0.0) + bdef.linear_velocity = math.vec3(400.0, 0.0, 0.0) + bullet = dynamics.create_body(world_id, &bdef) + sdef = dynamics.default_shape_def() + dynamics.create_sphere_shape(bullet, &sdef, manifold.sphere(math.vec3_zero(), 0.1)) + steps(world_id, 1) + w = dynamics.get_world(world_id.index1 - 1) + flags = dynamics.get_body_sim(w, dynamics.get_body_full_id(w, bullet)).flags + ensure("fast flag: set by a fast step", (flags & dynamics.IS_FAST) != 0) + locks = MotionLocks { linear_x: false, linear_y: false, linear_z: false, angular_x: true, angular_y: false, angular_z: false } + dynamics.body_set_motion_locks(bullet, locks) + flags = dynamics.get_body_sim(w, dynamics.get_body_full_id(w, bullet)).flags + ensure("fast flag: kept through the motion locks' flag sync", (flags & dynamics.IS_FAST) != 0) + steps(world_id, 1) + ensure("fast flag: valid after", valid_world(world_id)) + locator_end(world_id) + free(g_locator_boxes) +} + test_encoded_sims() { fixtures_ready() slab = hull.make_box_hull(2.5, 0.5, 2.5) @@ -1298,6 +1544,7 @@ main() { test_rocking_hull() test_encoded_sims() test_non_dynamic_joint_disable() + test_body_sim_locators() println("edge_cases: ${checks} checks") if failures == 0 { println("edge_cases: all checks passed") diff --git a/tests/test_physics_world.ae b/tests/test_physics_world.ae index 2d788cc..23effd3 100644 --- a/tests/test_physics_world.ae +++ b/tests/test_physics_world.ae @@ -15,6 +15,7 @@ import aephysics.dynamic_tree import aephysics.hull import aephysics.manifold import aephysics.mesh +import aephysics.height_field import aephysics.compound import aephysics.material import aephysics.shape @@ -28,6 +29,9 @@ import aephysics.joint_solver extern exit(code: int) extern calloc(count: int, size: int) -> ptr extern free(p: ptr) +extern cos(x: float) -> float +extern sin(x: float) -> float +extern sqrt(x: float) -> float var failures = 0 var checks = 0 @@ -566,6 +570,616 @@ test_enlarged_proxy_destroyed() { dynamics.destroy_world(world_id) } +// test_id.c's IdTest: a 64-bit value through a body, shape and joint id +// and back, and a world id through its 32 bits. +test_ids() { + x = (0x01234567 as long) << 32 | (0x89ABCDEF as long) + ensure("ids: a body id stores what it loaded", dynamics.store_body_id(dynamics.load_body_id(x)) == x) + ensure("ids: a shape id stores what it loaded", dynamics.store_shape_id(dynamics.load_shape_id(x)) == x) + ensure("ids: a joint id stores what it loaded", dynamics.store_joint_id(dynamics.load_joint_id(x)) == x) + b = dynamics.load_body_id(x) + ensure("ids: the index high, the world and generation below", b.index1 == 0x01234567 && b.world0 == 0x89AB && b.generation == 0xCDEF) + wid = WorldId { index1: 3, generation: 517 } + back = dynamics.load_world_id(dynamics.store_world_id(wid)) + ensure("ids: a world id through 32 bits", back.index1 == 3 && back.generation == 517) + // test_manifold.c's MaxPointCountTest. + ensure("the most points a manifold holds", dynamics.get_max_manifold_points() == dynamics.MAX_MANIFOLD_POINTS && dynamics.MAX_MANIFOLD_POINTS == 4) +} + +// test_world.c's TestOverflowColorPile (shared/overflow_color.c): 25 boxes +// in five rings about a tall heavy hub, each touching it from the first +// step, more dynamic contacts on one body than the dynamic colours hold, so +// some go to the overflow colour; ten steps through the overflow's solve. +// The awake contact count is the reference's: the graph's contacts and +// the awake non-touching ones. +test_overflow_color_pile() { + wdef = dynamics.default_world_def() + wdef.worker_count = 1 + world_id = dynamics.create_world(&wdef) + gdef = dynamics.default_body_def() + gdef.position = math.vec3(0.0, 0.0 - 1.0, 0.0) + ground = dynamics.create_body(world_id, &gdef) + slab = hull.make_box_hull(20.0, 1.0, 20.0) + sdef = dynamics.default_shape_def() + dynamics.create_hull_shape(ground, &sdef, slab) + hub_half_x = 0.5 + hub_half_y = 2.5 + hdef = dynamics.default_body_def() + hdef.kind = dynamics.BODY_DYNAMIC + hdef.position = math.vec3(0.0, hub_half_y, 0.0) + hub = dynamics.create_body(world_id, &hdef) + hub_box = hull.make_box_hull(hub_half_x, hub_half_y, 0.5) + hsdef = dynamics.default_shape_def() + hsdef.density = 50.0 + dynamics.create_hull_shape(hub, &hsdef, hub_box) + neighbor_half = 0.2 + ring_radius = hub_half_x + neighbor_half - 0.03 + neighbor_box = hull.make_box_hull(neighbor_half, neighbor_half, neighbor_half) + ndef = dynamics.default_shape_def() + ring = 0 + while ring < 5 { + y = neighbor_half + 0.05 + 0.5 * (ring as float) + offset = 0.0 + if (ring & 1) != 0 { offset = math.PI / 5.0 } + slot = 0 + while slot < 5 { + theta = offset + (2.0 * math.PI * (slot as float)) / 5.0 + bdef = dynamics.default_body_def() + bdef.kind = dynamics.BODY_DYNAMIC + bdef.position = math.vec3(ring_radius * cos(theta), y, ring_radius * sin(theta)) + body = dynamics.create_body(world_id, &bdef) + dynamics.create_hull_shape(body, &ndef, neighbor_box) + slot = slot + 1 + } + ring = ring + 1 + } + i = 0 + while i < 10 { + physics_world.step(world_id, 1.0 / 60.0, 4) + i = i + 1 + } + counters = physics_world.get_counters(world_id) + overflow = counters.color_counts[dynamics.GRAPH_COLOR_COUNT - 1] + ensure("overflow pile: some contacts in the overflow colour (${overflow})", overflow > 0) + in_colors = 0 + k = 0 + while k < dynamics.GRAPH_COLOR_COUNT { + in_colors = in_colors + counters.color_counts[k] + k = k + 1 + } + w = dynamics.get_world(world_id.index1 - 1) + non_touching = dynamics.get_solver_set(w, dynamics.AWAKE_SET).contact_indices.count + ensure("overflow pile: the colours hold every touching contact (${in_colors} of ${counters.contact_count}, ${non_touching} not touching)", + in_colors + non_touching == counters.contact_count) + ensure("overflow pile: the awake contacts are the graph's and the non-touching (${counters.awake_contact_count})", + counters.awake_contact_count == counters.contact_count) + dynamics.destroy_world(world_id) + hull.destroy_hull(slab) + hull.destroy_hull(hub_box) + hull.destroy_hull(neighbor_box) +} + +// test_world.c's TestSetWorkerCount: the count set between steps, 1 to 4, +// 4 again, then 0 and -5 (each becomes 1) and past the cap (becomes it), +// clamped as the reference clamps it. The reference moves its junkyard's +// pusher between the changes; here a pile steps between them, and the +// steps of every count give the run of one worker throughout, bit for bit. +pile_world(worker_count: int, slab: *HullData, cube: *HullData) -> WorldId { + wdef = dynamics.default_world_def() + wdef.worker_count = worker_count + world_id = dynamics.create_world(&wdef) + gdef = dynamics.default_body_def() + gdef.position = math.vec3(0.0, 0.0 - 0.5, 0.0) + ground = dynamics.create_body(world_id, &gdef) + sdef = dynamics.default_shape_def() + dynamics.create_hull_shape(ground, &sdef, slab) + bdef = dynamics.default_body_def() + bdef.kind = dynamics.BODY_DYNAMIC + i = 0 + while i < 64 { + bdef.position = math.vec3(((i % 4) as float) * 1.1 - 1.6, 0.5 + 1.05 * ((i / 16) as float), (((i / 4) % 4) as float) * 1.1 - 1.6) + body = dynamics.create_body(world_id, &bdef) + dynamics.create_hull_shape(body, &sdef, cube) + i = i + 1 + } + return world_id +} + +pile_sum(world_id: WorldId) -> float { + w = dynamics.get_world(world_id.index1 - 1) + sum = 0.0 + i = 0 + while i < w.bodies.count { + body = dynamics.body_at(w, i) + if body.set_index != dynamics.NULL_INDEX && body.kind == dynamics.BODY_DYNAMIC { + p = dynamics.get_body_sim(w, body).transform.p + sum = sum + p.x * 3.0 + p.y * 5.0 + p.z * 7.0 + } + i = i + 1 + } + return sum +} + +test_set_worker_count() { + slab = hull.make_box_hull(10.0, 0.5, 10.0) + cube = hull.make_cube_hull(0.5) + world_id = pile_world(1, slab, cube) + ensure("worker count: one", physics_world.get_worker_count(world_id) == 1) + steps_block = calloc(6, 4) + counts = steps_block as int[] + counts[0] = 4 + counts[1] = 4 + counts[2] = 0 + counts[3] = 0 - 5 + counts[4] = native.MAX_WORKERS + 10 + counts[5] = 2 + expected = [ 4, 4, 1, 1, native.MAX_WORKERS, 2 ] + physics_world.step(world_id, 1.0 / 60.0, 4) + i = 0 + while i < 6 { + physics_world.set_worker_count(world_id, counts[i]) + got = physics_world.get_worker_count(world_id) + ensure("worker count: set ${counts[i]}, gets ${got}", got == expected[i]) + j = 0 + while j < 5 { + physics_world.step(world_id, 1.0 / 60.0, 4) + j = j + 1 + } + ensure("worker count: valid stepping at ${got}", dynamics.validate_solver_sets(dynamics.get_world(world_id.index1 - 1))) + i = i + 1 + } + changed = pile_sum(world_id) + dynamics.destroy_world(world_id) + // The same pile on one worker throughout. + single = pile_world(1, slab, cube) + k = 0 + while k < 31 { + physics_world.step(single, 1.0 / 60.0, 4) + k = k + 1 + } + ensure("worker count: changing it between steps changes nothing (${changed} against ${pile_sum(single)})", changed == pile_sum(single)) + dynamics.destroy_world(single) + free(steps_block) + hull.destroy_hull(slab) + hull.destroy_hull(cube) +} + +// test_world.c's TestCompoundHitEvents: a sphere driven down onto either +// hull child of a static compound; its hit event carries the sphere's +// material id on one side and the struck child's (11 or 22) on the other, +// never the first child's for both. +test_compound_hit_events() { + side = 0 + while side < 2 { + expected = 11 as long + spawn_x = 0.0 - 3.0 + if side == 1 { + expected = 22 as long + spawn_x = 3.0 + } + wdef = dynamics.default_world_def() + wdef.hit_event_threshold = 1.0 + world_id = dynamics.create_world(&wdef) + box_a = hull.make_box_hull(1.0, 1.0, 1.0) + box_b = hull.make_box_hull(1.0, 1.0, 1.0) + mat_a = material.default_surface_material() + mat_a.user_material_id = 11 as long + mat_b = material.default_surface_material() + mat_b.user_material_id = 22 as long + hulls_block = calloc(2, sizeof(CompoundHullDef)) + hulls = hulls_block as CompoundHullDef[] + hulls[0] = compound.compound_hull_def(box_a, Transform { p: math.vec3(0.0 - 3.0, 0.0, 0.0), q: math.quat_identity() }, mat_a) + hulls[1] = compound.compound_hull_def(box_b, Transform { p: math.vec3(3.0, 0.0, 0.0), q: math.quat_identity() }, mat_b) + cdef = compound.compound_def() + cdef.hulls = hulls_block + cdef.hull_count = 2 + c = compound.create_compound(&cdef) + ensure("compound hit events: the compound made", c != null) + sbdef = dynamics.default_body_def() + holder = dynamics.create_body(world_id, &sbdef) + csdef = dynamics.default_shape_def() + dynamics.create_compound_shape(holder, &csdef, c) + bdef = dynamics.default_body_def() + bdef.kind = dynamics.BODY_DYNAMIC + bdef.gravity_scale = 0.0 + bdef.position = math.vec3(spawn_x, 3.0, 0.0) + bdef.linear_velocity = math.vec3(0.0, 0.0 - 30.0, 0.0) + ball = dynamics.create_body(world_id, &bdef) + bsdef = dynamics.default_shape_def() + bsdef.density = 1.0 + bsdef.enable_hit_events = true + bsdef.base_material.user_material_id = 99 as long + dynamics.create_sphere_shape(ball, &bsdef, manifold.sphere(math.vec3_zero(), 0.5)) + hits = 0 + got_a = 0 as long + got_b = 0 as long + i = 0 + while i < 30 { + physics_world.step(world_id, 1.0 / 60.0, 4) + count = physics_world.contact_hit_event_count(world_id) + if count > 0 && hits == 0 { + hit = physics_world.contact_hit_event(world_id, 0) + got_a = hit.user_material_id_a + got_b = hit.user_material_id_b + } + hits = hits + count + i = i + 1 + } + dynamics.destroy_world(world_id) + compound.destroy_compound(c) + hull.destroy_hull(box_a) + hull.destroy_hull(box_b) + free(hulls_block) + ensure("compound hit events (side ${side}): a hit (${hits})", hits >= 1) + ensure("compound hit events (side ${side}): the sphere's material (${got_a} ${got_b})", got_a == (99 as long) || got_b == (99 as long)) + ensure("compound hit events (side ${side}): the struck child's material", got_a == expected || got_b == expected) + side = side + 1 + } +} + +var g_friction_calls = 0 +var g_saw_child0 = false +var g_saw_child1 = false +var g_mixed_friction = 0.0 + +capture_friction(friction_a: float, id_a: long, friction_b: float, id_b: long) -> float { + g_friction_calls = g_friction_calls + 1 + if id_a == (101 as long) || id_b == (101 as long) { g_saw_child0 = true } + if id_a == (202 as long) || id_b == (202 as long) { + g_saw_child1 = true + g_mixed_friction = sqrt(friction_a * friction_b) + } + return sqrt(friction_a * friction_b) +} + +// TestCompoundContactMaterials: a unit-friction sphere struck down onto +// child 1 of a compound whose child 0 has friction 0.04 and child 1 0.81, +// for sphere, capsule and hull children: the friction callback sees child +// 1's material, never child 0's, and mixes to sqrt(0.81) = 0.9. +test_compound_contact_materials() { + mat0 = material.default_surface_material() + mat0.friction = 0.04 + mat0.user_material_id = 101 as long + mat1 = material.default_surface_material() + mat1.friction = 0.81 + mat1.user_material_id = 202 as long + box = hull.make_box_hull(0.5, 0.5, 0.5) + spheres_block = calloc(2, sizeof(CompoundSphereDef)) + spheres = spheres_block as CompoundSphereDef[] + spheres[0] = compound.compound_sphere_def(manifold.sphere(math.vec3(0.0 - 3.0, 0.0, 0.0), 0.5), mat0) + spheres[1] = compound.compound_sphere_def(manifold.sphere(math.vec3(3.0, 0.0, 0.0), 0.5), mat1) + capsules_block = calloc(2, sizeof(CompoundCapsuleDef)) + capsules = capsules_block as CompoundCapsuleDef[] + capsules[0] = compound.compound_capsule_def(manifold.capsule(math.vec3(0.0 - 3.0, 0.0, 0.0 - 0.5), math.vec3(0.0 - 3.0, 0.0, 0.5), 0.5), mat0) + capsules[1] = compound.compound_capsule_def(manifold.capsule(math.vec3(3.0, 0.0, 0.0 - 0.5), math.vec3(3.0, 0.0, 0.5), 0.5), mat1) + hulls_block = calloc(2, sizeof(CompoundHullDef)) + hulls = hulls_block as CompoundHullDef[] + hulls[0] = compound.compound_hull_def(box, Transform { p: math.vec3(0.0 - 3.0, 0.0, 0.0), q: math.quat_identity() }, mat0) + hulls[1] = compound.compound_hull_def(box, Transform { p: math.vec3(3.0, 0.0, 0.0), q: math.quat_identity() }, mat1) + kind = 0 + while kind < 3 { + cdef = compound.compound_def() + if kind == 0 { + cdef.spheres = spheres_block + cdef.sphere_count = 2 + } else if kind == 1 { + cdef.capsules = capsules_block + cdef.capsule_count = 2 + } else { + cdef.hulls = hulls_block + cdef.hull_count = 2 + } + c = compound.create_compound(&cdef) + ensure("compound materials (kind ${kind}): the compound made", c != null) + g_friction_calls = 0 + g_saw_child0 = false + g_saw_child1 = false + g_mixed_friction = 0.0 + wdef = dynamics.default_world_def() + wdef.friction_callback = capture_friction + world_id = dynamics.create_world(&wdef) + sbdef = dynamics.default_body_def() + holder = dynamics.create_body(world_id, &sbdef) + csdef = dynamics.default_shape_def() + dynamics.create_compound_shape(holder, &csdef, c) + bdef = dynamics.default_body_def() + bdef.kind = dynamics.BODY_DYNAMIC + bdef.gravity_scale = 0.0 + bdef.position = math.vec3(3.0, 3.0, 0.0) + bdef.linear_velocity = math.vec3(0.0, 0.0 - 30.0, 0.0) + ball = dynamics.create_body(world_id, &bdef) + bsdef = dynamics.default_shape_def() + bsdef.density = 1.0 + bsdef.base_material.friction = 1.0 + bsdef.base_material.user_material_id = 999 as long + dynamics.create_sphere_shape(ball, &bsdef, manifold.sphere(math.vec3_zero(), 0.5)) + i = 0 + while i < 30 { + physics_world.step(world_id, 1.0 / 60.0, 4) + i = i + 1 + } + dynamics.destroy_world(world_id) + compound.destroy_compound(c) + ensure("compound materials (kind ${kind}): the callback called (${g_friction_calls})", g_friction_calls > 0) + ensure("compound materials (kind ${kind}): never child 0's material", g_saw_child0 == false) + ensure("compound materials (kind ${kind}): child 1's material", g_saw_child1) + small("compound materials (kind ${kind}): mixed to 0.9", g_mixed_friction - 0.9, 0.00001) + kind = kind + 1 + } + hull.destroy_hull(box) + free(spheres_block) + free(capsules_block) + free(hulls_block) +} + +// test_broad_phase.c's BroadPhaseStressTest: a swarm of up to 64 bodies +// churning through every proxy change the world offers (made and destroyed, +// teleported, sped up, disabled and enabled, bullet on and off, its type +// changed, welded with collision off and toggled, the static clutter moved, +// the static tree rebuilt) among 60 static boxes and height fields, over +// kilometre height-field tiles two kilometres from the origin; after every +// step a zero step finds the pairs, and the pair set is checked against +// every live shape pair by brute force. Three seeds, at one worker, at four, +// and at three with sleep on (where only a missing pair is a bug: a sleeping +// contact is not refreshed, so a stale one may outlive its overlap). +var g_stress_seed: long = 0 + +stress_next() -> long { + x = g_stress_seed + x = (x ^ (x << 13)) & (4294967295 as long) + x = x ^ (x >> 17) + x = (x ^ (x << 5)) & (4294967295 as long) + g_stress_seed = x + return x +} + +stress_float(lower: float, upper: float) -> float { + u = ((stress_next() & (16777215 as long)) as float) / 16777216.0 + return lower + (upper - lower) * u +} + +stress_int(count: int) -> int { return (stress_next() % (count as long)) as int } + +stress_origin() -> Vec3 { return math.vec3(0.0 - 1900.0, 0.0, 1330.0) } + +stress_position() -> Vec3 { + e = 12.0 + x = stress_float(0.0 - e, e) + y = stress_float(0.0 - e, e) + z = stress_float(0.0 - e, e) + return math.add(stress_origin(), math.vec3(x, y, z)) +} + +stress_velocity() -> Vec3 { + s = 12.0 + if stress_int(4) == 0 { s = 40.0 } + x = stress_float(0.0 - s, s) + y = stress_float(0.0 - s, s) + z = stress_float(0.0 - s, s) + return math.vec3(x, y, z) +} + +stress_body(world_id: WorldId, box: *HullData) -> BodyId { + bdef = dynamics.default_body_def() + bdef.kind = dynamics.BODY_DYNAMIC + if stress_int(10) == 0 { bdef.kind = dynamics.BODY_KINEMATIC } + bdef.position = stress_position() + bdef.linear_velocity = stress_velocity() + bdef.is_bullet = stress_int(3) == 0 + bdef.gravity_scale = 0.0 + body = dynamics.create_body(world_id, &bdef) + sdef = dynamics.default_shape_def() + sdef.filter.category_bits = (1 as long) << stress_int(3) + if stress_int(4) == 0 { sdef.filter.mask_bits = 5 as long } + count = 1 + stress_int(2) + i = 0 + while i < count { + if stress_int(2) == 0 { + cx = stress_float(0.0 - 1.0, 1.0) + r = stress_float(0.2, 1.5) + dynamics.create_sphere_shape(body, &sdef, manifold.sphere(math.vec3(cx, 0.0, 0.0), r)) + } else { + dynamics.create_hull_shape(body, &sdef, box) + } + i = i + 1 + } + return body +} + +// The pair set against every live, filtered, overlapping pair of shapes. +stress_pairs_hold(world_id: WorldId, sleeping: bool) -> bool { + w = dynamics.get_world(world_id.index1 - 1) + count = w.shapes.count + i = 0 + while i < count { + a = dynamics.world_shape_at(w, i) + if a.id != dynamics.NULL_INDEX && a.proxy_key != dynamics.NULL_INDEX && a.sensor_index == dynamics.NULL_INDEX { + body_a = dynamics.body_at(w, a.body_id) + j = i + 1 + while j < count { + b = dynamics.world_shape_at(w, j) + if b.id != dynamics.NULL_INDEX && b.proxy_key != dynamics.NULL_INDEX && b.sensor_index == dynamics.NULL_INDEX && a.body_id != b.body_id { + body_b = dynamics.body_at(w, b.body_id) + expected = shape.should_shapes_collide(a.filter, b.filter) && dynamics.should_bodies_collide(w, body_a, body_b) && + math.aabb_overlaps(dynamics.shape_fat_aabb(w, i), dynamics.shape_fat_aabb(w, j)) + actual = broad_phase.has_pair(&w.broad_phase, basics.shape_pair_key(i, j, 0)) + if (sleeping && actual && expected == false) == false && expected != actual { return false } + } + j = j + 1 + } + } + i = i + 1 + } + return true +} + +broad_phase_stress(workers: int, seed: long, sleeping: bool) -> bool { + wdef = dynamics.default_world_def() + wdef.worker_count = workers + wdef.enable_sleep = sleeping + wdef.gravity = math.vec3_zero() + world_id = dynamics.create_world(&wdef) + g_stress_seed = seed + box = hull.make_box_hull(0.95, 0.6, 2.2) + // Kilometre tiles far from the origin under the swarm, as a streamed terrain. + tile = height_field.create_grid(65, 65, math.vec3(16.0, 1.0, 16.0), false) + i = 0 + while i < 9 { + tdef = dynamics.default_body_def() + tdef.position = math.vec3(0.0 - 3072.0 + 1024.0 * ((i % 3) as float), 0.0 - 30.0, 1024.0 * ((i / 3) as float)) + tile_body = dynamics.create_body(world_id, &tdef) + tsdef = dynamics.default_shape_def() + tsdef.enable_speculative_contact = false + dynamics.create_height_field_shape(tile_body, &tsdef, tile) + i = i + 1 + } + // Static clutter in the swarm; some asks for its contacts when made. + statics_block = calloc(60, sizeof(BodyId)) + statics = statics_block as BodyId[] + grid = height_field.create_grid(5, 5, math.vec3(2.0, 1.0, 2.0), false) + static_box = hull.make_box_hull(3.0, 0.5, 3.0) + i = 0 + while i < 60 { + sbdef = dynamics.default_body_def() + sbdef.position = stress_position() + statics[i] = dynamics.create_body(world_id, &sbdef) + ssdef = dynamics.default_shape_def() + ssdef.invoke_contact_creation = (i % 4) == 0 + ssdef.enable_speculative_contact = (i % 2) == 0 + if i % 3 == 0 { dynamics.create_height_field_shape(statics[i], &ssdef, grid) } + else { dynamics.create_hull_shape(statics[i], &ssdef, static_box) } + i = i + 1 + } + slots_block = calloc(64, sizeof(BodyId)) + slots = slots_block as BodyId[] + active_block = calloc(64, 4) + active = active_block as int[] + i = 0 + while i < 64 { + if stress_int(4) != 0 { + slots[i] = stress_body(world_id, box) + active[i] = 1 + } + i = i + 1 + } + joints_block = calloc(4, sizeof(JointId)) + joints = joints_block as JointId[] + joint_count = 0 + ok = true + step = 0 + while step < 400 && ok { + mutations = stress_int(4) + m = 0 + while m < mutations { + slot = stress_int(64) + action = stress_int(12) + if active[slot] == 0 { + if action < 6 { + slots[slot] = stress_body(world_id, box) + active[slot] = 1 + } + } else if action == 0 { + dynamics.destroy_body(slots[slot]) + active[slot] = 0 + } else if action == 1 || action == 2 { + dynamics.body_set_transform(slots[slot], stress_position(), math.quat_identity()) + } else if action == 3 { + dynamics.body_set_linear_velocity(slots[slot], stress_velocity()) + } else if action == 4 { + dynamics.body_disable(slots[slot]) + } else if action == 5 { + dynamics.body_enable(slots[slot]) + } else if action == 6 { + dynamics.body_set_bullet(slots[slot], stress_int(2) == 0) + } else if action == 7 { + kind = stress_int(3) + if kind == 0 { dynamics.body_set_type(slots[slot], dynamics.BODY_STATIC) } + else if kind == 1 { dynamics.body_set_type(slots[slot], dynamics.BODY_KINEMATIC) } + else { dynamics.body_set_type(slots[slot], dynamics.BODY_DYNAMIC) } + if dynamics.body_get_type(slots[slot]) != dynamics.BODY_STATIC { dynamics.body_set_linear_velocity(slots[slot], stress_velocity()) } + } else if action == 8 { + // A weld that stops collision, or one toggled. + if joint_count < 4 { + other = stress_int(64) + if other != slot && active[other] != 0 && dynamics.body_get_type(slots[slot]) == dynamics.BODY_DYNAMIC && + dynamics.body_get_type(slots[other]) == dynamics.BODY_DYNAMIC { + jdef = dynamics.default_weld_joint_def() + jdef.base.body_id_a = slots[slot] + jdef.base.body_id_b = slots[other] + jdef.base.collide_connected = false + joints[joint_count] = dynamics.create_weld_joint(world_id, &jdef) + joint_count = joint_count + 1 + } + } else { + joint = joints[stress_int(joint_count)] + if dynamics.joint_is_valid(joint) { dynamics.joint_set_collide_connected(joint, stress_int(2) == 0) } + } + } else if action == 9 { + dynamics.body_set_transform(statics[stress_int(60)], stress_position(), math.quat_identity()) + } else if action == 10 { + physics_world.rebuild_static_tree(world_id) + } + m = m + 1 + } + // Joints whose bodies were destroyed went with them. + j = 0 + while j < joint_count { + if dynamics.joint_is_valid(joints[j]) == false { + joints[j] = joints[joint_count - 1] + joint_count = joint_count - 1 + } else { + j = j + 1 + } + } + // The swarm kept in the clutter. + i = 0 + while i < 64 { + if active[i] != 0 && dynamics.body_is_enabled(slots[i]) { + d = math.sub(dynamics.body_get_position(slots[i]), stress_origin()) + if math.abs_float(d.x) > 24.0 || math.abs_float(d.y) > 24.0 || math.abs_float(d.z) > 24.0 { + dynamics.body_set_transform(slots[i], stress_position(), math.quat_identity()) + dynamics.body_set_linear_velocity(slots[i], stress_velocity()) + } + } + i = i + 1 + } + physics_world.step(world_id, 1.0 / 60.0, 4) + // A zero step finds the pairs and purges disjoint contacts, moving nothing. + physics_world.step(world_id, 0.0, 1) + if stress_pairs_hold(world_id, sleeping) == false { + println("physics_world: broad phase stress: pairs wrong at workers ${workers} seed ${seed} sleep ${sleeping} step ${step}") + ok = false + } + step = step + 1 + } + dynamics.destroy_world(world_id) + height_field.destroy_height_field(grid) + height_field.destroy_height_field(tile) + hull.destroy_hull(box) + hull.destroy_hull(static_box) + free(statics_block) + free(slots_block) + free(active_block) + free(joints_block) + return ok +} + +test_broad_phase_stress() { + seeds_block = calloc(3, 8) + seeds = seeds_block as long[] + seeds[0] = 12345 as long + seeds[1] = 12648430 as long + seeds[2] = 777 as long + i = 0 + while i < 3 { + ensure("broad phase stress, seed ${seeds[i]}: one worker", broad_phase_stress(1, seeds[i], false)) + ensure("broad phase stress, seed ${seeds[i]}: four workers", broad_phase_stress(4, seeds[i], false)) + ensure("broad phase stress, seed ${seeds[i]}: three workers with sleep", broad_phase_stress(3, seeds[i], true)) + i = i + 1 + } + free(seeds_block) +} + // test_broad_phase.c's StaticRebuildKeepsMarks: the broad phase keeps a // static proxy's pending pair mark in the static tree, so a full rebuild // between a static change and the next step must not wipe it. Both ways a @@ -1187,6 +1801,241 @@ test_body_queries() { free(ids_block) } +// test_body_query.c, the rest (#143): the hull-proxy overlaps, the +// compound on a turned body through both overlaps, and the mover's time +// of impact against one body in every case the reference asks. +hull_proxy(h: *HullData) -> ShapeProxy { return distance.shape_proxy(hull.hull_points(h) as ptr, h.vertex_count, 0.0) } + +overlap_count(world_id: WorldId, origin: Vec3, proxy: ShapeProxy) -> int { + counter = calloc(1, 4) + physics_world.overlap_shape(world_id, origin, proxy, shape.default_query_filter(), count_overlap, counter) + n = (counter as int[])[0] + free(counter) + return n +} + +test_overlap_proxies() { + // OverlapHullProxyEquivalence: a box baked about the target with a zero + // origin finds what the same box at the local origin finds with the + // target as origin; both miss far away. + wdef = dynamics.default_world_def() + world_id = dynamics.create_world(&wdef) + bdef = dynamics.default_body_def() + bdef.position = math.vec3(10.0, 0.0, 0.0) + body = dynamics.create_body(world_id, &bdef) + box = hull.make_box_hull(1.0, 1.0, 1.0) + sdef = dynamics.default_shape_def() + dynamics.create_hull_shape(body, &sdef, box) + physics_world.step(world_id, 1.0 / 60.0, 1) + local = hull.make_box_hull(5.0, 5.0, 5.0) + baked = hull.make_transformed_box_hull(5.0, 5.0, 5.0, identity_at(10.0, 0.0, 0.0)) + baked_hits = overlap_count(world_id, math.vec3_zero(), hull_proxy(baked)) + local_hits = overlap_count(world_id, math.vec3(10.0, 0.0, 0.0), hull_proxy(local)) + ensure("hull proxy: baked about the target, a hit (${baked_hits})", baked_hits == 1) + ensure("hull proxy: the local box at the target's origin agrees (${local_hits})", local_hits == baked_hits) + far = hull.make_transformed_box_hull(5.0, 5.0, 5.0, identity_at(100.0, 0.0, 0.0)) + baked_hits = overlap_count(world_id, math.vec3_zero(), hull_proxy(far)) + local_hits = overlap_count(world_id, math.vec3(100.0, 0.0, 0.0), hull_proxy(local)) + ensure("hull proxy: both miss far away", baked_hits == 0 && local_hits == 0) + dynamics.destroy_world(world_id) + hull.destroy_hull(box) + hull.destroy_hull(local) + hull.destroy_hull(baked) + hull.destroy_hull(far) + // OverlapHullProxyRotation: a 4 m bar along x reaches a box at x = 3; + // turned a quarter about y it lies along z and misses. + world_id = dynamics.create_world(&wdef) + bdef.position = math.vec3(3.0, 0.0, 0.0) + body = dynamics.create_body(world_id, &bdef) + cube = hull.make_box_hull(0.5, 0.5, 0.5) + dynamics.create_hull_shape(body, &sdef, cube) + physics_world.step(world_id, 1.0 / 60.0, 1) + aligned = hull.make_transformed_box_hull(4.0, 0.3, 0.3, math.transform_identity()) + turned = hull.make_transformed_box_hull(4.0, 0.3, 0.3, Transform { p: math.vec3_zero(), q: math.make_quat_from_axis_angle(math.vec3_axis_y(), 0.5 * math.PI) }) + ensure("hull proxy: the bar along x reaches the box", overlap_count(world_id, math.vec3_zero(), hull_proxy(aligned)) == 1) + ensure("hull proxy: turned along z it misses", overlap_count(world_id, math.vec3_zero(), hull_proxy(turned)) == 0) + dynamics.destroy_world(world_id) + hull.destroy_hull(cube) + hull.destroy_hull(aligned) + hull.destroy_hull(turned) + // OverlapCompoundRotatedBody: two sphere children at x = -3 and 3 on a + // body turned a quarter about z at (10, 20, 30); a point on the second + // child (swung to +y) overlaps, through the world and the body, and the + // gap between them stays empty. + world_id = dynamics.create_world(&wdef) + mat = material.default_surface_material() + spheres_block = calloc(2, sizeof(CompoundSphereDef)) + spheres = spheres_block as CompoundSphereDef[] + spheres[0] = compound.compound_sphere_def(manifold.sphere(math.vec3(0.0 - 3.0, 0.0, 0.0), 0.5), mat) + spheres[1] = compound.compound_sphere_def(manifold.sphere(math.vec3(3.0, 0.0, 0.0), 0.5), mat) + cdef = compound.compound_def() + cdef.spheres = spheres_block + cdef.sphere_count = 2 + c = compound.create_compound(&cdef) + pose = Transform { p: math.vec3(10.0, 20.0, 30.0), q: math.make_quat_from_axis_angle(math.vec3_axis_z(), 0.5 * math.PI) } + cbdef = dynamics.default_body_def() + cbdef.position = pose.p + cbdef.rotation = pose.q + holder = dynamics.create_body(world_id, &cbdef) + dynamics.create_compound_shape(holder, &sdef, c) + filter = shape.default_query_filter() + on_child = math.transform_point(pose, math.vec3(3.0, 0.0, 0.0)) + ensure("compound on a turned body: the world overlap finds the second child", overlap_count(world_id, math.vec3_zero(), point_proxy(on_child, 0.1)) == 1) + ensure("compound on a turned body: the body overlap too", physics_world.body_overlap_shape(holder, math.vec3_zero(), point_proxy(on_child, 0.1), filter, pose)) + in_gap = math.transform_point(pose, math.vec3_zero()) + ensure("compound on a turned body: the gap is empty to the world", overlap_count(world_id, math.vec3_zero(), point_proxy(in_gap, 0.25)) == 0) + ensure("compound on a turned body: and to the body", physics_world.body_overlap_shape(holder, math.vec3_zero(), point_proxy(in_gap, 0.25), filter, pose) == false) + dynamics.destroy_world(world_id) + compound.destroy_compound(c) + free(spheres_block) +} + +var g_toi_seed: long = 0 + +// The reference test's XorShift32: 24 bits over [lower, upper]. +toi_random(lower: float, upper: float) -> float { + x = g_toi_seed + x = (x ^ (x << 13)) & (4294967295 as long) + x = x ^ (x >> 17) + x = (x ^ (x << 5)) & (4294967295 as long) + g_toi_seed = x + unit = ((x >> 8) as float) / 16777216.0 + return lower + (upper - lower) * unit +} + +toi_random_vector(lower: float, upper: float) -> Vec3 { + x = toi_random(lower, upper) + y = toi_random(lower, upper) + z = toi_random(lower, upper) + return math.vec3(x, y, z) +} + +toi_random_direction() -> Vec3 { + while true { + v = toi_random_vector(0.0 - 1.0, 1.0) + len = math.length(v) + if 0.1 < len && len < 1.0 { return math.mul_sv(1.0 / len, v) } + } + return math.vec3_axis_x() +} + +test_mover_time_of_impact() { + ids_block = calloc(2, sizeof(BodyId)) + ids = ids_block as BodyId[] + filter = shape.default_query_filter() + sdef = dynamics.default_shape_def() + standing = Capsule { center1: math.vec3_zero(), center2: math.vec3(0.0, 1.0, 0.0), radius: 0.25 } + sweep = math.vec3(10.0, 0.0, 0.0) + // MoverTOIKeepsHitAfterMiss: an off-path sphere listed last does not + // erase the on-path hit. + world_id = query_world(ids) + dynamics.create_sphere_shape(ids[0], &sdef, manifold.sphere(math.vec3(5.0, 0.0, 20.0), 0.5)) + on_path = dynamics.create_sphere_shape(ids[0], &sdef, manifold.sphere(math.vec3(5.0, 0.0, 0.0), 0.5)) + toi = physics_world.body_time_of_impact_mover(ids[0], math.vec3_zero(), standing, sweep, filter, identity_at(0.0, 0.0, 0.0), identity_at(0.0, 0.0, 0.0)) + small("mover toi, a miss after the hit: the fraction", toi.fraction - 0.425, 0.01) + ensure("mover toi, a miss after the hit: the on-path sphere", same_shape(toi.shape_id, on_path)) + dynamics.destroy_world(world_id) + // MoverTOIMoverOffset: the mover and the body slid 2 m together. + world_id = query_world(ids) + box = hull.make_box_hull(0.5, 0.5, 0.5) + dynamics.create_hull_shape(ids[0], &sdef, box) + a = physics_world.body_time_of_impact_mover(ids[0], math.vec3_zero(), standing, sweep, filter, identity_at(5.0, 0.0, 0.0), identity_at(5.0, 0.0, 0.0)) + shifted = Capsule { center1: math.vec3(2.0, 0.0, 0.0), center2: math.vec3(2.0, 1.0, 0.0), radius: 0.25 } + b = physics_world.body_time_of_impact_mover(ids[0], math.vec3_zero(), shifted, sweep, filter, identity_at(7.0, 0.0, 0.0), identity_at(7.0, 0.0, 0.0)) + small("mover toi, both moved: the same fraction", b.fraction - a.fraction, 0.0001) + small("mover toi, both moved: the point moved by 2", b.point.x - a.point.x - 2.0, 0.001) + // MoverTOIFarFromOrigin: a million metres out, the same fraction and + // the point in the origin's frame. + origin = math.vec3(1000000.0, 0.0 - 2000000.0, 500000.0) + body_far = Transform { p: math.add(origin, math.vec3(5.0, 0.0, 0.0)), q: math.quat_identity() } + toi = physics_world.body_time_of_impact_mover(ids[0], origin, standing, sweep, filter, body_far, body_far) + small("mover toi, far out: the fraction", toi.fraction - 0.425, 0.01) + small("mover toi, far out: the normal", toi.normal.x + 1.0, 0.001) + small("mover toi, far out: the point in the origin's frame", (toi.point.x - origin.x) - 4.4, 0.5) + // MoverTOIFilter: a zero mask rejects every category. + no_mask = shape.default_query_filter() + no_mask.mask_bits = 0 as long + toi = physics_world.body_time_of_impact_mover(ids[0], math.vec3_zero(), standing, sweep, no_mask, identity_at(5.0, 0.0, 0.0), identity_at(5.0, 0.0, 0.0)) + small("mover toi, filtered: no hit", toi.fraction - 1.0, 0.000001) + dynamics.destroy_world(world_id) + // MoverTOIOverlapSkipsShape: the shape the mover starts in is skipped, + // the one on its path still hit. + world_id = query_world(ids) + dynamics.create_hull_shape(ids[0], &sdef, box) + offset_box = hull.make_offset_box_hull(0.5, 0.5, 0.5, math.vec3(5.0, 0.0, 0.0)) + on_box = dynamics.create_hull_shape(ids[0], &sdef, offset_box) + toi = physics_world.body_time_of_impact_mover(ids[0], math.vec3_zero(), standing, sweep, filter, identity_at(0.0, 0.0, 0.0), identity_at(0.0, 0.0, 0.0)) + small("mover toi, started inside one: the other's fraction", toi.fraction - 0.425, 0.01) + ensure("mover toi, started inside one: the other shape", same_shape(toi.shape_id, on_box)) + small("mover toi, started inside one: the normal", toi.normal.x + 1.0, 0.001) + dynamics.destroy_world(world_id) + // MoverTOIRotatingBody: a bar turning a quarter about y between the two + // transforms reaches a mover its start pose clears. + world_id = query_world(ids) + bar = hull.make_box_hull(2.0, 0.25, 0.25) + bar_shape = dynamics.create_hull_shape(ids[0], &sdef, bar) + at = math.vec3(0.0, 0.0, 2.1) + turned = Transform { p: math.vec3_zero(), q: math.make_quat_from_axis_angle(math.vec3_axis_y(), 0.5 * math.PI) } + spinning = physics_world.body_time_of_impact_mover(ids[0], at, standing, math.vec3_zero(), filter, identity_at(0.0, 0.0, 0.0), turned) + ensure("mover toi, a turning bar: a hit inside the sweep (${spinning.fraction})", 0.0 < spinning.fraction && spinning.fraction < 1.0) + still = physics_world.body_time_of_impact_mover(ids[0], at, standing, math.vec3_zero(), filter, identity_at(0.0, 0.0, 0.0), identity_at(0.0, 0.0, 0.0)) + ensure("mover toi, the bar held still: clear", math.abs_float(still.fraction - 1.0) < 0.000001 && still.shape_id.index1 == 0) + // MoverTOIMatchesSweep: 2,000 random movers, origins, translations and + // body turns; the query agrees with the raw time of impact it builds, + // any root strictly inside the sweep counting as a hit. + g_toi_seed = 2654435769 as long + points_block = calloc(2, sizeof(Vec3)) + points = points_block as Vec3[] + hits = 0 + agree = true + k = 0 + while k < 2000 { + c1 = toi_random_vector(0.0 - 0.2, 0.2) + length = toi_random(0.05, 1.5) + c2 = math.mul_add(c1, length, toi_random_direction()) + radius = toi_random(0.05, 0.5) + mover = Capsule { center1: c1, center2: c2, radius: radius } + points[0] = c1 + points[1] = c2 + o = toi_random_vector(0.0 - 3.5, 3.5) + travel = toi_random(0.0, 3.0) + translation = math.mul_sv(travel, toi_random_direction()) + axis1 = toi_random_direction() + angle1 = toi_random(0.0, math.PI) + t1 = Transform { p: math.vec3_zero(), q: math.make_quat_from_axis_angle(axis1, angle1) } + axis2 = toi_random_direction() + angle2 = toi_random(0.001, math.PI) + spin = math.make_quat_from_axis_angle(axis2, angle2) + shift = toi_random_vector(0.0 - 1.0, 1.0) + t2 = Transform { p: shift, q: math.mul_quat(spin, t1.q) } + result = physics_world.body_time_of_impact_mover(ids[0], o, mover, translation, filter, t1, t2) + xf1 = math.to_relative_transform(t1, o) + xf2 = math.to_relative_transform(t2, o) + input = TOIInput { proxy_a: hull_proxy(bar), proxy_b: distance.shape_proxy(points_block, 2, radius), + sweep_a: Sweep { local_center: math.vec3_zero(), c1: xf1.p, c2: xf2.p, q1: t1.q, q2: t2.q }, + sweep_b: Sweep { local_center: math.vec3_zero(), c1: math.vec3_zero(), c2: translation, q1: math.quat_identity(), q2: math.quat_identity() }, + max_fraction: 1.0 } + raw = distance.time_of_impact(&input) + if 0.0 < raw.fraction && raw.fraction < 1.0 { + same = math.abs_float(result.fraction - raw.fraction) < 0.000001 && same_shape(result.shape_id, bar_shape) && + math.is_normalized(result.normal) && math.length(math.sub(math.sub(result.point, o), raw.point)) < 0.0001 + if same == false { agree = false } + hits = hits + 1 + } else { + if math.abs_float(result.fraction - 1.0) >= 0.000001 || result.shape_id.index1 != 0 { agree = false } + } + k = k + 1 + } + ensure("mover toi against the raw sweep: every case agrees (${hits} hits of 2000)", agree) + ensure("mover toi against the raw sweep: some hits", hits > 0) + dynamics.destroy_world(world_id) + hull.destroy_hull(box) + hull.destroy_hull(offset_box) + hull.destroy_hull(bar) + free(points_block) + free(ids_block) +} + // --- the world queries ---------------------------------------------------------------------------------------------- count_overlap(id: ShapeId, context: ptr) -> bool { @@ -1358,6 +2207,12 @@ main() { test_def_flags() test_compound_shape_count() test_enlarged_proxy_destroyed() + test_ids() + test_overflow_color_pile() + test_set_worker_count() + test_compound_hit_events() + test_compound_contact_materials() + test_broad_phase_stress() test_restitution() test_joint_springs_and_locks() test_parallel_bookkeeping() @@ -1366,6 +2221,8 @@ main() { test_sensor_stepped() test_explosion() test_body_queries() + test_overlap_proxies() + test_mover_time_of_impact() test_world_queries() test_determinism() // The scratch stays allocated: the worlds, the masses, the sweep and query buffers, the solvers' dummies (the modules' scratch is reserved before the count). The mover's planes are each query's own now, freed with it.