diff --git a/README.md b/README.md index e4e24ad..18ba4d5 100644 --- a/README.md +++ b/README.md @@ -51,6 +51,7 @@ so a test written against the reference reads the same here. | `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` (259 checks: test_world.c whole, among it the hull database (equal hulls one world-owned copy, a caller's hull freed at once, transformed hulls baked once, every holder's release), 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.snapshot` | a world as bytes and back (the reference's world_snapshot.c): `save_world` and `load_world` keep everything a step reads (the settings, the id pools, the solver sets, bodies, shapes and their geometry once each, contacts with their manifolds and mesh caches, joints, sensors, islands, the trees and pair set, the graph's colours), so a loaded world steps on as the one it was taken from, bit for bit; a snapshot of another layout or any damage to its bytes loads no world | done, `test_snapshot.ae` (50 checks: a world of every kind of thing loaded at four moments and stepped 150 steps beside its original, the same hash and events every step; a loaded world saves to the same bytes; one worker's world loaded with four steps alike; every truncation and corruption refused, nothing leaked) | | `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, and on request (`Human.ankles`, past the reference) the feet as bones of their own on ankles, an elliptical cone through 50 degrees down, 35 up and 15 aside with 15 of roll, so a crouched figure's soles lie flat, 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` (115 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 ankles' four ranges measured through the joint and the soles flat under shins tipped 25 degrees over them) | [the reference's rain benchmark, 300 ragdolls over 400 steps, at 0.9x](bench/RESULTS.md#human) | | `aephysics` | the public API | | diff --git a/aephysics/basics/module.ae b/aephysics/basics/module.ae index dd37fbe..375b190 100644 --- a/aephysics/basics/module.ae +++ b/aephysics/basics/module.ae @@ -31,7 +31,7 @@ exports ( idpool_create, idpool_destroy, idpool_alloc, idpool_free, idpool_count, idpool_capacity, key_hash, shape_pair_key, SHAPE_POWER, CHILD_POWER, MAX_SHAPES, MAX_CHILD_SHAPES, set_create, set_destroy, set_clear, set_add, set_remove, set_contains, set_contains_hashed, - set_count, set_bytes, set_prefetch, + set_count, set_bytes, set_prefetch, SetItem, LongMap, map_create, map_destroy, map_clear, map_set, map_get, map_has, map_remove, map_count, map_bytes, hash_bytes, section_fits, ints_create, ints_destroy, ints_reserve, ints_push, ints_pop, ints_get, ints_set, ints_clear, diff --git a/aephysics/dynamics/module.ae b/aephysics/dynamics/module.ae index fa1c512..b0a0e92 100644 --- a/aephysics/dynamics/module.ae +++ b/aephysics/dynamics/module.ae @@ -88,7 +88,7 @@ exports ( body_set_sleep_threshold, body_get_sleep_threshold, body_get_shape_count, body_get_shapes, body_compute_aabb, body_set_motion_locks, body_get_motion_locks, body_get_flags, create_sphere_shape, create_capsule_shape, create_hull_shape, create_transformed_hull_shape, create_mesh_shape, create_height_field_shape, - HullEntry, world_hull_count, + HullEntry, world_hull_count, add_hull_to_database, allocate_manifolds, OwnedGeometry, adopt_geometry, world_at, destroy_solver_set, create_compound_shape, destroy_shape, shape_is_valid, get_shape_full_id, get_shape, shape_get_body, shape_get_aabb, shape_get_type, shape_get_density, shape_set_density, shape_get_filter, shape_set_filter, shape_get_surface_material, shape_set_surface_material, shape_get_material_count, shape_is_sensor, @@ -1245,6 +1245,7 @@ struct World { hull_heads: LongMap // a hull's hash to the first entry of that hash hull_free: int // the first free entry, chained through next hull_count: int // the distinct hulls held + owned_geometry: Buffer // OwnedGeometry: meshes, height fields and compounds the world was loaded with (aephysics.snapshot), freed with it sensors: Buffer // Sensor sensor_begin_events: Buffer // SensorBeginTouchEvent sensor_end_events0: Buffer // SensorEndTouchEvent, double buffered with the contacts' index @@ -1457,6 +1458,7 @@ create_world(def: *WorldDef) -> WorldId { w.hull_heads = basics.map_create(16) w.hull_free = NULL_INDEX w.hull_count = 0 + w.owned_geometry = basics.buffer_create(sizeof(OwnedGeometry), 0) 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) @@ -1597,6 +1599,13 @@ destroy_world(id: WorldId) { basics.buffer_destroy(&w.hull_entries) basics.map_destroy(&w.hull_heads) w.hull_count = 0 + i = 0 + while i < w.owned_geometry.count { + owned = (w.owned_geometry.data + i * sizeof(OwnedGeometry)) as *OwnedGeometry + basics.free_bytes(owned.block, owned.byte_count) + i = i + 1 + } + basics.buffer_destroy(&w.owned_geometry) basics.idpool_destroy(&w.shape_id_pool) basics.buffer_destroy(&w.contacts) basics.idpool_destroy(&w.contact_id_pool) @@ -2281,6 +2290,20 @@ remove_hull_from_database(w: *World, h: *HullData) { } } +// A block of geometry (a mesh's, a height field's, a compound's) the world +// owns: the copies a loaded snapshot's shapes point at, freed with the world. +struct OwnedGeometry { + block: ptr + byte_count: int +} + +adopt_geometry(w: *World, block: ptr, byte_count: int) { + index = basics.buffer_push(&w.owned_geometry) + owned = (w.owned_geometry.data + index * sizeof(OwnedGeometry)) as *OwnedGeometry + owned.block = block + owned.byte_count = byte_count +} + // The distinct hulls a world holds for its shapes. world_hull_count(world_id: WorldId) -> int { if world_is_valid(world_id) == false { return 0 } diff --git a/aephysics/height_field/module.ae b/aephysics/height_field/module.ae index 677a155..ac1815c 100644 --- a/aephysics/height_field/module.ae +++ b/aephysics/height_field/module.ae @@ -33,7 +33,7 @@ exports ( HEIGHT_FIELD_VERSION, HEIGHT_FIELD_HOLE, NULL_INDEX, height_field_def, height_field_quanta, height_field_quantum, height_field_material_indices, height_field_flags, height_field_triangle_count, height_field_height, - create_height_field, destroy_height_field, create_grid, create_wave, + create_height_field, destroy_height_field, height_field_bytes_valid, create_grid, create_wave, get_height_field_triangle, get_height_field_material, compute_height_field_aabb, ray_cast_height_field, shape_cast_height_field, overlap_height_field, query_height_field, collide_mover_and_height_field @@ -302,6 +302,25 @@ create_height_field(def: *HeightFieldDef) -> *HeightFieldData { destroy_height_field(h: *HeightFieldData) { basics.free_bytes(h as ptr, h.byte_count) } +// A height field in bytes from outside (a snapshot's, a recording's), at +// most `available` of them: the layout's version, its block inside the +// bytes, the grid two lines or more each way, a scale positive and finite, +// a quantisation range in order, and its three arrays inside its block. +height_field_bytes_valid(h: *HeightFieldData, available: int) -> bool { + if h == null || available < sizeof(HeightFieldData) { return false } + if h.version != HEIGHT_FIELD_VERSION { return false } + n = h.byte_count + if n < sizeof(HeightFieldData) || n > available { return false } + if h.column_count < 2 || h.row_count < 2 || h.column_count > n / 2 || h.row_count > (n / 2) / h.column_count { return false } + if math.is_valid_vec3(h.scale) == false || h.scale.x <= 0.0 || h.scale.y <= 0.0 || h.scale.z <= 0.0 { return false } + if math.is_valid_float(h.min_height) == false || math.is_valid_float(h.max_height) == false || h.min_height > h.max_height { return false } + if math.is_valid_float(h.height_scale) == false || h.height_scale <= 0.0 { return false } + cells = (h.column_count - 1) * (h.row_count - 1) + if basics.section_fits(h.heights_offset, h.column_count * h.row_count, 2, n) == false { return false } + if basics.section_fits(h.material_offset, cells, 1, n) == false { return false } + return basics.section_fits(h.flags_offset, 2 * cells, 1, n) +} + // A flat field, with a hole every sixteenth cell when asked. create_grid(row_count: int, column_count: int, scale: Vec3, make_holes: bool) -> *HeightFieldData { height_count = row_count * column_count diff --git a/aephysics/snapshot/module.ae b/aephysics/snapshot/module.ae new file mode 100644 index 0000000..31965b3 --- /dev/null +++ b/aephysics/snapshot/module.ae @@ -0,0 +1,945 @@ +// A world as bytes and back (Box3D's world_snapshot.c, #149): everything a +// step reads, so a world loaded from a snapshot steps on exactly as the +// world it was taken from. The settings, the id pools, the solver sets +// with their sims, the bodies, the shapes with their geometry (each block +// once, however many shapes share it), the fat boxes, the contacts with +// their manifolds and mesh caches, the joints, the sensors, the islands, +// the three trees and the pair set, and the constraint graph's colours. +// What a step builds fresh each time (its events, its scratch, the +// solver's constraints) is not kept, nor is what the host wires in (user +// data, callbacks, the worker count), which the caller's WorldDef gives +// the loaded world. +// +// A snapshot is this build's: its layout tag folds in the sizes of every +// record it copies whole and the geometry's versions, so one from another +// layout is refused rather than misread. Every count is checked against +// the bytes before anything is copied, every geometry block is checked as +// bytes from outside are (hull_bytes_valid and its kin), every pointer a +// record carried is cleared before the record is used, and the loaded +// world must pass the world's own validation; a snapshot that fails any +// of it loads no world. + +import std.string +import std.mem +import aephysics.math +import aephysics.native +import aephysics.basics +import aephysics.dynamic_tree +import aephysics.hull +import aephysics.mesh +import aephysics.height_field +import aephysics.compound +import aephysics.shape +import aephysics.broad_phase +import aephysics.mesh_contact +import aephysics.dynamics + +exports ( + SNAPSHOT_MAGIC, SNAPSHOT_VERSION, snapshot_layout, save_world, load_world, free_snapshot +) + +extern memcpy(dst: ptr, src: ptr, size: int) -> ptr +extern memset(block: ptr, value: int, size: int) -> ptr +extern strlen(s: ptr) -> int +extern malloc(size: int) -> ptr + +const SNAPSHOT_MAGIC = 0x4E535041 // "APSN", little-endian +const SNAPSHOT_VERSION = 1 +const NULL_INDEX = 0 - 1 + +// --- the bytes written ------------------------------------------------------------------------------- + +struct SnapWriter { + data: ptr + size: int + capacity: int +} + +writer_grow(wr: *SnapWriter, n: int) { + if wr.size + n <= wr.capacity { return } + capacity = math.max_int(math.max_int(2 * wr.capacity, wr.size + n), 4096) + block = basics.alloc(capacity) + if wr.data != null { + memcpy(block, wr.data, wr.size) + basics.free_bytes(wr.data, wr.capacity) + } + wr.data = block + wr.capacity = capacity +} + +w_bytes(wr: *SnapWriter, src: ptr, n: int) { + if n <= 0 { return } + writer_grow(wr, n) + memcpy(wr.data + wr.size, src, n) + wr.size = wr.size + n +} + +w_int(wr: *SnapWriter, v: int) { + holder = v + w_bytes(wr, (&holder) as ptr, 4) +} + +w_long(wr: *SnapWriter, v: long) { + holder = v + w_bytes(wr, (&holder) as ptr, 8) +} + +w_float(wr: *SnapWriter, v: float) { + holder = v + w_bytes(wr, (&holder) as ptr, 8) +} + +w_bool(wr: *SnapWriter, v: bool) { + if v { w_int(wr, 1) } else { w_int(wr, 0) } +} + +w_vec3(wr: *SnapWriter, v: Vec3) { + w_float(wr, v.x) + w_float(wr, v.y) + w_float(wr, v.z) +} + +// A buffer: its stride, its count, its items. +w_buffer(wr: *SnapWriter, b: *Buffer) { + w_int(wr, b.stride) + w_int(wr, b.count) + w_bytes(wr, b.data, b.count * b.stride) +} + +w_ints(wr: *SnapWriter, a: *Ints) { + w_int(wr, a.count) + w_bytes(wr, a.data, a.count * 4) +} + +w_bitset(wr: *SnapWriter, b: *BitSet) { + w_int(wr, b.block_count) + w_bytes(wr, b.bits, b.block_count * 8) +} + +w_idpool(wr: *SnapWriter, p: *IdPool) { + w_int(wr, p.next_index) + w_ints(wr, &p.free_ids) +} + +// A name: its length and its bytes, or -1 for none. +w_name(wr: *SnapWriter, name: ptr) { + if name == null { + w_int(wr, NULL_INDEX) + return + } + n = strlen(name) + w_int(wr, n) + w_bytes(wr, name, n) +} + +// --- the bytes read ---------------------------------------------------------------------------------- + +struct SnapReader { + data: ptr + size: int + at: int + ok: bool +} + +r_need(rd: *SnapReader, n: int) -> bool { + if rd.ok == false { return false } + if n < 0 || n > rd.size - rd.at { + rd.ok = false + return false + } + return true +} + +r_bytes(rd: *SnapReader, dst: ptr, n: int) { + if r_need(rd, n) == false { return } + if n > 0 { memcpy(dst, rd.data + rd.at, n) } + rd.at = rd.at + n +} + +r_int(rd: *SnapReader) -> int { + holder = 0 + r_bytes(rd, (&holder) as ptr, 4) + return holder +} + +r_long(rd: *SnapReader) -> long { + holder = 0 as long + r_bytes(rd, (&holder) as ptr, 8) + return holder +} + +r_float(rd: *SnapReader) -> float { + holder = 0.0 + r_bytes(rd, (&holder) as ptr, 8) + return holder +} + +r_bool(rd: *SnapReader) -> bool { + v = r_int(rd) + if v != 0 && v != 1 { rd.ok = false } + return v == 1 +} + +r_vec3(rd: *SnapReader) -> Vec3 { + x = r_float(rd) + y = r_float(rd) + z = r_float(rd) + return math.vec3(x, y, z) +} + +// A count the bytes left can hold, `size` bytes each, or the reader failed. +r_count(rd: *SnapReader, size: int) -> int { + count = r_int(rd) + if rd.ok == false { return 0 } + if count < 0 || basics.section_fits(rd.at, count, size, rd.size) == false { + rd.ok = false + return 0 + } + return count +} + +// A buffer of `stride`-byte items; an empty one when the bytes are not one. +r_buffer(rd: *SnapReader, stride: int) -> Buffer { + s = r_int(rd) + if rd.ok && s != stride { rd.ok = false } + count = r_count(rd, stride) + if rd.ok == false { return basics.buffer_create(stride, 0) } + b = basics.buffer_create(stride, count) + r_bytes(rd, b.data, count * stride) + b.count = count + return b +} + +r_ints(rd: *SnapReader) -> Ints { + count = r_count(rd, 4) + if rd.ok == false { return basics.ints_create(0) } + a = basics.ints_create(count) + r_bytes(rd, a.data, count * 4) + a.count = count + return a +} + +r_bitset(rd: *SnapReader) -> BitSet { + count = r_count(rd, 8) + b = basics.bitset_create(64 * math.max_int(count, 1)) + if rd.ok == false { return b } + r_bytes(rd, b.bits, count * 8) + b.block_count = count + return b +} + +r_idpool(rd: *SnapReader) -> IdPool { + next_index = r_int(rd) + free_ids = r_ints(rd) + if next_index < 0 { rd.ok = false } + return IdPool { free_ids: free_ids, next_index: next_index } +} + +// A name as a C copy (name_free's), or null. +r_name(rd: *SnapReader) -> ptr { + n = r_int(rd) + if rd.ok == false || n == NULL_INDEX { return null } + if r_need(rd, n) == false { return null } + block = malloc(n + 1) + memcpy(block, rd.data + rd.at, n) + memset(block + n, 0, 1) + rd.at = rd.at + n + return block +} + +// --- the layout -------------------------------------------------------------------------------------- + +// This build's layout: the size of every record copied whole and the +// versions of the geometry and saved trees, folded by FNV-1a. A snapshot +// carries it, and one of another layout is refused. +snapshot_layout() -> long { + hash = 0 - 3750763034362895579 + prime = 1099511628211 as long + sizes_block = basics.alloc(24 * 8) + sizes = sizes_block as long[] + sizes[0] = sizeof(Body) as long + sizes[1] = sizeof(WorldShape) as long + sizes[2] = sizeof(Contact) as long + sizes[3] = sizeof(Joint) as long + sizes[4] = sizeof(BodySim) as long + sizes[5] = sizeof(BodyState) as long + sizes[6] = sizeof(JointSim) as long + sizes[7] = sizeof(IslandSim) as long + sizes[8] = sizeof(ContactLink) as long + sizes[9] = sizeof(JointLink) as long + sizes[10] = sizeof(ContactSpec) as long + sizes[11] = sizeof(Manifold) as long + sizes[12] = sizeof(DynamicTreeNode) as long + sizes[13] = sizeof(TreeProxy) as long + sizes[14] = sizeof(TriangleCache) as long + sizes[15] = sizeof(SurfaceMaterial) as long + sizes[16] = sizeof(AABBf) as long + sizes[17] = sizeof(SetItem) as long + sizes[18] = hull.HULL_VERSION as long + sizes[19] = mesh.MESH_VERSION as long + sizes[20] = height_field.HEIGHT_FIELD_VERSION as long + sizes[21] = compound.COMPOUND_VERSION as long + sizes[22] = dynamic_tree.tree_save_version() + sizes[23] = dynamics.GRAPH_COLOR_COUNT as long + i = 0 + while i < 24 { + hash = (hash ^ sizes[i]) * prime + i = i + 1 + } + basics.free_bytes(sizes_block, 24 * 8) + return hash +} + +// --- the geometry, each block once -------------------------------------------------------------------- + +struct GeometryEntry { + kind: int + block: ptr + byte_count: int +} + +struct GeometryTable { + map: LongMap // a block's address to its entry + entries: Buffer // GeometryEntry +} + +geometry_entry(t: *GeometryTable, index: int) -> *GeometryEntry { return (t.entries.data + index * sizeof(GeometryEntry)) as *GeometryEntry } + +// The shape's geometry block, with its kind and length, or NULL_INDEX for a +// shape that holds none (a sphere, a capsule). +shape_geometry_index(t: *GeometryTable, s: *WorldShape) -> int { + kind = s.geometry.kind + if kind == shape.SHAPE_HULL { return table_index(t, kind, s.geometry.hull as ptr, s.geometry.hull.byte_count) } + if kind == shape.SHAPE_MESH { return table_index(t, kind, s.geometry.mesh.data as ptr, s.geometry.mesh.data.byte_count) } + if kind == shape.SHAPE_HEIGHT_FIELD { return table_index(t, kind, s.geometry.height_field as ptr, s.geometry.height_field.byte_count) } + if kind == shape.SHAPE_COMPOUND { return table_index(t, kind, s.geometry.compound as ptr, s.geometry.compound.byte_count) } + return NULL_INDEX +} + +table_index(t: *GeometryTable, kind: int, block: ptr, byte_count: int) -> int { + if block == null { return NULL_INDEX } + key = mem.ptr_to_long(block) + found = basics.map_get(&t.map, key, NULL_INDEX) + if found != NULL_INDEX { return found } + index = basics.buffer_push(&t.entries) + entry = geometry_entry(t, index) + entry.kind = kind + entry.block = block + entry.byte_count = byte_count + basics.map_set(&t.map, key, index) + return index +} + +// --- saving ------------------------------------------------------------------------------------------ + +// The world as one block of bytes and its length, the block the caller's +// to free (free_snapshot); null and 0 for a world that is not one or is +// mid-step. +save_world(world_id: WorldId) -> (ptr, int) { + if dynamics.world_is_valid(world_id) == false { return null, 0 } + w = dynamics.get_world(world_id.index1 - 1) + if w.locked { return null, 0 } + wr = SnapWriter { data: null, size: 0, capacity: 0 } + w_int(&wr, SNAPSHOT_MAGIC) + w_int(&wr, SNAPSHOT_VERSION) + w_long(&wr, snapshot_layout()) + + // The settings and the scalars a step carries over. + w_vec3(&wr, w.gravity) + w_float(&wr, w.hit_event_threshold) + w_float(&wr, w.restitution_threshold) + w_int(&wr, w.restitution_iterations) + w_bool(&wr, w.enable_restitution_propagation) + w_bool(&wr, w.enable_kinematic_static_softness) + w_float(&wr, w.max_linear_speed) + w_float(&wr, w.contact_speed) + w_float(&wr, w.contact_hertz) + w_float(&wr, w.contact_damping_ratio) + w_float(&wr, w.contact_recycle_distance) + w_bool(&wr, w.enable_sleep) + w_bool(&wr, w.enable_warm_starting) + w_bool(&wr, w.enable_continuous) + w_bool(&wr, w.enable_speculative) + w_long(&wr, w.step_index) + w_int(&wr, w.split_island_id) + w_int(&wr, w.split_island_candidate) + w_float(&wr, w.split_sleep_time) + w_int(&wr, w.compound_shape_count) + w_float(&wr, w.inv_h) + w_float(&wr, w.inv_dt) + + w_idpool(&wr, &w.body_id_pool) + w_idpool(&wr, &w.solver_set_id_pool) + w_idpool(&wr, &w.joint_id_pool) + w_idpool(&wr, &w.contact_id_pool) + w_idpool(&wr, &w.island_id_pool) + w_idpool(&wr, &w.shape_id_pool) + + // The solver sets; a free one is its index alone. + w_int(&wr, w.solver_sets.count) + i = 0 + while i < w.solver_sets.count { + set = dynamics.solver_set_at(w, i) + w_int(&wr, set.set_index) + if set.set_index != NULL_INDEX { + w_buffer(&wr, &set.body_sims) + w_buffer(&wr, &set.body_states) + w_buffer(&wr, &set.joint_sims) + w_ints(&wr, &set.contact_indices) + w_buffer(&wr, &set.island_sims) + } + i = i + 1 + } + + // The bodies, their pointers cleared in the copy, each name after. + w_int(&wr, w.bodies.count) + i = 0 + while i < w.bodies.count { + b = dynamics.body_at(w, i) + start = wr.size + w_bytes(&wr, b as ptr, sizeof(Body)) + body_copy = (wr.data + start) as *Body + body_copy.name = null + body_copy.user_data = null + w_name(&wr, b.name) + i = i + 1 + } + + // The geometry the live shapes hold, each block once. + table = GeometryTable { map: basics.map_create(16), entries: basics.buffer_create(sizeof(GeometryEntry), 0) } + indices_block = basics.alloc(math.max_int(w.shapes.count, 1) * 4) + indices = indices_block as int[] + i = 0 + while i < w.shapes.count { + s = dynamics.world_shape_at(w, i) + indices[i] = NULL_INDEX + if s.id != NULL_INDEX { indices[i] = shape_geometry_index(&table, s) } + i = i + 1 + } + w_int(&wr, table.entries.count) + i = 0 + while i < table.entries.count { + entry = geometry_entry(&table, i) + w_int(&wr, entry.kind) + w_int(&wr, entry.byte_count) + start = wr.size + w_bytes(&wr, entry.block, entry.byte_count) + if entry.kind == shape.SHAPE_COMPOUND { + // The copy's tree pointers cleared, as convert_compound_to_bytes clears them. + compound_copy = (wr.data + start) as *CompoundData + compound_copy.tree.nodes = null + compound_copy.tree.proxies = null + compound_copy.tree.stack = null + compound_copy.tree.stack_dist = null + } + i = i + 1 + } + + // The shapes, their pointers cleared in the copy; then each one's + // geometry entry, its materials and its name. + w_int(&wr, w.shapes.count) + i = 0 + while i < w.shapes.count { + s = dynamics.world_shape_at(w, i) + start = wr.size + w_bytes(&wr, s as ptr, sizeof(WorldShape)) + shape_copy = (wr.data + start) as *WorldShape + shape_copy.geometry.hull = null + shape_copy.geometry.mesh.data = null + shape_copy.geometry.height_field = null + shape_copy.geometry.compound = null + shape_copy.materials = null + shape_copy.user_data = null + shape_copy.name = null + w_int(&wr, indices[i]) + if s.id != NULL_INDEX && s.materials != null { + w_int(&wr, s.material_count) + w_bytes(&wr, s.materials, s.material_count * sizeof(SurfaceMaterial)) + } else { + w_int(&wr, 0) + } + if s.id != NULL_INDEX { w_name(&wr, s.name) } else { w_name(&wr, null) } + i = i + 1 + } + basics.free_bytes(indices_block, math.max_int(w.shapes.count, 1) * 4) + basics.map_destroy(&table.map) + basics.buffer_destroy(&table.entries) + + w_buffer(&wr, &w.fat_aabbs) + + // The contacts: the record with its pointers cleared, then a live + // contact's manifolds and, for a mesh contact, its triangles. + w_int(&wr, w.contacts.count) + i = 0 + while i < w.contacts.count { + c = dynamics.contact_at(w, i) + start = wr.size + w_bytes(&wr, c as ptr, sizeof(Contact)) + contact_copy = (wr.data + start) as *Contact + contact_copy.manifolds = null + is_mesh = (c.flags & dynamics.SIM_MESH_CONTACT) != 0 + if is_mesh { + // The copy says what a load makes of it: the triangles without + // their spare room. + copy_cache = dynamics.mesh_cache_of(contact_copy) + copy_cache.triangles = null + copy_cache.capacity = copy_cache.count + } + if c.contact_id != NULL_INDEX { + w_bytes(&wr, c.manifolds, c.manifold_count * sizeof(Manifold)) + if is_mesh { + cache = dynamics.mesh_cache_of(c) + w_int(&wr, cache.count) + w_bytes(&wr, cache.triangles, cache.count * sizeof(TriangleCache)) + } + } + i = i + 1 + } + + w_int(&wr, w.joints.count) + i = 0 + while i < w.joints.count { + start = wr.size + w_bytes(&wr, dynamics.joint_at(w, i) as ptr, sizeof(Joint)) + joint_copy = (wr.data + start) as *Joint + joint_copy.user_data = null + i = i + 1 + } + + w_int(&wr, w.sensors.count) + i = 0 + while i < w.sensors.count { + sensor = dynamics.sensor_at(w, i) + w_int(&wr, sensor.shape_id) + w_buffer(&wr, &sensor.hits) + w_buffer(&wr, &sensor.overlaps1) + w_buffer(&wr, &sensor.overlaps2) + i = i + 1 + } + + // The islands; a free one is its scalars alone. + w_int(&wr, w.islands.count) + i = 0 + while i < w.islands.count { + island = dynamics.island_at(w, i) + w_int(&wr, island.set_index) + w_int(&wr, island.local_index) + w_int(&wr, island.island_id) + w_int(&wr, island.constraint_remove_count) + if island.island_id != NULL_INDEX { + w_ints(&wr, &island.bodies) + w_buffer(&wr, &island.contacts) + w_buffer(&wr, &island.joints) + } + i = i + 1 + } + + // The trees as dynamic_tree saves them, and the pair set whole (its + // slots in place, so a walk over it meets the pairs in the same order). + t = 0 + while t < broad_phase.BODY_TYPE_COUNT { + block, byte_count = dynamic_tree.tree_save_bytes(broad_phase.broad_phase_tree(&w.broad_phase, t)) + w_int(&wr, byte_count) + w_bytes(&wr, block, byte_count) + if block != null { basics.free_bytes(block, byte_count) } + t = t + 1 + } + w_int(&wr, w.broad_phase.pair_set.capacity) + w_int(&wr, w.broad_phase.pair_set.count) + w_bytes(&wr, w.broad_phase.pair_set.items, w.broad_phase.pair_set.capacity * sizeof(SetItem)) + + // The colours: each one's bodies (but the overflow's), joints and contacts. + k = 0 + while k < dynamics.GRAPH_COLOR_COUNT { + color = graph_color(w, k) + if k < dynamics.OVERFLOW_INDEX { w_bitset(&wr, &color.body_set) } + w_buffer(&wr, &color.joint_sims) + w_ints(&wr, &color.convex_contacts) + w_buffer(&wr, &color.contacts) + k = k + 1 + } + + // The block trimmed to its length, so the caller frees what it holds. + exact = basics.alloc(wr.size) + memcpy(exact, wr.data, wr.size) + basics.free_bytes(wr.data, wr.capacity) + return exact, wr.size +} + +graph_color(w: *World, k: int) -> *GraphColor { return (w.constraint_graph.colors + k * sizeof(GraphColor)) as *GraphColor } + +// A snapshot block save_world gave. +free_snapshot(block: ptr, byte_count: int) { + if block != null { basics.free_bytes(block, byte_count) } +} + +// --- loading ----------------------------------------------------------------------------------------- + +// A world made from `def` (its workers, callbacks and user data; the +// snapshot gives everything else) and filled from the snapshot in `size` +// bytes; a null id when the bytes are not a snapshot of this layout or any +// part of them fails its checks, in which case nothing is left behind. +load_world(def: *WorldDef, data: ptr, size: int) -> WorldId { + null_id = WorldId { index1: 0, generation: 0 } + if data == null || size < 16 { return null_id } + rd = SnapReader { data: data, size: size, at: 0, ok: true } + if r_int(&rd) != SNAPSHOT_MAGIC || r_int(&rd) != SNAPSHOT_VERSION || r_long(&rd) != snapshot_layout() { return null_id } + world_id = dynamics.create_world(def) + if dynamics.world_is_valid(world_id) == false { return null_id } + w = dynamics.get_world(world_id.index1 - 1) + if fill_world(w, &rd) == false || rd.at != rd.size || world_valid(w) == false { + dynamics.destroy_world(world_id) + return null_id + } + return world_id +} + +// The world's own invariants over everything loaded. +world_valid(w: *World) -> bool { + if dynamics.validate_solver_sets(w) == false { return false } + if dynamics.validate_contacts(w) == false { return false } + if dynamics.validate_joints(w) == false { return false } + return broad_phase.validate_broad_phase(&w.broad_phase) +} + +// Every section into the fresh world. The world stays one destroy_world can +// free at every point: a container is replaced only by a whole one, and a +// record's pointers are cleared before it joins its container. +fill_world(w: *World, rd: *SnapReader) -> bool { + w.gravity = r_vec3(rd) + w.hit_event_threshold = r_float(rd) + w.restitution_threshold = r_float(rd) + w.restitution_iterations = r_int(rd) + w.enable_restitution_propagation = r_bool(rd) + w.enable_kinematic_static_softness = r_bool(rd) + w.max_linear_speed = r_float(rd) + w.contact_speed = r_float(rd) + w.contact_hertz = r_float(rd) + w.contact_damping_ratio = r_float(rd) + w.contact_recycle_distance = r_float(rd) + w.enable_sleep = r_bool(rd) + w.enable_warm_starting = r_bool(rd) + w.enable_continuous = r_bool(rd) + w.enable_speculative = r_bool(rd) + w.step_index = r_long(rd) + w.split_island_id = r_int(rd) + w.split_island_candidate = r_int(rd) + w.split_sleep_time = r_float(rd) + w.compound_shape_count = r_int(rd) + w.inv_h = r_float(rd) + w.inv_dt = r_float(rd) + if rd.ok == false { return false } + if w.restitution_iterations < 0 || w.restitution_iterations > dynamics.MAX_RESTITUTION_ITERATIONS { return false } + + // The fresh world's solver sets and pools give way first, then each + // pool and set is read whole. + i = 0 + while i < w.solver_sets.count { + if dynamics.solver_set_at(w, i).set_index != NULL_INDEX { dynamics.destroy_solver_set(w, i) } + i = i + 1 + } + w.solver_sets.count = 0 + replace_pool(&w.body_id_pool, r_idpool(rd)) + replace_pool(&w.solver_set_id_pool, r_idpool(rd)) + replace_pool(&w.joint_id_pool, r_idpool(rd)) + replace_pool(&w.contact_id_pool, r_idpool(rd)) + replace_pool(&w.island_id_pool, r_idpool(rd)) + replace_pool(&w.shape_id_pool, r_idpool(rd)) + if rd.ok == false { return false } + + set_count = r_count(rd, 4) + i = 0 + while i < set_count && rd.ok { + set_index = r_int(rd) + index = basics.buffer_push(&w.solver_sets) + set = dynamics.solver_set_at(w, index) + memset(set as ptr, 0, sizeof(SolverSet)) + set.set_index = NULL_INDEX + if set_index != NULL_INDEX && rd.ok { + set.body_sims = r_buffer(rd, sizeof(BodySim)) + set.body_states = r_buffer(rd, sizeof(BodyState)) + set.joint_sims = r_buffer(rd, sizeof(JointSim)) + set.contact_indices = r_ints(rd) + set.island_sims = r_buffer(rd, sizeof(IslandSim)) + set.set_index = set_index + if set_index != i { rd.ok = false } + } + i = i + 1 + } + if rd.ok == false || w.solver_sets.count < 3 { return false } + + // The bodies: each record with its pointers cleared, then its name. + body_count = r_count(rd, sizeof(Body)) + basics.buffer_destroy(&w.bodies) + w.bodies = basics.buffer_create(sizeof(Body), math.max_int(body_count, 16)) + i = 0 + while i < body_count && rd.ok { + index = basics.buffer_push(&w.bodies) + b = dynamics.body_at(w, index) + r_bytes(rd, b as ptr, sizeof(Body)) + b.name = null + b.user_data = null + b.name = r_name(rd) + i = i + 1 + } + if rd.ok == false { return false } + + // The geometry, each block copied into the world and checked as bytes + // from outside are: a hull goes into the hull database when a shape + // takes it, a mesh, height field or compound becomes the world's own. + geometry_count = r_count(rd, 8) + kinds_block = basics.alloc(math.max_int(geometry_count, 1) * 4) + kinds = kinds_block as int[] + blocks_block = basics.alloc(math.max_int(geometry_count, 1) * 8) + blocks = blocks_block as ptr[] + loaded = 0 + while loaded < geometry_count && rd.ok { + kind = r_int(rd) + byte_count = r_int(rd) + if byte_count <= 0 || r_need(rd, byte_count) == false { + rd.ok = false + break + } + block = basics.alloc(byte_count) + memcpy(block, rd.data + rd.at, byte_count) + rd.at = rd.at + byte_count + valid = false + if kind == shape.SHAPE_HULL { valid = hull.hull_bytes_valid(block as *HullData, byte_count) && (block as *HullData).byte_count == byte_count } + else if kind == shape.SHAPE_MESH { valid = mesh.mesh_bytes_valid(block as *MeshData, byte_count) && (block as *MeshData).byte_count == byte_count } + else if kind == shape.SHAPE_HEIGHT_FIELD { valid = height_field.height_field_bytes_valid(block as *HeightFieldData, byte_count) && (block as *HeightFieldData).byte_count == byte_count } + else if kind == shape.SHAPE_COMPOUND { valid = compound.convert_bytes_to_compound(block, byte_count) != null } + if valid == false { + basics.free_bytes(block, byte_count) + rd.ok = false + break + } + // The world owns all but the hulls, which the database copies. + if kind != shape.SHAPE_HULL { dynamics.adopt_geometry(w, block, byte_count) } + kinds[loaded] = kind + blocks[loaded] = block + loaded = loaded + 1 + } + + // The shapes: each record with its pointers cleared, then its geometry, + // materials and name. + shape_count = r_count(rd, sizeof(WorldShape)) + if rd.ok { + basics.buffer_destroy(&w.shapes) + w.shapes = basics.buffer_create(sizeof(WorldShape), math.max_int(shape_count, 16)) + } + i = 0 + while i < shape_count && rd.ok { + index = basics.buffer_push(&w.shapes) + s = dynamics.world_shape_at(w, index) + r_bytes(rd, s as ptr, sizeof(WorldShape)) + s.geometry.hull = null + s.geometry.mesh.data = null + s.geometry.height_field = null + s.geometry.compound = null + s.materials = null + s.user_data = null + s.name = null + geometry_index = r_int(rd) + material_count = r_count(rd, sizeof(SurfaceMaterial)) + if rd.ok == false { break } + if s.id != NULL_INDEX { + kind = s.geometry.kind + needs_block = kind == shape.SHAPE_HULL || kind == shape.SHAPE_MESH || kind == shape.SHAPE_HEIGHT_FIELD || kind == shape.SHAPE_COMPOUND + if kind < 0 || kind >= shape.SHAPE_KIND_COUNT { rd.ok = false } + else if needs_block { + if geometry_index < 0 || geometry_index >= loaded || kinds[geometry_index] != kind { rd.ok = false } + else if kind == shape.SHAPE_HULL { s.geometry.hull = dynamics.add_hull_to_database(w, blocks[geometry_index] as *HullData) } + else if kind == shape.SHAPE_MESH { s.geometry.mesh.data = blocks[geometry_index] as *MeshData } + else if kind == shape.SHAPE_HEIGHT_FIELD { s.geometry.height_field = blocks[geometry_index] as *HeightFieldData } + else { s.geometry.compound = blocks[geometry_index] as *CompoundData } + } else if geometry_index != NULL_INDEX { rd.ok = false } + } else if geometry_index != NULL_INDEX { rd.ok = false } + if material_count > 0 && rd.ok { + if s.id == NULL_INDEX || material_count != s.material_count { rd.ok = false } + else { + s.materials = basics.alloc(material_count * sizeof(SurfaceMaterial)) + r_bytes(rd, s.materials, material_count * sizeof(SurfaceMaterial)) + } + } + s.name = r_name(rd) + i = i + 1 + } + // The hulls' copies were only for the database to take its own from. + k = 0 + while k < loaded { + if kinds[k] == shape.SHAPE_HULL { hull.destroy_hull(blocks[k] as *HullData) } + k = k + 1 + } + basics.free_bytes(kinds_block, math.max_int(geometry_count, 1) * 4) + basics.free_bytes(blocks_block, math.max_int(geometry_count, 1) * 8) + if rd.ok == false { return false } + + fat = r_buffer(rd, sizeof(AABBf)) + basics.buffer_destroy(&w.fat_aabbs) + w.fat_aabbs = fat + if rd.ok == false || w.fat_aabbs.count != w.shapes.count { return false } + + // The contacts: each record with its manifolds and mesh cache cleared, + // then the live ones' manifolds into the world's allocators. + contact_count = r_count(rd, sizeof(Contact)) + if rd.ok { + basics.buffer_destroy(&w.contacts) + w.contacts = basics.buffer_create(sizeof(Contact), math.max_int(contact_count, 16)) + } + i = 0 + while i < contact_count && rd.ok { + index = basics.buffer_push(&w.contacts) + c = dynamics.contact_at(w, index) + r_bytes(rd, c as ptr, sizeof(Contact)) + manifold_count = c.manifold_count + c.manifolds = null + c.manifold_count = 0 + is_mesh = (c.flags & dynamics.SIM_MESH_CONTACT) != 0 + if is_mesh { + cache = dynamics.mesh_cache_of(c) + cache.triangles = null + cache.count = 0 + cache.capacity = 0 + } + if c.contact_id != NULL_INDEX { + if manifold_count < 0 || basics.section_fits(rd.at, manifold_count, sizeof(Manifold), rd.size) == false { + rd.ok = false + break + } + if manifold_count > 0 { + dynamics.allocate_manifolds(w, c, manifold_count) + r_bytes(rd, c.manifolds, manifold_count * sizeof(Manifold)) + } + if is_mesh { + triangle_count = r_count(rd, sizeof(TriangleCache)) + if triangle_count > 0 && rd.ok { + cache = dynamics.mesh_cache_of(c) + cache.triangles = basics.alloc(triangle_count * sizeof(TriangleCache)) + cache.capacity = triangle_count + r_bytes(rd, cache.triangles, triangle_count * sizeof(TriangleCache)) + cache.count = triangle_count + } + } + } else if manifold_count != 0 { rd.ok = false } + i = i + 1 + } + if rd.ok == false { return false } + + joint_count = r_count(rd, sizeof(Joint)) + basics.buffer_destroy(&w.joints) + w.joints = basics.buffer_create(sizeof(Joint), math.max_int(joint_count, 16)) + i = 0 + while i < joint_count && rd.ok { + index = basics.buffer_push(&w.joints) + j = dynamics.joint_at(w, index) + r_bytes(rd, j as ptr, sizeof(Joint)) + j.user_data = null + i = i + 1 + } + if rd.ok == false { return false } + + sensor_count = r_count(rd, 4) + i = 0 + while i < w.sensors.count { + sensor = dynamics.sensor_at(w, i) + basics.buffer_destroy(&sensor.hits) + basics.buffer_destroy(&sensor.overlaps1) + basics.buffer_destroy(&sensor.overlaps2) + i = i + 1 + } + w.sensors.count = 0 + i = 0 + while i < sensor_count && rd.ok { + index = basics.buffer_push(&w.sensors) + sensor = dynamics.sensor_at(w, index) + memset(sensor as ptr, 0, sizeof(Sensor)) + sensor.shape_id = r_int(rd) + sensor.hits = r_buffer(rd, 8) + sensor.overlaps1 = r_buffer(rd, 8) + sensor.overlaps2 = r_buffer(rd, 8) + i = i + 1 + } + if rd.ok == false { return false } + + island_count = r_count(rd, 16) + basics.buffer_destroy(&w.islands) + w.islands = basics.buffer_create(sizeof(Island), math.max_int(island_count, 16)) + i = 0 + while i < island_count && rd.ok { + index = basics.buffer_push(&w.islands) + island = dynamics.island_at(w, index) + memset(island as ptr, 0, sizeof(Island)) + island.set_index = r_int(rd) + island.local_index = r_int(rd) + island_id = r_int(rd) + island.constraint_remove_count = r_int(rd) + island.island_id = NULL_INDEX + if island_id != NULL_INDEX && rd.ok { + island.bodies = r_ints(rd) + island.contacts = r_buffer(rd, sizeof(ContactLink)) + island.joints = r_buffer(rd, sizeof(JointLink)) + island.island_id = island_id + } + i = i + 1 + } + if rd.ok == false { return false } + + // The trees: each loaded as dynamic_tree loads saved bytes (its own + // checks), then put in place of the fresh one. + t = 0 + while t < broad_phase.BODY_TYPE_COUNT && rd.ok { + byte_count = r_int(rd) + if r_need(rd, byte_count) == false { break } + aligned = basics.alloc(math.max_int(byte_count, 1)) + memcpy(aligned, rd.data + rd.at, byte_count) + rd.at = rd.at + byte_count + tree = dynamic_tree.tree_load_bytes(aligned, byte_count, 1.0) + basics.free_bytes(aligned, math.max_int(byte_count, 1)) + if tree.nodes == null { + rd.ok = false + break + } + slot = broad_phase.broad_phase_tree(&w.broad_phase, t) + dynamic_tree.tree_destroy(slot) + memcpy(slot as ptr, (&tree) as ptr, sizeof(DynamicTree)) + t = t + 1 + } + if rd.ok == false { return false } + capacity = r_int(rd) + pair_count = r_int(rd) + if rd.ok == false || capacity < 16 || basics.is_power_of_2(capacity) == false || pair_count < 0 || 2 * pair_count > capacity { return false } + if basics.section_fits(rd.at, capacity, sizeof(SetItem), rd.size) == false { return false } + basics.set_destroy(&w.broad_phase.pair_set) + w.broad_phase.pair_set = HashSet { items: basics.alloc(capacity * sizeof(SetItem)), capacity: capacity, count: pair_count } + r_bytes(rd, w.broad_phase.pair_set.items, capacity * sizeof(SetItem)) + + // The colours: each container read whole in place of the fresh one's; + // the step's constraints start empty. + k = 0 + while k < dynamics.GRAPH_COLOR_COUNT && rd.ok { + color = graph_color(w, k) + if k < dynamics.OVERFLOW_INDEX { + bodies = r_bitset(rd) + basics.bitset_destroy(&color.body_set) + color.body_set = bodies + } + joints = r_buffer(rd, sizeof(JointSim)) + basics.buffer_destroy(&color.joint_sims) + color.joint_sims = joints + convex = r_ints(rd) + basics.ints_destroy(&color.convex_contacts) + color.convex_contacts = convex + specs = r_buffer(rd, sizeof(ContactSpec)) + basics.buffer_destroy(&color.contacts) + color.contacts = specs + k = k + 1 + } + return rd.ok +} + +// The fresh world's pool given way to the loaded one. +replace_pool(dst: *IdPool, src: IdPool) { + basics.idpool_destroy(dst) + dst.free_ids = src.free_ids + dst.next_index = src.next_index +} diff --git a/design.md b/design.md index 23e1bab..3382411 100644 --- a/design.md +++ b/design.md @@ -363,7 +363,9 @@ started until its tests pass. allocator's zeroing was a millisecond a step (the reference's does not zero; poisoning the memory shows nothing reads it first). 15. **recording and replay**, `world_snapshot`: last, since they are the - tooling and not the engine. + tooling and not the engine. The snapshot is in (`aephysics.snapshot`, + #149's first part): a loaded world steps on bit for bit as the one it + was taken from. Recording, replay and the name cache build on it. 16. **benchmarks** (done, #141): `reference/benchmark/main.c`'s thirteen scenes and its SAT runs, against the reference's own scene functions on the same machine: the large pyramid, the many pyramids and the diff --git a/tests/test_snapshot.ae b/tests/test_snapshot.ae new file mode 100644 index 0000000..baf616c --- /dev/null +++ b/tests/test_snapshot.ae @@ -0,0 +1,527 @@ +// aephysics.snapshot (#149, the reference's world_snapshot.c): a world of +// every kind of thing (a box pyramid, spheres, capsules and rocks falling +// on a mesh, a height field and a baked compound, a static sensor, a +// hanging chain on hinges with a motor, a ragdoll with ankles and its +// filter joints, a weld, a prismatic spring, a rope, a wheel, a motor and a +// parallel joint, a disabled body, a kinematic paddle, a bullet, names, +// several materials on a mesh) saved and loaded at several moments: the +// loaded world hashes as the original at once and steps on alike, bit for +// bit, for 150 steps, with the same contact, sensor and hit events; +// a loaded world saved again gives the same bytes; the geometry it was +// loaded with is its own; and every truncation and corruption of the bytes +// loads no world and leaves nothing allocated. + +import std.string +import aephysics.math +import aephysics.basics +import aephysics.hull +import aephysics.mesh +import aephysics.height_field +import aephysics.material +import aephysics.compound +import aephysics.manifold +import aephysics.dynamics +import aephysics.joint_solver +import aephysics.physics_world +import aephysics.human +import aephysics.snapshot + +extern calloc(count: int, size: int) -> ptr +extern exit(code: int) +extern free(p: ptr) +extern memcmp(a: ptr, b: ptr, size: int) -> int +extern memcpy(dst: ptr, src: ptr, size: int) -> ptr + +var failures = 0 +var checks = 0 + +ensure(name: string, ok: bool) { + checks = checks + 1 + if !ok { + println("snapshot: FAIL ${name}") + failures = failures + 1 + } +} + +// The geometry the scene's shapes are made from, kept until the end: the +// hulls go into the worlds' databases, the rest the worlds point at. +var g_box: ptr = null +var g_rock: ptr = null +var g_wave: ptr = null +var g_field: ptr = null +var g_compound: ptr = null +var g_ragdoll: ptr = null + +fixtures_ready() { + if g_box != null { return } + g_box = hull.make_box_hull(0.4, 0.4, 0.4) as ptr + g_rock = hull.create_rock(0.35) as ptr + g_wave = mesh.create_wave_mesh(20, 20, 1.0, 0.4, 0.3, 0.2) as ptr + g_field = height_field.create_grid(12, 12, math.vec3(1.0, 1.0, 1.0), true) as ptr + hulls_block = calloc(2, sizeof(CompoundHullDef)) + defs = hulls_block as CompoundHullDef[] + mat = material.default_surface_material() + defs[0] = compound.compound_hull_def(g_box as *HullData, Transform { p: math.vec3(0.0 - 1.0, 0.0, 0.0), q: math.quat_identity() }, mat) + defs[1] = compound.compound_hull_def(g_box as *HullData, Transform { p: math.vec3(1.0, 0.5, 0.0), q: math.quat_identity() }, mat) + cdef = compound.compound_def() + cdef.hulls = hulls_block + cdef.hull_count = 2 + g_compound = compound.create_compound(&cdef) as ptr + free(hulls_block) + g_ragdoll = calloc(1, sizeof(Human)) +} + +fixtures_free() { + hull.destroy_hull(g_box as *HullData) + hull.destroy_hull(g_rock as *HullData) + mesh.destroy_mesh(g_wave as *MeshData) + height_field.destroy_height_field(g_field as *HeightFieldData) + compound.destroy_compound(g_compound as *CompoundData) + free(g_ragdoll) +} + +world_def() -> WorldDef { + wdef = dynamics.default_world_def() + wdef.gravity = math.vec3(0.0, 0.0 - 10.0, 0.0) + return wdef +} + +static_body(world_id: WorldId, p: Vec3, name: string) -> BodyId { + bdef = dynamics.default_body_def() + bdef.position = p + bdef.name = name + return dynamics.create_body(world_id, &bdef) +} + +dynamic_body(world_id: WorldId, p: Vec3) -> BodyId { + bdef = dynamics.default_body_def() + bdef.kind = dynamics.BODY_DYNAMIC + bdef.position = p + return dynamics.create_body(world_id, &bdef) +} + +// The scene: every kind of thing a world holds. +build_scene(world_id: WorldId) { + fixtures_ready() + sdef = dynamics.default_shape_def() + floor_def = dynamics.default_shape_def() + floor_def.name = "floor" + ground = static_body(world_id, math.vec3(0.0, 0.0 - 1.0, 0.0), "ground") + floor_hull = hull.make_box_hull(30.0, 1.0, 30.0) + dynamics.create_hull_shape(ground, &floor_def, floor_hull) + hull.destroy_hull(floor_hull) + + // A mesh with three materials, a height field with holes, a compound. + materials_block = calloc(3, sizeof(SurfaceMaterial)) + materials = materials_block as SurfaceMaterial[] + k = 0 + while k < 3 { + materials[k] = material.default_surface_material() + materials[k].friction = 0.2 + 0.3 * (k as float) + k = k + 1 + } + mdef = dynamics.default_shape_def() + mdef.materials = materials_block + mdef.material_count = 3 + dynamics.create_mesh_shape(static_body(world_id, math.vec3(8.0, 0.0, 0.0 - 4.0), "waves"), &mdef, g_wave as *MeshData, math.vec3_one()) + free(materials_block) + dynamics.create_height_field_shape(static_body(world_id, math.vec3(0.0 - 14.0, 0.0, 0.0 - 6.0), "field"), &sdef, g_field as *HeightFieldData) + dynamics.create_compound_shape(static_body(world_id, math.vec3(0.0 - 6.0, 0.4, 6.0), "compound"), &sdef, g_compound as *CompoundData) + + // A sensor over the pyramid, which the boxes report to. + sensor_def = dynamics.default_shape_def() + sensor_def.is_sensor = true + sensor_def.enable_sensor_events = true + sensor_hull = hull.make_box_hull(2.0, 2.0, 2.0) + dynamics.create_hull_shape(static_body(world_id, math.vec3(0.0, 1.0, 0.0), "sensor"), &sensor_def, sensor_hull) + hull.destroy_hull(sensor_hull) + + // A pyramid of boxes, each reporting to the sensor; one named. + box_def = dynamics.default_shape_def() + box_def.enable_sensor_events = true + row = 0 + while row < 5 { + col = 0 + while col < 5 - row { + b = dynamic_body(world_id, math.vec3(0.82 * (col as float) - 0.41 * ((4 - row) as float), 0.4 + 0.81 * (row as float), 0.0)) + if row == 4 { dynamics.body_set_name(b, "apex") } + dynamics.create_hull_shape(b, &box_def, g_box as *HullData) + col = col + 1 + } + row = row + 1 + } + + // Spheres, capsules and rocks onto the mesh, the field and the compound. + i = 0 + while i < 12 { + x = 6.0 + 0.9 * ((i % 4) as float) + z = 0.0 - 6.0 + 1.1 * ((i / 4) as float) + b = dynamic_body(world_id, math.vec3(x, 2.0 + 0.3 * (i as float), z)) + if i % 3 == 0 { dynamics.create_sphere_shape(b, &sdef, manifold.sphere(math.vec3_zero(), 0.3)) } + else if i % 3 == 1 { dynamics.create_capsule_shape(b, &sdef, Capsule { center1: math.vec3(0.0 - 0.3, 0.0, 0.0), center2: math.vec3(0.3, 0.0, 0.0), radius: 0.2 }) } + else { dynamics.create_hull_shape(b, &sdef, g_rock as *HullData) } + f = dynamic_body(world_id, math.vec3(0.0 - 14.0 + 1.3 * ((i % 4) as float), 2.5 + 0.2 * (i as float), 0.0 - 4.0 + 1.2 * ((i / 4) as float))) + dynamics.create_sphere_shape(f, &sdef, manifold.sphere(math.vec3_zero(), 0.35)) + c = dynamic_body(world_id, math.vec3(0.0 - 6.5 + 0.8 * ((i % 3) as float), 3.0 + 0.4 * (i as float), 6.0)) + dynamics.create_hull_shape(c, &sdef, g_rock as *HullData) + i = i + 1 + } + + // A chain of capsules on hinges from an anchor, its first hinge driven. + anchor = static_body(world_id, math.vec3(10.0, 8.0, 8.0), "anchor") + previous = anchor + i = 0 + while i < 6 { + link = dynamic_body(world_id, math.vec3(10.0 + 0.6 * (i as float) + 0.3, 8.0, 8.0)) + dynamics.create_capsule_shape(link, &sdef, Capsule { center1: math.vec3(0.0 - 0.25, 0.0, 0.0), center2: math.vec3(0.25, 0.0, 0.0), radius: 0.1 }) + rdef = dynamics.default_revolute_joint_def() + rdef.base.body_id_a = previous + rdef.base.body_id_b = link + rdef.base.local_frame_a = Transform { p: math.vec3(0.3, 0.0, 0.0), q: math.quat_identity() } + if i == 0 { rdef.base.local_frame_a = math.transform_identity() } + rdef.base.local_frame_b = Transform { p: math.vec3(0.0 - 0.3, 0.0, 0.0), q: math.quat_identity() } + if i == 0 { + rdef.enable_motor = true + rdef.motor_speed = 1.5 + rdef.max_motor_torque = 50.0 + } + dynamics.create_revolute_joint(world_id, &rdef) + previous = link + i = i + 1 + } + + // A ragdoll with ankles, its filter joints among them. + h = g_ragdoll as *Human + h.bones = null + h.filter_joints = null + h.filter_joint_count = 0 + h.is_spawned = false + h.ankles = true + human.create_human(h, world_id, math.vec3(4.0, 3.0, 6.0), 2.0, 1.0, 0.5, 1, null) + + // A pair of each other joint kind, off to the side. + joint_pair(world_id, 0, math.vec3(14.0, 3.0, 0.0 - 10.0)) + joint_pair(world_id, 1, math.vec3(16.0, 3.0, 0.0 - 10.0)) + joint_pair(world_id, 2, math.vec3(18.0, 3.0, 0.0 - 10.0)) + joint_pair(world_id, 3, math.vec3(20.0, 3.0, 0.0 - 10.0)) + joint_pair(world_id, 4, math.vec3(22.0, 3.0, 0.0 - 10.0)) + joint_pair(world_id, 5, math.vec3(24.0, 3.0, 0.0 - 10.0)) + + // A body disabled, in the disabled set with its shape out of the trees. + sleeper = dynamic_body(world_id, math.vec3(0.0 - 20.0, 4.0, 10.0)) + dynamics.create_hull_shape(sleeper, &sdef, g_box as *HullData) + dynamics.body_disable(sleeper) + + // A kinematic paddle sweeping and a bullet at the pyramid. + kdef = dynamics.default_body_def() + kdef.kind = dynamics.BODY_KINEMATIC + kdef.position = math.vec3(0.0 - 3.0, 0.5, 3.0) + kdef.linear_velocity = math.vec3(1.0, 0.0, 0.0) + kdef.angular_velocity = math.vec3(0.0, 1.0, 0.0) + paddle = dynamics.create_body(world_id, &kdef) + paddle_hull = hull.make_box_hull(1.0, 0.2, 0.2) + dynamics.create_hull_shape(paddle, &sdef, paddle_hull) + hull.destroy_hull(paddle_hull) + bdef = dynamics.default_body_def() + bdef.kind = dynamics.BODY_DYNAMIC + bdef.position = math.vec3(0.0, 1.5, 12.0) + bdef.linear_velocity = math.vec3(0.0, 0.0, 0.0 - 60.0) + bdef.is_bullet = true + bdef.name = "bullet" + bullet = dynamics.create_body(world_id, &bdef) + dynamics.create_sphere_shape(bullet, &sdef, manifold.sphere(math.vec3_zero(), 0.1)) +} + +// Two boxes joined by a joint of one kind: a weld, a prismatic spring, a +// rope, a wheel, a motor or a parallel joint. +joint_pair(world_id: WorldId, kind: int, p: Vec3) { + sdef = dynamics.default_shape_def() + a = dynamic_body(world_id, p) + dynamics.create_hull_shape(a, &sdef, g_box as *HullData) + b = dynamic_body(world_id, math.add(p, math.vec3(0.0, 0.0, 1.2))) + dynamics.create_hull_shape(b, &sdef, g_box as *HullData) + frame_a = Transform { p: math.vec3(0.0, 0.0, 0.6), q: math.quat_identity() } + frame_b = Transform { p: math.vec3(0.0, 0.0, 0.0 - 0.6), q: math.quat_identity() } + if kind == 0 { + weld_def = dynamics.default_weld_joint_def() + weld_def.base.body_id_a = a + weld_def.base.body_id_b = b + weld_def.base.local_frame_a = frame_a + weld_def.base.local_frame_b = frame_b + weld_def.linear_hertz = 5.0 + dynamics.create_weld_joint(world_id, &weld_def) + } else if kind == 1 { + prismatic_def = dynamics.default_prismatic_joint_def() + prismatic_def.base.body_id_a = a + prismatic_def.base.body_id_b = b + prismatic_def.base.local_frame_a = frame_a + prismatic_def.base.local_frame_b = frame_b + prismatic_def.enable_spring = true + prismatic_def.hertz = 2.0 + prismatic_def.damping_ratio = 0.3 + prismatic_def.enable_limit = true + prismatic_def.lower_translation = 0.0 - 0.3 + prismatic_def.upper_translation = 0.3 + dynamics.create_prismatic_joint(world_id, &prismatic_def) + } else if kind == 2 { + rope_def = dynamics.default_distance_joint_def() + rope_def.base.body_id_a = a + rope_def.base.body_id_b = b + rope_def.base.local_frame_a = frame_a + rope_def.base.local_frame_b = frame_b + rope_def.length = 0.2 + rope_def.enable_limit = true + rope_def.min_length = 0.1 + rope_def.max_length = 0.5 + dynamics.create_distance_joint(world_id, &rope_def) + } else if kind == 3 { + wheel_def = dynamics.default_wheel_joint_def() + wheel_def.base.body_id_a = a + wheel_def.base.body_id_b = b + wheel_def.base.local_frame_a = frame_a + wheel_def.base.local_frame_b = frame_b + wheel_def.enable_spin_motor = true + wheel_def.spin_speed = 4.0 + wheel_def.max_spin_torque = 10.0 + dynamics.create_wheel_joint(world_id, &wheel_def) + } else if kind == 4 { + motor_def = dynamics.default_motor_joint_def() + motor_def.base.body_id_a = a + motor_def.base.body_id_b = b + motor_def.base.local_frame_a = frame_a + motor_def.base.local_frame_b = frame_b + motor_def.angular_velocity = math.vec3(0.0, 1.0, 0.0) + motor_def.max_velocity_torque = 20.0 + dynamics.create_motor_joint(world_id, &motor_def) + } else { + parallel_def = dynamics.default_parallel_joint_def() + parallel_def.base.body_id_a = a + parallel_def.base.body_id_b = b + parallel_def.base.local_frame_a = frame_a + parallel_def.base.local_frame_b = frame_b + parallel_def.hertz = 3.0 + parallel_def.max_torque = 30.0 + dynamics.create_parallel_joint(world_id, ¶llel_def) + } +} + +// The events of the last step, folded into one number to compare. +events_of(world_id: WorldId) -> long { + w = dynamics.get_world(world_id.index1 - 1) + folded = dynamics.world_contact_begin_event_count(w) as long + folded = folded * (1009 as long) + (dynamics.world_contact_end_event_count(w) as long) + folded = folded * (1009 as long) + (dynamics.world_sensor_begin_event_count(w) as long) + folded = folded * (1009 as long) + (dynamics.world_sensor_end_event_count(w) as long) + folded = folded * (1009 as long) + (dynamics.world_contact_hit_event_count(w) as long) + return folded +} + +state_hash(world_id: WorldId) -> long { return dynamics.hash_world_state(dynamics.get_world(world_id.index1 - 1)) } + +steps(world_id: WorldId, count: int) { + i = 0 + while i < count { + physics_world.step(world_id, 1.0 / 60.0, 4) + i = i + 1 + } +} + +// The scene run `before` steps, saved and loaded; then both stepped 150 +// steps, their hashes compared every step. +round_trip(before: int) { + wdef = world_def() + original = dynamics.create_world(&wdef) + build_scene(original) + steps(original, before) + block, byte_count = snapshot.save_world(original) + ensure("round trip at ${before}: saved (${byte_count} bytes)", block != null && byte_count > 0) + loaded = snapshot.load_world(&wdef, block, byte_count) + ensure("round trip at ${before}: loaded", dynamics.world_is_valid(loaded)) + if dynamics.world_is_valid(loaded) == false { + snapshot.free_snapshot(block, byte_count) + human.destroy_human(g_ragdoll as *Human) + dynamics.destroy_world(original) + return + } + ensure("round trip at ${before}: the same state at once", state_hash(loaded) == state_hash(original)) + a = physics_world.get_counters(original) + b = physics_world.get_counters(loaded) + ensure("round trip at ${before}: the same bodies, shapes, contacts, joints and islands", + a.body_count == b.body_count && a.shape_count == b.shape_count && a.contact_count == b.contact_count && + a.joint_count == b.joint_count && a.island_count == b.island_count) + + // Saved again at once, the same bytes: the snapshot is a fixed point. + again, again_count = snapshot.save_world(loaded) + ensure("round trip at ${before}: a loaded world saves to the same bytes (${again_count} against ${byte_count}, first apart at ${first_difference(again, block, math.min_int(again_count, byte_count))})", + again_count == byte_count && memcmp(again, block, byte_count) == 0) + snapshot.free_snapshot(again, again_count) + + alike = true + first_apart = 0 - 1 + events_alike = true + events_seen = 0 as long + i = 0 + while i < 150 { + physics_world.step(original, 1.0 / 60.0, 4) + physics_world.step(loaded, 1.0 / 60.0, 4) + if alike && state_hash(original) != state_hash(loaded) { + alike = false + first_apart = i + } + if events_of(original) != events_of(loaded) { events_alike = false } + events_seen = events_seen + events_of(original) + i = i + 1 + } + ensure("round trip at ${before}: 150 steps alike, bit for bit (first apart at ${first_apart})", alike) + ensure("round trip at ${before}: the same contact, sensor and hit events every step (${events_seen} folded)", events_alike) + // Before the scene settles there are events to compare. + if before < 100 { ensure("round trip at ${before}: events happened to compare", events_seen > (0 as long)) } + a = physics_world.get_counters(original) + b = physics_world.get_counters(loaded) + ensure("round trip at ${before}: the same counts after (${a.contact_count} contacts, ${a.awake_contact_count} awake)", + a.contact_count == b.contact_count && a.awake_contact_count == b.awake_contact_count && a.island_count == b.island_count) + + // The loaded world's geometry is its own: the scene's blocks are not + // what its shapes hold. + w = dynamics.get_world(loaded.index1 - 1) + owns = w.owned_geometry.count == 3 + i = 0 + while i < w.shapes.count { + s = dynamics.world_shape_at(w, i) + if s.id != dynamics.NULL_INDEX { + if s.geometry.mesh.data == (g_wave as *MeshData) || s.geometry.height_field == (g_field as *HeightFieldData) || s.geometry.compound == (g_compound as *CompoundData) { owns = false } + } + i = i + 1 + } + ensure("round trip at ${before}: the mesh, the field and the compound are the loaded world's own", owns) + + snapshot.free_snapshot(block, byte_count) + human.destroy_human(g_ragdoll as *Human) + dynamics.destroy_world(original) + dynamics.destroy_world(loaded) +} + +first_difference(a: ptr, b: ptr, count: int) -> int { + x = a as byte[] + y = b as byte[] + i = 0 + while i < count { + if (x[i] as int) != (y[i] as int) { return i } + i = i + 1 + } + return 0 - 1 +} + +// Loaded with four workers what one stepped: the step's results do not +// depend on the worker count, so the two still step alike, bit for bit. +across_workers() { + wdef = world_def() + original = dynamics.create_world(&wdef) + build_scene(original) + steps(original, 20) + block, byte_count = snapshot.save_world(original) + four = world_def() + four.worker_count = 4 + loaded = snapshot.load_world(&four, block, byte_count) + ensure("workers: loaded with four", dynamics.world_is_valid(loaded)) + if dynamics.world_is_valid(loaded) { + alike = true + i = 0 + while i < 120 { + physics_world.step(original, 1.0 / 60.0, 4) + physics_world.step(loaded, 1.0 / 60.0, 4) + if state_hash(original) != state_hash(loaded) { alike = false } + i = i + 1 + } + ensure("workers: one and four step alike for 120 steps after the load", alike) + dynamics.destroy_world(loaded) + } + snapshot.free_snapshot(block, byte_count) + human.destroy_human(g_ragdoll as *Human) + dynamics.destroy_world(original) +} + +// Every damage to the bytes loads no world and leaves nothing behind. +corruptions() { + wdef = world_def() + original = dynamics.create_world(&wdef) + build_scene(original) + steps(original, 30) + block, byte_count = snapshot.save_world(original) + human.destroy_human(g_ragdoll as *Human) + dynamics.destroy_world(original) + + before = basics.alloc_count() + refused_all = true + cut = 0 + while cut < byte_count { + if loads(&wdef, block, cut) { refused_all = false } + cut = cut + 1 + cut / 7 + } + ensure("corruptions: every truncation is refused", refused_all) + ensure("corruptions: a byte too many is refused", loads_longer(&wdef, block, byte_count) == false) + ensure("corruptions: another magic is refused", loads_flipped(&wdef, block, byte_count, 0) == false) + ensure("corruptions: another version is refused", loads_flipped(&wdef, block, byte_count, 4) == false) + ensure("corruptions: another layout is refused", loads_flipped(&wdef, block, byte_count, 8) == false) + ensure("corruptions: no bytes are refused", dynamics.world_is_valid(snapshot.load_world(&wdef, null, 0)) == false) + ensure("corruptions: nothing left allocated (${basics.alloc_count() - before})", basics.alloc_count() == before) + good = snapshot.load_world(&wdef, block, byte_count) + ensure("corruptions: the bytes themselves still load", dynamics.world_is_valid(good)) + dynamics.destroy_world(good) + snapshot.free_snapshot(block, byte_count) +} + +loads(wdef: *WorldDef, block: ptr, count: int) -> bool { + id = snapshot.load_world(wdef, block, count) + if dynamics.world_is_valid(id) { + dynamics.destroy_world(id) + return true + } + return false +} + +loads_longer(wdef: *WorldDef, block: ptr, count: int) -> bool { + longer = calloc(1, count + 1) + memcpy(longer, block, count) + result = loads(wdef, longer, count + 1) + free(longer) + return result +} + +loads_flipped(wdef: *WorldDef, block: ptr, count: int, at: int) -> bool { + copy = calloc(1, count) + memcpy(copy, block, count) + bytes = copy as byte[] + bytes[at] = ((bytes[at] as int) ^ 1) as byte + result = loads(wdef, copy, count) + free(copy) + return result +} + +main() { + // The modules' scratch lives as long as the program: taken before the count. + dynamics.reserve_scratch() + compound.reserve_scratch() + height_field.reserve_scratch() + fixtures_ready() + // The first round trip also makes the solver's own buffers, kept for the + // program; the count is taken after it, so every later round trip and + // the corruptions must leave nothing. + round_trip(0) + before = basics.alloc_count() + round_trip(1) + round_trip(45) + round_trip(200) + across_workers() + corruptions() + ensure("every counted allocation was freed (${basics.alloc_count() - before})", basics.alloc_count() == before) + fixtures_free() + + println("snapshot: ${checks} checks") + if failures == 0 { + println("snapshot: all checks passed") + } else { + println("snapshot: ${failures} failure(s)") + exit(1) + } +}