From 09dafd69d2a6686292771b486fe8bac788ad2f15 Mon Sep 17 00:00:00 2001 From: Nicolas Maman Date: Sat, 19 Sep 2026 21:18:36 -0300 Subject: [PATCH] height_field: the height field with its casts, query and mover aephysics.height_field, Box3D's height_field.c in Aether: heights quantised to a global range so neighbouring fields line up, a material per cell with holes, the edge flags of every triangle against its four neighbours, either winding; the overlap, the ray and shape casts by a DDA walk of the swept box's leading corner through the cells, the character mover's planes and the box query. test_height_field.ae: the reference's create, index mapping on an asymmetric grid, winding normals, flat-field ray, overlap at the surface, the vertical casts straddling a cell boundary, the shape and ray casts against a brute force over every triangle of a wave, and the back side culled for both windings (the file roundtrip is not ported); plus the flags of a ridge and of holes, a scaled field, the box query, the mover. 113 checks. bench/height_field.ae against bench/height_field_box3d.c on a 512 x 512 wave with holes: the same hits and sums on every phase; the box query at parity, the ray walk 1.4x, the build 1.5x, the shape cast 2.1x. --- README.md | 3 +- aephysics/height_field/module.ae | 807 +++++++++++++++++++++++++++++++ aephysics/test_height_field.ae | 635 ++++++++++++++++++++++++ bench/RESULTS.md | 29 ++ bench/height_field.ae | 104 ++++ bench/height_field_box3d.c | 104 ++++ design.md | 25 +- 7 files changed, 1699 insertions(+), 8 deletions(-) create mode 100644 aephysics/height_field/module.ae create mode 100644 aephysics/test_height_field.ae create mode 100644 bench/height_field.ae create mode 100644 bench/height_field_box3d.c diff --git a/README.md b/README.md index 0ecbe98..bac6610 100644 --- a/README.md +++ b/README.md @@ -33,7 +33,8 @@ so a test written against the reference reads the same here. | `aephysics.manifold` | contact manifolds for sphere, capsule and hull in every pairing: the separating axis test with its cache, reference-face clipping, the feature pairs, reduction to four points | done, `test_manifold.ae` (43k checks, 7,000 pairs against a brute-force oracle); [same manifolds as the reference, warm cache at parity](bench/RESULTS.md#manifold) | | `aephysics.triangle_manifold` | one mesh triangle against a sphere, capsule or hull: back-side cull with hysteresis, GJK shallow, the separating axis test deep with the triangle's edges as zero-area faces, the feature recorded for the mesh contact's ghost-collision reduction | done, `test_triangle_manifold.ae` (1.5k checks); [same manifolds as the reference, within 10% on hulls](bench/RESULTS.md#triangle_manifold) | | `aephysics.mesh` | the triangle mesh: a BVH by binned SAH or median split with the triangles in depth-first order, vertex welding, edge flags, any scale including mirrored; overlap, ray cast, shape cast, the mover's planes, a box query | done, `test_mesh.ae` (1.6k checks); [same trees as the reference, traversals 1.7-2x](bench/RESULTS.md#mesh) | -| `aephysics.collision` | height field, shapes with mass properties, ray and shape casts | next | +| `aephysics.height_field` | the height field: quantised heights on a fixed diagonal, materials and holes per cell, edge flags per triangle, either winding; overlap, ray and shape casts by a walk along the grid, the mover's planes, a box query | done, `test_height_field.ae` (113 checks, casts against a brute force over a wave); [same results as the reference, query at parity, casts 1.4-2x](bench/RESULTS.md#height_field) | +| `aephysics.shape` | shapes with mass properties, ray and shape casts per shape, compounds | next | | `aephysics.dynamics` | bodies, contacts, the constraint graph, islands, the Soft Step solver, joints (spherical, revolute, prismatic, distance, motor, weld, wheel), sensors, the character mover, the world | | | `aephysics` | the public API | | diff --git a/aephysics/height_field/module.ae b/aephysics/height_field/module.ae new file mode 100644 index 0000000..8eafc93 --- /dev/null +++ b/aephysics/height_field/module.ae @@ -0,0 +1,807 @@ +// aephysics.height_field -- the height field: a grid of quantised +// heights on a fixed diagonal, with a material per cell (a hole is a +// material), edge flags per triangle for the ghost-collision reduction, +// and the queries over it: overlap, bounds, ray cast and shape cast by a +// walk along the grid, the character mover's planes, and a box query. +// +// The shape is Box3D's height_field.c (Erin Catto, MIT), the reference +// this engine is measured against: the convention that the index is +// row * column_count + column with x along the columns and z along the +// rows, the quantisation to a global height range so neighbouring fields +// line up, the two triangles of a cell {11, 21, 12} and {22, 12, 21}, the +// cast that rasterises the swept box's leading corner through the cells +// (DDA) and casts against every cell the box straddles, and the front +// side chosen by the winding. Names are the reference's without its +// prefix, in snake case: b3CreateHeightField is create_height_field. +// +// Differences: the heights are ints holding the 16-bit quantised values +// (Aether has no 16-bit type), the materials and flags ints; the debug +// file dump and load are not ported; the reference's SIMD tests are the +// mesh module's scalar ones. +import std.string +import aephysics.math +import aephysics.core +import aephysics.distance +import aephysics.manifold +import aephysics.triangle_manifold +import aephysics.mesh + +exports ( + HeightFieldDef, HeightFieldData, + HEIGHT_FIELD_VERSION, HEIGHT_FIELD_HOLE, NULL_INDEX, + height_field_def, height_field_heights, height_field_material_indices, height_field_flags, + height_field_triangle_count, height_field_height, + create_height_field, destroy_height_field, 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 +) + +extern memset(block: ptr, value: int, size: int) -> ptr +extern floor(x: float) -> float + +const NULL_INDEX = 0 - 1 +const HEIGHT_FIELD_VERSION = 0x0E41E5FB +const HEIGHT_FIELD_HOLE = 255 +const MAX_QUANTUM = 65535 +const MAX_AABB_MARGIN = 0.05 + +// What a height field is made from. No pointers into it are kept. +struct HeightFieldDef { + heights: ptr // float[count_x * count_z], the grid point heights, unscaled + material_indices: ptr // int[(count_x - 1) * (count_z - 1)] per cell, HEIGHT_FIELD_HOLE for a hole, or null + scale: Vec3 // every component positive + count_x: int // grid lines along x + count_z: int // grid lines along z + global_minimum_height: float // the quantisation range, shared by fields that must line up + global_maximum_height: float + clockwise_winding: bool // inverts the field along y +} + +// The field: one block, with the arrays at byte offsets. +struct HeightFieldData { + version: int + hash: long + byte_count: int + aabb: AABB + min_height: float + max_height: float + height_scale: float // the quantum + scale: Vec3 + column_count: int // along x + row_count: int // along z + heights_offset: int // int[] of quantised heights, one per grid point + material_offset: int // int[] per cell + flags_offset: int // int[] per triangle + clockwise: bool +} + +height_field_def() -> HeightFieldDef { + return HeightFieldDef { heights: null, material_indices: null, scale: math.vec3_one(), count_x: 0, count_z: 0, + global_minimum_height: 0.0 - 1.0, global_maximum_height: 1.0, clockwise_winding: false } +} + +height_field_heights(h: *HeightFieldData) -> int[] { return ((h as ptr) + h.heights_offset) as int[] } +height_field_material_indices(h: *HeightFieldData) -> int[] { return ((h as ptr) + h.material_offset) as int[] } +height_field_flags(h: *HeightFieldData) -> int[] { return ((h as ptr) + h.flags_offset) as int[] } +height_field_triangle_count(h: *HeightFieldData) -> int { return 2 * (h.column_count - 1) * (h.row_count - 1) } + +// The unscaled height of a grid point. +height_field_height(h: *HeightFieldData, index: int) -> float { + heights = height_field_heights(h) + return h.min_height + h.height_scale * (heights[index] as float) +} + +align8(x: int) -> int { return (x + 7) & (0 - 8) } + +// --- scratch --------------------------------------------------------------------- + +var g_triangle: ptr = null // Vec3[3] +var g_local: ptr = null // Vec3[128] + +scratch_ready() { + if g_triangle == null { + g_triangle = core.alloc(3 * sizeof(Vec3)) + g_local = core.alloc(128 * sizeof(Vec3)) + } +} + +// --- creation -------------------------------------------------------------------- + +// A scaled grid point from a column, a row and an unscaled height. +grid_point(scale: Vec3, column: int, row: int, height: float) -> Vec3 { + return math.mul(scale, math.vec3(column as float, height, row as float)) +} + +// The concavity of the edge between a triangle's plane and a neighbouring +// triangle (its normal, a vertex on it): concave, or flat (both flags). +edge_flags(plane: Plane, other_normal: Vec3, other_vertex: Vec3, edge: int) -> int { + cos_5_deg = 0.9962 + separation = math.plane_separation(plane, other_vertex) + cos_angle = math.dot(plane.normal, other_normal) + flags = 0 + if separation > 0.0 || cos_angle > cos_5_deg { flags = flags | (1 << edge) } + if separation < 0.0 || cos_angle > cos_5_deg { flags = flags | ((1 << edge) << 4) } + return flags +} + +// The field from its definition: the heights quantised to the global +// range, the bounds, and the edge flags of every triangle against its +// three neighbours (across the diagonal, and the cells above, below, +// left and right that are not holes). +create_height_field(def: *HeightFieldDef) -> *HeightFieldData { + column_count = def.count_x + row_count = def.count_z + height_count = column_count * row_count + if column_count < 2 || row_count < 2 || def.heights == null { return null } + cell_count = (column_count - 1) * (row_count - 1) + triangle_count = 2 * cell_count + + byte_count = align8(sizeof(HeightFieldData)) + heights_offset = byte_count + byte_count = byte_count + align8(height_count * 4) + material_offset = byte_count + byte_count = byte_count + align8(cell_count * 4) + flags_offset = byte_count + byte_count = byte_count + align8(triangle_count * 4) + h = core.alloc(byte_count) as *HeightFieldData + h.version = HEIGHT_FIELD_VERSION + h.byte_count = byte_count + h.scale = def.scale + h.column_count = column_count + h.row_count = row_count + h.heights_offset = heights_offset + h.material_offset = material_offset + h.flags_offset = flags_offset + h.clockwise = def.clockwise_winding + + quantised = height_field_heights(h) + materials = height_field_material_indices(h) + flags = height_field_flags(h) + source = def.heights as float[] + h.min_height = def.global_minimum_height + h.max_height = def.global_maximum_height + range = math.max_float(h.max_height - h.min_height, math.LINEAR_SLOP) + h.height_scale = range / (MAX_QUANTUM as float) + lower_bound = h.max_height + upper_bound = h.min_height + inv_quantum = 1.0 / h.height_scale + i = 0 + while i < height_count { + clamped = math.clamp_float(source[i], h.min_height, h.max_height) + scaled = (clamped - h.min_height) * inv_quantum + quantised[i] = math.min_float(scaled, MAX_QUANTUM as float) as int + lower_bound = math.min_float(lower_bound, clamped) + upper_bound = math.max_float(upper_bound, clamped) + i = i + 1 + } + // The decompressed heights give the flags their true convexity. + heights_block = core.alloc(height_count * 8) + heights = heights_block as float[] + i = 0 + while i < height_count { + heights[i] = h.min_height + h.height_scale * (quantised[i] as float) + i = i + 1 + } + source_materials = def.material_indices as int[] + i = 0 + while i < cell_count { + materials[i] = 0 + if def.material_indices != null { materials[i] = source_materials[i] } + i = i + 1 + } + h.aabb = AABB { lower: math.vec3(0.0, h.scale.y * lower_bound, 0.0), + upper: math.vec3(h.scale.x * ((column_count - 1) as float), h.scale.y * upper_bound, h.scale.z * ((row_count - 1) as float)) } + + scale = h.scale + triangle_index = 0 + row = 0 + while row < row_count - 1 { + column = 0 + while column < column_count - 1 { + triangle1 = triangle_index + triangle2 = triangle_index + 1 + triangle_index = triangle_index + 2 + flags[triangle1] = 0 + flags[triangle2] = 0 + cell = row * (column_count - 1) + column + if materials[cell] != HEIGHT_FIELD_HOLE { + index11 = row * column_count + column + index12 = index11 + 1 + index21 = (row + 1) * column_count + column + index22 = index21 + 1 + p11 = grid_point(scale, column, row, heights[index11]) + p12 = grid_point(scale, column + 1, row, heights[index12]) + p21 = grid_point(scale, column, row + 1, heights[index21]) + p22 = grid_point(scale, column + 1, row + 1, heights[index22]) + // Triangle 0: 11, 21, 12. Triangle 1: 22, 12, 21. + plane1 = math.make_plane_from_points(p11, p21, p12) + plane2 = math.make_plane_from_points(p22, p12, p21) + flags1 = 0 + flags2 = 0 + // Across the diagonal (edge 2 of both). + diagonal = edge_flags(plane1, plane2.normal, p22, 1) + flags1 = flags1 | diagonal + flags2 = flags2 | diagonal + // The cell above (row - 1): its triangle 1 against triangle 0's edge 3. + if row > 0 && materials[cell - (column_count - 1)] != HEIGHT_FIELD_HOLE { + r = row - 1 + q22 = grid_point(scale, column + 1, r + 1, heights[(r + 1) * column_count + column + 1]) + q12 = grid_point(scale, column + 1, r, heights[r * column_count + column + 1]) + q21 = grid_point(scale, column, r + 1, heights[(r + 1) * column_count + column]) + n = math.make_normal_from_points(q22, q12, q21) + flags1 = flags1 | edge_flags(plane1, n, q12, 2) + } + // The cell below (row + 1): its triangle 0 against triangle 1's edge 3. + if row + 1 < row_count - 1 && materials[cell + (column_count - 1)] != HEIGHT_FIELD_HOLE { + r = row + 1 + q11 = grid_point(scale, column, r, heights[r * column_count + column]) + q21 = grid_point(scale, column, r + 1, heights[(r + 1) * column_count + column]) + q12 = grid_point(scale, column + 1, r, heights[r * column_count + column + 1]) + n = math.make_normal_from_points(q11, q21, q12) + flags2 = flags2 | edge_flags(plane2, n, q21, 2) + } + // The cell to the left: its triangle 1 against triangle 0's edge 1. + if column > 0 && materials[cell - 1] != HEIGHT_FIELD_HOLE { + c = column - 1 + q22 = grid_point(scale, c + 1, row + 1, heights[(row + 1) * column_count + c + 1]) + q12 = grid_point(scale, c + 1, row, heights[row * column_count + c + 1]) + q21 = grid_point(scale, c, row + 1, heights[(row + 1) * column_count + c]) + n = math.make_normal_from_points(q22, q12, q21) + flags1 = flags1 | edge_flags(plane1, n, q21, 0) + } + // The cell to the right: its triangle 0 against triangle 1's edge 1. + if column + 1 < column_count - 1 && materials[cell + 1] != HEIGHT_FIELD_HOLE { + c = column + 1 + q11 = grid_point(scale, c, row, heights[row * column_count + c]) + q21 = grid_point(scale, c, row + 1, heights[(row + 1) * column_count + c]) + q12 = grid_point(scale, c + 1, row, heights[row * column_count + c + 1]) + n = math.make_normal_from_points(q11, q21, q12) + flags2 = flags2 | edge_flags(plane2, n, q12, 0) + } + flags[triangle1] = flags1 + flags[triangle2] = flags2 + } + column = column + 1 + } + row = row + 1 + } + core.free_bytes(heights_block, height_count * 8) + h.hash = 0 as long + h.hash = core.hash_bytes(h as ptr, h.byte_count) + return h +} + +destroy_height_field(h: *HeightFieldData) { core.free_bytes(h as ptr, h.byte_count) } + +// 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 + heights_block = core.alloc(height_count * 8) + cell_count = (row_count - 1) * (column_count - 1) + materials_block = core.alloc(cell_count * 4) + materials = materials_block as int[] + k = 0 + while k < cell_count { + if make_holes && k > 0 && k % 16 == 0 { materials[k] = HEIGHT_FIELD_HOLE } + k = k + 1 + } + def = HeightFieldDef { heights: heights_block, material_indices: materials_block, scale: scale, count_x: column_count, + count_z: row_count, global_minimum_height: 0.0 - 256.0, global_maximum_height: 256.0, clockwise_winding: false } + h = create_height_field(&def) + core.free_bytes(heights_block, height_count * 8) + core.free_bytes(materials_block, cell_count * 4) + return h +} + +// A field rippled by the product of two sines. +create_wave(row_count: int, column_count: int, scale: Vec3, row_frequency: float, column_frequency: float, make_holes: bool) -> *HeightFieldData { + height_count = row_count * column_count + heights_block = core.alloc(height_count * 8) + heights = heights_block as float[] + omega_z = 2.0 * math.PI * row_frequency + omega_x = 2.0 * math.PI * column_frequency + i = 0 + while i < row_count { + row_height = math.compute_cos_sin(omega_z * (i as float)).sine + j = 0 + while j < column_count { + heights[i * column_count + j] = row_height * math.compute_cos_sin(omega_x * (j as float)).sine + j = j + 1 + } + i = i + 1 + } + cell_count = (row_count - 1) * (column_count - 1) + materials_block = core.alloc(cell_count * 4) + materials = materials_block as int[] + k = 0 + while k < cell_count { + if make_holes && k > 0 && k % 16 == 0 { materials[k] = HEIGHT_FIELD_HOLE } + k = k + 1 + } + def = HeightFieldDef { heights: heights_block, material_indices: materials_block, scale: scale, count_x: column_count, + count_z: row_count, global_minimum_height: 0.0 - 256.0, global_maximum_height: 256.0, clockwise_winding: false } + h = create_height_field(&def) + core.free_bytes(heights_block, height_count * 8) + core.free_bytes(materials_block, cell_count * 4) + return h +} + +// --- cells and triangles -------------------------------------------------------------- + +// The four corners of a cell, scaled: 11 (column, row), 12 (column + 1, +// row), 21 (column, row + 1), 22 (column + 1, row + 1), into corners[0..3]. +cell_corners(h: *HeightFieldData, row: int, column: int, corners: Vec3[]) { + column_count = h.column_count + index11 = row * column_count + column + index21 = (row + 1) * column_count + column + corners[0] = grid_point(h.scale, column, row, height_field_height(h, index11)) + corners[1] = grid_point(h.scale, column + 1, row, height_field_height(h, index11 + 1)) + corners[2] = grid_point(h.scale, column, row + 1, height_field_height(h, index21)) + corners[3] = grid_point(h.scale, column + 1, row + 1, height_field_height(h, index21 + 1)) +} + +var g_corners: ptr = null + +corners_ready() -> Vec3[] { + if g_corners == null { g_corners = core.alloc(4 * sizeof(Vec3)) } + return g_corners as Vec3[] +} + +// A triangle of the field, wound counter-clockwise about its front (down +// for a clockwise field), with its flags made to match. +get_height_field_triangle(h: *HeightFieldData, triangle_index: int) -> Triangle { + flags = height_field_flags(h) + column_count = h.column_count + quad = triangle_index >> 1 + row = quad / (column_count - 1) + column = quad - row * (column_count - 1) + index11 = row * column_count + column + index12 = index11 + 1 + index21 = (row + 1) * column_count + column + index22 = index21 + 1 + corners = corners_ready() + cell_corners(h, row, column, corners) + t = Triangle { v1: corners[0], v2: corners[2], v3: corners[1], i1: index11, i2: index21, i3: index12, flags: flags[triangle_index] } + if (triangle_index & 1) != 0 { + t = Triangle { v1: corners[3], v2: corners[1], v3: corners[2], i1: index22, i2: index12, i3: index21, flags: flags[triangle_index] } + } + if h.clockwise { + // Reversing the winding swaps edges 1 and 3; the diagonal (edge 2) stays. + f = t.flags + edge1_bits = f & 0x11 + edge3_bits = f & 0x44 + f = f & (255 - 0x55) + f = f | (edge1_bits << 2) | (edge3_bits >> 2) + return Triangle { v1: t.v1, v2: t.v3, v3: t.v2, i1: t.i1, i2: t.i3, i3: t.i2, flags: f } + } + return t +} + +get_height_field_material(h: *HeightFieldData, triangle_index: int) -> int { + materials = height_field_material_indices(h) + return materials[triangle_index >> 1] +} + +compute_height_field_aabb(h: *HeightFieldData, t: Transform) -> AABB { return math.aabb_transform(t, h.aabb) } + +// --- casts --------------------------------------------------------------------------- + +// A ray is a shape cast of a point with no radius. +ray_cast_height_field(h: *HeightFieldData, origin: Vec3, translation: Vec3, max_fraction: float) -> CastOutput { + scratch_ready() + point = g_local as Vec3[] + point[0] = origin + return shape_cast_height_field(h, distance.shape_proxy(g_local, 1, 0.0), translation, max_fraction, false) +} + +// The proxy swept through the field: its box's leading corner is walked +// through the cells (a DDA on rows and columns), and every cell the box +// straddles at each step is cast against, front faces only, keeping the +// nearest hit. A point with no radius is a ray, tested exactly. +shape_cast_height_field(h: *HeightFieldData, proxy: ShapeProxy, translation: Vec3, max_fraction: float, can_encroach: bool) -> CastOutput { + shape_bounds = mesh.compute_proxy_aabb(proxy) + scale = h.scale + shape_start = math.aabb_center(shape_bounds) + shape_delta = math.mul_sv(max_fraction, translation) + shape_end = math.add(shape_start, shape_delta) + result = distance.empty_cast_output() + shape_extents = math.aabb_extents(shape_bounds) + margin = math.vec3(MAX_AABB_MARGIN, MAX_AABB_MARGIN, MAX_AABB_MARGIN) + combined = AABB { lower: math.sub(math.sub(h.aabb.lower, shape_extents), margin), upper: math.add(math.add(h.aabb.upper, shape_extents), margin) } + fractions_block = core.alloc(16) + fractions = fractions_block as float[] + fractions[0] = 0.0 + fractions[1] = 1.0 + intersects = math.ray_cast_aabb(combined, shape_start, shape_end, fractions) + min_fraction = fractions[0] + max_fraction_clamped = fractions[1] + core.free_bytes(fractions_block, 16) + if intersects == false { return result } + + // The clamped sweep walks the grid; the triangle casts use the whole one. + clamped_start = math.mul_add(shape_start, min_fraction, shape_delta) + clamped_delta = math.mul_sv(max_fraction_clamped - min_fraction, shape_delta) + clamped_end = math.add(clamped_start, clamped_delta) + center_start = clamped_start + center_end = clamped_end + // The walk starts from the leading corner of the shape's box. + sign_x = 1.0 + if translation.x >= 0.0 { clamped_start.x = clamped_start.x + shape_extents.x } else { + clamped_start.x = clamped_start.x - shape_extents.x + sign_x = 0.0 - 1.0 + } + sign_z = 1.0 + if translation.z >= 0.0 { clamped_start.z = clamped_start.z + shape_extents.z } else { + clamped_start.z = clamped_start.z - shape_extents.z + sign_z = 0.0 - 1.0 + } + clamped_end = math.add(clamped_start, clamped_delta) + column_start = floor(clamped_start.x / scale.x) as int + column_end = floor(clamped_end.x / scale.x) as int + row_start = floor(clamped_start.z / scale.z) as int + row_end = floor(clamped_end.z / scale.z) as int + abs_delta = math.abs_vec3(clamped_delta) + + delta_alpha_x = 0.0 + next_fraction_x = math.MAX_FLOAT + delta_column = 0 + if column_start < column_end { + delta_alpha_x = scale.x / abs_delta.x + next_fraction_x = (scale.x * ((column_start + 1) as float) - clamped_start.x) / abs_delta.x + delta_column = 1 + } else if column_end < column_start { + delta_alpha_x = scale.x / abs_delta.x + next_fraction_x = (clamped_start.x - scale.x * (column_start as float)) / abs_delta.x + delta_column = 0 - 1 + } + delta_alpha_z = 0.0 + next_fraction_z = math.MAX_FLOAT + delta_row = 0 + if row_start < row_end { + delta_alpha_z = scale.z / abs_delta.z + next_fraction_z = (scale.z * ((row_start + 1) as float) - clamped_start.z) / abs_delta.z + delta_row = 1 + } else if row_end < row_start { + delta_alpha_z = scale.z / abs_delta.z + next_fraction_z = (clamped_start.z - scale.z * (row_start as float)) / abs_delta.z + delta_row = 0 - 1 + } + + // The columns and rows the shape's box covers at the start. + box_column_head = column_start + box_row_head = row_start + box_column_tail = floor((clamped_start.x - 2.0 * sign_x * shape_extents.x) / scale.x) as int + box_row_tail = floor((clamped_start.z - 2.0 * sign_z * shape_extents.z) / scale.z) as int + best_fraction = max_fraction + // The walk's fractions are of the clamped sweep; the hits are of the + // whole translation. The affine map between them, so the exit test + // compares like with like. + grid_fraction_scale = max_fraction * (max_fraction_clamped - min_fraction) + grid_fraction_offset = max_fraction * min_fraction + row_count = h.row_count + column_count = h.column_count + materials = height_field_material_indices(h) + scratch_ready() + corners = corners_ready() + is_ray = proxy.count == 1 && proxy.radius == 0.0 + cast_bounds = AABB { lower: math.sub(math.min_vec3(center_start, center_end), shape_extents), + upper: math.add(math.max_vec3(center_start, center_end), shape_extents) } + clockwise = h.clockwise + + while true { + column1 = box_column_head + column2 = box_column_tail + if box_column_tail < box_column_head { + column1 = box_column_tail + column2 = box_column_head + } + row1 = box_row_head + row2 = box_row_tail + if box_row_tail < box_row_head { + row1 = box_row_tail + row2 = box_row_head + } + row = row1 + while row <= row2 { + if row >= 0 && row < row_count - 1 { + column = column1 + while column <= column2 { + if column >= 0 && column < column_count - 1 { + cell = row * (column_count - 1) + column + material = materials[cell] + if material != HEIGHT_FIELD_HOLE { + cell_corners(h, row, column, corners) + p11 = corners[0] + p12 = corners[1] + p21 = corners[2] + p22 = corners[3] + if clockwise { + swap = p12 + p12 = p21 + p21 = swap + } + bounds = AABB { lower: math.min_vec3(math.min_vec3(p11, p12), math.min_vec3(p21, p22)), + upper: math.max_vec3(math.max_vec3(p11, p12), math.max_vec3(p21, p22)) } + if math.aabb_overlaps(cast_bounds, bounds) { + triangle1 = 2 * cell + triangle2 = triangle1 + 1 + if is_ray { + alpha = mesh.intersect_ray_triangle(shape_start, translation, p11, p21, p12) + if alpha < best_fraction { + result.point = math.mul_add(shape_start, alpha, translation) + result.normal = math.normalize(math.cross(math.sub(p21, p11), math.sub(p12, p11))) + result.fraction = alpha + result.triangle_index = triangle1 + result.material_index = material + result.hit = true + best_fraction = alpha + } + alpha = mesh.intersect_ray_triangle(shape_start, translation, p22, p12, p21) + if alpha < best_fraction { + result.point = math.mul_add(shape_start, alpha, translation) + result.normal = math.normalize(math.cross(math.sub(p22, p21), math.sub(p12, p21))) + result.fraction = alpha + result.triangle_index = triangle2 + result.material_index = material + result.hit = true + best_fraction = alpha + } + } else { + if math.signed_volume(p11, p21, p12, shape_start) >= 0.0 { + best_fraction = cast_triangle(&result, proxy, translation, can_encroach, p11, p21, p12, best_fraction, triangle1, material) + } + if math.signed_volume(p21, p22, p12, shape_start) >= 0.0 { + best_fraction = cast_triangle(&result, proxy, translation, can_encroach, p21, p22, p12, best_fraction, triangle2, material) + } + } + } + } + } + column = column + 1 + } + } + row = row + 1 + } + + // The walk's fractions only grow, so this ends. + input_fraction_x = math.MAX_FLOAT + if next_fraction_x != math.MAX_FLOAT { input_fraction_x = grid_fraction_offset + next_fraction_x * grid_fraction_scale } + input_fraction_z = math.MAX_FLOAT + if next_fraction_z != math.MAX_FLOAT { input_fraction_z = grid_fraction_offset + next_fraction_z * grid_fraction_scale } + if input_fraction_x > best_fraction && input_fraction_z > best_fraction { break } + + if next_fraction_x <= next_fraction_z { + if box_column_head == column_end { break } + box_column_head = box_column_head + delta_column + box_column_tail = box_column_head + if shape_extents.z == 0.0 { + box_row_tail = box_row_head + } else { + row_intercept = clamped_start.z + next_fraction_x * clamped_delta.z + box_row_tail = floor((row_intercept - 2.0 * sign_z * shape_extents.z) / scale.z) as int + } + next_fraction_x = next_fraction_x + delta_alpha_x + } else { + if box_row_head == row_end { break } + box_row_head = box_row_head + delta_row + box_row_tail = box_row_head + if shape_extents.x == 0.0 { + box_column_tail = box_column_head + } else { + column_intercept = clamped_start.x + next_fraction_z * clamped_delta.x + box_column_tail = floor((column_intercept - 2.0 * sign_x * shape_extents.x) / scale.x) as int + } + next_fraction_z = next_fraction_z + delta_alpha_z + } + } + return result +} + +// One triangle of the field against the swept proxy, in the triangle's +// frame at its first vertex; the result taken if nearer. Returns the +// best fraction after. +cast_triangle(result: *CastOutput, proxy: ShapeProxy, translation: Vec3, can_encroach: bool, a: Vec3, b: Vec3, c: Vec3, + best_fraction: float, triangle_index: int, material: int) -> float { + tri = g_triangle as Vec3[] + tri[0] = math.vec3_zero() + tri[1] = math.sub(b, a) + tri[2] = math.sub(c, a) + pair = ShapeCastPairInput { proxy_a: distance.shape_proxy(g_triangle, 3, 0.0), proxy_b: proxy, + transform: Transform { p: math.neg(a), q: math.quat_identity() }, + translation_b: translation, max_fraction: best_fraction, can_encroach: can_encroach } + output = distance.shape_cast(&pair) + if output.hit { + result.normal = output.normal + result.point = math.add(output.point, a) + result.fraction = output.fraction + result.iterations = output.iterations + result.triangle_index = triangle_index + result.child_index = NULL_INDEX + result.material_index = material + result.hit = true + return output.fraction + } + return best_fraction +} + +// --- overlap, query, mover ---------------------------------------------------------------- + +// The cells a box covers, clamped to the field; false when none. +struct CellRange { + min_row: int + max_row: int + min_column: int + max_column: int +} + +cell_range(h: *HeightFieldData, bounds: AABB) -> CellRange { + scale = h.scale + return CellRange { min_row: floor(bounds.lower.z / scale.z) as int, max_row: floor(bounds.upper.z / scale.z) as int, + min_column: floor(bounds.lower.x / scale.x) as int, max_column: floor(bounds.upper.x / scale.x) as int } +} + +// Whether a proxy in world space comes within the overlap slop of any +// triangle under the field's transform. +overlap_height_field(h: *HeightFieldData, shape_transform: Transform, proxy: ShapeProxy) -> bool { + scratch_ready() + local_proxy = mesh.make_local_proxy(proxy, shape_transform, g_local) + bounds = mesh.compute_proxy_aabb(local_proxy) + range = cell_range(h, bounds) + center = math.aabb_center(bounds) + extent = math.aabb_extents(bounds) + input = DistanceInput { proxy_a: distance.shape_proxy(g_triangle, 3, 0.0), proxy_b: local_proxy, + transform: math.transform_identity(), use_radii: true } + cache = distance.empty_cache() + materials = height_field_material_indices(h) + corners = corners_ready() + tri = g_triangle as Vec3[] + tolerance = 0.1 * math.LINEAR_SLOP + row = range.min_row + while row <= range.max_row { + if row >= 0 && row < h.row_count - 1 { + column = range.min_column + while column <= range.max_column { + if column >= 0 && column < h.column_count - 1 { + cell = row * (h.column_count - 1) + column + if materials[cell] != HEIGHT_FIELD_HOLE { + cell_corners(h, row, column, corners) + if mesh.test_bounds_triangle_overlap(center, extent, corners[0], corners[2], corners[1]) { + tri[0] = corners[0] + tri[1] = corners[2] + tri[2] = corners[1] + cache.count = 0 + output = distance.shape_distance(&input, &cache, null, 0) + if output.distance < tolerance { return true } + } + if mesh.test_bounds_triangle_overlap(center, extent, corners[2], corners[3], corners[1]) { + tri[0] = corners[3] + tri[1] = corners[1] + tri[2] = corners[2] + cache.count = 0 + output = distance.shape_distance(&input, &cache, null, 0) + if output.distance < tolerance { return true } + } + } + } + column = column + 1 + } + } + row = row + 1 + } + return false +} + +// Every triangle of the cells a box covers, to the visitor, in index order. +query_height_field(h: *HeightFieldData, bounds: AABB, visitor: fn(Vec3, Vec3, Vec3, int, ptr) -> bool, context: ptr) { + range = cell_range(h, bounds) + materials = height_field_material_indices(h) + corners = corners_ready() + row = range.min_row + while row <= range.max_row { + if row >= 0 && row < h.row_count - 1 { + column = range.min_column + while column <= range.max_column { + if column >= 0 && column < h.column_count - 1 { + cell = row * (h.column_count - 1) + column + if materials[cell] != HEIGHT_FIELD_HOLE { + cell_corners(h, row, column, corners) + p11 = corners[0] + p12 = corners[1] + p21 = corners[2] + p22 = corners[3] + cell_bounds = AABB { lower: math.min_vec3(math.min_vec3(p11, p12), math.min_vec3(p21, p22)), + upper: math.max_vec3(math.max_vec3(p11, p12), math.max_vec3(p21, p22)) } + if math.aabb_overlaps(bounds, cell_bounds) { + triangle_index = 2 * cell + if h.clockwise { + if visitor(p11, p12, p21, triangle_index, context) == false { return } + if visitor(p22, p21, p12, triangle_index + 1, context) == false { return } + } else { + if visitor(p11, p21, p12, triangle_index, context) == false { return } + if visitor(p22, p12, p21, triangle_index + 1, context) == false { return } + } + } + } + } + column = column + 1 + } + } + row = row + 1 + } +} + +// The planes of the front-facing triangles the mover's capsule is within +// its radius of, up to the capacity; returns how many. +collide_mover_and_height_field(planes: PlaneResult[], capacity: int, h: *HeightFieldData, mover: Capsule) -> int { + if capacity == 0 { return 0 } + scratch_ready() + input = DistanceInput { proxy_a: distance.shape_proxy(g_triangle, 3, 0.0), proxy_b: distance.shape_proxy((&mover) as ptr, 2, 0.0), + transform: math.transform_identity(), use_radii: false } + cache = distance.empty_cache() + radius = mover.radius + center = math.lerp(mover.center1, mover.center2, 0.5) + bounds = math.aabb_inflate(AABB { lower: math.min_vec3(mover.center1, mover.center2), upper: math.max_vec3(mover.center1, mover.center2) }, radius) + bounds_center = math.aabb_center(bounds) + bounds_extent = math.aabb_extents(bounds) + range = cell_range(h, bounds) + materials = height_field_material_indices(h) + corners = corners_ready() + tri = g_triangle as Vec3[] + plane_count = 0 + row = range.min_row + while row <= range.max_row { + if row >= 0 && row < h.row_count - 1 { + column = range.min_column + while column <= range.max_column { + if column >= 0 && column < h.column_count - 1 { + cell = row * (h.column_count - 1) + column + material = materials[cell] + if material != HEIGHT_FIELD_HOLE { + cell_corners(h, row, column, corners) + p11 = corners[0] + p12 = corners[1] + p21 = corners[2] + p22 = corners[3] + if h.clockwise { + swap = p12 + p12 = p21 + p21 = swap + } + k = 0 + while k < 2 { + a = p11 + b = p21 + c = p12 + if k == 1 { + a = p22 + b = p12 + c = p21 + } + if mesh.test_bounds_triangle_overlap(bounds_center, bounds_extent, a, b, c) { + if math.signed_volume(a, b, c, center) >= 0.0 { + tri[0] = a + tri[1] = b + tri[2] = c + cache.count = 0 + output = distance.shape_distance(&input, &cache, null, 0) + if output.distance > 0.0 && output.distance <= radius { + planes[plane_count] = PlaneResult { plane: Plane { normal: output.normal, offset: radius - output.distance }, + point: output.point_a, triangle_index: 2 * cell + k, child_index: 0, + material_index: material } + plane_count = plane_count + 1 + if plane_count == capacity { return plane_count } + } + } + } + k = k + 1 + } + } + } + column = column + 1 + } + } + row = row + 1 + } + return plane_count +} diff --git a/aephysics/test_height_field.ae b/aephysics/test_height_field.ae new file mode 100644 index 0000000..9327e42 --- /dev/null +++ b/aephysics/test_height_field.ae @@ -0,0 +1,635 @@ +// aephysics.height_field against the reference's (Box3D's) +// test_height_field.c: the grid's bounds, the triangle index mapping on +// an asymmetric grid, the winding's normals, a ray onto a flat field, the +// overlap at the surface, the vertical casts straddling a cell boundary, +// the shape cast and the ray cast against a brute force over every +// triangle of a wave, and the back side culled for both windings; and +// beyond it the edge flags of a wave and of holes, the box query, the +// mover's planes, and a scaled field. The file roundtrip is not ported. + +import std.string +import aephysics.math +import aephysics.core +import aephysics.distance +import aephysics.manifold +import aephysics.triangle_manifold +import aephysics.mesh +import aephysics.height_field + +extern calloc(count: int, size: int) -> ptr +extern exit(code: int) +extern free(p: ptr) +extern sqrt(x: float) -> float + +var failures = 0 +var checks = 0 + +ensure(name: string, ok: bool) { + checks = checks + 1 + if !ok { + println("height_field: FAIL ${name}") + failures = failures + 1 + } +} + +small(name: string, value: float, tolerance: float) { + ensure("${name} (${value})", math.abs_float(value) < tolerance) +} + +// A flat 3 x 3 field at y = 0 with the given cell materials. +make_flat_field(materials: ptr, clockwise: bool) -> *HeightFieldData { + heights = calloc(9, 8) + def = height_field.height_field_def() + def.heights = heights + def.material_indices = materials + def.scale = math.vec3(1.0, 1.0, 1.0) + def.count_x = 3 + def.count_z = 3 + def.global_minimum_height = 0.0 - 1.0 + def.global_maximum_height = 1.0 + def.clockwise_winding = clockwise + h = height_field.create_height_field(&def) + free(heights) + return h +} + +test_create() { + h = height_field.create_grid(4, 4, math.vec3(1.0, 1.0, 1.0), false) + ensure("grid rows", h.row_count == 4) + ensure("grid columns", h.column_count == 4) + ensure("grid counter-clockwise", h.clockwise == false) + ensure("grid version", h.version == height_field.HEIGHT_FIELD_VERSION) + small("grid lower x", h.aabb.lower.x, math.EPSILON) + small("grid lower y", h.aabb.lower.y, math.EPSILON) + small("grid lower z", h.aabb.lower.z, math.EPSILON) + small("grid upper x", h.aabb.upper.x - 3.0, math.EPSILON) + small("grid upper y", h.aabb.upper.y, math.EPSILON) + small("grid upper z", h.aabb.upper.z - 3.0, math.EPSILON) + ensure("grid triangle count", height_field.height_field_triangle_count(h) == 18) + ensure("grid hash", h.hash != (0 as long)) + height_field.destroy_height_field(h) + + // An asymmetric grid catches a vertex stride (columns) confused with + // a cell stride (columns - 1). + row_count = 4 + column_count = 5 + h = height_field.create_grid(row_count, column_count, math.vec3(1.0, 1.0, 1.0), false) + triangle_count = 2 * (row_count - 1) * (column_count - 1) + bad = 0 + triangle_index = 0 + while triangle_index < triangle_count { + quad = triangle_index >> 1 + row = quad / (column_count - 1) + column = quad - row * (column_count - 1) + index11 = row * column_count + column + index12 = index11 + 1 + index21 = (row + 1) * column_count + column + index22 = index21 + 1 + t = height_field.get_height_field_triangle(h, triangle_index) + if (triangle_index & 1) == 0 { + if t.i1 != index11 || t.i2 != index21 || t.i3 != index12 { bad = bad + 1 } + } else { + if t.i1 != index22 || t.i2 != index12 || t.i3 != index21 { bad = bad + 1 } + } + // The vertices are where the indices say. + if t.v1.x != (t.i1 % column_count) as float || t.v1.z != (t.i1 / column_count) as float { bad = bad + 1 } + triangle_index = triangle_index + 1 + } + ensure("triangle indices on a 4 x 5 grid", bad == 0) + height_field.destroy_height_field(h) + + // The same flat field wound both ways: the normal of triangle 0 flips. + materials = calloc(4, 4) + ccw = make_flat_field(materials, false) + cw = make_flat_field(materials, true) + ta = height_field.get_height_field_triangle(ccw, 0) + tb = height_field.get_height_field_triangle(cw, 0) + na = math.make_normal_from_points(ta.v1, ta.v2, ta.v3) + nb = math.make_normal_from_points(tb.v1, tb.v2, tb.v3) + small("ccw normal x", na.x, math.EPSILON) + small("ccw normal y", na.y - 1.0, math.EPSILON) + small("ccw normal z", na.z, math.EPSILON) + small("cw normal x", nb.x, math.EPSILON) + small("cw normal y", nb.y + 1.0, math.EPSILON) + small("cw normal z", nb.z, math.EPSILON) + ensure("cw indices swapped", tb.i1 == ta.i1 && tb.i2 == ta.i3 && tb.i3 == ta.i2) + height_field.destroy_height_field(ccw) + height_field.destroy_height_field(cw) + free(materials) + + // Heights quantise to the global range and come back within a quantum; + // a height beyond the range is clamped. + heights_block = calloc(9, 8) + heights = heights_block as float[] + heights[4] = 0.75 + heights[8] = 3.0 + def = height_field.height_field_def() + def.heights = heights_block + def.scale = math.vec3(2.0, 0.5, 2.0) + def.count_x = 3 + def.count_z = 3 + h = height_field.create_height_field(&def) + small("quantised height", height_field.height_field_height(h, 4) - 0.75, 2.0 / 65535.0) + small("clamped height", height_field.height_field_height(h, 8) - 1.0, 0.000001) + small("scaled upper y", h.aabb.upper.y - 0.5, 0.000001) + small("scaled upper x", h.aabb.upper.x - 4.0, 0.000001) + small("scaled lower y", h.aabb.lower.y, 0.000001) + t = height_field.get_height_field_triangle(h, 7) + small("scaled vertex", t.v1.y - 0.5, 0.000001) + ensure("materials default to 0", height_field.get_height_field_material(h, 3) == 0) + height_field.destroy_height_field(h) + def.count_x = 1 + ensure("a one-column field is refused", height_field.create_height_field(&def) == null) + def.count_x = 3 + def.heights = null + ensure("a field without heights is refused", height_field.create_height_field(&def) == null) + free(heights_block) +} + +test_flags() { + // A flat field: every interior edge is flat, so it carries both bits; + // an edge on the border or against a hole carries none. + materials_block = calloc(9, 4) + materials = materials_block as int[] + materials[4] = height_field.HEIGHT_FIELD_HOLE + heights = calloc(16, 8) + def = height_field.height_field_def() + def.heights = heights + def.material_indices = materials_block + def.count_x = 4 + def.count_z = 4 + h = height_field.create_height_field(&def) + flags = height_field.height_field_flags(h) + // Cell 0 (corner): triangle 0 has the diagonal (edge 2) flat and + // edges 1 and 3 on the border. + ensure("corner triangle 0 flags", flags[0] == (triangle_manifold.CONCAVE_EDGE2 | triangle_manifold.INVERSE_CONCAVE_EDGE2)) + // Cell 0 triangle 1: diagonal flat, edge 1 against cell 1 (flat), edge 3 against cell 3 (flat). + ensure("corner triangle 1 flags", flags[1] == triangle_manifold.ALL_FLAT_EDGES) + // Cell 1 triangle 1: edge 3 against the hole (cell 4). + ensure("triangle above the hole", flags[3] == (triangle_manifold.ALL_FLAT_EDGES & (255 - triangle_manifold.CONCAVE_EDGE3 - triangle_manifold.INVERSE_CONCAVE_EDGE3))) + // Cell 5 triangle 0: edge 1 against the hole. + ensure("triangle right of the hole", flags[10] == (triangle_manifold.ALL_FLAT_EDGES & (255 - triangle_manifold.CONCAVE_EDGE1 - triangle_manifold.INVERSE_CONCAVE_EDGE1))) + ensure("the hole's triangles carry no flags", flags[8] == 0 && flags[9] == 0) + ensure("a hole's material", height_field.get_height_field_material(h, 9) == height_field.HEIGHT_FIELD_HOLE) + height_field.destroy_height_field(h) + + // A ridge along z at column 1: the edges against it are concave on + // the outside faces... the ridge's own faces meet convexly at the top. + hs = heights as float[] + hs[1] = 1.0 + hs[5] = 1.0 + hs[9] = 1.0 + hs[13] = 1.0 + materials[4] = 0 + h = height_field.create_height_field(&def) + flags = height_field.height_field_flags(h) + // Cell 0 triangle 1 (its edge 1 is x = 1, the ridge's crest, against cell 1 sloping down): inverse concave only. + ensure("convex ridge edge", (flags[1] & triangle_manifold.CONCAVE_EDGE1) == 0 && (flags[1] & triangle_manifold.INVERSE_CONCAVE_EDGE1) != 0) + // Cell 1 triangle 1 (edge 1 is x = 2, at the foot of the ridge): concave, not inverse. + ensure("concave foot edge", (flags[3] & triangle_manifold.CONCAVE_EDGE1) != 0 && (flags[3] & triangle_manifold.INVERSE_CONCAVE_EDGE1) == 0) + // The clockwise field swaps edges 1 and 3 on the returned triangle. + def.clockwise_winding = true + cw = height_field.create_height_field(&def) + t = height_field.get_height_field_triangle(cw, 3) + swapped = (flags[3] & 0x22) | ((flags[3] & 0x11) << 2) | ((flags[3] & 0x44) >> 2) + ensure("clockwise swaps the edge bits", t.flags == swapped && t.flags != flags[3]) + ensure("the concave foot edge became edge 3", (t.flags & triangle_manifold.CONCAVE_EDGE3) != 0 && (t.flags & triangle_manifold.INVERSE_CONCAVE_EDGE3) == 0) + height_field.destroy_height_field(cw) + height_field.destroy_height_field(h) + free(heights) + free(materials_block) +} + +test_ray_and_overlap() { + // A flat field with a tight range recovers y = 0 to a hundred-thousandth. + heights = calloc(16, 8) + materials = calloc(9, 4) + def = height_field.height_field_def() + def.heights = heights + def.material_indices = materials + def.count_x = 4 + def.count_z = 4 + h = height_field.create_height_field(&def) + out = height_field.ray_cast_height_field(h, math.vec3(1.25, 10.0, 1.25), math.vec3(0.0, 0.0 - 20.0, 0.0), 1.0) + ensure("flat ray hit", out.hit) + small("flat ray fraction", out.fraction - 0.5, 0.00001) + small("flat ray normal x", out.normal.x, 0.00001) + small("flat ray normal y", out.normal.y - 1.0, 0.00001) + small("flat ray normal z", out.normal.z, 0.00001) + ensure("flat ray triangle", out.triangle_index == 2 * (1 * 3 + 1)) + small("flat ray point", math.length(math.sub(out.point, math.vec3(1.25, 0.0, 1.25))), 0.0001) + // A ray that never reaches the field, and one outside it. + out = height_field.ray_cast_height_field(h, math.vec3(1.25, 10.0, 1.25), math.vec3(0.0, 0.0 - 5.0, 0.0), 1.0) + ensure("short ray misses", out.hit == false) + out = height_field.ray_cast_height_field(h, math.vec3(5.0, 10.0, 1.25), math.vec3(0.0, 0.0 - 20.0, 0.0), 1.0) + ensure("ray beside the field misses", out.hit == false) + // The world-space bounds under a transform. + t = Transform { p: math.vec3(10.0, 0.0, 0.0), q: math.quat_identity() } + bounds = height_field.compute_height_field_aabb(h, t) + small("transformed lower x", bounds.lower.x - 10.0, 0.000001) + small("transformed upper x", bounds.upper.x - 13.0, 0.000001) + height_field.destroy_height_field(h) + free(heights) + free(materials) + + // A sphere a diameter above the grid is clear; centred on it, it overlaps. + h = height_field.create_grid(4, 4, math.vec3(1.0, 1.0, 1.0), false) + center_block = calloc(1, sizeof(Vec3)) + center = center_block as Vec3[] + center[0] = math.vec3(1.5, 1.0, 1.5) + ensure("sphere above is clear", height_field.overlap_height_field(h, math.transform_identity(), distance.shape_proxy(center_block, 1, 0.5)) == false) + center[0] = math.vec3(1.5, 0.0, 1.5) + ensure("sphere at the surface overlaps", height_field.overlap_height_field(h, math.transform_identity(), distance.shape_proxy(center_block, 1, 0.5))) + // The field moved up by 2 under its transform: the sphere at y 1 is + // clear again from below... it is 1 below the surface, so inside. + t = Transform { p: math.vec3(0.0, 0.0 - 2.0, 0.0), q: math.quat_identity() } + center[0] = math.vec3(1.5, 1.0, 1.5) + ensure("sphere under the moved field is clear", height_field.overlap_height_field(h, t, distance.shape_proxy(center_block, 1, 0.5)) == false) + center[0] = math.vec3(1.5, 0.0 - 2.0, 1.5) + ensure("sphere at the moved surface overlaps", height_field.overlap_height_field(h, t, distance.shape_proxy(center_block, 1, 0.5))) + free(center_block) + height_field.destroy_height_field(h) +} + +test_straddle() { + // A vertical cast whose swept box straddles a cell boundary must test + // every cell it covers: the field is flat with only cell (0, 0) solid, + // and each sphere's centre is nudged just past one of its boundaries, + // leaving the solid cell on the trailing side of the walk. + materials_block = calloc(4, 4) + materials = materials_block as int[] + materials[1] = height_field.HEIGHT_FIELD_HOLE + materials[2] = height_field.HEIGHT_FIELD_HOLE + materials[3] = height_field.HEIGHT_FIELD_HOLE + h = make_flat_field(materials_block, false) + radius = 0.3 + center_block = calloc(1, sizeof(Vec3)) + center = center_block as Vec3[] + down = math.vec3(0.0, 0.0 - 20.0, 0.0) + // Across x = 1: contact on the cell edge, sqrt(0.05^2 + cy^2) = radius. + center[0] = math.vec3(1.05, 10.0, 0.5) + out = height_field.shape_cast_height_field(h, distance.shape_proxy(center_block, 1, radius), down, 1.0, false) + ensure("straddle x hit", out.hit) + small("straddle x fraction", out.fraction - 0.4852098, 0.002) + center[0] = math.vec3(0.5, 10.0, 1.05) + out = height_field.shape_cast_height_field(h, distance.shape_proxy(center_block, 1, radius), down, 1.0, false) + ensure("straddle z hit", out.hit) + small("straddle z fraction", out.fraction - 0.4852098, 0.002) + // Across the corner: contact on the vertex. + center[0] = math.vec3(1.05, 10.0, 1.05) + out = height_field.shape_cast_height_field(h, distance.shape_proxy(center_block, 1, radius), down, 1.0, false) + ensure("straddle corner hit", out.hit) + small("straddle corner fraction", out.fraction - 0.4854226, 0.002) + free(center_block) + height_field.destroy_height_field(h) + free(materials_block) +} + +// The proxy against every triangle that is not a hole: the truth the grid +// walk is checked against. +brute_force_shape_cast(h: *HeightFieldData, proxy: ShapeProxy, translation: Vec3, tri_block: ptr) -> CastOutput { + best = distance.empty_cast_output() + best_fraction = 1.0 + tri = tri_block as Vec3[] + materials = height_field.height_field_material_indices(h) + count = height_field.height_field_triangle_count(h) + t = 0 + while t < count { + if materials[t >> 1] != height_field.HEIGHT_FIELD_HOLE { + triangle = height_field.get_height_field_triangle(h, t) + tri[0] = triangle.v1 + tri[1] = triangle.v2 + tri[2] = triangle.v3 + pair = ShapeCastPairInput { proxy_a: distance.shape_proxy(tri_block, 3, 0.0), proxy_b: proxy, transform: math.transform_identity(), + translation_b: translation, max_fraction: best_fraction, can_encroach: false } + out = distance.shape_cast(&pair) + if out.hit && out.fraction < best_fraction { + best_fraction = out.fraction + best = out + best.triangle_index = t + } + } + t = t + 1 + } + return best +} + +brute_force_ray_cast(h: *HeightFieldData, origin: Vec3, translation: Vec3) -> CastOutput { + best = distance.empty_cast_output() + best_fraction = 1.0 + materials = height_field.height_field_material_indices(h) + count = height_field.height_field_triangle_count(h) + t = 0 + while t < count { + if materials[t >> 1] != height_field.HEIGHT_FIELD_HOLE { + triangle = height_field.get_height_field_triangle(h, t) + alpha = mesh.intersect_ray_triangle(origin, translation, triangle.v1, triangle.v2, triangle.v3) + if alpha < best_fraction { + best_fraction = alpha + best.hit = true + best.fraction = alpha + best.triangle_index = t + } + } + t = t + 1 + } + return best +} + +test_brute_force() { + h = height_field.create_wave(10, 10, math.vec3(2.0, 1.5, 2.0), 0.1, 0.03333, false) + origin_block = calloc(1, sizeof(Vec3)) + origin = origin_block as Vec3[] + tri_block = calloc(3, sizeof(Vec3)) + + // The sample's repro: a sphere moving only in z and y, where the walk + // used to stop a row early by comparing fractions of different sweeps. + origin[0] = math.vec3(14.5, 4.0, 11.913) + translation = math.vec3(0.0, 0.0 - 8.0, 6.397) + proxy = distance.shape_proxy(origin_block, 1, 0.2) + grid = height_field.shape_cast_height_field(h, proxy, translation, 1.0, false) + brute = brute_force_shape_cast(h, proxy, translation, tri_block) + ensure("repro brute hit", brute.hit) + ensure("repro grid hit", grid.hit == brute.hit) + small("repro fraction", grid.fraction - brute.fraction, 0.002) + + // Origins across the field with assorted directions and radii. + radii_block = calloc(3, 8) + radii = radii_block as float[] + radii[0] = 0.15 + radii[1] = 0.4 + radii[2] = 0.9 + deltas_block = calloc(8, sizeof(Vec3)) + deltas = deltas_block as Vec3[] + deltas[0] = math.vec3(0.0, 0.0 - 8.0, 0.0) + deltas[1] = math.vec3(0.0, 0.0 - 8.0, 6.4) + deltas[2] = math.vec3(5.1, 0.0 - 8.0, 0.0) + deltas[3] = math.vec3(0.0, 0.0 - 8.0, 0.0 - 6.4) + deltas[4] = math.vec3(0.0 - 5.1, 0.0 - 8.0, 0.0) + deltas[5] = math.vec3(6.0, 0.0 - 8.0, 5.0) + deltas[6] = math.vec3(0.0 - 7.0, 0.0 - 8.0, 4.0) + deltas[7] = math.vec3(9.0, 0.0 - 3.0, 0.0 - 9.0) + mismatches = 0 + hits = 0 + xi = 0 + while xi < 5 { + zi = 0 + while zi < 5 { + // The nudge keeps the swept box straddling cell boundaries. + origin[0] = math.vec3(1.0 + 4.0 * (xi as float) + 0.05, 4.0, 1.0 + 4.0 * (zi as float) + 0.05) + di = 0 + while di < 8 { + ri = 0 + while ri < 3 { + proxy = distance.shape_proxy(origin_block, 1, radii[ri]) + grid = height_field.shape_cast_height_field(h, proxy, deltas[di], 1.0, false) + brute = brute_force_shape_cast(h, proxy, deltas[di], tri_block) + diff = math.abs_float(grid.fraction - brute.fraction) + if grid.hit != brute.hit || (brute.hit && diff > 0.002) { + println(" mismatch: origin ${origin[0].x} ${origin[0].z} delta ${di} r ${radii[ri]} grid ${grid.hit} ${grid.fraction} brute ${brute.hit} ${brute.fraction} tri ${brute.triangle_index}") + mismatches = mismatches + 1 + } + if brute.hit { hits = hits + 1 } + ri = ri + 1 + } + di = di + 1 + } + zi = zi + 1 + } + xi = xi + 1 + } + ensure("shape casts match the brute force over the wave", mismatches == 0) + ensure("most shape casts hit (${hits} of 600)", hits > 400) + + // The ray cast goes through the same walk with a point proxy. + deltas[0] = math.vec3(0.0, 0.0 - 8.0, 0.0) + deltas[1] = math.vec3(0.0, 0.0 - 8.0, 12.0) + deltas[2] = math.vec3(12.0, 0.0 - 8.0, 0.0) + deltas[3] = math.vec3(0.0, 0.0 - 8.0, 0.0 - 12.0) + deltas[4] = math.vec3(0.0 - 12.0, 0.0 - 8.0, 0.0) + deltas[5] = math.vec3(14.0, 0.0 - 8.0, 11.0) + deltas[6] = math.vec3(0.0 - 13.0, 0.0 - 8.0, 9.0) + deltas[7] = math.vec3(16.0, 0.0 - 4.0, 0.0 - 15.0) + mismatches = 0 + hits = 0 + xi = 0 + while xi < 5 { + zi = 0 + while zi < 5 { + ray_origin = math.vec3(1.0 + 4.0 * (xi as float) + 0.05, 4.0, 1.0 + 4.0 * (zi as float) + 0.05) + di = 0 + while di < 8 { + grid = height_field.ray_cast_height_field(h, ray_origin, deltas[di], 1.0) + brute = brute_force_ray_cast(h, ray_origin, deltas[di]) + diff = math.abs_float(grid.fraction - brute.fraction) + if grid.hit != brute.hit || (brute.hit && diff > 0.0001) || (brute.hit && grid.triangle_index != brute.triangle_index) { + println(" mismatch: origin ${ray_origin.x} ${ray_origin.z} delta ${di} grid ${grid.hit} ${grid.fraction} tri ${grid.triangle_index} brute ${brute.hit} ${brute.fraction} tri ${brute.triangle_index}") + mismatches = mismatches + 1 + } + if brute.hit { hits = hits + 1 } + di = di + 1 + } + zi = zi + 1 + } + xi = xi + 1 + } + ensure("ray casts match the brute force over the wave", mismatches == 0) + ensure("most rays hit (${hits} of 200)", hits > 100) + + // A field with holes: the brute force skips them the same way. + height_field.destroy_height_field(h) + h = height_field.create_wave(10, 10, math.vec3(2.0, 1.5, 2.0), 0.1, 0.03333, true) + mismatches = 0 + xi = 0 + while xi < 5 { + zi = 0 + while zi < 5 { + ray_origin = math.vec3(1.0 + 4.0 * (xi as float) + 0.05, 4.0, 1.0 + 4.0 * (zi as float) + 0.05) + di = 0 + while di < 8 { + grid = height_field.ray_cast_height_field(h, ray_origin, deltas[di], 1.0) + brute = brute_force_ray_cast(h, ray_origin, deltas[di]) + if grid.hit != brute.hit || (brute.hit && grid.triangle_index != brute.triangle_index) { mismatches = mismatches + 1 } + di = di + 1 + } + zi = zi + 1 + } + xi = xi + 1 + } + ensure("ray casts match the brute force with holes", mismatches == 0) + height_field.destroy_height_field(h) + free(deltas_block) + free(radii_block) + free(tri_block) + free(origin_block) +} + +test_backside() { + materials_block = calloc(4, 4) + materials = materials_block as int[] + materials[0] = 1 + center_block = calloc(1, sizeof(Vec3)) + center = center_block as Vec3[] + down = math.vec3(0.0, 0.0 - 10.0, 0.0) + up = math.vec3(0.0, 10.0, 0.0) + + // Falling onto the front face reports the hit, the triangle and its + // material; rising from behind is culled. + h = make_flat_field(materials_block, false) + center[0] = math.vec3(0.3, 5.0, 0.25) + out = height_field.shape_cast_height_field(h, distance.shape_proxy(center_block, 1, 0.2), down, 1.0, false) + ensure("front shape hit", out.hit) + small("front shape fraction", out.fraction - 0.48, 0.01) + ensure("front shape normal", out.normal.y > 0.99) + ensure("front shape triangle", out.triangle_index == 0 || out.triangle_index == 1) + ensure("front shape material", out.material_index == 1) + center[0] = math.vec3(0.3, 0.0 - 5.0, 0.25) + out = height_field.shape_cast_height_field(h, distance.shape_proxy(center_block, 1, 0.2), up, 1.0, false) + ensure("back shape culled", out.hit == false) + out = height_field.ray_cast_height_field(h, math.vec3(0.3, 5.0, 0.25), down, 1.0) + ensure("front ray hit", out.hit) + small("front ray fraction", out.fraction - 0.5, 0.001) + ensure("front ray normal", out.normal.y > 0.99) + ensure("front ray triangle", out.triangle_index == 0 || out.triangle_index == 1) + ensure("front ray material", out.material_index == 1) + out = height_field.ray_cast_height_field(h, math.vec3(0.3, 0.0 - 5.0, 0.25), up, 1.0) + ensure("back ray culled", out.hit == false) + height_field.destroy_height_field(h) + + // A clockwise field faces down: the rising sphere hits, the falling one is culled. + h = make_flat_field(materials_block, true) + center[0] = math.vec3(0.3, 0.0 - 5.0, 0.25) + out = height_field.shape_cast_height_field(h, distance.shape_proxy(center_block, 1, 0.2), up, 1.0, false) + ensure("clockwise shape hit", out.hit) + small("clockwise shape fraction", out.fraction - 0.48, 0.01) + ensure("clockwise shape normal", out.normal.y < 0.0 - 0.99) + ensure("clockwise shape triangle", out.triangle_index == 0 || out.triangle_index == 1) + ensure("clockwise shape material", out.material_index == 1) + center[0] = math.vec3(0.3, 5.0, 0.25) + out = height_field.shape_cast_height_field(h, distance.shape_proxy(center_block, 1, 0.2), down, 1.0, false) + ensure("clockwise shape culled", out.hit == false) + out = height_field.ray_cast_height_field(h, math.vec3(0.3, 0.0 - 5.0, 0.25), up, 1.0) + ensure("clockwise ray hit", out.hit) + small("clockwise ray fraction", out.fraction - 0.5, 0.001) + ensure("clockwise ray normal", out.normal.y < 0.0 - 0.99) + ensure("clockwise ray triangle", out.triangle_index == 0 || out.triangle_index == 1) + ensure("clockwise ray material", out.material_index == 1) + out = height_field.ray_cast_height_field(h, math.vec3(0.3, 5.0, 0.25), down, 1.0) + ensure("clockwise ray culled", out.hit == false) + height_field.destroy_height_field(h) + free(center_block) + free(materials_block) +} + +var query_count = 0 +var query_index_sum = 0 +var query_normal_sum = 0.0 + +count_triangle(a: Vec3, b: Vec3, c: Vec3, triangle_index: int, context: ptr) -> bool { + query_count = query_count + 1 + query_index_sum = query_index_sum + triangle_index + query_normal_sum = query_normal_sum + math.make_normal_from_points(a, b, c).y + return true +} + +stop_at_first(a: Vec3, b: Vec3, c: Vec3, triangle_index: int, context: ptr) -> bool { + query_count = query_count + 1 + return false +} + +test_query_and_mover() { + h = height_field.create_grid(5, 5, math.vec3(1.0, 1.0, 1.0), false) + // A box within row 1, column 2 gets that cell's two triangles (cell 6: 12 and 13). + query_count = 0 + query_index_sum = 0 + query_normal_sum = 0.0 + height_field.query_height_field(h, AABB { lower: math.vec3(2.2, 0.0 - 0.1, 1.2), upper: math.vec3(2.8, 0.1, 1.8) }, count_triangle, null) + ensure("one cell's triangles", query_count == 2) + ensure("their indices", query_index_sum == 12 + 13) + small("they face up", query_normal_sum - 2.0, 0.000001) + // A box over the whole field gets all 32; beside it, none; above it, none. + query_count = 0 + height_field.query_height_field(h, AABB { lower: math.vec3(0.0 - 1.0, 0.0 - 1.0, 0.0 - 1.0), upper: math.vec3(5.0, 1.0, 5.0) }, count_triangle, null) + ensure("every triangle", query_count == 32) + query_count = 0 + height_field.query_height_field(h, AABB { lower: math.vec3(6.0, 0.0 - 1.0, 0.0), upper: math.vec3(7.0, 1.0, 1.0) }, count_triangle, null) + ensure("beside the field", query_count == 0) + query_count = 0 + height_field.query_height_field(h, AABB { lower: math.vec3(1.0, 2.0, 1.0), upper: math.vec3(2.0, 3.0, 2.0) }, count_triangle, null) + ensure("above the field", query_count == 0) + // The visitor can stop the query. + query_count = 0 + height_field.query_height_field(h, AABB { lower: math.vec3(0.0 - 1.0, 0.0 - 1.0, 0.0 - 1.0), upper: math.vec3(5.0, 1.0, 5.0) }, stop_at_first, null) + ensure("the visitor stops the query", query_count == 1) + height_field.destroy_height_field(h) + + // A clockwise field's query winds the triangles to face down. + materials_block = calloc(4, 4) + cw = make_flat_field(materials_block, true) + query_count = 0 + query_normal_sum = 0.0 + height_field.query_height_field(cw, AABB { lower: math.vec3(0.0 - 1.0, 0.0 - 1.0, 0.0 - 1.0), upper: math.vec3(3.0, 1.0, 3.0) }, count_triangle, null) + ensure("clockwise query count", query_count == 8) + small("clockwise query faces down", query_normal_sum + 8.0, 0.000001) + height_field.destroy_height_field(cw) + free(materials_block) + + // The mover: a capsule standing on the flat grid within its radius + // gets the planes of the triangles under it, pointing up, with the + // penetration as the offset; hovering, none; a clockwise field + // (facing down) gives it nothing from above. + h = height_field.create_grid(5, 5, math.vec3(1.0, 1.0, 1.0), false) + planes_block = calloc(8, sizeof(PlaneResult)) + planes = planes_block as PlaneResult[] + mover = manifold.capsule(math.vec3(2.5, 0.2, 2.5), math.vec3(2.5, 1.2, 2.5), 0.3) + count = height_field.collide_mover_and_height_field(planes, 8, h, mover) + ensure("mover planes (${count})", count == 2) + if count == 2 { + small("mover plane normal", planes[0].plane.normal.y - 1.0, 0.0001) + // The grid's quantum (512 / 65535) puts its surface 0.0039 under y = 0. + small("mover plane offset", planes[0].plane.offset - 0.1, 0.01) + ensure("mover triangle", planes[0].triangle_index == 20 && planes[1].triangle_index == 21) + ensure("mover material", planes[0].material_index == 0) + } + mover = manifold.capsule(math.vec3(2.5, 1.0, 2.5), math.vec3(2.5, 2.0, 2.5), 0.3) + ensure("hovering mover", height_field.collide_mover_and_height_field(planes, 8, h, mover) == 0) + mover = manifold.capsule(math.vec3(2.5, 0.2, 2.5), math.vec3(2.5, 1.2, 2.5), 0.3) + ensure("mover capacity", height_field.collide_mover_and_height_field(planes, 1, h, mover) == 1) + // A capsule spanning a corner of four cells gets up to eight planes. + mover = manifold.capsule(math.vec3(2.0, 0.2, 2.0), math.vec3(2.0, 1.2, 2.0), 0.3) + count = height_field.collide_mover_and_height_field(planes, 8, h, mover) + ensure("mover at a corner (${count})", count >= 4) + height_field.destroy_height_field(h) + materials_block = calloc(4, 4) + cw = make_flat_field(materials_block, true) + mover = manifold.capsule(math.vec3(1.0, 0.2, 1.0), math.vec3(1.0, 1.2, 1.0), 0.3) + ensure("clockwise field ignores the mover above", height_field.collide_mover_and_height_field(planes, 8, cw, mover) == 0) + mover = manifold.capsule(math.vec3(1.0, 0.0 - 1.2, 1.0), math.vec3(1.0, 0.0 - 0.2, 1.0), 0.3) + count = height_field.collide_mover_and_height_field(planes, 8, cw, mover) + ensure("clockwise field holds the mover below (${count})", count >= 1) + if count >= 1 { small("clockwise mover normal", planes[0].plane.normal.y + 1.0, 0.0001) } + height_field.destroy_height_field(cw) + free(materials_block) + free(planes_block) +} + +main() { + before = core.alloc_count() + test_create() + test_flags() + test_ray_and_overlap() + test_straddle() + test_brute_force() + test_backside() + test_query_and_mover() + // The module's scratch stays allocated: three blocks. + ensure("every other counted allocation was freed (${core.alloc_count() - before})", core.alloc_count() == before + 3) + + println("height_field: ${checks} checks") + if failures == 0 { + println("height_field: all checks passed") + } else { + println("height_field: ${failures} failure(s)") + exit(1) + } +} diff --git a/bench/RESULTS.md b/bench/RESULTS.md index d384f23..11d213c 100644 --- a/bench/RESULTS.md +++ b/bench/RESULTS.md @@ -216,3 +216,32 @@ positives the query permits). The build is 1.2-1.5x, with the welding map and the edge map through core's LongMap; the traversals are 1.7-2x, the reference's SIMD box tests against scalar ones on 48-byte double boxes, the same gap the dynamic tree's ray cast showed. + +## height_field + +`bench/height_field.ae` and `bench/height_field_box3d.c`: a 512 x 512 +wave field of 522,242 triangles with a hole every sixteenth cell, built +ten times; 100,000 rays cast down onto it at slight angles; 100,000 box +queries over it; 10,000 sphere shape casts onto it; 10,000 sphere +overlaps at its surface. + +| phase | aephysics | Box3D | +|---|---|---| +| 10 builds (522,242 triangles) | 200 ms | **131** | +| 100,000 ray casts | 16.0 | **11.3** | +| 100,000 box queries | **8.3** | 9.0 | +| 10,000 shape casts | 49.5 | **23.5** | +| 10,000 overlaps | 3.7 | **2.0** | + +The same fields come out: every ray hits on both (94,058, the sum of hit +heights within 0.1%, the quantised heights held as doubles here and +floats there), every shape cast hits on both with equal fraction sums, +the overlaps agree (3,160), the box query reports 2,171,942 triangles +here against 2,171,914 there (0.001% more, boundary cases of the cell +bounds test). The build is 1.5x, the quantisation and the edge flags of +half a million triangles; the ray walk 1.4x; the box query at parity +(no SIMD in the reference's); the shape cast 2.1x, the GJK cast per +straddled cell on top of distance's 1.3-1.5x; the overlap 1.85x. The +field is 4.2 MB here against 1.3 MB there: the reference packs 16-bit +heights and 8-bit materials and flags, which Aether cannot yet address, +so they are ints. diff --git a/bench/height_field.ae b/bench/height_field.ae new file mode 100644 index 0000000..b77638b --- /dev/null +++ b/bench/height_field.ae @@ -0,0 +1,104 @@ +// The height field on the same scenes as bench/height_field_box3d.c: a +// 512 x 512 wave field (522,242 triangles, a hole every sixteenth cell) +// built ten times; 100,000 ray casts down onto it; 100,000 box queries +// over it; 10,000 sphere shape casts onto it; 10,000 sphere overlaps at +// its surface. Single thread, wall time per phase, with the hit counts +// and sums as the checksum. +import std.string +import std.os +import aephysics.math +import aephysics.distance +import aephysics.height_field + +extern calloc(count: int, size: int) -> ptr +extern free(p: ptr) +extern sin(x: float) -> float +extern cos(x: float) -> float + +clock() -> long { return os.now_monotonic_ns() } +ms(ns: long) -> float { return (ns as float) / 1000000.0 } + +const RAYS = 100000 +const QUERIES = 100000 +const CASTS = 10000 +const OVERLAPS = 10000 + +var query_hits = 0 + +count_triangle(a: Vec3, b: Vec3, c: Vec3, triangle_index: int, context: ptr) -> bool { + query_hits = query_hits + 1 + return true +} + +main() { + scale = math.vec3(0.5, 2.0, 0.5) + t0 = clock() + data = height_field.create_wave(512, 512, scale, 0.02, 0.031, true) + i = 1 + while i < 10 { + height_field.destroy_height_field(data) + data = height_field.create_wave(512, 512, scale, 0.02, 0.031, true) + i = i + 1 + } + t1 = clock() + + ray_hits = 0 + ray_sum = 0.0 + i = 0 + while i < RAYS { + t = (i as float) / (RAYS as float) + origin = math.vec3(1.0 + 254.0 * t, 5.0, 128.0 + 120.0 * sin(40.0 * t)) + translation = math.vec3(3.0 * cos(7.0 * t), 0.0 - 10.0, 3.0 * sin(9.0 * t)) + out = height_field.ray_cast_height_field(data, origin, translation, 1.0) + if out.hit { + ray_hits = ray_hits + 1 + ray_sum = ray_sum + out.point.y + } + i = i + 1 + } + t2 = clock() + + query_hits = 0 + i = 0 + while i < QUERIES { + t = (i as float) / (QUERIES as float) + c = math.vec3(2.0 + 252.0 * t, 0.3 * sin(3.0 * t), 128.0 + 120.0 * cos(30.0 * t)) + h = math.vec3(0.6, 2.5, 0.6) + bounds = AABB { lower: math.sub(c, h), upper: math.add(c, h) } + height_field.query_height_field(data, bounds, count_triangle, null) + i = i + 1 + } + t3 = clock() + + cast_hits = 0 + cast_sum = 0.0 + start_block = calloc(1, sizeof(Vec3)) + start = start_block as Vec3[] + i = 0 + while i < CASTS { + t = (i as float) / (CASTS as float) + start[0] = math.vec3(2.0 + 252.0 * t, 5.0, 128.0 + 120.0 * sin(50.0 * t)) + translation = math.vec3(4.0 * cos(11.0 * t), 0.0 - 10.0, 4.0 * sin(13.0 * t)) + out = height_field.shape_cast_height_field(data, distance.shape_proxy(start_block, 1, 0.3), translation, 1.0, false) + if out.hit { + cast_hits = cast_hits + 1 + cast_sum = cast_sum + out.fraction + } + i = i + 1 + } + t4 = clock() + + overlap_hits = 0 + i = 0 + while i < OVERLAPS { + t = (i as float) / (OVERLAPS as float) + start[0] = math.vec3(2.0 + 252.0 * t, 1.0 * sin(17.0 * t), 128.0 + 120.0 * cos(23.0 * t)) + if height_field.overlap_height_field(data, math.transform_identity(), distance.shape_proxy(start_block, 1, 0.5)) { overlap_hits = overlap_hits + 1 } + i = i + 1 + } + t5 = clock() + + println("aephysics height_field: 10 builds ${ms(t1 - t0)} ms (${height_field.height_field_triangle_count(data)} triangles, ${data.byte_count} bytes), ${RAYS} rays ${ms(t2 - t1)} ms (${ray_hits} hits, sum ${ray_sum}), ${QUERIES} queries ${ms(t3 - t2)} ms (${query_hits} hits), ${CASTS} casts ${ms(t4 - t3)} ms (${cast_hits} hits, sum ${cast_sum}), ${OVERLAPS} overlaps ${ms(t5 - t4)} ms (${overlap_hits} hits)") + free(start_block) + height_field.destroy_height_field(data) +} diff --git a/bench/height_field_box3d.c b/bench/height_field_box3d.c new file mode 100644 index 0000000..7b1191c --- /dev/null +++ b/bench/height_field_box3d.c @@ -0,0 +1,104 @@ +// The height field of the reference on the same scenes as +// bench/height_field.ae: a 512 x 512 wave field (522,242 triangles, a +// hole every sixteenth cell) built ten times; 100,000 ray casts down onto +// it; 100,000 box queries over it; 10,000 sphere shape casts onto it; +// 10,000 sphere overlaps at its surface. Single thread, wall time per +// phase, with the hit counts and sums as the checksum. +#include "box3d/collision.h" +#include "box3d/math_functions.h" + +#include +#include +#include + +static double now_ms( void ) +{ + struct timespec ts; + timespec_get( &ts, TIME_UTC ); + return ts.tv_sec * 1000.0 + ts.tv_nsec / 1.0e6; +} + +static int g_queryHits; +static bool count_triangle( b3Vec3 a, b3Vec3 b, b3Vec3 c, int triangleIndex, void* context ) +{ + (void)a; (void)b; (void)c; (void)triangleIndex; (void)context; + g_queryHits += 1; + return true; +} + +#define RAYS 100000 +#define QUERIES 100000 +#define CASTS 10000 +#define OVERLAPS 10000 + +int main( void ) +{ + b3Vec3 scale = { 0.5f, 2.0f, 0.5f }; + double t0 = now_ms(); + b3HeightFieldData* data = NULL; + for ( int i = 0; i < 10; ++i ) + { + if ( data ) b3DestroyHeightField( data ); + data = b3CreateWave( 512, 512, scale, 0.02f, 0.031f, true ); + } + double t1 = now_ms(); + + int rayHits = 0; + double raySum = 0.0; + for ( int i = 0; i < RAYS; ++i ) + { + float t = (float)i / (float)RAYS; + b3RayCastInput input = { { 1.0f + 254.0f * t, 5.0f, 128.0f + 120.0f * sinf( 40.0f * t ) }, { 3.0f * cosf( 7.0f * t ), -10.0f, 3.0f * sinf( 9.0f * t ) }, 1.0f }; + b3CastOutput out = b3RayCastHeightField( data, &input ); + if ( out.hit ) + { + rayHits += 1; + raySum += out.point.y; + } + } + double t2 = now_ms(); + + g_queryHits = 0; + for ( int i = 0; i < QUERIES; ++i ) + { + float t = (float)i / (float)QUERIES; + b3Vec3 c = { 2.0f + 252.0f * t, 0.3f * sinf( 3.0f * t ), 128.0f + 120.0f * cosf( 30.0f * t ) }; + b3Vec3 h = { 0.6f, 2.5f, 0.6f }; + b3AABB bounds = { b3Sub( c, h ), b3Add( c, h ) }; + b3QueryHeightField( data, bounds, count_triangle, NULL ); + } + double t3 = now_ms(); + + int castHits = 0; + double castSum = 0.0; + for ( int i = 0; i < CASTS; ++i ) + { + float t = (float)i / (float)CASTS; + b3Vec3 start = { 2.0f + 252.0f * t, 5.0f, 128.0f + 120.0f * sinf( 50.0f * t ) }; + b3ShapeCastInput input = { { &start, 1, 0.3f }, { 4.0f * cosf( 11.0f * t ), -10.0f, 4.0f * sinf( 13.0f * t ) }, 1.0f, false }; + b3CastOutput out = b3ShapeCastHeightField( data, &input ); + if ( out.hit ) + { + castHits += 1; + castSum += out.fraction; + } + } + double t4 = now_ms(); + + int overlapHits = 0; + for ( int i = 0; i < OVERLAPS; ++i ) + { + float t = (float)i / (float)OVERLAPS; + b3Vec3 center = { 2.0f + 252.0f * t, 1.0f * sinf( 17.0f * t ), 128.0f + 120.0f * cosf( 23.0f * t ) }; + b3ShapeProxy proxy = { ¢er, 1, 0.5f }; + if ( b3OverlapHeightField( data, b3Transform_identity, &proxy ) ) overlapHits += 1; + } + double t5 = now_ms(); + + printf( "box3d height_field: 10 builds %.1f ms (%d triangles, %d bytes), %d rays %.2f ms (%d hits, sum %.3f), %d queries %.2f ms (%d hits), " + "%d casts %.2f ms (%d hits, sum %.3f), %d overlaps %.2f ms (%d hits)\n", + t1 - t0, 2 * ( data->rowCount - 1 ) * ( data->columnCount - 1 ), data->byteCount, RAYS, t2 - t1, rayHits, raySum, QUERIES, t3 - t2, g_queryHits, + CASTS, t4 - t3, castHits, castSum, OVERLAPS, t5 - t4, overlapHits ); + b3DestroyHeightField( data ); + return 0; +} diff --git a/design.md b/design.md index 3ff1f9d..59d0c28 100644 --- a/design.md +++ b/design.md @@ -76,21 +76,32 @@ started until its tests pass. mirrored scale, the mover. The same trees as the reference; traversals 1.7-2x (its SIMD box tests). The mesh contact's cluster reduction (mesh_contact.c) is dynamics-side and comes with the contacts. -9. **collision, static**: `height_field`, `shape` (mass properties, ray - and shape casts per shape). Tests: `test_collision`, `test_shape`, - `test_height_field`. -10. **dynamics**: `body`, `contact`, `constraint_graph` (graph colouring), +9. **height_field** (done): height_field.c as `aephysics.height_field`: + the heights quantised to a global range, materials and holes per + cell, the edge flags against the four neighbours, either winding; + overlap, the ray and shape casts by a DDA walk of the swept box's + leading corner through the cells, the mover's planes, the box query. + 113 checks: test_height_field.c's create, index mapping, winding, + flat ray, overlap, straddle, brute-force shape and ray casts over a + wave, back-side and clockwise culling (the file roundtrip is not + ported), plus the flags of a ridge and of holes, a scaled field, the + query and the mover. The same results as the reference; the query at + parity, the casts 1.4-2x. The heights, materials and flags are ints + for want of 16- and 8-bit arrays, 3x the reference's bytes. +10. **shape**: shape.c (mass properties, ray and shape casts per shape, + compounds). Test: `test_shape`. +11. **dynamics**: `body`, `contact`, `constraint_graph` (graph colouring), `solver_set`, `island`, `solver` (the Soft Step: sub-stepping, relax iterations, restitution), `contact_solver` (scalar first; the wide SIMD path second, measured), the joints (revolute, prismatic, distance, motor, weld, wheel, spherical), `sensor`, `mover` (the character mover), `physics_world`. Tests: `test_body`, `test_joint`, `test_world`, `test_mover`, `test_determinism`, `test_large_world`. -11. **parallel**: `parallel_for` and the scheduler over Aether's actors; +12. **parallel**: `parallel_for` and the scheduler over Aether's actors; the benchmarks by thread count as the original records them. -12. **recording and replay**, `world_snapshot`: last, since they are the +13. **recording and replay**, `world_snapshot`: last, since they are the tooling and not the engine. -13. **benchmarks**: `reference/benchmark/main.c`'s nine scenes ported, run +14. **benchmarks**: `reference/benchmark/main.c`'s nine scenes ported, run against the C build on the same machine, recorded under `benchmark/`. ## Measures