diff --git a/.gitignore b/.gitignore index 579c1a6ea1..5149d614fc 100644 --- a/.gitignore +++ b/.gitignore @@ -41,3 +41,4 @@ CLAUDE.md /tools/__pycache__/ **/*.obsidian +/xr/util/dumps/ diff --git a/AGENTS.md b/AGENTS.md index 832afdb468..dd5ae8a099 100644 --- a/AGENTS.md +++ b/AGENTS.md @@ -2,6 +2,7 @@ ## Execution Policy - Visual UI programs must be launched and controlled through the Makepad Studio remote protocol. +- Always use release builds for runtime validation, profiling, benchmarks, timing checks, or any performance-sensitive command. Use `--release` unless the user explicitly asks for a debug build. - Do not use mount observation or runnable discovery from the bridge client. The bridge must not claim mount ownership from Studio desktop. - Do not launch UI programs with raw `cargo run`, `cargo makepad`, or ad hoc cargo invocation when a runnable item exists. - Do not use bridge `Cargo` requests to run applications. Only launch apps from runnable items via bridge `RunItem`. @@ -136,6 +137,8 @@ Do not use `ObserveMount` from the bridge. That call is for mount ownership/subs Use direct shell cargo commands only for non-UI tasks such as `check`, `build`, `test`, and file/search operations. They are not a substitute for a fresh Studio re-run. +When those non-UI tasks are used for runtime behavior or performance measurements, prefer their release variants (`cargo run --release`, `cargo test --release`, `cargo build --release`). + ## Cargo.toml Setup ```toml diff --git a/Cargo.toml b/Cargo.toml index b79034407f..ee92fe16b9 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -35,6 +35,7 @@ workspace.members = [ "libs/svg", "libs/apple_sys", "libs/shared_bytes", + "libs/tsdf", "platform/network", "libs/filesystem_watcher", "libs/live_reload_core", @@ -52,13 +53,16 @@ workspace.members = [ "libs/openexr", "libs/git", "libs/fast_inflate", + "libs/lz4", "xr", "libs/cef", ] workspace.exclude = [ "libs/terminal_core", + "libs/ggml", "libs/voice", + "libs/voice2", "widgets/test", "libs/stitch", "libs/wasm_bridge/test", diff --git a/draw/src/geometry/geometry_gen.rs b/draw/src/geometry/geometry_gen.rs index c2c2bcce02..3cec6b1447 100644 --- a/draw/src/geometry/geometry_gen.rs +++ b/draw/src/geometry/geometry_gen.rs @@ -89,6 +89,14 @@ pub struct IcoVertex { pub normal: Vec4f, } +#[derive(Clone, Script, ScriptHook)] +pub struct DepthMeshVertex { + #[live] + pub pos: Vec4f, + #[live] + pub barycentric: Vec4f, +} + pub fn script_mod(vm: &mut ScriptVm) -> ScriptValue { let geom = vm.new_module(id!(geom)); // lets make a Quad geometry here @@ -123,6 +131,12 @@ pub fn script_mod(vm: &mut ScriptVm) -> ScriptValue { .into_geometry(vm.cx_mut()) .into_script_handle(vm); set_script_value!(vm, geom.IcoGeom = igen); + // Depth mesh geometry: position plus per-triangle barycentrics. + set_script_value_to_pod!(vm, geom.DepthMeshVertex); + let dgen = GeometryGen::from_triangle_depth_mesh() + .into_geometry(vm.cx_mut()) + .into_script_handle(vm); + set_script_value!(vm, geom.DepthMeshGeom = dgen); NIL } @@ -204,6 +218,21 @@ impl GeometryGen { g } + /// Placeholder single-triangle geometry for depth-mesh drawing with barycentrics. + pub fn from_triangle_depth_mesh() -> GeometryGen { + let mut g = Self::default(); + g.vertices.extend_from_slice(&[ + 0.0, 0.0, 0.0, 1.0, // pos xyz + pad + 1.0, 0.0, 0.0, 0.0, // barycentric xyz + pad + 0.0, 0.0, 0.0, 1.0, // pos xyz + pad + 0.0, 1.0, 0.0, 0.0, // barycentric xyz + pad + 0.0, 0.0, 0.0, 1.0, // pos xyz + pad + 0.0, 0.0, 1.0, 0.0, // barycentric xyz + pad + ]); + g.indices.extend_from_slice(&[0, 1, 2]); + g + } + pub fn from_cube_3d( width: f32, height: f32, diff --git a/draw/src/shader/draw_glyph.rs b/draw/src/shader/draw_glyph.rs index 59c2a4d1b4..b59b8f6a67 100644 --- a/draw/src/shader/draw_glyph.rs +++ b/draw/src/shader/draw_glyph.rs @@ -7,7 +7,7 @@ use { turtle::*, vector::{PathCmd, VectorPath}, }, - std::mem, + std::{cmp::Ordering, mem}, }; script_mod! { @@ -32,29 +32,70 @@ script_mod! { max_band_curves: 512.0 aa_2x2: 0.0 aa_4x4: 0.0 - aa_pad_px: 0.0 + aa_pad_px: uniform(float(1.0)) + slug_matrix_0: uniform(vec4(1.0, 0.0, 0.0, 0.0)) + slug_matrix_1: uniform(vec4(0.0, 1.0, 0.0, 0.0)) + slug_matrix_3: uniform(vec4(0.0, 0.0, 0.0, 1.0)) + slug_viewport_px: uniform(vec2(1.0, 1.0)) axis_relief: 0.0 stem_darken: 0.0 stem_darken_max: 0.125 + fill_flags: 0.0 pos: varying(vec2f) world: varying(vec4f) + saturate: fn(v: float) -> float { + return clamp(v, 0.0, 1.0) + } + + slug_dilate: fn(pos: vec2, tex: vec2, jac: vec4, normal: vec2) -> vec4 { + let n = normalize(normal) + let s = dot(self.slug_matrix_3.xy, pos) + self.slug_matrix_3.w + let t = dot(self.slug_matrix_3.xy, n) + + let u = ( + s * dot(self.slug_matrix_0.xy, n) + - t * (dot(self.slug_matrix_0.xy, pos) + self.slug_matrix_0.w) + ) * self.slug_viewport_px.x + let v = ( + s * dot(self.slug_matrix_1.xy, n) + - t * (dot(self.slug_matrix_1.xy, pos) + self.slug_matrix_1.w) + ) * self.slug_viewport_px.y + + let s2 = s * s + let st = s * t + let uv = u * u + v * v + let d = normal * ( + s2 * (st + sqrt(uv)) / max(uv - st * st, 0.0000001) + ) * self.aa_pad_px + + let vpos = pos + d + let vtex = vec2(tex.x + dot(d, jac.xy), tex.y + dot(d, jac.zw)) + return vec4(vtex.x, vtex.y, vpos.x, vpos.y) + } + vertex: fn() { - let p = mix(self.rect_pos, self.rect_pos + self.rect_size, self.geom.pos) - let p_clipped = clamp(p, self.draw_clip.xy, self.draw_clip.zw) - let mut local = (p_clipped - self.rect_pos) / self.rect_size - if self.aa_pad_px > 0.0001 { - let pad_uv = vec2( - min(self.aa_pad_px / max(self.rect_size.x, 1.0), 0.45), - min(self.aa_pad_px / max(self.rect_size.y, 1.0), 0.45) - ) - local = (local - pad_uv) / vec2( - max(1.0 - 2.0 * pad_uv.x, 0.0001), - max(1.0 - 2.0 * pad_uv.y, 0.0001) - ) + let pad_lpx = self.aa_pad_px / max(self.draw_pass.dpi_factor, 0.0001) + let content_rect_pos = self.rect_pos + vec2(pad_lpx, pad_lpx) + let content_rect_size = vec2( + max(self.rect_size.x - 2.0 * pad_lpx, 0.0001), + max(self.rect_size.y - 2.0 * pad_lpx, 0.0001) + ) + let p = mix(content_rect_pos, content_rect_pos + content_rect_size, self.geom.pos) + let jac = vec4(1.0 / content_rect_size.x, 0.0, 0.0, 1.0 / content_rect_size.y) + let corner = self.geom.pos * 2.0 - 1.0 + let normal = if dot(corner, corner) > 0.000001 { + corner + } else { + vec2(1.0, 0.0) } - self.pos = local + let dilated = self.slug_dilate(p, self.geom.pos, jac, normal) + let p_clipped = clamp(dilated.zw, self.draw_clip.xy, self.draw_clip.zw) + self.pos = vec2( + dilated.x + (p_clipped.x - dilated.z) * jac.x, + dilated.y + (p_clipped.y - dilated.w) * jac.w + ) self.world = self.draw_list.view_transform * vec4( p_clipped.x, p_clipped.y, @@ -99,159 +140,241 @@ script_mod! { return v.w } - root_contribution: fn( - t: float, - a: float, - b: float, - p1: vec2, - p2: vec2, - p3: vec2, - sample: vec2, - px_size: float - ) -> float { - if t >= -0.000001 && t < 0.999999 { - let s = 1.0 - t - let cx = s * s * p1.x + 2.0 * s * t * p2.x + t * t * p3.x - let dist = cx - sample.x - let frac = clamp(dist / max(px_size, 0.00001) + 0.5, 0.0, 1.0) - let dy = 2.0 * a * t + b - if dy > 0.0 { return frac } - if dy < 0.0 { return -frac } + calc_root_code: fn(y1: float, y2: float, y3: float) -> u32 { + let i1 = asuint(y1) >> u32(31) + let i2 = asuint(y2) >> u32(30) + let i3 = asuint(y3) >> u32(29) + + let shift = (i1 & u32(1)) | (i2 & u32(2)) | (i3 & u32(4)) + return (u32(11892) >> shift) & u32(257) + } + + solve_horiz_poly: fn(p12: vec4, p3: vec2) -> vec2 { + let a = p12.xy - p12.zw * 2.0 + p3 + let b = p12.xy - p12.zw + let ra = 1.0 / a.y + let rb = 0.5 / b.y + + let d = sqrt(max(b.y * b.y - a.y * p12.y, 0.0)) + let mut t1 = (b.y - d) * ra + let mut t2 = (b.y + d) * ra + if abs(a.y) < 1.0 / 65536.0 { + t1 = p12.y * rb + t2 = t1 } - return 0.0 + return vec2( + (a.x * t1 - b.x * 2.0) * t1 + p12.x, + (a.x * t2 - b.x * 2.0) * t2 + p12.x + ) } - linear_root_contribution: fn( - t: float, - dy: float, - p1: vec2, - p2: vec2, - p3: vec2, - sample: vec2, - px_size: float - ) -> float { - if t >= -0.000001 && t < 0.999999 { - let s = 1.0 - t - let cx = s * s * p1.x + 2.0 * s * t * p2.x + t * t * p3.x - let dist = cx - sample.x - let frac = clamp(dist / max(px_size, 0.00001) + 0.5, 0.0, 1.0) - if dy > 0.0 { return frac } - if dy < 0.0 { return -frac } + solve_vert_poly: fn(p12: vec4, p3: vec2) -> vec2 { + let a = p12.xy - p12.zw * 2.0 + p3 + let b = p12.xy - p12.zw + let ra = 1.0 / a.x + let rb = 0.5 / b.x + + let d = sqrt(max(b.x * b.x - a.x * p12.x, 0.0)) + let mut t1 = (b.x - d) * ra + let mut t2 = (b.x + d) * ra + if abs(a.x) < 1.0 / 65536.0 { + t1 = p12.x * rb + t2 = t1 } - return 0.0 + return vec2( + (a.y * t1 - b.y * 2.0) * t1 + p12.y, + (a.y * t2 - b.y * 2.0) * t2 + p12.y + ) } - curve_coverage: fn(p1: vec2, p2: vec2, p3: vec2, sample: vec2, px_size: float) -> float { - let y1 = p1.y - sample.y - let y2 = p2.y - sample.y - let y3 = p3.y - sample.y + scan_horizontal_list: fn(list_offset: float, list_count: float, sample: vec2, px_size: float) -> vec2 { + let limit = floor(list_count + 0.5) + var coverage = 0.0 + var weight = 0.0 + + var j = 0.0 + loop { + if j >= limit { break } + + let packed_idx = floor(j * 0.25) + let channel = j - packed_idx * 4.0 + let idx_data = self.fetch_band_texel(list_offset + packed_idx) + let curve_idx = self.pick_channel(idx_data, channel) + + let p12 = self.fetch_curve_texel(curve_idx * 2.0) - vec4(sample.x, sample.y, sample.x, sample.y) + let p3 = self.fetch_curve_texel(curve_idx * 2.0 + 1.0).xy - sample + if max(max(p12.x, p12.z), p3.x) / px_size < -0.5 { break } + + let code = self.calc_root_code(p12.y, p12.w, p3.y) + if code != u32(0) { + let r = self.solve_horiz_poly(p12, p3) / px_size + if (code & u32(1)) != u32(0) { + coverage = coverage + self.saturate(r.x + 0.5) + weight = max(weight, self.saturate(1.0 - abs(r.x) * 2.0)) + } + if code > u32(1) { + coverage = coverage - self.saturate(r.y + 0.5) + weight = max(weight, self.saturate(1.0 - abs(r.y) * 2.0)) + } + } + + j = j + 1.0 + } - let a = y1 - 2.0 * y2 + y3 - let b = 2.0 * (y2 - y1) - let c = y1 + return vec2(coverage, weight) + } - if abs(a) < 0.000001 { - if abs(b) < 0.000001 { - return 0.0 + scan_vertical_list: fn(list_offset: float, list_count: float, sample: vec2, px_size: float) -> vec2 { + let limit = floor(list_count + 0.5) + var coverage = 0.0 + var weight = 0.0 + + var j = 0.0 + loop { + if j >= limit { break } + + let packed_idx = floor(j * 0.25) + let channel = j - packed_idx * 4.0 + let idx_data = self.fetch_band_texel(list_offset + packed_idx) + let curve_idx = self.pick_channel(idx_data, channel) + + let p12 = self.fetch_curve_texel(curve_idx * 2.0) - vec4(sample.x, sample.y, sample.x, sample.y) + let p3 = self.fetch_curve_texel(curve_idx * 2.0 + 1.0).xy - sample + if max(max(p12.y, p12.w), p3.y) / px_size < -0.5 { break } + + let code = self.calc_root_code(p12.x, p12.z, p3.x) + if code != u32(0) { + let r = self.solve_vert_poly(p12, p3) / px_size + if (code & u32(1)) != u32(0) { + coverage = coverage - self.saturate(r.x + 0.5) + weight = max(weight, self.saturate(1.0 - abs(r.x) * 2.0)) + } + if code > u32(1) { + coverage = coverage + self.saturate(r.y + 0.5) + weight = max(weight, self.saturate(1.0 - abs(r.y) * 2.0)) + } } - let t = -c / b - return self.linear_root_contribution(t, b, p1, p2, p3, sample, px_size) + + j = j + 1.0 } - let disc = b * b - 4.0 * a * c - if disc < 0.0 { - return 0.0 + return vec2(coverage, weight) + } + + scan_horizontal_all: fn(sample: vec2, px_size: float) -> vec2 { + let limit = floor(self.curve_count + 0.5) + var coverage = 0.0 + var weight = 0.0 + + var i = 0.0 + loop { + if i >= limit { break } + + let curve_idx = self.curve_offset + i + let p12 = self.fetch_curve_texel(curve_idx * 2.0) - vec4(sample.x, sample.y, sample.x, sample.y) + let p3 = self.fetch_curve_texel(curve_idx * 2.0 + 1.0).xy - sample + let code = self.calc_root_code(p12.y, p12.w, p3.y) + if code != u32(0) { + let r = self.solve_horiz_poly(p12, p3) / px_size + if (code & u32(1)) != u32(0) { + coverage = coverage + self.saturate(r.x + 0.5) + weight = max(weight, self.saturate(1.0 - abs(r.x) * 2.0)) + } + if code > u32(1) { + coverage = coverage - self.saturate(r.y + 0.5) + weight = max(weight, self.saturate(1.0 - abs(r.y) * 2.0)) + } + } + + i = i + 1.0 } - let sqrt_disc = sqrt(max(disc, 0.0)) - let inv_2a = 0.5 / a - let t1 = (-b - sqrt_disc) * inv_2a - let t2 = (-b + sqrt_disc) * inv_2a + return vec2(coverage, weight) + } + scan_vertical_all: fn(sample: vec2, px_size: float) -> vec2 { + let limit = floor(self.curve_count + 0.5) var coverage = 0.0 - coverage = coverage + self.root_contribution(t1, a, b, p1, p2, p3, sample, px_size) - coverage = coverage + self.root_contribution(t2, a, b, p1, p2, p3, sample, px_size) - return coverage + var weight = 0.0 + + var i = 0.0 + loop { + if i >= limit { break } + + let curve_idx = self.curve_offset + i + let p12 = self.fetch_curve_texel(curve_idx * 2.0) - vec4(sample.x, sample.y, sample.x, sample.y) + let p3 = self.fetch_curve_texel(curve_idx * 2.0 + 1.0).xy - sample + let code = self.calc_root_code(p12.x, p12.z, p3.x) + if code != u32(0) { + let r = self.solve_vert_poly(p12, p3) / px_size + if (code & u32(1)) != u32(0) { + coverage = coverage - self.saturate(r.x + 0.5) + weight = max(weight, self.saturate(1.0 - abs(r.x) * 2.0)) + } + if code > u32(1) { + coverage = coverage + self.saturate(r.y + 0.5) + weight = max(weight, self.saturate(1.0 - abs(r.y) * 2.0)) + } + } + + i = i + 1.0 + } + + return vec2(coverage, weight) + } + + calc_coverage: fn(xcov: float, ycov: float, xwgt: float, ywgt: float) -> float { + let coverage = max( + abs(xcov * xwgt + ycov * ywgt) / max(xwgt + ywgt, 1.0 / 65536.0), + min(abs(xcov), abs(ycov)) + ) + if self.fill_flags >= 4096.0 { + return 1.0 - abs(1.0 - fract(coverage * 0.5) * 2.0) + } + return self.saturate(coverage) } alpha_at: fn(sample: vec2, px_x: float, px_y: float) -> float { var coverage_x = 0.0 var coverage_y = 0.0 + var weight_x = 0.0 + var weight_y = 0.0 if self.band_count > 0.5 { let num_bands = max(floor(self.band_count + 0.5), 1.0) - let band_idx = clamp(floor(sample.y * num_bands), 0.0, num_bands - 1.0) - let band_info = self.fetch_band_texel(self.band_offset + band_idx) - let list_offset = floor(band_info.x + 0.5) - let list_count = min(floor(band_info.y + 0.5), self.max_band_curves) - - var j = 0.0 - loop { - if j >= list_count { break } - - let packed_idx = floor(j * 0.25) - let channel = j - packed_idx * 4.0 - let idx_data = self.fetch_band_texel(list_offset + packed_idx) - let curve_idx = self.pick_channel(idx_data, channel) - - let t0 = self.fetch_curve_texel(curve_idx * 2.0) - let t1 = self.fetch_curve_texel(curve_idx * 2.0 + 1.0) - let p1 = vec2(t0.x, t0.y) - let p2 = vec2(t0.z, t0.w) - let p3 = vec2(t1.x, t1.y) - - coverage_x = coverage_x + self.curve_coverage(p1, p2, p3, sample, px_x) - coverage_y = coverage_y + self.curve_coverage( - vec2(p1.y, p1.x), - vec2(p2.y, p2.x), - vec2(p3.y, p3.x), - vec2(sample.y, sample.x), - px_y, - ) - - j = j + 1.0 - } + let h_band_idx = clamp(floor(sample.y * num_bands), 0.0, num_bands - 1.0) + let v_band_idx = clamp(floor(sample.x * num_bands), 0.0, num_bands - 1.0) + + let h_band_info = self.fetch_band_texel(self.band_offset + h_band_idx) + let h_band = self.scan_horizontal_list( + floor(h_band_info.x + 0.5), + h_band_info.y, + sample, + px_x, + ) + coverage_x = h_band.x + weight_x = h_band.y + + let v_band_info = self.fetch_band_texel(self.band_offset + num_bands + v_band_idx) + let v_band = self.scan_vertical_list( + floor(v_band_info.x + 0.5), + v_band_info.y, + sample, + px_y, + ) + coverage_y = v_band.x + weight_y = v_band.y } else { - let limit = min(floor(self.curve_count + 0.5), self.max_band_curves) - var i = 0.0 - loop { - if i >= limit { break } - - let curve_idx = self.curve_offset + i - let t0 = self.fetch_curve_texel(curve_idx * 2.0) - let t1 = self.fetch_curve_texel(curve_idx * 2.0 + 1.0) - let p1 = vec2(t0.x, t0.y) - let p2 = vec2(t0.z, t0.w) - let p3 = vec2(t1.x, t1.y) - - coverage_x = coverage_x + self.curve_coverage(p1, p2, p3, sample, px_x) - coverage_y = coverage_y + self.curve_coverage( - vec2(p1.y, p1.x), - vec2(p2.y, p2.x), - vec2(p3.y, p3.x), - vec2(sample.y, sample.x), - px_y, - ) - - i = i + 1.0 - } + let x_scan = self.scan_horizontal_all(sample, px_x) + coverage_x = x_scan.x + weight_x = x_scan.y + + let y_scan = self.scan_vertical_all(sample, px_y) + coverage_y = y_scan.x + weight_y = y_scan.y } - let ax = clamp(abs(coverage_x), 0.0, 1.0) - let ay = clamp(abs(coverage_y), 0.0, 1.0) - let a_min = min(ax, ay) - let a_max = max(ax, ay) - // If one axis reports near-solid coverage while the other axis - // drops sharply (numeric miss around tangencies/cusps), blend a bit - // toward the average. Keeps isolated pinhole artifacts from appearing. - let mismatch = a_max - a_min - let a_avg = 0.5 * (ax + ay) - let mismatch_t = clamp((mismatch - 0.15) / 0.35, 0.0, 1.0) - // Only apply relief once coverage is already interior on average. - // This avoids gray fringes near sharp outer edges (e.g. '4'). - let interior_t = clamp((a_avg - 0.55) / 0.30, 0.0, 1.0) - let relief = clamp(self.axis_relief * mismatch_t * interior_t, 0.0, 1.0) - return clamp(mix(a_min, a_avg, relief), 0.0, 1.0) + return self.calc_coverage(coverage_x, coverage_y, weight_x, weight_y) } fragment: fn() { @@ -307,8 +430,6 @@ script_mod! { } else { self.alpha_at(sample, px_x, px_y) } - // Apply stem darkening only around the transition band. - // This avoids lifting fully transparent background pixels. let darken = clamp(max(px_x, px_y) * self.stem_darken, 0.0, self.stem_darken_max) let edge_weight = clamp(1.0 - abs(alpha_base * 2.0 - 1.0), 0.0, 1.0) let alpha = clamp(alpha_base + darken * edge_weight, 0.0, 1.0) @@ -321,6 +442,7 @@ script_mod! { const CURVE_TEX_WIDTH: usize = 2048; const BAND_TEX_WIDTH: usize = 2048; const DEFAULT_NUM_BANDS: usize = 24; +const GLYPH_FILL_FLAG_EVEN_ODD: u32 = 0x1000; // Keep cubic approximation tight; loose flattening can cause local stem thinning // on curved symbols (e.g. infinity) even when AA is otherwise correct. const CUBIC_TO_QUAD_TOLERANCE: f32 = 0.05; @@ -336,6 +458,7 @@ pub struct GlyphLayerRef { pub curve_count: usize, pub band_offset: usize, pub band_count: usize, + pub flags: u32, } #[derive(Clone, Debug)] @@ -412,6 +535,7 @@ struct PendingLayer { color: Vec4f, curves: Vec, bounds: BBox, + flags: u32, } #[derive(Script, ScriptHook, Debug)] @@ -426,6 +550,8 @@ pub struct DrawGlyph { #[rust] pending_color: Vec4f, #[rust] + pending_flags: u32, + #[rust] curve_data: Vec, #[rust] band_data: Vec, @@ -478,13 +604,13 @@ pub struct DrawGlyph { #[live(0.0)] pub aa_4x4: f32, #[live(0.0)] - pub aa_pad_px: f32, - #[live(0.0)] pub axis_relief: f32, #[live(0.0)] pub stem_darken: f32, #[live(0.125)] pub stem_darken_max: f32, + #[live(0.0)] + pub fill_flags: f32, } impl DrawGlyph { @@ -493,6 +619,7 @@ impl DrawGlyph { self.path.clear(); self.pending_layers.clear(); self.pending_color = vec4(1.0, 1.0, 1.0, 1.0); + self.pending_flags = 0; } pub fn set_color(&mut self, r: f32, g: f32, b: f32, a: f32) { @@ -503,6 +630,14 @@ impl DrawGlyph { self.pending_color = color; } + pub fn set_even_odd_fill(&mut self, even_odd: bool) { + self.pending_flags = if even_odd { + GLYPH_FILL_FLAG_EVEN_ODD + } else { + 0 + }; + } + pub fn move_to(&mut self, x: f32, y: f32) { self.path.move_to(x, y); } @@ -546,7 +681,9 @@ impl DrawGlyph { color: self.pending_color, curves, bounds, + flags: self.pending_flags, }); + self.pending_flags = 0; } pub fn add_curves_layer(&mut self, color: Vec4f, curves: &[(Vec2f, Vec2f, Vec2f)]) { @@ -571,6 +708,7 @@ impl DrawGlyph { color, curves: out, bounds, + flags: self.pending_flags, }); } @@ -646,6 +784,7 @@ impl DrawGlyph { curve_count, band_offset, band_count: actual_band_count, + flags: layer.flags, }); } @@ -673,6 +812,7 @@ impl DrawGlyph { self.ensure_initialized(); self.path.clear(); self.pending_layers.clear(); + self.pending_flags = 0; self.shapes.clear(); self.curve_data.clear(); self.band_data.clear(); @@ -719,7 +859,7 @@ impl DrawGlyph { return; } self.update_draw_vars(cx); - let pad = self.aa_pad_px.max(0.0) as f64; + let pad = (self.get_aa_pad_px(cx.cx.cx) / cx.current_dpi_factor() as f32).max(0.0) as f64; let rect = if pad > 0.0 { crate::makepad_platform::Rect { pos: DVec2 { @@ -736,7 +876,6 @@ impl DrawGlyph { }; self.rect_pos = rect.pos.into(); self.rect_size = rect.size.into(); - if layers.len() == 1 { self.apply_layer(&layers[0], 0.0); self.push_instance(cx); @@ -766,16 +905,29 @@ impl DrawGlyph { self.band_tex_width = BAND_TEX_WIDTH; self.default_num_bands = DEFAULT_NUM_BANDS; self.pending_color = vec4(1.0, 1.0, 1.0, 1.0); + self.pending_flags = 0; self.curve_dirty = true; self.band_dirty = true; } + pub fn set_aa_pad_px(&mut self, cx: &Cx, aa_pad_px: f32) { + self.draw_vars + .set_uniform(cx, live_id!(aa_pad_px), &[aa_pad_px]); + } + + pub fn get_aa_pad_px(&self, cx: &mut Cx) -> f32 { + let mut value = [0.0]; + self.draw_vars.get_uniform(cx, live_id!(aa_pad_px), &mut value); + value[0] + } + fn apply_layer(&mut self, layer: &GlyphLayerRef, order: f32) { self.color = layer.color; self.curve_offset = layer.curve_offset as f32; self.curve_count = layer.curve_count as f32; self.band_offset = layer.band_offset as f32; self.band_count = layer.band_count as f32; + self.fill_flags = layer.flags as f32; self.layer_order = order; } @@ -802,31 +954,63 @@ impl DrawGlyph { } let band_offset = self.band_data.len() / 4; - let metadata_floats = num_bands * 4; + let metadata_floats = num_bands * 2 * 4; self.band_data .resize(self.band_data.len() + metadata_floats, 0.0); - let mut band_lists = vec![Vec::::new(); num_bands]; + let mut horizontal_bands = vec![Vec::::new(); num_bands]; + let mut vertical_bands = vec![Vec::::new(); num_bands]; let bands_f = num_bands as f32; - let max_band = (num_bands - 1) as isize; + let epsilon = 1.0 / 1024.0; for (curve_index, curve) in curves.iter().enumerate() { - let y_min = curve.p0.y.min(curve.p1.y).min(curve.p2.y).clamp(0.0, 1.0); - let y_max = curve.p0.y.max(curve.p1.y).max(curve.p2.y).clamp(0.0, 1.0); - let mut lo = (y_min * bands_f).floor() as isize; - let mut hi = (y_max * bands_f).floor() as isize; - lo = lo.clamp(0, max_band); - hi = hi.clamp(0, max_band); - if hi < lo { - std::mem::swap(&mut lo, &mut hi); + if !curve_is_horizontal(curve) { + if let Some((lo, hi)) = band_range( + curve.p0.y.min(curve.p1.y).min(curve.p2.y) - epsilon, + curve.p0.y.max(curve.p1.y).max(curve.p2.y) + epsilon, + bands_f, + num_bands, + ) { + for band in lo..=hi { + horizontal_bands[band].push(curve_index); + } + } } - let absolute_curve = (curve_offset + curve_index) as f32; - for band in lo..=hi { - band_lists[band as usize].push(absolute_curve); + + if !curve_is_vertical(curve) { + if let Some((lo, hi)) = band_range( + curve.p0.x.min(curve.p1.x).min(curve.p2.x) - epsilon, + curve.p0.x.max(curve.p1.x).max(curve.p2.x) + epsilon, + bands_f, + num_bands, + ) { + for band in lo..=hi { + vertical_bands[band].push(curve_index); + } + } } } - let mut list_texel_offset = band_offset + num_bands; - for (band, list) in band_lists.into_iter().enumerate() { + for list in &mut horizontal_bands { + list.sort_by(|a, b| { + curve_max_x(curves[*b]) + .partial_cmp(&curve_max_x(curves[*a])) + .unwrap_or(Ordering::Equal) + }); + } + for list in &mut vertical_bands { + list.sort_by(|a, b| { + curve_max_y(curves[*b]) + .partial_cmp(&curve_max_y(curves[*a])) + .unwrap_or(Ordering::Equal) + }); + } + + let mut list_texel_offset = band_offset + num_bands * 2; + for (band, list) in horizontal_bands + .into_iter() + .chain(vertical_bands.into_iter()) + .enumerate() + { let meta = (band_offset + band) * 4; self.band_data[meta] = list_texel_offset as f32; self.band_data[meta + 1] = list.len() as f32; @@ -836,7 +1020,7 @@ impl DrawGlyph { for chunk in list.chunks(4) { let mut texel = [0.0f32; 4]; for (i, value) in chunk.iter().enumerate() { - texel[i] = *value; + texel[i] = (curve_offset + *value) as f32; } self.band_data.extend_from_slice(&texel); list_texel_offset += 1; @@ -849,6 +1033,27 @@ impl DrawGlyph { fn update_draw_vars(&mut self, cx: &mut Cx2d) { self.ensure_initialized(); self.upload_textures(cx.cx.cx); + let pass_id = cx.pass_stack.last().unwrap().pass_id; + let draw_list_id = *cx.draw_list_stack.last().unwrap(); + let pass_uniforms = cx.passes[pass_id].pass_uniforms.clone(); + let view_transform = cx.draw_lists[draw_list_id].draw_list_uniforms.view_transform; + let model_view = Mat4f::mul(&pass_uniforms.camera_view, &view_transform); + let slug_matrix = Mat4f::mul(&pass_uniforms.camera_projection, &model_view); + let viewport = cx.current_pass_size(); + let dpi_factor = cx.current_dpi_factor() as f32; + let viewport_px = [ + (viewport.x as f32 * dpi_factor).max(1.0), + (viewport.y as f32 * dpi_factor).max(1.0), + ]; + + self.draw_vars + .set_uniform(cx.cx, live_id!(slug_matrix_0), &mat4_row(&slug_matrix, 0)); + self.draw_vars + .set_uniform(cx.cx, live_id!(slug_matrix_1), &mat4_row(&slug_matrix, 1)); + self.draw_vars + .set_uniform(cx.cx, live_id!(slug_matrix_3), &mat4_row(&slug_matrix, 3)); + self.draw_vars + .set_uniform(cx.cx, live_id!(slug_viewport_px), &viewport_px); self.draw_vars.texture_slots[0] = self.curve_texture.clone(); self.draw_vars.texture_slots[1] = self.band_texture.clone(); } @@ -936,6 +1141,10 @@ fn normalize_point(p: P2, bounds: BBox, inv_w: f32, inv_h: f32) -> P2 { } } +fn mat4_row(mat: &Mat4f, row: usize) -> [f32; 4] { + [mat.v[row], mat.v[row + 4], mat.v[row + 8], mat.v[row + 12]] +} + fn path_to_quads(path: &VectorPath) -> (Vec, BBox) { let mut curves = Vec::new(); let mut bounds = BBox::default(); @@ -996,6 +1205,37 @@ fn push_quad(curves: &mut Vec, bounds: &mut BBox, curve: QuadCurve) { curves.push(curve); } +fn band_range(min_value: f32, max_value: f32, bands_f: f32, num_bands: usize) -> Option<(usize, usize)> { + if num_bands == 0 { + return None; + } + let max_band = (num_bands - 1) as isize; + let mut lo = (min_value.clamp(0.0, 1.0) * bands_f).floor() as isize; + let mut hi = (max_value.clamp(0.0, 1.0) * bands_f).floor() as isize; + lo = lo.clamp(0, max_band); + hi = hi.clamp(0, max_band); + if hi < lo { + mem::swap(&mut lo, &mut hi); + } + Some((lo as usize, hi as usize)) +} + +fn curve_is_horizontal(curve: &QuadCurve) -> bool { + (curve.p0.y - curve.p1.y).abs() <= 0.000001 && (curve.p0.y - curve.p2.y).abs() <= 0.000001 +} + +fn curve_is_vertical(curve: &QuadCurve) -> bool { + (curve.p0.x - curve.p1.x).abs() <= 0.000001 && (curve.p0.x - curve.p2.x).abs() <= 0.000001 +} + +fn curve_max_x(curve: QuadCurve) -> f32 { + curve.p0.x.max(curve.p1.x).max(curve.p2.x) +} + +fn curve_max_y(curve: QuadCurve) -> f32 { + curve.p0.y.max(curve.p1.y).max(curve.p2.y) +} + fn same_point(a: P2, b: P2) -> bool { (a.x - b.x).abs() <= 0.000001 && (a.y - b.y).abs() <= 0.000001 } diff --git a/draw/src/shader/draw_svg.rs b/draw/src/shader/draw_svg.rs index bcd708c489..93ebad45ca 100644 --- a/draw/src/shader/draw_svg.rs +++ b/draw/src/shader/draw_svg.rs @@ -37,31 +37,9 @@ script_mod! { vertex: fn() { var pos = vec2(self.geom.x, self.geom.y); - // Fill fringe outer vertices (u=0, stroke_mult>1e5) have - // the outward normal stored in (v, stroke_dist). Expand - // them so the fringe is ~1px wide on screen regardless of - // svg_scale. We keep the edge centered like NanoVG: - // fill/body verts move inward by 0.5px, outer fringe verts - // move outward by 0.5px. - if self.geom.stroke_mult > 1e5 { - let normal = vec2(self.geom.v, self.geom.stroke_dist); - let nlen = length(normal); - if nlen > 0.0001 { - let un = normal / nlen; - // Convert screen px into local-space distance along un. - let screen_scale = length(vec2(un.x * self.svg_scale.x, un.y * self.svg_scale.y)); - if screen_scale > 0.0001 { - let half_px = 0.5 / screen_scale; - if self.geom.u < 0.25 { - // transparent outer fringe vertex - pos = pos + un * half_px; - } else if self.geom.u > 0.25 { - // opaque edge/body vertex - pos = pos - un * half_px; - } - } - } - } + // SVG fill uses pre-computed fringe (not GPU-expand), so fringe + // vertices already have physically offset positions. No GPU-side + // expansion needed. // Keep tessellated SVG geometry relative and apply rect_pos as the // final anchor so turtle alignment can move it like DrawQuad. let transformed_local = self.transform_svg_point(pos * self.svg_scale + self.svg_offset); @@ -139,7 +117,7 @@ script_mod! { if self.color.x >= 0.0 { // Replace base RGB with the override color, preserving the // vertex alpha (shape mask from tessellation). - return vec4(self.color.rgb * base.a, self.color.a * base.a) + return vec4(self.color.rgb * self.color.a * base.a, self.color.a * base.a) } return base } diff --git a/draw/src/shader/draw_svg_glyph.rs b/draw/src/shader/draw_svg_glyph.rs index e8540cdec6..be9a78b49a 100644 --- a/draw/src/shader/draw_svg_glyph.rs +++ b/draw/src/shader/draw_svg_glyph.rs @@ -5,7 +5,7 @@ use crate::{ svg::{self, SvgDefs, SvgDocument, SvgNode, SvgPaint, SvgStyle, SvgUse, Transform2d}, turtle::*, }; -use makepad_svg::path::{PathCmd, VectorPath}; +use makepad_svg::path::{FillRule, PathCmd, VectorPath}; script_mod! { use mod.pod.* @@ -206,9 +206,7 @@ impl DrawSvgGlyph { self.draw_super.clear_shapes(); self.draw_super.begin_shape(); emit_nodes_fill_only(&mut self.draw_super, &doc.root, &doc.defs, &base_xf); - // DrawGlyph's one-axis band acceleration can miss intersections when - // sampling the transposed axis. For SVG icons we favor correctness. - self.cached_shape = self.draw_super.commit_shape(Some(0)); + self.cached_shape = self.draw_super.commit_shape(None); self.cache_valid = true; } @@ -301,6 +299,7 @@ fn emit_nodes_fill_only( let xf = path.transform.then(parent_xf); if let Some(color) = resolve_fill_color(&path.style, defs) { dg.set_color_vec4(color); + dg.set_even_odd_fill(matches!(path.style.fill_rule, FillRule::EvenOdd)); emit_transformed_path(dg, &path.path, &xf); dg.fill_layer(); } @@ -309,6 +308,7 @@ fn emit_nodes_fill_only( let xf = rect.transform.then(parent_xf); if let Some(color) = resolve_fill_color(&rect.style, defs) { dg.set_color_vec4(color); + dg.set_even_odd_fill(matches!(rect.style.fill_rule, FillRule::EvenOdd)); let mut path = VectorPath::new(); let r = rect.rx.max(rect.ry); if r > 0.0 { @@ -324,6 +324,7 @@ fn emit_nodes_fill_only( let xf = circle.transform.then(parent_xf); if let Some(color) = resolve_fill_color(&circle.style, defs) { dg.set_color_vec4(color); + dg.set_even_odd_fill(matches!(circle.style.fill_rule, FillRule::EvenOdd)); let mut path = VectorPath::new(); path.circle(circle.cx, circle.cy, circle.r); emit_transformed_path(dg, &path, &xf); @@ -334,6 +335,7 @@ fn emit_nodes_fill_only( let xf = ellipse.transform.then(parent_xf); if let Some(color) = resolve_fill_color(&ellipse.style, defs) { dg.set_color_vec4(color); + dg.set_even_odd_fill(matches!(ellipse.style.fill_rule, FillRule::EvenOdd)); let mut path = VectorPath::new(); path.ellipse(ellipse.cx, ellipse.cy, ellipse.rx, ellipse.ry); emit_transformed_path(dg, &path, &xf); @@ -344,6 +346,7 @@ fn emit_nodes_fill_only( let xf = poly.transform.then(parent_xf); if let Some(color) = resolve_fill_color(&poly.style, defs) { dg.set_color_vec4(color); + dg.set_even_odd_fill(matches!(poly.style.fill_rule, FillRule::EvenOdd)); emit_points(dg, &poly.points, false, &xf); dg.fill_layer(); } @@ -352,6 +355,7 @@ fn emit_nodes_fill_only( let xf = poly.transform.then(parent_xf); if let Some(color) = resolve_fill_color(&poly.style, defs) { dg.set_color_vec4(color); + dg.set_even_odd_fill(matches!(poly.style.fill_rule, FillRule::EvenOdd)); emit_points(dg, &poly.points, true, &xf); dg.fill_layer(); } diff --git a/draw/src/shader/draw_text.rs b/draw/src/shader/draw_text.rs index 0b1a15b0d2..d9799f8f9f 100644 --- a/draw/src/shader/draw_text.rs +++ b/draw/src/shader/draw_text.rs @@ -19,7 +19,12 @@ use { turtle::*, turtle::{Align, Walk}, }, - std::{cell::RefCell, rc::Rc}, + std::{ + cell::RefCell, + collections::hash_map::DefaultHasher, + hash::{Hash, Hasher}, + rc::Rc, + }, }; script_mod! { @@ -823,6 +828,9 @@ pub struct FontMember { pub asc: f32, #[live] pub desc: f32, + /// Positive values map to the OpenType `wght` axis. `0.0` keeps the font default. + #[live(0.0)] + pub weight: f32, } #[derive(Debug, Clone, Script, PartialEq)] @@ -838,6 +846,7 @@ pub struct FontMemberDef { handle: ScriptHandle, asc: f32, desc: f32, + weight: f32, } impl FontFamily { @@ -852,7 +861,7 @@ impl FontFamily { if !font_member_is_needed_for_text(cx, member.handle, text) { continue; } - let font_id: FontId = (member.handle.index() as u64).into(); + let font_id = font_member_font_id(member); if !fonts.is_font_known(font_id) { let font_data = cx.get_resource_font_bytes(member.handle); @@ -865,6 +874,7 @@ impl FontFamily { index: 0, ascender_fudge_in_ems: member.asc, descender_fudge_in_ems: member.desc, + weight: font_member_weight(member), variations: Vec::new(), }, ); @@ -958,6 +968,23 @@ fn resource_basename(path: &str) -> Option<&str> { path.rsplit(['/', '\\']).next() } +fn font_member_weight(member: &FontMemberDef) -> Option { + if member.weight.is_finite() && member.weight > 0.0 { + Some(member.weight) + } else { + None + } +} + +fn font_member_font_id(member: &FontMemberDef) -> FontId { + let mut hasher = DefaultHasher::new(); + member.handle.index().hash(&mut hasher); + member.asc.to_bits().hash(&mut hasher); + member.desc.to_bits().hash(&mut hasher); + member.weight.to_bits().hash(&mut hasher); + FontId::from(hasher.finish()) +} + fn text_has_cjk(text: &str) -> bool { text.chars().any(is_cjk_char) } @@ -1051,6 +1078,7 @@ impl ScriptHook for FontFamily { handle: handle_ref.as_handle(), asc: member.asc, desc: member.desc, + weight: member.weight, }); } } diff --git a/draw/src/svg/render.rs b/draw/src/svg/render.rs index 851cca2d10..f06e900818 100644 --- a/draw/src/svg/render.rs +++ b/draw/src/svg/render.rs @@ -607,7 +607,9 @@ fn emit_shape( build_path(dv); let fill_alpha = style.fill_opacity * opacity; set_paint(dv, paint, defs, fill_alpha, xf, bbox, grad_map); - dv.fill_gpu(); + // Use pre-computed fringe (not GPU-expand) to avoid coincident-vertex + // triangles that cause Metal GPU rasterization artifacts. + dv.fill(); dv.cur_gradient_row_v = -1.0; // reset after fill dv.path.clear(); } diff --git a/draw/src/text/font_face.rs b/draw/src/text/font_face.rs index 27f3607b9c..e2932571a7 100644 --- a/draw/src/text/font_face.rs +++ b/draw/src/text/font_face.rs @@ -4,6 +4,9 @@ use {super::loader::FontData, rustybuzz, rustybuzz::ttf_parser}; pub struct FontFace { parsed: Rc, variations: Vec, + /// Cached `ttf_parser::Face` with the current variations applied. + /// Invalidated when `set_variations` is called. + cached_ttf_face: RefCell>>, /// Cached `rustybuzz::Face` built from the parsed `ttf_parser::Face`. /// Invalidated when `set_variations` is called, since variations affect /// the rustybuzz shaping tables. @@ -25,6 +28,7 @@ impl Clone for FontFace { Self { parsed: self.parsed.clone(), variations: self.variations.clone(), + cached_ttf_face: RefCell::new(None), cached_rb_face: RefCell::new(None), } } @@ -67,12 +71,29 @@ impl FontFace { Some(Self { parsed: Rc::new(parsed), variations: Vec::new(), + cached_ttf_face: RefCell::new(None), cached_rb_face: RefCell::new(None), }) } pub fn with_ttf_parser_face(&self, f: impl FnOnce(&ttf_parser::Face<'_>) -> R) -> R { - f(&self.parsed.face) + if self.variations.is_empty() { + return f(&self.parsed.face); + } + + { + let mut ttf_cache = self.cached_ttf_face.borrow_mut(); + if ttf_cache.is_none() { + let mut face = self.parsed.face.clone(); + for variation in &self.variations { + let _ = face.set_variation(variation.tag, variation.value); + } + *ttf_cache = Some(face); + } + } + + let ttf_cache = self.cached_ttf_face.borrow(); + f(ttf_cache.as_ref().unwrap()) } pub fn with_rustybuzz_face(&self, f: impl FnOnce(&rustybuzz::Face<'_>) -> R) -> R { @@ -102,6 +123,7 @@ impl FontFace { tag: ttf_parser::Tag::from_bytes(&tag.to_be_bytes()), value, })); + *self.cached_ttf_face.borrow_mut() = None; // Invalidate the cached rustybuzz face since variations affect shaping. *self.cached_rb_face.borrow_mut() = None; } diff --git a/draw/src/text/layouter.rs b/draw/src/text/layouter.rs index 552c84a5d1..e72f39b886 100644 --- a/draw/src/text/layouter.rs +++ b/draw/src/text/layouter.rs @@ -104,11 +104,19 @@ impl Layouter { } fn layout(&mut self, params: OwnedLayoutParams) -> LaidoutText { - let font_family = self + let font_family = match self .loader .get_or_load_font_family(params.style.font_family_id) - .cloned() - .expect("font family should be loaded before layout"); + { + Some(family) => family.clone(), + None => { + return LaidoutText { + text: params.text, + size_in_lpxs: Size::ZERO, + rows: Vec::new(), + }; + } + }; LayoutContext::new(font_family, params.text, params.style, params.options) .layout_multiline() } @@ -427,13 +435,16 @@ impl Fitter { font_size_in_lpxs: f32, segment_kind: SegmentKind, ) -> Self { - let lens: Vec<_> = match segment_kind { + let mut lens: Vec<_> = match segment_kind { SegmentKind::Word => text .split_word_bounds() .map(|segment| segment.len()) .collect(), SegmentKind::Grapheme => text.graphemes(true).map(|segment| segment.len()).collect(), }; + if matches!(segment_kind, SegmentKind::Word) { + merge_segments_for_line_breaking(&text, &mut lens); + } let widths_in_lpxs: Vec<_> = lens .iter() .copied() @@ -516,6 +527,88 @@ enum SegmentKind { Grapheme, } +/// Merges word-boundary segments to prevent line breaks at typographically +/// incorrect positions, following standard line-breaking conventions +/// (UAX#14 / CSS Text Module Level 3). +/// +/// Trailing/closing punctuation (`.` `,` `;` `)` etc.) is merged into the +/// preceding segment so it won't wrap to a new line by itself. Opening +/// punctuation (`(` `[` etc.) is merged into the following segment so it +/// won't be stranded at the end of a line. +fn merge_segments_for_line_breaking(text: &str, lens: &mut Vec) { + // Pass 1: Merge "no-break-before" segments (trailing/closing punctuation) + // into the preceding segment. + if lens.len() >= 2 { + let mut i = 1; + let mut byte_offset = lens[0]; + while i < lens.len() { + let seg_end = byte_offset + lens[i]; + let seg_text = &text[byte_offset..seg_end]; + if seg_text.chars().all(is_no_break_before_char) { + lens[i - 1] += lens[i]; + lens.remove(i); + } else { + i += 1; + } + byte_offset = seg_end; + } + } + + // Pass 2: Merge "no-break-after" segments (opening punctuation) + // into the following segment. + if lens.len() >= 2 { + let mut i = 0; + let mut byte_offset = 0; + while i + 1 < lens.len() { + let seg_end = byte_offset + lens[i]; + let seg_text = &text[byte_offset..seg_end]; + if seg_text.chars().all(is_no_break_after_char) { + lens[i + 1] = lens[i] + lens[i + 1]; + lens.remove(i); + } else { + byte_offset = seg_end; + i += 1; + } + } + } +} + +/// Characters before which a line break should not occur (UAX#14 classes +/// CL, CP, EX, IS and common typographic conventions). +fn is_no_break_before_char(c: char) -> bool { + matches!( + c, + // IS: Infix Numeric Separator + '.' | ',' | ':' | ';' + // EX: Exclamation / Interrogation + | '!' | '?' + // CP: Close Parenthesis, CL: Close Punctuation + | ')' | ']' | '}' + // CL: Closing quotation marks + | '\u{2019}' // ' + | '\u{201D}' // \u{201d} + | '\u{203A}' // › + | '\u{00BB}' // » + // Other common no-break-before characters + | '\u{2026}' // … + | '%' + | '\u{00B0}' // ° + ) +} + +/// Characters after which a line break should not occur (UAX#14 class OP). +fn is_no_break_after_char(c: char) -> bool { + matches!( + c, + '(' | '[' | '{' + // Opening quotation marks + | '\u{2018}' // ' + | '\u{201C}' // \u{201c} + | '\u{2039}' // ‹ + | '\u{00AB}' // « + ) +} + pub trait LayoutParams { fn to_owned(self) -> OwnedLayoutParams; fn text(&self) -> &str; @@ -985,7 +1078,8 @@ impl LaidoutGlyph { #[cfg(test)] mod tests { - use super::{parse_text_atlas_size_value, Size}; + use super::{merge_segments_for_line_breaking, parse_text_atlas_size_value, Size}; + use unicode_segmentation::UnicodeSegmentation; #[test] fn parses_text_atlas_size_from_env_value() { @@ -1005,4 +1099,80 @@ mod tests { assert_eq!(parse_text_atlas_size_value("64"), None); assert_eq!(parse_text_atlas_size_value("bogus"), None); } + + /// Helper: split text by word bounds and return segment lengths, + /// then apply merging, then reconstruct the segment strings. + fn merged_segments(text: &str) -> Vec { + let mut lens: Vec = text + .split_word_bounds() + .map(|s| s.len()) + .collect(); + merge_segments_for_line_breaking(text, &mut lens); + let mut result = Vec::new(); + let mut offset = 0; + for len in &lens { + result.push(text[offset..offset + len].to_string()); + offset += len; + } + result + } + + #[test] + fn trailing_punctuation_merges_into_preceding_word() { + assert_eq!(merged_segments("Hello."), vec!["Hello."]); + assert_eq!(merged_segments("Hello,"), vec!["Hello,"]); + assert_eq!(merged_segments("Hello;"), vec!["Hello;"]); + assert_eq!(merged_segments("Hello!"), vec!["Hello!"]); + assert_eq!(merged_segments("Hello?"), vec!["Hello?"]); + } + + #[test] + fn multiple_trailing_punctuation_marks_merge() { + assert_eq!(merged_segments("end.)"), vec!["end.)"]); + assert_eq!(merged_segments("end...)"), vec!["end...)"]); + } + + #[test] + fn punctuation_between_words_merges_correctly() { + assert_eq!( + merged_segments("Hello, world."), + vec!["Hello,", " ", "world."] + ); + } + + #[test] + fn opening_punctuation_merges_into_following_word() { + assert_eq!( + merged_segments("say (hello) now"), + vec!["say", " ", "(hello)", " ", "now"] + ); + } + + #[test] + fn consecutive_trailing_punctuation_chains_merge() { + // ")" and ":" are both no-break-before, so they chain-merge + // into the preceding word. + assert_eq!( + merged_segments("(News Hello): world"), + vec!["(News", " ", "Hello):", " ", "world"] + ); + } + + #[test] + fn no_merge_for_plain_words() { + assert_eq!( + merged_segments("hello world"), + vec!["hello", " ", "world"] + ); + } + + #[test] + fn single_word_no_change() { + assert_eq!(merged_segments("hello"), vec!["hello"]); + } + + #[test] + fn empty_text() { + assert_eq!(merged_segments(""), Vec::::new()); + } } diff --git a/draw/src/text/loader.rs b/draw/src/text/loader.rs index ffe2f46c3a..c12fc45712 100644 --- a/draw/src/text/loader.rs +++ b/draw/src/text/loader.rs @@ -137,8 +137,20 @@ impl Loader { fn load_font(&mut self, id: FontId) -> Option { let definition = self.font_definitions.remove(&id)?; let mut face = FontFace::from_data_and_index(definition.data, definition.index)?; - if !definition.variations.is_empty() { - face.set_variations(&definition.variations); + let mut variations = definition.variations; + if let Some(weight) = definition.weight { + const FONT_WEIGHT_AXIS_TAG: u32 = u32::from_be_bytes(*b"wght"); + if let Some(existing) = variations + .iter_mut() + .find(|(tag, _)| *tag == FONT_WEIGHT_AXIS_TAG) + { + existing.1 = weight; + } else { + variations.push((FONT_WEIGHT_AXIS_TAG, weight)); + } + } + if !variations.is_empty() { + face.set_variations(&variations); } Some(Font::new( id, @@ -168,6 +180,8 @@ pub struct FontDefinition { pub index: u32, pub ascender_fudge_in_ems: f32, pub descender_fudge_in_ems: f32, + /// Convenience mapping for the OpenType `wght` axis. `None` keeps the font default. + pub weight: Option, /// Font variation axis settings as (tag_u32, value) pairs. pub variations: Vec<(u32, f32)>, } @@ -199,6 +213,7 @@ mod tests { index: 0, ascender_fudge_in_ems: -0.1, descender_fudge_in_ems: 0.0, + weight: None, variations: Vec::new(), }, ); @@ -207,4 +222,100 @@ mod tests { let second = loader.get_or_load_font(font_id).expect("test font should load").clone(); assert!(std::rc::Rc::ptr_eq(&first, &second)); } + + fn bundled_variable_font_path() -> PathBuf { + PathBuf::from(env!("CARGO_MANIFEST_DIR")) + .join("../widgets/resources/jetbrains_mono_variable.ttf") + } + + fn outline_signature(font: &crate::text::font::Font, glyph_id: crate::text::font::GlyphId) -> u64 { + use crate::text::glyph_outline::Command; + use std::collections::hash_map::DefaultHasher; + use std::hash::{Hash, Hasher}; + + let outline = font + .glyph_outline(glyph_id) + .expect("glyph outline should exist"); + let mut hasher = DefaultHasher::new(); + for command in outline.commands() { + match command { + Command::MoveTo(p) => { + 0u8.hash(&mut hasher); + p.x.to_bits().hash(&mut hasher); + p.y.to_bits().hash(&mut hasher); + } + Command::LineTo(p) => { + 1u8.hash(&mut hasher); + p.x.to_bits().hash(&mut hasher); + p.y.to_bits().hash(&mut hasher); + } + Command::QuadTo(c, p) => { + 2u8.hash(&mut hasher); + c.x.to_bits().hash(&mut hasher); + c.y.to_bits().hash(&mut hasher); + p.x.to_bits().hash(&mut hasher); + p.y.to_bits().hash(&mut hasher); + } + Command::CurveTo(c1, c2, p) => { + 3u8.hash(&mut hasher); + c1.x.to_bits().hash(&mut hasher); + c1.y.to_bits().hash(&mut hasher); + c2.x.to_bits().hash(&mut hasher); + c2.y.to_bits().hash(&mut hasher); + p.x.to_bits().hash(&mut hasher); + p.y.to_bits().hash(&mut hasher); + } + Command::Close => { + 4u8.hash(&mut hasher); + } + } + } + hasher.finish() + } + + #[test] + fn variable_font_weight_changes_outline_shape() { + let mut loader = Loader::new(layouter::Settings::default().loader); + let font_data = SharedBytes::from_file_mmap_or_read(bundled_variable_font_path()) + .expect("variable font bytes should load"); + let regular_id: FontId = 0xD00D_0001_u64.into(); + let bold_id: FontId = 0xD00D_0002_u64.into(); + + loader.define_font( + regular_id, + FontDefinition { + data: font_data.clone(), + index: 0, + ascender_fudge_in_ems: 0.0, + descender_fudge_in_ems: 0.0, + weight: Some(200.0), + variations: Vec::new(), + }, + ); + loader.define_font( + bold_id, + FontDefinition { + data: font_data, + index: 0, + ascender_fudge_in_ems: 0.0, + descender_fudge_in_ems: 0.0, + weight: Some(800.0), + variations: Vec::new(), + }, + ); + + let regular = loader.get_or_load_font(regular_id).expect("test font should load").clone(); + let bold = loader.get_or_load_font(bold_id).expect("test font should load").clone(); + let glyph_id = regular.with_ttf_parser_face(|face| { + face.glyph_index('B') + .expect("test glyph should exist in variable font") + .0 + }); + + assert_ne!( + outline_signature(regular.as_ref(), glyph_id), + outline_signature(bold.as_ref(), glyph_id), + "different weight masters should produce different outlines" + ); + } } diff --git a/draw/src/turtle.rs b/draw/src/turtle.rs index e860cc2fe0..2f63173795 100644 --- a/draw/src/turtle.rs +++ b/draw/src/turtle.rs @@ -795,6 +795,11 @@ impl Turtle { self.height } + /// Sets the height of this turtle's rectangle. + pub fn set_height(&mut self, height: f64) { + self.height = height; + } + /// Returns the used width of this turtle's rectangle. pub fn used_width(&self) -> f64 { self.used_width @@ -1657,6 +1662,65 @@ impl<'a, 'b> Cx2d<'a, 'b> { }; } + /// Computes the maximum available height for the current turtle by walking up + /// the ancestor turtle stack and evaluating any `Fit { max }` constraints. + /// + /// Returns the tightest (smallest) max height found, accounting for padding + /// consumed by each ancestor layer. Returns `f64::MAX` if no ancestor has a + /// max constraint. + /// + /// This is useful for widgets like TextInput that need to know when to start + /// scrolling, even when their own walk height is unbounded `Fit`. + pub fn compute_max_height_from_ancestors(&self) -> f64 { + let mut max_height = f64::MAX; + let current = match self.turtles.last() { + Some(t) => t, + None => return f64::MAX, + }; + let mut consumed_padding = current.padding().height(); + + // Walk ancestors (skip self) + for ancestor in self.turtles.iter().rev().skip(1) { + if let Size::Fit { max: Some(max), .. } = ancestor.walk.height { + if let Some(ancestor_max) = max.eval_height(self) { + // The available height for the current turtle is the ancestor's + // max minus the padding/spacing consumed between the ancestor + // and the current turtle. + let available = ancestor_max - consumed_padding; + max_height = max_height.min(available); + } + } + + // If this ancestor has a known (non-NaN) height, it already constrains + // children via layout, so we don't need to look further up. + if !ancestor.height().is_nan() { + let available = ancestor.inner_height() - consumed_padding + current.padding().height(); + max_height = max_height.min(available); + break; + } + + consumed_padding += ancestor.padding().height(); + } + max_height + } + + /// Pushes a clip rect entry and returns the index of that entry in the align list. + /// The clip can later be modified via [`update_clip_rect_at`]. + pub fn push_clip_rect_tracked(&mut self, rect: Rect) -> usize { + let index = self.align_list.len(); + self.align_list + .push(AlignEntry::BeginClip(rect.pos, rect.pos + rect.size)); + index + } + + /// Updates a previously pushed clip rect at the given align list index. + pub fn update_clip_rect_at(&mut self, index: usize, rect: Rect) { + if let AlignEntry::BeginClip(clip_min, clip_max) = &mut self.align_list[index] { + *clip_min = rect.pos; + *clip_max = rect.pos + rect.size; + } + } + // Returns the end of the align list of the finished walk with the given index. fn finished_walk_align_list_end(&self, index: usize) -> usize { if index + 1 < self.finished_walks.len() { diff --git a/examples/splash/src/main.rs b/examples/splash/src/main.rs index f2631787b4..89a64df005 100644 --- a/examples/splash/src/main.rs +++ b/examples/splash/src/main.rs @@ -1,5 +1,11 @@ use makepad_widgets::animator::Animate; use makepad_widgets::file_tree::FileTree; +use makepad_widgets::shader::draw_glyph::GlyphShapeId; +use makepad_widgets::text::{ + font_family::FontFamilyId, + glyph_outline::Command as OutlineCommand, + layouter::{LaidoutGlyph, LaidoutText}, +}; use makepad_widgets::*; use std::path::Path; @@ -7,6 +13,7 @@ app_main!(App); script_mod! { use mod.prelude.widgets.* + use mod.text.* use mod.widgets.* let TestDraw = #(TestDraw::register_widget(vm)) { width: 250 @@ -22,6 +29,29 @@ script_mod! { draw_text.color: #0f0 } + let VarGlyphLabel = #(VarGlyphLabel::register_widget(vm)) { + width: Fit + height: Fit + color: #fff + draw_glyph +: { + aa_pad_px: 1.0 + } + draw_text +: { + text_style: TextStyle{ + font_family: FontFamily{ + latin := FontMember{ + res: crate_resource("self:../../widgets/resources/jetbrains_mono_variable.ttf") + asc: 0.0 + desc: 0.0 + weight: 400.0 + } + } + font_size: 18.0 + line_spacing: 1.2 + } + } + } + // =========================================== // SCROLLBAR TEST - Variable height items // =========================================== @@ -718,6 +748,185 @@ script_mod! { // MATH VIEW TAB - LaTeX math rendering // =========================================== + let TabVarTtf = SolidView{ + width: Fill height: Fill + draw_bg.color: #333 + ScrollYView{ + width: Fill height: Fill flow: Down padding: 15 spacing: 12 + + Label{text: "VarTtf - JetBrains Mono Variable" draw_text.color: #fff draw_text.text_style.font_size: 13} + Label{text: "Atlas text uses small 10px samples to stay on the SDF path. Glyph samples use the DrawGlyph backend." draw_text.color: #888 draw_text.text_style.font_size: 10} + + Hr{} + + Label{text: "Atlas Backend (SDF)" draw_text.color: #aaa draw_text.text_style.font_size: 10} + + Label{text: "Weight 200" draw_text.color: #777 draw_text.text_style.font_size: 9} + Label{ + text: "fn atlas_demo() -> mono_200 { 0123456789 }" + draw_text.color: #fff + draw_text +: { + text_style: TextStyle{ + font_family: FontFamily{ + latin := FontMember{ + res: crate_resource("self:../../widgets/resources/jetbrains_mono_variable.ttf") + asc: 0.0 + desc: 0.0 + weight: 200.0 + } + } + font_size: 10.0 + line_spacing: 1.2 + } + } + } + + Label{text: "Weight 400" draw_text.color: #777 draw_text.text_style.font_size: 9} + Label{ + text: "fn atlas_demo() -> mono_400 { 0123456789 }" + draw_text.color: #fff + draw_text +: { + text_style: TextStyle{ + font_family: FontFamily{ + latin := FontMember{ + res: crate_resource("self:../../widgets/resources/jetbrains_mono_variable.ttf") + asc: 0.0 + desc: 0.0 + weight: 400.0 + } + } + font_size: 10.0 + line_spacing: 1.2 + } + } + } + + Label{text: "Weight 700" draw_text.color: #777 draw_text.text_style.font_size: 9} + Label{ + text: "fn atlas_demo() -> mono_700 { 0123456789 }" + draw_text.color: #fff + draw_text +: { + text_style: TextStyle{ + font_family: FontFamily{ + latin := FontMember{ + res: crate_resource("self:../../widgets/resources/jetbrains_mono_variable.ttf") + asc: 0.0 + desc: 0.0 + weight: 700.0 + } + } + font_size: 10.0 + line_spacing: 1.2 + } + } + } + + Label{text: "Weight 900" draw_text.color: #777 draw_text.text_style.font_size: 9} + Label{ + text: "fn atlas_demo() -> mono_900 { 0123456789 }" + draw_text.color: #fff + draw_text +: { + text_style: TextStyle{ + font_family: FontFamily{ + latin := FontMember{ + res: crate_resource("self:../../widgets/resources/jetbrains_mono_variable.ttf") + asc: 0.0 + desc: 0.0 + weight: 900.0 + } + } + font_size: 10.0 + line_spacing: 1.2 + } + } + } + + Hr{} + + Label{text: "Glyph Backend (DrawGlyph)" draw_text.color: #aaa draw_text.text_style.font_size: 10} + + Label{text: "Weight 200" draw_text.color: #777 draw_text.text_style.font_size: 9} + VarGlyphLabel{ + text: "fn glyph_demo() -> mono_200 { 0123456789 }" + color: #fff + draw_text +: { + text_style: TextStyle{ + font_family: FontFamily{ + latin := FontMember{ + res: crate_resource("self:../../widgets/resources/jetbrains_mono_variable.ttf") + asc: 0.0 + desc: 0.0 + weight: 200.0 + } + } + font_size: 18.0 + line_spacing: 1.2 + } + } + } + + Label{text: "Weight 400" draw_text.color: #777 draw_text.text_style.font_size: 9} + VarGlyphLabel{ + text: "fn glyph_demo() -> mono_400 { 0123456789 }" + color: #fff + draw_text +: { + text_style: TextStyle{ + font_family: FontFamily{ + latin := FontMember{ + res: crate_resource("self:../../widgets/resources/jetbrains_mono_variable.ttf") + asc: 0.0 + desc: 0.0 + weight: 400.0 + } + } + font_size: 18.0 + line_spacing: 1.2 + } + } + } + + Label{text: "Weight 700" draw_text.color: #777 draw_text.text_style.font_size: 9} + VarGlyphLabel{ + text: "fn glyph_demo() -> mono_700 { 0123456789 }" + color: #fff + draw_text +: { + text_style: TextStyle{ + font_family: FontFamily{ + latin := FontMember{ + res: crate_resource("self:../../widgets/resources/jetbrains_mono_variable.ttf") + asc: 0.0 + desc: 0.0 + weight: 700.0 + } + } + font_size: 18.0 + line_spacing: 1.2 + } + } + } + + Label{text: "Weight 900" draw_text.color: #777 draw_text.text_style.font_size: 9} + VarGlyphLabel{ + text: "fn glyph_demo() -> mono_900 { 0123456789 }" + color: #fff + draw_text +: { + text_style: TextStyle{ + font_family: FontFamily{ + latin := FontMember{ + res: crate_resource("self:../../widgets/resources/jetbrains_mono_variable.ttf") + asc: 0.0 + desc: 0.0 + weight: 900.0 + } + } + font_size: 18.0 + line_spacing: 1.2 + } + } + } + } + } + let TabMathView = SolidView{ width: Fill height: Fill draw_bg.color: #333 @@ -1430,7 +1639,7 @@ script_mod! { right_split := DockSplitter{ axis: SplitterAxis.Vertical - align: SplitterAlign.FromB(250.0) + align: SplitterAlign.FromB(120.0) a: @center_tabs b: @bottom_tabs } @@ -1444,8 +1653,8 @@ script_mod! { // Center panel - content widgets center_tabs := DockTabs{ - tabs: [@bigtext_tab, @math_tab, @vector_tab, @media_tab, @markup_tab, @buttons_tab, @modal_tab, @lists_tab] - selected: 3 + tabs: [@varttf_tab, @bigtext_tab, @math_tab, @vector_tab, @media_tab, @markup_tab, @buttons_tab, @modal_tab, @lists_tab] + selected: 0 closable: true } @@ -1469,6 +1678,11 @@ script_mod! { kind: @TabSelectionTest } // Individual tabs + varttf_tab := DockTab{ + name: "VarTtf" + template: @CloseableTab + kind: @TabVarTtf } + bigtext_tab := DockTab{ name: "BigText" template: @CloseableTab @@ -1555,6 +1769,7 @@ script_mod! { kind: @TabModal } // Content templates by widget type + TabVarTtf := TabVarTtf{} TabBigText := TabBigText{} TabMathView := TabMathView{} TabVector := TabVector{} @@ -1862,6 +2077,35 @@ pub struct TestDraw { area: Area, } +#[derive(Script, ScriptHook, Widget)] +pub struct VarGlyphLabel { + #[uid] + uid: WidgetUid, + #[source] + source: ScriptObjectRef, + #[walk] + walk: Walk, + #[redraw] + #[live] + draw_glyph: DrawGlyph, + #[live] + draw_text: DrawText, + #[live] + text: String, + #[live] + color: Vec4, + #[rust] + old_text: String, + #[rust] + old_font_size: f32, + #[rust] + old_font_family_id: Option, + #[rust] + shape_id: Option, + #[rust] + size: Vec2f, +} + impl Widget for TestDraw { fn draw_walk(&mut self, cx: &mut Cx2d, _scope: &mut Scope, walk: Walk) -> DrawStep { cx.begin_turtle(walk, self.layout); @@ -1888,6 +2132,140 @@ impl Widget for TestDraw { fn handle_event(&mut self, _cx: &mut Cx, _event: &Event, _scope: &mut Scope) {} } +impl Widget for VarGlyphLabel { + fn draw_walk(&mut self, cx: &mut Cx2d, _scope: &mut Scope, mut walk: Walk) -> DrawStep { + self.rebuild_shape(cx); + walk.width = Size::Fixed(self.size.x as f64); + walk.height = Size::Fixed(self.size.y as f64); + let bounds = cx.walk_turtle(walk); + + let Some(shape_id) = self.shape_id else { + return DrawStep::done(); + }; + let (shape_origin, shape_size, mut layers) = { + let Some(shape) = self.draw_glyph.shape(shape_id) else { + return DrawStep::done(); + }; + (shape.origin, shape.size, shape.layers.clone()) + }; + + for layer in &mut layers { + layer.color = vec4( + self.color.x, + self.color.y, + self.color.z, + self.color.w * layer.color.w, + ); + } + + self.draw_glyph.draw_layers_abs( + cx, + rect( + bounds.pos.x + shape_origin.x as f64, + bounds.pos.y + shape_origin.y as f64, + shape_size.x as f64, + shape_size.y as f64, + ), + &layers, + ); + DrawStep::done() + } + + fn handle_event(&mut self, _cx: &mut Cx, _event: &Event, _scope: &mut Scope) {} +} + +impl VarGlyphLabel { + fn rebuild_shape(&mut self, cx: &mut Cx2d) { + let font_family_id = self.draw_text.text_style.font_family_id(); + let font_size = self.draw_text.text_style.font_size; + if self.text == self.old_text + && self.old_font_family_id == Some(font_family_id) + && self.old_font_size.to_bits() == font_size.to_bits() + { + return; + } + + self.old_text = self.text.clone(); + self.old_font_family_id = Some(font_family_id); + self.old_font_size = font_size; + self.shape_id = None; + self.size = vec2(0.0, 0.0); + self.draw_glyph.clear_shapes(); + + if self.text.is_empty() { + return; + } + + let laidout = self + .draw_text + .layout(cx.cx.cx, 0.0, 0.0, None, false, Align::default(), &self.text); + self.size = vec2(laidout.size_in_lpxs.width, laidout.size_in_lpxs.height); + self.shape_id = build_laidout_text_shape(&mut self.draw_glyph, laidout.as_ref()); + } +} + +fn build_laidout_text_shape(dg: &mut DrawGlyph, laidout: &LaidoutText) -> Option { + dg.begin_shape(); + dg.set_color(1.0, 1.0, 1.0, 1.0); + + for row in &laidout.rows { + for glyph in &row.glyphs { + append_laidout_glyph_outline(dg, glyph, row.origin_in_lpxs.x, row.origin_in_lpxs.y); + } + } + + dg.fill_layer(); + dg.commit_shape(None) +} + +fn append_laidout_glyph_outline( + dg: &mut DrawGlyph, + glyph: &LaidoutGlyph, + row_origin_x: f32, + row_origin_y: f32, +) { + let font = glyph.font.as_ref(); + let Some(outline) = font.glyph_outline(glyph.id) else { + return; + }; + + let glyph_x = row_origin_x + glyph.origin_in_lpxs.x + glyph.offset_in_lpxs(); + let glyph_y = row_origin_y; + let font_scale = glyph.font_size_in_lpxs / font.units_per_em(); + + for command in outline.commands() { + match command { + OutlineCommand::MoveTo(p) => { + dg.move_to(glyph_x + p.x * font_scale, glyph_y - p.y * font_scale); + } + OutlineCommand::LineTo(p) => { + dg.line_to(glyph_x + p.x * font_scale, glyph_y - p.y * font_scale); + } + OutlineCommand::QuadTo(c, p) => { + dg.quad_to( + glyph_x + c.x * font_scale, + glyph_y - c.y * font_scale, + glyph_x + p.x * font_scale, + glyph_y - p.y * font_scale, + ); + } + OutlineCommand::CurveTo(c1, c2, p) => { + dg.bezier_to( + glyph_x + c1.x * font_scale, + glyph_y - c1.y * font_scale, + glyph_x + c2.x * font_scale, + glyph_y - c2.y * font_scale, + glyph_x + p.x * font_scale, + glyph_y - p.y * font_scale, + ); + } + OutlineCommand::Close => { + dg.close(); + } + } + } +} + // NewsListTest widget demonstrating PortalList usage #[derive(Script, ScriptHook, Widget)] pub struct NewsListTest { diff --git a/examples/uizoo/src/app.rs b/examples/uizoo/src/app.rs index 439e746142..89e0a23f7b 100644 --- a/examples/uizoo/src/app.rs +++ b/examples/uizoo/src/app.rs @@ -229,6 +229,15 @@ impl MatchEvent for App { lbl.set_text(cx, &format!("{} {}", self.counter, txt)); } + if let Some(is_multiline) = self + .ui + .check_box(cx, ids!(multiline_toggle)) + .changed(actions) + { + let ti = self.ui.text_input(cx, ids!(multiline_toggleable)); + ti.set_is_multiline(cx, is_multiline); + } + if self.ui.button(cx, ids!(basicbutton)).clicked(&actions) { log!("BASIC BUTTON CLICKED {}", self.counter); self.counter += 1; diff --git a/examples/uizoo/src/tab_textinput.rs b/examples/uizoo/src/tab_textinput.rs index 08b88fbc05..6cb80d5c76 100644 --- a/examples/uizoo/src/tab_textinput.rs +++ b/examples/uizoo/src/tab_textinput.rs @@ -47,6 +47,71 @@ script_mod! { Hr{} H4{text: "TextInputGradientY"} TextInputGradientY{empty_text: "Inline Label"} + + Hr{} + H4{text: "Multiline TextInput (fixed height, scrollable)"} + P{text: "A multiline text input with a fixed height. Try typing or pasting enough text to overflow, then scroll with mouse wheel or drag the scrollbar."} + multiline_textinput := TextInput{ + empty_text: "Type multiple lines here..." + is_multiline: true + height: 150.0 + width: Fill + } + + Hr{} + H4{text: "Multiline TextInput (taller, with pre-filled content)"} + P{text: "A taller multiline text input pre-filled with content that overflows."} + multiline_prefilled := TextInput{ + empty_text: "Prefilled multiline" + is_multiline: true + height: 200.0 + width: Fill + text: "Line 1: The quick brown fox jumps over the lazy dog.\nLine 2: Pack my box with five dozen liquor jugs.\nLine 3: How vexingly quick daft zebras jump!\nLine 4: The five boxing wizards jump quickly.\nLine 5: Sphinx of black quartz, judge my vow.\nLine 6: Two driven jocks help fax my big quiz.\nLine 7: Crazy Frederick bought many very exquisite opal jewels.\nLine 8: We promptly judged antique ivory buckles for the next prize.\nLine 9: A mad boxer shot a quick, gloved jab to the jaw of his dizzy opponent.\nLine 10: Jaded zombies acted quaintly but kept driving their oxen forward." + } + + Hr{} + H4{text: "Multiline TextInput (read-only)"} + P{text: "A read-only multiline text input. You can scroll and select text, but cannot edit."} + multiline_readonly := TextInput{ + empty_text: "Read-only multiline" + is_multiline: true + is_read_only: true + height: 120.0 + width: Fill + text: "This is a read-only multiline text input.\nYou can scroll through the content and select text,\nbut you cannot modify it.\nLine 4\nLine 5\nLine 6\nLine 7\nLine 8\nLine 9\nLine 10" + } + + Hr{} + H4{text: "Multiline TextInput (Fit height with max bound)"} + P{text: "A multiline text input with Fit height that caps at 100px. The widget grows with content until hitting the max, then scrolls."} + TextInput{ + empty_text: "Type several lines to grow this TextInput, then scroll..." + is_multiline: true + height: Fit{max: FitBound.Abs(100)} + width: Fill + } + + Hr{} + H4{text: "Multiline TextInput (Fit height with relative max bound)"} + P{text: "A multiline text input with Fit height capped at 30% of the parent height. Uses FitBound.Rel with Base.Full."} + TextInput{ + empty_text: "Type several lines to test relative max height..." + is_multiline: true + height: Fit{max: FitBound.Rel{base: Base.Full, factor: 0.3}} + width: Fill + } + + Hr{} + H4{text: "Toggle Multiline"} + P{text: "Use the checkbox to toggle this text input between single-line and multiline mode."} + UIZooRowH{ + multiline_toggle := CheckBox{text: "Multiline"} + } + multiline_toggleable := TextInput{ + text: "Toggle me between single-line and multiline..." + height: Fit + width: Fill + } } } } diff --git a/examples/xr/src/align_test.rs b/examples/xr/src/align_test.rs index 8cab1bf97f..65317bb5e8 100644 --- a/examples/xr/src/align_test.rs +++ b/examples/xr/src/align_test.rs @@ -1,11 +1,10 @@ -use makepad_xr::{ +use makepad_widgets::{ makepad_derive_widget::*, makepad_draw::*, widget::*, widget_async::{ScriptAsyncId, ScriptAsyncResult}, - xr_node::xr_widget_world_transform, - *, }; +use makepad_xr::{net::*, scene::*}; script_mod! { use mod.prelude.widgets_internal.* @@ -61,8 +60,7 @@ impl AlignTest { return self.enabled; } self.enabled = enabled; - cx.xr_depth_mesh().set_surface_analysis_enabled(enabled); - cx.xr_depth_mesh().set_alignment_preview_enabled(enabled); + cx.xr_tsdf().set_surface_analysis_enabled(enabled); self.last_mesh_generation = 0; self.last_mesh_update_sequence = 0; self.last_solution = None; @@ -82,7 +80,7 @@ impl AlignTest { return; } - let Some(depth_mesh) = cx.xr_depth_mesh().latest_mesh() else { + let Some(snapshot) = cx.xr_tsdf().latest_tsdf_snapshot() else { self.last_solution = None; self.local_markers = None; self.remote_markers_local = None; @@ -90,53 +88,54 @@ impl AlignTest { "Test align: waiting for TSDF depth snapshot for loopback packets".to_string(); return; }; - - let mesh_unchanged = depth_mesh.mesh_generation == self.last_mesh_generation - && depth_mesh.update_sequence == self.last_mesh_update_sequence; + let mesh_unchanged = snapshot.generation == self.last_mesh_generation + && snapshot.update_sequence == self.last_mesh_update_sequence; if mesh_unchanged { return; } - self.last_mesh_generation = depth_mesh.mesh_generation; - self.last_mesh_update_sequence = depth_mesh.update_sequence; - let Some(_local_descriptor) = depth_mesh.alignment_descriptor.as_ref() else { + self.last_mesh_generation = snapshot.generation; + self.last_mesh_update_sequence = snapshot.update_sequence; + let Some(local_descriptor) = + XrNetAlignmentDescriptorFrame::from_tsdf_snapshot(snapshot.as_ref(), 0.0) + else { self.last_solution = None; self.local_markers = None; self.remote_markers_local = None; - self.last_status = Self::missing_patch_status(&depth_mesh); + self.last_status = Self::missing_patch_status(snapshot.as_ref()); self.redraw(cx); return; }; - let preview = &depth_mesh.alignment_preview; - self.local_markers = preview.local_markers; - self.remote_markers_local = preview.remote_markers_local; - self.last_solution = preview.solution; + let ground_truth_translation = vec3f(-0.82, 0.0, 0.67); + let ground_truth_yaw = 0.58f32; + let ground_truth_remote_to_local = Pose::new( + Quat::from_axis_angle(vec3f(0.0, 1.0, 0.0), ground_truth_yaw), + ground_truth_translation, + ) + .to_mat4(); + let local_to_remote = ground_truth_remote_to_local.invert(); + let remote_descriptor = local_descriptor.transformed(&local_to_remote); - let Some(solution) = preview.solution else { - if preview.local_markers.is_none() { + self.local_markers = local_descriptor.test_markers(); + self.remote_markers_local = remote_descriptor + .transformed(&ground_truth_remote_to_local) + .test_markers(); + self.last_solution = XrNetAlignmentDescriptorFrame::solve_remote_to_local( + &local_descriptor, + &remote_descriptor, + ); + + let Some(solution) = self.last_solution else { + if self.local_markers.is_none() { self.last_status = - "Test align: descriptor exists but floor marker frame is unstable".to_string(); + "Test align: heightmap exists but marker frame is unstable".to_string(); } else { - let (local_floor, local_wall) = - (preview.local_floor_sample_count, preview.local_wall_sample_count); - let (remote_floor, remote_wall) = - (preview.remote_floor_sample_count, preview.remote_wall_sample_count); - self.last_status = format!( - "Test align: no loopback solve yet | local samples {} (f{} w{}) | remote samples {} (f{} w{})", - preview.local_sample_count, - local_floor, - local_wall, - preview.remote_sample_count, - remote_floor, - remote_wall - ); + self.last_status = "Test align: waiting for loopback heightmap solve".to_string(); } self.redraw(cx); return; }; - let ground_truth_translation = vec3f(-0.82, 0.0, 0.67); - let ground_truth_yaw = 0.58f32; let position_error_cm = (solution.translation - ground_truth_translation).length() * 100.0; let yaw_error_deg = wrap_angle(solution.yaw_radians - ground_truth_yaw) .abs() @@ -157,16 +156,10 @@ impl AlignTest { self.redraw(cx); } - fn missing_patch_status(depth_mesh: &XrDepthMesh) -> String { - let debug = depth_mesh.alignment_debug; + fn missing_patch_status(snapshot: &TsdfPublishedSnapshot) -> String { format!( - "Test align: no TSDF align descriptor yet | near {} | floor cand {} -> {} | wall cand {} -> {} | tris {}", - debug.near_surface_voxel_count, - debug.floor_candidate_count, - debug.floor_sample_count, - debug.wall_candidate_count, - debug.wall_sample_count, - depth_mesh.mesh_triangle_count + "Test align: waiting for published heightmap | chunks {}", + snapshot.grid.chunk_count() ) } diff --git a/examples/xr/src/main.rs b/examples/xr/src/main.rs index 33b1901bb5..57cac2b28e 100644 --- a/examples/xr/src/main.rs +++ b/examples/xr/src/main.rs @@ -1,10 +1,14 @@ use makepad_widgets; use makepad_widgets::*; -use makepad_xr::*; +use makepad_xr::scene::*; app_main!(App); +const ACTIVITY_POSE_SYNC_INTERVAL_SECONDS: f64 = 0.6; +const ACTIVITY_POSE_SYNC_POSITION_EPSILON_METERS: f32 = 0.015; +const ACTIVITY_POSE_SYNC_ROTATION_EPSILON_DEGREES: f32 = 1.5; + script_mod! { use mod.prelude.widgets.* use mod.widgets.* @@ -61,7 +65,7 @@ script_mod! { scene_select := XrSelect{ pos: vec3(0.0, -0.02, -0.62) scale: vec3(0.5, 0.5, 0.5) - active_child: @ico_box_scene + active_child: @tanks_scene test_scene := XrNode{ on_render: ||{ @@ -271,7 +275,7 @@ script_mod! { pos: vec3(0.05, 0.20, -0.42) } - for index in 0..160 { + for index in 0..80 { let diffuse = #xa0a4aa let color = if index % 6 == 0 { #xff6f59 @@ -287,7 +291,8 @@ script_mod! { #xd58cff } IcoSphere{ - projectile_pool: true + spawn_pool: true + shared_object_policy: mod.widgets.XrSharedObjectPolicy.PooledOnDemand density: 0.75 friction: 0.48 restitution: 0.04 @@ -300,6 +305,72 @@ script_mod! { } } + tanks_scene := XrNode{ + pos: vec3(0.0, -0.16, 0.0) + scale: vec3(0.62, 0.62, 0.62) + on_render: ||{ + Platform{ + pos: vec3(0.05, -0.06, -0.10) + size: vec3(1.60, 0.08, 1.10) + color: #x283544 + } + + Cube{ + body: mod.widgets.XrBodyKind.Fixed + size: vec3(0.14, 0.12, 0.14) + corner_radius: 0.02 + roughness: 0.78 + metallic: 0.02 + color: #x516579 + pos: vec3(-0.34, 0.00, -0.26) + } + + Cube{ + body: mod.widgets.XrBodyKind.Fixed + size: vec3(0.18, 0.10, 0.18) + corner_radius: 0.02 + roughness: 0.78 + metallic: 0.02 + color: #x516579 + pos: vec3(0.34, -0.01, -0.32) + } + + Cube{ + body: mod.widgets.XrBodyKind.Fixed + size: vec3(0.54, 0.08, 0.12) + corner_radius: 0.018 + roughness: 0.82 + metallic: 0.0 + color: #x1f2b37 + pos: vec3(0.05, -0.02, -0.48) + } + + Cube{ + body: mod.widgets.XrBodyKind.Fixed + size: vec3(0.12, 0.16, 0.42) + corner_radius: 0.018 + roughness: 0.80 + metallic: 0.0 + color: #x202f3d + pos: vec3(-0.48, 0.02, -0.02) + } + + Cube{ + body: mod.widgets.XrBodyKind.Fixed + size: vec3(0.12, 0.16, 0.42) + corner_radius: 0.018 + roughness: 0.80 + metallic: 0.0 + color: #x202f3d + pos: vec3(0.58, 0.02, -0.02) + } + + Tank{ + pos: vec3(0.05, 0.03, -0.10) + } + } + } + helmet_scene := XrNode{ on_render: ||{ Platform{pos: vec3(0.05, -0.06, -0.10)} @@ -307,6 +378,7 @@ script_mod! { for col in 0..1 { Gltf{ body: mod.widgets.XrBodyKind.Dynamic + shared_object_policy: mod.widgets.XrSharedObjectPolicy.BootstrapShared physics_size: vec3(0.17, 0.21, 0.17) density: 0.9 friction: 0.7 @@ -361,13 +433,15 @@ script_mod! { } } } - xr_people_debug := XrPeopleDebug{} + xr_peer_sync := XrPeerSync{ + auto_alignment_enabled: false + } control_strip := XrView{ visible: false show_in_non_xr: true wrist_left: true - logical_size: vec2(920, 468) + logical_size: vec2(1220, 700) pixel_scale: 0.000215 dpi_factor: 2.0 SolidView{ @@ -422,6 +496,12 @@ script_mod! { on_press: || ui.scene_select.ico_shoot_scene() } + tanks_scene_button := XrUiButton{ + width: 72 + text: "Tanks" + on_press: || ui.scene_select.tanks_scene() + } + block_scene_button := XrUiButton{ width: 88 text: "Blocks" @@ -500,7 +580,7 @@ script_mod! { scene_status := Label{ width: Fill - text: "Default scene: 160 faceted icosahedra with sphere colliders. Shooter mode fires pooled icos from the main index fingertip." + text: "Default scene: tank mode. Drive with the right thumbstick in head-relative screen direction, and grab the tank directly with your hands." draw_text.color: #xe8f4ff } } @@ -518,6 +598,12 @@ script_mod! { draw_text.color: #xe8f4ff } + depth_resolution_3_button := XrUiButton{ + width: 64 + text: "3 cm" + on_press: || ui.root.set_depth_voxel_size(0.03) + } + depth_resolution_5_button := XrUiButton{ width: 64 text: "5 cm" @@ -577,112 +663,35 @@ script_mod! { View{ width: Fill - height: Fit - flow: Down - spacing: 8 - - SolidView{ - width: Fill - height: 32 - padding: Inset{left: 10 right: 10 top: 7 bottom: 7} - draw_bg.color: #x0d1824 - - peer_sync_status_field := Label{ - width: Fill - text: "Peers: off" - draw_text.color: #xe8f4ff - } - } - - SolidView{ - width: Fill - height: 32 - padding: Inset{left: 10 right: 10 top: 7 bottom: 7} - draw_bg.color: #x0d1824 - - network_status_field := Label{ - width: Fill - text: "Network: off" - draw_text.color: #x9ec8e8 - } - } - - SolidView{ - width: Fill - height: 32 - padding: Inset{left: 10 right: 10 top: 7 bottom: 7} - draw_bg.color: #x0d1824 - - alignment_debug_field := Label{ - width: Fill - text: "AlignDbg: off" - draw_text.color: #xffd29a - } - } - - SolidView{ - width: Fill - height: 32 - padding: Inset{left: 10 right: 10 top: 7 bottom: 7} - draw_bg.color: #x0d1824 - - plane_scan_field := Label{ - width: Fill - text: "PlaneScan: off" - draw_text.color: #x9af7c4 - } - } - - SolidView{ - width: Fill - height: 32 - padding: Inset{left: 10 right: 10 top: 7 bottom: 7} - draw_bg.color: #x0d1824 - - physics_geom_field := Label{ - width: Fill - text: "Physics geometry: waiting for frame" - draw_text.color: #xe8f4ff - } - } - - SolidView{ - width: Fill - height: 32 - padding: Inset{left: 10 right: 10 top: 7 bottom: 7} - draw_bg.color: #x0d1824 - - physics_timing_field := Label{ - width: Fill - text: "Physics compute: waiting for frame" - draw_text.color: #xe8f4ff - } - } + height: Fill + flow: Right + spacing: 12 - SolidView{ - width: Fill - height: 32 - padding: Inset{left: 10 right: 10 top: 7 bottom: 7} - draw_bg.color: #x0d1824 + View{ + width: Fill{weight: 66.0} + height: Fill + flow: Down + spacing: 8 - frame_cpu_field := Label{ + SolidView{ width: Fill - text: "CPU frame: waiting for frame" - draw_text.color: #xe8f4ff + height: Fit + padding: Inset{left: 10 right: 10 top: 6 bottom: 6} + draw_bg.color: #x0d1824 + + debug_field := Label{ + width: Fill + height: Fit + text: "Connected peers: 0" + draw_text.color: #xe8f4ff + draw_text.flow: Flow.Right{wrap: true} + } } } - SolidView{ - width: Fill - height: 32 - padding: Inset{left: 10 right: 10 top: 7 bottom: 7} - draw_bg.color: #x0d1824 - - xr_runtime_field := Label{ - width: Fill - text: "XR render scale: waiting for XR session" - draw_text.color: #xe8f4ff - } + View{ + width: Fill{weight: 34.0} + height: Fill } } } @@ -692,7 +701,7 @@ script_mod! { visible: false mode: mod.widgets.XrViewMode.StuckToWrist wrist_left: true - logical_size: vec2(112, 104) + logical_size: vec2(112, 188) pixel_scale: 0.00030 dpi_factor: 1.6 depth_scale: 120.0 @@ -720,6 +729,22 @@ script_mod! { draw_bg.border_radius: 12.0 on_press: || ui.root.reset_physics() } + + wrist_pose_button := XrUiButton{ + width: Fill + height: 40 + text: "Pose" + draw_bg.border_radius: 12.0 + on_press: || ui.root.reset_activity_pose() + } + + wrist_sync_status := Label{ + width: Fill + height: Fit + text: "P:0.0 C:0.0" + draw_text.color: #xe8f4ff + draw_text.flow: Flow.Right{wrap: true} + } } } @@ -735,57 +760,232 @@ pub struct App { #[rust] network_started: bool, #[rust] - last_physics_geometry_text: String, - #[rust] - last_physics_timing_text: String, + last_debug_text: String, #[rust] - last_frame_cpu_text: String, + suppress_activity_broadcast: Option, #[rust] - last_xr_runtime_text: String, + pending_shared_scene_reset: bool, #[rust] - last_peer_sync_status_text: String, + last_activity_pose_sync: Option, #[rust] - last_network_status_text: String, + last_activity_pose_sync_activity: Option, #[rust] - last_alignment_debug_text: String, - #[rust] - last_plane_scan_text: String, + last_activity_pose_sync_at: f64, } impl App { + fn poses_match(left: Pose, right: Pose) -> bool { + let translation_delta = (left.position - right.position).length(); + let rotation_dot = left + .orientation + .dot(right.orientation) + .abs() + .clamp(0.0, 1.0); + let rotation_delta_degrees = (2.0 * rotation_dot.acos()).to_degrees(); + translation_delta <= ACTIVITY_POSE_SYNC_POSITION_EPSILON_METERS + && rotation_delta_degrees <= ACTIVITY_POSE_SYNC_ROTATION_EPSILON_DEGREES + } + + fn current_activity(&self, cx: &mut Cx) -> Option { + self.ui + .widget(cx, ids!(scene_select)) + .borrow::() + .map(|select| select.activity_id()) + } + + fn active_scene_widget(&self, cx: &mut Cx) -> Option { + self.ui + .widget(cx, ids!(scene_select)) + .borrow::() + .and_then(|select| select.active_child_widget_ref()) + } + + fn apply_activity(&mut self, cx: &mut Cx, activity_id: XrActivityId) -> Option { + self.ui + .widget(cx, ids!(scene_select)) + .borrow_mut::() + .and_then(|mut select| select.set_activity(cx, activity_id)) + } + fn ensure_network_started(&mut self, cx: &mut Cx) { if self.network_started { return; } - if let Some(mut people_debug) = self + if let Some(mut peer_sync) = self .ui - .widget(cx, ids!(xr_people_debug)) - .borrow_mut::() + .widget(cx, ids!(xr_peer_sync)) + .borrow_mut::() { - people_debug.set_enabled(cx, true); + peer_sync.set_enabled(cx, true); self.network_started = true; } } + fn ensure_activity_announced(&mut self, cx: &mut Cx) { + let Some(activity_id) = self.current_activity(cx) else { + return; + }; + if let Some(mut peer_sync) = self + .ui + .widget(cx, ids!(xr_peer_sync)) + .borrow_mut::() + { + if peer_sync.enabled() && peer_sync.current_activity().is_none() { + let _ = peer_sync.set_local_activity(cx, activity_id); + } + } + } + + fn sync_authoritative_activity_pose(&mut self, cx: &mut Cx) { + let Some(activity_id) = self.current_activity(cx) else { + self.last_activity_pose_sync = None; + self.last_activity_pose_sync_activity = None; + self.last_activity_pose_sync_at = 0.0; + return; + }; + + let should_sync = self + .ui + .widget(cx, ids!(xr_peer_sync)) + .borrow::() + .is_some_and(|peer_sync| { + peer_sync.enabled() + && peer_sync.connected_peer_count() != 0 + && peer_sync.local_is_activity_authority() + }); + if !should_sync { + self.last_activity_pose_sync = None; + self.last_activity_pose_sync_activity = None; + self.last_activity_pose_sync_at = 0.0; + return; + } + + let Some(content_pose) = self.ui.borrow::().and_then(|root| root.content_pose()) else { + return; + }; + let now = Cx::time_now(); + let activity_changed = self.last_activity_pose_sync_activity != Some(activity_id); + let pose_changed = self + .last_activity_pose_sync + .is_none_or(|previous| !Self::poses_match(previous, content_pose)); + let interval_elapsed = now - self.last_activity_pose_sync_at + >= ACTIVITY_POSE_SYNC_INTERVAL_SECONDS; + if !(activity_changed || pose_changed || interval_elapsed) { + return; + } + + if let Some(mut peer_sync) = self + .ui + .widget(cx, ids!(xr_peer_sync)) + .borrow_mut::() + { + if peer_sync.send_activity_pose_reset(content_pose) { + self.last_activity_pose_sync = Some(content_pose); + self.last_activity_pose_sync_activity = Some(activity_id); + self.last_activity_pose_sync_at = now; + } + } + } + + fn refresh_spawnable_registry(&mut self, cx: &mut Cx, force: bool) { + let Some(activity_id) = self.current_activity(cx) else { + return; + }; + let peer_sync_widget = self.ui.widget(cx, ids!(xr_peer_sync)); + let should_refresh = force + || peer_sync_widget + .borrow::() + .is_some_and(|peer_sync| peer_sync.spawnable_activity() != Some(activity_id)); + if !should_refresh { + if let Some(mut peer_sync) = peer_sync_widget.borrow_mut::() { + peer_sync.flush_pending_shared_object_controls(cx); + } + return; + } + let Some(scene_widget) = self.active_scene_widget(cx) else { + return; + }; + let bindings = collect_scene_spawnable_objects(activity_id, &scene_widget); + { + if let Some(mut peer_sync) = peer_sync_widget.borrow_mut::() { + peer_sync.set_spawnable_objects(activity_id, bindings); + peer_sync.flush_pending_shared_object_controls(cx); + }; + } + } + + fn apply_remote_body_spawn(&mut self, cx: &mut Cx, spawn: XrBodySpawn) { + if let Some(mut root) = self.ui.borrow_mut::() { + root.spawn_body(cx, spawn); + } + } + + fn apply_remote_body_despawn(&mut self, cx: &mut Cx, widget_uid: WidgetUid) { + if let Some(mut root) = self.ui.borrow_mut::() { + root.despawn_body(cx, widget_uid); + } + } + + fn apply_body_impulse(&mut self, cx: &mut Cx, impulse: XrBodyImpulse) { + if let Some(mut root) = self.ui.borrow_mut::() { + root.apply_body_impulse(cx, impulse); + } + } + + fn publish_local_shared_object_states(&mut self, cx: &mut Cx) { + let runtime_bodies = self.ui.borrow::().map(|root| root.runtime_bodies()); + let Some(runtime_bodies) = runtime_bodies else { + return; + }; + let peer_sync_widget = self.ui.widget(cx, ids!(xr_peer_sync)); + let maybe_peer_sync = peer_sync_widget.borrow_mut::(); + if let Some(mut peer_sync) = maybe_peer_sync { + peer_sync.publish_local_shared_object_states(cx, runtime_bodies.as_ref()); + } + } + + fn apply_pending_shared_scene_reset(&mut self, cx: &mut Cx) { + if !self.pending_shared_scene_reset { + return; + } + let runtime_bodies = self.ui.borrow::().map(|root| root.runtime_bodies()); + let Some(runtime_bodies) = runtime_bodies else { + return; + }; + if runtime_bodies.is_empty() { + return; + } + let reset_applied = { + let peer_sync_widget = self.ui.widget(cx, ids!(xr_peer_sync)); + let reset_applied = + if let Some(mut peer_sync) = peer_sync_widget.borrow_mut::() { + peer_sync.reset_local_shared_bootstrap_objects(runtime_bodies.as_ref()); + true + } else { + false + }; + reset_applied + }; + if reset_applied { + self.pending_shared_scene_reset = false; + } + } + fn refresh_debug_fields(&mut self, cx: &mut Cx) { let ( surface_count, - vertex_count, - triangle_count, compute_ms, - physics_time_scale, - step_dt_ms, + query_ms, + rapier_ms, frame_cpu_ms, frame_update_cpu_ms, frame_draw_cpu_ms, ) = if let Some(root) = self.ui.borrow::() { ( root.physics_depth_query_surface_count(), - root.physics_depth_query_vertex_count(), - root.physics_depth_query_triangle_count(), root.physics_compute_ms(), - root.physics_time_scale(), - root.physics_step_dt_ms(), + root.physics_tsdf_query_ms(), + root.physics_rapier_step_ms(), root.frame_cpu_ms(), root.frame_update_cpu_ms(), root.frame_draw_cpu_ms(), @@ -793,158 +993,210 @@ impl App { } else { return; }; - let peer_sync_status_text = self - .ui - .widget(cx, ids!(xr_people_debug)) - .borrow::() - .map(|people_debug| people_debug.status_text().to_string()) - .unwrap_or_else(|| "Peers: unavailable".to_string()); - let network_status_text = self - .ui - .widget(cx, ids!(xr_people_debug)) - .borrow::() - .map(|people_debug| people_debug.network_status_text().to_string()) - .unwrap_or_else(|| "Network: unavailable".to_string()); - let alignment_debug_text = self + let connected_peers = self .ui - .widget(cx, ids!(xr_people_debug)) - .borrow::() - .map(|people_debug| people_debug.alignment_debug_text().to_string()) - .unwrap_or_else(|| "AlignDbg: unavailable".to_string()); - let plane_scan_text = self - .ui - .widget(cx, ids!(xr_people_debug)) - .borrow::() - .map(|people_debug| people_debug.plane_scan_text().to_string()) - .unwrap_or_else(|| "PlaneScan: unavailable".to_string()); - - if self.last_peer_sync_status_text != peer_sync_status_text { - self.ui - .widget(cx, ids!(peer_sync_status_field)) - .set_text(cx, &peer_sync_status_text); - self.last_peer_sync_status_text = peer_sync_status_text; - } - if self.last_network_status_text != network_status_text { + .widget(cx, ids!(xr_peer_sync)) + .borrow::() + .map(|peer_sync| { + ( + peer_sync.connected_peer_count(), + peer_sync.shared_object_count(), + peer_sync.touch_sync_status_text(), + peer_sync.local_touch_signal_text(), + peer_sync.remote_touch_signal_text(), + peer_sync.alignment_debug_text().to_string(), + peer_sync.alignment_state_text().to_string(), + peer_sync.peer_scene_text().to_string(), + ) + }) + .unwrap_or(( + 0, + 0, + "Touch sync: off".to_string(), + "TouchRaw: off".to_string(), + "TouchPeer: off".to_string(), + "AlignDbg: off".to_string(), + "AlignState: off".to_string(), + "PeerMap: off".to_string(), + )); + let tsdf_memory_mb = cx + .xr_tsdf() + .latest_tsdf_snapshot() + .as_ref() + .map(|snapshot| { + let grid = &snapshot.grid; + grid.heap_bytes() as f64 / 1_000_000.0 + }) + .unwrap_or(0.0); + let (depth_frames_seen, depth_frames_dropped) = cx + .xr_tsdf() + .state() + .read() + .ok() + .map(|state| (state.stats.frames_seen, state.stats.frames_dropped)) + .unwrap_or((0, 0)); + let depth_frames_kept = depth_frames_seen.saturating_sub(depth_frames_dropped); + let gpu_time_text = cx + .xr_gpu_frame_time_ms() + .map(|gpu_ms| format!("{gpu_ms:.2} ms")) + .unwrap_or_else(|| "waiting".to_string()); + let debug_text = format!( + "Connected peers: {}\nShared objects: {}\n{}\n{}\n{}\n{}\n{}\n{}\nPhysics planes: {surface_count}\nPhysics compute time: {compute_ms:.2} ms\nQuery time: {query_ms:.2} ms\nRapier time: {rapier_ms:.2} ms\nCPU frame time: {frame_cpu_ms:.2} ms\nUpdate time: {frame_update_cpu_ms:.2} ms\nDraw time: {frame_draw_cpu_ms:.2} ms\nTSDF size: {tsdf_memory_mb:.1} MB\nDepth frames kept: {depth_frames_kept}\nGPU time: {gpu_time_text}", + connected_peers.0, + connected_peers.1, + connected_peers.2, + connected_peers.3, + connected_peers.4, + connected_peers.5, + connected_peers.6, + connected_peers.7, + ); + if self.last_debug_text != debug_text { self.ui - .widget(cx, ids!(network_status_field)) - .set_text(cx, &network_status_text); - self.last_network_status_text = network_status_text; + .widget(cx, ids!(debug_field)) + .set_text(cx, &debug_text); + self.last_debug_text = debug_text; } - if self.last_alignment_debug_text != alignment_debug_text { - self.ui - .widget(cx, ids!(alignment_debug_field)) - .set_text(cx, &alignment_debug_text); - self.last_alignment_debug_text = alignment_debug_text; + let wrist_perf_text = format!( + "P:{:.1} C:{:.1}", + compute_ms + query_ms + rapier_ms, + frame_cpu_ms, + ); + self.ui + .widget(cx, ids!(wrist_sync_status)) + .set_text(cx, &wrist_perf_text); + } +} + +impl MatchEvent for App { + fn handle_actions(&mut self, cx: &mut Cx, actions: &Actions) { + let root_uid = self.ui.widget_uid(); + let scene_select_uid = self.ui.widget(cx, ids!(scene_select)).widget_uid(); + let peer_sync_widget = self.ui.widget(cx, ids!(xr_peer_sync)); + let peer_sync_uid = peer_sync_widget.widget_uid(); + + let mut remote_activity = None; + let mut remote_body_spawns = Vec::new(); + let mut remote_body_impulses = Vec::new(); + let mut remote_body_despawns = Vec::new(); + let mut remote_activity_pose_reset = None; + let mut local_activity = None; + let mut local_body_spawns = Vec::new(); + let mut local_activity_pose_reset = None; + let mut scene_changed = false; + + for action in actions { + let Some(widget_action) = action.as_widget_action() else { + continue; + }; + if widget_action.widget_uid == peer_sync_uid { + match widget_action.cast::() { + XrPeerSyncAction::ActivityChanged(activity_id) => { + remote_activity = Some(activity_id); + } + XrPeerSyncAction::ActivityPoseReset(pose) => { + remote_activity_pose_reset = Some(pose); + } + XrPeerSyncAction::BodySpawn(spawn) => { + remote_body_spawns.push(spawn); + } + XrPeerSyncAction::BodyImpulse(impulse) => { + remote_body_impulses.push(impulse); + } + XrPeerSyncAction::BodyDespawn(widget_uid) => { + remote_body_despawns.push(widget_uid); + } + XrPeerSyncAction::None => {} + } + } + if widget_action.widget_uid == root_uid + { + match widget_action.cast::() { + XrRootAction::PhysicsReset => { + self.pending_shared_scene_reset = true; + } + XrRootAction::ContentPoseReset(pose) => { + local_activity_pose_reset = Some(pose); + } + XrRootAction::None => {} + } + } + if widget_action.widget_uid == scene_select_uid { + if let XrSelectAction::ActiveChildChanged(activity_id) = + widget_action.cast::() + { + local_activity = Some(activity_id); + } + } + if let Some(body_spawn) = widget_action.action.downcast_ref::() { + local_body_spawns.push(*body_spawn); + } + if matches!( + widget_action.cast::(), + XrNodeAction::SceneChanged + ) { + scene_changed = true; + } } - if self.last_plane_scan_text != plane_scan_text { - self.ui - .widget(cx, ids!(plane_scan_field)) - .set_text(cx, &plane_scan_text); - self.last_plane_scan_text = plane_scan_text; + + if scene_changed { + self.refresh_spawnable_registry(cx, true); } - let geometry_text = format!( - "Physics geometry: {} planes, {} vertices, {} triangles", - surface_count, vertex_count, triangle_count - ); - if self.last_physics_geometry_text != geometry_text { - self.ui - .widget(cx, ids!(physics_geom_field)) - .set_text(cx, &geometry_text); - self.last_physics_geometry_text = geometry_text; + if let Some(activity_id) = remote_activity { + if self.current_activity(cx) != Some(activity_id) { + self.suppress_activity_broadcast = Some(activity_id); + if self.apply_activity(cx, activity_id).is_none() { + self.suppress_activity_broadcast = None; + } + } + self.refresh_spawnable_registry(cx, true); } - let timing_text = if step_dt_ms > 0.0 { - format!( - "Physics compute: {:.2} ms | tick {:.2} ms ({:.0} Hz) | sim {:.2}x", - compute_ms, - step_dt_ms, - 1000.0 / step_dt_ms, - physics_time_scale - ) - } else { - format!( - "Physics compute: {:.2} ms | sim {:.2}x", - compute_ms, physics_time_scale - ) - }; - if self.last_physics_timing_text != timing_text { - self.ui - .widget(cx, ids!(physics_timing_field)) - .set_text(cx, &timing_text); - self.last_physics_timing_text = timing_text; + if let Some(activity_id) = local_activity { + self.refresh_spawnable_registry(cx, true); + if self.suppress_activity_broadcast == Some(activity_id) { + self.suppress_activity_broadcast = None; + } else if let Some(mut peer_sync) = peer_sync_widget.borrow_mut::() { + let _ = peer_sync.set_local_activity(cx, activity_id); + } } - let frame_cpu_text = format!( - "CPU frame: {:.2} ms total | update {:.2} ms | draw {:.2} ms", - frame_cpu_ms, frame_update_cpu_ms, frame_draw_cpu_ms - ); - if self.last_frame_cpu_text != frame_cpu_text { - self.ui - .widget(cx, ids!(frame_cpu_field)) - .set_text(cx, &frame_cpu_text); - self.last_frame_cpu_text = frame_cpu_text; + if let Some(pose) = remote_activity_pose_reset { + if let Some(mut root) = self.ui.borrow_mut::() { + root.set_content_pose(cx, pose); + } + self.pending_shared_scene_reset = true; } - let xr_runtime_text = match ( - cx.xr_render_scale(), - cx.xr_display_refresh_rate_hz(), - cx.xr_effective_frame_rate_hz(), - cx.xr_gpu_frame_time_ms(), - ) { - (Some(scale), Some(refresh_hz), Some(effective_hz), Some(gpu_ms)) => format!( - "Depth: {:.0} cm | XR scale: {:.2} | refresh {:.1} Hz | cadence {:.1} Hz | GPU {:.2} ms", - cx.xr_depth_mesh().voxel_size_meters() * 100.0, - scale, - refresh_hz, - effective_hz, - gpu_ms - ), - (Some(scale), Some(refresh_hz), Some(effective_hz), None) => format!( - "Depth: {:.0} cm | XR scale: {:.2} | refresh {:.1} Hz | cadence {:.1} Hz | GPU waiting", - cx.xr_depth_mesh().voxel_size_meters() * 100.0, - scale, - refresh_hz, - effective_hz - ), - (Some(scale), Some(refresh_hz), None, Some(gpu_ms)) => format!( - "Depth: {:.0} cm | XR scale: {:.2} | refresh {:.1} Hz | cadence waiting | GPU {:.2} ms", - cx.xr_depth_mesh().voxel_size_meters() * 100.0, - scale, - refresh_hz, - gpu_ms - ), - (Some(scale), Some(refresh_hz), None, None) => format!( - "Depth: {:.0} cm | XR scale: {:.2} | refresh {:.1} Hz | cadence waiting | GPU waiting", - cx.xr_depth_mesh().voxel_size_meters() * 100.0, - scale, - refresh_hz - ), - (Some(scale), None, _, Some(gpu_ms)) => { - format!( - "Depth: {:.0} cm | XR scale: {:.2} | refresh waiting | GPU {:.2} ms", - cx.xr_depth_mesh().voxel_size_meters() * 100.0, - scale, - gpu_ms - ) + if let Some(pose) = local_activity_pose_reset { + self.pending_shared_scene_reset = true; + if let Some(mut peer_sync) = peer_sync_widget.borrow_mut::() { + let _ = peer_sync.send_activity_pose_reset(pose); } - (Some(scale), None, _, None) => { - format!( - "Depth: {:.0} cm | XR scale: {:.2} | refresh waiting | GPU waiting", - cx.xr_depth_mesh().voxel_size_meters() * 100.0, - scale - ) + } + + for widget_uid in remote_body_despawns { + self.apply_remote_body_despawn(cx, widget_uid); + } + + for spawn in remote_body_spawns { + self.apply_remote_body_spawn(cx, spawn); + } + + for impulse in remote_body_impulses { + self.apply_body_impulse(cx, impulse); + } + + if !local_body_spawns.is_empty() { + self.refresh_spawnable_registry(cx, false); + if let Some(mut peer_sync) = peer_sync_widget.borrow_mut::() { + for spawn in local_body_spawns { + if let Some(spawn) = peer_sync.send_local_body_spawn(spawn) { + self.apply_remote_body_spawn(cx, spawn); + } + } } - (None, _, _, _) => format!( - "Depth: {:.0} cm | XR render scale: not active", - cx.xr_depth_mesh().voxel_size_meters() * 100.0 - ), - }; - if self.last_xr_runtime_text != xr_runtime_text { - self.ui - .widget(cx, ids!(xr_runtime_field)) - .set_text(cx, &xr_runtime_text); - self.last_xr_runtime_text = xr_runtime_text; } } } @@ -957,10 +1209,20 @@ impl AppMain for App { } fn handle_event(&mut self, cx: &mut Cx, event: &Event) { + self.match_event(cx, event); self.ui.handle_event(cx, event, &mut Scope::empty()); if matches!(event, Event::Startup) { self.ensure_network_started(cx); } + self.ensure_activity_announced(cx); + self.sync_authoritative_activity_pose(cx); + self.refresh_spawnable_registry(cx, false); + self.apply_pending_shared_scene_reset(cx); + if matches!(event, Event::XrUpdate(_)) + || (matches!(event, Event::NextFrame(_)) && !cx.in_xr_mode()) + { + self.publish_local_shared_object_states(cx); + } self.refresh_debug_fields(cx); } } diff --git a/libs/apple_sys/src/lib.rs b/libs/apple_sys/src/lib.rs index 6ee0ca3494..7e4eb363ea 100644 --- a/libs/apple_sys/src/lib.rs +++ b/libs/apple_sys/src/lib.rs @@ -674,6 +674,28 @@ unsafe impl Encode for NSRect { } } +#[repr(C)] +#[derive(Copy, Debug, Clone)] +pub struct UIEdgeInsets { + pub top: f64, + pub left: f64, + pub bottom: f64, + pub right: f64, +} + +unsafe impl Encode for UIEdgeInsets { + fn encode() -> Encoding { + let encoding = format!( + "{{UIEdgeInsets={}{}{}{}}}", + f64::encode().as_str(), + f64::encode().as_str(), + f64::encode().as_str(), + f64::encode().as_str() + ); + unsafe { Encoding::from_str(&encoding) } + } +} + pub const NSURLSessionResponseAllow: u64 = 1; #[repr(u64)] // NSUInteger @@ -1099,6 +1121,15 @@ pub struct MTLViewport { pub zfar: f64, } +#[repr(C)] +#[derive(Copy, Clone, Debug)] +pub struct MTLScissorRect { + pub x: u64, + pub y: u64, + pub width: u64, + pub height: u64, +} + #[repr(C)] #[derive(Copy, Clone, Debug)] pub struct CGSize { diff --git a/libs/ggml/Cargo.toml b/libs/ggml/Cargo.toml new file mode 100644 index 0000000000..0b404b34c1 --- /dev/null +++ b/libs/ggml/Cargo.toml @@ -0,0 +1,17 @@ +[package] +name = "makepad-ggml" +version = "0.1.0" +edition = "2021" +description = "Metal-first, backend-neutral ggml runtime for Makepad" +license = "MIT" + +[features] +default = [] +metal-precompile = [] + +[dependencies] + +[target.'cfg(any(target_os = "macos", target_os = "ios"))'.dependencies] +makepad-objc-sys = { path = "../objc-sys", version = "1.0.0" } + +[workspace] diff --git a/libs/ggml/build.rs b/libs/ggml/build.rs new file mode 100644 index 0000000000..6aa591626e --- /dev/null +++ b/libs/ggml/build.rs @@ -0,0 +1,80 @@ +use std::env; +use std::fs; +use std::process::Command; + +fn main() { + println!("cargo:rerun-if-env-changed=MAKEPAD_GGML_METAL_PRECOMPILE"); + + if env::var("CARGO_CFG_TARGET_OS").unwrap_or_default() == "macos" { + build_metallib(); + } +} + +fn build_metallib() { + let precompile_default = env::var_os("CARGO_FEATURE_METAL_PRECOMPILE").is_some(); + let precompile_enabled = env::var("MAKEPAD_GGML_METAL_PRECOMPILE") + .ok() + .map(|v| { + let v = v.trim().to_ascii_lowercase(); + !(v.is_empty() || v == "0" || v == "false" || v == "no" || v == "off") + }) + .unwrap_or(precompile_default); + + let out_dir = env::var("OUT_DIR").unwrap(); + let manifest_dir = env::var("CARGO_MANIFEST_DIR").unwrap(); + let metal_dir = format!("{}/src/backend/metal/ggml", manifest_dir); + let metal_src = format!("{}/ggml-metal.metal", metal_dir); + let common_h = format!("{}/ggml-common.h", metal_dir); + let impl_h = format!("{}/ggml-metal-impl.h", metal_dir); + + println!("cargo:rerun-if-changed={}", metal_src); + println!("cargo:rerun-if-changed={}", common_h); + println!("cargo:rerun-if-changed={}", impl_h); + + let _ = fs::create_dir_all(&out_dir); + let air_path = format!("{}/ggml-metal.air", out_dir); + let metallib_path = format!("{}/ggml-default.metallib", out_dir); + + if !precompile_enabled { + let _ = fs::write(&metallib_path, []); + println!("cargo:rustc-env=MAKEPAD_GGML_METALLIB={}", metallib_path); + return; + } + + let metal_status = Command::new("xcrun") + .args([ + "--sdk", "macosx", "metal", "-O3", "-c", &metal_src, "-I", &metal_dir, "-o", &air_path, + ]) + .status(); + + let ok = metal_status.as_ref().is_ok_and(|s| s.success()); + if !ok { + println!( + "cargo:warning=failed to compile ggml-metal.metal to AIR; runtime source compile will be used" + ); + let _ = fs::write(&metallib_path, []); + println!("cargo:rustc-env=MAKEPAD_GGML_METALLIB={}", metallib_path); + return; + } + + let metallib_status = Command::new("xcrun") + .args([ + "--sdk", + "macosx", + "metallib", + &air_path, + "-o", + &metallib_path, + ]) + .status(); + + let ok = metallib_status.as_ref().is_ok_and(|s| s.success()); + if !ok { + println!( + "cargo:warning=failed to build ggml default metallib; runtime source compile will be used" + ); + let _ = fs::write(&metallib_path, []); + } + + println!("cargo:rustc-env=MAKEPAD_GGML_METALLIB={}", metallib_path); +} diff --git a/libs/ggml/src/backend/metal/compat.rs b/libs/ggml/src/backend/metal/compat.rs new file mode 100644 index 0000000000..7eb37ff3fb --- /dev/null +++ b/libs/ggml/src/backend/metal/compat.rs @@ -0,0 +1,6484 @@ +#[allow(dead_code)] +fn log_mul_mat_requested() -> bool { + false +} + +pub fn is_available() -> bool { + cfg!(target_os = "macos") +} + +pub fn try_matmul_nn_f32(a: &[f32], b: &[f32], m: usize, k: usize, n: usize) -> Option> { + imp::try_matmul_nn_f32(a, b, m, k, n) +} + +pub fn try_matmul_nt_f32(a: &[f32], bt: &[f32], m: usize, k: usize, n: usize) -> Option> { + imp::try_matmul_nt_f32(a, bt, m, k, n) +} + +pub fn try_matmul_nt_f32_bytes( + a: &[f32], + bt_bytes: &[u8], + m: usize, + k: usize, + n: usize, +) -> Option> { + imp::try_matmul_nt_f32_bytes(a, bt_bytes, m, k, n) +} + +pub fn try_matmul_nt_f16_bytes( + a: &[f32], + bt_f16_bytes: &[u8], + m: usize, + k: usize, + n: usize, +) -> Option> { + imp::try_matmul_nt_f16_bytes(a, bt_f16_bytes, m, k, n) +} + +pub fn try_matmul_nt_ggml_bytes( + a: &[f32], + bt_bytes: &[u8], + bt_ggml_type: u32, + m: usize, + k: usize, + n: usize, +) -> Option> { + imp::try_matmul_nt_ggml_bytes(a, bt_bytes, bt_ggml_type, m, k, n) +} + +pub fn try_matmul_nt_ggml_bytes_add_bias( + a: &[f32], + bt_bytes: &[u8], + bt_ggml_type: u32, + m: usize, + k: usize, + n: usize, + bias: &[f32], +) -> Option> { + imp::try_matmul_nt_ggml_bytes_add_bias(a, bt_bytes, bt_ggml_type, m, k, n, bias) +} + +pub fn try_flash_attn_f32_packed( + q: &[f32], + k: &[f32], + v: &[f32], + n_q: usize, + n_kv: usize, + n_head: usize, + d: usize, + scale: f32, +) -> Option> { + imp::try_flash_attn_f32_packed(q, k, v, n_q, n_kv, n_head, d, scale) +} + +pub fn clear_decoder_kv_cache() { + imp::clear_decoder_kv_cache(); +} + +#[allow(clippy::too_many_arguments)] +pub fn try_flash_attn_f32_self_kv_cache( + layer: usize, + q: &[f32], + k_all: &[f32], + v_all: &[f32], + n_kv: usize, + n_head: usize, + d: usize, + scale: f32, +) -> Option> { + imp::try_flash_attn_f32_self_kv_cache(layer, q, k_all, v_all, n_kv, n_head, d, scale) +} + +#[allow(clippy::too_many_arguments)] +pub fn try_flash_attn_f32_cross_kv_cache( + layer: usize, + q: &[f32], + k_cross: &[f32], + v_cross: &[f32], + n_q: usize, + n_kv: usize, + n_head: usize, + d: usize, + scale: f32, +) -> Option> { + imp::try_flash_attn_f32_cross_kv_cache(layer, q, k_cross, v_cross, n_q, n_kv, n_head, d, scale) +} + +pub fn try_add_f32(a: &[f32], a_shape: &[usize], b: &[f32], b_shape: &[usize]) -> Option> { + imp::try_add_f32(a, a_shape, b, b_shape) +} + +pub fn try_mul_f32(a: &[f32], a_shape: &[usize], b: &[f32], b_shape: &[usize]) -> Option> { + imp::try_mul_f32(a, a_shape, b, b_shape) +} + +pub fn try_gelu_f32(a: &[f32], shape: &[usize]) -> Option> { + imp::try_gelu_f32(a, shape) +} + +pub fn try_layer_norm_f32(x: &[f32], shape: &[usize], eps: f32) -> Option> { + imp::try_layer_norm_f32(x, shape, eps) +} + +pub fn try_rms_norm_f32(x: &[f32], shape: &[usize], eps: f32) -> Option> { + imp::try_rms_norm_f32(x, shape, eps) +} + +pub fn try_rms_norm_mul_f32( + x: &[f32], + x_shape: &[usize], + mul: &[f32], + mul_shape: &[usize], + eps: f32, +) -> Option> { + imp::try_rms_norm_mul_f32(x, x_shape, mul, mul_shape, eps) +} + +pub fn try_layer_norm_mul_add_f32( + x: &[f32], + x_shape: &[usize], + mul: &[f32], + mul_shape: &[usize], + add: &[f32], + add_shape: &[usize], + eps: f32, +) -> Option> { + imp::try_layer_norm_mul_add_f32(x, x_shape, mul, mul_shape, add, add_shape, eps) +} + +pub fn try_get_rows_ggml_bytes( + src: &[u8], + src_ggml_type: u32, + n_cols: usize, + n_rows: usize, + row_indices: &[i32], +) -> Option> { + imp::try_get_rows_ggml_bytes(src, src_ggml_type, n_cols, n_rows, row_indices) +} + +pub fn try_im2col_1d_f32( + input: &[f32], + ic: usize, + iw: usize, + kw: usize, + stride: usize, + pad: usize, +) -> Option> { + imp::try_im2col_1d_f32(input, ic, iw, kw, stride, pad) +} + +#[cfg(not(target_os = "macos"))] +mod imp { + pub(super) fn try_matmul_nn_f32( + _a: &[f32], + _b: &[f32], + _m: usize, + _k: usize, + _n: usize, + ) -> Option> { + None + } + + pub(super) fn try_matmul_nt_f32( + _a: &[f32], + _bt: &[f32], + _m: usize, + _k: usize, + _n: usize, + ) -> Option> { + None + } + + pub(super) fn try_matmul_nt_f32_bytes( + _a: &[f32], + _bt_bytes: &[u8], + _m: usize, + _k: usize, + _n: usize, + ) -> Option> { + None + } + + pub(super) fn try_matmul_nt_f16_bytes( + _a: &[f32], + _bt_f16_bytes: &[u8], + _m: usize, + _k: usize, + _n: usize, + ) -> Option> { + None + } + + pub(super) fn try_matmul_nt_ggml_bytes( + _a: &[f32], + _bt_bytes: &[u8], + _bt_ggml_type: u32, + _m: usize, + _k: usize, + _n: usize, + ) -> Option> { + None + } + + pub(super) fn try_matmul_nt_ggml_bytes_add_bias( + _a: &[f32], + _bt_bytes: &[u8], + _bt_ggml_type: u32, + _m: usize, + _k: usize, + _n: usize, + _bias: &[f32], + ) -> Option> { + None + } + + pub(super) fn try_flash_attn_f32_packed( + _q: &[f32], + _k: &[f32], + _v: &[f32], + _n_q: usize, + _n_kv: usize, + _n_head: usize, + _d: usize, + _scale: f32, + ) -> Option> { + None + } + + pub(super) fn clear_decoder_kv_cache() {} + + #[allow(clippy::too_many_arguments)] + pub(super) fn try_flash_attn_f32_self_kv_cache( + _layer: usize, + _q: &[f32], + _k_all: &[f32], + _v_all: &[f32], + _n_kv: usize, + _n_head: usize, + _d: usize, + _scale: f32, + ) -> Option> { + None + } + + #[allow(clippy::too_many_arguments)] + pub(super) fn try_flash_attn_f32_cross_kv_cache( + _layer: usize, + _q: &[f32], + _k_cross: &[f32], + _v_cross: &[f32], + _n_q: usize, + _n_kv: usize, + _n_head: usize, + _d: usize, + _scale: f32, + ) -> Option> { + None + } + + pub(super) fn try_add_f32( + _a: &[f32], + _a_shape: &[usize], + _b: &[f32], + _b_shape: &[usize], + ) -> Option> { + None + } + + pub(super) fn try_mul_f32( + _a: &[f32], + _a_shape: &[usize], + _b: &[f32], + _b_shape: &[usize], + ) -> Option> { + None + } + + pub(super) fn try_gelu_f32(_a: &[f32], _shape: &[usize]) -> Option> { + None + } + + pub(super) fn try_layer_norm_f32(_x: &[f32], _shape: &[usize], _eps: f32) -> Option> { + None + } + + pub(super) fn try_rms_norm_f32(_x: &[f32], _shape: &[usize], _eps: f32) -> Option> { + None + } + + pub(super) fn try_rms_norm_mul_f32( + _x: &[f32], + _x_shape: &[usize], + _mul: &[f32], + _mul_shape: &[usize], + _eps: f32, + ) -> Option> { + None + } + + pub(super) fn try_layer_norm_mul_add_f32( + _x: &[f32], + _x_shape: &[usize], + _mul: &[f32], + _mul_shape: &[usize], + _add: &[f32], + _add_shape: &[usize], + _eps: f32, + ) -> Option> { + None + } + + pub(super) fn try_get_rows_ggml_bytes( + _src: &[u8], + _src_ggml_type: u32, + _n_cols: usize, + _n_rows: usize, + _row_indices: &[i32], + ) -> Option> { + None + } + + pub(super) fn try_im2col_1d_f32( + _input: &[f32], + _ic: usize, + _iw: usize, + _kw: usize, + _stride: usize, + _pad: usize, + ) -> Option> { + None + } +} + +#[cfg(target_os = "macos")] +mod imp { + use crate::quant::{ + block_elements, block_size, f32_to_f16, ggml_type_name, GGML_TYPE_BF16, GGML_TYPE_F16, + GGML_TYPE_F32, GGML_TYPE_I32, GGML_TYPE_Q2_K, GGML_TYPE_Q3_K, GGML_TYPE_Q4_0, + GGML_TYPE_Q4_1, GGML_TYPE_Q4_K, GGML_TYPE_Q5_0, GGML_TYPE_Q5_1, GGML_TYPE_Q5_K, + GGML_TYPE_Q6_K, GGML_TYPE_Q8_0, + }; + use makepad_objc_sys::runtime::{nil, ObjcId, Object, YES}; + use makepad_objc_sys::{class, msg_send, sel, sel_impl}; + use std::cell::RefCell; + use std::collections::HashMap; + use std::ffi::{c_char, c_void, CStr}; + use std::ptr::NonNull; + use std::sync::OnceLock; + + const LOG_METAL_PIPELINES: bool = false; + const DISABLE_GGML_METAL_BF16: bool = false; + const DISABLE_GGML_METAL_TENSOR: bool = false; + const FORCE_ENABLE_GGML_METAL_TENSOR: bool = false; + const UTF8_ENCODING: u64 = 4; + const MTL_RESOURCE_STORAGE_MODE_SHARED: u64 = 0; + const MTL_RESOURCE_OPTIONS_STORAGE_MODE_PRIVATE: u64 = 32; + const MTL_STORAGE_MODE_PRIVATE: u64 = 2; + const MTL_GPU_FAMILY_APPLE6: u64 = 1006; + const MTL_GPU_FAMILY_METAL3: u64 = 5001; + const MTL_GPU_FAMILY_METAL4: u64 = 5002; + + const MTL_DATA_TYPE_INT: u64 = 29; + const MTL_DATA_TYPE_SHORT: u64 = 37; + const MTL_DATA_TYPE_BOOL: u64 = 53; + + const FC_FLASH_ATTN_EXT_PAD: i32 = 100; + const FC_FLASH_ATTN_EXT_BLK: i32 = 200; + const FC_FLASH_ATTN_EXT: i32 = 300; + const FC_FLASH_ATTN_EXT_VEC: i32 = 400; + const FC_FLASH_ATTN_EXT_VEC_REDUCE: i32 = 500; + const FC_MUL_MV: i32 = 600; + const FC_MUL_MM: i32 = 700; + const FC_UNARY: i32 = 1200; + const FC_BIN: i32 = 1300; + const OP_FLASH_ATTN_EXT_NQPSG: i32 = 8; + const OP_FLASH_ATTN_EXT_NCPSG: i32 = 64; + const OP_FLASH_ATTN_EXT_VEC_NQPSG: i32 = 1; + const OP_FLASH_ATTN_EXT_VEC_NCPSG: i32 = 32; + const OP_UNARY_NUM_GELU: i16 = 103; + const SCRATCH_FLASH_PAD: u8 = 1; + const SCRATCH_FLASH_BLK: u8 = 2; + const SCRATCH_FLASH_TMP: u8 = 3; + const SCRATCH_FLASH_OUT: u8 = 4; + const SCRATCH_FLASH_MASK: u8 = 5; + const SCRATCH_ENC_NORM0: u8 = 10; + const SCRATCH_ENC_NORM1: u8 = 11; + const SCRATCH_DEC_NORM0: u8 = 12; + const SCRATCH_DEC_NORM1: u8 = 13; + const SCRATCH_ENC_FLASH_K_F16: u8 = 14; + const SCRATCH_ENC_FLASH_V_F16: u8 = 15; + + const N_R0_Q4_0: i32 = 4; + const N_SG_Q4_0: i32 = 2; + + const N_R0_Q4_1: i32 = 4; + const N_SG_Q4_1: i32 = 2; + + const N_R0_Q5_0: i32 = 4; + const N_SG_Q5_0: i32 = 2; + + const N_R0_Q5_1: i32 = 4; + const N_SG_Q5_1: i32 = 2; + + const N_R0_Q8_0: i32 = 2; + const N_SG_Q8_0: i32 = 4; + const N_R0_Q2_K: i32 = 4; + const N_SG_Q2_K: i32 = 2; + const N_R0_Q3_K: i32 = 2; + const N_SG_Q3_K: i32 = 2; + const N_R0_Q4_K: i32 = 2; + const N_SG_Q4_K: i32 = 2; + const N_R0_Q5_K: i32 = 1; + const N_SG_Q5_K: i32 = 2; + const N_R0_Q6_K: i32 = 2; + const N_SG_Q6_K: i32 = 2; + + const _GGML_METAL_SOURCE_RAW: &str = include_str!("ggml/ggml-metal.metal"); + const _GGML_COMMON_H: &str = include_str!("ggml/ggml-common.h"); + const _GGML_METAL_IMPL_H: &str = include_str!("ggml/ggml-metal-impl.h"); + const _GGML_METALLIB_BYTES: &[u8] = include_bytes!(env!("MAKEPAD_GGML_METALLIB")); + + #[link(name = "Metal", kind = "framework")] + extern "C" { + fn MTLCreateSystemDefaultDevice() -> ObjcId; + fn MTLCopyAllDevices() -> ObjcId; + } + + #[link(name = "Foundation", kind = "framework")] + extern "C" {} + + #[repr(C)] + #[derive(Copy, Clone)] + struct MTLSize { + width: u64, + height: u64, + depth: u64, + } + + #[derive(Clone, Copy, Debug, Eq, Hash, PartialEq)] + struct BufferKey { + ptr: usize, + len: usize, + tag: u8, + } + + #[derive(Clone, Copy, Debug, Eq, Hash, PartialEq)] + enum Src0Type { + F32, + F16, + BF16, + Q4_0, + Q4_1, + Q5_0, + Q5_1, + Q8_0, + Q2_K, + Q3_K, + Q4_K, + Q5_K, + Q6_K, + } + + #[derive(Clone, Copy, Debug)] + enum FunctionConstantValue { + Int32(i32), + Int16(i16), + Bool(bool), + } + + #[derive(Clone, Copy, Debug)] + struct FunctionConstant { + idx: i32, + value: FunctionConstantValue, + } + + #[repr(C)] + #[derive(Copy, Clone)] + struct KArgsMulMm { + ne00: i32, + ne02: i32, + nb01: u64, + nb02: u64, + nb03: u64, + ne12: i32, + nb10: u64, + nb11: u64, + nb12: u64, + nb13: u64, + ne0: i32, + ne1: i32, + r2: i16, + r3: i16, + } + + #[repr(C)] + #[derive(Copy, Clone)] + struct KArgsMulMv { + ne00: i32, + ne01: i32, + ne02: i32, + nb00: u64, + nb01: u64, + nb02: u64, + nb03: u64, + ne10: i32, + ne11: i32, + ne12: i32, + nb10: u64, + nb11: u64, + nb12: u64, + nb13: u64, + ne0: i32, + ne1: i32, + nr0: i32, + r2: i16, + r3: i16, + } + + #[repr(C)] + #[derive(Copy, Clone)] + struct KArgsMulMvExt { + ne00: i32, + ne01: i32, + ne02: i32, + nb00: u64, + nb01: u64, + nb02: u64, + nb03: u64, + ne10: i32, + ne11: i32, + ne12: i32, + nb10: u64, + nb11: u64, + nb12: u64, + nb13: u64, + ne0: i32, + ne1: i32, + r2: i16, + r3: i16, + } + + #[repr(C)] + #[derive(Copy, Clone)] + struct KArgsCpy { + nk0: i64, + ne00: i64, + ne01: i64, + ne02: i64, + ne03: i64, + nb00: u64, + nb01: u64, + nb02: u64, + nb03: u64, + ne0: i64, + ne1: i64, + ne2: i64, + ne3: i64, + nb0: u64, + nb1: u64, + nb2: u64, + nb3: u64, + } + + #[repr(C)] + #[derive(Copy, Clone)] + struct KArgsFlashAttnExtPad { + ne11: i32, + ne_12_2: i32, + ne_12_3: i32, + nb11: u64, + nb12: u64, + nb13: u64, + nb21: u64, + nb22: u64, + nb23: u64, + ne31: i32, + ne32: i32, + ne33: i32, + nb31: u64, + nb32: u64, + nb33: u64, + } + + #[repr(C)] + #[derive(Copy, Clone)] + struct KArgsFlashAttnExtBlk { + ne01: i32, + ne30: i32, + ne31: i32, + ne32: i32, + ne33: i32, + nb31: u64, + nb32: u64, + nb33: u64, + } + + #[repr(C)] + #[derive(Copy, Clone)] + struct KArgsFlashAttnExt { + ne01: i32, + ne02: i32, + ne03: i32, + nb01: u64, + nb02: u64, + nb03: u64, + ne11: i32, + ne_12_2: i32, + ne_12_3: i32, + ns10: i32, + nb11: u64, + nb12: u64, + nb13: u64, + ns20: i32, + nb21: u64, + nb22: u64, + nb23: u64, + ne31: i32, + ne32: i32, + ne33: i32, + nb31: u64, + nb32: u64, + nb33: u64, + ne1: i32, + ne2: i32, + ne3: i32, + scale: f32, + max_bias: f32, + m0: f32, + m1: f32, + n_head_log2: i32, + logit_softcap: f32, + } + + #[repr(C)] + #[derive(Copy, Clone)] + struct KArgsFlashAttnExtVec { + ne01: i32, + ne02: i32, + ne03: i32, + nb01: u64, + nb02: u64, + nb03: u64, + ne11: i32, + ne_12_2: i32, + ne_12_3: i32, + ns10: i32, + nb11: u64, + nb12: u64, + nb13: u64, + ns20: i32, + nb21: u64, + nb22: u64, + nb23: u64, + ne31: i32, + ne32: i32, + ne33: i32, + nb31: u64, + nb32: u64, + nb33: u64, + ne1: i32, + ne2: i32, + ne3: i32, + scale: f32, + max_bias: f32, + m0: f32, + m1: f32, + n_head_log2: i32, + logit_softcap: f32, + } + + #[repr(C)] + #[derive(Copy, Clone)] + struct KArgsFlashAttnExtVecReduce { + nrows: i32, + } + + #[repr(C)] + #[derive(Copy, Clone)] + struct KArgsUnary { + ne00: i32, + ne01: i32, + ne02: i32, + ne03: i32, + nb00: u64, + nb01: u64, + nb02: u64, + nb03: u64, + ne0: i32, + ne1: i32, + ne2: i32, + ne3: i32, + nb0: u64, + nb1: u64, + nb2: u64, + nb3: u64, + slope: f32, + scale: f32, + bias: f32, + val: f32, + min: f32, + max: f32, + } + + #[repr(C)] + #[derive(Copy, Clone)] + struct KArgsBin { + ne00: i32, + ne01: i32, + ne02: i32, + ne03: i32, + nb00: u64, + nb01: u64, + nb02: u64, + nb03: u64, + ne10: i32, + ne11: i32, + ne12: i32, + ne13: i32, + nb10: u64, + nb11: u64, + nb12: u64, + nb13: u64, + ne0: i32, + ne1: i32, + ne2: i32, + ne3: i32, + nb0: u64, + nb1: u64, + nb2: u64, + nb3: u64, + offs: u64, + o1: [u64; 8], + } + + #[repr(C)] + #[derive(Copy, Clone)] + struct KArgsNorm { + ne00: i32, + ne00_t: i32, + nb1: u64, + nb2: u64, + nb3: u64, + eps: f32, + nef1: [i32; 3], + nef2: [i32; 3], + nef3: [i32; 3], + nbf1: [u64; 3], + nbf2: [u64; 3], + nbf3: [u64; 3], + } + + #[repr(C)] + #[derive(Copy, Clone)] + struct KArgsGetRows { + ne00t: i32, + ne00: i32, + nb01: u64, + nb02: u64, + nb03: u64, + ne10: i32, + nb10: u64, + nb11: u64, + nb12: u64, + nb1: u64, + nb2: u64, + nb3: u64, + } + + #[repr(C)] + #[derive(Copy, Clone)] + struct KArgsIm2Col { + ofs0: u64, + ofs1: u64, + iw: i32, + ih: i32, + chw: i32, + s0: i32, + s1: i32, + p0: i32, + p1: i32, + d0: i32, + d1: i32, + n: i32, + kh: i32, + kw: i32, + khw: i32, + } + + #[derive(Copy, Clone)] + struct Shape4 { + ne: [i32; 4], + nb: [u64; 4], + numel: usize, + } + + struct StrongId(NonNull); + + impl StrongId { + unsafe fn from_owned(id: ObjcId) -> Option { + NonNull::new(id).map(Self) + } + + unsafe fn from_unowned(id: ObjcId) -> Option { + if id.is_null() { + return None; + } + let _: () = msg_send![id, retain]; + NonNull::new(id).map(Self) + } + + fn as_id(&self) -> ObjcId { + self.0.as_ptr() + } + } + + impl Drop for StrongId { + fn drop(&mut self) { + unsafe { + let _: () = msg_send![self.0.as_ptr(), release]; + } + } + } + + struct AutoreleasePool(ObjcId); + + impl AutoreleasePool { + fn new() -> Self { + let pool: ObjcId = unsafe { msg_send![class!(NSAutoreleasePool), new] }; + Self(pool) + } + } + + impl Drop for AutoreleasePool { + fn drop(&mut self) { + if !self.0.is_null() { + unsafe { + let _: () = msg_send![self.0, release]; + } + } + } + } + + fn nsstring_to_string(ns_string: ObjcId) -> String { + if ns_string.is_null() { + return String::new(); + } + unsafe { + let utf8_ptr: *const c_char = msg_send![ns_string, UTF8String]; + if utf8_ptr.is_null() { + return String::new(); + } + CStr::from_ptr(utf8_ptr).to_string_lossy().into_owned() + } + } + + fn str_to_nsstring_owned(s: &str) -> ObjcId { + unsafe { + let ns_string: ObjcId = msg_send![class!(NSString), alloc]; + if ns_string.is_null() { + return nil; + } + msg_send![ + ns_string, + initWithBytes: s.as_ptr() as *const c_void + length: s.len() as u64 + encoding: UTF8_ENCODING + ] + } + } + + fn ns_error_to_string(error: ObjcId) -> String { + if error.is_null() { + return "unknown Metal error".to_string(); + } + unsafe { + let desc: ObjcId = msg_send![error, localizedDescription]; + nsstring_to_string(desc) + } + } + + fn device_supports_family(device: ObjcId, family: u64) -> bool { + unsafe { msg_send![device, supportsFamily: family] } + } + + fn metal_compile_feature_macros(device: ObjcId) -> (bool, bool) { + let mut has_bfloat = device_supports_family(device, MTL_GPU_FAMILY_METAL3) + || device_supports_family(device, MTL_GPU_FAMILY_APPLE6); + if DISABLE_GGML_METAL_BF16 { + has_bfloat = false; + } + + let mut has_tensor = device_supports_family(device, MTL_GPU_FAMILY_METAL4); + if DISABLE_GGML_METAL_TENSOR { + has_tensor = false; + } + + if !FORCE_ENABLE_GGML_METAL_TENSOR && has_tensor { + let dev_name_obj: ObjcId = unsafe { msg_send![device, name] }; + let dev_name = nsstring_to_string(dev_name_obj); + let tensor_whitelisted = dev_name.contains("M5") + || dev_name.contains("M6") + || dev_name.contains("A19") + || dev_name.contains("A20"); + if !tensor_whitelisted { + has_tensor = false; + } + } + + (has_bfloat, has_tensor) + } + + fn read_text_with_fallback(paths: &[&str], fallback: &str) -> String { + for path in paths { + if let Ok(text) = std::fs::read_to_string(path) { + return text; + } + } + fallback.to_string() + } + + fn build_ggml_source() -> String { + // Development path: prefer source files from disk so shader-pruning tools can + // iterate without rebuilding the Rust crate every edit. + let mut src = read_text_with_fallback( + &[ + concat!( + env!("CARGO_MANIFEST_DIR"), + "/src/backend/metal/ggml/ggml-metal.metal" + ), + "libs/ggml/src/backend/metal/ggml/ggml-metal.metal", + ], + _GGML_METAL_SOURCE_RAW, + ); + let common_h = read_text_with_fallback( + &[ + concat!( + env!("CARGO_MANIFEST_DIR"), + "/src/backend/metal/ggml/ggml-common.h" + ), + "libs/ggml/src/backend/metal/ggml/ggml-common.h", + ], + _GGML_COMMON_H, + ); + let impl_h = read_text_with_fallback( + &[ + concat!( + env!("CARGO_MANIFEST_DIR"), + "/src/backend/metal/ggml/ggml-metal-impl.h" + ), + "libs/ggml/src/backend/metal/ggml/ggml-metal-impl.h", + ], + _GGML_METAL_IMPL_H, + ); + src = src.replace("__embed_ggml-common.h__", &common_h); + src = src.replace("#include \"ggml-common.h\"", &common_h); + src = src.replace("#include \"ggml-metal-impl.h\"", &impl_h); + src + } + + fn src0_type_from_ggml(t: u32) -> Option { + match t { + GGML_TYPE_F32 => Some(Src0Type::F32), + GGML_TYPE_F16 => Some(Src0Type::F16), + GGML_TYPE_BF16 => Some(Src0Type::BF16), + GGML_TYPE_Q4_0 => Some(Src0Type::Q4_0), + GGML_TYPE_Q4_1 => Some(Src0Type::Q4_1), + GGML_TYPE_Q5_0 => Some(Src0Type::Q5_0), + GGML_TYPE_Q5_1 => Some(Src0Type::Q5_1), + GGML_TYPE_Q8_0 => Some(Src0Type::Q8_0), + GGML_TYPE_Q2_K => Some(Src0Type::Q2_K), + GGML_TYPE_Q3_K => Some(Src0Type::Q3_K), + GGML_TYPE_Q4_K => Some(Src0Type::Q4_K), + GGML_TYPE_Q5_K => Some(Src0Type::Q5_K), + GGML_TYPE_Q6_K => Some(Src0Type::Q6_K), + _ => None, + } + } + + fn src0_type_name(t: Src0Type) -> &'static str { + match t { + Src0Type::F32 => "f32", + Src0Type::F16 => "f16", + Src0Type::BF16 => "bf16", + Src0Type::Q4_0 => "q4_0", + Src0Type::Q4_1 => "q4_1", + Src0Type::Q5_0 => "q5_0", + Src0Type::Q5_1 => "q5_1", + Src0Type::Q8_0 => "q8_0", + Src0Type::Q2_K => "q2_K", + Src0Type::Q3_K => "q3_K", + Src0Type::Q4_K => "q4_K", + Src0Type::Q5_K => "q5_K", + Src0Type::Q6_K => "q6_K", + } + } + + fn src0_layout_bytes_per_row(t: Src0Type, k: usize) -> Result<(usize, u64), String> { + match t { + Src0Type::F32 => Ok(( + k.checked_mul(4) + .ok_or_else(|| "overflow computing f32 row bytes".to_string())?, + 4, + )), + Src0Type::F16 => Ok(( + k.checked_mul(2) + .ok_or_else(|| "overflow computing f16 row bytes".to_string())?, + 2, + )), + Src0Type::BF16 => Ok(( + k.checked_mul(2) + .ok_or_else(|| "overflow computing bf16 row bytes".to_string())?, + 2, + )), + Src0Type::Q4_0 + | Src0Type::Q4_1 + | Src0Type::Q5_0 + | Src0Type::Q5_1 + | Src0Type::Q8_0 + | Src0Type::Q2_K + | Src0Type::Q3_K + | Src0Type::Q4_K + | Src0Type::Q5_K + | Src0Type::Q6_K => { + let ggml_type = match t { + Src0Type::Q4_0 => GGML_TYPE_Q4_0, + Src0Type::Q4_1 => GGML_TYPE_Q4_1, + Src0Type::Q5_0 => GGML_TYPE_Q5_0, + Src0Type::Q5_1 => GGML_TYPE_Q5_1, + Src0Type::Q8_0 => GGML_TYPE_Q8_0, + Src0Type::Q2_K => GGML_TYPE_Q2_K, + Src0Type::Q3_K => GGML_TYPE_Q3_K, + Src0Type::Q4_K => GGML_TYPE_Q4_K, + Src0Type::Q5_K => GGML_TYPE_Q5_K, + Src0Type::Q6_K => GGML_TYPE_Q6_K, + _ => unreachable!(), + }; + let blck = block_elements(ggml_type); + if k % blck != 0 { + return Err(format!( + "quantized kernel requires K multiple of {}, got {}", + blck, k + )); + } + let bs = block_size(ggml_type); + let row = (k / blck) + .checked_mul(bs) + .ok_or_else(|| "overflow computing quantized row bytes".to_string())?; + Ok((row, bs as u64)) + } + } + } + + fn shape4_from_row_major(shape: &[usize], elem_bytes: u64) -> Result { + if shape.is_empty() { + return Err("shape must be non-empty".to_string()); + } + if shape.len() > 4 { + return Err(format!( + "shape rank > 4 is unsupported in metal elementwise path: {:?}", + shape + )); + } + let mut ne = [1i32; 4]; + for (i, &d) in shape.iter().rev().enumerate() { + ne[i] = i32::try_from(d).map_err(|_| format!("shape dim too large: {}", d))?; + } + let mut nb = [0u64; 4]; + nb[0] = elem_bytes; + for i in 1..4 { + nb[i] = nb[i - 1] + .checked_mul(ne[i - 1] as u64) + .ok_or_else(|| "overflow computing strides".to_string())?; + } + let numel = shape.iter().try_fold(1usize, |acc, &d| { + acc.checked_mul(d) + .ok_or_else(|| "overflow computing tensor numel".to_string()) + })?; + Ok(Shape4 { ne, nb, numel }) + } + + fn nrows(s: &Shape4) -> usize { + (s.ne[1] as usize) + .saturating_mul(s.ne[2] as usize) + .saturating_mul(s.ne[3] as usize) + } + + fn f32_slice_as_bytes(s: &[f32]) -> &[u8] { + unsafe { + std::slice::from_raw_parts( + s.as_ptr() as *const u8, + s.len() * std::mem::size_of::(), + ) + } + } + + fn flash_attn_supported_head_dim(d: usize) -> bool { + matches!( + d, + 32 | 40 | 48 | 64 | 72 | 80 | 96 | 112 | 128 | 192 | 256 | 576 + ) + } + + fn flash_attn_use_vec(n_q: usize, d: usize) -> bool { + n_q < 20 && d % 32 == 0 + } + + fn flash_attn_kv_elem_bytes(src_type: Src0Type) -> Result { + match src_type { + Src0Type::F32 => Ok(std::mem::size_of::()), + Src0Type::F16 => Ok(std::mem::size_of::()), + _ => Err(format!( + "unsupported flash-attn kv type for Metal path: {:?}", + src_type + )), + } + } + + #[derive(Clone, Copy, Default)] + struct FlashAttnExtParams { + has_mask: bool, + has_sinks: bool, + max_bias: f32, + logit_softcap: f32, + } + + fn pad_to(v: usize, align: usize) -> usize { + if align == 0 { + return v; + } + let rem = v % align; + if rem == 0 { + v + } else { + v + (align - rem) + } + } + + fn flash_attn_smem_bytes(dk: usize, dv: usize, _nsg: i32) -> usize { + let nqptg = OP_FLASH_ATTN_EXT_NQPSG as usize; + let ncpsg = OP_FLASH_ATTN_EXT_NCPSG as usize; + + // Matches ggml-metal FATTN_SMEM() for non-quantized f32 K/V. + let words = nqptg.saturating_mul(dk + 2 * pad_to(dv, 64) + 2 * (2 * ncpsg)); + pad_to(words.saturating_mul(std::mem::size_of::() / 2), 16) + } + + fn flash_attn_vec_smem_bytes(dk: usize, dv: usize, nsg: i32) -> usize { + let ncpsg = OP_FLASH_ATTN_EXT_VEC_NCPSG as usize; + let words = + (pad_to(dk, 128) + 4 * ncpsg + 2 * pad_to(dv, 128)).saturating_mul(nsg.max(1) as usize); + pad_to(words.saturating_mul(std::mem::size_of::() / 2), 16) + } + + fn flash_attn_ext_extra_pad_bytes( + n_q: usize, + n_kv: usize, + n_head: usize, + d: usize, + kv_elem_bytes: usize, + has_mask: bool, + use_vec: bool, + ) -> Result { + // Match ggml-metal: reserve non-vec sized padding space, but gate by the active kernel kvpad. + let reserve_ncpsg = OP_FLASH_ATTN_EXT_NCPSG as usize; + let active_ncpsg = if use_vec { + OP_FLASH_ATTN_EXT_VEC_NCPSG as usize + } else { + OP_FLASH_ATTN_EXT_NCPSG as usize + }; + let has_kvpad = n_kv % active_ncpsg != 0; + if !has_kvpad { + return Ok(0); + } + + let n_state = n_head + .checked_mul(d) + .ok_or_else(|| "overflow computing flash n_state".to_string())?; + let nb11 = n_state + .checked_mul(kv_elem_bytes) + .ok_or_else(|| "overflow computing flash nb11".to_string())?; + let nb21 = nb11; + + let k_term = nb11 + .checked_mul(n_head) + .ok_or_else(|| "overflow computing flash extra pad K bytes".to_string())?; + let v_term = nb21 + .checked_mul(n_head) + .ok_or_else(|| "overflow computing flash extra pad V bytes".to_string())?; + let mask_term = if has_mask { + std::mem::size_of::() + .checked_mul(n_q) + .ok_or_else(|| "overflow computing flash extra pad mask bytes".to_string())? + } else { + 0 + }; + + reserve_ncpsg + .checked_mul( + k_term + .checked_add(v_term) + .and_then(|v| v.checked_add(mask_term)) + .ok_or_else(|| "overflow computing flash extra pad size".to_string())?, + ) + .ok_or_else(|| "overflow computing flash extra pad size".to_string()) + } + + fn flash_attn_ext_extra_blk_bytes( + n_q: usize, + n_kv: usize, + has_mask: bool, + use_vec: bool, + ) -> Result { + if !has_mask { + return Ok(0); + } + + let nqptg = if use_vec { + OP_FLASH_ATTN_EXT_VEC_NQPSG as usize + } else { + OP_FLASH_ATTN_EXT_NQPSG as usize + }; + let ncpsg = if use_vec { + OP_FLASH_ATTN_EXT_VEC_NCPSG as usize + } else { + OP_FLASH_ATTN_EXT_NCPSG as usize + }; + + let ne1 = (n_q + nqptg - 1) / nqptg; + let ne0 = (n_kv + ncpsg - 1) / ncpsg; + let raw = ne0 + .checked_mul(ne1) + .ok_or_else(|| "overflow computing flash extra blk size".to_string())?; + + Ok(pad_to(raw, 32)) + } + + fn flash_attn_ext_extra_tmp_bytes( + n_q: usize, + n_head: usize, + d: usize, + nwg: usize, + ) -> Result { + let ne01_max = n_q.min(32); + std::mem::size_of::() + .checked_mul(ne01_max) + .and_then(|v| v.checked_mul(n_head)) + .and_then(|v| v.checked_mul(nwg)) + .and_then(|v| v.checked_mul(d + 2)) + .ok_or_else(|| "overflow computing flash extra tmp size".to_string()) + } + + fn can_use_mul_mv_ext(src0: Src0Type, ne00: i32, ne11: i32) -> bool { + if ne00 % 128 != 0 { + return false; + } + if !(2..=8).contains(&ne11) { + return false; + } + matches!( + src0, + Src0Type::F32 + | Src0Type::F16 + | Src0Type::BF16 + | Src0Type::Q4_0 + | Src0Type::Q4_1 + | Src0Type::Q5_0 + | Src0Type::Q5_1 + | Src0Type::Q8_0 + | Src0Type::Q2_K + | Src0Type::Q3_K + | Src0Type::Q4_K + | Src0Type::Q5_K + | Src0Type::Q6_K + ) + } + + struct PipelineState { + obj: StrongId, + smem: usize, + nsg: i32, + nr0: i32, + nr1: i32, + } + + struct DecoderKvLayer { + k: StrongId, + v: StrongId, + n_state: usize, + cap_rows: usize, + len_rows: usize, + } + + struct CrossKvLayer { + k: StrongId, + v: StrongId, + n_state: usize, + n_rows: usize, + src_k_ptr: usize, + src_v_ptr: usize, + src_k_len: usize, + src_v_len: usize, + } + + struct ScratchBuffer { + buf: StrongId, + cap_bytes: usize, + is_private: bool, + } + + struct MetalContext { + device: StrongId, + command_queue: StrongId, + library: StrongId, + pipeline_cache: HashMap, + cached_weight_buffers: HashMap, + scratch_buffers: HashMap, + matmul_out_buffers: HashMap, + decoder_kv_layers: HashMap, + cross_kv_layers: HashMap, + batch_depth: usize, + batch_command_buffer: Option, + batch_encoder: Option, + last_command_buffer: Option, + } + + impl MetalContext { + fn create_device() -> Option { + unsafe { + let dev = MTLCreateSystemDefaultDevice(); + if let Some(dev) = StrongId::from_owned(dev) { + return Some(dev); + } + + let all = MTLCopyAllDevices(); + if all.is_null() { + return None; + } + + let count: u64 = msg_send![all, count]; + let first: ObjcId = if count > 0 { + msg_send![all, objectAtIndex: 0u64] + } else { + nil + }; + let _: () = msg_send![all, release]; + + StrongId::from_unowned(first) + } + } + + fn new() -> Result { + let _pool = AutoreleasePool::new(); + + let device = Self::create_device().ok_or_else(|| { + "unable to create Metal device (MTLCreateSystemDefaultDevice and MTLCopyAllDevices returned nil)" + .to_string() + })?; + + let command_queue_obj: ObjcId = unsafe { msg_send![device.as_id(), newCommandQueue] }; + let command_queue = unsafe { StrongId::from_owned(command_queue_obj) } + .ok_or_else(|| "newCommandQueue returned nil".to_string())?; + + let library = match Self::load_library_from_metallib(device.as_id()) { + Ok(Some(lib)) => lib, + Ok(None) => { + let source = build_ggml_source(); + Self::compile_library(device.as_id(), &source)? + } + Err(err) => { + eprintln!( + "[ggml][metal] precompiled metallib load failed, compiling source: {}", + err + ); + let source = build_ggml_source(); + Self::compile_library(device.as_id(), &source)? + } + }; + + eprintln!("[ggml][metal] backend initialized (shared kernels)"); + + Ok(Self { + device, + command_queue, + library, + pipeline_cache: HashMap::new(), + cached_weight_buffers: HashMap::new(), + scratch_buffers: HashMap::new(), + matmul_out_buffers: HashMap::new(), + decoder_kv_layers: HashMap::new(), + cross_kv_layers: HashMap::new(), + batch_depth: 0, + batch_command_buffer: None, + batch_encoder: None, + last_command_buffer: None, + }) + } + + fn load_library_from_metallib(device: ObjcId) -> Result, String> { + if _GGML_METALLIB_BYTES.is_empty() { + return Ok(None); + } + + let _pool = AutoreleasePool::new(); + + let data_obj: ObjcId = unsafe { + msg_send![ + class!(NSData), + dataWithBytes: _GGML_METALLIB_BYTES.as_ptr() as *const c_void + length: _GGML_METALLIB_BYTES.len() as u64 + ] + }; + if data_obj.is_null() { + return Err("NSData::dataWithBytes returned nil".to_string()); + } + + let mut error: ObjcId = nil; + let library_obj: ObjcId = + unsafe { msg_send![device, newLibraryWithData: data_obj error: &mut error] }; + if library_obj.is_null() { + return Err(format!( + "newLibraryWithData failed: {}", + ns_error_to_string(error) + )); + } + + let library = unsafe { StrongId::from_owned(library_obj) } + .ok_or_else(|| "newLibraryWithData returned nil".to_string())?; + Ok(Some(library)) + } + + fn compile_library(device: ObjcId, source: &str) -> Result { + let _pool = AutoreleasePool::new(); + + let options_obj: ObjcId = unsafe { msg_send![class!(MTLCompileOptions), new] }; + let options = unsafe { StrongId::from_owned(options_obj) } + .ok_or_else(|| "MTLCompileOptions::new returned nil".to_string())?; + unsafe { + let _: () = msg_send![options.as_id(), setFastMathEnabled: YES]; + } + + let (has_bfloat, has_tensor) = metal_compile_feature_macros(device); + if has_bfloat || has_tensor { + let prep_obj: ObjcId = + unsafe { msg_send![class!(NSMutableDictionary), dictionary] }; + if !prep_obj.is_null() { + if has_bfloat { + let key_obj = str_to_nsstring_owned("GGML_METAL_HAS_BF16"); + let val_obj = str_to_nsstring_owned("1"); + let key = unsafe { StrongId::from_owned(key_obj) } + .ok_or_else(|| "failed to build metal macro key".to_string())?; + let val = unsafe { StrongId::from_owned(val_obj) } + .ok_or_else(|| "failed to build metal macro value".to_string())?; + unsafe { + let _: () = + msg_send![prep_obj, setObject: val.as_id() forKey: key.as_id()]; + } + } + if has_tensor { + let key_obj = str_to_nsstring_owned("GGML_METAL_HAS_TENSOR"); + let val_obj = str_to_nsstring_owned("1"); + let key = unsafe { StrongId::from_owned(key_obj) } + .ok_or_else(|| "failed to build metal macro key".to_string())?; + let val = unsafe { StrongId::from_owned(val_obj) } + .ok_or_else(|| "failed to build metal macro value".to_string())?; + unsafe { + let _: () = + msg_send![prep_obj, setObject: val.as_id() forKey: key.as_id()]; + } + } + unsafe { + let _: () = msg_send![options.as_id(), setPreprocessorMacros: prep_obj]; + } + } + } + + let source_obj = str_to_nsstring_owned(source); + let source_obj = unsafe { StrongId::from_owned(source_obj) } + .ok_or_else(|| "failed to create NSString for Metal source".to_string())?; + + let mut error: ObjcId = nil; + let library_obj: ObjcId = unsafe { + msg_send![ + device, + newLibraryWithSource: source_obj.as_id() + options: options.as_id() + error: &mut error + ] + }; + + unsafe { StrongId::from_owned(library_obj) }.ok_or_else(|| { + format!("newLibraryWithSource failed: {}", ns_error_to_string(error)) + }) + } + + fn new_buffer_with_bytes(&self, bytes: &[u8]) -> Result { + let obj: ObjcId = unsafe { + msg_send![ + self.device.as_id(), + newBufferWithBytes: bytes.as_ptr() as *const c_void + length: bytes.len() as u64 + options: MTL_RESOURCE_STORAGE_MODE_SHARED + ] + }; + unsafe { StrongId::from_owned(obj) } + .ok_or_else(|| format!("newBufferWithBytes failed for {} bytes", bytes.len())) + } + + fn new_buffer_with_length(&self, byte_len: usize) -> Result { + let obj: ObjcId = unsafe { + msg_send![ + self.device.as_id(), + newBufferWithLength: byte_len as u64 + options: MTL_RESOURCE_STORAGE_MODE_SHARED + ] + }; + unsafe { StrongId::from_owned(obj) } + .ok_or_else(|| format!("newBufferWithLength failed for {} bytes", byte_len)) + } + + fn new_buffer_with_length_private(&self, byte_len: usize) -> Result { + let obj: ObjcId = unsafe { + msg_send![ + self.device.as_id(), + newBufferWithLength: byte_len as u64 + options: MTL_RESOURCE_OPTIONS_STORAGE_MODE_PRIVATE + ] + }; + unsafe { StrongId::from_owned(obj) }.ok_or_else(|| { + format!("newBufferWithLength(private) failed for {} bytes", byte_len) + }) + } + + fn get_or_create_scratch_buffer( + &mut self, + kind: u8, + need_bytes: usize, + ) -> Result { + let need_bytes = need_bytes.max(1); + let is_private = matches!( + kind, + SCRATCH_FLASH_PAD + | SCRATCH_FLASH_BLK + | SCRATCH_FLASH_TMP + | SCRATCH_FLASH_OUT + | SCRATCH_ENC_NORM0 + | SCRATCH_ENC_NORM1 + | SCRATCH_DEC_NORM0 + | SCRATCH_DEC_NORM1 + ); + if let Some(entry) = self.scratch_buffers.get(&kind) { + if entry.cap_bytes >= need_bytes && entry.is_private == is_private { + return Ok(entry.buf.as_id()); + } + } + + let buf = if is_private { + self.new_buffer_with_length_private(need_bytes)? + } else { + self.new_buffer_with_length(need_bytes)? + }; + self.scratch_buffers.insert( + kind, + ScratchBuffer { + buf, + cap_bytes: need_bytes, + is_private, + }, + ); + + Ok(self.scratch_buffers.get(&kind).unwrap().buf.as_id()) + } + + fn get_or_create_matmul_out_buffer( + &mut self, + tag: u8, + need_bytes: usize, + ) -> Result { + let need_bytes = need_bytes.max(1); + if let Some(entry) = self.matmul_out_buffers.get(&tag) { + if entry.cap_bytes >= need_bytes { + return Ok(entry.buf.as_id()); + } + } + + // Mirror ggml-metal backend allocation for compute intermediates: + // matrix outputs are GPU-only private buffers. + let buf = self.new_buffer_with_length_private(need_bytes)?; + self.matmul_out_buffers.insert( + tag, + ScratchBuffer { + buf, + cap_bytes: need_bytes, + is_private: true, + }, + ); + + Ok(self.matmul_out_buffers.get(&tag).unwrap().buf.as_id()) + } + + fn buffer_length_bytes(&self, buffer: ObjcId) -> Result { + let len_u64: u64 = unsafe { msg_send![buffer, length] }; + usize::try_from(len_u64).map_err(|_| format!("buffer length too large: {}", len_u64)) + } + + fn zero_buffer_range( + &self, + buffer: ObjcId, + offset_bytes: usize, + len_bytes: usize, + ) -> Result<(), String> { + if len_bytes == 0 { + return Ok(()); + } + let buf_len = self.buffer_length_bytes(buffer)?; + let end = offset_bytes + .checked_add(len_bytes) + .ok_or_else(|| "overflow computing zero range end".to_string())?; + if end > buf_len { + return Err(format!( + "zero range out of bounds: offset={}, len={}, buffer_len={}", + offset_bytes, len_bytes, buf_len + )); + } + let ptr: *mut u8 = unsafe { msg_send![buffer, contents] }; + if ptr.is_null() { + return Err("buffer contents returned null".to_string()); + } + unsafe { + std::ptr::write_bytes(ptr.add(offset_bytes), 0u8, len_bytes); + } + Ok(()) + } + + fn prepare_decoder_self_mask_f16( + &mut self, + n_valid: usize, + n_total: usize, + ) -> Result { + if n_total == 0 || n_valid > n_total { + return Err(format!( + "invalid decoder self mask sizes: n_valid={}, n_total={}", + n_valid, n_total + )); + } + + let mask_bytes = n_total + .checked_mul(std::mem::size_of::()) + .ok_or_else(|| "overflow computing decoder self mask bytes".to_string())?; + let mask_id = self.get_or_create_scratch_buffer(SCRATCH_FLASH_MASK, mask_bytes)?; + let mask_cap = self.buffer_length_bytes(mask_id)?; + if mask_bytes > mask_cap { + return Err(format!( + "decoder self mask write exceeds buffer: need={}, cap={}", + mask_bytes, mask_cap + )); + } + let ptr: *mut u16 = unsafe { msg_send![mask_id, contents] }; + if ptr.is_null() { + return Err("decoder self mask buffer contents returned null".to_string()); + } + + let neg_inf_h = f32_to_f16(f32::NEG_INFINITY); + unsafe { + let dst = std::slice::from_raw_parts_mut(ptr, n_total); + for v in dst.iter_mut().take(n_valid) { + *v = 0; + } + for v in dst.iter_mut().skip(n_valid) { + *v = neg_inf_h; + } + } + + Ok(mask_id) + } + + fn copy_f32_buffer_contents( + &self, + buffer: ObjcId, + elems: usize, + ) -> Result, String> { + let byte_len = elems + .checked_mul(std::mem::size_of::()) + .ok_or_else(|| "overflow computing f32 copy byte length".to_string())?; + let cap = self.buffer_length_bytes(buffer)?; + if byte_len > cap { + return Err(format!( + "f32 read exceeds buffer: need={} bytes, cap={} bytes", + byte_len, cap + )); + } + let out_ptr: *const c_void = unsafe { msg_send![buffer, contents] }; + if out_ptr.is_null() { + return Err("output buffer contents returned null".to_string()); + } + + let mut out = vec![0.0f32; elems]; + unsafe { + std::ptr::copy_nonoverlapping(out_ptr as *const f32, out.as_mut_ptr(), elems); + } + Ok(out) + } + + fn copy_buffer_to_shared_staging( + &self, + src_buffer: ObjcId, + len_bytes: usize, + ) -> Result { + let len_bytes = len_bytes.max(1); + let src_len = self.buffer_length_bytes(src_buffer)?; + if len_bytes > src_len { + return Err(format!( + "staging copy exceeds source buffer: need={} bytes, src_len={} bytes", + len_bytes, src_len + )); + } + let dst_buffer = self.new_buffer_with_length(len_bytes)?; + + let command_buffer_obj: ObjcId = + unsafe { msg_send![self.command_queue.as_id(), commandBuffer] }; + let command_buffer = unsafe { StrongId::from_unowned(command_buffer_obj) } + .ok_or_else(|| "commandBuffer returned nil".to_string())?; + + let blit_encoder_obj: ObjcId = + unsafe { msg_send![command_buffer.as_id(), blitCommandEncoder] }; + let blit_encoder = unsafe { StrongId::from_unowned(blit_encoder_obj) } + .ok_or_else(|| "blitCommandEncoder returned nil".to_string())?; + + unsafe { + let _: () = msg_send![ + blit_encoder.as_id(), + copyFromBuffer: src_buffer + sourceOffset: 0u64 + toBuffer: dst_buffer.as_id() + destinationOffset: 0u64 + size: len_bytes as u64 + ]; + let _: () = msg_send![blit_encoder.as_id(), endEncoding]; + let _: () = msg_send![command_buffer.as_id(), commit]; + let _: () = msg_send![command_buffer.as_id(), waitUntilCompleted]; + } + + let status: u64 = unsafe { msg_send![command_buffer.as_id(), status] }; + if status == 5 { + let error: ObjcId = unsafe { msg_send![command_buffer.as_id(), error] }; + return Err(format!( + "Metal command buffer error (buffer staging copy): {}", + ns_error_to_string(error) + )); + } + + Ok(dst_buffer) + } + + fn copy_f32_buffer_contents_readable( + &self, + buffer: ObjcId, + elems: usize, + ) -> Result, String> { + let byte_len = elems + .checked_mul(std::mem::size_of::()) + .ok_or_else(|| "overflow computing readback byte length".to_string())?; + let storage_mode: u64 = unsafe { msg_send![buffer, storageMode] }; + if storage_mode == MTL_STORAGE_MODE_PRIVATE { + let staging = self.copy_buffer_to_shared_staging(buffer, byte_len)?; + self.copy_f32_buffer_contents(staging.as_id(), elems) + } else { + self.copy_f32_buffer_contents(buffer, elems) + } + } + + fn read_f32_buffer(&self, buffer: ObjcId, elems: usize) -> Result, String> { + self.wait_queue_idle()?; + self.copy_f32_buffer_contents_readable(buffer, elems) + } + + fn read_f32_buffers3( + &self, + b0: ObjcId, + b1: ObjcId, + b2: ObjcId, + elems: usize, + ) -> Result<(Vec, Vec, Vec), String> { + self.wait_queue_idle()?; + let o0 = self.copy_f32_buffer_contents_readable(b0, elems)?; + let o1 = self.copy_f32_buffer_contents_readable(b1, elems)?; + let o2 = self.copy_f32_buffer_contents_readable(b2, elems)?; + Ok((o0, o1, o2)) + } + + fn get_or_create_cached_f32_buffer( + &mut self, + data: &[f32], + tag: u8, + ) -> Result { + let bytes = f32_slice_as_bytes(data); + let key = BufferKey { + ptr: data.as_ptr() as usize, + len: bytes.len(), + tag, + }; + self.get_or_create_weight_buffer(key, bytes) + } + + fn get_or_create_weight_buffer( + &mut self, + key: BufferKey, + bytes: &[u8], + ) -> Result { + if !self.cached_weight_buffers.contains_key(&key) { + let buf = self.new_buffer_with_bytes(bytes)?; + self.cached_weight_buffers.insert(key, buf); + } + Ok(self.cached_weight_buffers.get(&key).unwrap().as_id()) + } + + fn clear_decoder_kv_cache(&mut self) { + self.decoder_kv_layers.clear(); + self.cross_kv_layers.clear(); + } + + fn ensure_decoder_kv_layer( + &mut self, + layer: usize, + n_state: usize, + need_rows: usize, + ) -> Result<(ObjcId, ObjcId), String> { + let need_rows = need_rows.max(1); + + if let Some(entry) = self.decoder_kv_layers.get(&layer) { + if entry.n_state == n_state && entry.cap_rows >= need_rows { + return Ok((entry.k.as_id(), entry.v.as_id())); + } + } + + let row_bytes = n_state + .checked_mul(std::mem::size_of::()) + .ok_or_else(|| "overflow computing decoder kv row bytes".to_string())?; + + let old = self.decoder_kv_layers.remove(&layer); + let cap_rows = if let Some(ref old) = old { + if old.n_state == n_state { + old.cap_rows.saturating_mul(2).max(need_rows).max(32) + } else { + need_rows.max(32) + } + } else { + need_rows.max(32) + }; + let total_bytes = cap_rows + .checked_mul(row_bytes) + .ok_or_else(|| "overflow computing decoder kv bytes".to_string())?; + + let new_k = self.new_buffer_with_length(total_bytes)?; + let new_v = self.new_buffer_with_length(total_bytes)?; + + let mut len_rows = 0usize; + if let Some(old) = old { + if old.n_state == n_state && old.len_rows > 0 { + let copy_rows = old.len_rows.min(cap_rows); + let copy_bytes = copy_rows + .checked_mul(row_bytes) + .ok_or_else(|| "overflow computing decoder kv copy bytes".to_string())?; + let old_k_ptr: *const u8 = unsafe { msg_send![old.k.as_id(), contents] }; + let old_v_ptr: *const u8 = unsafe { msg_send![old.v.as_id(), contents] }; + let new_k_ptr: *mut u8 = unsafe { msg_send![new_k.as_id(), contents] }; + let new_v_ptr: *mut u8 = unsafe { msg_send![new_v.as_id(), contents] }; + if old_k_ptr.is_null() + || old_v_ptr.is_null() + || new_k_ptr.is_null() + || new_v_ptr.is_null() + { + return Err("decoder kv buffer contents returned null".to_string()); + } + unsafe { + std::ptr::copy_nonoverlapping(old_k_ptr, new_k_ptr, copy_bytes); + std::ptr::copy_nonoverlapping(old_v_ptr, new_v_ptr, copy_bytes); + } + len_rows = copy_rows; + } + } + + self.decoder_kv_layers.insert( + layer, + DecoderKvLayer { + k: new_k, + v: new_v, + n_state, + cap_rows, + len_rows, + }, + ); + let entry = self + .decoder_kv_layers + .get(&layer) + .ok_or_else(|| "decoder kv layer insertion failed".to_string())?; + Ok((entry.k.as_id(), entry.v.as_id())) + } + + fn ensure_cross_kv_layer( + &mut self, + layer: usize, + n_state: usize, + n_rows: usize, + k_cross: &[f32], + v_cross: &[f32], + ) -> Result<(ObjcId, ObjcId), String> { + if n_state == 0 || n_rows == 0 { + return Err(format!( + "invalid cross kv dimensions: n_state={}, n_rows={}", + n_state, n_rows + )); + } + if k_cross.len() != v_cross.len() { + return Err(format!( + "cross kv k/v len mismatch: k={}, v={}", + k_cross.len(), + v_cross.len(), + )); + } + if k_cross.len() % n_state != 0 { + return Err(format!( + "cross kv len not divisible by n_state: len={}, n_state={}", + k_cross.len(), + n_state + )); + } + + let src_rows = k_cross.len() / n_state; + if src_rows == 0 { + return Err("cross kv has zero rows".to_string()); + } + if src_rows > n_rows { + return Err(format!( + "cross kv source rows exceed requested rows: src_rows={}, n_rows={}", + src_rows, n_rows + )); + } + + let src_k_ptr = k_cross.as_ptr() as usize; + let src_v_ptr = v_cross.as_ptr() as usize; + let src_k_len = k_cross.len(); + let src_v_len = v_cross.len(); + + if let Some(entry) = self.cross_kv_layers.get(&layer) { + if entry.n_state == n_state + && entry.n_rows == n_rows + && entry.src_k_ptr == src_k_ptr + && entry.src_v_ptr == src_v_ptr + && entry.src_k_len == src_k_len + && entry.src_v_len == src_v_len + { + return Ok((entry.k.as_id(), entry.v.as_id())); + } + } + + let row_bytes = n_state + .checked_mul(std::mem::size_of::()) + .ok_or_else(|| "overflow computing cross kv row bytes".to_string())?; + let total_bytes = n_rows + .checked_mul(row_bytes) + .ok_or_else(|| "overflow computing cross kv total bytes".to_string())?; + let copy_bytes = src_rows + .checked_mul(row_bytes) + .ok_or_else(|| "overflow computing cross kv copy bytes".to_string())?; + + let (k_buf, v_buf) = if src_rows == n_rows { + ( + self.new_buffer_with_bytes(f32_slice_as_bytes(k_cross))?, + self.new_buffer_with_bytes(f32_slice_as_bytes(v_cross))?, + ) + } else { + let k_buf = self.new_buffer_with_length(total_bytes)?; + let v_buf = self.new_buffer_with_length(total_bytes)?; + + let dst_k: *mut u8 = unsafe { msg_send![k_buf.as_id(), contents] }; + let dst_v: *mut u8 = unsafe { msg_send![v_buf.as_id(), contents] }; + if dst_k.is_null() || dst_v.is_null() { + return Err("cross kv buffer contents returned null".to_string()); + } + unsafe { + std::ptr::write_bytes(dst_k, 0u8, total_bytes); + std::ptr::write_bytes(dst_v, 0u8, total_bytes); + std::ptr::copy_nonoverlapping(k_cross.as_ptr() as *const u8, dst_k, copy_bytes); + std::ptr::copy_nonoverlapping(v_cross.as_ptr() as *const u8, dst_v, copy_bytes); + } + + (k_buf, v_buf) + }; + self.cross_kv_layers.insert( + layer, + CrossKvLayer { + k: k_buf, + v: v_buf, + n_state, + n_rows, + src_k_ptr, + src_v_ptr, + src_k_len, + src_v_len, + }, + ); + + let entry = self + .cross_kv_layers + .get(&layer) + .ok_or_else(|| "cross kv layer insertion failed".to_string())?; + Ok((entry.k.as_id(), entry.v.as_id())) + } + + fn compile_pipeline( + &self, + base_name: &str, + constants: &[FunctionConstant], + ) -> Result { + let _pool = AutoreleasePool::new(); + + let base_obj = str_to_nsstring_owned(base_name); + let base_obj = unsafe { StrongId::from_owned(base_obj) } + .ok_or_else(|| format!("failed to create NSString for function '{}'", base_name))?; + + let mut error: ObjcId = nil; + let func_obj: ObjcId = if constants.is_empty() { + unsafe { msg_send![self.library.as_id(), newFunctionWithName: base_obj.as_id()] } + } else { + let cv_obj: ObjcId = unsafe { msg_send![class!(MTLFunctionConstantValues), new] }; + let cv = unsafe { StrongId::from_owned(cv_obj) } + .ok_or_else(|| "MTLFunctionConstantValues::new returned nil".to_string())?; + + for c in constants { + unsafe { + match c.value { + FunctionConstantValue::Int32(v) => { + let _: () = msg_send![ + cv.as_id(), + setConstantValue: &v as *const i32 as *const c_void + type: MTL_DATA_TYPE_INT + atIndex: c.idx as u64 + ]; + } + FunctionConstantValue::Int16(v) => { + let _: () = msg_send![ + cv.as_id(), + setConstantValue: &v as *const i16 as *const c_void + type: MTL_DATA_TYPE_SHORT + atIndex: c.idx as u64 + ]; + } + FunctionConstantValue::Bool(v) => { + let b: u8 = if v { 1 } else { 0 }; + let _: () = msg_send![ + cv.as_id(), + setConstantValue: &b as *const u8 as *const c_void + type: MTL_DATA_TYPE_BOOL + atIndex: c.idx as u64 + ]; + } + } + } + } + + unsafe { + msg_send![ + self.library.as_id(), + newFunctionWithName: base_obj.as_id() + constantValues: cv.as_id() + error: &mut error + ] + } + }; + + let func = unsafe { StrongId::from_owned(func_obj) }.ok_or_else(|| { + format!( + "newFunctionWithName('{}') failed: {}", + base_name, + ns_error_to_string(error) + ) + })?; + + let mut error: ObjcId = nil; + let pipeline_obj: ObjcId = unsafe { + msg_send![ + self.device.as_id(), + newComputePipelineStateWithFunction: func.as_id() + error: &mut error + ] + }; + + unsafe { StrongId::from_owned(pipeline_obj) }.ok_or_else(|| { + format!( + "newComputePipelineStateWithFunction('{}') failed: {}", + base_name, + ns_error_to_string(error) + ) + }) + } + + fn get_or_compile_cached_pipeline( + &mut self, + cache_name: String, + base_name: &str, + constants: &[FunctionConstant], + smem: usize, + nr0: i32, + nr1: i32, + nsg: i32, + ) -> Result<(ObjcId, usize, i32, i32, i32), String> { + if !self.pipeline_cache.contains_key(&cache_name) { + if LOG_METAL_PIPELINES { + eprintln!("[ggml][metal] compile_pipeline base={}", base_name); + } + let compiled = self.compile_pipeline(base_name, constants)?; + self.pipeline_cache.insert( + cache_name.clone(), + PipelineState { + obj: compiled, + smem, + nsg, + nr0, + nr1, + }, + ); + } + + let p = self.pipeline_cache.get(&cache_name).unwrap(); + Ok((p.obj.as_id(), p.smem, p.nr0, p.nr1, p.nsg)) + } + + fn pipeline_max_threads(pipeline: ObjcId) -> u64 { + unsafe { msg_send![pipeline, maxTotalThreadsPerThreadgroup] } + } + + fn create_compute_encoder(&self, command_buffer: ObjcId) -> Result { + let encoder_obj: ObjcId = unsafe { msg_send![command_buffer, computeCommandEncoder] }; + + unsafe { StrongId::from_unowned(encoder_obj) } + .ok_or_else(|| "computeCommandEncoder returned nil".to_string()) + } + + fn begin_batch(&mut self) -> Result<(), String> { + if self.batch_depth == 0 { + let command_buffer_obj: ObjcId = + unsafe { msg_send![self.command_queue.as_id(), commandBuffer] }; + let command_buffer = unsafe { StrongId::from_unowned(command_buffer_obj) } + .ok_or_else(|| "commandBuffer returned nil".to_string())?; + + let encoder = self.create_compute_encoder(command_buffer.as_id())?; + + self.batch_command_buffer = Some(command_buffer); + self.batch_encoder = Some(encoder); + } + self.batch_depth += 1; + Ok(()) + } + + fn end_batch(&mut self) -> Result<(), String> { + if self.batch_depth == 0 { + return Err("end_batch called with no active batch".to_string()); + } + + self.batch_depth -= 1; + if self.batch_depth == 0 { + let command_buffer = self + .batch_command_buffer + .take() + .ok_or_else(|| "batch command buffer missing".to_string())?; + let encoder = self + .batch_encoder + .take() + .ok_or_else(|| "batch encoder missing".to_string())?; + + unsafe { + let _: () = msg_send![encoder.as_id(), endEncoding]; + let _: () = msg_send![command_buffer.as_id(), commit]; + } + + self.last_command_buffer = Some(command_buffer); + } + + Ok(()) + } + + fn with_batch(&mut self, f: F) -> Result + where + F: FnOnce(&mut Self) -> Result, + { + self.begin_batch()?; + let out = f(self); + let end_res = self.end_batch(); + match (out, end_res) { + (Ok(v), Ok(())) => Ok(v), + (Err(e), Ok(())) => Err(e), + (Ok(_), Err(e)) => Err(e), + (Err(e), Err(_)) => Err(e), + } + } + + fn begin_command_encoder( + &self, + ) -> Result<(ObjcId, ObjcId, Option<(StrongId, StrongId)>), String> { + if self.batch_depth > 0 { + let command_buffer = self + .batch_command_buffer + .as_ref() + .ok_or_else(|| "batch command buffer missing".to_string())?; + let encoder = self + .batch_encoder + .as_ref() + .ok_or_else(|| "batch encoder missing".to_string())?; + return Ok((command_buffer.as_id(), encoder.as_id(), None)); + } + + let command_buffer_obj: ObjcId = + unsafe { msg_send![self.command_queue.as_id(), commandBuffer] }; + let command_buffer = unsafe { StrongId::from_unowned(command_buffer_obj) } + .ok_or_else(|| "commandBuffer returned nil".to_string())?; + + let encoder = self.create_compute_encoder(command_buffer.as_id())?; + + Ok(( + command_buffer.as_id(), + encoder.as_id(), + Some((command_buffer, encoder)), + )) + } + + fn wait_queue_idle(&self) -> Result<(), String> { + if self.batch_depth > 0 { + return Err("wait_queue_idle called while command batch is active".to_string()); + } + + if let Some(command_buffer) = self.last_command_buffer.as_ref() { + let command_buffer_id = command_buffer.as_id(); + unsafe { + let _: () = msg_send![command_buffer_id, waitUntilCompleted]; + } + let status: u64 = unsafe { msg_send![command_buffer_id, status] }; + if status == 5 { + let error: ObjcId = unsafe { msg_send![command_buffer_id, error] }; + return Err(format!( + "Metal command buffer error (queue idle wait): {}", + ns_error_to_string(error) + )); + } + return Ok(()); + } + + let command_buffer_obj: ObjcId = + unsafe { msg_send![self.command_queue.as_id(), commandBuffer] }; + let command_buffer = unsafe { StrongId::from_unowned(command_buffer_obj) } + .ok_or_else(|| "commandBuffer returned nil".to_string())?; + unsafe { + let _: () = msg_send![command_buffer.as_id(), commit]; + let _: () = msg_send![command_buffer.as_id(), waitUntilCompleted]; + } + let status: u64 = unsafe { msg_send![command_buffer.as_id(), status] }; + if status == 5 { + let error: ObjcId = unsafe { msg_send![command_buffer.as_id(), error] }; + return Err(format!( + "Metal command buffer error (queue idle wait): {}", + ns_error_to_string(error) + )); + } + Ok(()) + } + + fn end_command_encoder( + &mut self, + handles: Option<(StrongId, StrongId)>, + ) -> Result<(), String> { + let Some((command_buffer, encoder)) = handles else { + return Ok(()); + }; + + unsafe { + let _: () = msg_send![encoder.as_id(), endEncoding]; + let _: () = msg_send![command_buffer.as_id(), commit]; + } + + self.last_command_buffer = Some(command_buffer); + + Ok(()) + } + + fn dispatch_mul_mv_ext( + &mut self, + src0: Src0Type, + src0_id: ObjcId, + src1_id: ObjcId, + dst_id: ObjcId, + ne00: i32, + ne01: i32, + ne10: i32, + ne11: i32, + nb00: u64, + nb01: u64, + nb10: u64, + nb11: u64, + ne0: i32, + ne1: i32, + ) -> Result<(), String> { + static LOG_ONCE: OnceLock<()> = OnceLock::new(); + if LOG_ONCE.set(()).is_ok() { + eprintln!("[ggml][metal] mul_mat dispatch: mul_mv_ext"); + } + + let nsg = 2i32; + let nxpsg = if ne00 % 256 == 0 && ne11 < 3 { + 16i32 + } else if ne00 % 128 == 0 { + 8i32 + } else { + 4i32 + }; + let nypsg = 32 / nxpsg; + let r0ptg = nypsg * nsg; + let r1ptg = match ne11 { + 2 => 2, + 3 | 6 => 3, + 4 | 7 | 8 => 4, + 5 => 5, + _ => return Err(format!("unsupported ne11 for mul_mv_ext: {}", ne11)), + }; + + let base = format!( + "kernel_mul_mv_ext_{}_{}_r1_{}", + src0_type_name(src0), + "f32", + r1ptg + ); + let name = format!("{}_nsg={}_nxpsg={}", base, nsg, nxpsg); + + let constants = [ + FunctionConstant { + idx: FC_MUL_MV + 0, + value: FunctionConstantValue::Int16(nsg as i16), + }, + FunctionConstant { + idx: FC_MUL_MV + 1, + value: FunctionConstantValue::Int16(nxpsg as i16), + }, + ]; + + let (pipeline, _smem, _nr0, _nr1, _pnsg) = + self.get_or_compile_cached_pipeline(name, &base, &constants, 0, 0, 0, nsg)?; + + let args = KArgsMulMvExt { + ne00, + ne01, + ne02: 1, + nb00, + nb01, + nb02: nb01 * ne01 as u64, + nb03: nb01 * ne01 as u64, + ne10, + ne11, + ne12: 1, + nb10, + nb11, + nb12: nb11 * ne11 as u64, + nb13: nb11 * ne11 as u64, + ne0, + ne1, + r2: 1, + r3: 1, + }; + + let (_command_buffer, encoder, encoder_handles) = self.begin_command_encoder()?; + unsafe { + let _: () = msg_send![encoder, setComputePipelineState: pipeline]; + let _: () = msg_send![ + encoder, + setBytes: &args as *const KArgsMulMvExt as *const c_void + length: std::mem::size_of::() as u64 + atIndex: 0u64 + ]; + let _: () = msg_send![encoder, setBuffer: src0_id offset: 0u64 atIndex: 1u64]; + let _: () = msg_send![encoder, setBuffer: src1_id offset: 0u64 atIndex: 2u64]; + let _: () = msg_send![encoder, setBuffer: dst_id offset: 0u64 atIndex: 3u64]; + + let tgs = MTLSize { + width: ((ne01 + r0ptg - 1) / r0ptg) as u64, + height: ((ne11 + r1ptg - 1) / r1ptg) as u64, + depth: 1, + }; + let tpg = MTLSize { + width: 32, + height: nsg as u64, + depth: 1, + }; + let _: () = msg_send![ + encoder, + dispatchThreadgroups: tgs + threadsPerThreadgroup: tpg + ]; + } + + self.end_command_encoder(encoder_handles) + } + + fn dispatch_mul_mm( + &mut self, + src0: Src0Type, + src0_id: ObjcId, + src1_id: ObjcId, + dst_id: ObjcId, + ne00: i32, + ne01: i32, + nb01: u64, + ne12: i32, + nb10: u64, + nb11: u64, + ne0: i32, + ne1: i32, + ) -> Result<(), String> { + static LOG_ONCE: OnceLock<()> = OnceLock::new(); + if LOG_ONCE.set(()).is_ok() { + eprintln!("[ggml][metal] mul_mat dispatch: mul_mm"); + } + + let bc_inp = ne00 % 32 != 0; + let bc_out = ne0 % 64 != 0 || ne1 % 32 != 0; + + let base = format!("kernel_mul_mm_{}_{}", src0_type_name(src0), "f32"); + let name = format!("{}_bci={}_bco={}", base, bc_inp as i32, bc_out as i32); + + let smem = if bc_out { + 8192usize + } else { + 4096usize + 2048usize + }; + let constants = [ + FunctionConstant { + idx: FC_MUL_MM + 0, + value: FunctionConstantValue::Bool(bc_inp), + }, + FunctionConstant { + idx: FC_MUL_MM + 1, + value: FunctionConstantValue::Bool(bc_out), + }, + ]; + + let (pipeline, pipeline_smem, _nr0, _nr1, _nsg) = + self.get_or_compile_cached_pipeline(name, &base, &constants, smem, 0, 0, 0)?; + + let args = KArgsMulMm { + ne00, + ne02: 1, + nb01, + nb02: nb01 * ne01 as u64, + nb03: nb01 * ne01 as u64, + ne12, + nb10, + nb11, + nb12: nb11 * ne1 as u64, + nb13: nb11 * ne1 as u64, + ne0, + ne1, + r2: 1, + r3: 1, + }; + + let (_command_buffer, encoder, encoder_handles) = self.begin_command_encoder()?; + unsafe { + let _: () = msg_send![encoder, setComputePipelineState: pipeline]; + let _: () = msg_send![ + encoder, + setBytes: &args as *const KArgsMulMm as *const c_void + length: std::mem::size_of::() as u64 + atIndex: 0u64 + ]; + let _: () = msg_send![encoder, setBuffer: src0_id offset: 0u64 atIndex: 1u64]; + let _: () = msg_send![encoder, setBuffer: src1_id offset: 0u64 atIndex: 2u64]; + let _: () = msg_send![encoder, setBuffer: dst_id offset: 0u64 atIndex: 3u64]; + let _: () = msg_send![ + encoder, + setThreadgroupMemoryLength: pipeline_smem as u64 + atIndex: 0u64 + ]; + + let tgs = MTLSize { + width: ((ne1 + 31) / 32) as u64, + height: ((ne01 + 63) / 64) as u64, + depth: ne12 as u64, + }; + let tpg = MTLSize { + width: 128, + height: 1, + depth: 1, + }; + let _: () = msg_send![ + encoder, + dispatchThreadgroups: tgs + threadsPerThreadgroup: tpg + ]; + } + + self.end_command_encoder(encoder_handles) + } + + fn dispatch_mul_mv( + &mut self, + src0: Src0Type, + src0_id: ObjcId, + src1_id: ObjcId, + dst_id: ObjcId, + ne00: i32, + ne01: i32, + ne10: i32, + ne11: i32, + nb00: u64, + nb01: u64, + nb10: u64, + nb11: u64, + ne0: i32, + ne1: i32, + ) -> Result<(), String> { + static LOG_ONCE: OnceLock<()> = OnceLock::new(); + if LOG_ONCE.set(()).is_ok() { + eprintln!("[ggml][metal] mul_mat dispatch: mul_mv"); + } + + let (nsg, nr0, nr1, smem, suffix) = match src0 { + Src0Type::F32 | Src0Type::F16 | Src0Type::BF16 => { + if ne00 < 32 { + (1, 32, 1, 0usize, "_short") + } else { + let nsg = ((ne00 + 127) / 128).min(4); + let nr0 = 2; + let smem = 32usize * std::mem::size_of::() * nr0 as usize; + let suffix = if ne00 % 4 == 0 { "_4" } else { "" }; + (nsg, nr0, 1, smem, suffix) + } + } + Src0Type::Q4_0 => (N_SG_Q4_0, N_R0_Q4_0, 1, 0usize, ""), + Src0Type::Q4_1 => (N_SG_Q4_1, N_R0_Q4_1, 1, 0usize, ""), + Src0Type::Q5_0 => (N_SG_Q5_0, N_R0_Q5_0, 1, 0usize, ""), + Src0Type::Q5_1 => (N_SG_Q5_1, N_R0_Q5_1, 1, 0usize, ""), + Src0Type::Q8_0 => ( + N_SG_Q8_0, + N_R0_Q8_0, + 1, + 32usize * std::mem::size_of::() * N_R0_Q8_0 as usize, + "", + ), + Src0Type::Q2_K => (N_SG_Q2_K, N_R0_Q2_K, 1, 0usize, ""), + Src0Type::Q3_K => (N_SG_Q3_K, N_R0_Q3_K, 1, 0usize, ""), + Src0Type::Q4_K => (N_SG_Q4_K, N_R0_Q4_K, 1, 0usize, ""), + Src0Type::Q5_K => (N_SG_Q5_K, N_R0_Q5_K, 1, 0usize, ""), + Src0Type::Q6_K => (N_SG_Q6_K, N_R0_Q6_K, 1, 0usize, ""), + }; + + let base = format!("kernel_mul_mv_{}_{}{}", src0_type_name(src0), "f32", suffix); + let name = format!("{}_nsg={}", base, nsg); + let constants = [FunctionConstant { + idx: FC_MUL_MV + 0, + value: FunctionConstantValue::Int16(nsg as i16), + }]; + + let (pipeline, _pipeline_smem, pn0, pn1, pnsg) = + self.get_or_compile_cached_pipeline(name, &base, &constants, smem, nr0, nr1, nsg)?; + + let args = KArgsMulMv { + ne00, + ne01, + ne02: 1, + nb00, + nb01, + nb02: nb01 * ne01 as u64, + nb03: nb01 * ne01 as u64, + ne10, + ne11, + ne12: 1, + nb10, + nb11, + nb12: nb11 * ne11 as u64, + nb13: nb11 * ne11 as u64, + ne0, + ne1, + nr0: pn0, + r2: 1, + r3: 1, + }; + + let (_command_buffer, encoder, encoder_handles) = self.begin_command_encoder()?; + unsafe { + let _: () = msg_send![encoder, setComputePipelineState: pipeline]; + let _: () = msg_send![ + encoder, + setBytes: &args as *const KArgsMulMv as *const c_void + length: std::mem::size_of::() as u64 + atIndex: 0u64 + ]; + let _: () = msg_send![encoder, setBuffer: src0_id offset: 0u64 atIndex: 1u64]; + let _: () = msg_send![encoder, setBuffer: src1_id offset: 0u64 atIndex: 2u64]; + let _: () = msg_send![encoder, setBuffer: dst_id offset: 0u64 atIndex: 3u64]; + + if smem > 0 { + let _: () = msg_send![ + encoder, + setThreadgroupMemoryLength: smem as u64 + atIndex: 0u64 + ]; + } + + let tg_x = if matches!( + src0, + Src0Type::F32 | Src0Type::F16 | Src0Type::BF16 | Src0Type::Q8_0 + ) { + (ne01 + pn0 - 1) / pn0 + } else { + (ne01 + pn0 * pnsg - 1) / (pn0 * pnsg) + }; + let tg_y = (ne11 + pn1 - 1) / pn1; + + let tgs = MTLSize { + width: tg_x as u64, + height: tg_y as u64, + depth: 1, + }; + let tpg = MTLSize { + width: 32, + height: pnsg as u64, + depth: 1, + }; + let _: () = msg_send![ + encoder, + dispatchThreadgroups: tgs + threadsPerThreadgroup: tpg + ]; + } + + self.end_command_encoder(encoder_handles) + } + + fn dispatch_unary_f32( + &mut self, + op_num: i16, + src0_id: ObjcId, + dst_id: ObjcId, + shape: &Shape4, + ) -> Result<(), String> { + let is_c4 = shape.ne[0] % 4 == 0; + let is_cnt = shape.numel < 32768; + + let base = if is_c4 { + "kernel_unary_f32_f32_4" + } else { + "kernel_unary_f32_f32" + }; + let name = format!("{}_op={}_cnt={}", base, op_num, is_cnt as i32); + + let constants = [ + FunctionConstant { + idx: FC_UNARY + 0, + value: FunctionConstantValue::Int16(op_num), + }, + FunctionConstant { + idx: FC_UNARY + 1, + value: FunctionConstantValue::Bool(is_cnt), + }, + ]; + + let (pipeline, _smem, _nr0, _nr1, _nsg) = + self.get_or_compile_cached_pipeline(name, base, &constants, 0, 0, 0, 0)?; + + let mut args = KArgsUnary { + ne00: shape.ne[0], + ne01: shape.ne[1], + ne02: shape.ne[2], + ne03: shape.ne[3], + nb00: shape.nb[0], + nb01: shape.nb[1], + nb02: shape.nb[2], + nb03: shape.nb[3], + ne0: shape.ne[0], + ne1: shape.ne[1], + ne2: shape.ne[2], + ne3: shape.ne[3], + nb0: shape.nb[0], + nb1: shape.nb[1], + nb2: shape.nb[2], + nb3: shape.nb[3], + slope: 0.0, + scale: 0.0, + bias: 0.0, + val: 0.0, + min: 0.0, + max: 0.0, + }; + + if is_c4 { + args.ne00 /= 4; + args.ne0 /= 4; + } + + let (_command_buffer, encoder, encoder_handles) = self.begin_command_encoder()?; + unsafe { + let _: () = msg_send![encoder, setComputePipelineState: pipeline]; + let _: () = msg_send![ + encoder, + setBytes: &args as *const KArgsUnary as *const c_void + length: std::mem::size_of::() as u64 + atIndex: 0u64 + ]; + let _: () = msg_send![encoder, setBuffer: src0_id offset: 0u64 atIndex: 1u64]; + let _: () = msg_send![encoder, setBuffer: dst_id offset: 0u64 atIndex: 2u64]; + + if is_cnt { + let n = if is_c4 { shape.numel / 4 } else { shape.numel }; + let tgs = MTLSize { + width: n as u64, + height: 1, + depth: 1, + }; + let tpg = MTLSize { + width: 1, + height: 1, + depth: 1, + }; + let _: () = msg_send![ + encoder, + dispatchThreadgroups: tgs + threadsPerThreadgroup: tpg + ]; + } else { + let nth_max = + std::cmp::min(256u64, Self::pipeline_max_threads(pipeline)).max(1u64); + let nth = std::cmp::min(args.ne00 as u64, nth_max).max(1u64); + let nk0 = ((args.ne00 as u64) + nth - 1) / nth; + + let tgs = MTLSize { + width: nk0.saturating_mul(shape.ne[1] as u64), + height: shape.ne[2] as u64, + depth: shape.ne[3] as u64, + }; + let tpg = MTLSize { + width: nth, + height: 1, + depth: 1, + }; + let _: () = msg_send![ + encoder, + dispatchThreadgroups: tgs + threadsPerThreadgroup: tpg + ]; + } + } + + self.end_command_encoder(encoder_handles) + } + + fn dispatch_cpy_f32_to_f16( + &mut self, + src_id: ObjcId, + dst_id: ObjcId, + ne00: usize, + ne01: usize, + nb00: u64, + nb01: u64, + nb0: u64, + nb1: u64, + ) -> Result<(), String> { + let base = "kernel_cpy_f32_f16"; + let (pipeline, _smem, _nr0, _nr1, _nsg) = + self.get_or_compile_cached_pipeline(base.to_string(), base, &[], 0, 0, 0, 0)?; + + let ne00_i64 = i64::try_from(ne00).map_err(|_| format!("ne00 too large: {}", ne00))?; + let ne01_i64 = i64::try_from(ne01).map_err(|_| format!("ne01 too large: {}", ne01))?; + let nb02 = nb01 + .checked_mul(ne01 as u64) + .ok_or_else(|| "overflow computing cpy nb02".to_string())?; + let nb2 = nb1 + .checked_mul(ne01 as u64) + .ok_or_else(|| "overflow computing cpy nb2".to_string())?; + let args = KArgsCpy { + nk0: ne00_i64, + ne00: ne00_i64, + ne01: ne01_i64, + ne02: 1, + ne03: 1, + nb00, + nb01, + nb02, + nb03: nb02, + ne0: ne00_i64, + ne1: ne01_i64, + ne2: 1, + ne3: 1, + nb0, + nb1, + nb2, + nb3: nb2, + }; + + let max_threads = Self::pipeline_max_threads(pipeline).max(1); + let nth = std::cmp::min(ne00.max(1) as u64, max_threads).max(1); + let nw0 = ((ne00.max(1) as u64) + nth - 1) / nth; + + let (_command_buffer, encoder, encoder_handles) = self.begin_command_encoder()?; + unsafe { + let _: () = msg_send![encoder, setComputePipelineState: pipeline]; + let _: () = msg_send![ + encoder, + setBytes: &args as *const KArgsCpy as *const c_void + length: std::mem::size_of::() as u64 + atIndex: 0u64 + ]; + let _: () = msg_send![encoder, setBuffer: src_id offset: 0u64 atIndex: 1u64]; + let _: () = msg_send![encoder, setBuffer: dst_id offset: 0u64 atIndex: 2u64]; + + let tgs = MTLSize { + width: nw0 + .checked_mul(ne01 as u64) + .ok_or_else(|| "overflow computing cpy threadgroups width".to_string())?, + height: 1, + depth: 1, + }; + let tpg = MTLSize { + width: nth, + height: 1, + depth: 1, + }; + let _: () = msg_send![ + encoder, + dispatchThreadgroups: tgs + threadsPerThreadgroup: tpg + ]; + } + + self.end_command_encoder(encoder_handles) + } + + fn dispatch_bin_f32( + &mut self, + op_num: i16, + src0_id: ObjcId, + src1_id: ObjcId, + dst_id: ObjcId, + src0_shape: &Shape4, + src1_shape: &Shape4, + ) -> Result<(), String> { + for d in 0..4 { + let b = src1_shape.ne[d]; + let a = src0_shape.ne[d]; + if b != 1 && b != a { + return Err(format!( + "binary broadcast mismatch at dim {}: lhs={}, rhs={}", + d, a, b + )); + } + } + + let is_c4 = src0_shape.ne[0] % 4 == 0 && src1_shape.ne[0] % 4 == 0; + let is_rb = nrows(src1_shape) == 1 && src0_shape.numel < 65536; + + let base = if is_c4 { + "kernel_bin_fuse_f32_f32_f32_4" + } else { + "kernel_bin_fuse_f32_f32_f32" + }; + let name = format!("{}_op={}_nf=1_rb={}", base, op_num, is_rb as i32); + + let constants = [ + FunctionConstant { + idx: FC_BIN + 0, + value: FunctionConstantValue::Int16(op_num), + }, + FunctionConstant { + idx: FC_BIN + 1, + value: FunctionConstantValue::Int16(1), + }, + FunctionConstant { + idx: FC_BIN + 2, + value: FunctionConstantValue::Bool(is_rb), + }, + ]; + + let (pipeline, _smem, _nr0, _nr1, _nsg) = + self.get_or_compile_cached_pipeline(name, base, &constants, 0, 0, 0, 0)?; + + let mut args = KArgsBin { + ne00: src0_shape.ne[0], + ne01: src0_shape.ne[1], + ne02: src0_shape.ne[2], + ne03: src0_shape.ne[3], + nb00: src0_shape.nb[0], + nb01: src0_shape.nb[1], + nb02: src0_shape.nb[2], + nb03: src0_shape.nb[3], + ne10: src1_shape.ne[0], + ne11: src1_shape.ne[1], + ne12: src1_shape.ne[2], + ne13: src1_shape.ne[3], + nb10: src1_shape.nb[0], + nb11: src1_shape.nb[1], + nb12: src1_shape.nb[2], + nb13: src1_shape.nb[3], + ne0: src0_shape.ne[0], + ne1: src0_shape.ne[1], + ne2: src0_shape.ne[2], + ne3: src0_shape.ne[3], + nb0: src0_shape.nb[0], + nb1: src0_shape.nb[1], + nb2: src0_shape.nb[2], + nb3: src0_shape.nb[3], + offs: 0, + o1: [0u64; 8], + }; + + if is_c4 { + args.ne00 /= 4; + args.ne10 /= 4; + args.ne0 /= 4; + } + + let (_command_buffer, encoder, encoder_handles) = self.begin_command_encoder()?; + unsafe { + let _: () = msg_send![encoder, setComputePipelineState: pipeline]; + let _: () = msg_send![ + encoder, + setBytes: &args as *const KArgsBin as *const c_void + length: std::mem::size_of::() as u64 + atIndex: 0u64 + ]; + let _: () = msg_send![encoder, setBuffer: src0_id offset: 0u64 atIndex: 1u64]; + let _: () = msg_send![encoder, setBuffer: src1_id offset: 0u64 atIndex: 2u64]; + let _: () = msg_send![encoder, setBuffer: dst_id offset: 0u64 atIndex: 3u64]; + + if is_rb { + let n = if is_c4 { + src0_shape.numel / 4 + } else { + src0_shape.numel + }; + let tgs = MTLSize { + width: n as u64, + height: 1, + depth: 1, + }; + let tpg = MTLSize { + width: 1, + height: 1, + depth: 1, + }; + let _: () = msg_send![ + encoder, + dispatchThreadgroups: tgs + threadsPerThreadgroup: tpg + ]; + } else { + let nth_max = + std::cmp::min(256u64, Self::pipeline_max_threads(pipeline)).max(1u64); + let mut nth = 1u64; + while 2 * nth < args.ne0 as u64 && nth < nth_max { + nth *= 2; + } + let tgs = MTLSize { + width: src0_shape.ne[1] as u64, + height: src0_shape.ne[2] as u64, + depth: src0_shape.ne[3] as u64, + }; + let tpg = MTLSize { + width: nth, + height: 1, + depth: 1, + }; + let _: () = msg_send![ + encoder, + dispatchThreadgroups: tgs + threadsPerThreadgroup: tpg + ]; + } + } + + self.end_command_encoder(encoder_handles) + } + + #[allow(clippy::too_many_arguments)] + fn dispatch_norm_f32( + &mut self, + src0_id: ObjcId, + src1_0_id: ObjcId, + src1_1_id: ObjcId, + dst_id: ObjcId, + src0_shape: &Shape4, + src1_0_shape: &Shape4, + src1_1_shape: &Shape4, + eps: f32, + n_fuse: i32, + ) -> Result<(), String> { + if src0_shape.ne[0] <= 0 { + return Err("norm ne0 must be positive".to_string()); + } + + let is_c4 = src0_shape.ne[0] % 4 == 0; + let suffix = if is_c4 { "_4" } else { "" }; + let base = match n_fuse { + 1 => format!("kernel_norm_f32{}", suffix), + 2 => format!("kernel_norm_mul_f32{}", suffix), + 3 => format!("kernel_norm_mul_add_f32{}", suffix), + _ => return Err(format!("unsupported norm fuse level: {}", n_fuse)), + }; + + let (pipeline, pipeline_smem, _nr0, _nr1, _nsg) = + self.get_or_compile_cached_pipeline(base.clone(), &base, &[], 32 * 4, 0, 0, 0)?; + + let ne00_t = if is_c4 { + src0_shape.ne[0] / 4 + } else { + src0_shape.ne[0] + }; + let args = KArgsNorm { + ne00: src0_shape.ne[0], + ne00_t, + nb1: src0_shape.nb[1], + nb2: src0_shape.nb[2], + nb3: src0_shape.nb[3], + eps, + nef1: [src0_shape.ne[1], src1_0_shape.ne[1], src1_1_shape.ne[1]], + nef2: [src0_shape.ne[2], src1_0_shape.ne[2], src1_1_shape.ne[2]], + nef3: [src0_shape.ne[3], src1_0_shape.ne[3], src1_1_shape.ne[3]], + nbf1: [src0_shape.nb[1], src1_0_shape.nb[1], src1_1_shape.nb[1]], + nbf2: [src0_shape.nb[2], src1_0_shape.nb[2], src1_1_shape.nb[2]], + nbf3: [src0_shape.nb[3], src1_0_shape.nb[3], src1_1_shape.nb[3]], + }; + + let mut nth = 32u64; + let nth_max = Self::pipeline_max_threads(pipeline).max(1u64); + while nth < args.ne00_t as u64 && nth < nth_max { + nth *= 2; + } + nth = std::cmp::min(nth, nth_max); + nth = std::cmp::min(nth, args.ne00_t.max(1) as u64); + + let (_command_buffer, encoder, encoder_handles) = self.begin_command_encoder()?; + unsafe { + let _: () = msg_send![encoder, setComputePipelineState: pipeline]; + let _: () = msg_send![ + encoder, + setBytes: &args as *const KArgsNorm as *const c_void + length: std::mem::size_of::() as u64 + atIndex: 0u64 + ]; + let _: () = msg_send![encoder, setBuffer: src0_id offset: 0u64 atIndex: 1u64]; + let _: () = msg_send![encoder, setBuffer: src1_0_id offset: 0u64 atIndex: 2u64]; + let _: () = msg_send![encoder, setBuffer: src1_1_id offset: 0u64 atIndex: 3u64]; + let _: () = msg_send![encoder, setBuffer: dst_id offset: 0u64 atIndex: 4u64]; + let _: () = msg_send![ + encoder, + setThreadgroupMemoryLength: pipeline_smem as u64 + atIndex: 0u64 + ]; + + let tgs = MTLSize { + width: src0_shape.ne[1] as u64, + height: src0_shape.ne[2] as u64, + depth: src0_shape.ne[3] as u64, + }; + let tpg = MTLSize { + width: nth, + height: 1, + depth: 1, + }; + let _: () = msg_send![ + encoder, + dispatchThreadgroups: tgs + threadsPerThreadgroup: tpg + ]; + } + + self.end_command_encoder(encoder_handles) + } + + #[allow(clippy::too_many_arguments)] + fn dispatch_rms_norm_f32( + &mut self, + src0_id: ObjcId, + src1_0_id: ObjcId, + src1_1_id: ObjcId, + dst_id: ObjcId, + src0_shape: &Shape4, + src1_0_shape: &Shape4, + src1_1_shape: &Shape4, + eps: f32, + n_fuse: i32, + ) -> Result<(), String> { + if src0_shape.ne[0] <= 0 { + return Err("rms_norm ne0 must be positive".to_string()); + } + + let is_c4 = src0_shape.ne[0] % 4 == 0; + let suffix = if is_c4 { "_4" } else { "" }; + let base = match n_fuse { + 1 => format!("kernel_rms_norm_f32{}", suffix), + 2 => format!("kernel_rms_norm_mul_f32{}", suffix), + 3 => format!("kernel_rms_norm_mul_add_f32{}", suffix), + _ => return Err(format!("unsupported rms_norm fuse level: {}", n_fuse)), + }; + + let (pipeline, pipeline_smem, _nr0, _nr1, _nsg) = + self.get_or_compile_cached_pipeline(base.clone(), &base, &[], 32 * 4, 0, 0, 0)?; + + let ne00_t = if is_c4 { + src0_shape.ne[0] / 4 + } else { + src0_shape.ne[0] + }; + let args = KArgsNorm { + ne00: src0_shape.ne[0], + ne00_t, + nb1: src0_shape.nb[1], + nb2: src0_shape.nb[2], + nb3: src0_shape.nb[3], + eps, + nef1: [src0_shape.ne[1], src1_0_shape.ne[1], src1_1_shape.ne[1]], + nef2: [src0_shape.ne[2], src1_0_shape.ne[2], src1_1_shape.ne[2]], + nef3: [src0_shape.ne[3], src1_0_shape.ne[3], src1_1_shape.ne[3]], + nbf1: [src0_shape.nb[1], src1_0_shape.nb[1], src1_1_shape.nb[1]], + nbf2: [src0_shape.nb[2], src1_0_shape.nb[2], src1_1_shape.nb[2]], + nbf3: [src0_shape.nb[3], src1_0_shape.nb[3], src1_1_shape.nb[3]], + }; + + let mut nth = 32u64; + let nth_max = Self::pipeline_max_threads(pipeline).max(1u64); + while nth < args.ne00_t as u64 && nth < nth_max { + nth *= 2; + } + nth = std::cmp::min(nth, nth_max); + nth = std::cmp::min(nth, args.ne00_t.max(1) as u64); + + let (_command_buffer, encoder, encoder_handles) = self.begin_command_encoder()?; + unsafe { + let _: () = msg_send![encoder, setComputePipelineState: pipeline]; + let _: () = msg_send![ + encoder, + setBytes: &args as *const KArgsNorm as *const c_void + length: std::mem::size_of::() as u64 + atIndex: 0u64 + ]; + let _: () = msg_send![encoder, setBuffer: src0_id offset: 0u64 atIndex: 1u64]; + let _: () = msg_send![encoder, setBuffer: src1_0_id offset: 0u64 atIndex: 2u64]; + let _: () = msg_send![encoder, setBuffer: src1_1_id offset: 0u64 atIndex: 3u64]; + let _: () = msg_send![encoder, setBuffer: dst_id offset: 0u64 atIndex: 4u64]; + let _: () = msg_send![ + encoder, + setThreadgroupMemoryLength: pipeline_smem as u64 + atIndex: 0u64 + ]; + + let tgs = MTLSize { + width: src0_shape.ne[1] as u64, + height: src0_shape.ne[2] as u64, + depth: src0_shape.ne[3] as u64, + }; + let tpg = MTLSize { + width: nth, + height: 1, + depth: 1, + }; + let _: () = msg_send![ + encoder, + dispatchThreadgroups: tgs + threadsPerThreadgroup: tpg + ]; + } + + self.end_command_encoder(encoder_handles) + } + + fn dispatch_get_rows_ggml( + &mut self, + src0_ggml_type: u32, + src0_id: ObjcId, + src0_shape: &Shape4, + src1_id: ObjcId, + src1_shape: &Shape4, + dst_id: ObjcId, + dst_shape: &Shape4, + ) -> Result<(), String> { + let base = format!("kernel_get_rows_{}", ggml_type_name(src0_ggml_type)); + let (pipeline, _smem, _nr0, _nr1, _nsg) = + self.get_or_compile_cached_pipeline(base.clone(), &base, &[], 0, 0, 0, 0)?; + + let is_quantized = !matches!( + src0_ggml_type, + GGML_TYPE_F32 | GGML_TYPE_F16 | GGML_TYPE_BF16 | GGML_TYPE_I32 + ); + let ne00t = if is_quantized { + src0_shape.ne[0] / 16 + } else { + src0_shape.ne[0] + }; + let args = KArgsGetRows { + ne00t, + ne00: src0_shape.ne[0], + nb01: src0_shape.nb[1], + nb02: src0_shape.nb[2], + nb03: src0_shape.nb[3], + ne10: src1_shape.ne[0], + nb10: src1_shape.nb[0], + nb11: src1_shape.nb[1], + nb12: src1_shape.nb[2], + nb1: dst_shape.nb[1], + nb2: dst_shape.nb[2], + nb3: dst_shape.nb[3], + }; + + let nth = std::cmp::min( + args.ne00t.max(1) as u64, + Self::pipeline_max_threads(pipeline).max(1), + ); + let nw0 = ((args.ne00t.max(1) as u64) + nth - 1) / nth; + + let (_command_buffer, encoder, encoder_handles) = self.begin_command_encoder()?; + unsafe { + let _: () = msg_send![encoder, setComputePipelineState: pipeline]; + let _: () = msg_send![ + encoder, + setBytes: &args as *const KArgsGetRows as *const c_void + length: std::mem::size_of::() as u64 + atIndex: 0u64 + ]; + let _: () = msg_send![encoder, setBuffer: src0_id offset: 0u64 atIndex: 1u64]; + let _: () = msg_send![encoder, setBuffer: src1_id offset: 0u64 atIndex: 2u64]; + let _: () = msg_send![encoder, setBuffer: dst_id offset: 0u64 atIndex: 3u64]; + + let tgs = MTLSize { + width: nw0 * (src1_shape.ne[0] as u64), + height: src1_shape.ne[1] as u64, + depth: src1_shape.ne[2] as u64, + }; + let tpg = MTLSize { + width: nth, + height: 1, + depth: 1, + }; + let _: () = msg_send![ + encoder, + dispatchThreadgroups: tgs + threadsPerThreadgroup: tpg + ]; + } + + self.end_command_encoder(encoder_handles) + } + + fn dispatch_im2col_1d_f32( + &mut self, + src_id: ObjcId, + dst_id: ObjcId, + ic: usize, + iw: usize, + kw: usize, + stride: usize, + pad: usize, + ow: usize, + ) -> Result<(), String> { + let base = "kernel_im2col_f32"; + let (pipeline, _smem, _nr0, _nr1, _nsg) = + self.get_or_compile_cached_pipeline(base.to_string(), base, &[], 0, 0, 0, 0)?; + + let ic_i32 = i32::try_from(ic).map_err(|_| format!("ic too large: {}", ic))?; + let iw_i32 = i32::try_from(iw).map_err(|_| format!("iw too large: {}", iw))?; + let kw_i32 = i32::try_from(kw).map_err(|_| format!("kw too large: {}", kw))?; + let ow_i32 = i32::try_from(ow).map_err(|_| format!("ow too large: {}", ow))?; + let stride_i32 = + i32::try_from(stride).map_err(|_| format!("stride too large: {}", stride))?; + let pad_i32 = i32::try_from(pad).map_err(|_| format!("pad too large: {}", pad))?; + + let chw = ic + .checked_mul(kw) + .ok_or_else(|| "overflow computing im2col CHW".to_string())?; + let ofs0 = ic + .checked_mul(iw) + .ok_or_else(|| "overflow computing im2col ofs0".to_string())?; + + let args = KArgsIm2Col { + ofs0: ofs0 as u64, + ofs1: iw as u64, + iw: iw_i32, + ih: 1, + chw: i32::try_from(chw).map_err(|_| format!("CHW too large: {}", chw))?, + s0: stride_i32, + s1: 1, + p0: pad_i32, + p1: 0, + d0: 1, + d1: 1, + n: 1, + kh: 1, + kw: kw_i32, + khw: kw_i32, + }; + + let max_threads = Self::pipeline_max_threads(pipeline); + let khkw = kw_i32 as u64; + if khkw == 0 || khkw > max_threads { + return Err(format!( + "invalid im2col thread shape: kh*kw={} max={}", + khkw, max_threads + )); + } + let ntptg0 = (max_threads / khkw).min(1).max(1); + + let (_command_buffer, encoder, encoder_handles) = self.begin_command_encoder()?; + unsafe { + let _: () = msg_send![encoder, setComputePipelineState: pipeline]; + let _: () = msg_send![ + encoder, + setBytes: &args as *const KArgsIm2Col as *const c_void + length: std::mem::size_of::() as u64 + atIndex: 0u64 + ]; + let _: () = msg_send![encoder, setBuffer: src_id offset: 0u64 atIndex: 1u64]; + let _: () = msg_send![encoder, setBuffer: dst_id offset: 0u64 atIndex: 2u64]; + + let tgs = MTLSize { + width: ic_i32 as u64, + height: 1, + depth: ow_i32 as u64, + }; + let tpg = MTLSize { + width: ntptg0, + height: 1, + depth: kw_i32 as u64, + }; + let _: () = msg_send![ + encoder, + dispatchThreadgroups: tgs + threadsPerThreadgroup: tpg + ]; + } + + self.end_command_encoder(encoder_handles) + } + + #[allow(clippy::too_many_arguments)] + fn dispatch_flash_attn_ext_pad( + &mut self, + k_id: ObjcId, + v_id: ObjcId, + mask_id: ObjcId, + pad_id: ObjcId, + has_mask: bool, + ncpsg: i32, + ne11: i32, + ne_12_2: i32, + ne_12_3: i32, + nb11: u64, + nb12: u64, + nb13: u64, + nb21: u64, + nb22: u64, + nb23: u64, + ne31: i32, + ne32: i32, + ne33: i32, + nb31: u64, + nb32: u64, + nb33: u64, + ) -> Result<(), String> { + let base = "kernel_flash_attn_ext_pad"; + let name = format!("{}_mask={}_ncpsg={}", base, has_mask as i32, ncpsg); + let constants = [ + FunctionConstant { + idx: FC_FLASH_ATTN_EXT_PAD + 0, + value: FunctionConstantValue::Bool(has_mask), + }, + FunctionConstant { + idx: FC_FLASH_ATTN_EXT_PAD + 25, + value: FunctionConstantValue::Int32(ncpsg), + }, + ]; + let (pipeline, _smem, _nr0, _nr1, _nsg) = + self.get_or_compile_cached_pipeline(name, base, &constants, 0, 0, 0, 0)?; + + let args = KArgsFlashAttnExtPad { + ne11, + ne_12_2, + ne_12_3, + nb11, + nb12, + nb13, + nb21, + nb22, + nb23, + ne31, + ne32, + ne33, + nb31, + nb32, + nb33, + }; + + let (_command_buffer, encoder, encoder_handles) = self.begin_command_encoder()?; + unsafe { + let _: () = msg_send![encoder, setComputePipelineState: pipeline]; + let _: () = msg_send![ + encoder, + setBytes: &args as *const KArgsFlashAttnExtPad as *const c_void + length: std::mem::size_of::() as u64 + atIndex: 0u64 + ]; + let _: () = msg_send![encoder, setBuffer: k_id offset: 0u64 atIndex: 1u64]; + let _: () = msg_send![encoder, setBuffer: v_id offset: 0u64 atIndex: 2u64]; + let _: () = msg_send![encoder, setBuffer: mask_id offset: 0u64 atIndex: 3u64]; + let _: () = msg_send![encoder, setBuffer: pad_id offset: 0u64 atIndex: 4u64]; + + let tgs = MTLSize { + width: ncpsg as u64, + height: ne_12_2.max(ne32) as u64, + depth: ne_12_3.max(ne33) as u64, + }; + let tpg = MTLSize { + width: 32, + height: 1, + depth: 1, + }; + let _: () = msg_send![ + encoder, + dispatchThreadgroups: tgs + threadsPerThreadgroup: tpg + ]; + } + + self.end_command_encoder(encoder_handles) + } + + #[allow(clippy::too_many_arguments)] + fn dispatch_flash_attn_ext_blk( + &mut self, + mask_id: ObjcId, + blk_id: ObjcId, + n_q: usize, + n_kv: usize, + ne31: i32, + ne32: i32, + ne33: i32, + nb31: u64, + nb32: u64, + nb33: u64, + nqptg: i32, + ncpsg: i32, + ) -> Result<(), String> { + let base = "kernel_flash_attn_ext_blk"; + let name = format!("{}_nqptg={}_ncpsg={}", base, nqptg, ncpsg); + let constants = [ + FunctionConstant { + idx: FC_FLASH_ATTN_EXT_BLK + 24, + value: FunctionConstantValue::Int32(nqptg), + }, + FunctionConstant { + idx: FC_FLASH_ATTN_EXT_BLK + 25, + value: FunctionConstantValue::Int32(ncpsg), + }, + ]; + let (pipeline, _smem, _nr0, _nr1, _nsg) = + self.get_or_compile_cached_pipeline(name, base, &constants, 0, 0, 0, 0)?; + + let ne01 = i32::try_from(n_q).map_err(|_| format!("n_q too large: {}", n_q))?; + let ne30 = i32::try_from(n_kv).map_err(|_| format!("n_kv too large: {}", n_kv))?; + let args = KArgsFlashAttnExtBlk { + ne01, + ne30, + ne31, + ne32, + ne33, + nb31, + nb32, + nb33, + }; + + let (_command_buffer, encoder, encoder_handles) = self.begin_command_encoder()?; + unsafe { + let _: () = msg_send![encoder, setComputePipelineState: pipeline]; + let _: () = msg_send![ + encoder, + setBytes: &args as *const KArgsFlashAttnExtBlk as *const c_void + length: std::mem::size_of::() as u64 + atIndex: 0u64 + ]; + let _: () = msg_send![encoder, setBuffer: mask_id offset: 0u64 atIndex: 1u64]; + let _: () = msg_send![encoder, setBuffer: blk_id offset: 0u64 atIndex: 2u64]; + + let nblk1 = ((ne01 + nqptg - 1) / nqptg) as u64; + let nblk0 = ((ne30 + ncpsg - 1) / ncpsg) as u64; + let tgs = MTLSize { + width: nblk0, + height: nblk1, + depth: (ne32 * ne33) as u64, + }; + let tpg = MTLSize { + width: 32, + height: 1, + depth: 1, + }; + let _: () = msg_send![ + encoder, + dispatchThreadgroups: tgs + threadsPerThreadgroup: tpg + ]; + } + + self.end_command_encoder(encoder_handles) + } + + fn dispatch_flash_attn_ext_f32( + &mut self, + q_id: ObjcId, + k_id: ObjcId, + v_id: ObjcId, + mask_id: ObjcId, + sinks_id: ObjcId, + pad_id: ObjcId, + blk_id: ObjcId, + dst_id: ObjcId, + n_q: usize, + n_kv: usize, + n_head: usize, + d: usize, + kv_type: Src0Type, + scale: f32, + has_mask: bool, + has_sinks: bool, + max_bias: f32, + logit_softcap: f32, + ) -> Result<(), String> { + if !flash_attn_supported_head_dim(d) { + return Err(format!( + "unsupported flash-attn head dim for kv kernel: {}", + d + )); + } + if d % 4 != 0 { + return Err(format!( + "flash-attn requires head dim divisible by 4 (float4 store), got {}", + d + )); + } + + let nsg = if d >= 512 { 8 } else { 4 }; + let nqptg = OP_FLASH_ATTN_EXT_NQPSG; + let ncpsg = OP_FLASH_ATTN_EXT_NCPSG; + let has_kvpad = n_kv % (ncpsg as usize) != 0; + let has_bias = max_bias != 0.0; + let has_scap = logit_softcap != 0.0; + + let ne01 = i32::try_from(n_q).map_err(|_| format!("n_q too large: {}", n_q))?; + let ne11 = i32::try_from(n_kv).map_err(|_| format!("n_kv too large: {}", n_kv))?; + let ne02 = + i32::try_from(n_head).map_err(|_| format!("n_head too large: {}", n_head))?; + let n_state = n_head + .checked_mul(d) + .ok_or_else(|| "overflow computing flash n_state".to_string())?; + let kv_elem_bytes = flash_attn_kv_elem_bytes(kv_type)? as u64; + + let nb01 = (n_state as u64) + .checked_mul(4) + .ok_or_else(|| "overflow computing flash nb01".to_string())?; + let nb02 = (d as u64) + .checked_mul(4) + .ok_or_else(|| "overflow computing flash nb02".to_string())?; + let nb03 = nb01 + .checked_mul(n_q as u64) + .ok_or_else(|| "overflow computing flash nb03".to_string())?; + + let nb11 = (n_state as u64) + .checked_mul(kv_elem_bytes) + .ok_or_else(|| "overflow computing flash nb11".to_string())?; + let nb12 = (d as u64) + .checked_mul(kv_elem_bytes) + .ok_or_else(|| "overflow computing flash nb12".to_string())?; + let nb13 = nb11 + .checked_mul(n_kv as u64) + .ok_or_else(|| "overflow computing flash nb13".to_string())?; + + let nb21 = nb11; + let nb22 = nb12; + let nb23 = nb13; + + let ne31 = ne01; + let ne32 = 1i32; + let ne33 = 1i32; + let nb31 = (n_kv as u64) + .checked_mul(2) + .ok_or_else(|| "overflow computing flash nb31".to_string())?; + let nb32 = nb31 + .checked_mul(n_q as u64) + .ok_or_else(|| "overflow computing flash nb32".to_string())?; + let nb33 = nb32; + + let n_head_log2 = if n_head <= 1 { + 1i32 + } else { + let p = (usize::BITS - 1) - n_head.leading_zeros(); + (1usize << p) as i32 + }; + let m0 = (2.0f32).powf(-(max_bias) / (n_head_log2 as f32)); + let m1 = (2.0f32).powf(-(max_bias / 2.0) / (n_head_log2 as f32)); + let scale_k = if has_scap { + scale / logit_softcap + } else { + scale + }; + let bc_mask = has_mask && (ne31 % 8 != 0); + let ns10 = i32::try_from(nb11 / kv_elem_bytes) + .map_err(|_| "overflow computing flash ns10".to_string())?; + let ns20 = i32::try_from(nb21 / kv_elem_bytes) + .map_err(|_| "overflow computing flash ns20".to_string())?; + + if has_kvpad { + self.dispatch_flash_attn_ext_pad( + k_id, v_id, mask_id, pad_id, has_mask, ncpsg, ne11, ne02, 1, nb11, nb12, nb13, + nb21, nb22, nb23, ne31, ne32, ne33, nb31, nb32, nb33, + )?; + } + if has_mask { + self.dispatch_flash_attn_ext_blk( + mask_id, blk_id, n_q, n_kv, ne31, ne32, ne33, nb31, nb32, nb33, nqptg, ncpsg, + )?; + } + + let base = format!( + "kernel_flash_attn_ext_{}_dk{}_dv{}", + src0_type_name(kv_type), + d, + d + ); + let name = format!( + "{}_mask={}_sinks={}_bias={}_scap={}_kvpad={}_bcm={}_ns10={}_ns20={}_nsg={}", + base, + has_mask as i32, + has_sinks as i32, + has_bias as i32, + has_scap as i32, + has_kvpad as i32, + bc_mask as i32, + ns10, + ns20, + nsg + ); + let constants = [ + FunctionConstant { + idx: FC_FLASH_ATTN_EXT + 0, + value: FunctionConstantValue::Bool(has_mask), + }, + FunctionConstant { + idx: FC_FLASH_ATTN_EXT + 1, + value: FunctionConstantValue::Bool(has_sinks), + }, + FunctionConstant { + idx: FC_FLASH_ATTN_EXT + 2, + value: FunctionConstantValue::Bool(has_bias), + }, + FunctionConstant { + idx: FC_FLASH_ATTN_EXT + 3, + value: FunctionConstantValue::Bool(has_scap), + }, + FunctionConstant { + idx: FC_FLASH_ATTN_EXT + 4, + value: FunctionConstantValue::Bool(has_kvpad), + }, + FunctionConstant { + idx: FC_FLASH_ATTN_EXT + 10, + value: FunctionConstantValue::Bool(bc_mask), + }, + FunctionConstant { + idx: FC_FLASH_ATTN_EXT + 20, + value: FunctionConstantValue::Int32(ns10), + }, + FunctionConstant { + idx: FC_FLASH_ATTN_EXT + 21, + value: FunctionConstantValue::Int32(ns20), + }, + FunctionConstant { + idx: FC_FLASH_ATTN_EXT + 22, + value: FunctionConstantValue::Int32(nsg), + }, + ]; + + let smem = flash_attn_smem_bytes(d, d, nsg); + let (pipeline, pipeline_smem, _nr0, _nr1, _pnsg) = + self.get_or_compile_cached_pipeline(name, &base, &constants, smem, 0, 0, nsg)?; + + let args = KArgsFlashAttnExt { + ne01, + ne02, + ne03: 1, + nb01, + nb02, + nb03, + ne11, + ne_12_2: ne02, + ne_12_3: 1, + ns10, + nb11, + nb12, + nb13, + ns20, + nb21, + nb22, + nb23, + ne31, + ne32, + ne33, + nb31, + nb32, + nb33, + ne1: ne02, + ne2: ne01, + ne3: 1, + scale: scale_k, + max_bias, + m0, + m1, + n_head_log2, + logit_softcap, + }; + + let (_command_buffer, encoder, encoder_handles) = self.begin_command_encoder()?; + unsafe { + let _: () = msg_send![encoder, setComputePipelineState: pipeline]; + let _: () = msg_send![ + encoder, + setBytes: &args as *const KArgsFlashAttnExt as *const c_void + length: std::mem::size_of::() as u64 + atIndex: 0u64 + ]; + let _: () = msg_send![encoder, setBuffer: q_id offset: 0u64 atIndex: 1u64]; + let _: () = msg_send![encoder, setBuffer: k_id offset: 0u64 atIndex: 2u64]; + let _: () = msg_send![encoder, setBuffer: v_id offset: 0u64 atIndex: 3u64]; + let _: () = msg_send![encoder, setBuffer: mask_id offset: 0u64 atIndex: 4u64]; + let _: () = msg_send![encoder, setBuffer: sinks_id offset: 0u64 atIndex: 5u64]; + let _: () = msg_send![encoder, setBuffer: pad_id offset: 0u64 atIndex: 6u64]; + let _: () = msg_send![encoder, setBuffer: blk_id offset: 0u64 atIndex: 7u64]; + let _: () = msg_send![encoder, setBuffer: dst_id offset: 0u64 atIndex: 8u64]; + let _: () = msg_send![ + encoder, + setThreadgroupMemoryLength: pipeline_smem as u64 + atIndex: 0u64 + ]; + + let tgs = MTLSize { + width: ((n_q as i32 + nqptg - 1) / nqptg) as u64, + height: n_head as u64, + depth: 1, + }; + let tpg = MTLSize { + width: 32, + height: nsg as u64, + depth: 1, + }; + let _: () = msg_send![ + encoder, + dispatchThreadgroups: tgs + threadsPerThreadgroup: tpg + ]; + } + + self.end_command_encoder(encoder_handles) + } + + fn dispatch_flash_attn_ext_vec_reduce_f32( + &mut self, + tmp_id: ObjcId, + dst_id: ObjcId, + nrows: usize, + d: usize, + nwg: i32, + ) -> Result<(), String> { + let base = "kernel_flash_attn_ext_vec_reduce"; + let name = format!("{}_dv={}_nwg={}", base, d, nwg); + let d_i32 = + i32::try_from(d).map_err(|_| format!("d too large for vec reduce: {}", d))?; + let constants = [ + FunctionConstant { + idx: FC_FLASH_ATTN_EXT_VEC_REDUCE + 0, + value: FunctionConstantValue::Int32(d_i32), + }, + FunctionConstant { + idx: FC_FLASH_ATTN_EXT_VEC_REDUCE + 1, + value: FunctionConstantValue::Int32(nwg), + }, + ]; + let (pipeline, _smem, _nr0, _nr1, _pnsg) = + self.get_or_compile_cached_pipeline(name, base, &constants, 0, 0, 0, 0)?; + + let nrows_i32 = i32::try_from(nrows) + .map_err(|_| format!("nrows too large for vec reduce: {}", nrows))?; + let args = KArgsFlashAttnExtVecReduce { nrows: nrows_i32 }; + + let tpg_width = (32i32) + .checked_mul(nwg) + .ok_or_else(|| "overflow computing vec reduce tpg width".to_string())?; + let max_threads = Self::pipeline_max_threads(pipeline); + if tpg_width as u64 > max_threads { + return Err(format!( + "vec reduce threadsPerThreadgroup={} exceeds max={}", + tpg_width, max_threads + )); + } + + let (_command_buffer, encoder, encoder_handles) = self.begin_command_encoder()?; + unsafe { + let _: () = msg_send![encoder, setComputePipelineState: pipeline]; + let _: () = msg_send![ + encoder, + setBytes: &args as *const KArgsFlashAttnExtVecReduce as *const c_void + length: std::mem::size_of::() as u64 + atIndex: 0u64 + ]; + let _: () = msg_send![encoder, setBuffer: tmp_id offset: 0u64 atIndex: 1u64]; + let _: () = msg_send![encoder, setBuffer: dst_id offset: 0u64 atIndex: 2u64]; + + let tgs = MTLSize { + width: nrows as u64, + height: 1, + depth: 1, + }; + let tpg = MTLSize { + width: tpg_width as u64, + height: 1, + depth: 1, + }; + let _: () = msg_send![ + encoder, + dispatchThreadgroups: tgs + threadsPerThreadgroup: tpg + ]; + } + + self.end_command_encoder(encoder_handles) + } + + fn dispatch_flash_attn_ext_vec_f32( + &mut self, + q_id: ObjcId, + k_id: ObjcId, + v_id: ObjcId, + mask_id: ObjcId, + sinks_id: ObjcId, + pad_id: ObjcId, + tmp_id: Option, + dst_id: ObjcId, + n_q: usize, + n_kv: usize, + n_head: usize, + d: usize, + kv_type: Src0Type, + scale: f32, + has_mask: bool, + has_sinks: bool, + max_bias: f32, + logit_softcap: f32, + ) -> Result<(), String> { + if d % 32 != 0 { + return Err(format!( + "flash-attn vec requires head dim divisible by 32, got {}", + d + )); + } + if !flash_attn_supported_head_dim(d) { + return Err(format!( + "unsupported flash-attn vec head dim for kv kernel: {}", + d + )); + } + + let nqptg = OP_FLASH_ATTN_EXT_VEC_NQPSG; + let ncpsg = OP_FLASH_ATTN_EXT_VEC_NCPSG; + let nhptg = 1i32; + let has_kvpad = n_kv % (ncpsg as usize) != 0; + let has_bias = max_bias != 0.0; + let has_scap = logit_softcap != 0.0; + + let nwg = 32i32; + let mut nsg = 1i32; + while (2i64) + .saturating_mul(nwg as i64) + .saturating_mul(nsg as i64) + .saturating_mul(ncpsg as i64) + < n_kv as i64 + && nsg < 4 + { + nsg *= 2; + } + + let ne01 = i32::try_from(n_q).map_err(|_| format!("n_q too large: {}", n_q))?; + let ne11 = i32::try_from(n_kv).map_err(|_| format!("n_kv too large: {}", n_kv))?; + let ne02 = + i32::try_from(n_head).map_err(|_| format!("n_head too large: {}", n_head))?; + let n_state = n_head + .checked_mul(d) + .ok_or_else(|| "overflow computing flash n_state".to_string())?; + let kv_elem_bytes = flash_attn_kv_elem_bytes(kv_type)? as u64; + + let nb01 = (n_state as u64) + .checked_mul(4) + .ok_or_else(|| "overflow computing flash nb01".to_string())?; + let nb02 = (d as u64) + .checked_mul(4) + .ok_or_else(|| "overflow computing flash nb02".to_string())?; + let nb03 = nb01 + .checked_mul(n_q as u64) + .ok_or_else(|| "overflow computing flash nb03".to_string())?; + + let nb11 = (n_state as u64) + .checked_mul(kv_elem_bytes) + .ok_or_else(|| "overflow computing flash nb11".to_string())?; + let nb12 = (d as u64) + .checked_mul(kv_elem_bytes) + .ok_or_else(|| "overflow computing flash nb12".to_string())?; + let nb13 = nb11 + .checked_mul(n_kv as u64) + .ok_or_else(|| "overflow computing flash nb13".to_string())?; + + let nb21 = nb11; + let nb22 = nb12; + let nb23 = nb13; + + let ne31 = ne01; + let ne32 = 1i32; + let ne33 = 1i32; + let nb31 = (n_kv as u64) + .checked_mul(2) + .ok_or_else(|| "overflow computing flash nb31".to_string())?; + let nb32 = nb31 + .checked_mul(n_q as u64) + .ok_or_else(|| "overflow computing flash nb32".to_string())?; + let nb33 = nb32; + + let n_head_log2 = if n_head <= 1 { + 1i32 + } else { + let p = (usize::BITS - 1) - n_head.leading_zeros(); + (1usize << p) as i32 + }; + let m0 = (2.0f32).powf(-(max_bias) / (n_head_log2 as f32)); + let m1 = (2.0f32).powf(-(max_bias / 2.0) / (n_head_log2 as f32)); + let scale_k = if has_scap { + scale / logit_softcap + } else { + scale + }; + let ns10 = i32::try_from(nb11 / kv_elem_bytes) + .map_err(|_| "overflow computing flash vec ns10".to_string())?; + let ns20 = i32::try_from(nb21 / kv_elem_bytes) + .map_err(|_| "overflow computing flash vec ns20".to_string())?; + + if has_kvpad { + self.dispatch_flash_attn_ext_pad( + k_id, v_id, mask_id, pad_id, has_mask, ncpsg, ne11, ne02, 1, nb11, nb12, nb13, + nb21, nb22, nb23, ne31, ne32, ne33, nb31, nb32, nb33, + )?; + } + + let base = format!( + "kernel_flash_attn_ext_vec_{}_dk{}_dv{}", + src0_type_name(kv_type), + d, + d + ); + let name = format!( + "{}_mask={}_sink={}_bias={}_scap={}_kvpad={}_ns10={}_ns20={}_nsg={}_nwg={}", + base, + has_mask as i32, + has_sinks as i32, + has_bias as i32, + has_scap as i32, + has_kvpad as i32, + ns10, + ns20, + nsg, + nwg + ); + let constants = [ + FunctionConstant { + idx: FC_FLASH_ATTN_EXT_VEC + 0, + value: FunctionConstantValue::Bool(has_mask), + }, + FunctionConstant { + idx: FC_FLASH_ATTN_EXT_VEC + 1, + value: FunctionConstantValue::Bool(has_sinks), + }, + FunctionConstant { + idx: FC_FLASH_ATTN_EXT_VEC + 2, + value: FunctionConstantValue::Bool(has_bias), + }, + FunctionConstant { + idx: FC_FLASH_ATTN_EXT_VEC + 3, + value: FunctionConstantValue::Bool(has_scap), + }, + FunctionConstant { + idx: FC_FLASH_ATTN_EXT_VEC + 4, + value: FunctionConstantValue::Bool(has_kvpad), + }, + FunctionConstant { + idx: FC_FLASH_ATTN_EXT_VEC + 20, + value: FunctionConstantValue::Int32(ns10), + }, + FunctionConstant { + idx: FC_FLASH_ATTN_EXT_VEC + 21, + value: FunctionConstantValue::Int32(ns20), + }, + FunctionConstant { + idx: FC_FLASH_ATTN_EXT_VEC + 22, + value: FunctionConstantValue::Int32(nsg), + }, + FunctionConstant { + idx: FC_FLASH_ATTN_EXT_VEC + 23, + value: FunctionConstantValue::Int32(nwg), + }, + ]; + + let smem = flash_attn_vec_smem_bytes(d, d, nsg); + let (pipeline, pipeline_smem, _nr0, _nr1, _pnsg) = + self.get_or_compile_cached_pipeline(name, &base, &constants, smem, 0, 0, nsg)?; + let max_threads = Self::pipeline_max_threads(pipeline); + let thread_width = (32i32) + .checked_mul(nsg) + .ok_or_else(|| "overflow computing vec thread width".to_string())?; + if thread_width as u64 > max_threads { + return Err(format!( + "flash-attn vec threadsPerThreadgroup={} exceeds max={}", + thread_width, max_threads + )); + } + + let args = KArgsFlashAttnExtVec { + ne01, + ne02, + ne03: 1, + nb01, + nb02, + nb03, + ne11, + ne_12_2: ne02, + ne_12_3: 1, + ns10, + nb11, + nb12, + nb13, + ns20, + nb21, + nb22, + nb23, + ne31, + ne32, + ne33, + nb31, + nb32, + nb33, + ne1: ne02, + ne2: ne01, + ne3: 1, + scale: scale_k, + max_bias, + m0, + m1, + n_head_log2, + logit_softcap, + }; + + let nrows = n_q + .checked_mul(n_head) + .ok_or_else(|| "overflow computing flash vec nrows".to_string())?; + + let out_id = if nwg == 1 { + dst_id + } else { + tmp_id + .ok_or_else(|| "flash-attn vec requires tmp buffer when nwg > 1".to_string())? + }; + + let (_command_buffer, encoder, encoder_handles) = self.begin_command_encoder()?; + unsafe { + let _: () = msg_send![encoder, setComputePipelineState: pipeline]; + let _: () = msg_send![ + encoder, + setBytes: &args as *const KArgsFlashAttnExtVec as *const c_void + length: std::mem::size_of::() as u64 + atIndex: 0u64 + ]; + let _: () = msg_send![encoder, setBuffer: q_id offset: 0u64 atIndex: 1u64]; + let _: () = msg_send![encoder, setBuffer: k_id offset: 0u64 atIndex: 2u64]; + let _: () = msg_send![encoder, setBuffer: v_id offset: 0u64 atIndex: 3u64]; + let _: () = msg_send![encoder, setBuffer: mask_id offset: 0u64 atIndex: 4u64]; + let _: () = msg_send![encoder, setBuffer: sinks_id offset: 0u64 atIndex: 5u64]; + let _: () = msg_send![encoder, setBuffer: pad_id offset: 0u64 atIndex: 6u64]; + let _: () = msg_send![encoder, setBuffer: out_id offset: 0u64 atIndex: 7u64]; + + let _: () = msg_send![ + encoder, + setThreadgroupMemoryLength: pipeline_smem as u64 + atIndex: 0u64 + ]; + + let tgs = MTLSize { + width: ((n_q as i32 + nqptg - 1) / nqptg) as u64, + height: ((n_head as i32 + nhptg - 1) / nhptg) as u64, + depth: nwg as u64, + }; + let tpg = MTLSize { + width: 32, + height: nsg as u64, + depth: 1, + }; + let _: () = msg_send![ + encoder, + dispatchThreadgroups: tgs + threadsPerThreadgroup: tpg + ]; + } + + self.end_command_encoder(encoder_handles)?; + + if nwg > 1 { + let tmp_id = tmp_id + .ok_or_else(|| "flash-attn vec requires tmp buffer when nwg > 1".to_string())?; + self.dispatch_flash_attn_ext_vec_reduce_f32(tmp_id, dst_id, nrows, d, nwg)?; + } + + Ok(()) + } + + fn flash_attn_f32_packed( + &mut self, + q: &[f32], + k: &[f32], + v: &[f32], + n_q: usize, + n_kv: usize, + n_head: usize, + d: usize, + scale: f32, + ) -> Result, String> { + if n_q == 0 || n_kv == 0 || n_head == 0 || d == 0 { + return Ok(Vec::new()); + } + + let q_need = n_q + .checked_mul(n_head) + .and_then(|v| v.checked_mul(d)) + .ok_or_else(|| "overflow computing flash q size".to_string())?; + if q.len() != q_need { + return Err(format!( + "flash q len mismatch: got {}, expected {}", + q.len(), + q_need + )); + } + + let kv_need = n_kv + .checked_mul(n_head) + .and_then(|v| v.checked_mul(d)) + .ok_or_else(|| "overflow computing flash kv size".to_string())?; + if k.len() != kv_need { + return Err(format!( + "flash k len mismatch: got {}, expected {}", + k.len(), + kv_need + )); + } + if v.len() != kv_need { + return Err(format!( + "flash v len mismatch: got {}, expected {}", + v.len(), + kv_need + )); + } + + let out_elems = n_q + .checked_mul(n_head) + .and_then(|v| v.checked_mul(d)) + .ok_or_else(|| "overflow computing flash output size".to_string())?; + let out_bytes = out_elems + .checked_mul(std::mem::size_of::()) + .ok_or_else(|| "overflow computing flash output bytes".to_string())?; + + let q_bytes = unsafe { + std::slice::from_raw_parts( + q.as_ptr() as *const u8, + q.len() * std::mem::size_of::(), + ) + }; + let k_bytes = unsafe { + std::slice::from_raw_parts( + k.as_ptr() as *const u8, + k.len() * std::mem::size_of::(), + ) + }; + let v_bytes = unsafe { + std::slice::from_raw_parts( + v.as_ptr() as *const u8, + v.len() * std::mem::size_of::(), + ) + }; + + let q_buf = self.new_buffer_with_bytes(q_bytes)?; + let k_buf = self.new_buffer_with_bytes(k_bytes)?; + let v_buf = self.new_buffer_with_bytes(v_bytes)?; + let dst_id = self.get_or_create_scratch_buffer(SCRATCH_FLASH_OUT, out_bytes)?; + + let params = FlashAttnExtParams::default(); + let kv_type = Src0Type::F32; + let kv_elem_bytes = flash_attn_kv_elem_bytes(kv_type)?; + let use_vec = flash_attn_use_vec(n_q, d); + let pad_bytes = flash_attn_ext_extra_pad_bytes( + n_q, + n_kv, + n_head, + d, + kv_elem_bytes, + params.has_mask, + use_vec, + )?; + let pad_id = self.get_or_create_scratch_buffer(SCRATCH_FLASH_PAD, pad_bytes)?; + + if use_vec { + let tmp_bytes = flash_attn_ext_extra_tmp_bytes(n_q, n_head, d, 32)?; + let tmp_id = self.get_or_create_scratch_buffer(SCRATCH_FLASH_TMP, tmp_bytes)?; + + self.dispatch_flash_attn_ext_vec_f32( + q_buf.as_id(), + k_buf.as_id(), + v_buf.as_id(), + q_buf.as_id(), // unused when has_mask=false + q_buf.as_id(), // unused when has_sinks=false + pad_id, + Some(tmp_id), + dst_id, + n_q, + n_kv, + n_head, + d, + kv_type, + scale, + params.has_mask, + params.has_sinks, + params.max_bias, + params.logit_softcap, + )?; + } else { + let blk_bytes = flash_attn_ext_extra_blk_bytes(n_q, n_kv, params.has_mask, false)?; + let blk_id = self.get_or_create_scratch_buffer(SCRATCH_FLASH_BLK, blk_bytes)?; + + self.dispatch_flash_attn_ext_f32( + q_buf.as_id(), + k_buf.as_id(), + v_buf.as_id(), + q_buf.as_id(), // unused when has_mask=false + q_buf.as_id(), // unused when has_sinks=false + pad_id, + blk_id, + dst_id, + n_q, + n_kv, + n_head, + d, + kv_type, + scale, + params.has_mask, + params.has_sinks, + params.max_bias, + params.logit_softcap, + )?; + } + + self.read_f32_buffer(dst_id, out_elems) + } + + #[allow(clippy::too_many_arguments)] + fn flash_attn_f32_self_kv_cache( + &mut self, + layer: usize, + q: &[f32], + k_all: &[f32], + v_all: &[f32], + n_kv: usize, + n_head: usize, + d: usize, + scale: f32, + ) -> Result, String> { + if n_kv == 0 || n_head == 0 || d == 0 { + return Ok(Vec::new()); + } + let n_state = n_head + .checked_mul(d) + .ok_or_else(|| "overflow computing n_state".to_string())?; + let kv_need = n_kv + .checked_mul(n_state) + .ok_or_else(|| "overflow computing decoder self kv size".to_string())?; + if q.len() != n_state || k_all.len() != kv_need || v_all.len() != kv_need { + return Err(format!( + "decoder self kv len mismatch: q={}, k_all={}, v_all={}, expected q={}, kv={}", + q.len(), + k_all.len(), + v_all.len(), + n_state, + kv_need + )); + } + + // Mirror whisper.cpp Metal path: self-KV cache length is padded to 32. + let n_kv_flash = pad_to(n_kv, 32); + let (k_id, v_id) = self.ensure_decoder_kv_layer(layer, n_state, n_kv_flash)?; + let row_bytes = n_state + .checked_mul(std::mem::size_of::()) + .ok_or_else(|| "overflow computing decoder kv row bytes".to_string())?; + let start_row = self + .decoder_kv_layers + .get(&layer) + .map(|e| e.len_rows.min(n_kv)) + .unwrap_or(0); + if start_row < n_kv { + let copy_rows = n_kv - start_row; + let copy_bytes = copy_rows + .checked_mul(row_bytes) + .ok_or_else(|| "overflow computing decoder kv copy bytes".to_string())?; + let offset = start_row + .checked_mul(row_bytes) + .ok_or_else(|| "overflow computing decoder kv copy offset".to_string())?; + let src_k = f32_slice_as_bytes(&k_all[start_row * n_state..n_kv * n_state]); + let src_v = f32_slice_as_bytes(&v_all[start_row * n_state..n_kv * n_state]); + let dst_k: *mut u8 = unsafe { msg_send![k_id, contents] }; + let dst_v: *mut u8 = unsafe { msg_send![v_id, contents] }; + if dst_k.is_null() || dst_v.is_null() { + return Err("decoder kv buffer contents returned null".to_string()); + } + unsafe { + std::ptr::copy_nonoverlapping(src_k.as_ptr(), dst_k.add(offset), copy_bytes); + std::ptr::copy_nonoverlapping(src_v.as_ptr(), dst_v.add(offset), copy_bytes); + } + if let Some(entry) = self.decoder_kv_layers.get_mut(&layer) { + entry.len_rows = n_kv; + } + } + + if n_kv < n_kv_flash { + let tail_off = n_kv + .checked_mul(row_bytes) + .ok_or_else(|| "overflow computing decoder kv tail offset".to_string())?; + let tail_len = (n_kv_flash - n_kv) + .checked_mul(row_bytes) + .ok_or_else(|| "overflow computing decoder kv tail bytes".to_string())?; + self.zero_buffer_range(k_id, tail_off, tail_len)?; + self.zero_buffer_range(v_id, tail_off, tail_len)?; + } + + let mask_id = self.prepare_decoder_self_mask_f16(n_kv, n_kv_flash)?; + let q_buf = self.new_buffer_with_bytes(f32_slice_as_bytes(q))?; + let out_buf = self.flash_attn_f32_from_buffers_with_params( + q_buf.as_id(), + k_id, + v_id, + 1, + n_kv_flash, + n_head, + d, + scale, + FlashAttnExtParams { + has_mask: true, + ..FlashAttnExtParams::default() + }, + Some(mask_id), + )?; + self.read_f32_buffer(out_buf.as_id(), n_state) + } + + #[allow(clippy::too_many_arguments)] + fn flash_attn_f32_cross_kv_cache( + &mut self, + layer: usize, + q: &[f32], + k_cross: &[f32], + v_cross: &[f32], + n_q: usize, + n_kv: usize, + n_head: usize, + d: usize, + scale: f32, + ) -> Result, String> { + if n_q == 0 || n_kv == 0 || n_head == 0 || d == 0 { + return Ok(Vec::new()); + } + let n_state = n_head + .checked_mul(d) + .ok_or_else(|| "overflow computing n_state".to_string())?; + let q_need = n_q + .checked_mul(n_state) + .ok_or_else(|| "overflow computing cross q size".to_string())?; + if q.len() != q_need { + return Err(format!( + "cross q len mismatch: got {}, expected {}", + q.len(), + q_need + )); + } + + let (k_id, v_id) = + self.ensure_cross_kv_layer(layer, n_state, n_kv, k_cross, v_cross)?; + let q_buf = self.new_buffer_with_bytes(f32_slice_as_bytes(q))?; + let out_buf = self.flash_attn_f32_from_buffers( + q_buf.as_id(), + k_id, + v_id, + n_q, + n_kv, + n_head, + d, + scale, + )?; + self.read_f32_buffer(out_buf.as_id(), q_need) + } + + fn flash_attn_f32_from_buffers( + &mut self, + q_id: ObjcId, + k_id: ObjcId, + v_id: ObjcId, + n_q: usize, + n_kv: usize, + n_head: usize, + d: usize, + scale: f32, + ) -> Result { + self.flash_attn_f32_from_buffers_with_params( + q_id, + k_id, + v_id, + n_q, + n_kv, + n_head, + d, + scale, + FlashAttnExtParams::default(), + None, + ) + } + + #[allow(clippy::too_many_arguments)] + fn flash_attn_f32_from_buffers_with_params( + &mut self, + q_id: ObjcId, + k_id: ObjcId, + v_id: ObjcId, + n_q: usize, + n_kv: usize, + n_head: usize, + d: usize, + scale: f32, + params: FlashAttnExtParams, + mask_id: Option, + ) -> Result { + self.flash_attn_f32_from_buffers_with_params_typed( + q_id, + k_id, + v_id, + n_q, + n_kv, + n_head, + d, + scale, + params, + mask_id, + Src0Type::F32, + ) + } + + #[allow(clippy::too_many_arguments)] + fn flash_attn_f32_from_buffers_with_params_typed( + &mut self, + q_id: ObjcId, + k_id: ObjcId, + v_id: ObjcId, + n_q: usize, + n_kv: usize, + n_head: usize, + d: usize, + scale: f32, + params: FlashAttnExtParams, + mask_id: Option, + kv_type: Src0Type, + ) -> Result { + let out_elems = n_q + .checked_mul(n_head) + .and_then(|v| v.checked_mul(d)) + .ok_or_else(|| "overflow computing flash output size".to_string())?; + let out_bytes = out_elems + .checked_mul(std::mem::size_of::()) + .ok_or_else(|| "overflow computing flash output bytes".to_string())?; + let kv_elem_bytes = flash_attn_kv_elem_bytes(kv_type)?; + + let dst_id = self.get_or_create_scratch_buffer(SCRATCH_FLASH_OUT, out_bytes)?; + + let use_vec = flash_attn_use_vec(n_q, d); + let pad_bytes = flash_attn_ext_extra_pad_bytes( + n_q, + n_kv, + n_head, + d, + kv_elem_bytes, + params.has_mask, + use_vec, + )?; + let pad_id = self.get_or_create_scratch_buffer(SCRATCH_FLASH_PAD, pad_bytes)?; + let mask_buf_id = if params.has_mask { + mask_id.ok_or_else(|| { + "flash-attn requested mask but no mask buffer was provided".to_string() + })? + } else { + q_id + }; + + if use_vec { + let tmp_bytes = flash_attn_ext_extra_tmp_bytes(n_q, n_head, d, 32)?; + let tmp_id = self.get_or_create_scratch_buffer(SCRATCH_FLASH_TMP, tmp_bytes)?; + + self.dispatch_flash_attn_ext_vec_f32( + q_id, + k_id, + v_id, + mask_buf_id, + q_id, // unused when has_sinks=false + pad_id, + Some(tmp_id), + dst_id, + n_q, + n_kv, + n_head, + d, + kv_type, + scale, + params.has_mask, + params.has_sinks, + params.max_bias, + params.logit_softcap, + )?; + } else { + let blk_bytes = flash_attn_ext_extra_blk_bytes(n_q, n_kv, params.has_mask, false)?; + let blk_id = self.get_or_create_scratch_buffer(SCRATCH_FLASH_BLK, blk_bytes)?; + + self.dispatch_flash_attn_ext_f32( + q_id, + k_id, + v_id, + mask_buf_id, + q_id, // unused when has_sinks=false + pad_id, + blk_id, + dst_id, + n_q, + n_kv, + n_head, + d, + kv_type, + scale, + params.has_mask, + params.has_sinks, + params.max_bias, + params.logit_softcap, + )?; + } + + unsafe { StrongId::from_unowned(dst_id) } + .ok_or_else(|| "flash-attn output buffer returned nil".to_string()) + } + + fn encoder_flash_kv_f16_buffer( + &mut self, + src_f32_id: ObjcId, + seq_len: usize, + n_kv_flash: usize, + n_state: usize, + scratch_kind: u8, + ) -> Result { + let row_bytes_f32 = n_state + .checked_mul(std::mem::size_of::()) + .ok_or_else(|| "overflow computing encoder f32 row bytes".to_string())?; + let row_bytes_f16 = n_state + .checked_mul(std::mem::size_of::()) + .ok_or_else(|| "overflow computing encoder f16 row bytes".to_string())?; + let total_bytes = n_kv_flash + .checked_mul(row_bytes_f16) + .ok_or_else(|| "overflow computing encoder f16 kv bytes".to_string())?; + let dst_id = self.get_or_create_scratch_buffer(scratch_kind, total_bytes)?; + + self.dispatch_cpy_f32_to_f16( + src_f32_id, + dst_id, + n_state, + seq_len, + std::mem::size_of::() as u64, + row_bytes_f32 as u64, + std::mem::size_of::() as u64, + row_bytes_f16 as u64, + )?; + + if seq_len < n_kv_flash { + let tail_off = seq_len + .checked_mul(row_bytes_f16) + .ok_or_else(|| "overflow computing encoder kv tail offset".to_string())?; + let tail_bytes = (n_kv_flash - seq_len) + .checked_mul(row_bytes_f16) + .ok_or_else(|| "overflow computing encoder kv tail bytes".to_string())?; + self.zero_buffer_range(dst_id, tail_off, tail_bytes)?; + } + + Ok(dst_id) + } + + #[allow(clippy::too_many_arguments)] + fn linear_from_src_buffer( + &mut self, + src_id: ObjcId, + m: usize, + k: usize, + w_bytes: &[u8], + w_ggml_type: u32, + n: usize, + bias: Option<&[f32]>, + weight_tag: u8, + bias_tag: u8, + ) -> Result { + let dst = self.matmul_nt_ggml_from_src1_buffer( + src_id, + w_bytes, + w_ggml_type, + m, + k, + n, + Some(weight_tag), + )?; + + if let Some(bias) = bias { + if bias.len() != n { + return Err(format!( + "linear bias len mismatch: got {}, expected {}", + bias.len(), + n + )); + } + let bias_id = self.get_or_create_cached_f32_buffer(bias, bias_tag)?; + let dst_shape = shape4_from_row_major(&[m, n], 4)?; + let bias_shape = shape4_from_row_major(&[n], 4)?; + self.dispatch_bin_f32( + 0, + dst.as_id(), + bias_id, + dst.as_id(), + &dst_shape, + &bias_shape, + )?; + } + + Ok(dst) + } + + #[allow(clippy::too_many_arguments)] + fn encoder_attn_block_f32( + &mut self, + x: &[f32], + seq_len: usize, + n_state: usize, + n_head: usize, + ln_w: &[f32], + ln_b: &[f32], + q_w_bytes: &[u8], + q_w_ggml_type: u32, + q_b: &[f32], + k_w_bytes: &[u8], + k_w_ggml_type: u32, + v_w_bytes: &[u8], + v_w_ggml_type: u32, + v_b: &[f32], + out_w_bytes: &[u8], + out_w_ggml_type: u32, + out_b: &[f32], + ) -> Result, String> { + if n_state == 0 || seq_len == 0 || n_head == 0 || n_state % n_head != 0 { + return Err(format!( + "invalid attn dimensions: seq_len={}, n_state={}, n_head={}", + seq_len, n_state, n_head + )); + } + let x_need = seq_len + .checked_mul(n_state) + .ok_or_else(|| "overflow computing x size".to_string())?; + if x.len() != x_need { + return Err(format!( + "x len mismatch: got {}, expected {}", + x.len(), + x_need + )); + } + if ln_w.len() != n_state || ln_b.len() != n_state { + return Err("layernorm affine size mismatch".to_string()); + } + if q_b.len() != n_state || v_b.len() != n_state || out_b.len() != n_state { + return Err("attention bias size mismatch".to_string()); + } + + let x_buf = self.new_buffer_with_bytes(f32_slice_as_bytes(x))?; + let x_shape = shape4_from_row_major(&[seq_len, n_state], 4)?; + let ln_shape = shape4_from_row_major(&[n_state], 4)?; + let ln_w_id = self.get_or_create_cached_f32_buffer(ln_w, 110)?; + let ln_b_id = self.get_or_create_cached_f32_buffer(ln_b, 111)?; + + let norm_buf = self.new_buffer_with_length(x_need * std::mem::size_of::())?; + self.dispatch_norm_f32( + x_buf.as_id(), + ln_w_id, + ln_b_id, + norm_buf.as_id(), + &x_shape, + &ln_shape, + &ln_shape, + 1e-5f32, + 3, + )?; + + let q_buf = self.linear_from_src_buffer( + norm_buf.as_id(), + seq_len, + n_state, + q_w_bytes, + q_w_ggml_type, + n_state, + Some(q_b), + 112, + 113, + )?; + let k_buf = self.linear_from_src_buffer( + norm_buf.as_id(), + seq_len, + n_state, + k_w_bytes, + k_w_ggml_type, + n_state, + None, + 114, + 0, + )?; + let v_buf = self.linear_from_src_buffer( + norm_buf.as_id(), + seq_len, + n_state, + v_w_bytes, + v_w_ggml_type, + n_state, + Some(v_b), + 115, + 116, + )?; + + let d = n_state / n_head; + let scale = 1.0f32 / (d as f32).sqrt(); + let attn_buf = self.flash_attn_f32_from_buffers( + q_buf.as_id(), + k_buf.as_id(), + v_buf.as_id(), + seq_len, + seq_len, + n_head, + d, + scale, + )?; + + let proj_buf = self.linear_from_src_buffer( + attn_buf.as_id(), + seq_len, + n_state, + out_w_bytes, + out_w_ggml_type, + n_state, + Some(out_b), + 117, + 118, + )?; + + self.dispatch_bin_f32( + 0, + proj_buf.as_id(), + x_buf.as_id(), + proj_buf.as_id(), + &x_shape, + &x_shape, + )?; + + self.read_f32_buffer(proj_buf.as_id(), x_need) + } + + #[allow(clippy::too_many_arguments)] + fn encoder_ffn_block_f32( + &mut self, + x: &[f32], + seq_len: usize, + n_state: usize, + ln_w: &[f32], + ln_b: &[f32], + w0_bytes: &[u8], + w0_ggml_type: u32, + b0: &[f32], + w1_bytes: &[u8], + w1_ggml_type: u32, + b1: &[f32], + ) -> Result, String> { + if n_state == 0 || seq_len == 0 { + return Err(format!( + "invalid ffn dimensions: seq_len={}, n_state={}", + seq_len, n_state + )); + } + let x_need = seq_len + .checked_mul(n_state) + .ok_or_else(|| "overflow computing x size".to_string())?; + if x.len() != x_need { + return Err(format!( + "x len mismatch: got {}, expected {}", + x.len(), + x_need + )); + } + if ln_w.len() != n_state || ln_b.len() != n_state { + return Err("layernorm affine size mismatch".to_string()); + } + let n_ff = b0.len(); + if n_ff == 0 || b1.len() != n_state { + return Err("ffn bias size mismatch".to_string()); + } + + let x_buf = self.new_buffer_with_bytes(f32_slice_as_bytes(x))?; + let x_shape = shape4_from_row_major(&[seq_len, n_state], 4)?; + let ln_shape = shape4_from_row_major(&[n_state], 4)?; + let ln_w_id = self.get_or_create_cached_f32_buffer(ln_w, 120)?; + let ln_b_id = self.get_or_create_cached_f32_buffer(ln_b, 121)?; + + let norm_buf = self.new_buffer_with_length(x_need * std::mem::size_of::())?; + self.dispatch_norm_f32( + x_buf.as_id(), + ln_w_id, + ln_b_id, + norm_buf.as_id(), + &x_shape, + &ln_shape, + &ln_shape, + 1e-5f32, + 3, + )?; + + let ff0_buf = self.linear_from_src_buffer( + norm_buf.as_id(), + seq_len, + n_state, + w0_bytes, + w0_ggml_type, + n_ff, + Some(b0), + 122, + 123, + )?; + + let ff0_shape = shape4_from_row_major(&[seq_len, n_ff], 4)?; + self.dispatch_unary_f32( + OP_UNARY_NUM_GELU, + ff0_buf.as_id(), + ff0_buf.as_id(), + &ff0_shape, + )?; + + let ff1_buf = self.linear_from_src_buffer( + ff0_buf.as_id(), + seq_len, + n_ff, + w1_bytes, + w1_ggml_type, + n_state, + Some(b1), + 124, + 125, + )?; + + self.dispatch_bin_f32( + 0, + ff1_buf.as_id(), + x_buf.as_id(), + ff1_buf.as_id(), + &x_shape, + &x_shape, + )?; + + self.read_f32_buffer(ff1_buf.as_id(), x_need) + } + + #[allow(clippy::too_many_arguments)] + fn encoder_layer_from_buffer_f32( + &mut self, + x_id: ObjcId, + seq_len: usize, + n_state: usize, + n_head: usize, + attn_ln_w: &[f32], + attn_ln_b: &[f32], + q_w_bytes: &[u8], + q_w_ggml_type: u32, + q_b: &[f32], + k_w_bytes: &[u8], + k_w_ggml_type: u32, + v_w_bytes: &[u8], + v_w_ggml_type: u32, + v_b: &[f32], + out_w_bytes: &[u8], + out_w_ggml_type: u32, + out_b: &[f32], + mlp_ln_w: &[f32], + mlp_ln_b: &[f32], + w0_bytes: &[u8], + w0_ggml_type: u32, + b0: &[f32], + w1_bytes: &[u8], + w1_ggml_type: u32, + b1: &[f32], + tag_base: u8, + ) -> Result { + if n_state == 0 || seq_len == 0 || n_head == 0 || n_state % n_head != 0 { + return Err(format!( + "invalid encoder layer dimensions: seq_len={}, n_state={}, n_head={}", + seq_len, n_state, n_head + )); + } + if attn_ln_w.len() != n_state + || attn_ln_b.len() != n_state + || mlp_ln_w.len() != n_state + || mlp_ln_b.len() != n_state + { + return Err("layernorm affine size mismatch".to_string()); + } + if q_b.len() != n_state || v_b.len() != n_state || out_b.len() != n_state { + return Err("attention bias size mismatch".to_string()); + } + let n_ff = b0.len(); + if n_ff == 0 || b1.len() != n_state { + return Err("ffn bias size mismatch".to_string()); + } + + let x_shape = shape4_from_row_major(&[seq_len, n_state], 4)?; + let ln_shape = shape4_from_row_major(&[n_state], 4)?; + + // Attention sub-block + let attn_ln_w_id = + self.get_or_create_cached_f32_buffer(attn_ln_w, tag_base.wrapping_add(0))?; + let attn_ln_b_id = + self.get_or_create_cached_f32_buffer(attn_ln_b, tag_base.wrapping_add(1))?; + let norm_bytes = x_shape + .numel + .checked_mul(std::mem::size_of::()) + .ok_or_else(|| "overflow computing encoder norm buffer bytes".to_string())?; + let norm0_id = self.get_or_create_scratch_buffer(SCRATCH_ENC_NORM0, norm_bytes)?; + self.dispatch_norm_f32( + x_id, + attn_ln_w_id, + attn_ln_b_id, + norm0_id, + &x_shape, + &ln_shape, + &ln_shape, + 1e-5f32, + 3, + )?; + + let q_buf = self.linear_from_src_buffer( + norm0_id, + seq_len, + n_state, + q_w_bytes, + q_w_ggml_type, + n_state, + Some(q_b), + tag_base.wrapping_add(2), + tag_base.wrapping_add(3), + )?; + + // Match whisper.cpp encoder flash-attn usage: keep KV rows padded. + let n_kv_flash = pad_to(seq_len, 256); + + let k_buf_f32 = self.linear_from_src_buffer( + norm0_id, + seq_len, + n_state, + k_w_bytes, + k_w_ggml_type, + n_state, + None, + tag_base.wrapping_add(4), + 0, + )?; + let v_buf_f32 = self.linear_from_src_buffer( + norm0_id, + seq_len, + n_state, + v_w_bytes, + v_w_ggml_type, + n_state, + Some(v_b), + tag_base.wrapping_add(5), + tag_base.wrapping_add(6), + )?; + let k_buf_f16 = self.encoder_flash_kv_f16_buffer( + k_buf_f32.as_id(), + seq_len, + n_kv_flash, + n_state, + SCRATCH_ENC_FLASH_K_F16, + )?; + let v_buf_f16 = self.encoder_flash_kv_f16_buffer( + v_buf_f32.as_id(), + seq_len, + n_kv_flash, + n_state, + SCRATCH_ENC_FLASH_V_F16, + )?; + + let d = n_state / n_head; + let scale = 1.0f32 / (d as f32).sqrt(); + let attn_buf = self.flash_attn_f32_from_buffers_with_params_typed( + q_buf.as_id(), + k_buf_f16, + v_buf_f16, + seq_len, + n_kv_flash, + n_head, + d, + scale, + FlashAttnExtParams::default(), + None, + Src0Type::F16, + )?; + let attn_res_buf = self.linear_from_src_buffer( + attn_buf.as_id(), + seq_len, + n_state, + out_w_bytes, + out_w_ggml_type, + n_state, + Some(out_b), + tag_base.wrapping_add(7), + tag_base.wrapping_add(8), + )?; + self.dispatch_bin_f32( + 0, + attn_res_buf.as_id(), + x_id, + attn_res_buf.as_id(), + &x_shape, + &x_shape, + )?; + + // FFN sub-block + let mlp_ln_w_id = + self.get_or_create_cached_f32_buffer(mlp_ln_w, tag_base.wrapping_add(9))?; + let mlp_ln_b_id = + self.get_or_create_cached_f32_buffer(mlp_ln_b, tag_base.wrapping_add(10))?; + let norm1_id = self.get_or_create_scratch_buffer(SCRATCH_ENC_NORM1, norm_bytes)?; + self.dispatch_norm_f32( + attn_res_buf.as_id(), + mlp_ln_w_id, + mlp_ln_b_id, + norm1_id, + &x_shape, + &ln_shape, + &ln_shape, + 1e-5f32, + 3, + )?; + + let ff0_buf = self.linear_from_src_buffer( + norm1_id, + seq_len, + n_state, + w0_bytes, + w0_ggml_type, + n_ff, + Some(b0), + tag_base.wrapping_add(11), + tag_base.wrapping_add(12), + )?; + let ff0_shape = shape4_from_row_major(&[seq_len, n_ff], 4)?; + self.dispatch_unary_f32( + OP_UNARY_NUM_GELU, + ff0_buf.as_id(), + ff0_buf.as_id(), + &ff0_shape, + )?; + let ff1_buf = self.linear_from_src_buffer( + ff0_buf.as_id(), + seq_len, + n_ff, + w1_bytes, + w1_ggml_type, + n_state, + Some(b1), + tag_base.wrapping_add(13), + tag_base.wrapping_add(14), + )?; + self.dispatch_bin_f32( + 0, + ff1_buf.as_id(), + attn_res_buf.as_id(), + ff1_buf.as_id(), + &x_shape, + &x_shape, + )?; + + Ok(ff1_buf) + } + + #[allow(clippy::too_many_arguments)] + fn encoder_layer_f32( + &mut self, + x: &[f32], + seq_len: usize, + n_state: usize, + n_head: usize, + attn_ln_w: &[f32], + attn_ln_b: &[f32], + q_w_bytes: &[u8], + q_w_ggml_type: u32, + q_b: &[f32], + k_w_bytes: &[u8], + k_w_ggml_type: u32, + v_w_bytes: &[u8], + v_w_ggml_type: u32, + v_b: &[f32], + out_w_bytes: &[u8], + out_w_ggml_type: u32, + out_b: &[f32], + mlp_ln_w: &[f32], + mlp_ln_b: &[f32], + w0_bytes: &[u8], + w0_ggml_type: u32, + b0: &[f32], + w1_bytes: &[u8], + w1_ggml_type: u32, + b1: &[f32], + ) -> Result, String> { + let x_need = seq_len + .checked_mul(n_state) + .ok_or_else(|| "overflow computing x size".to_string())?; + if x.len() != x_need { + return Err(format!( + "x len mismatch: got {}, expected {}", + x.len(), + x_need + )); + } + let x_buf = self.new_buffer_with_bytes(f32_slice_as_bytes(x))?; + + let ff1_buf = self.encoder_layer_from_buffer_f32( + x_buf.as_id(), + seq_len, + n_state, + n_head, + attn_ln_w, + attn_ln_b, + q_w_bytes, + q_w_ggml_type, + q_b, + k_w_bytes, + k_w_ggml_type, + v_w_bytes, + v_w_ggml_type, + v_b, + out_w_bytes, + out_w_ggml_type, + out_b, + mlp_ln_w, + mlp_ln_b, + w0_bytes, + w0_ggml_type, + b0, + w1_bytes, + w1_ggml_type, + b1, + 130, + )?; + self.read_f32_buffer(ff1_buf.as_id(), x_need) + } + + #[allow(clippy::too_many_arguments)] + fn decoder_self_qkv_step_f32( + &mut self, + x: &[f32], + n_state: usize, + attn_ln_w: &[f32], + attn_ln_b: &[f32], + q_w_bytes: &[u8], + q_w_ggml_type: u32, + q_b: &[f32], + k_w_bytes: &[u8], + k_w_ggml_type: u32, + v_w_bytes: &[u8], + v_w_ggml_type: u32, + v_b: &[f32], + ) -> Result<(Vec, Vec, Vec), String> { + if n_state == 0 { + return Err("decoder qkv: n_state is zero".to_string()); + } + if x.len() != n_state + || attn_ln_w.len() != n_state + || attn_ln_b.len() != n_state + || q_b.len() != n_state + || v_b.len() != n_state + { + return Err("decoder qkv: input/affine size mismatch".to_string()); + } + + let x_shape = shape4_from_row_major(&[1, n_state], 4)?; + let ln_shape = shape4_from_row_major(&[n_state], 4)?; + + let (q_buf, k_buf, v_buf) = self.with_batch(|ctx| { + let x_buf = ctx.new_buffer_with_bytes(f32_slice_as_bytes(x))?; + + let ln_w_id = ctx.get_or_create_cached_f32_buffer(attn_ln_w, 240)?; + let ln_b_id = ctx.get_or_create_cached_f32_buffer(attn_ln_b, 241)?; + let norm_bytes = n_state + .checked_mul(std::mem::size_of::()) + .ok_or_else(|| "overflow computing decoder qkv norm bytes".to_string())?; + let norm_id = ctx.get_or_create_scratch_buffer(SCRATCH_DEC_NORM0, norm_bytes)?; + ctx.dispatch_norm_f32( + x_buf.as_id(), + ln_w_id, + ln_b_id, + norm_id, + &x_shape, + &ln_shape, + &ln_shape, + 1e-5f32, + 3, + )?; + + let q_buf = ctx.linear_from_src_buffer( + norm_id, + 1, + n_state, + q_w_bytes, + q_w_ggml_type, + n_state, + Some(q_b), + 242, + 243, + )?; + let k_buf = ctx.linear_from_src_buffer( + norm_id, + 1, + n_state, + k_w_bytes, + k_w_ggml_type, + n_state, + None, + 244, + 0, + )?; + let v_buf = ctx.linear_from_src_buffer( + norm_id, + 1, + n_state, + v_w_bytes, + v_w_ggml_type, + n_state, + Some(v_b), + 245, + 246, + )?; + + Ok((q_buf, k_buf, v_buf)) + })?; + + self.read_f32_buffers3(q_buf.as_id(), k_buf.as_id(), v_buf.as_id(), n_state) + } + + #[allow(clippy::too_many_arguments)] + fn bin_f32( + &mut self, + op_num: i16, + a: &[f32], + a_shape: &[usize], + b: &[f32], + b_shape: &[usize], + ) -> Result, String> { + let a_s = shape4_from_row_major(a_shape, 4)?; + let b_s = shape4_from_row_major(b_shape, 4)?; + if a.len() != a_s.numel { + return Err(format!( + "lhs len mismatch: got {}, expected {}", + a.len(), + a_s.numel + )); + } + if b.len() != b_s.numel { + return Err(format!( + "rhs len mismatch: got {}, expected {}", + b.len(), + b_s.numel + )); + } + + let a_bytes = unsafe { + std::slice::from_raw_parts( + a.as_ptr() as *const u8, + a.len() * std::mem::size_of::(), + ) + }; + let b_bytes = unsafe { + std::slice::from_raw_parts( + b.as_ptr() as *const u8, + b.len() * std::mem::size_of::(), + ) + }; + + let a_buf = self.new_buffer_with_bytes(a_bytes)?; + let b_buf = self.new_buffer_with_bytes(b_bytes)?; + let dst_buf = self.new_buffer_with_length(a_s.numel * std::mem::size_of::())?; + + self.dispatch_bin_f32( + op_num, + a_buf.as_id(), + b_buf.as_id(), + dst_buf.as_id(), + &a_s, + &b_s, + )?; + self.read_f32_buffer(dst_buf.as_id(), a_s.numel) + } + + fn unary_gelu_f32(&mut self, a: &[f32], shape: &[usize]) -> Result, String> { + let s = shape4_from_row_major(shape, 4)?; + if a.len() != s.numel { + return Err(format!( + "unary len mismatch: got {}, expected {}", + a.len(), + s.numel + )); + } + + let a_bytes = unsafe { + std::slice::from_raw_parts( + a.as_ptr() as *const u8, + a.len() * std::mem::size_of::(), + ) + }; + + let a_buf = self.new_buffer_with_bytes(a_bytes)?; + let dst_buf = self.new_buffer_with_length(s.numel * std::mem::size_of::())?; + self.dispatch_unary_f32(OP_UNARY_NUM_GELU, a_buf.as_id(), dst_buf.as_id(), &s)?; + self.read_f32_buffer(dst_buf.as_id(), s.numel) + } + + fn norm_f32(&mut self, x: &[f32], x_shape: &[usize], eps: f32) -> Result, String> { + let s = shape4_from_row_major(x_shape, 4)?; + if x.len() != s.numel { + return Err(format!( + "norm len mismatch: got {}, expected {}", + x.len(), + s.numel + )); + } + + let x_bytes = unsafe { + std::slice::from_raw_parts( + x.as_ptr() as *const u8, + x.len() * std::mem::size_of::(), + ) + }; + + let x_buf = self.new_buffer_with_bytes(x_bytes)?; + let dst_buf = self.new_buffer_with_length(s.numel * std::mem::size_of::())?; + self.dispatch_norm_f32( + x_buf.as_id(), + x_buf.as_id(), + x_buf.as_id(), + dst_buf.as_id(), + &s, + &s, + &s, + eps, + 1, + )?; + self.read_f32_buffer(dst_buf.as_id(), s.numel) + } + + fn rms_norm_f32( + &mut self, + x: &[f32], + x_shape: &[usize], + eps: f32, + ) -> Result, String> { + let s = shape4_from_row_major(x_shape, 4)?; + if x.len() != s.numel { + return Err(format!( + "rms_norm len mismatch: got {}, expected {}", + x.len(), + s.numel + )); + } + + let x_bytes = unsafe { + std::slice::from_raw_parts( + x.as_ptr() as *const u8, + x.len() * std::mem::size_of::(), + ) + }; + + let x_buf = self.new_buffer_with_bytes(x_bytes)?; + let dst_buf = self.new_buffer_with_length(s.numel * std::mem::size_of::())?; + self.dispatch_rms_norm_f32( + x_buf.as_id(), + x_buf.as_id(), + x_buf.as_id(), + dst_buf.as_id(), + &s, + &s, + &s, + eps, + 1, + )?; + self.read_f32_buffer(dst_buf.as_id(), s.numel) + } + + fn rms_norm_mul_f32( + &mut self, + x: &[f32], + x_shape: &[usize], + mul: &[f32], + mul_shape: &[usize], + eps: f32, + ) -> Result, String> { + let x_s = shape4_from_row_major(x_shape, 4)?; + let m_s = shape4_from_row_major(mul_shape, 4)?; + + if x.len() != x_s.numel { + return Err(format!( + "rms_norm src len mismatch: got {}, expected {}", + x.len(), + x_s.numel + )); + } + if mul.len() != m_s.numel { + return Err(format!( + "rms_norm mul len mismatch: got {}, expected {}", + mul.len(), + m_s.numel + )); + } + + if m_s.ne[0] != x_s.ne[0] { + return Err(format!( + "rms_norm fuse ne0 mismatch: x={} mul={}", + x_s.ne[0], m_s.ne[0] + )); + } + for d in 1..4 { + if m_s.ne[d] != 1 && m_s.ne[d] != x_s.ne[d] { + return Err("rms_norm fuse broadcast mismatch".to_string()); + } + } + + let x_bytes = unsafe { + std::slice::from_raw_parts( + x.as_ptr() as *const u8, + x.len() * std::mem::size_of::(), + ) + }; + let mul_bytes = unsafe { + std::slice::from_raw_parts( + mul.as_ptr() as *const u8, + mul.len() * std::mem::size_of::(), + ) + }; + + let x_buf = self.new_buffer_with_bytes(x_bytes)?; + let mul_buf = self.new_buffer_with_bytes(mul_bytes)?; + let dst_buf = self.new_buffer_with_length(x_s.numel * std::mem::size_of::())?; + + self.dispatch_rms_norm_f32( + x_buf.as_id(), + mul_buf.as_id(), + x_buf.as_id(), + dst_buf.as_id(), + &x_s, + &m_s, + &x_s, + eps, + 2, + )?; + self.read_f32_buffer(dst_buf.as_id(), x_s.numel) + } + + #[allow(clippy::too_many_arguments)] + fn norm_mul_add_f32( + &mut self, + x: &[f32], + x_shape: &[usize], + mul: &[f32], + mul_shape: &[usize], + add: &[f32], + add_shape: &[usize], + eps: f32, + ) -> Result, String> { + let x_s = shape4_from_row_major(x_shape, 4)?; + let m_s = shape4_from_row_major(mul_shape, 4)?; + let a_s = shape4_from_row_major(add_shape, 4)?; + + if x.len() != x_s.numel { + return Err(format!( + "norm src len mismatch: got {}, expected {}", + x.len(), + x_s.numel + )); + } + if mul.len() != m_s.numel { + return Err(format!( + "norm mul len mismatch: got {}, expected {}", + mul.len(), + m_s.numel + )); + } + if add.len() != a_s.numel { + return Err(format!( + "norm add len mismatch: got {}, expected {}", + add.len(), + a_s.numel + )); + } + + if m_s.ne[0] != x_s.ne[0] || a_s.ne[0] != x_s.ne[0] { + return Err(format!( + "norm fuse ne0 mismatch: x={} mul={} add={}", + x_s.ne[0], m_s.ne[0], a_s.ne[0] + )); + } + for d in 1..4 { + if (m_s.ne[d] != 1 && m_s.ne[d] != x_s.ne[d]) + || (a_s.ne[d] != 1 && a_s.ne[d] != x_s.ne[d]) + { + return Err("norm fuse broadcast mismatch".to_string()); + } + } + + let x_bytes = unsafe { + std::slice::from_raw_parts( + x.as_ptr() as *const u8, + x.len() * std::mem::size_of::(), + ) + }; + let mul_bytes = unsafe { + std::slice::from_raw_parts( + mul.as_ptr() as *const u8, + mul.len() * std::mem::size_of::(), + ) + }; + let add_bytes = unsafe { + std::slice::from_raw_parts( + add.as_ptr() as *const u8, + add.len() * std::mem::size_of::(), + ) + }; + + let x_buf = self.new_buffer_with_bytes(x_bytes)?; + let mul_buf = self.new_buffer_with_bytes(mul_bytes)?; + let add_buf = self.new_buffer_with_bytes(add_bytes)?; + let dst_buf = self.new_buffer_with_length(x_s.numel * std::mem::size_of::())?; + + self.dispatch_norm_f32( + x_buf.as_id(), + mul_buf.as_id(), + add_buf.as_id(), + dst_buf.as_id(), + &x_s, + &m_s, + &a_s, + eps, + 3, + )?; + self.read_f32_buffer(dst_buf.as_id(), x_s.numel) + } + + fn get_rows_ggml_bytes( + &mut self, + src: &[u8], + src_ggml_type: u32, + n_cols: usize, + n_rows: usize, + row_indices: &[i32], + ) -> Result, String> { + if row_indices.is_empty() { + return Ok(Vec::new()); + } + + let src_ty = src0_type_from_ggml(src_ggml_type) + .ok_or_else(|| format!("unsupported ggml type for get_rows: {}", src_ggml_type))?; + let (row_bytes, elem_or_block) = src0_layout_bytes_per_row(src_ty, n_cols)?; + let expected_src = n_rows + .checked_mul(row_bytes) + .ok_or_else(|| "overflow computing get_rows source bytes".to_string())?; + if src.len() != expected_src { + return Err(format!( + "get_rows source len mismatch: got {}, expected {}", + src.len(), + expected_src + )); + } + + for &row in row_indices { + let row_ok = usize::try_from(row).ok().is_some_and(|row| row < n_rows); + if !row_ok { + return Err(format!( + "get_rows row index {} is out of range {}", + row, n_rows + )); + } + } + + let src_shape = shape4_from_row_major(&[n_cols, n_rows], elem_or_block)?; + let idx_shape = + shape4_from_row_major(&[row_indices.len()], std::mem::size_of::() as u64)?; + let dst_shape = shape4_from_row_major(&[n_cols, row_indices.len()], 4)?; + + let idx_bytes = unsafe { + std::slice::from_raw_parts( + row_indices.as_ptr() as *const u8, + row_indices.len() * std::mem::size_of::(), + ) + }; + + let src_buf = self.new_buffer_with_bytes(src)?; + let idx_buf = self.new_buffer_with_bytes(idx_bytes)?; + let dst_buf = + self.new_buffer_with_length(dst_shape.numel * std::mem::size_of::())?; + self.dispatch_get_rows_ggml( + src_ggml_type, + src_buf.as_id(), + &src_shape, + idx_buf.as_id(), + &idx_shape, + dst_buf.as_id(), + &dst_shape, + )?; + self.read_f32_buffer(dst_buf.as_id(), dst_shape.numel) + } + + fn im2col_1d_f32( + &mut self, + input: &[f32], + ic: usize, + iw: usize, + kw: usize, + stride: usize, + pad: usize, + ) -> Result, String> { + if kw == 0 || stride == 0 { + return Err("im2col requires kw>0 and stride>0".to_string()); + } + let expect = ic + .checked_mul(iw) + .ok_or_else(|| "overflow computing input size".to_string())?; + if input.len() != expect { + return Err(format!( + "im2col input len mismatch: got {}, expected {}", + input.len(), + expect + )); + } + let num = iw + .checked_add(pad.saturating_mul(2)) + .ok_or_else(|| "overflow computing im2col output numerator".to_string())?; + if num < kw { + return Ok(Vec::new()); + } + let ow = (num - kw) / stride + 1; + let chw = ic + .checked_mul(kw) + .ok_or_else(|| "overflow computing im2col CHW".to_string())?; + let out_elems = ow + .checked_mul(chw) + .ok_or_else(|| "overflow computing im2col output size".to_string())?; + + let input_bytes = unsafe { + std::slice::from_raw_parts( + input.as_ptr() as *const u8, + input.len() * std::mem::size_of::(), + ) + }; + let src_buf = self.new_buffer_with_bytes(input_bytes)?; + let dst_buf = self.new_buffer_with_length(out_elems * std::mem::size_of::())?; + self.dispatch_im2col_1d_f32( + src_buf.as_id(), + dst_buf.as_id(), + ic, + iw, + kw, + stride, + pad, + ow, + )?; + self.read_f32_buffer(dst_buf.as_id(), out_elems) + } + + fn matmul_nt_ggml_from_src1_buffer( + &mut self, + src1_id: ObjcId, + bt_bytes: &[u8], + bt_ggml_type: u32, + m: usize, + k: usize, + n: usize, + weight_cache_tag: Option, + ) -> Result { + if m == 0 || k == 0 || n == 0 { + return self + .new_buffer_with_length(m.saturating_mul(n) * std::mem::size_of::()); + } + + let src0 = src0_type_from_ggml(bt_ggml_type).ok_or_else(|| { + format!( + "unsupported src0 ggml_type for metal matmul: {}", + bt_ggml_type + ) + })?; + let (src0_row_bytes, nb00) = src0_layout_bytes_per_row(src0, k)?; + let expected_src0 = n + .checked_mul(src0_row_bytes) + .ok_or_else(|| "matmul overflow computing src0 bytes".to_string())?; + if bt_bytes.len() != expected_src0 { + return Err(format!( + "rhs len mismatch: got {}, expected {} (type {:?}, k={}, n={})", + bt_bytes.len(), + expected_src0, + src0, + k, + n + )); + } + + let ne00 = i32::try_from(k).map_err(|_| format!("k too large: {}", k))?; + let ne01 = i32::try_from(n).map_err(|_| format!("n too large: {}", n))?; + let ne10 = i32::try_from(k).map_err(|_| format!("k too large: {}", k))?; + let ne11 = i32::try_from(m).map_err(|_| format!("m too large: {}", m))?; + let ne0 = ne01; + let ne1 = ne11; + let nb01 = src0_row_bytes as u64; + let nb10 = 4u64; + let nb11 = (k as u64) + .checked_mul(4) + .ok_or_else(|| "overflow computing nb11".to_string())?; + let mn = m + .checked_mul(n) + .ok_or_else(|| "matmul overflow computing m*n".to_string())?; + + let mut src0_temp = None; + let src0_id = if let Some(tag) = weight_cache_tag { + let key = BufferKey { + ptr: bt_bytes.as_ptr() as usize, + len: bt_bytes.len(), + tag, + }; + self.get_or_create_weight_buffer(key, bt_bytes)? + } else { + let b = self.new_buffer_with_bytes(bt_bytes)?; + let id = b.as_id(); + src0_temp = Some(b); + id + }; + + let dst_bytes = mn + .checked_mul(std::mem::size_of::()) + .ok_or_else(|| "matmul overflow computing dst bytes".to_string())?; + let mut dst_temp = None; + let dst_id = if let Some(tag) = weight_cache_tag { + self.get_or_create_matmul_out_buffer(tag, dst_bytes)? + } else { + let b = self.new_buffer_with_length(dst_bytes)?; + let id = b.as_id(); + dst_temp = Some(b); + id + }; + let used_mul_mv_ext = can_use_mul_mv_ext(src0, ne00, ne11); + let used_mul_mm = ne00 >= 64 && ne11 > 8; + + let (kernel, compute_res) = if used_mul_mv_ext { + ( + "mul_mv_ext", + self.dispatch_mul_mv_ext( + src0, src0_id, src1_id, dst_id, ne00, ne01, ne10, ne11, nb00, nb01, nb10, + nb11, ne0, ne1, + ), + ) + } else if used_mul_mm { + match self.dispatch_mul_mm( + src0, src0_id, src1_id, dst_id, ne00, ne01, nb01, 1, nb10, nb11, ne0, ne1, + ) { + Ok(()) => ("mul_mm", Ok(())), + Err(e) => ("mul_mv", { + eprintln!( + "[ggml][metal] mul_mm failed for type {:?}, falling back to mul_mv: {}", + src0, e + ); + self.dispatch_mul_mv( + src0, src0_id, src1_id, dst_id, ne00, ne01, ne10, ne11, nb00, nb01, + nb10, nb11, ne0, ne1, + ) + }), + } + } else { + ( + "mul_mv", + self.dispatch_mul_mv( + src0, src0_id, src1_id, dst_id, ne00, ne01, ne10, ne11, nb00, nb01, nb10, + nb11, ne0, ne1, + ), + ) + }; + compute_res?; + if super::log_mul_mat_requested() { + eprintln!( + "[ggml][metal] mul_mat kernel={} src0={} src1=f32 ne00={} ne01={} ne11={} ne12=1 nb01={} nb11={}", + kernel, + src0_type_name(src0), + ne00, + ne01, + ne11, + nb01, + nb11 + ); + } + drop(src0_temp); + if let Some(dst_buffer) = dst_temp { + Ok(dst_buffer) + } else { + unsafe { StrongId::from_unowned(dst_id) } + .ok_or_else(|| "matmul scratch output buffer returned nil".to_string()) + } + } + + fn matmul_nt_ggml_bytes_impl( + &mut self, + a: &[f32], + bt_bytes: &[u8], + bt_ggml_type: u32, + m: usize, + k: usize, + n: usize, + weight_cache_tag: Option, + ) -> Result<(StrongId, usize, usize), String> { + let mk = m + .checked_mul(k) + .ok_or_else(|| "matmul overflow computing m*k".to_string())?; + if a.len() != mk { + return Err(format!( + "lhs len mismatch: got {}, expected {}", + a.len(), + mk + )); + } + + let a_bytes = unsafe { + std::slice::from_raw_parts( + a.as_ptr() as *const u8, + a.len() * std::mem::size_of::(), + ) + }; + let src1_buffer = self.new_buffer_with_bytes(a_bytes)?; + let dst_buffer = self.matmul_nt_ggml_from_src1_buffer( + src1_buffer.as_id(), + bt_bytes, + bt_ggml_type, + m, + k, + n, + weight_cache_tag, + )?; + + Ok((dst_buffer, m, n)) + } + + fn matmul_nn_f32( + &mut self, + a: &[f32], + b: &[f32], + m: usize, + k: usize, + n: usize, + ) -> Result, String> { + let mk = m + .checked_mul(k) + .ok_or_else(|| "overflow computing m*k".to_string())?; + let kn = k + .checked_mul(n) + .ok_or_else(|| "overflow computing k*n".to_string())?; + if a.len() != mk { + return Err(format!( + "lhs len mismatch: got {}, expected {}", + a.len(), + mk + )); + } + if b.len() != kn { + return Err(format!( + "rhs len mismatch: got {}, expected {}", + b.len(), + kn + )); + } + + let mut bt = vec![0.0f32; n * k]; + for i in 0..k { + for j in 0..n { + bt[j * k + i] = b[i * n + j]; + } + } + + let bt_bytes = unsafe { + std::slice::from_raw_parts( + bt.as_ptr() as *const u8, + bt.len() * std::mem::size_of::(), + ) + }; + + let (dst, mr, nr) = + self.matmul_nt_ggml_bytes_impl(a, bt_bytes, GGML_TYPE_F32, m, k, n, None)?; + self.read_f32_buffer(dst.as_id(), mr * nr) + } + + fn matmul_nt_f32( + &mut self, + a: &[f32], + bt: &[f32], + m: usize, + k: usize, + n: usize, + ) -> Result, String> { + let bt_bytes = unsafe { + std::slice::from_raw_parts( + bt.as_ptr() as *const u8, + bt.len() * std::mem::size_of::(), + ) + }; + let cache_tag = Some(1u8); + let (dst, mr, nr) = + self.matmul_nt_ggml_bytes_impl(a, bt_bytes, GGML_TYPE_F32, m, k, n, cache_tag)?; + self.read_f32_buffer(dst.as_id(), mr * nr) + } + + fn matmul_nt_f32_bytes( + &mut self, + a: &[f32], + bt_bytes: &[u8], + m: usize, + k: usize, + n: usize, + ) -> Result, String> { + let (dst, mr, nr) = + self.matmul_nt_ggml_bytes_impl(a, bt_bytes, GGML_TYPE_F32, m, k, n, Some(2u8))?; + self.read_f32_buffer(dst.as_id(), mr * nr) + } + + fn matmul_nt_f16_bytes( + &mut self, + a: &[f32], + bt_f16_bytes: &[u8], + m: usize, + k: usize, + n: usize, + ) -> Result, String> { + let (dst, mr, nr) = + self.matmul_nt_ggml_bytes_impl(a, bt_f16_bytes, GGML_TYPE_F16, m, k, n, Some(3u8))?; + self.read_f32_buffer(dst.as_id(), mr * nr) + } + + fn matmul_nt_ggml_bytes( + &mut self, + a: &[f32], + bt_bytes: &[u8], + bt_ggml_type: u32, + m: usize, + k: usize, + n: usize, + ) -> Result, String> { + let tag = match bt_ggml_type { + GGML_TYPE_F32 => 2u8, + GGML_TYPE_F16 => 3u8, + GGML_TYPE_BF16 => 9u8, + GGML_TYPE_Q4_0 => 4u8, + GGML_TYPE_Q4_1 => 5u8, + GGML_TYPE_Q5_0 => 6u8, + GGML_TYPE_Q5_1 => 7u8, + GGML_TYPE_Q8_0 => 8u8, + GGML_TYPE_Q2_K => 10u8, + GGML_TYPE_Q3_K => 11u8, + GGML_TYPE_Q4_K => 12u8, + GGML_TYPE_Q5_K => 13u8, + GGML_TYPE_Q6_K => 14u8, + _ => 0u8, + }; + let (dst, mr, nr) = + self.matmul_nt_ggml_bytes_impl(a, bt_bytes, bt_ggml_type, m, k, n, Some(tag))?; + self.read_f32_buffer(dst.as_id(), mr * nr) + } + + fn matmul_nt_ggml_bytes_add_bias( + &mut self, + a: &[f32], + bt_bytes: &[u8], + bt_ggml_type: u32, + m: usize, + k: usize, + n: usize, + bias: &[f32], + ) -> Result, String> { + if bias.len() != n { + return Err(format!( + "bias len mismatch for matmul+add: got {}, expected {}", + bias.len(), + n + )); + } + + let tag = match bt_ggml_type { + GGML_TYPE_F32 => 2u8, + GGML_TYPE_F16 => 3u8, + GGML_TYPE_BF16 => 9u8, + GGML_TYPE_Q4_0 => 4u8, + GGML_TYPE_Q4_1 => 5u8, + GGML_TYPE_Q5_0 => 6u8, + GGML_TYPE_Q5_1 => 7u8, + GGML_TYPE_Q8_0 => 8u8, + GGML_TYPE_Q2_K => 10u8, + GGML_TYPE_Q3_K => 11u8, + GGML_TYPE_Q4_K => 12u8, + GGML_TYPE_Q5_K => 13u8, + GGML_TYPE_Q6_K => 14u8, + _ => 0u8, + }; + + let (dst, mr, nr) = + self.matmul_nt_ggml_bytes_impl(a, bt_bytes, bt_ggml_type, m, k, n, Some(tag))?; + + let bias_shape = shape4_from_row_major(&[n], 4)?; + let dst_shape = shape4_from_row_major(&[m, n], 4)?; + let bias_bytes = unsafe { + std::slice::from_raw_parts( + bias.as_ptr() as *const u8, + bias.len() * std::mem::size_of::(), + ) + }; + let bias_buf = self.new_buffer_with_bytes(bias_bytes)?; + self.dispatch_bin_f32( + 0, + dst.as_id(), + bias_buf.as_id(), + dst.as_id(), + &dst_shape, + &bias_shape, + )?; + + self.read_f32_buffer(dst.as_id(), mr * nr) + } + } + + fn with_context(f: impl FnOnce(&mut MetalContext) -> Result) -> Option { + enum ContextState { + Uninitialized, + Disabled, + Ready(MetalContext), + } + + thread_local! { + static CONTEXT: RefCell = const { RefCell::new(ContextState::Uninitialized) }; + } + + CONTEXT.with(|ctx| { + let mut ctx = ctx.borrow_mut(); + + if matches!(&*ctx, ContextState::Uninitialized) { + *ctx = match MetalContext::new() { + Ok(created) => ContextState::Ready(created), + Err(err) => { + eprintln!("[ggml][metal] backend disabled: {}", err); + ContextState::Disabled + } + }; + } + + let ctx = match &mut *ctx { + ContextState::Ready(ctx) => ctx, + ContextState::Disabled | ContextState::Uninitialized => return None, + }; + match f(ctx) { + Ok(v) => Some(v), + Err(err) => { + eprintln!("[ggml][metal] compute failed: {}", err); + None + } + } + }) + } + + pub(super) fn try_matmul_nn_f32( + a: &[f32], + b: &[f32], + m: usize, + k: usize, + n: usize, + ) -> Option> { + with_context(|ctx| ctx.matmul_nn_f32(a, b, m, k, n)) + } + + pub(super) fn try_matmul_nt_f32( + a: &[f32], + bt: &[f32], + m: usize, + k: usize, + n: usize, + ) -> Option> { + with_context(|ctx| ctx.matmul_nt_f32(a, bt, m, k, n)) + } + + pub(super) fn try_matmul_nt_f32_bytes( + a: &[f32], + bt_bytes: &[u8], + m: usize, + k: usize, + n: usize, + ) -> Option> { + with_context(|ctx| ctx.matmul_nt_f32_bytes(a, bt_bytes, m, k, n)) + } + + pub(super) fn try_matmul_nt_f16_bytes( + a: &[f32], + bt_f16_bytes: &[u8], + m: usize, + k: usize, + n: usize, + ) -> Option> { + with_context(|ctx| ctx.matmul_nt_f16_bytes(a, bt_f16_bytes, m, k, n)) + } + + pub(super) fn try_matmul_nt_ggml_bytes( + a: &[f32], + bt_bytes: &[u8], + bt_ggml_type: u32, + m: usize, + k: usize, + n: usize, + ) -> Option> { + with_context(|ctx| ctx.matmul_nt_ggml_bytes(a, bt_bytes, bt_ggml_type, m, k, n)) + } + + pub(super) fn try_matmul_nt_ggml_bytes_add_bias( + a: &[f32], + bt_bytes: &[u8], + bt_ggml_type: u32, + m: usize, + k: usize, + n: usize, + bias: &[f32], + ) -> Option> { + with_context(|ctx| { + ctx.matmul_nt_ggml_bytes_add_bias(a, bt_bytes, bt_ggml_type, m, k, n, bias) + }) + } + + pub(super) fn try_flash_attn_f32_packed( + q: &[f32], + k: &[f32], + v: &[f32], + n_q: usize, + n_kv: usize, + n_head: usize, + d: usize, + scale: f32, + ) -> Option> { + with_context(|ctx| ctx.flash_attn_f32_packed(q, k, v, n_q, n_kv, n_head, d, scale)) + } + + pub(super) fn clear_decoder_kv_cache() { + let _ = with_context(|ctx| { + ctx.clear_decoder_kv_cache(); + Ok(()) + }); + } + + #[allow(clippy::too_many_arguments)] + pub(super) fn try_flash_attn_f32_self_kv_cache( + layer: usize, + q: &[f32], + k_all: &[f32], + v_all: &[f32], + n_kv: usize, + n_head: usize, + d: usize, + scale: f32, + ) -> Option> { + with_context(|ctx| { + ctx.flash_attn_f32_self_kv_cache(layer, q, k_all, v_all, n_kv, n_head, d, scale) + }) + } + + #[allow(clippy::too_many_arguments)] + pub(super) fn try_flash_attn_f32_cross_kv_cache( + layer: usize, + q: &[f32], + k_cross: &[f32], + v_cross: &[f32], + n_q: usize, + n_kv: usize, + n_head: usize, + d: usize, + scale: f32, + ) -> Option> { + with_context(|ctx| { + ctx.flash_attn_f32_cross_kv_cache( + layer, q, k_cross, v_cross, n_q, n_kv, n_head, d, scale, + ) + }) + } + + pub(super) fn try_add_f32( + a: &[f32], + a_shape: &[usize], + b: &[f32], + b_shape: &[usize], + ) -> Option> { + with_context(|ctx| ctx.bin_f32(0, a, a_shape, b, b_shape)) + } + + pub(super) fn try_mul_f32( + a: &[f32], + a_shape: &[usize], + b: &[f32], + b_shape: &[usize], + ) -> Option> { + with_context(|ctx| ctx.bin_f32(2, a, a_shape, b, b_shape)) + } + + pub(super) fn try_gelu_f32(a: &[f32], shape: &[usize]) -> Option> { + with_context(|ctx| ctx.unary_gelu_f32(a, shape)) + } + + pub(super) fn try_layer_norm_f32(x: &[f32], shape: &[usize], eps: f32) -> Option> { + with_context(|ctx| ctx.norm_f32(x, shape, eps)) + } + + pub(super) fn try_rms_norm_f32(x: &[f32], shape: &[usize], eps: f32) -> Option> { + with_context(|ctx| ctx.rms_norm_f32(x, shape, eps)) + } + + pub(super) fn try_rms_norm_mul_f32( + x: &[f32], + x_shape: &[usize], + mul: &[f32], + mul_shape: &[usize], + eps: f32, + ) -> Option> { + with_context(|ctx| ctx.rms_norm_mul_f32(x, x_shape, mul, mul_shape, eps)) + } + + pub(super) fn try_layer_norm_mul_add_f32( + x: &[f32], + x_shape: &[usize], + mul: &[f32], + mul_shape: &[usize], + add: &[f32], + add_shape: &[usize], + eps: f32, + ) -> Option> { + with_context(|ctx| ctx.norm_mul_add_f32(x, x_shape, mul, mul_shape, add, add_shape, eps)) + } + + pub(super) fn try_get_rows_ggml_bytes( + src: &[u8], + src_ggml_type: u32, + n_cols: usize, + n_rows: usize, + row_indices: &[i32], + ) -> Option> { + with_context(|ctx| ctx.get_rows_ggml_bytes(src, src_ggml_type, n_cols, n_rows, row_indices)) + } + + pub(super) fn try_im2col_1d_f32( + input: &[f32], + ic: usize, + iw: usize, + kw: usize, + stride: usize, + pad: usize, + ) -> Option> { + with_context(|ctx| ctx.im2col_1d_f32(input, ic, iw, kw, stride, pad)) + } + + #[allow(clippy::too_many_arguments)] + pub(super) fn try_encoder_attn_block_f32( + x: &[f32], + seq_len: usize, + n_state: usize, + n_head: usize, + ln_w: &[f32], + ln_b: &[f32], + q_w_bytes: &[u8], + q_w_ggml_type: u32, + q_b: &[f32], + k_w_bytes: &[u8], + k_w_ggml_type: u32, + v_w_bytes: &[u8], + v_w_ggml_type: u32, + v_b: &[f32], + out_w_bytes: &[u8], + out_w_ggml_type: u32, + out_b: &[f32], + ) -> Option> { + with_context(|ctx| { + ctx.encoder_attn_block_f32( + x, + seq_len, + n_state, + n_head, + ln_w, + ln_b, + q_w_bytes, + q_w_ggml_type, + q_b, + k_w_bytes, + k_w_ggml_type, + v_w_bytes, + v_w_ggml_type, + v_b, + out_w_bytes, + out_w_ggml_type, + out_b, + ) + }) + } + + #[allow(clippy::too_many_arguments)] + pub(super) fn try_encoder_ffn_block_f32( + x: &[f32], + seq_len: usize, + n_state: usize, + ln_w: &[f32], + ln_b: &[f32], + w0_bytes: &[u8], + w0_ggml_type: u32, + b0: &[f32], + w1_bytes: &[u8], + w1_ggml_type: u32, + b1: &[f32], + ) -> Option> { + with_context(|ctx| { + ctx.encoder_ffn_block_f32( + x, + seq_len, + n_state, + ln_w, + ln_b, + w0_bytes, + w0_ggml_type, + b0, + w1_bytes, + w1_ggml_type, + b1, + ) + }) + } + + #[allow(clippy::too_many_arguments)] + pub(super) fn try_encoder_layer_f32( + x: &[f32], + seq_len: usize, + n_state: usize, + n_head: usize, + attn_ln_w: &[f32], + attn_ln_b: &[f32], + q_w_bytes: &[u8], + q_w_ggml_type: u32, + q_b: &[f32], + k_w_bytes: &[u8], + k_w_ggml_type: u32, + v_w_bytes: &[u8], + v_w_ggml_type: u32, + v_b: &[f32], + out_w_bytes: &[u8], + out_w_ggml_type: u32, + out_b: &[f32], + mlp_ln_w: &[f32], + mlp_ln_b: &[f32], + w0_bytes: &[u8], + w0_ggml_type: u32, + b0: &[f32], + w1_bytes: &[u8], + w1_ggml_type: u32, + b1: &[f32], + ) -> Option> { + with_context(|ctx| { + ctx.encoder_layer_f32( + x, + seq_len, + n_state, + n_head, + attn_ln_w, + attn_ln_b, + q_w_bytes, + q_w_ggml_type, + q_b, + k_w_bytes, + k_w_ggml_type, + v_w_bytes, + v_w_ggml_type, + v_b, + out_w_bytes, + out_w_ggml_type, + out_b, + mlp_ln_w, + mlp_ln_b, + w0_bytes, + w0_ggml_type, + b0, + w1_bytes, + w1_ggml_type, + b1, + ) + }) + } + + #[allow(clippy::too_many_arguments)] + pub(super) fn try_decoder_self_qkv_step_f32( + x: &[f32], + n_state: usize, + attn_ln_w: &[f32], + attn_ln_b: &[f32], + q_w_bytes: &[u8], + q_w_ggml_type: u32, + q_b: &[f32], + k_w_bytes: &[u8], + k_w_ggml_type: u32, + v_w_bytes: &[u8], + v_w_ggml_type: u32, + v_b: &[f32], + ) -> Option<(Vec, Vec, Vec)> { + with_context(|ctx| { + ctx.decoder_self_qkv_step_f32( + x, + n_state, + attn_ln_w, + attn_ln_b, + q_w_bytes, + q_w_ggml_type, + q_b, + k_w_bytes, + k_w_ggml_type, + v_w_bytes, + v_w_ggml_type, + v_b, + ) + }) + } +} diff --git a/libs/ggml/src/backend/metal/compiled.rs b/libs/ggml/src/backend/metal/compiled.rs new file mode 100644 index 0000000000..a480e70d82 --- /dev/null +++ b/libs/ggml/src/backend/metal/compiled.rs @@ -0,0 +1,5502 @@ +use std::collections::BTreeMap; + +use crate::context::Context; +use crate::core::{ggml_pad, GGML_MEM_ALIGN}; +use crate::graph::{Graph, NodeId}; +use crate::op::Op; +use crate::tensor::{ + ggml_blck_size_for_type, ggml_type_size_for_type, BufferUsage, Tensor, TensorId, TensorType, +}; + +use super::selector::FC_SSM_CONV; +use super::{ + build_graph_plan, BufferStorageMode, FunctionConstantValue, MetalBufferBindingRef, + MetalDeviceFeatures, MetalGraphNodePlan, MetalPipeline, MetalRuntime, MetalSize, + MetalStageKind, +}; + +#[repr(C)] +#[derive(Copy, Clone)] +struct KArgsMulMm { + ne00: i32, + ne02: i32, + nb01: u64, + nb02: u64, + nb03: u64, + ne12: i32, + nb10: u64, + nb11: u64, + nb12: u64, + nb13: u64, + ne0: i32, + ne1: i32, + r2: i16, + r3: i16, +} + +#[repr(C)] +#[derive(Copy, Clone)] +struct KArgsMulMv { + ne00: i32, + ne01: i32, + ne02: i32, + nb00: u64, + nb01: u64, + nb02: u64, + nb03: u64, + ne10: i32, + ne11: i32, + ne12: i32, + nb10: u64, + nb11: u64, + nb12: u64, + nb13: u64, + ne0: i32, + ne1: i32, + nr0: i32, + r2: i16, + r3: i16, +} + +#[repr(C)] +#[derive(Copy, Clone)] +struct KArgsMulMvExt { + ne00: i32, + ne01: i32, + ne02: i32, + nb00: u64, + nb01: u64, + nb02: u64, + nb03: u64, + ne10: i32, + ne11: i32, + ne12: i32, + nb10: u64, + nb11: u64, + nb12: u64, + nb13: u64, + ne0: i32, + ne1: i32, + r2: i16, + r3: i16, +} + +#[repr(C)] +#[derive(Copy, Clone)] +struct KArgsCpy { + nk0: i64, + ne00: i64, + ne01: i64, + ne02: i64, + ne03: i64, + nb00: u64, + nb01: u64, + nb02: u64, + nb03: u64, + ne0: i64, + ne1: i64, + ne2: i64, + ne3: i64, + nb0: u64, + nb1: u64, + nb2: u64, + nb3: u64, +} + +#[repr(C)] +#[derive(Copy, Clone)] +struct KArgsBin { + ne00: i32, + ne01: i32, + ne02: i32, + ne03: i32, + nb00: u64, + nb01: u64, + nb02: u64, + nb03: u64, + ne10: i32, + ne11: i32, + ne12: i32, + ne13: i32, + nb10: u64, + nb11: u64, + nb12: u64, + nb13: u64, + ne0: i32, + ne1: i32, + ne2: i32, + ne3: i32, + nb0: u64, + nb1: u64, + nb2: u64, + nb3: u64, + offs: u64, + o1: [u64; 8], +} + +#[repr(C)] +#[derive(Copy, Clone)] +struct KArgsAddId { + ne0: i64, + ne1: i64, + nb01: u64, + nb02: u64, + nb11: u64, + nb21: u64, +} + +#[repr(C)] +#[derive(Copy, Clone)] +struct KArgsConcat { + ne00: i32, + ne01: i32, + ne02: i32, + ne03: i32, + nb00: u64, + nb01: u64, + nb02: u64, + nb03: u64, + ne10: i32, + ne11: i32, + ne12: i32, + ne13: i32, + nb10: u64, + nb11: u64, + nb12: u64, + nb13: u64, + ne0: i32, + ne1: i32, + ne2: i32, + ne3: i32, + nb0: u64, + nb1: u64, + nb2: u64, + nb3: u64, + dim: i32, +} + +#[repr(C)] +#[derive(Copy, Clone)] +struct KArgsUnary { + ne00: i32, + ne01: i32, + ne02: i32, + ne03: i32, + nb00: u64, + nb01: u64, + nb02: u64, + nb03: u64, + ne0: i32, + ne1: i32, + ne2: i32, + ne3: i32, + nb0: u64, + nb1: u64, + nb2: u64, + nb3: u64, + slope: f32, + scale: f32, + bias: f32, + val: f32, + min: f32, + max: f32, +} + +#[repr(C)] +#[derive(Copy, Clone)] +struct KArgsSumRows { + ne00: i64, + ne01: i64, + ne02: i64, + ne03: i64, + nb00: u64, + nb01: u64, + nb02: u64, + nb03: u64, + ne0: i64, + ne1: i64, + ne2: i64, + ne3: i64, + nb0: u64, + nb1: u64, + nb2: u64, + nb3: u64, +} + +#[repr(C)] +#[derive(Copy, Clone)] +struct KArgsSoftMax { + ne00: i32, + ne01: i32, + ne02: i32, + nb01: u64, + nb02: u64, + nb03: u64, + ne11: i32, + ne12: i32, + ne13: i32, + nb11: u64, + nb12: u64, + nb13: u64, + nb1: u64, + nb2: u64, + nb3: u64, + scale: f32, + max_bias: f32, + m0: f32, + m1: f32, + n_head_log2: i32, +} + +#[repr(C)] +#[derive(Copy, Clone)] +struct KArgsL2Norm { + ne00: i32, + ne01: i32, + ne02: i32, + ne03: i32, + nb00: u64, + nb01: u64, + nb02: u64, + nb03: u64, + ne0: i32, + ne1: i32, + ne2: i32, + ne3: i32, + nb0: u64, + nb1: u64, + nb2: u64, + nb3: u64, + eps: f32, +} + +#[repr(C)] +#[derive(Copy, Clone)] +struct KArgsNorm { + ne00: i32, + ne00_t: i32, + nb1: u64, + nb2: u64, + nb3: u64, + eps: f32, + nef1: [i32; 3], + nef2: [i32; 3], + nef3: [i32; 3], + nbf1: [u64; 3], + nbf2: [u64; 3], + nbf3: [u64; 3], +} + +#[repr(C)] +#[derive(Copy, Clone)] +struct KArgsGetRows { + ne00t: i32, + ne00: i32, + nb01: u64, + nb02: u64, + nb03: u64, + ne10: i32, + nb10: u64, + nb11: u64, + nb12: u64, + nb1: u64, + nb2: u64, + nb3: u64, +} + +#[repr(C)] +#[derive(Copy, Clone)] +struct KArgsSetRows { + nk0: i32, + ne01: i32, + nb01: u64, + nb02: u64, + nb03: u64, + ne11: i32, + ne12: i32, + nb10: u64, + nb11: u64, + nb12: u64, + nb1: u64, + nb2: u64, + nb3: u64, +} + +#[repr(C)] +#[derive(Copy, Clone)] +struct KArgsRope { + ne00: i32, + ne01: i32, + ne02: i32, + ne03: i32, + nb00: u64, + nb01: u64, + nb02: u64, + nb03: u64, + ne0: i32, + ne1: i32, + ne2: i32, + ne3: i32, + nb0: u64, + nb1: u64, + nb2: u64, + nb3: u64, + n_past: i32, + n_dims: i32, + n_ctx_orig: i32, + freq_base: f32, + freq_scale: f32, + ext_factor: f32, + attn_factor: f32, + beta_fast: f32, + beta_slow: f32, + sect_0: i32, + sect_1: i32, + sect_2: i32, + sect_3: i32, + src2: bool, +} + +#[repr(C)] +#[derive(Copy, Clone)] +struct KArgsFlashAttnExtPad { + ne11: i32, + ne_12_2: i32, + ne_12_3: i32, + nb11: u64, + nb12: u64, + nb13: u64, + nb21: u64, + nb22: u64, + nb23: u64, + ne31: i32, + ne32: i32, + ne33: i32, + nb31: u64, + nb32: u64, + nb33: u64, +} + +#[repr(C)] +#[derive(Copy, Clone)] +struct KArgsFlashAttnExtVec { + ne01: i32, + ne02: i32, + ne03: i32, + nb01: u64, + nb02: u64, + nb03: u64, + ne11: i32, + ne_12_2: i32, + ne_12_3: i32, + ns10: i32, + nb11: u64, + nb12: u64, + nb13: u64, + ns20: i32, + nb21: u64, + nb22: u64, + nb23: u64, + ne31: i32, + ne32: i32, + ne33: i32, + nb31: u64, + nb32: u64, + nb33: u64, + ne1: i32, + ne2: i32, + ne3: i32, + scale: f32, + max_bias: f32, + m0: f32, + m1: f32, + n_head_log2: i32, + logit_softcap: f32, +} + +#[repr(C)] +#[derive(Copy, Clone)] +struct KArgsFlashAttnExtVecReduce { + nrows: i32, +} + +#[repr(C)] +#[derive(Copy, Clone)] +struct KArgsSsmConv { + ne00: i64, + ne01: i64, + ne02: i64, + nb00: u64, + nb01: u64, + nb02: u64, + ne10: i64, + ne11: i64, + nb10: u64, + nb11: u64, + ne0: i64, + ne1: i64, + ne2: i64, + nb0: u64, + nb1: u64, + nb2: u64, +} + +#[repr(C)] +#[derive(Copy, Clone)] +struct KArgsMulMmIdMap0 { + ne02: i32, + ne10: i32, + ne11: i32, + nb11: u64, + nb12: u64, + ne21: i32, + ne20: i32, + nb21: u64, +} + +#[repr(C)] +#[derive(Copy, Clone)] +struct KArgsMulMmId { + ne00: i32, + ne02: i32, + nb01: u64, + nb02: u64, + nb03: u64, + ne11: i32, + nb10: u64, + nb11: u64, + nb12: u64, + nb13: u64, + ne20: i32, + ne21: i32, + ne0: i32, + ne1: i32, + r2: i16, + r3: i16, +} + +#[repr(C)] +#[derive(Copy, Clone)] +struct KArgsMulMvId { + nei0: i32, + nei1: i32, + nbi1: u64, + ne00: i32, + ne01: i32, + ne02: i32, + nb00: u64, + nb01: u64, + nb02: u64, + ne10: i32, + ne11: i32, + ne12: i32, + ne13: i32, + nb10: u64, + nb11: u64, + nb12: u64, + ne0: i32, + ne1: i32, + nb1: u64, + nr0: i32, +} + +#[repr(C)] +#[derive(Copy, Clone)] +struct KArgsGatedDeltaNet { + ne00: i32, + ne01: i32, + ne02: i32, + ne03: i32, + nb00: u64, + nb01: u64, + nb02: u64, + nb03: u64, + ne10: i32, + ne11: i32, + ne12: i32, + ne13: i32, + nb10: u64, + nb11: u64, + nb12: u64, + nb13: u64, + ne20: i32, + ne21: i32, + ne22: i32, + ne23: i32, + nb20: u64, + nb21: u64, + nb22: u64, + nb23: u64, + ns02: i32, + ns12: i32, + ns22: i32, + ne0: i32, + ne1: i32, + ne2: i32, + ne3: i32, + nb0: u64, + nb1: u64, + nb2: u64, + nb3: u64, +} + +#[repr(C)] +#[derive(Copy, Clone)] +struct KArgsArgsort { + ne00: i64, + ne01: i64, + ne02: i64, + ne03: i64, + nb00: u64, + nb01: u64, + nb02: u64, + nb03: u64, + ne0: i32, + ne1: i32, + ne2: i32, + ne3: i32, + top_k: i32, +} + +#[repr(C)] +#[derive(Copy, Clone)] +struct KArgsArgsortMerge { + ne00: i64, + ne01: i64, + ne02: i64, + ne03: i64, + nb00: u64, + nb01: u64, + nb02: u64, + nb03: u64, + ne0: i32, + ne1: i32, + ne2: i32, + ne3: i32, + top_k: i32, + len: i32, +} + +const FC_FLASH_ATTN_EXT_PAD: i32 = 100; +const OP_FLASH_ATTN_EXT_VEC_NQPSG: i32 = 1; +const OP_FLASH_ATTN_EXT_VEC_NCPSG: i32 = 32; + +#[derive(Clone, Copy, Debug)] +pub struct MetalGraphTensorWrite<'a> { + pub tensor_id: TensorId, + pub bytes: &'a [u8], +} + +#[derive(Clone, Debug, Eq, PartialEq)] +pub struct MetalTensorBinding { + pub tensor_id: TensorId, + pub usage: BufferUsage, + pub offset_bytes: usize, + pub size_bytes: usize, + pub is_view: bool, +} + +#[derive(Clone, Debug, Eq, PartialEq)] +pub struct MetalPreparedStage { + pub kind: MetalStageKind, + pub descriptor: super::MetalPipelineDescriptor, + pub c4: bool, + pub cnt: bool, +} + +#[derive(Clone, Debug, Eq, PartialEq)] +pub struct MetalPreparedNode { + pub node_id: NodeId, + pub tail_offset_bytes: usize, + pub stages: Vec, +} + +#[derive(Clone, Debug, Eq, PartialEq)] +pub struct MetalPreparedGraph { + pub features: MetalDeviceFeatures, + pub nodes: Vec, + pub bindings: BTreeMap, + pub main_buffer_size: usize, + pub tail_buffer_size: usize, +} + +#[derive(Clone, Debug)] +pub struct MetalCompiledStage { + pub kind: MetalStageKind, + pub descriptor: super::MetalPipelineDescriptor, + pub pipeline: MetalPipeline, + pub c4: bool, + pub cnt: bool, +} + +#[derive(Clone, Debug)] +pub struct MetalCompiledNode { + pub node_id: NodeId, + pub tail_offset_bytes: usize, + pub stages: Vec, +} + +#[derive(Clone, Debug)] +pub struct MetalCompiledGraph { + pub features: MetalDeviceFeatures, + pub nodes: Vec, + pub bindings: BTreeMap, + pub main_buffer_size: usize, + pub tail_buffer_size: usize, + pub main_buffer: super::MetalBuffer, + pub tail_buffer: Option, +} + +pub struct MetalGraphSession { + runtime: MetalRuntime, + compiled: MetalCompiledGraph, +} + +#[derive(Clone, Debug, Default, Eq, PartialEq)] +pub struct MetalGraphExecution { + pub outputs: BTreeMap>, +} + +pub fn prepare_graph( + ctx: &Context, + graph: &Graph, + features: MetalDeviceFeatures, +) -> Result { + let graph_plan = build_graph_plan(ctx, graph, features)?; + let bindings = collect_tensor_bindings(ctx, graph)?; + let main_buffer_size = ggml_pad(ctx.used_mem(), GGML_MEM_ALIGN); + let mut tail_cursor = 0usize; + let mut nodes = Vec::with_capacity(graph_plan.nodes.len()); + for plan in &graph_plan.nodes { + let tail_offset_bytes = if plan.program.resources.output_tail_bytes == 0 { + 0 + } else { + ggml_pad(tail_cursor, GGML_MEM_ALIGN) + }; + tail_cursor = tail_offset_bytes + .checked_add(plan.program.resources.output_tail_bytes) + .ok_or_else(|| "Metal graph tail offset overflow".to_string())?; + nodes.push(prepared_node_from_plan(plan, tail_offset_bytes)); + } + let tail_buffer_size = if graph_plan.total_output_tail_bytes == 0 { + ggml_pad(tail_cursor, GGML_MEM_ALIGN) + } else { + ggml_pad( + std::cmp::max(tail_cursor, graph_plan.total_output_tail_bytes), + GGML_MEM_ALIGN, + ) + }; + + Ok(MetalPreparedGraph { + features, + nodes, + bindings, + main_buffer_size, + tail_buffer_size, + }) +} + +pub fn compile_prepared_graph( + runtime: &MetalRuntime, + ctx: &Context, + prepared: &MetalPreparedGraph, + main_storage: BufferStorageMode, + tail_storage: BufferStorageMode, +) -> Result { + let main_bytes = collect_main_buffer_bytes(ctx, prepared.main_buffer_size)?; + let main_buffer = runtime.create_buffer_with_bytes(&main_bytes, main_storage)?; + let tail_buffer = if prepared.tail_buffer_size > 0 { + Some(runtime.create_buffer(prepared.tail_buffer_size, tail_storage)?) + } else { + None + }; + + let mut nodes = Vec::with_capacity(prepared.nodes.len()); + for node in &prepared.nodes { + let mut stages = Vec::with_capacity(node.stages.len()); + for stage in &node.stages { + let pipeline = runtime.get_or_compile_pipeline(&stage.descriptor)?; + stages.push(MetalCompiledStage { + kind: stage.kind, + descriptor: stage.descriptor.clone(), + pipeline, + c4: stage.c4, + cnt: stage.cnt, + }); + } + nodes.push(MetalCompiledNode { + node_id: node.node_id, + tail_offset_bytes: node.tail_offset_bytes, + stages, + }); + } + + Ok(MetalCompiledGraph { + features: prepared.features, + nodes, + bindings: prepared.bindings.clone(), + main_buffer_size: prepared.main_buffer_size, + tail_buffer_size: prepared.tail_buffer_size, + main_buffer, + tail_buffer, + }) +} + +pub fn compile_graph_session( + ctx: &Context, + prepared: &MetalPreparedGraph, + main_storage: BufferStorageMode, + tail_storage: BufferStorageMode, +) -> Result { + let runtime = MetalRuntime::new()?; + MetalGraphSession::from_runtime(runtime, ctx, prepared, main_storage, tail_storage) +} + +impl MetalGraphSession { + pub fn from_runtime( + runtime: MetalRuntime, + ctx: &Context, + prepared: &MetalPreparedGraph, + main_storage: BufferStorageMode, + tail_storage: BufferStorageMode, + ) -> Result { + let compiled = compile_prepared_graph(&runtime, ctx, prepared, main_storage, tail_storage)?; + Ok(Self { runtime, compiled }) + } + + pub fn runtime(&self) -> &MetalRuntime { + &self.runtime + } + + pub fn compiled(&self) -> &MetalCompiledGraph { + &self.compiled + } + + pub fn execute( + &self, + ctx: &Context, + inputs: &[MetalGraphTensorWrite<'_>], + outputs: &[TensorId], + ) -> Result { + execute_compiled_graph(&self.runtime, ctx, &self.compiled, inputs, outputs) + } +} + +pub fn execute_compiled_graph( + runtime: &MetalRuntime, + ctx: &Context, + compiled: &MetalCompiledGraph, + inputs: &[MetalGraphTensorWrite<'_>], + outputs: &[TensorId], +) -> Result { + for input in inputs { + let binding = binding(compiled, input.tensor_id)?; + let tensor = ctx + .tensor(input.tensor_id) + .ok_or_else(|| format!("input references invalid tensor {}", input.tensor_id))?; + if input.bytes.len() != tensor.nbytes() { + return Err(format!( + "input '{}' byte length mismatch: got {}, expected {}", + tensor.name().unwrap_or(""), + input.bytes.len(), + tensor.nbytes() + )); + } + runtime.write_buffer(&compiled.main_buffer, binding.offset_bytes, input.bytes)?; + } + + for node in &compiled.nodes { + let tensor = ctx + .tensor(node.node_id) + .ok_or_else(|| format!("compiled graph references invalid tensor {}", node.node_id))?; + execute_node(runtime, ctx, compiled, tensor, node)?; + } + + let mut execution = MetalGraphExecution::default(); + for &tensor_id in outputs { + let binding = binding(compiled, tensor_id)?; + execution.outputs.insert( + tensor_id, + runtime.read_buffer_range( + &compiled.main_buffer, + binding.offset_bytes, + binding.size_bytes, + )?, + ); + } + Ok(execution) +} + +fn prepared_node_from_plan( + plan: &MetalGraphNodePlan, + tail_offset_bytes: usize, +) -> MetalPreparedNode { + MetalPreparedNode { + node_id: plan.node_id, + tail_offset_bytes, + stages: plan + .program + .stages + .iter() + .map(|stage| MetalPreparedStage { + kind: stage.kind, + descriptor: stage.descriptor.clone(), + c4: stage.c4, + cnt: stage.cnt, + }) + .collect(), + } +} + +fn collect_tensor_bindings( + ctx: &Context, + graph: &Graph, +) -> Result, String> { + let tensors = ctx.tensors(); + let mut needed = graph.nodes.clone(); + needed.extend(graph.leafs.iter().copied()); + needed.sort_unstable(); + needed.dedup(); + + let mut bindings = BTreeMap::new(); + for tensor_id in needed { + let tensor = tensors + .get(tensor_id) + .ok_or_else(|| format!("graph references invalid tensor {}", tensor_id))?; + let offset_bytes = resolve_tensor_offset(tensors, tensor)?; + bindings.insert( + tensor_id, + MetalTensorBinding { + tensor_id, + usage: tensor.desc.usage, + offset_bytes, + size_bytes: tensor.nbytes(), + is_view: tensor.is_view(), + }, + ); + } + Ok(bindings) +} + +fn resolve_tensor_offset(tensors: &[Tensor], tensor: &Tensor) -> Result { + if tensor.is_view() { + let src_id = tensor + .view_src + .ok_or_else(|| format!("view tensor {} is missing view source", tensor.id))?; + let src = tensors.get(src_id).ok_or_else(|| { + format!( + "tensor {} references invalid view source {}", + tensor.id, src_id + ) + })?; + let src_offset = resolve_tensor_offset(tensors, src)?; + return src_offset + .checked_add(tensor.view_offs) + .ok_or_else(|| format!("tensor {} view offset overflow", tensor.id)); + } + + tensor + .data_offset + .ok_or_else(|| format!("tensor {} has no allocated data offset", tensor.id)) +} + +fn collect_main_buffer_bytes(ctx: &Context, len: usize) -> Result, String> { + let src = ctx.mem_buffer(); + let used = ctx.used_mem(); + if used > len { + return Err(format!( + "context memory image ({}) exceeds prepared main buffer size ({})", + used, len + )); + } + let mut bytes = vec![0u8; len.max(1)]; + bytes[..used].copy_from_slice(&src[..used]); + Ok(bytes) +} + +fn execute_node( + runtime: &MetalRuntime, + ctx: &Context, + compiled: &MetalCompiledGraph, + tensor: &Tensor, + node: &MetalCompiledNode, +) -> Result<(), String> { + match tensor.op { + Op::Concat => dispatch_concat(runtime, ctx, compiled, tensor, node), + Op::AddId => dispatch_add_id(runtime, ctx, compiled, tensor, node), + Op::GetRows => dispatch_get_rows(runtime, ctx, compiled, tensor, node), + Op::SetRows => dispatch_set_rows(runtime, ctx, compiled, tensor, node), + Op::Add | Op::Sub | Op::Mul | Op::Div => dispatch_bin(runtime, ctx, compiled, tensor, node), + Op::Scale + | Op::Fill + | Op::Clamp + | Op::LeakyRelu + | Op::Sqr + | Op::Sqrt + | Op::Sin + | Op::Cos + | Op::Log + | Op::Unary => dispatch_unary(runtime, ctx, compiled, tensor, node), + Op::SumRows | Op::Mean => dispatch_sum_rows(runtime, ctx, compiled, tensor, node), + Op::SoftMax => dispatch_soft_max(runtime, ctx, compiled, tensor, node), + Op::Norm => dispatch_norm(runtime, ctx, compiled, tensor, node, false), + Op::RmsNorm => dispatch_norm(runtime, ctx, compiled, tensor, node, true), + Op::L2Norm => dispatch_l2_norm(runtime, ctx, compiled, tensor, node), + Op::Rope => dispatch_rope(runtime, ctx, compiled, tensor, node), + Op::FlashAttnExt => dispatch_flash_attn_ext(runtime, ctx, compiled, tensor, node), + Op::SsmConv => dispatch_ssm_conv(runtime, ctx, compiled, tensor, node), + Op::GatedDeltaNet => dispatch_gated_delta_net(runtime, ctx, compiled, tensor, node), + Op::MulMat => dispatch_mul_mat(runtime, ctx, compiled, tensor, node), + Op::MulMatId => dispatch_mul_mat_id(runtime, ctx, compiled, tensor, node), + Op::Argsort => dispatch_argsort_like(runtime, ctx, compiled, tensor, node, false), + Op::TopK => dispatch_argsort_like(runtime, ctx, compiled, tensor, node, true), + Op::Dup | Op::Cpy | Op::Cont => dispatch_cpy(runtime, ctx, compiled, tensor, node), + Op::Set => dispatch_set(runtime, ctx, compiled, tensor, node), + other => Err(format!( + "Metal compiled executor does not support ggml op {} yet", + other.name() + )), + } +} + +fn dispatch_concat( + runtime: &MetalRuntime, + ctx: &Context, + compiled: &MetalCompiledGraph, + tensor: &Tensor, + node: &MetalCompiledNode, +) -> Result<(), String> { + let stage = main_stage(node, tensor.op)?; + let src0_id = tensor_src(tensor, 0)?; + let src1_id = tensor_src(tensor, 1)?; + let src0 = ctx + .tensor(src0_id) + .ok_or_else(|| format!("concat src0 {} is invalid", src0_id))?; + let src1 = ctx + .tensor(src1_id) + .ok_or_else(|| format!("concat src1 {} is invalid", src1_id))?; + let args = KArgsConcat { + ne00: i32_dim(src0, 0)?, + ne01: i32_dim(src0, 1)?, + ne02: i32_dim(src0, 2)?, + ne03: i32_dim(src0, 3)?, + nb00: u64::try_from(src0.nb[0]).map_err(|_| "concat nb00 exceeds u64".to_string())?, + nb01: u64::try_from(src0.nb[1]).map_err(|_| "concat nb01 exceeds u64".to_string())?, + nb02: u64::try_from(src0.nb[2]).map_err(|_| "concat nb02 exceeds u64".to_string())?, + nb03: u64::try_from(src0.nb[3]).map_err(|_| "concat nb03 exceeds u64".to_string())?, + ne10: i32_dim(src1, 0)?, + ne11: i32_dim(src1, 1)?, + ne12: i32_dim(src1, 2)?, + ne13: i32_dim(src1, 3)?, + nb10: u64::try_from(src1.nb[0]).map_err(|_| "concat nb10 exceeds u64".to_string())?, + nb11: u64::try_from(src1.nb[1]).map_err(|_| "concat nb11 exceeds u64".to_string())?, + nb12: u64::try_from(src1.nb[2]).map_err(|_| "concat nb12 exceeds u64".to_string())?, + nb13: u64::try_from(src1.nb[3]).map_err(|_| "concat nb13 exceeds u64".to_string())?, + ne0: i32_dim(tensor, 0)?, + ne1: i32_dim(tensor, 1)?, + ne2: i32_dim(tensor, 2)?, + ne3: i32_dim(tensor, 3)?, + nb0: u64::try_from(tensor.nb[0]).map_err(|_| "concat nb0 exceeds u64".to_string())?, + nb1: u64::try_from(tensor.nb[1]).map_err(|_| "concat nb1 exceeds u64".to_string())?, + nb2: u64::try_from(tensor.nb[2]).map_err(|_| "concat nb2 exceeds u64".to_string())?, + nb3: u64::try_from(tensor.nb[3]).map_err(|_| "concat nb3 exceeds u64".to_string())?, + dim: tensor.op_param_i32(0), + }; + + let nth = std::cmp::min(1024_u64, tensor.ne[0].max(1) as u64); + runtime.dispatch_compute( + &stage.pipeline, + bytes_of(&args), + &[ + buffer_ref(compiled, 1, src0_id), + buffer_ref(compiled, 2, src1_id), + buffer_ref(compiled, 3, tensor.id), + ], + &[], + MetalSize { + width: tensor.ne[1].max(1) as u64, + height: tensor.ne[2].max(1) as u64, + depth: tensor.ne[3].max(1) as u64, + }, + MetalSize { + width: nth, + height: 1, + depth: 1, + }, + ) +} + +fn dispatch_add_id( + runtime: &MetalRuntime, + ctx: &Context, + compiled: &MetalCompiledGraph, + tensor: &Tensor, + node: &MetalCompiledNode, +) -> Result<(), String> { + let stage = main_stage(node, tensor.op)?; + let src0_id = tensor_src(tensor, 0)?; + let src1_id = tensor_src(tensor, 1)?; + let ids_id = tensor_src(tensor, 2)?; + let src0 = ctx + .tensor(src0_id) + .ok_or_else(|| format!("add_id src0 {} is invalid", src0_id))?; + let src1 = ctx + .tensor(src1_id) + .ok_or_else(|| format!("add_id src1 {} is invalid", src1_id))?; + let ids = ctx + .tensor(ids_id) + .ok_or_else(|| format!("add_id ids {} is invalid", ids_id))?; + let args = KArgsAddId { + ne0: i64_dim(src0, 0)?, + ne1: i64_dim(src0, 1)?, + nb01: u64::try_from(src0.nb[1]).map_err(|_| "add_id nb01 exceeds u64".to_string())?, + nb02: u64::try_from(src0.nb[2]).map_err(|_| "add_id nb02 exceeds u64".to_string())?, + nb11: u64::try_from(src1.nb[1]).map_err(|_| "add_id nb11 exceeds u64".to_string())?, + nb21: u64::try_from(ids.nb[1]).map_err(|_| "add_id nb21 exceeds u64".to_string())?, + }; + let nth = std::cmp::min( + stage.pipeline.max_threads_per_threadgroup.max(1), + src0.ne[0].max(1) as u64, + ); + + runtime.dispatch_compute( + &stage.pipeline, + bytes_of(&args), + &[ + buffer_ref(compiled, 1, src0_id), + buffer_ref(compiled, 2, src1_id), + buffer_ref(compiled, 3, ids_id), + buffer_ref(compiled, 4, tensor.id), + ], + &[], + MetalSize { + width: src0.ne[1].max(1) as u64, + height: src0.ne[2].max(1) as u64, + depth: 1, + }, + MetalSize { + width: nth, + height: 1, + depth: 1, + }, + ) +} + +fn dispatch_get_rows( + runtime: &MetalRuntime, + ctx: &Context, + compiled: &MetalCompiledGraph, + tensor: &Tensor, + node: &MetalCompiledNode, +) -> Result<(), String> { + let stage = main_stage(node, tensor.op)?; + let src0_id = tensor_src(tensor, 0)?; + let src1_id = tensor_src(tensor, 1)?; + let src0 = ctx + .tensor(src0_id) + .ok_or_else(|| format!("get_rows src0 {} is invalid", src0_id))?; + let src1 = ctx + .tensor(src1_id) + .ok_or_else(|| format!("get_rows src1 {} is invalid", src1_id))?; + let src0_shape = shape4(src0)?; + let src1_shape = shape4(src1)?; + let dst_shape = shape4(tensor)?; + + let is_quantized = !matches!( + src0.desc.ty, + TensorType::F32 | TensorType::F16 | TensorType::BF16 | TensorType::I32 + ); + let ne00t = if is_quantized { + src0_shape.ne[0] / 16 + } else { + src0_shape.ne[0] + }; + let args = KArgsGetRows { + ne00t, + ne00: src0_shape.ne[0], + nb01: src0_shape.nb[1], + nb02: src0_shape.nb[2], + nb03: src0_shape.nb[3], + ne10: src1_shape.ne[0], + nb10: src1_shape.nb[0], + nb11: src1_shape.nb[1], + nb12: src1_shape.nb[2], + nb1: dst_shape.nb[1], + nb2: dst_shape.nb[2], + nb3: dst_shape.nb[3], + }; + + let nth = std::cmp::min( + args.ne00t.max(1) as u64, + stage.pipeline.max_threads_per_threadgroup.max(1), + ); + let nw0 = ((args.ne00t.max(1) as u64) + nth - 1) / nth; + + runtime.dispatch_compute( + &stage.pipeline, + bytes_of(&args), + &[ + buffer_ref(compiled, 1, src0_id), + buffer_ref(compiled, 2, src1_id), + buffer_ref(compiled, 3, tensor.id), + ], + &[], + MetalSize { + width: nw0 * (src1_shape.ne[0] as u64), + height: src1_shape.ne[1] as u64, + depth: src1_shape.ne[2] as u64, + }, + MetalSize { + width: nth, + height: 1, + depth: 1, + }, + ) +} + +fn dispatch_set_rows( + runtime: &MetalRuntime, + ctx: &Context, + compiled: &MetalCompiledGraph, + tensor: &Tensor, + node: &MetalCompiledNode, +) -> Result<(), String> { + let stage = main_stage(node, tensor.op)?; + let src0_id = tensor_src(tensor, 0)?; + let src1_id = tensor_src(tensor, 1)?; + let src0 = ctx + .tensor(src0_id) + .ok_or_else(|| format!("set_rows src0 {} is invalid", src0_id))?; + let src1 = ctx + .tensor(src1_id) + .ok_or_else(|| format!("set_rows src1 {} is invalid", src1_id))?; + + let nk0 = i32::try_from( + src0.ne[0] + / i64::try_from(ggml_blck_size_for_type(tensor.desc.ty)) + .map_err(|_| "set_rows block size exceeds i64".to_string())?, + ) + .map_err(|_| "set_rows nk0 exceeds i32".to_string())?; + let ne01 = i32_dim(src0, 1)?; + let ne02 = i32_dim(src0, 2)?; + let ne03 = i32_dim(src0, 3)?; + let ne11 = i32_dim(src1, 1)?; + let ne12 = i32_dim(src1, 2)?; + let args = KArgsSetRows { + nk0, + ne01, + nb01: u64::try_from(src0.nb[1]).map_err(|_| "set_rows nb01 exceeds u64".to_string())?, + nb02: u64::try_from(src0.nb[2]).map_err(|_| "set_rows nb02 exceeds u64".to_string())?, + nb03: u64::try_from(src0.nb[3]).map_err(|_| "set_rows nb03 exceeds u64".to_string())?, + ne11, + ne12, + nb10: u64::try_from(src1.nb[0]).map_err(|_| "set_rows nb10 exceeds u64".to_string())?, + nb11: u64::try_from(src1.nb[1]).map_err(|_| "set_rows nb11 exceeds u64".to_string())?, + nb12: u64::try_from(src1.nb[2]).map_err(|_| "set_rows nb12 exceeds u64".to_string())?, + nb1: u64::try_from(tensor.nb[1]).map_err(|_| "set_rows nb1 exceeds u64".to_string())?, + nb2: u64::try_from(tensor.nb[2]).map_err(|_| "set_rows nb2 exceeds u64".to_string())?, + nb3: u64::try_from(tensor.nb[3]).map_err(|_| "set_rows nb3 exceeds u64".to_string())?, + }; + + let max_threads = stage.pipeline.max_threads_per_threadgroup.max(1); + let mut nth = 32_u64; + while nth < (nk0.max(1) as u64) && nth < max_threads { + nth *= 2; + } + + let mut nrptg = 1_u64; + if nth > nk0.max(1) as u64 { + nrptg = nth.div_ceil(nk0.max(1) as u64); + nth = nk0.max(1) as u64; + if nrptg + .checked_mul(nth) + .ok_or_else(|| "set_rows threads per tg overflow".to_string())? + > max_threads + { + nrptg = nrptg.saturating_sub(1).max(1); + } + } + nth = nth.min(nk0.max(1) as u64); + + runtime.dispatch_compute( + &stage.pipeline, + bytes_of(&args), + &[ + buffer_ref(compiled, 1, src0_id), + buffer_ref(compiled, 2, src1_id), + buffer_ref(compiled, 3, tensor.id), + ], + &[], + MetalSize { + width: (ne01.max(1) as u64).div_ceil(nrptg), + height: ne02.max(1) as u64, + depth: ne03.max(1) as u64, + }, + MetalSize { + width: nth, + height: nrptg, + depth: 1, + }, + ) +} + +fn dispatch_bin( + runtime: &MetalRuntime, + ctx: &Context, + compiled: &MetalCompiledGraph, + tensor: &Tensor, + node: &MetalCompiledNode, +) -> Result<(), String> { + let stage = main_stage(node, tensor.op)?; + let src0_id = tensor_src(tensor, 0)?; + let src1_id = tensor_src(tensor, 1)?; + let src0 = ctx + .tensor(src0_id) + .ok_or_else(|| format!("binary src0 {} is invalid", src0_id))?; + let src1 = ctx + .tensor(src1_id) + .ok_or_else(|| format!("binary src1 {} is invalid", src1_id))?; + let src0_shape = shape4(src0)?; + let src1_shape = shape4(src1)?; + + for d in 0..4 { + let b = src1_shape.ne[d]; + let a = src0_shape.ne[d]; + if b != 1 && b != a { + return Err(format!( + "binary broadcast mismatch at dim {}: lhs={}, rhs={}", + d, a, b + )); + } + } + + let is_c4 = src0_shape.ne[0] % 4 == 0 && src1_shape.ne[0] % 4 == 0; + let is_rb = stage.cnt; + + let mut args = KArgsBin { + ne00: src0_shape.ne[0], + ne01: src0_shape.ne[1], + ne02: src0_shape.ne[2], + ne03: src0_shape.ne[3], + nb00: src0_shape.nb[0], + nb01: src0_shape.nb[1], + nb02: src0_shape.nb[2], + nb03: src0_shape.nb[3], + ne10: src1_shape.ne[0], + ne11: src1_shape.ne[1], + ne12: src1_shape.ne[2], + ne13: src1_shape.ne[3], + nb10: src1_shape.nb[0], + nb11: src1_shape.nb[1], + nb12: src1_shape.nb[2], + nb13: src1_shape.nb[3], + ne0: src0_shape.ne[0], + ne1: src0_shape.ne[1], + ne2: src0_shape.ne[2], + ne3: src0_shape.ne[3], + nb0: src0_shape.nb[0], + nb1: src0_shape.nb[1], + nb2: src0_shape.nb[2], + nb3: src0_shape.nb[3], + offs: 0, + o1: [0u64; 8], + }; + + if is_c4 { + args.ne00 /= 4; + args.ne10 /= 4; + args.ne0 /= 4; + } + + let (threadgroups, threads_per_threadgroup) = if is_rb { + ( + MetalSize { + width: args.ne0.max(1) as u64, + height: nrows(&src0_shape) as u64, + depth: 1, + }, + MetalSize { + width: 1, + height: 1, + depth: 1, + }, + ) + } else { + let nth_max = std::cmp::min(256u64, stage.pipeline.max_threads_per_threadgroup).max(1); + let mut nth = 1u64; + while 2 * nth < args.ne0 as u64 && nth < nth_max { + nth *= 2; + } + ( + MetalSize { + width: src0_shape.ne[1] as u64, + height: src0_shape.ne[2] as u64, + depth: src0_shape.ne[3] as u64, + }, + MetalSize { + width: nth, + height: 1, + depth: 1, + }, + ) + }; + + runtime.dispatch_compute( + &stage.pipeline, + bytes_of(&args), + &[ + buffer_ref(compiled, 1, src0_id), + buffer_ref(compiled, 2, src1_id), + buffer_ref(compiled, 3, tensor.id), + ], + &[], + threadgroups, + threads_per_threadgroup, + ) +} + +fn dispatch_unary( + runtime: &MetalRuntime, + ctx: &Context, + compiled: &MetalCompiledGraph, + tensor: &Tensor, + node: &MetalCompiledNode, +) -> Result<(), String> { + let stage = main_stage(node, tensor.op)?; + let src0_id = tensor_src(tensor, 0)?; + let src0 = ctx + .tensor(src0_id) + .ok_or_else(|| format!("unary src0 {} is invalid", src0_id))?; + let src0_shape = shape4(src0)?; + let dst_shape = shape4(tensor)?; + + let mut args = KArgsUnary { + ne00: src0_shape.ne[0], + ne01: src0_shape.ne[1], + ne02: src0_shape.ne[2], + ne03: src0_shape.ne[3], + nb00: src0_shape.nb[0], + nb01: src0_shape.nb[1], + nb02: src0_shape.nb[2], + nb03: src0_shape.nb[3], + ne0: dst_shape.ne[0], + ne1: dst_shape.ne[1], + ne2: dst_shape.ne[2], + ne3: dst_shape.ne[3], + nb0: dst_shape.nb[0], + nb1: dst_shape.nb[1], + nb2: dst_shape.nb[2], + nb3: dst_shape.nb[3], + slope: tensor.op_param_f32(0), + scale: tensor.op_param_f32(1), + bias: tensor.op_param_f32(2), + val: tensor.op_param_f32(3), + min: tensor.op_param_f32(4), + max: tensor.op_param_f32(5), + }; + + if stage.c4 { + args.ne00 /= 4; + args.ne0 /= 4; + } + + let (threadgroups, threads_per_threadgroup) = if stage.cnt { + let n = if stage.c4 { + src0_shape.numel / 4 + } else { + src0_shape.numel + }; + ( + MetalSize { + width: n as u64, + height: 1, + depth: 1, + }, + MetalSize { + width: 1, + height: 1, + depth: 1, + }, + ) + } else { + let nth_max = std::cmp::min(256u64, stage.pipeline.max_threads_per_threadgroup).max(1); + let mut nth = 1u64; + while 2 * nth < args.ne0 as u64 && nth < nth_max { + nth *= 2; + } + ( + MetalSize { + width: src0_shape.ne[1] as u64, + height: src0_shape.ne[2] as u64, + depth: src0_shape.ne[3] as u64, + }, + MetalSize { + width: nth, + height: 1, + depth: 1, + }, + ) + }; + + runtime.dispatch_compute( + &stage.pipeline, + bytes_of(&args), + &[ + buffer_ref(compiled, 1, src0_id), + buffer_ref(compiled, 2, tensor.id), + ], + &[], + threadgroups, + threads_per_threadgroup, + ) +} + +fn dispatch_soft_max( + runtime: &MetalRuntime, + ctx: &Context, + compiled: &MetalCompiledGraph, + tensor: &Tensor, + node: &MetalCompiledNode, +) -> Result<(), String> { + let stage = main_stage(node, tensor.op)?; + let src0_id = tensor_src(tensor, 0)?; + let src1_id = tensor_src_opt(tensor, 1); + let src2_id = tensor_src_opt(tensor, 2); + let src0 = ctx + .tensor(src0_id) + .ok_or_else(|| format!("soft_max src0 {} is invalid", src0_id))?; + let src1 = src1_id + .map(|id| { + ctx.tensor(id) + .ok_or_else(|| format!("soft_max src1 {} is invalid", id)) + }) + .transpose()?; + + let scale = tensor.op_param_f32(0); + let max_bias = tensor.op_param_f32(1); + let n_head = src0.ne[2].max(1) as u32; + let n_head_log2 = if n_head <= 1 { + 1_i32 + } else { + let p = (u32::BITS - 1) - n_head.leading_zeros(); + (1u32 << p) as i32 + }; + let m0 = if max_bias != 0.0 { + (2.0f32).powf(-(max_bias) / (n_head_log2 as f32)) + } else { + 1.0 + }; + let m1 = if max_bias != 0.0 { + (2.0f32).powf(-(max_bias / 2.0) / (n_head_log2 as f32)) + } else { + 1.0 + }; + + let src1_shape = src1.map(shape4).transpose()?.unwrap_or_default(); + let args = KArgsSoftMax { + ne00: i32_dim(src0, 0)?, + ne01: i32_dim(src0, 1)?, + ne02: i32_dim(src0, 2)?, + nb01: u64::try_from(src0.nb[1]).map_err(|_| "soft_max nb01 exceeds u64".to_string())?, + nb02: u64::try_from(src0.nb[2]).map_err(|_| "soft_max nb02 exceeds u64".to_string())?, + nb03: u64::try_from(src0.nb[3]).map_err(|_| "soft_max nb03 exceeds u64".to_string())?, + ne11: src1_shape.ne[1], + ne12: src1_shape.ne[2], + ne13: src1_shape.ne[3], + nb11: src1_shape.nb[1], + nb12: src1_shape.nb[2], + nb13: src1_shape.nb[3], + nb1: u64::try_from(tensor.nb[1]).map_err(|_| "soft_max nb1 exceeds u64".to_string())?, + nb2: u64::try_from(tensor.nb[2]).map_err(|_| "soft_max nb2 exceeds u64".to_string())?, + nb3: u64::try_from(tensor.nb[3]).map_err(|_| "soft_max nb3 exceeds u64".to_string())?, + scale, + max_bias, + m0, + m1, + n_head_log2, + }; + + let mut nth = 32_u64; + let limit = if stage.c4 { + (src0.ne[0] / 4).max(1) as u64 + } else { + src0.ne[0].max(1) as u64 + }; + while nth < limit + && nth + .checked_mul(src0.ne[1].max(1) as u64) + .and_then(|v| v.checked_mul(src0.ne[2].max(1) as u64)) + .and_then(|v| v.checked_mul(src0.ne[3].max(1) as u64)) + .unwrap_or(u64::MAX) + < 256 + { + nth *= 2; + } + nth = nth.min(stage.pipeline.max_threads_per_threadgroup.max(1)); + + runtime.dispatch_compute( + &stage.pipeline, + bytes_of(&args), + &[ + buffer_ref(compiled, 1, src0_id), + src1_id + .map(|id| buffer_ref(compiled, 2, id)) + .unwrap_or_else(|| buffer_ref(compiled, 2, src0_id)), + src2_id + .map(|id| buffer_ref(compiled, 3, id)) + .unwrap_or_else(|| buffer_ref(compiled, 3, src0_id)), + buffer_ref(compiled, 4, tensor.id), + ], + &[(0, stage.pipeline.smem_bytes)], + MetalSize { + width: src0.ne[1].max(1) as u64, + height: src0.ne[2].max(1) as u64, + depth: src0.ne[3].max(1) as u64, + }, + MetalSize { + width: nth.max(1), + height: 1, + depth: 1, + }, + ) +} + +fn dispatch_sum_rows( + runtime: &MetalRuntime, + ctx: &Context, + compiled: &MetalCompiledGraph, + tensor: &Tensor, + node: &MetalCompiledNode, +) -> Result<(), String> { + let stage = main_stage(node, tensor.op)?; + let src0_id = tensor_src(tensor, 0)?; + let src0 = ctx + .tensor(src0_id) + .ok_or_else(|| format!("sum_rows src0 {} is invalid", src0_id))?; + + let mut args = KArgsSumRows { + ne00: src0.ne[0], + ne01: src0.ne[1], + ne02: src0.ne[2], + ne03: src0.ne[3], + nb00: u64::try_from(src0.nb[0]).map_err(|_| "sum_rows nb00 exceeds u64".to_string())?, + nb01: u64::try_from(src0.nb[1]).map_err(|_| "sum_rows nb01 exceeds u64".to_string())?, + nb02: u64::try_from(src0.nb[2]).map_err(|_| "sum_rows nb02 exceeds u64".to_string())?, + nb03: u64::try_from(src0.nb[3]).map_err(|_| "sum_rows nb03 exceeds u64".to_string())?, + ne0: tensor.ne[0], + ne1: tensor.ne[1], + ne2: tensor.ne[2], + ne3: tensor.ne[3], + nb0: u64::try_from(tensor.nb[0]).map_err(|_| "sum_rows nb0 exceeds u64".to_string())?, + nb1: u64::try_from(tensor.nb[1]).map_err(|_| "sum_rows nb1 exceeds u64".to_string())?, + nb2: u64::try_from(tensor.nb[2]).map_err(|_| "sum_rows nb2 exceeds u64".to_string())?, + nb3: u64::try_from(tensor.nb[3]).map_err(|_| "sum_rows nb3 exceeds u64".to_string())?, + }; + + if stage.c4 { + args.ne00 /= 4; + args.ne0 /= 4; + } + + let max_threads = stage.pipeline.max_threads_per_threadgroup.max(1); + let mut nth = 32_u64; + while nth < (args.ne00.max(1) as u64) && nth < max_threads { + nth *= 2; + } + nth = nth.min(max_threads).min(args.ne00.max(1) as u64); + + runtime.dispatch_compute( + &stage.pipeline, + bytes_of(&args), + &[ + buffer_ref(compiled, 1, src0_id), + buffer_ref(compiled, 2, tensor.id), + ], + &[(0, stage.pipeline.smem_bytes)], + MetalSize { + width: src0.ne[1].max(1) as u64, + height: src0.ne[2].max(1) as u64, + depth: src0.ne[3].max(1) as u64, + }, + MetalSize { + width: nth, + height: 1, + depth: 1, + }, + ) +} + +fn dispatch_norm( + runtime: &MetalRuntime, + ctx: &Context, + compiled: &MetalCompiledGraph, + tensor: &Tensor, + node: &MetalCompiledNode, + rms: bool, +) -> Result<(), String> { + let stage = main_stage(node, tensor.op)?; + let src0_id = tensor_src(tensor, 0)?; + let src0 = ctx + .tensor(src0_id) + .ok_or_else(|| format!("norm src0 {} is invalid", src0_id))?; + let src0_shape = shape4(src0)?; + let empty_shape = Shape4::default(); + let is_c4 = src0_shape.ne[0] % 4 == 0; + let ne00_t = if is_c4 { + src0_shape.ne[0] / 4 + } else { + src0_shape.ne[0] + }; + let args = KArgsNorm { + ne00: src0_shape.ne[0], + ne00_t, + nb1: src0_shape.nb[1], + nb2: src0_shape.nb[2], + nb3: src0_shape.nb[3], + eps: tensor.op_param_f32(0), + nef1: [src0_shape.ne[1], empty_shape.ne[1], empty_shape.ne[1]], + nef2: [src0_shape.ne[2], empty_shape.ne[2], empty_shape.ne[2]], + nef3: [src0_shape.ne[3], empty_shape.ne[3], empty_shape.ne[3]], + nbf1: [src0_shape.nb[1], empty_shape.nb[1], empty_shape.nb[1]], + nbf2: [src0_shape.nb[2], empty_shape.nb[2], empty_shape.nb[2]], + nbf3: [src0_shape.nb[3], empty_shape.nb[3], empty_shape.nb[3]], + }; + + let mut nth = 32u64; + let nth_max = stage.pipeline.max_threads_per_threadgroup.max(1); + while nth < args.ne00_t as u64 && nth < nth_max { + nth *= 2; + } + nth = std::cmp::min(nth, nth_max); + nth = std::cmp::min(nth, args.ne00_t.max(1) as u64); + + let buffers = if rms || tensor.op == Op::RmsNorm || tensor.op == Op::Norm { + vec![ + buffer_ref(compiled, 1, src0_id), + dummy_buffer_ref(2, &compiled.main_buffer), + dummy_buffer_ref(3, &compiled.main_buffer), + buffer_ref(compiled, 4, tensor.id), + ] + } else { + unreachable!() + }; + + runtime.dispatch_compute( + &stage.pipeline, + bytes_of(&args), + &buffers, + &[(0, stage.pipeline.smem_bytes)], + MetalSize { + width: src0_shape.ne[1] as u64, + height: src0_shape.ne[2] as u64, + depth: src0_shape.ne[3] as u64, + }, + MetalSize { + width: nth, + height: 1, + depth: 1, + }, + ) +} + +fn dispatch_l2_norm( + runtime: &MetalRuntime, + ctx: &Context, + compiled: &MetalCompiledGraph, + tensor: &Tensor, + node: &MetalCompiledNode, +) -> Result<(), String> { + let stage = main_stage(node, tensor.op)?; + let src0_id = tensor_src(tensor, 0)?; + let src0 = ctx + .tensor(src0_id) + .ok_or_else(|| format!("l2_norm src0 {} is invalid", src0_id))?; + if !src0.is_contiguous_rows() { + return Err("l2_norm currently requires contiguous-row input".to_string()); + } + + let mut args = KArgsL2Norm { + ne00: i32_dim(src0, 0)?, + ne01: i32_dim(src0, 1)?, + ne02: i32_dim(src0, 2)?, + ne03: i32_dim(src0, 3)?, + nb00: u64::try_from(src0.nb[0]).map_err(|_| "l2_norm nb00 exceeds u64".to_string())?, + nb01: u64::try_from(src0.nb[1]).map_err(|_| "l2_norm nb01 exceeds u64".to_string())?, + nb02: u64::try_from(src0.nb[2]).map_err(|_| "l2_norm nb02 exceeds u64".to_string())?, + nb03: u64::try_from(src0.nb[3]).map_err(|_| "l2_norm nb03 exceeds u64".to_string())?, + ne0: i32_dim(tensor, 0)?, + ne1: i32_dim(tensor, 1)?, + ne2: i32_dim(tensor, 2)?, + ne3: i32_dim(tensor, 3)?, + nb0: u64::try_from(tensor.nb[0]).map_err(|_| "l2_norm nb0 exceeds u64".to_string())?, + nb1: u64::try_from(tensor.nb[1]).map_err(|_| "l2_norm nb1 exceeds u64".to_string())?, + nb2: u64::try_from(tensor.nb[2]).map_err(|_| "l2_norm nb2 exceeds u64".to_string())?, + nb3: u64::try_from(tensor.nb[3]).map_err(|_| "l2_norm nb3 exceeds u64".to_string())?, + eps: tensor.op_param_f32(0), + }; + if stage.c4 { + args.ne00 /= 4; + args.ne0 /= 4; + } + + let mut nth = 32_u64; + while nth < src0.ne[0].max(1) as u64 && nth < stage.pipeline.max_threads_per_threadgroup { + nth *= 2; + } + nth = nth.min(stage.pipeline.max_threads_per_threadgroup.max(1)); + + runtime.dispatch_compute( + &stage.pipeline, + bytes_of(&args), + &[ + buffer_ref(compiled, 1, src0_id), + buffer_ref(compiled, 2, tensor.id), + ], + &[(0, stage.pipeline.smem_bytes)], + MetalSize { + width: src0.ne[1].max(1) as u64, + height: src0.ne[2].max(1) as u64, + depth: src0.ne[3].max(1) as u64, + }, + MetalSize { + width: nth, + height: 1, + depth: 1, + }, + ) +} + +fn dispatch_mul_mat( + runtime: &MetalRuntime, + ctx: &Context, + compiled: &MetalCompiledGraph, + tensor: &Tensor, + node: &MetalCompiledNode, +) -> Result<(), String> { + let stage = main_stage(node, tensor.op)?; + let src0_id = tensor_src(tensor, 0)?; + let src1_id = tensor_src(tensor, 1)?; + let src0 = ctx + .tensor(src0_id) + .ok_or_else(|| format!("mul_mat src0 {} is invalid", src0_id))?; + let src1 = ctx + .tensor(src1_id) + .ok_or_else(|| format!("mul_mat src1 {} is invalid", src1_id))?; + let base = stage.descriptor.base_name.as_str(); + + if base.starts_with("kernel_mul_mm_") { + let ne00 = i32_dim(src0, 0)?; + let ne01 = i32_dim(src0, 1)?; + let ne12 = i32_dim(src1, 1)?; + let ne0 = i32_dim(tensor, 0)?; + let ne1 = i32_dim(tensor, 1)?; + let args = KArgsMulMm { + ne00, + ne02: 1, + nb01: u64::try_from(src0.nb[1]).map_err(|_| "mul_mm nb01 exceeds u64".to_string())?, + nb02: u64::try_from(src0.nb[1]).map_err(|_| "mul_mm nb02 exceeds u64".to_string())? + * ne01 as u64, + nb03: u64::try_from(src0.nb[1]).map_err(|_| "mul_mm nb03 exceeds u64".to_string())? + * ne01 as u64, + ne12, + nb10: u64::try_from(src1.nb[0]).map_err(|_| "mul_mm nb10 exceeds u64".to_string())?, + nb11: u64::try_from(src1.nb[1]).map_err(|_| "mul_mm nb11 exceeds u64".to_string())?, + nb12: u64::try_from(src1.nb[1]).map_err(|_| "mul_mm nb12 exceeds u64".to_string())? + * ne1 as u64, + nb13: u64::try_from(src1.nb[1]).map_err(|_| "mul_mm nb13 exceeds u64".to_string())? + * ne1 as u64, + ne0, + ne1, + r2: 1, + r3: 1, + }; + return runtime.dispatch_compute( + &stage.pipeline, + bytes_of(&args), + &[ + buffer_ref(compiled, 1, src0_id), + buffer_ref(compiled, 2, src1_id), + buffer_ref(compiled, 3, tensor.id), + ], + &[(0, stage.pipeline.smem_bytes)], + MetalSize { + width: ((ne1 + 31) / 32) as u64, + height: ((ne01 + 63) / 64) as u64, + depth: ne12 as u64, + }, + MetalSize { + width: 128, + height: 1, + depth: 1, + }, + ); + } + + if base.starts_with("kernel_mul_mv_ext_") { + let ne00 = i32_dim(src0, 0)?; + let ne01 = i32_dim(src0, 1)?; + let ne10 = i32_dim(src1, 0)?; + let ne11 = i32_dim(src1, 1)?; + let ne0 = i32_dim(tensor, 0)?; + let ne1 = i32_dim(tensor, 1)?; + let nsg = constant_i16(&stage.descriptor.constants, 601)? as i32; + let nxpsg = stage + .descriptor + .constants + .iter() + .find(|constant| constant.idx == 602) + .and_then(|constant| match constant.value { + FunctionConstantValue::Int16(value) => Some(i32::from(value)), + _ => None, + }) + .unwrap_or_else(|| { + if ne00 % 256 == 0 && ne11 < 3 { + 16 + } else if ne00 % 128 == 0 { + 8 + } else { + 4 + } + }); + let r1ptg = parse_trailing_i32(base, "_r1_")?; + let r0ptg = nxpsg * nsg; + let args = KArgsMulMvExt { + ne00, + ne01, + ne02: 1, + nb00: u64::try_from(src0.nb[0]) + .map_err(|_| "mul_mv_ext nb00 exceeds u64".to_string())?, + nb01: u64::try_from(src0.nb[1]) + .map_err(|_| "mul_mv_ext nb01 exceeds u64".to_string())?, + nb02: u64::try_from(src0.nb[1]) + .map_err(|_| "mul_mv_ext nb02 exceeds u64".to_string())? + * ne01 as u64, + nb03: u64::try_from(src0.nb[1]) + .map_err(|_| "mul_mv_ext nb03 exceeds u64".to_string())? + * ne01 as u64, + ne10, + ne11, + ne12: 1, + nb10: u64::try_from(src1.nb[0]) + .map_err(|_| "mul_mv_ext nb10 exceeds u64".to_string())?, + nb11: u64::try_from(src1.nb[1]) + .map_err(|_| "mul_mv_ext nb11 exceeds u64".to_string())?, + nb12: u64::try_from(src1.nb[1]) + .map_err(|_| "mul_mv_ext nb12 exceeds u64".to_string())? + * ne11 as u64, + nb13: u64::try_from(src1.nb[1]) + .map_err(|_| "mul_mv_ext nb13 exceeds u64".to_string())? + * ne11 as u64, + ne0, + ne1, + r2: 1, + r3: 1, + }; + return runtime.dispatch_compute( + &stage.pipeline, + bytes_of(&args), + &[ + buffer_ref(compiled, 1, src0_id), + buffer_ref(compiled, 2, src1_id), + buffer_ref(compiled, 3, tensor.id), + ], + &[], + MetalSize { + width: ((ne01 + r0ptg - 1) / r0ptg) as u64, + height: ((ne11 + r1ptg - 1) / r1ptg) as u64, + depth: 1, + }, + MetalSize { + width: 32, + height: nsg as u64, + depth: 1, + }, + ); + } + + if base.starts_with("kernel_mul_mv_") { + let ne00 = i32_dim(src0, 0)?; + let ne01 = i32_dim(src0, 1)?; + let ne10 = i32_dim(src1, 0)?; + let ne11 = i32_dim(src1, 1)?; + let ne0 = i32_dim(tensor, 0)?; + let ne1 = i32_dim(tensor, 1)?; + let args = KArgsMulMv { + ne00, + ne01, + ne02: 1, + nb00: u64::try_from(src0.nb[0]).map_err(|_| "mul_mv nb00 exceeds u64".to_string())?, + nb01: u64::try_from(src0.nb[1]).map_err(|_| "mul_mv nb01 exceeds u64".to_string())?, + nb02: u64::try_from(src0.nb[1]).map_err(|_| "mul_mv nb02 exceeds u64".to_string())? + * ne01 as u64, + nb03: u64::try_from(src0.nb[1]).map_err(|_| "mul_mv nb03 exceeds u64".to_string())? + * ne01 as u64, + ne10, + ne11, + ne12: 1, + nb10: u64::try_from(src1.nb[0]).map_err(|_| "mul_mv nb10 exceeds u64".to_string())?, + nb11: u64::try_from(src1.nb[1]).map_err(|_| "mul_mv nb11 exceeds u64".to_string())?, + nb12: u64::try_from(src1.nb[1]).map_err(|_| "mul_mv nb12 exceeds u64".to_string())? + * ne11 as u64, + nb13: u64::try_from(src1.nb[1]).map_err(|_| "mul_mv nb13 exceeds u64".to_string())? + * ne11 as u64, + ne0, + ne1, + nr0: stage.pipeline.nr0, + r2: 1, + r3: 1, + }; + + let scalar_like = matches!( + src0.desc.ty, + TensorType::F32 | TensorType::F16 | TensorType::BF16 | TensorType::Q8_0 + ); + let tg_x = if scalar_like { + (ne01 + stage.pipeline.nr0 - 1) / stage.pipeline.nr0 + } else { + (ne01 + stage.pipeline.nr0 * stage.pipeline.nsg - 1) + / (stage.pipeline.nr0 * stage.pipeline.nsg) + }; + let tg_y = (ne11 + stage.pipeline.nr1 - 1) / stage.pipeline.nr1; + let smem = if stage.pipeline.smem_bytes > 0 { + vec![(0, stage.pipeline.smem_bytes)] + } else { + Vec::new() + }; + + return runtime.dispatch_compute( + &stage.pipeline, + bytes_of(&args), + &[ + buffer_ref(compiled, 1, src0_id), + buffer_ref(compiled, 2, src1_id), + buffer_ref(compiled, 3, tensor.id), + ], + &smem, + MetalSize { + width: tg_x as u64, + height: tg_y as u64, + depth: 1, + }, + MetalSize { + width: 32, + height: stage.pipeline.nsg as u64, + depth: 1, + }, + ); + } + + Err(format!( + "unsupported mul_mat pipeline '{}'", + stage.descriptor.base_name + )) +} + +fn dispatch_mul_mat_id( + runtime: &MetalRuntime, + ctx: &Context, + compiled: &MetalCompiledGraph, + tensor: &Tensor, + node: &MetalCompiledNode, +) -> Result<(), String> { + let src0_id = tensor_src(tensor, 0)?; + let src1_id = tensor_src(tensor, 1)?; + let src2_id = tensor_src(tensor, 2)?; + let src0 = ctx + .tensor(src0_id) + .ok_or_else(|| format!("mul_mat_id src0 {} is invalid", src0_id))?; + let src1 = ctx + .tensor(src1_id) + .ok_or_else(|| format!("mul_mat_id src1 {} is invalid", src1_id))?; + let src2 = ctx + .tensor(src2_id) + .ok_or_else(|| format!("mul_mat_id src2 {} is invalid", src2_id))?; + let stage = main_stage(node, tensor.op)?; + let base = stage.descriptor.base_name.as_str(); + + if base.starts_with("kernel_mul_mm_id_") { + let map_stage = stage_kind(node, MetalStageKind::Aux, tensor.op)?; + let tpe_bytes = mul_mat_id_extra_tpe_bytes(src0)?; + let args_map0 = KArgsMulMmIdMap0 { + ne02: i32_dim(src0, 2)?, + ne10: i32_dim(src1, 0)?, + ne11: i32_dim(src1, 1)?, + nb11: u64::try_from(src1.nb[1]) + .map_err(|_| "mul_mm_id_map0 nb11 exceeds u64".to_string())?, + nb12: u64::try_from(src1.nb[2]) + .map_err(|_| "mul_mm_id_map0 nb12 exceeds u64".to_string())?, + ne21: i32_dim(src2, 1)?, + ne20: i32_dim(src2, 0)?, + nb21: u64::try_from(src2.nb[1]) + .map_err(|_| "mul_mm_id_map0 nb21 exceeds u64".to_string())?, + }; + + runtime.dispatch_compute( + &map_stage.pipeline, + bytes_of(&args_map0), + &[ + buffer_ref(compiled, 1, src2_id), + tail_node_buffer_ref(compiled, node, 2, 0)?, + tail_node_buffer_ref(compiled, node, 3, tpe_bytes)?, + ], + &[(0, map_stage.pipeline.smem_bytes)], + MetalSize { + width: 1, + height: 1, + depth: 1, + }, + MetalSize { + width: src0.ne[2].max(1) as u64, + height: 1, + depth: 1, + }, + )?; + + let args = KArgsMulMmId { + ne00: i32_dim(src0, 0)?, + ne02: i32_dim(src0, 2)?, + nb01: u64::try_from(src0.nb[1]) + .map_err(|_| "mul_mm_id nb01 exceeds u64".to_string())?, + nb02: u64::try_from(src0.nb[2]) + .map_err(|_| "mul_mm_id nb02 exceeds u64".to_string())?, + nb03: u64::try_from(src0.nb[3]) + .map_err(|_| "mul_mm_id nb03 exceeds u64".to_string())?, + ne11: i32_dim(src1, 1)?, + nb10: u64::try_from(src1.nb[0]) + .map_err(|_| "mul_mm_id nb10 exceeds u64".to_string())?, + nb11: u64::try_from(src1.nb[1]) + .map_err(|_| "mul_mm_id nb11 exceeds u64".to_string())?, + nb12: u64::try_from(src1.nb[2]) + .map_err(|_| "mul_mm_id nb12 exceeds u64".to_string())?, + nb13: u64::try_from(src1.nb[3]) + .map_err(|_| "mul_mm_id nb13 exceeds u64".to_string())?, + ne20: i32_dim(src2, 0)?, + ne21: i32_dim(src2, 1)?, + ne0: i32_dim(tensor, 0)?, + ne1: i32_dim(tensor, 1)?, + r2: 1, + r3: 1, + }; + + return runtime.dispatch_compute( + &stage.pipeline, + bytes_of(&args), + &[ + buffer_ref(compiled, 1, src0_id), + buffer_ref(compiled, 2, src1_id), + tail_node_buffer_ref(compiled, node, 3, 0)?, + tail_node_buffer_ref(compiled, node, 4, tpe_bytes)?, + buffer_ref(compiled, 5, tensor.id), + ], + &[(0, stage.pipeline.smem_bytes)], + MetalSize { + width: ((src2.ne[1] + 31) / 32) as u64, + height: ((src0.ne[1] + 63) / 64) as u64, + depth: src0.ne[2].max(1) as u64, + }, + MetalSize { + width: 128, + height: 1, + depth: 1, + }, + ); + } + + if base.starts_with("kernel_mul_mv_id_") { + let args = KArgsMulMvId { + nei0: i32_dim(src2, 0)?, + nei1: i32_dim(src2, 1)?, + nbi1: u64::try_from(src2.nb[1]) + .map_err(|_| "mul_mv_id nbi1 exceeds u64".to_string())?, + ne00: i32_dim(src0, 0)?, + ne01: i32_dim(src0, 1)?, + ne02: i32_dim(src0, 2)?, + nb00: u64::try_from(src0.nb[0]) + .map_err(|_| "mul_mv_id nb00 exceeds u64".to_string())?, + nb01: u64::try_from(src0.nb[1]) + .map_err(|_| "mul_mv_id nb01 exceeds u64".to_string())?, + nb02: u64::try_from(src0.nb[2]) + .map_err(|_| "mul_mv_id nb02 exceeds u64".to_string())?, + ne10: i32_dim(src1, 0)?, + ne11: i32_dim(src1, 1)?, + ne12: i32_dim(src1, 2)?, + ne13: i32_dim(src1, 3)?, + nb10: u64::try_from(src1.nb[0]) + .map_err(|_| "mul_mv_id nb10 exceeds u64".to_string())?, + nb11: u64::try_from(src1.nb[1]) + .map_err(|_| "mul_mv_id nb11 exceeds u64".to_string())?, + nb12: u64::try_from(src1.nb[2]) + .map_err(|_| "mul_mv_id nb12 exceeds u64".to_string())?, + ne0: i32_dim(tensor, 0)?, + ne1: i32_dim(tensor, 1)?, + nb1: u64::try_from(tensor.nb[1]) + .map_err(|_| "mul_mv_id nb1 exceeds u64".to_string())?, + nr0: stage.pipeline.nr0, + }; + + let scalar_like = matches!( + src0.desc.ty, + TensorType::F32 | TensorType::F16 | TensorType::BF16 | TensorType::Q8_0 + ); + let tg_x = if scalar_like { + (src0.ne[1] + stage.pipeline.nr0 as i64 - 1) / stage.pipeline.nr0 as i64 + } else { + (src0.ne[1] + (stage.pipeline.nr0 * stage.pipeline.nsg) as i64 - 1) + / (stage.pipeline.nr0 * stage.pipeline.nsg) as i64 + }; + let smem = if stage.pipeline.smem_bytes > 0 { + vec![(0, stage.pipeline.smem_bytes)] + } else { + Vec::new() + }; + + return runtime.dispatch_compute( + &stage.pipeline, + bytes_of(&args), + &[ + buffer_ref(compiled, 1, src0_id), + buffer_ref(compiled, 2, src1_id), + buffer_ref(compiled, 3, tensor.id), + buffer_ref(compiled, 4, src2_id), + ], + &smem, + MetalSize { + width: tg_x.max(1) as u64, + height: 1, + depth: (src2.ne[0] * src2.ne[1]).max(1) as u64, + }, + MetalSize { + width: 32, + height: stage.pipeline.nsg.max(1) as u64, + depth: 1, + }, + ); + } + + Err(format!( + "unsupported mul_mat_id pipeline '{}'", + stage.descriptor.base_name + )) +} + +fn dispatch_argsort_like( + runtime: &MetalRuntime, + ctx: &Context, + compiled: &MetalCompiledGraph, + tensor: &Tensor, + node: &MetalCompiledNode, + top_k_mode: bool, +) -> Result<(), String> { + let main_stage = main_stage(node, tensor.op)?; + let merge_stage = stage_kind(node, MetalStageKind::Merge, tensor.op)?; + let src0_id = tensor_src(tensor, 0)?; + let src0 = ctx + .tensor(src0_id) + .ok_or_else(|| format!("argsort src0 {} is invalid", src0_id))?; + + let mut nth = 1_i32; + while nth < src0.ne[0] as i32 + && 2 * nth + <= i32::try_from(main_stage.pipeline.max_threads_per_threadgroup).unwrap_or(i32::MAX) + { + nth *= 2; + } + let npr = ((src0.ne[0] as i32) + nth - 1) / nth; + let smem = ggml_pad( + usize::try_from(nth).map_err(|_| "argsort nth overflow".to_string())? + * std::mem::size_of::(), + 16, + ); + + let block_top_k = if top_k_mode { + std::cmp::min(nth, i32_dim(tensor, 0)?).max(1) + } else { + nth.max(1) + }; + let effective_ne0 = if top_k_mode && npr > 1 { + (npr - 1) * block_top_k + std::cmp::min(src0.ne[0] as i32 - (npr - 1) * nth, block_top_k) + } else if top_k_mode { + block_top_k + } else { + i32_dim(tensor, 0)? + }; + + let src_nb1 = u64::try_from(src0.nb[1]).map_err(|_| "argsort nb01 exceeds u64".to_string())?; + let src_nb2 = u64::try_from(src0.nb[2]).map_err(|_| "argsort nb02 exceeds u64".to_string())?; + let src_nb3 = u64::try_from(src0.nb[3]).map_err(|_| "argsort nb03 exceeds u64".to_string())?; + let dst_nb1 = + u64::try_from(tensor.nb[1]).map_err(|_| "argsort dst nb1 exceeds u64".to_string())?; + let dst_nb2 = + u64::try_from(tensor.nb[2]).map_err(|_| "argsort dst nb2 exceeds u64".to_string())?; + let dst_nb3 = + u64::try_from(tensor.nb[3]).map_err(|_| "argsort dst nb3 exceeds u64".to_string())?; + let tmp_row_bytes = ggml_type_size_for_type(TensorType::I32) + * usize::try_from(src0.ne[0]).map_err(|_| "argsort temp row bytes overflow".to_string())?; + + for i03 in 0..src0.ne[3] { + for i02 in 0..src0.ne[2] { + for i01 in 0..src0.ne[1] { + let row_index = usize::try_from(i01 + src0.ne[1] * (i02 + src0.ne[2] * i03)) + .map_err(|_| "argsort row index overflow".to_string())?; + let src_row_offset = usize::try_from( + i01 * src0.nb[1] as i64 + i02 * src0.nb[2] as i64 + i03 * src0.nb[3] as i64, + ) + .map_err(|_| "argsort src row offset overflow".to_string())?; + let dst_row_offset = usize::try_from( + i01 * tensor.nb[1] as i64 + + i02 * tensor.nb[2] as i64 + + i03 * tensor.nb[3] as i64, + ) + .map_err(|_| "argsort dst row offset overflow".to_string())?; + let tmp_row_offset = row_index + .checked_mul(tmp_row_bytes) + .ok_or_else(|| "argsort temp row offset overflow".to_string())?; + + let mut dst_binding = + buffer_ref_with_offset(compiled, 2, tensor.id, dst_row_offset)?; + let mut tmp_binding = tail_node_buffer_ref(compiled, node, 3, tmp_row_offset)?; + let mut merge_passes = 0usize; + let mut parity_len = block_top_k; + while parity_len < effective_ne0 { + merge_passes += 1; + parity_len <<= 1; + } + if merge_passes % 2 == 1 { + std::mem::swap(&mut dst_binding, &mut tmp_binding); + } + + let args = KArgsArgsort { + ne00: i64_dim(src0, 0)?, + ne01: 1, + ne02: 1, + ne03: 1, + nb00: u64::try_from(src0.nb[0]) + .map_err(|_| "argsort nb00 exceeds u64".to_string())?, + nb01: src_nb1, + nb02: src_nb2, + nb03: src_nb3, + ne0: effective_ne0, + ne1: 1, + ne2: 1, + ne3: 1, + top_k: block_top_k, + }; + + runtime.dispatch_compute( + &main_stage.pipeline, + bytes_of(&args), + &[ + MetalBufferBindingRef { + index: 1, + buffer: &compiled.main_buffer, + offset_bytes: binding(compiled, src0_id)? + .offset_bytes + .checked_add(src_row_offset) + .ok_or_else(|| "argsort src binding offset overflow".to_string())?, + }, + MetalBufferBindingRef { + index: 2, + buffer: dst_binding.buffer, + offset_bytes: dst_binding.offset_bytes, + }, + ], + &[(0, smem)], + MetalSize { + width: npr.max(1) as u64, + height: 1, + depth: 1, + }, + MetalSize { + width: nth.max(1) as u64, + height: 1, + depth: 1, + }, + )?; + + let mut len = block_top_k; + while len < effective_ne0 { + let nm = (effective_ne0 + 2 * len - 1) / (2 * len); + let merge_top_k = if top_k_mode && nm == 1 { + i32_dim(tensor, 0)? + } else { + effective_ne0 + }; + let args_merge = KArgsArgsortMerge { + ne00: i64_dim(src0, 0)?, + ne01: 1, + ne02: 1, + ne03: 1, + nb00: u64::try_from(src0.nb[0]) + .map_err(|_| "argsort_merge nb00 exceeds u64".to_string())?, + nb01: src_nb1, + nb02: src_nb2, + nb03: src_nb3, + ne0: effective_ne0, + ne1: 1, + ne2: 1, + ne3: 1, + top_k: merge_top_k, + len, + }; + let merge_nth = if top_k_mode { + std::cmp::min( + 512_u64, + std::cmp::min( + len.max(1) as u64, + merge_stage.pipeline.max_threads_per_threadgroup.max(1), + ), + ) + } else { + std::cmp::min( + 512_u64, + merge_stage.pipeline.max_threads_per_threadgroup.max(1), + ) + }; + + runtime.dispatch_compute( + &merge_stage.pipeline, + bytes_of(&args_merge), + &[ + MetalBufferBindingRef { + index: 1, + buffer: &compiled.main_buffer, + offset_bytes: binding(compiled, src0_id)? + .offset_bytes + .checked_add(src_row_offset) + .ok_or_else(|| { + "argsort src binding offset overflow".to_string() + })?, + }, + MetalBufferBindingRef { + index: 2, + buffer: dst_binding.buffer, + offset_bytes: dst_binding.offset_bytes, + }, + MetalBufferBindingRef { + index: 3, + buffer: tmp_binding.buffer, + offset_bytes: tmp_binding.offset_bytes, + }, + ], + &[], + MetalSize { + width: nm.max(1) as u64, + height: 1, + depth: 1, + }, + MetalSize { + width: merge_nth.max(1), + height: 1, + depth: 1, + }, + )?; + + std::mem::swap(&mut dst_binding, &mut tmp_binding); + len <<= 1; + } + } + } + } + + Ok(()) +} + +fn dispatch_rope( + runtime: &MetalRuntime, + ctx: &Context, + compiled: &MetalCompiledGraph, + tensor: &Tensor, + node: &MetalCompiledNode, +) -> Result<(), String> { + let stage = main_stage(node, tensor.op)?; + let src0_id = tensor_src(tensor, 0)?; + let src1_id = tensor_src(tensor, 1)?; + let src2_id = tensor_src_opt(tensor, 2); + let src0 = ctx + .tensor(src0_id) + .ok_or_else(|| format!("rope src0 {} is invalid", src0_id))?; + let src1 = ctx + .tensor(src1_id) + .ok_or_else(|| format!("rope src1 {} is invalid", src1_id))?; + + if src1.desc.ty != TensorType::I32 { + return Err(format!( + "rope positions tensor must be I32, got {}", + src1.desc.ty.name() + )); + } + + let src0_shape = shape4(src0)?; + let src1_shape = shape4(src1)?; + let dst_shape = shape4(tensor)?; + + if src1_shape.ne[0] % src0_shape.ne[2] != 0 || src1_shape.ne[0] < src0_shape.ne[2] { + return Err(format!( + "rope positions shape is incompatible: ne10={} ne02={}", + src1_shape.ne[0], src0_shape.ne[2] + )); + } + + let args = KArgsRope { + ne00: src0_shape.ne[0], + ne01: src0_shape.ne[1], + ne02: src0_shape.ne[2], + ne03: src0_shape.ne[3], + nb00: src0_shape.nb[0], + nb01: src0_shape.nb[1], + nb02: src0_shape.nb[2], + nb03: src0_shape.nb[3], + ne0: dst_shape.ne[0], + ne1: dst_shape.ne[1], + ne2: dst_shape.ne[2], + ne3: dst_shape.ne[3], + nb0: dst_shape.nb[0], + nb1: dst_shape.nb[1], + nb2: dst_shape.nb[2], + nb3: dst_shape.nb[3], + n_past: tensor.op_param_i32(0), + n_dims: tensor.op_param_i32(1), + n_ctx_orig: tensor.op_param_i32(4), + freq_base: tensor.op_param_f32(5), + freq_scale: tensor.op_param_f32(6), + ext_factor: tensor.op_param_f32(7), + attn_factor: tensor.op_param_f32(8), + beta_fast: tensor.op_param_f32(9), + beta_slow: tensor.op_param_f32(10), + sect_0: tensor.op_param_i32(11), + sect_1: tensor.op_param_i32(12), + sect_2: tensor.op_param_i32(13), + sect_3: tensor.op_param_i32(14), + src2: src2_id.is_some(), + }; + + let mut nth = std::cmp::min(1024_u64, src0_shape.ne[0].max(1) as u64); + nth = std::cmp::min(nth, stage.pipeline.max_threads_per_threadgroup.max(1)); + + let src2_binding = match src2_id { + Some(src2_id) => buffer_ref(compiled, 3, src2_id), + None => buffer_ref(compiled, 3, src0_id), + }; + + runtime.dispatch_compute( + &stage.pipeline, + bytes_of(&args), + &[ + buffer_ref(compiled, 1, src0_id), + buffer_ref(compiled, 2, src1_id), + src2_binding, + buffer_ref(compiled, 4, tensor.id), + ], + &[], + MetalSize { + width: src0_shape.ne[1] as u64, + height: src0_shape.ne[2] as u64, + depth: src0_shape.ne[3] as u64, + }, + MetalSize { + width: nth, + height: 1, + depth: 1, + }, + ) +} + +fn dispatch_ssm_conv( + runtime: &MetalRuntime, + ctx: &Context, + compiled: &MetalCompiledGraph, + tensor: &Tensor, + node: &MetalCompiledNode, +) -> Result<(), String> { + let stage = main_stage(node, tensor.op)?; + let src0_id = tensor_src(tensor, 0)?; + let src1_id = tensor_src(tensor, 1)?; + let src0 = ctx + .tensor(src0_id) + .ok_or_else(|| format!("ssm_conv src0 {} is invalid", src0_id))?; + let src1 = ctx + .tensor(src1_id) + .ok_or_else(|| format!("ssm_conv src1 {} is invalid", src1_id))?; + let args = KArgsSsmConv { + ne00: i64_dim(src0, 0)?, + ne01: i64_dim(src0, 1)?, + ne02: i64_dim(src0, 2)?, + nb00: u64::try_from(src0.nb[0]).map_err(|_| "ssm_conv nb00 exceeds u64".to_string())?, + nb01: u64::try_from(src0.nb[1]).map_err(|_| "ssm_conv nb01 exceeds u64".to_string())?, + nb02: u64::try_from(src0.nb[2]).map_err(|_| "ssm_conv nb02 exceeds u64".to_string())?, + ne10: i64_dim(src1, 0)?, + ne11: i64_dim(src1, 1)?, + nb10: u64::try_from(src1.nb[0]).map_err(|_| "ssm_conv nb10 exceeds u64".to_string())?, + nb11: u64::try_from(src1.nb[1]).map_err(|_| "ssm_conv nb11 exceeds u64".to_string())?, + ne0: i64_dim(tensor, 0)?, + ne1: i64_dim(tensor, 1)?, + ne2: i64_dim(tensor, 2)?, + nb0: u64::try_from(tensor.nb[0]).map_err(|_| "ssm_conv nb0 exceeds u64".to_string())?, + nb1: u64::try_from(tensor.nb[1]).map_err(|_| "ssm_conv nb1 exceeds u64".to_string())?, + nb2: u64::try_from(tensor.nb[2]).map_err(|_| "ssm_conv nb2 exceeds u64".to_string())?, + }; + + let use_batched = tensor.ne[1] > 1; + let threadgroups = if use_batched { + let batch_size = u64::try_from(constant_i16(&stage.descriptor.constants, FC_SSM_CONV + 0)?) + .map_err(|_| "ssm_conv batch size exceeds u64".to_string())?; + let n_token_batches = (tensor.ne[1].max(1) as u64).div_ceil(batch_size.max(1)); + MetalSize { + width: src0.ne[1].max(1) as u64, + height: n_token_batches, + depth: src0.ne[2].max(1) as u64, + } + } else { + MetalSize { + width: src0.ne[1].max(1) as u64, + height: tensor.ne[1].max(1) as u64, + depth: src0.ne[2].max(1) as u64, + } + }; + let threads = if use_batched { + MetalSize { + width: u64::try_from(constant_i16(&stage.descriptor.constants, FC_SSM_CONV + 0)?) + .map_err(|_| "ssm_conv batch size exceeds u64".to_string())?, + height: 1, + depth: 1, + } + } else { + MetalSize { + width: 1, + height: 1, + depth: 1, + } + }; + + runtime.dispatch_compute( + &stage.pipeline, + bytes_of(&args), + &[ + buffer_ref(compiled, 1, src0_id), + buffer_ref(compiled, 2, src1_id), + buffer_ref(compiled, 3, tensor.id), + ], + &[], + threadgroups, + threads, + ) +} + +fn dispatch_gated_delta_net( + runtime: &MetalRuntime, + ctx: &Context, + compiled: &MetalCompiledGraph, + tensor: &Tensor, + node: &MetalCompiledNode, +) -> Result<(), String> { + let stage = main_stage(node, tensor.op)?; + let src0_id = tensor_src(tensor, 0)?; + let src1_id = tensor_src(tensor, 1)?; + let src2_id = tensor_src(tensor, 2)?; + let src3_id = tensor_src(tensor, 3)?; + let src4_id = tensor_src(tensor, 4)?; + let src5_id = tensor_src(tensor, 5)?; + let src0 = ctx + .tensor(src0_id) + .ok_or_else(|| format!("gated_delta_net src0 {} is invalid", src0_id))?; + let src1 = ctx + .tensor(src1_id) + .ok_or_else(|| format!("gated_delta_net src1 {} is invalid", src1_id))?; + let src2 = ctx + .tensor(src2_id) + .ok_or_else(|| format!("gated_delta_net src2 {} is invalid", src2_id))?; + let args = KArgsGatedDeltaNet { + ne00: i32_dim(src0, 0)?, + ne01: i32_dim(src0, 1)?, + ne02: i32_dim(src0, 2)?, + ne03: i32_dim(src0, 3)?, + nb00: u64::try_from(src0.nb[0]).map_err(|_| "gated_delta nb00 exceeds u64".to_string())?, + nb01: u64::try_from(src0.nb[1]).map_err(|_| "gated_delta nb01 exceeds u64".to_string())?, + nb02: u64::try_from(src0.nb[2]).map_err(|_| "gated_delta nb02 exceeds u64".to_string())?, + nb03: u64::try_from(src0.nb[3]).map_err(|_| "gated_delta nb03 exceeds u64".to_string())?, + ne10: i32_dim(src1, 0)?, + ne11: i32_dim(src1, 1)?, + ne12: i32_dim(src1, 2)?, + ne13: i32_dim(src1, 3)?, + nb10: u64::try_from(src1.nb[0]).map_err(|_| "gated_delta nb10 exceeds u64".to_string())?, + nb11: u64::try_from(src1.nb[1]).map_err(|_| "gated_delta nb11 exceeds u64".to_string())?, + nb12: u64::try_from(src1.nb[2]).map_err(|_| "gated_delta nb12 exceeds u64".to_string())?, + nb13: u64::try_from(src1.nb[3]).map_err(|_| "gated_delta nb13 exceeds u64".to_string())?, + ne20: i32_dim(src2, 0)?, + ne21: i32_dim(src2, 1)?, + ne22: i32_dim(src2, 2)?, + ne23: i32_dim(src2, 3)?, + nb20: u64::try_from(src2.nb[0]).map_err(|_| "gated_delta nb20 exceeds u64".to_string())?, + nb21: u64::try_from(src2.nb[1]).map_err(|_| "gated_delta nb21 exceeds u64".to_string())?, + nb22: u64::try_from(src2.nb[2]).map_err(|_| "gated_delta nb22 exceeds u64".to_string())?, + nb23: u64::try_from(src2.nb[3]).map_err(|_| "gated_delta nb23 exceeds u64".to_string())?, + ns02: i32::try_from(src0.nb[2] / std::mem::size_of::()) + .map_err(|_| "gated_delta ns02 exceeds i32".to_string())?, + ns12: i32::try_from(src1.nb[2] / std::mem::size_of::()) + .map_err(|_| "gated_delta ns12 exceeds i32".to_string())?, + ns22: i32::try_from(src2.nb[2] / std::mem::size_of::()) + .map_err(|_| "gated_delta ns22 exceeds i32".to_string())?, + ne0: i32_dim(tensor, 0)?, + ne1: i32_dim(tensor, 1)?, + ne2: i32_dim(tensor, 2)?, + ne3: i32_dim(tensor, 3)?, + nb0: u64::try_from(tensor.nb[0]).map_err(|_| "gated_delta nb0 exceeds u64".to_string())?, + nb1: u64::try_from(tensor.nb[1]).map_err(|_| "gated_delta nb1 exceeds u64".to_string())?, + nb2: u64::try_from(tensor.nb[2]).map_err(|_| "gated_delta nb2 exceeds u64".to_string())?, + nb3: u64::try_from(tensor.nb[3]).map_err(|_| "gated_delta nb3 exceeds u64".to_string())?, + }; + + runtime.dispatch_compute( + &stage.pipeline, + bytes_of(&args), + &[ + buffer_ref(compiled, 1, src0_id), + buffer_ref(compiled, 2, src1_id), + buffer_ref(compiled, 3, src2_id), + buffer_ref(compiled, 4, src3_id), + buffer_ref(compiled, 5, src4_id), + buffer_ref(compiled, 6, src5_id), + buffer_ref(compiled, 7, tensor.id), + ], + &[], + MetalSize { + width: (src2.ne[0].max(1) as u64) / (stage.pipeline.nsg.max(1) as u64), + height: src2.ne[1].max(1) as u64, + depth: src2.ne[3].max(1) as u64, + }, + MetalSize { + width: 32, + height: stage.pipeline.nsg.max(1) as u64, + depth: 1, + }, + ) +} + +fn dispatch_flash_attn_ext( + runtime: &MetalRuntime, + ctx: &Context, + compiled: &MetalCompiledGraph, + tensor: &Tensor, + node: &MetalCompiledNode, +) -> Result<(), String> { + let q_id = tensor_src(tensor, 0)?; + let k_id = tensor_src(tensor, 1)?; + let v_id = tensor_src(tensor, 2)?; + let mask_id = tensor_src_opt(tensor, 3); + let sinks_id = tensor_src_opt(tensor, 4); + + let q = ctx + .tensor(q_id) + .ok_or_else(|| format!("flash_attn_ext q {} is invalid", q_id))?; + let k = ctx + .tensor(k_id) + .ok_or_else(|| format!("flash_attn_ext k {} is invalid", k_id))?; + let v = ctx + .tensor(v_id) + .ok_or_else(|| format!("flash_attn_ext v {} is invalid", v_id))?; + let mask = mask_id + .map(|id| { + ctx.tensor(id) + .ok_or_else(|| format!("flash_attn_ext mask {} is invalid", id)) + }) + .transpose()?; + let sinks = sinks_id + .map(|id| { + ctx.tensor(id) + .ok_or_else(|| format!("flash_attn_ext sinks {} is invalid", id)) + }) + .transpose()?; + + if q.desc.ty != TensorType::F32 { + return Err(format!( + "flash_attn_ext currently requires q to be F32, got {}", + q.desc.ty.name() + )); + } + if !flash_attn_supported_head_dim( + usize::try_from(q.ne[0]).map_err(|_| "flash_attn head dim overflow".to_string())?, + ) { + return Err(format!("unsupported flash_attn_ext head dim {}", q.ne[0])); + } + if k.desc.ty != v.desc.ty { + return Err(format!( + "flash_attn_ext requires k/v to share a type, got {} and {}", + k.desc.ty.name(), + v.desc.ty.name() + )); + } + + let q_shape = shape4(q)?; + let k_shape = shape4(k)?; + let v_shape = shape4(v)?; + let dst_shape = shape4(tensor)?; + + let has_mask = mask.is_some(); + let has_sinks = sinks.is_some(); + let max_bias = tensor.op_param_f32(1); + let logit_softcap = tensor.op_param_f32(2); + let has_bias = max_bias != 0.0; + let has_scap = logit_softcap != 0.0; + + if !flash_attn_ext_use_vec(q) { + return Err( + "flash_attn_ext compiled executor currently only supports the Metal vec path" + .to_string(), + ); + } + + let stage_main = node + .stages + .iter() + .find(|stage| { + stage + .descriptor + .base_name + .starts_with("kernel_flash_attn_ext_vec_") + }) + .ok_or_else(|| "flash_attn_ext is missing vec main stage".to_string())?; + let stage_reduce = node + .stages + .iter() + .find(|stage| stage.descriptor.base_name == "kernel_flash_attn_ext_vec_reduce") + .ok_or_else(|| "flash_attn_ext is missing vec reduce stage".to_string())?; + let stage_pad = node + .stages + .iter() + .find(|stage| stage.descriptor.base_name == "kernel_flash_attn_ext_pad"); + + let pad_bytes = flash_attn_ext_extra_pad_bytes(q, k, v, mask)?; + let blk_bytes = flash_attn_ext_extra_blk_bytes(q, mask)?; + let tmp_bytes = flash_attn_ext_extra_tmp_bytes(q, v)?; + + let has_kvpad = k_shape.ne[1] % OP_FLASH_ATTN_EXT_VEC_NCPSG != 0; + let nsg = flash_attn_ext_vec_nsg(k); + let nwg = 32_i32; + let n_head = + usize::try_from(q_shape.ne[2]).map_err(|_| "flash_attn n_head overflow".to_string())?; + let n_head_log2 = if n_head <= 1 { + 1_i32 + } else { + let p = (usize::BITS - 1) - n_head.leading_zeros(); + (1usize << p) as i32 + }; + let m0 = (2.0f32).powf(-(max_bias) / (n_head_log2 as f32)); + let m1 = (2.0f32).powf(-(max_bias / 2.0) / (n_head_log2 as f32)); + let scale = if has_scap { + tensor.op_param_f32(0) / logit_softcap + } else { + tensor.op_param_f32(0) + }; + + let default_mask = default_flash_mask_shape(&q_shape, &k_shape)?; + let mask_shape = match mask { + Some(mask) => shape4(mask)?, + None => default_mask, + }; + let ns10 = i32::try_from(k_shape.nb[1] / k_shape.nb[0]) + .map_err(|_| "flash_attn ns10 overflow".to_string())?; + let ns20 = i32::try_from(v_shape.nb[1] / v_shape.nb[0]) + .map_err(|_| "flash_attn ns20 overflow".to_string())?; + + if has_kvpad { + let stage_pad = + stage_pad.ok_or_else(|| "flash_attn_ext is missing vec pad stage".to_string())?; + let pad_ncpsg = constant_i32(&stage_pad.descriptor.constants, FC_FLASH_ATTN_EXT_PAD + 25)?; + let args = KArgsFlashAttnExtPad { + ne11: k_shape.ne[1], + ne_12_2: k_shape.ne[2], + ne_12_3: k_shape.ne[3], + nb11: k_shape.nb[1], + nb12: k_shape.nb[2], + nb13: k_shape.nb[3], + nb21: v_shape.nb[1], + nb22: v_shape.nb[2], + nb23: v_shape.nb[3], + ne31: mask_shape.ne[1], + ne32: mask_shape.ne[2], + ne33: mask_shape.ne[3], + nb31: mask_shape.nb[1], + nb32: mask_shape.nb[2], + nb33: mask_shape.nb[3], + }; + + runtime.dispatch_compute( + &stage_pad.pipeline, + bytes_of(&args), + &[ + buffer_ref(compiled, 1, k_id), + buffer_ref(compiled, 2, v_id), + mask_id + .map(|id| buffer_ref(compiled, 3, id)) + .unwrap_or_else(|| buffer_ref(compiled, 3, q_id)), + tail_node_buffer_ref(compiled, node, 4, 0)?, + ], + &[], + MetalSize { + width: pad_ncpsg as u64, + height: k_shape.ne[2].max(mask_shape.ne[2]) as u64, + depth: k_shape.ne[3].max(mask_shape.ne[3]) as u64, + }, + MetalSize { + width: 32, + height: 1, + depth: 1, + }, + )?; + } + + let args = KArgsFlashAttnExtVec { + ne01: q_shape.ne[1], + ne02: q_shape.ne[2], + ne03: q_shape.ne[3], + nb01: q_shape.nb[1], + nb02: q_shape.nb[2], + nb03: q_shape.nb[3], + ne11: k_shape.ne[1], + ne_12_2: k_shape.ne[2], + ne_12_3: k_shape.ne[3], + ns10, + nb11: k_shape.nb[1], + nb12: k_shape.nb[2], + nb13: k_shape.nb[3], + ns20, + nb21: v_shape.nb[1], + nb22: v_shape.nb[2], + nb23: v_shape.nb[3], + ne31: mask_shape.ne[1], + ne32: mask_shape.ne[2], + ne33: mask_shape.ne[3], + nb31: mask_shape.nb[1], + nb32: mask_shape.nb[2], + nb33: mask_shape.nb[3], + ne1: dst_shape.ne[1], + ne2: dst_shape.ne[2], + ne3: dst_shape.ne[3], + scale, + max_bias, + m0, + m1, + n_head_log2, + logit_softcap, + }; + + runtime.dispatch_compute( + &stage_main.pipeline, + bytes_of(&args), + &[ + buffer_ref(compiled, 1, q_id), + buffer_ref(compiled, 2, k_id), + buffer_ref(compiled, 3, v_id), + mask_id + .map(|id| buffer_ref(compiled, 4, id)) + .unwrap_or_else(|| buffer_ref(compiled, 4, q_id)), + sinks_id + .map(|id| buffer_ref(compiled, 5, id)) + .unwrap_or_else(|| buffer_ref(compiled, 5, q_id)), + tail_node_buffer_ref(compiled, node, 6, 0)?, + tail_node_buffer_ref(compiled, node, 7, pad_bytes + blk_bytes)?, + ], + &[(0, stage_main.pipeline.smem_bytes)], + MetalSize { + width: ((q_shape.ne[1] + OP_FLASH_ATTN_EXT_VEC_NQPSG - 1) / OP_FLASH_ATTN_EXT_VEC_NQPSG) + as u64, + height: q_shape.ne[2] as u64, + depth: (q_shape.ne[3] * nwg) as u64, + }, + MetalSize { + width: 32, + height: nsg as u64, + depth: 1, + }, + )?; + + let args_reduce = KArgsFlashAttnExtVecReduce { + nrows: i32::try_from( + usize::try_from(dst_shape.ne[1]) + .map_err(|_| "flash_attn reduce ne1 overflow".to_string())? + .checked_mul( + usize::try_from(dst_shape.ne[2]) + .map_err(|_| "flash_attn reduce ne2 overflow".to_string())?, + ) + .and_then(|v| v.checked_mul(usize::try_from(dst_shape.ne[3]).ok()?)) + .ok_or_else(|| "flash_attn reduce nrows overflow".to_string())?, + ) + .map_err(|_| "flash_attn reduce nrows exceeds i32".to_string())?, + }; + + let _ = tmp_bytes; + let _ = has_mask; + let _ = has_sinks; + let _ = has_bias; + + runtime.dispatch_compute( + &stage_reduce.pipeline, + bytes_of(&args_reduce), + &[ + tail_node_buffer_ref(compiled, node, 1, pad_bytes + blk_bytes)?, + buffer_ref(compiled, 2, tensor.id), + ], + &[], + MetalSize { + width: args_reduce.nrows as u64, + height: 1, + depth: 1, + }, + MetalSize { + width: (32 * nwg) as u64, + height: 1, + depth: 1, + }, + ) +} + +fn dispatch_cpy( + runtime: &MetalRuntime, + ctx: &Context, + compiled: &MetalCompiledGraph, + tensor: &Tensor, + node: &MetalCompiledNode, +) -> Result<(), String> { + let stage = main_stage(node, tensor.op)?; + let src0_id = tensor_src(tensor, 0)?; + let src0 = ctx + .tensor(src0_id) + .ok_or_else(|| format!("cpy src0 {} is invalid", src0_id))?; + dispatch_cpy_stage( + runtime, + compiled, + stage, + src0, + src0_id, + tensor.id, + tensor.desc.ty, + 0, + Layout4::from_tensor(tensor)?, + ) +} + +fn dispatch_set( + runtime: &MetalRuntime, + ctx: &Context, + compiled: &MetalCompiledGraph, + tensor: &Tensor, + node: &MetalCompiledNode, +) -> Result<(), String> { + let src0_id = tensor_src(tensor, 0)?; + let src1_id = tensor_src(tensor, 1)?; + let src0 = ctx + .tensor(src0_id) + .ok_or_else(|| format!("set src0 {} is invalid", src0_id))?; + let src1 = ctx + .tensor(src1_id) + .ok_or_else(|| format!("set src1 {} is invalid", src1_id))?; + + let pnb1 = usize::try_from(tensor.op_param_i32(0)) + .map_err(|_| format!("set pnb1 is negative: {}", tensor.op_param_i32(0)))?; + let pnb2 = usize::try_from(tensor.op_param_i32(1)) + .map_err(|_| format!("set pnb2 is negative: {}", tensor.op_param_i32(1)))?; + let pnb3 = usize::try_from(tensor.op_param_i32(2)) + .map_err(|_| format!("set pnb3 is negative: {}", tensor.op_param_i32(2)))?; + let offs = usize::try_from(tensor.op_param_i32(3)) + .map_err(|_| format!("set offs is negative: {}", tensor.op_param_i32(3)))?; + let inplace = tensor.op_param_i32(4) != 0; + + let dst_layout = Layout4 { + ne: [ + i64_dim(src1, 0)?, + i64_dim(src1, 1)?, + i64_dim(src1, 2)?, + i64_dim(src1, 3)?, + ], + nb: [ + ggml_type_size_for_type(tensor.desc.ty) as u64, + u64::try_from(pnb1).map_err(|_| "set pnb1 exceeds u64".to_string())?, + u64::try_from(pnb2).map_err(|_| "set pnb2 exceeds u64".to_string())?, + u64::try_from(pnb3).map_err(|_| "set pnb3 exceeds u64".to_string())?, + ], + }; + + let mut stages = node.stages.iter(); + if !inplace { + let stage = stages + .next() + .ok_or_else(|| "set op is missing copy stage".to_string())?; + dispatch_cpy_stage( + runtime, + compiled, + stage, + src0, + src0_id, + tensor.id, + tensor.desc.ty, + 0, + Layout4::from_tensor(tensor)?, + )?; + } + + let stage = stages + .next() + .ok_or_else(|| "set op is missing main stage".to_string())?; + dispatch_cpy_stage( + runtime, + compiled, + stage, + src1, + src1_id, + tensor.id, + tensor.desc.ty, + offs, + dst_layout, + ) +} + +fn dispatch_cpy_stage( + runtime: &MetalRuntime, + compiled: &MetalCompiledGraph, + stage: &MetalCompiledStage, + src: &Tensor, + src_id: TensorId, + dst_id: TensorId, + dst_ty: TensorType, + dst_extra_offset: usize, + dst_layout: Layout4, +) -> Result<(), String> { + let ne00 = i64_dim(src, 0)?; + let ne01 = i64_dim(src, 1)?; + let ne02 = i64_dim(src, 2)?; + let ne03 = i64_dim(src, 3)?; + let nb00 = u64::try_from(src.nb[0]).map_err(|_| "cpy nb00 exceeds u64".to_string())?; + let nb01 = u64::try_from(src.nb[1]).map_err(|_| "cpy nb01 exceeds u64".to_string())?; + let nb02 = u64::try_from(src.nb[2]).map_err(|_| "cpy nb02 exceeds u64".to_string())?; + let nb03 = u64::try_from(src.nb[3]).map_err(|_| "cpy nb03 exceeds u64".to_string())?; + + let src_blck = ggml_blck_size_for_type(src.desc.ty) as i64; + if ne00 % src_blck != 0 { + return Err(format!( + "cpy source dim0 {} is not divisible by block size {} for {}", + ne00, + src_blck, + src.desc.ty.name() + )); + } + + let mut nk0 = ne00; + if src.desc.ty.is_quantized() { + nk0 = ne00 / 16; + } else if dst_ty.is_quantized() { + nk0 = ne00 / ggml_blck_size_for_type(dst_ty) as i64; + } + + let max_threads = stage.pipeline.max_threads_per_threadgroup.max(1); + let mut nth = std::cmp::min(nk0.max(1) as u64, max_threads); + let mut nrptg = 1u64; + + if ggml_blck_size_for_type(src.desc.ty) == 1 && ggml_blck_size_for_type(dst_ty) == 1 { + if nth > nk0 as u64 { + nrptg = nth.div_ceil(nk0.max(1) as u64); + nth = nk0.max(1) as u64; + if nrptg * nth > max_threads { + nrptg = nrptg.saturating_sub(1).max(1); + } + } + } + + nth = std::cmp::min(nth, nk0.max(1) as u64); + + let args = KArgsCpy { + nk0, + ne00, + ne01, + ne02, + ne03, + nb00, + nb01, + nb02, + nb03, + ne0: dst_layout.ne[0], + ne1: dst_layout.ne[1], + ne2: dst_layout.ne[2], + ne3: dst_layout.ne[3], + nb0: dst_layout.nb[0], + nb1: dst_layout.nb[1], + nb2: dst_layout.nb[2], + nb3: dst_layout.nb[3], + }; + + let nw0 = if nrptg == 1 { + (nk0.max(1) as u64).div_ceil(nth) + } else { + 1 + }; + let width = nw0 + .checked_mul((ne01.max(1) as u64).div_ceil(nrptg)) + .ok_or_else(|| "overflow computing cpy threadgroup width".to_string())?; + + runtime.dispatch_compute( + &stage.pipeline, + bytes_of(&args), + &[ + buffer_ref(compiled, 1, src_id), + buffer_ref_with_offset(compiled, 2, dst_id, dst_extra_offset)?, + ], + &[], + MetalSize { + width, + height: ne02.max(1) as u64, + depth: ne03.max(1) as u64, + }, + MetalSize { + width: nth, + height: nrptg, + depth: 1, + }, + ) +} + +#[derive(Clone, Copy, Debug, Default)] +struct Shape4 { + ne: [i32; 4], + nb: [u64; 4], + numel: usize, +} + +#[derive(Clone, Copy, Debug)] +struct Layout4 { + ne: [i64; 4], + nb: [u64; 4], +} + +impl Layout4 { + fn from_tensor(tensor: &Tensor) -> Result { + Ok(Self { + ne: [ + i64_dim(tensor, 0)?, + i64_dim(tensor, 1)?, + i64_dim(tensor, 2)?, + i64_dim(tensor, 3)?, + ], + nb: [ + u64::try_from(tensor.nb[0]).map_err(|_| "layout nb0 exceeds u64".to_string())?, + u64::try_from(tensor.nb[1]).map_err(|_| "layout nb1 exceeds u64".to_string())?, + u64::try_from(tensor.nb[2]).map_err(|_| "layout nb2 exceeds u64".to_string())?, + u64::try_from(tensor.nb[3]).map_err(|_| "layout nb3 exceeds u64".to_string())?, + ], + }) + } +} + +fn shape4(tensor: &Tensor) -> Result { + let mut ne = [1i32; 4]; + let mut nb = [0u64; 4]; + for i in 0..4 { + ne[i] = i32::try_from(tensor.ne[i]).map_err(|_| { + format!( + "tensor '{}' ne[{}] exceeds i32", + tensor.name().unwrap_or(""), + i + ) + })?; + nb[i] = u64::try_from(tensor.nb[i]).map_err(|_| { + format!( + "tensor '{}' nb[{}] exceeds u64", + tensor.name().unwrap_or(""), + i + ) + })?; + } + let numel = usize::try_from(tensor.nelements()).map_err(|_| { + format!( + "tensor '{}' numel exceeds usize", + tensor.name().unwrap_or("") + ) + })?; + Ok(Shape4 { ne, nb, numel }) +} + +fn nrows(shape: &Shape4) -> usize { + (shape.ne[1] as usize) + .saturating_mul(shape.ne[2] as usize) + .saturating_mul(shape.ne[3] as usize) +} + +fn binding( + compiled: &MetalCompiledGraph, + tensor_id: TensorId, +) -> Result<&MetalTensorBinding, String> { + compiled + .bindings + .get(&tensor_id) + .ok_or_else(|| format!("compiled graph has no binding for tensor {}", tensor_id)) +} + +fn buffer_ref<'a>( + compiled: &'a MetalCompiledGraph, + index: u64, + tensor_id: TensorId, +) -> MetalBufferBindingRef<'a> { + let binding = compiled.bindings.get(&tensor_id).unwrap(); + MetalBufferBindingRef { + index, + buffer: &compiled.main_buffer, + offset_bytes: binding.offset_bytes, + } +} + +fn buffer_ref_with_offset<'a>( + compiled: &'a MetalCompiledGraph, + index: u64, + tensor_id: TensorId, + extra_offset: usize, +) -> Result, String> { + let binding = binding(compiled, tensor_id)?; + Ok(MetalBufferBindingRef { + index, + buffer: &compiled.main_buffer, + offset_bytes: binding + .offset_bytes + .checked_add(extra_offset) + .ok_or_else(|| format!("buffer binding offset overflow for tensor {}", tensor_id))?, + }) +} + +fn tail_buffer_ref<'a>( + compiled: &'a MetalCompiledGraph, + index: u64, + offset_bytes: usize, +) -> Result, String> { + let buffer = compiled + .tail_buffer + .as_ref() + .ok_or_else(|| "compiled graph is missing a tail buffer".to_string())?; + if offset_bytes > compiled.tail_buffer_size { + return Err(format!( + "tail buffer offset {} exceeds size {}", + offset_bytes, compiled.tail_buffer_size + )); + } + Ok(MetalBufferBindingRef { + index, + buffer, + offset_bytes, + }) +} + +fn tail_node_buffer_ref<'a>( + compiled: &'a MetalCompiledGraph, + node: &MetalCompiledNode, + index: u64, + local_offset: usize, +) -> Result, String> { + tail_buffer_ref( + compiled, + index, + node.tail_offset_bytes + .checked_add(local_offset) + .ok_or_else(|| "tail node buffer offset overflow".to_string())?, + ) +} + +fn dummy_buffer_ref<'a>(index: u64, buffer: &'a super::MetalBuffer) -> MetalBufferBindingRef<'a> { + MetalBufferBindingRef { + index, + buffer, + offset_bytes: 0, + } +} + +fn stage_kind<'a>( + node: &'a MetalCompiledNode, + kind: MetalStageKind, + op: Op, +) -> Result<&'a MetalCompiledStage, String> { + node.stages + .iter() + .find(|stage| stage.kind == kind) + .ok_or_else(|| format!("compiled node for op {} has no {:?} stage", op.name(), kind)) +} + +fn main_stage<'a>(node: &'a MetalCompiledNode, op: Op) -> Result<&'a MetalCompiledStage, String> { + stage_kind(node, MetalStageKind::Main, op) +} + +fn tensor_src(tensor: &Tensor, index: usize) -> Result { + tensor.src[index].ok_or_else(|| { + format!( + "tensor '{}' is missing source {}", + tensor.name().unwrap_or(""), + index + ) + }) +} + +fn tensor_src_opt(tensor: &Tensor, index: usize) -> Option { + tensor.src.get(index).and_then(|src| *src) +} + +fn i32_dim(tensor: &Tensor, dim: usize) -> Result { + i32::try_from(tensor.ne[dim]).map_err(|_| { + format!( + "tensor '{}' dim {} exceeds i32", + tensor.name().unwrap_or(""), + dim + ) + }) +} + +fn i64_dim(tensor: &Tensor, dim: usize) -> Result { + Ok(tensor.ne[dim]) +} + +fn constant_i16(constants: &[super::FunctionConstant], idx: i32) -> Result { + constants + .iter() + .find(|constant| constant.idx == idx) + .ok_or_else(|| format!("missing function constant {}", idx)) + .and_then(|constant| match constant.value { + FunctionConstantValue::Int16(value) => Ok(value), + _ => Err(format!("function constant {} is not i16", idx)), + }) +} + +fn constant_i32(constants: &[super::FunctionConstant], idx: i32) -> Result { + constants + .iter() + .find(|constant| constant.idx == idx) + .ok_or_else(|| format!("missing function constant {}", idx)) + .and_then(|constant| match constant.value { + FunctionConstantValue::Int32(value) => Ok(value), + _ => Err(format!("function constant {} is not i32", idx)), + }) +} + +fn flash_attn_supported_head_dim(d: usize) -> bool { + matches!( + d, + 32 | 40 | 48 | 64 | 72 | 80 | 96 | 112 | 128 | 192 | 256 | 576 + ) +} + +fn flash_attn_ext_use_vec(q: &Tensor) -> bool { + q.ne[1] < 20 && q.ne[0] % 32 == 0 +} + +fn flash_attn_ext_vec_nsg(k: &Tensor) -> i32 { + let mut nsg = 1_i32; + while 2 * 32 * nsg as i64 * i64::from(OP_FLASH_ATTN_EXT_VEC_NCPSG) < k.ne[1] && nsg < 4 { + nsg *= 2; + } + nsg +} + +fn flash_attn_ext_extra_pad_bytes( + q: &Tensor, + k: &Tensor, + v: &Tensor, + mask: Option<&Tensor>, +) -> Result { + let has_mask = mask.is_some(); + let mask_bytes = if has_mask { + ggml_type_size_for_type(TensorType::F16) + * usize::try_from(mask.unwrap().ne[1] * mask.unwrap().ne[2] * mask.unwrap().ne[3]) + .map_err(|_| "flash_attn pad mask bytes overflow".to_string())? + } else { + 0 + }; + let ncpsg = if flash_attn_ext_use_vec(q) { + OP_FLASH_ATTN_EXT_VEC_NCPSG + } else { + 64 + }; + Ok( + usize::try_from(ncpsg).map_err(|_| "flash_attn ncpsg overflow".to_string())? + * (k.nb[1] + * usize::try_from(k.ne[2] * k.ne[3]) + .map_err(|_| "flash_attn k dims overflow".to_string())? + + v.nb[1] + * usize::try_from(v.ne[2] * v.ne[3]) + .map_err(|_| "flash_attn v dims overflow".to_string())? + + mask_bytes), + ) +} + +fn flash_attn_ext_extra_blk_bytes(q: &Tensor, mask: Option<&Tensor>) -> Result { + let Some(mask) = mask else { + return Ok(0); + }; + let nqptg = if flash_attn_ext_use_vec(q) { + 1_i64 + } else { + 8_i64 + }; + let ncpsg = if flash_attn_ext_use_vec(q) { + 32_i64 + } else { + 64_i64 + }; + let ne1 = (q.ne[1] + nqptg - 1) / nqptg; + let ne0 = (mask.ne[0] + ncpsg - 1) / ncpsg; + let bytes = usize::try_from(ne0 * ne1 * mask.ne[2] * mask.ne[3]) + .map_err(|_| "flash_attn blk bytes overflow".to_string())?; + Ok(ggml_pad(bytes * std::mem::size_of::(), 32)) +} + +fn flash_attn_ext_extra_tmp_bytes(q: &Tensor, v: &Tensor) -> Result { + let nwg = 32_i64; + let ne01_max = q.ne[1].min(32); + let items = ne01_max * q.ne[2] * q.ne[3] * nwg * (v.ne[0] + 2); + Ok(ggml_type_size_for_type(TensorType::F32) + * usize::try_from(items).map_err(|_| "flash_attn tmp bytes overflow".to_string())?) +} + +fn mul_mat_id_extra_tpe_bytes(src0: &Tensor) -> Result { + Ok(ggml_type_size_for_type(TensorType::I32) + * usize::try_from(src0.ne[2]).map_err(|_| "mul_mat_id tpe bytes overflow".to_string())?) +} + +fn default_flash_mask_shape(q: &Shape4, k: &Shape4) -> Result { + let nb31 = (u64::try_from(k.ne[1]) + .map_err(|_| "flash_attn default mask ne1 overflow".to_string())?) + .checked_mul(ggml_type_size_for_type(TensorType::F16) as u64) + .ok_or_else(|| "flash_attn default mask nb31 overflow".to_string())?; + let nb32 = nb31 + .checked_mul( + u64::try_from(q.ne[1]).map_err(|_| "flash_attn default mask q overflow".to_string())?, + ) + .ok_or_else(|| "flash_attn default mask nb32 overflow".to_string())?; + Ok(Shape4 { + ne: [k.ne[1], q.ne[1], 1, 1], + nb: [ + ggml_type_size_for_type(TensorType::F16) as u64, + nb31, + nb32, + nb32, + ], + numel: usize::try_from(k.ne[1]) + .unwrap_or(0) + .saturating_mul(usize::try_from(q.ne[1]).unwrap_or(0)), + }) +} + +fn parse_trailing_i32(value: &str, marker: &str) -> Result { + value + .rsplit_once(marker) + .ok_or_else(|| format!("'{}' does not contain '{}'", value, marker)) + .and_then(|(_, tail)| { + tail.parse::() + .map_err(|err| format!("failed to parse integer from '{}': {}", value, err)) + }) +} + +fn bytes_of(value: &T) -> &[u8] { + unsafe { std::slice::from_raw_parts(value as *const T as *const u8, std::mem::size_of::()) } +} + +#[cfg(test)] +mod tests { + use super::*; + use crate::context::Context; + use crate::core::InitParams; + use crate::graph::Graph; + use crate::op::Op; + use crate::tensor::{ggml_row_size_for_type, BufferUsage, TensorType}; + + #[test] + fn prepare_graph_collects_bindings_and_tail_bytes() { + let mut ctx = Context::new(InitParams { + mem_size: 1 << 20, + mem_buffer: None, + no_alloc: false, + }); + + let a = ctx + .new_tensor_2d(TensorType::F32, 257, 4, BufferUsage::Activations) + .unwrap(); + let out = ctx + .new_tensor_2d(TensorType::I32, 257, 4, BufferUsage::Activations) + .unwrap(); + + { + let tensor = ctx.tensor_mut(out).unwrap(); + tensor.op = Op::Argsort; + tensor.src[0] = Some(a); + tensor.set_op_param_i32(0, 0); + } + + let mut graph = Graph::new(); + graph.add_leaf(a); + graph.add_node(out); + + let prepared = prepare_graph(&ctx, &graph, MetalDeviceFeatures::default()).unwrap(); + assert_eq!(prepared.nodes.len(), 1); + assert_eq!(prepared.bindings.len(), 2); + assert!(prepared.tail_buffer_size >= ctx.tensor(out).unwrap().nbytes()); + assert!(prepared.main_buffer_size >= ctx.used_mem()); + } + + #[test] + fn prepare_graph_assigns_distinct_tail_offsets_for_temp_nodes() { + let mut ctx = Context::new(InitParams { + mem_size: 1 << 20, + mem_buffer: None, + no_alloc: false, + }); + + let a = ctx + .new_tensor_2d(TensorType::F32, 257, 4, BufferUsage::Activations) + .unwrap(); + let sort = ctx.argsort(a, BufferUsage::Activations).unwrap(); + let topk = ctx.top_k(a, 8, BufferUsage::Activations).unwrap(); + + let mut graph = Graph::new(); + graph.add_leaf(a); + graph.add_node(sort); + graph.add_node(topk); + + let prepared = prepare_graph(&ctx, &graph, MetalDeviceFeatures::default()).unwrap(); + assert_eq!(prepared.nodes.len(), 2); + assert!(prepared.nodes[0].tail_offset_bytes < prepared.tail_buffer_size); + assert!(prepared.nodes[1].tail_offset_bytes < prepared.tail_buffer_size); + assert_ne!( + prepared.nodes[0].tail_offset_bytes, + prepared.nodes[1].tail_offset_bytes + ); + } + + #[test] + fn prepare_graph_resolves_view_offsets() { + let mut ctx = Context::new(InitParams { + mem_size: 1 << 20, + mem_buffer: None, + no_alloc: false, + }); + + let src = ctx + .new_tensor_2d(TensorType::F32, 16, 8, BufferUsage::Activations) + .unwrap(); + let view = ctx + .view(src, TensorType::F32, &[8, 8], &[4, 64], 32) + .unwrap(); + { + let tensor = ctx.tensor_mut(view).unwrap(); + tensor.op = Op::Cont; + tensor.src[0] = Some(src); + } + + let mut graph = Graph::new(); + graph.add_leaf(src); + graph.add_node(view); + + let prepared = prepare_graph(&ctx, &graph, MetalDeviceFeatures::default()).unwrap(); + let src_offset = prepared.bindings.get(&src).unwrap().offset_bytes; + let view_offset = prepared.bindings.get(&view).unwrap().offset_bytes; + assert_eq!(view_offset, src_offset + 32); + } + + #[test] + fn executes_rope_graph_on_metal_when_available() { + let runtime = match MetalRuntime::new() { + Ok(runtime) => runtime, + Err(_) => return, + }; + + let mut ctx = Context::new(InitParams { + mem_size: 1 << 20, + mem_buffer: None, + no_alloc: false, + }); + + let src = ctx + .new_tensor_3d(TensorType::F32, 4, 1, 2, BufferUsage::Activations) + .unwrap(); + let pos = ctx + .new_tensor_1d(TensorType::I32, 2, BufferUsage::Activations) + .unwrap(); + let rope = ctx.rope(src, pos, 4, 0, BufferUsage::Activations).unwrap(); + + let src_values = vec![1.0f32, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0]; + let pos_values = vec![0_i32, 1_i32]; + ctx.write_tensor_data(src, &f32s_to_bytes(&src_values)) + .unwrap(); + ctx.write_tensor_data(pos, &i32s_to_bytes(&pos_values)) + .unwrap(); + + let mut graph = Graph::new(); + graph.build_forward_expand(&ctx, rope).unwrap(); + + let prepared = prepare_graph(&ctx, &graph, runtime.features()).unwrap(); + let session = MetalGraphSession::from_runtime( + runtime, + &ctx, + &prepared, + BufferStorageMode::Shared, + BufferStorageMode::Shared, + ) + .unwrap(); + + let execution = session.execute(&ctx, &[], &[rope]).unwrap(); + let actual = bytes_to_f32s(execution.outputs.get(&rope).unwrap()); + let expected = cpu_rope_norm_f32(&src_values, &pos_values, 4, 1, 2); + + assert_eq!(actual.len(), expected.len()); + for (a, e) in actual.iter().zip(expected.iter()) { + assert!( + (a - e).abs() < 1.0e-4, + "rope output mismatch: actual={} expected={}", + a, + e + ); + } + } + + #[test] + fn executes_rope_multi_single_token_consistently_on_metal_when_available() { + let runtime = match MetalRuntime::new() { + Ok(runtime) => runtime, + Err(_) => return, + }; + + let mut full_ctx = Context::new(InitParams { + mem_size: 1 << 20, + mem_buffer: None, + no_alloc: false, + }); + + let d = 128_i64; + let n_head = 8_i64; + let n_tokens = 2_i64; + + let src_full = full_ctx + .new_tensor_3d( + TensorType::F32, + d, + n_head, + n_tokens, + BufferUsage::Activations, + ) + .unwrap(); + let pos_full = full_ctx + .new_tensor_1d(TensorType::I32, n_tokens, BufferUsage::Activations) + .unwrap(); + let rope_full = full_ctx + .rope_multi( + src_full, + pos_full, + None, + 32, + [11, 11, 10, 0], + crate::GGML_ROPE_TYPE_IMROPE, + 262_144, + 1_000_000.0, + 1.0, + 0.0, + 1.0, + 32.0, + 1.0, + BufferUsage::Activations, + ) + .unwrap(); + + let src_full_values = patterned_f32s((d * n_head * n_tokens) as usize, 0.05, -0.002); + full_ctx + .write_tensor_data(src_full, &f32s_to_bytes(&src_full_values)) + .unwrap(); + full_ctx + .write_tensor_data(pos_full, &i32s_to_bytes(&[0, 1])) + .unwrap(); + + let mut full_graph = Graph::new(); + full_graph + .build_forward_expand(&full_ctx, rope_full) + .unwrap(); + + let full_prepared = prepare_graph(&full_ctx, &full_graph, runtime.features()).unwrap(); + let full_session = MetalGraphSession::from_runtime( + runtime, + &full_ctx, + &full_prepared, + BufferStorageMode::Shared, + BufferStorageMode::Shared, + ) + .unwrap(); + let full_execution = full_session.execute(&full_ctx, &[], &[rope_full]).unwrap(); + let full_values = bytes_to_f32s(full_execution.outputs.get(&rope_full).unwrap()); + let token_width = (d * n_head) as usize; + let full_last_token = full_values[full_values.len() - token_width..].to_vec(); + + let runtime = match MetalRuntime::new() { + Ok(runtime) => runtime, + Err(_) => return, + }; + let mut step_ctx = Context::new(InitParams { + mem_size: 1 << 20, + mem_buffer: None, + no_alloc: false, + }); + let src_step = step_ctx + .new_tensor_3d(TensorType::F32, d, n_head, 1, BufferUsage::Activations) + .unwrap(); + let pos_step = step_ctx + .new_tensor_1d(TensorType::I32, 1, BufferUsage::Activations) + .unwrap(); + let rope_step = step_ctx + .rope_multi( + src_step, + pos_step, + None, + 32, + [11, 11, 10, 0], + crate::GGML_ROPE_TYPE_IMROPE, + 262_144, + 1_000_000.0, + 1.0, + 0.0, + 1.0, + 32.0, + 1.0, + BufferUsage::Activations, + ) + .unwrap(); + + let src_step_values = { + let mut values = Vec::with_capacity(token_width); + for head in 0..(n_head as usize) { + let base_offset = token_width + head * (d as usize); + values.extend_from_slice(&src_full_values[base_offset..base_offset + d as usize]); + } + values + }; + step_ctx + .write_tensor_data(src_step, &f32s_to_bytes(&src_step_values)) + .unwrap(); + step_ctx + .write_tensor_data(pos_step, &i32s_to_bytes(&[1])) + .unwrap(); + + let mut step_graph = Graph::new(); + step_graph + .build_forward_expand(&step_ctx, rope_step) + .unwrap(); + + let step_prepared = prepare_graph(&step_ctx, &step_graph, runtime.features()).unwrap(); + let step_session = MetalGraphSession::from_runtime( + runtime, + &step_ctx, + &step_prepared, + BufferStorageMode::Shared, + BufferStorageMode::Shared, + ) + .unwrap(); + let step_execution = step_session.execute(&step_ctx, &[], &[rope_step]).unwrap(); + let step_values = bytes_to_f32s(step_execution.outputs.get(&rope_step).unwrap()); + + assert_eq!(step_values.len(), full_last_token.len()); + for (a, e) in step_values.iter().zip(full_last_token.iter()) { + assert!( + (a - e).abs() < 1.0e-4, + "rope_multi token1 mismatch: actual={} expected={}", + a, + e + ); + } + } + + #[test] + fn executes_rope_multi_kv_head_shape_consistently_on_metal_when_available() { + let runtime = match MetalRuntime::new() { + Ok(runtime) => runtime, + Err(_) => return, + }; + + let mut full_ctx = Context::new(InitParams { + mem_size: 1 << 20, + mem_buffer: None, + no_alloc: false, + }); + + let d = 128_i64; + let n_head = 2_i64; + let n_tokens = 2_i64; + + let src_full = full_ctx + .new_tensor_3d( + TensorType::F32, + d, + n_head, + n_tokens, + BufferUsage::Activations, + ) + .unwrap(); + let pos_full = full_ctx + .new_tensor_1d(TensorType::I32, n_tokens, BufferUsage::Activations) + .unwrap(); + let rope_full = full_ctx + .rope_multi( + src_full, + pos_full, + None, + 32, + [11, 11, 10, 0], + crate::GGML_ROPE_TYPE_IMROPE, + 262_144, + 1_000_000.0, + 1.0, + 0.0, + 1.0, + 32.0, + 1.0, + BufferUsage::Activations, + ) + .unwrap(); + + let src_full_values = patterned_f32s((d * n_head * n_tokens) as usize, 0.02, 0.007); + full_ctx + .write_tensor_data(src_full, &f32s_to_bytes(&src_full_values)) + .unwrap(); + full_ctx + .write_tensor_data(pos_full, &i32s_to_bytes(&[0, 1])) + .unwrap(); + + let mut full_graph = Graph::new(); + full_graph + .build_forward_expand(&full_ctx, rope_full) + .unwrap(); + + let full_prepared = prepare_graph(&full_ctx, &full_graph, runtime.features()).unwrap(); + let full_session = MetalGraphSession::from_runtime( + runtime, + &full_ctx, + &full_prepared, + BufferStorageMode::Shared, + BufferStorageMode::Shared, + ) + .unwrap(); + let full_execution = full_session.execute(&full_ctx, &[], &[rope_full]).unwrap(); + let full_values = bytes_to_f32s(full_execution.outputs.get(&rope_full).unwrap()); + let token_width = (d * n_head) as usize; + let full_last_token = full_values[full_values.len() - token_width..].to_vec(); + + let runtime = match MetalRuntime::new() { + Ok(runtime) => runtime, + Err(_) => return, + }; + let mut step_ctx = Context::new(InitParams { + mem_size: 1 << 20, + mem_buffer: None, + no_alloc: false, + }); + let src_step = step_ctx + .new_tensor_3d(TensorType::F32, d, n_head, 1, BufferUsage::Activations) + .unwrap(); + let pos_step = step_ctx + .new_tensor_1d(TensorType::I32, 1, BufferUsage::Activations) + .unwrap(); + let rope_step = step_ctx + .rope_multi( + src_step, + pos_step, + None, + 32, + [11, 11, 10, 0], + crate::GGML_ROPE_TYPE_IMROPE, + 262_144, + 1_000_000.0, + 1.0, + 0.0, + 1.0, + 32.0, + 1.0, + BufferUsage::Activations, + ) + .unwrap(); + + let src_step_values = { + let mut values = Vec::with_capacity(token_width); + for head in 0..(n_head as usize) { + let base_offset = token_width + head * (d as usize); + values.extend_from_slice(&src_full_values[base_offset..base_offset + d as usize]); + } + values + }; + step_ctx + .write_tensor_data(src_step, &f32s_to_bytes(&src_step_values)) + .unwrap(); + step_ctx + .write_tensor_data(pos_step, &i32s_to_bytes(&[1])) + .unwrap(); + + let mut step_graph = Graph::new(); + step_graph + .build_forward_expand(&step_ctx, rope_step) + .unwrap(); + + let step_prepared = prepare_graph(&step_ctx, &step_graph, runtime.features()).unwrap(); + let step_session = MetalGraphSession::from_runtime( + runtime, + &step_ctx, + &step_prepared, + BufferStorageMode::Shared, + BufferStorageMode::Shared, + ) + .unwrap(); + let step_execution = step_session.execute(&step_ctx, &[], &[rope_step]).unwrap(); + let step_values = bytes_to_f32s(step_execution.outputs.get(&rope_step).unwrap()); + + assert_eq!(step_values.len(), full_last_token.len()); + for (a, e) in step_values.iter().zip(full_last_token.iter()) { + assert!( + (a - e).abs() < 1.0e-4, + "rope_multi kv-head token1 mismatch: actual={} expected={}", + a, + e + ); + } + } + + #[test] + fn executes_rms_norm_mul_single_token_consistently_on_metal_when_available() { + let runtime = match MetalRuntime::new() { + Ok(runtime) => runtime, + Err(_) => return, + }; + + let mut full_ctx = Context::new(InitParams { + mem_size: 1 << 20, + mem_buffer: None, + no_alloc: false, + }); + + let d = 128_i64; + let n_head = 8_i64; + let n_tokens = 2_i64; + let eps = 1.0e-6f32; + + let src_full = full_ctx + .new_tensor_3d( + TensorType::F32, + d, + n_head, + n_tokens, + BufferUsage::Activations, + ) + .unwrap(); + let weight = full_ctx + .new_tensor_2d(TensorType::F32, d, n_head, BufferUsage::Weights) + .unwrap(); + let norm_full = full_ctx + .rms_norm_eps(src_full, eps, BufferUsage::Activations) + .unwrap(); + let scaled_full = full_ctx + .binary_like_a(Op::Mul, norm_full, weight, BufferUsage::Activations) + .unwrap(); + + let src_full_values = patterned_f32s((d * n_head * n_tokens) as usize, -0.1, 0.003); + let weight_values = patterned_f32s((d * n_head) as usize, 0.5, -0.001); + full_ctx + .write_tensor_data(src_full, &f32s_to_bytes(&src_full_values)) + .unwrap(); + full_ctx + .write_tensor_data(weight, &f32s_to_bytes(&weight_values)) + .unwrap(); + + let mut full_graph = Graph::new(); + full_graph + .build_forward_expand(&full_ctx, scaled_full) + .unwrap(); + + let full_prepared = prepare_graph(&full_ctx, &full_graph, runtime.features()).unwrap(); + let full_session = MetalGraphSession::from_runtime( + runtime, + &full_ctx, + &full_prepared, + BufferStorageMode::Shared, + BufferStorageMode::Shared, + ) + .unwrap(); + let full_execution = full_session + .execute(&full_ctx, &[], &[scaled_full]) + .unwrap(); + let full_values = bytes_to_f32s(full_execution.outputs.get(&scaled_full).unwrap()); + let token_width = (d * n_head) as usize; + let full_last_token = full_values[full_values.len() - token_width..].to_vec(); + + let runtime = match MetalRuntime::new() { + Ok(runtime) => runtime, + Err(_) => return, + }; + let mut step_ctx = Context::new(InitParams { + mem_size: 1 << 20, + mem_buffer: None, + no_alloc: false, + }); + let src_step = step_ctx + .new_tensor_3d(TensorType::F32, d, n_head, 1, BufferUsage::Activations) + .unwrap(); + let weight_step = step_ctx + .new_tensor_2d(TensorType::F32, d, n_head, BufferUsage::Weights) + .unwrap(); + let norm_step = step_ctx + .rms_norm_eps(src_step, eps, BufferUsage::Activations) + .unwrap(); + let scaled_step = step_ctx + .binary_like_a(Op::Mul, norm_step, weight_step, BufferUsage::Activations) + .unwrap(); + + let src_step_values = { + let mut values = Vec::with_capacity(token_width); + for head in 0..(n_head as usize) { + let base_offset = token_width + head * (d as usize); + values.extend_from_slice(&src_full_values[base_offset..base_offset + d as usize]); + } + values + }; + step_ctx + .write_tensor_data(src_step, &f32s_to_bytes(&src_step_values)) + .unwrap(); + step_ctx + .write_tensor_data(weight_step, &f32s_to_bytes(&weight_values)) + .unwrap(); + + let mut step_graph = Graph::new(); + step_graph + .build_forward_expand(&step_ctx, scaled_step) + .unwrap(); + + let step_prepared = prepare_graph(&step_ctx, &step_graph, runtime.features()).unwrap(); + let step_session = MetalGraphSession::from_runtime( + runtime, + &step_ctx, + &step_prepared, + BufferStorageMode::Shared, + BufferStorageMode::Shared, + ) + .unwrap(); + let step_execution = step_session + .execute(&step_ctx, &[], &[scaled_step]) + .unwrap(); + let step_values = bytes_to_f32s(step_execution.outputs.get(&scaled_step).unwrap()); + + assert_eq!(step_values.len(), full_last_token.len()); + for (a, e) in step_values.iter().zip(full_last_token.iter()) { + assert!( + (a - e).abs() < 1.0e-4, + "rms_norm_mul token1 mismatch: actual={} expected={}", + a, + e + ); + } + } + + #[test] + fn executes_flash_attn_vec_graph_on_metal_when_available() { + let runtime = match MetalRuntime::new() { + Ok(runtime) => runtime, + Err(_) => return, + }; + + let mut ctx = Context::new(InitParams { + mem_size: 1 << 20, + mem_buffer: None, + no_alloc: false, + }); + + let d = 32_i64; + let n_q = 1_i64; + let n_kv = 3_i64; + + let q = ctx + .new_tensor_4d(TensorType::F32, d, n_q, 1, 1, BufferUsage::Activations) + .unwrap(); + let k = ctx + .new_tensor_4d(TensorType::F32, d, n_kv, 1, 1, BufferUsage::Activations) + .unwrap(); + let v = ctx + .new_tensor_4d(TensorType::F32, d, n_kv, 1, 1, BufferUsage::Activations) + .unwrap(); + let attn = ctx + .flash_attn_ext( + q, + k, + v, + None, + 1.0 / (d as f32).sqrt(), + 0.0, + 0.0, + BufferUsage::Activations, + ) + .unwrap(); + + let q_values = patterned_f32s(d as usize, 0.1, 0.01); + let k_values = patterned_f32s((d * n_kv) as usize, -0.05, 0.02); + let v_values = patterned_f32s((d * n_kv) as usize, 0.2, -0.015); + ctx.write_tensor_data(q, &f32s_to_bytes(&q_values)).unwrap(); + ctx.write_tensor_data(k, &f32s_to_bytes(&k_values)).unwrap(); + ctx.write_tensor_data(v, &f32s_to_bytes(&v_values)).unwrap(); + + let mut graph = Graph::new(); + graph.build_forward_expand(&ctx, attn).unwrap(); + + let prepared = prepare_graph(&ctx, &graph, runtime.features()).unwrap(); + let session = MetalGraphSession::from_runtime( + runtime, + &ctx, + &prepared, + BufferStorageMode::Shared, + BufferStorageMode::Shared, + ) + .unwrap(); + + let execution = session.execute(&ctx, &[], &[attn]).unwrap(); + let actual = bytes_to_f32s(execution.outputs.get(&attn).unwrap()); + let expected = cpu_flash_attn_f32( + &q_values, + &k_values, + &v_values, + d as usize, + n_q as usize, + n_kv as usize, + ); + + assert_eq!(actual.len(), expected.len()); + for (a, e) in actual.iter().zip(expected.iter()) { + assert!( + (a - e).abs() < 2.0e-3, + "flash_attn output mismatch: actual={} expected={}", + a, + e + ); + } + } + + #[test] + fn executes_flash_attn_vec_from_strided_cache_view_on_metal_when_available() { + let runtime = match MetalRuntime::new() { + Ok(runtime) => runtime, + Err(_) => return, + }; + + let mut ctx = Context::new(InitParams { + mem_size: 1 << 20, + mem_buffer: None, + no_alloc: false, + }); + + let d = 32_i64; + let n_q = 1_i64; + let n_kv = 2_i64; + + let q = ctx + .new_tensor_4d(TensorType::F32, d, n_q, 1, 1, BufferUsage::Activations) + .unwrap(); + let k_cache = ctx + .new_tensor_3d(TensorType::F32, d, n_kv, 1, BufferUsage::State) + .unwrap(); + let v_cache = ctx + .new_tensor_3d(TensorType::F32, d, n_kv, 1, BufferUsage::State) + .unwrap(); + let k_cur = ctx + .new_tensor_2d(TensorType::F32, d, 1, BufferUsage::Activations) + .unwrap(); + let v_cur = ctx + .new_tensor_2d(TensorType::F32, d, 1, BufferUsage::Activations) + .unwrap(); + let rows = ctx + .new_tensor_1d(TensorType::I32, 1, BufferUsage::Activations) + .unwrap(); + + let k_written = ctx + .set_rows(k_cache, k_cur, rows, BufferUsage::State) + .unwrap(); + let v_written = ctx + .set_rows(v_cache, v_cur, rows, BufferUsage::State) + .unwrap(); + + let k_view = ctx + .view_4d( + k_written, + d, + n_kv, + 1, + 1, + ggml_row_size_for_type(TensorType::F32, d).unwrap(), + ggml_row_size_for_type(TensorType::F32, d).unwrap(), + ggml_row_size_for_type(TensorType::F32, d * n_kv).unwrap(), + 0, + ) + .unwrap(); + let v_view = ctx + .view_4d( + v_written, + d, + n_kv, + 1, + 1, + ggml_row_size_for_type(TensorType::F32, d).unwrap(), + ggml_row_size_for_type(TensorType::F32, d).unwrap(), + ggml_row_size_for_type(TensorType::F32, d * n_kv).unwrap(), + 0, + ) + .unwrap(); + let attn = ctx + .flash_attn_ext( + q, + k_view, + v_view, + None, + 1.0 / (d as f32).sqrt(), + 0.0, + 0.0, + BufferUsage::Activations, + ) + .unwrap(); + + let q_values = patterned_f32s(d as usize, 0.25, -0.01); + let k0_values = patterned_f32s(d as usize, -0.15, 0.02); + let k1_values = patterned_f32s(d as usize, 0.35, -0.015); + let v0_values = patterned_f32s(d as usize, 0.05, 0.03); + let v1_values = patterned_f32s(d as usize, -0.2, 0.01); + + let mut k_cache_values = vec![0.0f32; (d * n_kv) as usize]; + let mut v_cache_values = vec![0.0f32; (d * n_kv) as usize]; + k_cache_values[..d as usize].copy_from_slice(&k0_values); + v_cache_values[..d as usize].copy_from_slice(&v0_values); + + ctx.write_tensor_data(q, &f32s_to_bytes(&q_values)).unwrap(); + ctx.write_tensor_data(k_cache, &f32s_to_bytes(&k_cache_values)) + .unwrap(); + ctx.write_tensor_data(v_cache, &f32s_to_bytes(&v_cache_values)) + .unwrap(); + ctx.write_tensor_data(k_cur, &f32s_to_bytes(&k1_values)) + .unwrap(); + ctx.write_tensor_data(v_cur, &f32s_to_bytes(&v1_values)) + .unwrap(); + ctx.write_tensor_data(rows, &i32s_to_bytes(&[1])).unwrap(); + + let mut graph = Graph::new(); + graph.build_forward_expand(&ctx, attn).unwrap(); + + let prepared = prepare_graph(&ctx, &graph, runtime.features()).unwrap(); + let session = MetalGraphSession::from_runtime( + runtime, + &ctx, + &prepared, + BufferStorageMode::Shared, + BufferStorageMode::Shared, + ) + .unwrap(); + + let execution = session.execute(&ctx, &[], &[attn]).unwrap(); + let actual = bytes_to_f32s(execution.outputs.get(&attn).unwrap()); + let mut expected_k = k0_values.clone(); + expected_k.extend_from_slice(&k1_values); + let mut expected_v = v0_values.clone(); + expected_v.extend_from_slice(&v1_values); + let expected = cpu_flash_attn_f32( + &q_values, + &expected_k, + &expected_v, + d as usize, + 1, + n_kv as usize, + ); + + assert_eq!(actual.len(), expected.len()); + for (a, e) in actual.iter().zip(expected.iter()) { + assert!( + (a - e).abs() < 2.0e-3, + "flash_attn strided cache output mismatch: actual={} expected={}", + a, + e + ); + } + } + + #[test] + fn executes_flash_attn_vec_from_strided_gqa_cache_view_on_metal_when_available() { + let runtime = match MetalRuntime::new() { + Ok(runtime) => runtime, + Err(_) => return, + }; + + let mut full_ctx = Context::new(InitParams { + mem_size: 1 << 20, + mem_buffer: None, + no_alloc: false, + }); + + let d = 32_i64; + let n_q = 2_i64; + let n_head = 4_i64; + let n_kv = 2_i64; + let n_kv_head = 2_i64; + + let q_base = full_ctx + .new_tensor_4d(TensorType::F32, d, n_head, n_q, 1, BufferUsage::Activations) + .unwrap(); + let k_base = full_ctx + .new_tensor_4d( + TensorType::F32, + d, + n_kv_head, + n_kv, + 1, + BufferUsage::Activations, + ) + .unwrap(); + let v_base = full_ctx + .new_tensor_4d( + TensorType::F32, + d, + n_kv_head, + n_kv, + 1, + BufferUsage::Activations, + ) + .unwrap(); + let q_full = full_ctx.permute(q_base, [0, 2, 1, 3]).unwrap(); + let k_full = full_ctx.permute(k_base, [0, 2, 1, 3]).unwrap(); + let v_full = full_ctx.permute(v_base, [0, 2, 1, 3]).unwrap(); + let full_attn = full_ctx + .flash_attn_ext( + q_full, + k_full, + v_full, + None, + 1.0 / (d as f32).sqrt(), + 0.0, + 0.0, + BufferUsage::Activations, + ) + .unwrap(); + + let q_base_values = patterned_f32s((d * n_q * n_head) as usize, -0.2, 0.004); + let k_base_values = patterned_f32s((d * n_kv * n_kv_head) as usize, 0.15, -0.006); + let v_base_values = patterned_f32s((d * n_kv * n_kv_head) as usize, -0.05, 0.008); + + full_ctx + .write_tensor_data(q_base, &f32s_to_bytes(&q_base_values)) + .unwrap(); + full_ctx + .write_tensor_data(k_base, &f32s_to_bytes(&k_base_values)) + .unwrap(); + full_ctx + .write_tensor_data(v_base, &f32s_to_bytes(&v_base_values)) + .unwrap(); + + let mut full_graph = Graph::new(); + full_graph + .build_forward_expand(&full_ctx, full_attn) + .unwrap(); + + let full_prepared = prepare_graph(&full_ctx, &full_graph, runtime.features()).unwrap(); + let full_session = MetalGraphSession::from_runtime( + runtime, + &full_ctx, + &full_prepared, + BufferStorageMode::Shared, + BufferStorageMode::Shared, + ) + .unwrap(); + let full_execution = full_session.execute(&full_ctx, &[], &[full_attn]).unwrap(); + let full_values = bytes_to_f32s(full_execution.outputs.get(&full_attn).unwrap()); + let last_token_width = (d * n_head) as usize; + let full_last_token = full_values[full_values.len() - last_token_width..].to_vec(); + + let mut decode_ctx = Context::new(InitParams { + mem_size: 1 << 20, + mem_buffer: None, + no_alloc: false, + }); + + let q_step = decode_ctx + .new_tensor_4d(TensorType::F32, d, 1, n_head, 1, BufferUsage::Activations) + .unwrap(); + let k_cache = decode_ctx + .new_tensor_3d(TensorType::F32, d * n_kv_head, n_kv, 1, BufferUsage::State) + .unwrap(); + let v_cache = decode_ctx + .new_tensor_3d(TensorType::F32, d * n_kv_head, n_kv, 1, BufferUsage::State) + .unwrap(); + let k_cur = decode_ctx + .new_tensor_2d(TensorType::F32, d * n_kv_head, 1, BufferUsage::Activations) + .unwrap(); + let v_cur = decode_ctx + .new_tensor_2d(TensorType::F32, d * n_kv_head, 1, BufferUsage::Activations) + .unwrap(); + let rows = decode_ctx + .new_tensor_1d(TensorType::I32, 1, BufferUsage::Activations) + .unwrap(); + + let k_written = decode_ctx + .set_rows(k_cache, k_cur, rows, BufferUsage::State) + .unwrap(); + let v_written = decode_ctx + .set_rows(v_cache, v_cur, rows, BufferUsage::State) + .unwrap(); + let k_view = decode_ctx + .view_4d( + k_written, + d, + n_kv, + n_kv_head, + 1, + ggml_row_size_for_type(TensorType::F32, d * n_kv_head).unwrap(), + ggml_row_size_for_type(TensorType::F32, d).unwrap(), + ggml_row_size_for_type(TensorType::F32, d * n_kv_head * n_kv).unwrap(), + 0, + ) + .unwrap(); + let v_view = decode_ctx + .view_4d( + v_written, + d, + n_kv, + n_kv_head, + 1, + ggml_row_size_for_type(TensorType::F32, d * n_kv_head).unwrap(), + ggml_row_size_for_type(TensorType::F32, d).unwrap(), + ggml_row_size_for_type(TensorType::F32, d * n_kv_head * n_kv).unwrap(), + 0, + ) + .unwrap(); + let decode_attn = decode_ctx + .flash_attn_ext( + q_step, + k_view, + v_view, + None, + 1.0 / (d as f32).sqrt(), + 0.0, + 0.0, + BufferUsage::Activations, + ) + .unwrap(); + + let q_step_values = { + let mut values = Vec::with_capacity(last_token_width); + for head in 0..(n_head as usize) { + let base_offset = (d as usize) * (n_head as usize) + head * (d as usize); + values.extend_from_slice(&q_base_values[base_offset..base_offset + d as usize]); + } + values + }; + let token0_k = { + let mut values = Vec::with_capacity((d * n_kv_head) as usize); + for head in 0..(n_kv_head as usize) { + let base_offset = head * (d as usize); + values.extend_from_slice(&k_base_values[base_offset..base_offset + d as usize]); + } + values + }; + let token1_k = { + let mut values = Vec::with_capacity((d * n_kv_head) as usize); + for head in 0..(n_kv_head as usize) { + let base_offset = (d as usize) * (n_kv_head as usize) + head * (d as usize); + values.extend_from_slice(&k_base_values[base_offset..base_offset + d as usize]); + } + values + }; + let token0_v = { + let mut values = Vec::with_capacity((d * n_kv_head) as usize); + for head in 0..(n_kv_head as usize) { + let base_offset = head * (d as usize); + values.extend_from_slice(&v_base_values[base_offset..base_offset + d as usize]); + } + values + }; + let token1_v = { + let mut values = Vec::with_capacity((d * n_kv_head) as usize); + for head in 0..(n_kv_head as usize) { + let base_offset = (d as usize) * (n_kv_head as usize) + head * (d as usize); + values.extend_from_slice(&v_base_values[base_offset..base_offset + d as usize]); + } + values + }; + let mut k_cache_values = vec![0.0f32; (d * n_kv_head * n_kv) as usize]; + let mut v_cache_values = vec![0.0f32; (d * n_kv_head * n_kv) as usize]; + k_cache_values[..token0_k.len()].copy_from_slice(&token0_k); + v_cache_values[..token0_v.len()].copy_from_slice(&token0_v); + + decode_ctx + .write_tensor_data(q_step, &f32s_to_bytes(&q_step_values)) + .unwrap(); + decode_ctx + .write_tensor_data(k_cache, &f32s_to_bytes(&k_cache_values)) + .unwrap(); + decode_ctx + .write_tensor_data(v_cache, &f32s_to_bytes(&v_cache_values)) + .unwrap(); + decode_ctx + .write_tensor_data(k_cur, &f32s_to_bytes(&token1_k)) + .unwrap(); + decode_ctx + .write_tensor_data(v_cur, &f32s_to_bytes(&token1_v)) + .unwrap(); + decode_ctx + .write_tensor_data(rows, &i32s_to_bytes(&[1])) + .unwrap(); + + let mut decode_graph = Graph::new(); + decode_graph + .build_forward_expand(&decode_ctx, decode_attn) + .unwrap(); + + let runtime = match MetalRuntime::new() { + Ok(runtime) => runtime, + Err(_) => return, + }; + let decode_prepared = + prepare_graph(&decode_ctx, &decode_graph, runtime.features()).unwrap(); + let decode_session = MetalGraphSession::from_runtime( + runtime, + &decode_ctx, + &decode_prepared, + BufferStorageMode::Shared, + BufferStorageMode::Shared, + ) + .unwrap(); + let decode_execution = decode_session + .execute(&decode_ctx, &[], &[decode_attn]) + .unwrap(); + let decode_values = bytes_to_f32s(decode_execution.outputs.get(&decode_attn).unwrap()); + + assert_eq!(decode_values.len(), full_last_token.len()); + for (a, e) in decode_values.iter().zip(full_last_token.iter()) { + assert!( + (a - e).abs() < 2.0e-3, + "flash_attn GQA cache output mismatch: actual={} expected={}", + a, + e + ); + } + } + + #[test] + fn executes_set_rows_graph_on_metal_when_available() { + let runtime = match MetalRuntime::new() { + Ok(runtime) => runtime, + Err(_) => return, + }; + + let mut ctx = Context::new(InitParams { + mem_size: 1 << 20, + mem_buffer: None, + no_alloc: false, + }); + + let dst = ctx + .new_tensor_2d(TensorType::F32, 4, 4, BufferUsage::State) + .unwrap(); + let src = ctx + .new_tensor_2d(TensorType::F32, 4, 2, BufferUsage::Activations) + .unwrap(); + let rows = ctx + .new_tensor_1d(TensorType::I32, 2, BufferUsage::Activations) + .unwrap(); + let out = ctx.set_rows(dst, src, rows, BufferUsage::State).unwrap(); + + ctx.write_tensor_data(dst, &f32s_to_bytes(&[0.0f32; 16])) + .unwrap(); + ctx.write_tensor_data( + src, + &f32s_to_bytes(&[ + 1.0, 2.0, 3.0, 4.0, // + 5.0, 6.0, 7.0, 8.0, + ]), + ) + .unwrap(); + ctx.write_tensor_data(rows, &i32s_to_bytes(&[1, 3])) + .unwrap(); + + let mut graph = Graph::new(); + graph.build_forward_expand(&ctx, out).unwrap(); + + let prepared = prepare_graph(&ctx, &graph, runtime.features()).unwrap(); + let session = MetalGraphSession::from_runtime( + runtime, + &ctx, + &prepared, + BufferStorageMode::Shared, + BufferStorageMode::Shared, + ) + .unwrap(); + + let execution = session.execute(&ctx, &[], &[out]).unwrap(); + let actual = bytes_to_f32s(execution.outputs.get(&out).unwrap()); + assert_eq!( + actual, + vec![ + 0.0, 0.0, 0.0, 0.0, // + 1.0, 2.0, 3.0, 4.0, // + 0.0, 0.0, 0.0, 0.0, // + 5.0, 6.0, 7.0, 8.0, + ] + ); + } + + #[test] + fn executes_add_sigmoid_graph_on_metal_when_available() { + let runtime = match MetalRuntime::new() { + Ok(runtime) => runtime, + Err(_) => return, + }; + + let mut ctx = Context::new(InitParams { + mem_size: 1 << 20, + mem_buffer: None, + no_alloc: false, + }); + + let a = ctx + .new_tensor_2d(TensorType::F32, 8, 2, BufferUsage::Activations) + .unwrap(); + let b = ctx + .new_tensor_2d(TensorType::F32, 8, 2, BufferUsage::Activations) + .unwrap(); + let add = ctx + .binary_like_a(Op::Add, a, b, BufferUsage::Activations) + .unwrap(); + let out = ctx + .unary(add, crate::op::UnaryOp::Sigmoid, BufferUsage::Activations) + .unwrap(); + + let a_values = patterned_f32s(16, -0.3, 0.07); + let b_values = patterned_f32s(16, 0.15, -0.05); + ctx.write_tensor_data(a, &f32s_to_bytes(&a_values)).unwrap(); + ctx.write_tensor_data(b, &f32s_to_bytes(&b_values)).unwrap(); + + let mut graph = Graph::new(); + graph.build_forward_expand(&ctx, out).unwrap(); + + let prepared = prepare_graph(&ctx, &graph, runtime.features()).unwrap(); + let session = MetalGraphSession::from_runtime( + runtime, + &ctx, + &prepared, + BufferStorageMode::Shared, + BufferStorageMode::Shared, + ) + .unwrap(); + + let execution = session.execute(&ctx, &[], &[out]).unwrap(); + let actual = bytes_to_f32s(execution.outputs.get(&out).unwrap()); + let expected = a_values + .iter() + .zip(b_values.iter()) + .map(|(a, b)| 1.0f32 / (1.0 + (-(a + b)).exp())) + .collect::>(); + + assert_eq!(actual.len(), expected.len()); + for (a, e) in actual.iter().zip(expected.iter()) { + assert!( + (a - e).abs() < 1.0e-5, + "add+sigmoid output mismatch: actual={} expected={}", + a, + e + ); + } + } + + #[test] + fn executes_softmax_graph_on_metal_when_available() { + let runtime = match MetalRuntime::new() { + Ok(runtime) => runtime, + Err(_) => return, + }; + + let mut ctx = Context::new(InitParams { + mem_size: 1 << 20, + mem_buffer: None, + no_alloc: false, + }); + + let src = ctx + .new_tensor_2d(TensorType::F32, 6, 2, BufferUsage::Activations) + .unwrap(); + let out = ctx.soft_max(src, BufferUsage::Activations).unwrap(); + let values = vec![ + 0.2, -0.1, 0.8, -0.7, 0.3, 0.5, // + -0.4, 1.2, 0.6, -0.2, 0.9, -1.0, + ]; + ctx.write_tensor_data(src, &f32s_to_bytes(&values)).unwrap(); + + let mut graph = Graph::new(); + graph.build_forward_expand(&ctx, out).unwrap(); + + let prepared = prepare_graph(&ctx, &graph, runtime.features()).unwrap(); + let session = MetalGraphSession::from_runtime( + runtime, + &ctx, + &prepared, + BufferStorageMode::Shared, + BufferStorageMode::Shared, + ) + .unwrap(); + + let execution = session.execute(&ctx, &[], &[out]).unwrap(); + let actual = bytes_to_f32s(execution.outputs.get(&out).unwrap()); + let expected = cpu_softmax_rows_f32(&values, 6); + + assert_eq!(actual.len(), expected.len()); + for (a, e) in actual.iter().zip(expected.iter()) { + assert!( + (a - e).abs() < 1.0e-5, + "softmax output mismatch: actual={} expected={}", + a, + e + ); + } + } + + #[test] + fn executes_sum_rows_graph_on_metal_when_available() { + let runtime = match MetalRuntime::new() { + Ok(runtime) => runtime, + Err(_) => return, + }; + + let mut ctx = Context::new(InitParams { + mem_size: 1 << 20, + mem_buffer: None, + no_alloc: false, + }); + + let src = ctx + .new_tensor_2d(TensorType::F32, 6, 3, BufferUsage::Activations) + .unwrap(); + let out = ctx.sum_rows(src, BufferUsage::Activations).unwrap(); + let values = vec![ + 0.2, -0.1, 0.8, -0.7, 0.3, 0.5, // + -0.4, 1.2, 0.6, -0.2, 0.9, -1.0, // + 1.0, 0.5, -0.3, 0.4, -0.6, 0.7, + ]; + ctx.write_tensor_data(src, &f32s_to_bytes(&values)).unwrap(); + + let mut graph = Graph::new(); + graph.build_forward_expand(&ctx, out).unwrap(); + + let prepared = prepare_graph(&ctx, &graph, runtime.features()).unwrap(); + let session = MetalGraphSession::from_runtime( + runtime, + &ctx, + &prepared, + BufferStorageMode::Shared, + BufferStorageMode::Shared, + ) + .unwrap(); + + let execution = session.execute(&ctx, &[], &[out]).unwrap(); + let actual = bytes_to_f32s(execution.outputs.get(&out).unwrap()); + let expected = cpu_sum_rows_f32(&values, 6); + + assert_eq!(actual.len(), expected.len()); + for (a, e) in actual.iter().zip(expected.iter()) { + assert!( + (a - e).abs() < 1.0e-5, + "sum_rows output mismatch: actual={} expected={}", + a, + e + ); + } + } + + #[test] + fn executes_top_k_graph_on_metal_when_available() { + let runtime = match MetalRuntime::new() { + Ok(runtime) => runtime, + Err(_) => return, + }; + + let mut ctx = Context::new(InitParams { + mem_size: 1 << 20, + mem_buffer: None, + no_alloc: false, + }); + + let src = ctx + .new_tensor_2d(TensorType::F32, 8, 2, BufferUsage::Activations) + .unwrap(); + let out = ctx.top_k(src, 3, BufferUsage::Activations).unwrap(); + let values = vec![ + 0.1, 0.6, -0.4, 1.5, 0.9, 0.2, -0.7, 0.8, // + 1.1, -0.5, 0.4, 0.7, 0.3, 1.6, -0.2, 0.0, + ]; + ctx.write_tensor_data(src, &f32s_to_bytes(&values)).unwrap(); + + let mut graph = Graph::new(); + graph.build_forward_expand(&ctx, out).unwrap(); + + let prepared = prepare_graph(&ctx, &graph, runtime.features()).unwrap(); + let session = MetalGraphSession::from_runtime( + runtime, + &ctx, + &prepared, + BufferStorageMode::Shared, + BufferStorageMode::Shared, + ) + .unwrap(); + + let execution = session.execute(&ctx, &[], &[out]).unwrap(); + let actual = bytes_to_i32s(execution.outputs.get(&out).unwrap()); + let expected = cpu_top_k_rows_i32(&values, 8, 3); + assert_eq!(actual, expected); + } + + #[test] + fn executes_argsort_graph_on_metal_when_available() { + let runtime = match MetalRuntime::new() { + Ok(runtime) => runtime, + Err(_) => return, + }; + + let mut ctx = Context::new(InitParams { + mem_size: 1 << 20, + mem_buffer: None, + no_alloc: false, + }); + + let src = ctx + .new_tensor_2d(TensorType::F32, 8, 2, BufferUsage::Activations) + .unwrap(); + let out = ctx.argsort(src, BufferUsage::Activations).unwrap(); + let values = vec![ + 0.1, 0.6, -0.4, 1.5, 0.9, 0.2, -0.7, 0.8, // + 1.1, -0.5, 0.4, 0.7, 0.3, 1.6, -0.2, 0.0, + ]; + ctx.write_tensor_data(src, &f32s_to_bytes(&values)).unwrap(); + + let mut graph = Graph::new(); + graph.build_forward_expand(&ctx, out).unwrap(); + + let prepared = prepare_graph(&ctx, &graph, runtime.features()).unwrap(); + let session = MetalGraphSession::from_runtime( + runtime, + &ctx, + &prepared, + BufferStorageMode::Shared, + BufferStorageMode::Shared, + ) + .unwrap(); + + let execution = session.execute(&ctx, &[], &[out]).unwrap(); + let actual = bytes_to_i32s(execution.outputs.get(&out).unwrap()); + let expected = cpu_argsort_rows_i32(&values, 8); + assert_eq!(actual, expected); + } + + #[test] + fn executes_add_id_graph_on_metal_when_available() { + let runtime = match MetalRuntime::new() { + Ok(runtime) => runtime, + Err(_) => return, + }; + + let mut ctx = Context::new(InitParams { + mem_size: 1 << 20, + mem_buffer: None, + no_alloc: false, + }); + + let src0 = ctx + .new_tensor_3d(TensorType::F32, 4, 2, 2, BufferUsage::Activations) + .unwrap(); + let bias = ctx + .new_tensor_2d(TensorType::F32, 4, 4, BufferUsage::Weights) + .unwrap(); + let ids = ctx + .new_tensor_2d(TensorType::I32, 2, 2, BufferUsage::Activations) + .unwrap(); + let out = ctx + .add_id(src0, bias, ids, BufferUsage::Activations) + .unwrap(); + + let src0_values = vec![ + 0.5, 0.7, 0.9, 1.1, // + 1.5, 1.7, 1.9, 2.1, // + -0.5, -0.7, -0.9, -1.1, // + -1.5, -1.7, -1.9, -2.1, + ]; + let bias_values = vec![ + 0.1, 0.2, 0.3, 0.4, // + 1.0, 1.1, 1.2, 1.3, // + 2.0, 2.1, 2.2, 2.3, // + 3.0, 3.1, 3.2, 3.3, + ]; + let ids_values = vec![2, 1, 0, 3]; + ctx.write_tensor_data(src0, &f32s_to_bytes(&src0_values)) + .unwrap(); + ctx.write_tensor_data(bias, &f32s_to_bytes(&bias_values)) + .unwrap(); + ctx.write_tensor_data(ids, &i32s_to_bytes(&ids_values)) + .unwrap(); + + let mut graph = Graph::new(); + graph.build_forward_expand(&ctx, out).unwrap(); + + let prepared = prepare_graph(&ctx, &graph, runtime.features()).unwrap(); + let session = MetalGraphSession::from_runtime( + runtime, + &ctx, + &prepared, + BufferStorageMode::Shared, + BufferStorageMode::Shared, + ) + .unwrap(); + + let execution = session.execute(&ctx, &[], &[out]).unwrap(); + let actual = bytes_to_f32s(execution.outputs.get(&out).unwrap()); + let expected = cpu_add_id_f32(&src0_values, &bias_values, &ids_values, 4, 2, 2, 4); + + assert_eq!(actual.len(), expected.len()); + for (a, e) in actual.iter().zip(expected.iter()) { + assert!( + (a - e).abs() < 1.0e-5, + "add_id output mismatch: actual={} expected={}", + a, + e + ); + } + } + + #[test] + fn executes_mul_mat_id_graph_on_metal_when_available() { + let runtime = match MetalRuntime::new() { + Ok(runtime) => runtime, + Err(_) => return, + }; + + let mut ctx = Context::new(InitParams { + mem_size: 1 << 22, + mem_buffer: None, + no_alloc: false, + }); + + let experts = ctx + .new_tensor_3d(TensorType::F32, 4, 3, 4, BufferUsage::Weights) + .unwrap(); + let input = ctx + .new_tensor_3d(TensorType::F32, 4, 1, 2, BufferUsage::Activations) + .unwrap(); + let ids = ctx + .new_tensor_2d(TensorType::I32, 2, 2, BufferUsage::Activations) + .unwrap(); + let out = ctx + .mul_mat_id(experts, input, ids, BufferUsage::Activations) + .unwrap(); + + let expert_values = patterned_f32s(4 * 3 * 4, -0.3, 0.05); + let input_values = vec![ + 0.2, -0.1, 0.4, 0.8, // + -0.3, 0.5, -0.6, 0.7, + ]; + let ids_values = vec![1, 3, 0, 2]; + ctx.write_tensor_data(experts, &f32s_to_bytes(&expert_values)) + .unwrap(); + ctx.write_tensor_data(input, &f32s_to_bytes(&input_values)) + .unwrap(); + ctx.write_tensor_data(ids, &i32s_to_bytes(&ids_values)) + .unwrap(); + + let mut graph = Graph::new(); + graph.build_forward_expand(&ctx, out).unwrap(); + + let prepared = prepare_graph(&ctx, &graph, runtime.features()).unwrap(); + let session = MetalGraphSession::from_runtime( + runtime, + &ctx, + &prepared, + BufferStorageMode::Shared, + BufferStorageMode::Shared, + ) + .unwrap(); + + let execution = session.execute(&ctx, &[], &[out]).unwrap(); + let actual = bytes_to_f32s(execution.outputs.get(&out).unwrap()); + let expected = + cpu_mul_mat_id_f32(&expert_values, &input_values, &ids_values, 4, 3, 4, 2, 2); + + assert_eq!(actual.len(), expected.len()); + for (a, e) in actual.iter().zip(expected.iter()) { + assert!( + (a - e).abs() < 1.0e-5, + "mul_mat_id output mismatch: actual={} expected={}", + a, + e + ); + } + } + + #[test] + fn executes_gated_delta_net_graph_on_metal_when_available() { + let runtime = match MetalRuntime::new() { + Ok(runtime) => runtime, + Err(_) => return, + }; + + let mut ctx = Context::new(InitParams { + mem_size: 1 << 22, + mem_buffer: None, + no_alloc: false, + }); + + let s_v = 32_i64; + let h_k = 1_i64; + let h_v = 2_i64; + let n_tokens = 1_i64; + let n_seqs = 1_i64; + + let q = ctx + .new_tensor_4d( + TensorType::F32, + s_v, + h_k, + n_tokens, + n_seqs, + BufferUsage::Activations, + ) + .unwrap(); + let k = ctx + .new_tensor_4d( + TensorType::F32, + s_v, + h_k, + n_tokens, + n_seqs, + BufferUsage::Activations, + ) + .unwrap(); + let v = ctx + .new_tensor_4d( + TensorType::F32, + s_v, + h_v, + n_tokens, + n_seqs, + BufferUsage::Activations, + ) + .unwrap(); + let g = ctx + .new_tensor_4d( + TensorType::F32, + 1, + h_v, + n_tokens, + n_seqs, + BufferUsage::Activations, + ) + .unwrap(); + let beta = ctx + .new_tensor_4d( + TensorType::F32, + 1, + h_v, + n_tokens, + n_seqs, + BufferUsage::Activations, + ) + .unwrap(); + let state = ctx + .new_tensor_4d( + TensorType::F32, + s_v, + s_v, + h_v, + n_seqs, + BufferUsage::Activations, + ) + .unwrap(); + let result = ctx + .gated_delta_net(q, k, v, g, beta, state, BufferUsage::Activations) + .unwrap(); + + let output = ctx + .view_4d( + result, + s_v, + h_v, + n_tokens, + n_seqs, + ggml_row_size_for_type(TensorType::F32, s_v).unwrap(), + ggml_row_size_for_type(TensorType::F32, s_v * h_v).unwrap(), + ggml_row_size_for_type(TensorType::F32, s_v * h_v * n_tokens).unwrap(), + 0, + ) + .unwrap(); + let new_state = ctx + .view_4d( + result, + s_v, + s_v, + h_v, + n_seqs, + ggml_row_size_for_type(TensorType::F32, s_v).unwrap(), + ggml_row_size_for_type(TensorType::F32, s_v * s_v).unwrap(), + ggml_row_size_for_type(TensorType::F32, s_v * s_v * h_v).unwrap(), + ggml_row_size_for_type(TensorType::F32, s_v * h_v * n_tokens * n_seqs).unwrap(), + ) + .unwrap(); + + let q_values = patterned_f32s((s_v * h_k * n_tokens * n_seqs) as usize, -0.2, 0.01); + let k_values = patterned_f32s((s_v * h_k * n_tokens * n_seqs) as usize, 0.15, -0.008); + let v_values = patterned_f32s((s_v * h_v * n_tokens * n_seqs) as usize, 0.05, 0.006); + let g_values = vec![-0.3, 0.2]; + let beta_values = vec![0.7, 0.4]; + let state_values = patterned_f32s((s_v * s_v * h_v * n_seqs) as usize, -0.05, 0.0005); + ctx.write_tensor_data(q, &f32s_to_bytes(&q_values)).unwrap(); + ctx.write_tensor_data(k, &f32s_to_bytes(&k_values)).unwrap(); + ctx.write_tensor_data(v, &f32s_to_bytes(&v_values)).unwrap(); + ctx.write_tensor_data(g, &f32s_to_bytes(&g_values)).unwrap(); + ctx.write_tensor_data(beta, &f32s_to_bytes(&beta_values)) + .unwrap(); + ctx.write_tensor_data(state, &f32s_to_bytes(&state_values)) + .unwrap(); + + let mut graph = Graph::new(); + graph.build_forward_expand(&ctx, output).unwrap(); + graph.build_forward_expand(&ctx, new_state).unwrap(); + + let prepared = prepare_graph(&ctx, &graph, runtime.features()).unwrap(); + let session = MetalGraphSession::from_runtime( + runtime, + &ctx, + &prepared, + BufferStorageMode::Shared, + BufferStorageMode::Shared, + ) + .unwrap(); + + let execution = session.execute(&ctx, &[], &[output, new_state]).unwrap(); + let actual_output = bytes_to_f32s(execution.outputs.get(&output).unwrap()); + let actual_state = bytes_to_f32s(execution.outputs.get(&new_state).unwrap()); + let (expected_output, expected_state) = cpu_gated_delta_net_f32( + &q_values, + &k_values, + &v_values, + &g_values, + &beta_values, + &state_values, + s_v as usize, + h_k as usize, + h_v as usize, + n_tokens as usize, + n_seqs as usize, + ); + + assert_eq!(actual_output.len(), expected_output.len()); + for (a, e) in actual_output.iter().zip(expected_output.iter()) { + assert!( + (a - e).abs() < 1.0e-4, + "gated_delta_net output mismatch: actual={} expected={}", + a, + e + ); + } + + assert_eq!(actual_state.len(), expected_state.len()); + for (a, e) in actual_state.iter().zip(expected_state.iter()) { + assert!( + (a - e).abs() < 1.0e-4, + "gated_delta_net state mismatch: actual={} expected={}", + a, + e + ); + } + } + + fn cpu_rope_norm_f32( + src: &[f32], + positions: &[i32], + n_dims: usize, + n_heads: usize, + n_tokens: usize, + ) -> Vec { + let ne0 = n_dims; + let mut dst = src.to_vec(); + for token in 0..n_tokens { + let theta_base = positions[token] as f32; + for head in 0..n_heads { + let base = (token * n_heads + head) * ne0; + let mut i0 = 0usize; + while i0 < ne0 { + let theta = theta_base * 10_000f32.powf(-(i0 as f32) / (n_dims as f32)); + let cos_theta = theta.cos(); + let sin_theta = theta.sin(); + let x0 = src[base + i0]; + let x1 = src[base + i0 + 1]; + dst[base + i0] = x0 * cos_theta - x1 * sin_theta; + dst[base + i0 + 1] = x0 * sin_theta + x1 * cos_theta; + i0 += 2; + } + } + } + dst + } + + fn f32s_to_bytes(values: &[f32]) -> Vec { + let mut out = Vec::with_capacity(values.len() * std::mem::size_of::()); + for value in values { + out.extend_from_slice(&value.to_le_bytes()); + } + out + } + + fn i32s_to_bytes(values: &[i32]) -> Vec { + let mut out = Vec::with_capacity(values.len() * std::mem::size_of::()); + for value in values { + out.extend_from_slice(&value.to_le_bytes()); + } + out + } + + fn bytes_to_i32s(bytes: &[u8]) -> Vec { + bytes + .chunks_exact(std::mem::size_of::()) + .map(|chunk| i32::from_le_bytes(chunk.try_into().unwrap())) + .collect() + } + + fn bytes_to_f32s(bytes: &[u8]) -> Vec { + bytes + .chunks_exact(std::mem::size_of::()) + .map(|chunk| f32::from_le_bytes(chunk.try_into().unwrap())) + .collect() + } + + fn patterned_f32s(len: usize, base: f32, step: f32) -> Vec { + (0..len).map(|i| base + step * (i as f32)).collect() + } + + fn cpu_softmax_rows_f32(values: &[f32], row_len: usize) -> Vec { + let mut out = vec![0.0; values.len()]; + for row in 0..(values.len() / row_len) { + let slice = &values[row * row_len..(row + 1) * row_len]; + let max = slice + .iter() + .copied() + .fold(f32::NEG_INFINITY, |acc, v| acc.max(v)); + let mut sum = 0.0f32; + for (i, value) in slice.iter().enumerate() { + let exp = (*value - max).exp(); + out[row * row_len + i] = exp; + sum += exp; + } + for i in 0..row_len { + out[row * row_len + i] /= sum; + } + } + out + } + + fn cpu_sum_rows_f32(values: &[f32], row_len: usize) -> Vec { + let mut out = vec![0.0; values.len() / row_len]; + for row in 0..out.len() { + out[row] = values[row * row_len..(row + 1) * row_len] + .iter() + .copied() + .sum(); + } + out + } + + fn cpu_top_k_rows_i32(values: &[f32], row_len: usize, k: usize) -> Vec { + let mut out = Vec::with_capacity((values.len() / row_len) * k); + for row in 0..(values.len() / row_len) { + let mut indices = (0..row_len).collect::>(); + indices + .sort_by(|&a, &b| values[row * row_len + b].total_cmp(&values[row * row_len + a])); + out.extend(indices.into_iter().take(k).map(|idx| idx as i32)); + } + out + } + + fn cpu_argsort_rows_i32(values: &[f32], row_len: usize) -> Vec { + let mut out = Vec::with_capacity(values.len()); + for row in 0..(values.len() / row_len) { + let mut indices = (0..row_len).collect::>(); + indices + .sort_by(|&a, &b| values[row * row_len + a].total_cmp(&values[row * row_len + b])); + out.extend(indices.into_iter().map(|idx| idx as i32)); + } + out + } + + fn cpu_add_id_f32( + src0: &[f32], + bias: &[f32], + ids: &[i32], + width: usize, + used_experts: usize, + tokens: usize, + total_experts: usize, + ) -> Vec { + let mut out = src0.to_vec(); + for token in 0..tokens { + for slot in 0..used_experts { + let expert = ids[token * used_experts + slot] as usize; + for i in 0..width { + let dst_idx = token * used_experts * width + slot * width + i; + out[dst_idx] += bias[expert * width + i]; + } + } + } + let _ = total_experts; + out + } + + fn cpu_mul_mat_id_f32( + experts: &[f32], + input: &[f32], + ids: &[i32], + in_dim: usize, + out_dim: usize, + expert_count: usize, + used_experts: usize, + tokens: usize, + ) -> Vec { + let mut out = vec![0.0f32; out_dim * used_experts * tokens]; + for token in 0..tokens { + for slot in 0..used_experts { + let expert = ids[token * used_experts + slot] as usize; + for o in 0..out_dim { + let mut acc = 0.0f32; + for i in 0..in_dim { + let w_idx = expert * (in_dim * out_dim) + o * in_dim + i; + let x_idx = token * in_dim + i; + acc += experts[w_idx] * input[x_idx]; + } + out[token * used_experts * out_dim + slot * out_dim + o] = acc; + } + } + } + let _ = expert_count; + out + } + + fn cpu_gated_delta_net_f32( + q: &[f32], + k: &[f32], + v: &[f32], + g: &[f32], + beta: &[f32], + state: &[f32], + s_v: usize, + h_k: usize, + h_v: usize, + n_tokens: usize, + n_seqs: usize, + ) -> (Vec, Vec) { + assert_eq!(q.len(), s_v * h_k * n_tokens * n_seqs); + assert_eq!(k.len(), s_v * h_k * n_tokens * n_seqs); + assert_eq!(v.len(), s_v * h_v * n_tokens * n_seqs); + assert_eq!(beta.len(), h_v * n_tokens * n_seqs); + assert_eq!(state.len(), s_v * s_v * h_v * n_seqs); + assert!( + g.len() == h_v * n_tokens * n_seqs || g.len() == s_v * h_v * n_tokens * n_seqs, + "gate tensor must be scalar or per-channel" + ); + + let kda = g.len() == s_v * h_v * n_tokens * n_seqs; + let scale = 1.0f32 / (s_v as f32).sqrt(); + let mut attn_out = vec![0.0f32; s_v * h_v * n_tokens * n_seqs]; + let mut state_out = state.to_vec(); + let mut delta = vec![0.0f32; s_v]; + + for seq in 0..n_seqs { + for head in 0..h_v { + let q_head = head % h_k; + let k_head = head % h_k; + let state_base = (seq * h_v + head) * s_v * s_v; + + for token in 0..n_tokens { + let q_base = ((seq * n_tokens + token) * h_k + q_head) * s_v; + let k_base = ((seq * n_tokens + token) * h_k + k_head) * s_v; + let v_base = ((seq * n_tokens + token) * h_v + head) * s_v; + let beta_idx = (seq * n_tokens + token) * h_v + head; + let beta_val = beta[beta_idx]; + + if kda { + let g_base = ((seq * n_tokens + token) * h_v + head) * s_v; + for row in 0..s_v { + let row_base = state_base + row * s_v; + for col in 0..s_v { + state_out[row_base + col] *= g[g_base + col].exp(); + } + } + } else { + let g_exp = g[beta_idx].exp(); + for idx in 0..(s_v * s_v) { + state_out[state_base + idx] *= g_exp; + } + } + + for row in 0..s_v { + let row_base = state_base + row * s_v; + let mut sum = 0.0f32; + for col in 0..s_v { + sum += state_out[row_base + col] * k[k_base + col]; + } + delta[row] = (v[v_base + row] - sum) * beta_val; + } + + for row in 0..s_v { + let row_base = state_base + row * s_v; + for col in 0..s_v { + state_out[row_base + col] += k[k_base + col] * delta[row]; + } + } + + let out_base = ((seq * n_tokens + token) * h_v + head) * s_v; + for row in 0..s_v { + let row_base = state_base + row * s_v; + let mut sum = 0.0f32; + for col in 0..s_v { + sum += state_out[row_base + col] * q[q_base + col]; + } + attn_out[out_base + row] = sum * scale; + } + } + } + } + + (attn_out, state_out) + } + + fn cpu_flash_attn_f32( + q: &[f32], + k: &[f32], + v: &[f32], + d: usize, + n_q: usize, + n_kv: usize, + ) -> Vec { + let mut out = vec![0.0f32; d * n_q]; + for iq in 0..n_q { + let q_row = &q[iq * d..(iq + 1) * d]; + let mut scores = vec![0.0f32; n_kv]; + for ik in 0..n_kv { + let k_row = &k[ik * d..(ik + 1) * d]; + let mut dot = 0.0f32; + for i in 0..d { + dot += q_row[i] * k_row[i]; + } + scores[ik] = dot / (d as f32).sqrt(); + } + let max_score = scores.iter().copied().fold(f32::NEG_INFINITY, f32::max); + let mut sum = 0.0f32; + for score in &mut scores { + *score = (*score - max_score).exp(); + sum += *score; + } + for score in &mut scores { + *score /= sum.max(f32::MIN_POSITIVE); + } + for ik in 0..n_kv { + let v_row = &v[ik * d..(ik + 1) * d]; + let w = scores[ik]; + for i in 0..d { + out[iq * d + i] += w * v_row[i]; + } + } + } + out + } +} diff --git a/libs/ggml/src/backend/metal/ggml/LICENSE b/libs/ggml/src/backend/metal/ggml/LICENSE new file mode 100644 index 0000000000..3714c93e66 --- /dev/null +++ b/libs/ggml/src/backend/metal/ggml/LICENSE @@ -0,0 +1,23 @@ +Source snapshot commit: 364c77f4ca2737e3287652e0e8a8c6dce3231bba + +MIT License + +Copyright (c) 2023-2026 The ggml authors + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in all +copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +SOFTWARE. diff --git a/libs/ggml/src/backend/metal/ggml/ggml-common.h b/libs/ggml/src/backend/metal/ggml/ggml-common.h new file mode 100644 index 0000000000..92cf739e7a --- /dev/null +++ b/libs/ggml/src/backend/metal/ggml/ggml-common.h @@ -0,0 +1,1889 @@ +#ifndef GGML_COMMON_DECL + +#if defined(GGML_COMMON_DECL_C) +#include + +typedef uint16_t ggml_half; +typedef uint32_t ggml_half2; + +#define GGML_COMMON_AGGR_U +#define GGML_COMMON_AGGR_S + +#define GGML_COMMON_DECL +#elif defined(GGML_COMMON_DECL_CPP) +#include + +typedef uint16_t ggml_half; +typedef uint32_t ggml_half2; + +// std-c++ allow anonymous unions but some compiler warn on it +#define GGML_COMMON_AGGR_U data +// std-c++ do not allow it. +#define GGML_COMMON_AGGR_S data + +#define GGML_COMMON_DECL +#elif defined(GGML_COMMON_DECL_METAL) +#include + +typedef half ggml_half; +typedef half2 ggml_half2; + +#define GGML_COMMON_AGGR_U +#define GGML_COMMON_AGGR_S + +#define GGML_COMMON_DECL +#elif defined(GGML_COMMON_DECL_CUDA) +#if defined(GGML_COMMON_DECL_MUSA) +#include +#else +#include +#endif +#include + +typedef half ggml_half; +typedef half2 ggml_half2; + +#define GGML_COMMON_AGGR_U +#define GGML_COMMON_AGGR_S data + +#define GGML_COMMON_DECL +#elif defined(GGML_COMMON_DECL_HIP) +#include +#include + +typedef half ggml_half; +typedef half2 ggml_half2; + +#define GGML_COMMON_AGGR_U +#define GGML_COMMON_AGGR_S data + +#define GGML_COMMON_DECL +#elif defined(GGML_COMMON_DECL_SYCL) +#include +#include + +typedef sycl::half ggml_half; +typedef sycl::half2 ggml_half2; + +#define GGML_COMMON_AGGR_U +#define GGML_COMMON_AGGR_S data + +#define GGML_COMMON_DECL +#endif + +#if defined(GGML_COMMON_DECL) + +#ifndef __cplusplus +#ifndef static_assert +#if defined(__STDC_VERSION__) && (__STDC_VERSION__ >= 201100L) +#define static_assert(cond, msg) _Static_assert(cond, msg) +#else +#define static_assert(cond, msg) struct global_scope_noop_trick +#endif +#endif +#endif // __cplusplus + +// QK = number of values after dequantization +// QK_K = super-block size + +#define QK_K 256 +#define K_SCALE_SIZE 12 + +#if defined(GGML_COMMON_DECL_CUDA) || defined(GGML_COMMON_DECL_HIP) || defined(GGML_COMMON_DECL_SYCL) +// QR = QK / number of values before dequantization +// QI = number of 32 bit integers before dequantization + +#define QI4_0 (QK4_0 / (4 * QR4_0)) +#define QR4_0 2 + +#define QI4_1 (QK4_1 / (4 * QR4_1)) +#define QR4_1 2 + +#define QI_MXFP4 (QK_MXFP4 / (4 * QR_MXFP4)) +#define QR_MXFP4 2 + +#define QI_NVFP4 (QK_NVFP4 / (4 * QR_NVFP4)) +#define QR_NVFP4 2 + +#define QI5_0 (QK5_0 / (4 * QR5_0)) +#define QR5_0 2 + +#define QI5_1 (QK5_1 / (4 * QR5_1)) +#define QR5_1 2 + +#define QI8_0 (QK8_0 / (4 * QR8_0)) +#define QR8_0 1 + +#define QI8_1 (QK8_1 / (4 * QR8_1)) +#define QR8_1 1 + +#define QI2_K (QK_K / (4*QR2_K)) +#define QR2_K 4 + +#define QI3_K (QK_K / (4*QR3_K)) +#define QR3_K 4 + +#define QI4_K (QK_K / (4*QR4_K)) +#define QR4_K 2 + +#define QI5_K (QK_K / (4*QR5_K)) +#define QR5_K 2 + +#define QI6_K (QK_K / (4*QR6_K)) +#define QR6_K 2 + +#define QI2_XXS (QK_K / (4*QR2_XXS)) +#define QR2_XXS 4 + +#define QI2_XS (QK_K / (4*QR2_XS)) +#define QR2_XS 4 + +#define QI2_S (QK_K / (4*QR2_S)) +#define QR2_S 4 + +#define QI3_XXS (QK_K / (4*QR3_XXS)) +#define QR3_XXS 4 + +#define QI3_XS (QK_K / (4*QR3_XS)) +#define QR3_XS 4 + +#define QI1_S (QK_K / (4*QR1_S)) +#define QR1_S 8 + +#define QI1_M (QK_K / (4*QR1_M)) +#define QR1_M 8 + +#define QI4_NL (QK4_NL / (4*QR4_NL)) +#define QR4_NL 2 + +#define QI4_XS (QK_K / (4*QR4_XS)) +#define QR4_XS 2 + +#define QI3_S (QK_K / (4*QR3_S)) +#define QR3_S 4 + +#endif // GGML_COMMON_DECL_CUDA || GGML_COMMON_DECL_HIP + +#ifdef _MSC_VER +#define GGML_EXTENSION +#else // _MSC_VER +#define GGML_EXTENSION __extension__ +#endif // _MSC_VER + +#define QK4_0 32 +typedef struct { + ggml_half d; // delta + uint8_t qs[QK4_0 / 2]; // nibbles / quants +} block_q4_0; +static_assert(sizeof(block_q4_0) == sizeof(ggml_half) + QK4_0 / 2, "wrong q4_0 block size/padding"); + +#define QK4_1 32 +typedef struct { + GGML_EXTENSION union { + struct { + ggml_half d; // delta + ggml_half m; // min + } GGML_COMMON_AGGR_S; + ggml_half2 dm; + } GGML_COMMON_AGGR_U; + uint8_t qs[QK4_1 / 2]; // nibbles / quants +} block_q4_1; +static_assert(sizeof(block_q4_1) == 2 * sizeof(ggml_half) + QK4_1 / 2, "wrong q4_1 block size/padding"); + +#define QK_MXFP4 32 +typedef struct { + uint8_t e; // E8M0 + uint8_t qs[QK_MXFP4/2]; +} block_mxfp4; +static_assert(sizeof(block_mxfp4) == sizeof(uint8_t) + QK_MXFP4/2, "wrong mxfp4 block size/padding"); + +#define QK_NVFP4 64 +#define QK_NVFP4_SUB 16 // sub-block size for per-group scales +typedef struct { + uint8_t d[QK_NVFP4/QK_NVFP4_SUB]; // UE4M3 scales (4 bytes, one per 16-element sub-block) + uint8_t qs[QK_NVFP4/2]; // packed 4-bit E2M1 values (32 bytes) +} block_nvfp4; +static_assert(sizeof(block_nvfp4) == sizeof(uint8_t)*(QK_NVFP4/QK_NVFP4_SUB) + QK_NVFP4/2, "wrong nvfp4 block size/padding"); + +#define QK5_0 32 +typedef struct { + ggml_half d; // delta + uint8_t qh[4]; // 5-th bit of quants + uint8_t qs[QK5_0 / 2]; // nibbles / quants +} block_q5_0; +static_assert(sizeof(block_q5_0) == sizeof(ggml_half) + sizeof(uint32_t) + QK5_0 / 2, "wrong q5_0 block size/padding"); + +#define QK5_1 32 +typedef struct { + GGML_EXTENSION union { + struct { + ggml_half d; // delta + ggml_half m; // min + } GGML_COMMON_AGGR_S; + ggml_half2 dm; + } GGML_COMMON_AGGR_U; + uint8_t qh[4]; // 5-th bit of quants + uint8_t qs[QK5_1 / 2]; // nibbles / quants +} block_q5_1; +static_assert(sizeof(block_q5_1) == 2 * sizeof(ggml_half) + sizeof(uint32_t) + QK5_1 / 2, "wrong q5_1 block size/padding"); + +#define QK8_0 32 +typedef struct { + ggml_half d; // delta + int8_t qs[QK8_0]; // quants +} block_q8_0; +static_assert(sizeof(block_q8_0) == sizeof(ggml_half) + QK8_0, "wrong q8_0 block size/padding"); + +#define QK8_1 32 +typedef struct { + GGML_EXTENSION union { + struct { + ggml_half d; // delta + ggml_half s; // d * sum(qs[i]) + } GGML_COMMON_AGGR_S; + ggml_half2 ds; + } GGML_COMMON_AGGR_U; + int8_t qs[QK8_1]; // quants +} block_q8_1; +static_assert(sizeof(block_q8_1) == 2*sizeof(ggml_half) + QK8_1, "wrong q8_1 block size/padding"); + +// +// Ternary quantization +// + +// 1.6875 bpw +typedef struct { + uint8_t qs[(QK_K - 4 * QK_K / 64) / 5]; // 5 elements per byte (3^5 = 243 < 256) + uint8_t qh[QK_K/64]; // 4 elements per byte + ggml_half d; +} block_tq1_0; +static_assert(sizeof(block_tq1_0) == sizeof(ggml_half) + QK_K / 64 + (QK_K - 4 * QK_K / 64) / 5, "wrong tq1_0 block size/padding"); + +// 2.0625 bpw +typedef struct { + uint8_t qs[QK_K/4]; // 2 bits per element + ggml_half d; +} block_tq2_0; +static_assert(sizeof(block_tq2_0) == sizeof(ggml_half) + QK_K / 4, "wrong tq2_0 block size/padding"); + +// +// Super-block quantization structures +// + +// 2-bit quantization +// weight is represented as x = a * q + b +// 16 blocks of 16 elements each +// Effectively 2.625 bits per weight +typedef struct { + uint8_t scales[QK_K/16]; // scales and mins, quantized with 4 bits + uint8_t qs[QK_K/4]; // quants + GGML_EXTENSION union { + struct { + ggml_half d; // super-block scale for quantized scales + ggml_half dmin; // super-block scale for quantized mins + } GGML_COMMON_AGGR_S; + ggml_half2 dm; + } GGML_COMMON_AGGR_U; +} block_q2_K; +static_assert(sizeof(block_q2_K) == 2*sizeof(ggml_half) + QK_K/16 + QK_K/4, "wrong q2_K block size/padding"); + +// 3-bit quantization +// weight is represented as x = a * q +// 16 blocks of 16 elements each +// Effectively 3.4375 bits per weight +typedef struct { + uint8_t hmask[QK_K/8]; // quants - high bit + uint8_t qs[QK_K/4]; // quants - low 2 bits + uint8_t scales[12]; // scales, quantized with 6 bits + ggml_half d; // super-block scale +} block_q3_K; +static_assert(sizeof(block_q3_K) == sizeof(ggml_half) + QK_K / 4 + QK_K / 8 + 12, "wrong q3_K block size/padding"); + +// 4-bit quantization +// 8 blocks of 32 elements each +// weight is represented as x = a * q + b +// Effectively 4.5 bits per weight +typedef struct { + GGML_EXTENSION union { + struct { + ggml_half d; // super-block scale for quantized scales + ggml_half dmin; // super-block scale for quantized mins + } GGML_COMMON_AGGR_S; + ggml_half2 dm; + } GGML_COMMON_AGGR_U; + uint8_t scales[K_SCALE_SIZE]; // scales and mins, quantized with 6 bits + uint8_t qs[QK_K/2]; // 4--bit quants +} block_q4_K; +static_assert(sizeof(block_q4_K) == 2*sizeof(ggml_half) + K_SCALE_SIZE + QK_K/2, "wrong q4_K block size/padding"); + +// 5-bit quantization +// 8 blocks of 32 elements each +// weight is represented as x = a * q + b +// Effectively 5.5 bits per weight +typedef struct { + GGML_EXTENSION union { + struct { + ggml_half d; // super-block scale for quantized scales + ggml_half dmin; // super-block scale for quantized mins + } GGML_COMMON_AGGR_S; + ggml_half2 dm; + } GGML_COMMON_AGGR_U; + uint8_t scales[K_SCALE_SIZE]; // scales and mins, quantized with 6 bits + uint8_t qh[QK_K/8]; // quants, high bit + uint8_t qs[QK_K/2]; // quants, low 4 bits +} block_q5_K; +static_assert(sizeof(block_q5_K) == 2*sizeof(ggml_half) + K_SCALE_SIZE + QK_K/2 + QK_K/8, "wrong q5_K block size/padding"); + +// 6-bit quantization +// weight is represented as x = a * q +// 16 blocks of 16 elements each +// Effectively 6.5625 bits per weight +typedef struct { + uint8_t ql[QK_K/2]; // quants, lower 4 bits + uint8_t qh[QK_K/4]; // quants, upper 2 bits + int8_t scales[QK_K/16]; // scales, quantized with 8 bits + ggml_half d; // super-block scale +} block_q6_K; +static_assert(sizeof(block_q6_K) == sizeof(ggml_half) + QK_K / 16 + 3*QK_K/4, "wrong q6_K block size/padding"); + +// This is only used for intermediate quantization and dot products +typedef struct { + float d; // delta + int8_t qs[QK_K]; // quants + int16_t bsums[QK_K/16]; // sum of quants in groups of 16 +} block_q8_K; +static_assert(sizeof(block_q8_K) == sizeof(float) + QK_K + QK_K/16*sizeof(int16_t), "wrong q8_K block size/padding"); + +// (Almost) "true" 2-bit quantization. +// Due to the need to use blocks as per ggml design, it ends up using +// 2.0625 bpw because of the 16-bit scale for each block of 256. +typedef struct { + ggml_half d; + uint16_t qs[QK_K/8]; +} block_iq2_xxs; +static_assert(sizeof(block_iq2_xxs) == sizeof(ggml_half) + QK_K/8*sizeof(uint16_t), "wrong iq2_xxs block size/padding"); + +// 2.3125 bpw quants +typedef struct { + ggml_half d; + uint16_t qs[QK_K/8]; + uint8_t scales[QK_K/32]; +} block_iq2_xs; +static_assert(sizeof(block_iq2_xs) == sizeof(ggml_half) + QK_K/8*sizeof(uint16_t) + QK_K/32, "wrong iq2_xs block size/padding"); + +// 2.5625 bpw quants +typedef struct { + ggml_half d; + uint8_t qs[QK_K/4]; + uint8_t qh[QK_K/32]; + uint8_t scales[QK_K/32]; +} block_iq2_s; +static_assert(sizeof(block_iq2_s) == sizeof(ggml_half) + QK_K/4 + QK_K/16, "wrong iq2_s block size/padding"); + +// (Almost) "true" 3-bit quantization. +// Due to the need to use blocks as per ggml design, it ends up using +// 3.0625 bpw because of the 16-bit scale for each block of 256. +typedef struct { + ggml_half d; + uint8_t qs[3*QK_K/8]; +} block_iq3_xxs; +static_assert(sizeof(block_iq3_xxs) == sizeof(ggml_half) + 3*(QK_K/8), "wrong iq3_xxs block size/padding"); + +// 3.4375 bpw +#define IQ3S_N_SCALE QK_K/64 +typedef struct { + ggml_half d; + uint8_t qs[QK_K/4]; + uint8_t qh[QK_K/32]; + uint8_t signs[QK_K/8]; + uint8_t scales[IQ3S_N_SCALE]; +} block_iq3_s; +static_assert(sizeof(block_iq3_s) == sizeof(ggml_half) + 13*(QK_K/32) + IQ3S_N_SCALE, "wrong iq3_s block size/padding"); + +// 1.5625 bpw +typedef struct { + ggml_half d; + uint8_t qs[QK_K/8]; + uint16_t qh[QK_K/32]; +} block_iq1_s; +static_assert(sizeof(block_iq1_s) == sizeof(ggml_half) + QK_K/8 + QK_K/16, "wrong iq1_s block size/padding"); + +// 1.75 bpw +typedef struct { + uint8_t qs[QK_K/8]; // grid index, low 8 bits + uint8_t qh[QK_K/16]; // grid index, high 3 bits + grid shift bit (for two groups of 8) + uint8_t scales[QK_K/32]; // 3-bit block scales (4-bit if QK_K == 64) +} block_iq1_m; +static_assert(sizeof(block_iq1_m) == QK_K/8 + QK_K/16 + QK_K/32, "wrong iq1_m block size/padding"); + +// Used by IQ1_M quants +typedef union { + ggml_half f16; + uint16_t u16; +} iq1m_scale_t; + +// Non-linear quants +#define QK4_NL 32 +typedef struct { + ggml_half d; + uint8_t qs[QK4_NL/2]; +} block_iq4_nl; +static_assert(sizeof(block_iq4_nl) == sizeof(ggml_half) + QK4_NL/2, "wrong iq4_nl block size/padding"); + +typedef struct { + ggml_half d; + uint16_t scales_h; + uint8_t scales_l[QK_K/64]; + uint8_t qs[QK_K/2]; +} block_iq4_xs; +static_assert(sizeof(block_iq4_xs) == sizeof(ggml_half) + sizeof(uint16_t) + QK_K/64 + QK_K/2, "wrong iq4_xs block size/padding"); + +#endif // GGML_COMMON_DECL +#endif // GGML_COMMON_DECL + +//////////////////////////////////////////////////////////////////////////////// + +#ifndef GGML_COMMON_IMPL + +#if defined(GGML_COMMON_IMPL_C) +#include + +#define GGML_TABLE_BEGIN(type, name, size) static const type name[size] = { +#define GGML_TABLE_END() }; + +#define GGML_COMMON_IMPL +#elif defined(GGML_COMMON_IMPL_CPP) +#include + +#define GGML_TABLE_BEGIN(type, name, size) static const type name[size] = { +#define GGML_TABLE_END() }; + +#define GGML_COMMON_IMPL +#elif defined(GGML_COMMON_IMPL_METAL) +#include + +#define GGML_TABLE_BEGIN(type, name, size) static const constant type name[size] = { +#define GGML_TABLE_END() }; + +#define GGML_COMMON_IMPL +#elif defined(GGML_COMMON_IMPL_CUDA) || defined(GGML_COMMON_IMPL_HIP) || defined(GGML_COMMON_IMPL_MUSA) +#include + +#define GGML_TABLE_BEGIN(type, name, size) static const __device__ type name[size] = { +#define GGML_TABLE_END() }; + +#define GGML_COMMON_IMPL +#elif defined(GGML_COMMON_IMPL_SYCL) + +#include + +#define GGML_TABLE_BEGIN(type, name, size) static const type name[size] = { +#define GGML_TABLE_END() }; + +#define GGML_COMMON_IMPL +#endif + +#if defined(GGML_COMMON_IMPL) + +GGML_TABLE_BEGIN(uint8_t, kmask_iq2xs, 8) + 1, 2, 4, 8, 16, 32, 64, 128 +GGML_TABLE_END() + +GGML_TABLE_BEGIN(uint8_t, ksigns_iq2xs, 128) + 0, 129, 130, 3, 132, 5, 6, 135, 136, 9, 10, 139, 12, 141, 142, 15, + 144, 17, 18, 147, 20, 149, 150, 23, 24, 153, 154, 27, 156, 29, 30, 159, + 160, 33, 34, 163, 36, 165, 166, 39, 40, 169, 170, 43, 172, 45, 46, 175, + 48, 177, 178, 51, 180, 53, 54, 183, 184, 57, 58, 187, 60, 189, 190, 63, + 192, 65, 66, 195, 68, 197, 198, 71, 72, 201, 202, 75, 204, 77, 78, 207, + 80, 209, 210, 83, 212, 85, 86, 215, 216, 89, 90, 219, 92, 221, 222, 95, + 96, 225, 226, 99, 228, 101, 102, 231, 232, 105, 106, 235, 108, 237, 238, 111, + 240, 113, 114, 243, 116, 245, 246, 119, 120, 249, 250, 123, 252, 125, 126, 255, +GGML_TABLE_END() + +GGML_TABLE_BEGIN(uint64_t, ksigns64, 128) + 0x0000000000000000, 0xff000000000000ff, 0xff0000000000ff00, 0x000000000000ffff, + 0xff00000000ff0000, 0x0000000000ff00ff, 0x0000000000ffff00, 0xff00000000ffffff, + 0xff000000ff000000, 0x00000000ff0000ff, 0x00000000ff00ff00, 0xff000000ff00ffff, + 0x00000000ffff0000, 0xff000000ffff00ff, 0xff000000ffffff00, 0x00000000ffffffff, + 0xff0000ff00000000, 0x000000ff000000ff, 0x000000ff0000ff00, 0xff0000ff0000ffff, + 0x000000ff00ff0000, 0xff0000ff00ff00ff, 0xff0000ff00ffff00, 0x000000ff00ffffff, + 0x000000ffff000000, 0xff0000ffff0000ff, 0xff0000ffff00ff00, 0x000000ffff00ffff, + 0xff0000ffffff0000, 0x000000ffffff00ff, 0x000000ffffffff00, 0xff0000ffffffffff, + 0xff00ff0000000000, 0x0000ff00000000ff, 0x0000ff000000ff00, 0xff00ff000000ffff, + 0x0000ff0000ff0000, 0xff00ff0000ff00ff, 0xff00ff0000ffff00, 0x0000ff0000ffffff, + 0x0000ff00ff000000, 0xff00ff00ff0000ff, 0xff00ff00ff00ff00, 0x0000ff00ff00ffff, + 0xff00ff00ffff0000, 0x0000ff00ffff00ff, 0x0000ff00ffffff00, 0xff00ff00ffffffff, + 0x0000ffff00000000, 0xff00ffff000000ff, 0xff00ffff0000ff00, 0x0000ffff0000ffff, + 0xff00ffff00ff0000, 0x0000ffff00ff00ff, 0x0000ffff00ffff00, 0xff00ffff00ffffff, + 0xff00ffffff000000, 0x0000ffffff0000ff, 0x0000ffffff00ff00, 0xff00ffffff00ffff, + 0x0000ffffffff0000, 0xff00ffffffff00ff, 0xff00ffffffffff00, 0x0000ffffffffffff, + 0xffff000000000000, 0x00ff0000000000ff, 0x00ff00000000ff00, 0xffff00000000ffff, + 0x00ff000000ff0000, 0xffff000000ff00ff, 0xffff000000ffff00, 0x00ff000000ffffff, + 0x00ff0000ff000000, 0xffff0000ff0000ff, 0xffff0000ff00ff00, 0x00ff0000ff00ffff, + 0xffff0000ffff0000, 0x00ff0000ffff00ff, 0x00ff0000ffffff00, 0xffff0000ffffffff, + 0x00ff00ff00000000, 0xffff00ff000000ff, 0xffff00ff0000ff00, 0x00ff00ff0000ffff, + 0xffff00ff00ff0000, 0x00ff00ff00ff00ff, 0x00ff00ff00ffff00, 0xffff00ff00ffffff, + 0xffff00ffff000000, 0x00ff00ffff0000ff, 0x00ff00ffff00ff00, 0xffff00ffff00ffff, + 0x00ff00ffffff0000, 0xffff00ffffff00ff, 0xffff00ffffffff00, 0x00ff00ffffffffff, + 0x00ffff0000000000, 0xffffff00000000ff, 0xffffff000000ff00, 0x00ffff000000ffff, + 0xffffff0000ff0000, 0x00ffff0000ff00ff, 0x00ffff0000ffff00, 0xffffff0000ffffff, + 0xffffff00ff000000, 0x00ffff00ff0000ff, 0x00ffff00ff00ff00, 0xffffff00ff00ffff, + 0x00ffff00ffff0000, 0xffffff00ffff00ff, 0xffffff00ffffff00, 0x00ffff00ffffffff, + 0xffffffff00000000, 0x00ffffff000000ff, 0x00ffffff0000ff00, 0xffffffff0000ffff, + 0x00ffffff00ff0000, 0xffffffff00ff00ff, 0xffffffff00ffff00, 0x00ffffff00ffffff, + 0x00ffffffff000000, 0xffffffffff0000ff, 0xffffffffff00ff00, 0x00ffffffff00ffff, + 0xffffffffffff0000, 0x00ffffffffff00ff, 0x00ffffffffffff00, 0xffffffffffffffff, +GGML_TABLE_END() + + +GGML_TABLE_BEGIN(uint64_t, iq2xxs_grid, 256) + 0x0808080808080808, 0x080808080808082b, 0x0808080808081919, 0x0808080808082b08, + 0x0808080808082b2b, 0x0808080808190819, 0x0808080808191908, 0x08080808082b0808, + 0x08080808082b082b, 0x08080808082b2b08, 0x08080808082b2b2b, 0x0808080819080819, + 0x0808080819081908, 0x0808080819190808, 0x0808080819192b08, 0x08080808192b0819, + 0x08080808192b1908, 0x080808082b080808, 0x080808082b08082b, 0x080808082b082b2b, + 0x080808082b2b082b, 0x0808081908080819, 0x0808081908081908, 0x0808081908190808, + 0x0808081908191919, 0x0808081919080808, 0x080808192b081908, 0x080808192b192b08, + 0x0808082b08080808, 0x0808082b0808082b, 0x0808082b082b082b, 0x0808082b2b08082b, + 0x0808190808080819, 0x0808190808081908, 0x0808190808190808, 0x08081908082b0819, + 0x08081908082b1908, 0x0808190819080808, 0x080819081908082b, 0x0808190819082b08, + 0x08081908192b0808, 0x080819082b080819, 0x080819082b081908, 0x080819082b190808, + 0x080819082b2b1908, 0x0808191908080808, 0x080819190808082b, 0x0808191908082b08, + 0x08081919082b0808, 0x080819191908192b, 0x08081919192b2b19, 0x080819192b080808, + 0x080819192b190819, 0x0808192b08082b19, 0x0808192b08190808, 0x0808192b19080808, + 0x0808192b2b081908, 0x0808192b2b2b1908, 0x08082b0808080808, 0x08082b0808081919, + 0x08082b0808082b08, 0x08082b0808191908, 0x08082b08082b2b08, 0x08082b0819080819, + 0x08082b0819081908, 0x08082b0819190808, 0x08082b081919082b, 0x08082b082b082b08, + 0x08082b1908081908, 0x08082b1919080808, 0x08082b2b0808082b, 0x08082b2b08191908, + 0x0819080808080819, 0x0819080808081908, 0x0819080808190808, 0x08190808082b0819, + 0x0819080819080808, 0x08190808192b0808, 0x081908082b081908, 0x081908082b190808, + 0x081908082b191919, 0x0819081908080808, 0x0819081908082b08, 0x08190819082b0808, + 0x0819081919190808, 0x0819081919192b2b, 0x081908192b080808, 0x0819082b082b1908, + 0x0819082b19081919, 0x0819190808080808, 0x0819190808082b08, 0x08191908082b0808, + 0x08191908082b1919, 0x0819190819082b19, 0x081919082b080808, 0x0819191908192b08, + 0x08191919192b082b, 0x0819192b08080808, 0x0819192b0819192b, 0x08192b0808080819, + 0x08192b0808081908, 0x08192b0808190808, 0x08192b0819080808, 0x08192b082b080819, + 0x08192b1908080808, 0x08192b1908081919, 0x08192b192b2b0808, 0x08192b2b19190819, + 0x082b080808080808, 0x082b08080808082b, 0x082b080808082b2b, 0x082b080819081908, + 0x082b0808192b0819, 0x082b08082b080808, 0x082b08082b08082b, 0x082b0819082b2b19, + 0x082b081919082b08, 0x082b082b08080808, 0x082b082b0808082b, 0x082b190808080819, + 0x082b190808081908, 0x082b190808190808, 0x082b190819080808, 0x082b19081919192b, + 0x082b191908080808, 0x082b191919080819, 0x082b1919192b1908, 0x082b192b2b190808, + 0x082b2b0808082b08, 0x082b2b08082b0808, 0x082b2b082b191908, 0x082b2b2b19081908, + 0x1908080808080819, 0x1908080808081908, 0x1908080808190808, 0x1908080808192b08, + 0x19080808082b0819, 0x19080808082b1908, 0x1908080819080808, 0x1908080819082b08, + 0x190808081919192b, 0x19080808192b0808, 0x190808082b080819, 0x190808082b081908, + 0x190808082b190808, 0x1908081908080808, 0x19080819082b0808, 0x19080819192b0819, + 0x190808192b080808, 0x190808192b081919, 0x1908082b08080819, 0x1908082b08190808, + 0x1908082b19082b08, 0x1908082b1919192b, 0x1908082b192b2b08, 0x1908190808080808, + 0x1908190808082b08, 0x19081908082b0808, 0x190819082b080808, 0x190819082b192b19, + 0x190819190819082b, 0x19081919082b1908, 0x1908192b08080808, 0x19082b0808080819, + 0x19082b0808081908, 0x19082b0808190808, 0x19082b0819080808, 0x19082b0819081919, + 0x19082b1908080808, 0x19082b1919192b08, 0x19082b19192b0819, 0x19082b192b08082b, + 0x19082b2b19081919, 0x19082b2b2b190808, 0x1919080808080808, 0x1919080808082b08, + 0x1919080808190819, 0x1919080808192b19, 0x19190808082b0808, 0x191908082b080808, + 0x191908082b082b08, 0x1919081908081908, 0x191908191908082b, 0x191908192b2b1908, + 0x1919082b2b190819, 0x191919082b190808, 0x191919082b19082b, 0x1919191908082b2b, + 0x1919192b08080819, 0x1919192b19191908, 0x19192b0808080808, 0x19192b0808190819, + 0x19192b0808192b19, 0x19192b08192b1908, 0x19192b1919080808, 0x19192b2b08082b08, + 0x192b080808081908, 0x192b080808190808, 0x192b080819080808, 0x192b0808192b2b08, + 0x192b081908080808, 0x192b081919191919, 0x192b082b08192b08, 0x192b082b192b0808, + 0x192b190808080808, 0x192b190808081919, 0x192b191908190808, 0x192b19190819082b, + 0x192b19192b081908, 0x192b2b081908082b, 0x2b08080808080808, 0x2b0808080808082b, + 0x2b08080808082b2b, 0x2b08080819080819, 0x2b0808082b08082b, 0x2b08081908081908, + 0x2b08081908192b08, 0x2b08081919080808, 0x2b08082b08190819, 0x2b08190808080819, + 0x2b08190808081908, 0x2b08190808190808, 0x2b08190808191919, 0x2b08190819080808, + 0x2b081908192b0808, 0x2b08191908080808, 0x2b0819191908192b, 0x2b0819192b191908, + 0x2b08192b08082b19, 0x2b08192b19080808, 0x2b08192b192b0808, 0x2b082b080808082b, + 0x2b082b1908081908, 0x2b082b2b08190819, 0x2b19080808081908, 0x2b19080808190808, + 0x2b190808082b1908, 0x2b19080819080808, 0x2b1908082b2b0819, 0x2b1908190819192b, + 0x2b1908192b080808, 0x2b19082b19081919, 0x2b19190808080808, 0x2b191908082b082b, + 0x2b19190819081908, 0x2b19191919190819, 0x2b192b082b080819, 0x2b192b19082b0808, + 0x2b2b08080808082b, 0x2b2b080819190808, 0x2b2b08082b081919, 0x2b2b081908082b19, + 0x2b2b082b08080808, 0x2b2b190808192b08, 0x2b2b2b0819190808, 0x2b2b2b1908081908, +GGML_TABLE_END() + +GGML_TABLE_BEGIN(uint64_t, iq2xs_grid, 512) + 0x0808080808080808, 0x080808080808082b, 0x0808080808081919, 0x0808080808082b08, + 0x0808080808082b2b, 0x0808080808190819, 0x0808080808191908, 0x080808080819192b, + 0x0808080808192b19, 0x08080808082b0808, 0x08080808082b082b, 0x08080808082b1919, + 0x08080808082b2b08, 0x0808080819080819, 0x0808080819081908, 0x080808081908192b, + 0x0808080819082b19, 0x0808080819190808, 0x080808081919082b, 0x0808080819191919, + 0x0808080819192b08, 0x08080808192b0819, 0x08080808192b1908, 0x080808082b080808, + 0x080808082b08082b, 0x080808082b081919, 0x080808082b082b08, 0x080808082b190819, + 0x080808082b191908, 0x080808082b192b19, 0x080808082b2b0808, 0x0808081908080819, + 0x0808081908081908, 0x080808190808192b, 0x0808081908082b19, 0x0808081908190808, + 0x080808190819082b, 0x0808081908191919, 0x0808081908192b08, 0x0808081908192b2b, + 0x08080819082b0819, 0x08080819082b1908, 0x0808081919080808, 0x080808191908082b, + 0x0808081919081919, 0x0808081919082b08, 0x0808081919190819, 0x0808081919191908, + 0x08080819192b0808, 0x08080819192b2b08, 0x080808192b080819, 0x080808192b081908, + 0x080808192b190808, 0x0808082b08080808, 0x0808082b0808082b, 0x0808082b08081919, + 0x0808082b08082b08, 0x0808082b08190819, 0x0808082b08191908, 0x0808082b082b0808, + 0x0808082b19080819, 0x0808082b19081908, 0x0808082b19190808, 0x0808082b19191919, + 0x0808082b2b080808, 0x0808082b2b082b2b, 0x0808190808080819, 0x0808190808081908, + 0x080819080808192b, 0x0808190808082b19, 0x0808190808190808, 0x080819080819082b, + 0x0808190808191919, 0x0808190808192b08, 0x08081908082b0819, 0x08081908082b1908, + 0x0808190819080808, 0x080819081908082b, 0x0808190819081919, 0x0808190819082b08, + 0x0808190819190819, 0x0808190819191908, 0x080819081919192b, 0x08081908192b0808, + 0x080819082b080819, 0x080819082b081908, 0x080819082b190808, 0x0808191908080808, + 0x080819190808082b, 0x0808191908081919, 0x0808191908082b08, 0x0808191908190819, + 0x0808191908191908, 0x08081919082b0808, 0x0808191919080819, 0x0808191919081908, + 0x0808191919190808, 0x08081919192b0819, 0x080819192b080808, 0x0808192b08080819, + 0x0808192b08081908, 0x0808192b08190808, 0x0808192b082b192b, 0x0808192b19080808, + 0x0808192b1908082b, 0x0808192b2b081908, 0x08082b0808080808, 0x08082b080808082b, + 0x08082b0808081919, 0x08082b0808082b08, 0x08082b0808082b2b, 0x08082b0808190819, + 0x08082b0808191908, 0x08082b08082b0808, 0x08082b08082b1919, 0x08082b0819080819, + 0x08082b0819081908, 0x08082b0819190808, 0x08082b0819192b08, 0x08082b082b080808, + 0x08082b082b2b0808, 0x08082b082b2b2b2b, 0x08082b1908080819, 0x08082b1908081908, + 0x08082b1908190808, 0x08082b1919080808, 0x08082b192b080819, 0x08082b192b082b19, + 0x08082b2b08080808, 0x08082b2b082b0808, 0x08082b2b082b2b08, 0x08082b2b2b19192b, + 0x08082b2b2b2b0808, 0x0819080808080819, 0x0819080808081908, 0x081908080808192b, + 0x0819080808082b19, 0x0819080808190808, 0x081908080819082b, 0x0819080808191919, + 0x0819080808192b08, 0x08190808082b0819, 0x08190808082b1908, 0x0819080819080808, + 0x081908081908082b, 0x0819080819081919, 0x0819080819082b08, 0x0819080819190819, + 0x0819080819191908, 0x08190808192b0808, 0x08190808192b2b2b, 0x081908082b080819, + 0x081908082b081908, 0x081908082b190808, 0x0819081908080808, 0x081908190808082b, + 0x0819081908081919, 0x0819081908082b08, 0x0819081908190819, 0x0819081908191908, + 0x08190819082b0808, 0x0819081919080819, 0x0819081919081908, 0x0819081919190808, + 0x081908192b080808, 0x081908192b191908, 0x081908192b19192b, 0x0819082b08080819, + 0x0819082b08081908, 0x0819082b0808192b, 0x0819082b08190808, 0x0819082b19080808, + 0x0819082b192b0808, 0x0819190808080808, 0x081919080808082b, 0x0819190808081919, + 0x0819190808082b08, 0x0819190808190819, 0x0819190808191908, 0x08191908082b0808, + 0x0819190819080819, 0x0819190819081908, 0x0819190819082b19, 0x0819190819190808, + 0x08191908192b1908, 0x081919082b080808, 0x0819191908080819, 0x0819191908081908, + 0x0819191908190808, 0x0819191919080808, 0x0819192b08080808, 0x0819192b08191908, + 0x0819192b19082b19, 0x08192b0808080819, 0x08192b0808081908, 0x08192b0808190808, + 0x08192b080819082b, 0x08192b0819080808, 0x08192b0819191908, 0x08192b082b08192b, + 0x08192b1908080808, 0x08192b1908081919, 0x08192b19192b192b, 0x08192b2b19190819, + 0x08192b2b2b2b2b19, 0x082b080808080808, 0x082b08080808082b, 0x082b080808081919, + 0x082b080808082b08, 0x082b080808082b2b, 0x082b080808190819, 0x082b080808191908, + 0x082b0808082b0808, 0x082b080819080819, 0x082b080819081908, 0x082b080819190808, + 0x082b08082b080808, 0x082b08082b2b0808, 0x082b081908080819, 0x082b081908081908, + 0x082b081908190808, 0x082b081919080808, 0x082b081919082b08, 0x082b0819192b1919, + 0x082b082b08080808, 0x082b082b082b082b, 0x082b082b2b080808, 0x082b082b2b2b2b08, + 0x082b190808080819, 0x082b190808081908, 0x082b190808190808, 0x082b1908082b2b19, + 0x082b190819080808, 0x082b191908080808, 0x082b191919080819, 0x082b19191919082b, + 0x082b19192b192b19, 0x082b192b08080819, 0x082b192b08192b2b, 0x082b192b2b2b192b, + 0x082b2b0808080808, 0x082b2b0808082b08, 0x082b2b0808082b2b, 0x082b2b08082b0808, + 0x082b2b0819191919, 0x082b2b082b082b08, 0x082b2b082b2b082b, 0x082b2b19192b2b08, + 0x082b2b192b190808, 0x082b2b2b08082b08, 0x082b2b2b082b0808, 0x082b2b2b2b08082b, + 0x082b2b2b2b082b08, 0x082b2b2b2b082b2b, 0x1908080808080819, 0x1908080808081908, + 0x190808080808192b, 0x1908080808082b19, 0x1908080808190808, 0x190808080819082b, + 0x1908080808191919, 0x1908080808192b08, 0x19080808082b0819, 0x19080808082b1908, + 0x1908080819080808, 0x190808081908082b, 0x1908080819081919, 0x1908080819082b08, + 0x1908080819082b2b, 0x1908080819190819, 0x1908080819191908, 0x19080808192b0808, + 0x19080808192b1919, 0x190808082b080819, 0x190808082b081908, 0x190808082b190808, + 0x1908081908080808, 0x190808190808082b, 0x1908081908081919, 0x1908081908082b08, + 0x1908081908190819, 0x1908081908191908, 0x19080819082b0808, 0x1908081919080819, + 0x1908081919081908, 0x1908081919190808, 0x190808192b080808, 0x190808192b081919, + 0x190808192b2b082b, 0x1908082b08080819, 0x1908082b08081908, 0x1908082b08190808, + 0x1908082b0819082b, 0x1908082b082b2b19, 0x1908082b19080808, 0x1908190808080808, + 0x190819080808082b, 0x1908190808081919, 0x1908190808082b08, 0x1908190808190819, + 0x1908190808191908, 0x1908190808192b19, 0x19081908082b0808, 0x1908190819080819, + 0x1908190819081908, 0x1908190819190808, 0x190819082b080808, 0x190819082b191908, + 0x1908191908080819, 0x1908191908081908, 0x1908191908190808, 0x19081919082b1908, + 0x1908191919080808, 0x190819192b192b2b, 0x1908192b08080808, 0x1908192b08082b2b, + 0x1908192b19081908, 0x1908192b19190808, 0x19082b0808080819, 0x19082b0808081908, + 0x19082b0808190808, 0x19082b0819080808, 0x19082b0819081919, 0x19082b0819191908, + 0x19082b08192b082b, 0x19082b1908080808, 0x19082b1908190819, 0x19082b1919081908, + 0x19082b1919190808, 0x19082b19192b2b19, 0x19082b2b08081908, 0x1919080808080808, + 0x191908080808082b, 0x1919080808081919, 0x1919080808082b08, 0x1919080808190819, + 0x1919080808191908, 0x19190808082b0808, 0x19190808082b2b08, 0x1919080819080819, + 0x1919080819081908, 0x1919080819190808, 0x191908082b080808, 0x1919081908080819, + 0x1919081908081908, 0x1919081908190808, 0x1919081908191919, 0x1919081919080808, + 0x191908191908082b, 0x1919082b08080808, 0x1919082b19081908, 0x1919082b2b2b2b2b, + 0x1919190808080819, 0x1919190808081908, 0x1919190808190808, 0x19191908082b0819, + 0x1919190819080808, 0x19191908192b0808, 0x191919082b080819, 0x191919082b2b0819, + 0x1919191908080808, 0x1919191908082b08, 0x191919192b080808, 0x191919192b082b08, + 0x1919192b082b0819, 0x1919192b192b2b08, 0x1919192b2b2b0819, 0x19192b0808080808, + 0x19192b0808191908, 0x19192b0819080819, 0x19192b0819190808, 0x19192b082b192b19, + 0x19192b1908192b2b, 0x19192b1919080808, 0x19192b191908082b, 0x19192b2b2b081919, + 0x192b080808080819, 0x192b080808081908, 0x192b080808190808, 0x192b080819080808, + 0x192b080819191908, 0x192b0808192b082b, 0x192b08082b08192b, 0x192b08082b2b2b19, + 0x192b081908080808, 0x192b082b082b1908, 0x192b082b19082b2b, 0x192b082b2b19082b, + 0x192b190808080808, 0x192b19080819192b, 0x192b191908190808, 0x192b191919080808, + 0x192b191919081919, 0x192b19192b2b1908, 0x192b2b0808080819, 0x192b2b08192b2b2b, + 0x192b2b19082b1919, 0x192b2b2b0808192b, 0x192b2b2b19191908, 0x192b2b2b192b082b, + 0x2b08080808080808, 0x2b0808080808082b, 0x2b08080808081919, 0x2b08080808082b08, + 0x2b08080808190819, 0x2b08080808191908, 0x2b080808082b0808, 0x2b080808082b2b2b, + 0x2b08080819080819, 0x2b08080819081908, 0x2b08080819190808, 0x2b0808082b080808, + 0x2b0808082b08082b, 0x2b0808082b2b2b08, 0x2b0808082b2b2b2b, 0x2b08081908080819, + 0x2b08081908081908, 0x2b0808190808192b, 0x2b08081908190808, 0x2b08081919080808, + 0x2b08081919190819, 0x2b08081919192b19, 0x2b08082b08080808, 0x2b08082b082b0808, + 0x2b08082b2b080808, 0x2b08082b2b08082b, 0x2b08082b2b2b0808, 0x2b08082b2b2b2b08, + 0x2b08190808080819, 0x2b08190808081908, 0x2b08190808190808, 0x2b0819080819082b, + 0x2b08190808191919, 0x2b08190819080808, 0x2b081908192b0808, 0x2b0819082b082b19, + 0x2b08191908080808, 0x2b08191919081908, 0x2b0819192b2b1919, 0x2b08192b08192b08, + 0x2b08192b192b2b2b, 0x2b082b0808080808, 0x2b082b0808082b08, 0x2b082b08082b1919, + 0x2b082b0819192b2b, 0x2b082b082b080808, 0x2b082b082b08082b, 0x2b082b082b2b2b08, + 0x2b082b190808192b, 0x2b082b2b082b082b, 0x2b082b2b2b080808, 0x2b082b2b2b082b08, + 0x2b082b2b2b19192b, 0x2b082b2b2b2b2b08, 0x2b19080808080819, 0x2b19080808081908, + 0x2b19080808190808, 0x2b19080819080808, 0x2b1908081919192b, 0x2b1908082b081908, + 0x2b19081908080808, 0x2b190819082b082b, 0x2b190819192b1908, 0x2b19082b1919192b, + 0x2b19082b2b082b19, 0x2b19190808080808, 0x2b19190808081919, 0x2b19190819081908, + 0x2b19190819190808, 0x2b19190819192b08, 0x2b191919082b2b19, 0x2b1919192b190808, + 0x2b1919192b19082b, 0x2b19192b19080819, 0x2b192b0819190819, 0x2b192b082b2b192b, + 0x2b192b1919082b19, 0x2b192b2b08191919, 0x2b192b2b192b0808, 0x2b2b080808080808, + 0x2b2b08080808082b, 0x2b2b080808082b08, 0x2b2b080808082b2b, 0x2b2b0808082b0808, + 0x2b2b0808082b2b2b, 0x2b2b08082b2b0808, 0x2b2b081919190819, 0x2b2b081919192b19, + 0x2b2b08192b2b192b, 0x2b2b082b08080808, 0x2b2b082b0808082b, 0x2b2b082b08082b08, + 0x2b2b082b082b2b2b, 0x2b2b082b2b080808, 0x2b2b082b2b2b0808, 0x2b2b190819080808, + 0x2b2b19082b191919, 0x2b2b192b192b1919, 0x2b2b192b2b192b08, 0x2b2b2b0808082b2b, + 0x2b2b2b08082b0808, 0x2b2b2b08082b082b, 0x2b2b2b08082b2b08, 0x2b2b2b082b2b0808, + 0x2b2b2b082b2b2b08, 0x2b2b2b1908081908, 0x2b2b2b192b081908, 0x2b2b2b192b08192b, + 0x2b2b2b2b082b2b08, 0x2b2b2b2b082b2b2b, 0x2b2b2b2b2b190819, 0x2b2b2b2b2b2b2b2b, +GGML_TABLE_END() + +GGML_TABLE_BEGIN(uint64_t, iq2s_grid, 1024) + 0x0808080808080808, 0x080808080808082b, 0x0808080808081919, 0x0808080808082b08, + 0x0808080808082b2b, 0x0808080808190819, 0x0808080808191908, 0x080808080819192b, + 0x0808080808192b19, 0x08080808082b0808, 0x08080808082b082b, 0x08080808082b1919, + 0x08080808082b2b08, 0x0808080819080819, 0x0808080819081908, 0x080808081908192b, + 0x0808080819082b19, 0x0808080819190808, 0x080808081919082b, 0x0808080819191919, + 0x0808080819192b08, 0x08080808192b0819, 0x08080808192b1908, 0x08080808192b192b, + 0x08080808192b2b19, 0x080808082b080808, 0x080808082b08082b, 0x080808082b081919, + 0x080808082b082b08, 0x080808082b190819, 0x080808082b191908, 0x080808082b2b0808, + 0x080808082b2b1919, 0x080808082b2b2b2b, 0x0808081908080819, 0x0808081908081908, + 0x080808190808192b, 0x0808081908082b19, 0x0808081908190808, 0x080808190819082b, + 0x0808081908191919, 0x0808081908192b08, 0x08080819082b0819, 0x08080819082b1908, + 0x0808081919080808, 0x080808191908082b, 0x0808081919081919, 0x0808081919082b08, + 0x0808081919190819, 0x0808081919191908, 0x080808191919192b, 0x0808081919192b19, + 0x08080819192b0808, 0x08080819192b1919, 0x08080819192b2b08, 0x080808192b080819, + 0x080808192b081908, 0x080808192b190808, 0x080808192b19082b, 0x080808192b191919, + 0x080808192b2b0819, 0x080808192b2b1908, 0x0808082b08080808, 0x0808082b0808082b, + 0x0808082b08081919, 0x0808082b08082b08, 0x0808082b08190819, 0x0808082b08191908, + 0x0808082b082b0808, 0x0808082b082b2b2b, 0x0808082b19080819, 0x0808082b19081908, + 0x0808082b1908192b, 0x0808082b19082b19, 0x0808082b19190808, 0x0808082b19191919, + 0x0808082b2b080808, 0x0808082b2b081919, 0x0808082b2b082b2b, 0x0808082b2b191908, + 0x0808082b2b2b082b, 0x0808190808080819, 0x0808190808081908, 0x080819080808192b, + 0x0808190808082b19, 0x0808190808190808, 0x080819080819082b, 0x0808190808191919, + 0x0808190808192b08, 0x08081908082b0819, 0x08081908082b1908, 0x08081908082b192b, + 0x08081908082b2b19, 0x0808190819080808, 0x080819081908082b, 0x0808190819081919, + 0x0808190819082b08, 0x0808190819082b2b, 0x0808190819190819, 0x0808190819191908, + 0x080819081919192b, 0x0808190819192b19, 0x08081908192b0808, 0x08081908192b082b, + 0x08081908192b1919, 0x080819082b080819, 0x080819082b081908, 0x080819082b08192b, + 0x080819082b082b19, 0x080819082b190808, 0x080819082b191919, 0x080819082b192b08, + 0x080819082b2b0819, 0x080819082b2b1908, 0x0808191908080808, 0x080819190808082b, + 0x0808191908081919, 0x0808191908082b08, 0x0808191908082b2b, 0x0808191908190819, + 0x0808191908191908, 0x080819190819192b, 0x0808191908192b19, 0x08081919082b0808, + 0x08081919082b1919, 0x08081919082b2b08, 0x0808191919080819, 0x0808191919081908, + 0x080819191908192b, 0x0808191919082b19, 0x0808191919190808, 0x080819191919082b, + 0x0808191919191919, 0x0808191919192b08, 0x08081919192b0819, 0x08081919192b1908, + 0x080819192b080808, 0x080819192b08082b, 0x080819192b081919, 0x080819192b082b08, + 0x080819192b190819, 0x080819192b191908, 0x080819192b2b0808, 0x0808192b08080819, + 0x0808192b08081908, 0x0808192b0808192b, 0x0808192b08082b19, 0x0808192b08190808, + 0x0808192b08191919, 0x0808192b19080808, 0x0808192b19081919, 0x0808192b19082b08, + 0x0808192b19190819, 0x0808192b19191908, 0x0808192b192b0808, 0x0808192b2b080819, + 0x0808192b2b081908, 0x0808192b2b190808, 0x08082b0808080808, 0x08082b080808082b, + 0x08082b0808081919, 0x08082b0808082b08, 0x08082b0808190819, 0x08082b0808191908, + 0x08082b080819192b, 0x08082b0808192b19, 0x08082b08082b0808, 0x08082b08082b1919, + 0x08082b08082b2b2b, 0x08082b0819080819, 0x08082b0819081908, 0x08082b081908192b, + 0x08082b0819082b19, 0x08082b0819190808, 0x08082b081919082b, 0x08082b0819191919, + 0x08082b0819192b08, 0x08082b08192b0819, 0x08082b08192b1908, 0x08082b082b080808, + 0x08082b082b081919, 0x08082b082b191908, 0x08082b082b2b2b2b, 0x08082b1908080819, + 0x08082b1908081908, 0x08082b1908190808, 0x08082b190819082b, 0x08082b1908191919, + 0x08082b1908192b08, 0x08082b19082b0819, 0x08082b1919080808, 0x08082b1919081919, + 0x08082b1919082b08, 0x08082b1919190819, 0x08082b1919191908, 0x08082b19192b0808, + 0x08082b192b080819, 0x08082b192b190808, 0x08082b2b08080808, 0x08082b2b08190819, + 0x08082b2b08191908, 0x08082b2b082b082b, 0x08082b2b082b2b08, 0x08082b2b082b2b2b, + 0x08082b2b19190808, 0x08082b2b2b192b19, 0x0819080808080819, 0x0819080808081908, + 0x081908080808192b, 0x0819080808082b19, 0x0819080808190808, 0x081908080819082b, + 0x0819080808191919, 0x0819080808192b08, 0x08190808082b0819, 0x08190808082b1908, + 0x08190808082b192b, 0x0819080819080808, 0x081908081908082b, 0x0819080819081919, + 0x0819080819082b08, 0x0819080819190819, 0x0819080819191908, 0x081908081919192b, + 0x0819080819192b19, 0x08190808192b0808, 0x08190808192b082b, 0x08190808192b1919, + 0x08190808192b2b08, 0x081908082b080819, 0x081908082b081908, 0x081908082b08192b, + 0x081908082b190808, 0x081908082b191919, 0x081908082b192b08, 0x081908082b2b0819, + 0x081908082b2b1908, 0x0819081908080808, 0x081908190808082b, 0x0819081908081919, + 0x0819081908082b08, 0x0819081908082b2b, 0x0819081908190819, 0x0819081908191908, + 0x081908190819192b, 0x0819081908192b19, 0x08190819082b0808, 0x08190819082b082b, + 0x08190819082b1919, 0x08190819082b2b08, 0x0819081919080819, 0x0819081919081908, + 0x081908191908192b, 0x0819081919082b19, 0x0819081919190808, 0x081908191919082b, + 0x0819081919191919, 0x0819081919192b08, 0x08190819192b0819, 0x08190819192b1908, + 0x081908192b080808, 0x081908192b08082b, 0x081908192b081919, 0x081908192b082b08, + 0x081908192b190819, 0x081908192b191908, 0x0819082b08080819, 0x0819082b08081908, + 0x0819082b08082b19, 0x0819082b08190808, 0x0819082b08191919, 0x0819082b082b0819, + 0x0819082b082b1908, 0x0819082b19080808, 0x0819082b19081919, 0x0819082b19190819, + 0x0819082b19191908, 0x0819082b2b080819, 0x0819082b2b081908, 0x0819082b2b190808, + 0x0819190808080808, 0x081919080808082b, 0x0819190808081919, 0x0819190808082b08, + 0x0819190808190819, 0x0819190808191908, 0x081919080819192b, 0x0819190808192b19, + 0x08191908082b0808, 0x08191908082b1919, 0x08191908082b2b08, 0x0819190819080819, + 0x0819190819081908, 0x081919081908192b, 0x0819190819082b19, 0x0819190819190808, + 0x081919081919082b, 0x0819190819191919, 0x0819190819192b08, 0x08191908192b0819, + 0x08191908192b1908, 0x081919082b080808, 0x081919082b08082b, 0x081919082b081919, + 0x081919082b082b08, 0x081919082b190819, 0x081919082b191908, 0x081919082b2b0808, + 0x0819191908080819, 0x0819191908081908, 0x081919190808192b, 0x0819191908082b19, + 0x0819191908190808, 0x081919190819082b, 0x0819191908191919, 0x0819191908192b08, + 0x08191919082b0819, 0x08191919082b1908, 0x0819191919080808, 0x081919191908082b, + 0x0819191919081919, 0x0819191919082b08, 0x0819191919190819, 0x0819191919191908, + 0x08191919192b0808, 0x081919192b080819, 0x081919192b081908, 0x081919192b190808, + 0x0819192b08080808, 0x0819192b08081919, 0x0819192b08082b08, 0x0819192b08190819, + 0x0819192b08191908, 0x0819192b082b0808, 0x0819192b19080819, 0x0819192b19081908, + 0x0819192b19190808, 0x0819192b2b080808, 0x0819192b2b2b2b2b, 0x08192b0808080819, + 0x08192b0808081908, 0x08192b080808192b, 0x08192b0808082b19, 0x08192b0808190808, + 0x08192b0808191919, 0x08192b0808192b08, 0x08192b08082b0819, 0x08192b0819080808, + 0x08192b081908082b, 0x08192b0819081919, 0x08192b0819082b08, 0x08192b0819190819, + 0x08192b0819191908, 0x08192b08192b0808, 0x08192b082b080819, 0x08192b082b081908, + 0x08192b1908080808, 0x08192b190808082b, 0x08192b1908081919, 0x08192b1908082b08, + 0x08192b1908190819, 0x08192b1908191908, 0x08192b19082b0808, 0x08192b1919080819, + 0x08192b1919081908, 0x08192b1919190808, 0x08192b19192b2b19, 0x08192b192b2b082b, + 0x08192b2b08081908, 0x08192b2b08190808, 0x08192b2b19080808, 0x08192b2b1919192b, + 0x082b080808080808, 0x082b08080808082b, 0x082b080808081919, 0x082b080808082b08, + 0x082b080808190819, 0x082b080808191908, 0x082b08080819192b, 0x082b080808192b19, + 0x082b0808082b0808, 0x082b0808082b1919, 0x082b0808082b2b2b, 0x082b080819080819, + 0x082b080819081908, 0x082b080819190808, 0x082b08081919082b, 0x082b080819191919, + 0x082b0808192b1908, 0x082b08082b080808, 0x082b08082b082b2b, 0x082b08082b191908, + 0x082b08082b2b2b2b, 0x082b081908080819, 0x082b081908081908, 0x082b081908190808, + 0x082b08190819082b, 0x082b081908191919, 0x082b0819082b0819, 0x082b081919080808, + 0x082b08191908082b, 0x082b081919081919, 0x082b081919190819, 0x082b081919191908, + 0x082b0819192b0808, 0x082b08192b080819, 0x082b08192b081908, 0x082b08192b190808, + 0x082b082b08080808, 0x082b082b08082b2b, 0x082b082b082b082b, 0x082b082b082b2b08, + 0x082b082b082b2b2b, 0x082b082b19081908, 0x082b082b19190808, 0x082b082b2b082b08, + 0x082b082b2b082b2b, 0x082b082b2b2b2b08, 0x082b190808080819, 0x082b190808081908, + 0x082b19080808192b, 0x082b190808082b19, 0x082b190808190808, 0x082b190808191919, + 0x082b190808192b08, 0x082b1908082b0819, 0x082b1908082b1908, 0x082b190819080808, + 0x082b19081908082b, 0x082b190819081919, 0x082b190819082b08, 0x082b190819190819, + 0x082b190819191908, 0x082b1908192b0808, 0x082b19082b080819, 0x082b19082b081908, + 0x082b19082b190808, 0x082b191908080808, 0x082b191908081919, 0x082b191908082b08, + 0x082b191908190819, 0x082b191908191908, 0x082b1919082b0808, 0x082b191919080819, + 0x082b191919081908, 0x082b191919190808, 0x082b1919192b192b, 0x082b19192b080808, + 0x082b192b08080819, 0x082b192b08081908, 0x082b192b08190808, 0x082b192b19080808, + 0x082b192b19192b19, 0x082b2b0808080808, 0x082b2b0808081919, 0x082b2b0808190819, + 0x082b2b0808191908, 0x082b2b0819080819, 0x082b2b0819081908, 0x082b2b0819190808, + 0x082b2b082b082b2b, 0x082b2b082b2b2b2b, 0x082b2b1908080819, 0x082b2b1908081908, + 0x082b2b1908190808, 0x082b2b192b191919, 0x082b2b2b08082b2b, 0x082b2b2b082b082b, + 0x082b2b2b192b1908, 0x082b2b2b2b082b08, 0x082b2b2b2b082b2b, 0x1908080808080819, + 0x1908080808081908, 0x190808080808192b, 0x1908080808082b19, 0x1908080808190808, + 0x190808080819082b, 0x1908080808191919, 0x1908080808192b08, 0x1908080808192b2b, + 0x19080808082b0819, 0x19080808082b1908, 0x19080808082b192b, 0x1908080819080808, + 0x190808081908082b, 0x1908080819081919, 0x1908080819082b08, 0x1908080819082b2b, + 0x1908080819190819, 0x1908080819191908, 0x190808081919192b, 0x1908080819192b19, + 0x19080808192b0808, 0x19080808192b082b, 0x19080808192b1919, 0x190808082b080819, + 0x190808082b081908, 0x190808082b190808, 0x190808082b191919, 0x190808082b192b08, + 0x190808082b2b0819, 0x190808082b2b1908, 0x1908081908080808, 0x190808190808082b, + 0x1908081908081919, 0x1908081908082b08, 0x1908081908190819, 0x1908081908191908, + 0x190808190819192b, 0x1908081908192b19, 0x19080819082b0808, 0x19080819082b082b, + 0x19080819082b1919, 0x1908081919080819, 0x1908081919081908, 0x190808191908192b, + 0x1908081919082b19, 0x1908081919190808, 0x190808191919082b, 0x1908081919191919, + 0x1908081919192b08, 0x19080819192b0819, 0x19080819192b1908, 0x190808192b080808, + 0x190808192b08082b, 0x190808192b081919, 0x190808192b082b08, 0x190808192b190819, + 0x190808192b191908, 0x190808192b2b0808, 0x1908082b08080819, 0x1908082b08081908, + 0x1908082b08190808, 0x1908082b0819082b, 0x1908082b08191919, 0x1908082b08192b08, + 0x1908082b082b1908, 0x1908082b19080808, 0x1908082b19081919, 0x1908082b19082b08, + 0x1908082b19190819, 0x1908082b19191908, 0x1908082b192b0808, 0x1908082b2b080819, + 0x1908082b2b081908, 0x1908190808080808, 0x190819080808082b, 0x1908190808081919, + 0x1908190808082b08, 0x1908190808082b2b, 0x1908190808190819, 0x1908190808191908, + 0x190819080819192b, 0x1908190808192b19, 0x19081908082b0808, 0x19081908082b082b, + 0x19081908082b1919, 0x19081908082b2b08, 0x1908190819080819, 0x1908190819081908, + 0x190819081908192b, 0x1908190819082b19, 0x1908190819190808, 0x190819081919082b, + 0x1908190819191919, 0x1908190819192b08, 0x19081908192b0819, 0x19081908192b1908, + 0x190819082b080808, 0x190819082b08082b, 0x190819082b081919, 0x190819082b082b08, + 0x190819082b190819, 0x190819082b191908, 0x190819082b2b0808, 0x1908191908080819, + 0x1908191908081908, 0x190819190808192b, 0x1908191908082b19, 0x1908191908190808, + 0x190819190819082b, 0x1908191908191919, 0x1908191908192b08, 0x19081919082b0819, + 0x19081919082b1908, 0x1908191919080808, 0x190819191908082b, 0x1908191919081919, + 0x1908191919082b08, 0x1908191919190819, 0x1908191919191908, 0x19081919192b0808, + 0x19081919192b2b2b, 0x190819192b080819, 0x190819192b081908, 0x190819192b190808, + 0x1908192b08080808, 0x1908192b0808082b, 0x1908192b08081919, 0x1908192b08082b08, + 0x1908192b08190819, 0x1908192b08191908, 0x1908192b082b0808, 0x1908192b19080819, + 0x1908192b19081908, 0x1908192b19190808, 0x1908192b2b080808, 0x1908192b2b2b1919, + 0x19082b0808080819, 0x19082b0808081908, 0x19082b0808082b19, 0x19082b0808190808, + 0x19082b080819082b, 0x19082b0808191919, 0x19082b0808192b08, 0x19082b08082b0819, + 0x19082b08082b1908, 0x19082b0819080808, 0x19082b081908082b, 0x19082b0819081919, + 0x19082b0819082b08, 0x19082b0819190819, 0x19082b0819191908, 0x19082b08192b0808, + 0x19082b082b081908, 0x19082b082b190808, 0x19082b1908080808, 0x19082b190808082b, + 0x19082b1908081919, 0x19082b1908082b08, 0x19082b1908190819, 0x19082b1908191908, + 0x19082b19082b0808, 0x19082b1919080819, 0x19082b1919081908, 0x19082b1919190808, + 0x19082b192b080808, 0x19082b192b19192b, 0x19082b2b08080819, 0x19082b2b08081908, + 0x19082b2b08190808, 0x19082b2b19080808, 0x1919080808080808, 0x191908080808082b, + 0x1919080808081919, 0x1919080808082b08, 0x1919080808190819, 0x1919080808191908, + 0x191908080819192b, 0x1919080808192b19, 0x19190808082b0808, 0x19190808082b082b, + 0x19190808082b1919, 0x19190808082b2b08, 0x1919080819080819, 0x1919080819081908, + 0x191908081908192b, 0x1919080819082b19, 0x1919080819190808, 0x191908081919082b, + 0x1919080819191919, 0x1919080819192b08, 0x19190808192b0819, 0x19190808192b1908, + 0x191908082b080808, 0x191908082b08082b, 0x191908082b081919, 0x191908082b082b08, + 0x191908082b190819, 0x191908082b191908, 0x1919081908080819, 0x1919081908081908, + 0x191908190808192b, 0x1919081908082b19, 0x1919081908190808, 0x191908190819082b, + 0x1919081908191919, 0x1919081908192b08, 0x19190819082b0819, 0x19190819082b1908, + 0x1919081919080808, 0x191908191908082b, 0x1919081919081919, 0x1919081919082b08, + 0x1919081919190819, 0x1919081919191908, 0x19190819192b0808, 0x191908192b080819, + 0x191908192b081908, 0x191908192b190808, 0x1919082b08080808, 0x1919082b08081919, + 0x1919082b08082b08, 0x1919082b08190819, 0x1919082b08191908, 0x1919082b082b0808, + 0x1919082b19080819, 0x1919082b19081908, 0x1919082b19190808, 0x1919082b192b2b19, + 0x1919082b2b080808, 0x1919190808080819, 0x1919190808081908, 0x191919080808192b, + 0x1919190808082b19, 0x1919190808190808, 0x191919080819082b, 0x1919190808191919, + 0x1919190808192b08, 0x19191908082b0819, 0x19191908082b1908, 0x1919190819080808, + 0x191919081908082b, 0x1919190819081919, 0x1919190819082b08, 0x1919190819190819, + 0x1919190819191908, 0x19191908192b0808, 0x191919082b080819, 0x191919082b081908, + 0x191919082b190808, 0x1919191908080808, 0x191919190808082b, 0x1919191908081919, + 0x1919191908082b08, 0x1919191908190819, 0x1919191908191908, 0x19191919082b0808, + 0x1919191919080819, 0x1919191919081908, 0x1919191919190808, 0x191919192b080808, + 0x1919192b08080819, 0x1919192b08081908, 0x1919192b08190808, 0x1919192b082b192b, + 0x1919192b19080808, 0x19192b0808080808, 0x19192b080808082b, 0x19192b0808081919, + 0x19192b0808082b08, 0x19192b0808190819, 0x19192b0808191908, 0x19192b08082b0808, + 0x19192b0819080819, 0x19192b0819081908, 0x19192b0819190808, 0x19192b0819192b2b, + 0x19192b082b080808, 0x19192b1908080819, 0x19192b1908081908, 0x19192b1908190808, + 0x19192b1919080808, 0x19192b2b08080808, 0x19192b2b08192b19, 0x19192b2b2b081919, + 0x19192b2b2b2b2b08, 0x192b080808080819, 0x192b080808081908, 0x192b08080808192b, + 0x192b080808190808, 0x192b08080819082b, 0x192b080808191919, 0x192b080808192b08, + 0x192b0808082b0819, 0x192b0808082b1908, 0x192b080819080808, 0x192b080819081919, + 0x192b080819082b08, 0x192b080819190819, 0x192b080819191908, 0x192b0808192b0808, + 0x192b08082b081908, 0x192b08082b190808, 0x192b081908080808, 0x192b08190808082b, + 0x192b081908081919, 0x192b081908082b08, 0x192b081908190819, 0x192b081908191908, + 0x192b0819082b0808, 0x192b081919080819, 0x192b081919081908, 0x192b081919190808, + 0x192b08192b080808, 0x192b08192b192b19, 0x192b082b08081908, 0x192b082b08190808, + 0x192b082b19080808, 0x192b082b1919192b, 0x192b082b2b2b0819, 0x192b190808080808, + 0x192b190808081919, 0x192b190808082b08, 0x192b190808190819, 0x192b190808191908, + 0x192b1908082b0808, 0x192b190819080819, 0x192b190819081908, 0x192b190819190808, + 0x192b19082b080808, 0x192b191908080819, 0x192b191908081908, 0x192b191908190808, + 0x192b191919080808, 0x192b191919082b2b, 0x192b1919192b2b08, 0x192b19192b19082b, + 0x192b192b08080808, 0x192b192b2b191908, 0x192b2b0808080819, 0x192b2b0808081908, + 0x192b2b0808190808, 0x192b2b08192b1919, 0x192b2b082b192b08, 0x192b2b1908080808, + 0x192b2b19082b2b2b, 0x192b2b2b1908082b, 0x192b2b2b2b2b0819, 0x2b08080808080808, + 0x2b0808080808082b, 0x2b08080808081919, 0x2b08080808082b08, 0x2b08080808190819, + 0x2b08080808191908, 0x2b08080808192b19, 0x2b080808082b0808, 0x2b080808082b1919, + 0x2b08080819080819, 0x2b08080819081908, 0x2b08080819190808, 0x2b0808081919082b, + 0x2b08080819191919, 0x2b08080819192b08, 0x2b080808192b0819, 0x2b0808082b080808, + 0x2b0808082b081919, 0x2b0808082b190819, 0x2b0808082b191908, 0x2b08081908080819, + 0x2b08081908081908, 0x2b08081908082b19, 0x2b08081908190808, 0x2b0808190819082b, + 0x2b08081908191919, 0x2b08081908192b08, 0x2b080819082b0819, 0x2b080819082b1908, + 0x2b08081919080808, 0x2b0808191908082b, 0x2b08081919081919, 0x2b08081919082b08, + 0x2b08081919190819, 0x2b08081919191908, 0x2b0808192b080819, 0x2b0808192b081908, + 0x2b0808192b190808, 0x2b0808192b2b2b19, 0x2b08082b08080808, 0x2b08082b08081919, + 0x2b08082b08082b2b, 0x2b08082b08190819, 0x2b08082b08191908, 0x2b08082b19080819, + 0x2b08082b19081908, 0x2b08082b19190808, 0x2b08190808080819, 0x2b08190808081908, + 0x2b0819080808192b, 0x2b08190808082b19, 0x2b08190808190808, 0x2b0819080819082b, + 0x2b08190808191919, 0x2b08190808192b08, 0x2b081908082b0819, 0x2b08190819080808, + 0x2b0819081908082b, 0x2b08190819081919, 0x2b08190819082b08, 0x2b08190819190819, + 0x2b08190819191908, 0x2b081908192b0808, 0x2b0819082b080819, 0x2b0819082b081908, + 0x2b0819082b190808, 0x2b08191908080808, 0x2b0819190808082b, 0x2b08191908081919, + 0x2b08191908082b08, 0x2b08191908190819, 0x2b08191908191908, 0x2b081919082b0808, + 0x2b08191919080819, 0x2b08191919081908, 0x2b08191919190808, 0x2b0819192b080808, + 0x2b0819192b082b2b, 0x2b08192b08080819, 0x2b08192b08081908, 0x2b08192b08190808, + 0x2b08192b082b2b19, 0x2b08192b19080808, 0x2b082b0808080808, 0x2b082b0808081919, + 0x2b082b0808190819, 0x2b082b0808191908, 0x2b082b0819080819, 0x2b082b0819081908, + 0x2b082b0819190808, 0x2b082b082b2b082b, 0x2b082b1908080819, 0x2b082b1908081908, + 0x2b082b1919080808, 0x2b082b19192b1919, 0x2b082b2b082b082b, 0x2b082b2b19192b08, + 0x2b082b2b19192b2b, 0x2b082b2b2b08082b, 0x2b082b2b2b2b082b, 0x2b19080808080819, + 0x2b19080808081908, 0x2b19080808082b19, 0x2b19080808190808, 0x2b1908080819082b, + 0x2b19080808191919, 0x2b19080808192b08, 0x2b190808082b1908, 0x2b19080819080808, + 0x2b1908081908082b, 0x2b19080819081919, 0x2b19080819082b08, 0x2b19080819190819, + 0x2b19080819191908, 0x2b190808192b0808, 0x2b1908082b080819, 0x2b1908082b081908, + 0x2b1908082b190808, 0x2b19081908080808, 0x2b19081908081919, 0x2b19081908190819, + 0x2b19081908191908, 0x2b19081919080819, 0x2b19081919081908, 0x2b19081919190808, + 0x2b19081919192b2b, 0x2b19082b08080819, 0x2b19082b08081908, 0x2b19082b08190808, + 0x2b19082b19080808, 0x2b19082b2b2b192b, 0x2b19190808080808, 0x2b1919080808082b, + 0x2b19190808081919, 0x2b19190808082b08, 0x2b19190808190819, 0x2b19190808191908, + 0x2b191908082b0808, 0x2b19190819080819, 0x2b19190819081908, 0x2b19190819190808, + 0x2b1919082b080808, 0x2b1919082b19192b, 0x2b19191908080819, 0x2b19191908081908, + 0x2b19191908190808, 0x2b19191919080808, 0x2b1919192b192b08, 0x2b1919192b2b0819, + 0x2b19192b08080808, 0x2b19192b1908192b, 0x2b19192b192b1908, 0x2b192b0808080819, + 0x2b192b0808081908, 0x2b192b0808190808, 0x2b192b08082b192b, 0x2b192b0819080808, + 0x2b192b082b2b2b19, 0x2b192b1908080808, 0x2b192b1919082b19, 0x2b192b191919082b, + 0x2b192b2b2b190808, 0x2b2b080808080808, 0x2b2b080808081919, 0x2b2b080808082b2b, + 0x2b2b080808191908, 0x2b2b0808082b082b, 0x2b2b0808082b2b2b, 0x2b2b080819080819, + 0x2b2b080819081908, 0x2b2b080819190808, 0x2b2b08082b2b082b, 0x2b2b08082b2b2b2b, + 0x2b2b081919080808, 0x2b2b0819192b1919, 0x2b2b082b0808082b, 0x2b2b082b08082b2b, + 0x2b2b082b082b082b, 0x2b2b082b082b2b08, 0x2b2b082b082b2b2b, 0x2b2b082b2b08082b, + 0x2b2b082b2b082b08, 0x2b2b082b2b082b2b, 0x2b2b082b2b2b2b08, 0x2b2b190808080819, + 0x2b2b190808081908, 0x2b2b190808190808, 0x2b2b190819080808, 0x2b2b19082b082b19, + 0x2b2b19082b2b1908, 0x2b2b191908080808, 0x2b2b191908192b19, 0x2b2b192b19190819, + 0x2b2b2b0808082b2b, 0x2b2b2b08082b2b08, 0x2b2b2b082b2b082b, 0x2b2b2b1919191908, + 0x2b2b2b192b08192b, 0x2b2b2b2b08082b08, 0x2b2b2b2b08082b2b, 0x2b2b2b2b082b0808, + 0x2b2b2b2b082b082b, 0x2b2b2b2b082b2b08, 0x2b2b2b2b2b082b08, 0x2b2b2b2b2b2b2b2b, +GGML_TABLE_END() + +GGML_TABLE_BEGIN(uint32_t, iq3xxs_grid, 256) + 0x04040404, 0x04040414, 0x04040424, 0x04040c0c, 0x04040c1c, 0x04040c3e, 0x04041404, 0x04041414, + 0x04041c0c, 0x04042414, 0x04043e1c, 0x04043e2c, 0x040c040c, 0x040c041c, 0x040c0c04, 0x040c0c14, + 0x040c140c, 0x040c142c, 0x040c1c04, 0x040c1c14, 0x040c240c, 0x040c2c24, 0x040c3e04, 0x04140404, + 0x04140414, 0x04140424, 0x04140c0c, 0x04141404, 0x04141414, 0x04141c0c, 0x04141c1c, 0x04141c3e, + 0x04142c0c, 0x04142c3e, 0x04143e2c, 0x041c040c, 0x041c043e, 0x041c0c04, 0x041c0c14, 0x041c142c, + 0x041c3e04, 0x04240c1c, 0x04241c3e, 0x04242424, 0x04242c3e, 0x04243e1c, 0x04243e2c, 0x042c040c, + 0x042c043e, 0x042c1c14, 0x042c2c14, 0x04341c2c, 0x04343424, 0x043e0c04, 0x043e0c24, 0x043e0c34, + 0x043e241c, 0x043e340c, 0x0c04040c, 0x0c04041c, 0x0c040c04, 0x0c040c14, 0x0c04140c, 0x0c04141c, + 0x0c041c04, 0x0c041c14, 0x0c041c24, 0x0c04243e, 0x0c042c04, 0x0c0c0404, 0x0c0c0414, 0x0c0c0c0c, + 0x0c0c1404, 0x0c0c1414, 0x0c14040c, 0x0c14041c, 0x0c140c04, 0x0c140c14, 0x0c14140c, 0x0c141c04, + 0x0c143e14, 0x0c1c0404, 0x0c1c0414, 0x0c1c1404, 0x0c1c1c0c, 0x0c1c2434, 0x0c1c3434, 0x0c24040c, + 0x0c24042c, 0x0c242c04, 0x0c2c1404, 0x0c2c1424, 0x0c2c2434, 0x0c2c3e0c, 0x0c34042c, 0x0c3e1414, + 0x0c3e2404, 0x14040404, 0x14040414, 0x14040c0c, 0x14040c1c, 0x14041404, 0x14041414, 0x14041434, + 0x14041c0c, 0x14042414, 0x140c040c, 0x140c041c, 0x140c042c, 0x140c0c04, 0x140c0c14, 0x140c140c, + 0x140c1c04, 0x140c341c, 0x140c343e, 0x140c3e04, 0x14140404, 0x14140414, 0x14140c0c, 0x14140c3e, + 0x14141404, 0x14141414, 0x14141c3e, 0x14142404, 0x14142c2c, 0x141c040c, 0x141c0c04, 0x141c0c24, + 0x141c3e04, 0x141c3e24, 0x14241c2c, 0x14242c1c, 0x142c041c, 0x142c143e, 0x142c240c, 0x142c3e24, + 0x143e040c, 0x143e041c, 0x143e0c34, 0x143e242c, 0x1c04040c, 0x1c040c04, 0x1c040c14, 0x1c04140c, + 0x1c04141c, 0x1c042c04, 0x1c04342c, 0x1c043e14, 0x1c0c0404, 0x1c0c0414, 0x1c0c1404, 0x1c0c1c0c, + 0x1c0c2424, 0x1c0c2434, 0x1c14040c, 0x1c14041c, 0x1c140c04, 0x1c14142c, 0x1c142c14, 0x1c143e14, + 0x1c1c0c0c, 0x1c1c1c1c, 0x1c241c04, 0x1c24243e, 0x1c243e14, 0x1c2c0404, 0x1c2c0434, 0x1c2c1414, + 0x1c2c2c2c, 0x1c340c24, 0x1c341c34, 0x1c34341c, 0x1c3e1c1c, 0x1c3e3404, 0x24040424, 0x24040c3e, + 0x24041c2c, 0x24041c3e, 0x24042c1c, 0x24042c3e, 0x240c3e24, 0x24141404, 0x24141c3e, 0x24142404, + 0x24143404, 0x24143434, 0x241c043e, 0x241c242c, 0x24240424, 0x24242c0c, 0x24243424, 0x242c142c, + 0x242c241c, 0x242c3e04, 0x243e042c, 0x243e0c04, 0x243e0c14, 0x243e1c04, 0x2c040c14, 0x2c04240c, + 0x2c043e04, 0x2c0c0404, 0x2c0c0434, 0x2c0c1434, 0x2c0c2c2c, 0x2c140c24, 0x2c141c14, 0x2c143e14, + 0x2c1c0414, 0x2c1c2c1c, 0x2c240c04, 0x2c24141c, 0x2c24143e, 0x2c243e14, 0x2c2c0414, 0x2c2c1c0c, + 0x2c342c04, 0x2c3e1424, 0x2c3e2414, 0x34041424, 0x34042424, 0x34042434, 0x34043424, 0x340c140c, + 0x340c340c, 0x34140c3e, 0x34143424, 0x341c1c04, 0x341c1c34, 0x34242424, 0x342c042c, 0x342c2c14, + 0x34341c1c, 0x343e041c, 0x343e140c, 0x3e04041c, 0x3e04042c, 0x3e04043e, 0x3e040c04, 0x3e041c14, + 0x3e042c14, 0x3e0c1434, 0x3e0c2404, 0x3e140c14, 0x3e14242c, 0x3e142c14, 0x3e1c0404, 0x3e1c0c2c, + 0x3e1c1c1c, 0x3e1c3404, 0x3e24140c, 0x3e24240c, 0x3e2c0404, 0x3e2c0414, 0x3e2c1424, 0x3e341c04, +GGML_TABLE_END() + +GGML_TABLE_BEGIN(uint32_t, iq3s_grid, 512) + 0x01010101, 0x01010103, 0x01010105, 0x0101010b, 0x0101010f, 0x01010301, 0x01010303, 0x01010305, + 0x01010309, 0x0101030d, 0x01010501, 0x01010503, 0x0101050b, 0x01010707, 0x01010901, 0x01010905, + 0x0101090b, 0x0101090f, 0x01010b03, 0x01010b07, 0x01010d01, 0x01010d05, 0x01010f03, 0x01010f09, + 0x01010f0f, 0x01030101, 0x01030103, 0x01030105, 0x01030109, 0x01030301, 0x01030303, 0x0103030b, + 0x01030501, 0x01030507, 0x0103050f, 0x01030703, 0x0103070b, 0x01030909, 0x01030d03, 0x01030d0b, + 0x01030f05, 0x01050101, 0x01050103, 0x0105010b, 0x0105010f, 0x01050301, 0x01050307, 0x0105030d, + 0x01050503, 0x0105050b, 0x01050701, 0x01050709, 0x01050905, 0x0105090b, 0x0105090f, 0x01050b03, + 0x01050b07, 0x01050f01, 0x01050f07, 0x01070107, 0x01070303, 0x0107030b, 0x01070501, 0x01070505, + 0x01070703, 0x01070707, 0x0107070d, 0x01070909, 0x01070b01, 0x01070b05, 0x01070d0f, 0x01070f03, + 0x01070f0b, 0x01090101, 0x01090307, 0x0109030f, 0x01090503, 0x01090509, 0x01090705, 0x01090901, + 0x01090907, 0x01090b03, 0x01090f01, 0x010b0105, 0x010b0109, 0x010b0501, 0x010b0505, 0x010b050d, + 0x010b0707, 0x010b0903, 0x010b090b, 0x010b090f, 0x010b0d0d, 0x010b0f07, 0x010d010d, 0x010d0303, + 0x010d0307, 0x010d0703, 0x010d0b05, 0x010d0f03, 0x010f0101, 0x010f0105, 0x010f0109, 0x010f0501, + 0x010f0505, 0x010f050d, 0x010f0707, 0x010f0b01, 0x010f0b09, 0x03010101, 0x03010103, 0x03010105, + 0x03010109, 0x03010301, 0x03010303, 0x03010307, 0x0301030b, 0x0301030f, 0x03010501, 0x03010505, + 0x03010703, 0x03010709, 0x0301070d, 0x03010b09, 0x03010b0d, 0x03010d03, 0x03010f05, 0x03030101, + 0x03030103, 0x03030107, 0x0303010d, 0x03030301, 0x03030309, 0x03030503, 0x03030701, 0x03030707, + 0x03030903, 0x03030b01, 0x03030b05, 0x03030f01, 0x03030f0d, 0x03050101, 0x03050305, 0x0305030b, + 0x0305030f, 0x03050501, 0x03050509, 0x03050705, 0x03050901, 0x03050907, 0x03050b0b, 0x03050d01, + 0x03050f05, 0x03070103, 0x03070109, 0x0307010f, 0x03070301, 0x03070307, 0x03070503, 0x0307050f, + 0x03070701, 0x03070709, 0x03070903, 0x03070d05, 0x03070f01, 0x03090107, 0x0309010b, 0x03090305, + 0x03090309, 0x03090703, 0x03090707, 0x03090905, 0x0309090d, 0x03090b01, 0x03090b09, 0x030b0103, + 0x030b0301, 0x030b0307, 0x030b0503, 0x030b0701, 0x030b0705, 0x030b0b03, 0x030d0501, 0x030d0509, + 0x030d050f, 0x030d0909, 0x030d090d, 0x030f0103, 0x030f0107, 0x030f0301, 0x030f0305, 0x030f0503, + 0x030f070b, 0x030f0903, 0x030f0d05, 0x030f0f01, 0x05010101, 0x05010103, 0x05010107, 0x0501010b, + 0x0501010f, 0x05010301, 0x05010305, 0x05010309, 0x0501030d, 0x05010503, 0x05010507, 0x0501050f, + 0x05010701, 0x05010705, 0x05010903, 0x05010907, 0x0501090b, 0x05010b01, 0x05010b05, 0x05010d0f, + 0x05010f01, 0x05010f07, 0x05010f0b, 0x05030101, 0x05030105, 0x05030301, 0x05030307, 0x0503030f, + 0x05030505, 0x0503050b, 0x05030703, 0x05030709, 0x05030905, 0x05030b03, 0x05050103, 0x05050109, + 0x0505010f, 0x05050503, 0x05050507, 0x05050701, 0x0505070f, 0x05050903, 0x05050b07, 0x05050b0f, + 0x05050f03, 0x05050f09, 0x05070101, 0x05070105, 0x0507010b, 0x05070303, 0x05070505, 0x05070509, + 0x05070703, 0x05070707, 0x05070905, 0x05070b01, 0x05070d0d, 0x05090103, 0x0509010f, 0x05090501, + 0x05090507, 0x05090705, 0x0509070b, 0x05090903, 0x05090f05, 0x05090f0b, 0x050b0109, 0x050b0303, + 0x050b0505, 0x050b070f, 0x050b0901, 0x050b0b07, 0x050b0f01, 0x050d0101, 0x050d0105, 0x050d010f, + 0x050d0503, 0x050d0b0b, 0x050d0d03, 0x050f010b, 0x050f0303, 0x050f050d, 0x050f0701, 0x050f0907, + 0x050f0b01, 0x07010105, 0x07010303, 0x07010307, 0x0701030b, 0x0701030f, 0x07010505, 0x07010703, + 0x07010707, 0x0701070b, 0x07010905, 0x07010909, 0x0701090f, 0x07010b03, 0x07010d07, 0x07010f03, + 0x07030103, 0x07030107, 0x0703010b, 0x07030309, 0x07030503, 0x07030507, 0x07030901, 0x07030d01, + 0x07030f05, 0x07030f0d, 0x07050101, 0x07050305, 0x07050501, 0x07050705, 0x07050709, 0x07050b01, + 0x07070103, 0x07070301, 0x07070309, 0x07070503, 0x07070507, 0x0707050f, 0x07070701, 0x07070903, + 0x07070907, 0x0707090f, 0x07070b0b, 0x07070f07, 0x07090107, 0x07090303, 0x0709030d, 0x07090505, + 0x07090703, 0x07090b05, 0x07090d01, 0x07090d09, 0x070b0103, 0x070b0301, 0x070b0305, 0x070b050b, + 0x070b0705, 0x070b0909, 0x070b0b0d, 0x070b0f07, 0x070d030d, 0x070d0903, 0x070f0103, 0x070f0107, + 0x070f0501, 0x070f0505, 0x070f070b, 0x09010101, 0x09010109, 0x09010305, 0x09010501, 0x09010509, + 0x0901050f, 0x09010705, 0x09010903, 0x09010b01, 0x09010f01, 0x09030105, 0x0903010f, 0x09030303, + 0x09030307, 0x09030505, 0x09030701, 0x0903070b, 0x09030907, 0x09030b03, 0x09030b0b, 0x09050103, + 0x09050107, 0x09050301, 0x0905030b, 0x09050503, 0x09050707, 0x09050901, 0x09050b0f, 0x09050d05, + 0x09050f01, 0x09070109, 0x09070303, 0x09070307, 0x09070501, 0x09070505, 0x09070703, 0x0907070b, + 0x09090101, 0x09090105, 0x09090509, 0x0909070f, 0x09090901, 0x09090f03, 0x090b010b, 0x090b010f, + 0x090b0503, 0x090b0d05, 0x090d0307, 0x090d0709, 0x090d0d01, 0x090f0301, 0x090f030b, 0x090f0701, + 0x090f0907, 0x090f0b03, 0x0b010105, 0x0b010301, 0x0b010309, 0x0b010505, 0x0b010901, 0x0b010909, + 0x0b01090f, 0x0b010b05, 0x0b010d0d, 0x0b010f09, 0x0b030103, 0x0b030107, 0x0b03010b, 0x0b030305, + 0x0b030503, 0x0b030705, 0x0b030f05, 0x0b050101, 0x0b050303, 0x0b050507, 0x0b050701, 0x0b05070d, + 0x0b050b07, 0x0b070105, 0x0b07010f, 0x0b070301, 0x0b07050f, 0x0b070909, 0x0b070b03, 0x0b070d0b, + 0x0b070f07, 0x0b090103, 0x0b090109, 0x0b090501, 0x0b090705, 0x0b09090d, 0x0b0b0305, 0x0b0b050d, + 0x0b0b0b03, 0x0b0b0b07, 0x0b0d0905, 0x0b0f0105, 0x0b0f0109, 0x0b0f0505, 0x0d010303, 0x0d010307, + 0x0d01030b, 0x0d010703, 0x0d010707, 0x0d010d01, 0x0d030101, 0x0d030501, 0x0d03050f, 0x0d030d09, + 0x0d050305, 0x0d050709, 0x0d050905, 0x0d050b0b, 0x0d050d05, 0x0d050f01, 0x0d070101, 0x0d070309, + 0x0d070503, 0x0d070901, 0x0d09050b, 0x0d090907, 0x0d090d05, 0x0d0b0101, 0x0d0b0107, 0x0d0b0709, + 0x0d0b0d01, 0x0d0d010b, 0x0d0d0901, 0x0d0f0303, 0x0d0f0307, 0x0f010101, 0x0f010109, 0x0f01010f, + 0x0f010501, 0x0f010505, 0x0f01070d, 0x0f010901, 0x0f010b09, 0x0f010d05, 0x0f030105, 0x0f030303, + 0x0f030509, 0x0f030907, 0x0f03090b, 0x0f050103, 0x0f050109, 0x0f050301, 0x0f05030d, 0x0f050503, + 0x0f050701, 0x0f050b03, 0x0f070105, 0x0f070705, 0x0f07070b, 0x0f070b07, 0x0f090103, 0x0f09010b, + 0x0f090307, 0x0f090501, 0x0f090b01, 0x0f0b0505, 0x0f0b0905, 0x0f0d0105, 0x0f0d0703, 0x0f0f0101, +GGML_TABLE_END() + +// TODO: fix name to kvalues_iq4_nl +GGML_TABLE_BEGIN(int8_t, kvalues_iq4nl, 16) + -127, -104, -83, -65, -49, -35, -22, -10, 1, 13, 25, 38, 53, 69, 89, 113, +GGML_TABLE_END() + +// e2m1 values (doubled) +// ref: https://www.opencompute.org/documents/ocp-microscaling-formats-mx-v1-0-spec-final-pdf +GGML_TABLE_BEGIN(int8_t, kvalues_mxfp4, 16) + 0, 1, 2, 3, 4, 6, 8, 12, 0, -1, -2, -3, -4, -6, -8, -12, +GGML_TABLE_END() + +#define NGRID_IQ1S 2048 +#define IQ1S_DELTA 0.125f +#define IQ1M_DELTA 0.125f +#if defined(GGML_COMMON_IMPL_C) +GGML_TABLE_BEGIN(uint64_t, iq1s_grid, NGRID_IQ1S) + 0xffffffffffffffff, 0xffffffffffffff01, 0xffffffffffff0000, 0xffffffffffff01ff, + 0xffffffffffff0101, 0xffffffffff00ff00, 0xffffffffff000000, 0xffffffffff01ffff, + 0xffffffffff01ff01, 0xffffffffff0101ff, 0xffffffffff010101, 0xffffffff00ff0000, + 0xffffffff0000ff00, 0xffffffff000000ff, 0xffffffff00000001, 0xffffffff00010000, + 0xffffffff01ffffff, 0xffffffff01ffff01, 0xffffffff01ff01ff, 0xffffffff01ff0101, + 0xffffffff01000000, 0xffffffff0101ffff, 0xffffffff0101ff01, 0xffffffff010101ff, + 0xffffffff01010101, 0xffffff00ffff00ff, 0xffffff00ffff0000, 0xffffff00ff00ff00, + 0xffffff00ff0000ff, 0xffffff00ff000001, 0xffffff00ff000100, 0xffffff00ff000101, + 0xffffff00ff010000, 0xffffff0000ffff00, 0xffffff0000ff0001, 0xffffff0000ff0100, + 0xffffff000000ff01, 0xffffff0000000000, 0xffffff0000000101, 0xffffff000001ff00, + 0xffffff00000100ff, 0xffffff0000010001, 0xffffff00000101ff, 0xffffff0001ff0000, + 0xffffff000100ff00, 0xffffff00010000ff, 0xffffff0001000001, 0xffffff0001010000, + 0xffffff01ffffffff, 0xffffff01ffffff01, 0xffffff01ffff01ff, 0xffffff01ffff0101, + 0xffffff01ff000000, 0xffffff01ff01ffff, 0xffffff01ff01ff01, 0xffffff01ff0101ff, + 0xffffff01ff010101, 0xffffff0100ff0000, 0xffffff010000ff00, 0xffffff0100000100, + 0xffffff01000100ff, 0xffffff0100010100, 0xffffff0101ffffff, 0xffffff0101ffff01, + 0xffffff0101ff01ff, 0xffffff0101ff0101, 0xffffff010100ff00, 0xffffff0101000000, + 0xffffff0101000100, 0xffffff010101ffff, 0xffffff010101ff01, 0xffffff01010101ff, + 0xffffff0101010101, 0xffff00ffff00ff00, 0xffff00ffff0000ff, 0xffff00ffff000001, + 0xffff00ffff010000, 0xffff00ff00ffff00, 0xffff00ff00ff0100, 0xffff00ff00000000, + 0xffff00ff00000101, 0xffff00ff000100ff, 0xffff00ff00010000, 0xffff00ff0100ff00, + 0xffff00ff01000100, 0xffff00ff01010000, 0xffff0000ffffff00, 0xffff0000ffff00ff, + 0xffff0000ffff0000, 0xffff0000ffff0001, 0xffff0000ff000000, 0xffff0000ff0001ff, + 0xffff0000ff000101, 0xffff0000ff010100, 0xffff000000ffffff, 0xffff000000ff0000, + 0xffff000000ff0101, 0xffff00000000ffff, 0xffff00000000ff00, 0xffff0000000000ff, + 0xffff000000000000, 0xffff000000000001, 0xffff000000000100, 0xffff00000001ffff, + 0xffff00000001ff01, 0xffff000000010000, 0xffff0000000101ff, 0xffff000000010101, + 0xffff000001ffff00, 0xffff00000100ff00, 0xffff000001000000, 0xffff0000010001ff, + 0xffff000001000101, 0xffff00000101ff00, 0xffff0000010100ff, 0xffff000001010000, + 0xffff000001010001, 0xffff000001010100, 0xffff0001ff0000ff, 0xffff0001ff000100, + 0xffff000100ffff00, 0xffff000100ff00ff, 0xffff00010000ffff, 0xffff00010000ff01, + 0xffff000100000000, 0xffff0001000001ff, 0xffff00010001ffff, 0xffff00010001ff00, + 0xffff000100010001, 0xffff000100010100, 0xffff000101ff0000, 0xffff00010100ff00, + 0xffff0001010000ff, 0xffff000101000100, 0xffff01ffffffffff, 0xffff01ffffffff01, + 0xffff01ffffff01ff, 0xffff01ffffff0101, 0xffff01ffff000000, 0xffff01ffff01ffff, + 0xffff01ffff01ff01, 0xffff01ffff0101ff, 0xffff01ffff010101, 0xffff01ff00ff0000, + 0xffff01ff0000ff00, 0xffff01ff00000001, 0xffff01ff00010000, 0xffff01ff01ffffff, + 0xffff01ff01ffff01, 0xffff01ff01ff01ff, 0xffff01ff01ff0101, 0xffff01ff01000000, + 0xffff01ff0101ffff, 0xffff01ff0101ff01, 0xffff01ff010101ff, 0xffff01ff01010101, + 0xffff0100ffff0000, 0xffff0100ff00ff00, 0xffff0100ff0000ff, 0xffff0100ff000100, + 0xffff0100ff0100ff, 0xffff0100ff010000, 0xffff010000ffff00, 0xffff01000000ffff, + 0xffff01000000ff00, 0xffff010000000000, 0xffff01000001ff00, 0xffff0100000100ff, + 0xffff010000010100, 0xffff01000100ff00, 0xffff0100010000ff, 0xffff010001000001, + 0xffff010001000100, 0xffff010001010000, 0xffff0101ffffffff, 0xffff0101ffffff01, + 0xffff0101ffff01ff, 0xffff0101ffff0101, 0xffff0101ff000000, 0xffff0101ff01ffff, + 0xffff0101ff01ff01, 0xffff0101ff0101ff, 0xffff0101ff010101, 0xffff010100ff0000, + 0xffff01010000ff00, 0xffff010100000100, 0xffff01010001ff00, 0xffff010100010000, + 0xffff010101ffffff, 0xffff010101ffff01, 0xffff010101ff0000, 0xffff010101ff01ff, + 0xffff010101ff0101, 0xffff010101000000, 0xffff01010101ffff, 0xffff01010101ff01, + 0xffff0101010101ff, 0xffff010101010101, 0xff00ffffff00ffff, 0xff00ffffff00ff00, + 0xff00ffffff0000ff, 0xff00ffffff000100, 0xff00ffffff0100ff, 0xff00ffffff010000, + 0xff00ffff00ffff00, 0xff00ffff00ff00ff, 0xff00ffff0000ffff, 0xff00ffff00000000, + 0xff00ffff000001ff, 0xff00ffff0001ff00, 0xff00ffff000100ff, 0xff00ffff00010000, + 0xff00ffff00010100, 0xff00ffff0100ff00, 0xff00ffff010000ff, 0xff00ffff01000001, + 0xff00ffff0101ff00, 0xff00ffff01010000, 0xff00ff00ffffff00, 0xff00ff00ffff00ff, + 0xff00ff00ffff0001, 0xff00ff00ffff0100, 0xff00ff00ff00ffff, 0xff00ff00ff00ff01, + 0xff00ff00ff000000, 0xff00ff00ff0001ff, 0xff00ff00ff01ff00, 0xff00ff00ff0100ff, + 0xff00ff00ff010100, 0xff00ff0000ff0000, 0xff00ff0000ff0101, 0xff00ff000000ffff, + 0xff00ff000000ff00, 0xff00ff000000ff01, 0xff00ff00000000ff, 0xff00ff0000000000, + 0xff00ff0000000001, 0xff00ff0000000100, 0xff00ff000001ffff, 0xff00ff0000010000, + 0xff00ff0001ff00ff, 0xff00ff000100ff01, 0xff00ff0001000000, 0xff00ff000101ff00, + 0xff00ff00010100ff, 0xff00ff01ff00ff00, 0xff00ff01ff0000ff, 0xff00ff01ff000001, + 0xff00ff01ff010000, 0xff00ff0100ffffff, 0xff00ff0100ff0001, 0xff00ff0100ff0100, + 0xff00ff010000ff01, 0xff00ff0100000000, 0xff00ff01000001ff, 0xff00ff0100000101, + 0xff00ff01000100ff, 0xff00ff0100010001, 0xff00ff0101ff0000, 0xff00ff010100ff00, + 0xff00ff01010000ff, 0xff00ff0101000001, 0xff00ff0101010000, 0xff0000ffffffff00, + 0xff0000ffffff0001, 0xff0000ffffff0100, 0xff0000ffff0000ff, 0xff0000ffff000000, + 0xff0000ffff0001ff, 0xff0000ffff000100, 0xff0000ffff01ff00, 0xff0000ffff010001, + 0xff0000ff00ffff00, 0xff0000ff00ff0000, 0xff0000ff00ff0001, 0xff0000ff00ff01ff, + 0xff0000ff00ff0101, 0xff0000ff0000ff00, 0xff0000ff000000ff, 0xff0000ff00000000, + 0xff0000ff00000001, 0xff0000ff00000100, 0xff0000ff0001ff01, 0xff0000ff00010000, + 0xff0000ff000101ff, 0xff0000ff01ff00ff, 0xff0000ff01ff0100, 0xff0000ff0100ffff, + 0xff0000ff010000ff, 0xff0000ff01000000, 0xff0000ff010001ff, 0xff0000ff01000100, + 0xff0000ff01000101, 0xff0000ff0101ff00, 0xff0000ff010100ff, 0xff0000ff01010000, + 0xff0000ff01010100, 0xff000000ffffff01, 0xff000000ffff0000, 0xff000000ffff0101, + 0xff000000ff00ff00, 0xff000000ff0000ff, 0xff000000ff000000, 0xff000000ff000001, + 0xff000000ff000100, 0xff000000ff01ffff, 0xff000000ff01ff01, 0xff000000ff010000, + 0xff000000ff0101ff, 0xff000000ff010101, 0xff00000000ffff00, 0xff00000000ff00ff, + 0xff00000000ff0000, 0xff00000000ff0001, 0xff0000000000ff00, 0xff0000000000ff01, + 0xff000000000000ff, 0xff00000000000000, 0xff00000000000001, 0xff00000000000100, + 0xff00000000000101, 0xff0000000001ff00, 0xff000000000100ff, 0xff00000000010000, + 0xff00000000010001, 0xff00000000010100, 0xff00000001ffffff, 0xff00000001ffff01, + 0xff00000001ff00ff, 0xff00000001ff0000, 0xff00000001ff01ff, 0xff00000001ff0101, + 0xff0000000100ffff, 0xff0000000100ff00, 0xff000000010000ff, 0xff00000001000000, + 0xff00000001000001, 0xff00000001000100, 0xff00000001000101, 0xff0000000101ffff, + 0xff0000000101ff01, 0xff00000001010000, 0xff000001ffffff00, 0xff000001ffff00ff, + 0xff000001ffff0000, 0xff000001ffff0001, 0xff000001ff000000, 0xff000001ff000001, + 0xff000001ff0001ff, 0xff000001ff000101, 0xff000001ff01ff00, 0xff000001ff010001, + 0xff00000100ffffff, 0xff00000100ffff01, 0xff00000100ff00ff, 0xff00000100ff0000, + 0xff00000100ff01ff, 0xff00000100ff0101, 0xff0000010000ff00, 0xff00000100000000, + 0xff00000100000001, 0xff000001000001ff, 0xff00000100000100, 0xff0000010001ff00, + 0xff000001000100ff, 0xff00000100010000, 0xff000001000101ff, 0xff00000100010100, + 0xff00000100010101, 0xff00000101ff0001, 0xff00000101ff0101, 0xff0000010100ff01, + 0xff00000101000000, 0xff000001010100ff, 0xff00000101010100, 0xff0001ffff00ff00, + 0xff0001ffff000001, 0xff0001ffff010000, 0xff0001ff00ffff00, 0xff0001ff00ff00ff, + 0xff0001ff00ff0001, 0xff0001ff00ff0100, 0xff0001ff0000ffff, 0xff0001ff00000000, + 0xff0001ff000001ff, 0xff0001ff00000101, 0xff0001ff0001ffff, 0xff0001ff0001ff00, + 0xff0001ff000100ff, 0xff0001ff00010001, 0xff0001ff00010100, 0xff0001ff01ff0000, + 0xff0001ff0100ff00, 0xff0001ff010000ff, 0xff0001ff01010000, 0xff000100ff00ffff, + 0xff000100ff00ff01, 0xff000100ff000000, 0xff000100ff000101, 0xff000100ff01ff00, + 0xff000100ff010000, 0xff00010000ffff01, 0xff00010000ff00ff, 0xff00010000ff0000, + 0xff00010000ff01ff, 0xff0001000000ff00, 0xff000100000000ff, 0xff00010000000000, + 0xff00010000000001, 0xff00010000000100, 0xff00010000000101, 0xff0001000001ffff, + 0xff00010000010000, 0xff00010000010101, 0xff00010001ff0100, 0xff0001000100ff00, + 0xff0001000100ff01, 0xff00010001000000, 0xff000100010001ff, 0xff0001000101ff00, + 0xff00010001010001, 0xff00010001010100, 0xff000101ffff0100, 0xff000101ff000001, + 0xff000101ff0100ff, 0xff000101ff010001, 0xff00010100ff00ff, 0xff00010100ff0001, + 0xff00010100ff0100, 0xff0001010000ffff, 0xff0001010000ff01, 0xff00010100000000, + 0xff000101000001ff, 0xff0001010001ff00, 0xff00010100010001, 0xff00010100010100, + 0xff00010101ff0000, 0xff0001010100ff00, 0xff00010101000001, 0xff00010101000101, + 0xff01ffffffffffff, 0xff01ffffffffff01, 0xff01ffffffff01ff, 0xff01ffffffff0101, + 0xff01ffffff000000, 0xff01ffffff01ffff, 0xff01ffffff01ff01, 0xff01ffffff010000, + 0xff01ffffff0101ff, 0xff01ffffff010101, 0xff01ffff00ff0000, 0xff01ffff0000ff00, + 0xff01ffff00000100, 0xff01ffff0001ff00, 0xff01ffff00010000, 0xff01ffff01ffffff, + 0xff01ffff01ffff01, 0xff01ffff01ff01ff, 0xff01ffff01ff0101, 0xff01ffff01000000, + 0xff01ffff0101ffff, 0xff01ffff0101ff01, 0xff01ffff01010000, 0xff01ffff010101ff, + 0xff01ffff01010101, 0xff01ff00ffff0000, 0xff01ff00ff00ff00, 0xff01ff00ff0000ff, + 0xff01ff00ff000100, 0xff01ff00ff010000, 0xff01ff0000ffff01, 0xff01ff0000ff00ff, + 0xff01ff0000ff0100, 0xff01ff0000000000, 0xff01ff00000001ff, 0xff01ff0000000101, + 0xff01ff000001ff00, 0xff01ff00000100ff, 0xff01ff0000010000, 0xff01ff0000010001, + 0xff01ff0001ff0000, 0xff01ff000100ffff, 0xff01ff0001000001, 0xff01ff0001000100, + 0xff01ff0001010000, 0xff01ff01ffffff00, 0xff01ff01ffff01ff, 0xff01ff01ffff0101, + 0xff01ff01ff00ff00, 0xff01ff01ff000000, 0xff01ff01ff01ffff, 0xff01ff01ff01ff01, + 0xff01ff01ff0101ff, 0xff01ff01ff010101, 0xff01ff0100ff0000, 0xff01ff010000ff00, + 0xff01ff0100000001, 0xff01ff0100000100, 0xff01ff0100010000, 0xff01ff0101ffff00, + 0xff01ff0101ff01ff, 0xff01ff0101ff0101, 0xff01ff010100ff00, 0xff01ff0101000000, + 0xff01ff010101ffff, 0xff01ff010101ff01, 0xff01ff01010101ff, 0xff01ff0101010101, + 0xff0100ffffff0000, 0xff0100ffff0000ff, 0xff0100ffff000001, 0xff0100ffff000100, + 0xff0100ffff010000, 0xff0100ff00ff00ff, 0xff0100ff00ff0000, 0xff0100ff00ff0001, + 0xff0100ff00ff0100, 0xff0100ff0000ff01, 0xff0100ff00000000, 0xff0100ff000001ff, + 0xff0100ff00000101, 0xff0100ff00010001, 0xff0100ff01ff0000, 0xff0100ff0100ff00, + 0xff0100ff010000ff, 0xff0100ff01000100, 0xff0100ff0101ff00, 0xff0100ff01010000, + 0xff010000ffff0100, 0xff010000ff000000, 0xff010000ff01ff00, 0xff010000ff010100, + 0xff01000000ffffff, 0xff01000000ff0000, 0xff01000000ff01ff, 0xff0100000000ff00, + 0xff010000000000ff, 0xff01000000000000, 0xff01000000000100, 0xff0100000001ff01, + 0xff01000000010000, 0xff010000000101ff, 0xff01000001ff0100, 0xff0100000100ffff, + 0xff010000010000ff, 0xff01000001000000, 0xff010000010001ff, 0xff01000001000101, + 0xff0100000101ff00, 0xff010000010100ff, 0xff01000001010001, 0xff01000001010100, + 0xff010001ffff0000, 0xff010001ff00ffff, 0xff010001ff00ff01, 0xff010001ff000100, + 0xff010001ff010000, 0xff01000100ffff00, 0xff01000100ff0100, 0xff01000100000000, + 0xff0100010001ffff, 0xff0100010001ff00, 0xff01000100010100, 0xff01000101ff00ff, + 0xff01000101ff0001, 0xff0100010100ffff, 0xff01000101000101, 0xff0101ffffffffff, + 0xff0101ffffffff01, 0xff0101ffffff01ff, 0xff0101ffffff0101, 0xff0101ffff000000, + 0xff0101ffff01ffff, 0xff0101ffff01ff01, 0xff0101ffff0101ff, 0xff0101ffff010101, + 0xff0101ff00ff0000, 0xff0101ff0000ff00, 0xff0101ff000000ff, 0xff0101ff00010000, + 0xff0101ff01ffffff, 0xff0101ff01ffff01, 0xff0101ff01ff01ff, 0xff0101ff01ff0101, + 0xff0101ff0101ffff, 0xff0101ff0101ff01, 0xff0101ff010101ff, 0xff0101ff01010101, + 0xff010100ffff0100, 0xff010100ff00ff00, 0xff010100ff0000ff, 0xff010100ff000100, + 0xff010100ff010000, 0xff01010000ff0001, 0xff01010000ff0100, 0xff0101000000ff01, + 0xff01010000000000, 0xff0101000001ff00, 0xff010100000100ff, 0xff01010000010001, + 0xff01010000010100, 0xff01010001ff0000, 0xff0101000100ffff, 0xff01010001000001, + 0xff01010001000100, 0xff010100010100ff, 0xff01010001010000, 0xff010101ffffffff, + 0xff010101ffffff01, 0xff010101ffff01ff, 0xff010101ffff0101, 0xff010101ff01ffff, + 0xff010101ff01ff01, 0xff010101ff0101ff, 0xff010101ff010101, 0xff01010100ff0000, + 0xff0101010000ff00, 0xff01010100000001, 0xff01010100000100, 0xff01010100010000, + 0xff01010101ffffff, 0xff01010101ffff01, 0xff01010101ff01ff, 0xff01010101ff0101, + 0xff01010101000000, 0xff0101010101ffff, 0xff0101010101ff01, 0xff010101010101ff, + 0xff01010101010101, 0x00ffffffffff0000, 0x00ffffffff00ff00, 0x00ffffffff000001, + 0x00ffffffff010000, 0x00ffffff00ff0100, 0x00ffffff0000ff01, 0x00ffffff00000000, + 0x00ffffff000001ff, 0x00ffffff00000101, 0x00ffffff0001ff00, 0x00ffffff000100ff, + 0x00ffffff00010001, 0x00ffffff010000ff, 0x00ffffff01000100, 0x00ffffff0101ff00, + 0x00ffffff01010001, 0x00ffff00ffffffff, 0x00ffff00ffffff00, 0x00ffff00ffff00ff, + 0x00ffff00ffff0001, 0x00ffff00ffff0100, 0x00ffff00ff00ff01, 0x00ffff00ff000000, + 0x00ffff00ff000001, 0x00ffff00ff0001ff, 0x00ffff00ff000101, 0x00ffff00ff01ff00, + 0x00ffff00ff010001, 0x00ffff00ff010100, 0x00ffff0000ff0000, 0x00ffff0000ff01ff, + 0x00ffff0000ff0101, 0x00ffff000000ff00, 0x00ffff00000000ff, 0x00ffff0000000000, + 0x00ffff0000000001, 0x00ffff0000000100, 0x00ffff0000000101, 0x00ffff0000010000, + 0x00ffff00000101ff, 0x00ffff0000010101, 0x00ffff0001ffff00, 0x00ffff0001ff00ff, + 0x00ffff0001ff0001, 0x00ffff000100ffff, 0x00ffff000100ff01, 0x00ffff0001000000, + 0x00ffff000101ffff, 0x00ffff000101ff00, 0x00ffff000101ff01, 0x00ffff01ffff0000, + 0x00ffff01ff00ff00, 0x00ffff01ff0000ff, 0x00ffff01ff000001, 0x00ffff01ff010000, + 0x00ffff0100ffff00, 0x00ffff010000ff01, 0x00ffff0100000000, 0x00ffff0100000101, + 0x00ffff01000100ff, 0x00ffff0100010100, 0x00ffff0101ff0100, 0x00ffff01010000ff, + 0x00ffff0101010000, 0x00ff00ffffffff00, 0x00ff00ffff000000, 0x00ff00ffff000100, + 0x00ff00ffff010100, 0x00ff00ff00ff0000, 0x00ff00ff00ff01ff, 0x00ff00ff00ff0101, + 0x00ff00ff0000ff00, 0x00ff00ff000000ff, 0x00ff00ff00000000, 0x00ff00ff00000001, + 0x00ff00ff0001ff00, 0x00ff00ff0001ff01, 0x00ff00ff00010000, 0x00ff00ff000101ff, + 0x00ff00ff00010101, 0x00ff00ff01ffff00, 0x00ff00ff01ff0001, 0x00ff00ff01ff0100, + 0x00ff00ff0100ffff, 0x00ff00ff0100ff01, 0x00ff00ff01000000, 0x00ff00ff0101ffff, + 0x00ff00ff0101ff00, 0x00ff00ff01010100, 0x00ff0000ffffff00, 0x00ff0000ffffff01, + 0x00ff0000ffff0000, 0x00ff0000ffff0101, 0x00ff0000ff00ff00, 0x00ff0000ff0000ff, + 0x00ff0000ff000000, 0x00ff0000ff000001, 0x00ff0000ff000100, 0x00ff0000ff01ffff, + 0x00ff0000ff010000, 0x00ff0000ff010101, 0x00ff000000ffff00, 0x00ff000000ff00ff, + 0x00ff000000ff0000, 0x00ff000000ff0001, 0x00ff000000ff0100, 0x00ff00000000ffff, + 0x00ff00000000ff00, 0x00ff0000000000ff, 0x00ff000000000000, 0x00ff000000000001, + 0x00ff0000000001ff, 0x00ff000000000100, 0x00ff00000001ff00, 0x00ff0000000100ff, + 0x00ff000000010000, 0x00ff000000010001, 0x00ff000000010100, 0x00ff000001ffff01, + 0x00ff000001ff00ff, 0x00ff000001ff0000, 0x00ff000001ff01ff, 0x00ff00000100ff00, + 0x00ff0000010000ff, 0x00ff000001000000, 0x00ff000001000001, 0x00ff000001000100, + 0x00ff000001000101, 0x00ff000001010000, 0x00ff0000010101ff, 0x00ff000001010101, + 0x00ff0001ffffff00, 0x00ff0001ffff0000, 0x00ff0001ffff0100, 0x00ff0001ff0000ff, + 0x00ff0001ff000000, 0x00ff0001ff0001ff, 0x00ff0001ff000101, 0x00ff0001ff01ff00, + 0x00ff0001ff0100ff, 0x00ff0001ff010100, 0x00ff000100ffffff, 0x00ff000100ffff01, + 0x00ff000100ff0000, 0x00ff000100ff01ff, 0x00ff00010000ffff, 0x00ff00010000ff00, + 0x00ff00010000ff01, 0x00ff000100000000, 0x00ff000100000001, 0x00ff000100000100, + 0x00ff00010001ff01, 0x00ff000100010000, 0x00ff0001000101ff, 0x00ff000101ffff00, + 0x00ff000101ff0000, 0x00ff000101ff0101, 0x00ff0001010000ff, 0x00ff000101000000, + 0x00ff00010101ff00, 0x00ff0001010100ff, 0x00ff000101010001, 0x00ff01ffffff0000, + 0x00ff01ffff00ff00, 0x00ff01ffff000000, 0x00ff01ffff000101, 0x00ff01ffff010000, + 0x00ff01ff00ffff01, 0x00ff01ff00ff0100, 0x00ff01ff0000ffff, 0x00ff01ff00000000, + 0x00ff01ff000001ff, 0x00ff01ff0001ff00, 0x00ff01ff000100ff, 0x00ff01ff00010001, + 0x00ff01ff00010100, 0x00ff01ff01ff0000, 0x00ff01ff0100ff00, 0x00ff01ff010000ff, + 0x00ff01ff01000001, 0x00ff01ff01000100, 0x00ff01ff01010000, 0x00ff0100ffffff00, + 0x00ff0100ffff0000, 0x00ff0100ffff0001, 0x00ff0100ffff0101, 0x00ff0100ff00ffff, + 0x00ff0100ff0000ff, 0x00ff0100ff000000, 0x00ff0100ff0001ff, 0x00ff0100ff01ff00, + 0x00ff0100ff0100ff, 0x00ff0100ff010001, 0x00ff010000ffffff, 0x00ff010000ff0000, + 0x00ff010000ff0101, 0x00ff01000000ff00, 0x00ff01000000ff01, 0x00ff0100000000ff, + 0x00ff010000000000, 0x00ff010000000001, 0x00ff010000000100, 0x00ff01000001ffff, + 0x00ff01000001ff01, 0x00ff010000010000, 0x00ff010000010001, 0x00ff010000010101, + 0x00ff010001ff0001, 0x00ff010001ff0100, 0x00ff01000100ff01, 0x00ff010001000000, + 0x00ff010001000001, 0x00ff0100010001ff, 0x00ff01000101ff00, 0x00ff0100010100ff, + 0x00ff010001010001, 0x00ff010001010100, 0x00ff0101ff000001, 0x00ff010100ff00ff, + 0x00ff010100ff0001, 0x00ff010100ff0100, 0x00ff010100000000, 0x00ff0101000001ff, + 0x00ff010100000101, 0x00ff0101000100ff, 0x00ff010100010100, 0x00ff0101010000ff, + 0x00ff010101010000, 0x0000ffffffffff00, 0x0000ffffffff00ff, 0x0000ffffffff0000, + 0x0000ffffffff0001, 0x0000ffffffff0100, 0x0000ffffff00ff01, 0x0000ffffff000000, + 0x0000ffffff000101, 0x0000ffffff01ff00, 0x0000ffffff0100ff, 0x0000ffffff010100, + 0x0000ffff00ffffff, 0x0000ffff00ff0000, 0x0000ffff00ff01ff, 0x0000ffff0000ff00, + 0x0000ffff000000ff, 0x0000ffff00000000, 0x0000ffff00000001, 0x0000ffff00000100, + 0x0000ffff00010000, 0x0000ffff000101ff, 0x0000ffff01ff0001, 0x0000ffff01ff0100, + 0x0000ffff01000000, 0x0000ffff010001ff, 0x0000ffff0101ffff, 0x0000ffff0101ff00, + 0x0000ffff01010001, 0x0000ffff01010100, 0x0000ff00ffff0000, 0x0000ff00ffff01ff, + 0x0000ff00ffff0100, 0x0000ff00ffff0101, 0x0000ff00ff00ff00, 0x0000ff00ff0000ff, + 0x0000ff00ff000000, 0x0000ff00ff000001, 0x0000ff00ff0001ff, 0x0000ff00ff000100, + 0x0000ff00ff01ffff, 0x0000ff00ff010000, 0x0000ff00ff010001, 0x0000ff00ff0101ff, + 0x0000ff00ff010101, 0x0000ff0000ffff00, 0x0000ff0000ff00ff, 0x0000ff0000ff0000, + 0x0000ff0000ff0001, 0x0000ff0000ff0100, 0x0000ff000000ffff, 0x0000ff000000ff00, + 0x0000ff000000ff01, 0x0000ff00000000ff, 0x0000ff0000000000, 0x0000ff0000000001, + 0x0000ff00000001ff, 0x0000ff0000000100, 0x0000ff0000000101, 0x0000ff000001ff00, + 0x0000ff00000100ff, 0x0000ff0000010000, 0x0000ff0000010001, 0x0000ff0000010100, + 0x0000ff0001ffff01, 0x0000ff0001ff0000, 0x0000ff000100ff00, 0x0000ff00010000ff, + 0x0000ff0001000000, 0x0000ff0001000001, 0x0000ff0001000100, 0x0000ff000101ffff, + 0x0000ff0001010000, 0x0000ff0001010101, 0x0000ff01ffffff00, 0x0000ff01ffff0001, + 0x0000ff01ff00ff01, 0x0000ff01ff000000, 0x0000ff01ff000101, 0x0000ff01ff01ff00, + 0x0000ff01ff0100ff, 0x0000ff0100ffff01, 0x0000ff0100ff0000, 0x0000ff0100ff0101, + 0x0000ff010000ff00, 0x0000ff01000000ff, 0x0000ff0100000000, 0x0000ff0100000001, + 0x0000ff0100000100, 0x0000ff010001ff01, 0x0000ff0100010000, 0x0000ff0101ff0000, + 0x0000ff010100ffff, 0x0000ff010100ff01, 0x0000ff0101000000, 0x0000ff0101000100, + 0x0000ff0101000101, 0x0000ff01010100ff, 0x000000ffffff00ff, 0x000000ffffff0000, + 0x000000ffff00ff00, 0x000000ffff0000ff, 0x000000ffff000000, 0x000000ffff000001, + 0x000000ffff0001ff, 0x000000ffff000100, 0x000000ffff01ff00, 0x000000ffff010000, + 0x000000ffff0101ff, 0x000000ffff010101, 0x000000ff00ffff00, 0x000000ff00ff00ff, + 0x000000ff00ff0000, 0x000000ff00ff0001, 0x000000ff00ff0100, 0x000000ff00ff0101, + 0x000000ff0000ffff, 0x000000ff0000ff00, 0x000000ff000000ff, 0x000000ff00000000, + 0x000000ff00000001, 0x000000ff000001ff, 0x000000ff00000100, 0x000000ff00000101, + 0x000000ff0001ff00, 0x000000ff0001ff01, 0x000000ff000100ff, 0x000000ff00010000, + 0x000000ff00010001, 0x000000ff00010100, 0x000000ff01ffffff, 0x000000ff01ff01ff, + 0x000000ff01ff0101, 0x000000ff0100ff00, 0x000000ff010000ff, 0x000000ff01000000, + 0x000000ff01000001, 0x000000ff01000100, 0x000000ff0101ff00, 0x000000ff010100ff, + 0x000000ff01010000, 0x000000ff01010101, 0x00000000ffffff00, 0x00000000ffffff01, + 0x00000000ffff00ff, 0x00000000ffff0000, 0x00000000ffff0001, 0x00000000ffff0100, + 0x00000000ff00ffff, 0x00000000ff00ff00, 0x00000000ff00ff01, 0x00000000ff0000ff, + 0x00000000ff000000, 0x00000000ff000001, 0x00000000ff000100, 0x00000000ff000101, + 0x00000000ff01ff00, 0x00000000ff0100ff, 0x00000000ff010000, 0x00000000ff010001, + 0x00000000ff010100, 0x0000000000ffffff, 0x0000000000ffff00, 0x0000000000ffff01, + 0x0000000000ff00ff, 0x0000000000ff0000, 0x0000000000ff0001, 0x0000000000ff01ff, + 0x0000000000ff0100, 0x000000000000ffff, 0x000000000000ff00, 0x000000000000ff01, + 0x00000000000000ff, 0x0000000000000000, 0x0000000000000001, 0x00000000000001ff, + 0x0000000000000100, 0x0000000000000101, 0x000000000001ffff, 0x000000000001ff00, + 0x00000000000100ff, 0x0000000000010000, 0x0000000000010001, 0x00000000000101ff, + 0x0000000000010100, 0x0000000000010101, 0x0000000001ffff00, 0x0000000001ff00ff, + 0x0000000001ff0000, 0x0000000001ff0100, 0x0000000001ff0101, 0x000000000100ffff, + 0x000000000100ff00, 0x00000000010000ff, 0x0000000001000000, 0x0000000001000001, + 0x00000000010001ff, 0x0000000001000100, 0x000000000101ff00, 0x00000000010100ff, + 0x0000000001010000, 0x0000000001010001, 0x0000000001010100, 0x00000001ffffffff, + 0x00000001ffffff00, 0x00000001ffffff01, 0x00000001ffff00ff, 0x00000001ffff0001, + 0x00000001ffff01ff, 0x00000001ffff0100, 0x00000001ff00ff00, 0x00000001ff0000ff, + 0x00000001ff000000, 0x00000001ff0001ff, 0x00000001ff000100, 0x00000001ff01ffff, + 0x00000001ff01ff00, 0x00000001ff01ff01, 0x00000001ff0100ff, 0x00000001ff010000, + 0x00000001ff010001, 0x00000001ff0101ff, 0x00000001ff010100, 0x0000000100ffff00, + 0x0000000100ff0000, 0x0000000100ff0001, 0x0000000100ff01ff, 0x0000000100ff0100, + 0x0000000100ff0101, 0x000000010000ffff, 0x000000010000ff00, 0x000000010000ff01, + 0x00000001000000ff, 0x0000000100000000, 0x0000000100000001, 0x00000001000001ff, + 0x0000000100000100, 0x0000000100000101, 0x000000010001ff00, 0x00000001000100ff, + 0x0000000100010000, 0x0000000100010100, 0x0000000101ffff01, 0x0000000101ff0000, + 0x0000000101ff0001, 0x0000000101ff01ff, 0x0000000101ff0100, 0x0000000101ff0101, + 0x000000010100ff00, 0x0000000101000000, 0x0000000101000101, 0x000000010101ff01, + 0x0000000101010000, 0x0000000101010001, 0x00000001010101ff, 0x0000000101010100, + 0x000001ffffff00ff, 0x000001ffffff0000, 0x000001ffffff0001, 0x000001ffffff0100, + 0x000001ffff00ffff, 0x000001ffff000000, 0x000001ffff0001ff, 0x000001ffff01ff00, + 0x000001ffff010101, 0x000001ff00ff0000, 0x000001ff00ff01ff, 0x000001ff00ff0101, + 0x000001ff0000ff00, 0x000001ff000000ff, 0x000001ff00000000, 0x000001ff00000001, + 0x000001ff000001ff, 0x000001ff00000100, 0x000001ff0001ffff, 0x000001ff0001ff01, + 0x000001ff000100ff, 0x000001ff00010000, 0x000001ff01ffff01, 0x000001ff01ff0100, + 0x000001ff0100ffff, 0x000001ff0100ff01, 0x000001ff01000000, 0x000001ff010001ff, + 0x000001ff0101ff00, 0x000001ff01010100, 0x00000100ffffff00, 0x00000100ffffff01, + 0x00000100ffff0000, 0x00000100ffff0101, 0x00000100ff00ff00, 0x00000100ff0000ff, + 0x00000100ff000000, 0x00000100ff000001, 0x00000100ff000100, 0x00000100ff010000, + 0x0000010000ffff00, 0x0000010000ff00ff, 0x0000010000ff0000, 0x0000010000ff0001, + 0x0000010000ff0100, 0x000001000000ffff, 0x000001000000ff00, 0x000001000000ff01, + 0x00000100000000ff, 0x0000010000000000, 0x0000010000000001, 0x00000100000001ff, + 0x0000010000000100, 0x0000010000000101, 0x000001000001ff00, 0x00000100000100ff, + 0x0000010000010000, 0x0000010000010001, 0x0000010000010100, 0x0000010001ffff00, + 0x0000010001ff0000, 0x0000010001ff0100, 0x000001000100ff00, 0x00000100010000ff, + 0x0000010001000000, 0x0000010001000001, 0x00000100010001ff, 0x0000010001000100, + 0x0000010001010000, 0x00000101ffff00ff, 0x00000101ffff01ff, 0x00000101ff000000, + 0x00000101ff000101, 0x00000101ff01ffff, 0x00000101ff010000, 0x00000101ff010001, + 0x00000101ff010100, 0x0000010100ff0000, 0x0000010100ff01ff, 0x0000010100ff0100, + 0x000001010000ff00, 0x0000010100000000, 0x0000010100000001, 0x00000101000001ff, + 0x0000010100000100, 0x000001010001ff01, 0x0000010100010000, 0x00000101000101ff, + 0x0000010100010101, 0x0000010101ffff00, 0x0000010101ff0101, 0x000001010100ff01, + 0x0000010101000000, 0x0000010101000001, 0x00000101010001ff, 0x0000010101000101, + 0x000001010101ff00, 0x0001ffffffff0000, 0x0001ffffff0000ff, 0x0001ffffff000001, + 0x0001ffffff000100, 0x0001ffffff010000, 0x0001ffff00ff00ff, 0x0001ffff0000ffff, + 0x0001ffff00000000, 0x0001ffff00000001, 0x0001ffff000001ff, 0x0001ffff00000101, + 0x0001ffff0001ff00, 0x0001ffff000100ff, 0x0001ffff00010001, 0x0001ffff00010100, + 0x0001ffff01ffff00, 0x0001ffff01000001, 0x0001ffff01010000, 0x0001ff00ffffff00, + 0x0001ff00ffff00ff, 0x0001ff00ffff0001, 0x0001ff00ffff0100, 0x0001ff00ff00ff01, + 0x0001ff00ff000000, 0x0001ff00ff01ff00, 0x0001ff00ff01ff01, 0x0001ff00ff010001, + 0x0001ff00ff010100, 0x0001ff0000ff0000, 0x0001ff0000ff0100, 0x0001ff000000ff00, + 0x0001ff0000000000, 0x0001ff0000000001, 0x0001ff0000000100, 0x0001ff0000010000, + 0x0001ff0000010001, 0x0001ff0000010101, 0x0001ff0001ff00ff, 0x0001ff0001ff0101, + 0x0001ff000100ff01, 0x0001ff0001000000, 0x0001ff000101ff00, 0x0001ff0001010001, + 0x0001ff0001010100, 0x0001ff01ff00ff00, 0x0001ff01ff000001, 0x0001ff01ff000100, + 0x0001ff0100ffffff, 0x0001ff0100ffff00, 0x0001ff0100ff0001, 0x0001ff0100000000, + 0x0001ff0100000001, 0x0001ff01000001ff, 0x0001ff010001ffff, 0x0001ff0101ff0000, + 0x0001ff010100ff00, 0x0001ff0101000001, 0x0001ff0101010000, 0x000100ffff00ff00, + 0x000100ffff00ff01, 0x000100ffff000000, 0x000100ffff000001, 0x000100ffff000101, + 0x000100ffff01ff00, 0x000100ffff010001, 0x000100ffff010100, 0x000100ff00ffffff, + 0x000100ff00ffff01, 0x000100ff00ff0000, 0x000100ff00ff01ff, 0x000100ff00ff0101, + 0x000100ff0000ff00, 0x000100ff000000ff, 0x000100ff00000000, 0x000100ff00000001, + 0x000100ff00000100, 0x000100ff00000101, 0x000100ff0001ffff, 0x000100ff0001ff01, + 0x000100ff00010000, 0x000100ff01ff00ff, 0x000100ff01ff0000, 0x000100ff01ff0100, + 0x000100ff0100ffff, 0x000100ff0100ff01, 0x000100ff010000ff, 0x000100ff01000000, + 0x000100ff01000001, 0x000100ff010001ff, 0x000100ff01000101, 0x000100ff0101ff00, + 0x000100ff010100ff, 0x000100ff01010100, 0x00010000ffff0000, 0x00010000ffff01ff, + 0x00010000ffff0101, 0x00010000ff00ff00, 0x00010000ff000000, 0x00010000ff000001, + 0x00010000ff000100, 0x0001000000ff00ff, 0x0001000000ff0000, 0x0001000000ff0001, + 0x0001000000ff0100, 0x000100000000ffff, 0x000100000000ff00, 0x00010000000000ff, + 0x0001000000000000, 0x0001000000000001, 0x0001000000000100, 0x000100000001ff00, + 0x00010000000100ff, 0x0001000000010000, 0x0001000000010001, 0x0001000000010100, + 0x0001000001ff0001, 0x0001000001ff0100, 0x0001000001ff0101, 0x000100000100ff00, + 0x0001000001000000, 0x0001000001000001, 0x0001000001000100, 0x0001000001000101, + 0x000100000101ff01, 0x0001000001010000, 0x0001000001010001, 0x00010000010101ff, + 0x00010001ffffff01, 0x00010001ffff0100, 0x00010001ff000000, 0x00010001ff01ffff, + 0x00010001ff010001, 0x00010001ff0101ff, 0x00010001ff010100, 0x0001000100ffffff, + 0x0001000100ff0000, 0x0001000100ff01ff, 0x0001000100ff0101, 0x000100010000ff00, + 0x00010001000000ff, 0x0001000100000000, 0x0001000100000001, 0x00010001000001ff, + 0x0001000100000101, 0x000100010001ffff, 0x0001000100010000, 0x00010001000101ff, + 0x0001000101ffffff, 0x0001000101ffff01, 0x0001000101ff0000, 0x0001000101ff0101, + 0x00010001010000ff, 0x0001000101000001, 0x00010001010001ff, 0x0001000101000100, + 0x000100010101ffff, 0x00010001010100ff, 0x0001000101010001, 0x0001000101010101, + 0x000101ffff000001, 0x000101ffff000100, 0x000101ffff010000, 0x000101ff00ffff00, + 0x000101ff0000ff01, 0x000101ff00000000, 0x000101ff00000101, 0x000101ff0001ff00, + 0x000101ff00010100, 0x000101ff01ff0000, 0x000101ff0100ff00, 0x000101ff010001ff, + 0x000101ff01010001, 0x00010100ffffff00, 0x00010100ffff00ff, 0x00010100ff00ffff, + 0x00010100ff000000, 0x00010100ff01ff00, 0x00010100ff0100ff, 0x00010100ff010001, + 0x00010100ff010100, 0x0001010000ffffff, 0x0001010000ffff00, 0x0001010000ff0000, + 0x0001010000ff0001, 0x0001010000ff01ff, 0x000101000000ff00, 0x00010100000000ff, + 0x0001010000000000, 0x0001010000000001, 0x0001010000000100, 0x000101000001ffff, + 0x0001010000010000, 0x0001010000010101, 0x0001010001ffff01, 0x0001010001ff00ff, + 0x0001010001ff0101, 0x0001010001000000, 0x000101000101ff00, 0x00010100010100ff, + 0x0001010001010000, 0x0001010001010100, 0x00010101ff00ff00, 0x00010101ff000001, + 0x00010101ff0001ff, 0x0001010100ffff00, 0x0001010100ff00ff, 0x0001010100ff0100, + 0x000101010000ffff, 0x0001010100000000, 0x00010101000001ff, 0x0001010100000101, + 0x00010101000100ff, 0x0001010100010000, 0x0001010100010100, 0x0001010101ff0001, + 0x00010101010000ff, 0x00010101010001ff, 0x0001010101000101, 0x0001010101010001, + 0x01ffffffffffffff, 0x01ffffffffffff01, 0x01ffffffffff01ff, 0x01ffffffffff0101, + 0x01ffffffff01ffff, 0x01ffffffff01ff01, 0x01ffffffff0101ff, 0x01ffffffff010101, + 0x01ffffff00ff0000, 0x01ffffff0000ffff, 0x01ffffff0000ff00, 0x01ffffff000000ff, + 0x01ffffff00000001, 0x01ffffff00000100, 0x01ffffff00010000, 0x01ffffff01ffffff, + 0x01ffffff01ffff01, 0x01ffffff01ff01ff, 0x01ffffff01ff0101, 0x01ffffff01000000, + 0x01ffffff0101ffff, 0x01ffffff0101ff01, 0x01ffffff010101ff, 0x01ffffff01010101, + 0x01ffff00ffff0000, 0x01ffff00ff00ff00, 0x01ffff00ff0000ff, 0x01ffff00ff000001, + 0x01ffff00ff000100, 0x01ffff00ff010000, 0x01ffff0000ffff00, 0x01ffff0000ff00ff, + 0x01ffff0000ff0100, 0x01ffff000000ffff, 0x01ffff000000ff01, 0x01ffff0000000000, + 0x01ffff0000000001, 0x01ffff00000001ff, 0x01ffff0000000100, 0x01ffff00000100ff, + 0x01ffff0000010001, 0x01ffff0000010100, 0x01ffff0001ff0000, 0x01ffff0001ff0100, + 0x01ffff00010000ff, 0x01ffff0001000001, 0x01ffff0001000100, 0x01ffff0001010000, + 0x01ffff01ffffffff, 0x01ffff01ffffff01, 0x01ffff01ffff01ff, 0x01ffff01ffff0101, + 0x01ffff01ff000000, 0x01ffff01ff01ffff, 0x01ffff01ff01ff01, 0x01ffff01ff0101ff, + 0x01ffff01ff010101, 0x01ffff010000ff00, 0x01ffff01000000ff, 0x01ffff0100000100, + 0x01ffff0100010000, 0x01ffff0101ffffff, 0x01ffff0101ffff01, 0x01ffff0101ff01ff, + 0x01ffff0101ff0101, 0x01ffff0101000000, 0x01ffff010101ffff, 0x01ffff010101ff01, + 0x01ffff01010101ff, 0x01ffff0101010101, 0x01ff00ffff0000ff, 0x01ff00ffff000100, + 0x01ff00ff00ffff00, 0x01ff00ff00ff00ff, 0x01ff00ff0000ff00, 0x01ff00ff00000000, + 0x01ff00ff00000101, 0x01ff00ff0001ff00, 0x01ff00ff000100ff, 0x01ff00ff00010100, + 0x01ff00ff010000ff, 0x01ff00ff01000100, 0x01ff0000ffffff00, 0x01ff0000ffff0100, + 0x01ff0000ff00ff01, 0x01ff0000ff000000, 0x01ff0000ff000101, 0x01ff0000ff010001, + 0x01ff0000ff010100, 0x01ff000000ffffff, 0x01ff000000ffff00, 0x01ff000000ff0000, + 0x01ff000000ff01ff, 0x01ff00000000ff00, 0x01ff0000000000ff, 0x01ff000000000000, + 0x01ff000000000001, 0x01ff000000000100, 0x01ff000000000101, 0x01ff000000010000, + 0x01ff000000010001, 0x01ff0000000101ff, 0x01ff000000010101, 0x01ff000001ffff00, + 0x01ff000001ff00ff, 0x01ff000001ff0001, 0x01ff000001ff0100, 0x01ff00000100ffff, + 0x01ff00000100ff01, 0x01ff000001000000, 0x01ff0000010001ff, 0x01ff000001010001, + 0x01ff0001ff00ff00, 0x01ff0001ff000001, 0x01ff0001ff000100, 0x01ff0001ff010000, + 0x01ff000100ffff00, 0x01ff000100ff00ff, 0x01ff000100ff0100, 0x01ff000100ff0101, + 0x01ff00010000ffff, 0x01ff000100000000, 0x01ff000100000100, 0x01ff000100000101, + 0x01ff00010001ff00, 0x01ff000100010001, 0x01ff000100010101, 0x01ff000101ff0000, + 0x01ff00010100ff00, 0x01ff000101000101, 0x01ff0001010100ff, 0x01ff01ffffffffff, + 0x01ff01ffffffff01, 0x01ff01ffffff01ff, 0x01ff01ffffff0101, 0x01ff01ffff000000, + 0x01ff01ffff01ffff, 0x01ff01ffff01ff01, 0x01ff01ffff0101ff, 0x01ff01ffff010101, + 0x01ff01ff00ffff00, 0x01ff01ff00ff0000, 0x01ff01ff0000ff00, 0x01ff01ff000000ff, + 0x01ff01ff00000100, 0x01ff01ff00010000, 0x01ff01ff00010100, 0x01ff01ff01ffffff, + 0x01ff01ff01ffff01, 0x01ff01ff01ff01ff, 0x01ff01ff01ff0101, 0x01ff01ff01000000, + 0x01ff01ff0101ffff, 0x01ff01ff0101ff01, 0x01ff01ff010101ff, 0x01ff01ff01010101, + 0x01ff0100ffff0000, 0x01ff0100ffff0001, 0x01ff0100ff00ff00, 0x01ff0100ff0000ff, + 0x01ff0100ff000001, 0x01ff0100ff010000, 0x01ff010000ffff00, 0x01ff010000ff00ff, + 0x01ff010000ff0001, 0x01ff010000ff0100, 0x01ff01000000ffff, 0x01ff01000000ff01, + 0x01ff010000000000, 0x01ff010000000101, 0x01ff01000001ff00, 0x01ff0100000100ff, + 0x01ff010001ff0000, 0x01ff010001000001, 0x01ff010001000100, 0x01ff010001010000, + 0x01ff0101ffffffff, 0x01ff0101ffffff01, 0x01ff0101ffff01ff, 0x01ff0101ffff0101, + 0x01ff0101ff000000, 0x01ff0101ff01ffff, 0x01ff0101ff01ff01, 0x01ff0101ff0101ff, + 0x01ff0101ff010101, 0x01ff010100ff0000, 0x01ff01010000ff00, 0x01ff0101000000ff, + 0x01ff010100000001, 0x01ff010101ffffff, 0x01ff010101ffff01, 0x01ff010101ff01ff, + 0x01ff010101ff0101, 0x01ff010101000000, 0x01ff01010101ffff, 0x01ff01010101ff01, + 0x01ff0101010101ff, 0x01ff010101010101, 0x0100ffffffff0000, 0x0100ffffff00ff00, + 0x0100ffffff000001, 0x0100ffffff0001ff, 0x0100ffffff000100, 0x0100ffffff010000, + 0x0100ffff00ffff00, 0x0100ffff00ff0001, 0x0100ffff00ff0100, 0x0100ffff00000000, + 0x0100ffff000001ff, 0x0100ffff00000101, 0x0100ffff00010100, 0x0100ffff00010101, + 0x0100ffff01ff0000, 0x0100ffff0100ff00, 0x0100ffff010000ff, 0x0100ffff01000001, + 0x0100ffff01000100, 0x0100ffff01010000, 0x0100ff00ffffff00, 0x0100ff00ffff00ff, + 0x0100ff00ffff0001, 0x0100ff00ffff0100, 0x0100ff00ff00ffff, 0x0100ff00ff000000, + 0x0100ff00ff0001ff, 0x0100ff00ff000101, 0x0100ff00ff01ff00, 0x0100ff00ff0100ff, + 0x0100ff00ff010001, 0x0100ff00ff010100, 0x0100ff0000ffffff, 0x0100ff0000ff0000, + 0x0100ff000000ffff, 0x0100ff000000ff00, 0x0100ff00000000ff, 0x0100ff0000000000, + 0x0100ff0000000001, 0x0100ff0000000100, 0x0100ff000001ff01, 0x0100ff0000010000, + 0x0100ff0001ff00ff, 0x0100ff0001ff0001, 0x0100ff000100ff01, 0x0100ff0001000000, + 0x0100ff00010001ff, 0x0100ff000101ff00, 0x0100ff00010100ff, 0x0100ff0001010001, + 0x0100ff0001010100, 0x0100ff01ffff0000, 0x0100ff01ff00ff00, 0x0100ff01ff0000ff, + 0x0100ff01ff000100, 0x0100ff01ff010000, 0x0100ff0100ff00ff, 0x0100ff0100ff0001, + 0x0100ff0100ff0100, 0x0100ff010000ffff, 0x0100ff010000ff01, 0x0100ff0100000000, + 0x0100ff01000001ff, 0x0100ff0100010001, 0x0100ff0100010100, 0x0100ff0101ff0000, + 0x0100ff01010000ff, 0x0100ff0101000001, 0x0100ff0101010100, 0x010000ffffffff00, + 0x010000ffffff00ff, 0x010000ffffff0001, 0x010000ffff00ffff, 0x010000ffff000000, + 0x010000ffff0001ff, 0x010000ffff010001, 0x010000ff00ffffff, 0x010000ff00ff0101, + 0x010000ff0000ff00, 0x010000ff000000ff, 0x010000ff00000000, 0x010000ff00000001, + 0x010000ff000001ff, 0x010000ff00000100, 0x010000ff0001ffff, 0x010000ff0001ff00, + 0x010000ff0001ff01, 0x010000ff00010000, 0x010000ff01ff00ff, 0x010000ff01ff0001, + 0x010000ff0100ff01, 0x010000ff010000ff, 0x010000ff01000000, 0x010000ff010001ff, + 0x010000ff0101ff00, 0x010000ff01010100, 0x01000000ffffffff, 0x01000000ffff0000, + 0x01000000ffff01ff, 0x01000000ffff0101, 0x01000000ff00ffff, 0x01000000ff00ff00, + 0x01000000ff0000ff, 0x01000000ff000000, 0x01000000ff000001, 0x01000000ff000100, + 0x01000000ff01ff00, 0x01000000ff010000, 0x01000000ff010100, 0x01000000ff010101, + 0x0100000000ffff00, 0x0100000000ff00ff, 0x0100000000ff0000, 0x0100000000ff0001, + 0x0100000000ff0100, 0x010000000000ffff, 0x010000000000ff00, 0x010000000000ff01, + 0x01000000000000ff, 0x0100000000000000, 0x0100000000000001, 0x01000000000001ff, + 0x0100000000000100, 0x0100000000000101, 0x010000000001ff00, 0x01000000000100ff, + 0x0100000000010000, 0x0100000000010001, 0x0100000000010100, 0x0100000001ffff00, + 0x0100000001ff0000, 0x0100000001ff01ff, 0x010000000100ff00, 0x010000000100ff01, + 0x01000000010000ff, 0x0100000001000000, 0x0100000001000001, 0x0100000001000100, + 0x0100000001000101, 0x010000000101ffff, 0x010000000101ff01, 0x0100000001010000, + 0x01000000010101ff, 0x0100000001010101, 0x01000001ffffff00, 0x01000001ffff00ff, + 0x01000001ff00ffff, 0x01000001ff000000, 0x01000001ff000100, 0x01000001ff01ffff, + 0x01000001ff010001, 0x01000001ff010100, 0x0100000100ff0000, 0x0100000100ff01ff, + 0x0100000100ff0100, 0x010000010000ff00, 0x010000010000ff01, 0x0100000100000000, + 0x0100000100000001, 0x0100000100000100, 0x0100000100010000, 0x01000001000101ff, + 0x0100000101ffff01, 0x0100000101ff00ff, 0x0100000101ff0100, 0x0100000101ff0101, + 0x010000010100ff01, 0x01000001010000ff, 0x0100000101000000, 0x01000001010100ff, + 0x0100000101010001, 0x0100000101010100, 0x010001ffffff0000, 0x010001ffff000001, + 0x010001ffff000100, 0x010001ffff010000, 0x010001ff00ffff00, 0x010001ff00ff0001, + 0x010001ff0000ffff, 0x010001ff0000ff01, 0x010001ff00000000, 0x010001ff00000001, + 0x010001ff00000101, 0x010001ff000100ff, 0x010001ff00010000, 0x010001ff01ff0000, + 0x010001ff0100ff00, 0x010001ff01000001, 0x010001ff01000100, 0x010001ff01010000, + 0x01000100ffff00ff, 0x01000100ffff0001, 0x01000100ffff0100, 0x01000100ff00ffff, + 0x01000100ff00ff01, 0x01000100ff000000, 0x01000100ff0001ff, 0x01000100ff000101, + 0x01000100ff01ffff, 0x01000100ff01ff00, 0x01000100ff0100ff, 0x01000100ff010001, + 0x0100010000ffffff, 0x0100010000ffff01, 0x0100010000ff0000, 0x0100010000ff01ff, + 0x0100010000ff0101, 0x010001000000ff00, 0x01000100000000ff, 0x0100010000000000, + 0x0100010000000001, 0x0100010000000100, 0x010001000001ff01, 0x0100010000010000, + 0x0100010000010001, 0x0100010000010101, 0x0100010001ffff00, 0x0100010001ff00ff, + 0x010001000100ffff, 0x010001000100ff01, 0x0100010001000000, 0x0100010001000101, + 0x010001000101ff00, 0x0100010001010001, 0x01000101ffff0000, 0x01000101ff000000, + 0x01000101ff010000, 0x0100010100ff00ff, 0x0100010100ff0001, 0x0100010100ff0100, + 0x010001010000ffff, 0x0100010100000000, 0x01000101000001ff, 0x010001010001ff00, + 0x0100010101ff0000, 0x010001010100ff00, 0x01000101010000ff, 0x0100010101000000, + 0x0100010101000001, 0x0101ffffffffffff, 0x0101ffffffffff01, 0x0101ffffffff01ff, + 0x0101ffffffff0101, 0x0101ffffff000000, 0x0101ffffff01ffff, 0x0101ffffff01ff01, + 0x0101ffffff0101ff, 0x0101ffffff010101, 0x0101ffff00ff0000, 0x0101ffff0000ff00, + 0x0101ffff000000ff, 0x0101ffff00000001, 0x0101ffff00000100, 0x0101ffff01ffffff, + 0x0101ffff01ffff01, 0x0101ffff01ff01ff, 0x0101ffff01ff0101, 0x0101ffff01000000, + 0x0101ffff0101ffff, 0x0101ffff0101ff01, 0x0101ffff010101ff, 0x0101ffff01010101, + 0x0101ff00ffff0000, 0x0101ff00ffff0100, 0x0101ff00ff00ff00, 0x0101ff00ff0000ff, + 0x0101ff00ff000001, 0x0101ff00ff000100, 0x0101ff00ff000101, 0x0101ff0000ff0001, + 0x0101ff0000ff0100, 0x0101ff000000ff00, 0x0101ff0000000000, 0x0101ff00000001ff, + 0x0101ff0000000101, 0x0101ff000001ff00, 0x0101ff00000100ff, 0x0101ff0001ff0000, + 0x0101ff000100ffff, 0x0101ff000100ff01, 0x0101ff0001000001, 0x0101ff0001000100, + 0x0101ff01ffffff01, 0x0101ff01ffff01ff, 0x0101ff01ffff0101, 0x0101ff01ff00ffff, + 0x0101ff01ff000100, 0x0101ff01ff01ff01, 0x0101ff01ff0101ff, 0x0101ff01ff010101, + 0x0101ff0100ff0000, 0x0101ff010000ff00, 0x0101ff0100000001, 0x0101ff0100000100, + 0x0101ff0100010000, 0x0101ff0101ffffff, 0x0101ff0101ffff01, 0x0101ff0101ff01ff, + 0x0101ff0101ff0101, 0x0101ff0101000000, 0x0101ff010101ffff, 0x0101ff010101ff01, + 0x0101ff01010101ff, 0x0101ff0101010101, 0x010100ffff000100, 0x010100ffff010000, + 0x010100ff00ffff00, 0x010100ff00ff00ff, 0x010100ff0000ffff, 0x010100ff000000ff, + 0x010100ff00000000, 0x010100ff000001ff, 0x010100ff00000101, 0x010100ff0001ff00, + 0x010100ff00010000, 0x010100ff00010001, 0x010100ff000101ff, 0x010100ff00010100, + 0x010100ff01ff0000, 0x01010000ffff0001, 0x01010000ffff0100, 0x01010000ff00ffff, + 0x01010000ff00ff01, 0x01010000ff000000, 0x01010000ff0001ff, 0x01010000ff010001, + 0x01010000ff010100, 0x0101000000ffff01, 0x0101000000ff0000, 0x010100000000ff00, + 0x01010000000000ff, 0x0101000000000000, 0x0101000000000001, 0x0101000000000100, + 0x0101000000010000, 0x0101000000010101, 0x0101000001ffff00, 0x0101000001ff00ff, + 0x0101000001ff0000, 0x0101000001ff0001, 0x0101000001ff0100, 0x010100000100ff01, + 0x0101000001000000, 0x01010000010001ff, 0x01010001ffff0000, 0x01010001ff00ff00, + 0x01010001ff000001, 0x01010001ff000101, 0x01010001ff01ff00, 0x01010001ff010000, + 0x0101000100ff00ff, 0x0101000100ff0001, 0x0101000100ff0101, 0x010100010000ff01, + 0x0101000100000000, 0x0101000100000001, 0x01010001000001ff, 0x010100010001ffff, + 0x010100010001ff01, 0x0101000101ff0001, 0x010100010100ffff, 0x0101000101000000, + 0x0101000101000001, 0x0101000101000100, 0x010100010101ff00, 0x01010001010100ff, + 0x0101000101010001, 0x010101ffffffffff, 0x010101ffffffff01, 0x010101ffffff01ff, + 0x010101ffffff0101, 0x010101ffff01ffff, 0x010101ffff01ff01, 0x010101ffff0101ff, + 0x010101ffff010101, 0x010101ff0000ff00, 0x010101ff000000ff, 0x010101ff00000001, + 0x010101ff00000100, 0x010101ff01ffffff, 0x010101ff01ffff01, 0x010101ff01ff01ff, + 0x010101ff01ff0101, 0x010101ff01000000, 0x010101ff0101ffff, 0x010101ff0101ff01, + 0x010101ff010101ff, 0x010101ff01010101, 0x01010100ffff0000, 0x01010100ff0000ff, + 0x01010100ff000100, 0x01010100ff01ff00, 0x01010100ff010000, 0x0101010000ffff00, + 0x010101000000ffff, 0x0101010000000000, 0x0101010000000101, 0x010101000001ff00, + 0x0101010000010001, 0x0101010000010100, 0x010101000100ffff, 0x0101010001000001, + 0x01010101ffffffff, 0x01010101ffffff01, 0x01010101ffff01ff, 0x01010101ffff0101, + 0x01010101ff01ffff, 0x01010101ff01ff01, 0x01010101ff0101ff, 0x01010101ff010101, + 0x010101010000ff00, 0x01010101000000ff, 0x0101010100000001, 0x0101010101ffffff, + 0x0101010101ffff01, 0x0101010101ff01ff, 0x0101010101ff0101, 0x0101010101000000, + 0x010101010101ffff, 0x010101010101ff01, 0x01010101010101ff, 0x0101010101010101, +GGML_TABLE_END() +#else +GGML_TABLE_BEGIN(uint32_t, iq1s_grid_gpu, NGRID_IQ1S) + 0x00000000, 0x00000002, 0x00000101, 0x00000200, 0x00000202, 0x00010001, 0x00010101, 0x00020000, + 0x00020002, 0x00020200, 0x00020202, 0x01000101, 0x01010001, 0x01010100, 0x01010102, 0x01020101, + 0x02000000, 0x02000002, 0x02000200, 0x02000202, 0x02010101, 0x02020000, 0x02020002, 0x02020200, + 0x02020202, 0x00000110, 0x00000111, 0x00010011, 0x00010110, 0x00010112, 0x00010211, 0x00010212, + 0x00020111, 0x01000011, 0x01000112, 0x01000211, 0x01010012, 0x01010111, 0x01010212, 0x01020011, + 0x01020110, 0x01020112, 0x01020210, 0x02000111, 0x02010011, 0x02010110, 0x02010112, 0x02020111, + 0x00000020, 0x00000022, 0x00000220, 0x00000222, 0x00010121, 0x00020020, 0x00020022, 0x00020220, + 0x00020222, 0x01000121, 0x01010021, 0x01010221, 0x01020120, 0x01020221, 0x02000020, 0x02000022, + 0x02000220, 0x02000222, 0x02010021, 0x02010121, 0x02010221, 0x02020020, 0x02020022, 0x02020220, + 0x02020222, 0x00011001, 0x00011100, 0x00011102, 0x00021101, 0x01001001, 0x01001201, 0x01011101, + 0x01011202, 0x01021100, 0x01021101, 0x02011001, 0x02011201, 0x02021101, 0x00001011, 0x00001110, + 0x00001111, 0x00001112, 0x00011111, 0x00011210, 0x00011212, 0x00021211, 0x01001010, 0x01001111, + 0x01001212, 0x01011010, 0x01011011, 0x01011110, 0x01011111, 0x01011112, 0x01011211, 0x01021010, + 0x01021012, 0x01021111, 0x01021210, 0x01021212, 0x02001011, 0x02011011, 0x02011111, 0x02011210, + 0x02011212, 0x02021011, 0x02021110, 0x02021111, 0x02021112, 0x02021211, 0x00011120, 0x00011221, + 0x01001021, 0x01001120, 0x01011020, 0x01011022, 0x01011121, 0x01011220, 0x01021020, 0x01021021, + 0x01021122, 0x01021221, 0x02001121, 0x02011021, 0x02011120, 0x02011221, 0x00002000, 0x00002002, + 0x00002200, 0x00002202, 0x00012101, 0x00022000, 0x00022002, 0x00022200, 0x00022202, 0x01002101, + 0x01012001, 0x01012102, 0x01022101, 0x02002000, 0x02002002, 0x02002200, 0x02002202, 0x02012101, + 0x02022000, 0x02022002, 0x02022200, 0x02022202, 0x00002111, 0x00012011, 0x00012110, 0x00012211, + 0x00022110, 0x00022111, 0x01002011, 0x01012010, 0x01012011, 0x01012111, 0x01022011, 0x01022110, + 0x01022211, 0x02012011, 0x02012110, 0x02012112, 0x02012211, 0x02022111, 0x00002020, 0x00002022, + 0x00002220, 0x00002222, 0x00012121, 0x00022020, 0x00022022, 0x00022220, 0x00022222, 0x01002121, + 0x01012021, 0x01012221, 0x01022021, 0x01022121, 0x02002020, 0x02002022, 0x02002121, 0x02002220, + 0x02002222, 0x02012121, 0x02022020, 0x02022022, 0x02022220, 0x02022222, 0x00110000, 0x00110001, + 0x00110100, 0x00110201, 0x00120100, 0x00120101, 0x01100001, 0x01100100, 0x01110000, 0x01110101, + 0x01110200, 0x01120001, 0x01120100, 0x01120101, 0x01120201, 0x02110001, 0x02110100, 0x02110102, + 0x02120001, 0x02120101, 0x00100011, 0x00100110, 0x00100112, 0x00100211, 0x00110010, 0x00110012, + 0x00110111, 0x00110210, 0x00120011, 0x00120110, 0x00120211, 0x01100111, 0x01100212, 0x01110010, + 0x01110011, 0x01110012, 0x01110110, 0x01110111, 0x01110112, 0x01110211, 0x01120010, 0x01120111, + 0x02100110, 0x02110012, 0x02110111, 0x02120011, 0x02120110, 0x00110021, 0x00110120, 0x00110122, + 0x00120121, 0x01100020, 0x01100122, 0x01100221, 0x01110022, 0x01110121, 0x01110220, 0x01110222, + 0x01120120, 0x01120122, 0x02100121, 0x02110021, 0x02110120, 0x02110122, 0x02120121, 0x00101001, + 0x00101102, 0x00101201, 0x00111100, 0x00111101, 0x00111200, 0x00111201, 0x00121001, 0x00121102, + 0x01101001, 0x01101101, 0x01101102, 0x01101200, 0x01101202, 0x01111001, 0x01111100, 0x01111101, + 0x01111102, 0x01111201, 0x01121002, 0x01121101, 0x01121200, 0x02101100, 0x02101201, 0x02111000, + 0x02111100, 0x02111101, 0x02111200, 0x02111201, 0x02111202, 0x02121001, 0x02121100, 0x02121101, + 0x02121201, 0x00101012, 0x00101111, 0x00101212, 0x00111011, 0x00111110, 0x00111111, 0x00111112, + 0x00111211, 0x00121010, 0x00121012, 0x00121111, 0x00121210, 0x00121212, 0x01101011, 0x01101110, + 0x01101111, 0x01101112, 0x01111011, 0x01111012, 0x01111110, 0x01111111, 0x01111112, 0x01111211, + 0x01111212, 0x01121011, 0x01121110, 0x01121111, 0x01121112, 0x01121211, 0x02101010, 0x02101012, + 0x02101110, 0x02101111, 0x02101210, 0x02101212, 0x02111010, 0x02111011, 0x02111110, 0x02111111, + 0x02111112, 0x02111211, 0x02111212, 0x02121010, 0x02121012, 0x02121111, 0x00101021, 0x00101120, + 0x00101121, 0x00101122, 0x00111121, 0x00111122, 0x00111220, 0x00111222, 0x00121021, 0x00121122, + 0x01101020, 0x01101022, 0x01101120, 0x01101121, 0x01101220, 0x01101222, 0x01111021, 0x01111121, + 0x01111122, 0x01111220, 0x01111221, 0x01121021, 0x01121120, 0x01121121, 0x01121220, 0x01121221, + 0x01121222, 0x02101122, 0x02101222, 0x02111022, 0x02111121, 0x02121120, 0x02121221, 0x00112001, + 0x00112102, 0x00122101, 0x01102001, 0x01102100, 0x01102102, 0x01102201, 0x01112000, 0x01112101, + 0x01112200, 0x01112202, 0x01122000, 0x01122001, 0x01122100, 0x01122102, 0x01122201, 0x02102101, + 0x02112001, 0x02112100, 0x02122101, 0x00112010, 0x00112012, 0x00112111, 0x00112212, 0x00122011, + 0x00122111, 0x01102012, 0x01102110, 0x01102111, 0x01102210, 0x01112011, 0x01112110, 0x01112111, + 0x01112112, 0x01112211, 0x01112212, 0x01122010, 0x01122111, 0x01122212, 0x02102211, 0x02112011, + 0x02112012, 0x02112111, 0x02112210, 0x02122011, 0x02122112, 0x02122211, 0x00102221, 0x00112122, + 0x00122120, 0x00122122, 0x01102120, 0x01102122, 0x01102221, 0x01112020, 0x01112022, 0x01112121, + 0x01112220, 0x01122021, 0x01122122, 0x01122221, 0x02102121, 0x02112021, 0x02112122, 0x02112222, + 0x00200000, 0x00200002, 0x00200200, 0x00200202, 0x00210101, 0x00220000, 0x00220002, 0x00220101, + 0x00220200, 0x00220202, 0x01200101, 0x01210001, 0x01210201, 0x01220001, 0x01220101, 0x02200000, + 0x02200002, 0x02200200, 0x02200202, 0x02210101, 0x02220000, 0x02220002, 0x02220101, 0x02220200, + 0x02220202, 0x00200111, 0x00210011, 0x00210110, 0x00210211, 0x00220111, 0x01200012, 0x01200110, + 0x01200211, 0x01210111, 0x01210210, 0x01210212, 0x01220011, 0x01220110, 0x01220111, 0x01220112, + 0x02200111, 0x02210010, 0x02210112, 0x02210211, 0x02220111, 0x00200021, 0x00200220, 0x00200222, + 0x00210021, 0x00210121, 0x00220020, 0x00220022, 0x00220220, 0x00220222, 0x01200121, 0x01210021, + 0x01210122, 0x01210221, 0x01220121, 0x02200021, 0x02200220, 0x02200222, 0x02210021, 0x02210121, + 0x02220020, 0x02220022, 0x02220220, 0x02220222, 0x00201101, 0x00211100, 0x00211102, 0x00211201, + 0x00221101, 0x01201100, 0x01201101, 0x01201102, 0x01201201, 0x01211002, 0x01211101, 0x01211200, + 0x01211202, 0x01221102, 0x02201101, 0x02211001, 0x02211100, 0x02211201, 0x02221001, 0x02221101, + 0x00201211, 0x00211111, 0x00221011, 0x00221211, 0x01201010, 0x01201111, 0x01201210, 0x01211011, + 0x01211110, 0x01211111, 0x01211211, 0x01221012, 0x01221111, 0x01221210, 0x02201211, 0x02211010, + 0x02211110, 0x02211111, 0x02211210, 0x02211212, 0x02221011, 0x02221110, 0x02221112, 0x02221211, + 0x00201121, 0x00211020, 0x00211022, 0x00211221, 0x00221121, 0x01201021, 0x01201221, 0x01211121, + 0x01221020, 0x01221021, 0x01221221, 0x02201120, 0x02201122, 0x02211020, 0x02211222, 0x00202000, + 0x00202002, 0x00202200, 0x00202202, 0x00212101, 0x00222000, 0x00222002, 0x00222200, 0x00222202, + 0x01202101, 0x01212001, 0x01212100, 0x01222101, 0x02202000, 0x02202002, 0x02202200, 0x02202202, + 0x02222000, 0x02222002, 0x02222200, 0x02222202, 0x00202211, 0x00212011, 0x00212110, 0x00212211, + 0x00222111, 0x01202112, 0x01202211, 0x01212012, 0x01212111, 0x01222011, 0x01222110, 0x01222112, + 0x01222211, 0x02202111, 0x02212010, 0x02212112, 0x02212211, 0x02222110, 0x02222111, 0x00202020, + 0x00202022, 0x00202220, 0x00202222, 0x00222020, 0x00222022, 0x00222220, 0x00222222, 0x01202121, + 0x01212021, 0x01212122, 0x01212221, 0x01222121, 0x02202020, 0x02202022, 0x02202220, 0x02202222, + 0x02212121, 0x02222020, 0x02222022, 0x02222220, 0x02222222, 0x10000101, 0x10010001, 0x10010102, + 0x10020101, 0x11000201, 0x11010002, 0x11010101, 0x11010200, 0x11010202, 0x11020001, 0x11020100, + 0x11020102, 0x12010100, 0x12010201, 0x12020001, 0x12020102, 0x10000010, 0x10000011, 0x10000110, + 0x10000112, 0x10000211, 0x10010012, 0x10010111, 0x10010112, 0x10010210, 0x10010212, 0x10020011, + 0x10020112, 0x10020211, 0x11000111, 0x11000210, 0x11000212, 0x11010011, 0x11010110, 0x11010111, + 0x11010112, 0x11010211, 0x11010212, 0x11020111, 0x11020210, 0x11020212, 0x12000011, 0x12000110, + 0x12000112, 0x12010010, 0x12010012, 0x12010111, 0x12020010, 0x12020011, 0x12020012, 0x10000121, + 0x10010021, 0x10010120, 0x10010122, 0x10020121, 0x11000021, 0x11010022, 0x11010121, 0x11010222, + 0x11020120, 0x11020221, 0x12000221, 0x12010120, 0x12020121, 0x10001001, 0x10011101, 0x10011201, + 0x10021201, 0x11001101, 0x11001200, 0x11001202, 0x11011001, 0x11011100, 0x11011101, 0x11011102, + 0x11021001, 0x11021002, 0x11021101, 0x11021200, 0x11021202, 0x12001001, 0x12001102, 0x12001201, + 0x12011000, 0x12011002, 0x12011101, 0x12021000, 0x12021001, 0x12021201, 0x10001011, 0x10001012, + 0x10001111, 0x10001212, 0x10011011, 0x10011110, 0x10011111, 0x10011112, 0x10011211, 0x10021010, + 0x10021111, 0x10021212, 0x11001011, 0x11001110, 0x11001111, 0x11001112, 0x11001211, 0x11011010, + 0x11011011, 0x11011110, 0x11011111, 0x11011112, 0x11011210, 0x11011211, 0x11021011, 0x11021110, + 0x11021111, 0x11021112, 0x11021211, 0x12001012, 0x12001110, 0x12001111, 0x12001210, 0x12011011, + 0x12011110, 0x12011111, 0x12011112, 0x12011211, 0x12011212, 0x12021111, 0x12021210, 0x12021212, + 0x10001021, 0x10001121, 0x10001221, 0x10011120, 0x10011121, 0x10011220, 0x10011222, 0x10021021, + 0x10021120, 0x10021221, 0x11001020, 0x11001022, 0x11001121, 0x11001220, 0x11011020, 0x11011021, + 0x11011022, 0x11011121, 0x11011122, 0x11011221, 0x11021022, 0x11021121, 0x11021220, 0x12001021, + 0x12001121, 0x12001222, 0x12011120, 0x12011121, 0x12021021, 0x12021120, 0x12021122, 0x10002101, + 0x10012001, 0x10012101, 0x10012202, 0x10022101, 0x11002002, 0x11002201, 0x11012000, 0x11012101, + 0x11012200, 0x11022001, 0x11022100, 0x11022102, 0x11022201, 0x12002101, 0x12012001, 0x12012100, + 0x12012102, 0x12012201, 0x12022101, 0x10002011, 0x10002111, 0x10002112, 0x10002212, 0x10012010, + 0x10012110, 0x10012111, 0x10012210, 0x10022011, 0x10022110, 0x10022112, 0x11002010, 0x11002111, + 0x11002212, 0x11012011, 0x11012012, 0x11012110, 0x11012111, 0x11012112, 0x11012211, 0x11022010, + 0x11022012, 0x11022111, 0x11022112, 0x11022212, 0x12002112, 0x12002211, 0x12012012, 0x12012111, + 0x12012112, 0x12012210, 0x12022011, 0x12022110, 0x12022112, 0x12022211, 0x10012122, 0x11002120, + 0x11002122, 0x11002221, 0x11012121, 0x11012220, 0x11012222, 0x11022120, 0x11022221, 0x12012120, + 0x12022121, 0x10100001, 0x10100100, 0x10100101, 0x10100102, 0x10100201, 0x10110002, 0x10110101, + 0x10110202, 0x10120001, 0x10120100, 0x10120201, 0x11100000, 0x11100101, 0x11100200, 0x11110001, + 0x11110100, 0x11110101, 0x11110102, 0x11110201, 0x11120101, 0x11120200, 0x12100102, 0x12100201, + 0x12110101, 0x12110200, 0x12120000, 0x12120001, 0x12120102, 0x12120201, 0x10100111, 0x10100210, + 0x10100211, 0x10100212, 0x10110011, 0x10110110, 0x10110111, 0x10110112, 0x10110210, 0x10110211, + 0x10120010, 0x10120111, 0x10120112, 0x10120210, 0x10120212, 0x11100011, 0x11100110, 0x11100111, + 0x11100112, 0x11100211, 0x11110010, 0x11110011, 0x11110012, 0x11110110, 0x11110111, 0x11110112, + 0x11110210, 0x11110211, 0x11110212, 0x11120011, 0x11120110, 0x11120111, 0x11120112, 0x11120211, + 0x12100012, 0x12100111, 0x12110011, 0x12110110, 0x12110111, 0x12110112, 0x12110211, 0x12120010, + 0x12120111, 0x12120212, 0x10100021, 0x10100122, 0x10110022, 0x10110121, 0x10110222, 0x10120021, + 0x10120120, 0x11100022, 0x11100121, 0x11100222, 0x11110021, 0x11110120, 0x11110121, 0x11110122, + 0x11110221, 0x11120022, 0x11120121, 0x12100121, 0x12110020, 0x12110022, 0x12110121, 0x12110221, + 0x12110222, 0x12120120, 0x10101100, 0x10101101, 0x10111001, 0x10111100, 0x10111101, 0x10111102, + 0x10111200, 0x10111201, 0x10121001, 0x10121101, 0x10121200, 0x10121202, 0x11101001, 0x11101100, + 0x11101101, 0x11101102, 0x11101201, 0x11101202, 0x11111000, 0x11111001, 0x11111100, 0x11111101, + 0x11111102, 0x11111200, 0x11111201, 0x11111202, 0x11121001, 0x11121002, 0x11121100, 0x11121101, + 0x11121102, 0x11121201, 0x12101000, 0x12101200, 0x12101202, 0x12111001, 0x12111100, 0x12111101, + 0x12111102, 0x12111201, 0x12121001, 0x12121100, 0x12121101, 0x12121202, 0x10101011, 0x10101012, + 0x10101110, 0x10101111, 0x10101112, 0x10101211, 0x10111010, 0x10111011, 0x10111012, 0x10111110, + 0x10111111, 0x10111112, 0x10111211, 0x10111212, 0x10121011, 0x10121110, 0x10121111, 0x10121112, + 0x10121211, 0x11101010, 0x11101011, 0x11101012, 0x11101110, 0x11101111, 0x11101112, 0x11101210, + 0x11101211, 0x11111010, 0x11111011, 0x11111012, 0x11111110, 0x11111111, 0x11111112, 0x11111210, + 0x11111211, 0x11111212, 0x11121010, 0x11121011, 0x11121110, 0x11121111, 0x11121112, 0x11121210, + 0x11121211, 0x11121212, 0x12101011, 0x12101110, 0x12101111, 0x12101211, 0x12101212, 0x12111010, + 0x12111011, 0x12111110, 0x12111111, 0x12111112, 0x12111210, 0x12111211, 0x12121011, 0x12121110, + 0x12121111, 0x12121112, 0x12121211, 0x10101020, 0x10101021, 0x10101022, 0x10101120, 0x10101122, + 0x10101220, 0x10101221, 0x10111021, 0x10111120, 0x10111121, 0x10111220, 0x10111221, 0x10121020, + 0x10121021, 0x10121022, 0x10121120, 0x10121121, 0x10121122, 0x10121220, 0x10121221, 0x11101021, + 0x11101121, 0x11101122, 0x11101220, 0x11101221, 0x11101222, 0x11111020, 0x11111021, 0x11111022, + 0x11111120, 0x11111121, 0x11111122, 0x11111220, 0x11111221, 0x11111222, 0x11121021, 0x11121120, + 0x11121121, 0x11121221, 0x12101022, 0x12101121, 0x12101122, 0x12101220, 0x12101221, 0x12101222, + 0x12111021, 0x12111121, 0x12111222, 0x12121022, 0x12121121, 0x12121122, 0x12121220, 0x12121221, + 0x10102100, 0x10102101, 0x10102102, 0x10102201, 0x10112000, 0x10112101, 0x10112200, 0x10122001, + 0x10122202, 0x11102101, 0x11102200, 0x11102202, 0x11112001, 0x11112100, 0x11112101, 0x11112102, + 0x11112200, 0x11112201, 0x11122000, 0x11122002, 0x11122100, 0x11122101, 0x12102002, 0x12102201, + 0x12112000, 0x12112002, 0x12112101, 0x12112200, 0x12122001, 0x12122201, 0x10102011, 0x10102012, + 0x10102111, 0x10102212, 0x10112011, 0x10112110, 0x10112111, 0x10112112, 0x10112211, 0x10122111, + 0x11102011, 0x11102110, 0x11102111, 0x11102112, 0x11102211, 0x11112010, 0x11112011, 0x11112012, + 0x11112110, 0x11112111, 0x11112112, 0x11112210, 0x11112211, 0x11112212, 0x11122011, 0x11122110, + 0x11122111, 0x11122112, 0x11122211, 0x12102011, 0x12102111, 0x12102211, 0x12112011, 0x12112110, + 0x12112111, 0x12112112, 0x12112210, 0x12112211, 0x12122111, 0x10102120, 0x10102220, 0x10112121, + 0x10112222, 0x10122020, 0x10122121, 0x10122122, 0x10122221, 0x11102121, 0x11102220, 0x11102221, + 0x11112021, 0x11112121, 0x11112122, 0x11112220, 0x11112221, 0x11122022, 0x11122121, 0x11122220, + 0x11122222, 0x12102021, 0x12102222, 0x12112022, 0x12112121, 0x12112122, 0x12112220, 0x12112222, + 0x12122021, 0x10200101, 0x10210100, 0x10210102, 0x10210201, 0x10220101, 0x11200100, 0x11210000, + 0x11210101, 0x11210102, 0x11210200, 0x11210202, 0x11220001, 0x11220100, 0x11220102, 0x11220201, + 0x12200001, 0x12210102, 0x12220101, 0x10200011, 0x10200110, 0x10200112, 0x10200211, 0x10210012, + 0x10210111, 0x10220011, 0x10220012, 0x10220112, 0x10220211, 0x11200111, 0x11200211, 0x11210011, + 0x11210111, 0x11210112, 0x11210211, 0x11220111, 0x11220112, 0x11220212, 0x12200110, 0x12200212, + 0x12210012, 0x12210111, 0x12220011, 0x12220112, 0x12220211, 0x10210021, 0x10210122, 0x10210221, + 0x11200020, 0x11200021, 0x11200122, 0x11210121, 0x11210122, 0x11210220, 0x11220020, 0x12200121, + 0x12210021, 0x12210122, 0x12220121, 0x10211001, 0x10211002, 0x10211101, 0x10211102, 0x10211202, + 0x10221001, 0x10221102, 0x10221201, 0x11201000, 0x11201002, 0x11201101, 0x11201200, 0x11201202, + 0x11211001, 0x11211100, 0x11211101, 0x11211102, 0x11211201, 0x11211202, 0x11221000, 0x11221002, + 0x11221101, 0x12201100, 0x12201101, 0x12201201, 0x12211000, 0x12211002, 0x12211100, 0x12211101, + 0x12211102, 0x12211200, 0x12211202, 0x12221001, 0x12221100, 0x12221201, 0x10201111, 0x10201210, + 0x10201212, 0x10211011, 0x10211111, 0x10211112, 0x10211211, 0x11201110, 0x11201111, 0x11201112, + 0x11201211, 0x11211010, 0x11211011, 0x11211110, 0x11211111, 0x11211112, 0x11211211, 0x11221011, + 0x11221110, 0x11221111, 0x11221112, 0x11221211, 0x12201112, 0x12201211, 0x12201212, 0x12211011, + 0x12211111, 0x12211112, 0x12211211, 0x12211212, 0x12221012, 0x12221111, 0x12221112, 0x12221210, + 0x10201022, 0x10201221, 0x10211121, 0x10221020, 0x10221122, 0x10221220, 0x10221221, 0x11201020, + 0x11201121, 0x11201220, 0x11201222, 0x11211021, 0x11211120, 0x11211121, 0x11211122, 0x11211220, + 0x11211222, 0x11221020, 0x11221121, 0x11221220, 0x12201020, 0x12201022, 0x12201121, 0x12201222, + 0x12211120, 0x12211122, 0x12211220, 0x12211221, 0x12221020, 0x12221120, 0x12221122, 0x12221222, + 0x10212102, 0x10212201, 0x10222101, 0x11202001, 0x11212002, 0x11212101, 0x11212202, 0x11222001, + 0x11222201, 0x12202101, 0x12212001, 0x12212200, 0x12222102, 0x10202011, 0x10202110, 0x10212010, + 0x10212111, 0x10222011, 0x10222110, 0x10222112, 0x10222211, 0x11202010, 0x11202011, 0x11202111, + 0x11202112, 0x11202210, 0x11212011, 0x11212110, 0x11212111, 0x11212112, 0x11212211, 0x11222010, + 0x11222111, 0x11222212, 0x12202012, 0x12202110, 0x12202212, 0x12212111, 0x12222011, 0x12222110, + 0x12222111, 0x12222211, 0x10212021, 0x10212122, 0x10212220, 0x11202021, 0x11202120, 0x11202221, + 0x11212020, 0x11212121, 0x11212220, 0x11212222, 0x11222120, 0x11222121, 0x11222221, 0x12202122, + 0x12212120, 0x12212220, 0x12212222, 0x12222122, 0x20000000, 0x20000002, 0x20000200, 0x20000202, + 0x20020000, 0x20020002, 0x20020200, 0x20020202, 0x21000101, 0x21010000, 0x21010001, 0x21010100, + 0x21010102, 0x21010201, 0x21020101, 0x22000000, 0x22000002, 0x22000200, 0x22000202, 0x22010101, + 0x22020000, 0x22020002, 0x22020200, 0x22020202, 0x20000111, 0x20010011, 0x20010110, 0x20010112, + 0x20010211, 0x20020111, 0x21000011, 0x21000110, 0x21000211, 0x21010010, 0x21010012, 0x21010111, + 0x21010112, 0x21010210, 0x21010211, 0x21020110, 0x21020112, 0x21020211, 0x22000111, 0x22000211, + 0x22010110, 0x22010112, 0x22010211, 0x22020111, 0x20000020, 0x20000022, 0x20000220, 0x20000222, + 0x20010121, 0x20020020, 0x20020022, 0x20020220, 0x20020222, 0x21010021, 0x21010120, 0x21010221, + 0x21020121, 0x22000020, 0x22000022, 0x22000220, 0x22000222, 0x22010121, 0x22020020, 0x22020022, + 0x22020220, 0x22020222, 0x20011100, 0x20011201, 0x21001001, 0x21001100, 0x21011001, 0x21011101, + 0x21011202, 0x21021001, 0x21021100, 0x21021201, 0x22011100, 0x22011201, 0x20001011, 0x20001211, + 0x20011012, 0x20011111, 0x20011212, 0x20021112, 0x20021211, 0x21001010, 0x21001011, 0x21001111, + 0x21001210, 0x21011011, 0x21011110, 0x21011111, 0x21011112, 0x21011211, 0x21011212, 0x21021111, + 0x21021112, 0x21021210, 0x21021212, 0x22001011, 0x22001110, 0x22001112, 0x22001211, 0x22011010, + 0x22011012, 0x22011111, 0x22011210, 0x22021112, 0x20011021, 0x20011122, 0x20011221, 0x20021121, + 0x21001021, 0x21001120, 0x21001221, 0x21001222, 0x21011020, 0x21011121, 0x21011221, 0x21011222, + 0x21021021, 0x21021122, 0x21021222, 0x22001121, 0x22011021, 0x22011222, 0x22021120, 0x20002000, + 0x20002002, 0x20002200, 0x20002202, 0x20012101, 0x20022000, 0x20022002, 0x20022200, 0x20022202, + 0x21002001, 0x21002101, 0x21012001, 0x21012100, 0x21012201, 0x21022101, 0x21022201, 0x22002000, + 0x22002002, 0x22002200, 0x22002202, 0x22012101, 0x22022000, 0x22022002, 0x22022200, 0x22022202, + 0x20002111, 0x20002112, 0x20012011, 0x20012110, 0x20012112, 0x20022111, 0x21002011, 0x21002110, + 0x21002112, 0x21002211, 0x21012010, 0x21012012, 0x21012111, 0x21012212, 0x21022011, 0x21022110, + 0x22002111, 0x22012112, 0x22012211, 0x22022111, 0x20002020, 0x20002022, 0x20002220, 0x20002222, + 0x20012121, 0x20022020, 0x20022022, 0x20022220, 0x20022222, 0x21002121, 0x21012021, 0x21012120, + 0x21012122, 0x22002020, 0x22002022, 0x22002220, 0x22002222, 0x22012121, 0x22022020, 0x22022022, + 0x22022220, 0x22022222, 0x20100101, 0x20110001, 0x20110102, 0x20110200, 0x20110201, 0x20120101, + 0x21100001, 0x21100102, 0x21100201, 0x21110101, 0x21110200, 0x21110202, 0x21120201, 0x21120202, + 0x22100101, 0x22110001, 0x22110100, 0x22110102, 0x22110201, 0x22120101, 0x20100011, 0x20100110, + 0x20100112, 0x20100211, 0x20110010, 0x20110111, 0x20110210, 0x20110212, 0x20120011, 0x20120110, + 0x20120112, 0x20120211, 0x21100010, 0x21100111, 0x21110010, 0x21110011, 0x21110110, 0x21110111, + 0x21110112, 0x21110211, 0x21120012, 0x21120111, 0x22100110, 0x22100112, 0x22110012, 0x22110111, + 0x22110210, 0x22120011, 0x22120110, 0x22120112, 0x22120211, 0x20100121, 0x20110021, 0x20110120, + 0x20110221, 0x20120121, 0x21100120, 0x21100122, 0x21100221, 0x21110020, 0x21110022, 0x21110121, + 0x21110220, 0x21120122, 0x21120221, 0x22100121, 0x22110120, 0x22110122, 0x22120221, 0x20101001, + 0x20101100, 0x20101102, 0x20111000, 0x20111101, 0x20111200, 0x20121102, 0x21101000, 0x21101202, + 0x21111001, 0x21111100, 0x21111101, 0x21111102, 0x21111200, 0x21111201, 0x21121000, 0x21121001, + 0x21121002, 0x21121101, 0x22101100, 0x22101102, 0x22111002, 0x22111100, 0x22111101, 0x22111200, + 0x22121001, 0x22121201, 0x20101010, 0x20101111, 0x20101210, 0x20101212, 0x20111010, 0x20111011, + 0x20111110, 0x20111111, 0x20111112, 0x20111211, 0x20121011, 0x20121111, 0x20121211, 0x20121212, + 0x21101011, 0x21101110, 0x21101111, 0x21101112, 0x21101211, 0x21111010, 0x21111011, 0x21111012, + 0x21111110, 0x21111111, 0x21111112, 0x21111210, 0x21111211, 0x21111212, 0x21121011, 0x21121110, + 0x21121111, 0x21121112, 0x21121211, 0x22101011, 0x22101111, 0x22101210, 0x22111011, 0x22111012, + 0x22111110, 0x22111111, 0x22111112, 0x22111211, 0x22111212, 0x22121010, 0x22121012, 0x22121111, + 0x22121210, 0x22121212, 0x20101021, 0x20101120, 0x20111020, 0x20111121, 0x20111221, 0x20121020, + 0x20121122, 0x20121221, 0x21101121, 0x21101220, 0x21101221, 0x21111021, 0x21111022, 0x21111121, + 0x21111122, 0x21111221, 0x21121121, 0x21121220, 0x22101022, 0x22101120, 0x22101221, 0x22101222, + 0x22111022, 0x22111120, 0x22111121, 0x22121120, 0x22121122, 0x22121221, 0x20102101, 0x20112102, + 0x20112201, 0x20122101, 0x21102001, 0x21102102, 0x21112000, 0x21112002, 0x21112101, 0x21112102, + 0x21112202, 0x21122100, 0x21122101, 0x22102101, 0x22112001, 0x22112102, 0x22112201, 0x22122101, + 0x20102110, 0x20102112, 0x20102211, 0x20112010, 0x20112012, 0x20112111, 0x20112210, 0x20112212, + 0x20122010, 0x20122011, 0x20122110, 0x20122112, 0x21102010, 0x21102012, 0x21102111, 0x21102210, + 0x21102212, 0x21112011, 0x21112110, 0x21112111, 0x21112112, 0x21112211, 0x21122012, 0x21122111, + 0x21122112, 0x21122212, 0x22102011, 0x22102110, 0x22112010, 0x22112012, 0x22112111, 0x22112212, + 0x22122011, 0x22122112, 0x20102121, 0x20112121, 0x20122121, 0x21102120, 0x21102122, 0x21102221, + 0x21112020, 0x21112121, 0x21112220, 0x21122021, 0x22102121, 0x22112021, 0x22112120, 0x22112121, + 0x22112122, 0x20200000, 0x20200002, 0x20200200, 0x20200202, 0x20210101, 0x20220000, 0x20220002, + 0x20220200, 0x20220202, 0x21200101, 0x21210001, 0x21210100, 0x21210102, 0x21210201, 0x22200000, + 0x22200002, 0x22200200, 0x22200202, 0x22210101, 0x22220000, 0x22220002, 0x22220200, 0x22220202, + 0x20200111, 0x20200211, 0x20210011, 0x20210110, 0x20210112, 0x20210211, 0x20210212, 0x21200112, + 0x21200211, 0x21210011, 0x21210111, 0x21210210, 0x21210212, 0x21220011, 0x21220110, 0x22200111, + 0x22210010, 0x22210012, 0x22210112, 0x22210211, 0x20200022, 0x20200220, 0x20200222, 0x20210020, + 0x20210221, 0x20220022, 0x20220220, 0x20220222, 0x21200121, 0x21210021, 0x21210122, 0x21210221, + 0x21220121, 0x22200020, 0x22200022, 0x22200220, 0x22200222, 0x22210121, 0x22220020, 0x22220022, + 0x22220220, 0x22220222, 0x20211201, 0x20221101, 0x21201001, 0x21201100, 0x21211000, 0x21211100, + 0x21211101, 0x21211200, 0x21211202, 0x21221001, 0x21221101, 0x21221102, 0x21221200, 0x21221201, + 0x22201101, 0x20201112, 0x20201211, 0x20211010, 0x20211012, 0x20211111, 0x20211210, 0x20221112, + 0x20221211, 0x21201012, 0x21201111, 0x21211011, 0x21211110, 0x21211111, 0x21211112, 0x21211211, + 0x21221111, 0x21221212, 0x22201011, 0x22201110, 0x22201111, 0x22201112, 0x22201211, 0x22211012, + 0x22211111, 0x22211210, 0x20201121, 0x20211021, 0x20211122, 0x20211222, 0x20221021, 0x20221121, + 0x21201120, 0x21201122, 0x21201222, 0x21211022, 0x21211121, 0x21211122, 0x21211220, 0x21221020, + 0x21221022, 0x22201122, 0x22211020, 0x22211121, 0x22211122, 0x22211221, 0x22221021, 0x22221120, + 0x22221122, 0x20202000, 0x20202002, 0x20202200, 0x20202202, 0x20222000, 0x20222002, 0x20222200, + 0x20222202, 0x21212001, 0x21212100, 0x21212102, 0x21212201, 0x22202000, 0x22202002, 0x22202200, + 0x22202202, 0x22212101, 0x22222000, 0x22222002, 0x22222200, 0x22222202, 0x20202111, 0x20212110, + 0x20212211, 0x20222011, 0x20222111, 0x21202011, 0x21212010, 0x21212111, 0x21212212, 0x21222011, + 0x21222112, 0x21222211, 0x22212010, 0x22212112, 0x20202020, 0x20202022, 0x20202220, 0x20202222, + 0x20222020, 0x20222022, 0x20222220, 0x20222222, 0x21212021, 0x21212120, 0x21212122, 0x22202020, + 0x22202022, 0x22202220, 0x22202222, 0x22212121, 0x22222020, 0x22222022, 0x22222220, 0x22222222, +GGML_TABLE_END() +#endif + +#endif // GGML_COMMON_IMPL +#endif // GGML_COMMON_IMPL diff --git a/libs/ggml/src/backend/metal/ggml/ggml-metal-impl.h b/libs/ggml/src/backend/metal/ggml/ggml-metal-impl.h new file mode 100644 index 0000000000..eb2253e029 --- /dev/null +++ b/libs/ggml/src/backend/metal/ggml/ggml-metal-impl.h @@ -0,0 +1,1135 @@ +#ifndef GGML_METAL_IMPL +#define GGML_METAL_IMPL + +// kernel parameters for mat-vec threadgroups +// +// N_R0: number of src0 rows to process per simdgroup +// N_SG: number of simdgroups per threadgroup +// +// TODO: for optimal performance, become function of the device and work size + +#define N_R0_Q4_0 4 +#define N_SG_Q4_0 2 + +#define N_R0_Q4_1 4 +#define N_SG_Q4_1 2 + +#define N_R0_Q5_0 4 +#define N_SG_Q5_0 2 + +#define N_R0_Q5_1 4 +#define N_SG_Q5_1 2 + +#define N_R0_Q8_0 2 +#define N_SG_Q8_0 4 + +#define N_R0_MXFP4 2 +#define N_SG_MXFP4 2 + +#define N_R0_Q2_K 4 +#define N_SG_Q2_K 2 + +#define N_R0_Q3_K 2 +#define N_SG_Q3_K 2 + +#define N_R0_Q4_K 2 +#define N_SG_Q4_K 2 + +#define N_R0_Q5_K 1 +#define N_SG_Q5_K 2 + +#define N_R0_Q6_K 2 +#define N_SG_Q6_K 2 + +#define N_R0_IQ1_S 4 +#define N_SG_IQ1_S 2 + +#define N_R0_IQ1_M 4 +#define N_SG_IQ1_M 2 + +#define N_R0_IQ2_XXS 4 +#define N_SG_IQ2_XXS 2 + +#define N_R0_IQ2_XS 4 +#define N_SG_IQ2_XS 2 + +#define N_R0_IQ2_S 4 +#define N_SG_IQ2_S 2 + +#define N_R0_IQ3_XXS 4 +#define N_SG_IQ3_XXS 2 + +#define N_R0_IQ3_S 4 +#define N_SG_IQ3_S 2 + +#define N_R0_IQ4_NL 2 +#define N_SG_IQ4_NL 2 + +#define N_R0_IQ4_XS 2 +#define N_SG_IQ4_XS 2 + +// function constants offsets +#define FC_FLASH_ATTN_EXT_PAD 100 +#define FC_FLASH_ATTN_EXT_BLK 200 +#define FC_FLASH_ATTN_EXT 300 +#define FC_FLASH_ATTN_EXT_VEC 400 +#define FC_FLASH_ATTN_EXT_VEC_REDUCE 500 +#define FC_MUL_MV 600 +#define FC_MUL_MM 700 +#define FC_ROPE 800 +#define FC_SSM_CONV 900 +#define FC_SOLVE_TRI 1000 +#define FC_COUNT_EQUAL 1100 +#define FC_UNARY 1200 +#define FC_BIN 1300 +#define FC_SUM_ROWS 1400 +#define FC_UPSCALE 1500 +#define FC_GATED_DELTA_NET 1600 + +// op-specific constants +#define OP_FLASH_ATTN_EXT_NQPSG 8 +#define OP_FLASH_ATTN_EXT_NCPSG 64 + +#define OP_FLASH_ATTN_EXT_VEC_NQPSG 1 +#define OP_FLASH_ATTN_EXT_VEC_NCPSG 32 + +#define OP_UNARY_NUM_SCALE 10 +#define OP_UNARY_NUM_FILL 11 +#define OP_UNARY_NUM_CLAMP 12 +#define OP_UNARY_NUM_SQR 13 +#define OP_UNARY_NUM_SQRT 14 +#define OP_UNARY_NUM_SIN 15 +#define OP_UNARY_NUM_COS 16 +#define OP_UNARY_NUM_LOG 17 +#define OP_UNARY_NUM_LEAKY_RELU 18 + +#define OP_UNARY_NUM_TANH 100 +#define OP_UNARY_NUM_RELU 101 +#define OP_UNARY_NUM_SIGMOID 102 +#define OP_UNARY_NUM_GELU 103 +#define OP_UNARY_NUM_GELU_ERF 104 +#define OP_UNARY_NUM_GELU_QUICK 105 +#define OP_UNARY_NUM_SILU 106 +#define OP_UNARY_NUM_ELU 107 +#define OP_UNARY_NUM_NEG 108 +#define OP_UNARY_NUM_ABS 109 +#define OP_UNARY_NUM_SGN 110 +#define OP_UNARY_NUM_STEP 111 +#define OP_UNARY_NUM_HARDSWISH 112 +#define OP_UNARY_NUM_HARDSIGMOID 113 +#define OP_UNARY_NUM_EXP 114 +#define OP_UNARY_NUM_SOFTPLUS 115 +#define OP_UNARY_NUM_EXPM1 116 +#define OP_UNARY_NUM_FLOOR 117 +#define OP_UNARY_NUM_CEIL 118 +#define OP_UNARY_NUM_ROUND 119 +#define OP_UNARY_NUM_TRUNC 120 + +#define OP_SUM_ROWS_NUM_SUM_ROWS 10 +#define OP_SUM_ROWS_NUM_MEAN 11 + +// kernel argument structs +// +// - element counters (e.g. ne00) typically use int32_t to reduce register usage +// however, be careful from int overflows when using those in the kernel implementation +// +// - strides (e.g. nb00) use uint64_t + +typedef struct { + int32_t ne00; + int32_t ne01; + int32_t ne02; + int32_t ne03; + uint64_t nb00; + uint64_t nb01; + uint64_t nb02; + uint64_t nb03; + int32_t ne10; + int32_t ne11; + int32_t ne12; + int32_t ne13; + uint64_t nb10; + uint64_t nb11; + uint64_t nb12; + uint64_t nb13; + int32_t ne0; + int32_t ne1; + int32_t ne2; + int32_t ne3; + uint64_t nb0; + uint64_t nb1; + uint64_t nb2; + uint64_t nb3; + int32_t dim; +} ggml_metal_kargs_concat; + +typedef struct { + int32_t ne00; + int32_t ne01; + int32_t ne02; + int32_t ne03; + uint64_t nb00; + uint64_t nb01; + uint64_t nb02; + uint64_t nb03; + int32_t ne0; + int32_t ne1; + int32_t ne2; + int32_t ne3; + uint64_t nb0; + uint64_t nb1; + uint64_t nb2; + uint64_t nb3; + float slope; + float scale; + float bias; + float val; + float min; + float max; +} ggml_metal_kargs_unary; + +typedef struct { + int32_t ne00; + int32_t ne01; + int32_t ne02; + int32_t ne03; + uint64_t nb00; + uint64_t nb01; + uint64_t nb02; + uint64_t nb03; + int32_t ne10; + int32_t ne11; + int32_t ne12; + int32_t ne13; + uint64_t nb10; + uint64_t nb11; + uint64_t nb12; + uint64_t nb13; + int32_t ne0; + int32_t ne1; + int32_t ne2; + int32_t ne3; + uint64_t nb0; + uint64_t nb1; + uint64_t nb2; + uint64_t nb3; + uint64_t offs; + uint64_t o1[8]; +} ggml_metal_kargs_bin; + +typedef struct { + int64_t ne0; + int64_t ne1; + size_t nb01; + size_t nb02; + size_t nb11; + size_t nb21; +} ggml_metal_kargs_add_id; + +typedef struct { + int32_t ne00; + int32_t ne01; + int32_t ne02; + int32_t ne03; + uint64_t nb00; + uint64_t nb01; + uint64_t nb02; + uint64_t nb03; + int32_t ne0; + int32_t ne1; + int32_t ne2; + int32_t ne3; + uint64_t nb0; + uint64_t nb1; + uint64_t nb2; + uint64_t nb3; +} ggml_metal_kargs_repeat; + +typedef struct { + int64_t nk0; + int64_t ne00; + int64_t ne01; + int64_t ne02; + int64_t ne03; + uint64_t nb00; + uint64_t nb01; + uint64_t nb02; + uint64_t nb03; + int64_t ne0; + int64_t ne1; + int64_t ne2; + int64_t ne3; + uint64_t nb0; + uint64_t nb1; + uint64_t nb2; + uint64_t nb3; +} ggml_metal_kargs_cpy; + +typedef struct { + int64_t ne10; + int64_t ne11; + int64_t ne12; + uint64_t nb10; + uint64_t nb11; + uint64_t nb12; + uint64_t nb13; + uint64_t nb1; + uint64_t nb2; + uint64_t nb3; + uint64_t offs; + bool inplace; +} ggml_metal_kargs_set; + +typedef struct { + int32_t ne00; + int32_t ne01; + int32_t ne02; + int32_t ne03; + uint64_t nb00; + uint64_t nb01; + uint64_t nb02; + uint64_t nb03; + int32_t ne0; + int32_t ne1; + int32_t ne2; + int32_t ne3; + uint64_t nb0; + uint64_t nb1; + uint64_t nb2; + uint64_t nb3; + int32_t n_past; + int32_t n_dims; + int32_t n_ctx_orig; + float freq_base; + float freq_scale; + float ext_factor; + float attn_factor; + float beta_fast; + float beta_slow; + int32_t sect_0; + int32_t sect_1; + int32_t sect_2; + int32_t sect_3; + bool src2; +} ggml_metal_kargs_rope; + +typedef struct { + int32_t ne11; + int32_t ne_12_2; // assume K and V are same shape + int32_t ne_12_3; + uint64_t nb11; + uint64_t nb12; + uint64_t nb13; + uint64_t nb21; + uint64_t nb22; + uint64_t nb23; + int32_t ne31; + int32_t ne32; + int32_t ne33; + uint64_t nb31; + uint64_t nb32; + uint64_t nb33; +} ggml_metal_kargs_flash_attn_ext_pad; + +typedef struct { + int32_t ne01; + int32_t ne30; + int32_t ne31; + int32_t ne32; + int32_t ne33; + uint64_t nb31; + uint64_t nb32; + uint64_t nb33; +} ggml_metal_kargs_flash_attn_ext_blk; + +typedef struct { + int32_t ne01; + int32_t ne02; + int32_t ne03; + uint64_t nb01; + uint64_t nb02; + uint64_t nb03; + int32_t ne11; + int32_t ne_12_2; // assume K and V are same shape + int32_t ne_12_3; + int32_t ns10; + uint64_t nb11; + uint64_t nb12; + uint64_t nb13; + int32_t ns20; + uint64_t nb21; + uint64_t nb22; + uint64_t nb23; + int32_t ne31; + int32_t ne32; + int32_t ne33; + uint64_t nb31; + uint64_t nb32; + uint64_t nb33; + int32_t ne1; + int32_t ne2; + int32_t ne3; + float scale; + float max_bias; + float m0; + float m1; + int32_t n_head_log2; + float logit_softcap; +} ggml_metal_kargs_flash_attn_ext; + +typedef struct { + int32_t ne01; + int32_t ne02; + int32_t ne03; + uint64_t nb01; + uint64_t nb02; + uint64_t nb03; + int32_t ne11; + int32_t ne_12_2; // assume K and V are same shape + int32_t ne_12_3; + int32_t ns10; + uint64_t nb11; + uint64_t nb12; + uint64_t nb13; + int32_t ns20; + uint64_t nb21; + uint64_t nb22; + uint64_t nb23; + int32_t ne31; + int32_t ne32; + int32_t ne33; + uint64_t nb31; + uint64_t nb32; + uint64_t nb33; + int32_t ne1; + int32_t ne2; + int32_t ne3; + float scale; + float max_bias; + float m0; + float m1; + int32_t n_head_log2; + float logit_softcap; +} ggml_metal_kargs_flash_attn_ext_vec; + +typedef struct { + int32_t nrows; +} ggml_metal_kargs_flash_attn_ext_vec_reduce; + +typedef struct { + int32_t ne00; + int32_t ne02; + uint64_t nb01; + uint64_t nb02; + uint64_t nb03; + int32_t ne12; + uint64_t nb10; + uint64_t nb11; + uint64_t nb12; + uint64_t nb13; + int32_t ne0; + int32_t ne1; + int16_t r2; + int16_t r3; +} ggml_metal_kargs_mul_mm; + +typedef struct { + int32_t ne00; + int32_t ne01; + int32_t ne02; + uint64_t nb00; + uint64_t nb01; + uint64_t nb02; + uint64_t nb03; + int32_t ne10; + int32_t ne11; + int32_t ne12; + uint64_t nb10; + uint64_t nb11; + uint64_t nb12; + uint64_t nb13; + int32_t ne0; + int32_t ne1; + int32_t nr0; + int16_t r2; + int16_t r3; +} ggml_metal_kargs_mul_mv; + +typedef struct { + int32_t ne00; + int32_t ne01; + int32_t ne02; + uint64_t nb00; + uint64_t nb01; + uint64_t nb02; + uint64_t nb03; + int32_t ne10; + int32_t ne11; + int32_t ne12; + uint64_t nb10; + uint64_t nb11; + uint64_t nb12; + uint64_t nb13; + int32_t ne0; + int32_t ne1; + int16_t r2; + int16_t r3; +} ggml_metal_kargs_mul_mv_ext; + +typedef struct { + int32_t ne02; + int32_t ne10; + int32_t ne11; // n_expert_used (bcast) + uint64_t nb11; + uint64_t nb12; + int32_t ne21; // n_tokens + int32_t ne20; // n_expert_used + uint64_t nb21; +} ggml_metal_kargs_mul_mm_id_map0; + +typedef struct { + int32_t ne00; + int32_t ne02; + uint64_t nb01; + uint64_t nb02; + uint64_t nb03; + int32_t ne11; + uint64_t nb10; + uint64_t nb11; + uint64_t nb12; + uint64_t nb13; + int32_t ne20; + int32_t ne21; + int32_t ne0; + int32_t ne1; + int16_t r2; + int16_t r3; +} ggml_metal_kargs_mul_mm_id; + +typedef struct { + int32_t nei0; + int32_t nei1; + uint64_t nbi1; + int32_t ne00; + int32_t ne01; + int32_t ne02; + uint64_t nb00; + uint64_t nb01; + uint64_t nb02; + int32_t ne10; + int32_t ne11; + int32_t ne12; + int32_t ne13; + uint64_t nb10; + uint64_t nb11; + uint64_t nb12; + int32_t ne0; + int32_t ne1; + uint64_t nb1; + int32_t nr0; +} ggml_metal_kargs_mul_mv_id; + +// NORM +// RMS_NORM +typedef struct { + int32_t ne00; + int32_t ne00_t; + uint64_t nb1; + uint64_t nb2; + uint64_t nb3; + float eps; + int32_t nef1[3]; + int32_t nef2[3]; + int32_t nef3[3]; + uint64_t nbf1[3]; + uint64_t nbf2[3]; + uint64_t nbf3[3]; +} ggml_metal_kargs_norm; + +typedef struct { + int32_t ne00; + int32_t ne01; + int32_t ne02; + int32_t ne03; + uint64_t nb00; + uint64_t nb01; + uint64_t nb02; + uint64_t nb03; + int32_t ne0; + int32_t ne1; + int32_t ne2; + int32_t ne3; + uint64_t nb0; + uint64_t nb1; + uint64_t nb2; + uint64_t nb3; + float eps; +} ggml_metal_kargs_l2_norm; + +typedef struct { + int64_t ne00; + int64_t ne01; + int64_t ne02; + uint64_t nb00; + uint64_t nb01; + uint64_t nb02; + int32_t ngrp; + float eps; +} ggml_metal_kargs_group_norm; + +typedef struct { + int32_t IC; + int32_t IL; + int32_t K; + int32_t s0; + uint64_t nb0; + uint64_t nb1; +} ggml_metal_kargs_conv_transpose_1d; + +typedef struct { + int32_t IC; + int32_t IH; + int32_t IW; + int32_t KH; + int32_t KW; + int32_t OC; + int32_t s0; + uint64_t nb0; + uint64_t nb1; + uint64_t nb2; +} ggml_metal_kargs_conv_transpose_2d; + +typedef struct { + uint64_t nb00; + uint64_t nb01; + uint64_t nb02; + uint64_t nb03; + uint64_t nb10; + uint64_t nb11; + uint64_t nb12; + uint64_t nb13; + uint64_t nb0; + uint64_t nb1; + uint64_t nb2; + uint64_t nb3; + int32_t IW; + int32_t IH; + int32_t KW; + int32_t KH; + int32_t IC; + int32_t OC; + int32_t OW; + int32_t OH; + int32_t N; + int32_t s0; + int32_t s1; + int32_t p0; + int32_t p1; + int32_t d0; + int32_t d1; +} ggml_metal_kargs_conv_2d; + +typedef struct { + uint64_t ofs0; + uint64_t ofs1; + int32_t IW; + int32_t IH; + int32_t CHW; + int32_t s0; + int32_t s1; + int32_t p0; + int32_t p1; + int32_t d0; + int32_t d1; + int32_t N; + int32_t KH; + int32_t KW; + int32_t KHW; // KH * KW, pre-computed on CPU to save GPU resources +} ggml_metal_kargs_im2col; + +typedef struct { + int32_t IW; + int32_t IH; + int32_t ID; + int32_t OW; + int32_t OH; + int32_t OD; + int32_t KW; + int32_t KH; + int32_t KD; + int32_t s0; + int32_t s1; + int32_t s2; + int32_t p0; + int32_t p1; + int32_t p2; + int32_t d0; + int32_t d1; + int32_t d2; + int32_t IC; + int32_t N; + int32_t OC; + uint64_t nb00; + uint64_t nb01; + uint64_t nb02; + uint64_t nb03; + uint64_t nb10; + uint64_t nb11; + uint64_t nb12; + uint64_t nb13; + uint64_t nb0; + uint64_t nb1; + uint64_t nb2; + uint64_t nb3; +} ggml_metal_kargs_conv_3d; + +typedef struct{ + int32_t ne00; + uint64_t nb01; + int32_t ne10; + uint64_t nb11; + int32_t ne0; + uint64_t nb1; + int32_t i00; + int32_t i10; + float alpha; + float limit; +} ggml_metal_kargs_glu; + +typedef struct { + uint64_t np; +} ggml_metal_kargs_sum; + +typedef struct { + int64_t ne00; + int64_t ne01; + int64_t ne02; + int64_t ne03; + uint64_t nb00; + uint64_t nb01; + uint64_t nb02; + uint64_t nb03; + int64_t ne0; + int64_t ne1; + int64_t ne2; + int64_t ne3; + uint64_t nb0; + uint64_t nb1; + uint64_t nb2; + uint64_t nb3; +} ggml_metal_kargs_sum_rows; + +typedef struct { + int64_t ne00; + int64_t ne01; + int64_t ne02; + int64_t ne03; + uint64_t nb00; + uint64_t nb01; + uint64_t nb02; + uint64_t nb03; + int64_t net0; + int64_t net1; + int64_t net2; + int64_t net3; + uint64_t nbt0; + uint64_t nbt1; + uint64_t nbt2; + uint64_t nbt3; + bool outb; +} ggml_metal_kargs_cumsum_blk; + +typedef struct { + int64_t ne00; + int64_t ne01; + int64_t ne02; + int64_t ne03; + uint64_t nb00; + uint64_t nb01; + uint64_t nb02; + uint64_t nb03; + int64_t net0; + int64_t net1; + int64_t net2; + int64_t net3; + uint64_t nbt0; + uint64_t nbt1; + uint64_t nbt2; + uint64_t nbt3; +} ggml_metal_kargs_cumsum_add; + +typedef struct { + int32_t ne00; + int32_t ne01; + int32_t ne02; + uint64_t nb01; + uint64_t nb02; + uint64_t nb03; + int32_t ne11; + int32_t ne12; + int32_t ne13; + uint64_t nb11; + uint64_t nb12; + uint64_t nb13; + uint64_t nb1; + uint64_t nb2; + uint64_t nb3; + float scale; + float max_bias; + float m0; + float m1; + int32_t n_head_log2; +} ggml_metal_kargs_soft_max; + +typedef struct { + int64_t ne00; + int64_t ne01; + int64_t ne02; + uint64_t nb00; + uint64_t nb01; + uint64_t nb02; + int64_t ne10; + int64_t ne11; + uint64_t nb10; + uint64_t nb11; + int64_t ne0; + int64_t ne1; + int64_t ne2; + uint64_t nb0; + uint64_t nb1; + uint64_t nb2; +} ggml_metal_kargs_ssm_conv; + +typedef struct { + int64_t d_state; + int64_t d_inner; + int64_t n_head; + int64_t n_group; + int64_t n_seq_tokens; + int64_t n_seqs; + uint64_t s_off; + uint64_t nb00; + uint64_t nb01; + uint64_t nb02; + uint64_t nb03; + uint64_t nb10; + uint64_t nb11; + uint64_t nb12; + uint64_t ns12; + uint64_t nb13; + uint64_t nb20; + uint64_t nb21; + uint64_t ns21; + uint64_t nb22; + int64_t ne30; + uint64_t nb31; + uint64_t nb41; + uint64_t nb42; + uint64_t ns42; + uint64_t nb43; + uint64_t nb51; + uint64_t nb52; + uint64_t ns52; + uint64_t nb53; + uint64_t nb0; +} ggml_metal_kargs_ssm_scan; + +typedef struct { + int32_t ne00; + int32_t ne01; + int32_t ne02; + int32_t ne03; + uint64_t nb00; + uint64_t nb01; + uint64_t nb02; + uint64_t nb03; + int32_t ne10; + int32_t ne11; + int32_t ne12; + int32_t ne13; + uint64_t nb10; + uint64_t nb11; + uint64_t nb12; + uint64_t nb13; + int32_t ne20; + int32_t ne21; + int32_t ne22; + int32_t ne23; + uint64_t nb20; + uint64_t nb21; + uint64_t nb22; + uint64_t nb23; + int32_t ns02; + int32_t ns12; + int32_t ns22; + int32_t ne0; + int32_t ne1; + int32_t ne2; + int32_t ne3; + uint64_t nb0; + uint64_t nb1; + uint64_t nb2; + uint64_t nb3; +} ggml_metal_kargs_gated_delta_net; + +typedef struct { + int32_t ne00; + int32_t ne01; + int32_t ne02; + int32_t ne03; + uint64_t nb00; + uint64_t nb01; + uint64_t nb02; + uint64_t nb03; + int32_t ne10; + int32_t ne11; + int32_t ne12; + int32_t ne13; + uint64_t nb10; + uint64_t nb11; + uint64_t nb12; + uint64_t nb13; + int32_t ne0; + int32_t ne1; + int32_t ne2; + int32_t ne3; + uint64_t nb0; + uint64_t nb1; + uint64_t nb2; + uint64_t nb3; +} ggml_metal_kargs_solve_tri; + +typedef struct { + int32_t ne00t; + int32_t ne00; + uint64_t nb01; + uint64_t nb02; + uint64_t nb03; + int32_t ne10; + uint64_t nb10; + uint64_t nb11; + uint64_t nb12; + uint64_t nb1; + uint64_t nb2; + uint64_t nb3; +} ggml_metal_kargs_get_rows; + +typedef struct { + int32_t nk0; + int32_t ne01; + uint64_t nb01; + uint64_t nb02; + uint64_t nb03; + int32_t ne11; + int32_t ne12; + uint64_t nb10; + uint64_t nb11; + uint64_t nb12; + uint64_t nb1; + uint64_t nb2; + uint64_t nb3; +} ggml_metal_kargs_set_rows; + +typedef struct { + int32_t ne00; + int32_t ne01; + int32_t ne02; + int32_t ne03; + uint64_t nb00; + uint64_t nb01; + uint64_t nb02; + uint64_t nb03; + int32_t ne0; + int32_t ne1; + int32_t ne2; + int32_t ne3; + uint64_t nb0; + uint64_t nb1; + uint64_t nb2; + uint64_t nb3; +} ggml_metal_kargs_diag; + +typedef struct { + int64_t ne00; + int64_t ne01; + int64_t ne02; + int64_t ne03; + uint64_t nb00; + uint64_t nb01; + uint64_t nb02; + uint64_t nb03; + int64_t ne0; + int64_t ne1; + int64_t ne2; + int64_t ne3; + uint64_t nb0; + uint64_t nb1; + uint64_t nb2; + uint64_t nb3; + float sf0; + float sf1; + float sf2; + float sf3; + float poffs; +} ggml_metal_kargs_upscale; + +typedef struct { + int64_t ne00; + int64_t ne01; + int64_t ne02; + int64_t ne03; + uint64_t nb00; + uint64_t nb01; + uint64_t nb02; + uint64_t nb03; + int64_t ne0; + int64_t ne1; + int64_t ne2; + int64_t ne3; + uint64_t nb0; + uint64_t nb1; + uint64_t nb2; + uint64_t nb3; +} ggml_metal_kargs_pad; + +typedef struct { + int64_t ne00; + int64_t ne01; + int64_t ne02; + int64_t ne03; + uint64_t nb00; + uint64_t nb01; + uint64_t nb02; + uint64_t nb03; + int64_t ne0; + int64_t ne1; + int64_t ne2; + int64_t ne3; + uint64_t nb0; + uint64_t nb1; + uint64_t nb2; + uint64_t nb3; + int32_t p0; + int32_t p1; +} ggml_metal_kargs_pad_reflect_1d; + +typedef struct { + uint64_t nb1; + int dim; + int max_period; +} ggml_metal_kargs_timestep_embedding; + +typedef struct { + int32_t ne00; + int32_t ne01; + int32_t ne02; + int32_t ne03; + uint64_t nb00; + uint64_t nb01; + uint64_t nb02; + uint64_t nb03; + int32_t ne0; + int32_t ne1; + int32_t ne2; + int32_t ne3; + uint64_t nb0; + uint64_t nb1; + uint64_t nb2; + uint64_t nb3; +} ggml_metal_kargs_tri; + +typedef struct { + int32_t ne00; + int32_t ne01; + int32_t ne02; + int32_t ne03; + uint64_t nb00; + uint64_t nb01; + uint64_t nb02; + uint64_t nb03; + int32_t ne0; + int32_t ne1; + int32_t ne2; + int32_t ne3; + int32_t top_k; +} ggml_metal_kargs_argsort; + +typedef struct { + int64_t ne00; + int64_t ne01; + int64_t ne02; + int64_t ne03; + uint64_t nb00; + uint64_t nb01; + uint64_t nb02; + uint64_t nb03; + int32_t ne0; + int32_t ne1; + int32_t ne2; + int32_t ne3; + int32_t top_k; + int32_t len; +} ggml_metal_kargs_argsort_merge; + +typedef struct { + int64_t ne0; + float start; + float step; +} ggml_metal_kargs_arange; + +typedef struct { + int64_t val; +} ggml_metal_kargs_memset; + +typedef struct { + int32_t ne00; + int32_t ne01; + int32_t ne02; + int32_t ne03; + uint64_t nb00; + uint64_t nb01; + uint64_t nb02; + uint64_t nb03; + uint64_t nb10; + uint64_t nb11; + uint64_t nb12; + uint64_t nb13; +} ggml_metal_kargs_count_equal; + +typedef struct { + int32_t k0; + int32_t k1; + int32_t s0; + int32_t s1; + int32_t p0; + int32_t p1; + int64_t IH; + int64_t IW; + int64_t OH; + int64_t OW; + int64_t np; +} ggml_metal_kargs_pool_2d; + +typedef struct { + int32_t k0; + int32_t s0; + int32_t p0; + int64_t IW; + int64_t OW; + int64_t np; +} ggml_metal_kargs_pool_1d; + +typedef struct { + int64_t ne00; + uint64_t nb01; +} ggml_metal_kargs_argmax; + +typedef struct { + int64_t np; +} ggml_metal_kargs_opt_step_adamw; + +typedef struct { + int64_t np; +} ggml_metal_kargs_opt_step_sgd; + +#endif // GGML_METAL_IMPL diff --git a/libs/ggml/src/backend/metal/ggml/ggml-metal.metal b/libs/ggml/src/backend/metal/ggml/ggml-metal.metal new file mode 100644 index 0000000000..2074211594 --- /dev/null +++ b/libs/ggml/src/backend/metal/ggml/ggml-metal.metal @@ -0,0 +1,10360 @@ +#define GGML_COMMON_DECL_METAL +#define GGML_COMMON_IMPL_METAL +#if defined(GGML_METAL_EMBED_LIBRARY) +__embed_ggml-common.h__ +#else +#include "ggml-common.h" +#endif +#include "ggml-metal-impl.h" + +#include + +#ifdef GGML_METAL_HAS_TENSOR +#include + +#include +#endif + +using namespace metal; + +#define MAX(x, y) ((x) > (y) ? (x) : (y)) +#define MIN(x, y) ((x) < (y) ? (x) : (y)) +#define SWAP(x, y) { auto tmp = (x); (x) = (y); (y) = tmp; } + +#define PAD2(x, n) (((x) + (n) - 1) & ~((n) - 1)) + +#define FOR_UNROLL(x) _Pragma("clang loop unroll(full)") for (x) + +#define N_SIMDWIDTH 32 // assuming SIMD group size is 32 + +// ref: https://developer.apple.com/metal/Metal-Shading-Language-Specification.pdf +// +// cmd: +// .../usr/bin/metal -dM -E -c ggml/src/ggml-metal/ggml-metal.metal +// .../usr/bin/metal -dM -E -c -target air64-apple-ios14.0 ggml/src/ggml-metal/ggml-metal.metal +// +#if __METAL_VERSION__ < 310 && defined(GGML_METAL_HAS_BF16) +#undef GGML_METAL_HAS_BF16 +#endif + +#if defined(GGML_METAL_HAS_BF16) +typedef matrix bfloat4x4; +typedef matrix bfloat2x4; +#endif + +constexpr constant static float kvalues_iq4nl_f[16] = { + -127.f, -104.f, -83.f, -65.f, -49.f, -35.f, -22.f, -10.f, 1.f, 13.f, 25.f, 38.f, 53.f, 69.f, 89.f, 113.f +}; + +constexpr constant static float kvalues_mxfp4_f[16] = { + 0, .5f, 1.f, 1.5f, 2.f, 3.f, 4.f, 6.f, -0, -.5f, -1.f, -1.5f, -2.f, -3.f, -4.f, -6.f +}; + +static inline int best_index_int8(int n, constant float * val, float x) { + if (x <= val[0]) return 0; + if (x >= val[n-1]) return n-1; + int ml = 0, mu = n-1; + while (mu-ml > 1) { + int mav = (ml+mu)/2; + if (x < val[mav]) mu = mav; else ml = mav; + } + return x - val[mu-1] < val[mu] - x ? mu-1 : mu; +} + +static inline float e8m0_to_fp32(uint8_t x) { + uint32_t bits; + + if (x == 0) { + bits = 0x00400000; + } else { + bits = (uint32_t) x << 23; + } + + return as_type(bits); +} + +static inline float dot(float x, float y) { + return x*y; +} + +static inline float sum(float x) { + return x; +} + +static inline float sum(float4 x) { + return x[0] + x[1] + x[2] + x[3]; +} + +// NOTE: this is not dequantizing - we are simply fitting the template +template +void dequantize_f32(device const float4x4 * src, short il, thread type4x4 & reg) { + reg = (type4x4)(*src); +} + +template +void dequantize_f32_t4(device const float4 * src, short il, thread type4 & reg) { + reg = (type4)(*src); +} + +template +void dequantize_f16(device const half4x4 * src, short il, thread type4x4 & reg) { + reg = (type4x4)(*src); +} + +template +void dequantize_f16_t4(device const half4 * src, short il, thread type4 & reg) { + reg = (type4)(*(src)); +} + +#if defined(GGML_METAL_HAS_BF16) +template +void dequantize_bf16(device const bfloat4x4 * src, short il, thread type4x4 & reg) { + reg = (type4x4)(*src); +} + +template +void dequantize_bf16_t4(device const bfloat4 * src, short il, thread type4 & reg) { + reg = (type4)(*(src)); +} +#endif + +template +void dequantize_q4_0(device const block_q4_0 * xb, short il, thread type4x4 & reg) { + device const uint16_t * qs = ((device const uint16_t *)xb + 1); + const float d1 = il ? (xb->d / 16.h) : xb->d; + const float d2 = d1 / 256.f; + const float md = -8.h * xb->d; + const ushort mask0 = il ? 0x00F0 : 0x000F; + const ushort mask1 = mask0 << 8; + + float4x4 reg_f; + + for (int i = 0; i < 8; i++) { + reg_f[i/2][2*(i%2) + 0] = d1 * (qs[i] & mask0) + md; + reg_f[i/2][2*(i%2) + 1] = d2 * (qs[i] & mask1) + md; + } + + reg = (type4x4) reg_f; +} + +template +void dequantize_q4_0_t4(device const block_q4_0 * xb, short il, thread type4 & reg) { + device const uint16_t * qs = ((device const uint16_t *)xb + 1); + const float d1 = (il/4) ? (xb->d / 16.h) : xb->d; + const float d2 = d1 / 256.f; + const float md = -8.h * xb->d; + const ushort mask0 = (il/4) ? 0x00F0 : 0x000F; + const ushort mask1 = mask0 << 8; + + for (int i = 0; i < 2; i++) { + reg[2*i + 0] = d1 * (qs[2*(il%4) + i] & mask0) + md; + reg[2*i + 1] = d2 * (qs[2*(il%4) + i] & mask1) + md; + } +} + +void quantize_q4_0(device const float * src, device block_q4_0 & dst) { +#pragma METAL fp math_mode(safe) + float amax = 0.0f; // absolute max + float max = 0.0f; + + for (int j = 0; j < QK4_0; j++) { + const float v = src[j]; + if (amax < fabs(v)) { + amax = fabs(v); + max = v; + } + } + + const float d = max / -8; + const float id = d ? 1.0f/d : 0.0f; + + dst.d = d; + + for (int j = 0; j < QK4_0/2; ++j) { + const float x0 = src[0 + j]*id; + const float x1 = src[QK4_0/2 + j]*id; + + const uint8_t xi0 = MIN(15, (int8_t)(x0 + 8.5f)); + const uint8_t xi1 = MIN(15, (int8_t)(x1 + 8.5f)); + + dst.qs[j] = xi0; + dst.qs[j] |= xi1 << 4; + } +} + +void quantize_q4_1(device const float * src, device block_q4_1 & dst) { +#pragma METAL fp math_mode(safe) + float min = FLT_MAX; + float max = -FLT_MAX; + + for (int j = 0; j < QK4_1; j++) { + const float v = src[j]; + if (min > v) min = v; + if (max < v) max = v; + } + + const float d = (max - min) / ((1 << 4) - 1); + const float id = d ? 1.0f/d : 0.0f; + + dst.d = d; + dst.m = min; + + for (int j = 0; j < QK4_1/2; ++j) { + const float x0 = (src[0 + j] - min)*id; + const float x1 = (src[QK4_1/2 + j] - min)*id; + + const uint8_t xi0 = MIN(15, (int8_t)(x0 + 0.5f)); + const uint8_t xi1 = MIN(15, (int8_t)(x1 + 0.5f)); + + dst.qs[j] = xi0; + dst.qs[j] |= xi1 << 4; + } +} + +void quantize_q5_0(device const float * src, device block_q5_0 & dst) { +#pragma METAL fp math_mode(safe) + float amax = 0.0f; // absolute max + float max = 0.0f; + + for (int j = 0; j < QK5_0; j++) { + const float v = src[j]; + if (amax < fabs(v)) { + amax = fabs(v); + max = v; + } + } + + const float d = max / -16; + const float id = d ? 1.0f/d : 0.0f; + + dst.d = d; + + uint32_t qh = 0; + for (int j = 0; j < QK5_0/2; ++j) { + const float x0 = src[0 + j]*id; + const float x1 = src[QK5_0/2 + j]*id; + + const uint8_t xi0 = MIN(31, (int8_t)(x0 + 16.5f)); + const uint8_t xi1 = MIN(31, (int8_t)(x1 + 16.5f)); + + dst.qs[j] = (xi0 & 0xf) | ((xi1 & 0xf) << 4); + qh |= ((xi0 & 0x10u) >> 4) << (j + 0); + qh |= ((xi1 & 0x10u) >> 4) << (j + QK5_0/2); + } + + thread const uint8_t * qh8 = (thread const uint8_t *)&qh; + + for (int j = 0; j < 4; ++j) { + dst.qh[j] = qh8[j]; + } +} + +void quantize_q5_1(device const float * src, device block_q5_1 & dst) { +#pragma METAL fp math_mode(safe) + float max = src[0]; + float min = src[0]; + + for (int j = 1; j < QK5_1; j++) { + const float v = src[j]; + min = v < min ? v : min; + max = v > max ? v : max; + } + + const float d = (max - min) / 31; + const float id = d ? 1.0f/d : 0.0f; + + dst.d = d; + dst.m = min; + + uint32_t qh = 0; + for (int j = 0; j < QK5_1/2; ++j) { + const float x0 = (src[0 + j] - min)*id; + const float x1 = (src[QK5_1/2 + j] - min)*id; + + const uint8_t xi0 = (uint8_t)(x0 + 0.5f); + const uint8_t xi1 = (uint8_t)(x1 + 0.5f); + + dst.qs[j] = (xi0 & 0xf) | ((xi1 & 0xf) << 4); + qh |= ((xi0 & 0x10u) >> 4) << (j + 0); + qh |= ((xi1 & 0x10u) >> 4) << (j + QK5_1/2); + } + + thread const uint8_t * qh8 = (thread const uint8_t *)&qh; + + for (int j = 0; j < 4; ++j) { + dst.qh[j] = qh8[j]; + } +} + +void quantize_q8_0(device const float * src, device block_q8_0 & dst) { +#pragma METAL fp math_mode(safe) + float amax = 0.0f; // absolute max + + for (int j = 0; j < QK8_0; j++) { + const float v = src[j]; + amax = MAX(amax, fabs(v)); + } + + const float d = amax / ((1 << 7) - 1); + const float id = d ? 1.0f/d : 0.0f; + + dst.d = d; + + for (int j = 0; j < QK8_0; ++j) { + const float x0 = src[j]*id; + + dst.qs[j] = round(x0); + } +} + +void quantize_iq4_nl(device const float * src, device block_iq4_nl & dst) { +#pragma METAL fp math_mode(safe) + float amax = 0.0f; // absolute max + float max = 0.0f; + + for (int j = 0; j < QK4_NL; j++) { + const float v = src[j]; + if (amax < fabs(v)) { + amax = fabs(v); + max = v; + } + } + + const float d = max / kvalues_iq4nl_f[0]; + const float id = d ? 1.0f/d : 0.0f; + + float sumqx = 0, sumq2 = 0; + for (int j = 0; j < QK4_NL/2; ++j) { + const float x0 = src[0 + j]*id; + const float x1 = src[QK4_NL/2 + j]*id; + + const uint8_t xi0 = best_index_int8(16, kvalues_iq4nl_f, x0); + const uint8_t xi1 = best_index_int8(16, kvalues_iq4nl_f, x1); + + dst.qs[j] = xi0 | (xi1 << 4); + + const float v0 = kvalues_iq4nl_f[xi0]; + const float v1 = kvalues_iq4nl_f[xi1]; + const float w0 = src[0 + j]*src[0 + j]; + const float w1 = src[QK4_NL/2 + j]*src[QK4_NL/2 + j]; + sumqx += w0*v0*src[j] + w1*v1*src[QK4_NL/2 + j]; + sumq2 += w0*v0*v0 + w1*v1*v1; + + } + + dst.d = sumq2 > 0 ? sumqx/sumq2 : d; +} + +template +void dequantize_q4_1(device const block_q4_1 * xb, short il, thread type4x4 & reg) { + device const uint16_t * qs = ((device const uint16_t *)xb + 2); + const float d1 = il ? (xb->d / 16.h) : xb->d; + const float d2 = d1 / 256.f; + const float m = xb->m; + const ushort mask0 = il ? 0x00F0 : 0x000F; + const ushort mask1 = mask0 << 8; + + float4x4 reg_f; + + for (int i = 0; i < 8; i++) { + reg_f[i/2][2*(i%2) + 0] = ((qs[i] & mask0) * d1) + m; + reg_f[i/2][2*(i%2) + 1] = ((qs[i] & mask1) * d2) + m; + } + + reg = (type4x4) reg_f; +} + +template +void dequantize_q4_1_t4(device const block_q4_1 * xb, short il, thread type4 & reg) { + device const uint16_t * qs = ((device const uint16_t *)xb + 2); + const float d1 = (il/4) ? (xb->d / 16.h) : xb->d; + const float d2 = d1 / 256.f; + const float m = xb->m; + const ushort mask0 = (il/4) ? 0x00F0 : 0x000F; + const ushort mask1 = mask0 << 8; + + for (int i = 0; i < 2; i++) { + reg[2*i + 0] = d1 * (qs[2*(il%4) + i] & mask0) + m; + reg[2*i + 1] = d2 * (qs[2*(il%4) + i] & mask1) + m; + } +} + +template +void dequantize_q5_0(device const block_q5_0 * xb, short il, thread type4x4 & reg) { + device const uint16_t * qs = ((device const uint16_t *)xb + 3); + const float d = xb->d; + const float md = -16.h * xb->d; + const ushort mask = il ? 0x00F0 : 0x000F; + + const uint32_t qh = *((device const uint32_t *)xb->qh); + + const int x_mv = il ? 4 : 0; + + const int gh_mv = il ? 12 : 0; + const int gh_bk = il ? 0 : 4; + + float4x4 reg_f; + + for (int i = 0; i < 8; i++) { + // extract the 5-th bits for x0 and x1 + const uint8_t xh_0 = ((qh >> (gh_mv + 2*i )) << gh_bk) & 0x10; + const uint8_t xh_1 = ((qh >> (gh_mv + 2*i+1)) << gh_bk) & 0x10; + + // combine the 4-bits from qs with the 5th bit + const int32_t x0 = ((((qs[i] ) & mask) >> x_mv) | xh_0); + const int32_t x1 = ((((qs[i] >> 8) & mask) >> x_mv) | xh_1); + + reg_f[i/2][2*(i%2) + 0] = d * x0 + md; + reg_f[i/2][2*(i%2) + 1] = d * x1 + md; + } + + reg = (type4x4) reg_f; +} + +template +void dequantize_q5_0_t4(device const block_q5_0 * xb, short il, thread type4 & reg) { + device const uint16_t * qs = ((device const uint16_t *)xb + 3); + const float d = xb->d; + const float md = -16.h * xb->d; + const ushort mask = (il/4) ? 0x00F0 : 0x000F; + + const uint32_t qh = *((device const uint32_t *)xb->qh); + + const int x_mv = (il/4) ? 4 : 0; + + const int gh_mv = (il/4) ? 12 : 0; + const int gh_bk = (il/4) ? 0 : 4; + + for (int ii = 0; ii < 2; ii++) { + int i = 2*(il%4) + ii; + + // extract the 5-th bits for x0 and x1 + const uint8_t xh_0 = ((qh >> (gh_mv + 2*i )) << gh_bk) & 0x10; + const uint8_t xh_1 = ((qh >> (gh_mv + 2*i+1)) << gh_bk) & 0x10; + + // combine the 4-bits from qs with the 5th bit + const int32_t x0 = ((((qs[i] ) & mask) >> x_mv) | xh_0); + const int32_t x1 = ((((qs[i] >> 8) & mask) >> x_mv) | xh_1); + + reg[2*ii + 0] = d * x0 + md; + reg[2*ii + 1] = d * x1 + md; + } +} + +template +void dequantize_q5_1(device const block_q5_1 * xb, short il, thread type4x4 & reg) { + device const uint16_t * qs = ((device const uint16_t *)xb + 4); + const float d = xb->d; + const float m = xb->m; + const ushort mask = il ? 0x00F0 : 0x000F; + + const uint32_t qh = *((device const uint32_t *)xb->qh); + + const int x_mv = il ? 4 : 0; + + const int gh_mv = il ? 12 : 0; + const int gh_bk = il ? 0 : 4; + + float4x4 reg_f; + + for (int i = 0; i < 8; i++) { + // extract the 5-th bits for x0 and x1 + const uint8_t xh_0 = ((qh >> (gh_mv + 2*i )) << gh_bk) & 0x10; + const uint8_t xh_1 = ((qh >> (gh_mv + 2*i+1)) << gh_bk) & 0x10; + + // combine the 4-bits from qs with the 5th bit + const int32_t x0 = ((((qs[i] ) & mask) >> x_mv) | xh_0); + const int32_t x1 = ((((qs[i] >> 8) & mask) >> x_mv) | xh_1); + + reg_f[i/2][2*(i%2) + 0] = d * x0 + m; + reg_f[i/2][2*(i%2) + 1] = d * x1 + m; + } + + reg = (type4x4) reg_f; +} + +template +void dequantize_q5_1_t4(device const block_q5_1 * xb, short il, thread type4 & reg) { + device const uint16_t * qs = ((device const uint16_t *)xb + 4); + const float d = xb->d; + const float m = xb->m; + const ushort mask = (il/4) ? 0x00F0 : 0x000F; + + const uint32_t qh = *((device const uint32_t *)xb->qh); + + const int x_mv = (il/4) ? 4 : 0; + + const int gh_mv = (il/4) ? 12 : 0; + const int gh_bk = (il/4) ? 0 : 4; + + for (int ii = 0; ii < 2; ii++) { + int i = 2*(il%4) + ii; + + // extract the 5-th bits for x0 and x1 + const uint8_t xh_0 = ((qh >> (gh_mv + 2*i )) << gh_bk) & 0x10; + const uint8_t xh_1 = ((qh >> (gh_mv + 2*i+1)) << gh_bk) & 0x10; + + // combine the 4-bits from qs with the 5th bit + const int32_t x0 = ((((qs[i] ) & mask) >> x_mv) | xh_0); + const int32_t x1 = ((((qs[i] >> 8) & mask) >> x_mv) | xh_1); + + reg[2*ii + 0] = d * x0 + m; + reg[2*ii + 1] = d * x1 + m; + } +} + +template +void dequantize_q8_0(device const block_q8_0 *xb, short il, thread type4x4 & reg) { + device const int8_t * qs = ((device const int8_t *)xb->qs); + const float d = xb->d; + + float4x4 reg_f; + + for (int i = 0; i < 16; i++) { + reg_f[i/4][i%4] = (qs[i + 16*il] * d); + } + + reg = (type4x4) reg_f; +} + +template +void dequantize_q8_0_t4(device const block_q8_0 *xb, short il, thread type4 & reg) { + device const int8_t * qs = ((device const int8_t *)xb->qs); + const float d = xb->d; + + for (int i = 0; i < 4; i++) { + reg[i] = (qs[4*(il%4) + i + 16*(il/4)] * d); + } +} + +template +void dequantize_mxfp4(device const block_mxfp4 * xb, short il, thread type4x4 & reg) { + device const uint8_t * q2 = (device const uint8_t *)xb->qs; + + const float d = e8m0_to_fp32(xb->e); + const uint8_t shr = il >= 1 ? 4 : 0; + + for (int i = 0; i < 4; ++i) { + reg[i][0] = d * kvalues_mxfp4_f[(q2[4*i + 0] >> shr) & 0x0F]; + reg[i][1] = d * kvalues_mxfp4_f[(q2[4*i + 1] >> shr) & 0x0F]; + reg[i][2] = d * kvalues_mxfp4_f[(q2[4*i + 2] >> shr) & 0x0F]; + reg[i][3] = d * kvalues_mxfp4_f[(q2[4*i + 3] >> shr) & 0x0F]; + } +} + +template +void dequantize_mxfp4_t4(device const block_mxfp4 * xb, short il, thread type4 & reg) { + device const uint8_t * q2 = (device const uint8_t *)xb->qs; + + const float d = e8m0_to_fp32(xb->e); + const short il4 = il%4; + + const uint8_t shr = il >= 4 ? 4 : 0; + + reg[0] = d * kvalues_mxfp4_f[(q2[4*il4 + 0] >> shr) & 0x0F]; + reg[1] = d * kvalues_mxfp4_f[(q2[4*il4 + 1] >> shr) & 0x0F]; + reg[2] = d * kvalues_mxfp4_f[(q2[4*il4 + 2] >> shr) & 0x0F]; + reg[3] = d * kvalues_mxfp4_f[(q2[4*il4 + 3] >> shr) & 0x0F]; +} + +template +void dequantize_q2_K(device const block_q2_K *xb, short il, thread type4x4 & reg) { + const float d = xb->d; + const float min = xb->dmin; + device const uint8_t * q = (device const uint8_t *)xb->qs; + float dl, ml; + uint8_t sc = xb->scales[il]; + + q = q + 32*(il/8) + 16*(il&1); + il = (il/2)%4; + + half coef = il>1 ? (il>2 ? 1/64.h : 1/16.h) : (il>0 ? 1/4.h : 1.h); + uchar mask = il>1 ? (il>2 ? 192 : 48) : (il>0 ? 12 : 3); + dl = d * (sc & 0xF) * coef, ml = min * (sc >> 4); + for (int i = 0; i < 16; ++i) { + reg[i/4][i%4] = dl * (q[i] & mask) - ml; + } +} + +template +void dequantize_q3_K(device const block_q3_K *xb, short il, thread type4x4 & reg) { + const half d_all = xb->d; + device const uint8_t * q = (device const uint8_t *)xb->qs; + device const uint8_t * h = (device const uint8_t *)xb->hmask; + device const int8_t * scales = (device const int8_t *)xb->scales; + + q = q + 32 * (il/8) + 16 * (il&1); + h = h + 16 * (il&1); + uint8_t m = 1 << (il/2); + uint16_t kmask1 = (il/4)>1 ? ((il/4)>2 ? 192 : 48) : \ + ((il/4)>0 ? 12 : 3); + uint16_t kmask2 = il/8 ? 0xF0 : 0x0F; + uint16_t scale_2 = scales[il%8], scale_1 = scales[8 + il%4]; + int16_t dl_int = (il/4)&1 ? (scale_2&kmask2) | ((scale_1&kmask1) << 2) + : (scale_2&kmask2) | ((scale_1&kmask1) << 4); + float dl = il<8 ? d_all * (dl_int - 32.f) : d_all * (dl_int / 16.f - 32.f); + const float ml = 4.f * dl; + + il = (il/2) & 3; + const half coef = il>1 ? (il>2 ? 1/64.h : 1/16.h) : (il>0 ? 1/4.h : 1.h); + const uint8_t mask = il>1 ? (il>2 ? 192 : 48) : (il>0 ? 12 : 3); + dl *= coef; + + for (int i = 0; i < 16; ++i) { + reg[i/4][i%4] = dl * (q[i] & mask) - (h[i] & m ? 0 : ml); + } +} + +static inline uchar2 get_scale_min_k4_just2(int j, int k, device const uchar * q) { + return j < 4 ? uchar2{uchar(q[j+0+k] & 63), uchar(q[j+4+k] & 63)} + : uchar2{uchar((q[j+4+k] & 0xF) | ((q[j-4+k] & 0xc0) >> 2)), uchar((q[j+4+k] >> 4) | ((q[j-0+k] & 0xc0) >> 2))}; +} + +template +void dequantize_q4_K(device const block_q4_K * xb, short il, thread type4x4 & reg) { + device const uchar * q = xb->qs; + + short is = (il/4) * 2; + q = q + (il/4) * 32 + 16 * (il&1); + il = il & 3; + const uchar2 sc = get_scale_min_k4_just2(is, il/2, xb->scales); + const float d = il < 2 ? xb->d : xb->d / 16.h; + const float min = xb->dmin; + const float dl = d * sc[0]; + const float ml = min * sc[1]; + + const ushort mask = il < 2 ? 0x0F : 0xF0; + for (int i = 0; i < 16; ++i) { + reg[i/4][i%4] = dl * (q[i] & mask) - ml; + } +} + +template +void dequantize_q5_K(device const block_q5_K *xb, short il, thread type4x4 & reg) { + device const uint8_t * q = xb->qs; + device const uint8_t * qh = xb->qh; + + short is = (il/4) * 2; + q = q + 32 * (il/4) + 16 * (il&1); + qh = qh + 16 * (il&1); + uint8_t ul = 1 << (il/2); + il = il & 3; + const uchar2 sc = get_scale_min_k4_just2(is, il/2, xb->scales); + const float d = il < 2 ? xb->d : xb->d / 16.f; + const float min = xb->dmin; + const float dl = d * sc[0]; + const float ml = min * sc[1]; + + const ushort mask = il<2 ? 0x0F : 0xF0; + const float qh_val = il<2 ? 16.f : 256.f; + for (int i = 0; i < 16; ++i) { + reg[i/4][i%4] = dl * ((q[i] & mask) + (qh[i] & ul ? qh_val : 0)) - ml; + } +} + +template +void dequantize_q6_K(device const block_q6_K *xb, short il, thread type4x4 & reg) { + const half d_all = xb->d; + device const uint16_t * ql = (device const uint16_t *)xb->ql; + device const uint16_t * qh = (device const uint16_t *)xb->qh; + device const int8_t * scales = (device const int8_t *)xb->scales; + + ql = ql + 32*(il/8) + 16*((il/2)&1) + 8*(il&1); + qh = qh + 16*(il/8) + 8*(il&1); + float sc = scales[(il%2) + 2 * ((il/2))]; + il = (il/2) & 3; + + const uint32_t kmask1 = il>1 ? (il>2 ? 0xC0C0C0C0 : 0x30303030) : (il>0 ? 0x0C0C0C0C : 0x03030303); + const uint32_t kmask2 = il>1 ? 0xF0F0F0F0 : 0x0F0F0F0F; + const float ml = d_all * sc * 32.f; + const float dl0 = d_all * sc; + const float dl1 = dl0 / 256.f; + const float dl2 = dl0 / (256.f * 256.f); + const float dl3 = dl0 / (256.f * 256.f * 256.f); + const uint8_t shr_h = il>2 ? 2 : 0; + const uint8_t shl_h = il>1 ? 0 : (il>0 ? 2 : 4); + const uint8_t shr_l = il>1 ? 4 : 0; + for (int i = 0; i < 4; ++i) { + const uint32_t low = (ql[2*i] | (uint32_t)(ql[2*i+1] << 16)) & kmask2; + const uint32_t high = (qh[2*i] | (uint32_t)(qh[2*i+1] << 16)) & kmask1; + const uint32_t q = ((high << shl_h) >> shr_h) | (low >> shr_l); + reg[i][0] = dl0 * ((half)(q & 0xFF)) - ml; + reg[i][1] = dl1 * ((float)(q & 0xFF00)) - ml; + reg[i][2] = dl2 * ((float)(q & 0xFF0000)) - ml; + reg[i][3] = dl3 * ((float)(q & 0xFF000000)) - ml; + } +} + +template +void dequantize_iq2_xxs(device const block_iq2_xxs * xb, short il, thread type4x4 & reg) { + // il is 0...15 for QK_K = 256 => index of block of 32 is il/2 + const float d = xb->d; + const int ib32 = il/2; + il = il%2; + // il = 0 or 1. il = 0 processes the first 16 quants in a block of 32, il = 1 the second 16 + // each block of 32 needs 2 uint32_t's for the quants & scale, so 4 uint16_t's. + device const uint16_t * q2 = xb->qs + 4*ib32; + const uint32_t aux32_g = q2[0] | (q2[1] << 16); + const uint32_t aux32_s = q2[2] | (q2[3] << 16); + thread const uint8_t * aux8 = (thread const uint8_t *)&aux32_g; + const float dl = d * (0.5f + (aux32_s >> 28)) * 0.25f; + constant uint8_t * grid = (constant uint8_t *)(iq2xxs_grid + aux8[2*il+0]); + uint8_t signs = ksigns_iq2xs[(aux32_s >> 14*il) & 127]; + for (int i = 0; i < 8; ++i) { + reg[i/4][i%4] = dl * grid[i] * (signs & kmask_iq2xs[i] ? -1.f : 1.f); + } + grid = (constant uint8_t *)(iq2xxs_grid + aux8[2*il+1]); + signs = ksigns_iq2xs[(aux32_s >> (14*il+7)) & 127]; + for (int i = 0; i < 8; ++i) { + reg[2+i/4][i%4] = dl * grid[i] * (signs & kmask_iq2xs[i] ? -1.f : 1.f); + } +} + +template +void dequantize_iq2_xs(device const block_iq2_xs * xb, short il, thread type4x4 & reg) { + // il is 0...15 for QK_K = 256 => index of block of 32 is il/2 + const float d = xb->d; + const int ib32 = il/2; + il = il%2; + // il = 0 or 1. il = 0 processes the first 16 quants in a block of 32, il = 1 the second 16 + device const uint16_t * q2 = xb->qs + 4*ib32; + const float dl = d * (0.5f + ((xb->scales[ib32] >> 4*il) & 0xf)) * 0.25f; + constant uint8_t * grid = (constant uint8_t *)(iq2xs_grid + (q2[2*il+0] & 511)); + uint8_t signs = ksigns_iq2xs[q2[2*il+0] >> 9]; + for (int i = 0; i < 8; ++i) { + reg[i/4][i%4] = dl * grid[i] * (signs & kmask_iq2xs[i] ? -1.f : 1.f); + } + grid = (constant uint8_t *)(iq2xs_grid + (q2[2*il+1] & 511)); + signs = ksigns_iq2xs[q2[2*il+1] >> 9]; + for (int i = 0; i < 8; ++i) { + reg[2+i/4][i%4] = dl * grid[i] * (signs & kmask_iq2xs[i] ? -1.f : 1.f); + } +} + +template +void dequantize_iq3_xxs(device const block_iq3_xxs * xb, short il, thread type4x4 & reg) { + // il is 0...15 for QK_K = 256 => index of block of 32 is il/2 + const float d = xb->d; + const int ib32 = il/2; + il = il%2; + // il = 0 or 1. il = 0 processes the first 16 quants in a block of 32, il = 1 the second 16 + device const uint8_t * q3 = xb->qs + 8*ib32; + device const uint16_t * gas = (device const uint16_t *)(xb->qs + QK_K/4) + 2*ib32; + const uint32_t aux32 = gas[0] | (gas[1] << 16); + const float dl = d * (0.5f + (aux32 >> 28)) * 0.5f; + constant uint8_t * grid1 = (constant uint8_t *)(iq3xxs_grid + q3[4*il+0]); + constant uint8_t * grid2 = (constant uint8_t *)(iq3xxs_grid + q3[4*il+1]); + uint8_t signs = ksigns_iq2xs[(aux32 >> 14*il) & 127]; + for (int i = 0; i < 4; ++i) { + reg[0][i] = dl * grid1[i] * (signs & kmask_iq2xs[i+0] ? -1.f : 1.f); + reg[1][i] = dl * grid2[i] * (signs & kmask_iq2xs[i+4] ? -1.f : 1.f); + } + grid1 = (constant uint8_t *)(iq3xxs_grid + q3[4*il+2]); + grid2 = (constant uint8_t *)(iq3xxs_grid + q3[4*il+3]); + signs = ksigns_iq2xs[(aux32 >> (14*il+7)) & 127]; + for (int i = 0; i < 4; ++i) { + reg[2][i] = dl * grid1[i] * (signs & kmask_iq2xs[i+0] ? -1.f : 1.f); + reg[3][i] = dl * grid2[i] * (signs & kmask_iq2xs[i+4] ? -1.f : 1.f); + } +} + +template +void dequantize_iq3_s(device const block_iq3_s * xb, short il, thread type4x4 & reg) { + // il is 0...15 for QK_K = 256 => index of block of 32 is il/2 + const float d = xb->d; + const int ib32 = il/2; + il = il%2; + // il = 0 or 1. il = 0 processes the first 16 quants in a block of 32, il = 1 the second 16 + device const uint8_t * qs = xb->qs + 8*ib32; + device const uint8_t * signs = xb->signs + 4*ib32 + 2*il; + const uint8_t qh = xb->qh[ib32] >> 4*il; + const float dl = d * (1 + 2*((xb->scales[ib32/2] >> 4*(ib32%2)) & 0xf)); + constant uint8_t * grid1 = (constant uint8_t *)(iq3s_grid + (qs[4*il+0] | ((qh << 8) & 256))); + constant uint8_t * grid2 = (constant uint8_t *)(iq3s_grid + (qs[4*il+1] | ((qh << 7) & 256))); + for (int i = 0; i < 4; ++i) { + reg[0][i] = dl * grid1[i] * select(1, -1, signs[0] & kmask_iq2xs[i+0]); + reg[1][i] = dl * grid2[i] * select(1, -1, signs[0] & kmask_iq2xs[i+4]); + } + grid1 = (constant uint8_t *)(iq3s_grid + (qs[4*il+2] | ((qh << 6) & 256))); + grid2 = (constant uint8_t *)(iq3s_grid + (qs[4*il+3] | ((qh << 5) & 256))); + for (int i = 0; i < 4; ++i) { + reg[2][i] = dl * grid1[i] * select(1, -1, signs[1] & kmask_iq2xs[i+0]); + reg[3][i] = dl * grid2[i] * select(1, -1, signs[1] & kmask_iq2xs[i+4]); + } +} + +template +void dequantize_iq2_s(device const block_iq2_s * xb, short il, thread type4x4 & reg) { + // il is 0...15 for QK_K = 256 => index of block of 32 is il/2 + const float d = xb->d; + const int ib32 = il/2; + il = il%2; + // il = 0 or 1. il = 0 processes the first 16 quants in a block of 32, il = 1 the second 16 + device const uint8_t * qs = xb->qs + 4*ib32 + 2*il; + device const uint8_t * signs = qs + QK_K/8; + const uint8_t qh = xb->qh[ib32] >> 4*il; + const float dl = d * (0.5f + ((xb->scales[ib32] >> 4*il) & 0xf)) * 0.25f; + constant uint8_t * grid1 = (constant uint8_t *)(iq2s_grid + (qs[0] | ((qh << 8) & 0x300))); + constant uint8_t * grid2 = (constant uint8_t *)(iq2s_grid + (qs[1] | ((qh << 6) & 0x300))); + for (int i = 0; i < 8; ++i) { + reg[i/4+0][i%4] = dl * grid1[i] * select(1, -1, signs[0] & kmask_iq2xs[i]); + reg[i/4+2][i%4] = dl * grid2[i] * select(1, -1, signs[1] & kmask_iq2xs[i]); + } +} + +template +void dequantize_iq1_s(device const block_iq1_s * xb, short il, thread type4x4 & reg) { + // il is 0...15 for QK_K = 256 => index of block of 32 is il/2 + const int ib32 = il/2; + il = il%2; + const float d = xb->d; + device const uint8_t * qs = xb->qs + 4*ib32 + 2*il; + device const uint16_t * qh = xb->qh; + const float dl = d * (2*((qh[ib32] >> 12) & 7) + 1); + const float ml = dl * (qh[ib32] & 0x8000 ? -1 - IQ1S_DELTA : -1 + IQ1S_DELTA); + const uint16_t h = qh[ib32] >> 6*il; + constant uint8_t * grid1 = (constant uint8_t *)(iq1s_grid_gpu + (qs[0] | ((h << 8) & 0x700))); + constant uint8_t * grid2 = (constant uint8_t *)(iq1s_grid_gpu + (qs[1] | ((h << 5) & 0x700))); + for (int i = 0; i < 4; ++i) { + reg[0][i] = dl * (grid1[i] & 0xf) + ml; + reg[1][i] = dl * (grid1[i] >> 4) + ml; + reg[2][i] = dl * (grid2[i] & 0xf) + ml; + reg[3][i] = dl * (grid2[i] >> 4) + ml; + } +} + +template +void dequantize_iq1_m(device const block_iq1_m * xb, short il, thread type4x4 & reg) { + // il is 0...15 for QK_K = 256 => index of block of 32 is il/2 + const int ib32 = il/2; + il = il%2; + device const uint16_t * sc = (device const uint16_t *)xb->scales; + + iq1m_scale_t scale; + scale.u16 = (sc[0] >> 12) | ((sc[1] >> 8) & 0x00f0) | ((sc[2] >> 4) & 0x0f00) | (sc[3] & 0xf000); + const float d = scale.f16; + + device const uint8_t * qs = xb->qs + 4*ib32 + 2*il; + device const uint8_t * qh = xb->qh + 2*ib32 + il; + + const float dl = d * (2*((sc[ib32/2] >> (6*(ib32%2)+3*il)) & 7) + 1); + const float ml1 = dl * (qh[0] & 0x08 ? -1 - IQ1M_DELTA : -1 + IQ1M_DELTA); + const float ml2 = dl * (qh[0] & 0x80 ? -1 - IQ1M_DELTA : -1 + IQ1M_DELTA); + constant uint8_t * grid1 = (constant uint8_t *)(iq1s_grid_gpu + (qs[0] | ((qh[0] << 8) & 0x700))); + constant uint8_t * grid2 = (constant uint8_t *)(iq1s_grid_gpu + (qs[1] | ((qh[0] << 4) & 0x700))); + for (int i = 0; i < 4; ++i) { + reg[0][i] = dl * (grid1[i] & 0xf) + ml1; + reg[1][i] = dl * (grid1[i] >> 4) + ml1; + reg[2][i] = dl * (grid2[i] & 0xf) + ml2; + reg[3][i] = dl * (grid2[i] >> 4) + ml2; + } +} + +template +void dequantize_iq4_nl(device const block_iq4_nl * xb, short il, thread type4x4 & reg) { + device const uint16_t * q4 = (device const uint16_t *)xb->qs; + const float d = xb->d; + uint32_t aux32; + thread const uint8_t * q8 = (thread const uint8_t *)&aux32; + for (int i = 0; i < 4; ++i) { + aux32 = ((q4[2*i] | (q4[2*i+1] << 16)) >> 4*il) & 0x0f0f0f0f; + reg[i][0] = d * kvalues_iq4nl_f[q8[0]]; + reg[i][1] = d * kvalues_iq4nl_f[q8[1]]; + reg[i][2] = d * kvalues_iq4nl_f[q8[2]]; + reg[i][3] = d * kvalues_iq4nl_f[q8[3]]; + } +} + +template +void dequantize_iq4_nl_t4(device const block_iq4_nl * xb, short il, thread type4 & reg) { + device const uint16_t * q4 = (device const uint16_t *)xb->qs; + const float d = xb->d; + uint32_t aux32; + thread const uint8_t * q8 = (thread const uint8_t *)&aux32; + aux32 = ((q4[2*(il%4)] | (q4[2*(il%4)+1] << 16)) >> 4*(il/4)) & 0x0f0f0f0f; + reg[0] = d * kvalues_iq4nl_f[q8[0]]; + reg[1] = d * kvalues_iq4nl_f[q8[1]]; + reg[2] = d * kvalues_iq4nl_f[q8[2]]; + reg[3] = d * kvalues_iq4nl_f[q8[3]]; +} + +template +void dequantize_iq4_xs(device const block_iq4_xs * xb, short il, thread type4x4 & reg) { + // il is 0...15 for QK_K = 256 => index of block of 32 is il/2 + const int ib32 = il/2; + il = il%2; + // il = 0 or 1. il = 0 processes the first 16 quants in a block of 32, il = 1 the second 16 + device const uint32_t * q4 = (device const uint32_t *)xb->qs + 4*ib32; + const int ls = ((xb->scales_l[ib32/2] >> 4*(ib32%2)) & 0xf) | (((xb->scales_h >> 2*ib32) & 3) << 4); + const float d = (float)xb->d * (ls - 32); + uint32_t aux32; + thread const uint8_t * q8 = (thread const uint8_t *)&aux32; + for (int i = 0; i < 4; ++i) { + aux32 = (q4[i] >> 4*il) & 0x0f0f0f0f; + reg[i][0] = d * kvalues_iq4nl_f[q8[0]]; + reg[i][1] = d * kvalues_iq4nl_f[q8[1]]; + reg[i][2] = d * kvalues_iq4nl_f[q8[2]]; + reg[i][3] = d * kvalues_iq4nl_f[q8[3]]; + } +} + +enum ggml_sort_order { + GGML_SORT_ORDER_ASC, + GGML_SORT_ORDER_DESC, +}; + +constant float GELU_COEF_A = 0.044715f; +constant float GELU_QUICK_COEF = -1.702f; +constant float SQRT_2_OVER_PI = 0.79788456080286535587989211986876f; +constant float SQRT_2_INV = 0.70710678118654752440084436210484f; + +// based on Abramowitz and Stegun formula 7.1.26 or similar Hastings' approximation +// ref: https://www.johndcook.com/blog/python_erf/ +constant float p_erf = 0.3275911f; +constant float a1_erf = 0.254829592f; +constant float a2_erf = -0.284496736f; +constant float a3_erf = 1.421413741f; +constant float a4_erf = -1.453152027f; +constant float a5_erf = 1.061405429f; + +template +inline T erf_approx(T x) { + T sign_x = sign(x); + x = fabs(x); + T t = 1.0f / (1.0f + p_erf * x); + T y = 1.0f - (((((a5_erf * t + a4_erf) * t) + a3_erf) * t + a2_erf) * t + a1_erf) * t * exp(-x * x); + return sign_x * y; +} + +template T elu_approx(T x); + +template<> inline float elu_approx(float x) { + return (x > 0.f) ? x : (exp(x) - 1); +} + +template<> inline float4 elu_approx(float4 x) { + float4 res; + + res[0] = (x[0] > 0.0f) ? x[0] : (exp(x[0]) - 1.0f); + res[1] = (x[1] > 0.0f) ? x[1] : (exp(x[1]) - 1.0f); + res[2] = (x[2] > 0.0f) ? x[2] : (exp(x[2]) - 1.0f); + res[3] = (x[3] > 0.0f) ? x[3] : (exp(x[3]) - 1.0f); + + return res; +} + +constant short FC_unary_op [[function_constant(FC_UNARY + 0)]]; +constant bool FC_unary_cnt[[function_constant(FC_UNARY + 1)]]; + +template +kernel void kernel_unary_impl( + constant ggml_metal_kargs_unary & args, + device const char * src0, + device char * dst, + uint3 tgpig[[threadgroup_position_in_grid]], + ushort3 tpitg[[thread_position_in_threadgroup]], + ushort3 ntg[[threads_per_threadgroup]]) { +#define FC_OP FC_unary_op +#define FC_CNT FC_unary_cnt + + device const T0 * src0_ptr; + device T * dst_ptr; + + int i0; + + if (FC_CNT) { + i0 = tgpig.x; + + src0_ptr = (device const T0 *) (src0); + dst_ptr = (device T *) (dst); + } else { + const int i03 = tgpig.z; + const int i02 = tgpig.y; + const int k0 = tgpig.x/args.ne01; + const int i01 = tgpig.x - k0*args.ne01; + + i0 = k0*ntg.x + tpitg.x; + + src0_ptr = (device const T0 *) (src0 + i03*args.nb03 + i02*args.nb02 + i01*args.nb01); + dst_ptr = (device T *) (dst + i03*args.nb3 + i02*args.nb2 + i01*args.nb1 ); + } + + { + //threadgroup_barrier(mem_flags::mem_none); + + if (!FC_CNT) { + if (i0 >= args.ne0) { + return; + } + } + + const TC x = (TC) src0_ptr[i0]; + + if (FC_OP == OP_UNARY_NUM_SCALE) { + dst_ptr[i0] = (T) (args.scale * x + args.bias); + } + + if (FC_OP == OP_UNARY_NUM_FILL) { + dst_ptr[i0] = (T) args.val; + } + + if (FC_OP == OP_UNARY_NUM_CLAMP) { + dst_ptr[i0] = (T) clamp(x, args.min, args.max); + } + + if (FC_OP == OP_UNARY_NUM_SQR) { + dst_ptr[i0] = (T) (x * x); + } + + if (FC_OP == OP_UNARY_NUM_SQRT) { + dst_ptr[i0] = (T) sqrt(x); + } + + if (FC_OP == OP_UNARY_NUM_SIN) { + dst_ptr[i0] = (T) sin(x); + } + + if (FC_OP == OP_UNARY_NUM_COS) { + dst_ptr[i0] = (T) cos(x); + } + + if (FC_OP == OP_UNARY_NUM_LOG) { + dst_ptr[i0] = (T) log(x); + } + + if (FC_OP == OP_UNARY_NUM_LEAKY_RELU) { + dst_ptr[i0] = (T) (TC(x > 0)*x + TC(x <= 0)*(x * args.slope)); + } + + if (FC_OP == OP_UNARY_NUM_TANH) { + dst_ptr[i0] = (T) precise::tanh(x); + } + + if (FC_OP == OP_UNARY_NUM_RELU) { + dst_ptr[i0] = (T) fmax(0, x); + } + + if (FC_OP == OP_UNARY_NUM_SIGMOID) { + dst_ptr[i0] = (T) (1 / (1 + exp(-x))); + } + + if (FC_OP == OP_UNARY_NUM_GELU) { + dst_ptr[i0] = (T) (0.5*x*(1 + precise::tanh(SQRT_2_OVER_PI*x*(1 + GELU_COEF_A*x*x)))); + } + + if (FC_OP == OP_UNARY_NUM_GELU_ERF) { + dst_ptr[i0] = (T) (0.5*x*(1 + erf_approx(SQRT_2_INV*x))); + } + + if (FC_OP == OP_UNARY_NUM_GELU_QUICK) { + dst_ptr[i0] = (T) (x * (1/(1 + exp(GELU_QUICK_COEF*x)))); + } + + if (FC_OP == OP_UNARY_NUM_SILU) { + dst_ptr[i0] = (T) (x / (1 + exp(-x))); + } + + if (FC_OP == OP_UNARY_NUM_ELU) { + dst_ptr[i0] = (T) elu_approx(x); + } + + if (FC_OP == OP_UNARY_NUM_NEG) { + dst_ptr[i0] = (T) -x; + } + + if (FC_OP == OP_UNARY_NUM_ABS) { + dst_ptr[i0] = (T) fabs(x); + } + + if (FC_OP == OP_UNARY_NUM_SGN) { + dst_ptr[i0] = T(x > 0) - T(x < 0); + } + + if (FC_OP == OP_UNARY_NUM_STEP) { + dst_ptr[i0] = T(x > 0); + } + + if (FC_OP == OP_UNARY_NUM_HARDSWISH) { + dst_ptr[i0] = (T) (x * fmax(0, fmin(1, x/6 + 0.5))); + } + + if (FC_OP == OP_UNARY_NUM_HARDSIGMOID) { + dst_ptr[i0] = (T) fmax(0, fmin(1, x/6 + 0.5)); + } + + if (FC_OP == OP_UNARY_NUM_EXP) { + dst_ptr[i0] = (T) exp(x); + } + + if (FC_OP == OP_UNARY_NUM_SOFTPLUS) { + dst_ptr[i0] = (T) select(log(1 + exp(x)), x, x > 20); + } + + if (FC_OP == OP_UNARY_NUM_EXPM1) { + // TODO: precise implementation + dst_ptr[i0] = (T) (exp(x) - 1); + } + + if (FC_OP == OP_UNARY_NUM_FLOOR) { + dst_ptr[i0] = (T) floor(x); + } + + if (FC_OP == OP_UNARY_NUM_CEIL) { + dst_ptr[i0] = (T) ceil(x); + } + + if (FC_OP == OP_UNARY_NUM_ROUND) { + dst_ptr[i0] = (T) round(x); + } + + if (FC_OP == OP_UNARY_NUM_TRUNC) { + dst_ptr[i0] = (T) trunc(x); + } + } + +#undef FC_OP +#undef FC_CNT +} + +typedef decltype(kernel_unary_impl) kernel_unary_t; + +template [[host_name("kernel_unary_f32_f32")]] kernel kernel_unary_t kernel_unary_impl; +template [[host_name("kernel_unary_f32_f32_4")]] kernel kernel_unary_t kernel_unary_impl; +template [[host_name("kernel_unary_f16_f16")]] kernel kernel_unary_t kernel_unary_impl; +template [[host_name("kernel_unary_f16_f16_4")]] kernel kernel_unary_t kernel_unary_impl; + +// OP: 0 - add, 1 - sub, 2 - mul, 3 - div +constant short FC_bin_op [[function_constant(FC_BIN + 0)]]; +constant short FC_bin_f [[function_constant(FC_BIN + 1)]]; +constant bool FC_bin_rb [[function_constant(FC_BIN + 2)]]; +constant bool FC_bin_cb [[function_constant(FC_BIN + 3)]]; + +template +kernel void kernel_bin_fuse_impl( + constant ggml_metal_kargs_bin & args, + device const char * src0, + device const char * src1, + device char * dst, + uint3 tgpig[[threadgroup_position_in_grid]], + ushort3 tpitg[[thread_position_in_threadgroup]], + ushort3 ntg[[threads_per_threadgroup]]) { +#define FC_OP FC_bin_op +#define FC_F FC_bin_f +#define FC_RB FC_bin_rb +#define FC_CB FC_bin_cb + + if (FC_RB) { + // row broadcast + const uint i0 = tgpig.y*args.ne00 + tgpig.x; + const uint i1 = FC_CB ? tgpig.x%args.ne10 : tgpig.x; + + device const T0 * src0_row = (device const T0 *) (src0); + device T * dst_row = (device T *) (dst); + + if (FC_F == 1) { + device const T1 * src1_row = (device const T1 *) (src1 + args.o1[0]); + + if (FC_OP == 0) { + dst_row[i0] = src0_row[i0] + src1_row[i1]; + } + + if (FC_OP == 1) { + dst_row[i0] = src0_row[i0] - src1_row[i1]; + } + + if (FC_OP == 2) { + dst_row[i0] = src0_row[i0] * src1_row[i1]; + } + + if (FC_OP == 3) { + dst_row[i0] = src0_row[i0] / src1_row[i1]; + } + } else { + T0 res = src0_row[i0]; + + if (FC_OP == 0) { + FOR_UNROLL (short j = 0; j < FC_F; ++j) { + res += ((device const T1 *) (src1 + args.o1[j]))[i1]; + } + } + + if (FC_OP == 1) { + FOR_UNROLL (short j = 0; j < FC_F; ++j) { + res -= ((device const T1 *) (src1 + args.o1[j]))[i1]; + } + } + + if (FC_OP == 2) { + FOR_UNROLL (short j = 0; j < FC_F; ++j) { + res *= ((device const T1 *) (src1 + args.o1[j]))[i1]; + } + } + + if (FC_OP == 3) { + FOR_UNROLL (short j = 0; j < FC_F; ++j) { + res /= ((device const T1 *) (src1 + args.o1[j]))[i1]; + } + } + + dst_row[i0] = res; + } + } else { + const int i03 = tgpig.z; + const int i02 = tgpig.y; + const int i01 = tgpig.x; + + if (i01 >= args.ne01) { + return; + } + + const int i13 = i03%args.ne13; + const int i12 = i02%args.ne12; + const int i11 = i01%args.ne11; + + device const T0 * src0_ptr = (device const T0 *) (src0 + i03*args.nb03 + i02*args.nb02 + i01*args.nb01 + args.offs); + device T * dst_ptr = (device T *) (dst + i03*args.nb3 + i02*args.nb2 + i01*args.nb1 + args.offs); + + if (FC_F == 1) { + device const T1 * src1_ptr = (device const T1 *) (src1 + args.o1[0] + i13*args.nb13 + i12*args.nb12 + i11*args.nb11); + + for (int i0 = tpitg.x; i0 < args.ne0; i0 += ntg.x) { + const int i10 = FC_CB ? i0%args.ne10 : i0; + + if (FC_OP == 0) { + dst_ptr[i0] = src0_ptr[i0] + src1_ptr[i10]; + } + + if (FC_OP == 1) { + dst_ptr[i0] = src0_ptr[i0] - src1_ptr[i10]; + } + + if (FC_OP == 2) { + dst_ptr[i0] = src0_ptr[i0] * src1_ptr[i10]; + } + + if (FC_OP == 3) { + dst_ptr[i0] = src0_ptr[i0] / src1_ptr[i10]; + } + } + } else { + device const T1 * src1_ptr[8]; + FOR_UNROLL (short j = 0; j < FC_F; ++j) { + src1_ptr[j] = (device const T1 *) (src1 + args.o1[j] + i13*args.nb13 + i12*args.nb12 + i11*args.nb11); + } + + for (int i0 = tpitg.x; i0 < args.ne0; i0 += ntg.x) { + const int i10 = FC_CB ? i0%args.ne10 : i0; + + T res = src0_ptr[i0]; + + if (FC_OP == 0) { + FOR_UNROLL (short j = 0; j < FC_F; ++j) { + res += src1_ptr[j][i10]; + } + } + + if (FC_OP == 1) { + FOR_UNROLL (short j = 0; j < FC_F; ++j) { + res -= src1_ptr[j][i10]; + } + } + + if (FC_OP == 2) { + FOR_UNROLL (short j = 0; j < FC_F; ++j) { + res *= src1_ptr[j][i10]; + } + } + + if (FC_OP == 3) { + FOR_UNROLL (short j = 0; j < FC_F; ++j) { + res /= src1_ptr[j][i10]; + } + } + + dst_ptr[i0] = res; + } + } + } + +#undef FC_OP +#undef FC_F +#undef FC_RB +#undef FC_CB +} + +typedef decltype(kernel_bin_fuse_impl) kernel_bin_fuse_t; + +template [[host_name("kernel_bin_fuse_f32_f32_f32")]] kernel kernel_bin_fuse_t kernel_bin_fuse_impl; +template [[host_name("kernel_bin_fuse_f32_f32_f32_4")]] kernel kernel_bin_fuse_t kernel_bin_fuse_impl; + +kernel void kernel_add_id( + constant ggml_metal_kargs_add_id & args, + device const char * src0, + device const char * src1, + device const char * src2, + device char * dst, + uint3 tgpig[[threadgroup_position_in_grid]], + ushort3 tpitg[[thread_position_in_threadgroup]], + ushort3 ntg[[threads_per_threadgroup]]) { + const int i1 = tgpig.x; + const int i2 = tgpig.y; + + const int i11 = *((device const int32_t *) (src2 + i1*sizeof(int32_t) + i2*args.nb21)); + + const size_t nb1 = args.ne0 * sizeof(float); + const size_t nb2 = args.ne1 * nb1; + + device float * dst_row = (device float *)((device char *)dst + i1*nb1 + i2*nb2); + device const float * src0_row = (device const float *)((device char *)src0 + i1*args.nb01 + i2*args.nb02); + device const float * src1_row = (device const float *)((device char *)src1 + i11*args.nb11); + + for (int i0 = tpitg.x; i0 < args.ne0; i0 += ntg.x) { + dst_row[i0] = src0_row[i0] + src1_row[i0]; + } +} + +template +kernel void kernel_repeat( + constant ggml_metal_kargs_repeat & args, + device const char * src0, + device char * dst, + uint3 tgpig[[threadgroup_position_in_grid]], + ushort3 tpitg[[thread_position_in_threadgroup]], + ushort3 ntg[[threads_per_threadgroup]]) { + const int i3 = tgpig.z; + const int i2 = tgpig.y; + const int i1 = tgpig.x; + + const int i03 = i3%args.ne03; + const int i02 = i2%args.ne02; + const int i01 = i1%args.ne01; + + device const char * src0_ptr = src0 + i03*args.nb03 + i02*args.nb02 + i01*args.nb01; + device char * dst_ptr = dst + i3*args.nb3 + i2*args.nb2 + i1*args.nb1; + + for (int i0 = tpitg.x; i0 < args.ne0; i0 += ntg.x) { + const int i00 = i0%args.ne00; + *((device T *)(dst_ptr + i0*args.nb0)) = *((device T *)(src0_ptr + i00*args.nb00)); + } +} + +typedef decltype(kernel_repeat) kernel_repeat_t; + +template [[host_name("kernel_repeat_f32")]] kernel kernel_repeat_t kernel_repeat; +template [[host_name("kernel_repeat_f16")]] kernel kernel_repeat_t kernel_repeat; +template [[host_name("kernel_repeat_i32")]] kernel kernel_repeat_t kernel_repeat; +template [[host_name("kernel_repeat_i16")]] kernel kernel_repeat_t kernel_repeat; + +kernel void kernel_reglu_f32( + constant ggml_metal_kargs_glu & args, + device const char * src0, + device const char * src1, + device char * dst, + uint tgpig[[threadgroup_position_in_grid]], + uint tpitg[[thread_position_in_threadgroup]], + uint ntg[[threads_per_threadgroup]]) { + device const float * src0_row = (device const float *) ((device const char *) src0 + tgpig*args.nb01) + args.i00; + device const float * src1_row = (device const float *) ((device const char *) src1 + tgpig*args.nb11) + args.i10; + device float * dst_row = (device float *) ((device char *) dst + tgpig*args.nb1); + + for (int i0 = tpitg; i0 < args.ne0; i0 += ntg) { + const float x0 = src0_row[i0]; + const float x1 = src1_row[i0]; + + dst_row[i0] = x0*x1*(x0 > 0.0f); + } +} + +kernel void kernel_geglu_f32( + constant ggml_metal_kargs_glu & args, + device const char * src0, + device const char * src1, + device char * dst, + uint tgpig[[threadgroup_position_in_grid]], + uint tpitg[[thread_position_in_threadgroup]], + uint ntg[[threads_per_threadgroup]]) { + device const float * src0_row = (device const float *) ((device const char *) src0 + tgpig*args.nb01) + args.i00; + device const float * src1_row = (device const float *) ((device const char *) src1 + tgpig*args.nb11) + args.i10; + device float * dst_row = (device float *) ((device char *) dst + tgpig*args.nb1); + + for (int i0 = tpitg; i0 < args.ne0; i0 += ntg) { + const float x0 = src0_row[i0]; + const float x1 = src1_row[i0]; + + const float gelu = 0.5f*x0*(1.0f + precise::tanh(SQRT_2_OVER_PI*x0*(1.0f + GELU_COEF_A*x0*x0))); + + dst_row[i0] = gelu*x1; + } +} + +kernel void kernel_swiglu_f32( + constant ggml_metal_kargs_glu & args, + device const char * src0, + device const char * src1, + device char * dst, + uint tgpig[[threadgroup_position_in_grid]], + uint tpitg[[thread_position_in_threadgroup]], + uint ntg[[threads_per_threadgroup]]) { + device const float * src0_row = (device const float *) ((device const char *) src0 + tgpig*args.nb01) + args.i00; + device const float * src1_row = (device const float *) ((device const char *) src1 + tgpig*args.nb11) + args.i10; + device float * dst_row = (device float *) ((device char *) dst + tgpig*args.nb1); + + for (int i0 = tpitg; i0 < args.ne0; i0 += ntg) { + const float x0 = src0_row[i0]; + const float x1 = src1_row[i0]; + + const float silu = x0 / (1.0f + exp(-x0)); + + dst_row[i0] = silu*x1; + } +} + +kernel void kernel_swiglu_oai_f32( + constant ggml_metal_kargs_glu & args, + device const char * src0, + device const char * src1, + device char * dst, + uint tgpig[[threadgroup_position_in_grid]], + uint tpitg[[thread_position_in_threadgroup]], + uint ntg[[threads_per_threadgroup]]) { + device const float * src0_row = (device const float *) ((device const char *) src0 + tgpig*args.nb01) + args.i00; + device const float * src1_row = (device const float *) ((device const char *) src1 + tgpig*args.nb11) + args.i10; + device float * dst_row = (device float *) ((device char *) dst + tgpig*args.nb1); + + for (int i0 = tpitg; i0 < args.ne0; i0 += ntg) { + float x0 = src0_row[i0]; + float x1 = src1_row[i0]; + + x0 = min(x0, args.limit); + x1 = max(min(x1, args.limit), -args.limit); + + float out_glu = x0 / (1.0f + exp(-x0 * args.alpha)); + out_glu = out_glu * (1.0f + x1); + + dst_row[i0] = out_glu; + } +} + +kernel void kernel_geglu_erf_f32( + constant ggml_metal_kargs_glu & args, + device const char * src0, + device const char * src1, + device char * dst, + uint tgpig[[threadgroup_position_in_grid]], + uint tpitg[[thread_position_in_threadgroup]], + uint ntg[[threads_per_threadgroup]]) { + device const float * src0_row = (device const float *) ((device const char *) src0 + tgpig*args.nb01) + args.i00; + device const float * src1_row = (device const float *) ((device const char *) src1 + tgpig*args.nb11) + args.i10; + device float * dst_row = (device float *) ((device char *) dst + tgpig*args.nb1); + + for (int i0 = tpitg; i0 < args.ne0; i0 += ntg) { + const float x0 = src0_row[i0]; + const float x1 = src1_row[i0]; + + const float gelu_erf = 0.5f*x0*(1.0f+erf_approx(x0*SQRT_2_INV)); + + dst_row[i0] = gelu_erf*x1; + } +} + +kernel void kernel_geglu_quick_f32( + constant ggml_metal_kargs_glu & args, + device const char * src0, + device const char * src1, + device char * dst, + uint tgpig[[threadgroup_position_in_grid]], + uint tpitg[[thread_position_in_threadgroup]], + uint ntg[[threads_per_threadgroup]]) { + device const float * src0_row = (device const float *) ((device const char *) src0 + tgpig*args.nb01) + args.i00; + device const float * src1_row = (device const float *) ((device const char *) src1 + tgpig*args.nb11) + args.i10; + device float * dst_row = (device float *) ((device char *) dst + tgpig*args.nb1); + + for (int i0 = tpitg; i0 < args.ne0; i0 += ntg) { + const float x0 = src0_row[i0]; + const float x1 = src1_row[i0]; + + const float gelu_quick = x0*(1.0f/(1.0f+exp(GELU_QUICK_COEF*x0))); + + dst_row[i0] = gelu_quick*x1; + } +} + +kernel void kernel_op_sum_f32( + constant ggml_metal_kargs_sum & args, + device const float * src0, + device float * dst, + threadgroup float * shmem_f32 [[threadgroup(0)]], + uint3 tgpig[[threadgroup_position_in_grid]], + ushort3 tpitg[[thread_position_in_threadgroup]], + ushort sgitg[[simdgroup_index_in_threadgroup]], + ushort tiisg[[thread_index_in_simdgroup]], + ushort3 ntg[[threads_per_threadgroup]]) { + + if (args.np == 0) { + return; + } + + // TODO: become function constant + const uint nsg = (ntg.x + 31) / 32; + + float sumf = 0; + + for (uint64_t i0 = tpitg.x; i0 < args.np; i0 += ntg.x) { + sumf += src0[i0]; + } + + sumf = simd_sum(sumf); + + if (tiisg == 0) { + shmem_f32[sgitg] = sumf; + } + + threadgroup_barrier(mem_flags::mem_threadgroup); + + float total = 0; + + if (sgitg == 0) { + float v = 0; + + if (tpitg.x < nsg) { + v = shmem_f32[tpitg.x]; + } + + total = simd_sum(v); + + if (tpitg.x == 0) { + dst[0] = total; + } + } +} + +constant short FC_sum_rows_op [[function_constant(FC_SUM_ROWS + 0)]]; + +template +kernel void kernel_sum_rows_impl( + constant ggml_metal_kargs_sum_rows & args, + device const char * src0, + device char * dst, + threadgroup char * shmem [[threadgroup(0)]], + uint3 tgpig[[threadgroup_position_in_grid]], + ushort3 tpitg[[thread_position_in_threadgroup]], + ushort sgitg[[simdgroup_index_in_threadgroup]], + ushort tiisg[[thread_index_in_simdgroup]], + ushort3 ntg[[threads_per_threadgroup]]) { +#define FC_OP FC_sum_rows_op + + const int i3 = tgpig.z; + const int i2 = tgpig.y; + const int i1 = tgpig.x; + + threadgroup T0 * shmem_t = (threadgroup T0 *) shmem; + + if (sgitg == 0) { + shmem_t[tiisg] = 0.0f; + } + + device const T0 * src_row = (device const T0 *) (src0 + i1*args.nb01 + i2*args.nb02 + i3*args.nb03); + device T * dst_row = (device T *) (dst + i1*args.nb1 + i2*args.nb2 + i3*args.nb3); + + T0 sumf = T0(0.0f); + + for (int64_t i0 = tpitg.x; i0 < args.ne00; i0 += ntg.x) { + sumf += src_row[i0]; + } + + sumf = simd_sum(sumf); + + threadgroup_barrier(mem_flags::mem_threadgroup); + + if (tiisg == 0) { + shmem_t[sgitg] = sumf; + } + + threadgroup_barrier(mem_flags::mem_threadgroup); + + sumf = shmem_t[tiisg]; + sumf = simd_sum(sumf); + + if (tpitg.x == 0) { + if (FC_OP == OP_SUM_ROWS_NUM_MEAN) { + if (is_same::value) { + dst_row[0] = sum(sumf) / (4*args.ne00); + } else { + dst_row[0] = sum(sumf) / args.ne00; + } + } else { + dst_row[0] = sum(sumf); + } + } + +#undef FC_OP +} + +typedef decltype(kernel_sum_rows_impl) kernel_sum_rows_t; + +template [[host_name("kernel_sum_rows_f32_f32")]] kernel kernel_sum_rows_t kernel_sum_rows_impl; +template [[host_name("kernel_sum_rows_f32_f32_4")]] kernel kernel_sum_rows_t kernel_sum_rows_impl; + +template +kernel void kernel_cumsum_blk( + constant ggml_metal_kargs_cumsum_blk & args, + device const char * src0, + device char * tmp, + device char * dst, + threadgroup char * shmem [[threadgroup(0)]], + uint3 tgpig[[threadgroup_position_in_grid]], + ushort3 tpitg[[thread_position_in_threadgroup]], + ushort sgitg[[simdgroup_index_in_threadgroup]], + ushort tiisg[[thread_index_in_simdgroup]], + ushort3 ntg[[threads_per_threadgroup]]) { + const int ib = tgpig[0]/args.ne01; + + const int i00 = ib*ntg.x; + const int i01 = tgpig[0]%args.ne01; + const int i02 = tgpig[1]; + const int i03 = tgpig[2]; + + device const float * src0_row = (device const float *) (src0 + + args.nb01*i01 + + args.nb02*i02 + + args.nb03*i03); + + threadgroup float * shmem_f32 = (threadgroup float *) shmem; + + float v = 0.0f; + + if (i00 + tpitg.x < args.ne00) { + v = src0_row[i00 + tpitg.x]; + } + + float s = simd_prefix_inclusive_sum(v); + + if (tiisg == N_SIMDWIDTH - 1) { + shmem_f32[sgitg] = s; + } + + threadgroup_barrier(mem_flags::mem_threadgroup); + + if (sgitg == 0) { + shmem_f32[tiisg] = simd_prefix_exclusive_sum(shmem_f32[tiisg]); + } + + threadgroup_barrier(mem_flags::mem_threadgroup); + + s += shmem_f32[sgitg]; + + device float * dst_row = (device float *) dst + + args.ne00*i01 + + args.ne00*args.ne01*i02 + + args.ne00*args.ne01*args.ne02*i03; + + if (i00 + tpitg.x < args.ne00) { + dst_row[i00 + tpitg.x] = s; + } + + if (args.outb && tpitg.x == ntg.x - 1) { + device float * tmp_row = (device float *) tmp + + args.net0*i01 + + args.net0*args.net1*i02 + + args.net0*args.net1*args.net2*i03; + + tmp_row[ib] = s; + } +} + +typedef decltype(kernel_cumsum_blk) kernel_cumsum_blk_t; + +template [[host_name("kernel_cumsum_blk_f32")]] kernel kernel_cumsum_blk_t kernel_cumsum_blk; + +template +kernel void kernel_cumsum_add( + constant ggml_metal_kargs_cumsum_add & args, + device const char * tmp, + device char * dst, + uint3 tgpig[[threadgroup_position_in_grid]], + ushort3 tpitg[[thread_position_in_threadgroup]], + ushort sgitg[[simdgroup_index_in_threadgroup]], + ushort tiisg[[thread_index_in_simdgroup]], + ushort3 ntg[[threads_per_threadgroup]]) { + const int ib = tgpig[0]/args.ne01; + + if (ib == 0) { + return; + } + + const int i00 = ib*ntg.x; + const int i01 = tgpig[0]%args.ne01; + const int i02 = tgpig[1]; + const int i03 = tgpig[2]; + + device const float * tmp_row = (device const float *) (tmp + + args.nbt1*i01 + + args.nbt2*i02 + + args.nbt3*i03); + + device float * dst_row = (device float *) dst + + args.ne00*i01 + + args.ne00*args.ne01*i02 + + args.ne00*args.ne01*args.ne02*i03; + + if (i00 + tpitg.x < args.ne00) { + dst_row[i00 + tpitg.x] += tmp_row[ib - 1]; + } +} + +typedef decltype(kernel_cumsum_add) kernel_cumsum_add_t; + +template [[host_name("kernel_cumsum_add_f32")]] kernel kernel_cumsum_add_t kernel_cumsum_add; + + +template +bool _ggml_vec_tri_cmp(const int i, const int r); + +template<> +bool _ggml_vec_tri_cmp(const int i, const int r) { + return i < r; +} + +template<> +bool _ggml_vec_tri_cmp(const int i, const int r) { + return i <= r; +} + +template<> +bool _ggml_vec_tri_cmp(const int i, const int r) { + return i > r; +} + +template<> +bool _ggml_vec_tri_cmp(const int i, const int r) { + return i >= r; +} + +template +kernel void kernel_tri( + constant ggml_metal_kargs_tri & args, + device const char * src0, + device const char * dst, + uint3 tgpig[[threadgroup_position_in_grid]], + ushort3 tpitg[[thread_position_in_threadgroup]], + ushort3 ntg[[threads_per_threadgroup]]) { + const int i3 = tgpig.z; + const int i2 = tgpig.y; + const int i1 = tgpig.x; + + if (i3 >= args.ne03 || i2 >= args.ne02 || i1 >= args.ne01) { + return; + } + + device const T * src_row = (device const T *) ((device const char *) src0 + i1*args.nb01 + i2*args.nb02 + i3*args.nb03); + device T * dst_row = (device T *) ((device char *) dst + i1*args.nb1 + i2*args.nb2 + i3*args.nb3); + + // Each thread is a single element of the row if ne00 < max threads per + // threadgroup, so this will loop once for each index that this thread is + // responsible for + for (int64_t i0 = tpitg.x; i0 < args.ne00; i0 += ntg.x) { + // Use the comparison as a mask for branchless + dst_row[i0] = static_cast(_ggml_vec_tri_cmp(i0, i1)) * src_row[i0]; + } +} + +typedef decltype(kernel_tri) kernel_tri_t; + +template [[host_name("kernel_tri_f32_0")]] kernel kernel_tri_t kernel_tri; +template [[host_name("kernel_tri_f32_1")]] kernel kernel_tri_t kernel_tri; +template [[host_name("kernel_tri_f32_2")]] kernel kernel_tri_t kernel_tri; +template [[host_name("kernel_tri_f32_3")]] kernel kernel_tri_t kernel_tri; +template [[host_name("kernel_tri_f16_0")]] kernel kernel_tri_t kernel_tri; +template [[host_name("kernel_tri_f16_1")]] kernel kernel_tri_t kernel_tri; +template [[host_name("kernel_tri_f16_2")]] kernel kernel_tri_t kernel_tri; +template [[host_name("kernel_tri_f16_3")]] kernel kernel_tri_t kernel_tri; +#if defined(GGML_METAL_HAS_BF16) +template [[host_name("kernel_tri_bf16_0")]] kernel kernel_tri_t kernel_tri; +template [[host_name("kernel_tri_bf16_1")]] kernel kernel_tri_t kernel_tri; +template [[host_name("kernel_tri_bf16_2")]] kernel kernel_tri_t kernel_tri; +template [[host_name("kernel_tri_bf16_3")]] kernel kernel_tri_t kernel_tri; +#endif + +template +kernel void kernel_soft_max( + constant ggml_metal_kargs_soft_max & args, + device const char * src0, + device const char * src1, + device const char * src2, + device char * dst, + threadgroup float * buf [[threadgroup(0)]], + uint3 tgpig[[threadgroup_position_in_grid]], + uint3 tpitg[[thread_position_in_threadgroup]], + uint sgitg[[simdgroup_index_in_threadgroup]], + uint tiisg[[thread_index_in_simdgroup]], + uint3 tptg[[threads_per_threadgroup]]) { + const int32_t i03 = tgpig.z; + const int32_t i02 = tgpig.y; + const int32_t i01 = tgpig.x; + + const int32_t i13 = i03%args.ne13; + const int32_t i12 = i02%args.ne12; + const int32_t i11 = i01; + + device const float * psrc0 = (device const float *) (src0 + i01*args.nb01 + i02*args.nb02 + i03*args.nb03); + device const T * pmask = src1 != src0 ? (device const T * ) (src1 + i11*args.nb11 + i12*args.nb12 + i13*args.nb13) : nullptr; + device const float * psrc2 = src2 != src0 ? (device const float *) (src2) : nullptr; + device float * pdst = (device float *) (dst + i01*args.nb1 + i02*args.nb2 + i03*args.nb3); + + float slope = 1.0f; + + // ALiBi + if (args.max_bias > 0.0f) { + const int32_t h = i02; + + const float base = h < args.n_head_log2 ? args.m0 : args.m1; + const int exp = h < args.n_head_log2 ? h + 1 : 2*(h - args.n_head_log2) + 1; + + slope = pow(base, exp); + } + + // parallel max + float lmax = psrc2 ? psrc2[i02] : -INFINITY; + + for (int i00 = tpitg.x; i00 < args.ne00; i00 += tptg.x) { + lmax = MAX(lmax, psrc0[i00]*args.scale + (pmask ? slope*pmask[i00] : 0.0f)); + } + + // find the max value in the block + float max_val = simd_max(lmax); + if (tptg.x > N_SIMDWIDTH) { + if (sgitg == 0) { + buf[tiisg] = -INFINITY; + } + + threadgroup_barrier(mem_flags::mem_threadgroup); + + if (tiisg == 0) { + buf[sgitg] = max_val; + } + + threadgroup_barrier(mem_flags::mem_threadgroup); + + max_val = buf[tiisg]; + max_val = simd_max(max_val); + } + + // parallel sum + float lsum = 0.0f; + for (int i00 = tpitg.x; i00 < args.ne00; i00 += tptg.x) { + const float exp_psrc0 = exp((psrc0[i00]*args.scale + (pmask ? slope*pmask[i00] : 0.0f)) - max_val); + lsum += exp_psrc0; + pdst[i00] = exp_psrc0; + } + + // This barrier fixes a failing test + // ref: https://github.com/ggml-org/ggml/pull/621#discussion_r1425156335 + threadgroup_barrier(mem_flags::mem_none); + + float sum = simd_sum(lsum); + + if (tptg.x > N_SIMDWIDTH) { + if (sgitg == 0) { + buf[tiisg] = 0.0f; + } + + threadgroup_barrier(mem_flags::mem_threadgroup); + + if (tiisg == 0) { + buf[sgitg] = sum; + } + + threadgroup_barrier(mem_flags::mem_threadgroup); + + sum = buf[tiisg]; + sum = simd_sum(sum); + } + + if (psrc2) { + sum += exp(psrc2[i02] - max_val); + } + + const float inv_sum = 1.0f/sum; + + for (int i00 = tpitg.x; i00 < args.ne00; i00 += tptg.x) { + pdst[i00] *= inv_sum; + } +} + +template +kernel void kernel_soft_max_4( + constant ggml_metal_kargs_soft_max & args, + device const char * src0, + device const char * src1, + device const char * src2, + device char * dst, + threadgroup float * buf [[threadgroup(0)]], + uint3 tgpig[[threadgroup_position_in_grid]], + uint3 tpitg[[thread_position_in_threadgroup]], + uint sgitg[[simdgroup_index_in_threadgroup]], + uint tiisg[[thread_index_in_simdgroup]], + uint3 tptg[[threads_per_threadgroup]]) { + const int32_t i03 = tgpig.z; + const int32_t i02 = tgpig.y; + const int32_t i01 = tgpig.x; + + const int32_t i13 = i03%args.ne13; + const int32_t i12 = i02%args.ne12; + const int32_t i11 = i01; + + device const float4 * psrc4 = (device const float4 *) (src0 + i01*args.nb01 + i02*args.nb02 + i03*args.nb03); + device const T * pmask = src1 != src0 ? (device const T * ) (src1 + i11*args.nb11 + i12*args.nb12 + i13*args.nb13) : nullptr; + device const float * psrc2 = src2 != src0 ? (device const float * ) (src2) : nullptr; + device float4 * pdst4 = (device float4 *) (dst + i01*args.nb1 + i02*args.nb2 + i03*args.nb3); + + float slope = 1.0f; + + if (args.max_bias > 0.0f) { + const int32_t h = i02; + + const float base = h < args.n_head_log2 ? args.m0 : args.m1; + const int exp = h < args.n_head_log2 ? h + 1 : 2*(h - args.n_head_log2) + 1; + + slope = pow(base, exp); + } + + // parallel max + float4 lmax4 = psrc2 ? psrc2[i02] : -INFINITY; + + for (int i00 = tpitg.x; i00 < args.ne00/4; i00 += tptg.x) { + lmax4 = fmax(lmax4, psrc4[i00]*args.scale + (float4)((pmask ? slope*pmask[i00] : 0.0f))); + } + + const float lmax = MAX(MAX(lmax4[0], lmax4[1]), MAX(lmax4[2], lmax4[3])); + + float max_val = simd_max(lmax); + if (tptg.x > N_SIMDWIDTH) { + if (sgitg == 0) { + buf[tiisg] = -INFINITY; + } + + threadgroup_barrier(mem_flags::mem_threadgroup); + + if (tiisg == 0) { + buf[sgitg] = max_val; + } + + threadgroup_barrier(mem_flags::mem_threadgroup); + + max_val = buf[tiisg]; + max_val = simd_max(max_val); + } + + // parallel sum + float4 lsum4 = 0.0f; + for (int i00 = tpitg.x; i00 < args.ne00/4; i00 += tptg.x) { + const float4 exp_psrc4 = exp((psrc4[i00]*args.scale + (float4)((pmask ? slope*pmask[i00] : 0.0f))) - max_val); + lsum4 += exp_psrc4; + pdst4[i00] = exp_psrc4; + } + + const float lsum = lsum4[0] + lsum4[1] + lsum4[2] + lsum4[3]; + + // This barrier fixes a failing test + // ref: https://github.com/ggml-org/ggml/pull/621#discussion_r1425156335 + threadgroup_barrier(mem_flags::mem_none); + + float sum = simd_sum(lsum); + + if (tptg.x > N_SIMDWIDTH) { + if (sgitg == 0) { + buf[tiisg] = 0.0f; + } + + threadgroup_barrier(mem_flags::mem_threadgroup); + + if (tiisg == 0) { + buf[sgitg] = sum; + } + + threadgroup_barrier(mem_flags::mem_threadgroup); + + sum = buf[tiisg]; + sum = simd_sum(sum); + } + + if (psrc2) { + sum += exp(psrc2[i02] - max_val); + } + + const float inv_sum = 1.0f/sum; + + for (int i00 = tpitg.x; i00 < args.ne00/4; i00 += tptg.x) { + pdst4[i00] *= inv_sum; + } +} + +typedef decltype(kernel_soft_max) kernel_soft_max_t; +typedef decltype(kernel_soft_max_4) kernel_soft_max_4_t; + +template [[host_name("kernel_soft_max_f16")]] kernel kernel_soft_max_t kernel_soft_max; +template [[host_name("kernel_soft_max_f32")]] kernel kernel_soft_max_t kernel_soft_max; +template [[host_name("kernel_soft_max_f16_4")]] kernel kernel_soft_max_4_t kernel_soft_max_4; +template [[host_name("kernel_soft_max_f32_4")]] kernel kernel_soft_max_4_t kernel_soft_max_4; + +// ref: ggml.c:ggml_compute_forward_ssm_conv_f32 +kernel void kernel_ssm_conv_f32_f32( + constant ggml_metal_kargs_ssm_conv & args, + device const void * src0, + device const void * src1, + device float * dst, + uint3 tgpig[[threadgroup_position_in_grid]], + uint3 tpitg[[thread_position_in_threadgroup]], + uint3 ntg[[threads_per_threadgroup]]) { + const int64_t ir = tgpig.x; + const int64_t i2 = tgpig.y; + const int64_t i3 = tgpig.z; + + const int64_t nc = args.ne10; + //const int64_t ncs = args.ne00; + //const int64_t nr = args.ne01; + //const int64_t n_t = args.ne1; + //const int64_t n_s = args.ne2; + + device const float * s = (device const float *) ((device const char *) src0 + ir*args.nb01 + i2*args.nb00 + i3*args.nb02); + device const float * c = (device const float *) ((device const char *) src1 + ir*args.nb11); + device float * x = (device float *) ((device char *) dst + ir*args.nb0 + i2*args.nb1 + i3*args.nb2); + + float sumf = 0.0f; + + for (int64_t i0 = 0; i0 < nc; ++i0) { + sumf += s[i0] * c[i0]; + } + + x[0] = sumf; +} + +kernel void kernel_ssm_conv_f32_f32_4( + constant ggml_metal_kargs_ssm_conv & args, + device const void * src0, + device const void * src1, + device float * dst, + uint3 tgpig[[threadgroup_position_in_grid]], + uint3 tpitg[[thread_position_in_threadgroup]], + uint3 ntg[[threads_per_threadgroup]]) { + const int64_t ir = tgpig.x; + const int64_t i2 = tgpig.y; + const int64_t i3 = tgpig.z; + + const int64_t nc = args.ne10; + //const int64_t ncs = args.ne00; + //const int64_t nr = args.ne01; + //const int64_t n_t = args.ne1; + //const int64_t n_s = args.ne2; + + device const float4 * s = (device const float4 *) ((device const char *) src0 + ir*args.nb01 + i2*args.nb00 + i3*args.nb02); + device const float4 * c = (device const float4 *) ((device const char *) src1 + ir*args.nb11); + device float * x = (device float *) ((device char *) dst + ir*args.nb0 + i2*args.nb1 + i3*args.nb2); + + float sumf = 0.0f; + + for (int64_t i0 = 0; i0 < nc/4; ++i0) { + sumf += dot(s[i0], c[i0]); + } + + x[0] = sumf; +} + +constant short FC_ssm_conv_bs [[function_constant(FC_SSM_CONV + 0)]]; + +// Batched version: each threadgroup processes multiple tokens for better efficiency +// Thread layout: each thread handles one token, threadgroup covers BATCH_SIZE tokens +kernel void kernel_ssm_conv_f32_f32_batched( + constant ggml_metal_kargs_ssm_conv & args, + device const void * src0, + device const void * src1, + device float * dst, + uint3 tgpig[[threadgroup_position_in_grid]], + uint3 tpitg[[thread_position_in_threadgroup]], + uint3 ntg[[threads_per_threadgroup]]) { + // tgpig.x = row index (ir) + // tgpig.y = batch of tokens (i2_base / BATCH_SIZE) + // tgpig.z = sequence index (i3) + // tpitg.x = thread within batch (0..BATCH_SIZE-1) + const short BATCH_SIZE = FC_ssm_conv_bs; + + const int64_t ir = tgpig.x; + const int64_t i2_base = tgpig.y * BATCH_SIZE; + const int64_t i3 = tgpig.z; + const int64_t i2_off = tpitg.x; + const int64_t i2 = i2_base + i2_off; + + const int64_t nc = args.ne10; // conv kernel size (typically 4) + const int64_t n_t = args.ne1; // number of tokens + + // Bounds check for partial batches at the end + if (i2 >= n_t) { + return; + } + + // Load conv weights (shared across all tokens for this row) + device const float * c = (device const float *) ((device const char *) src1 + ir*args.nb11); + + // Load source for this specific token + device const float * s = (device const float *) ((device const char *) src0 + ir*args.nb01 + i2*args.nb00 + i3*args.nb02); + + // Output location for this token + device float * x = (device float *) ((device char *) dst + ir*args.nb0 + i2*args.nb1 + i3*args.nb2); + + float sumf = 0.0f; + for (int64_t i0 = 0; i0 < nc; ++i0) { + sumf += s[i0] * c[i0]; + } + + x[0] = sumf; +} + +kernel void kernel_ssm_conv_f32_f32_batched_4( + constant ggml_metal_kargs_ssm_conv & args, + device const void * src0, + device const void * src1, + device float * dst, + uint3 tgpig[[threadgroup_position_in_grid]], + uint3 tpitg[[thread_position_in_threadgroup]], + uint3 ntg[[threads_per_threadgroup]]) { + // tgpig.x = row index (ir) + // tgpig.y = batch of tokens (i2_base / BATCH_SIZE) + // tgpig.z = sequence index (i3) + // tpitg.x = thread within batch (0..BATCH_SIZE-1) + const short BATCH_SIZE = FC_ssm_conv_bs; + + const int64_t ir = tgpig.x; + const int64_t i2_base = tgpig.y * BATCH_SIZE; + const int64_t i3 = tgpig.z; + const int64_t i2_off = tpitg.x; + const int64_t i2 = i2_base + i2_off; + + const int64_t nc = args.ne10; // conv kernel size (typically 4) + const int64_t n_t = args.ne1; // number of tokens + + // Bounds check for partial batches at the end + if (i2 >= n_t) { + return; + } + + // Load conv weights (shared across all tokens for this row) + device const float4 * c = (device const float4 *) ((device const char *) src1 + ir*args.nb11); + + // Load source for this specific token + device const float4 * s = (device const float4 *) ((device const char *) src0 + ir*args.nb01 + i2*args.nb00 + i3*args.nb02); + + // Output location for this token + device float * x = (device float *) ((device char *) dst + ir*args.nb0 + i2*args.nb1 + i3*args.nb2); + + float sumf = 0.0f; + for (int64_t i0 = 0; i0 < nc/4; ++i0) { + sumf += dot(s[i0], c[i0]); + } + + x[0] = sumf; +} + +// ref: ggml.c:ggml_compute_forward_ssm_scan_f32, Mamba-2 part +// Optimized version: reduces redundant memory loads by having one thread load shared values +kernel void kernel_ssm_scan_f32( + constant ggml_metal_kargs_ssm_scan & args, + device const void * src0, + device const void * src1, + device const void * src2, + device const void * src3, + device const void * src4, + device const void * src5, + device const void * src6, + device float * dst, + threadgroup float * shared [[threadgroup(0)]], + uint3 tgpig[[threadgroup_position_in_grid]], + ushort3 tpitg[[thread_position_in_threadgroup]], + ushort sgitg[[simdgroup_index_in_threadgroup]], + ushort tiisg[[thread_index_in_simdgroup]], + ushort sgptg[[simdgroups_per_threadgroup]], + uint3 tgpg[[threadgroups_per_grid]]) { + constexpr short NW = N_SIMDWIDTH; + + // Shared memory layout: + // [0..sgptg*NW-1]: partial sums for reduction (existing) + // [sgptg*NW..sgptg*NW+sgptg-1]: pre-computed x_dt values for each token in batch + // [sgptg*NW+sgptg..sgptg*NW+2*sgptg-1]: pre-computed dA values for each token in batch + threadgroup float * shared_sums = shared; + threadgroup float * shared_x_dt = shared + sgptg * NW; + threadgroup float * shared_dA = shared + sgptg * NW + sgptg; + + shared_sums[tpitg.x] = 0.0f; + + const int32_t i0 = tpitg.x; + const int32_t i1 = tgpig.x; + const int32_t ir = tgpig.y; // current head + const int32_t i3 = tgpig.z; // current seq + + const int32_t nc = args.d_state; + const int32_t nr = args.d_inner; + const int32_t nh = args.n_head; + const int32_t ng = args.n_group; + const int32_t n_t = args.n_seq_tokens; + + const int32_t s_off = args.s_off; + + device const int32_t * ids = (device const int32_t *) src6; + + device const float * s0_buff = (device const float *) ((device const char *) src0 + ir*args.nb02 + ids[i3]*args.nb03); + device float * s_buff = (device float *) ((device char *) dst + ir*args.nb02 + i3*args.nb03 + s_off); + + const int32_t i = i0 + i1*nc; + const int32_t g = ir / (nh / ng); // repeat_interleave + + float s0 = s0_buff[i]; + float s = 0.0f; + + device const float * A = (device const float *) ((device const char *) src3 + ir*args.nb31); // {ne30, nh} + + const float A0 = A[i0%args.ne30]; + + device const float * x = (device const float *)((device const char *) src1 + i1*args.nb10 + ir*args.nb11 + i3*args.nb13); // {dim, nh, nt, ns} + device const float * dt = (device const float *)((device const char *) src2 + ir*args.nb20 + i3*args.nb22); // {nh, nt, ns} + device const float * B = (device const float *)((device const char *) src4 + g*args.nb41 + i3*args.nb43); // {d_state, ng, nt, ns} + device const float * C = (device const float *)((device const char *) src5 + g*args.nb51 + i3*args.nb53); // {d_state, ng, nt, ns} + + device float * y = dst + (i1 + ir*(nr) + i3*(n_t*nh*nr)); // {dim, nh, nt, ns} + + for (int i2 = 0; i2 < n_t; i2 += sgptg) { + threadgroup_barrier(mem_flags::mem_threadgroup); + + // Pre-compute x_dt and dA for this batch of tokens + // Only first sgptg threads do the loads and expensive math + if (i0 < sgptg && i2 + i0 < n_t) { + // ns12 and ns21 are element strides (nb12/nb10, nb21/nb20) + device const float * x_t = x + i0 * args.ns12; + device const float * dt_t = dt + i0 * args.ns21; + + const float dt0 = dt_t[0]; + const float dtsp = dt0 <= 20.0f ? log(1.0f + exp(dt0)) : dt0; + shared_x_dt[i0] = x_t[0] * dtsp; + shared_dA[i0] = dtsp; // Store dtsp, compute exp(dtsp * A0) per-thread since A0 varies + } + + threadgroup_barrier(mem_flags::mem_threadgroup); + + for (int t = 0; t < sgptg && i2 + t < n_t; t++) { + const float x_dt = shared_x_dt[t]; + const float dA = exp(shared_dA[t] * A0); + + s = (s0 * dA) + (B[i0] * x_dt); + + const float sumf = simd_sum(s * C[i0]); + + if (tiisg == 0) { + shared_sums[t*NW + sgitg] = sumf; + } + + // recurse + s0 = s; + + B += args.ns42; + C += args.ns52; + } + + // Advance pointers for next batch + x += sgptg * args.ns12; + dt += sgptg * args.ns21; + + threadgroup_barrier(mem_flags::mem_threadgroup); + + const float sumf = simd_sum(shared_sums[sgitg*NW + tiisg]); + + if (tiisg == 0 && i2 + sgitg < n_t) { + y[sgitg*nh*nr] = sumf; + } + + y += sgptg*nh*nr; + } + + s_buff[i] = s; +} + +kernel void kernel_rwkv_wkv6_f32( + device const float * k, + device const float * v, + device const float * r, + device const float * tf, + device const float * td, + device const float * state_in, + device float * dst, + constant uint & B, + constant uint & T, + constant uint & C, + constant uint & H, + uint3 tgpig[[threadgroup_position_in_grid]], + uint3 tpitg[[thread_position_in_threadgroup]], + uint3 ntg[[threads_per_threadgroup]]) { + + const uint head_size = 64; // TODO: support head_size = 128 + const uint batch_id = tgpig.x / H; + const uint head_id = tgpig.x % H; + const uint tid = tpitg.x; + + if (batch_id >= B || head_id >= H) { + return; + } + + const uint state_size = C * head_size; + const uint n_seq_tokens = T / B; + + threadgroup float _k[head_size]; + threadgroup float _r[head_size]; + threadgroup float _tf[head_size]; + threadgroup float _td[head_size]; + + float state[head_size]; + + for (uint i = 0; i < head_size; i++) { + state[i] = state_in[batch_id * state_size + head_id * head_size * head_size + + i * head_size + tid]; + } + + threadgroup_barrier(mem_flags::mem_threadgroup); + _tf[tid] = tf[head_id * head_size + tid]; + threadgroup_barrier(mem_flags::mem_threadgroup); + + const uint start_t = batch_id * n_seq_tokens * C + head_id * head_size + tid; + const uint end_t = (batch_id + 1) * n_seq_tokens * C + head_id * head_size + tid; + + for (uint t = start_t; t < end_t; t += C) { + threadgroup_barrier(mem_flags::mem_threadgroup); + _k[tid] = k[t]; + _r[tid] = r[t]; + _td[tid] = td[t]; + threadgroup_barrier(mem_flags::mem_threadgroup); + + const float v_val = v[t]; + float y = 0.0; + + for (uint j = 0; j < head_size; j += 4) { + float4 k_vec = float4(_k[j], _k[j+1], _k[j+2], _k[j+3]); + float4 r_vec = float4(_r[j], _r[j+1], _r[j+2], _r[j+3]); + float4 tf_vec = float4(_tf[j], _tf[j+1], _tf[j+2], _tf[j+3]); + float4 td_vec = float4(_td[j], _td[j+1], _td[j+2], _td[j+3]); + float4 s_vec = float4(state[j], state[j+1], state[j+2], state[j+3]); + + float4 kv = k_vec * v_val; + + float4 temp = tf_vec * kv + s_vec; + y += dot(r_vec, temp); + + s_vec = s_vec * td_vec + kv; + state[j] = s_vec[0]; + state[j+1] = s_vec[1]; + state[j+2] = s_vec[2]; + state[j+3] = s_vec[3]; + } + + dst[t] = y; + } + + for (uint i = 0; i < head_size; i++) { + dst[T * C + batch_id * state_size + head_id * head_size * head_size + + i * head_size + tid] = state[i]; + } +} + +kernel void kernel_rwkv_wkv7_f32( + device const float * r, + device const float * w, + device const float * k, + device const float * v, + device const float * a, + device const float * b, + device const float * state_in, + device float * dst, + constant uint & B, + constant uint & T, + constant uint & C, + constant uint & H, + uint3 tgpig[[threadgroup_position_in_grid]], + uint3 tpitg[[thread_position_in_threadgroup]], + uint3 ntg[[threads_per_threadgroup]]) { + + const uint head_size = 64; // TODO: support head_size = 128 + const uint batch_id = tgpig.x / H; + const uint head_id = tgpig.x % H; + const uint tid = tpitg.x; + + if (batch_id >= B || head_id >= H) { + return; + } + + const uint state_size = C * head_size; + const uint n_seq_tokens = T / B; + + threadgroup float _r[head_size]; + threadgroup float _w[head_size]; + threadgroup float _k[head_size]; + threadgroup float _a[head_size]; + threadgroup float _b[head_size]; + + float state[head_size]; + + for (uint i = 0; i < head_size; i++) { + state[i] = state_in[batch_id * state_size + head_id * head_size * head_size + + tid * head_size + i]; + } + + const uint start_t = batch_id * n_seq_tokens * C + head_id * head_size + tid; + const uint end_t = (batch_id + 1) * n_seq_tokens * C + head_id * head_size + tid; + + for (uint t = start_t; t < end_t; t += C) { + threadgroup_barrier(mem_flags::mem_threadgroup); + _r[tid] = r[t]; + _w[tid] = w[t]; + _k[tid] = k[t]; + _a[tid] = a[t]; + _b[tid] = b[t]; + threadgroup_barrier(mem_flags::mem_threadgroup); + + const float v_val = v[t]; + float y = 0.0, sa = 0.0; + + float4 sa_vec(0.0); + + for (uint j = 0; j < head_size; j += 4) { + float4 a_vec = float4(_a[j], _a[j+1], _a[j+2], _a[j+3]); + float4 s_vec = float4(state[j], state[j+1], state[j+2], state[j+3]); + sa_vec += a_vec * s_vec; + } + sa = sa_vec[0] + sa_vec[1] + sa_vec[2] + sa_vec[3]; + + for (uint j = 0; j < head_size; j += 4) { + float4 r_vec = float4(_r[j], _r[j+1], _r[j+2], _r[j+3]); + float4 w_vec = float4(_w[j], _w[j+1], _w[j+2], _w[j+3]); + float4 k_vec = float4(_k[j], _k[j+1], _k[j+2], _k[j+3]); + float4 b_vec = float4(_b[j], _b[j+1], _b[j+2], _b[j+3]); + float4 s_vec = float4(state[j], state[j+1], state[j+2], state[j+3]); + + float4 kv = k_vec * v_val; + + s_vec = s_vec * w_vec + kv + sa * b_vec; + y += dot(s_vec, r_vec); + + state[j] = s_vec[0]; + state[j+1] = s_vec[1]; + state[j+2] = s_vec[2]; + state[j+3] = s_vec[3]; + } + + dst[t] = y; + } + + for (uint i = 0; i < head_size; i++) { + dst[T * C + batch_id * state_size + head_id * head_size * head_size + + tid * head_size + i] = state[i]; + } +} + +constant short FC_gated_delta_net_ne20 [[function_constant(FC_GATED_DELTA_NET + 0)]]; +constant short FC_gated_delta_net_ne30 [[function_constant(FC_GATED_DELTA_NET + 1)]]; + +#if 1 +template +kernel void kernel_gated_delta_net_impl( + constant ggml_metal_kargs_gated_delta_net & args, + device const char * q, + device const char * k, + device const char * v, + device const char * g, + device const char * b, + device const char * s, + device char * dst, + uint3 tgpig[[threadgroup_position_in_grid]], + uint3 tpitg[[thread_position_in_threadgroup]], + uint3 ntg[[threads_per_threadgroup]]) { +#define S_v FC_gated_delta_net_ne20 +#define G FC_gated_delta_net_ne30 + + const uint tx = tpitg.x; + const uint ty = tpitg.y; + + const uint i23 = tgpig.z; // B + const uint i21 = tgpig.y; // H + const uint i20 = tgpig.x*NSG + ty; + + const uint i01 = i21 % args.ne01; + const uint i11 = i21 % args.ne11; + + const float scale = 1.0f / sqrt((float)S_v); + + // state is stored transposed: M[i20][is] = S[is][i20], so row i20 is contiguous + device const float * s_ptr = (device const float *) (s) + (i23*args.ne21 + i21)*S_v*S_v + i20*S_v; + + float ls[NSG]; + + FOR_UNROLL (short j = 0; j < NSG; j++) { + const short is = tx*NSG + j; + ls[j] = s_ptr[is]; + } + + device float * dst_attn = (device float *) (dst) + (i23*args.ne22*args.ne21 + i21)*S_v + i20; + + device const float * q_ptr = (device const float *) (q + i23*args.nb03 + i01*args.nb01); + device const float * k_ptr = (device const float *) (k + i23*args.nb13 + i11*args.nb11); + device const float * v_ptr = (device const float *) (v + i23*args.nb23 + i21*args.nb21); + + device const float * b_ptr = (device const float *) (b) + (i23*args.ne22*args.ne21 + i21); + device const float * g_ptr = (device const float *) (g) + (i23*args.ne22*args.ne21 + i21)*G; + + for (short t = 0; t < args.ne22; t++) { + float s_k = 0.0f; + + if (G == 1) { + const float g_exp = exp(g_ptr[0]); + + FOR_UNROLL (short j = 0; j < NSG; j++) { + const short is = tx*NSG + j; + ls[j] *= g_exp; + + s_k += ls[j]*k_ptr[is]; + } + } else { + // KDA + FOR_UNROLL (short j = 0; j < NSG; j++) { + const short is = tx*NSG + j; + ls[j] *= exp(g_ptr[is]); + + s_k += ls[j]*k_ptr[is]; + } + } + + s_k = simd_sum(s_k); + + const float d = (v_ptr[i20] - s_k)*b_ptr[0]; + + float y = 0.0f; + + FOR_UNROLL (short j = 0; j < NSG; j++) { + const short is = tx*NSG + j; + ls[j] += k_ptr[is]*d; + + y += ls[j]*q_ptr[is]; + } + + y = simd_sum(y); + + if (tx == 0) { + dst_attn[t*args.ne21*S_v] = y*scale; + } + + q_ptr += args.ns02; + k_ptr += args.ns12; + v_ptr += args.ns22; + + b_ptr += args.ne21; + g_ptr += args.ne21*G; + } + + device float * dst_state = (device float *) (dst) + args.ne23*args.ne22*args.ne21*S_v + (i23*args.ne21 + i21)*S_v*S_v + i20*S_v; + + FOR_UNROLL (short j = 0; j < NSG; j++) { + const short is = tx*NSG + j; + dst_state[is] = ls[j]; + } + +#undef S_v +#undef G +} + +typedef decltype(kernel_gated_delta_net_impl<4>) kernel_gated_delta_net_t; + +template [[host_name("kernel_gated_delta_net_f32_1")]] kernel kernel_gated_delta_net_t kernel_gated_delta_net_impl<1>; +template [[host_name("kernel_gated_delta_net_f32_2")]] kernel kernel_gated_delta_net_t kernel_gated_delta_net_impl<2>; +template [[host_name("kernel_gated_delta_net_f32_4")]] kernel kernel_gated_delta_net_t kernel_gated_delta_net_impl<4>; + +#else +// a simplified version of the above +// no performance improvement, so keep the above version for now + +template +kernel void kernel_gated_delta_net_impl( + constant ggml_metal_kargs_gated_delta_net & args, + device const char * q, + device const char * k, + device const char * v, + device const char * g, + device const char * b, + device const char * s, + device char * dst, + uint3 tgpig[[threadgroup_position_in_grid]], + uint3 tpitg[[thread_position_in_threadgroup]], + uint3 ntg[[threads_per_threadgroup]]) { +#define S_v FC_gated_delta_net_ne20 +#define G FC_gated_delta_net_ne30 + + const uint tx = tpitg.x; + const uint ty = tpitg.y; + + const uint i23 = tgpig.z; // B + const uint i21 = tgpig.y; // H + const uint i20 = tgpig.x*NSG + ty; + + const uint i01 = i21 % args.ne01; + const uint i11 = i21 % args.ne11; + + const float scale = 1.0f / sqrt((float)S_v); + + device const float * s_ptr = (device const float *) (s) + (i23*args.ne21 + i21)*S_v*S_v + i20; + + float lsf[NSG]; + + FOR_UNROLL (short j = 0; j < NSG; j++) { + const short is = tx*NSG + j; + lsf[j] = s_ptr[is*S_v]; + } + + thread T * ls = (thread T *) (lsf); + + device float * dst_attn = (device float *) (dst) + (i23*args.ne22*args.ne21 + i21)*S_v + i20; + + device const float * q_ptr = (device const float *) (q + i23*args.nb03 + i01*args.nb01); + device const float * k_ptr = (device const float *) (k + i23*args.nb13 + i11*args.nb11); + device const float * v_ptr = (device const float *) (v + i23*args.nb23 + i21*args.nb21); + + device const float * b_ptr = (device const float *) (b) + (i23*args.ne22*args.ne21 + i21); + device const float * g_ptr = (device const float *) (g) + (i23*args.ne22*args.ne21 + i21)*G; + + for (short t = 0; t < args.ne22; t++) { + device const T * qt_ptr = (device const T *) (q_ptr); + device const T * kt_ptr = (device const T *) (k_ptr); + device const T * gt_ptr = (device const T *) (g_ptr); + + if (G == 1) { + *ls *= exp(g_ptr[0]); + } else { + // KDA + *ls *= exp(gt_ptr[tx]); + } + + const float s_k = simd_sum(dot(*ls, kt_ptr[tx])); + + const float d = (v_ptr[i20] - s_k)*b_ptr[0]; + + *ls += kt_ptr[tx]*d; + + const float y = simd_sum(dot(*ls, qt_ptr[tx])); + + if (tx == 0) { + *dst_attn = y*scale; + } + + q_ptr += args.ns02; + k_ptr += args.ns12; + v_ptr += args.ns22; + + b_ptr += args.ne21; + g_ptr += args.ne21*G; + + dst_attn += args.ne21*S_v; + } + + device float * dst_state = (device float *) (dst) + args.ne23*args.ne22*args.ne21*S_v + (i23*args.ne21 + i21)*S_v*S_v + i20; + device T * dstt_state = (device T *) (dst_state); + + FOR_UNROLL (short j = 0; j < NSG; j++) { + const short is = tx*NSG + j; + dst_state[is*S_v] = lsf[j]; + } + +#undef S_v +#undef G +} + +typedef decltype(kernel_gated_delta_net_impl) kernel_gated_delta_net_t; + +template [[host_name("kernel_gated_delta_net_f32_1")]] kernel kernel_gated_delta_net_t kernel_gated_delta_net_impl; +template [[host_name("kernel_gated_delta_net_f32_2")]] kernel kernel_gated_delta_net_t kernel_gated_delta_net_impl; +template [[host_name("kernel_gated_delta_net_f32_4")]] kernel kernel_gated_delta_net_t kernel_gated_delta_net_impl; +#endif + +constant short FC_solve_tri_nsg [[function_constant(FC_SOLVE_TRI + 0)]]; +constant short FC_solve_tri_n [[function_constant(FC_SOLVE_TRI + 1)]]; +constant short FC_solve_tri_k [[function_constant(FC_SOLVE_TRI + 2)]]; + +kernel void kernel_solve_tri_f32( + constant ggml_metal_kargs_solve_tri & args, + device const char * src0, + device const char * src1, + device char * dst, + threadgroup char * shmem [[threadgroup(0)]], + ushort3 tgpig[[threadgroup_position_in_grid]], + ushort sgitg[[simdgroup_index_in_threadgroup]], + ushort tiisg[[thread_index_in_simdgroup]], + ushort3 ntg[[threads_per_threadgroup]]) { + constexpr short NW = N_SIMDWIDTH; + + const short NSG = FC_solve_tri_nsg; + const short N = FC_solve_tri_n; + const short K = FC_solve_tri_k; + const short NP = PAD2(N, NW); + + const int32_t i03 = tgpig.z; + const int32_t i02 = tgpig.y; + const int32_t i01 = tgpig.x*NSG + sgitg; + + threadgroup float * sh0 = (threadgroup float *) shmem; + + device const float * src0_ptr = (device const float *)(src0 + i02 * args.nb02 + i03 * args.nb03) + sgitg*N; + device const float * src1_ptr = (device const float *)(src1 + i02 * args.nb12 + i03 * args.nb13) + i01; + device float * dst_ptr = (device float *)(dst + i02 * args.nb2 + i03 * args.nb3) + i01; + + for (short rr = 0; rr < N; rr += NSG) { + threadgroup_barrier(mem_flags::mem_threadgroup); + + { + threadgroup float * sh0_cur = sh0 + sgitg*NP; + + for (short t = 0; t*NW < N; ++t) { + const short idx = t*NW + tiisg; + sh0_cur[idx] = src0_ptr[idx]; + } + + src0_ptr += NSG*N; + } + + threadgroup_barrier(mem_flags::mem_threadgroup); + + if (i01 >= args.ne10) { + continue; + } + + for (short ir = 0; ir < NSG && rr + ir < N; ++ir) { + const short r = rr + ir; + + threadgroup float * sh0_cur = sh0 + ir*NP; + + float sum = 0.0f; + + for (short t = 0; t*NW < r; ++t) { + const short idx = t*NW + tiisg; + sum += sh0_cur[idx] * dst_ptr[idx*K] * (idx < r); + } + + sum = simd_sum(sum); + + if (tiisg == 0) { + const float diag = sh0_cur[r]; + + dst_ptr[r*K] = (src1_ptr[r*K] - sum) / diag; + } + } + } +} + +kernel void kernel_argmax_f32( + constant ggml_metal_kargs_argmax & args, + device const char * src0, + device char * dst, + threadgroup char * shmem [[threadgroup(0)]], + uint tgpig[[threadgroup_position_in_grid]], + uint tpitg[[thread_position_in_threadgroup]], + uint sgitg[[simdgroup_index_in_threadgroup]], + uint tiisg[[thread_index_in_simdgroup]], + uint ntg[[threads_per_threadgroup]]) { + device const float * x_row = (device const float *) ((device const char *) src0 + tgpig * args.nb01); + + float lmax = -INFINITY; + int32_t larg = -1; + + for (int i00 = tpitg; i00 < args.ne00; i00 += ntg) { + if (x_row[i00] > lmax) { + lmax = x_row[i00]; + larg = i00; + } + } + + // find the argmax value in the block + float max_val = simd_max(lmax); + int32_t arg_val = simd_max(select(-1, larg, lmax == max_val)); + + device int32_t * dst_i32 = (device int32_t *) dst; + + threadgroup float * shared_maxval = (threadgroup float *) shmem; + threadgroup int32_t * shared_argmax = (threadgroup int32_t *) shmem + N_SIMDWIDTH; + + if (ntg > N_SIMDWIDTH) { + if (sgitg == 0) { + shared_maxval[tiisg] = -INFINITY; + shared_argmax[tiisg] = -1; + } + + threadgroup_barrier(mem_flags::mem_threadgroup); + + if (tiisg == 0) { + shared_maxval[sgitg] = max_val; + shared_argmax[sgitg] = arg_val; + } + + threadgroup_barrier(mem_flags::mem_threadgroup); + + max_val = shared_maxval[tiisg]; + arg_val = shared_argmax[tiisg]; + + float max_val_reduced = simd_max(max_val); + int32_t arg_val_reduced = simd_max(select(-1, arg_val, max_val == max_val_reduced)); + + dst_i32[tgpig] = arg_val_reduced; + + return; + } + + dst_i32[tgpig] = arg_val; +} + +// F == 1 : norm (no fuse) +// F == 2 : norm + mul +// F == 3 : norm + mul + add +template +kernel void kernel_norm_fuse_impl( + constant ggml_metal_kargs_norm & args, + device const char * src0, + device const char * src1_0, + device const char * src1_1, + device char * dst, + threadgroup float * shmem_f32 [[threadgroup(0)]], + uint3 tgpig[[threadgroup_position_in_grid]], + ushort3 tpitg[[thread_position_in_threadgroup]], + ushort sgitg[[simdgroup_index_in_threadgroup]], + ushort tiisg[[thread_index_in_simdgroup]], + ushort3 ntg[[threads_per_threadgroup]]) { + if (sgitg == 0) { + shmem_f32[tiisg] = 0.0f; + } + + const int i01 = tgpig.x; + const int i02 = tgpig.y; + const int i03 = tgpig.z; + + device const T * x = (device const T *) (src0 + i03*args.nbf3[0] + i02*args.nbf2[0] + i01*args.nbf1[0]); + + device const T * f0 = (device const T *) (src1_0 + (i03%args.nef3[1])*args.nbf3[1] + (i02%args.nef2[1])*args.nbf2[1] + (i01%args.nef1[1])*args.nbf1[1]); + device const T * f1 = (device const T *) (src1_1 + (i03%args.nef3[2])*args.nbf3[2] + (i02%args.nef2[2])*args.nbf2[2] + (i01%args.nef1[2])*args.nbf1[2]); + + T sumft(0.0f); + + float sumf = 0.0f; + + for (int i00 = tpitg.x; i00 < args.ne00_t; i00 += ntg.x) { + sumft += x[i00]; + } + sumf = dot(sumft, T(1.0f)); + sumf = simd_sum(sumf); + + threadgroup_barrier(mem_flags::mem_threadgroup); + + if (tiisg == 0) { + shmem_f32[sgitg] = sumf; + } + + threadgroup_barrier(mem_flags::mem_threadgroup); + + sumf = shmem_f32[tiisg]; + sumf = simd_sum(sumf); + + const float mean = sumf/args.ne00; + + device T * y = (device T *) (dst + i03*args.nb3 + i02*args.nb2 + i01*args.nb1); + + sumf = 0.0f; + for (int i00 = tpitg.x; i00 < args.ne00_t; i00 += ntg.x) { + y[i00] = x[i00] - mean; + sumf += dot(y[i00], y[i00]); + } + sumf = simd_sum(sumf); + + threadgroup_barrier(mem_flags::mem_threadgroup); + + if (tiisg == 0) { + shmem_f32[sgitg] = sumf; + } + + threadgroup_barrier(mem_flags::mem_threadgroup); + + sumf = shmem_f32[tiisg]; + sumf = simd_sum(sumf); + + const float variance = sumf/args.ne00; + + const float scale = 1.0f/sqrt(variance + args.eps); + for (int i00 = tpitg.x; i00 < args.ne00_t; i00 += ntg.x) { + if (F == 1) { + y[i00] = (y[i00]*scale); + } + if (F == 2) { + y[i00] = (y[i00]*scale)*f0[i00]; + } + if (F == 3) { + y[i00] = (y[i00]*scale)*f0[i00] + f1[i00]; + } + } +} + +typedef decltype(kernel_norm_fuse_impl) kernel_norm_fuse_t; + +template [[host_name("kernel_norm_f32")]] kernel kernel_norm_fuse_t kernel_norm_fuse_impl; +template [[host_name("kernel_norm_mul_f32")]] kernel kernel_norm_fuse_t kernel_norm_fuse_impl; +template [[host_name("kernel_norm_mul_add_f32")]] kernel kernel_norm_fuse_t kernel_norm_fuse_impl; + +template [[host_name("kernel_norm_f32_4")]] kernel kernel_norm_fuse_t kernel_norm_fuse_impl; +template [[host_name("kernel_norm_mul_f32_4")]] kernel kernel_norm_fuse_t kernel_norm_fuse_impl; +template [[host_name("kernel_norm_mul_add_f32_4")]] kernel kernel_norm_fuse_t kernel_norm_fuse_impl; + +// F == 1 : rms_norm (no fuse) +// F == 2 : rms_norm + mul +// F == 3 : rms_norm + mul + add +template +kernel void kernel_rms_norm_fuse_impl( + constant ggml_metal_kargs_norm & args, + device const char * src0, + device const char * src1_0, + device const char * src1_1, + device char * dst, + threadgroup float * shmem_f32 [[threadgroup(0)]], + uint3 tgpig[[threadgroup_position_in_grid]], + ushort3 tpitg[[thread_position_in_threadgroup]], + ushort sgitg[[simdgroup_index_in_threadgroup]], + ushort tiisg[[thread_index_in_simdgroup]], + ushort3 ntg[[threads_per_threadgroup]]) { + if (sgitg == 0) { + shmem_f32[tiisg] = 0.0f; + } + + const int i01 = tgpig.x; + const int i02 = tgpig.y; + const int i03 = tgpig.z; + + device const T * x = (device const T *) (src0 + i03*args.nbf3[0] + i02*args.nbf2[0] + i01*args.nbf1[0]); + + device const T * f0 = (device const T *) (src1_0 + (i03%args.nef3[1])*args.nbf3[1] + (i02%args.nef2[1])*args.nbf2[1] + (i01%args.nef1[1])*args.nbf1[1]); + device const T * f1 = (device const T *) (src1_1 + (i03%args.nef3[2])*args.nbf3[2] + (i02%args.nef2[2])*args.nbf2[2] + (i01%args.nef1[2])*args.nbf1[2]); + + float sumf = 0.0f; + + // parallel sum + for (int i00 = tpitg.x; i00 < args.ne00_t; i00 += ntg.x) { + sumf += dot(x[i00], x[i00]); + } + sumf = simd_sum(sumf); + + threadgroup_barrier(mem_flags::mem_threadgroup); + + if (tiisg == 0) { + shmem_f32[sgitg] = sumf; + } + + threadgroup_barrier(mem_flags::mem_threadgroup); + + sumf = shmem_f32[tiisg]; + sumf = simd_sum(sumf); + + const float mean = sumf/args.ne00; + const float scale = 1.0f/sqrt(mean + args.eps); + + device T * y = (device T *) (dst + i03*args.nb3 + i02*args.nb2 + i01*args.nb1); + for (int i00 = tpitg.x; i00 < args.ne00_t; i00 += ntg.x) { + if (F == 1) { + y[i00] = (x[i00]*scale); + } + if (F == 2) { + y[i00] = (x[i00]*scale)*f0[i00]; + } + if (F == 3) { + y[i00] = (x[i00]*scale)*f0[i00] + f1[i00]; + } + } +} + +typedef decltype(kernel_rms_norm_fuse_impl) kernel_rms_norm_fuse_t; + +template [[host_name("kernel_rms_norm_f32")]] kernel kernel_rms_norm_fuse_t kernel_rms_norm_fuse_impl; +template [[host_name("kernel_rms_norm_mul_f32")]] kernel kernel_rms_norm_fuse_t kernel_rms_norm_fuse_impl; +template [[host_name("kernel_rms_norm_mul_add_f32")]] kernel kernel_rms_norm_fuse_t kernel_rms_norm_fuse_impl; + +template [[host_name("kernel_rms_norm_f32_4")]] kernel kernel_rms_norm_fuse_t kernel_rms_norm_fuse_impl; +template [[host_name("kernel_rms_norm_mul_f32_4")]] kernel kernel_rms_norm_fuse_t kernel_rms_norm_fuse_impl; +template [[host_name("kernel_rms_norm_mul_add_f32_4")]] kernel kernel_rms_norm_fuse_t kernel_rms_norm_fuse_impl; + +template +kernel void kernel_l2_norm_impl( + constant ggml_metal_kargs_l2_norm & args, + device const char * src0, + device char * dst, + threadgroup float * shmem_f32 [[threadgroup(0)]], + uint3 tgpig[[threadgroup_position_in_grid]], + ushort3 tpitg[[thread_position_in_threadgroup]], + ushort sgitg[[simdgroup_index_in_threadgroup]], + ushort tiisg[[thread_index_in_simdgroup]], + ushort3 ntg[[threads_per_threadgroup]]) { + const int i03 = tgpig.z; + const int i02 = tgpig.y; + const int i01 = tgpig.x; + + if (sgitg == 0) { + shmem_f32[tiisg] = 0.0f; + } + + device const T0 * x = (device const T0 *) (src0 + i03*args.nb03 + i02*args.nb02 + i01*args.nb01); + device T * y = (device T *) (dst + i03*args.nb3 + i02*args.nb2 + i01*args.nb1); + + float sumf = 0.0f; + + // parallel sum + for (int i00 = tpitg.x; i00 < args.ne00; i00 += ntg.x) { + sumf += dot(x[i00], x[i00]); + } + sumf = simd_sum(sumf); + + threadgroup_barrier(mem_flags::mem_threadgroup); + + if (tiisg == 0) { + shmem_f32[sgitg] = sumf; + } + + threadgroup_barrier(mem_flags::mem_threadgroup); + + sumf = shmem_f32[tiisg]; + sumf = simd_sum(sumf); + + const float scale = 1.0f/max(sqrt(sumf), args.eps); + + for (int i00 = tpitg.x; i00 < args.ne00; i00 += ntg.x) { + y[i00] = x[i00] * scale; + } +} + +typedef decltype(kernel_l2_norm_impl) kernel_l2_norm_t; + +template [[host_name("kernel_l2_norm_f32_f32")]] kernel kernel_l2_norm_t kernel_l2_norm_impl; +template [[host_name("kernel_l2_norm_f32_f32_4")]] kernel kernel_l2_norm_t kernel_l2_norm_impl; + +kernel void kernel_group_norm_f32( + constant ggml_metal_kargs_group_norm & args, + device const float * src0, + device float * dst, + threadgroup float * buf [[threadgroup(0)]], + uint tgpig[[threadgroup_position_in_grid]], + uint tpitg[[thread_position_in_threadgroup]], + uint sgitg[[simdgroup_index_in_threadgroup]], + uint tiisg[[thread_index_in_simdgroup]], + uint ntg[[threads_per_threadgroup]]) { + const int64_t ne = args.ne00*args.ne01*args.ne02; + const int64_t gs = args.ne00*args.ne01*((args.ne02 + args.ngrp - 1) / args.ngrp); + + int start = tgpig * gs; + int end = start + gs; + + start += tpitg; + + if (end >= ne) { + end = ne; + } + + float tmp = 0.0f; // partial sum for thread in warp + + for (int j = start; j < end; j += ntg) { + tmp += src0[j]; + } + + threadgroup_barrier(mem_flags::mem_threadgroup); + tmp = simd_sum(tmp); + if (ntg > N_SIMDWIDTH) { + if (sgitg == 0) { + buf[tiisg] = 0.0f; + } + + threadgroup_barrier(mem_flags::mem_threadgroup); + + if (tiisg == 0) { + buf[sgitg] = tmp; + } + + threadgroup_barrier(mem_flags::mem_threadgroup); + + tmp = buf[tiisg]; + tmp = simd_sum(tmp); + } + + const float mean = tmp / gs; + tmp = 0.0f; + + for (int j = start; j < end; j += ntg) { + float xi = src0[j] - mean; + dst[j] = xi; + tmp += xi * xi; + } + + tmp = simd_sum(tmp); + if (ntg > N_SIMDWIDTH) { + if (sgitg == 0) { + buf[tiisg] = 0.0f; + } + + threadgroup_barrier(mem_flags::mem_threadgroup); + + if (tiisg == 0) { + buf[sgitg] = tmp; + } + + threadgroup_barrier(mem_flags::mem_threadgroup); + + tmp = buf[tiisg]; + tmp = simd_sum(tmp); + } + + const float variance = tmp / gs; + const float scale = 1.0f/sqrt(variance + args.eps); + for (int j = start; j < end; j += ntg) { + dst[j] *= scale; + } +} + +// function for calculate inner product between half a q4_0 block and 16 floats (yl), sumy is SUM(yl[i]) +// il indicates where the q4 quants begin (0 or QK4_0/4) +// we assume that the yl's have been multiplied with the appropriate scale factor +// that corresponds to the missing bit shifts (1, 1/16, 1/256, 1/4096) +inline float block_q_n_dot_y(device const block_q4_0 * qb_curr, float sumy, thread float * yl, int il) { + float d = qb_curr->d; + + float acc[4] = { 0.0f, 0.0f, 0.0f, 0.0f }; + + device const uint16_t * qs = ((device const uint16_t *) qb_curr + 1 + il/2); + + for (int i = 0; i < 8; i += 2) { + acc[0] += yl[i + 0] * (qs[i / 2] & 0x000F); + acc[1] += yl[i + 1] * (qs[i / 2] & 0x0F00); + acc[2] += yl[i + 8] * (qs[i / 2] & 0x00F0); + acc[3] += yl[i + 9] * (qs[i / 2] & 0xF000); + } + + return d * (sumy * -8.f + acc[0] + acc[1] + acc[2] + acc[3]); +} + +// function for calculate inner product between half a q4_1 block and 16 floats (yl), sumy is SUM(yl[i]) +// il indicates where the q4 quants begin (0 or QK4_0/4) +// we assume that the yl's have been multiplied with the appropriate scale factor +// that corresponds to the missing bit shifts (1, 1/16, 1/256, 1/4096) +inline float block_q_n_dot_y(device const block_q4_1 * qb_curr, float sumy, thread float * yl, int il) { + float d = qb_curr->d; + float m = qb_curr->m; + + float acc[4] = { 0.0f, 0.0f, 0.0f, 0.0f }; + + device const uint16_t * qs = ((device const uint16_t *) qb_curr + 2 + il/2); + + for (int i = 0; i < 8; i+=2) { + acc[0] += yl[i + 0] * (qs[i / 2] & 0x000F); + acc[1] += yl[i + 1] * (qs[i / 2] & 0x0F00); + acc[2] += yl[i + 8] * (qs[i / 2] & 0x00F0); + acc[3] += yl[i + 9] * (qs[i / 2] & 0xF000); + } + + return d * (acc[0] + acc[1] + acc[2] + acc[3]) + sumy * m; +} + +// function for calculate inner product between half a q5_0 block and 16 floats (yl), sumy is SUM(yl[i]) +// il indicates where the q5 quants begin (0 or QK5_0/4) +// we assume that the yl's have been multiplied with the appropriate scale factor +// that corresponds to the missing bit shifts (1, 1/16, 1/256, 1/4096) +inline float block_q_n_dot_y(device const block_q5_0 * qb_curr, float sumy, thread float * yl, int il) { + float d = qb_curr->d; + + float acc[4] = { 0.0f, 0.0f, 0.0f, 0.0f }; + + device const uint16_t * qs = ((device const uint16_t *)qb_curr + 3 + il/2); + const uint32_t qh = *((device const uint32_t *)qb_curr->qh); + + for (int i = 0; i < 8; i+=2) { + acc[0] += yl[i + 0] * ((qs[i / 2] & 0x000F) | ((qh >> (i+0+il ) << 4 ) & 0x00010)); + acc[1] += yl[i + 1] * ((qs[i / 2] & 0x0F00) | ((qh >> (i+1+il ) << 12) & 0x01000)); + acc[2] += yl[i + 8] * ((qs[i / 2] & 0x00F0) | ((qh >> (i+0+il+QK5_0/2) << 8 ) & 0x00100)); + acc[3] += yl[i + 9] * ((qs[i / 2] & 0xF000) | ((qh >> (i+1+il+QK5_0/2) << 16) & 0x10000)); + } + + return d * (sumy * -16.f + acc[0] + acc[1] + acc[2] + acc[3]); +} + +// function for calculate inner product between half a q5_1 block and 16 floats (yl), sumy is SUM(yl[i]) +// il indicates where the q5 quants begin (0 or QK5_1/4) +// we assume that the yl's have been multiplied with the appropriate scale factor +// that corresponds to the missing bit shifts (1, 1/16, 1/256, 1/4096) +inline float block_q_n_dot_y(device const block_q5_1 * qb_curr, float sumy, thread float * yl, int il) { + float d = qb_curr->d; + float m = qb_curr->m; + + float acc[4] = { 0.0f, 0.0f, 0.0f, 0.0f }; + + device const uint16_t * qs = ((device const uint16_t *)qb_curr + 4 + il/2); + const uint32_t qh = *((device const uint32_t *)qb_curr->qh); + + for (int i = 0; i < 8; i+=2) { + acc[0] += yl[i + 0] * ((qs[i / 2] & 0x000F) | ((qh >> (i+0+il ) << 4 ) & 0x00010)); + acc[1] += yl[i + 1] * ((qs[i / 2] & 0x0F00) | ((qh >> (i+1+il ) << 12) & 0x01000)); + acc[2] += yl[i + 8] * ((qs[i / 2] & 0x00F0) | ((qh >> (i+0+il+QK5_0/2) << 8 ) & 0x00100)); + acc[3] += yl[i + 9] * ((qs[i / 2] & 0xF000) | ((qh >> (i+1+il+QK5_0/2) << 16) & 0x10000)); + } + + return d * (acc[0] + acc[1] + acc[2] + acc[3]) + sumy * m; +} + +template +static inline void helper_mv_reduce_and_write( + device float * dst_f32, + float sumf[NR0], + const int r0, + const int ne01, + ushort tiisg, + ushort sgitg, + threadgroup char * shmem) { + constexpr short NW = N_SIMDWIDTH; + + threadgroup float * shmem_f32[NR0]; + + for (short row = 0; row < NR0; ++row) { + shmem_f32[row] = (threadgroup float *) shmem + NW*row; + + if (sgitg == 0) { + shmem_f32[row][tiisg] = 0.0f; + } + + sumf[row] = simd_sum(sumf[row]); + } + + threadgroup_barrier(mem_flags::mem_threadgroup); + + for (short row = 0; row < NR0; ++row) { + if (tiisg == 0) { + shmem_f32[row][sgitg] = sumf[row]; + } + } + + threadgroup_barrier(mem_flags::mem_threadgroup); + + for (short row = 0; row < NR0 && r0 + row < ne01; ++row) { + float tot = simd_sum(shmem_f32[row][tiisg]); + + if (tiisg == 0 && sgitg == 0) { + dst_f32[r0 + row] = tot; + } + } +} + +constant short FC_mul_mv_nsg [[function_constant(FC_MUL_MV + 0)]]; +constant short FC_mul_mv_nxpsg [[function_constant(FC_MUL_MV + 1)]]; + +template +void mul_vec_q_n_f32_impl( + args_t args, + device const char * src0, + device const char * src1, + device char * dst, + threadgroup char * shmem, + uint3 tgpig, + ushort tiisg, + ushort sgitg) { + const short NSG = FC_mul_mv_nsg; + + constexpr short NW = N_SIMDWIDTH; + constexpr short NQ = 16; + + const int nb = args.ne00/QK4_0; + + const int r0 = (tgpig.x*NSG + sgitg)*NR0; + //const int r0 = tgpig.x*NR0; + const int r1 = tgpig.y; + const int im = tgpig.z; + + const uint i12 = im%args.ne12; + const uint i13 = im/args.ne12; + + //const uint64_t offset0 = r0*args.nb01 + (i12/args.r2)*args.nb02 + (i13/args.r3)*args.nb03; + const uint64_t offset1 = r1*args.nb11 + (i12 )*args.nb12 + (i13 )*args.nb13; + + //device const block_q_type * x = (device const block_q_type *) (src0 + offset0); + device const float * y = (device const float *) (src1 + offset1); + + // pointers to src0 rows + device const block_q_type * ax[NR0]; + FOR_UNROLL (int row = 0; row < NR0; ++row) { + const uint64_t offset0 = (r0 + row)*args.nb01 + (i12/args.r2)*args.nb02 + (i13/args.r3)*args.nb03; + + ax[row] = (device const block_q_type *) ((device char *) src0 + offset0); + } + + float sumf[NR0] = {0.f}; + + const short ix = (tiisg/(NW/NQ)); + const short il = (tiisg%(NW/NQ))*8; + + //const int ib0 = sgitg*NQ + ix; + const int ib0 = ix; + + float yl[16]; // src1 vector cache + + //device const float * yb = y + ix*QK4_0 + il; + device const float * yb = y + ib0*QK4_0 + il; + + // each thread in a SIMD group deals with half a block. + //for (int ib = ib0; ib < nb; ib += NSG*NQ) { + for (int ib = ib0; ib < nb; ib += NQ) { + float sumy[2] = { 0.f, 0.f }; + + FOR_UNROLL (short i = 0; i < 8; i += 2) { + sumy[0] += yb[i + 0] + yb[i + 1]; + yl[i + 0] = yb[i + 0]; + yl[i + 1] = yb[i + 1]/256.f; + + sumy[1] += yb[i + 16] + yb[i + 17]; + yl[i + 8] = yb[i + 16]/16.f; + yl[i + 9] = yb[i + 17]/4096.f; + } + + FOR_UNROLL (short row = 0; row < NR0; row++) { + sumf[row] += block_q_n_dot_y(ax[row] + ib, sumy[0] + sumy[1], yl, il); + } + + yb += QK4_0 * 16; + //yb += NSG*NQ*QK4_0; + } + + device float * dst_f32 = (device float *) dst + im*args.ne0*args.ne1 + r1*args.ne0; + + //helper_mv_reduce_and_write(dst_f32, sumf, r0, args.ne01, tiisg, sgitg, shmem); + + for (int row = 0; row < NR0; ++row) { + const float tot = simd_sum(sumf[row]); + + if (tiisg == 0 && r0 + row < args.ne01) { + dst_f32[r0 + row] = tot; + } + } +} + +kernel void kernel_mul_mv_q4_0_f32( + constant ggml_metal_kargs_mul_mv & args, + device const char * src0, + device const char * src1, + device char * dst, + threadgroup char * shmem [[threadgroup(0)]], + uint3 tgpig[[threadgroup_position_in_grid]], + ushort tiisg[[thread_index_in_simdgroup]], + ushort sgitg[[simdgroup_index_in_threadgroup]]) { + mul_vec_q_n_f32_impl(args, src0, src1, dst, shmem, tgpig, tiisg, sgitg); +} + +kernel void kernel_mul_mv_q4_1_f32( + constant ggml_metal_kargs_mul_mv & args, + device const char * src0, + device const char * src1, + device char * dst, + threadgroup char * shmem [[threadgroup(0)]], + uint3 tgpig[[threadgroup_position_in_grid]], + ushort tiisg[[thread_index_in_simdgroup]], + ushort sgitg[[simdgroup_index_in_threadgroup]]) { + mul_vec_q_n_f32_impl(args, src0, src1, dst, shmem, tgpig, tiisg, sgitg); +} + +kernel void kernel_mul_mv_q5_0_f32( + constant ggml_metal_kargs_mul_mv & args, + device const char * src0, + device const char * src1, + device char * dst, + threadgroup char * shmem [[threadgroup(0)]], + uint3 tgpig[[threadgroup_position_in_grid]], + ushort tiisg[[thread_index_in_simdgroup]], + ushort sgitg[[simdgroup_index_in_threadgroup]]) { + mul_vec_q_n_f32_impl(args, src0, src1, dst, shmem, tgpig, tiisg, sgitg); +} + +kernel void kernel_mul_mv_q5_1_f32( + constant ggml_metal_kargs_mul_mv & args, + device const char * src0, + device const char * src1, + device char * dst, + threadgroup char * shmem [[threadgroup(0)]], + uint3 tgpig[[threadgroup_position_in_grid]], + ushort tiisg[[thread_index_in_simdgroup]], + ushort sgitg[[simdgroup_index_in_threadgroup]]) { + mul_vec_q_n_f32_impl(args, src0, src1, dst, shmem, tgpig, tiisg, sgitg); +} + +template +void kernel_mul_mv_q8_0_f32_impl( + args_t args, + device const char * src0, + device const char * src1, + device char * dst, + threadgroup char * shmem, + uint3 tgpig, + ushort tiisg, + ushort sgitg) { + const short NSG = FC_mul_mv_nsg; + + constexpr short NW = N_SIMDWIDTH; + constexpr short NQ = 8; + + const int nb = args.ne00/QK8_0; + + const int r0 = tgpig.x*NR0; + const int r1 = tgpig.y; + const int im = tgpig.z; + + const uint i12 = im%args.ne12; + const uint i13 = im/args.ne12; + + //const uint64_t offset0 = r0*args.nb01 + (i12/args.r2)*args.nb02 + (i13/args.r3)*args.nb03; + const uint64_t offset1 = r1*args.nb11 + (i12 )*args.nb12 + (i13 )*args.nb13; + + //device const block_q8_0 * x = (device const block_q8_0 *) (src0 + offset0); + device const float * y = (device const float *) (src1 + offset1); + + // pointers to src0 rows + device const block_q8_0 * ax[NR0]; + FOR_UNROLL (short row = 0; row < NR0; ++row) { + const uint64_t offset0 = (r0 + row)*args.nb01 + (i12/args.r2)*args.nb02 + (i13/args.r3)*args.nb03; + + ax[row] = (device const block_q8_0 *) ((device char *) src0 + offset0); + } + + float sumf[NR0] = { 0.f }; + + const short ix = tiisg/(NW/NQ); + const short il = tiisg%(NW/NQ); + + const int ib0 = sgitg*NQ + ix; + + float yl[NQ]; + + device const float * yb = y + ib0*QK8_0 + il*NQ; + + // each thread in a SIMD group deals with NQ quants at a time + for (int ib = ib0; ib < nb; ib += NSG*NQ) { + for (short i = 0; i < NQ; ++i) { + yl[i] = yb[i]; + } + + for (short row = 0; row < NR0; row++) { + device const int8_t * qs = ax[row][ib].qs + il*NQ; + + float sumq = 0.f; + FOR_UNROLL (short i = 0; i < NQ; ++i) { + sumq += qs[i] * yl[i]; + } + + sumf[row] += sumq*ax[row][ib].d; + } + + yb += NSG*NQ*QK8_0; + } + + device float * dst_f32 = (device float *) dst + (uint64_t)im*args.ne0*args.ne1 + (uint64_t)r1*args.ne0; + + helper_mv_reduce_and_write(dst_f32, sumf, r0, args.ne01, tiisg, sgitg, shmem); +} + +[[host_name("kernel_mul_mv_q8_0_f32")]] +kernel void kernel_mul_mv_q8_0_f32( + constant ggml_metal_kargs_mul_mv & args, + device const char * src0, + device const char * src1, + device char * dst, + threadgroup char * shmem [[threadgroup(0)]], + uint3 tgpig[[threadgroup_position_in_grid]], + ushort tiisg[[thread_index_in_simdgroup]], + ushort sgitg[[simdgroup_index_in_threadgroup]]) { + kernel_mul_mv_q8_0_f32_impl(args, src0, src1, dst, shmem, tgpig, tiisg, sgitg); +} + +// mat-vec kernel processing in chunks of float4 +// chpb - chunks per quantization block +template +void kernel_mul_mv_ext_q4_f32_impl( + constant ggml_metal_kargs_mul_mv_ext & args, + device const char * src0, + device const char * src1, + device char * dst, + uint3 tgpig[[threadgroup_position_in_grid]], + ushort tiisg[[thread_index_in_simdgroup]], + ushort sgitg[[simdgroup_index_in_threadgroup]]) { + const short NSG = FC_mul_mv_nsg; + const short nxpsg = FC_mul_mv_nxpsg; + + const short chpt = 4; // chunks per thread + + //const short nxpsg = (32); + const short nypsg = (32/nxpsg); + + const short tx = tiisg%nxpsg; + const short ty = tiisg/nxpsg; + + const int i01 = tgpig.x*(nypsg*NSG) + nypsg*sgitg + ty; + const int i11 = tgpig.y*r1ptg; + const int i1m = tgpig.z; + + const int i12 = i1m%args.ne12; + const int i13 = i1m/args.ne12; + + const uint64_t offset0 = i01*args.nb01 + (i12/args.r2)*args.nb02 + (i13/args.r3)*args.nb03; + const uint64_t offset1 = i11*args.nb11 + (i12 )*args.nb12 + (i13 )*args.nb13; + + device const q_t * xq = (i01 < args.ne01) ? (device const q_t *) (src0 + offset0) + tx/chpb : (device const q_t *) src0; + + device const float4 * y4[r1ptg]; + + for (int ir1 = 0; ir1 < r1ptg; ++ir1) { + y4[ir1] = (i11 + ir1 < args.ne11) ? (device const float4 *) (src1 + offset1 + ir1*args.nb11) + tx : (device const float4 *) src1; + } + + float sumf[r1ptg] = { [ 0 ... r1ptg - 1 ] = 0.0f }; + + short cch = tx%chpb; // current chunk index + + for (int ich = tx; 4*ich < args.ne00; ich += chpt*nxpsg) { + float4 lx[chpt]; + +#pragma unroll(chpt) + for (short ch = 0; ch < chpt; ++ch) { + deq_t4(xq, cch, lx[ch]); + + cch += nxpsg; + if (cch >= chpb) { + xq += cch/chpb; + cch %= chpb; + } + } + +#pragma unroll(chpt) + for (short ch = 0; ch < chpt; ++ch) { +#pragma unroll(r1ptg) + for (short ir1 = 0; ir1 < r1ptg; ++ir1) { + sumf[ir1] += dot(lx[ch], y4[ir1][ch*nxpsg]); + } + } + +#pragma unroll(r1ptg) + for (short ir1 = 0; ir1 < r1ptg; ++ir1) { + y4[ir1] += chpt*nxpsg; + } + } + + // reduce only the threads in each row + for (short ir1 = 0; ir1 < r1ptg; ++ir1) { + if (nxpsg >= 32) { + sumf[ir1] += simd_shuffle_down(sumf[ir1], 16); + } + if (nxpsg >= 16) { + sumf[ir1] += simd_shuffle_down(sumf[ir1], 8); + } + if (nxpsg >= 8) { + sumf[ir1] += simd_shuffle_down(sumf[ir1], 4); + } + if (nxpsg >= 4) { + sumf[ir1] += simd_shuffle_down(sumf[ir1], 2); + } + if (nxpsg >= 2) { + sumf[ir1] += simd_shuffle_down(sumf[ir1], 1); + } + + //sumf[ir1] = simd_sum(sumf[ir1]); + } + + if (tx == 0) { + for (short ir1 = 0; ir1 < r1ptg && i11 + ir1 < args.ne11; ++ir1) { + device float * dst_f32 = (device float *) dst + (uint64_t)i1m*args.ne0*args.ne1 + (uint64_t)(i11 + ir1)*args.ne0; + + if (i01 < args.ne01) { + dst_f32[i01] = sumf[ir1]; + } + } + } +} + +// mat-vec kernel processing in chunks of float4x4 +template +void kernel_mul_mv_ext_q4x4_f32_impl( + constant ggml_metal_kargs_mul_mv_ext & args, + device const char * src0, + device const char * src1, + device char * dst, + uint3 tgpig[[threadgroup_position_in_grid]], + ushort tiisg[[thread_index_in_simdgroup]], + ushort sgitg[[simdgroup_index_in_threadgroup]]) { + const short NSG = FC_mul_mv_nsg; + const short nxpsg = FC_mul_mv_nxpsg; + + const short chpt = 1; + + //const short nxpsg = (32); + const short nypsg = (32/nxpsg); + + const short tx = tiisg%nxpsg; + const short ty = tiisg/nxpsg; + + const int i01 = tgpig.x*(nypsg*NSG) + nypsg*sgitg + ty; + const int i11 = tgpig.y*r1ptg; + const int i1m = tgpig.z; + + const int i12 = i1m%args.ne12; + const int i13 = i1m/args.ne12; + + const uint64_t offset0 = i01*args.nb01 + (i12/args.r2)*args.nb02 + (i13/args.r3)*args.nb03; + const uint64_t offset1 = i11*args.nb11 + (i12 )*args.nb12 + (i13 )*args.nb13; + + device const q_t * xq = (i01 < args.ne01) ? (device const q_t *) (src0 + offset0) + tx/chpb : (device const q_t *) src0; + + device const float4x4 * y4x4[r1ptg]; + + for (int ir1 = 0; ir1 < r1ptg; ++ir1) { + y4x4[ir1] = (i11 + ir1 < args.ne11) ? (device const float4x4 *) (src1 + offset1 + ir1*args.nb11) + tx : (device const float4x4 *) src1; + } + + float sumf[r1ptg] = { [ 0 ... r1ptg - 1 ] = 0.0f }; + + short cch = tx%chpb; + + for (int ich = tx; 16*ich < args.ne00; ich += chpt*nxpsg) { + float4x4 lx[chpt]; + +#pragma unroll(chpt) + for (short ch = 0; ch < chpt; ++ch) { + deq_t4x4(xq, cch, lx[ch]); + + cch += nxpsg; + if (cch >= chpb) { + xq += cch/chpb; + cch %= chpb; + } + } + +#pragma unroll(chpt) + for (short ch = 0; ch < chpt; ++ch) { +#pragma unroll(r1ptg) + for (short ir1 = 0; ir1 < r1ptg; ++ir1) { + sumf[ir1] += + dot(lx[ch][0], y4x4[ir1][ch*nxpsg][0]) + + dot(lx[ch][1], y4x4[ir1][ch*nxpsg][1]) + + dot(lx[ch][2], y4x4[ir1][ch*nxpsg][2]) + + dot(lx[ch][3], y4x4[ir1][ch*nxpsg][3]); + + } + } + +#pragma unroll(r1ptg) + for (short ir1 = 0; ir1 < r1ptg; ++ir1) { + y4x4[ir1] += chpt*nxpsg; + } + } + + for (short ir1 = 0; ir1 < r1ptg; ++ir1) { + if (nxpsg >= 32) { + sumf[ir1] += simd_shuffle_down(sumf[ir1], 16); + } + if (nxpsg >= 16) { + sumf[ir1] += simd_shuffle_down(sumf[ir1], 8); + } + if (nxpsg >= 8) { + sumf[ir1] += simd_shuffle_down(sumf[ir1], 4); + } + if (nxpsg >= 4) { + sumf[ir1] += simd_shuffle_down(sumf[ir1], 2); + } + if (nxpsg >= 2) { + sumf[ir1] += simd_shuffle_down(sumf[ir1], 1); + } + + //sumf[ir1] = simd_sum(sumf[ir1]); + } + + if (tx == 0) { + for (short ir1 = 0; ir1 < r1ptg && i11 + ir1 < args.ne11; ++ir1) { + device float * dst_f32 = (device float *) dst + (uint64_t)i1m*args.ne0*args.ne1 + (uint64_t)(i11 + ir1)*args.ne0; + + if (i01 < args.ne01) { + dst_f32[i01] = sumf[ir1]; + } + } + } +} + +// dispatchers needed for compile-time nxpsg +// epb - elements per quantization block +template +kernel void kernel_mul_mv_ext_q4_f32_disp( + constant ggml_metal_kargs_mul_mv_ext & args, + device const char * src0, + device const char * src1, + device char * dst, + uint3 tgpig[[threadgroup_position_in_grid]], + ushort tiisg[[thread_index_in_simdgroup]], + ushort sgitg[[simdgroup_index_in_threadgroup]]) { + kernel_mul_mv_ext_q4_f32_impl(args, src0, src1, dst, tgpig, tiisg, sgitg); +} + +template +kernel void kernel_mul_mv_ext_q4x4_f32_disp( + constant ggml_metal_kargs_mul_mv_ext & args, + device const char * src0, + device const char * src1, + device char * dst, + uint3 tgpig[[threadgroup_position_in_grid]], + ushort tiisg[[thread_index_in_simdgroup]], + ushort sgitg[[simdgroup_index_in_threadgroup]]) { + kernel_mul_mv_ext_q4x4_f32_impl(args, src0, src1, dst, tgpig, tiisg, sgitg); +} + +typedef decltype(kernel_mul_mv_ext_q4_f32_disp <2, block_q8_0, 32, dequantize_q8_0_t4>) mul_mv_ext_q4_f32_t; +typedef decltype(kernel_mul_mv_ext_q4x4_f32_disp<2, block_q4_K, 256, dequantize_q4_K>) mul_mv_ext_q4x4_f32_t; + +template [[host_name("kernel_mul_mv_ext_f32_f32_r1_2")]] kernel mul_mv_ext_q4_f32_t kernel_mul_mv_ext_q4_f32_disp<2, float4, 4, dequantize_f32_t4>; +template [[host_name("kernel_mul_mv_ext_f32_f32_r1_3")]] kernel mul_mv_ext_q4_f32_t kernel_mul_mv_ext_q4_f32_disp<3, float4, 4, dequantize_f32_t4>; +template [[host_name("kernel_mul_mv_ext_f32_f32_r1_4")]] kernel mul_mv_ext_q4_f32_t kernel_mul_mv_ext_q4_f32_disp<4, float4, 4, dequantize_f32_t4>; +template [[host_name("kernel_mul_mv_ext_f32_f32_r1_5")]] kernel mul_mv_ext_q4_f32_t kernel_mul_mv_ext_q4_f32_disp<5, float4, 4, dequantize_f32_t4>; + +template [[host_name("kernel_mul_mv_ext_f16_f32_r1_2")]] kernel mul_mv_ext_q4_f32_t kernel_mul_mv_ext_q4_f32_disp<2, half4, 4, dequantize_f16_t4>; +template [[host_name("kernel_mul_mv_ext_f16_f32_r1_3")]] kernel mul_mv_ext_q4_f32_t kernel_mul_mv_ext_q4_f32_disp<3, half4, 4, dequantize_f16_t4>; +template [[host_name("kernel_mul_mv_ext_f16_f32_r1_4")]] kernel mul_mv_ext_q4_f32_t kernel_mul_mv_ext_q4_f32_disp<4, half4, 4, dequantize_f16_t4>; +template [[host_name("kernel_mul_mv_ext_f16_f32_r1_5")]] kernel mul_mv_ext_q4_f32_t kernel_mul_mv_ext_q4_f32_disp<5, half4, 4, dequantize_f16_t4>; + +#if defined(GGML_METAL_HAS_BF16) +template [[host_name("kernel_mul_mv_ext_bf16_f32_r1_2")]] kernel mul_mv_ext_q4_f32_t kernel_mul_mv_ext_q4_f32_disp<2, bfloat4, 4, dequantize_bf16_t4>; +template [[host_name("kernel_mul_mv_ext_bf16_f32_r1_3")]] kernel mul_mv_ext_q4_f32_t kernel_mul_mv_ext_q4_f32_disp<3, bfloat4, 4, dequantize_bf16_t4>; +template [[host_name("kernel_mul_mv_ext_bf16_f32_r1_4")]] kernel mul_mv_ext_q4_f32_t kernel_mul_mv_ext_q4_f32_disp<4, bfloat4, 4, dequantize_bf16_t4>; +template [[host_name("kernel_mul_mv_ext_bf16_f32_r1_5")]] kernel mul_mv_ext_q4_f32_t kernel_mul_mv_ext_q4_f32_disp<5, bfloat4, 4, dequantize_bf16_t4>; +#endif + +template [[host_name("kernel_mul_mv_ext_q4_0_f32_r1_2")]] kernel mul_mv_ext_q4_f32_t kernel_mul_mv_ext_q4_f32_disp<2, block_q4_0, 32, dequantize_q4_0_t4>; +template [[host_name("kernel_mul_mv_ext_q4_0_f32_r1_3")]] kernel mul_mv_ext_q4_f32_t kernel_mul_mv_ext_q4_f32_disp<3, block_q4_0, 32, dequantize_q4_0_t4>; +template [[host_name("kernel_mul_mv_ext_q4_0_f32_r1_4")]] kernel mul_mv_ext_q4_f32_t kernel_mul_mv_ext_q4_f32_disp<4, block_q4_0, 32, dequantize_q4_0_t4>; +template [[host_name("kernel_mul_mv_ext_q4_0_f32_r1_5")]] kernel mul_mv_ext_q4_f32_t kernel_mul_mv_ext_q4_f32_disp<5, block_q4_0, 32, dequantize_q4_0_t4>; + +template [[host_name("kernel_mul_mv_ext_q4_1_f32_r1_2")]] kernel mul_mv_ext_q4_f32_t kernel_mul_mv_ext_q4_f32_disp<2, block_q4_1, 32, dequantize_q4_1_t4>; +template [[host_name("kernel_mul_mv_ext_q4_1_f32_r1_3")]] kernel mul_mv_ext_q4_f32_t kernel_mul_mv_ext_q4_f32_disp<3, block_q4_1, 32, dequantize_q4_1_t4>; +template [[host_name("kernel_mul_mv_ext_q4_1_f32_r1_4")]] kernel mul_mv_ext_q4_f32_t kernel_mul_mv_ext_q4_f32_disp<4, block_q4_1, 32, dequantize_q4_1_t4>; +template [[host_name("kernel_mul_mv_ext_q4_1_f32_r1_5")]] kernel mul_mv_ext_q4_f32_t kernel_mul_mv_ext_q4_f32_disp<5, block_q4_1, 32, dequantize_q4_1_t4>; + +template [[host_name("kernel_mul_mv_ext_q5_0_f32_r1_2")]] kernel mul_mv_ext_q4_f32_t kernel_mul_mv_ext_q4_f32_disp<2, block_q5_0, 32, dequantize_q5_0_t4>; +template [[host_name("kernel_mul_mv_ext_q5_0_f32_r1_3")]] kernel mul_mv_ext_q4_f32_t kernel_mul_mv_ext_q4_f32_disp<3, block_q5_0, 32, dequantize_q5_0_t4>; +template [[host_name("kernel_mul_mv_ext_q5_0_f32_r1_4")]] kernel mul_mv_ext_q4_f32_t kernel_mul_mv_ext_q4_f32_disp<4, block_q5_0, 32, dequantize_q5_0_t4>; +template [[host_name("kernel_mul_mv_ext_q5_0_f32_r1_5")]] kernel mul_mv_ext_q4_f32_t kernel_mul_mv_ext_q4_f32_disp<5, block_q5_0, 32, dequantize_q5_0_t4>; + +template [[host_name("kernel_mul_mv_ext_q5_1_f32_r1_2")]] kernel mul_mv_ext_q4_f32_t kernel_mul_mv_ext_q4_f32_disp<2, block_q5_1, 32, dequantize_q5_1_t4>; +template [[host_name("kernel_mul_mv_ext_q5_1_f32_r1_3")]] kernel mul_mv_ext_q4_f32_t kernel_mul_mv_ext_q4_f32_disp<3, block_q5_1, 32, dequantize_q5_1_t4>; +template [[host_name("kernel_mul_mv_ext_q5_1_f32_r1_4")]] kernel mul_mv_ext_q4_f32_t kernel_mul_mv_ext_q4_f32_disp<4, block_q5_1, 32, dequantize_q5_1_t4>; +template [[host_name("kernel_mul_mv_ext_q5_1_f32_r1_5")]] kernel mul_mv_ext_q4_f32_t kernel_mul_mv_ext_q4_f32_disp<5, block_q5_1, 32, dequantize_q5_1_t4>; + +template [[host_name("kernel_mul_mv_ext_q8_0_f32_r1_2")]] kernel mul_mv_ext_q4_f32_t kernel_mul_mv_ext_q4_f32_disp<2, block_q8_0, 32, dequantize_q8_0_t4>; +template [[host_name("kernel_mul_mv_ext_q8_0_f32_r1_3")]] kernel mul_mv_ext_q4_f32_t kernel_mul_mv_ext_q4_f32_disp<3, block_q8_0, 32, dequantize_q8_0_t4>; +template [[host_name("kernel_mul_mv_ext_q8_0_f32_r1_4")]] kernel mul_mv_ext_q4_f32_t kernel_mul_mv_ext_q4_f32_disp<4, block_q8_0, 32, dequantize_q8_0_t4>; +template [[host_name("kernel_mul_mv_ext_q8_0_f32_r1_5")]] kernel mul_mv_ext_q4_f32_t kernel_mul_mv_ext_q4_f32_disp<5, block_q8_0, 32, dequantize_q8_0_t4>; + +template [[host_name("kernel_mul_mv_ext_mxfp4_f32_r1_2")]] kernel mul_mv_ext_q4_f32_t kernel_mul_mv_ext_q4_f32_disp<2, block_mxfp4, 32, dequantize_mxfp4_t4>; +template [[host_name("kernel_mul_mv_ext_mxfp4_f32_r1_3")]] kernel mul_mv_ext_q4_f32_t kernel_mul_mv_ext_q4_f32_disp<3, block_mxfp4, 32, dequantize_mxfp4_t4>; +template [[host_name("kernel_mul_mv_ext_mxfp4_f32_r1_4")]] kernel mul_mv_ext_q4_f32_t kernel_mul_mv_ext_q4_f32_disp<4, block_mxfp4, 32, dequantize_mxfp4_t4>; +template [[host_name("kernel_mul_mv_ext_mxfp4_f32_r1_5")]] kernel mul_mv_ext_q4_f32_t kernel_mul_mv_ext_q4_f32_disp<5, block_mxfp4, 32, dequantize_mxfp4_t4>; + +template [[host_name("kernel_mul_mv_ext_iq4_nl_f32_r1_2")]] kernel mul_mv_ext_q4_f32_t kernel_mul_mv_ext_q4_f32_disp<2, block_iq4_nl, 32, dequantize_iq4_nl_t4>; +template [[host_name("kernel_mul_mv_ext_iq4_nl_f32_r1_3")]] kernel mul_mv_ext_q4_f32_t kernel_mul_mv_ext_q4_f32_disp<3, block_iq4_nl, 32, dequantize_iq4_nl_t4>; +template [[host_name("kernel_mul_mv_ext_iq4_nl_f32_r1_4")]] kernel mul_mv_ext_q4_f32_t kernel_mul_mv_ext_q4_f32_disp<4, block_iq4_nl, 32, dequantize_iq4_nl_t4>; +template [[host_name("kernel_mul_mv_ext_iq4_nl_f32_r1_5")]] kernel mul_mv_ext_q4_f32_t kernel_mul_mv_ext_q4_f32_disp<5, block_iq4_nl, 32, dequantize_iq4_nl_t4>; + +template [[host_name("kernel_mul_mv_ext_q4_K_f32_r1_2")]] kernel mul_mv_ext_q4x4_f32_t kernel_mul_mv_ext_q4x4_f32_disp<2, block_q4_K, 256, dequantize_q4_K>; +template [[host_name("kernel_mul_mv_ext_q4_K_f32_r1_3")]] kernel mul_mv_ext_q4x4_f32_t kernel_mul_mv_ext_q4x4_f32_disp<3, block_q4_K, 256, dequantize_q4_K>; +template [[host_name("kernel_mul_mv_ext_q4_K_f32_r1_4")]] kernel mul_mv_ext_q4x4_f32_t kernel_mul_mv_ext_q4x4_f32_disp<4, block_q4_K, 256, dequantize_q4_K>; +template [[host_name("kernel_mul_mv_ext_q4_K_f32_r1_5")]] kernel mul_mv_ext_q4x4_f32_t kernel_mul_mv_ext_q4x4_f32_disp<5, block_q4_K, 256, dequantize_q4_K>; + +template [[host_name("kernel_mul_mv_ext_q5_K_f32_r1_2")]] kernel mul_mv_ext_q4x4_f32_t kernel_mul_mv_ext_q4x4_f32_disp<2, block_q5_K, 256, dequantize_q5_K>; +template [[host_name("kernel_mul_mv_ext_q5_K_f32_r1_3")]] kernel mul_mv_ext_q4x4_f32_t kernel_mul_mv_ext_q4x4_f32_disp<3, block_q5_K, 256, dequantize_q5_K>; +template [[host_name("kernel_mul_mv_ext_q5_K_f32_r1_4")]] kernel mul_mv_ext_q4x4_f32_t kernel_mul_mv_ext_q4x4_f32_disp<4, block_q5_K, 256, dequantize_q5_K>; +template [[host_name("kernel_mul_mv_ext_q5_K_f32_r1_5")]] kernel mul_mv_ext_q4x4_f32_t kernel_mul_mv_ext_q4x4_f32_disp<5, block_q5_K, 256, dequantize_q5_K>; + +template [[host_name("kernel_mul_mv_ext_q6_K_f32_r1_2")]] kernel mul_mv_ext_q4x4_f32_t kernel_mul_mv_ext_q4x4_f32_disp<2, block_q6_K, 256, dequantize_q6_K>; +template [[host_name("kernel_mul_mv_ext_q6_K_f32_r1_3")]] kernel mul_mv_ext_q4x4_f32_t kernel_mul_mv_ext_q4x4_f32_disp<3, block_q6_K, 256, dequantize_q6_K>; +template [[host_name("kernel_mul_mv_ext_q6_K_f32_r1_4")]] kernel mul_mv_ext_q4x4_f32_t kernel_mul_mv_ext_q4x4_f32_disp<4, block_q6_K, 256, dequantize_q6_K>; +template [[host_name("kernel_mul_mv_ext_q6_K_f32_r1_5")]] kernel mul_mv_ext_q4x4_f32_t kernel_mul_mv_ext_q4x4_f32_disp<5, block_q6_K, 256, dequantize_q6_K>; + +template [[host_name("kernel_mul_mv_ext_q2_K_f32_r1_2")]] kernel mul_mv_ext_q4x4_f32_t kernel_mul_mv_ext_q4x4_f32_disp<2, block_q2_K, 256, dequantize_q2_K>; +template [[host_name("kernel_mul_mv_ext_q2_K_f32_r1_3")]] kernel mul_mv_ext_q4x4_f32_t kernel_mul_mv_ext_q4x4_f32_disp<3, block_q2_K, 256, dequantize_q2_K>; +template [[host_name("kernel_mul_mv_ext_q2_K_f32_r1_4")]] kernel mul_mv_ext_q4x4_f32_t kernel_mul_mv_ext_q4x4_f32_disp<4, block_q2_K, 256, dequantize_q2_K>; +template [[host_name("kernel_mul_mv_ext_q2_K_f32_r1_5")]] kernel mul_mv_ext_q4x4_f32_t kernel_mul_mv_ext_q4x4_f32_disp<5, block_q2_K, 256, dequantize_q2_K>; + +template [[host_name("kernel_mul_mv_ext_q3_K_f32_r1_2")]] kernel mul_mv_ext_q4x4_f32_t kernel_mul_mv_ext_q4x4_f32_disp<2, block_q3_K, 256, dequantize_q3_K>; +template [[host_name("kernel_mul_mv_ext_q3_K_f32_r1_3")]] kernel mul_mv_ext_q4x4_f32_t kernel_mul_mv_ext_q4x4_f32_disp<3, block_q3_K, 256, dequantize_q3_K>; +template [[host_name("kernel_mul_mv_ext_q3_K_f32_r1_4")]] kernel mul_mv_ext_q4x4_f32_t kernel_mul_mv_ext_q4x4_f32_disp<4, block_q3_K, 256, dequantize_q3_K>; +template [[host_name("kernel_mul_mv_ext_q3_K_f32_r1_5")]] kernel mul_mv_ext_q4x4_f32_t kernel_mul_mv_ext_q4x4_f32_disp<5, block_q3_K, 256, dequantize_q3_K>; + +template +void kernel_mul_mv_t_t_impl( + args_t args, + device const char * src0, + device const char * src1, + device char * dst, + threadgroup char * shmem, + uint3 tgpig, + ushort tiisg, + ushort sgitg) { + const short NSG = FC_mul_mv_nsg; + + constexpr short NW = N_SIMDWIDTH; + constexpr short NB = 32; + constexpr short NF = 8; + + const int nb = args.ne00/NB; + + const int r0 = tgpig.x*NR0; + const int r1 = tgpig.y; + const int im = tgpig.z; + + const uint i12 = im%args.ne12; + const uint i13 = im/args.ne12; + + //const uint64_t offset0 = r0*args.nb01 + (i12/args.r2)*args.nb02 + (i13/args.r3)*args.nb03; + const uint64_t offset1 = r1*args.nb11 + (i12 )*args.nb12 + (i13 )*args.nb13; + + //device const T0 * x = (device const T0 *) (src0 + offset0); + device const T1 * y = (device const T1 *) (src1 + offset1); + + // pointers to src0 rows + device const T0 * ax [NR0]; + FOR_UNROLL (short row = 0; row < NR0; ++row) { + const uint64_t offset0 = (r0 + row)*args.nb01 + (i12/args.r2)*args.nb02 + (i13/args.r3)*args.nb03; + + ax[row] = (device const T0 *) ((device char *) src0 + offset0); + } + + float sumf[NR0] = { 0.f }; + + const short ix = tiisg/(NW/NF); + const short il = tiisg%(NW/NF); + + const int ib0 = sgitg*NF + ix; + + T1 yl[NF]; + + device const T1 * yb = y + (ib0*NB + il*NF); + + for (int ib = ib0; ib < nb; ib += NSG*NF) { + for (short i = 0; i < NF; ++i) { + yl[i] = yb[i]; + } + + for (short row = 0; row < NR0; row++) { + device const T0 * xb = ax[row] + (ib*NB + il*NF); + + float sumq = 0.f; + FOR_UNROLL (short i = 0; i < NF; ++i) { + sumq += xb[i] * yl[i]; + } + + sumf[row] += sumq; + } + + yb += NSG*NF*NW; + } + + for (int i = nb*NB + sgitg*NW + tiisg; i < args.ne00; i += NW*NSG) { + for (short row = 0; row < NR0; row++) { + sumf[row] += ax[row][i] * y[i]; + } + } + + device float * dst_f32 = (device float *) dst + (uint64_t)im*args.ne0*args.ne1 + (uint64_t)r1*args.ne0; + + helper_mv_reduce_and_write(dst_f32, sumf, r0, args.ne01, tiisg, sgitg, shmem); +} + +template +void kernel_mul_mv_t_t_disp( + args_t args, + device const char * src0, + device const char * src1, + device char * dst, + threadgroup char * shmem, + uint3 tgpig, + ushort tiisg, + ushort sgitg) { + switch (args.nr0) { + //case 1: kernel_mul_mv_t_t_impl(args, src0, src1, dst, shmem, tgpig, tiisg, sgitg); break; + case 2: kernel_mul_mv_t_t_impl(args, src0, src1, dst, shmem, tgpig, tiisg, sgitg); break; + //case 3: kernel_mul_mv_t_t_impl(args, src0, src1, dst, shmem, tgpig, tiisg, sgitg); break; + //case 4: kernel_mul_mv_t_t_impl(args, src0, src1, dst, shmem, tgpig, tiisg, sgitg); break; + } +} + +template +kernel void kernel_mul_mv_t_t( + constant ggml_metal_kargs_mul_mv & args, + device const char * src0, + device const char * src1, + device char * dst, + threadgroup char * shmem [[threadgroup(0)]], + uint3 tgpig[[threadgroup_position_in_grid]], + ushort tiisg[[thread_index_in_simdgroup]], + ushort sgitg[[simdgroup_index_in_threadgroup]]) { + kernel_mul_mv_t_t_disp(args, src0, src1, dst, shmem, tgpig, tiisg, sgitg); +} + +typedef decltype(kernel_mul_mv_t_t) mul_mv_t_t; + +template [[host_name("kernel_mul_mv_f32_f32")]] kernel mul_mv_t_t kernel_mul_mv_t_t; +template [[host_name("kernel_mul_mv_f16_f32")]] kernel mul_mv_t_t kernel_mul_mv_t_t; +template [[host_name("kernel_mul_mv_f16_f16")]] kernel mul_mv_t_t kernel_mul_mv_t_t; +#if defined(GGML_METAL_HAS_BF16) +template [[host_name("kernel_mul_mv_bf16_f32")]] kernel mul_mv_t_t kernel_mul_mv_t_t; +template [[host_name("kernel_mul_mv_bf16_bf16")]] kernel mul_mv_t_t kernel_mul_mv_t_t; +#endif + +template +void kernel_mul_mv_t_t_4_impl( + args_t args, + device const char * src0, + device const char * src1, + device char * dst, + threadgroup char * shmem, + uint3 tgpig, + ushort tiisg, + ushort sgitg) { + const short NSG = FC_mul_mv_nsg; + + constexpr short NW = N_SIMDWIDTH; + constexpr short NB = 32; + constexpr short NF = 16; + constexpr short NF4 = NF/4; + + const int nb = args.ne00/NB; + + const int r0 = tgpig.x*NR0; + const int r1 = tgpig.y; + const int im = tgpig.z; + + const uint i12 = im%args.ne12; + const uint i13 = im/args.ne12; + + //const uint64_t offset0 = r0*args.nb01 + (i12/args.r2)*args.nb02 + (i13/args.r3)*args.nb03; + const uint64_t offset1 = r1*args.nb11 + (i12 )*args.nb12 + (i13 )*args.nb13; + + device const T1 * y = (device const T1 *) (src1 + offset1); + device const T14 * y4 = (device const T14 *) (src1 + offset1); + + // pointers to src0 rows + device const T0 * ax [NR0]; + device const T04 * ax4[NR0]; + FOR_UNROLL (short row = 0; row < NR0; ++row) { + const uint64_t offset0 = (r0 + row)*args.nb01 + (i12/args.r2)*args.nb02 + (i13/args.r3)*args.nb03; + + ax [row] = (device const T0 *) ((device char *) src0 + offset0); + ax4[row] = (device const T04 *) ((device char *) src0 + offset0); + } + + float sumf[NR0] = { 0.f }; + + const short ix = tiisg/(NW/NF); + const short il = tiisg%(NW/NF); + + const int ib0 = sgitg*NF + ix; + + T14 yl4[NF4]; + + device const T14 * yb4 = y4 + (ib0*NB + il*NF)/4; + + for (int ib = ib0; ib < nb; ib += NSG*NF) { + for (short i = 0; i < NF4; ++i) { + yl4[i] = yb4[i]; + } + + for (short row = 0; row < NR0; row++) { + device const T04 * xb4 = ax4[row] + (ib*NB + il*NF)/4; + + float sumq = 0.f; + FOR_UNROLL (short i = 0; i < NF4; ++i) { + sumq += dot(float4(xb4[i]), float4(yl4[i])); + } + + sumf[row] += sumq; + } + + yb4 += NSG*NF*NW/4; + } + + for (int i = nb*NB + sgitg*NW + tiisg; i < args.ne00; i += NW*NSG) { + for (short row = 0; row < NR0; row++) { + sumf[row] += ax[row][i] * y[i]; + } + } + + device float * dst_f32 = (device float *) dst + (uint64_t)im*args.ne0*args.ne1 + (uint64_t)r1*args.ne0; + + helper_mv_reduce_and_write(dst_f32, sumf, r0, args.ne01, tiisg, sgitg, shmem); +} + +template +void kernel_mul_mv_t_t_4_disp( + args_t args, + device const char * src0, + device const char * src1, + device char * dst, + threadgroup char * shmem, + uint3 tgpig, + ushort tiisg, + ushort sgitg) { + switch (args.nr0) { + //case 1: kernel_mul_mv_t_t_4_impl(args, src0, src1, dst, shmem, tgpig, tiisg, sgitg); break; + case 2: kernel_mul_mv_t_t_4_impl(args, src0, src1, dst, shmem, tgpig, tiisg, sgitg); break; + //case 3: kernel_mul_mv_t_t_4_impl(args, src0, src1, dst, shmem, tgpig, tiisg, sgitg); break; + //case 4: kernel_mul_mv_t_t_4_impl(args, src0, src1, dst, shmem, tgpig, tiisg, sgitg); break; + }; +} + +template +kernel void kernel_mul_mv_t_t_4( + constant ggml_metal_kargs_mul_mv & args, + device const char * src0, + device const char * src1, + device char * dst, + threadgroup char * shmem [[threadgroup(0)]], + uint3 tgpig[[threadgroup_position_in_grid]], + ushort tiisg[[thread_index_in_simdgroup]], + ushort sgitg[[simdgroup_index_in_threadgroup]]) { + kernel_mul_mv_t_t_4_disp(args, src0, src1, dst, shmem, tgpig, tiisg, sgitg); +} + +typedef decltype(kernel_mul_mv_t_t_4) mul_mv_t_t_4; + +template [[host_name("kernel_mul_mv_f32_f32_4")]] kernel mul_mv_t_t_4 kernel_mul_mv_t_t_4; +template [[host_name("kernel_mul_mv_f16_f32_4")]] kernel mul_mv_t_t_4 kernel_mul_mv_t_t_4; +template [[host_name("kernel_mul_mv_f16_f16_4")]] kernel mul_mv_t_t_4 kernel_mul_mv_t_t_4; +#if defined(GGML_METAL_HAS_BF16) +template [[host_name("kernel_mul_mv_bf16_f32_4")]] kernel mul_mv_t_t_4 kernel_mul_mv_t_t_4; +template [[host_name("kernel_mul_mv_bf16_bf16_4")]] kernel mul_mv_t_t_4 kernel_mul_mv_t_t_4; +#endif + +template +void kernel_mul_mv_t_t_short_impl( + args_t args, + device const char * src0, + device const char * src1, + device char * dst, + uint3 tgpig, + ushort tiisg) { + const int r0 = tgpig.x*32 + tiisg; + const int r1 = tgpig.y; + const int im = tgpig.z; + + if (r0 >= args.ne01) { + return; + } + + const uint i12 = im%args.ne12; + const uint i13 = im/args.ne12; + + const uint64_t offset0 = r0*args.nb01 + (i12/args.r2)*args.nb02 + (i13/args.r3)*args.nb03; + + device const T0 * x = (device const T0 *) (src0 + offset0); + + device float * dst_f32 = (device float *) dst + (uint64_t)im*args.ne0*args.ne1; + + const uint64_t offset1 = r1*args.nb11 + (i12 )*args.nb12 + (i13 )*args.nb13; + + device const T1 * y = (device const T1 *) (src1 + offset1); + + float res = 0.0f; + + for (int i = 0; i < args.ne00; ++i) { + res += (float) x[i] * (float) y[i]; + } + + dst_f32[(uint64_t)r1*args.ne0 + r0] = res; +} + +template +kernel void kernel_mul_mv_t_t_short( + constant ggml_metal_kargs_mul_mv & args, + device const char * src0, + device const char * src1, + device char * dst, + uint3 tgpig[[threadgroup_position_in_grid]], + ushort tiisg[[thread_index_in_simdgroup]]) { + kernel_mul_mv_t_t_short_impl( + args, + src0, + src1, + dst, + tgpig, + tiisg); +} + +typedef decltype(kernel_mul_mv_t_t_short) mul_mv_t_t_short_t; + +template [[host_name("kernel_mul_mv_f32_f32_short")]] kernel mul_mv_t_t_short_t kernel_mul_mv_t_t_short; +template [[host_name("kernel_mul_mv_f16_f32_short")]] kernel mul_mv_t_t_short_t kernel_mul_mv_t_t_short; +template [[host_name("kernel_mul_mv_f16_f16_short")]] kernel mul_mv_t_t_short_t kernel_mul_mv_t_t_short; +#if defined(GGML_METAL_HAS_BF16) +template [[host_name("kernel_mul_mv_bf16_f32_short")]] kernel mul_mv_t_t_short_t kernel_mul_mv_t_t_short; +template [[host_name("kernel_mul_mv_bf16_bf16_short")]] kernel mul_mv_t_t_short_t kernel_mul_mv_t_t_short; +#endif + +constant bool FC_rope_is_imrope [[function_constant(FC_ROPE + 0)]]; + +static float rope_yarn_ramp(const float low, const float high, const int i0) { + const float y = (i0 / 2 - low) / max(0.001f, high - low); + return 1.0f - min(1.0f, max(0.0f, y)); +} + +// YaRN algorithm based on LlamaYaRNScaledRotaryEmbedding.py from https://github.com/jquesnelle/yarn +// MIT licensed. Copyright (c) 2023 Jeffrey Quesnelle and Bowen Peng. +static void rope_yarn( + float theta_extrap, float freq_scale, float corr_dims[2], int i0, float ext_factor, float mscale, + thread float * cos_theta, thread float * sin_theta) { + // Get n-d rotational scaling corrected for extrapolation + float theta_interp = freq_scale * theta_extrap; + float theta = theta_interp; + if (ext_factor != 0.0f) { + float ramp_mix = rope_yarn_ramp(corr_dims[0], corr_dims[1], i0) * ext_factor; + theta = theta_interp * (1 - ramp_mix) + theta_extrap * ramp_mix; + + // Get n-d magnitude scaling corrected for interpolation + mscale *= 1.0f + 0.1f * log(1.0f / freq_scale); + } + *cos_theta = cos(theta) * mscale; + *sin_theta = sin(theta) * mscale; +} + +// Apparently solving `n_rot = 2pi * x * base^((2 * max_pos_emb) / n_dims)` for x, we get +// `corr_fac(n_rot) = n_dims * log(max_pos_emb / (n_rot * 2pi)) / (2 * log(base))` +static float rope_yarn_corr_factor(int n_dims, int n_ctx_orig, float n_rot, float base) { + return n_dims * log(n_ctx_orig / (n_rot * 2 * M_PI_F)) / (2 * log(base)); +} + +static void rope_yarn_corr_dims( + int n_dims, int n_ctx_orig, float freq_base, float beta_fast, float beta_slow, float dims[2] +) { + // start and end correction dims + dims[0] = max(0.0f, floor(rope_yarn_corr_factor(n_dims, n_ctx_orig, beta_fast, freq_base))); + dims[1] = min(n_dims - 1.0f, ceil(rope_yarn_corr_factor(n_dims, n_ctx_orig, beta_slow, freq_base))); +} + +template +kernel void kernel_rope_norm( + constant ggml_metal_kargs_rope & args, + device const char * src0, + device const char * src1, + device const char * src2, + device char * dst, + ushort tiitg[[thread_index_in_threadgroup]], + ushort3 tptg [[threads_per_threadgroup]], + uint3 tgpig[[threadgroup_position_in_grid]]) { + const int i3 = tgpig[2]; + const int i2 = tgpig[1]; + const int i1 = tgpig[0]; + + float corr_dims[2]; + rope_yarn_corr_dims(args.n_dims, args.n_ctx_orig, args.freq_base, args.beta_fast, args.beta_slow, corr_dims); + + device const int32_t * pos = (device const int32_t *) src1; + + const float theta_base = (float) pos[i2]; + const float inv_ndims = -1.f/args.n_dims; + + float cos_theta; + float sin_theta; + + for (int i0 = 2*tiitg; i0 < args.ne0; i0 += 2*tptg.x) { + if (i0 < args.n_dims) { + const int ic = i0/2; + + const float theta = theta_base * pow(args.freq_base, inv_ndims*i0); + + const float freq_factor = args.src2 ? ((device const float *) src2)[ic] : 1.0f; + + rope_yarn(theta/freq_factor, args.freq_scale, corr_dims, i0, args.ext_factor, args.attn_factor, &cos_theta, &sin_theta); + + device const T * const src = (device T *)(src0 + i3*args.nb03 + i2*args.nb02 + i1*args.nb01 + i0*args.nb00); + device T * dst_data = (device T *)( dst + i3*args.nb3 + i2*args.nb2 + i1*args.nb1 + i0*args.nb0); + + const float x0 = src[0]; + const float x1 = src[1]; + + dst_data[0] = x0*cos_theta - x1*sin_theta; + dst_data[1] = x0*sin_theta + x1*cos_theta; + } else { + device const T * const src = (device T *)(src0 + i3*args.nb03 + i2*args.nb02 + i1*args.nb01 + i0*args.nb00); + device T * dst_data = (device T *)( dst + i3*args.nb3 + i2*args.nb2 + i1*args.nb1 + i0*args.nb0); + + dst_data[0] = src[0]; + dst_data[1] = src[1]; + } + } +} + +template +kernel void kernel_rope_neox( + constant ggml_metal_kargs_rope & args, + device const char * src0, + device const char * src1, + device const char * src2, + device char * dst, + ushort tiitg[[thread_index_in_threadgroup]], + ushort3 tptg [[threads_per_threadgroup]], + uint3 tgpig[[threadgroup_position_in_grid]]) { + const int i3 = tgpig[2]; + const int i2 = tgpig[1]; + const int i1 = tgpig[0]; + + float corr_dims[2]; + rope_yarn_corr_dims(args.n_dims, args.n_ctx_orig, args.freq_base, args.beta_fast, args.beta_slow, corr_dims); + + device const int32_t * pos = (device const int32_t *) src1; + + const float theta_base = (float) pos[i2]; + const float inv_ndims = -1.f/args.n_dims; + + float cos_theta; + float sin_theta; + + for (int i0 = 2*tiitg; i0 < args.ne0; i0 += 2*tptg.x) { + if (i0 < args.n_dims) { + const int ic = i0/2; + + const float theta = theta_base * pow(args.freq_base, inv_ndims*i0); + + const float freq_factor = args.src2 ? ((device const float *) src2)[ic] : 1.0f; + + rope_yarn(theta/freq_factor, args.freq_scale, corr_dims, i0, args.ext_factor, args.attn_factor, &cos_theta, &sin_theta); + + device const T * const src = (device T *)(src0 + i3*args.nb03 + i2*args.nb02 + i1*args.nb01 + ic*args.nb00); + device T * dst_data = (device T *)( dst + i3*args.nb3 + i2*args.nb2 + i1*args.nb1 + ic*args.nb0); + + const float x0 = src[0]; + const float x1 = src[args.n_dims/2]; + + dst_data[0] = x0*cos_theta - x1*sin_theta; + dst_data[args.n_dims/2] = x0*sin_theta + x1*cos_theta; + } else { + device const T * const src = (device T *)(src0 + i3*args.nb03 + i2*args.nb02 + i1*args.nb01 + i0*args.nb00); + device T * dst_data = (device T *)( dst + i3*args.nb3 + i2*args.nb2 + i1*args.nb1 + i0*args.nb0); + + dst_data[0] = src[0]; + dst_data[1] = src[1]; + } + } +} + +template +kernel void kernel_rope_multi( + constant ggml_metal_kargs_rope & args, + device const char * src0, + device const char * src1, + device const char * src2, + device char * dst, + ushort tiitg[[thread_index_in_threadgroup]], + ushort3 tptg [[threads_per_threadgroup]], + uint3 tgpig[[threadgroup_position_in_grid]]) { + const int i3 = tgpig[2]; + const int i2 = tgpig[1]; + const int i1 = tgpig[0]; + + float corr_dims[2]; + rope_yarn_corr_dims(args.n_dims, args.n_ctx_orig, args.freq_base, args.beta_fast, args.beta_slow, corr_dims); + + device const int32_t * pos = (device const int32_t *) src1; + + const float inv_ndims = -1.f/args.n_dims; + + float cos_theta; + float sin_theta; + + for (int i0 = 2*tiitg; i0 < args.ne0; i0 += 2*tptg.x) { + if (i0 < args.n_dims) { + const int ic = i0/2; + + // mrope theta calculations + // note: the rest is the same as kernel_rope_neox + const int sect_dims = args.sect_0 + args.sect_1 + args.sect_2 + args.sect_3; + const int sec_w01 = args.sect_0 + args.sect_1; // end of section 1 + const int sec_w012 = args.sect_0 + args.sect_1 + args.sect_2; // end of section 2 + const int sector = ic % sect_dims; + + float theta_base; + if (FC_rope_is_imrope) { + if (sector % 3 == 1 && sector < 3 * args.sect_1) { // h + theta_base = (float) pos[i2 + args.ne02 * 1]; + } else if (sector % 3 == 2 && sector < 3 * args.sect_2) { // w + theta_base = (float) pos[i2 + args.ne02 * 2]; + } else if (sector % 3 == 0 && sector < 3 * args.sect_0) { // t + theta_base = (float) pos[i2 + args.ne02 * 0]; + } else { // e + theta_base = (float) pos[i2 + args.ne02 * 3]; + } + } else { + if (sector < args.sect_0) { + theta_base = (float) pos[i2]; + } else if (sector < sec_w01) { + theta_base = (float) pos[i2 + args.ne02 * 1]; + } else if (sector < sec_w012) { + theta_base = (float) pos[i2 + args.ne02 * 2]; + } else { + theta_base = (float) pos[i2 + args.ne02 * 3]; + } + } + // end of mrope + + const float theta = theta_base * pow(args.freq_base, inv_ndims*i0); + + const float freq_factor = args.src2 ? ((device const float *) src2)[ic] : 1.0f; + + rope_yarn(theta/freq_factor, args.freq_scale, corr_dims, i0, args.ext_factor, args.attn_factor, &cos_theta, &sin_theta); + + device const T * const src = (device T *)(src0 + i3*args.nb03 + i2*args.nb02 + i1*args.nb01 + ic*args.nb00); + device T * dst_data = (device T *)( dst + i3*args.nb3 + i2*args.nb2 + i1*args.nb1 + ic*args.nb0); + + const float x0 = src[0]; + const float x1 = src[args.n_dims/2]; + + dst_data[0] = x0*cos_theta - x1*sin_theta; + dst_data[args.n_dims/2] = x0*sin_theta + x1*cos_theta; + } else { + device const T * const src = (device T *)(src0 + i3*args.nb03 + i2*args.nb02 + i1*args.nb01 + i0*args.nb00); + device T * dst_data = (device T *)( dst + i3*args.nb3 + i2*args.nb2 + i1*args.nb1 + i0*args.nb0); + + dst_data[0] = src[0]; + dst_data[1] = src[1]; + } + } +} + +template +kernel void kernel_rope_vision( + constant ggml_metal_kargs_rope & args, + device const char * src0, + device const char * src1, + device const char * src2, + device char * dst, + ushort tiitg[[thread_index_in_threadgroup]], + ushort3 tptg [[threads_per_threadgroup]], + uint3 tgpig[[threadgroup_position_in_grid]]) { + const int i3 = tgpig[2]; + const int i2 = tgpig[1]; + const int i1 = tgpig[0]; + + float corr_dims[2]; + rope_yarn_corr_dims(args.n_dims, args.n_ctx_orig, args.freq_base, args.beta_fast, args.beta_slow, corr_dims); + + device const int32_t * pos = (device const int32_t *) src1; + + const float inv_ndims = -1.f/args.n_dims; + + float cos_theta; + float sin_theta; + + for (int i0 = 2*tiitg; i0 < args.ne0; i0 += 2*tptg.x) { + if (i0 < 2*args.n_dims) { // different from kernel_rope_multi + const int ic = i0/2; + + // mrope theta calculations (only support 2 dimensions) + const int sect_dims = args.sect_0 + args.sect_1; + const int sector = ic % sect_dims; + + float p; + float theta_base; + if (sector < args.sect_1) { + p = (float) sector; + theta_base = (float) pos[i2]; + } else { + p = (float) sector - args.sect_0; + theta_base = (float) pos[i2 + args.ne02]; + } + + const float theta = theta_base * pow(args.freq_base, 2.0f * inv_ndims * p); + // end of mrope + + const float freq_factor = args.src2 ? ((device const float *) src2)[ic] : 1.0f; + + rope_yarn(theta/freq_factor, args.freq_scale, corr_dims, i0, args.ext_factor, args.attn_factor, &cos_theta, &sin_theta); + + device const T * const src = (device T *)(src0 + i3*args.nb03 + i2*args.nb02 + i1*args.nb01 + ic*args.nb00); + device T * dst_data = (device T *)( dst + i3*args.nb3 + i2*args.nb2 + i1*args.nb1 + ic*args.nb0); + + const float x0 = src[0]; + const float x1 = src[args.n_dims]; // different from kernel_rope_multi + + dst_data[0] = x0*cos_theta - x1*sin_theta; + dst_data[args.n_dims] = x0*sin_theta + x1*cos_theta; // different from kernel_rope_multi + } else { + device const T * const src = (device T *)(src0 + i3*args.nb03 + i2*args.nb02 + i1*args.nb01 + i0*args.nb00); + device T * dst_data = (device T *)( dst + i3*args.nb3 + i2*args.nb2 + i1*args.nb1 + i0*args.nb0); + + dst_data[0] = src[0]; + dst_data[1] = src[1]; + } + } +} + +typedef decltype(kernel_rope_norm) kernel_rope_norm_t; +typedef decltype(kernel_rope_neox) kernel_rope_neox_t; +typedef decltype(kernel_rope_multi) kernel_rope_multi_t; +typedef decltype(kernel_rope_vision) kernel_rope_vision_t; + +template [[host_name("kernel_rope_norm_f32")]] kernel kernel_rope_norm_t kernel_rope_norm; +template [[host_name("kernel_rope_norm_f16")]] kernel kernel_rope_norm_t kernel_rope_norm; + +template [[host_name("kernel_rope_neox_f32")]] kernel kernel_rope_neox_t kernel_rope_neox; +template [[host_name("kernel_rope_neox_f16")]] kernel kernel_rope_neox_t kernel_rope_neox; + +template [[host_name("kernel_rope_multi_f32")]] kernel kernel_rope_multi_t kernel_rope_multi; +template [[host_name("kernel_rope_multi_f16")]] kernel kernel_rope_multi_t kernel_rope_multi; + +template [[host_name("kernel_rope_vision_f32")]] kernel kernel_rope_vision_t kernel_rope_vision; +template [[host_name("kernel_rope_vision_f16")]] kernel kernel_rope_vision_t kernel_rope_vision; + +typedef void (im2col_t)( + constant ggml_metal_kargs_im2col & args, + device const float * x, + device char * dst, + uint3 tgpig[[threadgroup_position_in_grid]], + uint3 tgpg[[threadgroups_per_grid]], + uint3 tpitg[[thread_position_in_threadgroup]], + uint3 ntg[[threads_per_threadgroup]]); + +template +kernel void kernel_im2col( + constant ggml_metal_kargs_im2col & args, + device const float * x, + device char * dst, + uint3 tgpig[[threadgroup_position_in_grid]], + uint3 tgpg[[threadgroups_per_grid]], + uint3 tpitg[[thread_position_in_threadgroup]], + uint3 ntg[[threads_per_threadgroup]]) { +// const int64_t IC = tgpg[0]; + const int64_t OH = tgpg[1]; + const int64_t OW = tgpg[2]; + + const int64_t KH = ntg[1]; + const int64_t KW = ntg[2]; + + int64_t in = tpitg[0]; + const int64_t ikh = tpitg[1]; + const int64_t ikw = tpitg[2]; + + const int64_t iic = tgpig[0]; + const int64_t ioh = tgpig[1]; + const int64_t iow = tgpig[2]; + + const int64_t iiw = iow*args.s0 + ikw*args.d0 - args.p0; + const int64_t iih = ioh*args.s1 + ikh*args.d1 - args.p1; + + int64_t offset_dst = (in*OH*OW + ioh*OW + iow)*args.CHW + (iic*(KH*KW) + ikh*KW + ikw); + + device T * pdst = (device T *) (dst); + + if (iih < 0 || iih >= args.IH || iiw < 0 || iiw >= args.IW) { + while (in < args.N) { + pdst[offset_dst] = 0.0f; + offset_dst += ntg[0]*args.CHW*OH*OW; + + in += ntg[0]; + } + } else { + int64_t offset_src = in*args.ofs0 + iic*args.ofs1 + iih*args.IW + iiw; + + while (in < args.N) { + pdst[offset_dst] = x[offset_src]; + + offset_dst += ntg[0]*args.CHW*OH*OW; + offset_src += ntg[0]*args.ofs0; + + in += ntg[0]; + } + } +} + +template [[host_name("kernel_im2col_f32")]] kernel im2col_t kernel_im2col; +template [[host_name("kernel_im2col_f16")]] kernel im2col_t kernel_im2col; + +// TODO: obsolete -- remove +//typedef void (im2col_ext_t)( +// constant ggml_metal_kargs_im2col & args, +// device const float * x, +// device char * dst, +// uint3 tgpig[[threadgroup_position_in_grid]], +// uint3 tgpg[[threadgroups_per_grid]], +// uint3 tpitg[[thread_position_in_threadgroup]], +// uint3 ntg[[threads_per_threadgroup]]); +// +//template +//kernel void kernel_im2col_ext( +// constant ggml_metal_kargs_im2col & args, +// device const float * x, +// device char * dst, +// uint3 tgpig[[threadgroup_position_in_grid]], +// uint3 tgpg[[threadgroups_per_grid]], // tgpg[0] = D x IC x KH x KW, CHW = IC x KH x KW +// uint3 tpitg[[thread_position_in_threadgroup]], +// uint3 ntg[[threads_per_threadgroup]]) { // [M, 1, 1] +// const int64_t KHW = (int64_t)args.KHW; +// +// const int64_t d = tgpig[0] / args.CHW; +// const int64_t chw = tgpig[0] % args.CHW; +// const int64_t tgpig_0 = chw / KHW; // 0 ~ (IC - 1) +// const int64_t HW = tgpig[0] % KHW; +// +// const int64_t tpitg_0 = (d * ntg[0]) + tpitg[0]; +// if (tpitg_0 >= args.N) { +// return; +// } +// +// const int64_t tpitg_1 = HW / args.KW; +// const int64_t tpitg_2 = HW % args.KW; +// +// const int64_t iiw = tgpig[2] * args.s0 + tpitg_2 * args.d0 - args.p0; +// const int64_t iih = tgpig[1] * args.s1 + tpitg_1 * args.d1 - args.p1; +// +// const int64_t offset_dst = +// (tpitg_0 * tgpg[1] * tgpg[2] + tgpig[1] * tgpg[2] + tgpig[2]) * args.CHW + +// (tgpig_0 * KHW + tpitg_1 * args.KW + tpitg_2); +// +// device T * pdst = (device T *) (dst); +// +// if (iih < 0 || iih >= args.IH || iiw < 0 || iiw >= args.IW) { +// pdst[offset_dst] = 0.0f; +// } else { +// const int64_t offset_src = tpitg_0 * args.ofs0 + tgpig_0 * args.ofs1; +// pdst[offset_dst] = x[offset_src + iih * args.IW + iiw]; +// } +//} +// +//template [[host_name("kernel_im2col_ext_f32")]] kernel im2col_ext_t kernel_im2col_ext; +//template [[host_name("kernel_im2col_ext_f16")]] kernel im2col_ext_t kernel_im2col_ext; + +template +kernel void kernel_conv_2d( + constant ggml_metal_kargs_conv_2d & args, + device const char * weights, + device const char * src, + device char * dst, + uint3 tgpig[[threadgroup_position_in_grid]], + uint3 tgpg[[threadgroups_per_grid]], + uint3 tpitg[[thread_position_in_threadgroup]], + uint3 ntg[[threads_per_threadgroup]]) { + + const uint threads_per_tg = ntg.x * ntg.y * ntg.z; + const uint tg_index = (tgpig.z * tgpg.y + tgpig.y) * tgpg.x + tgpig.x; + const uint local_thread = tpitg.z * (ntg.x * ntg.y) + tpitg.y * ntg.x + tpitg.x; + const uint thread_index = tg_index * threads_per_tg + local_thread; + const uint64_t total_threads = (uint64_t) threads_per_tg * tgpg.x * tgpg.y * tgpg.z; + const uint64_t total_outputs = (uint64_t) args.N * args.OC * args.OH * args.OW; + + for (uint64_t index = thread_index; index < total_outputs; index += total_threads) { + uint64_t tmp = index; + + const int32_t ow = tmp % args.OW; tmp /= args.OW; + const int32_t oh = tmp % args.OH; tmp /= args.OH; + const int32_t oc = tmp % args.OC; tmp /= args.OC; + const int32_t n = tmp; + + float acc = 0.0f; + + const int32_t base_x = ow*args.s0 - args.p0; + const int32_t base_y = oh*args.s1 - args.p1; + + int32_t ky_start = 0; + if (base_y < 0) { + ky_start = (-base_y + args.d1 - 1)/args.d1; + } + int32_t ky_end = args.KH; + const int32_t y_max = args.IH - 1 - base_y; + if (y_max < 0) { + ky_end = ky_start; + } else if (base_y + (args.KH - 1)*args.d1 >= args.IH) { + ky_end = min(ky_end, y_max/args.d1 + 1); + } + + int32_t kx_start = 0; + if (base_x < 0) { + kx_start = (-base_x + args.d0 - 1)/args.d0; + } + int32_t kx_end = args.KW; + const int32_t x_max = args.IW - 1 - base_x; + if (x_max < 0) { + kx_end = kx_start; + } else if (base_x + (args.KW - 1)*args.d0 >= args.IW) { + kx_end = min(kx_end, x_max/args.d0 + 1); + } + + if (ky_start < ky_end && kx_start < kx_end) { + const uint64_t src_base_n = (uint64_t) n * args.nb13; + const uint64_t w_base_oc = (uint64_t) oc * args.nb03; + + for (int32_t ic = 0; ic < args.IC; ++ic) { + const uint64_t src_base_nc = src_base_n + (uint64_t) ic * args.nb12; + const uint64_t w_base_ocic = w_base_oc + (uint64_t) ic * args.nb02; + + for (int32_t ky = ky_start; ky < ky_end; ++ky) { + const int32_t iy = base_y + ky*args.d1; + const uint64_t src_base_row = src_base_nc + (uint64_t) iy * args.nb11; + const uint64_t w_base_row = w_base_ocic + (uint64_t) ky * args.nb01; + + for (int32_t kx = kx_start; kx < kx_end; ++kx) { + const int32_t ix = base_x + kx*args.d0; + const uint64_t src_offs = src_base_row + (uint64_t) ix * args.nb10; + const uint64_t w_offs = w_base_row + (uint64_t) kx * args.nb00; + + const float x = *(device const float *)(src + src_offs); + const float w = (float) (*(device const TK *)(weights + w_offs)); + + acc += x * w; + } + } + } + } + + const uint64_t dst_offs = + (uint64_t) n * args.nb3 + + (uint64_t) oc * args.nb2 + + (uint64_t) oh * args.nb1 + + (uint64_t) ow * args.nb0; + + *(device float *)(dst + dst_offs) = acc; + } +} + +template [[host_name("kernel_conv_2d_f32_f32")]] +kernel void kernel_conv_2d( + constant ggml_metal_kargs_conv_2d & args, + device const char * weights, + device const char * src, + device char * dst, + uint3 tgpig[[threadgroup_position_in_grid]], + uint3 tgpg[[threadgroups_per_grid]], + uint3 tpitg[[thread_position_in_threadgroup]], + uint3 ntg[[threads_per_threadgroup]]); + +template [[host_name("kernel_conv_2d_f16_f32")]] +kernel void kernel_conv_2d( + constant ggml_metal_kargs_conv_2d & args, + device const char * weights, + device const char * src, + device char * dst, + uint3 tgpig[[threadgroup_position_in_grid]], + uint3 tgpg[[threadgroups_per_grid]], + uint3 tpitg[[thread_position_in_threadgroup]], + uint3 ntg[[threads_per_threadgroup]]); + +typedef void (conv_transpose_1d_t)( + constant ggml_metal_kargs_conv_transpose_1d & args, + device const float * src0, + device const float * src1, + device char * dst, + uint3 tgpig[[threadgroup_position_in_grid]], + uint3 tgpg[[threadgroups_per_grid]]); + +template +kernel void kernel_conv_transpose_1d( + constant ggml_metal_kargs_conv_transpose_1d & args, + device const T * src0, + device const float * src1, + device char * dst, + uint3 tgpig[[threadgroup_position_in_grid]], + uint3 tgpg[[threadgroups_per_grid]]) { + + float v = 0.0f; + + for (int64_t c = 0; c < args.IC; c++) { + const int32_t kernel_offset = c * tgpg[1] * args.K + args.K * tgpig[1]; + const int32_t input_offset = c * args.IL; + + for (int64_t i = 0; i < args.IL; i++) { + if (tgpig[0] >= i * args.s0 && tgpig[0] < i * args.s0 + args.K) { + v += src0[kernel_offset + tgpig[0] - i * args.s0] * src1[input_offset + i]; + } + } + } + + device float * dst_ptr = (device float *) (dst + tgpig[0] * args.nb0 + tgpig[1] * args.nb1); + + dst_ptr[0] = v; +} + +template [[host_name("kernel_conv_transpose_1d_f32_f32")]] +kernel void kernel_conv_transpose_1d( + constant ggml_metal_kargs_conv_transpose_1d & args, + device const float * src0, + device const float * src1, + device char * dst, + uint3 tgpig[[threadgroup_position_in_grid]], + uint3 tgpg[[threadgroups_per_grid]]); + +template [[host_name("kernel_conv_transpose_1d_f16_f32")]] +kernel void kernel_conv_transpose_1d( + constant ggml_metal_kargs_conv_transpose_1d & args, + device const half * src0, + device const float * src1, + device char * dst, + uint3 tgpig[[threadgroup_position_in_grid]], + uint3 tgpg[[threadgroups_per_grid]]); + + +typedef void (conv_transpose_2d_t)( + constant ggml_metal_kargs_conv_transpose_2d & args, + device const float * src0, + device const float * src1, + device char * dst, + uint3 tgpig[[threadgroup_position_in_grid]], + uint3 tgpg[[threadgroups_per_grid]]); + +template +kernel void kernel_conv_transpose_2d( + constant ggml_metal_kargs_conv_transpose_2d & args, + device const T * src0, + device const float * src1, + device char * dst, + threadgroup float * shared_sum [[threadgroup(0)]], + uint3 tgpig[[threadgroup_position_in_grid]], + uint3 tpitg[[thread_position_in_threadgroup]], + uint3 ntg[[threads_per_threadgroup]]) { + + const int64_t out_x = tgpig[0]; + const int64_t out_y = tgpig[1]; + const int64_t out_c = tgpig[2]; + + const int64_t kw = tpitg[0]; + const int64_t kh = tpitg[1]; + + float v = 0.0f; + + for (int64_t in_c = 0; in_c < args.IC; in_c++) { + int64_t in_y = out_y - kh; + + if (in_y < 0 || in_y % args.s0) continue; + + in_y /= args.s0; + + if (in_y >= args.IH) continue; + + int64_t in_x = out_x - kw; + + if (in_x < 0 || in_x % args.s0) continue; + + in_x /= args.s0; + + if (in_x >= args.IW) continue; + + const int64_t input_idx = (args.IW * args.IH) * in_c + (args.IW) * in_y + in_x; + const int64_t kernel_idx = (args.KH * args.KW * args.OC) * in_c + (args.KH * args.KW) * out_c + (args.KW) * kh + kw; + + v += (float)src0[kernel_idx] * src1[input_idx]; + } + + const uint tid = tpitg.y * ntg.x + tpitg.x; + shared_sum[tid] = v; + + threadgroup_barrier(mem_flags::mem_threadgroup); + + if (tid == 0) { + float total = 0.0f; + const uint num_threads = ntg.x * ntg.y; + for (uint i = 0; i < num_threads; i++) { + total += shared_sum[i]; + } + + device float * dst_ptr = (device float *) (dst + out_x*args.nb0 + out_y * args.nb1 + out_c*args.nb2); + dst_ptr[0] = total; + } +} + +template [[host_name("kernel_conv_transpose_2d_f32_f32")]] +kernel void kernel_conv_transpose_2d( + constant ggml_metal_kargs_conv_transpose_2d & args, + device const float * src0, + device const float * src1, + device char * dst, + threadgroup float * shared_sum [[threadgroup(0)]], + uint3 tgpig[[threadgroup_position_in_grid]], + uint3 tpitg[[thread_position_in_threadgroup]], + uint3 ntg[[threads_per_threadgroup]]); + +template [[host_name("kernel_conv_transpose_2d_f16_f32")]] +kernel void kernel_conv_transpose_2d( + constant ggml_metal_kargs_conv_transpose_2d & args, + device const half * src0, + device const float * src1, + device char * dst, + threadgroup float * shared_sum [[threadgroup(0)]], + uint3 tgpig[[threadgroup_position_in_grid]], + uint3 tpitg[[thread_position_in_threadgroup]], + uint3 ntg[[threads_per_threadgroup]]); + +constant bool FC_upscale_aa [[function_constant(FC_UPSCALE + 0)]]; + +kernel void kernel_upscale_nearest_f32( + constant ggml_metal_kargs_upscale & args, + device const char * src0, + device char * dst, + uint3 tgpig[[threadgroup_position_in_grid]], + uint3 tpitg[[thread_position_in_threadgroup]], + uint3 ntg[[threads_per_threadgroup]]) { + + const int64_t i3 = tgpig.z; + const int64_t i2 = tgpig.y; + const int64_t i1 = tgpig.x; + + const int64_t i03 = i3/args.sf3; + const int64_t i02 = i2/args.sf2; + const int64_t i01 = i1/args.sf1; + + for (int i0 = tpitg.x; i0 < args.ne0; i0 += ntg.x) { + const int64_t i00 = i0/args.sf0; + + device const float * src0_ptr = (device const float *) (src0 + i03*args.nb03 + i02*args.nb02 + i01*args.nb01 + i00*args.nb00); + device float * dst_ptr = (device float *) (dst + i3*args.nb3 + i2*args.nb2 + i1*args.nb1 + i0*args.nb0); + + dst_ptr[0] = src0_ptr[0]; + } +} + +static inline float bilinear_tri(float x) { + return MAX(0.0f, 1.0f - fabs(x)); +} + +kernel void kernel_upscale_bilinear_f32( + constant ggml_metal_kargs_upscale & args, + device const char * src0, + device char * dst, + uint3 tgpig[[threadgroup_position_in_grid]], + uint3 tpitg[[thread_position_in_threadgroup]], + uint3 ntg[[threads_per_threadgroup]]) { + + const int64_t i3 = tgpig.z; + const int64_t i2 = tgpig.y; + const int64_t i1 = tgpig.x; + + const int64_t i03 = i3 / args.sf3; + const int64_t i02 = i2 / args.sf2; + + const float f01 = ((float)i1 + args.poffs) / args.sf1 - args.poffs; + const int64_t i01 = MAX(0, MIN(args.ne01 - 1, (int64_t)floor(f01))); + const int64_t i01p = MAX(0, MIN(args.ne01 - 1, i01 + 1)); + const float fd1 = MAX(0.0f, MIN(1.0f, f01 - (float)i01)); + + src0 += i03*args.nb03 + i02*args.nb02; + + device float * dst_ptr = (device float *)(dst + i3*args.nb3 + i2*args.nb2 + i1*args.nb1); + + if (FC_upscale_aa) { + const float support0 = MAX(1.0f, 1.0f / args.sf0); + const float invscale0 = 1.0f / support0; + const float support1 = MAX(1.0f, 1.0f / args.sf1); + const float invscale1 = 1.0f / support1; + + for (int i0 = tpitg.x; i0 < args.ne0; i0 += ntg.x) { + const float f00 = ((float)i0 + args.poffs) / args.sf0 - args.poffs; + + int64_t x_min = MAX((int64_t)0, (int64_t)floor(f00 - support0 + args.poffs)); + int64_t x_max = MIN(args.ne00, (int64_t)ceil (f00 + support0 + args.poffs)); + + int64_t y_min = MAX((int64_t)0, (int64_t)floor(f01 - support1 + args.poffs)); + int64_t y_max = MIN(args.ne01, (int64_t)ceil (f01 + support1 + args.poffs)); + + float sum = 0.0f; + float wsum = 0.0f; + + for (int64_t sy = y_min; sy < y_max; ++sy) { + const float wy = MAX(0.0f, 1.0f - fabs((float)sy - f01) * invscale1); + for (int64_t sx = x_min; sx < x_max; ++sx) { + const float wx = MAX(0.0f, 1.0f - fabs((float)sx - f00) * invscale0); + const float w = wx * wy; + const device const float * src_ptr = (device const float *)(src0 + sy*args.nb01 + sx*args.nb00); + sum += (*src_ptr) * w; + wsum += w; + } + } + + const float v = (wsum > 0.0f) ? (sum / wsum) : 0.0f; + dst_ptr[i0] = v; + } + } else { + for (int i0 = tpitg.x; i0 < args.ne0; i0 += ntg.x) { + const float f00 = ((float)i0 + args.poffs) / args.sf0 - args.poffs; + const int64_t i00 = MAX(0, MIN(args.ne00 - 1, (int64_t)floor(f00))); + const int64_t i00p = MAX(0, MIN(args.ne00 - 1, i00 + 1)); + const float fd0 = MAX(0.0f, MIN(1.0f, f00 - (float)i00)); + + device const float * src00 = (device const float *)(src0 + i01*args.nb01 + i00*args.nb00); + device const float * src10 = (device const float *)(src0 + i01*args.nb01 + i00p*args.nb00); + device const float * src01 = (device const float *)(src0 + i01p*args.nb01 + i00*args.nb00); + device const float * src11 = (device const float *)(src0 + i01p*args.nb01 + i00p*args.nb00); + + const float v = + (*src00) * (1.0f - fd0) * (1.0f - fd1) + + (*src10) * fd0 * (1.0f - fd1) + + (*src01) * (1.0f - fd0) * fd1 + + (*src11) * fd0 * fd1; + + dst_ptr[i0] = v; + } + } +} + +template +kernel void kernel_conv_3d( + constant ggml_metal_kargs_conv_3d & args, + device const char * src0, // Weights [IC * OC, KD, KH, KW] + device const char * src1, // Inputs [IC * N, ID, IH, IW] + device char * dst, // Outputs [OC * N, OD, OH, OW] + uint3 tgpig[[threadgroup_position_in_grid]], + uint3 tpitg[[thread_position_in_threadgroup]]) { + + // 1. Un-flatten the spatial dimension from Grid X + int64_t spatial_idx = tgpig.x * 32 + tpitg.x; + + if (spatial_idx >= args.OW * args.OH * args.OD) { + return; // Thread falls outside the spatial volume + } + + int64_t od = spatial_idx / (args.OW * args.OH); + int64_t oh = (spatial_idx / args.OW) % args.OH; + int64_t ow = spatial_idx % args.OW; + + // 2. Map Y to Channels, Z to Batch + int64_t oc = tgpig.y; + int64_t batch_idx = tgpig.z; + + // 3. Calculate anchor coordinates in the Input volume + int64_t i_w_base = ow * args.s0 - args.p0; + int64_t i_h_base = oh * args.s1 - args.p1; + int64_t i_d_base = od * args.s2 - args.p2; + + float sum = 0.0f; + + // 4. Gather Loop (Iterate over Input Channels -> Depth -> Height -> Width) + for (int64_t ic = 0; ic < args.IC; ++ic) { + + // ggml packs batch and channel together in the 4th dimension + int64_t src_cn_idx = batch_idx * args.IC + ic; + int64_t w_cn_idx = oc * args.IC + ic; + + for (int64_t kz = 0; kz < args.KD; ++kz) { + int64_t id = i_d_base + kz * args.d2; + if (id < 0 || id >= args.ID) continue; // Boundary check (Padding) + + for (int64_t ky = 0; ky < args.KH; ++ky) { + int64_t ih = i_h_base + ky * args.d1; + if (ih < 0 || ih >= args.IH) continue; + + for (int64_t kx = 0; kx < args.KW; ++kx) { + int64_t iw = i_w_base + kx * args.d0; + if (iw < 0 || iw >= args.IW) continue; + + // Convert multi-dimensional coordinates to flat byte offsets + int64_t w_idx = kx*args.nb00 + ky*args.nb01 + kz*args.nb02 + w_cn_idx*args.nb03; + int64_t i_idx = iw*args.nb10 + ih*args.nb11 + id*args.nb12 + src_cn_idx*args.nb13; + + // Dereference memory and cast weights to f32 if they were f16 + float w_val = (float)*(device const T*)((device const char*)src0 + w_idx); + float i_val = *(device const float*)((device const char*)src1 + i_idx); + + sum += w_val * i_val; + } + } + } + } + + // 5. Write the accumulated value out to RAM + int64_t dst_cn_idx = batch_idx * args.OC + oc; + int64_t d_idx = ow*args.nb0 + oh*args.nb1 + od*args.nb2 + dst_cn_idx*args.nb3; + + *(device float*)(dst + d_idx) = sum; +} + +// Explicit instantiations so the JIT compiler can find them by name +template [[host_name("kernel_conv_3d_f32_f32")]] +kernel void kernel_conv_3d( + constant ggml_metal_kargs_conv_3d & args, + device const char * src0, + device const char * src1, + device char * dst, + uint3 tgpig[[threadgroup_position_in_grid]], + uint3 tpitg[[thread_position_in_threadgroup]]); + +// Explicit instantiation for f16 weights +template [[host_name("kernel_conv_3d_f16_f32")]] +kernel void kernel_conv_3d( + constant ggml_metal_kargs_conv_3d & args, + device const char * src0, + device const char * src1, + device char * dst, + uint3 tgpig[[threadgroup_position_in_grid]], + uint3 tpitg[[thread_position_in_threadgroup]]); + + +static inline float bicubic_weight1(float x) { + const float a = -0.75f; + return ((a + 2) * x - (a + 3)) * x * x + 1; +} + +static inline float bicubic_weight2(float x) { + const float a = -0.75f; + return ((a * x - 5 * a) * x + 8 * a) * x - 4 * a; +} + +kernel void kernel_upscale_bicubic_f32( + constant ggml_metal_kargs_upscale & args, + device const char * src0, + device char * dst, + uint3 tgpig[[threadgroup_position_in_grid]], + uint3 tpitg[[thread_position_in_threadgroup]], + uint3 ntg[[threads_per_threadgroup]]) { + + const int64_t i3 = tgpig.z; + const int64_t i2 = tgpig.y; + const int64_t i1 = tgpig.x; + + const int64_t i03 = i3 / args.sf3; + const int64_t i02 = i2 / args.sf2; + + const float f01 = ((float)i1 + args.poffs) / args.sf1 - args.poffs; + const int64_t i01 = (int64_t)floor(f01); + const float fd1 = f01 - (float)i01; + + const float w_y0 = bicubic_weight2(fd1 + 1.0f); + const float w_y1 = bicubic_weight1(fd1); + const float w_y2 = bicubic_weight1(1.0f - fd1); + const float w_y3 = bicubic_weight2(2.0f - fd1); + + const device const char * src_slice = src0 + i03 * args.nb03 + i02 * args.nb02; + + device float * dst_ptr = (device float *)(dst + i3 * args.nb3 + i2 * args.nb2 + i1 * args.nb1); + + for (int i0 = tpitg.x; i0 < args.ne0; i0 += ntg.x) { + const float f00 = ((float)i0 + args.poffs) / args.sf0 - args.poffs; + const int64_t i00 = (int64_t)floor(f00); + const float fd0 = f00 - (float)i00; + + const float w_x0 = bicubic_weight2(fd0 + 1.0f); + const float w_x1 = bicubic_weight1(fd0); + const float w_x2 = bicubic_weight1(1.0f - fd0); + const float w_x3 = bicubic_weight2(2.0f - fd0); + + float sum = 0.0f; + + for (int dy = -1; dy <= 2; ++dy) { + const int64_t iy = MAX(0, MIN(args.ne01 - 1, i01 + dy)); + const float wy = (dy == -1) ? w_y0 : (dy == 0) ? w_y1 : (dy == 1) ? w_y2 : w_y3; + + for (int dx = -1; dx <= 2; ++dx) { + const int64_t ix = MAX(0, MIN(args.ne00 - 1, i00 + dx)); + const float wx = (dx == -1) ? w_x0 : (dx == 0) ? w_x1 : (dx == 1) ? w_x2 : w_x3; + + const device const float * src_ptr = (device const float *)(src_slice + iy * args.nb01 + ix * args.nb00); + sum += (*src_ptr) * wx * wy; + } + } + + dst_ptr[i0] = sum; + } +} + +kernel void kernel_pad_f32( + constant ggml_metal_kargs_pad & args, + device const char * src0, + device char * dst, + uint3 tgpig[[threadgroup_position_in_grid]], + uint3 tpitg[[thread_position_in_threadgroup]], + uint3 ntg[[threads_per_threadgroup]]) { + + const int64_t i3 = tgpig.z; + const int64_t i2 = tgpig.y; + const int64_t i1 = tgpig.x; + + const int64_t i03 = i3; + const int64_t i02 = i2; + const int64_t i01 = i1; + + device const float * src0_ptr = (device const float *) (src0 + i03*args.nb03 + i02*args.nb02 + i01*args.nb01); + device float * dst_ptr = (device float *) (dst + i3*args.nb3 + i2*args.nb2 + i1*args.nb1); + + if (i1 < args.ne01 && i2 < args.ne02 && i3 < args.ne03) { + for (int i0 = tpitg.x; i0 < args.ne0; i0 += ntg.x) { + if (i0 < args.ne00) { + dst_ptr[i0] = src0_ptr[i0]; + } else { + dst_ptr[i0] = 0.0f; + } + } + + return; + } + + for (int i0 = tpitg.x; i0 < args.ne0; i0 += ntg.x) { + dst_ptr[i0] = 0.0f; + } +} + +kernel void kernel_pad_reflect_1d_f32( + constant ggml_metal_kargs_pad_reflect_1d & args, + device const char * src0, + device char * dst, + uint3 tgpig[[threadgroup_position_in_grid]], + uint3 tgpg[[threadgroups_per_grid]], + uint3 tpitg[[thread_position_in_threadgroup]], + uint3 ntg[[threads_per_threadgroup]]) { + + const int64_t i3 = tgpig.z; + const int64_t i2 = tgpig.y; + const int64_t i1 = tgpig.x; + + const int64_t i03 = i3; + const int64_t i02 = i2; + const int64_t i01 = i1; + + device const float * src0_ptr = (device const float *) (src0 + i03*args.nb03 + i02*args.nb02 + i01*args.nb01); + device float * dst_ptr = (device float *) (dst + i3*args.nb3 + i2*args.nb2 + i1*args.nb1); + + if (i1 < args.ne01 && i2 < args.ne02 && i3 < args.ne03) { + for (int i0 = tpitg.x; i0 < args.ne0; i0 += ntg.x) { + if (i0 < args.p0) { + dst_ptr[i0] = src0_ptr[args.p0 - i0]; + } else if (i0 < args.ne0 - args.p1) { + dst_ptr[i0] = src0_ptr[i0 - args.p0]; + } else { + dst_ptr[i0] = src0_ptr[(args.ne0 - args.p1 - args.p0) - (args.p1 + 1 - (args.ne0 - i0)) - 1]; + } + } + } +} + +kernel void kernel_arange_f32( + constant ggml_metal_kargs_arange & args, + device char * dst, + uint3 tgpig[[threadgroup_position_in_grid]], + uint3 tpitg[[thread_position_in_threadgroup]], + uint3 ntg[[threads_per_threadgroup]]) { + + device float * dst_ptr = (device float *) dst; + + for (int i0 = tpitg.x; i0 < args.ne0; i0 += ntg.x) { + dst_ptr[i0] = args.start + args.step * i0; + } +} + +kernel void kernel_timestep_embedding_f32( + constant ggml_metal_kargs_timestep_embedding & args, + device const char * src0, + device char * dst, + uint3 tgpig[[threadgroup_position_in_grid]], + uint3 tpitg[[thread_position_in_threadgroup]], + uint3 ntg[[threads_per_threadgroup]]) { + + int i = tgpig.x; + device float * embed_data = (device float *)(dst + i*args.nb1); + + int half_ = args.dim / 2; + for (int j = tpitg.x; j < half_; j += ntg.x) { + float timestep = ((device float *)src0)[i]; + float freq = (float)exp(-log((float)args.max_period) * j / half_); + float arg = timestep * freq; + embed_data[j ] = cos(arg); + embed_data[j + half_] = sin(arg); + } + + if (args.dim % 2 != 0 && tpitg.x == 0) { + embed_data[2 * half_] = 0.f; + } +} + +// bitonic sort implementation following the CUDA kernels as reference +typedef void (argsort_t)( + constant ggml_metal_kargs_argsort & args, + device const char * src0, + device int32_t * dst, + threadgroup int32_t * shmem_i32 [[threadgroup(0)]], + uint3 tgpig[[threadgroup_position_in_grid]], + ushort3 tpitg[[thread_position_in_threadgroup]], + ushort3 ntg[[threads_per_threadgroup]]); + +template +kernel void kernel_argsort_f32_i32( + constant ggml_metal_kargs_argsort & args, + device const char * src0, + device int32_t * dst, + threadgroup int32_t * shmem_i32 [[threadgroup(0)]], + uint3 tgpig[[threadgroup_position_in_grid]], + ushort3 tpitg[[thread_position_in_threadgroup]], + ushort3 ntg[[threads_per_threadgroup]]) { + // bitonic sort + const int col = tpitg[0]; + const int ib = tgpig[0] / args.ne01; + + const int i00 = ib*ntg.x; + const int i01 = tgpig[0] % args.ne01; + const int i02 = tgpig[1]; + const int i03 = tgpig[2]; + + device const float * src0_row = (device const float *) (src0 + args.nb01*i01 + args.nb02*i02 + args.nb03*i03); + + // initialize indices + shmem_i32[col] = i00 + col; + + threadgroup_barrier(mem_flags::mem_threadgroup); + + for (int k = 2; k <= ntg.x; k *= 2) { + for (int j = k / 2; j > 0; j /= 2) { + int ixj = col ^ j; + if (ixj > col) { + if ((col & k) == 0) { + if (shmem_i32[col] >= args.ne00 || + (shmem_i32[ixj] < args.ne00 && (order == GGML_SORT_ORDER_ASC ? + src0_row[shmem_i32[col]] > src0_row[shmem_i32[ixj]] : + src0_row[shmem_i32[col]] < src0_row[shmem_i32[ixj]])) + ) { + SWAP(shmem_i32[col], shmem_i32[ixj]); + } + } else { + if (shmem_i32[ixj] >= args.ne00 || + (shmem_i32[col] < args.ne00 && (order == GGML_SORT_ORDER_ASC ? + src0_row[shmem_i32[col]] < src0_row[shmem_i32[ixj]] : + src0_row[shmem_i32[col]] > src0_row[shmem_i32[ixj]])) + ) { + SWAP(shmem_i32[col], shmem_i32[ixj]); + } + } + } + + threadgroup_barrier(mem_flags::mem_threadgroup); + } + } + + const int64_t i0 = ib*args.top_k; + + // copy the result to dst without the padding + if (i0 + col < args.ne0 && col < args.top_k) { + dst += i0 + args.ne0*i01 + args.ne0*args.ne1*i02 + args.ne0*args.ne1*args.ne2*i03; + + dst[col] = shmem_i32[col]; + } +} + +template [[host_name("kernel_argsort_f32_i32_asc")]] kernel argsort_t kernel_argsort_f32_i32; +template [[host_name("kernel_argsort_f32_i32_desc")]] kernel argsort_t kernel_argsort_f32_i32; + +typedef void (argsort_merge_t)( + constant ggml_metal_kargs_argsort_merge & args, + device const char * src0, + device const int32_t * tmp, + device int32_t * dst, + uint3 tgpig[[threadgroup_position_in_grid]], + ushort3 tpitg[[thread_position_in_threadgroup]], + ushort3 ntg[[threads_per_threadgroup]]); + +template +kernel void kernel_argsort_merge_f32_i32( + constant ggml_metal_kargs_argsort_merge & args, + device const char * src0, + device const int32_t * tmp, + device int32_t * dst, + uint3 tgpig[[threadgroup_position_in_grid]], + ushort3 tpitg[[thread_position_in_threadgroup]], + ushort3 ntg[[threads_per_threadgroup]]) { + + const int im = tgpig[0] / args.ne01; + const int i01 = tgpig[0] % args.ne01; + const int i02 = tgpig[1]; + const int i03 = tgpig[2]; + + const int start = im * (2 * args.len); + + const int len0 = MIN(args.len, MAX(0, args.ne0 - (int)(start))); + const int len1 = MIN(args.len, MAX(0, args.ne0 - (int)(start + args.len))); + + const int total = len0 + len1; + + device const int32_t * tmp0 = tmp + start + + i01*args.ne0 + + i02*args.ne0*args.ne01 + + i03*args.ne0*args.ne01*args.ne02; + + device const int32_t * tmp1 = tmp0 + args.len; + + dst += start + + i01*args.top_k + + i02*args.top_k*args.ne01 + + i03*args.top_k*args.ne01*args.ne02; + + device const float * src0_row = (device const float *)(src0 + + args.nb01*i01 + + args.nb02*i02 + + args.nb03*i03); + + if (total == 0) { + return; + } + + const int chunk = (total + ntg.x - 1) / ntg.x; + + const int k0 = tpitg.x * chunk; + const int k1 = MIN(MIN(k0 + chunk, total), args.top_k); + + if (k0 >= args.top_k) { + return; + } + + if (k0 >= total) { + return; + } + + int low = k0 > len1 ? k0 - len1 : 0; + int high = MIN(k0, len0); + + // binary-search partition (i, j) such that i + j = k + while (low < high) { + const int mid = (low + high) >> 1; + + const int32_t idx0 = tmp0[mid]; + const int32_t idx1 = tmp1[k0 - mid - 1]; + + const float val0 = src0_row[idx0]; + const float val1 = src0_row[idx1]; + + bool take_left; + if (order == GGML_SORT_ORDER_ASC) { + take_left = (val0 <= val1); + } else { + take_left = (val0 >= val1); + } + + if (take_left) { + low = mid + 1; + } else { + high = mid; + } + } + + int i = low; + int j = k0 - i; + + // keep the merge fronts into registers + int32_t idx0 = 0; + float val0 = 0.0f; + if (i < len0) { + idx0 = tmp0[i]; + val0 = src0_row[idx0]; + } + + int32_t idx1 = 0; + float val1 = 0.0f; + if (j < len1) { + idx1 = tmp1[j]; + val1 = src0_row[idx1]; + } + + for (int k = k0; k < k1; ++k) { + int32_t out_idx; + + if (i >= len0) { + while (k < k1) { + dst[k++] = tmp1[j++]; + } + break; + } else if (j >= len1) { + while (k < k1) { + dst[k++] = tmp0[i++]; + } + break; + } else { + bool take_left; + + if (order == GGML_SORT_ORDER_ASC) { + take_left = (val0 <= val1); + } else { + take_left = (val0 >= val1); + } + + if (take_left) { + out_idx = idx0; + ++i; + if (i < len0) { + idx0 = tmp0[i]; + val0 = src0_row[idx0]; + } + } else { + out_idx = idx1; + ++j; + if (j < len1) { + idx1 = tmp1[j]; + val1 = src0_row[idx1]; + } + } + } + + dst[k] = out_idx; + } +} + +template [[host_name("kernel_argsort_merge_f32_i32_asc")]] kernel argsort_merge_t kernel_argsort_merge_f32_i32; +template [[host_name("kernel_argsort_merge_f32_i32_desc")]] kernel argsort_merge_t kernel_argsort_merge_f32_i32; + +constant bool FC_flash_attn_ext_pad_has_mask [[function_constant(FC_FLASH_ATTN_EXT_PAD + 0)]]; + +constant int32_t FC_flash_attn_ext_pad_ncpsg [[function_constant(FC_FLASH_ATTN_EXT_PAD + 25)]]; + +// pad the last chunk of C elements of k and v into a an extra pad buffer +kernel void kernel_flash_attn_ext_pad( + constant ggml_metal_kargs_flash_attn_ext_pad & args, + device const char * k, + device const char * v, + device const char * mask, + device char * dst, + uint3 tgpig[[threadgroup_position_in_grid]], + ushort tiitg[[thread_index_in_threadgroup]], + ushort3 ntg[[threads_per_threadgroup]]) { + const int32_t C = FC_flash_attn_ext_pad_ncpsg; + + device char * k_pad = dst; + device char * v_pad = k_pad + args.nb11*C*args.ne_12_2*args.ne_12_3; + device char * mask_pad = v_pad + args.nb21*C*args.ne_12_2*args.ne_12_3; + + const int32_t icp = args.ne11 % C; + const int32_t ic0 = args.ne11 - icp; + + const int32_t i1 = tgpig[0]; + const int32_t i2 = tgpig[1]; + const int32_t i3 = tgpig[2]; + + if (i2 < args.ne_12_2 && i3 < args.ne_12_3) { + device const char * k_src = k + args.nb11*(ic0 + i1) + args.nb12*i2 + args.nb13*i3; + device const char * v_src = v + args.nb21*(ic0 + i1) + args.nb22*i2 + args.nb23*i3; + + device char * k_dst = k_pad + args.nb11*i1 + args.nb11*C*i2 + args.nb11*C*args.ne_12_2*i3; + device char * v_dst = v_pad + args.nb21*i1 + args.nb21*C*i2 + args.nb21*C*args.ne_12_2*i3; + + if (i1 >= icp) { + // here it is not important the exact value that will be used as we rely on masking out the scores in the attention + for (uint64_t i = tiitg; i < args.nb11; i += ntg.x) { + k_dst[i] = 0; + } + for (uint64_t i = tiitg; i < args.nb21; i += ntg.x) { + v_dst[i] = 0; + } + } else { + for (uint64_t i = tiitg; i < args.nb11; i += ntg.x) { + k_dst[i] = k_src[i]; + } + for (uint64_t i = tiitg; i < args.nb21; i += ntg.x) { + v_dst[i] = v_src[i]; + } + } + } + + if (FC_flash_attn_ext_pad_has_mask) { + if (i2 < args.ne32 && i3 < args.ne33) { + for (int ib = i1; ib < args.ne31; ib += C) { + device const half * mask_src = (device const half *)(mask + args.nb31*ib + args.nb32*i2 + args.nb33*i3) + ic0; + device half * mask_dst = (device half *)(mask_pad) + C*ib + C*args.ne31*i2 + C*args.ne31*args.ne32*i3; + + for (int i = tiitg; i < C; i += ntg.x) { + if (i >= icp) { + mask_dst[i] = -MAXHALF; + } else { + mask_dst[i] = mask_src[i]; + } + } + } + } + } +} + +constant int32_t FC_flash_attn_ext_blk_nqptg [[function_constant(FC_FLASH_ATTN_EXT_BLK + 24)]]; +constant int32_t FC_flash_attn_ext_blk_ncpsg [[function_constant(FC_FLASH_ATTN_EXT_BLK + 25)]]; + +// scan the blocks of the mask that are not masked +// 0 - masked (i.e. full of -INF, skip) +// 1 - not masked (i.e. at least one element of the mask is not -INF) +// 2 - all zero +kernel void kernel_flash_attn_ext_blk( + constant ggml_metal_kargs_flash_attn_ext_blk & args, + device const char * mask, + device char * dst, + uint3 tgpig[[threadgroup_position_in_grid]], + ushort tiisg[[thread_index_in_simdgroup]]) { + // block size C x Q + const int32_t Q = FC_flash_attn_ext_blk_nqptg; + const int32_t C = FC_flash_attn_ext_blk_ncpsg; + + constexpr short NW = N_SIMDWIDTH; + + const int32_t i3 = tgpig[2]/args.ne32; + const int32_t i2 = tgpig[2]%args.ne32; + const int32_t i1 = tgpig[1]; + const int32_t i0 = tgpig[0]; + + char res = i0*C + C > args.ne30 ? 1 : 0; + + device const half * mask_src = (device const half *) (mask + (i1*Q)*args.nb31 + i2*args.nb32 + i3*args.nb33) + i0*C + tiisg; + + // detailed check of the elements of the block + if ((C > NW || Q > 1) && res == 0) { + half mmin = MAXHALF; + half mmax = -MAXHALF; + + FOR_UNROLL (short j = 0; j < Q; ++j) { + FOR_UNROLL (short ii = 0; ii < C/NW; ++ii) { + mmin = min(mmin, mask_src[ii*NW]); + mmax = max(mmax, mask_src[ii*NW]); + } + + mask_src += args.nb31/2; + } + + mmin = simd_min(mmin); + mmax = simd_max(mmax); + + if (mmax > -MAXHALF) { + if (mmin == 0.0 && mmax == 0.0) { + res = 2; + } else { + res = 1; + } + } + } + + const int32_t nblk1 = ((args.ne01 + Q - 1)/Q); + const int32_t nblk0 = ((args.ne30 + C - 1)/C); + + if (tiisg == 0) { + dst[((i3*args.ne32 + i2)*nblk1 + i1)*nblk0 + i0] = res; + } +} + +constant bool FC_flash_attn_ext_has_mask [[function_constant(FC_FLASH_ATTN_EXT + 0)]]; +constant bool FC_flash_attn_ext_has_sinks [[function_constant(FC_FLASH_ATTN_EXT + 1)]]; +constant bool FC_flash_attn_ext_has_bias [[function_constant(FC_FLASH_ATTN_EXT + 2)]]; +constant bool FC_flash_attn_ext_has_scap [[function_constant(FC_FLASH_ATTN_EXT + 3)]]; +constant bool FC_flash_attn_ext_has_kvpad [[function_constant(FC_FLASH_ATTN_EXT + 4)]]; + +constant bool FC_flash_attn_ext_bc_mask [[function_constant(FC_FLASH_ATTN_EXT + 10)]]; + +//constant float FC_flash_attn_ext_scale [[function_constant(FC_FLASH_ATTN_EXT + 10)]]; +//constant float FC_flash_attn_ext_max_bias [[function_constant(FC_FLASH_ATTN_EXT + 11)]]; +//constant float FC_flash_attn_ext_logit_softcap [[function_constant(FC_FLASH_ATTN_EXT + 12)]]; + +constant int32_t FC_flash_attn_ext_ns10 [[function_constant(FC_FLASH_ATTN_EXT + 20)]]; +constant int32_t FC_flash_attn_ext_ns20 [[function_constant(FC_FLASH_ATTN_EXT + 21)]]; +constant int32_t FC_flash_attn_ext_nsg [[function_constant(FC_FLASH_ATTN_EXT + 22)]]; + +// ref: https://arxiv.org/pdf/2307.08691.pdf +template< + typename q_t, // query types in shared memory + typename q4_t, + typename q8x8_t, + typename k_t, // key types in shared memory + typename k4x4_t, + typename k8x8_t, + typename v_t, // value types in shared memory + typename v4x4_t, + typename v8x8_t, + typename qk_t, // Q*K types + typename qk8x8_t, + typename s_t, // soft-max types + typename s2_t, + typename s8x8_t, + typename o_t, // attention accumulation types + typename o4_t, + typename o8x8_t, + typename kd4x4_t, // key type in device memory + short nl_k, + void (*deq_k)(device const kd4x4_t *, short, thread k4x4_t &), + typename vd4x4_t, // value type in device memory + short nl_v, + void (*deq_v)(device const vd4x4_t *, short, thread v4x4_t &), + short DK, // K head size + short DV, // V head size + short Q, // queries per threadgroup + short C, // cache items per threadgroup + short NSG> // number of simd groups +void kernel_flash_attn_ext_impl( + constant ggml_metal_kargs_flash_attn_ext & args, + device const char * q, + device const char * k, + device const char * v, + device const char * mask, + device const char * sinks, + device const char * pad, + device const char * blk, + device char * dst, + threadgroup half * shmem_f16, + uint3 tgpig, + ushort tiisg, + ushort sgitg) { + const ushort iq3 = tgpig[2]; + const ushort iq2 = tgpig[1]; + const ushort iq1 = tgpig[0]*Q; + +#define NS10 (FC_flash_attn_ext_ns10) +#define NS20 (FC_flash_attn_ext_ns20) + + // note: I had some concerns that using this instead of the ugly macros above was affecting performance + // need to re-check carefully and if no regressions are observerd - remove the macros + // the concerns is that maybe using const variables requires extra registers? but not sure if the compiler + // is clever enough to avoid this. unfortunately, using constexpr is not possible with FC + //const short NS10 = FC_flash_attn_ext_ns10; + //const short NS20 = FC_flash_attn_ext_ns20; + + constexpr short KV = 8; + + constexpr short DK4 = DK/4; + constexpr short DK8 = DK/8; + constexpr short DK16 = DK/16; + constexpr short DV4 = DV/4; + //constexpr short DV8 = DV/8; + constexpr short DV16 = DV/16; + + constexpr short PV = PAD2(DV, 64); + constexpr short PV4 = PV/4; + constexpr short PV8 = PV/8; + //constexpr short PV16 = PV/16; + + constexpr short NW = N_SIMDWIDTH; + constexpr short NQ = Q/NSG; + constexpr short SH = 2*C; // shared memory per simdgroup (s_t == float) + + constexpr short TS = 2*SH; + constexpr short T = DK + 2*PV; // shared memory size per query in (half) + + threadgroup q_t * sq = (threadgroup q_t *) (shmem_f16 + 0*T); // holds the query data + threadgroup q4_t * sq4 = (threadgroup q4_t *) (shmem_f16 + 0*T); // same as above but in q4_t + threadgroup o_t * so = (threadgroup o_t *) (shmem_f16 + 0*T + Q*DK); // the result for all queries in 8x8 matrices (the O matrix from the paper) + threadgroup o4_t * so4 = (threadgroup o4_t *) (shmem_f16 + 0*T + Q*DK); + threadgroup s_t * ss = (threadgroup s_t *) (shmem_f16 + Q*T); // scratch buffer for attention, mask and diagonal matrix + threadgroup s2_t * ss2 = (threadgroup s2_t *) (shmem_f16 + Q*T); // same as above but in s2_t + + threadgroup k_t * sk = (threadgroup k_t *) (shmem_f16 + sgitg*(4*16*KV) + Q*T + Q*TS); // scratch buffer to load K in shared memory + threadgroup k4x4_t * sk4x4 = (threadgroup k4x4_t *) (shmem_f16 + sgitg*(4*16*KV) + Q*T + Q*TS); // same as above but in k4x4_t + + threadgroup v_t * sv = (threadgroup v_t *) (shmem_f16 + sgitg*(4*16*KV) + Q*T + Q*TS); // scratch buffer to load V in shared memory + threadgroup v4x4_t * sv4x4 = (threadgroup v4x4_t *) (shmem_f16 + sgitg*(4*16*KV) + Q*T + Q*TS); // same as above but in v4x4_t + + // mask storage in shared mem + threadgroup half2 * sm2 = (threadgroup half2 *) (shmem_f16 + Q*T + 2*C); + + // per-query mask pointers + device const half2 * pm2[NQ]; + + FOR_UNROLL (short jj = 0; jj < NQ; ++jj) { + const short j = jj*NSG + sgitg; + + pm2[jj] = (device const half2 *) ((device const char *) mask + (iq1 + j)*args.nb31 + (iq2%args.ne32)*args.nb32 + (iq3%args.ne33)*args.nb33); + } + + { + const int32_t nblk1 = ((args.ne01 + Q - 1)/Q); + const int32_t nblk0 = ((args.ne11 + C - 1)/C); + + blk += (((iq3%args.ne33)*args.ne32 + (iq2%args.ne32))*nblk1 + iq1/Q)*nblk0; + } + + { + q += iq1*args.nb01 + iq2*args.nb02 + iq3*args.nb03; + + const short ikv2 = iq2/(args.ne02/args.ne_12_2); + const short ikv3 = iq3/(args.ne03/args.ne_12_3); + + k += ikv2*args.nb12 + ikv3*args.nb13; + v += ikv2*args.nb22 + ikv3*args.nb23; + } + + // load heads from Q to shared memory + FOR_UNROLL (short jj = 0; jj < NQ; ++jj) { + const short j = jj*NSG + sgitg; + + device const float4 * q4 = (device const float4 *) ((device const char *) q + j*args.nb01); + + for (short i = tiisg; i < DK4; i += NW) { + if (iq1 + j < args.ne01) { + sq4[j*DK4 + i] = (q4_t) q4[i]; + } else { + sq4[j*DK4 + i] = 0; + } + } + } + + // zero out + FOR_UNROLL (short jj = 0; jj < NQ; ++jj) { + const short j = jj*NSG + sgitg; + + for (short i = tiisg; i < DV4; i += NW) { + so4[j*PV4 + i] = 0; + } + + for (short i = tiisg; i < SH; i += NW) { + ss[j*SH + i] = 0.0f; + } + } + + threadgroup_barrier(mem_flags::mem_threadgroup); + + float S[NQ] = { [0 ... NQ-1] = 0.0f }; + + { + float M[NQ] = { [0 ... NQ-1] = -FLT_MAX/2 }; + + float slope = 1.0f; + + // ALiBi + if (FC_flash_attn_ext_has_bias) { + const short h = iq2; + + const float base = h < args.n_head_log2 ? args.m0 : args.m1; + const short exph = h < args.n_head_log2 ? h + 1 : 2*(h - args.n_head_log2) + 1; + + slope = pow(base, exph); + } + + // loop over the KV cache + // each simdgroup handles blocks of Q rows and C columns + for (int ic0 = 0; ; ++ic0) { + int ic = ic0*C; + if (ic >= args.ne11) { + break; + } + + // the last partial chunk uses the pad buffer as source + if (FC_flash_attn_ext_has_kvpad && ic + C > args.ne11) { + k = pad; + v = k + args.nb11*C*args.ne_12_2*args.ne_12_3; + mask = v + args.nb21*C*args.ne_12_2*args.ne_12_3; + + const short ikv2 = iq2/(args.ne02/args.ne_12_2); + const short ikv3 = iq3/(args.ne03/args.ne_12_3); + + k += (ikv2 + ikv3*args.ne_12_2)*args.nb11*C; + v += (ikv2 + ikv3*args.ne_12_2)*args.nb21*C; + + if (!FC_flash_attn_ext_has_mask) { + threadgroup half * sm = (threadgroup half *) (sm2); + + FOR_UNROLL (short jj = 0; jj < NQ; ++jj) { + const short j = jj*NSG + sgitg; + + for (short i = tiisg; i < C; i += NW) { + if (ic + i >= args.ne11) { + sm[2*j*SH + i] = -MAXHALF; + } + } + } + } else { + FOR_UNROLL (short jj = 0; jj < NQ; ++jj) { + const short j = jj*NSG + sgitg; + + pm2[jj] = (device const half2 *) ((device const half *) mask + + (iq1 + j)*C + + (iq2%args.ne32)*(C*args.ne31) + + (iq3%args.ne33)*(C*args.ne31*args.ne32)); + } + } + + ic = 0; + } + + char blk_cur = 1; + + // read the mask into shared mem + if (FC_flash_attn_ext_has_mask) { + blk_cur = blk[ic0]; + + if (blk_cur == 0) { + FOR_UNROLL (short jj = 0; jj < NQ; ++jj) { + pm2[jj] += NW; + } + + continue; + } + + if (blk_cur == 1) { + FOR_UNROLL (short jj = 0; jj < NQ; ++jj) { + const short j = jj*NSG + sgitg; + + if (FC_flash_attn_ext_bc_mask) { + sm2[j*SH + tiisg] = (iq1 + j) < args.ne31 ? pm2[jj][tiisg] : half2(-MAXHALF, -MAXHALF); + } else { + sm2[j*SH + tiisg] = pm2[jj][tiisg]; + } + + pm2[jj] += NW; + } + } else if (blk_cur == 2) { + FOR_UNROLL (short jj = 0; jj < NQ; ++jj) { + pm2[jj] += NW; + } + } + +#if 0 + // note: old -INF block optimization - obsoleted by pre-computing non-masked blocks + + threadgroup_barrier(mem_flags::mem_threadgroup); + + // used to detect blocks full of -INF + // skip only when the entire threadgroup is masked + half2 smax2(-MAXHALF/2, -MAXHALF/2); + + FOR_UNROLL (short j = 0; j < Q; ++j) { + smax2 = max(smax2, sm2[j*SH + tiisg]); + } + + smax2 = simd_max(smax2); + + if (max(smax2[0], smax2[1]) <= -MAXHALF/2) { + // this barrier is important + threadgroup_barrier(mem_flags::mem_threadgroup); + + continue; + } +#endif + } + + // Q*K^T + // this is compile-time check, so it does not have runtime overhead + if (is_same::value) { + // we can read directly from global memory + device const k_t * pk = (device const k_t *) (k + ic*args.nb11); + threadgroup const q_t * pq = sq; + threadgroup s_t * ps = ss; + + pk += sgitg*(8*NS10); + ps += sgitg*(8*1); + + static_assert((C/8) % NSG == 0, ""); + + constexpr short NC = (C/8)/NSG; + + FOR_UNROLL (short cc = 0; cc < NC; ++cc) { + qk8x8_t mqk = make_filled_simdgroup_matrix((qk_t) 0.0f); + + if (DK % 16 != 0) { + k8x8_t mk; + q8x8_t mq; + + FOR_UNROLL (short i = 0; i < DK8; ++i) { + simdgroup_barrier(mem_flags::mem_none); + + simdgroup_load(mk, pk + 8*i, NS10, 0, true); + simdgroup_load(mq, pq + 8*i, DK); + + simdgroup_barrier(mem_flags::mem_none); + + simdgroup_multiply_accumulate(mqk, mq, mk, mqk); + } + } else { + k8x8_t mk[2]; + q8x8_t mq[2]; + + // note: too much unroll can tank the performance for large heads + #pragma unroll (MIN(DK8/2, 4*NSG)) + for (short i = 0; i < DK8/2; ++i) { + simdgroup_barrier(mem_flags::mem_none); + + simdgroup_load(mq[0], pq + 0*8 + 16*i, DK); + simdgroup_load(mq[1], pq + 1*8 + 16*i, DK); + + simdgroup_load(mk[0], pk + 0*8 + 16*i, NS10, 0, true); + simdgroup_load(mk[1], pk + 1*8 + 16*i, NS10, 0, true); + + simdgroup_barrier(mem_flags::mem_none); + + simdgroup_multiply_accumulate(mqk, mq[0], mk[0], mqk); + simdgroup_multiply_accumulate(mqk, mq[1], mk[1], mqk); + } + } + + simdgroup_store(mqk, ps, SH, 0, false); + + pk += 8*(NSG*NS10); + ps += 8*(NSG); + } + } else { + // TODO: this is the quantized K cache branch - not optimized yet + for (short ccc = 0; ccc < (C/8)/NSG; ++ccc) { + const short cc = ccc*NSG + sgitg; + + const short tx = tiisg%4; + const short ty = tiisg/4; + + qk8x8_t mqk = make_filled_simdgroup_matrix((qk_t) 0.0f); + + for (short ii = 0; ii < DK16; ii += 4) { + device const kd4x4_t * pk4x4 = (device const kd4x4_t *) (k + ((ic + 8*cc + ty)*args.nb11)); + + if (DK16%4 == 0) { + // the head is evenly divisible by 4*16 = 64, so no need for bound checks + { + k4x4_t tmp; + deq_k(pk4x4 + (ii + tx)/nl_k, (ii + tx)%nl_k, tmp); + sk4x4[4*ty + tx] = tmp; + } + + simdgroup_barrier(mem_flags::mem_threadgroup); + + FOR_UNROLL (short k = 0; k < 4; ++k) { + k8x8_t mk; + q8x8_t mq; + + simdgroup_load(mk, sk + 16*k + 0*8, 4*16, 0, true); // transpose + simdgroup_load(mq, sq + (2*(ii + k) + 0)*8, DK); + simdgroup_multiply_accumulate(mqk, mq, mk, mqk); + + simdgroup_load(mk, sk + 16*k + 1*8, 4*16, 0, true); // transpose + simdgroup_load(mq, sq + (2*(ii + k) + 1)*8, DK); + simdgroup_multiply_accumulate(mqk, mq, mk, mqk); + } + } else { + if (ii + tx < DK16) { + k4x4_t tmp; + deq_k(pk4x4 + (ii + tx)/nl_k, (ii + tx)%nl_k, tmp); + sk4x4[4*ty + tx] = tmp; + } + + simdgroup_barrier(mem_flags::mem_threadgroup); + + for (short k = 0; k < 4 && ii + k < DK16; ++k) { + k8x8_t mk; + q8x8_t mq; + + simdgroup_load(mk, sk + 16*k + 0*8, 4*16, 0, true); // transpose + simdgroup_load(mq, sq + (2*(ii + k) + 0)*8, DK); + simdgroup_multiply_accumulate(mqk, mq, mk, mqk); + + simdgroup_load(mk, sk + 16*k + 1*8, 4*16, 0, true); // transpose + simdgroup_load(mq, sq + (2*(ii + k) + 1)*8, DK); + simdgroup_multiply_accumulate(mqk, mq, mk, mqk); + } + } + } + + simdgroup_store(mqk, ss + 8*cc, SH, 0, false); + } + } + + threadgroup_barrier(mem_flags::mem_threadgroup); + + // online softmax + FOR_UNROLL (short jj = 0; jj < NQ; ++jj) { + const short j = jj*NSG + sgitg; + + const float m = M[jj]; + + // scale and apply the logitcap / mask + float2 s2 = ss2[j*SH/2 + tiisg]*args.scale; + + if (FC_flash_attn_ext_has_scap) { + s2 = args.logit_softcap*precise::tanh(s2); + } + + // mqk = mqk + slope*mask + if (blk_cur != 2) { + if (FC_flash_attn_ext_has_bias) { + s2 += s2_t(sm2[j*SH + tiisg])*slope; + } else { + s2 += s2_t(sm2[j*SH + tiisg]); + } + } + + M[jj] = simd_max(max(M[jj], max(s2[0], s2[1]))); + + const float ms = exp(m - M[jj]); + const float2 vs2 = exp(s2 - M[jj]); + + S[jj] = S[jj]*ms + simd_sum(vs2[0] + vs2[1]); + + // the P matrix from the paper (Q rows, C columns) + ss2[j*SH/2 + tiisg] = vs2; + + if (DV4 % NW == 0) { + FOR_UNROLL (short ii = 0; ii < DV4/NW; ++ii) { + const short i = ii*NW + tiisg; + + so4[j*PV4 + i] *= ms; + } + } else { + for (short i = tiisg; i < DV4; i += NW) { + so4[j*PV4 + i] *= ms; + } + } + } + + threadgroup_barrier(mem_flags::mem_threadgroup); + + // O = O + (Q*K^T)*V + { + // we can read directly from global memory + if (is_same::value) { + static_assert(PV8 % NSG == 0, ""); + + constexpr short NO = PV8/NSG; + + o8x8_t lo[NO]; + + { + auto sot = so + 8*sgitg; + + FOR_UNROLL (short ii = 0; ii < NO; ++ii) { + simdgroup_load(lo[ii], sot, PV, 0, false); + + sot += 8*NSG; + } + } + + { + device const v_t * pv = (device const v_t *) (v + ic*args.nb21); + + pv += 8*sgitg; + + if (DV <= 64) { + FOR_UNROLL (short cc = 0; cc < C/8; ++cc) { + s8x8_t vs; + simdgroup_load(vs, ss + 8*cc, SH, 0, false); + + FOR_UNROLL (short ii = 0; ii < NO/2; ++ii) { + v8x8_t mv[2]; + + simdgroup_load(mv[0], pv + 0*NSG + 16*ii*NSG, NS20, 0, false); + simdgroup_load(mv[1], pv + 8*NSG + 16*ii*NSG, NS20, 0, false); + + simdgroup_multiply_accumulate(lo[2*ii + 0], vs, mv[0], lo[2*ii + 0]); + simdgroup_multiply_accumulate(lo[2*ii + 1], vs, mv[1], lo[2*ii + 1]); + } + + pv += 8*NS20; + } + } else { + constexpr short NC = (C/8)/2; + + FOR_UNROLL (short cc = 0; cc < NC; ++cc) { + s8x8_t vs[2]; + + simdgroup_load(vs[0], ss + 16*cc + 0, SH, 0, false); + simdgroup_load(vs[1], ss + 16*cc + 8, SH, 0, false); + + FOR_UNROLL (short ii = 0; ii < NO/2; ++ii) { + v8x8_t mv[4]; + + simdgroup_load(mv[0], pv + 0*NSG + 16*ii*NSG + 0*8*NS20, NS20, 0, false); + simdgroup_load(mv[1], pv + 8*NSG + 16*ii*NSG + 0*8*NS20, NS20, 0, false); + simdgroup_load(mv[2], pv + 0*NSG + 16*ii*NSG + 1*8*NS20, NS20, 0, false); + simdgroup_load(mv[3], pv + 8*NSG + 16*ii*NSG + 1*8*NS20, NS20, 0, false); + + simdgroup_multiply_accumulate(lo[2*ii + 0], vs[0], mv[0], lo[2*ii + 0]); + simdgroup_multiply_accumulate(lo[2*ii + 1], vs[0], mv[1], lo[2*ii + 1]); + simdgroup_multiply_accumulate(lo[2*ii + 0], vs[1], mv[2], lo[2*ii + 0]); + simdgroup_multiply_accumulate(lo[2*ii + 1], vs[1], mv[3], lo[2*ii + 1]); + } + + pv += 2*8*NS20; + } + } + } + + { + auto sot = so + 8*sgitg; + + FOR_UNROLL (short ii = 0; ii < NO; ++ii) { + simdgroup_store(lo[ii], sot, PV, 0, false); + + sot += 8*NSG; + } + } + } else { + // TODO: this is the quantized V cache branch - not optimized yet + + const short tx = tiisg%4; + const short ty = tiisg/4; + + for (short cc = 0; cc < C/8; ++cc) { + s8x8_t vs; + simdgroup_load(vs, ss + 8*cc, SH, 0, false); + + for (short ii = 4*sgitg; ii < DV16; ii += 4*NSG) { + device const vd4x4_t * pv4x4 = (device const vd4x4_t *) (v + ((ic + 8*cc + ty)*args.nb21)); + + if (DV16%4 == 0) { + // no need for bound checks + { + v4x4_t tmp; + deq_v(pv4x4 + (ii + tx)/nl_v, (ii + tx)%nl_v, tmp); + sv4x4[4*ty + tx] = tmp; + } + + simdgroup_barrier(mem_flags::mem_threadgroup); + + FOR_UNROLL (short k = 0; k < 4; ++k) { + v8x8_t mv[2]; + o8x8_t lo[2]; + + simdgroup_load(mv[0], sv + 16*k + 0*8, 4*16, 0, false); + simdgroup_load(mv[1], sv + 16*k + 1*8, 4*16, 0, false); + simdgroup_load(lo[0], so + 8*(2*(ii + k) + 0), PV, 0, false); + simdgroup_load(lo[1], so + 8*(2*(ii + k) + 1), PV, 0, false); + + simdgroup_multiply_accumulate(lo[0], vs, mv[0], lo[0]); + simdgroup_multiply_accumulate(lo[1], vs, mv[1], lo[1]); + + simdgroup_store(lo[0], so + 8*(2*(ii + k) + 0), PV, 0, false); + simdgroup_store(lo[1], so + 8*(2*(ii + k) + 1), PV, 0, false); + } + } else { + if (ii + tx < DV16) { + v4x4_t tmp; + deq_v(pv4x4 + (ii + tx)/nl_v, (ii + tx)%nl_v, tmp); + sv4x4[4*ty + tx] = tmp; + } + + simdgroup_barrier(mem_flags::mem_threadgroup); + + for (short k = 0; k < 4 && ii + k < DV16; ++k) { + v8x8_t mv[2]; + o8x8_t lo[2]; + + simdgroup_load(mv[0], sv + 16*k + 0*8, 4*16, 0, false); + simdgroup_load(mv[1], sv + 16*k + 1*8, 4*16, 0, false); + simdgroup_load(lo[0], so + 8*(2*(ii + k) + 0), PV, 0, false); + simdgroup_load(lo[1], so + 8*(2*(ii + k) + 1), PV, 0, false); + + simdgroup_multiply_accumulate(lo[0], vs, mv[0], lo[0]); + simdgroup_multiply_accumulate(lo[1], vs, mv[1], lo[1]); + + simdgroup_store(lo[0], so + 8*(2*(ii + k) + 0), PV, 0, false); + simdgroup_store(lo[1], so + 8*(2*(ii + k) + 1), PV, 0, false); + } + } + } + } + } + } + + threadgroup_barrier(mem_flags::mem_threadgroup); + } + + if (FC_flash_attn_ext_has_sinks) { + FOR_UNROLL (short jj = 0; jj < NQ; ++jj) { + const short j = jj*NSG + sgitg; + + const float m = M[jj]; + const float s = tiisg == 0 ? ((device const float *) sinks)[iq2] : -FLT_MAX/2; + + M[jj] = simd_max(max(M[jj], s)); + + const float ms = exp(m - M[jj]); + const float vs = exp(s - M[jj]); + + S[jj] = S[jj]*ms + simd_sum(vs); + + for (short i = tiisg; i < DV4; i += NW) { + so4[j*PV4 + i] *= ms; + } + } + } + } + + // store to global memory + for (short jj = 0; jj < NQ; ++jj) { + const short j = jj*NSG + sgitg; + if (iq1 + j >= args.ne01) { + break; + } + + device float4 * dst4 = (device float4 *) dst + ((uint64_t)iq3*args.ne2*args.ne1 + iq2 + (uint64_t)(iq1 + j)*args.ne1)*DV4; + + const float scale = S[jj] == 0.0 ? 0.0f : 1.0f/S[jj]; + + if (DV4 % NW == 0) { + FOR_UNROLL (short ii = 0; ii < DV4/NW; ++ii) { + const short i = ii*NW + tiisg; + + dst4[i] = (float4) so4[j*PV4 + i]*scale; + } + } else { + for (short i = tiisg; i < DV4; i += NW) { + dst4[i] = (float4) so4[j*PV4 + i]*scale; + } + } + } + +#undef NS10 +#undef NS20 +} + +template< + typename q_t, // query types in shared memory + typename q4_t, + typename q8x8_t, + typename k_t, // key types in shared memory + typename k4x4_t, + typename k8x8_t, + typename v_t, // value types in shared memory + typename v4x4_t, + typename v8x8_t, + typename qk_t, // Q*K types + typename qk8x8_t, + typename s_t, // soft-max types + typename s2_t, + typename s8x8_t, + typename o_t, // attention accumulation types + typename o4_t, + typename o8x8_t, + typename kd4x4_t, // key type in device memory + short nl_k, + void (*deq_k)(device const kd4x4_t *, short, thread k4x4_t &), + typename vd4x4_t, // value type in device memory + short nl_v, + void (*deq_v)(device const vd4x4_t *, short, thread v4x4_t &), + short DK, // K head size + short DV, // V head size + short Q = OP_FLASH_ATTN_EXT_NQPSG, // queries per threadgroup + short C = OP_FLASH_ATTN_EXT_NCPSG> // cache items per threadgroup +kernel void kernel_flash_attn_ext( + constant ggml_metal_kargs_flash_attn_ext & args, + device const char * q, + device const char * k, + device const char * v, + device const char * mask, + device const char * sinks, + device const char * pad, + device const char * blk, + device char * dst, + threadgroup half * shmem_f16 [[threadgroup(0)]], + uint3 tgpig[[threadgroup_position_in_grid]], + ushort tiisg[[thread_index_in_simdgroup]], + ushort sgitg[[simdgroup_index_in_threadgroup]]) { +#define FWD_TMPL q_t, q4_t, q8x8_t, k_t, k4x4_t, k8x8_t, v_t, v4x4_t, v8x8_t, qk_t, qk8x8_t, s_t, s2_t, s8x8_t, o_t, o4_t, o8x8_t, kd4x4_t, nl_k, deq_k, vd4x4_t, nl_v, deq_v, DK, DV, Q, C +#define FWD_ARGS args, q, k, v, mask, sinks, pad, blk, dst, shmem_f16, tgpig, tiisg, sgitg + switch (FC_flash_attn_ext_nsg) { + // note: disabled cases to reduce library load time + //case 1: kernel_flash_attn_ext_impl(FWD_ARGS); break; + //case 2: kernel_flash_attn_ext_impl(FWD_ARGS); break; + case 4: kernel_flash_attn_ext_impl(FWD_ARGS); break; + case 8: kernel_flash_attn_ext_impl(FWD_ARGS); break; + } +#undef FWD_TMPL +#undef FWD_ARGS +} + +// TODO: this is quite ugly. in the future these types will be hardcoded in the kernel, but for now keep them as +// template to be able to explore different combinations +// +#define FA_TYPES \ + half, half4, simdgroup_half8x8, \ + half, half4x4, simdgroup_half8x8, \ + half, half4x4, simdgroup_half8x8, \ + float, simdgroup_float8x8, \ + float, float2, simdgroup_float8x8, \ + float, float4, simdgroup_float8x8 + //half, half4, simdgroup_half8x8 + +#define FA_TYPES_BF \ + bfloat, bfloat4, simdgroup_bfloat8x8, \ + bfloat, bfloat4x4, simdgroup_bfloat8x8, \ + bfloat, bfloat4x4, simdgroup_bfloat8x8, \ + float, simdgroup_float8x8, \ + float, float2, simdgroup_float8x8, \ + half, half4, simdgroup_half8x8 + //float, float4, simdgroup_float8x8 + +#define FA_TYPES_F32 \ + half, half4, simdgroup_half8x8, \ + float, float4x4, simdgroup_float8x8, \ + float, float4x4, simdgroup_float8x8, \ + float, simdgroup_float8x8, \ + float, float2, simdgroup_float8x8, \ + float, float4, simdgroup_float8x8 + //half, half4, simdgroup_half8x8 + +typedef decltype(kernel_flash_attn_ext) flash_attn_ext_t; + +template [[host_name("kernel_flash_attn_ext_f32_dk32_dv32" )]] kernel flash_attn_ext_t kernel_flash_attn_ext; +template [[host_name("kernel_flash_attn_ext_f32_dk40_dv40" )]] kernel flash_attn_ext_t kernel_flash_attn_ext; +template [[host_name("kernel_flash_attn_ext_f32_dk48_dv48" )]] kernel flash_attn_ext_t kernel_flash_attn_ext; +template [[host_name("kernel_flash_attn_ext_f32_dk64_dv64" )]] kernel flash_attn_ext_t kernel_flash_attn_ext; +template [[host_name("kernel_flash_attn_ext_f32_dk72_dv72" )]] kernel flash_attn_ext_t kernel_flash_attn_ext; +template [[host_name("kernel_flash_attn_ext_f32_dk80_dv80" )]] kernel flash_attn_ext_t kernel_flash_attn_ext; +template [[host_name("kernel_flash_attn_ext_f32_dk96_dv96" )]] kernel flash_attn_ext_t kernel_flash_attn_ext; +template [[host_name("kernel_flash_attn_ext_f32_dk112_dv112")]] kernel flash_attn_ext_t kernel_flash_attn_ext; +template [[host_name("kernel_flash_attn_ext_f32_dk128_dv128")]] kernel flash_attn_ext_t kernel_flash_attn_ext; +template [[host_name("kernel_flash_attn_ext_f32_dk192_dv192")]] kernel flash_attn_ext_t kernel_flash_attn_ext; +template [[host_name("kernel_flash_attn_ext_f32_dk192_dv128")]] kernel flash_attn_ext_t kernel_flash_attn_ext; +template [[host_name("kernel_flash_attn_ext_f32_dk256_dv256")]] kernel flash_attn_ext_t kernel_flash_attn_ext; +template [[host_name("kernel_flash_attn_ext_f32_dk320_dv256")]] kernel flash_attn_ext_t kernel_flash_attn_ext; +template [[host_name("kernel_flash_attn_ext_f32_dk512_dv512")]] kernel flash_attn_ext_t kernel_flash_attn_ext; +template [[host_name("kernel_flash_attn_ext_f32_dk576_dv512")]] kernel flash_attn_ext_t kernel_flash_attn_ext; + +template [[host_name("kernel_flash_attn_ext_f16_dk32_dv32" )]] kernel flash_attn_ext_t kernel_flash_attn_ext; +template [[host_name("kernel_flash_attn_ext_f16_dk40_dv40" )]] kernel flash_attn_ext_t kernel_flash_attn_ext; +template [[host_name("kernel_flash_attn_ext_f16_dk48_dv48" )]] kernel flash_attn_ext_t kernel_flash_attn_ext; +template [[host_name("kernel_flash_attn_ext_f16_dk64_dv64" )]] kernel flash_attn_ext_t kernel_flash_attn_ext; +template [[host_name("kernel_flash_attn_ext_f16_dk72_dv72" )]] kernel flash_attn_ext_t kernel_flash_attn_ext; +template [[host_name("kernel_flash_attn_ext_f16_dk80_dv80" )]] kernel flash_attn_ext_t kernel_flash_attn_ext; +template [[host_name("kernel_flash_attn_ext_f16_dk96_dv96" )]] kernel flash_attn_ext_t kernel_flash_attn_ext; +template [[host_name("kernel_flash_attn_ext_f16_dk112_dv112")]] kernel flash_attn_ext_t kernel_flash_attn_ext; +template [[host_name("kernel_flash_attn_ext_f16_dk128_dv128")]] kernel flash_attn_ext_t kernel_flash_attn_ext; +template [[host_name("kernel_flash_attn_ext_f16_dk192_dv192")]] kernel flash_attn_ext_t kernel_flash_attn_ext; +template [[host_name("kernel_flash_attn_ext_f16_dk192_dv128")]] kernel flash_attn_ext_t kernel_flash_attn_ext; +template [[host_name("kernel_flash_attn_ext_f16_dk256_dv256")]] kernel flash_attn_ext_t kernel_flash_attn_ext; +template [[host_name("kernel_flash_attn_ext_f16_dk320_dv256")]] kernel flash_attn_ext_t kernel_flash_attn_ext; +template [[host_name("kernel_flash_attn_ext_f16_dk512_dv512")]] kernel flash_attn_ext_t kernel_flash_attn_ext; +template [[host_name("kernel_flash_attn_ext_f16_dk576_dv512")]] kernel flash_attn_ext_t kernel_flash_attn_ext; + +#if defined(GGML_METAL_HAS_BF16) +template [[host_name("kernel_flash_attn_ext_bf16_dk32_dv32" )]] kernel flash_attn_ext_t kernel_flash_attn_ext; +template [[host_name("kernel_flash_attn_ext_bf16_dk40_dv40" )]] kernel flash_attn_ext_t kernel_flash_attn_ext; +template [[host_name("kernel_flash_attn_ext_bf16_dk48_dv48" )]] kernel flash_attn_ext_t kernel_flash_attn_ext; +template [[host_name("kernel_flash_attn_ext_bf16_dk64_dv64" )]] kernel flash_attn_ext_t kernel_flash_attn_ext; +template [[host_name("kernel_flash_attn_ext_bf16_dk72_dv72" )]] kernel flash_attn_ext_t kernel_flash_attn_ext; +template [[host_name("kernel_flash_attn_ext_bf16_dk80_dv80" )]] kernel flash_attn_ext_t kernel_flash_attn_ext; +template [[host_name("kernel_flash_attn_ext_bf16_dk96_dv96" )]] kernel flash_attn_ext_t kernel_flash_attn_ext; +template [[host_name("kernel_flash_attn_ext_bf16_dk112_dv112")]] kernel flash_attn_ext_t kernel_flash_attn_ext; +template [[host_name("kernel_flash_attn_ext_bf16_dk128_dv128")]] kernel flash_attn_ext_t kernel_flash_attn_ext; +template [[host_name("kernel_flash_attn_ext_bf16_dk192_dv192")]] kernel flash_attn_ext_t kernel_flash_attn_ext; +template [[host_name("kernel_flash_attn_ext_bf16_dk192_dv128")]] kernel flash_attn_ext_t kernel_flash_attn_ext; +template [[host_name("kernel_flash_attn_ext_bf16_dk256_dv256")]] kernel flash_attn_ext_t kernel_flash_attn_ext; +template [[host_name("kernel_flash_attn_ext_bf16_dk320_dv256")]] kernel flash_attn_ext_t kernel_flash_attn_ext; +template [[host_name("kernel_flash_attn_ext_bf16_dk512_dv512")]] kernel flash_attn_ext_t kernel_flash_attn_ext; +template [[host_name("kernel_flash_attn_ext_bf16_dk576_dv512")]] kernel flash_attn_ext_t kernel_flash_attn_ext; +#endif + +template [[host_name("kernel_flash_attn_ext_q4_0_dk32_dv32" )]] kernel flash_attn_ext_t kernel_flash_attn_ext; +template [[host_name("kernel_flash_attn_ext_q4_0_dk40_dv40" )]] kernel flash_attn_ext_t kernel_flash_attn_ext; +template [[host_name("kernel_flash_attn_ext_q4_0_dk48_dv48" )]] kernel flash_attn_ext_t kernel_flash_attn_ext; +template [[host_name("kernel_flash_attn_ext_q4_0_dk64_dv64" )]] kernel flash_attn_ext_t kernel_flash_attn_ext; +template [[host_name("kernel_flash_attn_ext_q4_0_dk72_dv72" )]] kernel flash_attn_ext_t kernel_flash_attn_ext; +template [[host_name("kernel_flash_attn_ext_q4_0_dk80_dv80" )]] kernel flash_attn_ext_t kernel_flash_attn_ext; +template [[host_name("kernel_flash_attn_ext_q4_0_dk96_dv96" )]] kernel flash_attn_ext_t kernel_flash_attn_ext; +template [[host_name("kernel_flash_attn_ext_q4_0_dk112_dv112")]] kernel flash_attn_ext_t kernel_flash_attn_ext; +template [[host_name("kernel_flash_attn_ext_q4_0_dk128_dv128")]] kernel flash_attn_ext_t kernel_flash_attn_ext; +template [[host_name("kernel_flash_attn_ext_q4_0_dk192_dv192")]] kernel flash_attn_ext_t kernel_flash_attn_ext; +template [[host_name("kernel_flash_attn_ext_q4_0_dk192_dv128")]] kernel flash_attn_ext_t kernel_flash_attn_ext; +template [[host_name("kernel_flash_attn_ext_q4_0_dk256_dv256")]] kernel flash_attn_ext_t kernel_flash_attn_ext; +template [[host_name("kernel_flash_attn_ext_q4_0_dk320_dv256")]] kernel flash_attn_ext_t kernel_flash_attn_ext; +template [[host_name("kernel_flash_attn_ext_q4_0_dk512_dv512")]] kernel flash_attn_ext_t kernel_flash_attn_ext; +template [[host_name("kernel_flash_attn_ext_q4_0_dk576_dv512")]] kernel flash_attn_ext_t kernel_flash_attn_ext; + +template [[host_name("kernel_flash_attn_ext_q4_1_dk32_dv32" )]] kernel flash_attn_ext_t kernel_flash_attn_ext; +template [[host_name("kernel_flash_attn_ext_q4_1_dk40_dv40" )]] kernel flash_attn_ext_t kernel_flash_attn_ext; +template [[host_name("kernel_flash_attn_ext_q4_1_dk48_dv48" )]] kernel flash_attn_ext_t kernel_flash_attn_ext; +template [[host_name("kernel_flash_attn_ext_q4_1_dk64_dv64" )]] kernel flash_attn_ext_t kernel_flash_attn_ext; +template [[host_name("kernel_flash_attn_ext_q4_1_dk72_dv72" )]] kernel flash_attn_ext_t kernel_flash_attn_ext; +template [[host_name("kernel_flash_attn_ext_q4_1_dk80_dv80" )]] kernel flash_attn_ext_t kernel_flash_attn_ext; +template [[host_name("kernel_flash_attn_ext_q4_1_dk96_dv96" )]] kernel flash_attn_ext_t kernel_flash_attn_ext; +template [[host_name("kernel_flash_attn_ext_q4_1_dk112_dv112")]] kernel flash_attn_ext_t kernel_flash_attn_ext; +template [[host_name("kernel_flash_attn_ext_q4_1_dk128_dv128")]] kernel flash_attn_ext_t kernel_flash_attn_ext; +template [[host_name("kernel_flash_attn_ext_q4_1_dk192_dv192")]] kernel flash_attn_ext_t kernel_flash_attn_ext; +template [[host_name("kernel_flash_attn_ext_q4_1_dk192_dv128")]] kernel flash_attn_ext_t kernel_flash_attn_ext; +template [[host_name("kernel_flash_attn_ext_q4_1_dk256_dv256")]] kernel flash_attn_ext_t kernel_flash_attn_ext; +template [[host_name("kernel_flash_attn_ext_q4_1_dk320_dv256")]] kernel flash_attn_ext_t kernel_flash_attn_ext; +template [[host_name("kernel_flash_attn_ext_q4_1_dk512_dv512")]] kernel flash_attn_ext_t kernel_flash_attn_ext; +template [[host_name("kernel_flash_attn_ext_q4_1_dk576_dv512")]] kernel flash_attn_ext_t kernel_flash_attn_ext; + +template [[host_name("kernel_flash_attn_ext_q5_0_dk32_dv32" )]] kernel flash_attn_ext_t kernel_flash_attn_ext; +template [[host_name("kernel_flash_attn_ext_q5_0_dk40_dv40" )]] kernel flash_attn_ext_t kernel_flash_attn_ext; +template [[host_name("kernel_flash_attn_ext_q5_0_dk48_dv48" )]] kernel flash_attn_ext_t kernel_flash_attn_ext; +template [[host_name("kernel_flash_attn_ext_q5_0_dk64_dv64" )]] kernel flash_attn_ext_t kernel_flash_attn_ext; +template [[host_name("kernel_flash_attn_ext_q5_0_dk72_dv72" )]] kernel flash_attn_ext_t kernel_flash_attn_ext; +template [[host_name("kernel_flash_attn_ext_q5_0_dk80_dv80" )]] kernel flash_attn_ext_t kernel_flash_attn_ext; +template [[host_name("kernel_flash_attn_ext_q5_0_dk96_dv96" )]] kernel flash_attn_ext_t kernel_flash_attn_ext; +template [[host_name("kernel_flash_attn_ext_q5_0_dk112_dv112")]] kernel flash_attn_ext_t kernel_flash_attn_ext; +template [[host_name("kernel_flash_attn_ext_q5_0_dk128_dv128")]] kernel flash_attn_ext_t kernel_flash_attn_ext; +template [[host_name("kernel_flash_attn_ext_q5_0_dk192_dv192")]] kernel flash_attn_ext_t kernel_flash_attn_ext; +template [[host_name("kernel_flash_attn_ext_q5_0_dk192_dv128")]] kernel flash_attn_ext_t kernel_flash_attn_ext; +template [[host_name("kernel_flash_attn_ext_q5_0_dk256_dv256")]] kernel flash_attn_ext_t kernel_flash_attn_ext; +template [[host_name("kernel_flash_attn_ext_q5_0_dk320_dv256")]] kernel flash_attn_ext_t kernel_flash_attn_ext; +template [[host_name("kernel_flash_attn_ext_q5_0_dk512_dv512")]] kernel flash_attn_ext_t kernel_flash_attn_ext; +template [[host_name("kernel_flash_attn_ext_q5_0_dk576_dv512")]] kernel flash_attn_ext_t kernel_flash_attn_ext; + +template [[host_name("kernel_flash_attn_ext_q5_1_dk32_dv32" )]] kernel flash_attn_ext_t kernel_flash_attn_ext; +template [[host_name("kernel_flash_attn_ext_q5_1_dk40_dv40" )]] kernel flash_attn_ext_t kernel_flash_attn_ext; +template [[host_name("kernel_flash_attn_ext_q5_1_dk48_dv48" )]] kernel flash_attn_ext_t kernel_flash_attn_ext; +template [[host_name("kernel_flash_attn_ext_q5_1_dk64_dv64" )]] kernel flash_attn_ext_t kernel_flash_attn_ext; +template [[host_name("kernel_flash_attn_ext_q5_1_dk72_dv72" )]] kernel flash_attn_ext_t kernel_flash_attn_ext; +template [[host_name("kernel_flash_attn_ext_q5_1_dk80_dv80" )]] kernel flash_attn_ext_t kernel_flash_attn_ext; +template [[host_name("kernel_flash_attn_ext_q5_1_dk96_dv96" )]] kernel flash_attn_ext_t kernel_flash_attn_ext; +template [[host_name("kernel_flash_attn_ext_q5_1_dk112_dv112")]] kernel flash_attn_ext_t kernel_flash_attn_ext; +template [[host_name("kernel_flash_attn_ext_q5_1_dk128_dv128")]] kernel flash_attn_ext_t kernel_flash_attn_ext; +template [[host_name("kernel_flash_attn_ext_q5_1_dk192_dv192")]] kernel flash_attn_ext_t kernel_flash_attn_ext; +template [[host_name("kernel_flash_attn_ext_q5_1_dk192_dv128")]] kernel flash_attn_ext_t kernel_flash_attn_ext; +template [[host_name("kernel_flash_attn_ext_q5_1_dk256_dv256")]] kernel flash_attn_ext_t kernel_flash_attn_ext; +template [[host_name("kernel_flash_attn_ext_q5_1_dk320_dv256")]] kernel flash_attn_ext_t kernel_flash_attn_ext; +template [[host_name("kernel_flash_attn_ext_q5_1_dk512_dv512")]] kernel flash_attn_ext_t kernel_flash_attn_ext; +template [[host_name("kernel_flash_attn_ext_q5_1_dk576_dv512")]] kernel flash_attn_ext_t kernel_flash_attn_ext; + +template [[host_name("kernel_flash_attn_ext_q8_0_dk32_dv32" )]] kernel flash_attn_ext_t kernel_flash_attn_ext; +template [[host_name("kernel_flash_attn_ext_q8_0_dk40_dv40" )]] kernel flash_attn_ext_t kernel_flash_attn_ext; +template [[host_name("kernel_flash_attn_ext_q8_0_dk48_dv48" )]] kernel flash_attn_ext_t kernel_flash_attn_ext; +template [[host_name("kernel_flash_attn_ext_q8_0_dk64_dv64" )]] kernel flash_attn_ext_t kernel_flash_attn_ext; +template [[host_name("kernel_flash_attn_ext_q8_0_dk72_dv72" )]] kernel flash_attn_ext_t kernel_flash_attn_ext; +template [[host_name("kernel_flash_attn_ext_q8_0_dk80_dv80" )]] kernel flash_attn_ext_t kernel_flash_attn_ext; +template [[host_name("kernel_flash_attn_ext_q8_0_dk96_dv96" )]] kernel flash_attn_ext_t kernel_flash_attn_ext; +template [[host_name("kernel_flash_attn_ext_q8_0_dk112_dv112")]] kernel flash_attn_ext_t kernel_flash_attn_ext; +template [[host_name("kernel_flash_attn_ext_q8_0_dk128_dv128")]] kernel flash_attn_ext_t kernel_flash_attn_ext; +template [[host_name("kernel_flash_attn_ext_q8_0_dk192_dv192")]] kernel flash_attn_ext_t kernel_flash_attn_ext; +template [[host_name("kernel_flash_attn_ext_q8_0_dk192_dv128")]] kernel flash_attn_ext_t kernel_flash_attn_ext; +template [[host_name("kernel_flash_attn_ext_q8_0_dk256_dv256")]] kernel flash_attn_ext_t kernel_flash_attn_ext; +template [[host_name("kernel_flash_attn_ext_q8_0_dk320_dv256")]] kernel flash_attn_ext_t kernel_flash_attn_ext; +template [[host_name("kernel_flash_attn_ext_q8_0_dk512_dv512")]] kernel flash_attn_ext_t kernel_flash_attn_ext; +template [[host_name("kernel_flash_attn_ext_q8_0_dk576_dv512")]] kernel flash_attn_ext_t kernel_flash_attn_ext; + +#undef FA_TYPES +#undef FA_TYPES_BF +#undef FA_TYPES_F32 + +constant bool FC_flash_attn_ext_vec_has_mask [[function_constant(FC_FLASH_ATTN_EXT_VEC + 0)]]; +constant bool FC_flash_attn_ext_vec_has_sinks [[function_constant(FC_FLASH_ATTN_EXT_VEC + 1)]]; +constant bool FC_flash_attn_ext_vec_has_bias [[function_constant(FC_FLASH_ATTN_EXT_VEC + 2)]]; +constant bool FC_flash_attn_ext_vec_has_scap [[function_constant(FC_FLASH_ATTN_EXT_VEC + 3)]]; +constant bool FC_flash_attn_ext_vec_has_kvpad [[function_constant(FC_FLASH_ATTN_EXT_VEC + 4)]]; + +//constant float FC_flash_attn_ext_vec_scale [[function_constant(FC_FLASH_ATTN_EXT_VEC + 10)]]; +//constant float FC_flash_attn_ext_vec_max_bias [[function_constant(FC_FLASH_ATTN_EXT_VEC + 11)]]; +//constant float FC_flash_attn_ext_vec_logit_softcap [[function_constant(FC_FLASH_ATTN_EXT_VEC + 12)]]; + +constant int32_t FC_flash_attn_ext_vec_ns10 [[function_constant(FC_FLASH_ATTN_EXT_VEC + 20)]]; +constant int32_t FC_flash_attn_ext_vec_ns20 [[function_constant(FC_FLASH_ATTN_EXT_VEC + 21)]]; +constant int32_t FC_flash_attn_ext_vec_nsg [[function_constant(FC_FLASH_ATTN_EXT_VEC + 22)]]; +constant int32_t FC_flash_attn_ext_vec_nwg [[function_constant(FC_FLASH_ATTN_EXT_VEC + 23)]]; + +template< + typename q4_t, // query types in shared memory + typename k4_t, // key types in shared memory + typename v4_t, // value types in shared memory + typename qk_t, // Q*K types + typename s_t, // soft-max types + typename s4_t, + typename o4_t, // attention accumulation types + typename kd4_t, // key type in device memory + short nl_k, + void (*deq_k_t4)(device const kd4_t *, short, thread k4_t &), + typename vd4_t, // value type in device memory + short nl_v, + void (*deq_v_t4)(device const vd4_t *, short, thread v4_t &), + short DK, // K head size + short DV, // V head size + short NE = 4, // head elements per thread + short Q = OP_FLASH_ATTN_EXT_VEC_NQPSG, // queries per threadgroup + short C = OP_FLASH_ATTN_EXT_VEC_NCPSG> // cache items per threadgroup +kernel void kernel_flash_attn_ext_vec( + constant ggml_metal_kargs_flash_attn_ext_vec & args, + device const char * q, + device const char * k, + device const char * v, + device const char * mask, + device const char * sinks, + device const char * pad, + device char * dst, + threadgroup half * shmem_f16 [[threadgroup(0)]], + uint3 tgpig[[threadgroup_position_in_grid]], + ushort tiisg[[thread_index_in_simdgroup]], + ushort sgitg[[simdgroup_index_in_threadgroup]]) { + static_assert(DK % 32 == 0, "DK must be divisible by 32"); + static_assert(DV % 32 == 0, "DV must be divisible by 32"); + +#define NWG (FC_flash_attn_ext_vec_nwg) +#define NSG (FC_flash_attn_ext_vec_nsg) + +#define NS10 (FC_flash_attn_ext_vec_ns10) +#define NS20 (FC_flash_attn_ext_vec_ns20) + + const short iwg = tgpig[2]%NWG; + + const ushort iq3 = tgpig[2]/NWG; + const ushort iq2 = tgpig[1]; + const ushort iq1 = tgpig[0]; + + constexpr short DK4 = DK/4; + constexpr short DV4 = DV/4; + + constexpr short PK = PAD2(DK, 128); + constexpr short PK4 = PK/4; + + constexpr short PV = PAD2(DV, 128); + constexpr short PV4 = PV/4; + + constexpr short NW = N_SIMDWIDTH; + constexpr short NL = NW/NE; // note: this can be adjusted to support different head sizes and simdgroup work loads + constexpr short SH = 4*C; // shared memory per simdgroup + + static_assert(DK4 % NL == 0, "DK4 must be divisible by NL"); + static_assert(DV4 % NL == 0, "DV4 must be divisible by NL"); + + //const short T = PK + NSG*SH; // shared memory size per query in (half) + + //threadgroup q_t * sq = (threadgroup q_t *) (shmem_f16 + 0*PK); // holds the query data + threadgroup q4_t * sq4 = (threadgroup q4_t *) (shmem_f16 + 0*PK); // same as above but in q4_t + threadgroup s_t * ss = (threadgroup s_t *) (shmem_f16 + sgitg*SH + NSG*PK); // scratch buffer for attention + threadgroup s4_t * ss4 = (threadgroup s4_t *) (shmem_f16 + sgitg*SH + NSG*PK); // same as above but in s4_t + threadgroup half * sm = (threadgroup half *) (shmem_f16 + sgitg*SH + 2*C + NSG*PK); // scratch buffer for mask + threadgroup o4_t * so4 = (threadgroup o4_t *) (shmem_f16 + 2*sgitg*PV + NSG*PK + NSG*SH); // scratch buffer for the results + + // store the result for all queries in shared memory (the O matrix from the paper) + so4 += tiisg; + + { + q += iq1*args.nb01 + iq2*args.nb02 + iq3*args.nb03; + + const short ikv2 = iq2/(args.ne02/args.ne_12_2); + const short ikv3 = iq3/(args.ne03/args.ne_12_3); + + k += ikv2*args.nb12 + ikv3*args.nb13; + v += ikv2*args.nb22 + ikv3*args.nb23; + } + + // load heads from Q to shared memory + device const float4 * q4 = (device const float4 *) ((device const char *) q); + + if (iq1 < args.ne01) { + for (short i = tiisg; i < PK4; i += NW) { + if (i < DK4) { + sq4[i] = (q4_t) q4[i]; + } else { + sq4[i] = (q4_t) 0.0f; + } + } + } + + // zero out so + for (short i = 0; i < DV4/NL; ++i) { + so4[i*NL] = (o4_t) 0.0f; + } + + // zero out shared memory SH + for (short i = tiisg; i < SH/4; i += NW) { + ss4[i] = (s4_t) 0.0f; + } + + threadgroup_barrier(mem_flags::mem_threadgroup); + + { + float S = 0.0f; + float M = -FLT_MAX/2; + + // thread indices inside the simdgroup + const short tx = tiisg%NL; + const short ty = tiisg/NL; + + // pointer to the mask + device const half * pm = (device const half *) (mask + iq1*args.nb31 + (iq2%args.ne32)*args.nb32 + (iq3%args.ne33)*args.nb33); + + float slope = 1.0f; + + // ALiBi + if (FC_flash_attn_ext_vec_has_bias) { + const short h = iq2; + + const float base = h < args.n_head_log2 ? args.m0 : args.m1; + const short exph = h < args.n_head_log2 ? h + 1 : 2*(h - args.n_head_log2) + 1; + + slope = pow(base, exph); + } + + // loop over the KV cache + // each simdgroup handles blocks of Q rows and C columns + for (int ic0 = iwg*NSG + sgitg; ; ic0 += NWG*NSG) { + int ic = ic0*C; + if (ic >= args.ne11) { + break; + } + + // the last partial chunk uses the pad buffer as source + if (FC_flash_attn_ext_vec_has_kvpad && ic + C > args.ne11) { + k = pad; + v = k + args.nb11*C*args.ne_12_2*args.ne_12_3; + mask = v + args.nb21*C*args.ne_12_2*args.ne_12_3; + + const short ikv2 = iq2/(args.ne02/args.ne_12_2); + const short ikv3 = iq3/(args.ne03/args.ne_12_3); + + k += (ikv2 + ikv3*args.ne_12_2)*args.nb11*C; + v += (ikv2 + ikv3*args.ne_12_2)*args.nb21*C; + + if (!FC_flash_attn_ext_vec_has_mask) { + if (ic + tiisg >= args.ne11) { + sm[tiisg] = -MAXHALF; + } + } else { + pm = (device const half *) (mask) + + iq1*C + + (iq2%args.ne32)*(C*args.ne31) + + (iq3%args.ne33)*(C*args.ne31*args.ne32); + } + + ic = 0; + } + + if (FC_flash_attn_ext_vec_has_mask) { + sm[tiisg] = pm[ic + tiisg]; + } + + // skip -INF blocks + if (simd_max(sm[tiisg]) <= -MAXHALF) { + continue; + } + + // Q*K^T + { + device const k4_t * pk4 = (device const k4_t *) (k + ic*args.nb11); + threadgroup const q4_t * pq4 = sq4; + + pk4 += ty*NS10/4 + tx; + pq4 += tx; + + qk_t mqk[C/NE] = { [ 0 ... C/NE - 1] = 0.0f }; + + // each simdgroup processes 1 query and NE (NW/NL) cache elements + FOR_UNROLL (short cc = 0; cc < C/NE; ++cc) { + if (is_same::value) { + FOR_UNROLL (short ii = 0; ii < DK4/NL; ++ii) { + mqk[cc] += dot((float4) pk4[cc*NE*NS10/4 + ii*NL], (float4) pq4[ii*NL]); + } + } else { + device const kd4_t * pk = (device const kd4_t *) (k + ((ic + NE*cc + ty)*args.nb11)); + + k4_t mk; + + FOR_UNROLL (short ii = 0; ii < DK4/NL; ++ii) { + const short i = ii*NL + tx; + + deq_k_t4(pk + i/nl_k, i%nl_k, mk); + + mqk[cc] += dot((float4) mk, (float4) sq4[i]); + } + } + + if (NE == 1) { + mqk[cc] = simd_sum(mqk[cc]); + } else { + // simdgroup reduce (NE = 4) + // [ 0 .. 7] -> [ 0] + // [ 8 .. 15] -> [ 8] + // [16 .. 23] -> [16] + // [24 .. 31] -> [24] + if (NE <= 1) { + mqk[cc] += simd_shuffle_down(mqk[cc], 16); + } + if (NE <= 2) { + mqk[cc] += simd_shuffle_down(mqk[cc], 8); + } + if (NE <= 4) { + mqk[cc] += simd_shuffle_down(mqk[cc], 4); + } + if (NE <= 8) { + mqk[cc] += simd_shuffle_down(mqk[cc], 2); + } + if (NE <= 16) { + mqk[cc] += simd_shuffle_down(mqk[cc], 1); + } + + // broadcast + mqk[cc] = simd_shuffle(mqk[cc], NL*ty); + } + } + + if (FC_flash_attn_ext_vec_has_mask && + !FC_flash_attn_ext_vec_has_scap && + !FC_flash_attn_ext_vec_has_bias) { + ss[NE*tx + ty] = fma(mqk[tx], args.scale, (qk_t) sm[NE*tx + ty]); + } else { + mqk[tx] *= args.scale; + + if (FC_flash_attn_ext_vec_has_scap) { + mqk[tx] = args.logit_softcap*precise::tanh(mqk[tx]); + } + + if (FC_flash_attn_ext_vec_has_bias) { + mqk[tx] += (qk_t) sm[NE*tx + ty]*slope; + } else { + mqk[tx] += (qk_t) sm[NE*tx + ty]; + } + + ss[NE*tx + ty] = mqk[tx]; + } + } + + simdgroup_barrier(mem_flags::mem_threadgroup); + + // online softmax + { + const float m = M; + const float s = ss[tiisg]; + + M = simd_max(max(M, s)); + + const float ms = exp(m - M); + const float vs = exp(s - M); + + S = S*ms + simd_sum(vs); + + // the P matrix from the paper (Q rows, C columns) + ss[tiisg] = vs; + + // O = diag(ms)*O + if ((DV4/NL % NW == 0) || ty == 0) { + FOR_UNROLL (short ii = 0; ii < DV4/NL; ++ii) { + so4[ii*NL] *= ms; + } + } + } + + simdgroup_barrier(mem_flags::mem_threadgroup); + + // O = O + (Q*K^T)*V + { + o4_t lo[DV4/NL]; + FOR_UNROLL (short ii = 0; ii < DV4/NL; ++ii) { + lo[ii] = 0.0f; + } + + if (is_same::value) { + device const v4_t * pv4 = (device const v4_t *) (v + ic*args.nb21); + + pv4 += ty*NS20/4 + tx; + + const auto sst = ss + ty; + + FOR_UNROLL (short cc = 0; cc < C/NE; ++cc) { + FOR_UNROLL (short ii = 0; ii < DV4/NL; ++ii) { + lo[ii] += o4_t(float4(pv4[cc*NE*NS20/4 + ii*NL])*float4(sst[cc*NE])); + } + } + } else { + FOR_UNROLL (short cc = 0; cc < C/NE; ++cc) { + device const vd4_t * pv4 = (device const vd4_t *) (v + ((ic + NE*cc + ty)*args.nb21)); + + FOR_UNROLL (short ii = 0; ii < DV4/NL; ++ii) { + const short i = ii*NL + tx; + + v4_t mv; + deq_v_t4(pv4 + i/nl_v, i%nl_v, mv); + + lo[ii] += o4_t(float4(mv)*float4(ss[NE*cc + ty])); + } + } + } + + FOR_UNROLL (short ii = 0; ii < DV4/NL; ++ii) { + if (NE > 1) { + lo[ii][0] += simd_shuffle_down(lo[ii][0], 16); + lo[ii][1] += simd_shuffle_down(lo[ii][1], 16); + lo[ii][2] += simd_shuffle_down(lo[ii][2], 16); + lo[ii][3] += simd_shuffle_down(lo[ii][3], 16); + } + + if (NE > 2) { + lo[ii][0] += simd_shuffle_down(lo[ii][0], 8); + lo[ii][1] += simd_shuffle_down(lo[ii][1], 8); + lo[ii][2] += simd_shuffle_down(lo[ii][2], 8); + lo[ii][3] += simd_shuffle_down(lo[ii][3], 8); + } + + if (NE > 4) { + lo[ii][0] += simd_shuffle_down(lo[ii][0], 4); + lo[ii][1] += simd_shuffle_down(lo[ii][1], 4); + lo[ii][2] += simd_shuffle_down(lo[ii][2], 4); + lo[ii][3] += simd_shuffle_down(lo[ii][3], 4); + } + + if (NE > 8) { + lo[ii][0] += simd_shuffle_down(lo[ii][0], 2); + lo[ii][1] += simd_shuffle_down(lo[ii][1], 2); + lo[ii][2] += simd_shuffle_down(lo[ii][2], 2); + lo[ii][3] += simd_shuffle_down(lo[ii][3], 2); + } + + if (NE > 16) { + lo[ii][0] += simd_shuffle_down(lo[ii][0], 1); + lo[ii][1] += simd_shuffle_down(lo[ii][1], 1); + lo[ii][2] += simd_shuffle_down(lo[ii][2], 1); + lo[ii][3] += simd_shuffle_down(lo[ii][3], 1); + } + } + + if ((DV4/NL % NW == 0) || ty == 0) { + FOR_UNROLL (short ii = 0; ii < DV4/NL; ++ii) { + so4[ii*NL] += lo[ii]; + } + } + } + } + + if (FC_flash_attn_ext_vec_has_sinks && sgitg == 0 && iwg == 0) { + const float m = M; + const float s = tiisg == 0 ? ((device const float *) sinks)[iq2] : -FLT_MAX/2; + + M = simd_max(max(M, s)); + + const float ms = exp(m - M); + const float vs = exp(s - M); + + S = S*ms + simd_sum(vs); + + if ((DV4/NL % NW == 0) || ty == 0) { + FOR_UNROLL (short ii = 0; ii < DV4/NL; ++ii) { + so4[ii*NL] *= ms; + } + } + } + + // these are needed for reducing the results from the simdgroups (reuse the ss buffer) + if (tiisg == 0) { + ss[0] = (s_t) S; + ss[1] = (s_t) M; + } + } + + so4 -= tiisg; + + threadgroup_barrier(mem_flags::mem_threadgroup); + + // parallel reduce + for (short r = NSG/2; r > 0; r >>= 1) { + if (sgitg < r) { + const float S0 = ss[ 0]; + const float S1 = ss[r*(SH/2) + 0]; + + const float M0 = ss[ 1]; + const float M1 = ss[r*(SH/2) + 1]; + + const float M = max(M0, M1); + + const float ms0 = exp(M0 - M); + const float ms1 = exp(M1 - M); + + const float S = S0*ms0 + S1*ms1; + + if (tiisg == 0) { + ss[0] = S; + ss[1] = M; + } + + // O_0 = diag(ms0)*O_0 + diag(ms1)*O_1 + for (short i = tiisg; i < DV4; i += NW) { + so4[i] = so4[i]*ms0 + so4[i + r*PV4]*ms1; + } + } + + threadgroup_barrier(mem_flags::mem_threadgroup); + } + + // final rescale with 1/S and store to global memory + if (sgitg == 0) { + const int64_t nrows = args.ne3*args.ne2*args.ne1; + const int64_t rid = iq3*args.ne2*args.ne1 + iq2 + iq1*args.ne1; + + device float4 * dst4 = (device float4 *) dst; + device float * dst1 = (device float *) dst + nrows*DV*NWG; // the S and M are stored after the results + + const float S = NWG == 1 ? (ss[0] == 0.0f ? 0.0f : 1.0f/ss[0]) : 1.0f; + + // interleave the workgroup data + for (short i = tiisg; i < DV4; i += NW) { + dst4[rid*DV4*NWG + NWG*i + iwg] = (float4) so4[i]*S; + } + + // store S and M + if (NWG > 1) { + if (tiisg == 0) { + dst1[rid*(2*NWG) + 2*iwg + 0] = ss[0]; + dst1[rid*(2*NWG) + 2*iwg + 1] = ss[1]; + } + } + } + +#undef NWG +#undef NSG +#undef NS10 +#undef NS20 +} + +// note: I think the s_t can be half instead of float, because the Q*K scaling is done before storing to shared mem +// in the other (non-vec) kernel, we need s_t to also be float because we scale during the soft_max +// +#define FA_TYPES \ + half4, \ + half4, \ + half4, \ + float, \ + float, float4, \ + float4 + +#define FA_TYPES_F32 \ + half4, \ + float4, \ + float4, \ + float, \ + float, float4, \ + float4 + +typedef decltype(kernel_flash_attn_ext_vec) flash_attn_ext_vec_t; + +template [[host_name("kernel_flash_attn_ext_vec_f32_dk32_dv32")]] kernel flash_attn_ext_vec_t kernel_flash_attn_ext_vec; +template [[host_name("kernel_flash_attn_ext_vec_f16_dk32_dv32")]] kernel flash_attn_ext_vec_t kernel_flash_attn_ext_vec; +#if defined(GGML_METAL_HAS_BF16) +template [[host_name("kernel_flash_attn_ext_vec_bf16_dk32_dv32")]] kernel flash_attn_ext_vec_t kernel_flash_attn_ext_vec; +#endif +template [[host_name("kernel_flash_attn_ext_vec_q4_0_dk32_dv32")]] kernel flash_attn_ext_vec_t kernel_flash_attn_ext_vec; +template [[host_name("kernel_flash_attn_ext_vec_q4_1_dk32_dv32")]] kernel flash_attn_ext_vec_t kernel_flash_attn_ext_vec; +template [[host_name("kernel_flash_attn_ext_vec_q5_0_dk32_dv32")]] kernel flash_attn_ext_vec_t kernel_flash_attn_ext_vec; +template [[host_name("kernel_flash_attn_ext_vec_q5_1_dk32_dv32")]] kernel flash_attn_ext_vec_t kernel_flash_attn_ext_vec; +template [[host_name("kernel_flash_attn_ext_vec_q8_0_dk32_dv32")]] kernel flash_attn_ext_vec_t kernel_flash_attn_ext_vec; + +template [[host_name("kernel_flash_attn_ext_vec_f32_dk64_dv64")]] kernel flash_attn_ext_vec_t kernel_flash_attn_ext_vec; +template [[host_name("kernel_flash_attn_ext_vec_f16_dk64_dv64")]] kernel flash_attn_ext_vec_t kernel_flash_attn_ext_vec; +#if defined(GGML_METAL_HAS_BF16) +template [[host_name("kernel_flash_attn_ext_vec_bf16_dk64_dv64")]] kernel flash_attn_ext_vec_t kernel_flash_attn_ext_vec; +#endif +template [[host_name("kernel_flash_attn_ext_vec_q4_0_dk64_dv64")]] kernel flash_attn_ext_vec_t kernel_flash_attn_ext_vec; +template [[host_name("kernel_flash_attn_ext_vec_q4_1_dk64_dv64")]] kernel flash_attn_ext_vec_t kernel_flash_attn_ext_vec; +template [[host_name("kernel_flash_attn_ext_vec_q5_0_dk64_dv64")]] kernel flash_attn_ext_vec_t kernel_flash_attn_ext_vec; +template [[host_name("kernel_flash_attn_ext_vec_q5_1_dk64_dv64")]] kernel flash_attn_ext_vec_t kernel_flash_attn_ext_vec; +template [[host_name("kernel_flash_attn_ext_vec_q8_0_dk64_dv64")]] kernel flash_attn_ext_vec_t kernel_flash_attn_ext_vec; + +template [[host_name("kernel_flash_attn_ext_vec_f32_dk96_dv96")]] kernel flash_attn_ext_vec_t kernel_flash_attn_ext_vec; +template [[host_name("kernel_flash_attn_ext_vec_f16_dk96_dv96")]] kernel flash_attn_ext_vec_t kernel_flash_attn_ext_vec; +#if defined(GGML_METAL_HAS_BF16) +template [[host_name("kernel_flash_attn_ext_vec_bf16_dk96_dv96")]] kernel flash_attn_ext_vec_t kernel_flash_attn_ext_vec; +#endif +template [[host_name("kernel_flash_attn_ext_vec_q4_0_dk96_dv96")]] kernel flash_attn_ext_vec_t kernel_flash_attn_ext_vec; +template [[host_name("kernel_flash_attn_ext_vec_q4_1_dk96_dv96")]] kernel flash_attn_ext_vec_t kernel_flash_attn_ext_vec; +template [[host_name("kernel_flash_attn_ext_vec_q5_0_dk96_dv96")]] kernel flash_attn_ext_vec_t kernel_flash_attn_ext_vec; +template [[host_name("kernel_flash_attn_ext_vec_q5_1_dk96_dv96")]] kernel flash_attn_ext_vec_t kernel_flash_attn_ext_vec; +template [[host_name("kernel_flash_attn_ext_vec_q8_0_dk96_dv96")]] kernel flash_attn_ext_vec_t kernel_flash_attn_ext_vec; + +template [[host_name("kernel_flash_attn_ext_vec_f32_dk128_dv128")]] kernel flash_attn_ext_vec_t kernel_flash_attn_ext_vec; +template [[host_name("kernel_flash_attn_ext_vec_f16_dk128_dv128")]] kernel flash_attn_ext_vec_t kernel_flash_attn_ext_vec; +#if defined(GGML_METAL_HAS_BF16) +template [[host_name("kernel_flash_attn_ext_vec_bf16_dk128_dv128")]] kernel flash_attn_ext_vec_t kernel_flash_attn_ext_vec; +#endif +template [[host_name("kernel_flash_attn_ext_vec_q4_0_dk128_dv128")]] kernel flash_attn_ext_vec_t kernel_flash_attn_ext_vec; +template [[host_name("kernel_flash_attn_ext_vec_q4_1_dk128_dv128")]] kernel flash_attn_ext_vec_t kernel_flash_attn_ext_vec; +template [[host_name("kernel_flash_attn_ext_vec_q5_0_dk128_dv128")]] kernel flash_attn_ext_vec_t kernel_flash_attn_ext_vec; +template [[host_name("kernel_flash_attn_ext_vec_q5_1_dk128_dv128")]] kernel flash_attn_ext_vec_t kernel_flash_attn_ext_vec; +template [[host_name("kernel_flash_attn_ext_vec_q8_0_dk128_dv128")]] kernel flash_attn_ext_vec_t kernel_flash_attn_ext_vec; + +template [[host_name("kernel_flash_attn_ext_vec_f32_dk192_dv192")]] kernel flash_attn_ext_vec_t kernel_flash_attn_ext_vec; +template [[host_name("kernel_flash_attn_ext_vec_f16_dk192_dv192")]] kernel flash_attn_ext_vec_t kernel_flash_attn_ext_vec; +#if defined(GGML_METAL_HAS_BF16) +template [[host_name("kernel_flash_attn_ext_vec_bf16_dk192_dv192")]] kernel flash_attn_ext_vec_t kernel_flash_attn_ext_vec; +#endif +template [[host_name("kernel_flash_attn_ext_vec_q4_0_dk192_dv192")]] kernel flash_attn_ext_vec_t kernel_flash_attn_ext_vec; +template [[host_name("kernel_flash_attn_ext_vec_q4_1_dk192_dv192")]] kernel flash_attn_ext_vec_t kernel_flash_attn_ext_vec; +template [[host_name("kernel_flash_attn_ext_vec_q5_0_dk192_dv192")]] kernel flash_attn_ext_vec_t kernel_flash_attn_ext_vec; +template [[host_name("kernel_flash_attn_ext_vec_q5_1_dk192_dv192")]] kernel flash_attn_ext_vec_t kernel_flash_attn_ext_vec; +template [[host_name("kernel_flash_attn_ext_vec_q8_0_dk192_dv192")]] kernel flash_attn_ext_vec_t kernel_flash_attn_ext_vec; + +template [[host_name("kernel_flash_attn_ext_vec_f32_dk192_dv128")]] kernel flash_attn_ext_vec_t kernel_flash_attn_ext_vec; +template [[host_name("kernel_flash_attn_ext_vec_f16_dk192_dv128")]] kernel flash_attn_ext_vec_t kernel_flash_attn_ext_vec; +#if defined(GGML_METAL_HAS_BF16) +template [[host_name("kernel_flash_attn_ext_vec_bf16_dk192_dv128")]] kernel flash_attn_ext_vec_t kernel_flash_attn_ext_vec; +#endif +template [[host_name("kernel_flash_attn_ext_vec_q4_0_dk192_dv128")]] kernel flash_attn_ext_vec_t kernel_flash_attn_ext_vec; +template [[host_name("kernel_flash_attn_ext_vec_q4_1_dk192_dv128")]] kernel flash_attn_ext_vec_t kernel_flash_attn_ext_vec; +template [[host_name("kernel_flash_attn_ext_vec_q5_0_dk192_dv128")]] kernel flash_attn_ext_vec_t kernel_flash_attn_ext_vec; +template [[host_name("kernel_flash_attn_ext_vec_q5_1_dk192_dv128")]] kernel flash_attn_ext_vec_t kernel_flash_attn_ext_vec; +template [[host_name("kernel_flash_attn_ext_vec_q8_0_dk192_dv128")]] kernel flash_attn_ext_vec_t kernel_flash_attn_ext_vec; + +template [[host_name("kernel_flash_attn_ext_vec_f32_dk256_dv256")]] kernel flash_attn_ext_vec_t kernel_flash_attn_ext_vec; +template [[host_name("kernel_flash_attn_ext_vec_f16_dk256_dv256")]] kernel flash_attn_ext_vec_t kernel_flash_attn_ext_vec; +#if defined(GGML_METAL_HAS_BF16) +template [[host_name("kernel_flash_attn_ext_vec_bf16_dk256_dv256")]] kernel flash_attn_ext_vec_t kernel_flash_attn_ext_vec; +#endif +template [[host_name("kernel_flash_attn_ext_vec_q4_0_dk256_dv256")]] kernel flash_attn_ext_vec_t kernel_flash_attn_ext_vec; +template [[host_name("kernel_flash_attn_ext_vec_q4_1_dk256_dv256")]] kernel flash_attn_ext_vec_t kernel_flash_attn_ext_vec; +template [[host_name("kernel_flash_attn_ext_vec_q5_0_dk256_dv256")]] kernel flash_attn_ext_vec_t kernel_flash_attn_ext_vec; +template [[host_name("kernel_flash_attn_ext_vec_q5_1_dk256_dv256")]] kernel flash_attn_ext_vec_t kernel_flash_attn_ext_vec; +template [[host_name("kernel_flash_attn_ext_vec_q8_0_dk256_dv256")]] kernel flash_attn_ext_vec_t kernel_flash_attn_ext_vec; + +template [[host_name("kernel_flash_attn_ext_vec_f32_dk320_dv256")]] kernel flash_attn_ext_vec_t kernel_flash_attn_ext_vec; +template [[host_name("kernel_flash_attn_ext_vec_f16_dk320_dv256")]] kernel flash_attn_ext_vec_t kernel_flash_attn_ext_vec; +#if defined(GGML_METAL_HAS_BF16) +template [[host_name("kernel_flash_attn_ext_vec_bf16_dk320_dv256")]] kernel flash_attn_ext_vec_t kernel_flash_attn_ext_vec; +#endif +template [[host_name("kernel_flash_attn_ext_vec_q4_0_dk320_dv256")]] kernel flash_attn_ext_vec_t kernel_flash_attn_ext_vec; +template [[host_name("kernel_flash_attn_ext_vec_q4_1_dk320_dv256")]] kernel flash_attn_ext_vec_t kernel_flash_attn_ext_vec; +template [[host_name("kernel_flash_attn_ext_vec_q5_0_dk320_dv256")]] kernel flash_attn_ext_vec_t kernel_flash_attn_ext_vec; +template [[host_name("kernel_flash_attn_ext_vec_q5_1_dk320_dv256")]] kernel flash_attn_ext_vec_t kernel_flash_attn_ext_vec; +template [[host_name("kernel_flash_attn_ext_vec_q8_0_dk320_dv256")]] kernel flash_attn_ext_vec_t kernel_flash_attn_ext_vec; + +template [[host_name("kernel_flash_attn_ext_vec_f32_dk512_dv512")]] kernel flash_attn_ext_vec_t kernel_flash_attn_ext_vec; +template [[host_name("kernel_flash_attn_ext_vec_f16_dk512_dv512")]] kernel flash_attn_ext_vec_t kernel_flash_attn_ext_vec; +#if defined(GGML_METAL_HAS_BF16) +template [[host_name("kernel_flash_attn_ext_vec_bf16_dk512_dv512")]] kernel flash_attn_ext_vec_t kernel_flash_attn_ext_vec; +#endif +template [[host_name("kernel_flash_attn_ext_vec_q4_0_dk512_dv512")]] kernel flash_attn_ext_vec_t kernel_flash_attn_ext_vec; +template [[host_name("kernel_flash_attn_ext_vec_q4_1_dk512_dv512")]] kernel flash_attn_ext_vec_t kernel_flash_attn_ext_vec; +template [[host_name("kernel_flash_attn_ext_vec_q5_0_dk512_dv512")]] kernel flash_attn_ext_vec_t kernel_flash_attn_ext_vec; +template [[host_name("kernel_flash_attn_ext_vec_q5_1_dk512_dv512")]] kernel flash_attn_ext_vec_t kernel_flash_attn_ext_vec; +template [[host_name("kernel_flash_attn_ext_vec_q8_0_dk512_dv512")]] kernel flash_attn_ext_vec_t kernel_flash_attn_ext_vec; + +template [[host_name("kernel_flash_attn_ext_vec_f32_dk576_dv512")]] kernel flash_attn_ext_vec_t kernel_flash_attn_ext_vec; +template [[host_name("kernel_flash_attn_ext_vec_f16_dk576_dv512")]] kernel flash_attn_ext_vec_t kernel_flash_attn_ext_vec; +#if defined(GGML_METAL_HAS_BF16) +template [[host_name("kernel_flash_attn_ext_vec_bf16_dk576_dv512")]] kernel flash_attn_ext_vec_t kernel_flash_attn_ext_vec; +#endif +template [[host_name("kernel_flash_attn_ext_vec_q4_0_dk576_dv512")]] kernel flash_attn_ext_vec_t kernel_flash_attn_ext_vec; +template [[host_name("kernel_flash_attn_ext_vec_q4_1_dk576_dv512")]] kernel flash_attn_ext_vec_t kernel_flash_attn_ext_vec; +template [[host_name("kernel_flash_attn_ext_vec_q5_0_dk576_dv512")]] kernel flash_attn_ext_vec_t kernel_flash_attn_ext_vec; +template [[host_name("kernel_flash_attn_ext_vec_q5_1_dk576_dv512")]] kernel flash_attn_ext_vec_t kernel_flash_attn_ext_vec; +template [[host_name("kernel_flash_attn_ext_vec_q8_0_dk576_dv512")]] kernel flash_attn_ext_vec_t kernel_flash_attn_ext_vec; + +#undef FA_TYPES +#undef FA_TYPES_F32 + +constant int32_t FC_flash_attn_ext_vec_reduce_DV [[function_constant(FC_FLASH_ATTN_EXT_VEC_REDUCE + 0)]]; +constant int32_t FC_flash_attn_ext_vec_reduce_NWG [[function_constant(FC_FLASH_ATTN_EXT_VEC_REDUCE + 1)]]; + +kernel void kernel_flash_attn_ext_vec_reduce( + constant ggml_metal_kargs_flash_attn_ext_vec_reduce & args, + device const char * htmp, + device char * dst, + uint tgpig[[threadgroup_position_in_grid]], + ushort tiisg[[thread_index_in_simdgroup]], + ushort sgitg[[simdgroup_index_in_threadgroup]]) { +#define NWG (FC_flash_attn_ext_vec_reduce_NWG) +#define DV (FC_flash_attn_ext_vec_reduce_DV) + + const uint64_t rid = tgpig; + + const short iwg = tiisg; + + device const float * ss = (device const float *) htmp + (uint64_t)args.nrows*DV*NWG; + + float S = ss[rid*(2*NWG) + 2*iwg + 0]; + float M = ss[rid*(2*NWG) + 2*iwg + 1]; + + const float m = simd_max(M); + const float ms = exp(M - m); + + S = simd_sum(S*ms); + S = S == 0.0f ? 0.0f : 1.0f/S; + + const short DV4 = DV/4; + + device const float4 * htmp4 = (device const float4 *) htmp + rid*DV4*NWG; + device float4 * dst4 = (device float4 *) dst + rid*DV4; + + for (short i = sgitg; i < DV4; i += NWG) { + const float4 v = simd_sum(htmp4[i*NWG + iwg]*ms); + + if (iwg == 0) { + dst4[i] = v*S; + } + } + +#undef NWG +#undef DV +} + +template +kernel void kernel_cpy_t_t( + constant ggml_metal_kargs_cpy & args, + device const char * src0, + device char * dst, + uint3 tgpig[[threadgroup_position_in_grid]], + ushort tiitg[[thread_index_in_threadgroup]], + ushort3 ntg[[threads_per_threadgroup]]) { + const int i03 = tgpig[2]; + const int i02 = tgpig[1]; + const int i01 = ntg[1] == 1 ? tgpig[0]%args.ne01 : tgpig[0]*ntg[1] + tiitg/ntg[0]; + const int iw0 = ntg[1] == 1 ? tgpig[0]/args.ne01 : 0; + + const int64_t n = i03*args.ne02*args.ne01*args.ne00 + i02*args.ne01*args.ne00 + i01*args.ne00; + + const int64_t i3 = n/(args.ne2*args.ne1*args.ne0); + const int64_t i2 = (n - i3*args.ne2*args.ne1*args.ne0)/(args.ne1*args.ne0); + const int64_t i1 = (n - i3*args.ne2*args.ne1*args.ne0 - i2*args.ne1*args.ne0)/args.ne0; + const int64_t i0 = (n - i3*args.ne2*args.ne1*args.ne0 - i2*args.ne1*args.ne0 - i1*args.ne0); + + device T1 * dst_data = (device T1 *) (dst + i3*args.nb3 + i2*args.nb2 + i1*args.nb1 + i0*args.nb0); + + for (int64_t i00 = iw0*ntg[0] + tiitg%ntg[0]; i00 < args.ne00; ) { + device const T0 * src = (device T0 *)(src0 + i03*args.nb03 + i02*args.nb02 + i01*args.nb01 + i00*args.nb00); + dst_data[i00] = (T1) src[0]; + break; + } +} + +typedef decltype(kernel_cpy_t_t) kernel_cpy_t; + +template [[host_name("kernel_cpy_f32_f32")]] kernel kernel_cpy_t kernel_cpy_t_t; +template [[host_name("kernel_cpy_f32_f16")]] kernel kernel_cpy_t kernel_cpy_t_t; +template [[host_name("kernel_cpy_f32_i32")]] kernel kernel_cpy_t kernel_cpy_t_t; +template [[host_name("kernel_cpy_i32_f32")]] kernel kernel_cpy_t kernel_cpy_t_t; +template [[host_name("kernel_cpy_i32_i32")]] kernel kernel_cpy_t kernel_cpy_t_t; +#if defined(GGML_METAL_HAS_BF16) +template [[host_name("kernel_cpy_f32_bf16")]] kernel kernel_cpy_t kernel_cpy_t_t; +#endif +template [[host_name("kernel_cpy_f16_f32")]] kernel kernel_cpy_t kernel_cpy_t_t; +template [[host_name("kernel_cpy_f16_f16")]] kernel kernel_cpy_t kernel_cpy_t_t; +#if defined(GGML_METAL_HAS_BF16) +template [[host_name("kernel_cpy_bf16_f32")]] kernel kernel_cpy_t kernel_cpy_t_t; +template [[host_name("kernel_cpy_bf16_bf16")]] kernel kernel_cpy_t kernel_cpy_t_t; +#endif + +template +kernel void kernel_cpy_f32_q( + constant ggml_metal_kargs_cpy & args, + device const char * src0, + device char * dst, + uint3 tgpig[[threadgroup_position_in_grid]], + ushort tiitg[[thread_index_in_threadgroup]], + ushort3 ntg[[threads_per_threadgroup]]) { + const int i03 = tgpig[2]; + const int i02 = tgpig[1]; + const int i01 = ntg[1] == 1 ? tgpig[0]%args.ne01 : tgpig[0]*ntg[1] + tiitg/ntg[0]; + const int iw0 = ntg[1] == 1 ? tgpig[0]/args.ne01 : 0; + + const int64_t n = i03*args.ne02*args.ne01*args.ne00 + i02*args.ne01*args.ne00 + i01*args.ne00; + + const int64_t i3 = n / (args.ne2*args.ne1*args.ne0); + const int64_t i2 = (n - i3*args.ne2*args.ne1*args.ne0) / (args.ne1*args.ne0); + const int64_t i1 = (n - i3*args.ne2*args.ne1*args.ne0 - i2*args.ne1*args.ne0) / args.ne0; + const int64_t i0 = (n - i3*args.ne2*args.ne1*args.ne0 - i2*args.ne1*args.ne0 - i1*args.ne0)/QK; + + device block_q * dst_data = (device block_q *)(dst + i3*args.nb3 + i2*args.nb2 + i1*args.nb1 + i0*args.nb0); + + for (int64_t i00 = iw0*ntg[0] + tiitg%ntg[0]; i00 < args.nk0; ) { + device const float * src = (device const float *)(src0 + i03*args.nb03 + i02*args.nb02 + i01*args.nb01 + (i00*QK)*args.nb00); + + quantize_func(src, dst_data[i00]); + + break; + } +} + +typedef decltype(kernel_cpy_f32_q) cpy_f_q_t; + +template [[host_name("kernel_cpy_f32_q8_0")]] kernel cpy_f_q_t kernel_cpy_f32_q; +template [[host_name("kernel_cpy_f32_q4_0")]] kernel cpy_f_q_t kernel_cpy_f32_q; +template [[host_name("kernel_cpy_f32_q4_1")]] kernel cpy_f_q_t kernel_cpy_f32_q; +template [[host_name("kernel_cpy_f32_q5_0")]] kernel cpy_f_q_t kernel_cpy_f32_q; +template [[host_name("kernel_cpy_f32_q5_1")]] kernel cpy_f_q_t kernel_cpy_f32_q; +template [[host_name("kernel_cpy_f32_iq4_nl")]] kernel cpy_f_q_t kernel_cpy_f32_q; + +template +kernel void kernel_cpy_q_f32( + constant ggml_metal_kargs_cpy & args, + device const char * src0, + device char * dst, + uint3 tgpig[[threadgroup_position_in_grid]], + ushort tiitg[[thread_index_in_threadgroup]], + ushort3 ntg[[threads_per_threadgroup]]) { + const int i03 = tgpig[2]; + const int i02 = tgpig[1]; + const int i01 = ntg[1] == 1 ? tgpig[0]%args.ne01 : tgpig[0]*ntg[1] + tiitg/ntg[0]; + const int iw0 = ntg[1] == 1 ? tgpig[0]/args.ne01 : 0; + + const int64_t n = i03*args.ne02*args.ne01*args.ne00 + i02*args.ne01*args.ne00 + i01*args.ne00; + + const int64_t i3 = n/(args.ne2*args.ne1*args.ne0); + const int64_t i2 = (n - i3*args.ne2*args.ne1*args.ne0)/(args.ne1*args.ne0); + const int64_t i1 = (n - i3*args.ne2*args.ne1*args.ne0 - i2*args.ne1*args.ne0)/args.ne0; + const int64_t i0 = (n - i3*args.ne2*args.ne1*args.ne0 - i2*args.ne1*args.ne0 - i1*args.ne0); + + device const block_q * src_data = (device const block_q *)(src0 + i03*args.nb03 + i02*args.nb02 + i01*args.nb01); + device T4x4 * dst_data = (device T4x4 *)(dst + i3*args.nb3 + i2*args.nb2 + i1*args.nb1 + i0*args.nb0); + + for (int64_t i00 = iw0*ntg[0] + tiitg%ntg[0]; i00 < args.nk0; ) { + T4x4 temp; + dequantize_func(src_data + i00/nl, i00%nl, temp); + dst_data[i00] = temp; + + break; + } +} + +typedef decltype(kernel_cpy_q_f32) cpy_q_f_t; + +template [[host_name("kernel_cpy_q4_0_f32")]] kernel cpy_q_f_t kernel_cpy_q_f32; +template [[host_name("kernel_cpy_q4_1_f32")]] kernel cpy_q_f_t kernel_cpy_q_f32; +template [[host_name("kernel_cpy_q5_0_f32")]] kernel cpy_q_f_t kernel_cpy_q_f32; +template [[host_name("kernel_cpy_q5_1_f32")]] kernel cpy_q_f_t kernel_cpy_q_f32; +template [[host_name("kernel_cpy_q8_0_f32")]] kernel cpy_q_f_t kernel_cpy_q_f32; + +template [[host_name("kernel_cpy_q4_0_f16")]] kernel cpy_q_f_t kernel_cpy_q_f32; +template [[host_name("kernel_cpy_q4_1_f16")]] kernel cpy_q_f_t kernel_cpy_q_f32; +template [[host_name("kernel_cpy_q5_0_f16")]] kernel cpy_q_f_t kernel_cpy_q_f32; +template [[host_name("kernel_cpy_q5_1_f16")]] kernel cpy_q_f_t kernel_cpy_q_f32; +template [[host_name("kernel_cpy_q8_0_f16")]] kernel cpy_q_f_t kernel_cpy_q_f32; + +kernel void kernel_concat( + constant ggml_metal_kargs_concat & args, + device const char * src0, + device const char * src1, + device char * dst, + uint3 tgpig[[threadgroup_position_in_grid]], + ushort3 tpitg[[thread_position_in_threadgroup]], + ushort3 ntg[[threads_per_threadgroup]]) { + + const int i3 = tgpig.z; + const int i2 = tgpig.y; + const int i1 = tgpig.x; + + int o[4] = {0, 0, 0, 0}; + o[args.dim] = args.dim == 0 ? args.ne00 : (args.dim == 1 ? args.ne01 : (args.dim == 2 ? args.ne02 : args.ne03)); + + device const float * x; + + for (int i0 = tpitg.x; i0 < args.ne0; i0 += ntg.x) { + if (i0 < args.ne00 && i1 < args.ne01 && i2 < args.ne02 && i3 < args.ne03) { + x = (device const float *)(src0 + (i3 )*args.nb03 + (i2 )*args.nb02 + (i1 )*args.nb01 + (i0 )*args.nb00); + } else { + x = (device const float *)(src1 + (i3 - o[3])*args.nb13 + (i2 - o[2])*args.nb12 + (i1 - o[1])*args.nb11 + (i0 - o[0])*args.nb10); + } + + device float * y = (device float *)(dst + i3*args.nb3 + i2*args.nb2 + i1*args.nb1 + i0*args.nb0); + + *y = *x; + } +} + +template +void kernel_mul_mv_q2_K_f32_impl( + args_t args, + device const char * src0, + device const char * src1, + device char * dst, + threadgroup char * shmem, + uint3 tgpig, + ushort tiisg, + ushort sgitg) { + const short NSG = FC_mul_mv_nsg; + + const int nb = args.ne00/QK_K; + + const int r0 = tgpig.x; + const int r1 = tgpig.y; + const int im = tgpig.z; + + const int first_row = (r0 * NSG + sgitg) * nr0; + + const uint i12 = im%args.ne12; + const uint i13 = im/args.ne12; + + const uint64_t offset0 = first_row*args.nb01 + (i12/args.r2)*args.nb02 + (i13/args.r3)*args.nb03; + const uint64_t offset1 = r1*args.nb11 + (i12 )*args.nb12 + (i13 )*args.nb13; + + device const block_q2_K * x = (device const block_q2_K *) (src0 + offset0); + device const float * y = (device const float *) (src1 + offset1); + + float yl[32]; + float sumf[nr0]={0.f}; + + const short ix = tiisg/8; // 0...3 + const short it = tiisg%8; // 0...7 + const short iq = it/4; // 0 or 1 + const short ir = it%4; // 0...3 + const short is = (8*ir)/16;// 0 or 1 + + device const float * y4 = y + ix * QK_K + 128 * iq + 8 * ir; + + for (int ib = ix; ib < nb; ib += 4) { + float4 sumy = {0.f, 0.f, 0.f, 0.f}; + for (short i = 0; i < 8; ++i) { + yl[i+ 0] = y4[i+ 0]; sumy[0] += yl[i+ 0]; + yl[i+ 8] = y4[i+32]; sumy[1] += yl[i+ 8]; + yl[i+16] = y4[i+64]; sumy[2] += yl[i+16]; + yl[i+24] = y4[i+96]; sumy[3] += yl[i+24]; + } + + device const uint8_t * sc = (device const uint8_t *)x[ib].scales + 8*iq + is; + device const uint16_t * qs = (device const uint16_t *)x[ib].qs + 16 * iq + 4 * ir; + device const half * dh = &x[ib].d; + + for (short row = 0; row < nr0; row++) { + float4 acc1 = {0.f, 0.f, 0.f, 0.f}; + float4 acc2 = {0.f, 0.f, 0.f, 0.f}; + for (int i = 0; i < 8; i += 2) { + acc1[0] += yl[i+ 0] * (qs[i/2] & 0x0003); + acc2[0] += yl[i+ 1] * (qs[i/2] & 0x0300); + acc1[1] += yl[i+ 8] * (qs[i/2] & 0x000c); + acc2[1] += yl[i+ 9] * (qs[i/2] & 0x0c00); + acc1[2] += yl[i+16] * (qs[i/2] & 0x0030); + acc2[2] += yl[i+17] * (qs[i/2] & 0x3000); + acc1[3] += yl[i+24] * (qs[i/2] & 0x00c0); + acc2[3] += yl[i+25] * (qs[i/2] & 0xc000); + } + float dall = dh[0]; + float dmin = dh[1] * 1.f/16.f; + sumf[row] += dall * ((acc1[0] + 1.f/256.f * acc2[0]) * (sc[0] & 0xF) * 1.f/ 1.f + + (acc1[1] + 1.f/256.f * acc2[1]) * (sc[2] & 0xF) * 1.f/ 4.f + + (acc1[2] + 1.f/256.f * acc2[2]) * (sc[4] & 0xF) * 1.f/16.f + + (acc1[3] + 1.f/256.f * acc2[3]) * (sc[6] & 0xF) * 1.f/64.f) - + dmin * (sumy[0] * (sc[0] & 0xF0) + sumy[1] * (sc[2] & 0xF0) + sumy[2] * (sc[4] & 0xF0) + sumy[3] * (sc[6] & 0xF0)); + + qs += args.nb01/2; + sc += args.nb01; + dh += args.nb01/2; + } + + y4 += 4 * QK_K; + } + + device float * dst_f32 = (device float *) dst + (uint64_t)im*args.ne0*args.ne1 + (uint64_t)r1*args.ne0; + + for (int row = 0; row < nr0 && first_row + row < args.ne0; ++row) { + float sum_all = simd_sum(sumf[row]); + if (tiisg == 0) { + dst_f32[first_row + row] = sum_all; + } + } +} + +[[host_name("kernel_mul_mv_q2_K_f32")]] +kernel void kernel_mul_mv_q2_K_f32( + constant ggml_metal_kargs_mul_mv & args, + device const char * src0, + device const char * src1, + device char * dst, + uint3 tgpig[[threadgroup_position_in_grid]], + ushort tiisg[[thread_index_in_simdgroup]], + ushort sgitg[[simdgroup_index_in_threadgroup]]) { + + kernel_mul_mv_q2_K_f32_impl(args, src0, src1, dst, nullptr, tgpig, tiisg, sgitg); +} + +template +void kernel_mul_mv_q3_K_f32_impl( + args_t args, + device const char * src0, + device const char * src1, + device char * dst, + threadgroup char * shmem, + uint3 tgpig, + ushort tiisg, + ushort sgitg) { + const short NSG = FC_mul_mv_nsg; + + const int nb = args.ne00/QK_K; + + const int r0 = tgpig.x; + const int r1 = tgpig.y; + const int im = tgpig.z; + + const int first_row = (r0 * NSG + sgitg) * nr0; + + const uint i12 = im%args.ne12; + const uint i13 = im/args.ne12; + + const uint64_t offset0 = first_row*args.nb01 + (i12/args.r2)*args.nb02 + (i13/args.r3)*args.nb03; + const uint64_t offset1 = r1*args.nb11 + (i12 )*args.nb12 + (i13 )*args.nb13; + + device const block_q3_K * x = (device const block_q3_K *) (src0 + offset0); + device const float * yy = (device const float *) (src1 + offset1); + + float yl[32]; + + //const uint16_t kmask1 = 0x3030; + //const uint16_t kmask2 = 0x0f0f; + + const short tid = tiisg/4; + const short ix = tiisg%4; + const short ip = tid/4; // 0 or 1 + const short il = 2*((tid%4)/2); // 0 or 2 + const short ir = tid%2; + const short l0 = 8*ir; + + // One would think that the Metal compiler would figure out that ip and il can only have + // 4 possible states, and optimize accordingly. Well, no. It needs help, and we do it + // with these two tales. + // + // Possible masks for the high bit + const ushort4 mm[4] = {{0x0001, 0x0100, 0x0002, 0x0200}, // ip = 0, il = 0 + {0x0004, 0x0400, 0x0008, 0x0800}, // ip = 0, il = 2 + {0x0010, 0x1000, 0x0020, 0x2000}, // ip = 1, il = 0 + {0x0040, 0x4000, 0x0080, 0x8000}}; // ip = 1, il = 2 + + // Possible masks for the low 2 bits + const int4 qm[2] = {{0x0003, 0x0300, 0x000c, 0x0c00}, {0x0030, 0x3000, 0x00c0, 0xc000}}; + + const ushort4 hm = mm[2*ip + il/2]; + + const short shift = 2*il; + + const float v1 = il == 0 ? 4.f : 64.f; + const float v2 = 4.f * v1; + + const uint16_t s_shift1 = 4*ip; + const uint16_t s_shift2 = s_shift1 + il; + + const short q_offset = 32*ip + l0; + const short y_offset = 128*ip + 32*il + l0; + + device const float * y1 = yy + ix*QK_K + y_offset; + + uint32_t scales32, aux32; + thread uint16_t * scales16 = (thread uint16_t *)&scales32; + thread const int8_t * scales = (thread const int8_t *)&scales32; + + float sumf1[nr0] = {0.f}; + float sumf2[nr0] = {0.f}; + + for (int i = ix; i < nb; i += 4) { + for (short l = 0; l < 8; ++l) { + yl[l+ 0] = y1[l+ 0]; + yl[l+ 8] = y1[l+16]; + yl[l+16] = y1[l+32]; + yl[l+24] = y1[l+48]; + } + + device const uint16_t * q = (device const uint16_t *)(x[i].qs + q_offset); + device const uint16_t * h = (device const uint16_t *)(x[i].hmask + l0); + device const uint16_t * a = (device const uint16_t *)(x[i].scales); + device const half * dh = &x[i].d; + + for (short row = 0; row < nr0; ++row) { + const float d_all = (float)dh[0]; + + scales16[0] = a[4]; + scales16[1] = a[5]; + aux32 = ((scales32 >> s_shift2) << 4) & 0x30303030; + scales16[0] = a[il+0]; + scales16[1] = a[il+1]; + scales32 = ((scales32 >> s_shift1) & 0x0f0f0f0f) | aux32; + + float s1 = 0, s2 = 0, s3 = 0, s4 = 0, s5 = 0, s6 = 0; + for (short l = 0; l < 8; l += 2) { + const int32_t qs = q[l/2]; + s1 += yl[l+0] * (qs & qm[il/2][0]); + s2 += yl[l+1] * (qs & qm[il/2][1]); + s3 += ((h[l/2] & hm[0]) ? 0.f : yl[l+0]) + ((h[l/2] & hm[1]) ? 0.f : yl[l+1]); + s4 += yl[l+16] * (qs & qm[il/2][2]); + s5 += yl[l+17] * (qs & qm[il/2][3]); + s6 += ((h[l/2] & hm[2]) ? 0.f : yl[l+16]) + ((h[l/2] & hm[3]) ? 0.f : yl[l+17]); + } + float d1 = d_all * (s1 + 1.f/256.f * s2 - s3*v1); + float d2 = d_all * (s4 + 1.f/256.f * s5 - s6*v2); + sumf1[row] += d1 * (scales[0] - 32); + sumf2[row] += d2 * (scales[2] - 32); + + s1 = s2 = s3 = s4 = s5 = s6 = 0; + for (short l = 0; l < 8; l += 2) { + const int32_t qs = q[l/2+8]; + s1 += yl[l+8] * (qs & qm[il/2][0]); + s2 += yl[l+9] * (qs & qm[il/2][1]); + s3 += ((h[l/2+8] & hm[0]) ? 0.f : yl[l+8]) + ((h[l/2+8] & hm[1]) ? 0.f : yl[l+9]); + s4 += yl[l+24] * (qs & qm[il/2][2]); + s5 += yl[l+25] * (qs & qm[il/2][3]); + s6 += ((h[l/2+8] & hm[2]) ? 0.f : yl[l+24]) + ((h[l/2+8] & hm[3]) ? 0.f : yl[l+25]); + } + d1 = d_all * (s1 + 1.f/256.f * s2 - s3*v1); + d2 = d_all * (s4 + 1.f/256.f * s5 - s6*v2); + sumf1[row] += d1 * (scales[1] - 32); + sumf2[row] += d2 * (scales[3] - 32); + + q += args.nb01/2; + h += args.nb01/2; + a += args.nb01/2; + dh += args.nb01/2; + } + + y1 += 4 * QK_K; + } + + for (int row = 0; row < nr0; ++row) { + const float sumf = (sumf1[row] + 0.25f * sumf2[row]) / (1 << shift); + sumf1[row] = simd_sum(sumf); + } + + device float * dst_f32 = (device float *) dst + (uint64_t)im*args.ne0*args.ne1 + (uint64_t)r1*args.ne0; + + if (tiisg == 0) { + for (int row = 0; row < nr0 && first_row + row < args.ne0; ++row) { + dst_f32[first_row + row] = sumf1[row]; + } + } +} + +[[host_name("kernel_mul_mv_q3_K_f32")]] +kernel void kernel_mul_mv_q3_K_f32( + constant ggml_metal_kargs_mul_mv & args, + device const char * src0, + device const char * src1, + device char * dst, + uint3 tgpig[[threadgroup_position_in_grid]], + ushort tiisg[[thread_index_in_simdgroup]], + ushort sgitg[[simdgroup_index_in_threadgroup]]) { + + kernel_mul_mv_q3_K_f32_impl(args, src0, src1, dst, nullptr, tgpig, tiisg, sgitg); +} + +template +void kernel_mul_mv_q4_K_f32_impl( + args_t args, + device const char * src0, + device const char * src1, + device char * dst, + threadgroup char * shmem, + uint3 tgpig, + ushort tiisg, + ushort sgitg) { + const short NSG = FC_mul_mv_nsg; + + constexpr uint16_t kmask1 = 0x3f3f; + constexpr uint16_t kmask2 = 0x0f0f; + constexpr uint16_t kmask3 = 0xc0c0; + + const short ix = tiisg/8; // 0...3 + const short it = tiisg%8; // 0...7 + const short iq = it/4; // 0 or 1 + const short ir = it%4; // 0...3 + + const int nb = args.ne00/QK_K; + + const int r0 = tgpig.x; + const int r1 = tgpig.y; + const int im = tgpig.z; + + const int first_row = (r0 * NSG + sgitg) * nr0; + + const uint i12 = im%args.ne12; + const uint i13 = im/args.ne12; + + const uint64_t offset0 = first_row*args.nb01 + (i12/args.r2)*args.nb02 + (i13/args.r3)*args.nb03; + const uint64_t offset1 = r1*args.nb11 + (i12 )*args.nb12 + (i13 )*args.nb13; + + device const block_q4_K * x = (device const block_q4_K *) (src0 + offset0); + device const float * y = (device const float *) (src1 + offset1); + + float yl[16]; + float yh[16]; + + float sumf[nr0]={0.f}; + + device const float * y4 = y + ix * QK_K + 64 * iq + 8 * ir; + + uint16_t sc16[4]; + thread const uint8_t * sc8 = (thread const uint8_t *)sc16; + + for (int ib = ix; ib < nb; ib += 4) { + float4 sumy = {0.f, 0.f, 0.f, 0.f}; + + for (short i = 0; i < 8; ++i) { + yl[i+0] = y4[i+ 0]; sumy[0] += yl[i+0]; + yl[i+8] = y4[i+ 32]; sumy[1] += yl[i+8]; + yh[i+0] = y4[i+128]; sumy[2] += yh[i+0]; + yh[i+8] = y4[i+160]; sumy[3] += yh[i+8]; + } + + device const uint16_t * sc = (device const uint16_t *)x[ib].scales + iq; + device const uint16_t * q1 = (device const uint16_t *)x[ib].qs + 16 * iq + 4 * ir; + device const half * dh = &x[ib].d; + + for (short row = 0; row < nr0; row++) { + sc16[0] = sc[0] & kmask1; + sc16[1] = sc[2] & kmask1; + sc16[2] = ((sc[4] >> 0) & kmask2) | ((sc[0] & kmask3) >> 2); + sc16[3] = ((sc[4] >> 4) & kmask2) | ((sc[2] & kmask3) >> 2); + + device const uint16_t * q2 = q1 + 32; + + float4 acc1 = {0.f, 0.f, 0.f, 0.f}; + float4 acc2 = {0.f, 0.f, 0.f, 0.f}; + + FOR_UNROLL (short i = 0; i < 4; ++i) { + acc1[0] += yl[2*i + 0] * (q1[i] & 0x000F); + acc1[1] += yl[2*i + 1] * (q1[i] & 0x0F00); + acc1[2] += yl[2*i + 8] * (q1[i] & 0x00F0); + acc1[3] += yl[2*i + 9] * (q1[i] & 0xF000); + acc2[0] += yh[2*i + 0] * (q2[i] & 0x000F); + acc2[1] += yh[2*i + 1] * (q2[i] & 0x0F00); + acc2[2] += yh[2*i + 8] * (q2[i] & 0x00F0); + acc2[3] += yh[2*i + 9] * (q2[i] & 0xF000); + } + + sumf[row] += dh[0] * ((acc1[0] + 1.f/256.f * acc1[1]) * sc8[0] + + (acc1[2] + 1.f/256.f * acc1[3]) * sc8[1] * 1.f/16.f + + (acc2[0] + 1.f/256.f * acc2[1]) * sc8[4] + + (acc2[2] + 1.f/256.f * acc2[3]) * sc8[5] * 1.f/16.f) - + dh[1] * (sumy[0] * sc8[2] + sumy[1] * sc8[3] + sumy[2] * sc8[6] + sumy[3] * sc8[7]); + + q1 += args.nb01/2; + sc += args.nb01/2; + dh += args.nb01/2; + } + + y4 += 4 * QK_K; + } + + device float * dst_f32 = (device float *) dst + (int64_t)im*args.ne0*args.ne1 + (int64_t)r1*args.ne0; + + for (int row = 0; row < nr0 && first_row + row < args.ne0; ++row) { + float sum_all = simd_sum(sumf[row]); + if (tiisg == 0) { + dst_f32[first_row + row] = sum_all; + } + } +} + +[[host_name("kernel_mul_mv_q4_K_f32")]] +kernel void kernel_mul_mv_q4_K_f32( + constant ggml_metal_kargs_mul_mv & args, + device const char * src0, + device const char * src1, + device char * dst, + uint3 tgpig[[threadgroup_position_in_grid]], + ushort tiisg[[thread_index_in_simdgroup]], + ushort sgitg[[simdgroup_index_in_threadgroup]]) { + + kernel_mul_mv_q4_K_f32_impl(args, src0, src1, dst, nullptr, tgpig, tiisg, sgitg); +} + +template +void kernel_mul_mv_q5_K_f32_impl( + args_t args, + device const char * src0, + device const char * src1, + device char * dst, + threadgroup char * shmem, + uint3 tgpig, + ushort tiisg, + ushort sgitg) { + const short NSG = FC_mul_mv_nsg; + + const int nb = args.ne00/QK_K; + + const int r0 = tgpig.x; + const int r1 = tgpig.y; + const int im = tgpig.z; + + const int first_row = (r0 * NSG + sgitg) * nr0; + + const uint i12 = im%args.ne12; + const uint i13 = im/args.ne12; + + const uint64_t offset0 = first_row*args.nb01 + (i12/args.r2)*args.nb02 + (i13/args.r3)*args.nb03; + const uint64_t offset1 = r1*args.nb11 + (i12 )*args.nb12 + (i13 )*args.nb13; + + device const block_q5_K * x = (device const block_q5_K *) (src0 + offset0); + device const float * yy = (device const float *) (src1 + offset1); + + float sumf[nr0]={0.f}; + + float yl[16], yh[16]; + + constexpr uint16_t kmask1 = 0x3f3f; + constexpr uint16_t kmask2 = 0x0f0f; + constexpr uint16_t kmask3 = 0xc0c0; + + const short tid = tiisg/4; + const short ix = tiisg%4; + const short iq = tid/4; + const short ir = tid%4; + + const short l0 = 8*ir; + const short q_offset = 32*iq + l0; + const short y_offset = 64*iq + l0; + + const uint8_t hm1 = 1u << (2*iq); + const uint8_t hm2 = hm1 << 1; + const uint8_t hm3 = hm1 << 4; + const uint8_t hm4 = hm2 << 4; + + uint16_t sc16[4]; + thread const uint8_t * sc8 = (thread const uint8_t *)sc16; + + device const float * y1 = yy + ix*QK_K + y_offset; + + for (int i = ix; i < nb; i += 4) { + device const uint8_t * q1 = x[i].qs + q_offset; + device const uint8_t * qh = x[i].qh + l0; + device const half * dh = &x[i].d; + device const uint16_t * a = (device const uint16_t *)x[i].scales + iq; + + device const float * y2 = y1 + 128; + float4 sumy = {0.f, 0.f, 0.f, 0.f}; + for (short l = 0; l < 8; ++l) { + yl[l+0] = y1[l+ 0]; sumy[0] += yl[l+0]; + yl[l+8] = y1[l+32]; sumy[1] += yl[l+8]; + yh[l+0] = y2[l+ 0]; sumy[2] += yh[l+0]; + yh[l+8] = y2[l+32]; sumy[3] += yh[l+8]; + } + + for (short row = 0; row < nr0; ++row) { + device const uint8_t * q2 = q1 + 64; + + sc16[0] = a[0] & kmask1; + sc16[1] = a[2] & kmask1; + sc16[2] = ((a[4] >> 0) & kmask2) | ((a[0] & kmask3) >> 2); + sc16[3] = ((a[4] >> 4) & kmask2) | ((a[2] & kmask3) >> 2); + + float4 acc1 = {0.f}; + float4 acc2 = {0.f}; + FOR_UNROLL (short l = 0; l < 8; ++l) { + uint8_t h = qh[l]; + acc1[0] += yl[l+0] * (q1[l] & 0x0F); + acc1[1] += yl[l+8] * (q1[l] & 0xF0); + acc1[2] += yh[l+0] * (q2[l] & 0x0F); + acc1[3] += yh[l+8] * (q2[l] & 0xF0); + acc2[0] += h & hm1 ? yl[l+0] : 0.f; + acc2[1] += h & hm2 ? yl[l+8] : 0.f; + acc2[2] += h & hm3 ? yh[l+0] : 0.f; + acc2[3] += h & hm4 ? yh[l+8] : 0.f; + } + + sumf[row] += dh[0] * (sc8[0] * (acc1[0] + 16.f*acc2[0]) + + sc8[1] * (acc1[1]/16.f + 16.f*acc2[1]) + + sc8[4] * (acc1[2] + 16.f*acc2[2]) + + sc8[5] * (acc1[3]/16.f + 16.f*acc2[3])) - + dh[1] * (sumy[0] * sc8[2] + sumy[1] * sc8[3] + sumy[2] * sc8[6] + sumy[3] * sc8[7]); + + q1 += args.nb01; + qh += args.nb01; + dh += args.nb01/2; + a += args.nb01/2; + } + + y1 += 4 * QK_K; + } + + device float * dst_f32 = (device float *) dst + (uint64_t)im*args.ne0*args.ne1 + (uint64_t)r1*args.ne0; + + for (int row = 0; row < nr0 && first_row + row < args.ne0; ++row) { + const float tot = simd_sum(sumf[row]); + if (tiisg == 0) { + dst_f32[first_row + row] = tot; + } + } +} + +[[host_name("kernel_mul_mv_q5_K_f32")]] +kernel void kernel_mul_mv_q5_K_f32( + constant ggml_metal_kargs_mul_mv & args, + device const char * src0, + device const char * src1, + device char * dst, + uint3 tgpig[[threadgroup_position_in_grid]], + ushort tiisg[[thread_index_in_simdgroup]], + ushort sgitg[[simdgroup_index_in_threadgroup]]) { + + kernel_mul_mv_q5_K_f32_impl(args, src0, src1, dst, nullptr, tgpig, tiisg, sgitg); +} + +template +void kernel_mul_mv_q6_K_f32_impl( + args_t args, + device const char * src0, + device const char * src1, + device char * dst, + threadgroup char * shmem, + uint3 tgpig, + ushort tiisg, + ushort sgitg) { + const short NSG = FC_mul_mv_nsg; + + constexpr uint8_t kmask1 = 0x03; + constexpr uint8_t kmask2 = 0x0C; + constexpr uint8_t kmask3 = 0x30; + constexpr uint8_t kmask4 = 0xC0; + + const int nb = args.ne00/QK_K; + + const int r0 = tgpig.x; + const int r1 = tgpig.y; + const int im = tgpig.z; + + const int first_row = (r0 * NSG + sgitg) * nr0; + + const uint i12 = im%args.ne12; + const uint i13 = im/args.ne12; + + const uint64_t offset0 = first_row*args.nb01 + (i12/args.r2)*args.nb02 + (i13/args.r3)*args.nb03; + const uint64_t offset1 = r1*args.nb11 + (i12 )*args.nb12 + (i13 )*args.nb13; + + device const block_q6_K * x = (device const block_q6_K *) (src0 + offset0); + device const float * yy = (device const float *) (src1 + offset1); + + float sumf[nr0] = { 0.f }; + + float yl[16]; + + const short tid = tiisg/2; + const short ix = tiisg%2; + const short ip = tid/8; // 0 or 1 + const short il = tid%8; + const short l0 = 4*il; + const short is = 8*ip + l0/16; + + const short y_offset = 128*ip + l0; + const short q_offset_l = 64*ip + l0; + const short q_offset_h = 32*ip + l0; + + for (int i = ix; i < nb; i += 2) { + device const uint8_t * q1 = x[i].ql + q_offset_l; + device const uint8_t * q2 = q1 + 32; + device const uint8_t * qh = x[i].qh + q_offset_h; + device const int8_t * sc = x[i].scales + is; + device const half * dh = &x[i].d; + + device const float * y = yy + i * QK_K + y_offset; + + for (short l = 0; l < 4; ++l) { + yl[4*l + 0] = y[l + 0]; + yl[4*l + 1] = y[l + 32]; + yl[4*l + 2] = y[l + 64]; + yl[4*l + 3] = y[l + 96]; + } + + for (short row = 0; row < nr0; ++row) { + float4 sums = {0.f, 0.f, 0.f, 0.f}; + + FOR_UNROLL (short l = 0; l < 4; ++l) { + sums[0] += yl[4*l + 0] * ((int8_t)((q1[l] & 0xF) | ((qh[l] & kmask1) << 4)) - 32); + sums[1] += yl[4*l + 1] * ((int8_t)((q2[l] & 0xF) | ((qh[l] & kmask2) << 2)) - 32); + sums[2] += yl[4*l + 2] * ((int8_t)((q1[l] >> 4) | ((qh[l] & kmask3) << 0)) - 32); + sums[3] += yl[4*l + 3] * ((int8_t)((q2[l] >> 4) | ((qh[l] & kmask4) >> 2)) - 32); + } + + sumf[row] += dh[0] * (sums[0] * sc[0] + sums[1] * sc[2] + sums[2] * sc[4] + sums[3] * sc[6]); + + q1 += args.nb01; + q2 += args.nb01; + qh += args.nb01; + sc += args.nb01; + dh += args.nb01/2; + } + } + + device float * dst_f32 = (device float *) dst + (uint64_t)im*args.ne0*args.ne1 + (uint64_t)r1*args.ne0; + + for (int row = 0; row < nr0 && first_row + row < args.ne0; ++row) { + float sum_all = simd_sum(sumf[row]); + if (tiisg == 0) { + dst_f32[first_row + row] = sum_all; + } + } +} + +[[host_name("kernel_mul_mv_q6_K_f32")]] +kernel void kernel_mul_mv_q6_K_f32( + constant ggml_metal_kargs_mul_mv & args, + device const char * src0, + device const char * src1, + device char * dst, + uint3 tgpig[[threadgroup_position_in_grid]], + ushort tiisg[[thread_index_in_simdgroup]], + ushort sgitg[[simdgroup_index_in_threadgroup]]) { + + kernel_mul_mv_q6_K_f32_impl(args, src0, src1, dst, nullptr, tgpig, tiisg, sgitg); +} + +// ======================= "True" 2-bit + +template +void kernel_mul_mv_iq2_xxs_f32_impl( + args_t args, + device const char * src0, + device const char * src1, + device char * dst, + threadgroup char * shmem, + uint3 tgpig, + ushort tiisg, + ushort sgitg) { + const short NSG = FC_mul_mv_nsg; + + const int nb = args.ne00/QK_K; + + const int r0 = tgpig.x; + const int r1 = tgpig.y; + const int im = tgpig.z; + + const int first_row = (r0 * NSG + sgitg) * nr0; + + const uint i12 = im%args.ne12; + const uint i13 = im/args.ne12; + + const uint64_t offset0 = first_row*args.nb01 + (i12/args.r2)*args.nb02 + (i13/args.r3)*args.nb03; + const uint64_t offset1 = r1*args.nb11 + (i12 )*args.nb12 + (i13 )*args.nb13; + + device const block_iq2_xxs * x = (device const block_iq2_xxs *) (src0 + offset0); + device const float * y = (device const float *) (src1 + offset1); + + float yl[32]; + float sumf[nr0]={0.f}; + + const int nb32 = nb * (QK_K / 32); + + threadgroup uint64_t * svalues = (threadgroup uint64_t *)(shmem); + threadgroup uint8_t * ssigns = (threadgroup uint8_t *)(svalues + 256); + { + int nval = 4; + int pos = (32*sgitg + tiisg)*nval; + for (int i = 0; i < nval; ++i) svalues[pos + i] = iq2xxs_grid[pos + i]; + nval = 2; + pos = (32*sgitg + tiisg)*nval; + for (int i = 0; i < nval; ++i) ssigns[pos+i] = ksigns_iq2xs[pos+i]; + threadgroup_barrier(mem_flags::mem_threadgroup); + } + + const int ix = tiisg; + + device const float * y4 = y + 32 * ix; + + for (int ib32 = ix; ib32 < nb32; ib32 += 32) { + for (short i = 0; i < 32; ++i) { + yl[i] = y4[i]; + } + + const int ibl = ib32 / (QK_K / 32); + const int ib = ib32 % (QK_K / 32); + + device const block_iq2_xxs * xr = x + ibl; + device const uint16_t * q2 = xr->qs + 4 * ib; + device const half * dh = &xr->d; + + for (short row = 0; row < nr0; row++) { + const float db = dh[0]; + device const uint8_t * aux8 = (device const uint8_t *)q2; + const uint32_t aux32 = q2[2] | (q2[3] << 16); + const float d = db * (0.5f + (aux32 >> 28)); + + float sum = 0; + for (short l = 0; l < 4; ++l) { + const threadgroup uint8_t * grid = (const threadgroup uint8_t *)(svalues + aux8[l]); + const uint8_t signs = ssigns[(aux32 >> 7*l) & 127]; + for (short j = 0; j < 8; ++j) { + sum += yl[8*l + j] * grid[j] * (signs & kmask_iq2xs[j] ? -1.f : 1.f); + } + } + sumf[row] += d * sum; + + dh += args.nb01/2; + q2 += args.nb01/2; + } + + y4 += 32 * 32; + } + + device float * dst_f32 = (device float *) dst + (uint64_t)im*args.ne0*args.ne1 + (uint64_t)r1*args.ne0; + + for (int row = 0; row < nr0 && first_row + row < args.ne0; ++row) { + float sum_all = simd_sum(sumf[row]); + if (tiisg == 0) { + dst_f32[first_row + row] = sum_all * 0.25f; + } + } +} + +[[host_name("kernel_mul_mv_iq2_xxs_f32")]] +kernel void kernel_mul_mv_iq2_xxs_f32( + constant ggml_metal_kargs_mul_mv & args, + device const char * src0, + device const char * src1, + device char * dst, + threadgroup char * shmem [[threadgroup(0)]], + uint3 tgpig[[threadgroup_position_in_grid]], + ushort tiisg[[thread_index_in_simdgroup]], + ushort sgitg[[simdgroup_index_in_threadgroup]]) { + kernel_mul_mv_iq2_xxs_f32_impl(args, src0, src1, dst, shmem, tgpig, tiisg, sgitg); +} + +template +void kernel_mul_mv_iq2_xs_f32_impl( + args_t args, + device const char * src0, + device const char * src1, + device char * dst, + threadgroup char * shmem, + uint3 tgpig, + ushort tiisg, + ushort sgitg) { + const short NSG = FC_mul_mv_nsg; + + const int nb = args.ne00/QK_K; + + const int r0 = tgpig.x; + const int r1 = tgpig.y; + const int im = tgpig.z; + + const int first_row = (r0 * NSG + sgitg) * nr0; + + const uint i12 = im%args.ne12; + const uint i13 = im/args.ne12; + + const uint64_t offset0 = first_row*args.nb01 + (i12/args.r2)*args.nb02 + (i13/args.r3)*args.nb03; + const uint64_t offset1 = r1*args.nb11 + (i12 )*args.nb12 + (i13 )*args.nb13; + + device const block_iq2_xs * x = (device const block_iq2_xs *) (src0 + offset0); + device const float * y = (device const float *) (src1 + offset1); + + float yl[32]; + float sumf[nr0]={0.f}; + + const int nb32 = nb * (QK_K / 32); + + threadgroup uint64_t * svalues = (threadgroup uint64_t *)(shmem); + threadgroup uint8_t * ssigns = (threadgroup uint8_t *)(svalues + 512); + { + int nval = 8; + int pos = (32*sgitg + tiisg)*nval; + for (int i = 0; i < nval; ++i) svalues[pos + i] = iq2xs_grid[pos + i]; + nval = 2; + pos = (32*sgitg + tiisg)*nval; + for (int i = 0; i < nval; ++i) ssigns[pos+i] = ksigns_iq2xs[pos+i]; + threadgroup_barrier(mem_flags::mem_threadgroup); + } + + const int ix = tiisg; + + device const float * y4 = y + 32 * ix; + + for (int ib32 = ix; ib32 < nb32; ib32 += 32) { + for (short i = 0; i < 32; ++i) { + yl[i] = y4[i]; + } + + const int ibl = ib32 / (QK_K / 32); + const int ib = ib32 % (QK_K / 32); + + device const block_iq2_xs * xr = x + ibl; + device const uint16_t * q2 = xr->qs + 4 * ib; + device const uint8_t * sc = xr->scales + ib; + device const half * dh = &xr->d; + + for (short row = 0; row < nr0; row++) { + const float db = dh[0]; + const uint8_t ls1 = sc[0] & 0xf; + const uint8_t ls2 = sc[0] >> 4; + const float d1 = db * (0.5f + ls1); + const float d2 = db * (0.5f + ls2); + + float sum1 = 0, sum2 = 0; + for (short l = 0; l < 2; ++l) { + const threadgroup uint8_t * grid = (const threadgroup uint8_t *)(svalues + (q2[l] & 511)); + const uint8_t signs = ssigns[(q2[l] >> 9)]; + for (short j = 0; j < 8; ++j) { + sum1 += yl[8*l + j] * grid[j] * (signs & kmask_iq2xs[j] ? -1.f : 1.f); + } + } + for (short l = 2; l < 4; ++l) { + const threadgroup uint8_t * grid = (const threadgroup uint8_t *)(svalues + (q2[l] & 511)); + const uint8_t signs = ssigns[(q2[l] >> 9)]; + for (short j = 0; j < 8; ++j) { + sum2 += yl[8*l + j] * grid[j] * (signs & kmask_iq2xs[j] ? -1.f : 1.f); + } + } + sumf[row] += d1 * sum1 + d2 * sum2; + + dh += args.nb01/2; + q2 += args.nb01/2; + sc += args.nb01; + } + + y4 += 32 * 32; + } + + device float * dst_f32 = (device float *) dst + (uint64_t)im*args.ne0*args.ne1 + (uint64_t)r1*args.ne0; + + for (int row = 0; row < nr0 && first_row + row < args.ne0; ++row) { + float sum_all = simd_sum(sumf[row]); + if (tiisg == 0) { + dst_f32[first_row + row] = sum_all * 0.25f; + } + } +} + +[[host_name("kernel_mul_mv_iq2_xs_f32")]] +kernel void kernel_mul_mv_iq2_xs_f32( + constant ggml_metal_kargs_mul_mv & args, + device const char * src0, + device const char * src1, + device char * dst, + threadgroup char * shmem [[threadgroup(0)]], + uint3 tgpig[[threadgroup_position_in_grid]], + ushort tiisg[[thread_index_in_simdgroup]], + ushort sgitg[[simdgroup_index_in_threadgroup]]) { + + kernel_mul_mv_iq2_xs_f32_impl(args, src0, src1, dst, shmem, tgpig, tiisg, sgitg); +} + +template +void kernel_mul_mv_iq3_xxs_f32_impl( + args_t args, + device const char * src0, + device const char * src1, + device char * dst, + threadgroup char * shmem, + uint3 tgpig, + ushort tiisg, + ushort sgitg) { + const short NSG = FC_mul_mv_nsg; + + const int nb = args.ne00/QK_K; + + const int r0 = tgpig.x; + const int r1 = tgpig.y; + const int im = tgpig.z; + + const int first_row = (r0 * NSG + sgitg) * nr0; + + const uint i12 = im%args.ne12; + const uint i13 = im/args.ne12; + + const uint64_t offset0 = first_row*args.nb01 + (i12/args.r2)*args.nb02 + (i13/args.r3)*args.nb03; + const uint64_t offset1 = r1*args.nb11 + (i12 )*args.nb12 + (i13 )*args.nb13; + + device const block_iq3_xxs * x = (device const block_iq3_xxs *) (src0 + offset0); + device const float * y = (device const float *) (src1 + offset1); + + float yl[32]; + float sumf[nr0]={0.f}; + + const int nb32 = nb * (QK_K / 32); + + threadgroup uint32_t * svalues = (threadgroup uint32_t *)(shmem); + threadgroup uint8_t * ssigns = (threadgroup uint8_t *)(svalues + 256); + { + int nval = 4; + int pos = (32*sgitg + tiisg)*nval; + for (int i = 0; i < nval; ++i) svalues[pos + i] = iq3xxs_grid[pos + i]; + nval = 2; + pos = (32*sgitg + tiisg)*nval; + for (int i = 0; i < nval; ++i) ssigns[pos+i] = ksigns_iq2xs[pos+i]; + threadgroup_barrier(mem_flags::mem_threadgroup); + } + + const int ix = tiisg; + + device const float * y4 = y + 32 * ix; + + for (int ib32 = ix; ib32 < nb32; ib32 += 32) { + for (short i = 0; i < 32; ++i) { + yl[i] = y4[i]; + } + + const int ibl = ib32 / (QK_K / 32); + const int ib = ib32 % (QK_K / 32); + + device const block_iq3_xxs * xr = x + ibl; + device const uint8_t * q3 = xr->qs + 8 * ib; + device const uint16_t * gas = (device const uint16_t *)(xr->qs + QK_K/4) + 2 * ib; + device const half * dh = &xr->d; + + for (short row = 0; row < nr0; row++) { + const float db = dh[0]; + const uint32_t aux32 = gas[0] | (gas[1] << 16); + const float d = db * (0.5f + (aux32 >> 28)); + + float2 sum = {0}; + for (short l = 0; l < 4; ++l) { + const threadgroup uint8_t * grid1 = (const threadgroup uint8_t *)(svalues + q3[2*l+0]); + const threadgroup uint8_t * grid2 = (const threadgroup uint8_t *)(svalues + q3[2*l+1]); + const uint8_t signs = ssigns[(aux32 >> 7*l) & 127]; + for (short j = 0; j < 4; ++j) { + sum[0] += yl[8*l + j + 0] * grid1[j] * (signs & kmask_iq2xs[j+0] ? -1.f : 1.f); + sum[1] += yl[8*l + j + 4] * grid2[j] * (signs & kmask_iq2xs[j+4] ? -1.f : 1.f); + } + } + sumf[row] += d * (sum[0] + sum[1]); + + dh += args.nb01/2; + q3 += args.nb01; + gas += args.nb01/2; + } + + y4 += 32 * 32; + } + + device float * dst_f32 = (device float *) dst + (uint64_t)im*args.ne0*args.ne1 + (uint64_t)r1*args.ne0; + + for (int row = 0; row < nr0 && first_row + row < args.ne0; ++row) { + float sum_all = simd_sum(sumf[row]); + if (tiisg == 0) { + dst_f32[first_row + row] = sum_all * 0.5f; + } + } +} + +[[host_name("kernel_mul_mv_iq3_xxs_f32")]] +kernel void kernel_mul_mv_iq3_xxs_f32( + constant ggml_metal_kargs_mul_mv & args, + device const char * src0, + device const char * src1, + device char * dst, + threadgroup char * shmem [[threadgroup(0)]], + uint3 tgpig[[threadgroup_position_in_grid]], + ushort tiisg[[thread_index_in_simdgroup]], + ushort sgitg[[simdgroup_index_in_threadgroup]]) { + + kernel_mul_mv_iq3_xxs_f32_impl(args, src0, src1, dst, shmem, tgpig, tiisg, sgitg); +} + +template +void kernel_mul_mv_iq3_s_f32_impl( + args_t args, + device const char * src0, + device const char * src1, + device char * dst, + threadgroup char * shmem, + uint3 tgpig, + ushort tiisg, + ushort sgitg) { + const short NSG = FC_mul_mv_nsg; + + const int nb = args.ne00/QK_K; + + const int r0 = tgpig.x; + const int r1 = tgpig.y; + const int im = tgpig.z; + + const int first_row = (r0 * NSG + sgitg) * nr0; + + const uint i12 = im%args.ne12; + const uint i13 = im/args.ne12; + + const uint64_t offset0 = first_row*args.nb01 + (i12/args.r2)*args.nb02 + (i13/args.r3)*args.nb03; + const uint64_t offset1 = r1*args.nb11 + (i12 )*args.nb12 + (i13 )*args.nb13; + + device const block_iq3_s * x = (device const block_iq3_s *) (src0 + offset0); + device const float * y = (device const float *) (src1 + offset1); + + float yl[32]; + float sumf[nr0]={0.f}; + + const int nb32 = nb * (QK_K / 32); + + threadgroup uint32_t * svalues = (threadgroup uint32_t *) shmem; + { + int nval = 8; + int pos = (32*sgitg + tiisg)*nval; + for (int i = 0; i < nval; ++i) svalues[pos + i] = iq3s_grid[pos + i]; + threadgroup_barrier(mem_flags::mem_threadgroup); + } + + const int ix = tiisg; + + device const float * y4 = y + 32 * ix; + + for (int ib32 = ix; ib32 < nb32; ib32 += 32) { + for (short i = 0; i < 32; ++i) { + yl[i] = y4[i]; + } + + const int ibl = ib32 / (QK_K / 32); + const int ib = ib32 % (QK_K / 32); + + device const block_iq3_s * xr = x + ibl; + device const uint8_t * qs = xr->qs + 8 * ib; + device const uint8_t * qh = xr->qh + ib; + device const uint8_t * sc = xr->scales + (ib/2); + device const uint8_t * signs = xr->signs + 4 * ib; + device const half * dh = &xr->d; + + for (short row = 0; row < nr0; row++) { + const float db = dh[0]; + const float d = db * (1 + 2*((sc[0] >> 4*(ib%2)) & 0xf)); + + float2 sum = {0}; + for (short l = 0; l < 4; ++l) { + const threadgroup uint32_t * table1 = qh[0] & kmask_iq2xs[2*l+0] ? svalues + 256 : svalues; + const threadgroup uint32_t * table2 = qh[0] & kmask_iq2xs[2*l+1] ? svalues + 256 : svalues; + const threadgroup uint8_t * grid1 = (const threadgroup uint8_t *)(table1 + qs[2*l+0]); + const threadgroup uint8_t * grid2 = (const threadgroup uint8_t *)(table2 + qs[2*l+1]); + for (short j = 0; j < 4; ++j) { + sum[0] += yl[8*l + j + 0] * grid1[j] * select(1, -1, signs[l] & kmask_iq2xs[j+0]); + sum[1] += yl[8*l + j + 4] * grid2[j] * select(1, -1, signs[l] & kmask_iq2xs[j+4]); + } + } + sumf[row] += d * (sum[0] + sum[1]); + + dh += args.nb01/2; + qs += args.nb01; + qh += args.nb01; + sc += args.nb01; + signs += args.nb01; + } + + y4 += 32 * 32; + } + + device float * dst_f32 = (device float *) dst + (uint64_t)im*args.ne0*args.ne1 + (uint64_t)r1*args.ne0; + + for (int row = 0; row < nr0 && first_row + row < args.ne0; ++row) { + float sum_all = simd_sum(sumf[row]); + if (tiisg == 0) { + dst_f32[first_row + row] = sum_all; + } + } +} + +[[host_name("kernel_mul_mv_iq3_s_f32")]] +kernel void kernel_mul_mv_iq3_s_f32( + constant ggml_metal_kargs_mul_mv & args, + device const char * src0, + device const char * src1, + device char * dst, + threadgroup char * shmem [[threadgroup(0)]], + uint3 tgpig[[threadgroup_position_in_grid]], + ushort tiisg[[thread_index_in_simdgroup]], + ushort sgitg[[simdgroup_index_in_threadgroup]]) { + + kernel_mul_mv_iq3_s_f32_impl(args, src0, src1, dst, shmem, tgpig, tiisg, sgitg); +} + +template +void kernel_mul_mv_iq2_s_f32_impl( + args_t args, + device const char * src0, + device const char * src1, + device char * dst, + threadgroup char * shmem, + uint3 tgpig, + ushort tiisg, + ushort sgitg) { + const short NSG = FC_mul_mv_nsg; + + const int nb = args.ne00/QK_K; + + const int r0 = tgpig.x; + const int r1 = tgpig.y; + const int im = tgpig.z; + + const int first_row = (r0 * NSG + sgitg) * nr0; + + const uint i12 = im%args.ne12; + const uint i13 = im/args.ne12; + + const uint64_t offset0 = first_row*args.nb01 + (i12/args.r2)*args.nb02 + (i13/args.r3)*args.nb03; + const uint64_t offset1 = r1*args.nb11 + (i12 )*args.nb12 + (i13 )*args.nb13; + + device const block_iq2_s * x = (device const block_iq2_s *) (src0 + offset0); + device const float * y = (device const float *) (src1 + offset1); + + float yl[32]; + float sumf[nr0]={0.f}; + + const int nb32 = nb * (QK_K / 32); + + //threadgroup uint64_t * svalues = (threadgroup uint64_t *) shmem; + //{ + // int nval = 32; + // int pos = (32*sgitg + tiisg)*nval; + // for (int i = 0; i < nval; ++i) svalues[pos + i] = iq2s_grid[pos + i]; + // threadgroup_barrier(mem_flags::mem_threadgroup); + //} + + const short ix = tiisg; + + device const float * y4 = y + 32 * ix; + + for (int ib32 = ix; ib32 < nb32; ib32 += 32) { + for (short i = 0; i < 32; ++i) { + yl[i] = y4[i]; + } + + const int ibl = ib32 / (QK_K / 32); + const int ib = ib32 % (QK_K / 32); + + device const block_iq2_s * xr = x + ibl; + device const uint8_t * qs = xr->qs + 4 * ib; + device const uint8_t * qh = xr->qh + ib; + device const uint8_t * sc = xr->scales + ib; + device const uint8_t * signs = qs + QK_K/8; + device const half * dh = &xr->d; + + for (short row = 0; row < nr0; row++) { + const float db = dh[0]; + const float d1 = db * (0.5f + (sc[0] & 0xf)); + const float d2 = db * (0.5f + (sc[0] >> 4)); + + float2 sum = {0}; + for (short l = 0; l < 2; ++l) { + //const threadgroup uint8_t * grid1 = (const threadgroup uint8_t *)(svalues + (qs[l+0] | ((qh[0] << (8-2*l)) & 0x300))); + //const threadgroup uint8_t * grid2 = (const threadgroup uint8_t *)(svalues + (qs[l+2] | ((qh[0] << (4-2*l)) & 0x300))); + constant uint8_t * grid1 = (constant uint8_t *)(iq2s_grid + (qs[l+0] | ((qh[0] << (8-2*l)) & 0x300))); + constant uint8_t * grid2 = (constant uint8_t *)(iq2s_grid + (qs[l+2] | ((qh[0] << (4-2*l)) & 0x300))); + for (short j = 0; j < 8; ++j) { + sum[0] += yl[8*l + j + 0] * grid1[j] * select(1, -1, signs[l+0] & kmask_iq2xs[j]); + sum[1] += yl[8*l + j + 16] * grid2[j] * select(1, -1, signs[l+2] & kmask_iq2xs[j]); + } + } + sumf[row] += d1 * sum[0] + d2 * sum[1]; + + dh += args.nb01/2; + qs += args.nb01; + qh += args.nb01; + sc += args.nb01; + signs += args.nb01; + } + + y4 += 32 * 32; + } + + device float * dst_f32 = (device float *) dst + (uint64_t)im*args.ne0*args.ne1 + (uint64_t)r1*args.ne0; + + for (int row = 0; row < nr0 && first_row + row < args.ne0; ++row) { + float sum_all = simd_sum(sumf[row]); + if (tiisg == 0) { + dst_f32[first_row + row] = sum_all * 0.25f; + } + } +} + +[[host_name("kernel_mul_mv_iq2_s_f32")]] +kernel void kernel_mul_mv_iq2_s_f32( + constant ggml_metal_kargs_mul_mv & args, + device const char * src0, + device const char * src1, + device char * dst, + threadgroup char * shmem [[threadgroup(0)]], + uint3 tgpig[[threadgroup_position_in_grid]], + ushort tiisg[[thread_index_in_simdgroup]], + ushort sgitg[[simdgroup_index_in_threadgroup]]) { + + kernel_mul_mv_iq2_s_f32_impl(args, src0, src1, dst, shmem, tgpig, tiisg, sgitg); +} + +template +void kernel_mul_mv_iq1_s_f32_impl( + args_t args, + device const char * src0, + device const char * src1, + device char * dst, + threadgroup char * shmem, + uint3 tgpig, + ushort tiisg, + ushort sgitg) { + const short NSG = FC_mul_mv_nsg; + + const int nb = args.ne00/QK_K; + + const int r0 = tgpig.x; + const int r1 = tgpig.y; + const int im = tgpig.z; + + const int first_row = (r0 * NSG + sgitg) * nr0; + + const uint i12 = im%args.ne12; + const uint i13 = im/args.ne12; + + const uint64_t offset0 = first_row*args.nb01 + (i12/args.r2)*args.nb02 + (i13/args.r3)*args.nb03; + const uint64_t offset1 = r1*args.nb11 + (i12 )*args.nb12 + (i13 )*args.nb13; + + device const block_iq1_s * x = (device const block_iq1_s *) (src0 + offset0); + device const float * y = (device const float *) (src1 + offset1); + + float yl[32]; + float sumf[nr0]={0.f}; + + const int nb32 = nb * (QK_K / 32); + + const short ix = tiisg; + + device const float * y4 = y + 32 * ix; + + for (int ib32 = ix; ib32 < nb32; ib32 += 32) { + float sumy = 0; + for (short i = 0; i < 32; ++i) { + yl[i] = y4[i]; + sumy += yl[i]; + } + + const int ibl = ib32 / (QK_K / 32); + const int ib = ib32 % (QK_K / 32); + + device const block_iq1_s * xr = x + ibl; + device const uint8_t * qs = xr->qs + 4 * ib; + device const uint16_t * qh = xr->qh + ib; + device const half * dh = &xr->d; + + for (short row = 0; row < nr0; row++) { + constant uint8_t * grid1 = (constant uint8_t *)(iq1s_grid_gpu + (qs[0] | ((qh[0] << 8) & 0x700))); + constant uint8_t * grid2 = (constant uint8_t *)(iq1s_grid_gpu + (qs[1] | ((qh[0] << 5) & 0x700))); + constant uint8_t * grid3 = (constant uint8_t *)(iq1s_grid_gpu + (qs[2] | ((qh[0] << 2) & 0x700))); + constant uint8_t * grid4 = (constant uint8_t *)(iq1s_grid_gpu + (qs[3] | ((qh[0] >> 1) & 0x700))); + + float sum = 0; + for (short j = 0; j < 4; ++j) { + sum += yl[j+ 0] * (grid1[j] & 0xf) + yl[j+ 4] * (grid1[j] >> 4) + + yl[j+ 8] * (grid2[j] & 0xf) + yl[j+12] * (grid2[j] >> 4) + + yl[j+16] * (grid3[j] & 0xf) + yl[j+20] * (grid3[j] >> 4) + + yl[j+24] * (grid4[j] & 0xf) + yl[j+28] * (grid4[j] >> 4); + } + sumf[row] += (float)dh[0] * (sum + sumy * (qh[0] & 0x8000 ? -1 - IQ1S_DELTA : -1 + IQ1S_DELTA)) * (2*((qh[0] >> 12) & 7) + 1); + + dh += args.nb01/2; + qs += args.nb01; + qh += args.nb01/2; + } + + y4 += 32 * 32; + } + + device float * dst_f32 = (device float *) dst + (uint64_t)im*args.ne0*args.ne1 + (uint64_t)r1*args.ne0; + + for (int row = 0; row < nr0 && first_row + row < args.ne0; ++row) { + float sum_all = simd_sum(sumf[row]); + if (tiisg == 0) { + dst_f32[first_row + row] = sum_all; + } + } +} + +[[host_name("kernel_mul_mv_iq1_s_f32")]] +kernel void kernel_mul_mv_iq1_s_f32( + constant ggml_metal_kargs_mul_mv & args, + device const char * src0, + device const char * src1, + device char * dst, + uint3 tgpig[[threadgroup_position_in_grid]], + ushort tiisg[[thread_index_in_simdgroup]], + ushort sgitg[[simdgroup_index_in_threadgroup]]) { + + kernel_mul_mv_iq1_s_f32_impl(args, src0, src1, dst, nullptr, tgpig, tiisg, sgitg); +} + +template +void kernel_mul_mv_iq1_m_f32_impl( + args_t args, + device const char * src0, + device const char * src1, + device char * dst, + threadgroup char * shmem, + uint3 tgpig, + ushort tiisg, + ushort sgitg) { + const short NSG = FC_mul_mv_nsg; + + const int nb = args.ne00/QK_K; + + const int r0 = tgpig.x; + const int r1 = tgpig.y; + const int im = tgpig.z; + + const int first_row = (r0 * NSG + sgitg) * nr0; + + const uint i12 = im%args.ne12; + const uint i13 = im/args.ne12; + + const uint64_t offset0 = first_row*args.nb01 + (i12/args.r2)*args.nb02 + (i13/args.r3)*args.nb03; + const uint64_t offset1 = r1*args.nb11 + (i12 )*args.nb12 + (i13 )*args.nb13; + + device const block_iq1_m * x = (device const block_iq1_m *) (src0 + offset0); + device const float * y = (device const float *) (src1 + offset1); + + float yl[32]; + float sumf[nr0]={0.f}; + + const int nb32 = nb * (QK_K / 32); + + const short ix = tiisg; + + device const float * y4 = y + 32 * ix; + + iq1m_scale_t scale; + + for (int ib32 = ix; ib32 < nb32; ib32 += 32) { + float4 sumy = {0.f}; + for (short i = 0; i < 8; ++i) { + yl[i+ 0] = y4[i+ 0]; sumy[0] += yl[i+ 0]; + yl[i+ 8] = y4[i+ 8]; sumy[1] += yl[i+ 8]; + yl[i+16] = y4[i+16]; sumy[2] += yl[i+16]; + yl[i+24] = y4[i+24]; sumy[3] += yl[i+24]; + } + + const int ibl = ib32 / (QK_K / 32); + const int ib = ib32 % (QK_K / 32); + + device const block_iq1_m * xr = x + ibl; + device const uint8_t * qs = xr->qs + 4 * ib; + device const uint8_t * qh = xr->qh + 2 * ib; + device const uint16_t * sc = (device const uint16_t *)xr->scales; + + for (short row = 0; row < nr0; row++) { + scale.u16 = (sc[0] >> 12) | ((sc[1] >> 8) & 0x00f0) | ((sc[2] >> 4) & 0x0f00) | (sc[3] & 0xf000); + + constant uint8_t * grid1 = (constant uint8_t *)(iq1s_grid_gpu + (qs[0] | ((qh[0] << 8) & 0x700))); + constant uint8_t * grid2 = (constant uint8_t *)(iq1s_grid_gpu + (qs[1] | ((qh[0] << 4) & 0x700))); + constant uint8_t * grid3 = (constant uint8_t *)(iq1s_grid_gpu + (qs[2] | ((qh[1] << 8) & 0x700))); + constant uint8_t * grid4 = (constant uint8_t *)(iq1s_grid_gpu + (qs[3] | ((qh[1] << 4) & 0x700))); + + float2 sum = {0.f}; + for (short j = 0; j < 4; ++j) { + sum[0] += yl[j+ 0] * (grid1[j] & 0xf) + yl[j+ 4] * (grid1[j] >> 4) + + yl[j+ 8] * (grid2[j] & 0xf) + yl[j+12] * (grid2[j] >> 4); + sum[1] += yl[j+16] * (grid3[j] & 0xf) + yl[j+20] * (grid3[j] >> 4) + + yl[j+24] * (grid4[j] & 0xf) + yl[j+28] * (grid4[j] >> 4); + } + const float delta1 = sumy[0] * (qh[0] & 0x08 ? -1 - IQ1M_DELTA : -1 + IQ1M_DELTA) + sumy[1] * (qh[0] & 0x80 ? -1 - IQ1M_DELTA : -1 + IQ1M_DELTA); + const float delta2 = sumy[2] * (qh[1] & 0x08 ? -1 - IQ1M_DELTA : -1 + IQ1M_DELTA) + sumy[3] * (qh[1] & 0x80 ? -1 - IQ1M_DELTA : -1 + IQ1M_DELTA); + + sumf[row] += (float)scale.f16 * ((sum[0] + delta1) * (2*((sc[ib/2] >> (6*(ib%2)+0)) & 7) + 1) + + (sum[1] + delta2) * (2*((sc[ib/2] >> (6*(ib%2)+3)) & 7) + 1)); + + sc += args.nb01/2; + qs += args.nb01; + qh += args.nb01; + } + + y4 += 32 * 32; + } + + device float * dst_f32 = (device float *) dst + (uint64_t)im*args.ne0*args.ne1 + (uint64_t)r1*args.ne0; + + for (int row = 0; row < nr0 && first_row + row < args.ne0; ++row) { + float sum_all = simd_sum(sumf[row]); + if (tiisg == 0) { + dst_f32[first_row + row] = sum_all; + } + } +} + +[[host_name("kernel_mul_mv_iq1_m_f32")]] +kernel void kernel_mul_mv_iq1_m_f32( + constant ggml_metal_kargs_mul_mv & args, + device const char * src0, + device const char * src1, + device char * dst, + uint3 tgpig[[threadgroup_position_in_grid]], + ushort tiisg[[thread_index_in_simdgroup]], + ushort sgitg[[simdgroup_index_in_threadgroup]]) { + + kernel_mul_mv_iq1_m_f32_impl(args, src0, src1, dst, nullptr, tgpig, tiisg, sgitg); +} + +template +void kernel_mul_mv_iq4_nl_f32_impl( + args_t args, + device const char * src0, + device const char * src1, + device char * dst, + threadgroup char * shmem, + uint3 tgpig, + ushort tiisg, + ushort sgitg) { + const short NSG = FC_mul_mv_nsg; + + threadgroup float * shmem_f32 = (threadgroup float *) shmem; + + const int r0 = tgpig.x; + const int r1 = tgpig.y; + const int im = tgpig.z; + + const int first_row = (r0 * NSG + sgitg) * NR0; + + const uint i12 = im%args.ne12; + const uint i13 = im/args.ne12; + + const uint64_t offset0 = first_row*args.nb01 + (i12/args.r2)*args.nb02 + (i13/args.r3)*args.nb03; + const uint64_t offset1 = r1*args.nb11 + (i12 )*args.nb12 + (i13 )*args.nb13; + + device const block_iq4_nl * x = (device const block_iq4_nl *) (src0 + offset0); + device const float * y = (device const float *) (src1 + offset1); + + const int nb = args.ne00/QK4_NL; + const int ns01 = args.nb01/args.nb00; + + const short ix = tiisg/2; // 0...15 + const short it = tiisg%2; // 0 or 1 + + shmem_f32[tiisg] = kvalues_iq4nl_f[tiisg%16]; + threadgroup_barrier(mem_flags::mem_threadgroup); + + float4 yl[4]; + float sumf[NR0]={0.f}; + + device const float * yb = y + ix*QK4_NL + it*8; + + uint32_t aux32[2]; + thread const uint8_t * q8 = (thread const uint8_t *)aux32; + + float4 qf1, qf2; + + // [TAG_MUL_MV_WEIRD] + for (int ib = ix; ib < nb && ib < ns01; ib += 16) { + device const float4 * y4 = (device const float4 *)yb; + yl[0] = y4[0]; + yl[1] = y4[4]; + yl[2] = y4[1]; + yl[3] = y4[5]; + + for (short row = 0; row < NR0; row++) { + device const block_iq4_nl & xb = x[row*ns01 + ib]; + device const uint16_t * q4 = (device const uint16_t *)(xb.qs + 8*it); + + float4 acc1 = {0.f}, acc2 = {0.f}; + + aux32[0] = q4[0] | (q4[1] << 16); + aux32[1] = (aux32[0] >> 4) & 0x0f0f0f0f; + aux32[0] &= 0x0f0f0f0f; + qf1 = {shmem_f32[q8[0]], shmem_f32[q8[1]], shmem_f32[q8[2]], shmem_f32[q8[3]]}; + qf2 = {shmem_f32[q8[4]], shmem_f32[q8[5]], shmem_f32[q8[6]], shmem_f32[q8[7]]}; + acc1 += yl[0] * qf1; + acc2 += yl[1] * qf2; + + aux32[0] = q4[2] | (q4[3] << 16); + aux32[1] = (aux32[0] >> 4) & 0x0f0f0f0f; + aux32[0] &= 0x0f0f0f0f; + qf1 = {shmem_f32[q8[0]], shmem_f32[q8[1]], shmem_f32[q8[2]], shmem_f32[q8[3]]}; + qf2 = {shmem_f32[q8[4]], shmem_f32[q8[5]], shmem_f32[q8[6]], shmem_f32[q8[7]]}; + acc1 += yl[2] * qf1; + acc2 += yl[3] * qf2; + + acc1 += acc2; + + sumf[row] += (float)xb.d * (acc1[0] + acc1[1] + acc1[2] + acc1[3]); + } + + yb += 16 * QK4_NL; + } + + device float * dst_f32 = (device float *) dst + (uint64_t)im*args.ne0*args.ne1 + (uint64_t)r1*args.ne0; + + for (int row = 0; row < NR0 && first_row + row < args.ne0; ++row) { + float sum_all = simd_sum(sumf[row]); + if (tiisg == 0) { + dst_f32[first_row + row] = sum_all; + } + } +} + +[[host_name("kernel_mul_mv_iq4_nl_f32")]] +kernel void kernel_mul_mv_iq4_nl_f32( + constant ggml_metal_kargs_mul_mv & args, + device const char * src0, + device const char * src1, + device char * dst, + threadgroup char * shmem [[threadgroup(0)]], + uint3 tgpig[[threadgroup_position_in_grid]], + ushort tiisg[[thread_index_in_simdgroup]], + ushort sgitg[[simdgroup_index_in_threadgroup]]) { + + kernel_mul_mv_iq4_nl_f32_impl(args, src0, src1, dst, shmem, tgpig, tiisg, sgitg); +} + +template +void kernel_mul_mv_iq4_xs_f32_impl( + args_t args, + device const char * src0, + device const char * src1, + device char * dst, + threadgroup char * shmem, + uint3 tgpig, + ushort tiisg, + ushort sgitg) { + const short NSG = FC_mul_mv_nsg; + + threadgroup float * shmem_f32 = (threadgroup float *) shmem; + + const int r0 = tgpig.x; + const int r1 = tgpig.y; + const int im = tgpig.z; + const int first_row = (r0 * NSG + sgitg) * NR0; + + const uint i12 = im%args.ne12; + const uint i13 = im/args.ne12; + + const uint64_t offset0 = first_row*args.nb01 + (i12/args.r2)*args.nb02 + (i13/args.r3)*args.nb03; + const uint64_t offset1 = r1*args.nb11 + (i12 )*args.nb12 + (i13 )*args.nb13; + + device const block_iq4_xs * x = (device const block_iq4_xs *) (src0 + offset0); + device const float * y = (device const float *) (src1 + offset1); + + const int nb = args.ne00/QK_K; + const int ns01 = args.nb01/args.nb00; + + const short ix = tiisg/16; // 0 or 1 + const short it = tiisg%16; // 0...15 + const short ib = it/2; + const short il = it%2; + + shmem_f32[tiisg] = kvalues_iq4nl_f[tiisg%16]; + threadgroup_barrier(mem_flags::mem_threadgroup); + + float4 yl[4]; + float sumf[NR0]={0.f}; + + device const float * yb = y + ix * QK_K + ib * 32 + il * 8; + + uint32_t aux32[2]; + thread const uint8_t * q8 = (thread const uint8_t *)aux32; + + float4 qf1, qf2; + + // [TAG_MUL_MV_WEIRD] + for (int ibl = ix; ibl < nb && ibl < ns01; ibl += 2) { + device const float4 * y4 = (device const float4 *)yb; + yl[0] = y4[0]; + yl[1] = y4[4]; + yl[2] = y4[1]; + yl[3] = y4[5]; + + for (short row = 0; row < NR0; ++row) { + device const block_iq4_xs & xb = x[row*ns01 + ibl]; + device const uint32_t * q4 = (device const uint32_t *)(xb.qs + 16*ib + 8*il); + + float4 acc1 = {0.f}, acc2 = {0.f}; + + aux32[0] = (q4[0] ) & 0x0f0f0f0f; + aux32[1] = (q4[0] >> 4) & 0x0f0f0f0f; + qf1 = {shmem_f32[q8[0]], shmem_f32[q8[1]], shmem_f32[q8[2]], shmem_f32[q8[3]]}; + qf2 = {shmem_f32[q8[4]], shmem_f32[q8[5]], shmem_f32[q8[6]], shmem_f32[q8[7]]}; + acc1 += yl[0] * qf1; + acc2 += yl[1] * qf2; + + aux32[0] = (q4[1] ) & 0x0f0f0f0f; + aux32[1] = (q4[1] >> 4) & 0x0f0f0f0f; + qf1 = {shmem_f32[q8[0]], shmem_f32[q8[1]], shmem_f32[q8[2]], shmem_f32[q8[3]]}; + qf2 = {shmem_f32[q8[4]], shmem_f32[q8[5]], shmem_f32[q8[6]], shmem_f32[q8[7]]}; + acc1 += yl[2] * qf1; + acc2 += yl[3] * qf2; + + acc1 += acc2; + + const int ls = (((xb.scales_l[ib/2] >> 4*(ib%2)) & 0xf) | (((xb.scales_h >> 2*ib) & 3) << 4)) - 32; + sumf[row] += (float)xb.d * ls * (acc1[0] + acc1[1] + acc1[2] + acc1[3]); + } + + yb += 2 * QK_K; + } + + device float * dst_f32 = (device float *) dst + (uint64_t)im*args.ne0*args.ne1 + (uint64_t)r1*args.ne0; + + for (int row = 0; row < NR0 && first_row + row < args.ne0; ++row) { + float sum_all = simd_sum(sumf[row]); + if (tiisg == 0) { + dst_f32[first_row + row] = sum_all; + } + } +} + +[[host_name("kernel_mul_mv_iq4_xs_f32")]] +kernel void kernel_mul_mv_iq4_xs_f32( + constant ggml_metal_kargs_mul_mv & args, + device const char * src0, + device const char * src1, + device char * dst, + threadgroup char * shmem [[threadgroup(0)]], + uint3 tgpig[[threadgroup_position_in_grid]], + ushort tiisg[[thread_index_in_simdgroup]], + ushort sgitg[[simdgroup_index_in_threadgroup]]) { + + kernel_mul_mv_iq4_xs_f32_impl(args, src0, src1, dst, shmem, tgpig, tiisg, sgitg); +} + +template +void kernel_mul_mv_mxfp4_f32_impl( + args_t args, + device const char * src0, + device const char * src1, + device char * dst, + threadgroup char * shmem, + uint3 tgpig, + ushort tiisg, + ushort sgitg) { + const short NSG = FC_mul_mv_nsg; + + threadgroup float * shmem_f32 = (threadgroup float *) shmem; + + const int r0 = tgpig.x; + const int r1 = tgpig.y; + const int im = tgpig.z; + + const int first_row = (r0 * NSG + sgitg) * NR0; + + const uint i12 = im%args.ne12; + const uint i13 = im/args.ne12; + + const uint64_t offset0 = first_row*args.nb01 + (i12/args.r2)*args.nb02 + (i13/args.r3)*args.nb03; + const uint64_t offset1 = r1*args.nb11 + (i12 )*args.nb12 + (i13 )*args.nb13; + + device const block_mxfp4 * x = (device const block_mxfp4 *) (src0 + offset0); + device const float * y = (device const float *) (src1 + offset1); + + const int nb = args.ne00/QK_MXFP4; + const int ns01 = args.nb01/args.nb00; // this can be larger than nb for permuted src0 tensors + + const short ix = tiisg/2; // 0...15 + const short it = tiisg%2; // 0 or 1 + + shmem_f32[tiisg] = kvalues_mxfp4_f[tiisg%16]; + threadgroup_barrier(mem_flags::mem_threadgroup); + + float4 yl[4]; + float sumf[NR0]={0.f}; + + device const float * yb = y + ix*QK_MXFP4 + it*8; + + // note: just the check `ib < nb` is enough, but adding the redundant `&& ib < ns01` check makes the kernel a bit faster + // no idea why that is - needs some deeper investigation [TAG_MUL_MV_WEIRD] + for (int ib = ix; ib < nb && ib < ns01; ib += 16) { + device const float4 * y4 = (device const float4 *) yb; + + yl[0] = y4[0]; + yl[1] = y4[4]; + yl[2] = y4[1]; + yl[3] = y4[5]; + + FOR_UNROLL (short row = 0; row < NR0; row++) { + device const block_mxfp4 & xb = x[row*ns01 + ib]; + device const uint8_t * q2 = (device const uint8_t *)(xb.qs + 8*it); + + float4 acc1 = yl[0]*float4(shmem_f32[q2[0] & 0x0F], shmem_f32[q2[1] & 0x0F], shmem_f32[q2[2] & 0x0F], shmem_f32[q2[3] & 0x0F]); + float4 acc2 = yl[1]*float4(shmem_f32[q2[0] >> 4 ], shmem_f32[q2[1] >> 4 ], shmem_f32[q2[2] >> 4 ], shmem_f32[q2[3] >> 4 ]); + float4 acc3 = yl[2]*float4(shmem_f32[q2[4] & 0x0F], shmem_f32[q2[5] & 0x0F], shmem_f32[q2[6] & 0x0F], shmem_f32[q2[7] & 0x0F]); + float4 acc4 = yl[3]*float4(shmem_f32[q2[4] >> 4 ], shmem_f32[q2[5] >> 4 ], shmem_f32[q2[6] >> 4 ], shmem_f32[q2[7] >> 4 ]); + + acc1 = (acc1 + acc3) + (acc2 + acc4); + + sumf[row] += e8m0_to_fp32(xb.e) * ((acc1[0] + acc1[1]) + (acc1[2] + acc1[3])); + } + + yb += 16 * QK_MXFP4; + } + + device float * dst_f32 = (device float *) dst + (uint64_t)im*args.ne0*args.ne1 + (uint64_t)r1*args.ne0; + + for (int row = 0; row < NR0 && first_row + row < args.ne0; ++row) { + float sum_all = simd_sum(sumf[row]); + if (tiisg == 0) { + dst_f32[first_row + row] = sum_all; + } + } +} + +[[host_name("kernel_mul_mv_mxfp4_f32")]] +kernel void kernel_mul_mv_mxfp4_f32( + constant ggml_metal_kargs_mul_mv & args, + device const char * src0, + device const char * src1, + device char * dst, + threadgroup char * shmem [[threadgroup(0)]], + uint3 tgpig[[threadgroup_position_in_grid]], + ushort tiisg[[thread_index_in_simdgroup]], + ushort sgitg[[simdgroup_index_in_threadgroup]]) { + + kernel_mul_mv_mxfp4_f32_impl(args, src0, src1, dst, shmem, tgpig, tiisg, sgitg); +} + +template +kernel void kernel_get_rows_q( + constant ggml_metal_kargs_get_rows & args, + device const void * src0, + device const void * src1, + device void * dst, + uint3 tgpig[[threadgroup_position_in_grid]], + ushort tiitg[[thread_index_in_threadgroup]], + ushort3 ntg [[threads_per_threadgroup]]) { + const int32_t iw0 = tgpig.x/args.ne10; + const int32_t i10 = tgpig.x%args.ne10; + const int32_t i11 = tgpig.y; + const int32_t i12 = tgpig.z; + + const int32_t r = ((const device int32_t *) ((const device char *) src1 + i12*args.nb12 + i11*args.nb11 + i10*args.nb10))[0]; + + const int32_t i02 = i11; + const int32_t i03 = i12; + + auto psrc = (device const block_q *) ((const device char *) src0 + i03*args.nb03 + i02*args.nb02 + r*args.nb01); + auto pdst = (device float4x4 *) (( device char *) dst + i12*args.nb3 + i11*args.nb2 + i10*args.nb1); + + for (int ind = iw0*ntg.x + tiitg; ind < args.ne00t;) { + float4x4 temp; + dequantize_func(psrc + ind/nl, ind%nl, temp); + pdst[ind] = temp; + + break; + } +} + +template +kernel void kernel_get_rows_f( + constant ggml_metal_kargs_get_rows & args, + device const void * src0, + device const void * src1, + device void * dst, + uint3 tgpig[[threadgroup_position_in_grid]], + ushort tiitg[[thread_index_in_threadgroup]], + ushort3 ntg [[threads_per_threadgroup]]) { + const int32_t iw0 = tgpig.x/args.ne10; + const int32_t i10 = tgpig.x%args.ne10; + const int32_t i11 = tgpig.y; + const int32_t i12 = tgpig.z; + + const int32_t r = ((const device int32_t *) ((const device char *) src1 + i12*args.nb12 + i11*args.nb11 + i10*args.nb10))[0]; + + const int32_t i02 = i11; + const int32_t i03 = i12; + + auto psrc = (const device T0 *) ((const device char *) src0 + i03*args.nb03 + i02*args.nb02 + r*args.nb01); + auto pdst = ( device T *) (( device char *) dst + i12*args.nb3 + i11*args.nb2 + i10*args.nb1); + + for (int ind = iw0*ntg.x + tiitg; ind < args.ne00t;) { + pdst[ind] = psrc[ind]; + + break; + } +} + +template +kernel void kernel_set_rows_q32( + constant ggml_metal_kargs_set_rows & args, + device const void * src0, + device const void * src1, + device float * dst, + uint3 tgpig[[threadgroup_position_in_grid]], + uint tiitg[[thread_index_in_threadgroup]], + uint3 tptg [[threads_per_threadgroup]]) { + const int32_t i03 = tgpig.z; + const int32_t i02 = tgpig.y; + + const int32_t i12 = i03%args.ne12; + const int32_t i11 = i02%args.ne11; + + const int32_t i01 = tgpig.x*tptg.y + tiitg/tptg.x; + if (i01 >= args.ne01) { + return; + } + + const int32_t i10 = i01; + const TI i1 = ((const device TI *) ((const device char *) src1 + i10*args.nb10 + i11*args.nb11 + i12*args.nb12))[0]; + + device block_q * dst_row = ( device block_q *) (( device char *) dst + i1*args.nb1 + i02*args.nb2 + i03*args.nb3); + const device float * src_row = (const device float *) ((const device char *) src0 + i01*args.nb01 + i02*args.nb02 + i03*args.nb03); + + for (int ind = tiitg%tptg.x; ind < args.nk0; ind += tptg.x) { + quantize_func(src_row + 32*ind, dst_row[ind]); + } +} + +template +kernel void kernel_set_rows_f( + constant ggml_metal_kargs_set_rows & args, + device const void * src0, + device const void * src1, + device float * dst, + uint3 tgpig[[threadgroup_position_in_grid]], + uint tiitg[[thread_index_in_threadgroup]], + uint3 tptg [[threads_per_threadgroup]]) { + const int32_t i03 = tgpig.z; + const int32_t i02 = tgpig.y; + + const int32_t i12 = i03%args.ne12; + const int32_t i11 = i02%args.ne11; + + const int32_t i01 = tgpig.x*tptg.y + tiitg/tptg.x; + if (i01 >= args.ne01) { + return; + } + + const int32_t i10 = i01; + const TI i1 = ((const device TI *) ((const device char *) src1 + i10*args.nb10 + i11*args.nb11 + i12*args.nb12))[0]; + + device T * dst_row = ( device T *) (( device char *) dst + i1*args.nb1 + i02*args.nb2 + i03*args.nb3); + const device float * src_row = (const device float *) ((const device char *) src0 + i01*args.nb01 + i02*args.nb02 + i03*args.nb03); + + for (int ind = tiitg%tptg.x; ind < args.nk0; ind += tptg.x) { + dst_row[ind] = (T) src_row[ind]; + } +} + +kernel void kernel_diag_f32( + constant ggml_metal_kargs_diag & args, + device const char * src0, + device char * dst, + uint3 tgpig[[threadgroup_position_in_grid]], + ushort tiitg[[thread_index_in_threadgroup]]) { + constexpr short NW = N_SIMDWIDTH; + + const int32_t i3 = tgpig.z; + const int32_t i2 = tgpig.y; + const int32_t i1 = tgpig.x; + + device const float * src0_ptr = (device const float *)(src0 + i2*args.nb02 + i3*args.nb03); + device float * dst_ptr = (device float *)(dst + i1*args.nb01 + i2*args.nb2 + i3*args.nb3); + + for (int i0 = tiitg; i0 < args.ne0; i0 += NW) { + dst_ptr[i0] = i0 == i1 ? src0_ptr[i0] : 0.0f; + } +} + +constant bool FC_mul_mm_bc_inp [[function_constant(FC_MUL_MM + 0)]]; +constant bool FC_mul_mm_bc_out [[function_constant(FC_MUL_MM + 1)]]; + +// each block_q contains 16*nl weights +template +kernel void kernel_mul_mm( + constant ggml_metal_kargs_mul_mm & args, + device const char * src0, + device const char * src1, + device char * dst, + threadgroup char * shmem [[threadgroup(0)]], + uint3 tgpig[[threadgroup_position_in_grid]], + ushort tiitg[[thread_index_in_threadgroup]], + ushort sgitg[[simdgroup_index_in_threadgroup]]) { + + threadgroup S0 * sa = (threadgroup S0 *)(shmem); + threadgroup S1 * sb = (threadgroup S1 *)(shmem + 4096); + +#ifdef GGML_METAL_HAS_TENSOR + threadgroup float * sc = (threadgroup float *)(shmem); +#endif + + constexpr int NR0 = 64; + constexpr int NR1 = 32; + + constexpr int NK = 32; + constexpr int NL0 = NK/16; + constexpr int NL1 = NK/8; + + const int im = tgpig.z; + const int r0 = tgpig.y*NR0; + const int r1 = tgpig.x*NR1; + + // if this block is of 64x32 shape or smaller + const short nr0 = (args.ne0 - r0 < NR0) ? (args.ne0 - r0) : NR0; + const short nr1 = (args.ne1 - r1 < NR1) ? (args.ne1 - r1) : NR1; + + // a thread shouldn't load data outside of the matrix + const short lr0 = ((short)tiitg/NL0) < nr0 ? ((short)tiitg/NL0) : nr0 - 1; // 0 .. 63 + const short lr1 = ((short)tiitg/NL1) < nr1 ? ((short)tiitg/NL1) : nr1 - 1; // 0 .. 31 + + const short il0 = (tiitg % NL0); + + short il = il0; + + const int i12 = im%args.ne12; + const int i13 = im/args.ne12; + + const uint64_t offset0 = (i12/args.r2)*args.nb02 + (i13/args.r3)*args.nb03; + const short offset1 = il0/nl; + + device const block_q * x = (device const block_q *)(src0 + args.nb01*(r0 + lr0) + offset0) + offset1; + + const short iy = 8*(tiitg % NL1); + + device const T1 * y = (device const T1 *)(src1 + + args.nb13*i13 + + args.nb12*i12 + + args.nb11*(r1 + lr1) + + args.nb10*iy); + +#ifndef GGML_METAL_HAS_TENSOR + S0_8x8 ma[4]; + S1_8x8 mb[2]; + + simdgroup_float8x8 mc[8]; + + for (short i = 0; i < 8; i++){ + mc[i] = make_filled_simdgroup_matrix(0.f); + } +#else + auto tA = tensor, tensor_inline>(sa, dextents(NK, NR0)); + auto tB = tensor, tensor_inline>(sb, dextents(NR1, NK )); + + mpp::tensor_ops::matmul2d< + mpp::tensor_ops::matmul2d_descriptor(NR1, NR0, NK, false, true, false, mpp::tensor_ops::matmul2d_descriptor::mode::multiply_accumulate), + execution_simdgroups<4>> mm; + + auto cT = mm.get_destination_cooperative_tensor(); +#endif + + for (int loop_k = 0; loop_k < args.ne00; loop_k += NK) { +#ifndef GGML_METAL_HAS_TENSOR + // load data and store to threadgroup memory + if (is_same::value && FC_mul_mm_bc_inp) { + threadgroup_barrier(mem_flags::mem_threadgroup); + + // no need for dequantization + for (short i = 0; i < 16; i++) { + const short sx = 2*il0 + i/8; + const short sy = (tiitg/NL0)/8; + + //const short lx = i%8; + //const short ly = (tiitg/NL0)%8; + const short lx = (tiitg/NL0)%8; + const short ly = i%8; + + const short ib = 8*sx + sy; + + *(sa + 64*ib + 8*ly + lx) = loop_k + 16*il + i < args.ne00 ? *((device T0 *) x + i) : 0; + } + } else { + S0_4x4 temp_a; + dequantize_func(x, il, temp_a); + + threadgroup_barrier(mem_flags::mem_threadgroup); + + FOR_UNROLL (short i = 0; i < 16; i++) { + const short sx = 2*il0 + i/8; + const short sy = (tiitg/NL0)/8; + + //const short lx = i%8; + //const short ly = (tiitg/NL0)%8; + const short lx = (tiitg/NL0)%8; + const short ly = i%8; + + const short ib = 8*sx + sy; + + // NOTE: this is massively slower.. WTF? + //sa[64*ib + 8*ly + lx] = temp_a[i/4][i%4]; + + *(sa + 64*ib + 8*ly + lx) = temp_a[i/4][i%4]; + } + } + + if (FC_mul_mm_bc_inp) { + for (short i = 0; i < 8; ++i) { + const short sx = (tiitg%NL1); + const short sy = (tiitg/NL1)/8; + + const short lx = i; + const short ly = (tiitg/NL1)%8; + //const short lx = (tiitg/NL1)%8; + //const short ly = i; + + const short ib = 4*sx + sy; + + *(sb + 64*ib + 8*ly + lx) = loop_k + iy + i < args.ne00 ? (S1) *((device T1 *) y + i) : 0; + } + } else { + const short sx = (tiitg%NL1); + const short sy = (tiitg/NL1)/8; + + //const short dx = sx; + //const short dy = sy; + + const short ly = (tiitg/NL1)%8; + + const short ib = 4*sx + sy; + + *(threadgroup S1_2x4 *)(sb + 64*ib + 8*ly) = (S1_2x4)(*((device T1_2x4 *) y)); + } +#else + // load data and store to threadgroup memory + if (is_same::value && FC_mul_mm_bc_inp) { + threadgroup_barrier(mem_flags::mem_threadgroup); + + // no need for dequantization + for (short i = 0; i < 16; i++) { + const short sx = 2*il0 + i/8; + const short sy = (tiitg/NL0)/8; + + const short lx = i%8; + const short ly = (tiitg/NL0)%8; + //const short lx = (tiitg/NL0)%8; + //const short ly = i%8; + + *(sa + NK*(8*sy + ly) + 8*sx + lx) = loop_k + 16*il + i < args.ne00 ? *((device T0 *) x + i) : 0; + } + } else { + S0_4x4 temp_a; + dequantize_func(x, il, temp_a); + + threadgroup_barrier(mem_flags::mem_threadgroup); + + FOR_UNROLL (short i = 0; i < 16; i++) { + const short sx = 2*il0 + i/8; + const short sy = (tiitg/NL0)/8; + + const short lx = i%8; + const short ly = (tiitg/NL0)%8; + //const short lx = (tiitg/NL0)%8; + //const short ly = i%8; + + *(sa + NK*(8*sy + ly) + 8*sx + lx) = temp_a[i/4][i%4]; + } + } + + if (FC_mul_mm_bc_inp) { + for (short i = 0; i < 8; ++i) { + const short sx = (tiitg%NL1); + const short sy = (tiitg/NL1)/8; + + const short lx = i; + const short ly = (tiitg/NL1)%8; + //const short lx = (tiitg/NL1)%8; + //const short ly = i; + + *(sb + NK*(8*sy + ly) + 8*sx + lx) = loop_k + iy + i < args.ne00 ? (S1) *((device T1 *) y + i) : 0; + } + } else { + const short sx = (tiitg%NL1); + const short sy = (tiitg/NL1)/8; + + //const short lx = i; + const short ly = (tiitg/NL1)%8; + //const short lx = (tiitg/NL1)%8; + //const short ly = i; + + *(threadgroup S1_2x4 *)(sb + NK*(8*sy + ly) + 8*sx) = (S1_2x4)(*((device T1_2x4 *) y)); + } +#endif + + il = (il + 2 < nl) ? il + 2 : il % 2; + x = (il < 2) ? x + (2 + nl - 1)/nl : x; + + y += NK; + + threadgroup_barrier(mem_flags::mem_threadgroup); + +#ifndef GGML_METAL_HAS_TENSOR + // load matrices from threadgroup memory and conduct outer products + threadgroup const S0 * lsma = (sa + 4*64*(sgitg%2)); + threadgroup const S1 * lsmb = (sb + 2*64*(sgitg/2)); + + FOR_UNROLL (short ik = 0; ik < NK/8; ik++) { + simdgroup_barrier(mem_flags::mem_none); + + FOR_UNROLL (short i = 0; i < 4; i++) { + simdgroup_load(ma[i], lsma + 64*i, 8, 0, false); + } + + simdgroup_barrier(mem_flags::mem_none); + + FOR_UNROLL (short i = 0; i < 2; i++) { + simdgroup_load(mb[i], lsmb + 64*i, 8, 0, false); + } + + simdgroup_barrier(mem_flags::mem_none); + + FOR_UNROLL (short i = 0; i < 8; i++){ + simdgroup_multiply_accumulate(mc[i], mb[i/4], ma[i%4], mc[i]); + } + + lsma += 8*64; + lsmb += 4*64; + } +#else + auto sA = tA.slice(0, 0); + auto sB = tB.slice(0, 0); + + mm.run(sB, sA, cT); +#endif + } + + if (!FC_mul_mm_bc_out || (r0 + NR0 <= args.ne0 && r1 + NR1 <= args.ne1)) { + // if no bounds checks on the output are needed, we can directly write to device memory +#ifdef GGML_METAL_HAS_TENSOR + device float * C = (device float *) dst + + r0 + \ + r1 * args.ne0 + im*args.ne1*args.ne0; + + auto tC = tensor, tensor_inline>(C, dextents(args.ne0, NR1)); + cT.store(tC); +#else + device float * C = (device float *) dst + + (r0 + 32*(sgitg & 1)) + \ + (r1 + 16*(sgitg >> 1)) * args.ne0 + im*args.ne1*args.ne0; + + for (short i = 0; i < 8; i++) { + simdgroup_store(mc[i], C + 8*(i%4) + 8*args.ne0*(i/4), args.ne0, 0, false); + } +#endif + } else { + // block is smaller than 64x32, we should avoid writing data outside of the matrix + threadgroup_barrier(mem_flags::mem_threadgroup); + + threadgroup float * temp_str = ((threadgroup float *) shmem) + 32*(sgitg&1) + (16*(sgitg >> 1))*NR0; + +#ifdef GGML_METAL_HAS_TENSOR + auto tC = tensor, tensor_inline>(sc, dextents(NR0, NR1)); + cT.store(tC); +#else + for (short i = 0; i < 8; i++) { + simdgroup_store(mc[i], temp_str + 8*(i%4) + 8*NR0*(i/4), NR0, 0, false); + } +#endif + + threadgroup_barrier(mem_flags::mem_threadgroup); + + if (sgitg == 0) { + for (int j = tiitg; j < nr1; j += NR1) { + device float * D = (device float *) dst + r0 + (r1 + j)*args.ne0 + im*args.ne1*args.ne0; + device float4 * D4 = (device float4 *) D; + + threadgroup float * C = temp_str + (j*NR0); + threadgroup float4 * C4 = (threadgroup float4 *) C; + + int i = 0; + for (; i < nr0/4; i++) { + *(D4 + i) = *(C4 + i); + } + + i *= 4; + for (; i < nr0; i++) { + *(D + i) = *(C + i); + } + } + } + } +} + +template // n_expert_used +kernel void kernel_mul_mm_id_map0( + constant ggml_metal_kargs_mul_mm_id_map0 & args, + device const char * src2, + device char * htpe, + device char * hids, + threadgroup char * shmem [[threadgroup(0)]], + ushort tpitg[[thread_position_in_threadgroup]], + ushort ntg[[threads_per_threadgroup]]) { + const short ide = tpitg; // expert id + + uint32_t n_all = 0; + + device int32_t * ids_i32 = (device int32_t *) hids + ide*args.ne21; + + for (int i21 = 0; i21 < args.ne21; i21 += ntg) { // n_tokens + if (i21 + tpitg < args.ne21) { + device const int32_t * src2_i32 = (device const int32_t *) (src2 + (i21 + tpitg)*args.nb21); + + threadgroup uint16_t * sids = (threadgroup uint16_t *) shmem + tpitg*ne20; + + #pragma unroll(ne20) + for (short i20 = 0; i20 < ne20; i20++) { + sids[i20] = src2_i32[i20]; + } + } + + threadgroup_barrier(mem_flags::mem_threadgroup); + + for (short t = 0; t < ntg; t++) { + if (i21 + t >= args.ne21) { + break; + } + + threadgroup const uint16_t * sids = (threadgroup const uint16_t *) shmem + t*ne20; + + short sel = 0; + #pragma unroll(ne20) + for (short i20 = 0; i20 < ne20; i20++) { + sel += (sids[i20] == ide)*(i20 + 1); + } + + ids_i32[n_all] = (i21 + t)*ne20 + sel - 1; + + n_all += sel > 0; + } + + threadgroup_barrier(mem_flags::mem_threadgroup); + } + + device uint32_t * tpe_u32 = (device uint32_t *) (htpe); + tpe_u32[ide] = n_all; +} + +typedef decltype(kernel_mul_mm_id_map0<1>) kernel_mul_mm_id_map0_t; + +template [[host_name("kernel_mul_mm_id_map0_ne20_1" )]] kernel kernel_mul_mm_id_map0_t kernel_mul_mm_id_map0<1>; +template [[host_name("kernel_mul_mm_id_map0_ne20_2" )]] kernel kernel_mul_mm_id_map0_t kernel_mul_mm_id_map0<2>; +template [[host_name("kernel_mul_mm_id_map0_ne20_4" )]] kernel kernel_mul_mm_id_map0_t kernel_mul_mm_id_map0<4>; +template [[host_name("kernel_mul_mm_id_map0_ne20_5" )]] kernel kernel_mul_mm_id_map0_t kernel_mul_mm_id_map0<5>; +template [[host_name("kernel_mul_mm_id_map0_ne20_6" )]] kernel kernel_mul_mm_id_map0_t kernel_mul_mm_id_map0<6>; +template [[host_name("kernel_mul_mm_id_map0_ne20_8" )]] kernel kernel_mul_mm_id_map0_t kernel_mul_mm_id_map0<8>; +template [[host_name("kernel_mul_mm_id_map0_ne20_10")]] kernel kernel_mul_mm_id_map0_t kernel_mul_mm_id_map0<10>; +template [[host_name("kernel_mul_mm_id_map0_ne20_16")]] kernel kernel_mul_mm_id_map0_t kernel_mul_mm_id_map0<16>; +template [[host_name("kernel_mul_mm_id_map0_ne20_22")]] kernel kernel_mul_mm_id_map0_t kernel_mul_mm_id_map0<22>; + +template +kernel void kernel_mul_mm_id( + constant ggml_metal_kargs_mul_mm_id & args, + device const char * src0, + device const char * src1, + device const char * htpe, + device const char * hids, + device char * dst, + threadgroup char * shmem [[threadgroup(0)]], + uint3 tgpig[[threadgroup_position_in_grid]], + ushort tiitg[[thread_index_in_threadgroup]], + ushort tiisg[[thread_index_in_simdgroup]], + ushort sgitg[[simdgroup_index_in_threadgroup]]) { + threadgroup S0 * sa = (threadgroup S0 *)(shmem); + threadgroup S1 * sb = (threadgroup S1 *)(shmem + 4096); + +#ifdef GGML_METAL_HAS_TENSOR + threadgroup float * sc = (threadgroup float *)(shmem); +#endif + + constexpr int NR0 = 64; + constexpr int NR1 = 32; + + constexpr int NK = 32; + constexpr int NL0 = NK/16; + constexpr int NL1 = NK/8; + + const int im = tgpig.z; // expert + const int r0 = tgpig.y*NR0; + const int r1 = tgpig.x*NR1; + + device const uint32_t * tpe_u32 = (device const uint32_t *) (htpe); + device const int32_t * ids_i32 = (device const int32_t *) (hids); + + const int32_t neh1 = tpe_u32[im]; + + if (r1 >= neh1) { + return; + } + + // if this block is of 64x32 shape or smaller + const short nr0 = (args.ne0 - r0 < NR0) ? (args.ne0 - r0) : NR0; + const short nr1 = ( neh1 - r1 < NR1) ? ( neh1 - r1) : NR1; + + // a thread shouldn't load data outside of the matrix + const short lr0 = ((short)tiitg/NL0) < nr0 ? ((short)tiitg/NL0) : nr0 - 1; // 0 .. 63 + const short lr1 = ((short)tiitg/NL1) < nr1 ? ((short)tiitg/NL1) : nr1 - 1; // 0 .. 31 + + const short il0 = (tiitg % NL0); + + short il = il0; + + const int id = ids_i32[im*args.ne21 + r1 + lr1]; + + const short i11 = (id % args.ne20) % args.ne11; + const short i12 = (id / args.ne20); + const short i13 = 0; + + const uint64_t offset0 = im*args.nb02 + i13*args.nb03; + const short offset1 = il0/nl; + + device const block_q * x = (device const block_q *)(src0 + args.nb01*(r0 + lr0) + offset0) + offset1; + + const short iy = 8*(tiitg % NL1); + + device const T1 * y = (device const T1 *)(src1 + + args.nb13*i13 + + args.nb12*i12 + + args.nb11*i11 + + args.nb10*iy); + +#ifndef GGML_METAL_HAS_TENSOR + S0_8x8 ma[4]; + S1_8x8 mb[2]; + + simdgroup_float8x8 mc[8]; + + for (short i = 0; i < 8; i++){ + mc[i] = make_filled_simdgroup_matrix(0.f); + } +#else + auto tA = tensor, tensor_inline>(sa, dextents(NK, NR0)); + auto tB = tensor, tensor_inline>(sb, dextents(NR1, NK )); + + mpp::tensor_ops::matmul2d< + mpp::tensor_ops::matmul2d_descriptor(NR1, NR0, NK, false, true, false, mpp::tensor_ops::matmul2d_descriptor::mode::multiply_accumulate), + execution_simdgroups<4>> mm; + + auto cT = mm.get_destination_cooperative_tensor(); +#endif + + for (int loop_k = 0; loop_k < args.ne00; loop_k += NK) { +#ifndef GGML_METAL_HAS_TENSOR + // load data and store to threadgroup memory + if (is_same::value && FC_mul_mm_bc_inp) { + threadgroup_barrier(mem_flags::mem_threadgroup); + + // no need for dequantization + for (short i = 0; i < 16; i++) { + const short sx = 2*il0 + i/8; + const short sy = (tiitg/NL0)/8; + + //const short lx = i%8; + //const short ly = (tiitg/NL0)%8; + const short lx = (tiitg/NL0)%8; + const short ly = i%8; + + const short ib = 8*sx + sy; + + *(sa + 64*ib + 8*ly + lx) = loop_k + 16*il + i < args.ne00 ? *((device T0 *) x + i) : 0; + } + } else { + S0_4x4 temp_a; + dequantize_func(x, il, temp_a); + + threadgroup_barrier(mem_flags::mem_threadgroup); + + FOR_UNROLL (short i = 0; i < 16; i++) { + const short sx = 2*il0 + i/8; + const short sy = (tiitg/NL0)/8; + + //const short lx = i%8; + //const short ly = (tiitg/NL0)%8; + const short lx = (tiitg/NL0)%8; + const short ly = i%8; + + const short ib = 8*sx + sy; + + // NOTE: this is massively slower.. WTF? + //sa[64*ib + 8*ly + lx] = temp_a[i/4][i%4]; + + *(sa + 64*ib + 8*ly + lx) = temp_a[i/4][i%4]; + } + } + + if (FC_mul_mm_bc_inp) { + for (short i = 0; i < 8; ++i) { + const short sx = (tiitg%NL1); + const short sy = (tiitg/NL1)/8; + + const short lx = i; + const short ly = (tiitg/NL1)%8; + //const short lx = (tiitg/NL1)%8; + //const short ly = i; + + const short ib = 4*sx + sy; + + *(sb + 64*ib + 8*ly + lx) = loop_k + iy + i < args.ne00 ? (S1) *((device T1 *) y + i) : 0; + } + } else { + const short sx = (tiitg%NL1); + const short sy = (tiitg/NL1)/8; + + //const short dx = sx; + //const short dy = sy; + + const short ly = (tiitg/NL1)%8; + + const short ib = 4*sx + sy; + + *(threadgroup S1_2x4 *)(sb + 64*ib + 8*ly) = (S1_2x4)(*((device T1_2x4 *) y)); + } +#else + // load data and store to threadgroup memory + if (is_same::value && FC_mul_mm_bc_inp) { + threadgroup_barrier(mem_flags::mem_threadgroup); + + // no need for dequantization + for (short i = 0; i < 16; i++) { + const short sx = 2*il0 + i/8; + const short sy = (tiitg/NL0)/8; + + const short lx = i%8; + const short ly = (tiitg/NL0)%8; + //const short lx = (tiitg/NL0)%8; + //const short ly = i%8; + + *(sa + NK*(8*sy + ly) + 8*sx + lx) = loop_k + 16*il + i < args.ne00 ? *((device T0 *) x + i) : 0; + } + } else { + S0_4x4 temp_a; + dequantize_func(x, il, temp_a); + + threadgroup_barrier(mem_flags::mem_threadgroup); + + FOR_UNROLL (short i = 0; i < 16; i++) { + const short sx = 2*il0 + i/8; + const short sy = (tiitg/NL0)/8; + + const short lx = i%8; + const short ly = (tiitg/NL0)%8; + //const short lx = (tiitg/NL0)%8; + //const short ly = i%8; + + *(sa + NK*(8*sy + ly) + 8*sx + lx) = temp_a[i/4][i%4]; + } + } + + if (FC_mul_mm_bc_inp) { + for (short i = 0; i < 8; ++i) { + const short sx = (tiitg%NL1); + const short sy = (tiitg/NL1)/8; + + const short lx = i; + const short ly = (tiitg/NL1)%8; + //const short lx = (tiitg/NL1)%8; + //const short ly = i; + + *(sb + NK*(8*sy + ly) + 8*sx + lx) = loop_k + iy + i < args.ne00 ? (S1) *((device T1 *) y + i) : 0; + } + } else { + const short sx = (tiitg%NL1); + const short sy = (tiitg/NL1)/8; + + //const short lx = i; + const short ly = (tiitg/NL1)%8; + //const short lx = (tiitg/NL1)%8; + //const short ly = i; + + *(threadgroup S1_2x4 *)(sb + NK*(8*sy + ly) + 8*sx) = (S1_2x4)(*((device T1_2x4 *) y)); + } +#endif + + il = (il + 2 < nl) ? il + 2 : il % 2; + x = (il < 2) ? x + (2 + nl - 1)/nl : x; + + y += NK; + + threadgroup_barrier(mem_flags::mem_threadgroup); + +#ifndef GGML_METAL_HAS_TENSOR + // load matrices from threadgroup memory and conduct outer products + threadgroup const S0 * lsma = (sa + 4*64*(sgitg%2)); + threadgroup const S1 * lsmb = (sb + 2*64*(sgitg/2)); + + FOR_UNROLL (short ik = 0; ik < NK/8; ik++) { + simdgroup_barrier(mem_flags::mem_none); + + FOR_UNROLL (short i = 0; i < 4; i++) { + simdgroup_load(ma[i], lsma + 64*i, 8, 0, false); + } + + simdgroup_barrier(mem_flags::mem_none); + + FOR_UNROLL (short i = 0; i < 2; i++) { + simdgroup_load(mb[i], lsmb + 64*i, 8, 0, false); + } + + simdgroup_barrier(mem_flags::mem_none); + + FOR_UNROLL (short i = 0; i < 8; i++){ + simdgroup_multiply_accumulate(mc[i], mb[i/4], ma[i%4], mc[i]); + } + + lsma += 8*64; + lsmb += 4*64; + } +#else + auto sA = tA.slice(0, 0); + auto sB = tB.slice(0, 0); + + mm.run(sB, sA, cT); +#endif + } + + // block is smaller than 64x32, we should avoid writing data outside of the matrix + threadgroup_barrier(mem_flags::mem_threadgroup); + +#ifdef GGML_METAL_HAS_TENSOR + auto tC = tensor, tensor_inline>(sc, dextents(NR0, NR1)); + cT.store(tC); +#else + threadgroup float * temp_str = ((threadgroup float *) shmem) + 32*(sgitg&1) + (16*(sgitg >> 1))*NR0; + + for (short i = 0; i < 8; i++) { + simdgroup_store(mc[i], temp_str + 8*(i%4) + 8*NR0*(i/4), NR0, 0, false); + } +#endif + + threadgroup_barrier(mem_flags::mem_threadgroup); + + for (short j = sgitg; j < nr1; j += 4) { + const int id = ids_i32[im*args.ne21 + r1 + j]; + + const short ide = id % args.ne20; + const short idt = id / args.ne20; + + device float * D = (device float *) dst + r0 + ide*args.ne0 + idt*args.ne1*args.ne0; + device float4 * D4 = (device float4 *) D; + + threadgroup float * C = (threadgroup float *) shmem + j*NR0; + threadgroup float4 * C4 = (threadgroup float4 *) C; + + int i = tiisg; + for (; i < nr0/4; i += 32) { + *(D4 + i) = *(C4 + i); + } + + i = (4*(nr0/4)) + tiisg; + for (; i < nr0; i += 32) { + *(D + i) = *(C + i); + } + } +} + +#define QK_NL 16 + +// +// get rows +// + +typedef decltype(kernel_get_rows_f) get_rows_f_t; + +template [[host_name("kernel_get_rows_f32")]] kernel get_rows_f_t kernel_get_rows_f; +template [[host_name("kernel_get_rows_f16")]] kernel get_rows_f_t kernel_get_rows_f; +template [[host_name("kernel_get_rows_i32")]] kernel get_rows_f_t kernel_get_rows_f; +#if defined(GGML_METAL_HAS_BF16) +template [[host_name("kernel_get_rows_bf16")]] kernel get_rows_f_t kernel_get_rows_f; +#endif + +typedef decltype(kernel_get_rows_q) get_rows_q_t; + +template [[host_name("kernel_get_rows_q4_0")]] kernel get_rows_q_t kernel_get_rows_q; +template [[host_name("kernel_get_rows_q4_1")]] kernel get_rows_q_t kernel_get_rows_q; +template [[host_name("kernel_get_rows_q5_0")]] kernel get_rows_q_t kernel_get_rows_q; +template [[host_name("kernel_get_rows_q5_1")]] kernel get_rows_q_t kernel_get_rows_q; +template [[host_name("kernel_get_rows_q8_0")]] kernel get_rows_q_t kernel_get_rows_q; +template [[host_name("kernel_get_rows_mxfp4")]] kernel get_rows_q_t kernel_get_rows_q; +template [[host_name("kernel_get_rows_q2_K")]] kernel get_rows_q_t kernel_get_rows_q; +template [[host_name("kernel_get_rows_q3_K")]] kernel get_rows_q_t kernel_get_rows_q; +template [[host_name("kernel_get_rows_q4_K")]] kernel get_rows_q_t kernel_get_rows_q; +template [[host_name("kernel_get_rows_q5_K")]] kernel get_rows_q_t kernel_get_rows_q; +template [[host_name("kernel_get_rows_q6_K")]] kernel get_rows_q_t kernel_get_rows_q; +template [[host_name("kernel_get_rows_iq2_xxs")]] kernel get_rows_q_t kernel_get_rows_q; +template [[host_name("kernel_get_rows_iq2_xs")]] kernel get_rows_q_t kernel_get_rows_q; +template [[host_name("kernel_get_rows_iq3_xxs")]] kernel get_rows_q_t kernel_get_rows_q; +template [[host_name("kernel_get_rows_iq3_s")]] kernel get_rows_q_t kernel_get_rows_q; +template [[host_name("kernel_get_rows_iq2_s")]] kernel get_rows_q_t kernel_get_rows_q; +template [[host_name("kernel_get_rows_iq1_s")]] kernel get_rows_q_t kernel_get_rows_q; +template [[host_name("kernel_get_rows_iq1_m")]] kernel get_rows_q_t kernel_get_rows_q; +template [[host_name("kernel_get_rows_iq4_nl")]] kernel get_rows_q_t kernel_get_rows_q; +template [[host_name("kernel_get_rows_iq4_xs")]] kernel get_rows_q_t kernel_get_rows_q; + +// +// set rows +// + +typedef decltype(kernel_set_rows_f) set_rows_f_t; + +template [[host_name("kernel_set_rows_f32_i64")]] kernel set_rows_f_t kernel_set_rows_f; +template [[host_name("kernel_set_rows_f32_i32")]] kernel set_rows_f_t kernel_set_rows_f; +template [[host_name("kernel_set_rows_f16_i64")]] kernel set_rows_f_t kernel_set_rows_f; +template [[host_name("kernel_set_rows_f16_i32")]] kernel set_rows_f_t kernel_set_rows_f; +#if defined(GGML_METAL_HAS_BF16) +template [[host_name("kernel_set_rows_bf16_i64")]] kernel set_rows_f_t kernel_set_rows_f; +template [[host_name("kernel_set_rows_bf16_i32")]] kernel set_rows_f_t kernel_set_rows_f; +#endif + +typedef decltype(kernel_set_rows_q32) set_rows_q32_t; + +template [[host_name("kernel_set_rows_q8_0_i64")]] kernel set_rows_q32_t kernel_set_rows_q32; +template [[host_name("kernel_set_rows_q8_0_i32")]] kernel set_rows_q32_t kernel_set_rows_q32; +template [[host_name("kernel_set_rows_q4_0_i64")]] kernel set_rows_q32_t kernel_set_rows_q32; +template [[host_name("kernel_set_rows_q4_0_i32")]] kernel set_rows_q32_t kernel_set_rows_q32; +template [[host_name("kernel_set_rows_q4_1_i64")]] kernel set_rows_q32_t kernel_set_rows_q32; +template [[host_name("kernel_set_rows_q4_1_i32")]] kernel set_rows_q32_t kernel_set_rows_q32; +template [[host_name("kernel_set_rows_q5_0_i64")]] kernel set_rows_q32_t kernel_set_rows_q32; +template [[host_name("kernel_set_rows_q5_0_i32")]] kernel set_rows_q32_t kernel_set_rows_q32; +template [[host_name("kernel_set_rows_q5_1_i64")]] kernel set_rows_q32_t kernel_set_rows_q32; +template [[host_name("kernel_set_rows_q5_1_i32")]] kernel set_rows_q32_t kernel_set_rows_q32; +template [[host_name("kernel_set_rows_iq4_nl_i64")]] kernel set_rows_q32_t kernel_set_rows_q32; +template [[host_name("kernel_set_rows_iq4_nl_i32")]] kernel set_rows_q32_t kernel_set_rows_q32; + +// +// matrix-matrix multiplication +// + +typedef decltype(kernel_mul_mm) mul_mm_t; + +template [[host_name("kernel_mul_mm_f32_f32")]] kernel mul_mm_t kernel_mul_mm; +template [[host_name("kernel_mul_mm_f16_f32")]] kernel mul_mm_t kernel_mul_mm; +#if defined(GGML_METAL_HAS_BF16) +template [[host_name("kernel_mul_mm_bf16_f32")]] kernel mul_mm_t kernel_mul_mm; +#endif +template [[host_name("kernel_mul_mm_q4_0_f32")]] kernel mul_mm_t kernel_mul_mm; +template [[host_name("kernel_mul_mm_q4_1_f32")]] kernel mul_mm_t kernel_mul_mm; +template [[host_name("kernel_mul_mm_q5_0_f32")]] kernel mul_mm_t kernel_mul_mm; +template [[host_name("kernel_mul_mm_q5_1_f32")]] kernel mul_mm_t kernel_mul_mm; +template [[host_name("kernel_mul_mm_q8_0_f32")]] kernel mul_mm_t kernel_mul_mm; +template [[host_name("kernel_mul_mm_mxfp4_f32")]] kernel mul_mm_t kernel_mul_mm; +template [[host_name("kernel_mul_mm_q2_K_f32")]] kernel mul_mm_t kernel_mul_mm; +template [[host_name("kernel_mul_mm_q3_K_f32")]] kernel mul_mm_t kernel_mul_mm; +template [[host_name("kernel_mul_mm_q4_K_f32")]] kernel mul_mm_t kernel_mul_mm; +template [[host_name("kernel_mul_mm_q5_K_f32")]] kernel mul_mm_t kernel_mul_mm; +template [[host_name("kernel_mul_mm_q6_K_f32")]] kernel mul_mm_t kernel_mul_mm; +template [[host_name("kernel_mul_mm_iq2_xxs_f32")]] kernel mul_mm_t kernel_mul_mm; +template [[host_name("kernel_mul_mm_iq2_xs_f32")]] kernel mul_mm_t kernel_mul_mm; +template [[host_name("kernel_mul_mm_iq3_xxs_f32")]] kernel mul_mm_t kernel_mul_mm; +template [[host_name("kernel_mul_mm_iq3_s_f32")]] kernel mul_mm_t kernel_mul_mm; +template [[host_name("kernel_mul_mm_iq2_s_f32")]] kernel mul_mm_t kernel_mul_mm; +template [[host_name("kernel_mul_mm_iq1_s_f32")]] kernel mul_mm_t kernel_mul_mm; +template [[host_name("kernel_mul_mm_iq1_m_f32")]] kernel mul_mm_t kernel_mul_mm; +template [[host_name("kernel_mul_mm_iq4_nl_f32")]] kernel mul_mm_t kernel_mul_mm; +template [[host_name("kernel_mul_mm_iq4_xs_f32")]] kernel mul_mm_t kernel_mul_mm; + +template [[host_name("kernel_mul_mm_f32_f16")]] kernel mul_mm_t kernel_mul_mm; +template [[host_name("kernel_mul_mm_f16_f16")]] kernel mul_mm_t kernel_mul_mm; +template [[host_name("kernel_mul_mm_q4_0_f16")]] kernel mul_mm_t kernel_mul_mm; +template [[host_name("kernel_mul_mm_q4_1_f16")]] kernel mul_mm_t kernel_mul_mm; +template [[host_name("kernel_mul_mm_q5_0_f16")]] kernel mul_mm_t kernel_mul_mm; +template [[host_name("kernel_mul_mm_q5_1_f16")]] kernel mul_mm_t kernel_mul_mm; +template [[host_name("kernel_mul_mm_q8_0_f16")]] kernel mul_mm_t kernel_mul_mm; +template [[host_name("kernel_mul_mm_mxfp4_f16")]] kernel mul_mm_t kernel_mul_mm; +template [[host_name("kernel_mul_mm_q2_K_f16")]] kernel mul_mm_t kernel_mul_mm; +template [[host_name("kernel_mul_mm_q3_K_f16")]] kernel mul_mm_t kernel_mul_mm; +template [[host_name("kernel_mul_mm_q4_K_f16")]] kernel mul_mm_t kernel_mul_mm; +template [[host_name("kernel_mul_mm_q5_K_f16")]] kernel mul_mm_t kernel_mul_mm; +template [[host_name("kernel_mul_mm_q6_K_f16")]] kernel mul_mm_t kernel_mul_mm; +template [[host_name("kernel_mul_mm_iq2_xxs_f16")]] kernel mul_mm_t kernel_mul_mm; +template [[host_name("kernel_mul_mm_iq2_xs_f16")]] kernel mul_mm_t kernel_mul_mm; +template [[host_name("kernel_mul_mm_iq3_xxs_f16")]] kernel mul_mm_t kernel_mul_mm; +template [[host_name("kernel_mul_mm_iq3_s_f16")]] kernel mul_mm_t kernel_mul_mm; +template [[host_name("kernel_mul_mm_iq2_s_f16")]] kernel mul_mm_t kernel_mul_mm; +template [[host_name("kernel_mul_mm_iq1_s_f16")]] kernel mul_mm_t kernel_mul_mm; +template [[host_name("kernel_mul_mm_iq1_m_f16")]] kernel mul_mm_t kernel_mul_mm; +template [[host_name("kernel_mul_mm_iq4_nl_f16")]] kernel mul_mm_t kernel_mul_mm; +template [[host_name("kernel_mul_mm_iq4_xs_f16")]] kernel mul_mm_t kernel_mul_mm; + +// +// indirect matrix-matrix multiplication +// + +typedef decltype(kernel_mul_mm_id) mul_mm_id; + +template [[host_name("kernel_mul_mm_id_f32_f32")]] kernel mul_mm_id kernel_mul_mm_id; +template [[host_name("kernel_mul_mm_id_f16_f32")]] kernel mul_mm_id kernel_mul_mm_id; +#if defined(GGML_METAL_HAS_BF16) +template [[host_name("kernel_mul_mm_id_bf16_f32")]] kernel mul_mm_id kernel_mul_mm_id; +#endif +template [[host_name("kernel_mul_mm_id_q4_0_f32")]] kernel mul_mm_id kernel_mul_mm_id; +template [[host_name("kernel_mul_mm_id_q4_1_f32")]] kernel mul_mm_id kernel_mul_mm_id; +template [[host_name("kernel_mul_mm_id_q5_0_f32")]] kernel mul_mm_id kernel_mul_mm_id; +template [[host_name("kernel_mul_mm_id_q5_1_f32")]] kernel mul_mm_id kernel_mul_mm_id; +template [[host_name("kernel_mul_mm_id_q8_0_f32")]] kernel mul_mm_id kernel_mul_mm_id; +template [[host_name("kernel_mul_mm_id_mxfp4_f32")]] kernel mul_mm_id kernel_mul_mm_id; +template [[host_name("kernel_mul_mm_id_q2_K_f32")]] kernel mul_mm_id kernel_mul_mm_id; +template [[host_name("kernel_mul_mm_id_q3_K_f32")]] kernel mul_mm_id kernel_mul_mm_id; +template [[host_name("kernel_mul_mm_id_q4_K_f32")]] kernel mul_mm_id kernel_mul_mm_id; +template [[host_name("kernel_mul_mm_id_q5_K_f32")]] kernel mul_mm_id kernel_mul_mm_id; +template [[host_name("kernel_mul_mm_id_q6_K_f32")]] kernel mul_mm_id kernel_mul_mm_id; +template [[host_name("kernel_mul_mm_id_iq2_xxs_f32")]] kernel mul_mm_id kernel_mul_mm_id; +template [[host_name("kernel_mul_mm_id_iq2_xs_f32")]] kernel mul_mm_id kernel_mul_mm_id; +template [[host_name("kernel_mul_mm_id_iq3_xxs_f32")]] kernel mul_mm_id kernel_mul_mm_id; +template [[host_name("kernel_mul_mm_id_iq3_s_f32")]] kernel mul_mm_id kernel_mul_mm_id; +template [[host_name("kernel_mul_mm_id_iq2_s_f32")]] kernel mul_mm_id kernel_mul_mm_id; +template [[host_name("kernel_mul_mm_id_iq1_s_f32")]] kernel mul_mm_id kernel_mul_mm_id; +template [[host_name("kernel_mul_mm_id_iq1_m_f32")]] kernel mul_mm_id kernel_mul_mm_id; +template [[host_name("kernel_mul_mm_id_iq4_nl_f32")]] kernel mul_mm_id kernel_mul_mm_id; +template [[host_name("kernel_mul_mm_id_iq4_xs_f32")]] kernel mul_mm_id kernel_mul_mm_id; + +template [[host_name("kernel_mul_mm_id_f32_f16")]] kernel mul_mm_id kernel_mul_mm_id; +template [[host_name("kernel_mul_mm_id_f16_f16")]] kernel mul_mm_id kernel_mul_mm_id; +template [[host_name("kernel_mul_mm_id_q4_0_f16")]] kernel mul_mm_id kernel_mul_mm_id; +template [[host_name("kernel_mul_mm_id_q4_1_f16")]] kernel mul_mm_id kernel_mul_mm_id; +template [[host_name("kernel_mul_mm_id_q5_0_f16")]] kernel mul_mm_id kernel_mul_mm_id; +template [[host_name("kernel_mul_mm_id_q5_1_f16")]] kernel mul_mm_id kernel_mul_mm_id; +template [[host_name("kernel_mul_mm_id_q8_0_f16")]] kernel mul_mm_id kernel_mul_mm_id; +template [[host_name("kernel_mul_mm_id_mxfp4_f16")]] kernel mul_mm_id kernel_mul_mm_id; +template [[host_name("kernel_mul_mm_id_q2_K_f16")]] kernel mul_mm_id kernel_mul_mm_id; +template [[host_name("kernel_mul_mm_id_q3_K_f16")]] kernel mul_mm_id kernel_mul_mm_id; +template [[host_name("kernel_mul_mm_id_q4_K_f16")]] kernel mul_mm_id kernel_mul_mm_id; +template [[host_name("kernel_mul_mm_id_q5_K_f16")]] kernel mul_mm_id kernel_mul_mm_id; +template [[host_name("kernel_mul_mm_id_q6_K_f16")]] kernel mul_mm_id kernel_mul_mm_id; +template [[host_name("kernel_mul_mm_id_iq2_xxs_f16")]] kernel mul_mm_id kernel_mul_mm_id; +template [[host_name("kernel_mul_mm_id_iq2_xs_f16")]] kernel mul_mm_id kernel_mul_mm_id; +template [[host_name("kernel_mul_mm_id_iq3_xxs_f16")]] kernel mul_mm_id kernel_mul_mm_id; +template [[host_name("kernel_mul_mm_id_iq3_s_f16")]] kernel mul_mm_id kernel_mul_mm_id; +template [[host_name("kernel_mul_mm_id_iq2_s_f16")]] kernel mul_mm_id kernel_mul_mm_id; +template [[host_name("kernel_mul_mm_id_iq1_s_f16")]] kernel mul_mm_id kernel_mul_mm_id; +template [[host_name("kernel_mul_mm_id_iq1_m_f16")]] kernel mul_mm_id kernel_mul_mm_id; +template [[host_name("kernel_mul_mm_id_iq4_nl_f16")]] kernel mul_mm_id kernel_mul_mm_id; +template [[host_name("kernel_mul_mm_id_iq4_xs_f16")]] kernel mul_mm_id kernel_mul_mm_id; + +// +// matrix-vector multiplication +// + +typedef void (kernel_mul_mv_disp_t)( + ggml_metal_kargs_mul_mv args, + device const char * src0, + device const char * src1, + device char * dst, + uint3 tgpig, + ushort tiisg); + +typedef void (kernel_mul_mv2_disp_t)( + ggml_metal_kargs_mul_mv args, + device const char * src0, + device const char * src1, + device char * dst, + threadgroup char * shmem, + uint3 tgpig, + ushort tiisg, + ushort sgitg); + +template +void mmv_fn( + ggml_metal_kargs_mul_mv args, + device const char * src0, + device const char * src1, + device char * dst, + threadgroup char * shmem, + uint3 tgpig, + ushort tiitg, + ushort tiisg, + ushort sgitg) { + disp_fn(args, src0, src1, dst, tgpig, tiisg); +} + +template +void mmv_fn( + ggml_metal_kargs_mul_mv args, + device const char * src0, + device const char * src1, + device char * dst, + threadgroup char * shmem, + uint3 tgpig, + ushort tiitg, + ushort tiisg, + ushort sgitg) { + disp_fn(args, src0, src1, dst, shmem, tgpig, tiisg, sgitg); +} + +typedef decltype(mmv_fn>) mul_mv_disp_fn_t; + +template +kernel void kernel_mul_mv_id( + constant ggml_metal_kargs_mul_mv_id & args, + device const char * src0s, + device const char * src1, + device char * dst, + device const char * ids, + threadgroup char * shmem [[threadgroup(0)]], + uint3 tgpig[[threadgroup_position_in_grid]], + ushort tiitg[[thread_index_in_threadgroup]], + ushort tiisg[[thread_index_in_simdgroup]], + ushort sgitg[[simdgroup_index_in_threadgroup]]) { + const int iid1 = tgpig.z/args.nei0; + const int idx = tgpig.z%args.nei0; + + tgpig.z = 0; + + const int32_t i02 = ((device const int32_t *) (ids + iid1*args.nbi1))[idx]; + + const int64_t i11 = idx % args.ne11; + const int64_t i12 = iid1; + + const int64_t i1 = idx; + const int64_t i2 = i12; + + device const char * src0_cur = src0s + i02*args.nb02; + device const char * src1_cur = src1 + i11*args.nb11 + i12*args.nb12; + + device char * dst_cur = dst + (i1*args.ne0 + i2*args.ne1*args.ne0)*sizeof(float); + + ggml_metal_kargs_mul_mv args0 = { + /*.ne00 =*/ args.ne00, + /*.ne01 =*/ args.ne01, + /*.ne02 =*/ 1, // args.ne02, + /*.nb00 =*/ args.nb00, + /*.nb01 =*/ args.nb01, + /*.nb02 =*/ args.nb02, + /*.nb03 =*/ args.nb02, // args.ne02 == 1 + /*.ne10 =*/ args.ne10, + /*.ne11 =*/ 1, // args.ne11, + /*.ne12 =*/ 1, // args.ne12, + /*.nb10 =*/ args.nb10, + /*.nb11 =*/ args.nb11, + /*.nb12 =*/ args.nb12, + /*.nb13 =*/ args.nb12, // ne12 == 1 + /*.ne0 =*/ args.ne0, + /*.ne1 =*/ 1, // args.ne1, + /*.nr0 =*/ args.nr0, + /*.r2 =*/ 1, + /*.r3 =*/ 1, + }; + + disp_fn( + args0, + /* src0 */ src0_cur, + /* src1 */ src1_cur, + /* dst */ dst_cur, + shmem, + tgpig, + tiitg, + tiisg, + sgitg); +} + +typedef decltype(kernel_mul_mv_id>>) kernel_mul_mv_id_t; + +typedef decltype(kernel_mul_mv_id>>) kernel_mul_mv_id_4_t; + +template [[host_name("kernel_mul_mv_id_f32_f32")]] kernel kernel_mul_mv_id_t kernel_mul_mv_id>>; +template [[host_name("kernel_mul_mv_id_f16_f32")]] kernel kernel_mul_mv_id_t kernel_mul_mv_id>>; +#if defined(GGML_METAL_HAS_BF16) +template [[host_name("kernel_mul_mv_id_bf16_f32")]] kernel kernel_mul_mv_id_t kernel_mul_mv_id>>; +#endif +template [[host_name("kernel_mul_mv_id_f32_f32_4")]] kernel kernel_mul_mv_id_4_t kernel_mul_mv_id>>; +template [[host_name("kernel_mul_mv_id_f16_f32_4")]] kernel kernel_mul_mv_id_4_t kernel_mul_mv_id>>; +#if defined(GGML_METAL_HAS_BF16) +template [[host_name("kernel_mul_mv_id_bf16_f32_4")]] kernel kernel_mul_mv_id_4_t kernel_mul_mv_id>>; +#endif + +template [[host_name("kernel_mul_mv_id_q8_0_f32")]] kernel kernel_mul_mv_id_t kernel_mul_mv_id>>; + +template [[host_name("kernel_mul_mv_id_q4_0_f32")]] kernel kernel_mul_mv_id_t kernel_mul_mv_id>>; +template [[host_name("kernel_mul_mv_id_q4_1_f32")]] kernel kernel_mul_mv_id_t kernel_mul_mv_id>>; +template [[host_name("kernel_mul_mv_id_q5_0_f32")]] kernel kernel_mul_mv_id_t kernel_mul_mv_id>>; +template [[host_name("kernel_mul_mv_id_q5_1_f32")]] kernel kernel_mul_mv_id_t kernel_mul_mv_id>>; + +template [[host_name("kernel_mul_mv_id_mxfp4_f32")]] kernel kernel_mul_mv_id_t kernel_mul_mv_id>>; + +template [[host_name("kernel_mul_mv_id_q2_K_f32")]] kernel kernel_mul_mv_id_t kernel_mul_mv_id>>; +template [[host_name("kernel_mul_mv_id_q3_K_f32")]] kernel kernel_mul_mv_id_t kernel_mul_mv_id>>; +template [[host_name("kernel_mul_mv_id_q4_K_f32")]] kernel kernel_mul_mv_id_t kernel_mul_mv_id>>; +template [[host_name("kernel_mul_mv_id_q5_K_f32")]] kernel kernel_mul_mv_id_t kernel_mul_mv_id>>; +template [[host_name("kernel_mul_mv_id_q6_K_f32")]] kernel kernel_mul_mv_id_t kernel_mul_mv_id>>; +template [[host_name("kernel_mul_mv_id_iq1_s_f32")]] kernel kernel_mul_mv_id_t kernel_mul_mv_id>>; +template [[host_name("kernel_mul_mv_id_iq1_m_f32")]] kernel kernel_mul_mv_id_t kernel_mul_mv_id>>; +template [[host_name("kernel_mul_mv_id_iq2_xxs_f32")]] kernel kernel_mul_mv_id_t kernel_mul_mv_id>>; +template [[host_name("kernel_mul_mv_id_iq2_xs_f32")]] kernel kernel_mul_mv_id_t kernel_mul_mv_id>>; +template [[host_name("kernel_mul_mv_id_iq3_xxs_f32")]] kernel kernel_mul_mv_id_t kernel_mul_mv_id>>; +template [[host_name("kernel_mul_mv_id_iq3_s_f32")]] kernel kernel_mul_mv_id_t kernel_mul_mv_id>>; +template [[host_name("kernel_mul_mv_id_iq2_s_f32")]] kernel kernel_mul_mv_id_t kernel_mul_mv_id>>; +template [[host_name("kernel_mul_mv_id_iq4_nl_f32")]] kernel kernel_mul_mv_id_t kernel_mul_mv_id>>; +template [[host_name("kernel_mul_mv_id_iq4_xs_f32")]] kernel kernel_mul_mv_id_t kernel_mul_mv_id>>; + +kernel void kernel_pool_2d_max_f32( + constant ggml_metal_kargs_pool_2d & args, + device const float * src0, + device float * dst, + uint gid[[thread_position_in_grid]]) { + + if (gid >= args.np) { + return; + } + + const int idx = gid; + const int I_HW = args.IH * args.IW; + const int O_HW = args.OH * args.OW; + const int nc = idx / O_HW; + const int cur_oh = idx % O_HW / args.OW; + const int cur_ow = idx % O_HW % args.OW; + + device const float * i_ptr = src0 + nc * I_HW; + device float * o_ptr = dst + nc * O_HW; + + const int start_h = cur_oh * args.s1 - args.p1; + const int bh = MAX(0, start_h); + const int eh = MIN(args.IH, start_h + args.k1); + const int start_w = cur_ow * args.s0 - args.p0; + const int bw = MAX(0, start_w); + const int ew = MIN(args.IW, start_w + args.k0); + + float res = -INFINITY; + + for (int i = bh; i < eh; i += 1) { + for (int j = bw; j < ew; j += 1) { + res = MAX(res, i_ptr[i * args.IW + j]); + } + } + + o_ptr[cur_oh * args.OW + cur_ow] = res; +} + +kernel void kernel_pool_2d_avg_f32( + constant ggml_metal_kargs_pool_2d & args, + device const float * src0, + device float * dst, + uint gid[[thread_position_in_grid]]) { + + if (gid >= args.np) { + return; + } + + const int idx = gid; + const int I_HW = args.IH * args.IW; + const int O_HW = args.OH * args.OW; + const int nc = idx / O_HW; + const int cur_oh = idx % O_HW / args.OW; + const int cur_ow = idx % O_HW % args.OW; + + device const float * i_ptr = src0 + nc * I_HW; + device float * o_ptr = dst + nc * O_HW; + + const int start_h = cur_oh * args.s1 - args.p1; + const int bh = MAX(0, start_h); + const int eh = MIN(args.IH, start_h + args.k1); + const int start_w = cur_ow * args.s0 - args.p0; + const int bw = MAX(0, start_w); + const int ew = MIN(args.IW, start_w + args.k0); + // const float scale = 1. / ((eh - bh) * (ew - bw)); + const float scale = 1. / (args.k0 * args.k1); + + float res = 0; + + for (int i = bh; i < eh; i += 1) { + for (int j = bw; j < ew; j += 1) { + float cur = i_ptr[i * args.IW + j]; + res += cur * scale; + } + } + + o_ptr[cur_oh * args.OW + cur_ow] = res; +} + + +kernel void kernel_pool_1d_max_f32( + constant ggml_metal_kargs_pool_1d & args, + device const float * src, + device float * dst, + uint gid [[thread_position_in_grid]] +) { + + if (gid >= args.np) { + return; + } + + const int ow = (int)gid % args.OW; + const int row = (int)gid / args.OW; + + const int base = ow * args.s0 - args.p0; + + float acc = -INFINITY; + + const int src_off = row * args.IW; + const int dst_off = row * args.OW; + + for (int ki = 0; ki < args.k0; ++ki) { + int j = base + ki; + if (j < 0 || j >= args.IW){ + continue; + } + float v = src[src_off + j]; + acc = max(acc, v); + } + + dst[dst_off + ow] = acc; +} + +kernel void kernel_pool_1d_avg_f32( + constant ggml_metal_kargs_pool_1d & args, + device const float * src, + device float * dst, + uint gid [[thread_position_in_grid]] +) { + + if (gid >= args.np) { + return; + } + + const int ow = (int)gid % args.OW; + const int row = (int)gid / args.OW; + + const int base = ow * args.s0 - args.p0; + + float acc = 0.0f; + int cnt = 0; + + const int src_off = row * args.IW; + const int dst_off = row * args.OW; + + for (int ki = 0; ki < args.k0; ++ki) { + const int j = base + ki; + if (j < 0 || j >= args.IW) { + continue; + } + acc += src[src_off + j]; + cnt += 1; + } + + dst[dst_off + ow] = (cnt > 0) ? (acc / (float)cnt) : 0.0f; +} + +kernel void kernel_opt_step_adamw_f32( + constant ggml_metal_kargs_opt_step_adamw & args, + device float * x, + device const float * g, + device float * g_m, + device float * g_v, + device const float * pars, + uint gid[[thread_position_in_grid]]) { + + if (gid >= args.np) { + return; + } + + const float alpha = pars[0]; + const float beta1 = pars[1]; + const float beta2 = pars[2]; + const float eps = pars[3]; + const float wd = pars[4]; + const float beta1h = pars[5]; + const float beta2h = pars[6]; + + const float gi = g[gid]; + const float gmi = g_m[gid] * beta1 + gi * (1.0f - beta1); + const float gvi = g_v[gid] * beta2 + gi * gi * (1.0f - beta2); + + g_m[gid] = gmi; + g_v[gid] = gvi; + + const float mh = gmi * beta1h; + const float vh = sqrt(gvi * beta2h) + eps; + + x[gid] = x[gid] * (1.0f - alpha * wd) - alpha * mh / vh; +} + +kernel void kernel_opt_step_sgd_f32( + constant ggml_metal_kargs_opt_step_sgd & args, + device float * x, + device const float * g, + device const float * pars, + uint gid[[thread_position_in_grid]]) { + + if (gid >= args.np) { + return; + } + + x[gid] = x[gid] * (1.0f - pars[0] * pars[1]) - pars[0] * g[gid]; +} + +template +kernel void kernel_memset( + constant ggml_metal_kargs_memset & args, + device T * dst, + uint tpig[[thread_position_in_grid]]) { + dst[tpig] = args.val; +} + +typedef decltype(kernel_memset) kernel_memset_t; + +template [[host_name("kernel_memset_i64")]] kernel kernel_memset_t kernel_memset; + +constant short FC_count_equal_nsg [[function_constant(FC_COUNT_EQUAL + 0)]]; + +template +kernel void kernel_count_equal( + constant ggml_metal_kargs_count_equal & args, + device const char * src0, + device const char * src1, + device atomic_int * dst, + threadgroup int32_t * shmem_i32 [[threadgroup(0)]], + uint3 tgpig[[threadgroup_position_in_grid]], + ushort3 tpitg[[thread_position_in_threadgroup]], + ushort sgitg[[simdgroup_index_in_threadgroup]], + ushort tiisg[[thread_index_in_simdgroup]], + ushort3 ntg[[threads_per_threadgroup]]) { + const short NSG = FC_count_equal_nsg; + + const int i3 = tgpig.z; + const int i2 = tgpig.y; + const int i1 = tgpig.x; + + if (i3 >= args.ne03 || i2 >= args.ne02 || i1 >= args.ne01) { + return; + } + + int sum = 0; + + device const char * base0 = src0 + i1*args.nb01 + i2*args.nb02 + i3*args.nb03; + device const char * base1 = src1 + i1*args.nb11 + i2*args.nb12 + i3*args.nb13; + + for (int64_t i0 = tpitg.x; i0 < args.ne00; i0 += ntg.x) { + const T v0 = *(device const T *)(base0 + i0*args.nb00); + const T v1 = *(device const T *)(base1 + i0*args.nb10); + sum += (v0 == v1); + } + + sum = simd_sum(sum); + + if (tiisg == 0) { + shmem_i32[sgitg] = sum; + } + + threadgroup_barrier(mem_flags::mem_threadgroup); + + if (sgitg == 0) { + float v = 0.0f; + if (tpitg.x < NSG) { + v = shmem_i32[tpitg.x]; + } + + float total = simd_sum(v); + if (tpitg.x == 0) { + atomic_fetch_add_explicit(dst, (int32_t) total, memory_order_relaxed); + } + } +} + +typedef decltype(kernel_count_equal) kernel_count_equal_t; + +template [[host_name("kernel_count_equal_i32")]] kernel kernel_count_equal_t kernel_count_equal; diff --git a/libs/ggml/src/backend/metal/mod.rs b/libs/ggml/src/backend/metal/mod.rs new file mode 100644 index 0000000000..3106172122 --- /dev/null +++ b/libs/ggml/src/backend/metal/mod.rs @@ -0,0 +1,9 @@ +mod compat; +mod compiled; +mod runtime; +mod selector; + +pub use compat::*; +pub use compiled::*; +pub use runtime::*; +pub use selector::*; diff --git a/libs/ggml/src/backend/metal/runtime.rs b/libs/ggml/src/backend/metal/runtime.rs new file mode 100644 index 0000000000..099989cee8 --- /dev/null +++ b/libs/ggml/src/backend/metal/runtime.rs @@ -0,0 +1,1246 @@ +#[cfg(not(target_os = "macos"))] +mod imp { + use crate::backend::{BackendCapabilities, BackendInfo, BackendKind}; + + pub type MetalResult = Result; + + #[derive(Clone, Copy, Debug, Eq, Hash, PartialEq)] + pub struct MetalSize { + pub width: u64, + pub height: u64, + pub depth: u64, + } + + #[derive(Clone, Copy, Debug)] + pub struct MetalBufferBindingRef<'a> { + pub index: u64, + pub buffer: &'a MetalBuffer, + pub offset_bytes: usize, + } + + #[derive(Clone, Copy, Debug, Eq, Hash, PartialEq)] + pub enum BufferStorageMode { + Shared, + Private, + } + + #[derive(Clone, Copy, Debug, Eq, Hash, PartialEq)] + pub enum FunctionConstantValue { + Int32(i32), + Int16(i16), + Bool(bool), + } + + #[derive(Clone, Copy, Debug, Eq, Hash, PartialEq)] + pub struct FunctionConstant { + pub idx: i32, + pub value: FunctionConstantValue, + } + + #[derive(Clone, Debug, Eq, PartialEq)] + pub struct MetalPipelineDescriptor { + pub cache_name: String, + pub base_name: String, + pub constants: Vec, + pub smem_bytes: usize, + pub nr0: i32, + pub nr1: i32, + pub nsg: i32, + } + + #[derive(Clone, Copy, Debug, Default, Eq, PartialEq)] + pub struct MetalDeviceFeatures { + pub has_bfloat: bool, + pub has_tensor: bool, + pub has_simdgroup_mm: bool, + } + + #[derive(Clone, Debug)] + pub struct MetalBuffer { + size_bytes: usize, + storage: BufferStorageMode, + } + + impl MetalBuffer { + pub fn size_bytes(&self) -> usize { + self.size_bytes + } + + pub fn storage(&self) -> BufferStorageMode { + self.storage + } + } + + #[derive(Clone, Debug)] + pub struct MetalPipeline { + pub smem_bytes: usize, + pub nr0: i32, + pub nr1: i32, + pub nsg: i32, + pub max_threads_per_threadgroup: u64, + } + + pub struct MetalRuntime { + info: BackendInfo, + features: MetalDeviceFeatures, + } + + impl MetalRuntime { + pub fn is_available() -> bool { + false + } + + pub fn new() -> MetalResult { + Err("Metal runtime is only available on macOS in this port".to_string()) + } + + pub fn backend_info(&self) -> &BackendInfo { + &self.info + } + + pub fn features(&self) -> MetalDeviceFeatures { + self.features + } + + pub fn create_buffer_with_bytes( + &self, + _bytes: &[u8], + _storage: BufferStorageMode, + ) -> MetalResult { + Err("Metal runtime is unavailable on this target".to_string()) + } + + pub fn create_buffer( + &self, + _size_bytes: usize, + _storage: BufferStorageMode, + ) -> MetalResult { + Err("Metal runtime is unavailable on this target".to_string()) + } + + pub fn get_or_compile_pipeline( + &self, + _desc: &MetalPipelineDescriptor, + ) -> MetalResult { + Err("Metal runtime is unavailable on this target".to_string()) + } + + pub fn read_buffer( + &self, + _buffer: &MetalBuffer, + _len_bytes: usize, + ) -> MetalResult> { + Err("Metal runtime is unavailable on this target".to_string()) + } + + pub fn read_buffer_range( + &self, + _buffer: &MetalBuffer, + _offset_bytes: usize, + _len_bytes: usize, + ) -> MetalResult> { + Err("Metal runtime is unavailable on this target".to_string()) + } + + pub fn write_buffer( + &self, + _buffer: &MetalBuffer, + _offset_bytes: usize, + _bytes: &[u8], + ) -> MetalResult<()> { + Err("Metal runtime is unavailable on this target".to_string()) + } + + pub fn dispatch_compute( + &self, + _pipeline: &MetalPipeline, + _args_bytes: &[u8], + _buffers: &[MetalBufferBindingRef<'_>], + _threadgroup_memory_lengths: &[(u64, usize)], + _threadgroups: MetalSize, + _threads_per_threadgroup: MetalSize, + ) -> MetalResult<()> { + Err("Metal runtime is unavailable on this target".to_string()) + } + + pub fn wait_idle(&self) -> MetalResult<()> { + Err("Metal runtime is unavailable on this target".to_string()) + } + } + + impl Default for MetalRuntime { + fn default() -> Self { + Self { + info: BackendInfo { + kind: BackendKind::Metal, + name: "Unavailable".to_string(), + description: "Metal runtime stub".to_string(), + capabilities: BackendCapabilities::default(), + }, + features: MetalDeviceFeatures::default(), + } + } + } +} + +#[cfg(target_os = "macos")] +mod imp { + use crate::backend::{BackendCapabilities, BackendInfo, BackendKind}; + use makepad_objc_sys::runtime::{nil, ObjcId, Object, YES}; + use makepad_objc_sys::{class, msg_send, sel, sel_impl}; + use std::cell::RefCell; + use std::collections::HashMap; + use std::ffi::{c_char, c_void, CStr}; + use std::ptr::NonNull; + + pub type MetalResult = Result; + + #[derive(Clone, Copy, Debug, Eq, Hash, PartialEq)] + pub struct MetalSize { + pub width: u64, + pub height: u64, + pub depth: u64, + } + + #[derive(Clone, Copy, Debug)] + pub struct MetalBufferBindingRef<'a> { + pub index: u64, + pub buffer: &'a MetalBuffer, + pub offset_bytes: usize, + } + + const UTF8_ENCODING: u64 = 4; + const MTL_RESOURCE_STORAGE_MODE_SHARED: u64 = 0; + const MTL_RESOURCE_OPTIONS_STORAGE_MODE_PRIVATE: u64 = 32; + const MTL_STORAGE_MODE_PRIVATE: u64 = 2; + const MTL_GPU_FAMILY_APPLE6: u64 = 1006; + const MTL_GPU_FAMILY_APPLE7: u64 = 1007; + const MTL_GPU_FAMILY_METAL3: u64 = 5001; + const MTL_GPU_FAMILY_METAL4: u64 = 5002; + const MTL_DATA_TYPE_INT: u64 = 29; + const MTL_DATA_TYPE_SHORT: u64 = 37; + const MTL_DATA_TYPE_BOOL: u64 = 53; + + const GGML_METAL_SOURCE_RAW: &str = include_str!("ggml/ggml-metal.metal"); + const GGML_COMMON_H: &str = include_str!("ggml/ggml-common.h"); + const GGML_METAL_IMPL_H: &str = include_str!("ggml/ggml-metal-impl.h"); + const GGML_METALLIB_BYTES: &[u8] = include_bytes!(env!("MAKEPAD_GGML_METALLIB")); + + #[link(name = "Metal", kind = "framework")] + extern "C" { + fn MTLCreateSystemDefaultDevice() -> ObjcId; + fn MTLCopyAllDevices() -> ObjcId; + } + + #[link(name = "Foundation", kind = "framework")] + extern "C" {} + + #[repr(C)] + #[derive(Copy, Clone)] + struct MTLSize { + width: u64, + height: u64, + depth: u64, + } + + #[derive(Clone, Copy, Debug, Eq, Hash, PartialEq)] + pub enum BufferStorageMode { + Shared, + Private, + } + + #[derive(Clone, Copy, Debug, Eq, Hash, PartialEq)] + pub enum FunctionConstantValue { + Int32(i32), + Int16(i16), + Bool(bool), + } + + #[derive(Clone, Copy, Debug, Eq, Hash, PartialEq)] + pub struct FunctionConstant { + pub idx: i32, + pub value: FunctionConstantValue, + } + + #[derive(Clone, Debug, Eq, PartialEq)] + pub struct MetalPipelineDescriptor { + pub cache_name: String, + pub base_name: String, + pub constants: Vec, + pub smem_bytes: usize, + pub nr0: i32, + pub nr1: i32, + pub nsg: i32, + } + + #[derive(Clone, Copy, Debug, Default, Eq, PartialEq)] + pub struct MetalDeviceFeatures { + pub has_bfloat: bool, + pub has_tensor: bool, + pub has_simdgroup_mm: bool, + } + + #[derive(Debug)] + struct StrongId(NonNull); + + impl StrongId { + unsafe fn from_owned(id: ObjcId) -> Option { + NonNull::new(id).map(Self) + } + + unsafe fn from_unowned(id: ObjcId) -> Option { + if id.is_null() { + return None; + } + let _: () = msg_send![id, retain]; + NonNull::new(id).map(Self) + } + + fn as_id(&self) -> ObjcId { + self.0.as_ptr() + } + } + + impl Clone for StrongId { + fn clone(&self) -> Self { + unsafe { + let _: () = msg_send![self.as_id(), retain]; + } + Self(self.0) + } + } + + impl Drop for StrongId { + fn drop(&mut self) { + unsafe { + let _: () = msg_send![self.0.as_ptr(), release]; + } + } + } + + struct AutoreleasePool(ObjcId); + + impl AutoreleasePool { + fn new() -> Self { + let pool: ObjcId = unsafe { msg_send![class!(NSAutoreleasePool), new] }; + Self(pool) + } + } + + impl Drop for AutoreleasePool { + fn drop(&mut self) { + if !self.0.is_null() { + unsafe { + let _: () = msg_send![self.0, release]; + } + } + } + } + + fn nsstring_to_string(ns_string: ObjcId) -> String { + if ns_string.is_null() { + return String::new(); + } + unsafe { + let utf8_ptr: *const c_char = msg_send![ns_string, UTF8String]; + if utf8_ptr.is_null() { + return String::new(); + } + CStr::from_ptr(utf8_ptr).to_string_lossy().into_owned() + } + } + + fn str_to_nsstring_owned(s: &str) -> ObjcId { + unsafe { + let ns_string: ObjcId = msg_send![class!(NSString), alloc]; + if ns_string.is_null() { + return nil; + } + msg_send![ + ns_string, + initWithBytes: s.as_ptr() as *const c_void + length: s.len() as u64 + encoding: UTF8_ENCODING + ] + } + } + + fn ns_error_to_string(error: ObjcId) -> String { + if error.is_null() { + return "unknown Metal error".to_string(); + } + unsafe { + let desc: ObjcId = msg_send![error, localizedDescription]; + nsstring_to_string(desc) + } + } + + fn device_supports_family(device: ObjcId, family: u64) -> bool { + unsafe { msg_send![device, supportsFamily: family] } + } + + fn metal_compile_feature_macros(device: ObjcId) -> MetalDeviceFeatures { + MetalDeviceFeatures { + has_bfloat: device_supports_family(device, MTL_GPU_FAMILY_METAL3) + || device_supports_family(device, MTL_GPU_FAMILY_APPLE6), + has_tensor: device_supports_family(device, MTL_GPU_FAMILY_METAL4), + has_simdgroup_mm: device_supports_family(device, MTL_GPU_FAMILY_APPLE7), + } + } + + fn read_text_with_fallback(paths: &[&str], fallback: &str) -> String { + for path in paths { + if let Ok(text) = std::fs::read_to_string(path) { + return text; + } + } + fallback.to_string() + } + + fn build_ggml_source() -> String { + let mut src = read_text_with_fallback( + &[ + concat!( + env!("CARGO_MANIFEST_DIR"), + "/src/backend/metal/ggml/ggml-metal.metal" + ), + "libs/ggml/src/backend/metal/ggml/ggml-metal.metal", + ], + GGML_METAL_SOURCE_RAW, + ); + let common_h = read_text_with_fallback( + &[ + concat!( + env!("CARGO_MANIFEST_DIR"), + "/src/backend/metal/ggml/ggml-common.h" + ), + "libs/ggml/src/backend/metal/ggml/ggml-common.h", + ], + GGML_COMMON_H, + ); + let impl_h = read_text_with_fallback( + &[ + concat!( + env!("CARGO_MANIFEST_DIR"), + "/src/backend/metal/ggml/ggml-metal-impl.h" + ), + "libs/ggml/src/backend/metal/ggml/ggml-metal-impl.h", + ], + GGML_METAL_IMPL_H, + ); + + src = src.replace("__embed_ggml-common.h__", &common_h); + src = src.replace("#include \"ggml-common.h\"", &common_h); + src = src.replace("#include \"ggml-metal-impl.h\"", &impl_h); + src + } + + #[derive(Clone, Debug)] + pub struct MetalBuffer { + obj: StrongId, + size_bytes: usize, + storage: BufferStorageMode, + } + + impl MetalBuffer { + pub fn size_bytes(&self) -> usize { + self.size_bytes + } + + pub fn storage(&self) -> BufferStorageMode { + self.storage + } + + pub(crate) fn as_id(&self) -> ObjcId { + self.obj.as_id() + } + } + + #[derive(Clone, Debug)] + pub struct MetalPipeline { + #[allow(dead_code)] + obj: StrongId, + pub smem_bytes: usize, + pub nr0: i32, + pub nr1: i32, + pub nsg: i32, + pub max_threads_per_threadgroup: u64, + } + + impl MetalPipeline { + #[allow(dead_code)] + pub(crate) fn as_id(&self) -> ObjcId { + self.obj.as_id() + } + } + + struct MetalContext { + device: StrongId, + command_queue: StrongId, + library: StrongId, + pipeline_cache: HashMap, + last_command_buffer: Option, + } + + pub struct MetalRuntime { + ctx: RefCell, + info: BackendInfo, + features: MetalDeviceFeatures, + } + + impl MetalRuntime { + pub fn is_available() -> bool { + MetalContext::create_device().is_some() + } + + pub fn new() -> MetalResult { + let _pool = AutoreleasePool::new(); + let device = MetalContext::create_device() + .ok_or_else(|| "unable to create Metal device".to_string())?; + + let name_obj: ObjcId = unsafe { msg_send![device.as_id(), name] }; + let name = nsstring_to_string(name_obj); + let max_buffer_size: u64 = unsafe { msg_send![device.as_id(), maxBufferLength] }; + let features = metal_compile_feature_macros(device.as_id()); + + let command_queue_obj: ObjcId = unsafe { msg_send![device.as_id(), newCommandQueue] }; + let command_queue = unsafe { StrongId::from_owned(command_queue_obj) } + .ok_or_else(|| "newCommandQueue returned nil".to_string())?; + + let library = match MetalContext::load_library_from_metallib(device.as_id()) { + Ok(Some(lib)) => lib, + Ok(None) => { + let source = build_ggml_source(); + MetalContext::compile_library(device.as_id(), &source)? + } + Err(err) => { + eprintln!( + "[ggml][metal] precompiled metallib load failed, compiling source: {}", + err + ); + let source = build_ggml_source(); + MetalContext::compile_library(device.as_id(), &source)? + } + }; + + let info = BackendInfo { + kind: BackendKind::Metal, + name: name.clone(), + description: format!("Apple Metal device '{}'", name), + capabilities: BackendCapabilities { + bf16: features.has_bfloat, + tensor_cores: features.has_tensor, + max_buffer_size: usize::try_from(max_buffer_size).ok(), + max_threadgroup_memory: None, + subgroup_width: None, + asynchronous: false, + host_buffer: false, + buffer_from_host_ptr: false, + events: false, + }, + }; + + Ok(Self { + ctx: RefCell::new(MetalContext { + device, + command_queue, + library, + pipeline_cache: HashMap::new(), + last_command_buffer: None, + }), + info, + features, + }) + } + + pub fn backend_info(&self) -> &BackendInfo { + &self.info + } + + pub fn features(&self) -> MetalDeviceFeatures { + self.features + } + + pub fn create_buffer_with_bytes( + &self, + bytes: &[u8], + storage: BufferStorageMode, + ) -> MetalResult { + if bytes.is_empty() { + return self.create_buffer(0, storage); + } + let ctx = self.ctx.borrow(); + match storage { + BufferStorageMode::Shared => { + let obj = ctx.new_buffer_with_bytes(bytes)?; + Ok(MetalBuffer { + obj, + size_bytes: bytes.len(), + storage, + }) + } + BufferStorageMode::Private => { + let dst = ctx.new_buffer_with_length_private(bytes.len().max(1))?; + let staging = ctx.new_buffer_with_bytes(bytes)?; + ctx.copy_between_buffers(staging.as_id(), dst.as_id(), bytes.len())?; + Ok(MetalBuffer { + obj: dst, + size_bytes: bytes.len(), + storage, + }) + } + } + } + + pub fn create_buffer( + &self, + size_bytes: usize, + storage: BufferStorageMode, + ) -> MetalResult { + let ctx = self.ctx.borrow(); + let size_bytes = size_bytes.max(1); + let obj = match storage { + BufferStorageMode::Shared => ctx.new_buffer_with_length(size_bytes)?, + BufferStorageMode::Private => ctx.new_buffer_with_length_private(size_bytes)?, + }; + Ok(MetalBuffer { + obj, + size_bytes, + storage, + }) + } + + pub fn get_or_compile_pipeline( + &self, + desc: &MetalPipelineDescriptor, + ) -> MetalResult { + let mut ctx = self.ctx.borrow_mut(); + if let Some(pipeline) = ctx.pipeline_cache.get(&desc.cache_name) { + return Ok(pipeline.clone()); + } + + let obj = ctx.compile_pipeline(&desc.base_name, &desc.constants)?; + let max_threads_per_threadgroup = MetalContext::pipeline_max_threads(obj.as_id()); + let pipeline = MetalPipeline { + obj, + smem_bytes: desc.smem_bytes, + nr0: desc.nr0, + nr1: desc.nr1, + nsg: desc.nsg, + max_threads_per_threadgroup, + }; + ctx.pipeline_cache + .insert(desc.cache_name.clone(), pipeline.clone()); + Ok(pipeline) + } + + pub fn read_buffer(&self, buffer: &MetalBuffer, len_bytes: usize) -> MetalResult> { + let ctx = self.ctx.borrow(); + if len_bytes > buffer.size_bytes { + return Err(format!( + "requested read of {} bytes exceeds buffer size {}", + len_bytes, buffer.size_bytes + )); + } + ctx.read_buffer_bytes(buffer.as_id(), len_bytes) + } + + pub fn read_buffer_range( + &self, + buffer: &MetalBuffer, + offset_bytes: usize, + len_bytes: usize, + ) -> MetalResult> { + let ctx = self.ctx.borrow(); + if offset_bytes > buffer.size_bytes + || len_bytes > buffer.size_bytes.saturating_sub(offset_bytes) + { + return Err(format!( + "requested read of {} bytes at offset {} exceeds buffer size {}", + len_bytes, offset_bytes, buffer.size_bytes + )); + } + ctx.read_buffer_bytes_range(buffer.as_id(), offset_bytes, len_bytes) + } + + pub fn write_buffer( + &self, + buffer: &MetalBuffer, + offset_bytes: usize, + bytes: &[u8], + ) -> MetalResult<()> { + let ctx = self.ctx.borrow(); + if offset_bytes > buffer.size_bytes + || bytes.len() > buffer.size_bytes.saturating_sub(offset_bytes) + { + return Err(format!( + "requested write of {} bytes at offset {} exceeds buffer size {}", + bytes.len(), + offset_bytes, + buffer.size_bytes + )); + } + ctx.write_buffer_bytes(buffer.as_id(), offset_bytes, bytes) + } + + pub fn dispatch_compute( + &self, + pipeline: &MetalPipeline, + args_bytes: &[u8], + buffers: &[MetalBufferBindingRef<'_>], + threadgroup_memory_lengths: &[(u64, usize)], + threadgroups: MetalSize, + threads_per_threadgroup: MetalSize, + ) -> MetalResult<()> { + self.ctx.borrow_mut().dispatch_compute( + pipeline, + args_bytes, + buffers, + threadgroup_memory_lengths, + threadgroups, + threads_per_threadgroup, + ) + } + + pub fn wait_idle(&self) -> MetalResult<()> { + self.ctx.borrow().wait_queue_idle() + } + } + + impl MetalContext { + fn create_device() -> Option { + unsafe { + let dev = MTLCreateSystemDefaultDevice(); + if let Some(dev) = StrongId::from_owned(dev) { + return Some(dev); + } + + let all = MTLCopyAllDevices(); + if all.is_null() { + return None; + } + + let count: u64 = msg_send![all, count]; + let first: ObjcId = if count > 0 { + msg_send![all, objectAtIndex: 0u64] + } else { + nil + }; + let _: () = msg_send![all, release]; + + StrongId::from_unowned(first) + } + } + + fn load_library_from_metallib(device: ObjcId) -> MetalResult> { + if GGML_METALLIB_BYTES.is_empty() { + return Ok(None); + } + + let _pool = AutoreleasePool::new(); + let data_obj: ObjcId = unsafe { + msg_send![ + class!(NSData), + dataWithBytes: GGML_METALLIB_BYTES.as_ptr() as *const c_void + length: GGML_METALLIB_BYTES.len() as u64 + ] + }; + if data_obj.is_null() { + return Err("NSData::dataWithBytes returned nil".to_string()); + } + + let mut error: ObjcId = nil; + let library_obj: ObjcId = + unsafe { msg_send![device, newLibraryWithData: data_obj error: &mut error] }; + if library_obj.is_null() { + return Err(format!( + "newLibraryWithData failed: {}", + ns_error_to_string(error) + )); + } + + let library = unsafe { StrongId::from_owned(library_obj) } + .ok_or_else(|| "newLibraryWithData returned nil".to_string())?; + Ok(Some(library)) + } + + fn compile_library(device: ObjcId, source: &str) -> MetalResult { + let _pool = AutoreleasePool::new(); + + let options_obj: ObjcId = unsafe { msg_send![class!(MTLCompileOptions), new] }; + let options = unsafe { StrongId::from_owned(options_obj) } + .ok_or_else(|| "MTLCompileOptions::new returned nil".to_string())?; + unsafe { + let _: () = msg_send![options.as_id(), setFastMathEnabled: YES]; + } + + let features = metal_compile_feature_macros(device); + if features.has_bfloat || features.has_tensor { + let prep_obj: ObjcId = + unsafe { msg_send![class!(NSMutableDictionary), dictionary] }; + if !prep_obj.is_null() { + if features.has_bfloat { + let key = unsafe { + StrongId::from_owned(str_to_nsstring_owned("GGML_METAL_HAS_BF16")) + } + .ok_or_else(|| "failed to build metal macro key".to_string())?; + let val = unsafe { StrongId::from_owned(str_to_nsstring_owned("1")) } + .ok_or_else(|| "failed to build metal macro value".to_string())?; + unsafe { + let _: () = + msg_send![prep_obj, setObject: val.as_id() forKey: key.as_id()]; + } + } + if features.has_tensor { + let key = unsafe { + StrongId::from_owned(str_to_nsstring_owned("GGML_METAL_HAS_TENSOR")) + } + .ok_or_else(|| "failed to build metal macro key".to_string())?; + let val = unsafe { StrongId::from_owned(str_to_nsstring_owned("1")) } + .ok_or_else(|| "failed to build metal macro value".to_string())?; + unsafe { + let _: () = + msg_send![prep_obj, setObject: val.as_id() forKey: key.as_id()]; + } + } + unsafe { + let _: () = msg_send![options.as_id(), setPreprocessorMacros: prep_obj]; + } + } + } + + let source_obj = unsafe { StrongId::from_owned(str_to_nsstring_owned(source)) } + .ok_or_else(|| "failed to create NSString for Metal source".to_string())?; + + let mut error: ObjcId = nil; + let library_obj: ObjcId = unsafe { + msg_send![ + device, + newLibraryWithSource: source_obj.as_id() + options: options.as_id() + error: &mut error + ] + }; + + unsafe { StrongId::from_owned(library_obj) }.ok_or_else(|| { + format!("newLibraryWithSource failed: {}", ns_error_to_string(error)) + }) + } + + fn new_buffer_with_bytes(&self, bytes: &[u8]) -> MetalResult { + let obj: ObjcId = unsafe { + msg_send![ + self.device.as_id(), + newBufferWithBytes: bytes.as_ptr() as *const c_void + length: bytes.len().max(1) as u64 + options: MTL_RESOURCE_STORAGE_MODE_SHARED + ] + }; + unsafe { StrongId::from_owned(obj) } + .ok_or_else(|| format!("newBufferWithBytes failed for {} bytes", bytes.len())) + } + + fn new_buffer_with_length(&self, byte_len: usize) -> MetalResult { + let obj: ObjcId = unsafe { + msg_send![ + self.device.as_id(), + newBufferWithLength: byte_len as u64 + options: MTL_RESOURCE_STORAGE_MODE_SHARED + ] + }; + unsafe { StrongId::from_owned(obj) } + .ok_or_else(|| format!("newBufferWithLength failed for {} bytes", byte_len)) + } + + fn new_buffer_with_length_private(&self, byte_len: usize) -> MetalResult { + let obj: ObjcId = unsafe { + msg_send![ + self.device.as_id(), + newBufferWithLength: byte_len as u64 + options: MTL_RESOURCE_OPTIONS_STORAGE_MODE_PRIVATE + ] + }; + unsafe { StrongId::from_owned(obj) }.ok_or_else(|| { + format!("newBufferWithLength(private) failed for {} bytes", byte_len) + }) + } + + fn buffer_length_bytes(&self, buffer: ObjcId) -> MetalResult { + let len_u64: u64 = unsafe { msg_send![buffer, length] }; + usize::try_from(len_u64).map_err(|_| format!("buffer length too large: {}", len_u64)) + } + + fn copy_between_buffers( + &self, + src_buffer: ObjcId, + dst_buffer: ObjcId, + len_bytes: usize, + ) -> MetalResult<()> { + self.copy_between_buffers_ranges(src_buffer, 0, dst_buffer, 0, len_bytes) + } + + fn copy_between_buffers_ranges( + &self, + src_buffer: ObjcId, + src_offset: usize, + dst_buffer: ObjcId, + dst_offset: usize, + len_bytes: usize, + ) -> MetalResult<()> { + let len_bytes = len_bytes.max(1); + let src_len = self.buffer_length_bytes(src_buffer)?; + let dst_len = self.buffer_length_bytes(dst_buffer)?; + if src_offset > src_len + || dst_offset > dst_len + || len_bytes > src_len.saturating_sub(src_offset) + || len_bytes > dst_len.saturating_sub(dst_offset) + { + return Err(format!( + "buffer copy exceeds bounds: src_offset={}, dst_offset={}, len={}, src_len={}, dst_len={}", + src_offset, dst_offset, len_bytes, src_len, dst_len + )); + } + + let command_buffer_obj: ObjcId = + unsafe { msg_send![self.command_queue.as_id(), commandBuffer] }; + let command_buffer = unsafe { StrongId::from_unowned(command_buffer_obj) } + .ok_or_else(|| "commandBuffer returned nil".to_string())?; + + let blit_encoder_obj: ObjcId = + unsafe { msg_send![command_buffer.as_id(), blitCommandEncoder] }; + let blit_encoder = unsafe { StrongId::from_unowned(blit_encoder_obj) } + .ok_or_else(|| "blitCommandEncoder returned nil".to_string())?; + + unsafe { + let _: () = msg_send![ + blit_encoder.as_id(), + copyFromBuffer: src_buffer + sourceOffset: src_offset as u64 + toBuffer: dst_buffer + destinationOffset: dst_offset as u64 + size: len_bytes as u64 + ]; + let _: () = msg_send![blit_encoder.as_id(), endEncoding]; + let _: () = msg_send![command_buffer.as_id(), commit]; + let _: () = msg_send![command_buffer.as_id(), waitUntilCompleted]; + } + + let status: u64 = unsafe { msg_send![command_buffer.as_id(), status] }; + if status == 5 { + let error: ObjcId = unsafe { msg_send![command_buffer.as_id(), error] }; + return Err(format!( + "Metal command buffer error (buffer copy): {}", + ns_error_to_string(error) + )); + } + + Ok(()) + } + + fn copy_buffer_to_shared_staging( + &self, + src_buffer: ObjcId, + len_bytes: usize, + ) -> MetalResult { + let dst = self.new_buffer_with_length(len_bytes)?; + self.copy_between_buffers(src_buffer, dst.as_id(), len_bytes)?; + Ok(dst) + } + + fn copy_buffer_range_to_shared_staging( + &self, + src_buffer: ObjcId, + src_offset: usize, + len_bytes: usize, + ) -> MetalResult { + let dst = self.new_buffer_with_length(len_bytes.max(1))?; + self.copy_between_buffers_ranges(src_buffer, src_offset, dst.as_id(), 0, len_bytes)?; + Ok(dst) + } + + fn read_buffer_bytes(&self, buffer: ObjcId, len_bytes: usize) -> MetalResult> { + self.read_buffer_bytes_range(buffer, 0, len_bytes) + } + + fn read_buffer_bytes_range( + &self, + buffer: ObjcId, + offset_bytes: usize, + len_bytes: usize, + ) -> MetalResult> { + self.wait_queue_idle()?; + let cap = self.buffer_length_bytes(buffer)?; + if offset_bytes > cap || len_bytes > cap.saturating_sub(offset_bytes) { + return Err(format!( + "requested read of {} bytes at offset {} exceeds buffer size {}", + len_bytes, offset_bytes, cap + )); + } + + let (readable, readable_offset) = { + let storage_mode: u64 = unsafe { msg_send![buffer, storageMode] }; + if storage_mode == MTL_STORAGE_MODE_PRIVATE { + ( + self.copy_buffer_range_to_shared_staging(buffer, offset_bytes, len_bytes)?, + 0usize, + ) + } else { + ( + unsafe { StrongId::from_unowned(buffer) } + .ok_or_else(|| "buffer handle became invalid".to_string())?, + offset_bytes, + ) + } + }; + + let ptr: *const u8 = unsafe { msg_send![readable.as_id(), contents] }; + if ptr.is_null() { + return Err("buffer contents returned null".to_string()); + } + + let mut out = vec![0u8; len_bytes]; + unsafe { + std::ptr::copy_nonoverlapping( + ptr.add(readable_offset), + out.as_mut_ptr(), + len_bytes, + ); + } + Ok(out) + } + + fn write_buffer_bytes( + &self, + buffer: ObjcId, + offset_bytes: usize, + bytes: &[u8], + ) -> MetalResult<()> { + let cap = self.buffer_length_bytes(buffer)?; + if offset_bytes > cap || bytes.len() > cap.saturating_sub(offset_bytes) { + return Err(format!( + "requested write of {} bytes at offset {} exceeds buffer size {}", + bytes.len(), + offset_bytes, + cap + )); + } + + let storage_mode: u64 = unsafe { msg_send![buffer, storageMode] }; + if storage_mode == MTL_STORAGE_MODE_PRIVATE { + let staging = self.new_buffer_with_bytes(bytes)?; + self.copy_between_buffers_ranges( + staging.as_id(), + 0, + buffer, + offset_bytes, + bytes.len(), + )?; + return Ok(()); + } + + let ptr: *mut u8 = unsafe { msg_send![buffer, contents] }; + if ptr.is_null() { + return Err("buffer contents returned null".to_string()); + } + unsafe { + std::ptr::copy_nonoverlapping(bytes.as_ptr(), ptr.add(offset_bytes), bytes.len()); + } + Ok(()) + } + + fn compile_pipeline( + &self, + base_name: &str, + constants: &[FunctionConstant], + ) -> MetalResult { + let _pool = AutoreleasePool::new(); + let base_obj = unsafe { StrongId::from_owned(str_to_nsstring_owned(base_name)) } + .ok_or_else(|| format!("failed to create NSString for '{}'", base_name))?; + + let mut error: ObjcId = nil; + let func_obj: ObjcId = if constants.is_empty() { + unsafe { msg_send![self.library.as_id(), newFunctionWithName: base_obj.as_id()] } + } else { + let cv_obj: ObjcId = unsafe { msg_send![class!(MTLFunctionConstantValues), new] }; + let cv = unsafe { StrongId::from_owned(cv_obj) } + .ok_or_else(|| "MTLFunctionConstantValues::new returned nil".to_string())?; + + for c in constants { + unsafe { + match c.value { + FunctionConstantValue::Int32(v) => { + let _: () = msg_send![ + cv.as_id(), + setConstantValue: &v as *const i32 as *const c_void + type: MTL_DATA_TYPE_INT + atIndex: c.idx as u64 + ]; + } + FunctionConstantValue::Int16(v) => { + let _: () = msg_send![ + cv.as_id(), + setConstantValue: &v as *const i16 as *const c_void + type: MTL_DATA_TYPE_SHORT + atIndex: c.idx as u64 + ]; + } + FunctionConstantValue::Bool(v) => { + let b: u8 = if v { 1 } else { 0 }; + let _: () = msg_send![ + cv.as_id(), + setConstantValue: &b as *const u8 as *const c_void + type: MTL_DATA_TYPE_BOOL + atIndex: c.idx as u64 + ]; + } + } + } + } + + unsafe { + msg_send![ + self.library.as_id(), + newFunctionWithName: base_obj.as_id() + constantValues: cv.as_id() + error: &mut error + ] + } + }; + + let func = unsafe { StrongId::from_owned(func_obj) }.ok_or_else(|| { + format!( + "newFunctionWithName('{}') failed: {}", + base_name, + ns_error_to_string(error) + ) + })?; + + let mut error: ObjcId = nil; + let pipeline_obj: ObjcId = unsafe { + msg_send![ + self.device.as_id(), + newComputePipelineStateWithFunction: func.as_id() + error: &mut error + ] + }; + + unsafe { StrongId::from_owned(pipeline_obj) }.ok_or_else(|| { + format!( + "newComputePipelineStateWithFunction('{}') failed: {}", + base_name, + ns_error_to_string(error) + ) + }) + } + + fn pipeline_max_threads(pipeline: ObjcId) -> u64 { + unsafe { msg_send![pipeline, maxTotalThreadsPerThreadgroup] } + } + + fn dispatch_compute( + &mut self, + pipeline: &MetalPipeline, + args_bytes: &[u8], + buffers: &[MetalBufferBindingRef<'_>], + threadgroup_memory_lengths: &[(u64, usize)], + threadgroups: MetalSize, + threads_per_threadgroup: MetalSize, + ) -> MetalResult<()> { + let command_buffer_obj: ObjcId = + unsafe { msg_send![self.command_queue.as_id(), commandBuffer] }; + let command_buffer = unsafe { StrongId::from_unowned(command_buffer_obj) } + .ok_or_else(|| "commandBuffer returned nil".to_string())?; + let encoder_obj: ObjcId = + unsafe { msg_send![command_buffer.as_id(), computeCommandEncoder] }; + let encoder = unsafe { StrongId::from_unowned(encoder_obj) } + .ok_or_else(|| "computeCommandEncoder returned nil".to_string())?; + + unsafe { + let _: () = msg_send![encoder.as_id(), setComputePipelineState: pipeline.as_id()]; + if !args_bytes.is_empty() { + let _: () = msg_send![ + encoder.as_id(), + setBytes: args_bytes.as_ptr() as *const c_void + length: args_bytes.len() as u64 + atIndex: 0u64 + ]; + } + + for binding in buffers { + let _: () = msg_send![ + encoder.as_id(), + setBuffer: binding.buffer.as_id() + offset: binding.offset_bytes as u64 + atIndex: binding.index + ]; + } + + for &(index, length) in threadgroup_memory_lengths { + let _: () = msg_send![ + encoder.as_id(), + setThreadgroupMemoryLength: length as u64 + atIndex: index + ]; + } + + let tgs = MTLSize { + width: threadgroups.width, + height: threadgroups.height, + depth: threadgroups.depth, + }; + let tpg = MTLSize { + width: threads_per_threadgroup.width, + height: threads_per_threadgroup.height, + depth: threads_per_threadgroup.depth, + }; + let _: () = msg_send![ + encoder.as_id(), + dispatchThreadgroups: tgs + threadsPerThreadgroup: tpg + ]; + let _: () = msg_send![encoder.as_id(), endEncoding]; + let _: () = msg_send![command_buffer.as_id(), commit]; + } + + self.last_command_buffer = Some(command_buffer); + Ok(()) + } + + fn wait_queue_idle(&self) -> MetalResult<()> { + if let Some(command_buffer) = self.last_command_buffer.as_ref() { + unsafe { + let _: () = msg_send![command_buffer.as_id(), waitUntilCompleted]; + } + let status: u64 = unsafe { msg_send![command_buffer.as_id(), status] }; + if status == 5 { + let error: ObjcId = unsafe { msg_send![command_buffer.as_id(), error] }; + return Err(format!( + "Metal command buffer error (queue idle wait): {}", + ns_error_to_string(error) + )); + } + return Ok(()); + } + + let command_buffer_obj: ObjcId = + unsafe { msg_send![self.command_queue.as_id(), commandBuffer] }; + let command_buffer = unsafe { StrongId::from_unowned(command_buffer_obj) } + .ok_or_else(|| "commandBuffer returned nil".to_string())?; + unsafe { + let _: () = msg_send![command_buffer.as_id(), commit]; + let _: () = msg_send![command_buffer.as_id(), waitUntilCompleted]; + } + let status: u64 = unsafe { msg_send![command_buffer.as_id(), status] }; + if status == 5 { + let error: ObjcId = unsafe { msg_send![command_buffer.as_id(), error] }; + return Err(format!( + "Metal command buffer error (queue idle wait): {}", + ns_error_to_string(error) + )); + } + Ok(()) + } + } +} + +pub use imp::*; diff --git a/libs/ggml/src/backend/metal/selector.rs b/libs/ggml/src/backend/metal/selector.rs new file mode 100644 index 0000000000..f3ffd2e85d --- /dev/null +++ b/libs/ggml/src/backend/metal/selector.rs @@ -0,0 +1,2076 @@ +use crate::context::Context; +use crate::core::{ + ggml_pad, PoolOp, ScaleMode, SortOrder, GGML_ROPE_TYPE_IMROPE, GGML_ROPE_TYPE_MROPE, + GGML_ROPE_TYPE_NEOX, GGML_ROPE_TYPE_VISION, GGML_SCALE_FLAG_ANTIALIAS, +}; +use crate::graph::{Graph, NodeId}; +use crate::op::{GluOp, Op, UnaryOp}; +use crate::tensor::{ggml_type_size_for_type, Tensor, TensorType}; + +use super::{ + FunctionConstant, FunctionConstantValue, MetalDeviceFeatures, MetalPipelineDescriptor, +}; + +const FC_FLASH_ATTN_EXT_PAD: i32 = 100; +const FC_FLASH_ATTN_EXT_BLK: i32 = 200; +const FC_FLASH_ATTN_EXT: i32 = 300; +const FC_FLASH_ATTN_EXT_VEC: i32 = 400; +const FC_FLASH_ATTN_EXT_VEC_REDUCE: i32 = 500; +const FC_MUL_MV: i32 = 600; +const FC_MUL_MM: i32 = 700; +const FC_ROPE: i32 = 800; +pub(super) const FC_SSM_CONV: i32 = 900; +const FC_SOLVE_TRI: i32 = 1000; +const FC_COUNT_EQUAL: i32 = 1100; +const FC_UNARY: i32 = 1200; +const FC_BIN: i32 = 1300; +const FC_SUM_ROWS: i32 = 1400; +const FC_UPSCALE: i32 = 1500; +pub(super) const FC_GATED_DELTA_NET: i32 = 1600; + +const OP_FLASH_ATTN_EXT_NQPSG: i32 = 8; +const OP_FLASH_ATTN_EXT_NCPSG: i32 = 64; +const OP_FLASH_ATTN_EXT_VEC_NQPSG: i32 = 1; +const OP_FLASH_ATTN_EXT_VEC_NCPSG: i32 = 32; + +const OP_UNARY_NUM_SCALE: i16 = 10; +const OP_UNARY_NUM_FILL: i16 = 11; +const OP_UNARY_NUM_CLAMP: i16 = 12; +const OP_UNARY_NUM_SQR: i16 = 13; +const OP_UNARY_NUM_SQRT: i16 = 14; +const OP_UNARY_NUM_SIN: i16 = 15; +const OP_UNARY_NUM_COS: i16 = 16; +const OP_UNARY_NUM_LOG: i16 = 17; +const OP_UNARY_NUM_LEAKY_RELU: i16 = 18; +const OP_UNARY_NUM_TANH: i16 = 100; +const OP_UNARY_NUM_RELU: i16 = 101; +const OP_UNARY_NUM_SIGMOID: i16 = 102; +const OP_UNARY_NUM_GELU: i16 = 103; +const OP_UNARY_NUM_GELU_ERF: i16 = 104; +const OP_UNARY_NUM_GELU_QUICK: i16 = 105; +const OP_UNARY_NUM_SILU: i16 = 106; +const OP_UNARY_NUM_ELU: i16 = 107; +const OP_UNARY_NUM_NEG: i16 = 108; +const OP_UNARY_NUM_ABS: i16 = 109; +const OP_UNARY_NUM_SGN: i16 = 110; +const OP_UNARY_NUM_STEP: i16 = 111; +const OP_UNARY_NUM_HARDSWISH: i16 = 112; +const OP_UNARY_NUM_HARDSIGMOID: i16 = 113; +const OP_UNARY_NUM_EXP: i16 = 114; +const OP_UNARY_NUM_SOFTPLUS: i16 = 115; +const OP_UNARY_NUM_EXPM1: i16 = 116; +const OP_UNARY_NUM_FLOOR: i16 = 117; +const OP_UNARY_NUM_CEIL: i16 = 118; +const OP_UNARY_NUM_ROUND: i16 = 119; +const OP_UNARY_NUM_TRUNC: i16 = 120; + +const OP_SUM_ROWS_NUM_SUM_ROWS: i16 = 10; +const OP_SUM_ROWS_NUM_MEAN: i16 = 11; + +const N_R0_Q4_0: i32 = 4; +const N_SG_Q4_0: i32 = 2; +const N_R0_Q4_1: i32 = 4; +const N_SG_Q4_1: i32 = 2; +const N_R0_Q5_0: i32 = 4; +const N_SG_Q5_0: i32 = 2; +const N_R0_Q5_1: i32 = 4; +const N_SG_Q5_1: i32 = 2; +const N_R0_Q8_0: i32 = 2; +const N_SG_Q8_0: i32 = 4; +const N_R0_MXFP4: i32 = 2; +const N_SG_MXFP4: i32 = 2; +const N_R0_Q2_K: i32 = 4; +const N_SG_Q2_K: i32 = 2; +const N_R0_Q3_K: i32 = 2; +const N_SG_Q3_K: i32 = 2; +const N_R0_Q4_K: i32 = 2; +const N_SG_Q4_K: i32 = 2; +const N_R0_Q5_K: i32 = 1; +const N_SG_Q5_K: i32 = 2; +const N_R0_Q6_K: i32 = 2; +const N_SG_Q6_K: i32 = 2; +const N_R0_IQ1_S: i32 = 4; +const N_SG_IQ1_S: i32 = 2; +const N_R0_IQ1_M: i32 = 4; +const N_SG_IQ1_M: i32 = 2; +const N_R0_IQ2_XXS: i32 = 4; +const N_SG_IQ2_XXS: i32 = 2; +const N_R0_IQ2_XS: i32 = 4; +const N_SG_IQ2_XS: i32 = 2; +const N_R0_IQ2_S: i32 = 4; +const N_SG_IQ2_S: i32 = 2; +const N_R0_IQ3_XXS: i32 = 4; +const N_SG_IQ3_XXS: i32 = 2; +const N_R0_IQ3_S: i32 = 4; +const N_SG_IQ3_S: i32 = 2; +const N_R0_IQ4_NL: i32 = 2; +const N_SG_IQ4_NL: i32 = 2; +const N_R0_IQ4_XS: i32 = 2; +const N_SG_IQ4_XS: i32 = 2; + +#[derive(Clone, Copy, Debug, Eq, PartialEq)] +pub enum MetalStageKind { + Main, + Copy, + Aux, + Merge, + Reduce, +} + +#[derive(Clone, Debug, Eq, PartialEq)] +pub struct MetalPipelineSpec { + pub kind: MetalStageKind, + pub descriptor: MetalPipelineDescriptor, + pub c4: bool, + pub cnt: bool, +} + +#[derive(Clone, Copy, Debug, Default, Eq, PartialEq)] +pub struct MetalOpResources { + pub output_tail_bytes: usize, +} + +#[derive(Clone, Debug, Default, Eq, PartialEq)] +pub struct MetalOpProgram { + pub stages: Vec, + pub resources: MetalOpResources, +} + +#[derive(Clone, Debug, Eq, PartialEq)] +pub struct MetalGraphNodePlan { + pub node_id: NodeId, + pub program: MetalOpProgram, +} + +#[derive(Clone, Debug, Default, Eq, PartialEq)] +pub struct MetalGraphPlan { + pub nodes: Vec, + pub total_output_tail_bytes: usize, +} + +pub fn supports_program(tensors: &[Tensor], op: &Tensor, features: MetalDeviceFeatures) -> bool { + build_program(tensors, op, features).is_ok() +} + +pub fn build_program( + tensors: &[Tensor], + op: &Tensor, + features: MetalDeviceFeatures, +) -> Result { + match op.op { + Op::Concat => program_concat(), + Op::Add | Op::Sub | Op::Mul | Op::Div => program_bin(tensors, op, 1), + Op::AddId => program_add_id(), + Op::Repeat => program_repeat(tensors, op), + Op::Acc => program_acc(tensors, op), + Op::Scale + | Op::Fill + | Op::Clamp + | Op::LeakyRelu + | Op::Sqr + | Op::Sqrt + | Op::Sin + | Op::Cos + | Op::Log + | Op::Unary => program_unary(tensors, op), + Op::Glu => program_glu(tensors, op), + Op::Sum => program_sum(tensors, op), + Op::SumRows | Op::Mean => program_sum_rows(tensors, op), + Op::CumSum => program_cumsum(tensors, op), + Op::SoftMax => program_soft_max(tensors, op), + Op::SsmConv => program_ssm_conv(tensors, op), + Op::SsmScan => program_ssm_scan(tensors, op), + Op::RwkvWkv6 | Op::RwkvWkv7 => program_rwkv(tensors, op), + Op::GatedDeltaNet => program_gated_delta_net(tensors, op), + Op::SolveTri => program_solve_tri(tensors, op), + Op::MulMat => program_mul_mat(tensors, op, features), + Op::MulMatId => program_mul_mat_id(tensors, op, features), + Op::GetRows => program_get_rows(tensors, op), + Op::SetRows => program_set_rows(tensors, op), + Op::Diag => program_diag(tensors, op), + Op::L2Norm => program_l2_norm(tensors, op), + Op::GroupNorm => program_group_norm(), + Op::Norm | Op::RmsNorm => program_norm(tensors, op, 1), + Op::Rope => program_rope(tensors, op), + Op::Im2col => program_im2col(op), + Op::Conv2d => program_conv2d(tensors, op), + Op::ConvTranspose1d => program_conv_transpose_1d(tensors, op), + Op::ConvTranspose2d => program_conv_transpose_2d(tensors, op), + Op::Conv3d => program_conv3d(tensors, op), + Op::Upscale => program_upscale(tensors, op), + Op::Pad => program_pad(tensors, op), + Op::PadReflect1d => program_pad_reflect_1d(tensors, op), + Op::Arange => program_arange(op), + Op::TimestepEmbedding => program_timestep_embedding(tensors, op), + Op::Argsort => program_argsort(tensors, op), + Op::TopK => program_top_k(tensors, op), + Op::Tri => program_tri(tensors, op), + Op::FlashAttnExt => program_flash_attn_ext(tensors, op), + Op::Set => program_set(tensors, op), + Op::Dup | Op::Cpy | Op::Cont => program_cpy(tensors, op), + Op::Pool1d => program_pool_1d(tensors, op), + Op::Pool2d => program_pool_2d(tensors, op), + Op::ArgMax => program_argmax(tensors, op), + Op::OptStepAdamw => program_opt_step_adamw(tensors, op), + Op::OptStepSgd => program_opt_step_sgd(tensors, op), + Op::CountEqual => program_count_equal(tensors, op), + _ => Err(format!( + "Metal selector does not support ggml op {} yet", + op.op.name() + )), + } +} + +pub fn build_graph_plan( + ctx: &Context, + graph: &Graph, + features: MetalDeviceFeatures, +) -> Result { + let tensors = ctx.tensors(); + let mut nodes = Vec::with_capacity(graph.nodes.len()); + let mut total_output_tail_bytes = 0usize; + + for &node_id in &graph.nodes { + let op = tensors + .get(node_id) + .ok_or_else(|| format!("graph references invalid tensor {}", node_id))?; + if is_metadata_only_op(op.op) { + continue; + } + let program = build_program(tensors, op, features)?; + total_output_tail_bytes = total_output_tail_bytes + .checked_add(program.resources.output_tail_bytes) + .ok_or_else(|| "Metal graph tail-byte accounting overflow".to_string())?; + nodes.push(MetalGraphNodePlan { node_id, program }); + } + + Ok(MetalGraphPlan { + nodes, + total_output_tail_bytes, + }) +} + +fn is_metadata_only_op(op: Op) -> bool { + matches!(op, Op::View | Op::Reshape | Op::Permute | Op::Transpose) +} + +fn program_concat() -> Result { + Ok(program_with_stage(stage_simple( + MetalStageKind::Main, + "kernel_concat", + "kernel_concat", + ))) +} + +fn program_add_id() -> Result { + Ok(program_with_stage(stage_simple( + MetalStageKind::Main, + "kernel_add_id", + "kernel_add_id", + ))) +} + +fn program_repeat(tensors: &[Tensor], op: &Tensor) -> Result { + let src0 = src(tensors, op, 0)?; + let base = format!("kernel_repeat_{}", src0.desc.ty.name()); + Ok(program_with_stage(stage_simple( + MetalStageKind::Main, + &base, + &base, + ))) +} + +fn program_pool_1d(tensors: &[Tensor], op: &Tensor) -> Result { + let src0 = src(tensors, op, 0)?; + let pool = pool_name(op.op_param_i32(0))?; + let base = format!("kernel_pool_1d_{}_{}", pool, src0.desc.ty.name()); + Ok(program_with_stage(stage_simple( + MetalStageKind::Main, + &base, + &base, + ))) +} + +fn program_pool_2d(tensors: &[Tensor], op: &Tensor) -> Result { + let src0 = src(tensors, op, 0)?; + let pool = pool_name(op.op_param_i32(0))?; + let base = format!("kernel_pool_2d_{}_{}", pool, src0.desc.ty.name()); + Ok(program_with_stage(stage_simple( + MetalStageKind::Main, + &base, + &base, + ))) +} + +fn program_get_rows(tensors: &[Tensor], op: &Tensor) -> Result { + let src0 = src(tensors, op, 0)?; + let base = format!("kernel_get_rows_{}", src0.desc.ty.name()); + Ok(program_with_stage(stage_simple( + MetalStageKind::Main, + &base, + &base, + ))) +} + +fn program_set_rows(tensors: &[Tensor], op: &Tensor) -> Result { + let idx = src(tensors, op, 1)?; + let base = format!( + "kernel_set_rows_{}_{}", + op.desc.ty.name(), + idx.desc.ty.name() + ); + Ok(program_with_stage(stage_simple( + MetalStageKind::Main, + &base, + &base, + ))) +} + +fn program_diag(tensors: &[Tensor], op: &Tensor) -> Result { + let src0 = src(tensors, op, 0)?; + let base = format!("kernel_diag_{}", src0.desc.ty.name()); + let cache = format!("{}_n={}", base, src0.ne[0]); + Ok(program_with_stage(stage( + MetalStageKind::Main, + &base, + &cache, + Vec::new(), + 0, + 0, + 0, + 1, + false, + false, + ))) +} + +fn program_unary(tensors: &[Tensor], op: &Tensor) -> Result { + let src0 = src(tensors, op, 0)?; + let op_num = unary_num(op)?; + let is_c4 = src0.ne[0] % 4 == 0; + let is_cnt = src0.is_contiguous() && op.nelements() < 32_768; + let suffix = if is_c4 { "_4" } else { "" }; + let base = format!( + "kernel_unary_{}_{}{}", + src0.desc.ty.name(), + op.desc.ty.name(), + suffix + ); + let cache = format!("{}_op={}_cnt={}", base, op_num, bool_num(is_cnt)); + Ok(program_with_stage(stage( + MetalStageKind::Main, + &base, + &cache, + vec![ + i16_const(FC_UNARY + 0, op_num), + bool_const(FC_UNARY + 1, is_cnt), + ], + 0, + 0, + 0, + 0, + is_c4, + is_cnt, + ))) +} + +fn program_glu(tensors: &[Tensor], op: &Tensor) -> Result { + let src0 = src(tensors, op, 0)?; + let op_name = glu_kernel_name(op)?; + let base = format!("kernel_{}_{}", op_name, src0.desc.ty.name()); + Ok(program_with_stage(stage_simple( + MetalStageKind::Main, + &base, + &base, + ))) +} + +fn program_sum(tensors: &[Tensor], op: &Tensor) -> Result { + let src0 = src(tensors, op, 0)?; + let base = format!("kernel_op_sum_{}", src0.desc.ty.name()); + Ok(program_with_stage(stage_simple( + MetalStageKind::Main, + &base, + &base, + ))) +} + +fn program_sum_rows(tensors: &[Tensor], op: &Tensor) -> Result { + let src0 = src(tensors, op, 0)?; + let op_num = match op.op { + Op::SumRows => OP_SUM_ROWS_NUM_SUM_ROWS, + Op::Mean => OP_SUM_ROWS_NUM_MEAN, + _ => return Err(format!("expected sum_rows/mean, got {}", op.op.name())), + }; + let is_c4 = src0.ne[0] % 4 == 0; + let suffix = if is_c4 { "_4" } else { "" }; + let base = format!( + "kernel_sum_rows_{}_{}{}", + src0.desc.ty.name(), + op.desc.ty.name(), + suffix + ); + let cache = format!("{}_op={}", base, op_num); + let mut smem = 32 * std::mem::size_of::(); + if is_c4 { + smem *= 4; + } + Ok(program_with_stage(stage( + MetalStageKind::Main, + &base, + &cache, + vec![i16_const(FC_SUM_ROWS + 0, op_num)], + smem, + 0, + 0, + 0, + is_c4, + false, + ))) +} + +fn program_cumsum(tensors: &[Tensor], op: &Tensor) -> Result { + let src0 = src(tensors, op, 0)?; + let ty = src0.desc.ty.name(); + Ok(MetalOpProgram { + stages: vec![ + stage_simple( + MetalStageKind::Aux, + &format!("kernel_cumsum_blk_{ty}"), + &format!("kernel_cumsum_blk_{ty}"), + ), + stage_simple( + MetalStageKind::Main, + &format!("kernel_cumsum_add_{ty}"), + &format!("kernel_cumsum_add_{ty}"), + ), + ], + resources: MetalOpResources::default(), + }) +} + +fn program_soft_max(tensors: &[Tensor], op: &Tensor) -> Result { + let src0 = src(tensors, op, 0)?; + let src1_ty = src_opt(tensors, op, 1) + .map(|tensor| tensor.desc.ty) + .unwrap_or(TensorType::F32); + let suffix = if src0.ne[0] % 4 == 0 { "_4" } else { "" }; + let base = format!("kernel_soft_max_{}{}", src1_ty.name(), suffix); + Ok(program_with_stage(stage( + MetalStageKind::Main, + &base, + &base, + Vec::new(), + 32 * std::mem::size_of::(), + 0, + 0, + 0, + false, + false, + ))) +} + +fn program_ssm_conv(tensors: &[Tensor], op: &Tensor) -> Result { + let src0 = src(tensors, op, 0)?; + let src1 = src(tensors, op, 1)?; + let use_batched = op.ne[1] > 1; + let suffix = if src1.ne[0] % 4 == 0 { "_4" } else { "" }; + if use_batched { + let batch_size = match op.ne[1] { + x if x > 128 => 256, + x if x > 64 => 128, + x if x > 32 => 64, + x if x > 16 => 32, + x if x > 8 => 16, + x if x > 4 => 8, + _ => 2, + }; + let base = format!( + "kernel_ssm_conv_{}_{}_batched{}", + src0.desc.ty.name(), + src1.desc.ty.name(), + suffix + ); + let cache = format!("{}_ssm_conv_bs={}", base, batch_size); + return Ok(program_with_stage(stage( + MetalStageKind::Main, + &base, + &cache, + vec![i16_const(FC_SSM_CONV + 0, batch_size as i16)], + 0, + 0, + 0, + 0, + false, + false, + ))); + } + let base = format!( + "kernel_ssm_conv_{}_{}{}", + src0.desc.ty.name(), + src1.desc.ty.name(), + suffix + ); + Ok(program_with_stage(stage_simple( + MetalStageKind::Main, + &base, + &base, + ))) +} + +fn program_ssm_scan(tensors: &[Tensor], op: &Tensor) -> Result { + let src0 = src(tensors, op, 0)?; + let nsg = i32::try_from((src0.ne[0] + 31) / 32) + .map_err(|_| "ssm_scan nsg exceeds i32".to_string())?; + let base = format!("kernel_ssm_scan_{}", src0.desc.ty.name()); + let cache = format!("{}_nsg={}", base, nsg); + let smem = usize::try_from(nsg).map_err(|_| "ssm_scan nsg exceeds usize".to_string())? + * (32 + 2) + * std::mem::size_of::(); + Ok(program_with_stage(stage( + MetalStageKind::Main, + &base, + &cache, + Vec::new(), + smem, + 0, + 0, + 0, + false, + false, + ))) +} + +fn program_rwkv(tensors: &[Tensor], op: &Tensor) -> Result { + let src0 = src(tensors, op, 0)?; + let prefix = match op.op { + Op::RwkvWkv6 => "kernel_rwkv_wkv6", + Op::RwkvWkv7 => "kernel_rwkv_wkv7", + _ => return Err(format!("expected rwkv op, got {}", op.op.name())), + }; + let base = format!("{prefix}_{}", src0.desc.ty.name()); + Ok(program_with_stage(stage_simple( + MetalStageKind::Main, + &base, + &base, + ))) +} + +fn program_gated_delta_net(tensors: &[Tensor], op: &Tensor) -> Result { + let src0 = src(tensors, op, 0)?; + let src2 = src(tensors, op, 2)?; + let src3 = src(tensors, op, 3)?; + let ne20 = + i16::try_from(src2.ne[0]).map_err(|_| "gated_delta_net ne20 exceeds i16".to_string())?; + let ne30 = + i16::try_from(src3.ne[0]).map_err(|_| "gated_delta_net ne30 exceeds i16".to_string())?; + let nsg = i32::try_from(src2.ne[0] / 32) + .map_err(|_| "gated_delta_net nsg exceeds i32".to_string())?; + let base = format!("kernel_gated_delta_net_{}_{}", src0.desc.ty.name(), nsg); + let cache = format!("{}_ne20={}_ne30={}", base, ne20, ne30); + Ok(program_with_stage(stage( + MetalStageKind::Main, + &base, + &cache, + vec![ + i16_const(FC_GATED_DELTA_NET + 0, ne20), + i16_const(FC_GATED_DELTA_NET + 1, ne30), + ], + 0, + 0, + 0, + nsg, + false, + false, + ))) +} + +fn program_solve_tri(tensors: &[Tensor], op: &Tensor) -> Result { + let src0 = src(tensors, op, 0)?; + let src1 = src(tensors, op, 1)?; + let nsg = 8_i16; + let n = i16::try_from(src1.ne[1]).map_err(|_| "solve_tri n exceeds i16".to_string())?; + let k = i16::try_from(src1.ne[0]).map_err(|_| "solve_tri k exceeds i16".to_string())?; + let base = format!("kernel_solve_tri_{}", src0.desc.ty.name()); + let cache = format!("{}_nsg={}_n={}_k={}", base, nsg, n, k); + let smem = ggml_pad( + ggml_pad(n as usize, 32) * nsg as usize * std::mem::size_of::(), + 16, + ); + Ok(program_with_stage(stage( + MetalStageKind::Main, + &base, + &cache, + vec![ + i16_const(FC_SOLVE_TRI + 0, nsg), + i16_const(FC_SOLVE_TRI + 1, n), + i16_const(FC_SOLVE_TRI + 2, k), + ], + smem, + 0, + 0, + nsg as i32, + false, + false, + ))) +} + +fn program_mul_mat( + tensors: &[Tensor], + op: &Tensor, + features: MetalDeviceFeatures, +) -> Result { + let src0 = src(tensors, op, 0)?; + let src1 = src(tensors, op, 1)?; + let ne00 = src0.ne[0]; + let ne11 = src1.ne[1]; + + if use_mul_mv_ext(src0, src1) { + let nsg = 2_i16; + let nxpsg = if ne00 % 256 == 0 && ne11 < 3 { + 16_i16 + } else if ne00 % 128 == 0 { + 8_i16 + } else { + 4_i16 + }; + let r1ptg = match ne11 { + 2 => 2_i16, + 3 | 6 => 3_i16, + 4 | 7 | 8 => 4_i16, + 5 => 5_i16, + _ => return Err(format!("unsupported mul_mv_ext batch size {}", ne11)), + }; + let base = format!( + "kernel_mul_mv_ext_{}_{}_r1_{}", + src0.desc.ty.name(), + src1.desc.ty.name(), + r1ptg + ); + let cache = format!("{}_nsg={}_nxpsg={}", base, nsg, nxpsg); + return Ok(program_with_stage(stage( + MetalStageKind::Main, + &base, + &cache, + vec![ + i16_const(FC_MUL_MV + 0, nsg), + i16_const(FC_MUL_MV + 1, nxpsg), + ], + 0, + 0, + 0, + nsg as i32, + false, + false, + ))); + } + + if features.has_simdgroup_mm + && ne00 >= 64 + && ne11 > 8 + && !src0.is_transposed() + && !src1.is_transposed() + { + let bc_inp = src0.ne[0] % 32 != 0; + let bc_out = op.ne[0] % 64 != 0 || op.ne[1] % 32 != 0; + let base = format!( + "kernel_mul_mm_{}_{}", + src0.desc.ty.name(), + src1.desc.ty.name() + ); + let cache = format!("{}_bci={}_bco={}", base, bool_num(bc_inp), bool_num(bc_out)); + return Ok(program_with_stage(stage( + MetalStageKind::Main, + &base, + &cache, + vec![ + bool_const(FC_MUL_MM + 0, bc_inp), + bool_const(FC_MUL_MM + 1, bc_out), + ], + if bc_out { 8192 } else { 4096 + 2048 }, + 0, + 0, + 0, + false, + false, + ))); + } + + let params = mul_mv_params(src0.desc.ty, src0.ne[0], true)?; + let suffix = params.suffix; + let base = format!( + "kernel_mul_mv_{}_{}{}", + src0.desc.ty.name(), + src1.desc.ty.name(), + suffix + ); + let cache = format!("{}_nsg={}", base, params.nsg); + Ok(program_with_stage(stage( + MetalStageKind::Main, + &base, + &cache, + vec![i16_const(FC_MUL_MV + 0, params.nsg as i16)], + params.smem, + params.nr0, + params.nr1, + params.nsg, + false, + false, + ))) +} + +fn program_mul_mat_id( + tensors: &[Tensor], + op: &Tensor, + features: MetalDeviceFeatures, +) -> Result { + let src0 = src(tensors, op, 0)?; + let src1 = src(tensors, op, 1)?; + let src2 = src(tensors, op, 2)?; + let ne00 = src0.ne[0]; + let ne02 = src0.ne[2]; + let ne20 = src2.ne[0]; + let ne21 = src2.ne[1]; + + if features.has_simdgroup_mm && ne00 >= 64 && ne21 >= 32 { + let map_base = format!("kernel_mul_mm_id_map0_ne20_{}", ne20); + let map_cache = format!("{}_ne02={}", map_base, ne02); + let bc_inp = src0.ne[0] % 32 != 0; + let mm_base = format!( + "kernel_mul_mm_id_{}_{}", + src0.desc.ty.name(), + src1.desc.ty.name() + ); + let mm_cache = format!("{}_bci={}", mm_base, bool_num(bc_inp)); + return Ok(MetalOpProgram { + stages: vec![ + stage( + MetalStageKind::Aux, + &map_base, + &map_cache, + Vec::new(), + usize::try_from(ne02 * ne20) + .map_err(|_| "mul_mm_id_map0 smem overflow".to_string())? + * std::mem::size_of::(), + 0, + 0, + 0, + false, + false, + ), + stage( + MetalStageKind::Main, + &mm_base, + &mm_cache, + vec![bool_const(FC_MUL_MM + 0, bc_inp)], + 8192, + 0, + 0, + 0, + false, + false, + ), + ], + resources: MetalOpResources { + output_tail_bytes: mul_mat_id_extra_tpe(src0)? + mul_mat_id_extra_ids(src0, src2)?, + }, + }); + } + + let params = mul_mv_params(src0.desc.ty, src0.ne[0], false)?; + let base = format!( + "kernel_mul_mv_id_{}_{}{}", + src0.desc.ty.name(), + src1.desc.ty.name(), + params.suffix + ); + let cache = format!("{}_nsg={}", base, params.nsg); + Ok(program_with_stage(stage( + MetalStageKind::Main, + &base, + &cache, + vec![i16_const(FC_MUL_MV + 0, params.nsg as i16)], + params.smem, + params.nr0, + params.nr1, + params.nsg, + false, + false, + ))) +} + +fn program_argmax(tensors: &[Tensor], op: &Tensor) -> Result { + let src0 = src(tensors, op, 0)?; + let base = format!("kernel_argmax_{}", src0.desc.ty.name()); + Ok(program_with_stage(stage( + MetalStageKind::Main, + &base, + &base, + Vec::new(), + 32 * (std::mem::size_of::() + std::mem::size_of::()), + 0, + 0, + 0, + false, + false, + ))) +} + +fn program_argsort(tensors: &[Tensor], op: &Tensor) -> Result { + let src0 = src(tensors, op, 0)?; + let order = sort_name(op.op_param_i32(0))?; + let base = format!( + "kernel_argsort_{}_{}_{}", + src0.desc.ty.name(), + op.desc.ty.name(), + order + ); + let merge_base = format!( + "kernel_argsort_merge_{}_{}_{}", + src0.desc.ty.name(), + op.desc.ty.name(), + order + ); + Ok(MetalOpProgram { + stages: vec![ + stage_simple(MetalStageKind::Main, &base, &base), + stage_simple(MetalStageKind::Merge, &merge_base, &merge_base), + ], + resources: MetalOpResources { + output_tail_bytes: op.nbytes(), + }, + }) +} + +fn program_top_k(tensors: &[Tensor], op: &Tensor) -> Result { + let src0 = src(tensors, op, 0)?; + let order = SortOrder::Desc.name(); + let base = format!( + "kernel_argsort_{}_{}_{}", + src0.desc.ty.name(), + op.desc.ty.name(), + order + ); + let merge_base = format!( + "kernel_argsort_merge_{}_{}_{}", + src0.desc.ty.name(), + op.desc.ty.name(), + order + ); + Ok(MetalOpProgram { + stages: vec![ + stage_simple(MetalStageKind::Main, &base, &base), + stage_simple(MetalStageKind::Merge, &merge_base, &merge_base), + ], + resources: MetalOpResources { + output_tail_bytes: usize::try_from(src0.nelements()) + .map_err(|_| "top_k temp size overflow".to_string())? + * std::mem::size_of::(), + }, + }) +} + +fn program_bin(tensors: &[Tensor], op: &Tensor, n_fuse: i16) -> Result { + let src0 = src(tensors, op, 0)?; + let src1 = src(tensors, op, 1)?; + let op_num = match op.op { + Op::Add => 0_i16, + Op::Sub => 1_i16, + Op::Mul => 2_i16, + Op::Div => 3_i16, + _ => return Err(format!("expected binary op, got {}", op.op.name())), + }; + let is_c4 = (src0.ne[0] % 4 == 0) && (src1.ne[0] % 4 == 0); + let is_cb = src0.ne[0] != src1.ne[0]; + let is_rb = src0.is_contiguous() + && src1.is_contiguous() + && src1.nrows() == 1 + && op.nelements() < 65_536; + let suffix = if is_c4 { "_4" } else { "" }; + let base = format!( + "kernel_bin_fuse_{}_{}_{}{}", + src0.desc.ty.name(), + src1.desc.ty.name(), + op.desc.ty.name(), + suffix + ); + let cache = format!( + "{}_op={}_nf={}_rb={}_cb={}", + base, + op_num, + n_fuse, + bool_num(is_rb), + bool_num(is_cb) + ); + Ok(program_with_stage(stage( + MetalStageKind::Main, + &base, + &cache, + vec![ + i16_const(FC_BIN + 0, op_num), + i16_const(FC_BIN + 1, n_fuse), + bool_const(FC_BIN + 2, is_rb), + bool_const(FC_BIN + 3, is_cb), + ], + 0, + 0, + 0, + 0, + is_c4, + is_rb, + ))) +} + +fn program_bin_one_add() -> MetalPipelineSpec { + stage( + MetalStageKind::Main, + "kernel_bin_fuse_f32_f32_f32", + "kernel_bin_fuse_f32_f32_f32_op=0_nf=1", + vec![ + i16_const(FC_BIN + 0, 0), + i16_const(FC_BIN + 1, 1), + bool_const(FC_BIN + 2, false), + ], + 0, + 0, + 0, + 0, + false, + false, + ) +} + +fn program_l2_norm(tensors: &[Tensor], op: &Tensor) -> Result { + let src0 = src(tensors, op, 0)?; + let is_c4 = src0.ne[0] % 4 == 0; + let suffix = if is_c4 { "_4" } else { "" }; + let base = format!( + "kernel_l2_norm_{}_{}{}", + src0.desc.ty.name(), + op.desc.ty.name(), + suffix + ); + Ok(program_with_stage(stage( + MetalStageKind::Main, + &base, + &base, + Vec::new(), + 32 * std::mem::size_of::(), + 0, + 0, + 0, + is_c4, + false, + ))) +} + +fn program_group_norm() -> Result { + Ok(program_with_stage(stage( + MetalStageKind::Main, + "kernel_group_norm_f32", + "kernel_group_norm_f32", + Vec::new(), + 32 * std::mem::size_of::(), + 0, + 0, + 0, + false, + false, + ))) +} + +fn program_norm(tensors: &[Tensor], op: &Tensor, n_fuse: i32) -> Result { + let suffix = if op.ne[0] % 4 == 0 { "_4" } else { "" }; + let base = match op.op { + Op::Norm => match n_fuse { + 1 => format!("kernel_norm_f32{suffix}"), + 2 => format!("kernel_norm_mul_f32{suffix}"), + 3 => format!("kernel_norm_mul_add_f32{suffix}"), + _ => return Err(format!("unsupported norm fusion count {}", n_fuse)), + }, + Op::RmsNorm => match n_fuse { + 1 => format!("kernel_rms_norm_f32{suffix}"), + 2 => format!("kernel_rms_norm_mul_f32{suffix}"), + 3 => format!("kernel_rms_norm_mul_add_f32{suffix}"), + _ => return Err(format!("unsupported rms_norm fusion count {}", n_fuse)), + }, + _ => return Err(format!("expected norm op, got {}", op.op.name())), + }; + let _ = src(tensors, op, 0)?; + Ok(program_with_stage(stage( + MetalStageKind::Main, + &base, + &base, + Vec::new(), + 32 * std::mem::size_of::(), + 0, + 0, + 0, + false, + false, + ))) +} + +fn program_rope(tensors: &[Tensor], op: &Tensor) -> Result { + let src0 = src(tensors, op, 0)?; + let mode = op.op_param_i32(2); + let is_neox = mode & GGML_ROPE_TYPE_NEOX != 0; + let is_mrope = mode & GGML_ROPE_TYPE_MROPE != 0; + let is_imrope = mode == GGML_ROPE_TYPE_IMROPE; + let is_vision = mode == GGML_ROPE_TYPE_VISION; + let base = if is_neox { + format!("kernel_rope_neox_{}", src0.desc.ty.name()) + } else if (is_mrope || is_imrope) && !is_vision { + format!("kernel_rope_multi_{}", src0.desc.ty.name()) + } else if is_vision { + format!("kernel_rope_vision_{}", src0.desc.ty.name()) + } else { + format!("kernel_rope_norm_{}", src0.desc.ty.name()) + }; + let cache = format!("{}_imrope={}", base, bool_num(is_imrope)); + Ok(program_with_stage(stage( + MetalStageKind::Main, + &base, + &cache, + vec![bool_const(FC_ROPE + 0, is_imrope)], + 0, + 0, + 0, + 0, + false, + false, + ))) +} + +fn program_im2col(op: &Tensor) -> Result { + let base = format!("kernel_im2col_{}", op.desc.ty.name()); + Ok(program_with_stage(stage_simple( + MetalStageKind::Main, + &base, + &base, + ))) +} + +fn program_conv_transpose_1d(tensors: &[Tensor], op: &Tensor) -> Result { + let src0 = src(tensors, op, 0)?; + let src1 = src(tensors, op, 1)?; + let base = format!( + "kernel_conv_transpose_1d_{}_{}", + src0.desc.ty.name(), + src1.desc.ty.name() + ); + Ok(program_with_stage(stage_simple( + MetalStageKind::Main, + &base, + &base, + ))) +} + +fn program_conv_transpose_2d(tensors: &[Tensor], op: &Tensor) -> Result { + let src0 = src(tensors, op, 0)?; + let src1 = src(tensors, op, 1)?; + let base = format!( + "kernel_conv_transpose_2d_{}_{}", + src0.desc.ty.name(), + src1.desc.ty.name() + ); + Ok(program_with_stage(stage_simple( + MetalStageKind::Main, + &base, + &base, + ))) +} + +fn program_conv2d(tensors: &[Tensor], op: &Tensor) -> Result { + let src0 = src(tensors, op, 0)?; + let src1 = src(tensors, op, 1)?; + let base = format!( + "kernel_conv_2d_{}_{}", + src0.desc.ty.name(), + src1.desc.ty.name() + ); + Ok(program_with_stage(stage_simple( + MetalStageKind::Main, + &base, + &base, + ))) +} + +fn program_conv3d(tensors: &[Tensor], op: &Tensor) -> Result { + let src0 = src(tensors, op, 0)?; + let src1 = src(tensors, op, 1)?; + let base = format!( + "kernel_conv_3d_{}_{}", + src0.desc.ty.name(), + src1.desc.ty.name() + ); + Ok(program_with_stage(stage_simple( + MetalStageKind::Main, + &base, + &base, + ))) +} + +fn program_upscale(tensors: &[Tensor], op: &Tensor) -> Result { + let src0 = src(tensors, op, 0)?; + let mode_flags = op.op_param_i32(0); + let mode = ScaleMode::from_i32(mode_flags & 0xFF) + .ok_or_else(|| format!("unsupported upscale mode {}", mode_flags & 0xFF))?; + let antialias = (mode_flags & GGML_SCALE_FLAG_ANTIALIAS) != 0; + let base = match mode { + ScaleMode::Nearest => format!("kernel_upscale_nearest_{}", src0.desc.ty.name()), + ScaleMode::Bilinear => format!("kernel_upscale_bilinear_{}", src0.desc.ty.name()), + ScaleMode::Bicubic => format!("kernel_upscale_bicubic_{}", src0.desc.ty.name()), + }; + let cache = format!("{}_aa={}", base, bool_num(antialias)); + Ok(program_with_stage(stage( + MetalStageKind::Main, + &base, + &cache, + vec![bool_const(FC_UPSCALE + 0, antialias)], + 0, + 0, + 0, + 0, + false, + false, + ))) +} + +fn program_pad(tensors: &[Tensor], op: &Tensor) -> Result { + let src0 = src(tensors, op, 0)?; + let base = format!("kernel_pad_{}", src0.desc.ty.name()); + Ok(program_with_stage(stage_simple( + MetalStageKind::Main, + &base, + &base, + ))) +} + +fn program_pad_reflect_1d(tensors: &[Tensor], op: &Tensor) -> Result { + let src0 = src(tensors, op, 0)?; + let base = format!("kernel_pad_reflect_1d_{}", src0.desc.ty.name()); + Ok(program_with_stage(stage_simple( + MetalStageKind::Main, + &base, + &base, + ))) +} + +fn program_arange(op: &Tensor) -> Result { + let base = format!("kernel_arange_{}", op.desc.ty.name()); + Ok(program_with_stage(stage_simple( + MetalStageKind::Main, + &base, + &base, + ))) +} + +fn program_timestep_embedding(tensors: &[Tensor], op: &Tensor) -> Result { + let src0 = src(tensors, op, 0)?; + let base = format!("kernel_timestep_embedding_{}", src0.desc.ty.name()); + Ok(program_with_stage(stage_simple( + MetalStageKind::Main, + &base, + &base, + ))) +} + +fn program_tri(tensors: &[Tensor], op: &Tensor) -> Result { + let src0 = src(tensors, op, 0)?; + let ttype = op.op_param_i32(0); + let base = format!("kernel_tri_{}_{}", src0.desc.ty.name(), ttype); + Ok(program_with_stage(stage_simple( + MetalStageKind::Main, + &base, + &base, + ))) +} + +fn program_opt_step_adamw(tensors: &[Tensor], op: &Tensor) -> Result { + let src0 = src(tensors, op, 0)?; + let base = format!("kernel_opt_step_adamw_{}", src0.desc.ty.name()); + Ok(program_with_stage(stage_simple( + MetalStageKind::Main, + &base, + &base, + ))) +} + +fn program_opt_step_sgd(tensors: &[Tensor], op: &Tensor) -> Result { + let src0 = src(tensors, op, 0)?; + let base = format!("kernel_opt_step_sgd_{}", src0.desc.ty.name()); + Ok(program_with_stage(stage_simple( + MetalStageKind::Main, + &base, + &base, + ))) +} + +fn program_count_equal(tensors: &[Tensor], op: &Tensor) -> Result { + let src0 = src(tensors, op, 0)?; + let mut nsg = 1_i16; + while 32 * i64::from(nsg) < src0.ne[0] && nsg < 32 { + nsg *= 2; + } + let base = format!("kernel_count_equal_{}", src0.desc.ty.name()); + let cache = format!("{}_nsg={}", base, nsg); + Ok(program_with_stage(stage( + MetalStageKind::Main, + &base, + &cache, + vec![i16_const(FC_COUNT_EQUAL + 0, nsg)], + 0, + 0, + 0, + nsg as i32, + false, + false, + ))) +} + +fn program_cpy(tensors: &[Tensor], op: &Tensor) -> Result { + let src0 = src(tensors, op, 0)?; + Ok(program_with_stage(cpy_stage( + MetalStageKind::Main, + src0.desc.ty, + op.desc.ty, + ))) +} + +fn program_set(tensors: &[Tensor], op: &Tensor) -> Result { + let src0 = src(tensors, op, 0)?; + let src1 = src(tensors, op, 1)?; + let inplace = op.op_param_i32(4) != 0; + let mut stages = Vec::new(); + if !inplace { + stages.push(cpy_stage(MetalStageKind::Copy, src0.desc.ty, op.desc.ty)); + } + stages.push(cpy_stage(MetalStageKind::Main, src1.desc.ty, op.desc.ty)); + Ok(MetalOpProgram { + stages, + resources: MetalOpResources::default(), + }) +} + +fn program_acc(tensors: &[Tensor], op: &Tensor) -> Result { + let src0 = src(tensors, op, 0)?; + let inplace = op.op_param_i32(4) != 0; + let mut stages = Vec::new(); + if !inplace { + stages.push(cpy_stage(MetalStageKind::Copy, src0.desc.ty, op.desc.ty)); + } + stages.push(program_bin_one_add()); + Ok(MetalOpProgram { + stages, + resources: MetalOpResources::default(), + }) +} + +fn program_flash_attn_ext(tensors: &[Tensor], op: &Tensor) -> Result { + let q = src(tensors, op, 0)?; + let k = src(tensors, op, 1)?; + let v = src(tensors, op, 2)?; + let mask = src_opt(tensors, op, 3); + + let has_mask = mask.is_some(); + let has_sinks = src_opt(tensors, op, 4).is_some(); + let max_bias = op.op_param_f32(1); + let logit_softcap = op.op_param_f32(2); + let has_bias = max_bias != 0.0; + let has_scap = logit_softcap != 0.0; + let use_vec = flash_attn_ext_use_vec(q); + + let extra_pad = flash_attn_ext_extra_pad(q, k, v, mask)?; + let extra_blk = flash_attn_ext_extra_blk(q, mask)?; + let extra_tmp = flash_attn_ext_extra_tmp(q, v)?; + + let mut stages = Vec::new(); + + if use_vec { + let ncpsg = OP_FLASH_ATTN_EXT_VEC_NCPSG; + let has_kvpad = k.ne[1] % i64::from(ncpsg) != 0; + if has_kvpad { + let pad_base = "kernel_flash_attn_ext_pad"; + let pad_cache = format!("{}_mask={}_ncpsg={}", pad_base, bool_num(has_mask), ncpsg); + stages.push(stage( + MetalStageKind::Aux, + pad_base, + &pad_cache, + vec![ + bool_const(FC_FLASH_ATTN_EXT_PAD + 0, has_mask), + i32_const(FC_FLASH_ATTN_EXT_PAD + 25, ncpsg), + ], + 0, + 0, + 0, + 0, + false, + false, + )); + } + + let dk = k.ne[0]; + let dv = v.ne[0]; + let ns10 = k.nb[1] / k.nb[0]; + let ns20 = v.nb[1] / v.nb[0]; + let mut nsg = 1_i32; + let nwg = 32_i32; + while 2 * i64::from(nwg) * i64::from(nsg) * i64::from(ncpsg) < k.ne[1] && nsg < 4 { + nsg *= 2; + } + let vec_base = format!( + "kernel_flash_attn_ext_vec_{}_dk{}_dv{}", + k.desc.ty.name(), + dk, + dv + ); + let vec_cache = format!( + "{}_mask={}_sink={}_bias={}_scap={}_kvpad={}_ns10={}_ns20={}_nsg={}_nwg={}", + vec_base, + bool_num(has_mask), + bool_num(has_sinks), + bool_num(has_bias), + bool_num(has_scap), + bool_num(has_kvpad), + ns10, + ns20, + nsg, + nwg + ); + stages.push(stage( + MetalStageKind::Main, + &vec_base, + &vec_cache, + vec![ + bool_const(FC_FLASH_ATTN_EXT_VEC + 0, has_mask), + bool_const(FC_FLASH_ATTN_EXT_VEC + 1, has_sinks), + bool_const(FC_FLASH_ATTN_EXT_VEC + 2, has_bias), + bool_const(FC_FLASH_ATTN_EXT_VEC + 3, has_scap), + bool_const(FC_FLASH_ATTN_EXT_VEC + 4, has_kvpad), + i32_const( + FC_FLASH_ATTN_EXT_VEC + 20, + i32::try_from(ns10).map_err(|_| "flash_attn ns10 overflow".to_string())?, + ), + i32_const( + FC_FLASH_ATTN_EXT_VEC + 21, + i32::try_from(ns20).map_err(|_| "flash_attn ns20 overflow".to_string())?, + ), + i32_const(FC_FLASH_ATTN_EXT_VEC + 22, nsg), + i32_const(FC_FLASH_ATTN_EXT_VEC + 23, nwg), + ], + flash_attn_vec_smem_bytes( + usize::try_from(dk).map_err(|_| "flash_attn dk overflow".to_string())?, + usize::try_from(dv).map_err(|_| "flash_attn dv overflow".to_string())?, + nsg, + ), + 0, + 0, + nsg, + false, + false, + )); + + let reduce_base = "kernel_flash_attn_ext_vec_reduce".to_string(); + let reduce_cache = format!("{}_dv={}_nwg={}", reduce_base, dv, nwg); + stages.push(stage( + MetalStageKind::Reduce, + &reduce_base, + &reduce_cache, + vec![ + i32_const( + FC_FLASH_ATTN_EXT_VEC_REDUCE + 0, + i32::try_from(dv).map_err(|_| "flash_attn dv overflow".to_string())?, + ), + i32_const(FC_FLASH_ATTN_EXT_VEC_REDUCE + 1, nwg), + ], + 0, + 0, + 0, + 0, + false, + false, + )); + } else { + let nqptg = OP_FLASH_ATTN_EXT_NQPSG; + let ncpsg = OP_FLASH_ATTN_EXT_NCPSG; + let has_kvpad = k.ne[1] % i64::from(ncpsg) != 0; + if has_kvpad { + let pad_base = "kernel_flash_attn_ext_pad"; + let pad_cache = format!("{}_mask={}_ncpsg={}", pad_base, bool_num(has_mask), ncpsg); + stages.push(stage( + MetalStageKind::Aux, + pad_base, + &pad_cache, + vec![ + bool_const(FC_FLASH_ATTN_EXT_PAD + 0, has_mask), + i32_const(FC_FLASH_ATTN_EXT_PAD + 25, ncpsg), + ], + 0, + 0, + 0, + 0, + false, + false, + )); + } + if has_mask { + let blk_base = "kernel_flash_attn_ext_blk"; + let blk_cache = format!("{}_nqptg={}_ncpsg={}", blk_base, nqptg, ncpsg); + stages.push(stage( + MetalStageKind::Aux, + blk_base, + &blk_cache, + vec![ + i32_const(FC_FLASH_ATTN_EXT_BLK + 24, nqptg), + i32_const(FC_FLASH_ATTN_EXT_BLK + 25, ncpsg), + ], + 0, + 0, + 0, + 0, + false, + false, + )); + } + + let dk = k.ne[0]; + let dv = v.ne[0]; + let ns10 = k.nb[1] / k.nb[0]; + let ns20 = v.nb[1] / v.nb[0]; + let bc_mask = has_mask && mask.unwrap().ne[1] % 8 != 0; + let nsg = if q.ne[0] >= 512 { 8_i32 } else { 4_i32 }; + let main_base = format!( + "kernel_flash_attn_ext_{}_dk{}_dv{}", + k.desc.ty.name(), + dk, + dv + ); + let main_cache = format!( + "{}_mask={}_sinks={}_bias={}_scap={}_kvpad={}_bcm={}_ns10={}_ns20={}_nsg={}", + main_base, + bool_num(has_mask), + bool_num(has_sinks), + bool_num(has_bias), + bool_num(has_scap), + bool_num(has_kvpad), + bool_num(bc_mask), + ns10, + ns20, + nsg + ); + stages.push(stage( + MetalStageKind::Main, + &main_base, + &main_cache, + vec![ + bool_const(FC_FLASH_ATTN_EXT + 0, has_mask), + bool_const(FC_FLASH_ATTN_EXT + 1, has_sinks), + bool_const(FC_FLASH_ATTN_EXT + 2, has_bias), + bool_const(FC_FLASH_ATTN_EXT + 3, has_scap), + bool_const(FC_FLASH_ATTN_EXT + 4, has_kvpad), + bool_const(FC_FLASH_ATTN_EXT + 10, bc_mask), + i32_const( + FC_FLASH_ATTN_EXT + 20, + i32::try_from(ns10).map_err(|_| "flash_attn ns10 overflow".to_string())?, + ), + i32_const( + FC_FLASH_ATTN_EXT + 21, + i32::try_from(ns20).map_err(|_| "flash_attn ns20 overflow".to_string())?, + ), + i32_const(FC_FLASH_ATTN_EXT + 22, nsg), + ], + flash_attn_smem_bytes( + usize::try_from(dk).map_err(|_| "flash_attn dk overflow".to_string())?, + usize::try_from(dv).map_err(|_| "flash_attn dv overflow".to_string())?, + nsg, + ), + 0, + 0, + nsg, + false, + false, + )); + } + + Ok(MetalOpProgram { + stages, + resources: MetalOpResources { + output_tail_bytes: extra_pad + extra_blk + extra_tmp, + }, + }) +} + +fn src<'a>(tensors: &'a [Tensor], op: &Tensor, index: usize) -> Result<&'a Tensor, String> { + let id = op + .src + .get(index) + .and_then(|src| *src) + .ok_or_else(|| format!("op {} missing src{}", op.op.name(), index))?; + tensors + .get(id) + .ok_or_else(|| format!("op {} references invalid tensor {}", op.op.name(), id)) +} + +fn src_opt<'a>(tensors: &'a [Tensor], op: &Tensor, index: usize) -> Option<&'a Tensor> { + let id = op.src.get(index).and_then(|src| *src)?; + tensors.get(id) +} + +fn program_with_stage(stage: MetalPipelineSpec) -> MetalOpProgram { + MetalOpProgram { + stages: vec![stage], + resources: MetalOpResources::default(), + } +} + +fn stage_simple(kind: MetalStageKind, base: &str, cache: &str) -> MetalPipelineSpec { + stage(kind, base, cache, Vec::new(), 0, 0, 0, 0, false, false) +} + +fn cpy_stage(kind: MetalStageKind, src: TensorType, dst: TensorType) -> MetalPipelineSpec { + let base = format!("kernel_cpy_{}_{}", src.name(), dst.name()); + stage(kind, &base, &base, Vec::new(), 0, 0, 0, 0, false, false) +} + +fn stage( + kind: MetalStageKind, + base_name: &str, + cache_name: &str, + constants: Vec, + smem_bytes: usize, + nr0: i32, + nr1: i32, + nsg: i32, + c4: bool, + cnt: bool, +) -> MetalPipelineSpec { + MetalPipelineSpec { + kind, + descriptor: MetalPipelineDescriptor { + cache_name: cache_name.to_string(), + base_name: base_name.to_string(), + constants, + smem_bytes, + nr0, + nr1, + nsg, + }, + c4, + cnt, + } +} + +fn i16_const(idx: i32, value: i16) -> FunctionConstant { + FunctionConstant { + idx, + value: FunctionConstantValue::Int16(value), + } +} + +fn i32_const(idx: i32, value: i32) -> FunctionConstant { + FunctionConstant { + idx, + value: FunctionConstantValue::Int32(value), + } +} + +fn bool_const(idx: i32, value: bool) -> FunctionConstant { + FunctionConstant { + idx, + value: FunctionConstantValue::Bool(value), + } +} + +fn bool_num(value: bool) -> i32 { + if value { + 1 + } else { + 0 + } +} + +fn pool_name(value: i32) -> Result<&'static str, String> { + PoolOp::from_i32(value) + .map(PoolOp::name) + .ok_or_else(|| format!("unsupported pool op {}", value)) +} + +fn sort_name(value: i32) -> Result<&'static str, String> { + SortOrder::from_i32(value) + .map(SortOrder::name) + .ok_or_else(|| format!("unsupported sort order {}", value)) +} + +fn glu_kernel_name(op: &Tensor) -> Result<&'static str, String> { + let glu = op + .glu_op() + .ok_or_else(|| format!("GLU op missing subtype for {}", op.op.name()))?; + Ok(match glu { + GluOp::Reglu => "reglu", + GluOp::Geglu => "geglu", + GluOp::Swiglu => "swiglu", + GluOp::SwigluOai => "swiglu_oai", + GluOp::GegluErf => "geglu_erf", + GluOp::GegluQuick => "geglu_quick", + }) +} + +fn unary_num(op: &Tensor) -> Result { + Ok(match op.op { + Op::Scale => OP_UNARY_NUM_SCALE, + Op::Fill => OP_UNARY_NUM_FILL, + Op::Clamp => OP_UNARY_NUM_CLAMP, + Op::Sqr => OP_UNARY_NUM_SQR, + Op::Sqrt => OP_UNARY_NUM_SQRT, + Op::Sin => OP_UNARY_NUM_SIN, + Op::Cos => OP_UNARY_NUM_COS, + Op::Log => OP_UNARY_NUM_LOG, + Op::LeakyRelu => OP_UNARY_NUM_LEAKY_RELU, + Op::Unary => match op + .unary_op() + .ok_or_else(|| "missing unary subtype".to_string())? + { + UnaryOp::Tanh => OP_UNARY_NUM_TANH, + UnaryOp::Relu => OP_UNARY_NUM_RELU, + UnaryOp::Sigmoid => OP_UNARY_NUM_SIGMOID, + UnaryOp::Gelu => OP_UNARY_NUM_GELU, + UnaryOp::GeluErf => OP_UNARY_NUM_GELU_ERF, + UnaryOp::GeluQuick => OP_UNARY_NUM_GELU_QUICK, + UnaryOp::Silu => OP_UNARY_NUM_SILU, + UnaryOp::Elu => OP_UNARY_NUM_ELU, + UnaryOp::Neg => OP_UNARY_NUM_NEG, + UnaryOp::Abs => OP_UNARY_NUM_ABS, + UnaryOp::Sgn => OP_UNARY_NUM_SGN, + UnaryOp::Step => OP_UNARY_NUM_STEP, + UnaryOp::Hardswish => OP_UNARY_NUM_HARDSWISH, + UnaryOp::Hardsigmoid => OP_UNARY_NUM_HARDSIGMOID, + UnaryOp::Exp => OP_UNARY_NUM_EXP, + UnaryOp::SoftPlus => OP_UNARY_NUM_SOFTPLUS, + UnaryOp::Expm1 => OP_UNARY_NUM_EXPM1, + UnaryOp::Floor => OP_UNARY_NUM_FLOOR, + UnaryOp::Ceil => OP_UNARY_NUM_CEIL, + UnaryOp::Round => OP_UNARY_NUM_ROUND, + UnaryOp::Trunc => OP_UNARY_NUM_TRUNC, + other => { + return Err(format!( + "Metal selector does not support unary op {} yet", + other.name() + )) + } + }, + _ => return Err(format!("expected unary op, got {}", op.op.name())), + }) +} + +fn use_mul_mv_ext(src0: &Tensor, src1: &Tensor) -> bool { + src1.desc.ty == TensorType::F32 + && src0.ne[0] % 128 == 0 + && ((matches!( + src0.desc.ty, + TensorType::F32 + | TensorType::F16 + | TensorType::BF16 + | TensorType::Q4_0 + | TensorType::Q4_1 + | TensorType::Q5_0 + | TensorType::Q5_1 + | TensorType::Q8_0 + | TensorType::MXFP4 + | TensorType::IQ4Nl + ) && (2..=8).contains(&src1.ne[1])) + || (matches!( + src0.desc.ty, + TensorType::Q4K + | TensorType::Q5K + | TensorType::Q6K + | TensorType::Q2K + | TensorType::Q3K + ) && (4..=8).contains(&src1.ne[1]))) +} + +struct MulMvParams { + nsg: i32, + nr0: i32, + nr1: i32, + smem: usize, + suffix: &'static str, +} + +fn mul_mv_params(ty: TensorType, ne00: i64, allow_short: bool) -> Result { + let (nsg, nr0, nr1, smem, suffix) = match ty { + TensorType::F32 | TensorType::F16 | TensorType::BF16 => { + if allow_short && ne00 < 32 { + (1, 32, 1, 0, "_short") + } else { + let nsg = i32::min( + 4, + i32::try_from((ne00 + 127) / 128) + .map_err(|_| "mul_mv nsg overflow".to_string())?, + ); + let suffix = if ne00 % 4 == 0 { "_4" } else { "" }; + (nsg, 2, 1, 32 * std::mem::size_of::() * 2, suffix) + } + } + TensorType::Q4_0 => (N_SG_Q4_0, N_R0_Q4_0, 1, 0, ""), + TensorType::Q4_1 => (N_SG_Q4_1, N_R0_Q4_1, 1, 0, ""), + TensorType::Q5_0 => (N_SG_Q5_0, N_R0_Q5_0, 1, 0, ""), + TensorType::Q5_1 => (N_SG_Q5_1, N_R0_Q5_1, 1, 0, ""), + TensorType::Q8_0 => ( + N_SG_Q8_0, + N_R0_Q8_0, + 1, + 32 * std::mem::size_of::() * usize::try_from(N_R0_Q8_0).unwrap_or(0), + "", + ), + TensorType::MXFP4 => ( + N_SG_MXFP4, + N_R0_MXFP4, + 1, + 32 * std::mem::size_of::(), + "", + ), + TensorType::Q2K => (N_SG_Q2_K, N_R0_Q2_K, 1, 0, ""), + TensorType::Q3K => (N_SG_Q3_K, N_R0_Q3_K, 1, 0, ""), + TensorType::Q4K => (N_SG_Q4_K, N_R0_Q4_K, 1, 0, ""), + TensorType::Q5K => (N_SG_Q5_K, N_R0_Q5_K, 1, 0, ""), + TensorType::Q6K => (N_SG_Q6_K, N_R0_Q6_K, 1, 0, ""), + TensorType::IQ2Xxs => (N_SG_IQ2_XXS, N_R0_IQ2_XXS, 1, 256 * 8 + 128, ""), + TensorType::IQ2Xs => (N_SG_IQ2_XS, N_R0_IQ2_XS, 1, 512 * 8 + 128, ""), + TensorType::IQ3Xxs => (N_SG_IQ3_XXS, N_R0_IQ3_XXS, 1, 256 * 4 + 128, ""), + TensorType::IQ3S => (N_SG_IQ3_S, N_R0_IQ3_S, 1, 512 * 4, ""), + TensorType::IQ2S => (N_SG_IQ2_S, N_R0_IQ2_S, 1, 0, ""), + TensorType::IQ1S => (N_SG_IQ1_S, N_R0_IQ1_S, 1, 0, ""), + TensorType::IQ1M => (N_SG_IQ1_M, N_R0_IQ1_M, 1, 0, ""), + TensorType::IQ4Nl => ( + N_SG_IQ4_NL, + N_R0_IQ4_NL, + 1, + 32 * std::mem::size_of::(), + "", + ), + TensorType::IQ4Xs => ( + N_SG_IQ4_XS, + N_R0_IQ4_XS, + 1, + 32 * std::mem::size_of::(), + "", + ), + other => { + return Err(format!( + "unsupported Metal mul_mv source type {}", + other.name() + )) + } + }; + + Ok(MulMvParams { + nsg, + nr0, + nr1, + smem, + suffix, + }) +} + +fn flash_attn_ext_use_vec(q: &Tensor) -> bool { + q.ne[1] < 20 && q.ne[0] % 32 == 0 +} + +fn pad_to(v: usize, align: usize) -> usize { + if align == 0 { + return v; + } + let rem = v % align; + if rem == 0 { + v + } else { + v + (align - rem) + } +} + +fn flash_attn_smem_bytes(dk: usize, dv: usize, _nsg: i32) -> usize { + let nqptg = OP_FLASH_ATTN_EXT_NQPSG as usize; + let ncpsg = OP_FLASH_ATTN_EXT_NCPSG as usize; + let words = nqptg.saturating_mul(dk + 2 * pad_to(dv, 64) + 2 * (2 * ncpsg)); + pad_to(words.saturating_mul(std::mem::size_of::() / 2), 16) +} + +fn flash_attn_vec_smem_bytes(dk: usize, dv: usize, nsg: i32) -> usize { + let ncpsg = OP_FLASH_ATTN_EXT_VEC_NCPSG as usize; + let words = + (pad_to(dk, 128) + 4 * ncpsg + 2 * pad_to(dv, 128)).saturating_mul(nsg.max(1) as usize); + pad_to(words.saturating_mul(std::mem::size_of::() / 2), 16) +} + +fn flash_attn_ext_extra_pad( + q: &Tensor, + k: &Tensor, + v: &Tensor, + mask: Option<&Tensor>, +) -> Result { + let has_mask = mask.is_some(); + let has_kvpad = true; + if !has_kvpad { + return Ok(0); + } + let mask_bytes = if has_mask { + ggml_type_size_for_type(TensorType::F16) + * usize::try_from(mask.unwrap().ne[1] * mask.unwrap().ne[2] * mask.unwrap().ne[3]) + .map_err(|_| "flash_attn pad mask bytes overflow".to_string())? + } else { + 0 + }; + let ncpsg = if flash_attn_ext_use_vec(q) { + OP_FLASH_ATTN_EXT_VEC_NCPSG + } else { + OP_FLASH_ATTN_EXT_NCPSG + }; + Ok( + usize::try_from(ncpsg).map_err(|_| "flash_attn ncpsg overflow".to_string())? + * (k.nb[1] + * usize::try_from(k.ne[2] * k.ne[3]) + .map_err(|_| "flash_attn k dims overflow".to_string())? + + v.nb[1] + * usize::try_from(v.ne[2] * v.ne[3]) + .map_err(|_| "flash_attn v dims overflow".to_string())? + + mask_bytes), + ) +} + +fn flash_attn_ext_extra_blk(q: &Tensor, mask: Option<&Tensor>) -> Result { + let Some(mask) = mask else { + return Ok(0); + }; + let is_vec = flash_attn_ext_use_vec(q); + let nqptg = if is_vec { + OP_FLASH_ATTN_EXT_VEC_NQPSG + } else { + OP_FLASH_ATTN_EXT_NQPSG + }; + let ncpsg = if is_vec { + OP_FLASH_ATTN_EXT_VEC_NCPSG + } else { + OP_FLASH_ATTN_EXT_NCPSG + }; + let ne1 = (q.ne[1] + i64::from(nqptg) - 1) / i64::from(nqptg); + let ne0 = (mask.ne[0] + i64::from(ncpsg) - 1) / i64::from(ncpsg); + let bytes = usize::try_from(ne0 * ne1 * mask.ne[2] * mask.ne[3]) + .map_err(|_| "flash_attn blk bytes overflow".to_string())?; + Ok(ggml_pad(bytes * std::mem::size_of::(), 32)) +} + +fn flash_attn_ext_extra_tmp(q: &Tensor, v: &Tensor) -> Result { + let nwg = 32_i64; + let ne01_max = q.ne[1].min(32); + let items = ne01_max * q.ne[2] * q.ne[3] * nwg * (v.ne[0] + 2); + Ok(ggml_type_size_for_type(TensorType::F32) + * usize::try_from(items).map_err(|_| "flash_attn tmp bytes overflow".to_string())?) +} + +fn mul_mat_id_extra_tpe(src0: &Tensor) -> Result { + Ok(ggml_type_size_for_type(TensorType::I32) + * usize::try_from(src0.ne[2]).map_err(|_| "mul_mat_id tpe bytes overflow".to_string())?) +} + +fn mul_mat_id_extra_ids(src0: &Tensor, src2: &Tensor) -> Result { + Ok(ggml_type_size_for_type(TensorType::I32) + * usize::try_from(src0.ne[2] * src2.ne[1]) + .map_err(|_| "mul_mat_id ids bytes overflow".to_string())?) +} + +#[cfg(test)] +mod tests { + use super::*; + use crate::context::Context; + use crate::core::{InitParams, GGML_ROPE_TYPE_NEOX, GGML_ROPE_TYPE_NORMAL}; + use crate::graph::Graph; + use crate::tensor::{BufferUsage, TensorDesc, TensorLayout}; + + fn ctx() -> Context { + Context::new(InitParams { + mem_size: 0, + mem_buffer: None, + no_alloc: true, + }) + } + + fn tensor(ctx: &mut Context, ty: TensorType, ne: &[i64]) -> usize { + let layout = TensorLayout::for_ggml(ty, ne).unwrap(); + let desc = TensorDesc::new(ty, layout, BufferUsage::Activations); + ctx.new_op_tensor(desc, Op::None, &[]).unwrap() + } + + #[test] + fn unary_selector_matches_kernel_name() { + let mut ctx = ctx(); + let src = tensor(&mut ctx, TensorType::F32, &[128, 1, 1, 1]); + let dst = tensor(&mut ctx, TensorType::F32, &[128, 1, 1, 1]); + let tensors = ctx.tensors().to_vec(); + let mut op = tensors[dst].clone(); + op.op = Op::Unary; + op.src[0] = Some(src); + op.set_unary_op(UnaryOp::Gelu); + + let program = build_program(&tensors, &op, MetalDeviceFeatures::default()).unwrap(); + assert_eq!( + program.stages[0].descriptor.base_name, + "kernel_unary_f32_f32_4" + ); + assert_eq!( + program.stages[0].descriptor.cache_name, + "kernel_unary_f32_f32_4_op=103_cnt=1" + ); + } + + #[test] + fn rope_selector_picks_neox_kernel() { + let mut ctx = ctx(); + let src = tensor(&mut ctx, TensorType::F32, &[128, 8, 1, 1]); + let pos = tensor(&mut ctx, TensorType::I32, &[32, 1, 1, 1]); + let dst = tensor(&mut ctx, TensorType::F32, &[128, 8, 1, 1]); + let tensors = ctx.tensors().to_vec(); + let mut op = tensors[dst].clone(); + op.op = Op::Rope; + op.src[0] = Some(src); + op.src[1] = Some(pos); + op.set_op_param_i32(2, GGML_ROPE_TYPE_NEOX); + + let program = build_program(&tensors, &op, MetalDeviceFeatures::default()).unwrap(); + assert_eq!( + program.stages[0].descriptor.base_name, + "kernel_rope_neox_f32" + ); + } + + #[test] + fn rope_selector_picks_normal_kernel() { + let mut ctx = ctx(); + let src = tensor(&mut ctx, TensorType::F32, &[128, 8, 1, 1]); + let pos = tensor(&mut ctx, TensorType::I32, &[32, 1, 1, 1]); + let dst = tensor(&mut ctx, TensorType::F32, &[128, 8, 1, 1]); + let tensors = ctx.tensors().to_vec(); + let mut op = tensors[dst].clone(); + op.op = Op::Rope; + op.src[0] = Some(src); + op.src[1] = Some(pos); + op.set_op_param_i32(2, GGML_ROPE_TYPE_NORMAL); + + let program = build_program(&tensors, &op, MetalDeviceFeatures::default()).unwrap(); + assert_eq!( + program.stages[0].descriptor.base_name, + "kernel_rope_norm_f32" + ); + } + + #[test] + fn argsort_selector_adds_merge_and_temp() { + let mut ctx = ctx(); + let src = tensor(&mut ctx, TensorType::F32, &[257, 4, 1, 1]); + let dst = tensor(&mut ctx, TensorType::I32, &[257, 4, 1, 1]); + let tensors = ctx.tensors().to_vec(); + let mut op = tensors[dst].clone(); + op.op = Op::Argsort; + op.src[0] = Some(src); + op.set_op_param_i32(0, SortOrder::Desc as i32); + + let program = build_program(&tensors, &op, MetalDeviceFeatures::default()).unwrap(); + assert_eq!(program.stages.len(), 2); + assert_eq!(program.resources.output_tail_bytes, op.nbytes()); + } + + #[test] + fn mul_mat_selector_uses_mul_mm_when_feature_allows() { + let mut ctx = ctx(); + let a = tensor(&mut ctx, TensorType::F16, &[128, 64, 1, 1]); + let b = tensor(&mut ctx, TensorType::F32, &[128, 16, 1, 1]); + let dst = tensor(&mut ctx, TensorType::F32, &[64, 16, 1, 1]); + let tensors = ctx.tensors().to_vec(); + let mut op = tensors[dst].clone(); + op.op = Op::MulMat; + op.src[0] = Some(a); + op.src[1] = Some(b); + + let program = build_program( + &tensors, + &op, + MetalDeviceFeatures { + has_simdgroup_mm: true, + ..MetalDeviceFeatures::default() + }, + ) + .unwrap(); + assert_eq!( + program.stages[0].descriptor.base_name, + "kernel_mul_mm_f16_f32" + ); + } + + #[test] + fn flash_attn_selector_builds_vec_pipeline_and_reduce() { + let mut ctx = ctx(); + let q = tensor(&mut ctx, TensorType::F32, &[128, 8, 2, 1]); + let k = tensor(&mut ctx, TensorType::F16, &[128, 64, 2, 1]); + let v = tensor(&mut ctx, TensorType::F16, &[128, 64, 2, 1]); + let dst = tensor(&mut ctx, TensorType::F32, &[128, 8, 2, 1]); + let tensors = ctx.tensors().to_vec(); + let mut op = tensors[dst].clone(); + op.op = Op::FlashAttnExt; + op.src[0] = Some(q); + op.src[1] = Some(k); + op.src[2] = Some(v); + + let program = build_program(&tensors, &op, MetalDeviceFeatures::default()).unwrap(); + assert_eq!(program.stages.len(), 2); + assert!(program.stages[0] + .descriptor + .base_name + .starts_with("kernel_flash_attn_ext_vec_f16_dk128_dv128")); + assert_eq!( + program.stages[1].descriptor.base_name, + "kernel_flash_attn_ext_vec_reduce" + ); + assert!(program.resources.output_tail_bytes > 0); + } + + #[test] + fn graph_plan_accumulates_tail_bytes() { + let mut ctx = ctx(); + let src = tensor(&mut ctx, TensorType::F32, &[257, 4, 1, 1]); + let dst = tensor(&mut ctx, TensorType::I32, &[257, 4, 1, 1]); + + { + let op = ctx.tensor_mut(dst).unwrap(); + op.op = Op::Argsort; + op.src[0] = Some(src); + op.set_op_param_i32(0, SortOrder::Asc as i32); + } + + let mut graph = Graph::new(); + graph.add_node(dst); + + let plan = build_graph_plan(&ctx, &graph, MetalDeviceFeatures::default()).unwrap(); + assert_eq!(plan.nodes.len(), 1); + assert_eq!( + plan.total_output_tail_bytes, + ctx.tensor(dst).unwrap().nbytes() + ); + } + + #[test] + fn graph_plan_skips_metadata_only_nodes() { + let mut ctx = ctx(); + let src = tensor(&mut ctx, TensorType::F32, &[16, 8, 1, 1]); + let view = ctx + .view(src, TensorType::F32, &[8, 8], &[4, 64], 32) + .unwrap(); + let cont = ctx.cont(view).unwrap(); + + let mut graph = Graph::new(); + graph.build_forward_expand(&ctx, cont).unwrap(); + + let plan = build_graph_plan(&ctx, &graph, MetalDeviceFeatures::default()).unwrap(); + assert_eq!(plan.nodes.len(), 1); + assert_eq!(plan.nodes[0].node_id, cont); + } +} diff --git a/libs/ggml/src/backend/mod.rs b/libs/ggml/src/backend/mod.rs new file mode 100644 index 0000000000..d86ba844be --- /dev/null +++ b/libs/ggml/src/backend/mod.rs @@ -0,0 +1,127 @@ +use crate::core::Status; +use crate::graph::Graph; +use crate::tensor::Tensor; + +#[derive(Clone, Copy, Debug, Eq, PartialEq, Hash)] +pub enum BackendKind { + Metal, + Vulkan, + Cuda, +} + +#[derive(Clone, Debug, Default, Eq, PartialEq)] +pub struct BackendCapabilities { + pub bf16: bool, + pub tensor_cores: bool, + pub max_buffer_size: Option, + pub max_threadgroup_memory: Option, + pub subgroup_width: Option, + pub asynchronous: bool, + pub host_buffer: bool, + pub buffer_from_host_ptr: bool, + pub events: bool, +} + +#[derive(Clone, Debug, Eq, PartialEq)] +pub struct BackendInfo { + pub kind: BackendKind, + pub name: String, + pub description: String, + pub capabilities: BackendCapabilities, +} + +#[derive(Clone, Copy, Debug, Eq, PartialEq, Hash)] +pub enum BackendBufferUsage { + Any, + Weights, + Compute, +} + +#[derive(Clone, Copy, Debug, Eq, PartialEq, Hash)] +pub enum BackendDeviceType { + Cpu, + Gpu, + Igpu, + Accel, +} + +#[derive(Clone, Debug, Default, Eq, PartialEq)] +pub struct BackendDeviceCaps { + pub asynchronous: bool, + pub host_buffer: bool, + pub buffer_from_host_ptr: bool, + pub events: bool, +} + +#[derive(Clone, Debug, Default, Eq, PartialEq)] +pub struct BackendDeviceProps { + pub name: String, + pub description: String, + pub memory_free: usize, + pub memory_total: usize, + pub device_type: Option, + pub device_id: Option, + pub caps: BackendDeviceCaps, +} + +pub trait BackendBufferType { + fn name(&self) -> &str; + fn alignment(&self) -> usize; + fn max_size(&self) -> Option { + None + } + fn is_host(&self) -> bool { + false + } + fn alloc_size(&self, tensor: &Tensor) -> usize { + tensor.nbytes() + } +} + +pub trait BackendBuffer { + fn name(&self) -> &str; + fn size(&self) -> usize; + fn alignment(&self) -> usize; + fn max_size(&self) -> Option { + None + } + fn is_host(&self) -> bool { + false + } + fn usage(&self) -> BackendBufferUsage { + BackendBufferUsage::Any + } + fn set_usage(&mut self, _usage: BackendBufferUsage) {} + fn clear(&mut self, _value: u8) {} +} + +pub trait BackendGraphPlan {} + +pub trait BackendEvent { + fn synchronize(&self) -> Result<(), String>; +} + +pub trait Backend { + type BufferType: BackendBufferType; + type Buffer: BackendBuffer; + type Event: BackendEvent; + type GraphPlan: BackendGraphPlan; + + fn info(&self) -> &BackendInfo; + fn default_buffer_type(&self) -> &Self::BufferType; + fn alloc_buffer(&self, size: usize) -> Result; + fn alignment(&self) -> usize; + fn max_size(&self) -> Option; + fn synchronize(&self) -> Result<(), String>; + fn graph_plan_create(&self, _graph: &Graph) -> Result { + Err("graph planning not implemented".to_string()) + } + fn graph_plan_compute(&self, _plan: &Self::GraphPlan) -> Status { + Status::Failed + } + fn graph_compute(&self, _graph: &Graph) -> Status { + Status::Failed + } +} + +pub mod metal; diff --git a/libs/ggml/src/context.rs b/libs/ggml/src/context.rs new file mode 100644 index 0000000000..421acb7b0e --- /dev/null +++ b/libs/ggml/src/context.rs @@ -0,0 +1,1928 @@ +use std::collections::HashMap; + +use crate::core::{ggml_pad, InitParams, GGML_MEM_ALIGN, GGML_MROPE_SECTIONS}; +use crate::op::{GluOp, Op, Prec, UnaryOp}; +use crate::tensor::{ + ggml_type_size_for_type, BufferUsage, Tensor, TensorDesc, TensorId, TensorLayout, TensorType, +}; + +#[derive(Clone, Debug)] +pub struct Context { + mem_size: usize, + mem_buffer: Vec, + no_alloc: bool, + next_data_offset: usize, + max_tensor_size: usize, + tensors: Vec, + name_to_tensor: HashMap, +} + +impl Context { + pub fn new(params: InitParams) -> Self { + let mem_buffer = match params.mem_buffer { + Some(buffer) => buffer, + None if params.mem_size > 0 => vec![0; params.mem_size], + None => Vec::new(), + }; + + let mem_size = params.mem_size.max(mem_buffer.len()); + + Self { + mem_size, + mem_buffer, + no_alloc: params.no_alloc, + next_data_offset: 0, + max_tensor_size: 0, + tensors: Vec::new(), + name_to_tensor: HashMap::new(), + } + } + + pub fn reset(&mut self) { + self.next_data_offset = 0; + self.max_tensor_size = 0; + self.tensors.clear(); + self.name_to_tensor.clear(); + } + + pub fn used_mem(&self) -> usize { + self.next_data_offset + } + + pub fn get_no_alloc(&self) -> bool { + self.no_alloc + } + + pub fn set_no_alloc(&mut self, no_alloc: bool) { + self.no_alloc = no_alloc; + } + + pub fn mem_buffer(&self) -> &[u8] { + &self.mem_buffer + } + + pub fn mem_buffer_mut(&mut self) -> &mut [u8] { + &mut self.mem_buffer + } + + pub fn mem_size(&self) -> usize { + self.mem_size + } + + pub fn max_tensor_size(&self) -> usize { + self.max_tensor_size + } + + pub fn tensors(&self) -> &[Tensor] { + &self.tensors + } + + pub fn tensor(&self, id: TensorId) -> Option<&Tensor> { + self.tensors.get(id) + } + + pub fn tensor_mut(&mut self, id: TensorId) -> Option<&mut Tensor> { + self.tensors.get_mut(id) + } + + pub fn get_tensor(&self, name: &str) -> Option { + self.name_to_tensor.get(name).copied() + } + + pub fn new_buffer(&mut self, nbytes: usize) -> Result { + let offset = ggml_pad(self.next_data_offset, GGML_MEM_ALIGN); + let end = offset + .checked_add(nbytes) + .ok_or_else(|| "buffer allocation overflow".to_string())?; + if end > self.mem_size { + return Err(format!( + "context buffer too small: need {} bytes, have {} bytes", + end, self.mem_size + )); + } + self.next_data_offset = end; + Ok(offset) + } + + pub fn new_tensor( + &mut self, + ty: TensorType, + n_dims: usize, + ne: &[i64], + usage: BufferUsage, + ) -> Result { + if n_dims == 0 || n_dims > 4 { + return Err(format!("invalid tensor rank {}", n_dims)); + } + if ne.len() != n_dims { + return Err(format!("rank {} but got {} extents", n_dims, ne.len())); + } + + let layout = TensorLayout::for_ggml(ty, ne)?; + let desc = TensorDesc::new(ty, layout, usage); + self.push_tensor(Tensor::from_desc(self.tensors.len(), desc), true) + } + + pub fn new_named_tensor( + &mut self, + name: impl Into, + ty: TensorType, + n_dims: usize, + ne: &[i64], + usage: BufferUsage, + ) -> Result { + if n_dims == 0 || n_dims > 4 { + return Err(format!("invalid tensor rank {}", n_dims)); + } + if ne.len() != n_dims { + return Err(format!("rank {} but got {} extents", n_dims, ne.len())); + } + + let layout = TensorLayout::for_ggml(ty, ne)?; + let desc = TensorDesc::new(ty, layout, usage).with_name(name); + self.push_tensor(Tensor::from_desc(self.tensors.len(), desc), true) + } + + pub fn new_tensor_1d( + &mut self, + ty: TensorType, + ne0: i64, + usage: BufferUsage, + ) -> Result { + self.new_tensor(ty, 1, &[ne0], usage) + } + + pub fn new_tensor_2d( + &mut self, + ty: TensorType, + ne0: i64, + ne1: i64, + usage: BufferUsage, + ) -> Result { + self.new_tensor(ty, 2, &[ne0, ne1], usage) + } + + pub fn new_tensor_3d( + &mut self, + ty: TensorType, + ne0: i64, + ne1: i64, + ne2: i64, + usage: BufferUsage, + ) -> Result { + self.new_tensor(ty, 3, &[ne0, ne1, ne2], usage) + } + + pub fn new_tensor_4d( + &mut self, + ty: TensorType, + ne0: i64, + ne1: i64, + ne2: i64, + ne3: i64, + usage: BufferUsage, + ) -> Result { + self.new_tensor(ty, 4, &[ne0, ne1, ne2, ne3], usage) + } + + pub fn dup_tensor(&mut self, src: TensorId) -> Result { + let tensor = self + .tensor(src) + .ok_or_else(|| format!("invalid tensor id {}", src))?; + let desc = TensorDesc::new( + tensor.desc.ty, + tensor.desc.layout.clone(), + tensor.desc.usage, + ); + self.push_tensor(Tensor::from_desc(self.tensors.len(), desc), true) + } + + pub fn view_tensor(&mut self, src: TensorId) -> Result { + let tensor = self + .tensor(src) + .ok_or_else(|| format!("invalid tensor id {}", src))?; + let mut view = Tensor::from_desc( + self.tensors.len(), + TensorDesc::new( + tensor.desc.ty, + tensor.desc.layout.clone(), + tensor.desc.usage, + ), + ); + view.view_src = Some(src); + view.view_offs = 0; + view.buffer_id = tensor.buffer_id; + view.data_offset = tensor.data_offset; + self.push_tensor(view, false) + } + + pub fn set_tensor_name(&mut self, id: TensorId, name: impl Into) -> Result<(), String> { + let existing = self + .tensor(id) + .ok_or_else(|| format!("invalid tensor id {}", id))? + .name() + .map(str::to_owned); + if let Some(existing) = existing { + self.name_to_tensor.remove(&existing); + } + let new_name = { + let tensor = self + .tensor_mut(id) + .ok_or_else(|| format!("invalid tensor id {}", id))?; + tensor.set_name(name.into()); + tensor.name().map(str::to_owned) + }; + if let Some(name) = new_name { + self.name_to_tensor.insert(name, id); + } + Ok(()) + } + + pub fn set_tensor_layout(&mut self, id: TensorId, layout: TensorLayout) -> Result<(), String> { + let tensor = self + .tensor_mut(id) + .ok_or_else(|| format!("invalid tensor id {}", id))?; + tensor.ne = layout.extents4(); + tensor.nb = layout.strides_bytes(); + tensor.desc.layout = layout; + Ok(()) + } + + pub fn tensor_data(&self, id: TensorId) -> Result<&[u8], String> { + let tensor = self + .tensor(id) + .ok_or_else(|| format!("invalid tensor id {}", id))?; + let offset = tensor + .data_offset + .ok_or_else(|| format!("tensor {} has no allocated data offset", id))?; + let end = offset + .checked_add(tensor.nbytes()) + .ok_or_else(|| format!("tensor {} byte range overflow", id))?; + self.mem_buffer.get(offset..end).ok_or_else(|| { + format!( + "tensor {} byte range [{}..{}) is out of bounds", + id, offset, end + ) + }) + } + + pub fn tensor_data_mut(&mut self, id: TensorId) -> Result<&mut [u8], String> { + let tensor = self + .tensor(id) + .ok_or_else(|| format!("invalid tensor id {}", id))?; + let offset = tensor + .data_offset + .ok_or_else(|| format!("tensor {} has no allocated data offset", id))?; + let end = offset + .checked_add(tensor.nbytes()) + .ok_or_else(|| format!("tensor {} byte range overflow", id))?; + self.mem_buffer.get_mut(offset..end).ok_or_else(|| { + format!( + "tensor {} byte range [{}..{}) is out of bounds", + id, offset, end + ) + }) + } + + pub fn write_tensor_data(&mut self, id: TensorId, bytes: &[u8]) -> Result<(), String> { + let dst = self.tensor_data_mut(id)?; + if dst.len() != bytes.len() { + return Err(format!( + "tensor {} byte length mismatch: dst={} src={}", + id, + dst.len(), + bytes.len() + )); + } + dst.copy_from_slice(bytes); + Ok(()) + } + + pub fn view( + &mut self, + src: TensorId, + ty: TensorType, + extents: &[i64], + strides_bytes: &[usize], + offset_bytes: usize, + ) -> Result { + let usage = self + .tensor(src) + .ok_or_else(|| format!("invalid tensor id {}", src))? + .desc + .usage; + let layout = TensorLayout::from_parts(extents.len(), extents, strides_bytes)?; + let mut tensor = Tensor::from_desc(self.tensors.len(), TensorDesc::new(ty, layout, usage)); + tensor.op = Op::View; + tensor.view_src = Some(src); + tensor.view_offs = offset_bytes; + tensor.src[0] = Some(src); + self.push_tensor(tensor, false) + } + + pub fn view_1d( + &mut self, + src: TensorId, + ne0: i64, + offset_bytes: usize, + ) -> Result { + let ty = self + .tensor(src) + .ok_or_else(|| format!("invalid tensor id {}", src))? + .desc + .ty; + self.view( + src, + ty, + &[ne0], + &[ggml_type_size_for_type(ty)], + offset_bytes, + ) + } + + pub fn view_2d( + &mut self, + src: TensorId, + ne0: i64, + ne1: i64, + nb1: usize, + offset_bytes: usize, + ) -> Result { + let ty = self + .tensor(src) + .ok_or_else(|| format!("invalid tensor id {}", src))? + .desc + .ty; + self.view( + src, + ty, + &[ne0, ne1], + &[ggml_type_size_for_type(ty), nb1], + offset_bytes, + ) + } + + pub fn view_3d( + &mut self, + src: TensorId, + ne0: i64, + ne1: i64, + ne2: i64, + nb1: usize, + nb2: usize, + offset_bytes: usize, + ) -> Result { + let ty = self + .tensor(src) + .ok_or_else(|| format!("invalid tensor id {}", src))? + .desc + .ty; + self.view( + src, + ty, + &[ne0, ne1, ne2], + &[ggml_type_size_for_type(ty), nb1, nb2], + offset_bytes, + ) + } + + pub fn view_4d( + &mut self, + src: TensorId, + ne0: i64, + ne1: i64, + ne2: i64, + ne3: i64, + nb1: usize, + nb2: usize, + nb3: usize, + offset_bytes: usize, + ) -> Result { + let ty = self + .tensor(src) + .ok_or_else(|| format!("invalid tensor id {}", src))? + .desc + .ty; + self.view( + src, + ty, + &[ne0, ne1, ne2, ne3], + &[ggml_type_size_for_type(ty), nb1, nb2, nb3], + offset_bytes, + ) + } + + pub fn new_view_op_tensor( + &mut self, + desc: TensorDesc, + op: Op, + src: TensorId, + offset_bytes: usize, + ) -> Result { + self.new_view_op_tensor_with_srcs(desc, op, src, offset_bytes, &[src]) + } + + pub fn new_view_op_tensor_with_srcs( + &mut self, + desc: TensorDesc, + op: Op, + view_src: TensorId, + offset_bytes: usize, + srcs: &[TensorId], + ) -> Result { + let source = self + .tensor(view_src) + .ok_or_else(|| format!("invalid tensor id {}", view_src))?; + let mut tensor = Tensor::from_desc(self.tensors.len(), desc); + tensor.op = op; + tensor.view_src = Some(view_src); + tensor.view_offs = offset_bytes; + tensor.buffer_id = source.buffer_id; + tensor.data_offset = source.data_offset; + for (i, src) in srcs.iter().copied().enumerate() { + if i >= tensor.src.len() { + return Err(format!("too many op sources: {}", srcs.len())); + } + tensor.src[i] = Some(src); + } + self.push_tensor(tensor, false) + } + + pub fn new_op_tensor( + &mut self, + desc: TensorDesc, + op: Op, + srcs: &[TensorId], + ) -> Result { + let mut tensor = Tensor::from_desc(self.tensors.len(), desc); + tensor.op = op; + for (i, src) in srcs.iter().copied().enumerate() { + if i >= tensor.src.len() { + return Err(format!("too many op sources: {}", srcs.len())); + } + tensor.src[i] = Some(src); + } + self.push_tensor(tensor, true) + } + + pub fn unary( + &mut self, + a: TensorId, + unary: UnaryOp, + usage: BufferUsage, + ) -> Result { + let src = self + .tensor(a) + .ok_or_else(|| format!("invalid tensor id {}", a))?; + let id = self.new_op_tensor( + TensorDesc::new(src.desc.ty, src.desc.layout.clone(), usage), + Op::Unary, + &[a], + )?; + self.tensor_mut(id).unwrap().set_unary_op(unary); + Ok(id) + } + + pub fn clamp( + &mut self, + src: TensorId, + min: f32, + max: f32, + usage: BufferUsage, + ) -> Result { + let tensor = self + .tensor(src) + .ok_or_else(|| format!("invalid tensor id {}", src))?; + let id = self.new_op_tensor( + TensorDesc::new(tensor.desc.ty, tensor.desc.layout.clone(), usage), + Op::Clamp, + &[src], + )?; + let tensor = self.tensor_mut(id).unwrap(); + tensor.set_op_param_f32(4, min); + tensor.set_op_param_f32(5, max); + Ok(id) + } + + pub fn scale( + &mut self, + src: TensorId, + scale: f32, + usage: BufferUsage, + ) -> Result { + let tensor = self + .tensor(src) + .ok_or_else(|| format!("invalid tensor id {}", src))?; + let id = self.new_op_tensor( + TensorDesc::new(tensor.desc.ty, tensor.desc.layout.clone(), usage), + Op::Scale, + &[src], + )?; + self.tensor_mut(id).unwrap().set_op_param_f32(1, scale); + Ok(id) + } + + pub fn glu(&mut self, a: TensorId, glu: GluOp, usage: BufferUsage) -> Result { + let src = self + .tensor(a) + .ok_or_else(|| format!("invalid tensor id {}", a))?; + let id = self.new_op_tensor( + TensorDesc::new(src.desc.ty, src.desc.layout.clone(), usage), + Op::Glu, + &[a], + )?; + self.tensor_mut(id).unwrap().set_glu_op(glu); + Ok(id) + } + + pub fn binary_like_a( + &mut self, + op: Op, + a: TensorId, + b: TensorId, + usage: BufferUsage, + ) -> Result { + let src = self + .tensor(a) + .ok_or_else(|| format!("invalid tensor id {}", a))?; + self.new_op_tensor( + TensorDesc::new(src.desc.ty, src.desc.layout.clone(), usage), + op, + &[a, b], + ) + } + + pub fn mul_mat( + &mut self, + a: TensorId, + b: TensorId, + usage: BufferUsage, + ) -> Result { + let ta = self + .tensor(a) + .ok_or_else(|| format!("invalid tensor id {}", a))?; + let tb = self + .tensor(b) + .ok_or_else(|| format!("invalid tensor id {}", b))?; + let ne = [ta.ne[1], tb.ne[1], tb.ne[2], tb.ne[3]]; + let layout = TensorLayout::for_ggml(TensorType::F32, &ne)?; + self.new_op_tensor( + TensorDesc::new(TensorType::F32, layout, usage), + Op::MulMat, + &[a, b], + ) + } + + pub fn mul_mat_id( + &mut self, + as_tensor: TensorId, + b: TensorId, + ids: TensorId, + usage: BufferUsage, + ) -> Result { + let as_ref = self + .tensor(as_tensor) + .ok_or_else(|| format!("invalid tensor id {}", as_tensor))?; + let b_ref = self + .tensor(b) + .ok_or_else(|| format!("invalid tensor id {}", b))?; + let ids_ref = self + .tensor(ids) + .ok_or_else(|| format!("invalid tensor id {}", ids))?; + + if ids_ref.desc.ty != TensorType::I32 { + return Err(format!( + "mul_mat_id ids tensor must be I32, got {}", + ids_ref.desc.ty.name() + )); + } + if as_ref.is_transposed() { + return Err("mul_mat_id expert tensor must not be transposed".to_string()); + } + if b_ref.is_transposed() { + return Err("mul_mat_id input tensor must not be transposed".to_string()); + } + if as_ref.ne[3] != 1 { + return Err(format!( + "mul_mat_id expert tensor must be 3D, got ne3={}", + as_ref.ne[3] + )); + } + if b_ref.ne[3] != 1 { + return Err(format!( + "mul_mat_id input tensor must be 3D, got ne3={}", + b_ref.ne[3] + )); + } + if ids_ref.ne[2] != 1 || ids_ref.ne[3] != 1 { + return Err(format!( + "mul_mat_id ids tensor must be 2D, got ne2={} ne3={}", + ids_ref.ne[2], ids_ref.ne[3] + )); + } + if ids_ref.ne[1] != b_ref.ne[2] { + return Err(format!( + "mul_mat_id ids rows {} do not match input tokens {}", + ids_ref.ne[1], b_ref.ne[2] + )); + } + if as_ref.ne[0] != b_ref.ne[0] { + return Err(format!( + "mul_mat_id source width mismatch: experts={} input={}", + as_ref.ne[0], b_ref.ne[0] + )); + } + if ids_ref.ne[0] % b_ref.ne[1] != 0 { + return Err(format!( + "mul_mat_id ids expert count {} is not broadcast-compatible with input experts {}", + ids_ref.ne[0], b_ref.ne[1] + )); + } + + let layout = TensorLayout::for_ggml( + TensorType::F32, + &[as_ref.ne[1], ids_ref.ne[0], b_ref.ne[2], 1], + )?; + self.new_op_tensor( + TensorDesc::new(TensorType::F32, layout, usage), + Op::MulMatId, + &[as_tensor, b, ids], + ) + } + + pub fn add_id( + &mut self, + a: TensorId, + b: TensorId, + ids: TensorId, + usage: BufferUsage, + ) -> Result { + let a_ref = self + .tensor(a) + .ok_or_else(|| format!("invalid tensor id {}", a))?; + let b_ref = self + .tensor(b) + .ok_or_else(|| format!("invalid tensor id {}", b))?; + let ids_ref = self + .tensor(ids) + .ok_or_else(|| format!("invalid tensor id {}", ids))?; + + if ids_ref.desc.ty != TensorType::I32 { + return Err(format!( + "add_id ids tensor must be I32, got {}", + ids_ref.desc.ty.name() + )); + } + if a_ref.ne[0] != b_ref.ne[0] { + return Err(format!( + "add_id width mismatch: lhs={} rhs={}", + a_ref.ne[0], b_ref.ne[0] + )); + } + if a_ref.ne[1] != ids_ref.ne[0] { + return Err(format!( + "add_id lhs dim1 {} does not match ids dim0 {}", + a_ref.ne[1], ids_ref.ne[0] + )); + } + if a_ref.ne[2] != ids_ref.ne[1] { + return Err(format!( + "add_id lhs dim2 {} does not match ids dim1 {}", + a_ref.ne[2], ids_ref.ne[1] + )); + } + + self.new_op_tensor( + TensorDesc::new(a_ref.desc.ty, a_ref.desc.layout.clone(), usage), + Op::AddId, + &[a, b, ids], + ) + } + + pub fn concat( + &mut self, + a: TensorId, + b: TensorId, + dim: usize, + usage: BufferUsage, + ) -> Result { + let ta = self + .tensor(a) + .ok_or_else(|| format!("invalid tensor id {}", a))?; + let tb = self + .tensor(b) + .ok_or_else(|| format!("invalid tensor id {}", b))?; + let rank = ta.desc.layout.rank().max(tb.desc.layout.rank()); + if dim >= 4 { + return Err(format!("concat dimension {} exceeds ggml rank 4", dim)); + } + if dim >= rank { + return Err(format!( + "concat dimension {} exceeds tensor rank {}", + dim, rank + )); + } + if ta.desc.ty != tb.desc.ty { + return Err(format!( + "concat requires matching types, got {} and {}", + ta.desc.ty.name(), + tb.desc.ty.name() + )); + } + let mut ne = ta.ne; + for (idx, (&ea, &eb)) in ta.ne[..rank].iter().zip(tb.ne[..rank].iter()).enumerate() { + if idx == dim { + ne[idx] = ea + .checked_add(eb) + .ok_or_else(|| "concat extent overflow".to_string())?; + } else if ea != eb { + return Err(format!( + "concat requires matching extent at dim {}: {} vs {}", + idx, ea, eb + )); + } + } + let layout = TensorLayout::for_ggml(ta.desc.ty, &ne[..rank])?; + let id = self.new_op_tensor( + TensorDesc::new(ta.desc.ty, layout, usage), + Op::Concat, + &[a, b], + )?; + self.tensor_mut(id).unwrap().set_op_param_i32(0, dim as i32); + Ok(id) + } + + pub fn reshape(&mut self, src: TensorId, extents: &[i64]) -> Result { + let tensor = self + .tensor(src) + .ok_or_else(|| format!("invalid tensor id {}", src))?; + let layout = TensorLayout::for_ggml(tensor.desc.ty, extents)?; + self.new_view_op_tensor( + TensorDesc::new(tensor.desc.ty, layout, tensor.desc.usage), + Op::Reshape, + src, + 0, + ) + } + + pub fn cpy( + &mut self, + src: TensorId, + dst: TensorId, + usage: BufferUsage, + ) -> Result { + let src_tensor = self + .tensor(src) + .ok_or_else(|| format!("invalid tensor id {}", src))?; + let dst_tensor = self + .tensor(dst) + .ok_or_else(|| format!("invalid tensor id {}", dst))?; + if src_tensor.nelements() != dst_tensor.nelements() { + return Err(format!( + "cpy requires matching element counts, got {} and {}", + src_tensor.nelements(), + dst_tensor.nelements() + )); + } + let desc = TensorDesc::new(dst_tensor.desc.ty, dst_tensor.desc.layout.clone(), usage); + self.new_view_op_tensor_with_srcs(desc, Op::Cpy, dst, 0, &[src, dst]) + } + + pub fn cont(&mut self, src: TensorId) -> Result { + let tensor = self + .tensor(src) + .ok_or_else(|| format!("invalid tensor id {}", src))?; + self.new_op_tensor( + TensorDesc::new( + tensor.desc.ty, + TensorLayout::for_ggml(tensor.desc.ty, tensor.desc.layout.extents())?, + tensor.desc.usage, + ), + Op::Cont, + &[src], + ) + } + + pub fn cont_2d(&mut self, src: TensorId, ne0: i64, ne1: i64) -> Result { + let tensor = self + .tensor(src) + .ok_or_else(|| format!("invalid tensor id {}", src))?; + self.new_op_tensor( + TensorDesc::new( + tensor.desc.ty, + TensorLayout::for_ggml(tensor.desc.ty, &[ne0, ne1])?, + tensor.desc.usage, + ), + Op::Cont, + &[src], + ) + } + + pub fn cont_3d( + &mut self, + src: TensorId, + ne0: i64, + ne1: i64, + ne2: i64, + ) -> Result { + let tensor = self + .tensor(src) + .ok_or_else(|| format!("invalid tensor id {}", src))?; + self.new_op_tensor( + TensorDesc::new( + tensor.desc.ty, + TensorLayout::for_ggml(tensor.desc.ty, &[ne0, ne1, ne2])?, + tensor.desc.usage, + ), + Op::Cont, + &[src], + ) + } + + pub fn cont_4d( + &mut self, + src: TensorId, + ne0: i64, + ne1: i64, + ne2: i64, + ne3: i64, + ) -> Result { + let tensor = self + .tensor(src) + .ok_or_else(|| format!("invalid tensor id {}", src))?; + self.new_op_tensor( + TensorDesc::new( + tensor.desc.ty, + TensorLayout::for_ggml(tensor.desc.ty, &[ne0, ne1, ne2, ne3])?, + tensor.desc.usage, + ), + Op::Cont, + &[src], + ) + } + + pub fn permute(&mut self, src: TensorId, axes: [usize; 4]) -> Result { + let tensor = self + .tensor(src) + .ok_or_else(|| format!("invalid tensor id {}", src))?; + let rank = tensor.desc.layout.rank(); + let mut extents = Vec::with_capacity(rank); + let mut strides = Vec::with_capacity(rank); + for &src_idx in axes.iter().take(rank) { + if src_idx >= rank { + return Err(format!( + "permute axis {} exceeds tensor '{}' rank {}", + src_idx, + tensor.name().unwrap_or(""), + rank + )); + } + extents.push(tensor.ne[src_idx]); + strides.push(tensor.nb[src_idx]); + } + let layout = TensorLayout::from_parts(rank, &extents, &strides)?; + self.new_view_op_tensor( + TensorDesc::new(tensor.desc.ty, layout, tensor.desc.usage), + Op::Permute, + src, + 0, + ) + } + + pub fn transpose(&mut self, src: TensorId) -> Result { + self.permute(src, [1, 0, 2, 3]).map(|id| { + let tensor = self.tensor_mut(id).unwrap(); + tensor.op = Op::Transpose; + id + }) + } + + pub fn repeat( + &mut self, + src: TensorId, + shape_of: TensorId, + usage: BufferUsage, + ) -> Result { + let target = self + .tensor(shape_of) + .ok_or_else(|| format!("invalid tensor id {}", shape_of))?; + self.new_op_tensor( + TensorDesc::new(target.desc.ty, target.desc.layout.clone(), usage), + Op::Repeat, + &[src, shape_of], + ) + } + + pub fn repeat_4d( + &mut self, + src: TensorId, + ne0: i64, + ne1: i64, + ne2: i64, + ne3: i64, + usage: BufferUsage, + ) -> Result { + let ty = self + .tensor(src) + .ok_or_else(|| format!("invalid tensor id {}", src))? + .desc + .ty; + let layout = TensorLayout::for_ggml(ty, &[ne0, ne1, ne2, ne3])?; + self.new_op_tensor(TensorDesc::new(ty, layout, usage), Op::Repeat, &[src]) + } + + pub fn sum_rows(&mut self, src: TensorId, usage: BufferUsage) -> Result { + let tensor = self + .tensor(src) + .ok_or_else(|| format!("invalid tensor id {}", src))?; + let ne = [1, tensor.ne[1], tensor.ne[2], tensor.ne[3]]; + let layout = TensorLayout::for_ggml(tensor.desc.ty, &ne)?; + self.new_op_tensor( + TensorDesc::new(tensor.desc.ty, layout, usage), + Op::SumRows, + &[src], + ) + } + + pub fn cumsum(&mut self, src: TensorId, usage: BufferUsage) -> Result { + let tensor = self + .tensor(src) + .ok_or_else(|| format!("invalid tensor id {}", src))?; + self.new_op_tensor( + TensorDesc::new(tensor.desc.ty, tensor.desc.layout.clone(), usage), + Op::CumSum, + &[src], + ) + } + + pub fn diag(&mut self, src: TensorId, usage: BufferUsage) -> Result { + let tensor = self + .tensor(src) + .ok_or_else(|| format!("invalid tensor id {}", src))?; + self.new_op_tensor( + TensorDesc::new(tensor.desc.ty, tensor.desc.layout.clone(), usage), + Op::Diag, + &[src], + ) + } + + pub fn tri(&mut self, src: TensorId, usage: BufferUsage) -> Result { + let tensor = self + .tensor(src) + .ok_or_else(|| format!("invalid tensor id {}", src))?; + self.new_op_tensor( + TensorDesc::new(tensor.desc.ty, tensor.desc.layout.clone(), usage), + Op::Tri, + &[src], + ) + } + + pub fn solve_tri( + &mut self, + a: TensorId, + b: TensorId, + usage: BufferUsage, + ) -> Result { + self.binary_like_a(Op::SolveTri, a, b, usage) + } + + pub fn get_rows( + &mut self, + src: TensorId, + rows: TensorId, + usage: BufferUsage, + ) -> Result { + let tensor = self + .tensor(src) + .ok_or_else(|| format!("invalid tensor id {}", src))?; + let rows_tensor = self + .tensor(rows) + .ok_or_else(|| format!("invalid tensor id {}", rows))?; + let ne = [ + tensor.ne[0], + rows_tensor.ne[0], + rows_tensor.ne[1], + rows_tensor.ne[2], + ]; + let ty = if tensor.desc.ty == TensorType::I32 { + TensorType::I32 + } else { + TensorType::F32 + }; + let layout = TensorLayout::for_ggml(ty, &ne)?; + self.new_op_tensor( + TensorDesc::new(ty, layout, usage), + Op::GetRows, + &[src, rows], + ) + } + + pub fn set_rows( + &mut self, + dst: TensorId, + src: TensorId, + rows: TensorId, + usage: BufferUsage, + ) -> Result { + let dst_tensor = self + .tensor(dst) + .ok_or_else(|| format!("invalid tensor id {}", dst))?; + let src_tensor = self + .tensor(src) + .ok_or_else(|| format!("invalid tensor id {}", src))?; + let rows_tensor = self + .tensor(rows) + .ok_or_else(|| format!("invalid tensor id {}", rows))?; + if dst_tensor.ne[0] != src_tensor.ne[0] { + return Err(format!( + "set_rows requires dst dim0 {} to match src dim0 {}", + dst_tensor.ne[0], src_tensor.ne[0] + )); + } + if dst_tensor.ne[2] != src_tensor.ne[2] || dst_tensor.ne[3] != src_tensor.ne[3] { + return Err(format!( + "set_rows requires dst dims2/3 [{}, {}] to match src dims2/3 [{}, {}]", + dst_tensor.ne[2], dst_tensor.ne[3], src_tensor.ne[2], src_tensor.ne[3] + )); + } + if src_tensor.ne[1] != rows_tensor.ne[0] { + return Err(format!( + "set_rows requires src dim1 {} to match rows dim0 {}", + src_tensor.ne[1], rows_tensor.ne[0] + )); + } + if rows_tensor.desc.ty != TensorType::I32 && rows_tensor.desc.ty != TensorType::I64 { + return Err(format!( + "set_rows rows tensor must be I32 or I64, got {}", + rows_tensor.desc.ty.name() + )); + } + if src_tensor.desc.ty != TensorType::F32 { + return Err(format!( + "set_rows currently requires F32 src rows, got {}", + src_tensor.desc.ty.name() + )); + } + let desc = TensorDesc::new(dst_tensor.desc.ty, dst_tensor.desc.layout.clone(), usage); + self.new_view_op_tensor_with_srcs(desc, Op::SetRows, dst, 0, &[src, rows, dst]) + } + + pub fn soft_max(&mut self, src: TensorId, usage: BufferUsage) -> Result { + let tensor = self + .tensor(src) + .ok_or_else(|| format!("invalid tensor id {}", src))?; + let id = self.new_op_tensor( + TensorDesc::new(tensor.desc.ty, tensor.desc.layout.clone(), usage), + Op::SoftMax, + &[src], + )?; + let tensor = self.tensor_mut(id).unwrap(); + tensor.set_op_param_f32(0, 1.0); + tensor.set_op_param_f32(1, 0.0); + Ok(id) + } + + pub fn flash_attn_ext( + &mut self, + q: TensorId, + k: TensorId, + v: TensorId, + mask: Option, + scale: f32, + max_bias: f32, + logit_softcap: f32, + usage: BufferUsage, + ) -> Result { + let q_tensor = self + .tensor(q) + .ok_or_else(|| format!("invalid tensor id {}", q))?; + let k_tensor = self + .tensor(k) + .ok_or_else(|| format!("invalid tensor id {}", k))?; + let v_tensor = self + .tensor(v) + .ok_or_else(|| format!("invalid tensor id {}", v))?; + + if q_tensor.ne[3] != k_tensor.ne[3] || q_tensor.ne[3] != v_tensor.ne[3] { + return Err("flash_attn_ext requires q/k/v to agree on dim3".to_string()); + } + + if let Some(mask) = mask { + let mask_tensor = self + .tensor(mask) + .ok_or_else(|| format!("invalid tensor id {}", mask))?; + if mask_tensor.desc.ty != TensorType::F16 { + return Err(format!( + "flash_attn_ext mask must be F16, got {}", + mask_tensor.desc.ty.name() + )); + } + } + + let ne = [ + v_tensor.ne[0], + q_tensor.ne[2], + q_tensor.ne[1], + q_tensor.ne[3], + ]; + let layout = TensorLayout::for_ggml(TensorType::F32, &ne)?; + let mut srcs = vec![q, k, v]; + if let Some(mask) = mask { + srcs.push(mask); + } + + let id = self.new_op_tensor( + TensorDesc::new(TensorType::F32, layout, usage), + Op::FlashAttnExt, + &srcs, + )?; + let tensor = self.tensor_mut(id).unwrap(); + tensor.set_op_param_f32(0, scale); + tensor.set_op_param_f32(1, max_bias); + tensor.set_op_param_f32(2, logit_softcap); + tensor.set_op_param_i32(3, Prec::Default as i32); + Ok(id) + } + + pub fn flash_attn_ext_set_prec( + &mut self, + tensor_id: TensorId, + prec: Prec, + ) -> Result<(), String> { + let tensor = self + .tensor_mut(tensor_id) + .ok_or_else(|| format!("invalid tensor id {}", tensor_id))?; + if tensor.op != Op::FlashAttnExt { + return Err(format!( + "tensor '{}' is not a flash_attn_ext op", + tensor.name().unwrap_or("") + )); + } + tensor.set_op_param_i32(3, prec as i32); + Ok(()) + } + + pub fn flash_attn_ext_add_sinks( + &mut self, + tensor_id: TensorId, + sinks: Option, + ) -> Result<(), String> { + let tensor = self + .tensor_mut(tensor_id) + .ok_or_else(|| format!("invalid tensor id {}", tensor_id))?; + if tensor.op != Op::FlashAttnExt { + return Err(format!( + "tensor '{}' is not a flash_attn_ext op", + tensor.name().unwrap_or("") + )); + } + tensor.src[4] = sinks; + Ok(()) + } + + pub fn rms_norm(&mut self, src: TensorId, usage: BufferUsage) -> Result { + self.rms_norm_eps(src, 0.0, usage) + } + + pub fn rms_norm_eps( + &mut self, + src: TensorId, + eps: f32, + usage: BufferUsage, + ) -> Result { + let tensor = self + .tensor(src) + .ok_or_else(|| format!("invalid tensor id {}", src))?; + let id = self.new_op_tensor( + TensorDesc::new(tensor.desc.ty, tensor.desc.layout.clone(), usage), + Op::RmsNorm, + &[src], + )?; + self.tensor_mut(id).unwrap().set_op_param_f32(0, eps); + Ok(id) + } + + pub fn l2_norm(&mut self, src: TensorId, usage: BufferUsage) -> Result { + self.l2_norm_eps(src, 0.0, usage) + } + + pub fn l2_norm_eps( + &mut self, + src: TensorId, + eps: f32, + usage: BufferUsage, + ) -> Result { + let tensor = self + .tensor(src) + .ok_or_else(|| format!("invalid tensor id {}", src))?; + let id = self.new_op_tensor( + TensorDesc::new(tensor.desc.ty, tensor.desc.layout.clone(), usage), + Op::L2Norm, + &[src], + )?; + self.tensor_mut(id).unwrap().set_op_param_f32(0, eps); + Ok(id) + } + + pub fn rope( + &mut self, + src: TensorId, + positions: TensorId, + n_dims: i32, + mode: i32, + usage: BufferUsage, + ) -> Result { + self.rope_ext( + src, positions, None, n_dims, mode, 0, 10_000.0, 1.0, 0.0, 1.0, 0.0, 0.0, usage, + ) + } + + pub fn rope_inplace( + &mut self, + src: TensorId, + positions: TensorId, + n_dims: i32, + mode: i32, + ) -> Result { + self.rope_ext_inplace( + src, positions, None, n_dims, mode, 0, 10_000.0, 1.0, 0.0, 1.0, 0.0, 0.0, + ) + } + + #[allow(clippy::too_many_arguments)] + pub fn rope_ext( + &mut self, + src: TensorId, + positions: TensorId, + freq_factors: Option, + n_dims: i32, + mode: i32, + n_ctx_orig: i32, + freq_base: f32, + freq_scale: f32, + ext_factor: f32, + attn_factor: f32, + beta_fast: f32, + beta_slow: f32, + usage: BufferUsage, + ) -> Result { + self.rope_impl( + src, + positions, + freq_factors, + n_dims, + None, + mode, + n_ctx_orig, + freq_base, + freq_scale, + ext_factor, + attn_factor, + beta_fast, + beta_slow, + false, + Some(usage), + ) + } + + #[allow(clippy::too_many_arguments)] + pub fn rope_ext_inplace( + &mut self, + src: TensorId, + positions: TensorId, + freq_factors: Option, + n_dims: i32, + mode: i32, + n_ctx_orig: i32, + freq_base: f32, + freq_scale: f32, + ext_factor: f32, + attn_factor: f32, + beta_fast: f32, + beta_slow: f32, + ) -> Result { + self.rope_impl( + src, + positions, + freq_factors, + n_dims, + None, + mode, + n_ctx_orig, + freq_base, + freq_scale, + ext_factor, + attn_factor, + beta_fast, + beta_slow, + true, + None, + ) + } + + #[allow(clippy::too_many_arguments)] + pub fn rope_multi( + &mut self, + src: TensorId, + positions: TensorId, + freq_factors: Option, + n_dims: i32, + sections: [i32; GGML_MROPE_SECTIONS], + mode: i32, + n_ctx_orig: i32, + freq_base: f32, + freq_scale: f32, + ext_factor: f32, + attn_factor: f32, + beta_fast: f32, + beta_slow: f32, + usage: BufferUsage, + ) -> Result { + self.rope_impl( + src, + positions, + freq_factors, + n_dims, + Some(sections), + mode, + n_ctx_orig, + freq_base, + freq_scale, + ext_factor, + attn_factor, + beta_fast, + beta_slow, + false, + Some(usage), + ) + } + + #[allow(clippy::too_many_arguments)] + pub fn rope_multi_inplace( + &mut self, + src: TensorId, + positions: TensorId, + freq_factors: Option, + n_dims: i32, + sections: [i32; GGML_MROPE_SECTIONS], + mode: i32, + n_ctx_orig: i32, + freq_base: f32, + freq_scale: f32, + ext_factor: f32, + attn_factor: f32, + beta_fast: f32, + beta_slow: f32, + ) -> Result { + self.rope_impl( + src, + positions, + freq_factors, + n_dims, + Some(sections), + mode, + n_ctx_orig, + freq_base, + freq_scale, + ext_factor, + attn_factor, + beta_fast, + beta_slow, + true, + None, + ) + } + + pub fn pad( + &mut self, + src: TensorId, + left: i64, + right: i64, + usage: BufferUsage, + ) -> Result { + let tensor = self + .tensor(src) + .ok_or_else(|| format!("invalid tensor id {}", src))?; + let mut ne = tensor.ne; + ne[0] += left + right; + let layout = TensorLayout::for_ggml(tensor.desc.ty, &ne)?; + self.new_op_tensor( + TensorDesc::new(tensor.desc.ty, layout, usage), + Op::Pad, + &[src], + ) + } + + pub fn argsort(&mut self, src: TensorId, usage: BufferUsage) -> Result { + let tensor = self + .tensor(src) + .ok_or_else(|| format!("invalid tensor id {}", src))?; + let layout = TensorLayout::for_ggml(TensorType::I32, &tensor.ne)?; + self.new_op_tensor( + TensorDesc::new(TensorType::I32, layout, usage), + Op::Argsort, + &[src], + ) + } + + pub fn top_k(&mut self, src: TensorId, k: i64, usage: BufferUsage) -> Result { + let tensor = self + .tensor(src) + .ok_or_else(|| format!("invalid tensor id {}", src))?; + let ne = [k, tensor.ne[1], tensor.ne[2], tensor.ne[3]]; + let layout = TensorLayout::for_ggml(TensorType::I32, &ne)?; + self.new_op_tensor( + TensorDesc::new(TensorType::I32, layout, usage), + Op::TopK, + &[src], + ) + } + + pub fn ssm_conv( + &mut self, + sx: TensorId, + c: TensorId, + usage: BufferUsage, + ) -> Result { + let sx_tensor = self + .tensor(sx) + .ok_or_else(|| format!("invalid tensor id {}", sx))?; + let c_tensor = self + .tensor(c) + .ok_or_else(|| format!("invalid tensor id {}", c))?; + if sx_tensor.desc.layout.rank() != 3 { + return Err("ssm_conv requires a 3D source tensor".to_string()); + } + if c_tensor.desc.layout.rank() != 2 { + return Err("ssm_conv requires a 2D kernel tensor".to_string()); + } + let d_conv = c_tensor.ne[0]; + let d_inner = c_tensor.ne[1]; + let n_t = sx_tensor.ne[0] + .checked_sub(d_conv) + .and_then(|v| v.checked_add(1)) + .ok_or_else(|| "ssm_conv token extent underflow".to_string())?; + if sx_tensor.ne[1] != d_inner { + return Err(format!( + "ssm_conv source dim1 {} does not match kernel dim1 {}", + sx_tensor.ne[1], d_inner + )); + } + let layout = TensorLayout::for_ggml(TensorType::F32, &[d_inner, n_t, sx_tensor.ne[2]])?; + self.new_op_tensor( + TensorDesc::new(TensorType::F32, layout, usage), + Op::SsmConv, + &[sx, c], + ) + } + + pub fn gated_delta_net( + &mut self, + q: TensorId, + k: TensorId, + v: TensorId, + g: TensorId, + beta: TensorId, + state: TensorId, + usage: BufferUsage, + ) -> Result { + let v_tensor = self + .tensor(v) + .ok_or_else(|| format!("invalid tensor id {}", v))?; + let state_tensor = self + .tensor(state) + .ok_or_else(|| format!("invalid tensor id {}", state))?; + let sv = v_tensor.ne[0]; + let h = v_tensor.ne[1]; + let n_tokens = v_tensor.ne[2]; + let n_seqs = v_tensor.ne[3]; + let expected_state = sv + .checked_mul(sv) + .and_then(|x| x.checked_mul(h)) + .and_then(|x| x.checked_mul(n_seqs)) + .ok_or_else(|| "gated_delta_net state element overflow".to_string())?; + if state_tensor.nelements() != expected_state { + return Err(format!( + "gated_delta_net state elements {} do not match expected {}", + state_tensor.nelements(), + expected_state + )); + } + let layout = TensorLayout::for_ggml( + TensorType::F32, + &[sv * h, n_tokens * n_seqs + sv * n_seqs, 1, 1], + )?; + self.new_op_tensor( + TensorDesc::new(TensorType::F32, layout, usage), + Op::GatedDeltaNet, + &[q, k, v, g, beta, state], + ) + } + + fn push_tensor(&mut self, mut tensor: Tensor, alloc_data: bool) -> Result { + let nbytes = tensor.nbytes(); + self.max_tensor_size = self.max_tensor_size.max(nbytes); + + if alloc_data && !self.no_alloc && nbytes > 0 { + let offset = ggml_pad(self.next_data_offset, GGML_MEM_ALIGN); + let end = offset + .checked_add(nbytes) + .ok_or_else(|| "tensor allocation overflow".to_string())?; + if end > self.mem_size { + return Err(format!( + "context out of memory allocating {} bytes for tensor", + nbytes + )); + } + tensor.data_offset = Some(offset); + self.next_data_offset = end; + } + + let id = tensor.id; + if let Some(name) = tensor.name() { + self.name_to_tensor.insert(name.to_string(), id); + } + + self.tensors.push(tensor); + Ok(id) + } + + #[allow(clippy::too_many_arguments)] + fn rope_impl( + &mut self, + src: TensorId, + positions: TensorId, + freq_factors: Option, + n_dims: i32, + sections: Option<[i32; GGML_MROPE_SECTIONS]>, + mode: i32, + n_ctx_orig: i32, + freq_base: f32, + freq_scale: f32, + ext_factor: f32, + attn_factor: f32, + beta_fast: f32, + beta_slow: f32, + inplace: bool, + usage: Option, + ) -> Result { + let src_tensor = self + .tensor(src) + .ok_or_else(|| format!("invalid tensor id {}", src))?; + let pos_tensor = self + .tensor(positions) + .ok_or_else(|| format!("invalid tensor id {}", positions))?; + + if pos_tensor.desc.ty != TensorType::I32 { + return Err(format!( + "rope positions tensor must be I32, got {}", + pos_tensor.desc.ty.name() + )); + } + + if n_dims < 0 { + return Err(format!("rope n_dims must be non-negative, got {}", n_dims)); + } + + if let Some(freq_factors) = freq_factors { + let freq_tensor = self + .tensor(freq_factors) + .ok_or_else(|| format!("invalid tensor id {}", freq_factors))?; + if freq_tensor.desc.ty != TensorType::F32 { + return Err(format!( + "rope freq_factors tensor must be F32, got {}", + freq_tensor.desc.ty.name() + )); + } + if freq_tensor.ne[0] < i64::from(n_dims / 2) { + return Err(format!( + "rope freq_factors is too short: need at least {}, got {}", + n_dims / 2, + freq_tensor.ne[0] + )); + } + } + + let desc = TensorDesc::new( + src_tensor.desc.ty, + src_tensor.desc.layout.clone(), + usage.unwrap_or(src_tensor.desc.usage), + ); + let mut srcs = Vec::with_capacity(3); + srcs.push(src); + srcs.push(positions); + if let Some(freq_factors) = freq_factors { + srcs.push(freq_factors); + } + + let id = if inplace { + self.new_view_op_tensor_with_srcs(desc, Op::Rope, src, 0, &srcs)? + } else { + self.new_op_tensor(desc, Op::Rope, &srcs)? + }; + + let tensor = self.tensor_mut(id).unwrap(); + tensor.set_op_param_i32(0, 0); + tensor.set_op_param_i32(1, n_dims); + tensor.set_op_param_i32(2, mode); + tensor.set_op_param_i32(3, 0); + tensor.set_op_param_i32(4, n_ctx_orig); + tensor.set_op_param_f32(5, freq_base); + tensor.set_op_param_f32(6, freq_scale); + tensor.set_op_param_f32(7, ext_factor); + tensor.set_op_param_f32(8, attn_factor); + tensor.set_op_param_f32(9, beta_fast); + tensor.set_op_param_f32(10, beta_slow); + for (i, section) in sections + .unwrap_or([0; GGML_MROPE_SECTIONS]) + .into_iter() + .enumerate() + { + tensor.set_op_param_i32(11 + i, section); + } + + Ok(id) + } +} + +#[cfg(test)] +mod tests { + use super::*; + use crate::core::{GGML_ROPE_TYPE_IMROPE, GGML_ROPE_TYPE_MROPE}; + use crate::tensor::ggml_row_size_for_type; + + #[test] + fn allocates_basic_tensor_metadata() { + let mut ctx = Context::new(InitParams { + mem_size: 4096, + mem_buffer: None, + no_alloc: false, + }); + + let tensor = ctx + .new_tensor_2d(TensorType::F32, 16, 8, BufferUsage::Activations) + .unwrap(); + + let t = ctx.tensor(tensor).unwrap(); + assert_eq!(t.ne, [16, 8, 1, 1]); + assert_eq!(t.nb[0], 4); + assert!(t.data_offset.is_some()); + } + + #[test] + fn mul_mat_uses_upstream_result_shape() { + let mut ctx = Context::new(InitParams { + mem_size: 1 << 20, + mem_buffer: None, + no_alloc: true, + }); + + let a = ctx + .new_tensor_2d(TensorType::F16, 128, 64, BufferUsage::Weights) + .unwrap(); + let b = ctx + .new_tensor_2d(TensorType::F16, 128, 32, BufferUsage::Activations) + .unwrap(); + + let out = ctx.mul_mat(a, b, BufferUsage::Activations).unwrap(); + let t = ctx.tensor(out).unwrap(); + assert_eq!(t.ne, [64, 32, 1, 1]); + assert_eq!(t.desc.ty, TensorType::F32); + assert_eq!(ggml_row_size_for_type(t.desc.ty, t.ne[0]).unwrap(), 256); + } + + #[test] + fn writes_named_tensor_bytes() { + let mut ctx = Context::new(InitParams { + mem_size: 4096, + mem_buffer: None, + no_alloc: false, + }); + + let tensor = ctx + .new_named_tensor( + "weights.test", + TensorType::F32, + 2, + &[4, 2], + BufferUsage::Weights, + ) + .unwrap(); + ctx.write_tensor_data( + tensor, + &[ + 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20, 21, 22, 23, + 24, 25, 26, 27, 28, 29, 30, 31, 32, + ], + ) + .unwrap(); + + assert_eq!(ctx.get_tensor("weights.test"), Some(tensor)); + assert_eq!(ctx.tensor_data(tensor).unwrap()[0], 1); + assert_eq!(ctx.tensor_data(tensor).unwrap()[31], 32); + } + + #[test] + fn op_tensors_allocate_data_when_context_allocates() { + let mut ctx = Context::new(InitParams { + mem_size: 1 << 12, + mem_buffer: None, + no_alloc: false, + }); + + let a = ctx + .new_tensor_2d(TensorType::F32, 8, 4, BufferUsage::Weights) + .unwrap(); + let b = ctx + .new_tensor_2d(TensorType::F32, 8, 2, BufferUsage::Activations) + .unwrap(); + let out = ctx.mul_mat(a, b, BufferUsage::Activations).unwrap(); + + assert!(ctx.tensor(out).unwrap().data_offset.is_some()); + assert_eq!( + ctx.tensor_data(out).unwrap().len(), + ctx.tensor(out).unwrap().nbytes() + ); + } + + #[test] + fn reshape_creates_view_tensor() { + let mut ctx = Context::new(InitParams { + mem_size: 1 << 12, + mem_buffer: None, + no_alloc: false, + }); + + let src = ctx + .new_tensor_2d(TensorType::F32, 8, 4, BufferUsage::Activations) + .unwrap(); + let reshaped = ctx.reshape(src, &[4, 8]).unwrap(); + + let src_tensor = ctx.tensor(src).unwrap(); + let reshaped_tensor = ctx.tensor(reshaped).unwrap(); + assert!(reshaped_tensor.is_view()); + assert_eq!(reshaped_tensor.view_src, Some(src)); + assert_eq!(reshaped_tensor.data_offset, src_tensor.data_offset); + } + + #[test] + fn cont_materializes_contiguous_layout() { + let mut ctx = Context::new(InitParams { + mem_size: 1 << 12, + mem_buffer: None, + no_alloc: false, + }); + + let src = ctx + .new_tensor_2d(TensorType::F32, 16, 2, BufferUsage::Activations) + .unwrap(); + let view = ctx.view_3d(src, 4, 2, 2, 32, 64, 16).unwrap(); + let cont = ctx.cont_2d(view, 8, 2).unwrap(); + + let view_tensor = ctx.tensor(view).unwrap(); + let cont_tensor = ctx.tensor(cont).unwrap(); + assert!(!view_tensor.is_contiguous()); + assert!(cont_tensor.is_contiguous()); + assert_eq!(cont_tensor.ne, [8, 2, 1, 1]); + assert_eq!(cont_tensor.nb[0], 4); + assert_eq!(cont_tensor.nb[1], 32); + } + + #[test] + fn get_rows_matches_upstream_output_type() { + let mut ctx = Context::new(InitParams { + mem_size: 1 << 12, + mem_buffer: None, + no_alloc: true, + }); + + let src_q = ctx + .new_tensor_2d(TensorType::Q5K, 256, 4, BufferUsage::Weights) + .unwrap(); + let rows = ctx + .new_tensor_1d(TensorType::I32, 2, BufferUsage::Activations) + .unwrap(); + let out_q = ctx.get_rows(src_q, rows, BufferUsage::Activations).unwrap(); + assert_eq!(ctx.tensor(out_q).unwrap().desc.ty, TensorType::F32); + + let src_i = ctx + .new_tensor_2d(TensorType::I32, 8, 4, BufferUsage::Weights) + .unwrap(); + let out_i = ctx.get_rows(src_i, rows, BufferUsage::Activations).unwrap(); + assert_eq!(ctx.tensor(out_i).unwrap().desc.ty, TensorType::I32); + } + + #[test] + fn set_rows_creates_inplace_view_op() { + let mut ctx = Context::new(InitParams { + mem_size: 1 << 12, + mem_buffer: None, + no_alloc: true, + }); + + let dst = ctx + .new_tensor_2d(TensorType::F16, 8, 4, BufferUsage::State) + .unwrap(); + let src = ctx + .new_tensor_2d(TensorType::F32, 8, 2, BufferUsage::Activations) + .unwrap(); + let rows = ctx + .new_tensor_1d(TensorType::I32, 2, BufferUsage::Activations) + .unwrap(); + let out = ctx.set_rows(dst, src, rows, BufferUsage::State).unwrap(); + + let tensor = ctx.tensor(out).unwrap(); + assert_eq!(tensor.op, Op::SetRows); + assert!(tensor.is_view()); + assert_eq!(tensor.view_src, Some(dst)); + assert_eq!(tensor.src[0], Some(src)); + assert_eq!(tensor.src[1], Some(rows)); + assert_eq!(tensor.src[2], Some(dst)); + assert_eq!(tensor.desc.ty, TensorType::F16); + assert_eq!(tensor.desc.usage, BufferUsage::State); + } + + #[test] + fn rope_multi_stores_upstream_params() { + let mut ctx = Context::new(InitParams { + mem_size: 1 << 12, + mem_buffer: None, + no_alloc: true, + }); + + let src = ctx + .new_tensor_3d(TensorType::F32, 128, 8, 2, BufferUsage::Activations) + .unwrap(); + let pos = ctx + .new_tensor_2d(TensorType::I32, 8, 4, BufferUsage::Activations) + .unwrap(); + let freq = ctx + .new_tensor_1d(TensorType::F32, 32, BufferUsage::Weights) + .unwrap(); + + let rope = ctx + .rope_multi( + src, + pos, + Some(freq), + 64, + [11, 11, 10, 0], + GGML_ROPE_TYPE_MROPE | GGML_ROPE_TYPE_IMROPE, + 262_144, + 1_000_000.0, + 0.5, + 0.25, + 1.5, + 32.0, + 1.0, + BufferUsage::Activations, + ) + .unwrap(); + + let tensor = ctx.tensor(rope).unwrap(); + assert_eq!(tensor.op, Op::Rope); + assert_eq!(tensor.src[0], Some(src)); + assert_eq!(tensor.src[1], Some(pos)); + assert_eq!(tensor.src[2], Some(freq)); + assert_eq!(tensor.op_param_i32(1), 64); + assert_eq!( + tensor.op_param_i32(2), + GGML_ROPE_TYPE_MROPE | GGML_ROPE_TYPE_IMROPE + ); + assert_eq!(tensor.op_param_i32(4), 262_144); + assert_eq!(tensor.op_param_i32(11), 11); + assert_eq!(tensor.op_param_i32(12), 11); + assert_eq!(tensor.op_param_i32(13), 10); + assert_eq!(tensor.op_param_i32(14), 0); + assert_eq!(tensor.op_param_f32(5), 1_000_000.0); + assert_eq!(tensor.op_param_f32(6), 0.5); + assert_eq!(tensor.op_param_f32(7), 0.25); + assert_eq!(tensor.op_param_f32(8), 1.5); + assert_eq!(tensor.op_param_f32(9), 32.0); + assert_eq!(tensor.op_param_f32(10), 1.0); + } + + #[test] + fn rope_inplace_creates_view_op() { + let mut ctx = Context::new(InitParams { + mem_size: 1 << 12, + mem_buffer: None, + no_alloc: false, + }); + + let src = ctx + .new_tensor_2d(TensorType::F32, 8, 2, BufferUsage::Activations) + .unwrap(); + let pos = ctx + .new_tensor_1d(TensorType::I32, 2, BufferUsage::Activations) + .unwrap(); + let rope = ctx.rope_inplace(src, pos, 8, 0).unwrap(); + + let tensor = ctx.tensor(rope).unwrap(); + assert_eq!(tensor.op, Op::Rope); + assert_eq!(tensor.view_src, Some(src)); + assert_eq!(tensor.src[0], Some(src)); + assert_eq!(tensor.src[1], Some(pos)); + assert_eq!(tensor.data_offset, ctx.tensor(src).unwrap().data_offset); + } +} diff --git a/libs/ggml/src/core.rs b/libs/ggml/src/core.rs new file mode 100644 index 0000000000..658d8d8a78 --- /dev/null +++ b/libs/ggml/src/core.rs @@ -0,0 +1,168 @@ +pub const GGML_FILE_MAGIC: u32 = 0x67676d6c; +pub const GGML_FILE_VERSION: u32 = 2; + +pub const GGML_QNT_VERSION: u32 = 2; +pub const GGML_QNT_VERSION_FACTOR: u32 = 1000; + +pub const GGML_MAX_DIMS: usize = 4; +pub const GGML_MAX_PARAMS: usize = 2048; +pub const GGML_MAX_SRC: usize = 10; +pub const GGML_MAX_N_THREADS: usize = 512; +pub const GGML_MAX_OP_PARAMS: usize = 64; +pub const GGML_MAX_NAME: usize = 64; +pub const GGML_DEFAULT_N_THREADS: usize = 4; +pub const GGML_DEFAULT_GRAPH_SIZE: usize = 2048; + +#[cfg(target_pointer_width = "32")] +pub const GGML_MEM_ALIGN: usize = 4; +#[cfg(all(not(target_pointer_width = "32"), target_os = "emscripten"))] +pub const GGML_MEM_ALIGN: usize = 8; +#[cfg(all(not(target_pointer_width = "32"), not(target_os = "emscripten")))] +pub const GGML_MEM_ALIGN: usize = 16; + +#[inline] +pub const fn ggml_pad(value: usize, alignment: usize) -> usize { + ((value + alignment - 1) / alignment) * alignment +} + +#[repr(i32)] +#[derive(Clone, Copy, Debug, Eq, PartialEq, Hash)] +pub enum Status { + AllocFailed = -2, + Failed = -1, + Success = 0, + Aborted = 1, +} + +impl Status { + pub fn as_str(self) -> &'static str { + match self { + Self::AllocFailed => "alloc_failed", + Self::Failed => "failed", + Self::Success => "success", + Self::Aborted => "aborted", + } + } +} + +#[repr(u32)] +#[derive(Clone, Copy, Debug, Eq, PartialEq, Hash)] +pub enum ObjectType { + Tensor = 0, + Graph = 1, + WorkBuffer = 2, +} + +#[repr(u32)] +#[derive(Clone, Copy, Debug, Eq, PartialEq, Hash)] +pub enum LogLevel { + None = 0, + Debug = 1, + Info = 2, + Warn = 3, + Error = 4, + Continue = 5, +} + +#[repr(u32)] +#[derive(Clone, Copy, Debug, Eq, PartialEq, Hash)] +pub enum TensorFlag { + Input = 1, + Output = 2, + Param = 4, + Loss = 8, + Compute = 16, +} + +#[repr(u32)] +#[derive(Clone, Copy, Debug, Eq, PartialEq, Hash)] +pub enum TriType { + UpperDiag = 0, + Upper = 1, + LowerDiag = 2, + Lower = 3, +} + +pub const GGML_ROPE_TYPE_NORMAL: i32 = 0; +pub const GGML_ROPE_TYPE_NEOX: i32 = 2; +pub const GGML_ROPE_TYPE_MROPE: i32 = 8; +pub const GGML_ROPE_TYPE_VISION: i32 = 24; +pub const GGML_ROPE_TYPE_IMROPE: i32 = 40; +pub const GGML_MROPE_SECTIONS: usize = 4; + +#[repr(i32)] +#[derive(Clone, Copy, Debug, Eq, PartialEq, Hash)] +pub enum PoolOp { + Max = 0, + Avg = 1, +} + +impl PoolOp { + pub fn from_i32(value: i32) -> Option { + Some(match value { + 0 => Self::Max, + 1 => Self::Avg, + _ => return None, + }) + } + + pub fn name(self) -> &'static str { + match self { + Self::Max => "max", + Self::Avg => "avg", + } + } +} + +#[repr(i32)] +#[derive(Clone, Copy, Debug, Eq, PartialEq, Hash)] +pub enum ScaleMode { + Nearest = 0, + Bilinear = 1, + Bicubic = 2, +} + +impl ScaleMode { + pub fn from_i32(value: i32) -> Option { + Some(match value { + 0 => Self::Nearest, + 1 => Self::Bilinear, + 2 => Self::Bicubic, + _ => return None, + }) + } +} + +pub const GGML_SCALE_FLAG_ALIGN_CORNERS: i32 = 1 << 8; +pub const GGML_SCALE_FLAG_ANTIALIAS: i32 = 1 << 9; + +#[repr(i32)] +#[derive(Clone, Copy, Debug, Eq, PartialEq, Hash)] +pub enum SortOrder { + Asc = 0, + Desc = 1, +} + +impl SortOrder { + pub fn from_i32(value: i32) -> Option { + Some(match value { + 0 => Self::Asc, + 1 => Self::Desc, + _ => return None, + }) + } + + pub fn name(self) -> &'static str { + match self { + Self::Asc => "asc", + Self::Desc => "desc", + } + } +} + +#[derive(Clone, Debug, Default, Eq, PartialEq)] +pub struct InitParams { + pub mem_size: usize, + pub mem_buffer: Option>, + pub no_alloc: bool, +} diff --git a/libs/ggml/src/graph.rs b/libs/ggml/src/graph.rs new file mode 100644 index 0000000000..ce2f75381c --- /dev/null +++ b/libs/ggml/src/graph.rs @@ -0,0 +1,167 @@ +use std::collections::HashSet; + +use crate::context::Context; +use crate::tensor::{Tensor, TensorId}; + +pub type NodeId = TensorId; + +#[repr(u32)] +#[derive(Clone, Copy, Debug, Eq, PartialEq, Hash)] +pub enum GraphEvalOrder { + LeftToRight = 0, + RightToLeft = 1, +} + +#[derive(Clone, Debug)] +pub struct Graph { + pub size: usize, + pub nodes: Vec, + pub grads: Vec>, + pub grad_accs: Vec>, + pub leafs: Vec, + pub order: GraphEvalOrder, +} + +impl Default for Graph { + fn default() -> Self { + Self::with_size(crate::core::GGML_DEFAULT_GRAPH_SIZE, false) + } +} + +impl Graph { + pub fn new() -> Self { + Self::default() + } + + pub fn with_size(size: usize, grads: bool) -> Self { + Self { + size, + nodes: Vec::with_capacity(size), + grads: if grads { + Vec::with_capacity(size) + } else { + Vec::new() + }, + grad_accs: if grads { + Vec::with_capacity(size) + } else { + Vec::new() + }, + leafs: Vec::with_capacity(size), + order: GraphEvalOrder::LeftToRight, + } + } + + pub fn clear(&mut self) { + self.nodes.clear(); + self.grads.clear(); + self.grad_accs.clear(); + self.leafs.clear(); + } + + pub fn graph_size(&self) -> usize { + self.size + } + + pub fn n_nodes(&self) -> usize { + self.nodes.len() + } + + pub fn n_leafs(&self) -> usize { + self.leafs.len() + } + + pub fn node(&self, i: isize) -> Option { + if i >= 0 { + self.nodes.get(i as usize).copied() + } else { + let idx = self.nodes.len().checked_sub(i.unsigned_abs())?; + self.nodes.get(idx).copied() + } + } + + pub fn add_node(&mut self, tensor: TensorId) { + if !self.nodes.contains(&tensor) { + self.nodes.push(tensor); + } + } + + pub fn add_leaf(&mut self, tensor: TensorId) { + if !self.leafs.contains(&tensor) { + self.leafs.push(tensor); + } + } + + pub fn get_tensor<'a>(&self, ctx: &'a Context, name: &str) -> Option<&'a Tensor> { + self.nodes + .iter() + .chain(self.leafs.iter()) + .copied() + .find_map(|id| { + let tensor = ctx.tensor(id)?; + (tensor.name() == Some(name)).then_some(tensor) + }) + } + + pub fn get_grad(&self, node: TensorId) -> Option { + self.nodes + .iter() + .position(|¤t| current == node) + .and_then(|index| self.grads.get(index).copied().flatten()) + } + + pub fn get_grad_acc(&self, node: TensorId) -> Option { + self.nodes + .iter() + .position(|¤t| current == node) + .and_then(|index| self.grad_accs.get(index).copied().flatten()) + } + + pub fn build_forward_expand(&mut self, ctx: &Context, tensor: TensorId) -> Result<(), String> { + let mut visited = HashSet::new(); + self.build_forward_expand_impl(ctx, tensor, &mut visited) + } + + pub fn view(&self, i0: usize, i1: usize) -> Self { + let end = i1.min(self.nodes.len()); + let start = i0.min(end); + Self { + size: end - start, + nodes: self.nodes[start..end].to_vec(), + grads: Vec::new(), + grad_accs: Vec::new(), + leafs: Vec::new(), + order: self.order, + } + } + + fn build_forward_expand_impl( + &mut self, + ctx: &Context, + tensor: TensorId, + visited: &mut HashSet, + ) -> Result<(), String> { + if !visited.insert(tensor) { + return Ok(()); + } + + let t = ctx + .tensor(tensor) + .ok_or_else(|| format!("invalid tensor id {}", tensor))?; + + for src in t.src.iter().flatten().copied() { + self.build_forward_expand_impl(ctx, src, visited)?; + } + if let Some(view_src) = t.view_src { + self.build_forward_expand_impl(ctx, view_src, visited)?; + } + + let is_leaf = t.op == crate::op::Op::None && t.src.iter().all(Option::is_none); + if is_leaf { + self.add_leaf(tensor); + } else { + self.add_node(tensor); + } + Ok(()) + } +} diff --git a/libs/ggml/src/lib.rs b/libs/ggml/src/lib.rs new file mode 100644 index 0000000000..1d06a1ec9d --- /dev/null +++ b/libs/ggml/src/lib.rs @@ -0,0 +1,37 @@ +pub mod backend; +pub mod context; +pub mod core; +pub mod graph; +pub mod op; +pub mod plan; +pub mod quant; +pub mod runtime; +pub mod tensor; + +pub use backend::{ + Backend, BackendBuffer, BackendBufferType, BackendBufferUsage, BackendDeviceCaps, + BackendDeviceProps, BackendDeviceType, BackendEvent, BackendGraphPlan, +}; +pub use backend::{BackendCapabilities, BackendInfo, BackendKind}; +pub use context::Context; +pub use core::{ + ggml_pad, InitParams, LogLevel, ObjectType, PoolOp, ScaleMode, SortOrder, Status, TensorFlag, + TriType, GGML_DEFAULT_GRAPH_SIZE, GGML_DEFAULT_N_THREADS, GGML_FILE_MAGIC, GGML_FILE_VERSION, + GGML_MAX_DIMS, GGML_MAX_NAME, GGML_MAX_N_THREADS, GGML_MAX_OP_PARAMS, GGML_MAX_PARAMS, + GGML_MAX_SRC, GGML_MEM_ALIGN, GGML_MROPE_SECTIONS, GGML_QNT_VERSION, GGML_QNT_VERSION_FACTOR, + GGML_ROPE_TYPE_IMROPE, GGML_ROPE_TYPE_MROPE, GGML_ROPE_TYPE_NEOX, GGML_ROPE_TYPE_NORMAL, + GGML_ROPE_TYPE_VISION, GGML_SCALE_FLAG_ALIGN_CORNERS, GGML_SCALE_FLAG_ANTIALIAS, +}; +pub use graph::{Graph, GraphEvalOrder, NodeId}; +pub use op::{ + ggml_glu_op_name, ggml_op_name, ggml_op_symbol, ggml_unary_op_name, Ftype, GluOp, Op, Prec, + UnaryOp, +}; +pub use plan::{BufferSlice, ExecutionPlan}; +pub use quant::*; +pub use runtime::{CompiledGraph, RuntimeConfig}; +pub use tensor::{ + ggml_blck_size_for_type, ggml_ftype_to_tensor_type, ggml_row_size_for_type, + ggml_type_size_for_type, BufferUsage, Tensor, TensorDesc, TensorFlags, TensorId, TensorLayout, + TensorType, +}; diff --git a/libs/ggml/src/op.rs b/libs/ggml/src/op.rs new file mode 100644 index 0000000000..c76dc8ae61 --- /dev/null +++ b/libs/ggml/src/op.rs @@ -0,0 +1,470 @@ +#[repr(i32)] +#[derive(Clone, Copy, Debug, Eq, PartialEq, Hash)] +pub enum Prec { + Default = 0, + F32 = 10, +} + +#[repr(i32)] +#[derive(Clone, Copy, Debug, Eq, PartialEq, Hash)] +pub enum Ftype { + Unknown = -1, + AllF32 = 0, + MostlyF16 = 1, + MostlyQ4_0 = 2, + MostlyQ4_1 = 3, + MostlyQ4_1SomeF16 = 4, + MostlyQ8_0 = 7, + MostlyQ5_0 = 8, + MostlyQ5_1 = 9, + MostlyQ2K = 10, + MostlyQ3K = 11, + MostlyQ4K = 12, + MostlyQ5K = 13, + MostlyQ6K = 14, + MostlyIq2Xxs = 15, + MostlyIq2Xs = 16, + MostlyIq3Xxs = 17, + MostlyIq1S = 18, + MostlyIq4Nl = 19, + MostlyIq3S = 20, + MostlyIq2S = 21, + MostlyIq4Xs = 22, + MostlyIq1M = 23, + MostlyBf16 = 24, + MostlyMxfp4 = 25, + MostlyNvfp4 = 26, +} + +#[repr(u32)] +#[derive(Clone, Copy, Debug, Eq, PartialEq, Hash)] +pub enum Op { + None = 0, + Dup, + Add, + AddId, + Add1, + Acc, + Sub, + Mul, + Div, + Sqr, + Sqrt, + Log, + Sin, + Cos, + Sum, + SumRows, + CumSum, + Mean, + ArgMax, + CountEqual, + Repeat, + RepeatBack, + Concat, + SiluBack, + Norm, + RmsNorm, + RmsNormBack, + GroupNorm, + L2Norm, + MulMat, + MulMatId, + OutProd, + Scale, + Set, + Cpy, + Cont, + Reshape, + View, + Permute, + Transpose, + GetRows, + GetRowsBack, + SetRows, + Diag, + DiagMaskInf, + DiagMaskZero, + SoftMax, + SoftMaxBack, + Rope, + RopeBack, + Clamp, + ConvTranspose1d, + Im2col, + Im2colBack, + Im2col3d, + Conv2d, + Conv3d, + Conv2dDw, + ConvTranspose2d, + Pool1d, + Pool2d, + Pool2dBack, + Upscale, + Pad, + PadReflect1d, + Roll, + Arange, + TimestepEmbedding, + Argsort, + TopK, + LeakyRelu, + Tri, + Fill, + FlashAttnExt, + FlashAttnBack, + SsmConv, + SsmScan, + WinPart, + WinUnpart, + GetRelPos, + AddRelPos, + RwkvWkv6, + GatedLinearAttn, + RwkvWkv7, + SolveTri, + GatedDeltaNet, + Unary, + MapCustom1, + MapCustom2, + MapCustom3, + Custom, + CrossEntropyLoss, + CrossEntropyLossBack, + OptStepAdamw, + OptStepSgd, + Glu, +} + +#[repr(u32)] +#[derive(Clone, Copy, Debug, Eq, PartialEq, Hash)] +pub enum UnaryOp { + Abs = 0, + Sgn, + Neg, + Step, + Tanh, + Elu, + Relu, + Sigmoid, + Gelu, + GeluQuick, + Silu, + Hardswish, + Hardsigmoid, + Exp, + Expm1, + SoftPlus, + GeluErf, + XiElu, + Floor, + Ceil, + Round, + Trunc, +} + +#[repr(u32)] +#[derive(Clone, Copy, Debug, Eq, PartialEq, Hash)] +pub enum GluOp { + Reglu = 0, + Geglu, + Swiglu, + SwigluOai, + GegluErf, + GegluQuick, +} + +const OP_NAMES: [&str; 96] = [ + "NONE", + "DUP", + "ADD", + "ADD_ID", + "ADD1", + "ACC", + "SUB", + "MUL", + "DIV", + "SQR", + "SQRT", + "LOG", + "SIN", + "COS", + "SUM", + "SUM_ROWS", + "CUMSUM", + "MEAN", + "ARGMAX", + "COUNT_EQUAL", + "REPEAT", + "REPEAT_BACK", + "CONCAT", + "SILU_BACK", + "NORM", + "RMS_NORM", + "RMS_NORM_BACK", + "GROUP_NORM", + "L2_NORM", + "MUL_MAT", + "MUL_MAT_ID", + "OUT_PROD", + "SCALE", + "SET", + "CPY", + "CONT", + "RESHAPE", + "VIEW", + "PERMUTE", + "TRANSPOSE", + "GET_ROWS", + "GET_ROWS_BACK", + "SET_ROWS", + "DIAG", + "DIAG_MASK_INF", + "DIAG_MASK_ZERO", + "SOFT_MAX", + "SOFT_MAX_BACK", + "ROPE", + "ROPE_BACK", + "CLAMP", + "CONV_TRANSPOSE_1D", + "IM2COL", + "IM2COL_BACK", + "IM2COL_3D", + "CONV_2D", + "CONV_3D", + "CONV_2D_DW", + "CONV_TRANSPOSE_2D", + "POOL_1D", + "POOL_2D", + "POOL_2D_BACK", + "UPSCALE", + "PAD", + "PAD_REFLECT_1D", + "ROLL", + "ARANGE", + "TIMESTEP_EMBEDDING", + "ARGSORT", + "TOP_K", + "LEAKY_RELU", + "TRI", + "FILL", + "FLASH_ATTN_EXT", + "FLASH_ATTN_BACK", + "SSM_CONV", + "SSM_SCAN", + "WIN_PART", + "WIN_UNPART", + "GET_REL_POS", + "ADD_REL_POS", + "RWKV_WKV6", + "GATED_LINEAR_ATTN", + "RWKV_WKV7", + "SOLVE_TRI", + "GATED_DELTA_NET", + "UNARY", + "MAP_CUSTOM1", + "MAP_CUSTOM2", + "MAP_CUSTOM3", + "CUSTOM", + "CROSS_ENTROPY_LOSS", + "CROSS_ENTROPY_LOSS_BACK", + "OPT_STEP_ADAMW", + "OPT_STEP_SGD", + "GLU", +]; + +const OP_SYMBOLS: [&str; 96] = [ + "none", + "x", + "x+y", + "x[i]+y", + "x+y", + "view(x,nb,offset)+=y->x", + "x-y", + "x*y", + "x/y", + "x^2", + "√x", + "log(x)", + "sin(x)", + "cos(x)", + "Σx", + "Σx_k", + "cumsum(x)", + "Σx/n", + "argmax(x)", + "count_equal(x)", + "repeat(x)", + "repeat_back(x)", + "concat(x, y)", + "silu_back(x)", + "norm(x)", + "rms_norm(x)", + "rms_norm_back(x)", + "group_norm(x)", + "l2_norm(x)", + "X*Y", + "X[i]*Y", + "X*Y", + "x*v", + "y-\\>view(x)", + "x-\\>y", + "cont(x)", + "reshape(x)", + "view(x)", + "permute(x)", + "transpose(x)", + "get_rows(x)", + "get_rows_back(x)", + "set_rows(x)", + "diag(x)", + "diag_mask_inf(x)", + "diag_mask_zero(x)", + "soft_max(x)", + "soft_max_back(x)", + "rope(x)", + "rope_back(x)", + "clamp(x)", + "conv_transpose_1d(x)", + "im2col(x)", + "im2col_back(x)", + "im2col_3d(x)", + "conv_2d(x)", + "conv_3d(x)", + "conv_2d_dw(x)", + "conv_transpose_2d(x)", + "pool_1d(x)", + "pool_2d(x)", + "pool_2d_back(x)", + "upscale(x)", + "pad(x)", + "pad_reflect_1d(x)", + "roll(x)", + "arange(start, stop, step)", + "timestep_embedding(timesteps, dim, max_period)", + "argsort(x)", + "top_k(x)", + "leaky_relu(x)", + "tri(x)", + "fill(x, c)", + "flash_attn_ext(x)", + "flash_attn_back(x)", + "ssm_conv(x)", + "ssm_scan(x)", + "win_part(x)", + "win_unpart(x)", + "get_rel_pos(x)", + "add_rel_pos(x)", + "rwkv_wkv6(k, v, r, tf, td, s)", + "gated_linear_attn(k, v, q, gate, s)", + "rwkv_wkv7(r, w, k, v, a, b, s)", + "A X = B, A triangular, solve X", + "gated_delta_net(q, k, v, g, beta, s)", + "unary(x)", + "map_custom(x)", + "map_custom(x,y)", + "map_custom(x,y,z)", + "custom(x)", + "cross_entropy_loss(x,y)", + "cross_entropy_loss_back(x,y)", + "adamw(x)", + "sgd(x)", + "glu(x)", +]; + +const UNARY_NAMES: [&str; 22] = [ + "ABS", + "SGN", + "NEG", + "STEP", + "TANH", + "ELU", + "RELU", + "SIGMOID", + "GELU", + "GELU_QUICK", + "SILU", + "HARDSWISH", + "HARDSIGMOID", + "EXP", + "EXPM1", + "SOFTPLUS", + "GELU_ERF", + "XIELU", + "FLOOR", + "CEIL", + "ROUND", + "TRUNC", +]; + +const GLU_NAMES: [&str; 6] = [ + "REGLU", + "GEGLU", + "SWIGLU", + "SWIGLU_OAI", + "GEGLU_ERF", + "GEGLU_QUICK", +]; + +impl Op { + pub fn from_u32(value: u32) -> Option { + if value < OP_NAMES.len() as u32 { + Some(unsafe { std::mem::transmute(value) }) + } else { + None + } + } + + pub fn name(self) -> &'static str { + OP_NAMES[self as usize] + } + + pub fn symbol(self) -> &'static str { + OP_SYMBOLS[self as usize] + } +} + +impl UnaryOp { + pub fn from_u32(value: u32) -> Option { + if value < UNARY_NAMES.len() as u32 { + Some(unsafe { std::mem::transmute(value) }) + } else { + None + } + } + + pub fn name(self) -> &'static str { + UNARY_NAMES[self as usize] + } +} + +impl GluOp { + pub fn from_u32(value: u32) -> Option { + if value < GLU_NAMES.len() as u32 { + Some(unsafe { std::mem::transmute(value) }) + } else { + None + } + } + + pub fn name(self) -> &'static str { + GLU_NAMES[self as usize] + } +} + +pub fn ggml_op_name(op: Op) -> &'static str { + op.name() +} + +pub fn ggml_op_symbol(op: Op) -> &'static str { + op.symbol() +} + +pub fn ggml_unary_op_name(op: UnaryOp) -> &'static str { + op.name() +} + +pub fn ggml_glu_op_name(op: GluOp) -> &'static str { + op.name() +} diff --git a/libs/ggml/src/plan.rs b/libs/ggml/src/plan.rs new file mode 100644 index 0000000000..4e4dd915d6 --- /dev/null +++ b/libs/ggml/src/plan.rs @@ -0,0 +1,26 @@ +use crate::graph::NodeId; +use crate::tensor::BufferUsage; + +#[derive(Clone, Copy, Debug, Eq, PartialEq)] +pub struct BufferSlice { + pub usage: BufferUsage, + pub offset_bytes: usize, + pub size_bytes: usize, +} + +#[derive(Clone, Debug, Default)] +pub struct ExecutionPlan { + pub allocations: Vec, + pub execution_order: Vec, + pub reusable: bool, +} + +impl ExecutionPlan { + pub fn push_allocation(&mut self, slice: BufferSlice) { + self.allocations.push(slice); + } + + pub fn push_node(&mut self, id: NodeId) { + self.execution_order.push(id); + } +} diff --git a/libs/ggml/src/quant.rs b/libs/ggml/src/quant.rs new file mode 100644 index 0000000000..7658385e0f --- /dev/null +++ b/libs/ggml/src/quant.rs @@ -0,0 +1,623 @@ +// Quantization block formats matching GGML's layout. +// All blocks quantize 32 elements (QK=32). + +pub const QK: usize = 32; + +/// Convert f16 (IEEE 754 half-precision) to f32. +#[inline] +pub fn f16_to_f32(h: u16) -> f32 { + let sign = ((h >> 15) & 1) as u32; + let exp = ((h >> 10) & 0x1f) as u32; + let mant = (h & 0x3ff) as u32; + + if exp == 0 { + if mant == 0 { + return f32::from_bits(sign << 31); + } + // subnormal + let mut e = 0i32; + let mut m = mant; + while (m & 0x400) == 0 { + m <<= 1; + e -= 1; + } + m &= 0x3ff; + let exp32 = (127 - 15 + 1 + e) as u32; + return f32::from_bits((sign << 31) | (exp32 << 23) | (m << 13)); + } + if exp == 31 { + if mant == 0 { + return f32::from_bits((sign << 31) | (0xff << 23)); + } + return f32::from_bits((sign << 31) | (0xff << 23) | (mant << 13)); + } + let exp32 = exp + (127 - 15); + f32::from_bits((sign << 31) | (exp32 << 23) | (mant << 13)) +} + +/// Convert f32 to f16. +#[inline] +pub fn f32_to_f16(f: f32) -> u16 { + let b = f.to_bits(); + let sign = ((b >> 16) & 0x8000) as u16; + let exp = ((b >> 23) & 0xff) as i32; + let mant = b & 0x7fffff; + + if exp == 0xff { + if mant == 0 { + return sign | 0x7c00; + } + return sign | 0x7c00 | ((mant >> 13) as u16).max(1); + } + let unbiased = exp - 127; + if unbiased > 15 { + return sign | 0x7c00; + } + if unbiased < -14 { + // subnormal or zero + if unbiased < -24 { + return sign; + } + let shift = (-1 - unbiased) as u32; + let m = (0x800000 | mant) >> (shift + 1); + // round to nearest even + let round = (m >> 13) as u16; + return sign | round; + } + let h_exp = ((unbiased + 15) as u16) << 10; + let h_mant = (mant >> 13) as u16; + sign | h_exp | h_mant +} + +// ---- Dequantization for each block type ---- + +/// Q4_0: 4-bit quantization, block = 2 bytes (f16 scale) + 16 bytes (32 nibbles) +/// Total: 18 bytes per 32 elements +pub fn dequantize_q4_0(block: &[u8], out: &mut [f32]) { + debug_assert!(block.len() >= 18); + debug_assert!(out.len() >= QK); + let d = f16_to_f32(u16::from_le_bytes([block[0], block[1]])); + let qs = &block[2..18]; + for j in 0..QK / 2 { + let lo = (qs[j] & 0x0f) as i32 - 8; + let hi = ((qs[j] >> 4) & 0x0f) as i32 - 8; + out[j] = lo as f32 * d; + out[j + QK / 2] = hi as f32 * d; + } +} + +/// Q4_1: 4-bit with min, block = 2 bytes (f16 d) + 2 bytes (f16 m) + 16 bytes +/// Total: 20 bytes per 32 elements +pub fn dequantize_q4_1(block: &[u8], out: &mut [f32]) { + debug_assert!(block.len() >= 20); + debug_assert!(out.len() >= QK); + let d = f16_to_f32(u16::from_le_bytes([block[0], block[1]])); + let m = f16_to_f32(u16::from_le_bytes([block[2], block[3]])); + let qs = &block[4..20]; + for j in 0..QK / 2 { + let lo = (qs[j] & 0x0f) as f32; + let hi = ((qs[j] >> 4) & 0x0f) as f32; + out[j] = lo * d + m; + out[j + QK / 2] = hi * d + m; + } +} + +/// Q5_0: 5-bit quantization, block = 2 bytes (f16 d) + 4 bytes (high bits) + 16 bytes (low nibbles) +/// Total: 22 bytes per 32 elements +pub fn dequantize_q5_0(block: &[u8], out: &mut [f32]) { + debug_assert!(block.len() >= 22); + debug_assert!(out.len() >= QK); + let d = f16_to_f32(u16::from_le_bytes([block[0], block[1]])); + let qh = u32::from_le_bytes([block[2], block[3], block[4], block[5]]); + let qs = &block[6..22]; + for j in 0..QK / 2 { + let xh_0 = ((qh >> j) & 1) as i32; + let xh_1 = ((qh >> (j + 16)) & 1) as i32; + let lo = ((qs[j] & 0x0f) as i32 | (xh_0 << 4)) - 16; + let hi = (((qs[j] >> 4) & 0x0f) as i32 | (xh_1 << 4)) - 16; + out[j] = lo as f32 * d; + out[j + QK / 2] = hi as f32 * d; + } +} + +/// Q5_1: 5-bit with min, block = 2+2 bytes (f16 d,m) + 4 bytes (high bits) + 16 bytes +/// Total: 24 bytes per 32 elements +pub fn dequantize_q5_1(block: &[u8], out: &mut [f32]) { + debug_assert!(block.len() >= 24); + debug_assert!(out.len() >= QK); + let d = f16_to_f32(u16::from_le_bytes([block[0], block[1]])); + let m = f16_to_f32(u16::from_le_bytes([block[2], block[3]])); + let qh = u32::from_le_bytes([block[4], block[5], block[6], block[7]]); + let qs = &block[8..24]; + for j in 0..QK / 2 { + let xh_0 = ((qh >> j) & 1) as u32; + let xh_1 = ((qh >> (j + 16)) & 1) as u32; + let lo = (qs[j] & 0x0f) as u32 | (xh_0 << 4); + let hi = ((qs[j] >> 4) & 0x0f) as u32 | (xh_1 << 4); + out[j] = lo as f32 * d + m; + out[j + QK / 2] = hi as f32 * d + m; + } +} + +/// Q8_0: 8-bit quantization, block = 2 bytes (f16 d) + 32 bytes (int8) +/// Total: 34 bytes per 32 elements +pub fn dequantize_q8_0(block: &[u8], out: &mut [f32]) { + debug_assert!(block.len() >= 34); + debug_assert!(out.len() >= QK); + let d = f16_to_f32(u16::from_le_bytes([block[0], block[1]])); + for j in 0..QK { + out[j] = (block[2 + j] as i8) as f32 * d; + } +} + +// ---- Vector dot products (quantized × f32 → partial sum) ---- +// Used in matrix multiply: dot product of a quantized row with an f32 row + +/// Dot product: one Q4_0 block (32 quantized values) dot 32 f32 values +pub fn vec_dot_q4_0_f32(block: &[u8], v: &[f32]) -> f32 { + let d = f16_to_f32(u16::from_le_bytes([block[0], block[1]])); + let qs = &block[2..18]; + let mut sum = 0.0f32; + for j in 0..QK / 2 { + let lo = (qs[j] & 0x0f) as i32 - 8; + let hi = ((qs[j] >> 4) & 0x0f) as i32 - 8; + sum += lo as f32 * v[j]; + sum += hi as f32 * v[j + QK / 2]; + } + sum * d +} + +/// Dot product: one Q5_0 block dot 32 f32 values +pub fn vec_dot_q5_0_f32(block: &[u8], v: &[f32]) -> f32 { + let d = f16_to_f32(u16::from_le_bytes([block[0], block[1]])); + let qh = u32::from_le_bytes([block[2], block[3], block[4], block[5]]); + let qs = &block[6..22]; + let mut sum = 0.0f32; + for j in 0..QK / 2 { + let xh_0 = ((qh >> j) & 1) as i32; + let xh_1 = ((qh >> (j + 16)) & 1) as i32; + let lo = ((qs[j] & 0x0f) as i32 | (xh_0 << 4)) - 16; + let hi = (((qs[j] >> 4) & 0x0f) as i32 | (xh_1 << 4)) - 16; + sum += lo as f32 * v[j]; + sum += hi as f32 * v[j + QK / 2]; + } + sum * d +} + +/// Dot product: one Q5_0 block dot one Q8_0 block (32 values each). +/// Returns dequantized f32 sum. +#[inline] +pub fn vec_dot_q5_0_q8_0(a: &[u8], b: &[u8]) -> f32 { + let da = f16_to_f32(u16::from_le_bytes([a[0], a[1]])); + let db = f16_to_f32(u16::from_le_bytes([b[0], b[1]])); + let qh = u32::from_le_bytes([a[2], a[3], a[4], a[5]]); + let qs = &a[6..22]; + let y = &b[2..34]; + + let mut sumi = 0i32; + for j in 0..QK / 2 { + let xh_0 = ((qh >> j) & 1) as i32; + let xh_1 = ((qh >> (j + 16)) & 1) as i32; + let x0 = ((qs[j] & 0x0f) as i32 | (xh_0 << 4)) - 16; + let x1 = (((qs[j] >> 4) & 0x0f) as i32 | (xh_1 << 4)) - 16; + sumi += x0 * (y[j] as i8 as i32); + sumi += x1 * (y[j + QK / 2] as i8 as i32); + } + + (sumi as f32) * da * db +} + +/// Dot product: one Q8_0 block dot 32 f32 values +#[inline] +pub fn vec_dot_q8_0_f32(block: &[u8], v: &[f32]) -> f32 { + vec_dot_q8_0_f32_simd(block, v) +} + +/// Dot product: one Q8_0 block dot another Q8_0 block (32 values each). +/// Returns dequantized f32 sum. +#[inline] +pub fn vec_dot_q8_0_q8_0(a: &[u8], b: &[u8]) -> f32 { + let da = f16_to_f32(u16::from_le_bytes([a[0], a[1]])); + let db = f16_to_f32(u16::from_le_bytes([b[0], b[1]])); + let sum = vec_dot_q8_0_q8_0_i32(&a[2..34], &b[2..34]); + (sum as f32) * da * db +} + +#[cfg(target_arch = "aarch64")] +#[inline] +fn vec_dot_q8_0_q8_0_i32(qa: &[u8], qb: &[u8]) -> i32 { + unsafe { vec_dot_q8_0_q8_0_i32_neon_mul(qa, qb) } +} + +#[cfg(target_arch = "aarch64")] +unsafe fn vec_dot_q8_0_q8_0_i32_neon_mul(qa: &[u8], qb: &[u8]) -> i32 { + use std::arch::aarch64::*; + let mut acc = vdupq_n_s32(0); + for i in (0..QK).step_by(16) { + let a8 = vld1q_s8(qa.as_ptr().add(i) as *const i8); + let b8 = vld1q_s8(qb.as_ptr().add(i) as *const i8); + + let a_lo = vmovl_s8(vget_low_s8(a8)); + let b_lo = vmovl_s8(vget_low_s8(b8)); + let p_lo = vmulq_s16(a_lo, b_lo); + + let a_hi = vmovl_s8(vget_high_s8(a8)); + let b_hi = vmovl_s8(vget_high_s8(b8)); + let p_hi = vmulq_s16(a_hi, b_hi); + + acc = vaddq_s32(acc, vpaddlq_s16(p_lo)); + acc = vaddq_s32(acc, vpaddlq_s16(p_hi)); + } + vaddvq_s32(acc) +} + +#[cfg(not(target_arch = "aarch64"))] +#[inline] +fn vec_dot_q8_0_q8_0_i32(qa: &[u8], qb: &[u8]) -> i32 { + let mut sum = 0i32; + let mut i = 0; + while i + 3 < QK { + sum += (qa[i] as i8 as i32) * (qb[i] as i8 as i32); + sum += (qa[i + 1] as i8 as i32) * (qb[i + 1] as i8 as i32); + sum += (qa[i + 2] as i8 as i32) * (qb[i + 2] as i8 as i32); + sum += (qa[i + 3] as i8 as i32) * (qb[i + 3] as i8 as i32); + i += 4; + } + while i < QK { + sum += (qa[i] as i8 as i32) * (qb[i] as i8 as i32); + i += 1; + } + sum +} + +#[cfg(target_arch = "aarch64")] +#[inline] +fn vec_dot_q8_0_f32_simd(block: &[u8], v: &[f32]) -> f32 { + use std::arch::aarch64::*; + let d = f16_to_f32(u16::from_le_bytes([block[0], block[1]])); + let qs = &block[2..34]; + unsafe { + let mut sum0 = vdupq_n_f32(0.0); + let mut sum1 = vdupq_n_f32(0.0); + let mut sum2 = vdupq_n_f32(0.0); + let mut sum3 = vdupq_n_f32(0.0); + // Process 16 elements per iteration, 2 iterations for 32 elements + for i in (0..32).step_by(16) { + // Load 16 int8 values and widen to f32 + let q0 = vld1_s8(qs.as_ptr().add(i) as *const i8); + let q0_16 = vmovl_s8(q0); // i8x8 -> i16x8 + let q0_lo = vmovl_s16(vget_low_s16(q0_16)); // i16x4 -> i32x4 + let q0_hi = vmovl_s16(vget_high_s16(q0_16)); + let w0 = vcvtq_f32_s32(q0_lo); + let w1 = vcvtq_f32_s32(q0_hi); + + let q1 = vld1_s8(qs.as_ptr().add(i + 8) as *const i8); + let q1_16 = vmovl_s8(q1); + let q1_lo = vmovl_s16(vget_low_s16(q1_16)); + let q1_hi = vmovl_s16(vget_high_s16(q1_16)); + let w2 = vcvtq_f32_s32(q1_lo); + let w3 = vcvtq_f32_s32(q1_hi); + + let v0 = vld1q_f32(v.as_ptr().add(i)); + let v1 = vld1q_f32(v.as_ptr().add(i + 4)); + let v2 = vld1q_f32(v.as_ptr().add(i + 8)); + let v3 = vld1q_f32(v.as_ptr().add(i + 12)); + + sum0 = vfmaq_f32(sum0, w0, v0); + sum1 = vfmaq_f32(sum1, w1, v1); + sum2 = vfmaq_f32(sum2, w2, v2); + sum3 = vfmaq_f32(sum3, w3, v3); + } + sum0 = vaddq_f32(sum0, sum1); + sum2 = vaddq_f32(sum2, sum3); + sum0 = vaddq_f32(sum0, sum2); + vaddvq_f32(sum0) * d + } +} + +#[cfg(target_arch = "x86_64")] +#[inline] +fn vec_dot_q8_0_f32_simd(block: &[u8], v: &[f32]) -> f32 { + if is_x86_feature_detected!("avx2") && is_x86_feature_detected!("fma") { + unsafe { vec_dot_q8_0_f32_avx2(block, v) } + } else if is_x86_feature_detected!("avx") && is_x86_feature_detected!("fma") { + unsafe { vec_dot_q8_0_f32_avx(block, v) } + } else { + vec_dot_q8_0_f32_scalar(block, v) + } +} + +#[cfg(target_arch = "x86_64")] +#[target_feature(enable = "avx2,fma")] +unsafe fn vec_dot_q8_0_f32_avx2(block: &[u8], v: &[f32]) -> f32 { + use std::arch::x86_64::*; + let d = f16_to_f32(u16::from_le_bytes([block[0], block[1]])); + let qs_ptr = block.as_ptr().add(2) as *const i8; + let mut sum = _mm256_setzero_ps(); + for i in (0..32).step_by(8) { + let q8 = _mm_loadl_epi64(qs_ptr.add(i) as *const __m128i); + let q32 = _mm256_cvtepi8_epi32(q8); + let qf = _mm256_cvtepi32_ps(q32); + let vf = _mm256_loadu_ps(v.as_ptr().add(i)); + sum = _mm256_fmadd_ps(qf, vf, sum); + } + let hi = _mm256_extractf128_ps(sum, 1); + let lo = _mm256_castps256_ps128(sum); + let sum128 = _mm_add_ps(lo, hi); + let shuf = _mm_movehdup_ps(sum128); + let sums = _mm_add_ps(sum128, shuf); + let shuf2 = _mm_movehl_ps(sums, sums); + let sums2 = _mm_add_ss(sums, shuf2); + _mm_cvtss_f32(sums2) * d +} + +#[cfg(target_arch = "x86_64")] +#[target_feature(enable = "avx,fma")] +unsafe fn vec_dot_q8_0_f32_avx(block: &[u8], v: &[f32]) -> f32 { + use std::arch::x86_64::*; + let d = f16_to_f32(u16::from_le_bytes([block[0], block[1]])); + let qs = &block[2..34]; + let mut sum0 = _mm256_setzero_ps(); + let mut sum1 = _mm256_setzero_ps(); + let mut sum2 = _mm256_setzero_ps(); + let mut sum3 = _mm256_setzero_ps(); + // Process 8 elements at a time, 4 iterations for 32 elements + for i in (0..32).step_by(8) { + // Load 8 int8 and convert to f32 + let mut w = [0.0f32; 8]; + for j in 0..8 { + w[j] = (qs[i + j] as i8) as f32; + } + let wv = _mm256_loadu_ps(w.as_ptr()); + let vv = _mm256_loadu_ps(v.as_ptr().add(i)); + match i { + 0 => sum0 = _mm256_fmadd_ps(wv, vv, sum0), + 8 => sum1 = _mm256_fmadd_ps(wv, vv, sum1), + 16 => sum2 = _mm256_fmadd_ps(wv, vv, sum2), + _ => sum3 = _mm256_fmadd_ps(wv, vv, sum3), + } + } + sum0 = _mm256_add_ps(sum0, sum1); + sum2 = _mm256_add_ps(sum2, sum3); + sum0 = _mm256_add_ps(sum0, sum2); + let hi = _mm256_extractf128_ps(sum0, 1); + let lo = _mm256_castps256_ps128(sum0); + let sum128 = _mm_add_ps(lo, hi); + let shuf = _mm_movehdup_ps(sum128); + let sums = _mm_add_ps(sum128, shuf); + let shuf2 = _mm_movehl_ps(sums, sums); + let sums2 = _mm_add_ss(sums, shuf2); + _mm_cvtss_f32(sums2) * d +} + +#[cfg(not(any(target_arch = "aarch64", target_arch = "x86_64")))] +#[inline] +fn vec_dot_q8_0_f32_simd(block: &[u8], v: &[f32]) -> f32 { + vec_dot_q8_0_f32_scalar(block, v) +} + +#[inline] +#[cfg(not(target_arch = "aarch64"))] +fn vec_dot_q8_0_f32_scalar(block: &[u8], v: &[f32]) -> f32 { + let d = f16_to_f32(u16::from_le_bytes([block[0], block[1]])); + let mut sum = 0.0f32; + for j in 0..QK { + sum += (block[2 + j] as i8) as f32 * v[j]; + } + sum * d +} + +pub const QK_K: usize = 256; +pub const QK4_NL: usize = 32; +pub const QK_MXFP4: usize = 32; +pub const QK_NVFP4: usize = 64; + +/// GGML type constants copied from upstream `ggml.h`. +pub const GGML_TYPE_F32: u32 = 0; +pub const GGML_TYPE_F16: u32 = 1; +pub const GGML_TYPE_Q4_0: u32 = 2; +pub const GGML_TYPE_Q4_1: u32 = 3; +pub const GGML_TYPE_Q5_0: u32 = 6; +pub const GGML_TYPE_Q5_1: u32 = 7; +pub const GGML_TYPE_Q8_0: u32 = 8; +pub const GGML_TYPE_Q8_1: u32 = 9; +pub const GGML_TYPE_Q2_K: u32 = 10; +pub const GGML_TYPE_Q3_K: u32 = 11; +pub const GGML_TYPE_Q4_K: u32 = 12; +pub const GGML_TYPE_Q5_K: u32 = 13; +pub const GGML_TYPE_Q6_K: u32 = 14; +pub const GGML_TYPE_Q8_K: u32 = 15; +pub const GGML_TYPE_IQ2_XXS: u32 = 16; +pub const GGML_TYPE_IQ2_XS: u32 = 17; +pub const GGML_TYPE_IQ3_XXS: u32 = 18; +pub const GGML_TYPE_IQ1_S: u32 = 19; +pub const GGML_TYPE_IQ4_NL: u32 = 20; +pub const GGML_TYPE_IQ3_S: u32 = 21; +pub const GGML_TYPE_IQ2_S: u32 = 22; +pub const GGML_TYPE_IQ4_XS: u32 = 23; +pub const GGML_TYPE_I8: u32 = 24; +pub const GGML_TYPE_I16: u32 = 25; +pub const GGML_TYPE_I32: u32 = 26; +pub const GGML_TYPE_I64: u32 = 27; +pub const GGML_TYPE_F64: u32 = 28; +pub const GGML_TYPE_IQ1_M: u32 = 29; +pub const GGML_TYPE_BF16: u32 = 30; +pub const GGML_TYPE_TQ1_0: u32 = 34; +pub const GGML_TYPE_TQ2_0: u32 = 35; +pub const GGML_TYPE_MXFP4: u32 = 39; +pub const GGML_TYPE_NVFP4: u32 = 40; +pub const GGML_TYPE_COUNT: u32 = 41; + +pub fn ggml_type_name(ggml_type: u32) -> &'static str { + match ggml_type { + GGML_TYPE_F32 => "f32", + GGML_TYPE_F16 => "f16", + GGML_TYPE_Q4_0 => "q4_0", + GGML_TYPE_Q4_1 => "q4_1", + GGML_TYPE_Q5_0 => "q5_0", + GGML_TYPE_Q5_1 => "q5_1", + GGML_TYPE_Q8_0 => "q8_0", + GGML_TYPE_Q8_1 => "q8_1", + GGML_TYPE_Q2_K => "q2_K", + GGML_TYPE_Q3_K => "q3_K", + GGML_TYPE_Q4_K => "q4_K", + GGML_TYPE_Q5_K => "q5_K", + GGML_TYPE_Q6_K => "q6_K", + GGML_TYPE_Q8_K => "q8_K", + GGML_TYPE_IQ2_XXS => "iq2_xxs", + GGML_TYPE_IQ2_XS => "iq2_xs", + GGML_TYPE_IQ3_XXS => "iq3_xxs", + GGML_TYPE_IQ1_S => "iq1_s", + GGML_TYPE_IQ4_NL => "iq4_nl", + GGML_TYPE_IQ3_S => "iq3_s", + GGML_TYPE_IQ2_S => "iq2_s", + GGML_TYPE_IQ4_XS => "iq4_xs", + GGML_TYPE_I8 => "i8", + GGML_TYPE_I16 => "i16", + GGML_TYPE_I32 => "i32", + GGML_TYPE_I64 => "i64", + GGML_TYPE_F64 => "f64", + GGML_TYPE_IQ1_M => "iq1_m", + GGML_TYPE_BF16 => "bf16", + GGML_TYPE_TQ1_0 => "tq1_0", + GGML_TYPE_TQ2_0 => "tq2_0", + GGML_TYPE_MXFP4 => "mxfp4", + GGML_TYPE_NVFP4 => "nvfp4", + _ => "unknown", + } +} + +pub fn is_quantized_type(ggml_type: u32) -> bool { + !matches!( + ggml_type, + GGML_TYPE_F32 + | GGML_TYPE_F16 + | GGML_TYPE_I8 + | GGML_TYPE_I16 + | GGML_TYPE_I32 + | GGML_TYPE_I64 + | GGML_TYPE_F64 + | GGML_TYPE_BF16 + ) +} + +/// Type/block size in bytes for one ggml storage block. +pub fn block_size(ggml_type: u32) -> usize { + match ggml_type { + GGML_TYPE_F32 => 4, + GGML_TYPE_F16 => 2, + GGML_TYPE_Q4_0 => 18, + GGML_TYPE_Q4_1 => 20, + GGML_TYPE_Q5_0 => 22, + GGML_TYPE_Q5_1 => 24, + GGML_TYPE_Q8_0 => 34, + GGML_TYPE_Q8_1 => 36, + GGML_TYPE_Q2_K => 84, + GGML_TYPE_Q3_K => 110, + GGML_TYPE_Q4_K => 144, + GGML_TYPE_Q5_K => 176, + GGML_TYPE_Q6_K => 210, + GGML_TYPE_Q8_K => 292, + GGML_TYPE_IQ2_XXS => 66, + GGML_TYPE_IQ2_XS => 74, + GGML_TYPE_IQ3_XXS => 98, + GGML_TYPE_IQ1_S => 50, + GGML_TYPE_IQ4_NL => 18, + GGML_TYPE_IQ3_S => 110, + GGML_TYPE_IQ2_S => 82, + GGML_TYPE_IQ4_XS => 136, + GGML_TYPE_I8 => 1, + GGML_TYPE_I16 => 2, + GGML_TYPE_I32 => 4, + GGML_TYPE_I64 => 8, + GGML_TYPE_F64 => 8, + GGML_TYPE_IQ1_M => 56, + GGML_TYPE_BF16 => 2, + GGML_TYPE_TQ1_0 => 54, + GGML_TYPE_TQ2_0 => 66, + GGML_TYPE_MXFP4 => 17, + GGML_TYPE_NVFP4 => 36, + _ => panic!("unsupported ggml type {}", ggml_type), + } +} + +/// Number of dequantized elements represented by one storage block. +pub fn block_elements(ggml_type: u32) -> usize { + match ggml_type { + GGML_TYPE_F32 | GGML_TYPE_F16 | GGML_TYPE_I8 | GGML_TYPE_I16 | GGML_TYPE_I32 + | GGML_TYPE_I64 | GGML_TYPE_F64 | GGML_TYPE_BF16 => 1, + GGML_TYPE_Q4_0 | GGML_TYPE_Q4_1 | GGML_TYPE_Q5_0 | GGML_TYPE_Q5_1 | GGML_TYPE_Q8_0 + | GGML_TYPE_Q8_1 | GGML_TYPE_MXFP4 | GGML_TYPE_IQ4_NL => QK, + GGML_TYPE_NVFP4 => QK_NVFP4, + GGML_TYPE_Q2_K | GGML_TYPE_Q3_K | GGML_TYPE_Q4_K | GGML_TYPE_Q5_K | GGML_TYPE_Q6_K + | GGML_TYPE_Q8_K | GGML_TYPE_IQ2_XXS | GGML_TYPE_IQ2_XS | GGML_TYPE_IQ3_XXS + | GGML_TYPE_IQ1_S | GGML_TYPE_IQ3_S | GGML_TYPE_IQ2_S | GGML_TYPE_IQ4_XS + | GGML_TYPE_IQ1_M | GGML_TYPE_TQ1_0 | GGML_TYPE_TQ2_0 => QK_K, + _ => panic!("unsupported ggml type {}", ggml_type), + } +} + +/// Quantize a row of f32 values into one Q8_0 block (32 elements -> 34 bytes). +/// Finds the absmax, computes scale = absmax/127, quantizes each value to i8. +pub fn quantize_q8_0_block(input: &[f32], out: &mut [u8]) { + debug_assert!(input.len() >= QK); + debug_assert!(out.len() >= 34); + let mut amax = 0.0f32; + for j in 0..QK { + let a = input[j].abs(); + if a > amax { + amax = a; + } + } + let d = amax / 127.0; + let id = if d != 0.0 { 1.0 / d } else { 0.0 }; + let dh = f32_to_f16(d); + out[0] = dh as u8; + out[1] = (dh >> 8) as u8; + for j in 0..QK { + let v = (input[j] * id).round(); + let v = v.max(-128.0).min(127.0) as i8; + out[2 + j] = v as u8; + } +} + +/// Quantize an entire f32 slice to Q8_0 format. Length must be a multiple of QK(32). +pub fn quantize_f32_to_q8_0(input: &[f32]) -> Vec { + let n = input.len(); + assert_eq!( + n % QK, + 0, + "quantize_f32_to_q8_0: length must be multiple of {}", + QK + ); + let nb = n / QK; + let bs = 34; // block_size for Q8_0 + let mut out = vec![0u8; nb * bs]; + for b in 0..nb { + quantize_q8_0_block(&input[b * QK..], &mut out[b * bs..]); + } + out +} + +/// Quantize an F16 raw byte slice to Q8_0 format. Length in elements must be multiple of QK(32). +pub fn quantize_f16_to_q8_0(f16_data: &[u8], n_elements: usize) -> Vec { + assert_eq!( + n_elements % QK, + 0, + "quantize_f16_to_q8_0: length must be multiple of {}", + QK + ); + let nb = n_elements / QK; + let bs = 34; + let mut out = vec![0u8; nb * bs]; + let mut tmp = [0.0f32; QK]; + for b in 0..nb { + let base = b * QK * 2; + for j in 0..QK { + let off = base + j * 2; + tmp[j] = f16_to_f32(u16::from_le_bytes([f16_data[off], f16_data[off + 1]])); + } + quantize_q8_0_block(&tmp, &mut out[b * bs..]); + } + out +} diff --git a/libs/ggml/src/runtime.rs b/libs/ggml/src/runtime.rs new file mode 100644 index 0000000000..7495356360 --- /dev/null +++ b/libs/ggml/src/runtime.rs @@ -0,0 +1,23 @@ +use crate::backend::BackendInfo; +use crate::graph::Graph; +use crate::plan::ExecutionPlan; + +#[derive(Clone, Debug, Default)] +pub struct RuntimeConfig; + +#[derive(Clone, Debug)] +pub struct CompiledGraph { + pub graph: Graph, + pub plan: ExecutionPlan, + pub backend: BackendInfo, +} + +impl CompiledGraph { + pub fn new(graph: Graph, plan: ExecutionPlan, backend: BackendInfo) -> Self { + Self { + graph, + plan, + backend, + } + } +} diff --git a/libs/ggml/src/tensor.rs b/libs/ggml/src/tensor.rs new file mode 100644 index 0000000000..2d3db1c474 --- /dev/null +++ b/libs/ggml/src/tensor.rs @@ -0,0 +1,630 @@ +use crate::core::{TensorFlag, GGML_MAX_DIMS, GGML_MAX_NAME, GGML_MAX_OP_PARAMS, GGML_MAX_SRC}; +use crate::op::{Ftype, GluOp, Op, UnaryOp}; +use crate::quant::*; + +pub type TensorId = usize; + +#[repr(u32)] +#[derive(Clone, Copy, Debug, Eq, PartialEq, Hash)] +pub enum TensorType { + F32 = GGML_TYPE_F32, + F16 = GGML_TYPE_F16, + Q4_0 = GGML_TYPE_Q4_0, + Q4_1 = GGML_TYPE_Q4_1, + Q5_0 = GGML_TYPE_Q5_0, + Q5_1 = GGML_TYPE_Q5_1, + Q8_0 = GGML_TYPE_Q8_0, + Q8_1 = GGML_TYPE_Q8_1, + Q2K = GGML_TYPE_Q2_K, + Q3K = GGML_TYPE_Q3_K, + Q4K = GGML_TYPE_Q4_K, + Q5K = GGML_TYPE_Q5_K, + Q6K = GGML_TYPE_Q6_K, + Q8K = GGML_TYPE_Q8_K, + IQ2Xxs = GGML_TYPE_IQ2_XXS, + IQ2Xs = GGML_TYPE_IQ2_XS, + IQ3Xxs = GGML_TYPE_IQ3_XXS, + IQ1S = GGML_TYPE_IQ1_S, + IQ4Nl = GGML_TYPE_IQ4_NL, + IQ3S = GGML_TYPE_IQ3_S, + IQ2S = GGML_TYPE_IQ2_S, + IQ4Xs = GGML_TYPE_IQ4_XS, + I8 = GGML_TYPE_I8, + I16 = GGML_TYPE_I16, + I32 = GGML_TYPE_I32, + I64 = GGML_TYPE_I64, + F64 = GGML_TYPE_F64, + IQ1M = GGML_TYPE_IQ1_M, + BF16 = GGML_TYPE_BF16, + TQ1_0 = GGML_TYPE_TQ1_0, + TQ2_0 = GGML_TYPE_TQ2_0, + MXFP4 = GGML_TYPE_MXFP4, + NVFP4 = GGML_TYPE_NVFP4, +} + +impl TensorType { + pub fn from_ggml_type(id: u32) -> Option { + Some(match id { + GGML_TYPE_F32 => Self::F32, + GGML_TYPE_F16 => Self::F16, + GGML_TYPE_Q4_0 => Self::Q4_0, + GGML_TYPE_Q4_1 => Self::Q4_1, + GGML_TYPE_Q5_0 => Self::Q5_0, + GGML_TYPE_Q5_1 => Self::Q5_1, + GGML_TYPE_Q8_0 => Self::Q8_0, + GGML_TYPE_Q8_1 => Self::Q8_1, + GGML_TYPE_Q2_K => Self::Q2K, + GGML_TYPE_Q3_K => Self::Q3K, + GGML_TYPE_Q4_K => Self::Q4K, + GGML_TYPE_Q5_K => Self::Q5K, + GGML_TYPE_Q6_K => Self::Q6K, + GGML_TYPE_Q8_K => Self::Q8K, + GGML_TYPE_IQ2_XXS => Self::IQ2Xxs, + GGML_TYPE_IQ2_XS => Self::IQ2Xs, + GGML_TYPE_IQ3_XXS => Self::IQ3Xxs, + GGML_TYPE_IQ1_S => Self::IQ1S, + GGML_TYPE_IQ4_NL => Self::IQ4Nl, + GGML_TYPE_IQ3_S => Self::IQ3S, + GGML_TYPE_IQ2_S => Self::IQ2S, + GGML_TYPE_IQ4_XS => Self::IQ4Xs, + GGML_TYPE_I8 => Self::I8, + GGML_TYPE_I16 => Self::I16, + GGML_TYPE_I32 => Self::I32, + GGML_TYPE_I64 => Self::I64, + GGML_TYPE_F64 => Self::F64, + GGML_TYPE_IQ1_M => Self::IQ1M, + GGML_TYPE_BF16 => Self::BF16, + GGML_TYPE_TQ1_0 => Self::TQ1_0, + GGML_TYPE_TQ2_0 => Self::TQ2_0, + GGML_TYPE_MXFP4 => Self::MXFP4, + GGML_TYPE_NVFP4 => Self::NVFP4, + _ => return None, + }) + } + + pub fn ggml_type(self) -> u32 { + self as u32 + } + + pub fn name(self) -> &'static str { + ggml_type_name(self.ggml_type()) + } + + pub fn block_size(self) -> usize { + block_elements(self.ggml_type()) + } + + pub fn scalar_size_bytes(self) -> Option { + match self { + Self::F16 | Self::BF16 | Self::I16 => Some(2), + Self::F32 | Self::I32 => Some(4), + Self::F64 | Self::I64 => Some(8), + Self::I8 => Some(1), + Self::Q4_0 + | Self::Q4_1 + | Self::Q5_0 + | Self::Q5_1 + | Self::Q8_0 + | Self::Q8_1 + | Self::Q2K + | Self::Q3K + | Self::Q4K + | Self::Q5K + | Self::Q6K + | Self::Q8K + | Self::IQ2Xxs + | Self::IQ2Xs + | Self::IQ3Xxs + | Self::IQ1S + | Self::IQ4Nl + | Self::IQ3S + | Self::IQ2S + | Self::IQ4Xs + | Self::IQ1M + | Self::TQ1_0 + | Self::TQ2_0 + | Self::MXFP4 + | Self::NVFP4 => None, + } + } + + pub fn is_quantized(self) -> bool { + self.scalar_size_bytes().is_none() + } +} + +#[derive(Clone, Copy, Debug, Eq, PartialEq)] +pub enum BufferUsage { + Weights, + Activations, + Scratch, + State, + Readback, +} + +#[derive(Clone, Debug, Eq, PartialEq)] +pub struct TensorLayout { + rank: usize, + extents: [i64; 4], + strides_bytes: [usize; 4], +} + +impl TensorLayout { + pub fn row_major(extents: &[i64], elem_size: usize) -> Result { + if extents.is_empty() { + return Err("tensor rank must be at least 1".to_string()); + } + if extents.len() > 4 { + return Err(format!("tensor rank {} exceeds ggml rank 4", extents.len())); + } + + let mut padded_extents = [1_i64; 4]; + let mut padded_strides = [0usize; 4]; + + for (i, &extent) in extents.iter().enumerate() { + if extent < 0 { + return Err(format!("negative extent at dimension {}: {}", i, extent)); + } + padded_extents[i] = extent; + } + + padded_strides[0] = elem_size; + for i in 1..4 { + padded_strides[i] = padded_strides[i - 1] + .checked_mul(usize::try_from(padded_extents[i - 1]).map_err(|_| { + format!( + "extent {} at dimension {} does not fit in usize", + padded_extents[i - 1], + i - 1 + ) + })?) + .ok_or_else(|| "overflow computing row-major strides".to_string())?; + } + + Ok(Self { + rank: extents.len(), + extents: padded_extents, + strides_bytes: padded_strides, + }) + } + + pub fn for_ggml(ty: TensorType, extents: &[i64]) -> Result { + if extents.is_empty() { + return Err("tensor rank must be at least 1".to_string()); + } + if extents.len() > GGML_MAX_DIMS { + return Err(format!("tensor rank {} exceeds ggml rank 4", extents.len())); + } + + let mut padded_extents = [1_i64; GGML_MAX_DIMS]; + for (i, &extent) in extents.iter().enumerate() { + if extent < 0 { + return Err(format!("negative extent at dimension {}: {}", i, extent)); + } + padded_extents[i] = extent; + } + + let mut strides = [0_usize; GGML_MAX_DIMS]; + strides[0] = ggml_type_size_for_type(ty); + if extents.len() > 1 { + strides[1] = ggml_row_size_for_type(ty, padded_extents[0])?; + for i in 2..GGML_MAX_DIMS { + strides[i] = strides[i - 1] + .checked_mul(usize::try_from(padded_extents[i - 1]).map_err(|_| { + format!( + "extent {} at dimension {} does not fit in usize", + padded_extents[i - 1], + i - 1 + ) + })?) + .ok_or_else(|| "overflow computing ggml strides".to_string())?; + } + } + + Ok(Self { + rank: extents.len(), + extents: padded_extents, + strides_bytes: strides, + }) + } + + pub fn from_parts( + rank: usize, + extents: &[i64], + strides_bytes: &[usize], + ) -> Result { + if !(1..=4).contains(&rank) { + return Err(format!("invalid tensor rank {}", rank)); + } + if extents.len() != rank { + return Err(format!("rank {} but got {} extents", rank, extents.len())); + } + if strides_bytes.len() != rank { + return Err(format!( + "rank {} but got {} strides", + rank, + strides_bytes.len() + )); + } + let mut padded_extents = [1_i64; 4]; + let mut padded_strides = [0_usize; 4]; + for i in 0..rank { + padded_extents[i] = extents[i]; + padded_strides[i] = strides_bytes[i]; + } + Ok(Self { + rank, + extents: padded_extents, + strides_bytes: padded_strides, + }) + } + + pub fn rank(&self) -> usize { + self.rank + } + + pub fn extents(&self) -> &[i64] { + &self.extents[..self.rank] + } + + pub fn extents4(&self) -> [i64; 4] { + self.extents + } + + pub fn strides_bytes(&self) -> [usize; 4] { + self.strides_bytes + } + + pub fn num_elements(&self) -> i64 { + self.extents().iter().copied().product() + } +} + +#[derive(Clone, Debug, Eq, PartialEq)] +pub struct TensorDesc { + pub name: Option, + pub ty: TensorType, + pub layout: TensorLayout, + pub usage: BufferUsage, +} + +impl TensorDesc { + pub fn new(ty: TensorType, layout: TensorLayout, usage: BufferUsage) -> Self { + Self { + name: None, + ty, + layout, + usage, + } + } + + pub fn with_name(mut self, name: impl Into) -> Self { + let mut name = name.into(); + name.truncate(GGML_MAX_NAME.saturating_sub(1)); + self.name = Some(name); + self + } +} + +#[derive(Clone, Copy, Debug, Default, Eq, PartialEq, Hash)] +pub struct TensorFlags(pub u32); + +impl TensorFlags { + pub const EMPTY: Self = Self(0); + pub const INPUT: Self = Self(TensorFlag::Input as u32); + pub const OUTPUT: Self = Self(TensorFlag::Output as u32); + pub const PARAM: Self = Self(TensorFlag::Param as u32); + pub const LOSS: Self = Self(TensorFlag::Loss as u32); + pub const COMPUTE: Self = Self(TensorFlag::Compute as u32); + + pub fn contains(self, other: Self) -> bool { + (self.0 & other.0) == other.0 + } + + pub fn insert(&mut self, other: Self) { + self.0 |= other.0; + } +} + +#[derive(Clone, Debug)] +pub struct Tensor { + pub id: TensorId, + pub desc: TensorDesc, + pub ne: [i64; GGML_MAX_DIMS], + pub nb: [usize; GGML_MAX_DIMS], + pub op: Op, + pub op_params: [i32; GGML_MAX_OP_PARAMS / std::mem::size_of::()], + pub flags: TensorFlags, + pub src: [Option; GGML_MAX_SRC], + pub view_src: Option, + pub view_offs: usize, + pub buffer_id: Option, + pub data_offset: Option, + pub extra: Option, +} + +impl Tensor { + pub fn from_desc(id: TensorId, desc: TensorDesc) -> Self { + Self { + id, + ne: desc.layout.extents4(), + nb: desc.layout.strides_bytes(), + desc, + op: Op::None, + op_params: [0; GGML_MAX_OP_PARAMS / std::mem::size_of::()], + flags: TensorFlags::EMPTY, + src: [None; GGML_MAX_SRC], + view_src: None, + view_offs: 0, + buffer_id: None, + data_offset: None, + extra: None, + } + } + + pub fn name(&self) -> Option<&str> { + self.desc.name.as_deref() + } + + pub fn set_name(&mut self, name: impl Into) { + let mut name = name.into(); + name.truncate(GGML_MAX_NAME.saturating_sub(1)); + self.desc.name = Some(name); + } + + pub fn set_input(&mut self) { + self.flags.insert(TensorFlags::INPUT); + } + + pub fn set_output(&mut self) { + self.flags.insert(TensorFlags::OUTPUT); + } + + pub fn set_param(&mut self) { + self.flags.insert(TensorFlags::PARAM); + } + + pub fn set_loss(&mut self) { + self.flags.insert(TensorFlags::LOSS); + } + + pub fn n_dims(&self) -> usize { + for i in (1..GGML_MAX_DIMS).rev() { + if self.ne[i] > 1 { + return i + 1; + } + } + 1 + } + + pub fn nelements(&self) -> i64 { + self.ne.iter().copied().product() + } + + pub fn nrows(&self) -> i64 { + self.ne[1] * self.ne[2] * self.ne[3] + } + + pub fn nbytes(&self) -> usize { + if self.ne.iter().any(|&ne| ne <= 0) { + return 0; + } + + let blck_size = ggml_blck_size_for_type(self.desc.ty); + let mut nbytes = if blck_size == 1 { + ggml_type_size_for_type(self.desc.ty) + } else { + usize::try_from(self.ne[0]).unwrap_or(0) * self.nb[0] / blck_size + }; + + let start = if blck_size == 1 { 0 } else { 1 }; + for i in start..GGML_MAX_DIMS { + nbytes += usize::try_from(self.ne[i] - 1).unwrap_or(0) * self.nb[i]; + } + + nbytes + } + + pub fn is_quantized(&self) -> bool { + self.desc.ty.is_quantized() + } + + pub fn is_scalar(&self) -> bool { + self.ne == [1, 1, 1, 1] + } + + pub fn is_vector(&self) -> bool { + self.ne[1] == 1 && self.ne[2] == 1 && self.ne[3] == 1 + } + + pub fn is_matrix(&self) -> bool { + self.ne[2] == 1 && self.ne[3] == 1 + } + + pub fn is_3d(&self) -> bool { + self.ne[3] == 1 + } + + pub fn is_transposed(&self) -> bool { + self.nb[0] > self.nb[1] + } + + pub fn is_permuted(&self) -> bool { + self.nb[0] > self.nb[1] || self.nb[1] > self.nb[2] || self.nb[2] > self.nb[3] + } + + pub fn is_contiguous(&self) -> bool { + self.is_contiguous_n(0) + } + + pub fn is_contiguous_1(&self) -> bool { + self.is_contiguous_n(1) + } + + pub fn is_contiguous_2(&self) -> bool { + self.is_contiguous_n(2) + } + + pub fn is_contiguously_allocated(&self) -> bool { + self.nbytes() + == (self.nelements() as usize) * ggml_type_size_for_type(self.desc.ty) + / ggml_blck_size_for_type(self.desc.ty) + } + + pub fn is_contiguous_channels(&self) -> bool { + self.nb[0] > self.nb[2] + && self.nb[1] > self.nb[0] + && self.nb[2] == ggml_type_size_for_type(self.desc.ty) + } + + pub fn is_contiguous_rows(&self) -> bool { + self.ne[0] == ggml_blck_size_for_type(self.desc.ty) as i64 + || self.nb[0] == ggml_type_size_for_type(self.desc.ty) + } + + pub fn is_empty(&self) -> bool { + self.ne.iter().any(|&ne| ne == 0) + } + + pub fn is_view(&self) -> bool { + self.view_src.is_some() + } + + pub fn are_same_shape(&self, other: &Self) -> bool { + self.ne == other.ne + } + + pub fn are_same_stride(&self, other: &Self) -> bool { + self.nb == other.nb + } + + pub fn can_repeat(&self, other: &Self) -> bool { + self.is_empty() + .then_some(other.is_empty()) + .unwrap_or_else(|| { + other.ne[0] % self.ne[0] == 0 + && other.ne[1] % self.ne[1] == 0 + && other.ne[2] % self.ne[2] == 0 + && other.ne[3] % self.ne[3] == 0 + }) + } + + pub fn op_desc(&self) -> &'static str { + match self.op { + Op::Unary => self + .unary_op() + .map(UnaryOp::name) + .unwrap_or_else(|| self.op.name()), + Op::Glu => self + .glu_op() + .map(GluOp::name) + .unwrap_or_else(|| self.op.name()), + op => op.name(), + } + } + + pub fn set_op_param_i32(&mut self, index: usize, value: i32) { + self.op_params[index] = value; + } + + pub fn op_param_i32(&self, index: usize) -> i32 { + self.op_params[index] + } + + pub fn set_op_param_f32(&mut self, index: usize, value: f32) { + self.op_params[index] = value.to_bits() as i32; + } + + pub fn op_param_f32(&self, index: usize) -> f32 { + f32::from_bits(self.op_params[index] as u32) + } + + pub fn set_unary_op(&mut self, op: UnaryOp) { + self.set_op_param_i32(0, op as i32); + } + + pub fn unary_op(&self) -> Option { + UnaryOp::from_u32(self.op_param_i32(0) as u32) + } + + pub fn set_glu_op(&mut self, op: GluOp) { + self.set_op_param_i32(0, op as i32); + } + + pub fn glu_op(&self) -> Option { + GluOp::from_u32(self.op_param_i32(0) as u32) + } + + fn is_contiguous_n(&self, n: usize) -> bool { + let mut next_nb = ggml_type_size_for_type(self.desc.ty); + if self.ne[0] != ggml_blck_size_for_type(self.desc.ty) as i64 && self.nb[0] != next_nb { + return false; + } + next_nb *= usize::try_from(self.ne[0]).unwrap_or(0) / ggml_blck_size_for_type(self.desc.ty); + for i in 1..GGML_MAX_DIMS { + if i > n { + if self.ne[i] != 1 && self.nb[i] != next_nb { + return false; + } + next_nb *= usize::try_from(self.ne[i]).unwrap_or(0); + } else { + next_nb = usize::try_from(self.ne[i]).unwrap_or(0) * self.nb[i]; + } + } + true + } +} + +pub fn ggml_blck_size_for_type(ty: TensorType) -> usize { + block_elements(ty.ggml_type()) +} + +pub fn ggml_type_size_for_type(ty: TensorType) -> usize { + block_size(ty.ggml_type()) +} + +pub fn ggml_row_size_for_type(ty: TensorType, ne: i64) -> Result { + if ne < 0 { + return Err(format!("negative row extent {}", ne)); + } + let block_elements = ggml_blck_size_for_type(ty) as i64; + if ne % block_elements != 0 { + return Err(format!( + "row extent {} is not divisible by block size {} for type {}", + ne, + block_elements, + ty.name() + )); + } + Ok(ggml_type_size_for_type(ty) * usize::try_from(ne / block_elements).unwrap_or(0)) +} + +pub fn ggml_ftype_to_tensor_type(ftype: Ftype) -> Option { + Some(match ftype { + Ftype::AllF32 => TensorType::F32, + Ftype::MostlyF16 => TensorType::F16, + Ftype::MostlyBf16 => TensorType::BF16, + Ftype::MostlyQ4_0 => TensorType::Q4_0, + Ftype::MostlyQ4_1 => TensorType::Q4_1, + Ftype::MostlyQ5_0 => TensorType::Q5_0, + Ftype::MostlyQ5_1 => TensorType::Q5_1, + Ftype::MostlyQ8_0 => TensorType::Q8_0, + Ftype::MostlyMxfp4 => TensorType::MXFP4, + Ftype::MostlyNvfp4 => TensorType::NVFP4, + Ftype::MostlyQ2K => TensorType::Q2K, + Ftype::MostlyQ3K => TensorType::Q3K, + Ftype::MostlyQ4K => TensorType::Q4K, + Ftype::MostlyQ5K => TensorType::Q5K, + Ftype::MostlyQ6K => TensorType::Q6K, + Ftype::MostlyIq2Xxs => TensorType::IQ2Xxs, + Ftype::MostlyIq2Xs => TensorType::IQ2Xs, + Ftype::MostlyIq3Xxs => TensorType::IQ3Xxs, + Ftype::MostlyIq1S => TensorType::IQ1S, + Ftype::MostlyIq1M => TensorType::IQ1M, + Ftype::MostlyIq4Nl => TensorType::IQ4Nl, + Ftype::MostlyIq4Xs => TensorType::IQ4Xs, + Ftype::MostlyIq3S => TensorType::IQ3S, + Ftype::MostlyIq2S => TensorType::IQ2S, + Ftype::Unknown | Ftype::MostlyQ4_1SomeF16 => return None, + }) +} diff --git a/libs/llama/Cargo.toml b/libs/llama/Cargo.toml new file mode 100644 index 0000000000..39df953c73 --- /dev/null +++ b/libs/llama/Cargo.toml @@ -0,0 +1,23 @@ +[package] +name = "makepad-llama" +version = "0.1.0" +edition = "2021" +description = "Rust llama.cpp port scaffold built on makepad-ggml" +license = "MIT" + +[dependencies] +makepad-ggml = { path = "../ggml" } + +[workspace] + +[[bin]] +name = "llama-load" +path = "src/bin/llama_load.rs" + +[[bin]] +name = "llama-compare" +path = "src/bin/llama_compare.rs" + +[[bin]] +name = "llama-generate" +path = "src/bin/llama_generate.rs" diff --git a/libs/llama/src/bin/llama_compare.rs b/libs/llama/src/bin/llama_compare.rs new file mode 100644 index 0000000000..c939419a83 --- /dev/null +++ b/libs/llama/src/bin/llama_compare.rs @@ -0,0 +1,2493 @@ +use std::ffi::OsString; +use std::fs; +use std::path::{Path, PathBuf}; +use std::process::{Command, Output}; +use std::time::{SystemTime, UNIX_EPOCH}; + +use makepad_ggml::backend::metal::{ + prepare_graph, BufferStorageMode, MetalGraphSession, MetalGraphTensorWrite, MetalRuntime, +}; +use makepad_ggml::{ + f32_to_f16, Context, TensorId, TensorType, GGML_ROPE_TYPE_IMROPE, GGML_ROPE_TYPE_MROPE, +}; +use makepad_llama::{ + build_delta_net_recurrent_decode_graph, compile_attention_block_metal, + compile_attention_decode_metal, compile_delta_net_recurrent_decode_metal, + compile_hybrid_decode_metal, execute_attention_block_graph_metal_cached, + execute_attention_decode_graph_metal_cached, + execute_delta_net_recurrent_decode_graph_metal_cached, + execute_hybrid_decode_graph_metal_cached, prepare_attention_block_graph, + prepare_attention_decode_graph, qwen35moe_attention_block_layout, + qwen35moe_attention_decode_spec, qwen35moe_delta_net_recurrent_decode_spec, + qwen35moe_first_attention_block_spec, qwen35moe_first_recurrent_block_spec, + qwen35moe_hybrid_decode_spec, qwen35moe_recurrent_block_layout, AttentionRopeSpec, + HybridCacheLayout, HybridCacheShape, HybridCacheTypes, LlamaModel, LlamaVocab, + LogitsProbeInput, +}; + +const DEFAULT_PROMPT: &str = "The capital of France is"; +const DEFAULT_TOP_K: usize = 10; +const DEFAULT_UPSTREAM_DEBUG: &str = + "local/llama.cpp/build-arm64-apple-clang-release/bin/llama-debug"; + +struct Args { + model_path: PathBuf, + prompt: String, + upstream_debug_path: PathBuf, + top_k: usize, +} + +struct UpstreamReference { + token_ids: Vec, + logits: Vec, + output_dir: PathBuf, +} + +fn main() { + match run() { + Ok(()) => {} + Err(err) => { + eprintln!("llama-compare failed: {err}"); + std::process::exit(1); + } + } +} + +fn run() -> Result<(), Box> { + let args = parse_args(std::env::args_os())?; + let model = LlamaModel::load(&args.model_path)?; + model.validate_layout()?; + let vocab = LlamaVocab::from_model(&model).ok(); + let upstream = run_upstream_debug(&args)?; + + let input_token_labels = format_token_list(&upstream.token_ids, vocab.as_ref()); + println!("model: {}", args.model_path.display()); + println!("prompt: {}", args.prompt); + println!("input.token_count: {}", upstream.token_ids.len()); + println!("input.tokens: {:?}", upstream.token_ids); + println!("input.token_pieces: {:?}", input_token_labels); + println!("upstream.output_dir: {}", upstream.output_dir.display()); + + let rust_logits = run_rust_hybrid_decode(&model, &upstream.token_ids)?; + let rust_batched_logits = match run_rust_hybrid_decode_batched(&model, &upstream.token_ids) { + Ok(logits) => Some(logits), + Err(err) => { + println!("compare.batched.error: {}", err); + None + } + }; + if rust_logits.len() != upstream.logits.len() { + return Err(format!( + "logit length mismatch: rust={} upstream={}", + rust_logits.len(), + upstream.logits.len() + ) + .into()); + } + let rust_top = top_k_logits(&rust_logits, args.top_k); + let upstream_top = top_k_logits(&upstream.logits, args.top_k); + let stats = compare_logits(&rust_logits, &upstream.logits); + let rust_top_ids = rust_top.iter().map(|(id, _)| *id).collect::>(); + let upstream_top_ids = upstream_top.iter().map(|(id, _)| *id).collect::>(); + let top_overlap = rust_top_ids + .iter() + .filter(|id| upstream_top_ids.contains(id)) + .count(); + + println!( + "compare.same_top1: {}", + rust_top.first().map(|(id, _)| *id) == upstream_top.first().map(|(id, _)| *id) + ); + println!( + "compare.same_top{}_ids: {}", + args.top_k, + rust_top_ids == upstream_top_ids + ); + println!( + "compare.top{}_overlap: {}/{}", + args.top_k, top_overlap, args.top_k + ); + println!("compare.max_abs_diff: {:.9}", stats.max_abs_diff); + println!("compare.mean_abs_diff: {:.9}", stats.mean_abs_diff); + println!("compare.rms_diff: {:.9}", stats.rms_diff); + println!("compare.cosine_similarity: {:.9}", stats.cosine_similarity); + if let Some(rust_batched_logits) = &rust_batched_logits { + if rust_batched_logits.len() != upstream.logits.len() { + return Err(format!( + "batched logit length mismatch: rust={} upstream={}", + rust_batched_logits.len(), + upstream.logits.len() + ) + .into()); + } + let rust_batched_top = top_k_logits(rust_batched_logits, args.top_k); + let batched_stats = compare_logits(rust_batched_logits, &upstream.logits); + let rust_batched_top_ids = rust_batched_top + .iter() + .map(|(id, _)| *id) + .collect::>(); + let batched_top_overlap = rust_batched_top_ids + .iter() + .filter(|id| upstream_top_ids.contains(id)) + .count(); + println!( + "compare.batched.same_top1: {}", + rust_batched_top.first().map(|(id, _)| *id) == upstream_top.first().map(|(id, _)| *id) + ); + println!( + "compare.batched.same_top{}_ids: {}", + args.top_k, + rust_batched_top_ids == upstream_top_ids + ); + println!( + "compare.batched.top{}_overlap: {}/{}", + args.top_k, batched_top_overlap, args.top_k + ); + println!( + "compare.batched.max_abs_diff: {:.9}", + batched_stats.max_abs_diff + ); + println!( + "compare.batched.mean_abs_diff: {:.9}", + batched_stats.mean_abs_diff + ); + println!("compare.batched.rms_diff: {:.9}", batched_stats.rms_diff); + println!( + "compare.batched.cosine_similarity: {:.9}", + batched_stats.cosine_similarity + ); + println!( + "rust_batched.next.top{}: {:?}", + args.top_k, + describe_top_k(&rust_batched_top, vocab.as_ref()) + ); + } + + println!( + "upstream.next.top{}: {:?}", + args.top_k, + describe_top_k(&upstream_top, vocab.as_ref()) + ); + println!( + "rust.next.top{}: {:?}", + args.top_k, + describe_top_k(&rust_top, vocab.as_ref()) + ); + if upstream.token_ids.len() > 1 { + let attention_check_f16 = + attention_cache_self_check(&model, &upstream.token_ids[..2], TensorType::F16)?; + let attention_check_f32 = + attention_cache_self_check(&model, &upstream.token_ids[..2], TensorType::F32)?; + let attention_decode_batch_check = + attention_decode_batch_self_check(&model, &upstream.token_ids[..2])?; + let recurrent_check = recurrent_cache_self_check(&model, &upstream.token_ids[..2])?; + let recurrent_step_cpu_check = recurrent_step_cpu_check(&model, &upstream.token_ids[..2])?; + println!( + "attention_cache.f16.layer{}._same_top1: {}", + attention_check_f16.layer_index, attention_check_f16.same_top1 + ); + println!( + "attention_cache.f16.layer{}._hidden_max_abs_diff: {:.9}", + attention_check_f16.layer_index, attention_check_f16.hidden_stats.max_abs_diff + ); + println!( + "attention_cache.f16.layer{}._hidden_mean_abs_diff: {:.9}", + attention_check_f16.layer_index, attention_check_f16.hidden_stats.mean_abs_diff + ); + println!( + "attention_cache.f16.layer{}._hidden_rms_diff: {:.9}", + attention_check_f16.layer_index, attention_check_f16.hidden_stats.rms_diff + ); + println!( + "attention_cache.f16.layer{}._hidden_cosine_similarity: {:.9}", + attention_check_f16.layer_index, attention_check_f16.hidden_stats.cosine_similarity + ); + println!( + "attention_cache.f32.layer{}._same_top1: {}", + attention_check_f32.layer_index, attention_check_f32.same_top1 + ); + println!( + "attention_cache.f32.layer{}._hidden_max_abs_diff: {:.9}", + attention_check_f32.layer_index, attention_check_f32.hidden_stats.max_abs_diff + ); + println!( + "attention_cache.f32.layer{}._hidden_mean_abs_diff: {:.9}", + attention_check_f32.layer_index, attention_check_f32.hidden_stats.mean_abs_diff + ); + println!( + "attention_cache.f32.layer{}._hidden_rms_diff: {:.9}", + attention_check_f32.layer_index, attention_check_f32.hidden_stats.rms_diff + ); + println!( + "attention_cache.f32.layer{}._hidden_cosine_similarity: {:.9}", + attention_check_f32.layer_index, attention_check_f32.hidden_stats.cosine_similarity + ); + println!( + "attention_decode_batch.layer{}._hidden_max_abs_diff: {:.9}", + attention_decode_batch_check.layer_index, + attention_decode_batch_check.hidden_stats.max_abs_diff + ); + println!( + "attention_decode_batch.layer{}._hidden_mean_abs_diff: {:.9}", + attention_decode_batch_check.layer_index, + attention_decode_batch_check.hidden_stats.mean_abs_diff + ); + println!( + "attention_decode_batch.layer{}._hidden_rms_diff: {:.9}", + attention_decode_batch_check.layer_index, + attention_decode_batch_check.hidden_stats.rms_diff + ); + println!( + "attention_decode_batch.layer{}._hidden_cosine_similarity: {:.9}", + attention_decode_batch_check.layer_index, + attention_decode_batch_check.hidden_stats.cosine_similarity + ); + println!( + "attention_decode_batch.layer{}._k_cache_max_abs_diff: {:.9}", + attention_decode_batch_check.layer_index, + attention_decode_batch_check.k_cache_stats.max_abs_diff + ); + println!( + "attention_decode_batch.layer{}._v_cache_max_abs_diff: {:.9}", + attention_decode_batch_check.layer_index, + attention_decode_batch_check.v_cache_stats.max_abs_diff + ); + println!( + "recurrent_cache.layer{}._same_top1: {}", + recurrent_check.layer_index, recurrent_check.same_top1 + ); + println!( + "recurrent_cache.layer{}._hidden_max_abs_diff: {:.9}", + recurrent_check.layer_index, recurrent_check.hidden_stats.max_abs_diff + ); + println!( + "recurrent_cache.layer{}._hidden_mean_abs_diff: {:.9}", + recurrent_check.layer_index, recurrent_check.hidden_stats.mean_abs_diff + ); + println!( + "recurrent_cache.layer{}._hidden_rms_diff: {:.9}", + recurrent_check.layer_index, recurrent_check.hidden_stats.rms_diff + ); + println!( + "recurrent_cache.layer{}._hidden_cosine_similarity: {:.9}", + recurrent_check.layer_index, recurrent_check.hidden_stats.cosine_similarity + ); + println!( + "recurrent_cache.layer{}._r_cache_max_abs_diff: {:.9}", + recurrent_check.layer_index, recurrent_check.r_cache_stats.max_abs_diff + ); + println!( + "recurrent_cache.layer{}._s_cache_max_abs_diff: {:.9}", + recurrent_check.layer_index, recurrent_check.s_cache_stats.max_abs_diff + ); + println!( + "recurrent_step_cpu.layer{}._conv_output_max_abs_diff: {:.9}", + recurrent_step_cpu_check.layer_index, + recurrent_step_cpu_check.conv_output_cpu_stats.max_abs_diff + ); + println!( + "recurrent_step_cpu.layer{}._q_conv_max_abs_diff: {:.9}", + recurrent_step_cpu_check.layer_index, + recurrent_step_cpu_check.q_conv_cpu_stats.max_abs_diff + ); + println!( + "recurrent_step_cpu.layer{}._k_conv_max_abs_diff: {:.9}", + recurrent_step_cpu_check.layer_index, + recurrent_step_cpu_check.k_conv_cpu_stats.max_abs_diff + ); + println!( + "recurrent_step_cpu.layer{}._output_view_max_abs_diff: {:.9}", + recurrent_step_cpu_check.layer_index, + recurrent_step_cpu_check.output_view_cpu_stats.max_abs_diff + ); + let recurrent_tensor_check = recurrent_tensor_check(&model, &upstream.token_ids[..2])?; + println!( + "recurrent_tensor.layer{}._z_max_abs_diff: {:.9}", + recurrent_tensor_check.layer_index, recurrent_tensor_check.z_stats.max_abs_diff + ); + println!( + "recurrent_tensor.layer{}._beta_max_abs_diff: {:.9}", + recurrent_tensor_check.layer_index, recurrent_tensor_check.beta_stats.max_abs_diff + ); + println!( + "recurrent_tensor.layer{}._gate_max_abs_diff: {:.9}", + recurrent_tensor_check.layer_index, recurrent_tensor_check.gate_stats.max_abs_diff + ); + println!( + "recurrent_tensor.layer{}._conv_output_max_abs_diff: {:.9}", + recurrent_tensor_check.layer_index, + recurrent_tensor_check.conv_output_stats.max_abs_diff + ); + println!( + "recurrent_tensor.layer{}._output_view_max_abs_diff: {:.9}", + recurrent_tensor_check.layer_index, + recurrent_tensor_check.output_view_stats.max_abs_diff + ); + println!( + "recurrent_tensor.layer{}._output_norm_max_abs_diff: {:.9}", + recurrent_tensor_check.layer_index, + recurrent_tensor_check.output_norm_stats.max_abs_diff + ); + println!( + "recurrent_tensor.layer{}._z_silu_max_abs_diff: {:.9}", + recurrent_tensor_check.layer_index, recurrent_tensor_check.z_silu_stats.max_abs_diff + ); + println!( + "recurrent_tensor.layer{}._gated_output_max_abs_diff: {:.9}", + recurrent_tensor_check.layer_index, + recurrent_tensor_check.gated_output_stats.max_abs_diff + ); + println!( + "recurrent_tensor.layer{}._final_output_max_abs_diff: {:.9}", + recurrent_tensor_check.layer_index, + recurrent_tensor_check.final_output_stats.max_abs_diff + ); + let attention_tensor_check = + attention_cache_tensor_check(&model, &upstream.token_ids[..2])?; + println!( + "attention_tensor.layer{}._q_proj_max_abs_diff: {:.9}", + attention_tensor_check.layer_index, attention_tensor_check.q_proj_stats.max_abs_diff + ); + println!( + "attention_tensor.layer{}._q_pre_max_abs_diff: {:.9}", + attention_tensor_check.layer_index, attention_tensor_check.q_pre_stats.max_abs_diff + ); + println!( + "attention_tensor.layer{}._q_norm_max_abs_diff: {:.9}", + attention_tensor_check.layer_index, attention_tensor_check.q_norm_stats.max_abs_diff + ); + println!( + "attention_tensor.layer{}._k_norm_max_abs_diff: {:.9}", + attention_tensor_check.layer_index, attention_tensor_check.k_norm_stats.max_abs_diff + ); + println!( + "attention_tensor.layer{}._q_max_abs_diff: {:.9}", + attention_tensor_check.layer_index, attention_tensor_check.q_stats.max_abs_diff + ); + println!( + "attention_tensor.layer{}._k_store_max_abs_diff: {:.9}", + attention_tensor_check.layer_index, attention_tensor_check.k_store_stats.max_abs_diff + ); + println!( + "attention_tensor.layer{}._v_store_max_abs_diff: {:.9}", + attention_tensor_check.layer_index, attention_tensor_check.v_store_stats.max_abs_diff + ); + println!( + "attention_tensor.layer{}._k_cache_max_abs_diff: {:.9}", + attention_tensor_check.layer_index, attention_tensor_check.k_cache_stats.max_abs_diff + ); + println!( + "attention_tensor.layer{}._v_cache_max_abs_diff: {:.9}", + attention_tensor_check.layer_index, attention_tensor_check.v_cache_stats.max_abs_diff + ); + println!( + "attention_tensor.layer{}._attn_max_abs_diff: {:.9}", + attention_tensor_check.layer_index, attention_tensor_check.attn_stats.max_abs_diff + ); + println!( + "attention_tensor.layer{}._full_attn_cpu_max_abs_diff: {:.9}", + attention_tensor_check.layer_index, + attention_tensor_check.full_attn_cpu_stats.max_abs_diff + ); + println!( + "attention_tensor.layer{}._decode_attn_cpu_max_abs_diff: {:.9}", + attention_tensor_check.layer_index, + attention_tensor_check.decode_attn_cpu_stats.max_abs_diff + ); + } + + if let (Some((rust_top1, rust_logit)), Some((upstream_top1, upstream_logit))) = + (rust_top.first(), upstream_top.first()) + { + println!("compare.top1.rust_id: {}", rust_top1); + println!("compare.top1.upstream_id: {}", upstream_top1); + println!("compare.top1.rust_logit: {:.9}", rust_logit); + println!("compare.top1.upstream_logit: {:.9}", upstream_logit); + if let Some(vocab) = &vocab { + println!( + "compare.top1.rust_piece: {}", + vocab + .escaped_piece(*rust_top1) + .unwrap_or_else(|| "".to_owned()) + ); + println!( + "compare.top1.upstream_piece: {}", + vocab + .escaped_piece(*upstream_top1) + .unwrap_or_else(|| "".to_owned()) + ); + } + } + + Ok(()) +} + +fn parse_args( + args: impl IntoIterator, +) -> Result> { + let mut args = args.into_iter(); + let _exe = args.next(); + + let model_path = args + .next() + .ok_or("usage: llama-compare [prompt words ...]")?; + let prompt_parts = args.collect::>(); + let prompt = if prompt_parts.is_empty() { + DEFAULT_PROMPT.to_owned() + } else { + prompt_parts + .iter() + .map(|part| part.to_string_lossy()) + .collect::>() + .join(" ") + }; + + Ok(Args { + model_path: PathBuf::from(model_path), + prompt, + upstream_debug_path: PathBuf::from(DEFAULT_UPSTREAM_DEBUG), + top_k: DEFAULT_TOP_K, + }) +} + +fn run_upstream_debug(args: &Args) -> Result> { + let output_dir = std::env::temp_dir().join(format!( + "makepad-llama-compare-{}-{}", + std::process::id(), + SystemTime::now().duration_since(UNIX_EPOCH)?.as_millis() + )); + fs::create_dir_all(&output_dir)?; + + let output = Command::new(&args.upstream_debug_path) + .arg("-m") + .arg(&args.model_path) + .arg("-p") + .arg(&args.prompt) + .arg("--save-logits") + .arg("--logits-output-dir") + .arg(&output_dir) + .arg("-ngl") + .arg("999") + .arg("-fa") + .arg("1") + .arg("-ctk") + .arg("f16") + .arg("-ctv") + .arg("f16") + .output()?; + ensure_success("llama-debug", &output)?; + + let model_name = args + .model_path + .file_stem() + .and_then(|stem| stem.to_str()) + .ok_or("failed to derive model stem for upstream output files")?; + let base = output_dir.join(format!("llamacpp-{model_name}")); + let logits = read_f32_file(&base.with_extension("bin"))?; + let token_ids = + read_i32_file(&base.with_file_name(format!("llamacpp-{model_name}-tokens.bin")))?; + + Ok(UpstreamReference { + token_ids, + logits, + output_dir, + }) +} + +fn run_rust_hybrid_decode( + model: &LlamaModel, + token_ids: &[i32], +) -> Result, Box> { + if token_ids.is_empty() { + return Err("upstream prompt produced no tokens".into()); + } + + let plan = model.execution_plan()?; + let cache_shape = HybridCacheShape { + n_ctx_seq: u32::try_from(token_ids.len())?, + n_seq_max: 1, + }; + let cache_types = HybridCacheTypes { + attention_k_type: TensorType::F16, + attention_v_type: TensorType::F16, + recurrent_r_type: TensorType::F32, + recurrent_s_type: TensorType::F32, + }; + let extra_bytes = plan + .hybrid_cache + .as_ref() + .map(|template| HybridCacheLayout::new(template.materialize(cache_shape, cache_types))) + .transpose()? + .map_or(0, |layout| layout.total_bytes); + let extra_bytes = extra_bytes.saturating_add(256 << 20); + let mut loaded = plan + .full_weights + .allocate_and_load_with_extra(&model.gguf, extra_bytes)?; + let spec = qwen35moe_hybrid_decode_spec( + model, + cache_shape.n_ctx_seq, + cache_shape.n_seq_max, + cache_types.attention_k_type, + cache_types.attention_v_type, + cache_types.recurrent_r_type, + cache_types.recurrent_s_type, + )?; + let compiled = compile_hybrid_decode_metal(&mut loaded, &spec, 1)?; + + let mut final_logits = None; + for (position, token_id) in token_ids.iter().copied().enumerate() { + let position_i32 = i32::try_from(position)?; + let run = execute_hybrid_decode_graph_metal_cached( + &compiled, + &mut loaded, + LogitsProbeInput::TokenIds(std::slice::from_ref(&token_id)), + &[position_i32], + position + 1, + )?; + final_logits = Some(run.logits); + } + + final_logits.ok_or_else(|| "hybrid decode did not produce logits".into()) +} + +fn run_rust_hybrid_decode_batched( + model: &LlamaModel, + token_ids: &[i32], +) -> Result, Box> { + if token_ids.is_empty() { + return Err("upstream prompt produced no tokens".into()); + } + + let plan = model.execution_plan()?; + let cache_shape = HybridCacheShape { + n_ctx_seq: u32::try_from(token_ids.len())?, + n_seq_max: 1, + }; + let cache_types = HybridCacheTypes { + attention_k_type: TensorType::F16, + attention_v_type: TensorType::F16, + recurrent_r_type: TensorType::F32, + recurrent_s_type: TensorType::F32, + }; + let extra_bytes = plan + .hybrid_cache + .as_ref() + .map(|template| HybridCacheLayout::new(template.materialize(cache_shape, cache_types))) + .transpose()? + .map_or(0, |layout| layout.total_bytes); + let extra_bytes = extra_bytes.saturating_add(256 << 20); + let mut loaded = plan + .full_weights + .allocate_and_load_with_extra(&model.gguf, extra_bytes)?; + let spec = qwen35moe_hybrid_decode_spec( + model, + cache_shape.n_ctx_seq, + cache_shape.n_seq_max, + cache_types.attention_k_type, + cache_types.attention_v_type, + cache_types.recurrent_r_type, + cache_types.recurrent_s_type, + )?; + let compiled = compile_hybrid_decode_metal(&mut loaded, &spec, token_ids.len())?; + let positions = (0..token_ids.len()) + .map(i32::try_from) + .collect::, _>>()?; + let run = execute_hybrid_decode_graph_metal_cached( + &compiled, + &mut loaded, + LogitsProbeInput::TokenIds(token_ids), + &positions, + token_ids.len(), + )?; + if run.vocab_size == 0 || run.logits.len() % run.vocab_size != 0 { + return Err(format!( + "batched hybrid decode produced malformed logits: len={} vocab_size={}", + run.logits.len(), + run.vocab_size + ) + .into()); + } + let last = run + .logits + .len() + .checked_sub(run.vocab_size) + .ok_or("batched hybrid decode logits were unexpectedly empty")?; + Ok(run.logits[last..].to_vec()) +} + +fn ensure_success(name: &str, output: &Output) -> Result<(), Box> { + if output.status.success() { + return Ok(()); + } + let stdout = String::from_utf8_lossy(&output.stdout); + let stderr = String::from_utf8_lossy(&output.stderr); + Err(format!( + "{name} exited with {}.\nstdout:\n{}\nstderr:\n{}", + output.status, stdout, stderr + ) + .into()) +} + +fn read_f32_file(path: &Path) -> Result, Box> { + let bytes = fs::read(path)?; + if bytes.len() % std::mem::size_of::() != 0 { + return Err(format!("file '{}' is not a multiple of f32 size", path.display()).into()); + } + Ok(bytes + .chunks_exact(std::mem::size_of::()) + .map(|chunk| f32::from_le_bytes(chunk.try_into().unwrap())) + .collect()) +} + +fn read_i32_file(path: &Path) -> Result, Box> { + let bytes = fs::read(path)?; + if bytes.len() % std::mem::size_of::() != 0 { + return Err(format!("file '{}' is not a multiple of i32 size", path.display()).into()); + } + Ok(bytes + .chunks_exact(std::mem::size_of::()) + .map(|chunk| i32::from_le_bytes(chunk.try_into().unwrap())) + .collect()) +} + +fn top_k_logits(logits: &[f32], k: usize) -> Vec<(i32, f32)> { + let mut values = logits + .iter() + .copied() + .enumerate() + .map(|(index, value)| (i32::try_from(index).unwrap(), value)) + .collect::>(); + values.sort_by(|a, b| b.1.total_cmp(&a.1).then_with(|| a.0.cmp(&b.0))); + values.truncate(k.min(values.len())); + values +} + +fn format_token_list(token_ids: &[i32], vocab: Option<&LlamaVocab>) -> Vec { + token_ids + .iter() + .map(|&token_id| match vocab { + Some(vocab) => format!( + "{}:{}", + token_id, + vocab + .escaped_piece(token_id) + .unwrap_or_else(|| "".to_owned()) + ), + None => token_id.to_string(), + }) + .collect() +} + +fn describe_top_k(top_k: &[(i32, f32)], vocab: Option<&LlamaVocab>) -> Vec { + top_k + .iter() + .map(|(token_id, logit)| match vocab { + Some(vocab) => format!( + "{}:{:.6}:{}", + token_id, + logit, + vocab + .escaped_piece(*token_id) + .unwrap_or_else(|| "".to_owned()) + ), + None => format!("{}:{:.6}", token_id, logit), + }) + .collect() +} + +struct LogitComparison { + max_abs_diff: f64, + mean_abs_diff: f64, + rms_diff: f64, + cosine_similarity: f64, +} + +struct AttentionCacheSelfCheck { + layer_index: u32, + same_top1: bool, + hidden_stats: LogitComparison, +} + +struct AttentionDecodeBatchSelfCheck { + layer_index: u32, + hidden_stats: LogitComparison, + k_cache_stats: LogitComparison, + v_cache_stats: LogitComparison, +} + +struct AttentionCacheTensorCheck { + layer_index: u32, + q_proj_stats: LogitComparison, + q_pre_stats: LogitComparison, + q_norm_stats: LogitComparison, + k_norm_stats: LogitComparison, + q_stats: LogitComparison, + k_store_stats: LogitComparison, + v_store_stats: LogitComparison, + k_cache_stats: LogitComparison, + v_cache_stats: LogitComparison, + attn_stats: LogitComparison, + full_attn_cpu_stats: LogitComparison, + decode_attn_cpu_stats: LogitComparison, +} + +struct RecurrentCacheSelfCheck { + layer_index: u32, + same_top1: bool, + hidden_stats: LogitComparison, + r_cache_stats: LogitComparison, + s_cache_stats: LogitComparison, +} + +struct RecurrentTensorCheck { + layer_index: u32, + z_stats: LogitComparison, + beta_stats: LogitComparison, + gate_stats: LogitComparison, + conv_output_stats: LogitComparison, + output_view_stats: LogitComparison, + output_norm_stats: LogitComparison, + z_silu_stats: LogitComparison, + gated_output_stats: LogitComparison, + final_output_stats: LogitComparison, +} + +struct RecurrentStepCpuCheck { + layer_index: u32, + conv_output_cpu_stats: LogitComparison, + q_conv_cpu_stats: LogitComparison, + k_conv_cpu_stats: LogitComparison, + output_view_cpu_stats: LogitComparison, +} + +fn attention_cache_self_check( + model: &LlamaModel, + token_ids: &[i32], + cache_type: TensorType, +) -> Result> { + let (layer_index, block_spec) = qwen35moe_first_attention_block_spec(model)?; + let layout = qwen35moe_attention_block_layout(model, layer_index)?; + let decode_spec = qwen35moe_attention_decode_spec( + model, + layer_index, + u32::try_from(token_ids.len())?, + 1, + cache_type, + cache_type, + )?; + let positions = (0..token_ids.len()) + .map(i32::try_from) + .collect::, _>>()?; + + let mut full_loaded = layout.allocate_and_load_with_extra(&model.gguf, 64 << 20)?; + let compiled_full = + compile_attention_block_metal(&mut full_loaded, &block_spec, token_ids.len())?; + let full_run = execute_attention_block_graph_metal_cached( + &compiled_full, + &mut full_loaded, + LogitsProbeInput::TokenIds(token_ids), + &positions, + )?; + + let mut decode_loaded = layout.allocate_and_load_with_extra(&model.gguf, 64 << 20)?; + let compiled_decode = compile_attention_decode_metal(&mut decode_loaded, &decode_spec, 1)?; + let mut decode_last_hidden = None; + for (position, token_id) in token_ids.iter().copied().enumerate() { + let run = execute_attention_decode_graph_metal_cached( + &compiled_decode, + &mut decode_loaded, + LogitsProbeInput::TokenIds(std::slice::from_ref(&token_id)), + &[i32::try_from(position)?], + position + 1, + )?; + decode_last_hidden = Some(run.hidden); + } + let decode_last_hidden = + decode_last_hidden.ok_or("attention decode self-check did not produce hidden output")?; + let hidden_size = decode_last_hidden.len(); + let full_last_hidden = full_run + .hidden + .get( + full_run + .hidden + .len() + .checked_sub(hidden_size) + .ok_or("attention block hidden output was shorter than expected")?.., + ) + .ok_or("attention block hidden slice was out of range")?; + let full_top1 = top_k_logits(full_last_hidden, 1) + .first() + .copied() + .ok_or("attention block self-check produced no logits-like values")?; + let decode_top1 = top_k_logits(&decode_last_hidden, 1) + .first() + .copied() + .ok_or("attention decode self-check produced no hidden values")?; + + Ok(AttentionCacheSelfCheck { + layer_index, + same_top1: full_top1.0 == decode_top1.0, + hidden_stats: compare_logits(full_last_hidden, &decode_last_hidden), + }) +} + +fn recurrent_cache_self_check( + model: &LlamaModel, + token_ids: &[i32], +) -> Result> { + let (layer_index, _) = qwen35moe_first_recurrent_block_spec(model)?; + let layout = qwen35moe_recurrent_block_layout(model, layer_index)?; + let spec = qwen35moe_delta_net_recurrent_decode_spec( + model, + layer_index, + 1, + TensorType::F32, + TensorType::F32, + )?; + + let mut full_loaded = layout.allocate_and_load_with_extra(&model.gguf, 64 << 20)?; + let full_compiled = + compile_delta_net_recurrent_decode_metal(&mut full_loaded, &spec, token_ids.len())?; + let full_run = execute_delta_net_recurrent_decode_graph_metal_cached( + &full_compiled, + &mut full_loaded, + LogitsProbeInput::TokenIds(token_ids), + )?; + let full_r_cache = read_tensor_f32s(&full_loaded.ctx, "recur_decode.r_cache")?; + let full_s_cache = read_tensor_f32s(&full_loaded.ctx, "recur_decode.s_cache")?; + + let mut decode_loaded = layout.allocate_and_load_with_extra(&model.gguf, 64 << 20)?; + let decode_compiled = compile_delta_net_recurrent_decode_metal(&mut decode_loaded, &spec, 1)?; + let mut decode_last_hidden = None; + for &token_id in token_ids { + let run = execute_delta_net_recurrent_decode_graph_metal_cached( + &decode_compiled, + &mut decode_loaded, + LogitsProbeInput::TokenIds(std::slice::from_ref(&token_id)), + )?; + decode_last_hidden = Some(run.hidden); + } + let decode_last_hidden = + decode_last_hidden.ok_or("recurrent decode self-check did not produce hidden output")?; + let decode_r_cache = read_tensor_f32s(&decode_loaded.ctx, "recur_decode.r_cache")?; + let decode_s_cache = read_tensor_f32s(&decode_loaded.ctx, "recur_decode.s_cache")?; + let full_last_hidden = last_token_slice(&full_run.hidden, full_run.hidden_size)?; + + let hidden_top1 = top_k_logits(full_last_hidden, 1) + .first() + .copied() + .ok_or("recurrent full decode produced no hidden values")?; + let decode_top1 = top_k_logits(&decode_last_hidden, 1) + .first() + .copied() + .ok_or("recurrent step decode produced no hidden values")?; + + Ok(RecurrentCacheSelfCheck { + layer_index, + same_top1: hidden_top1.0 == decode_top1.0, + hidden_stats: compare_logits(full_last_hidden, &decode_last_hidden), + r_cache_stats: compare_logits(&full_r_cache, &decode_r_cache), + s_cache_stats: compare_logits(&full_s_cache, &decode_s_cache), + }) +} + +fn recurrent_tensor_check( + model: &LlamaModel, + token_ids: &[i32], +) -> Result> { + let (layer_index, _) = qwen35moe_first_recurrent_block_spec(model)?; + let layout = qwen35moe_recurrent_block_layout(model, layer_index)?; + let spec = qwen35moe_delta_net_recurrent_decode_spec( + model, + layer_index, + 1, + TensorType::F32, + TensorType::F32, + )?; + + let mut full_loaded = layout.allocate_and_load_with_extra(&model.gguf, 64 << 20)?; + let full_runtime = MetalRuntime::new()?; + let full_features = full_runtime.features(); + let mut full_graph = build_delta_net_recurrent_decode_graph( + &mut full_loaded.ctx, + &full_loaded.tensor_ids, + &spec, + token_ids.len(), + )?; + let full_z_id = add_hidden_token_checkpoint_by_name( + &mut full_loaded.ctx, + "recur_decode.z", + "recur_decode.z_ck", + )?; + let full_beta_id = add_hidden_token_checkpoint_by_name( + &mut full_loaded.ctx, + "recur_decode.beta", + "recur_decode.beta_ck", + )?; + let full_gate_id = add_hidden_token_checkpoint_by_name( + &mut full_loaded.ctx, + "recur_decode.gate", + "recur_decode.gate_ck", + )?; + let full_conv_output_id = add_hidden_token_checkpoint_by_name( + &mut full_loaded.ctx, + "recur_decode.conv_output", + "recur_decode.conv_output_ck", + )?; + let full_output_view_id = add_hidden_token_checkpoint_by_name( + &mut full_loaded.ctx, + "recur_decode.output_view", + "recur_decode.output_view_ck", + )?; + let full_output_norm_id = add_hidden_token_checkpoint_by_name( + &mut full_loaded.ctx, + "recur_decode.output_norm", + "recur_decode.output_norm_ck", + )?; + let full_z_silu_id = add_hidden_token_checkpoint_by_name( + &mut full_loaded.ctx, + "recur_decode.z_silu", + "recur_decode.z_silu_ck", + )?; + let full_gated_output_id = add_hidden_token_checkpoint_by_name( + &mut full_loaded.ctx, + "recur_decode.gated_output", + "recur_decode.gated_output_ck", + )?; + let full_final_output_id = add_hidden_token_checkpoint_by_name( + &mut full_loaded.ctx, + "recur_decode.final_output", + "recur_decode.final_output_ck", + )?; + for tensor_id in [ + full_z_id, + full_beta_id, + full_gate_id, + full_conv_output_id, + full_output_view_id, + full_output_norm_id, + full_z_silu_id, + full_gated_output_id, + full_final_output_id, + ] { + full_graph + .graph + .build_forward_expand(&full_loaded.ctx, tensor_id)?; + } + let full_prepared = prepare_graph(&full_loaded.ctx, &full_graph.graph, full_features)?; + let full_session = MetalGraphSession::from_runtime( + full_runtime, + &full_loaded.ctx, + &full_prepared, + BufferStorageMode::Shared, + BufferStorageMode::Shared, + )?; + let full_token_bytes = i32s_to_bytes(token_ids); + let full_outputs = [ + full_z_id, + full_beta_id, + full_gate_id, + full_conv_output_id, + full_output_view_id, + full_output_norm_id, + full_z_silu_id, + full_gated_output_id, + full_final_output_id, + ]; + let full_execution = full_session.execute( + &full_loaded.ctx, + &[MetalGraphTensorWrite { + tensor_id: full_graph.input_primary, + bytes: &full_token_bytes, + }], + &full_outputs, + )?; + let full_z = output_last_token_slice(&full_loaded.ctx, &full_execution, full_z_id)?; + let full_beta = output_last_token_slice(&full_loaded.ctx, &full_execution, full_beta_id)?; + let full_gate = output_last_token_slice(&full_loaded.ctx, &full_execution, full_gate_id)?; + let full_conv_output = + output_last_token_slice(&full_loaded.ctx, &full_execution, full_conv_output_id)?; + let full_output_view = + output_last_token_slice(&full_loaded.ctx, &full_execution, full_output_view_id)?; + let full_output_norm = + output_last_token_slice(&full_loaded.ctx, &full_execution, full_output_norm_id)?; + let full_z_silu = output_last_token_slice(&full_loaded.ctx, &full_execution, full_z_silu_id)?; + let full_gated_output = + output_last_token_slice(&full_loaded.ctx, &full_execution, full_gated_output_id)?; + let full_final_output = + output_last_token_slice(&full_loaded.ctx, &full_execution, full_final_output_id)?; + + let mut step_loaded = layout.allocate_and_load_with_extra(&model.gguf, 64 << 20)?; + let step_runtime = MetalRuntime::new()?; + let step_features = step_runtime.features(); + let mut step_graph = build_delta_net_recurrent_decode_graph( + &mut step_loaded.ctx, + &step_loaded.tensor_ids, + &spec, + 1, + )?; + let step_z_id = add_hidden_token_checkpoint_by_name( + &mut step_loaded.ctx, + "recur_decode.z", + "recur_decode.z_ck", + )?; + let step_beta_id = add_hidden_token_checkpoint_by_name( + &mut step_loaded.ctx, + "recur_decode.beta", + "recur_decode.beta_ck", + )?; + let step_gate_id = add_hidden_token_checkpoint_by_name( + &mut step_loaded.ctx, + "recur_decode.gate", + "recur_decode.gate_ck", + )?; + let step_conv_output_id = add_hidden_token_checkpoint_by_name( + &mut step_loaded.ctx, + "recur_decode.conv_output", + "recur_decode.conv_output_ck", + )?; + let step_output_view_id = add_hidden_token_checkpoint_by_name( + &mut step_loaded.ctx, + "recur_decode.output_view", + "recur_decode.output_view_ck", + )?; + let step_output_norm_id = add_hidden_token_checkpoint_by_name( + &mut step_loaded.ctx, + "recur_decode.output_norm", + "recur_decode.output_norm_ck", + )?; + let step_z_silu_id = add_hidden_token_checkpoint_by_name( + &mut step_loaded.ctx, + "recur_decode.z_silu", + "recur_decode.z_silu_ck", + )?; + let step_gated_output_id = add_hidden_token_checkpoint_by_name( + &mut step_loaded.ctx, + "recur_decode.gated_output", + "recur_decode.gated_output_ck", + )?; + let step_final_output_id = add_hidden_token_checkpoint_by_name( + &mut step_loaded.ctx, + "recur_decode.final_output", + "recur_decode.final_output_ck", + )?; + for tensor_id in [ + step_z_id, + step_beta_id, + step_gate_id, + step_conv_output_id, + step_output_view_id, + step_output_norm_id, + step_z_silu_id, + step_gated_output_id, + step_final_output_id, + ] { + step_graph + .graph + .build_forward_expand(&step_loaded.ctx, tensor_id)?; + } + let step_prepared = prepare_graph(&step_loaded.ctx, &step_graph.graph, step_features)?; + let step_session = MetalGraphSession::from_runtime( + step_runtime, + &step_loaded.ctx, + &step_prepared, + BufferStorageMode::Shared, + BufferStorageMode::Shared, + )?; + let step_outputs = [ + step_z_id, + step_beta_id, + step_gate_id, + step_conv_output_id, + step_output_view_id, + step_output_norm_id, + step_z_silu_id, + step_gated_output_id, + step_final_output_id, + ]; + let mut step_z = None; + let mut step_beta = None; + let mut step_gate = None; + let mut step_conv_output = None; + let mut step_output_view = None; + let mut step_output_norm = None; + let mut step_z_silu = None; + let mut step_gated_output = None; + let mut step_final_output = None; + for &token_id in token_ids { + let step_token_bytes = i32s_to_bytes(&[token_id]); + let execution = step_session.execute( + &step_loaded.ctx, + &[MetalGraphTensorWrite { + tensor_id: step_graph.input_primary, + bytes: &step_token_bytes, + }], + &step_outputs, + )?; + step_z = Some(output_last_token_slice( + &step_loaded.ctx, + &execution, + step_z_id, + )?); + step_beta = Some(output_last_token_slice( + &step_loaded.ctx, + &execution, + step_beta_id, + )?); + step_gate = Some(output_last_token_slice( + &step_loaded.ctx, + &execution, + step_gate_id, + )?); + step_conv_output = Some(output_last_token_slice( + &step_loaded.ctx, + &execution, + step_conv_output_id, + )?); + step_output_view = Some(output_last_token_slice( + &step_loaded.ctx, + &execution, + step_output_view_id, + )?); + step_output_norm = Some(output_last_token_slice( + &step_loaded.ctx, + &execution, + step_output_norm_id, + )?); + step_z_silu = Some(output_last_token_slice( + &step_loaded.ctx, + &execution, + step_z_silu_id, + )?); + step_gated_output = Some(output_last_token_slice( + &step_loaded.ctx, + &execution, + step_gated_output_id, + )?); + step_final_output = Some(output_last_token_slice( + &step_loaded.ctx, + &execution, + step_final_output_id, + )?); + } + + Ok(RecurrentTensorCheck { + layer_index, + z_stats: compare_logits( + &full_z, + step_z.as_deref().ok_or("missing recurrent step z")?, + ), + beta_stats: compare_logits( + &full_beta, + step_beta.as_deref().ok_or("missing recurrent step beta")?, + ), + gate_stats: compare_logits( + &full_gate, + step_gate.as_deref().ok_or("missing recurrent step gate")?, + ), + conv_output_stats: compare_logits( + &full_conv_output, + step_conv_output + .as_deref() + .ok_or("missing recurrent step conv_output")?, + ), + output_view_stats: compare_logits( + &full_output_view, + step_output_view + .as_deref() + .ok_or("missing recurrent step output_view")?, + ), + output_norm_stats: compare_logits( + &full_output_norm, + step_output_norm + .as_deref() + .ok_or("missing recurrent step output_norm")?, + ), + z_silu_stats: compare_logits( + &full_z_silu, + step_z_silu + .as_deref() + .ok_or("missing recurrent step z_silu")?, + ), + gated_output_stats: compare_logits( + &full_gated_output, + step_gated_output + .as_deref() + .ok_or("missing recurrent step gated_output")?, + ), + final_output_stats: compare_logits( + &full_final_output, + step_final_output + .as_deref() + .ok_or("missing recurrent step final_output")?, + ), + }) +} + +fn recurrent_step_cpu_check( + model: &LlamaModel, + token_ids: &[i32], +) -> Result> { + if token_ids.len() < 2 { + return Err("recurrent step cpu check requires at least two tokens".into()); + } + + let (layer_index, block_spec) = qwen35moe_first_recurrent_block_spec(model)?; + let layout = qwen35moe_recurrent_block_layout(model, layer_index)?; + let spec = qwen35moe_delta_net_recurrent_decode_spec( + model, + layer_index, + 1, + TensorType::F32, + TensorType::F32, + )?; + + let mut loaded = layout.allocate_and_load_with_extra(&model.gguf, 64 << 20)?; + let runtime = MetalRuntime::new()?; + let features = runtime.features(); + let mut graph = + build_delta_net_recurrent_decode_graph(&mut loaded.ctx, &loaded.tensor_ids, &spec, 1)?; + let conv_input_id = add_hidden_token_checkpoint_by_name( + &mut loaded.ctx, + "recur_decode.conv_input", + "recur_decode.conv_input_ck", + )?; + let beta_id = add_hidden_token_checkpoint_by_name( + &mut loaded.ctx, + "recur_decode.beta", + "recur_decode.beta_ck", + )?; + let gate_id = add_hidden_token_checkpoint_by_name( + &mut loaded.ctx, + "recur_decode.gate", + "recur_decode.gate_ck", + )?; + let q_conv_id = add_hidden_token_checkpoint_by_name( + &mut loaded.ctx, + "recur_decode.q_conv_predelta", + "recur_decode.q_conv_ck", + )?; + let k_conv_id = add_hidden_token_checkpoint_by_name( + &mut loaded.ctx, + "recur_decode.k_conv_predelta", + "recur_decode.k_conv_ck", + )?; + let conv_output_id = add_hidden_token_checkpoint_by_name( + &mut loaded.ctx, + "recur_decode.conv_output", + "recur_decode.conv_output_ck", + )?; + let output_view_id = add_hidden_token_checkpoint_by_name( + &mut loaded.ctx, + "recur_decode.output_view", + "recur_decode.output_view_ck", + )?; + for tensor_id in [ + conv_input_id, + beta_id, + gate_id, + q_conv_id, + k_conv_id, + conv_output_id, + output_view_id, + ] { + graph.graph.build_forward_expand(&loaded.ctx, tensor_id)?; + } + let prepared = prepare_graph(&loaded.ctx, &graph.graph, features)?; + let session = MetalGraphSession::from_runtime( + runtime, + &loaded.ctx, + &prepared, + BufferStorageMode::Shared, + BufferStorageMode::Shared, + )?; + + let first_token_bytes = i32s_to_bytes(&[token_ids[0]]); + let first_execution = session.execute( + &loaded.ctx, + &[MetalGraphTensorWrite { + tensor_id: graph.input_primary, + bytes: &first_token_bytes, + }], + &[graph.s_cache], + )?; + let state_before = bytes_to_f32s( + first_execution + .outputs + .get(&graph.s_cache) + .ok_or("missing recurrent step s_cache output after first token")?, + ); + + let second_token_bytes = i32s_to_bytes(&[token_ids[1]]); + let execution = session.execute( + &loaded.ctx, + &[MetalGraphTensorWrite { + tensor_id: graph.input_primary, + bytes: &second_token_bytes, + }], + &[ + conv_input_id, + beta_id, + gate_id, + q_conv_id, + k_conv_id, + conv_output_id, + output_view_id, + ], + )?; + + let conv_input = bytes_to_f32s( + execution + .outputs + .get(&conv_input_id) + .ok_or("missing recurrent step conv_input output")?, + ); + let beta = bytes_to_f32s( + execution + .outputs + .get(&beta_id) + .ok_or("missing recurrent step beta output")?, + ); + let gate = bytes_to_f32s( + execution + .outputs + .get(&gate_id) + .ok_or("missing recurrent step gate output")?, + ); + let q_conv = bytes_to_f32s( + execution + .outputs + .get(&q_conv_id) + .ok_or("missing recurrent step q_conv output")?, + ); + let k_conv = bytes_to_f32s( + execution + .outputs + .get(&k_conv_id) + .ok_or("missing recurrent step k_conv output")?, + ); + let conv_output = bytes_to_f32s( + execution + .outputs + .get(&conv_output_id) + .ok_or("missing recurrent step conv_output output")?, + ); + let output_view = bytes_to_f32s( + execution + .outputs + .get(&output_view_id) + .ok_or("missing recurrent step output_view output")?, + ); + + let conv_kernel_id = loaded + .tensor_ids + .get(&block_spec.conv_kernel_name) + .copied() + .ok_or("missing recurrent conv kernel tensor id")?; + let conv_kernel_tensor = loaded + .ctx + .tensor(conv_kernel_id) + .ok_or("invalid recurrent conv kernel tensor")?; + let conv_kernel_size = usize::try_from(conv_kernel_tensor.ne[0])?; + let conv_channels = usize::try_from(conv_kernel_tensor.ne[1])?; + let conv_kernel = read_tensor_f32s(&loaded.ctx, &block_spec.conv_kernel_name)?; + + let conv_output_cpu = + cpu_ssm_conv_single_token(&conv_input, &conv_kernel, conv_kernel_size, conv_channels)?; + + let key_head_dim = usize::try_from(block_spec.key_head_dim)?; + let key_head_count = usize::try_from(block_spec.key_head_count)?; + let value_head_dim = usize::try_from(block_spec.value_head_dim)?; + let value_head_count = usize::try_from(block_spec.value_head_count)?; + + let q_width = key_head_dim + .checked_mul(key_head_count) + .ok_or("overflow computing recurrent q width")?; + let k_width = q_width; + let v_width = value_head_dim + .checked_mul(value_head_count) + .ok_or("overflow computing recurrent v width")?; + let expected_qkv = q_width + .checked_add(k_width) + .and_then(|v| v.checked_add(v_width)) + .ok_or("overflow computing recurrent qkv width")?; + if conv_output.len() != expected_qkv || conv_output_cpu.len() != expected_qkv { + return Err(format!( + "unexpected recurrent conv output lengths: metal={} cpu={} expected {}", + conv_output.len(), + conv_output_cpu.len(), + expected_qkv + ) + .into()); + } + + let q_cpu = cpu_l2_norm_heads( + &conv_output[..q_width], + key_head_dim, + key_head_count, + block_spec.rms_epsilon, + )?; + let k_cpu = cpu_l2_norm_heads( + &conv_output[q_width..q_width + k_width], + key_head_dim, + key_head_count, + block_spec.rms_epsilon, + )?; + let output_view_cpu = cpu_gated_delta_net_last_token( + &q_cpu, + &k_cpu, + &conv_output[q_width + k_width..], + &gate, + &beta, + &state_before, + key_head_count, + value_head_count, + value_head_dim, + )?; + + Ok(RecurrentStepCpuCheck { + layer_index, + conv_output_cpu_stats: compare_logits(&conv_output, &conv_output_cpu), + q_conv_cpu_stats: compare_logits(&q_conv, &q_cpu), + k_conv_cpu_stats: compare_logits(&k_conv, &k_cpu), + output_view_cpu_stats: compare_logits(&output_view, &output_view_cpu), + }) +} + +fn attention_decode_batch_self_check( + model: &LlamaModel, + token_ids: &[i32], +) -> Result> { + let (layer_index, _) = qwen35moe_first_attention_block_spec(model)?; + let layout = qwen35moe_attention_block_layout(model, layer_index)?; + let spec = qwen35moe_attention_decode_spec( + model, + layer_index, + u32::try_from(token_ids.len())?, + 1, + TensorType::F32, + TensorType::F32, + )?; + let positions = (0..token_ids.len()) + .map(i32::try_from) + .collect::, _>>()?; + + let mut batched_loaded = layout.allocate_and_load_with_extra(&model.gguf, 64 << 20)?; + let batched_compiled = + compile_attention_decode_metal(&mut batched_loaded, &spec, token_ids.len())?; + let batched_run = execute_attention_decode_graph_metal_cached( + &batched_compiled, + &mut batched_loaded, + LogitsProbeInput::TokenIds(token_ids), + &positions, + token_ids.len(), + )?; + let batched_last_hidden = last_token_slice(&batched_run.hidden, batched_run.hidden_size)?; + let batched_k_cache = read_tensor_f32s(&batched_loaded.ctx, "attn_decode.k_cache")?; + let batched_v_cache = read_tensor_f32s(&batched_loaded.ctx, "attn_decode.v_cache")?; + + let mut step_loaded = layout.allocate_and_load_with_extra(&model.gguf, 64 << 20)?; + let step_compiled = compile_attention_decode_metal(&mut step_loaded, &spec, 1)?; + let mut step_last_hidden = None; + for (position, token_id) in token_ids.iter().copied().enumerate() { + let run = execute_attention_decode_graph_metal_cached( + &step_compiled, + &mut step_loaded, + LogitsProbeInput::TokenIds(std::slice::from_ref(&token_id)), + &[i32::try_from(position)?], + position + 1, + )?; + step_last_hidden = Some(run.hidden); + } + let step_last_hidden = step_last_hidden + .ok_or("attention decode batch self-check did not produce hidden output")?; + let step_k_cache = read_tensor_f32s(&step_loaded.ctx, "attn_decode.k_cache")?; + let step_v_cache = read_tensor_f32s(&step_loaded.ctx, "attn_decode.v_cache")?; + + Ok(AttentionDecodeBatchSelfCheck { + layer_index, + hidden_stats: compare_logits(batched_last_hidden, &step_last_hidden), + k_cache_stats: compare_logits(&batched_k_cache, &step_k_cache), + v_cache_stats: compare_logits(&batched_v_cache, &step_v_cache), + }) +} + +fn add_contiguous_checkpoint( + ctx: &mut Context, + src: TensorId, + name: &str, +) -> Result> { + let tensor = ctx + .tensor(src) + .ok_or_else(|| format!("invalid tensor id {src} for checkpoint {name}"))?; + let cont = ctx.cont_2d(src, tensor.ne[0], tensor.ne[1])?; + ctx.set_tensor_name(cont, name)?; + Ok(cont) +} + +fn add_hidden_token_checkpoint_by_name( + ctx: &mut Context, + src_name: &str, + checkpoint_name: &str, +) -> Result> { + let src = ctx + .get_tensor(src_name) + .ok_or_else(|| format!("missing tensor '{src_name}'"))?; + let tensor = ctx + .tensor(src) + .ok_or_else(|| format!("invalid tensor id {src} for checkpoint {checkpoint_name}"))?; + let rank = tensor.desc.layout.rank(); + let cont = if rank <= 2 { + ctx.cont_2d(src, tensor.ne[0], tensor.ne[1])? + } else { + ctx.cont_2d( + src, + tensor.ne[0] * tensor.ne[1], + tensor.ne[2] * tensor.ne[3], + )? + }; + ctx.set_tensor_name(cont, checkpoint_name)?; + Ok(cont) +} + +fn encode_rope_positions( + rope: &AttentionRopeSpec, + positions: &[i32], + n_tokens: usize, +) -> Result, Box> { + let n_components = + if rope.mode == GGML_ROPE_TYPE_IMROPE || (rope.mode & GGML_ROPE_TYPE_MROPE) != 0 { + 4 + } else { + 1 + }; + + if n_components == 1 { + if positions.len() != n_tokens { + return Err(format!( + "rope positions length mismatch: got {}, expected {}", + positions.len(), + n_tokens + ) + .into()); + } + return Ok(positions.to_vec()); + } + + let expanded_len = n_tokens + .checked_mul(n_components) + .ok_or("overflow computing expanded rope positions")?; + if positions.len() == expanded_len { + return Ok(positions.to_vec()); + } + if positions.len() != n_tokens { + return Err(format!( + "mrope positions length mismatch: got {}, expected {} or {}", + positions.len(), + n_tokens, + expanded_len + ) + .into()); + } + + let mut expanded = vec![0_i32; expanded_len]; + expanded[..n_tokens].copy_from_slice(positions); + expanded[n_tokens..2 * n_tokens].copy_from_slice(positions); + expanded[2 * n_tokens..3 * n_tokens].copy_from_slice(positions); + Ok(expanded) +} + +fn causal_mask_f16_bytes(n_tokens: usize) -> Vec { + let mut bytes = Vec::with_capacity(n_tokens * n_tokens * std::mem::size_of::()); + let zero = f32_to_f16(0.0); + let neg_inf = f32_to_f16(f32::NEG_INFINITY); + for query in 0..n_tokens { + for key in 0..n_tokens { + let value = if key > query { neg_inf } else { zero }; + bytes.extend_from_slice(&value.to_le_bytes()); + } + } + bytes +} + +fn causal_mask_f32_bytes(n_tokens: usize) -> Vec { + let mut bytes = Vec::with_capacity(n_tokens * n_tokens * std::mem::size_of::()); + for query in 0..n_tokens { + for key in 0..n_tokens { + let value = if key > query { f32::NEG_INFINITY } else { 0.0 }; + bytes.extend_from_slice(&value.to_le_bytes()); + } + } + bytes +} + +fn causal_mask_bytes_for_tensor( + ctx: &Context, + tensor_id: TensorId, + n_tokens: usize, +) -> Result, Box> { + let tensor = ctx + .tensor(tensor_id) + .ok_or_else(|| format!("invalid attention mask tensor id {tensor_id}"))?; + Ok(match tensor.desc.ty { + TensorType::F16 => causal_mask_f16_bytes(n_tokens), + TensorType::F32 => causal_mask_f32_bytes(n_tokens), + other => { + return Err(format!("unsupported attention mask tensor type {}", other.name()).into()) + } + }) +} + +fn attention_cache_tensor_check( + model: &LlamaModel, + token_ids: &[i32], +) -> Result> { + let (layer_index, block_spec) = qwen35moe_first_attention_block_spec(model)?; + let layout = qwen35moe_attention_block_layout(model, layer_index)?; + let decode_spec = qwen35moe_attention_decode_spec( + model, + layer_index, + u32::try_from(token_ids.len())?, + 1, + TensorType::F32, + TensorType::F32, + )?; + let positions = (0..token_ids.len()) + .map(i32::try_from) + .collect::, _>>()?; + let full_rope_positions = block_spec + .rope + .as_ref() + .map(|rope| encode_rope_positions(rope, &positions, token_ids.len())) + .transpose()?; + + let mut full_loaded = layout.allocate_and_load_with_extra(&model.gguf, 64 << 20)?; + let full_runtime = MetalRuntime::new()?; + let full_features = full_runtime.features(); + let (mut full_block, _) = prepare_attention_block_graph( + &mut full_loaded.ctx, + &full_loaded.tensor_ids, + &block_spec, + token_ids.len(), + full_features, + )?; + let full_token_bytes = i32s_to_bytes(token_ids); + let full_pos_bytes = i32s_to_bytes( + full_rope_positions + .as_deref() + .ok_or("missing full rope positions")?, + ); + let full_q_store_id = full_loaded + .ctx + .get_tensor("attn.q_store") + .ok_or("missing attn.q_store")?; + let full_q_proj_id = full_loaded + .ctx + .get_tensor("attn.q_proj") + .ok_or("missing attn.q_proj")?; + let full_q_pre_store_id = full_loaded + .ctx + .get_tensor("attn.q_pre_store") + .ok_or("missing attn.q_pre_store")?; + let full_q_pre_cont_id = + add_contiguous_checkpoint(&mut full_loaded.ctx, full_q_pre_store_id, "attn.q_pre_cont")?; + let full_q_norm_store_id = full_loaded + .ctx + .get_tensor("attn.q_norm_store") + .ok_or("missing attn.q_norm_store")?; + let full_q_norm_cont_id = add_contiguous_checkpoint( + &mut full_loaded.ctx, + full_q_norm_store_id, + "attn.q_norm_cont", + )?; + let full_k_store_id = full_loaded + .ctx + .get_tensor("attn.k_store") + .ok_or("missing attn.k_store")?; + let full_k_cont_id = + add_contiguous_checkpoint(&mut full_loaded.ctx, full_k_store_id, "attn.k_cont")?; + let full_k_norm_store_id = full_loaded + .ctx + .get_tensor("attn.k_norm_store") + .ok_or("missing attn.k_norm_store")?; + let full_k_norm_cont_id = add_contiguous_checkpoint( + &mut full_loaded.ctx, + full_k_norm_store_id, + "attn.k_norm_cont", + )?; + let full_v_store_id = full_loaded + .ctx + .get_tensor("attn.v_store") + .ok_or("missing attn.v_store")?; + let full_v_cont_id = + add_contiguous_checkpoint(&mut full_loaded.ctx, full_v_store_id, "attn.v_cont")?; + let full_q_cont_id = + add_contiguous_checkpoint(&mut full_loaded.ctx, full_q_store_id, "attn.q_cont")?; + let full_attn_id = full_loaded + .ctx + .get_tensor("attn.attn_flat") + .ok_or("missing attn.attn_flat")?; + full_block + .graph + .build_forward_expand(&full_loaded.ctx, full_q_cont_id)?; + full_block + .graph + .build_forward_expand(&full_loaded.ctx, full_q_proj_id)?; + full_block + .graph + .build_forward_expand(&full_loaded.ctx, full_q_pre_cont_id)?; + full_block + .graph + .build_forward_expand(&full_loaded.ctx, full_q_norm_cont_id)?; + full_block + .graph + .build_forward_expand(&full_loaded.ctx, full_k_cont_id)?; + full_block + .graph + .build_forward_expand(&full_loaded.ctx, full_k_norm_cont_id)?; + full_block + .graph + .build_forward_expand(&full_loaded.ctx, full_v_cont_id)?; + full_block + .graph + .build_forward_expand(&full_loaded.ctx, full_attn_id)?; + let full_prepared = prepare_graph(&full_loaded.ctx, &full_block.graph, full_features)?; + let full_session = MetalGraphSession::from_runtime( + full_runtime, + &full_loaded.ctx, + &full_prepared, + BufferStorageMode::Shared, + BufferStorageMode::Shared, + )?; + let full_outputs = [ + full_q_cont_id, + full_q_proj_id, + full_q_pre_cont_id, + full_q_norm_cont_id, + full_k_cont_id, + full_k_norm_cont_id, + full_v_cont_id, + full_attn_id, + ]; + let full_mask_bytes = full_block + .input_mask + .map(|input_mask| { + causal_mask_bytes_for_tensor(&full_loaded.ctx, input_mask, token_ids.len()) + }) + .transpose()?; + let mut full_writes = vec![MetalGraphTensorWrite { + tensor_id: full_block.input_primary, + bytes: &full_token_bytes, + }]; + if let Some(input_positions) = full_block.input_positions { + full_writes.push(MetalGraphTensorWrite { + tensor_id: input_positions, + bytes: &full_pos_bytes, + }); + } + if let Some(input_mask) = full_block.input_mask { + full_writes.push(MetalGraphTensorWrite { + tensor_id: input_mask, + bytes: full_mask_bytes + .as_deref() + .ok_or("missing full attention mask")?, + }); + } + let full_execution = full_session.execute(&full_loaded.ctx, &full_writes, &full_outputs)?; + let full_q_store = bytes_to_f32s( + full_execution + .outputs + .get(&full_q_cont_id) + .ok_or("missing full q_store output")?, + ); + let full_q_proj = bytes_to_f32s( + full_execution + .outputs + .get(&full_q_proj_id) + .ok_or("missing full q_proj output")?, + ); + let full_q_pre_store = bytes_to_f32s( + full_execution + .outputs + .get(&full_q_pre_cont_id) + .ok_or("missing full q_pre_store output")?, + ); + let full_q_norm_store = bytes_to_f32s( + full_execution + .outputs + .get(&full_q_norm_cont_id) + .ok_or("missing full q_norm_store output")?, + ); + let full_k_store = bytes_to_f32s( + full_execution + .outputs + .get(&full_k_cont_id) + .ok_or("missing full k_store output")?, + ); + let full_k_norm_store = bytes_to_f32s( + full_execution + .outputs + .get(&full_k_norm_cont_id) + .ok_or("missing full k_norm_store output")?, + ); + let full_v_store = bytes_to_f32s( + full_execution + .outputs + .get(&full_v_cont_id) + .ok_or("missing full v_store output")?, + ); + let full_attn = bytes_to_f32s( + full_execution + .outputs + .get(&full_attn_id) + .ok_or("missing full attn output")?, + ); + + let q_row_width = full_q_store.len() / token_ids.len(); + let k_row_width = full_k_store.len() / token_ids.len(); + let v_row_width = full_v_store.len() / token_ids.len(); + let attn_row_width = full_attn.len() / token_ids.len(); + + let mut decode_loaded = layout.allocate_and_load_with_extra(&model.gguf, 64 << 20)?; + let decode_runtime = MetalRuntime::new()?; + let decode_features = decode_runtime.features(); + let (mut decode_graph, _) = prepare_attention_decode_graph( + &mut decode_loaded.ctx, + &decode_loaded.tensor_ids, + &decode_spec, + 1, + decode_features, + )?; + let decode_rope_positions = decode_spec + .block + .rope + .as_ref() + .map(|rope| encode_rope_positions(rope, &[positions[1]], 1)) + .transpose()?; + + let k_cache_len = decode_loaded + .ctx + .tensor(decode_graph.k_cache) + .ok_or("invalid decode k_cache tensor")? + .nelements() + .try_into()?; + let v_cache_len = decode_loaded + .ctx + .tensor(decode_graph.v_cache) + .ok_or("invalid decode v_cache tensor")? + .nelements() + .try_into()?; + let mut k_cache = vec![0.0f32; k_cache_len]; + let mut v_cache = vec![0.0f32; v_cache_len]; + k_cache[..k_row_width].copy_from_slice(&full_k_store[..k_row_width]); + v_cache[..v_row_width].copy_from_slice(&full_v_store[..v_row_width]); + decode_loaded + .ctx + .write_tensor_data(decode_graph.k_cache, &f32s_to_bytes(&k_cache))?; + decode_loaded + .ctx + .write_tensor_data(decode_graph.v_cache, &f32s_to_bytes(&v_cache))?; + + let decode_q_store_id = decode_loaded + .ctx + .get_tensor("attn_decode.q_store") + .ok_or("missing attn_decode.q_store")?; + let decode_q_cont_id = add_contiguous_checkpoint( + &mut decode_loaded.ctx, + decode_q_store_id, + "attn_decode.q_cont", + )?; + let decode_q_proj_id = decode_loaded + .ctx + .get_tensor("attn_decode.q_proj") + .ok_or("missing attn_decode.q_proj")?; + let decode_q_pre_store_id = decode_loaded + .ctx + .get_tensor("attn_decode.q_pre_store") + .ok_or("missing attn_decode.q_pre_store")?; + let decode_q_pre_cont_id = add_contiguous_checkpoint( + &mut decode_loaded.ctx, + decode_q_pre_store_id, + "attn_decode.q_pre_cont", + )?; + let decode_q_norm_store_id = decode_loaded + .ctx + .get_tensor("attn_decode.q_norm_store") + .ok_or("missing attn_decode.q_norm_store")?; + let decode_q_norm_cont_id = add_contiguous_checkpoint( + &mut decode_loaded.ctx, + decode_q_norm_store_id, + "attn_decode.q_norm_cont", + )?; + let decode_k_store_id = decode_loaded + .ctx + .get_tensor("attn_decode.k_store") + .ok_or("missing attn_decode.k_store")?; + let decode_k_cont_id = add_contiguous_checkpoint( + &mut decode_loaded.ctx, + decode_k_store_id, + "attn_decode.k_cont", + )?; + let decode_k_norm_store_id = decode_loaded + .ctx + .get_tensor("attn_decode.k_norm_store") + .ok_or("missing attn_decode.k_norm_store")?; + let decode_k_norm_cont_id = add_contiguous_checkpoint( + &mut decode_loaded.ctx, + decode_k_norm_store_id, + "attn_decode.k_norm_cont", + )?; + let decode_v_store_id = decode_loaded + .ctx + .get_tensor("attn_decode.v_store") + .ok_or("missing attn_decode.v_store")?; + let decode_v_cont_id = add_contiguous_checkpoint( + &mut decode_loaded.ctx, + decode_v_store_id, + "attn_decode.v_cont", + )?; + let decode_attn_id = decode_loaded + .ctx + .get_tensor("attn_decode.attn_flat") + .ok_or("missing attn_decode.attn_flat")?; + decode_graph + .graph + .build_forward_expand(&decode_loaded.ctx, decode_q_cont_id)?; + decode_graph + .graph + .build_forward_expand(&decode_loaded.ctx, decode_q_proj_id)?; + decode_graph + .graph + .build_forward_expand(&decode_loaded.ctx, decode_q_pre_cont_id)?; + decode_graph + .graph + .build_forward_expand(&decode_loaded.ctx, decode_q_norm_cont_id)?; + decode_graph + .graph + .build_forward_expand(&decode_loaded.ctx, decode_k_cont_id)?; + decode_graph + .graph + .build_forward_expand(&decode_loaded.ctx, decode_k_norm_cont_id)?; + decode_graph + .graph + .build_forward_expand(&decode_loaded.ctx, decode_v_cont_id)?; + decode_graph + .graph + .build_forward_expand(&decode_loaded.ctx, decode_graph.k_cache)?; + decode_graph + .graph + .build_forward_expand(&decode_loaded.ctx, decode_graph.v_cache)?; + decode_graph + .graph + .build_forward_expand(&decode_loaded.ctx, decode_attn_id)?; + let decode_prepared = prepare_graph(&decode_loaded.ctx, &decode_graph.graph, decode_features)?; + + let decode_session = MetalGraphSession::from_runtime( + decode_runtime, + &decode_loaded.ctx, + &decode_prepared, + BufferStorageMode::Shared, + BufferStorageMode::Shared, + )?; + let decode_token_bytes = i32s_to_bytes(&[token_ids[1]]); + let decode_pos_bytes = i32s_to_bytes(&[positions[1]]); + let decode_rope_pos_bytes = decode_rope_positions.as_deref().map(i32s_to_bytes); + let decode_outputs = [ + decode_q_cont_id, + decode_q_proj_id, + decode_q_pre_cont_id, + decode_q_norm_cont_id, + decode_k_cont_id, + decode_k_norm_cont_id, + decode_v_cont_id, + decode_graph.k_cache, + decode_graph.v_cache, + decode_attn_id, + ]; + let decode_writes = [ + MetalGraphTensorWrite { + tensor_id: decode_graph.input_primary, + bytes: &decode_token_bytes, + }, + MetalGraphTensorWrite { + tensor_id: decode_graph.input_positions, + bytes: &decode_pos_bytes, + }, + ]; + let mut decode_writes = decode_writes.to_vec(); + if let Some(input_rope_positions) = decode_graph.input_rope_positions { + decode_writes.push(MetalGraphTensorWrite { + tensor_id: input_rope_positions, + bytes: decode_rope_pos_bytes + .as_deref() + .ok_or("missing decode rope positions")?, + }); + } + let decode_execution = + decode_session.execute(&decode_loaded.ctx, &decode_writes, &decode_outputs)?; + let decode_q_store = bytes_to_f32s( + decode_execution + .outputs + .get(&decode_q_cont_id) + .ok_or("missing decode q_store output")?, + ); + let decode_q_proj = bytes_to_f32s( + decode_execution + .outputs + .get(&decode_q_proj_id) + .ok_or("missing decode q_proj output")?, + ); + let decode_q_pre_store = bytes_to_f32s( + decode_execution + .outputs + .get(&decode_q_pre_cont_id) + .ok_or("missing decode q_pre_store output")?, + ); + let decode_q_norm_store = bytes_to_f32s( + decode_execution + .outputs + .get(&decode_q_norm_cont_id) + .ok_or("missing decode q_norm_store output")?, + ); + let decode_k_store = bytes_to_f32s( + decode_execution + .outputs + .get(&decode_k_cont_id) + .ok_or("missing decode k_store output")?, + ); + let decode_k_norm_store = bytes_to_f32s( + decode_execution + .outputs + .get(&decode_k_norm_cont_id) + .ok_or("missing decode k_norm_store output")?, + ); + let decode_v_store = bytes_to_f32s( + decode_execution + .outputs + .get(&decode_v_cont_id) + .ok_or("missing decode v_store output")?, + ); + let decode_k_cache = bytes_to_f32s( + decode_execution + .outputs + .get(&decode_graph.k_cache) + .ok_or("missing decode k_cache output")?, + ); + let decode_v_cache = bytes_to_f32s( + decode_execution + .outputs + .get(&decode_graph.v_cache) + .ok_or("missing decode v_cache output")?, + ); + let decode_attn = bytes_to_f32s( + decode_execution + .outputs + .get(&decode_attn_id) + .ok_or("missing decode attn output")?, + ); + let attn_cpu = cpu_flash_attn_gqa_last_token( + &full_q_store[q_row_width..], + &full_k_store, + &full_v_store, + usize::try_from(block_spec.q_head_dim)?, + usize::try_from(block_spec.q_head_count)?, + usize::try_from(block_spec.kv_head_count)?, + token_ids.len(), + )?; + + Ok(AttentionCacheTensorCheck { + layer_index, + q_proj_stats: compare_logits( + &decode_q_proj, + &full_q_proj[full_q_proj.len() / token_ids.len()..], + ), + q_pre_stats: compare_logits(&decode_q_pre_store, &full_q_pre_store[q_row_width..]), + q_norm_stats: compare_logits(&decode_q_norm_store, &full_q_norm_store[q_row_width..]), + k_norm_stats: compare_logits(&decode_k_norm_store, &full_k_norm_store[k_row_width..]), + q_stats: compare_logits(&decode_q_store, &full_q_store[q_row_width..]), + k_store_stats: compare_logits(&decode_k_store, &full_k_store[k_row_width..]), + v_store_stats: compare_logits(&decode_v_store, &full_v_store[v_row_width..]), + k_cache_stats: compare_logits(&decode_k_cache[..full_k_store.len()], &full_k_store), + v_cache_stats: compare_logits(&decode_v_cache[..full_v_store.len()], &full_v_store), + attn_stats: compare_logits(&decode_attn, &full_attn[attn_row_width..]), + full_attn_cpu_stats: compare_logits(&full_attn[attn_row_width..], &attn_cpu), + decode_attn_cpu_stats: compare_logits(&decode_attn, &attn_cpu), + }) +} + +fn cpu_flash_attn_gqa_last_token( + q_last: &[f32], + k_all: &[f32], + v_all: &[f32], + head_dim: usize, + q_head_count: usize, + kv_head_count: usize, + n_tokens: usize, +) -> Result, Box> { + if q_head_count == 0 || kv_head_count == 0 || q_head_count % kv_head_count != 0 { + return Err(format!( + "invalid gqa shape: q_head_count={} kv_head_count={}", + q_head_count, kv_head_count + ) + .into()); + } + let q_expected = head_dim + .checked_mul(q_head_count) + .ok_or("overflow computing q attention width")?; + let kv_expected = head_dim + .checked_mul(kv_head_count) + .ok_or("overflow computing kv attention width")?; + if q_last.len() != q_expected { + return Err(format!( + "invalid q_last length: got {}, expected {}", + q_last.len(), + q_expected + ) + .into()); + } + if k_all.len() != kv_expected * n_tokens || v_all.len() != kv_expected * n_tokens { + return Err(format!( + "invalid kv lengths: k={} v={} expected {}", + k_all.len(), + v_all.len(), + kv_expected * n_tokens + ) + .into()); + } + + let heads_per_kv = q_head_count / kv_head_count; + let scale = 1.0f32 / (head_dim as f32).sqrt(); + let mut out = vec![0.0f32; q_expected]; + + for q_head in 0..q_head_count { + let kv_head = q_head / heads_per_kv; + let q_row = &q_last[q_head * head_dim..(q_head + 1) * head_dim]; + let mut scores = vec![0.0f32; n_tokens]; + for token in 0..n_tokens { + let k_offset = token * kv_expected + kv_head * head_dim; + let k_row = &k_all[k_offset..k_offset + head_dim]; + let mut dot = 0.0f32; + for i in 0..head_dim { + dot += q_row[i] * k_row[i]; + } + scores[token] = dot * scale; + } + + let max_score = scores.iter().copied().fold(f32::NEG_INFINITY, f32::max); + let mut sum = 0.0f32; + for score in &mut scores { + *score = (*score - max_score).exp(); + sum += *score; + } + for score in &mut scores { + *score /= sum.max(f32::MIN_POSITIVE); + } + + let out_row = &mut out[q_head * head_dim..(q_head + 1) * head_dim]; + for token in 0..n_tokens { + let v_offset = token * kv_expected + kv_head * head_dim; + let v_row = &v_all[v_offset..v_offset + head_dim]; + let weight = scores[token]; + for i in 0..head_dim { + out_row[i] += weight * v_row[i]; + } + } + } + + Ok(out) +} + +fn cpu_ssm_conv_single_token( + conv_input: &[f32], + conv_kernel: &[f32], + kernel_size: usize, + channels: usize, +) -> Result, Box> { + let expected = kernel_size + .checked_mul(channels) + .ok_or("overflow computing ssm conv tensor size")?; + if conv_input.len() != expected || conv_kernel.len() != expected { + return Err(format!( + "invalid ssm conv lengths: input={} kernel={} expected={}", + conv_input.len(), + conv_kernel.len(), + expected + ) + .into()); + } + + let mut out = vec![0.0f32; channels]; + for channel in 0..channels { + let row_offset = channel + .checked_mul(kernel_size) + .ok_or("overflow computing ssm conv row offset")?; + let mut sum = 0.0f32; + for tap in 0..kernel_size { + sum += conv_input[row_offset + tap] * conv_kernel[row_offset + tap]; + } + out[channel] = sum / (1.0 + (-sum).exp()); + } + Ok(out) +} + +fn cpu_l2_norm_heads( + values: &[f32], + head_dim: usize, + head_count: usize, + epsilon: f32, +) -> Result, Box> { + let expected = head_dim + .checked_mul(head_count) + .ok_or("overflow computing l2_norm length")?; + if values.len() != expected { + return Err(format!( + "invalid l2_norm input length: got {}, expected {}", + values.len(), + expected + ) + .into()); + } + + let mut out = vec![0.0f32; values.len()]; + for head in 0..head_count { + let start = head + .checked_mul(head_dim) + .ok_or("overflow computing l2_norm head offset")?; + let end = start + head_dim; + let row = &values[start..end]; + let norm = row.iter().map(|v| v * v).sum::() + epsilon; + let scale = norm.sqrt().max(f32::MIN_POSITIVE); + for (dst, src) in out[start..end].iter_mut().zip(row.iter().copied()) { + *dst = src / scale; + } + } + Ok(out) +} + +fn cpu_gated_delta_net_last_token( + q: &[f32], + k: &[f32], + v: &[f32], + gate: &[f32], + beta: &[f32], + state: &[f32], + key_head_count: usize, + value_head_count: usize, + value_head_dim: usize, +) -> Result, Box> { + if key_head_count == 0 || value_head_count == 0 || value_head_count % key_head_count != 0 { + return Err(format!( + "invalid recurrent head shape: key_head_count={} value_head_count={}", + key_head_count, value_head_count + ) + .into()); + } + + let key_width = value_head_dim + .checked_mul(key_head_count) + .ok_or("overflow computing recurrent key width")?; + let value_width = value_head_dim + .checked_mul(value_head_count) + .ok_or("overflow computing recurrent value width")?; + let state_per_head = value_head_dim + .checked_mul(value_head_dim) + .ok_or("overflow computing recurrent state size")?; + let expected_state = state_per_head + .checked_mul(value_head_count) + .ok_or("overflow computing recurrent state width")?; + if q.len() != key_width || k.len() != key_width || v.len() != value_width { + return Err(format!( + "invalid recurrent qkv lengths: q={} k={} v={} expected q/k={} v={}", + q.len(), + k.len(), + v.len(), + key_width, + value_width + ) + .into()); + } + if state.len() != expected_state { + return Err(format!( + "invalid recurrent state length: got {}, expected {}", + state.len(), + expected_state + ) + .into()); + } + if beta.len() != value_head_count { + return Err(format!( + "invalid recurrent beta length: got {}, expected {}", + beta.len(), + value_head_count + ) + .into()); + } + if gate.len() != value_head_count && gate.len() != value_width { + return Err(format!( + "invalid recurrent gate length: got {}, expected {} or {}", + gate.len(), + value_head_count, + value_width + ) + .into()); + } + + let scale = 1.0f32 / (value_head_dim as f32).sqrt(); + let mut out = vec![0.0f32; value_width]; + + for v_head in 0..value_head_count { + let q_head = v_head % key_head_count; + let q_row = &q[q_head * value_head_dim..(q_head + 1) * value_head_dim]; + let k_row = &k[q_head * value_head_dim..(q_head + 1) * value_head_dim]; + let v_row = &v[v_head * value_head_dim..(v_head + 1) * value_head_dim]; + let beta_value = beta[v_head]; + let gate_row = if gate.len() == value_head_count { + None + } else { + Some(&gate[v_head * value_head_dim..(v_head + 1) * value_head_dim]) + }; + let gate_scalar = if gate.len() == value_head_count { + Some(gate[v_head]) + } else { + None + }; + let state_offset = v_head + .checked_mul(state_per_head) + .ok_or("overflow computing recurrent state head offset")?; + let state_head = &state[state_offset..state_offset + state_per_head]; + let out_row = &mut out[v_head * value_head_dim..(v_head + 1) * value_head_dim]; + + for row in 0..value_head_dim { + let row_offset = row + .checked_mul(value_head_dim) + .ok_or("overflow computing recurrent state row offset")?; + let state_row = &state_head[row_offset..row_offset + value_head_dim]; + let mut decayed = vec![0.0f32; value_head_dim]; + for col in 0..value_head_dim { + let decay = if let Some(gate_scalar) = gate_scalar { + gate_scalar.exp() + } else { + gate_row.ok_or("missing recurrent gate row")?[col].exp() + }; + decayed[col] = state_row[col] * decay; + } + + let dot_k = decayed + .iter() + .zip(k_row.iter()) + .map(|(a, b)| a * b) + .sum::(); + let delta = (v_row[row] - dot_k) * beta_value; + let mut updated = vec![0.0f32; value_head_dim]; + for col in 0..value_head_dim { + updated[col] = decayed[col] + k_row[col] * delta; + } + out_row[row] = updated + .iter() + .zip(q_row.iter()) + .map(|(a, b)| a * b) + .sum::() + * scale; + } + } + + Ok(out) +} + +fn compare_logits(rust: &[f32], upstream: &[f32]) -> LogitComparison { + let mut max_abs_diff = 0.0_f64; + let mut abs_sum = 0.0_f64; + let mut sq_sum = 0.0_f64; + let mut dot = 0.0_f64; + let mut rust_norm = 0.0_f64; + let mut upstream_norm = 0.0_f64; + + for (&rust, &upstream) in rust.iter().zip(upstream.iter()) { + let rust = rust as f64; + let upstream = upstream as f64; + let diff = (rust - upstream).abs(); + max_abs_diff = max_abs_diff.max(diff); + abs_sum += diff; + sq_sum += diff * diff; + dot += rust * upstream; + rust_norm += rust * rust; + upstream_norm += upstream * upstream; + } + + let count = rust.len().max(1) as f64; + let cosine_similarity = if rust_norm == 0.0 || upstream_norm == 0.0 { + 0.0 + } else { + dot / (rust_norm.sqrt() * upstream_norm.sqrt()) + }; + + LogitComparison { + max_abs_diff, + mean_abs_diff: abs_sum / count, + rms_diff: (sq_sum / count).sqrt(), + cosine_similarity, + } +} + +fn last_token_slice( + values: &[f32], + hidden_size: usize, +) -> Result<&[f32], Box> { + if hidden_size == 0 { + return Err("hidden_size was zero".into()); + } + let start = values + .len() + .checked_sub(hidden_size) + .ok_or("value buffer shorter than hidden size")?; + Ok(&values[start..]) +} + +fn output_last_token_slice( + ctx: &Context, + execution: &makepad_ggml::backend::metal::MetalGraphExecution, + tensor_id: TensorId, +) -> Result, Box> { + let values = bytes_to_f32s( + execution + .outputs + .get(&tensor_id) + .ok_or_else(|| format!("missing execution output for tensor {tensor_id}"))?, + ); + let tensor = ctx + .tensor(tensor_id) + .ok_or_else(|| format!("invalid tensor id {tensor_id}"))?; + Ok(last_token_slice( + &values, + usize::try_from(tensor.ne[0]).map_err(|_| "tensor width does not fit in usize")?, + )? + .to_vec()) +} + +fn bytes_to_f32s(bytes: &[u8]) -> Vec { + bytes + .chunks_exact(std::mem::size_of::()) + .map(|chunk| f32::from_le_bytes(chunk.try_into().unwrap())) + .collect() +} + +fn read_tensor_f32s(ctx: &Context, name: &str) -> Result, Box> { + let tensor_id = ctx + .get_tensor(name) + .ok_or_else(|| format!("missing tensor '{name}'"))?; + Ok(bytes_to_f32s(ctx.tensor_data(tensor_id)?)) +} + +fn f32s_to_bytes(values: &[f32]) -> Vec { + values + .iter() + .flat_map(|value| value.to_le_bytes()) + .collect() +} + +fn i32s_to_bytes(values: &[i32]) -> Vec { + values + .iter() + .flat_map(|value| value.to_le_bytes()) + .collect() +} diff --git a/libs/llama/src/bin/llama_generate.rs b/libs/llama/src/bin/llama_generate.rs new file mode 100644 index 0000000000..4fb19f1202 --- /dev/null +++ b/libs/llama/src/bin/llama_generate.rs @@ -0,0 +1,384 @@ +use std::ffi::OsString; +use std::io::Write; +use std::path::PathBuf; +use std::process::{Command, Output}; +use std::time::Instant; + +use makepad_llama::{LlamaSession, LlamaSessionConfig, LlamaStopReason}; + +const DEFAULT_MAX_NEW_TOKENS: usize = 64; +const DEFAULT_TOKENIZE_BIN: &str = + "local/llama.cpp/build-arm64-apple-clang-release/bin/llama-tokenize"; +const DEFAULT_UPSTREAM_COMPLETION_BIN: &str = + "local/llama.cpp/build-arm64-apple-clang-release/bin/llama-completion"; + +struct Args { + model_path: PathBuf, + prompt: String, + max_new_tokens: usize, + tokenize_bin: PathBuf, + upstream_completion_bin: PathBuf, + no_bos: bool, + dump_token_ids: bool, + no_stream: bool, + verify_upstream: bool, +} + +fn main() { + match run() { + Ok(()) => {} + Err(err) => { + eprintln!("llama-generate failed: {err}"); + std::process::exit(1); + } + } +} + +fn run() -> Result<(), Box> { + let args = parse_args(std::env::args_os())?; + let prompt_token_ids = tokenize_prompt(&args)?; + if prompt_token_ids.is_empty() { + return Err("tokenizer produced no prompt tokens".into()); + } + + if args.dump_token_ids { + eprintln!("prompt.token_ids: {:?}", prompt_token_ids); + } + + let max_context = prompt_token_ids + .len() + .checked_add(args.max_new_tokens) + .ok_or("overflow computing total generation context")?; + let mut session = LlamaSession::load( + &args.model_path, + LlamaSessionConfig { + max_context: Some(u32::try_from(max_context)?), + ..LlamaSessionConfig::default() + }, + )?; + + if !args.no_stream { + print!("{}", args.prompt); + std::io::stdout().flush()?; + } + + let total_start = Instant::now(); + let prefill_start = Instant::now(); + session.append_tokens(&prompt_token_ids)?; + let prefill_elapsed = prefill_start.elapsed(); + + let generation_start = Instant::now(); + let generation = session.continue_greedy(args.max_new_tokens)?; + let generation_elapsed = generation_start.elapsed(); + let total_elapsed = total_start.elapsed(); + + if !args.no_stream { + print!("{}", generation.text); + println!(); + } + + if args.dump_token_ids { + eprintln!("generated.token_ids: {:?}", generation.token_ids); + } + + if args.verify_upstream { + let upstream_output = run_upstream_completion(&args)?; + verify_exact_output(&generation.text, &upstream_output)?; + eprintln!("verify.upstream.exact_text_match: true"); + eprintln!("verify.upstream.generated_bytes: {}", upstream_output.len()); + } + + eprintln!("stop.reason: {}", stop_reason_name(generation.stop_reason)); + eprintln!("prompt.tokens: {}", prompt_token_ids.len()); + eprintln!("generated.tokens: {}", generation.token_ids.len()); + eprintln!("prefill.seconds: {:.3}", prefill_elapsed.as_secs_f64()); + eprintln!( + "prefill.tok_s: {:.3}", + tok_per_second(prompt_token_ids.len(), prefill_elapsed.as_secs_f64()) + ); + eprintln!( + "generation.seconds: {:.3}", + generation_elapsed.as_secs_f64() + ); + eprintln!( + "generation.tok_s: {:.3}", + tok_per_second(generation.token_ids.len(), generation_elapsed.as_secs_f64()) + ); + eprintln!("total.seconds: {:.3}", total_elapsed.as_secs_f64()); + eprintln!( + "total.tok_s: {:.3}", + tok_per_second( + prompt_token_ids.len() + generation.token_ids.len(), + total_elapsed.as_secs_f64() + ) + ); + Ok(()) +} + +fn parse_args( + args: impl IntoIterator, +) -> Result> { + let mut args = args.into_iter(); + let _exe = args.next(); + + let mut model_path = None; + let mut prompt = None; + let mut prompt_parts = Vec::new(); + let mut max_new_tokens = DEFAULT_MAX_NEW_TOKENS; + let mut tokenize_bin = PathBuf::from(DEFAULT_TOKENIZE_BIN); + let mut upstream_completion_bin = PathBuf::from(DEFAULT_UPSTREAM_COMPLETION_BIN); + let mut no_bos = false; + let mut dump_token_ids = false; + let mut no_stream = false; + let mut verify_upstream = false; + + while let Some(arg) = args.next() { + match arg.to_string_lossy().as_ref() { + "-h" | "--help" => { + print_usage(); + std::process::exit(0); + } + "--max-new-tokens" => { + let value = args.next().ok_or("--max-new-tokens requires a value")?; + max_new_tokens = value.to_string_lossy().parse()?; + } + "--prompt" => { + let value = args.next().ok_or("--prompt requires a value")?; + prompt = Some(value.to_string_lossy().into_owned()); + } + "--tokenize-bin" => { + let value = args.next().ok_or("--tokenize-bin requires a value")?; + tokenize_bin = PathBuf::from(value); + } + "--upstream-completion-bin" => { + let value = args + .next() + .ok_or("--upstream-completion-bin requires a value")?; + upstream_completion_bin = PathBuf::from(value); + } + "--no-bos" => { + no_bos = true; + } + "--dump-token-ids" => { + dump_token_ids = true; + } + "--no-stream" => { + no_stream = true; + } + "--verify-upstream" => { + verify_upstream = true; + } + _ if model_path.is_none() => { + model_path = Some(PathBuf::from(arg)); + } + _ => { + prompt_parts.push(arg.to_string_lossy().into_owned()); + } + } + } + + let model_path = model_path.ok_or_else(|| { + print_usage(); + "usage: llama-generate [--max-new-tokens N] [--prompt TEXT | prompt words ...]" + })?; + let prompt = prompt.unwrap_or_else(|| prompt_parts.join(" ")); + if prompt.is_empty() { + return Err("missing prompt text".into()); + } + + Ok(Args { + model_path, + prompt, + max_new_tokens, + tokenize_bin, + upstream_completion_bin, + no_bos, + dump_token_ids, + no_stream, + verify_upstream, + }) +} + +fn print_usage() { + eprintln!( + "usage: llama-generate [--max-new-tokens N] [--tokenize-bin PATH] [--upstream-completion-bin PATH] [--no-bos] [--dump-token-ids] [--no-stream] [--verify-upstream] [--prompt TEXT | prompt words ...]" + ); +} + +fn tokenize_prompt(args: &Args) -> Result, Box> { + let mut command = Command::new(&args.tokenize_bin); + command + .arg("-m") + .arg(&args.model_path) + .arg("--ids") + .arg("--log-disable") + .arg("-p") + .arg(&args.prompt); + if args.no_bos { + command.arg("--no-bos"); + } + + let output = command.output()?; + ensure_success("llama-tokenize", &output)?; + parse_token_id_list(&String::from_utf8(output.stdout)?) +} + +fn run_upstream_completion(args: &Args) -> Result> { + let max_context = args + .max_new_tokens + .checked_add(tokenize_prompt(args)?.len()) + .ok_or("overflow computing upstream completion context")?; + let mut command = Command::new(&args.upstream_completion_bin); + command + .arg("-m") + .arg(&args.model_path) + .arg("-p") + .arg(&args.prompt) + .arg("-n") + .arg(args.max_new_tokens.to_string()) + .arg("-c") + .arg(max_context.to_string()) + .arg("-no-cnv") + .arg("--simple-io") + .arg("--no-display-prompt") + .arg("--no-warmup") + .arg("--log-disable") + .arg("--seed") + .arg("0") + .arg("--temp") + .arg("0") + .arg("--top-k") + .arg("1") + .arg("--top-p") + .arg("1") + .arg("--repeat-penalty") + .arg("1") + .arg("--presence-penalty") + .arg("0") + .arg("--frequency-penalty") + .arg("0") + .arg("--dry-multiplier") + .arg("0") + .arg("-fa") + .arg("on") + .arg("-ctk") + .arg("f16") + .arg("-ctv") + .arg("f16"); + if args.no_bos { + command + .arg("--override-kv") + .arg("tokenizer.ggml.add_bos_token=bool:false"); + } + + let output = command.output()?; + ensure_success("llama-completion", &output)?; + Ok(String::from_utf8(output.stdout)?) +} + +fn ensure_success(name: &str, output: &Output) -> Result<(), Box> { + if output.status.success() { + return Ok(()); + } + Err(format!( + "{name} exited with {}.\nstdout:\n{}\nstderr:\n{}", + output.status, + String::from_utf8_lossy(&output.stdout), + String::from_utf8_lossy(&output.stderr) + ) + .into()) +} + +fn parse_token_id_list(stdout: &str) -> Result, Box> { + let line = stdout + .lines() + .rev() + .find(|line| { + let line = line.trim(); + line.starts_with('[') && line.ends_with(']') + }) + .ok_or("tokenizer output did not contain an id list")?; + let line = line.trim(); + let inner = line + .strip_prefix('[') + .and_then(|line| line.strip_suffix(']')) + .ok_or("tokenizer id list was malformed")? + .trim(); + if inner.is_empty() { + return Ok(Vec::new()); + } + inner + .split(',') + .map(|part| part.trim().parse::().map_err(Into::into)) + .collect() +} + +fn verify_exact_output( + rust_output: &str, + upstream_output: &str, +) -> Result<(), Box> { + if rust_output == upstream_output { + return Ok(()); + } + + let diff = first_diff_byte(rust_output.as_bytes(), upstream_output.as_bytes()); + let diff_message = match diff { + Some((index, rust_byte, upstream_byte)) => format!( + "first difference at byte {}: rust={:?} upstream={:?}", + index, rust_byte, upstream_byte + ), + None => format!( + "output length mismatch: rust={} upstream={}", + rust_output.len(), + upstream_output.len() + ), + }; + Err(format!( + "exact upstream verification failed: {diff_message}\nrust.preview: {:?}\nupstream.preview: {:?}", + preview_text(rust_output, 160), + preview_text(upstream_output, 160) + ) + .into()) +} + +fn first_diff_byte(lhs: &[u8], rhs: &[u8]) -> Option<(usize, Option, Option)> { + let common_len = lhs.len().min(rhs.len()); + for index in 0..common_len { + if lhs[index] != rhs[index] { + return Some((index, Some(lhs[index]), Some(rhs[index]))); + } + } + if lhs.len() == rhs.len() { + None + } else { + Some(( + common_len, + lhs.get(common_len).copied(), + rhs.get(common_len).copied(), + )) + } +} + +fn preview_text(text: &str, max_chars: usize) -> String { + let mut preview = text.chars().take(max_chars).collect::(); + if text.chars().count() > max_chars { + preview.push_str("..."); + } + preview +} + +fn stop_reason_name(reason: LlamaStopReason) -> &'static str { + match reason { + LlamaStopReason::MaxNewTokens => "max_new_tokens", + LlamaStopReason::EndOfSequence => "eos_token", + LlamaStopReason::PaddingToken => "padding_token", + } +} + +fn tok_per_second(token_count: usize, seconds: f64) -> f64 { + if seconds > 0.0 { + token_count as f64 / seconds + } else { + 0.0 + } +} diff --git a/libs/llama/src/bin/llama_load.rs b/libs/llama/src/bin/llama_load.rs new file mode 100644 index 0000000000..b705ed2a4a --- /dev/null +++ b/libs/llama/src/bin/llama_load.rs @@ -0,0 +1,937 @@ +use std::time::Instant; + +use makepad_ggml::{ + backend::metal::{ + compile_prepared_graph, execute_compiled_graph, is_available as metal_available, + BufferStorageMode, MetalDeviceFeatures, MetalGraphTensorWrite, MetalRuntime, + }, + TensorType, +}; +use makepad_llama::{ + compile_attention_block_metal, compile_attention_decode_metal, + compile_delta_net_recurrent_decode_metal, compile_hybrid_decode_metal, + compile_logits_probe_metal, compile_moe_ffn_metal, execute_attention_block_graph_metal_cached, + execute_attention_decode_graph_metal_cached, + execute_delta_net_recurrent_decode_graph_metal_cached, + execute_hybrid_decode_graph_metal_cached, execute_logits_probe_graph_metal, + execute_logits_probe_graph_metal_cached, execute_logits_probe_metal, + execute_moe_ffn_graph_metal_cached, prepare_attention_block_graph, + prepare_attention_decode_graph, prepare_delta_net_recurrent_decode_graph, + prepare_hybrid_decode_graph, prepare_logits_probe_graph, prepare_moe_ffn_graph, + qwen35moe_attention_block_layout, qwen35moe_attention_decode_spec, + qwen35moe_delta_net_recurrent_decode_spec, qwen35moe_first_attention_block_spec, + qwen35moe_first_moe_ffn_spec, qwen35moe_first_recurrent_block_spec, + qwen35moe_hybrid_decode_spec, qwen35moe_moe_ffn_layout, qwen35moe_recurrent_block_layout, + GraphBatch, HybridCacheLayout, HybridCacheShape, HybridCacheTypes, LlamaModel, + LogitsProbeInput, ModelLayerRole, +}; + +fn main() { + let mut args = std::env::args(); + let _exe = args.next(); + let path = match args.next() { + Some(path) => path, + None => { + eprintln!("usage: llama-load "); + std::process::exit(2); + } + }; + + match run(&path) { + Ok(()) => {} + Err(err) => { + eprintln!("llama-load failed: {}", err); + std::process::exit(1); + } + } +} + +fn run(path: &str) -> Result<(), Box> { + let model = LlamaModel::load(path)?; + model.validate_layout()?; + let plan = model.execution_plan()?; + let qwen35moe = model.qwen35moe_tensors()?; + let layout = qwen35moe.weight_layout()?; + let (attention_layer, attention_spec) = qwen35moe_first_attention_block_spec(&model)?; + let (moe_layer, moe_spec) = qwen35moe_first_moe_ffn_spec(&model)?; + let (recurrent_layer, _recurrent_spec) = qwen35moe_first_recurrent_block_spec(&model)?; + let attention_decode_spec = qwen35moe_attention_decode_spec( + &model, + attention_layer, + 8, + 1, + TensorType::F16, + TensorType::F16, + )?; + let recurrent_decode_spec = qwen35moe_delta_net_recurrent_decode_spec( + &model, + recurrent_layer, + 1, + TensorType::F32, + TensorType::F32, + )?; + let hybrid_decode_spec = qwen35moe_hybrid_decode_spec( + &model, + 8, + 1, + TensorType::F16, + TensorType::F16, + TensorType::F32, + TensorType::F32, + )?; + let attention_layout = qwen35moe_attention_block_layout(&model, attention_layer)?; + let moe_layout = qwen35moe_moe_ffn_layout(&model, moe_layer)?; + let recurrent_layout = qwen35moe_recurrent_block_layout(&model, recurrent_layer)?; + let probe_layout = qwen35moe_probe_layout(&qwen35moe)?; + let probe_loaded = probe_layout.allocate_and_load(&model.gguf)?; + let cache_layout = plan + .hybrid_cache + .as_ref() + .map(|template| { + HybridCacheLayout::new(template.materialize( + HybridCacheShape { + n_ctx_seq: 4096, + n_seq_max: 8, + }, + HybridCacheTypes { + attention_k_type: TensorType::F16, + attention_v_type: TensorType::F16, + recurrent_r_type: TensorType::F32, + recurrent_s_type: TensorType::F32, + }, + )) + }) + .transpose()?; + let hybrid_decode_cache_layout = plan + .hybrid_cache + .as_ref() + .map(|template| { + HybridCacheLayout::new(template.materialize( + HybridCacheShape { + n_ctx_seq: 8, + n_seq_max: 1, + }, + HybridCacheTypes { + attention_k_type: TensorType::F16, + attention_v_type: TensorType::F16, + recurrent_r_type: TensorType::F32, + recurrent_s_type: TensorType::F32, + }, + )) + }) + .transpose()?; + let mut tail_probe_loaded = plan + .tail_probe + .weights + .allocate_and_load_with_extra(&model.gguf, plan.tail_probe.extra_activation_bytes)?; + let (tail_probe_graph, tail_probe_prepared) = prepare_logits_probe_graph( + &mut tail_probe_loaded.ctx, + &tail_probe_loaded.tensor_ids, + &plan.tail_probe.spec, + GraphBatch { + n_tokens: 1, + n_outputs: 1, + }, + MetalDeviceFeatures::default(), + ) + .map_err(|err| std::io::Error::other(format!("prepare_logits_probe_graph: {err}")))?; + let mut attention_loaded = + attention_layout.allocate_and_load_with_extra(&model.gguf, 32 << 20)?; + let (attention_graph, attention_prepared) = prepare_attention_block_graph( + &mut attention_loaded.ctx, + &attention_loaded.tensor_ids, + &attention_spec, + 1, + MetalDeviceFeatures::default(), + ) + .map_err(|err| std::io::Error::other(format!("prepare_attention_block_graph: {err}")))?; + let mut attention_decode_loaded = + attention_layout.allocate_and_load_with_extra(&model.gguf, 64 << 20)?; + let (attention_decode_graph, attention_decode_prepared) = prepare_attention_decode_graph( + &mut attention_decode_loaded.ctx, + &attention_decode_loaded.tensor_ids, + &attention_decode_spec, + 1, + MetalDeviceFeatures::default(), + ) + .map_err(|err| std::io::Error::other(format!("prepare_attention_decode_graph: {err}")))?; + let mut recurrent_decode_loaded = + recurrent_layout.allocate_and_load_with_extra(&model.gguf, 64 << 20)?; + let (recurrent_decode_graph, recurrent_decode_prepared) = + prepare_delta_net_recurrent_decode_graph( + &mut recurrent_decode_loaded.ctx, + &recurrent_decode_loaded.tensor_ids, + &recurrent_decode_spec, + 1, + MetalDeviceFeatures::default(), + ) + .map_err(|err| { + std::io::Error::other(format!("prepare_delta_net_recurrent_decode_graph: {err}")) + })?; + let mut moe_loaded = moe_layout.allocate_and_load_with_extra(&model.gguf, 64 << 20)?; + let (moe_graph, moe_prepared) = prepare_moe_ffn_graph( + &mut moe_loaded.ctx, + &moe_loaded.tensor_ids, + &moe_spec, + 1, + MetalDeviceFeatures::default(), + ) + .map_err(|err| std::io::Error::other(format!("prepare_moe_ffn_graph: {err}")))?; + + println!("path: {}", model.gguf.path.display()); + println!("version: {}", model.gguf.version); + println!("alignment: {}", model.gguf.alignment); + println!("data_offset: {}", model.gguf.data_offset); + println!("file_size: {}", model.gguf.file_size); + println!("n_kv: {}", model.gguf.kv.len()); + println!("n_tensors: {}", model.gguf.tensors.len()); + println!("architecture: {}", model.architecture.name()); + if let Some(name) = &model.general.name { + println!("general.name: {}", name); + } + if let Some(model_type) = &model.general.model_type { + println!("general.type: {}", model_type); + } + if let Some(file_type) = model.general.file_type { + println!("general.file_type: {}", file_type); + } + if let Some(qv) = model.general.quantization_version { + println!("general.quantization_version: {}", qv); + } + + if let Some(cfg) = &model.qwen35moe { + println!("qwen35moe.block_count: {}", cfg.block_count); + println!("qwen35moe.context_length: {}", cfg.context_length); + println!("qwen35moe.embedding_length: {}", cfg.embedding_length); + println!( + "qwen35moe.attention.head_count: {}", + cfg.attention_head_count + ); + println!( + "qwen35moe.attention.head_count_kv: {}", + cfg.attention_head_count_kv + ); + println!( + "qwen35moe.rope.dimension_sections: {:?}", + cfg.rope_dimension_sections + ); + println!("qwen35moe.rope.freq_base: {}", cfg.rope_freq_base); + println!("qwen35moe.expert_count: {}", cfg.expert_count); + println!("qwen35moe.expert_used_count: {}", cfg.expert_used_count); + println!( + "qwen35moe.full_attention_interval: {}", + cfg.full_attention_interval + ); + } + println!( + "qwen35moe.unique_tensors: {}", + qwen35moe.unique_tensor_count() + ); + println!( + "qwen35moe.total_tensor_bytes: {}", + qwen35moe.total_tensor_bytes() + ); + println!("qwen35moe.layout_total_bytes: {}", layout.total_bytes); + println!( + "execution.inventory_unique_tensors: {}", + plan.inventory.unique_tensor_count() + ); + println!( + "execution.inventory_total_tensor_bytes: {}", + plan.inventory.total_tensor_bytes() + ); + println!( + "execution.full_weight_bytes: {}", + plan.full_weights.total_bytes + ); + println!("execution.layer_count: {}", plan.layer_count()); + println!( + "execution.attention_layers: {}", + plan.inventory + .count_layers_with_role(ModelLayerRole::Attention) + ); + println!( + "execution.recurrent_layers: {}", + plan.inventory + .count_layers_with_role(ModelLayerRole::Recurrent) + ); + println!( + "qwen35moe.probe_layout_total_bytes: {}", + probe_layout.total_bytes + ); + println!("qwen35moe.probe_tensors: {}", probe_loaded.tensor_ids.len()); + println!("qwen35moe.attention_block_layer: {}", attention_layer); + println!( + "qwen35moe.attention_block_weight_bytes: {}", + attention_layout.total_bytes + ); + println!( + "qwen35moe.attention_block_tensors: {}", + attention_loaded.tensor_ids.len() + ); + println!("qwen35moe.recurrent_block_layer: {}", recurrent_layer); + println!( + "qwen35moe.recurrent_block_weight_bytes: {}", + recurrent_layout.total_bytes + ); + println!( + "qwen35moe.recurrent_block_tensors: {}", + recurrent_decode_loaded.tensor_ids.len() + ); + println!("qwen35moe.moe_ffn_layer: {}", moe_layer); + println!("qwen35moe.moe_ffn_weight_bytes: {}", moe_layout.total_bytes); + println!("qwen35moe.moe_ffn_tensors: {}", moe_loaded.tensor_ids.len()); + if let Some(cache_layout) = &cache_layout { + println!("qwen35moe.cache_total_bytes: {}", cache_layout.total_bytes); + println!( + "qwen35moe.cache_attention_layers: {}", + cache_layout.spec.attention_layers.len() + ); + println!( + "qwen35moe.cache_recurrent_layers: {}", + cache_layout.spec.recurrent_layers.len() + ); + } + if let Some(cache_layout) = &hybrid_decode_cache_layout { + println!( + "qwen35moe.hybrid_decode_cache_total_bytes: {}", + cache_layout.total_bytes + ); + } + println!( + "qwen35moe.tail_probe_weight_bytes: {}", + tail_probe_loaded.ctx.used_mem() + ); + println!( + "qwen35moe.tail_probe_nodes: {}", + tail_probe_graph.graph.n_nodes() + ); + println!( + "qwen35moe.tail_probe_main_buffer_size: {}", + tail_probe_prepared.main_buffer_size + ); + println!( + "qwen35moe.tail_probe_tail_buffer_size: {}", + tail_probe_prepared.tail_buffer_size + ); + println!( + "qwen35moe.attention_block_nodes: {}", + attention_graph.graph.n_nodes() + ); + println!( + "qwen35moe.attention_block_main_buffer_size: {}", + attention_prepared.main_buffer_size + ); + println!( + "qwen35moe.attention_block_tail_buffer_size: {}", + attention_prepared.tail_buffer_size + ); + println!( + "qwen35moe.attention_decode_nodes: {}", + attention_decode_graph.graph.n_nodes() + ); + println!( + "qwen35moe.attention_decode_main_buffer_size: {}", + attention_decode_prepared.main_buffer_size + ); + println!( + "qwen35moe.attention_decode_tail_buffer_size: {}", + attention_decode_prepared.tail_buffer_size + ); + println!( + "qwen35moe.recurrent_decode_nodes: {}", + recurrent_decode_graph.graph.n_nodes() + ); + println!( + "qwen35moe.recurrent_decode_main_buffer_size: {}", + recurrent_decode_prepared.main_buffer_size + ); + println!( + "qwen35moe.recurrent_decode_tail_buffer_size: {}", + recurrent_decode_prepared.tail_buffer_size + ); + println!("qwen35moe.moe_ffn_nodes: {}", moe_graph.graph.n_nodes()); + println!( + "qwen35moe.moe_ffn_main_buffer_size: {}", + moe_prepared.main_buffer_size + ); + println!( + "qwen35moe.moe_ffn_tail_buffer_size: {}", + moe_prepared.tail_buffer_size + ); + println!("metal.compat_available: {}", metal_available()); + println!("qwen35moe.layer_count: {}", qwen35moe.layers.len()); + let attention_layers = qwen35moe + .layers + .iter() + .filter(|layer| layer.kind.name() == "attention") + .count(); + let recurrent_layers = qwen35moe.layers.len() - attention_layers; + println!("qwen35moe.attention_layers: {}", attention_layers); + println!("qwen35moe.recurrent_layers: {}", recurrent_layers); + if let Some(layer0) = qwen35moe.layers.first() { + println!("qwen35moe.layer0.kind: {}", layer0.kind.name()); + println!( + "qwen35moe.layer0.moe_merged_gate_up: {}", + layer0.moe.uses_merged_gate_up() + ); + } + + if let Some(first) = model.gguf.tensors.first() { + println!( + "tensor.first: {} type={} dims={:?} size_bytes={} offset={}", + first.name, + first.tensor_type.name(), + first.dimensions, + first.size_bytes, + first.offset + ); + } + + if let Some(last) = model.gguf.tensors.last() { + println!( + "tensor.last: {} type={} dims={:?} size_bytes={} offset={}", + last.name, + last.tensor_type.name(), + last.dimensions, + last.size_bytes, + last.offset + ); + } + + println!( + "tensor.sample: {}", + model.gguf.tensor_summary("blk.0.attn_qkv.weight")? + ); + println!( + "tensor.sample: {}", + model + .gguf + .tensor_summary("blk.39.post_attention_norm.weight")? + ); + + if metal_available() { + let zero_embed = vec![0.0f32; plan.embedding_length as usize]; + let moe_embed = (0..plan.embedding_length as usize) + .map(|index| ((index % 31) as f32 - 15.0) * 0.01) + .collect::>(); + let token_ids = [0_i32]; + let positions = [0_i32]; + let hybrid_decode_extra = hybrid_decode_cache_layout + .as_ref() + .map(|layout| layout.total_bytes) + .unwrap_or(0) + .saturating_add(256 << 20); + let mut hybrid_decode_loaded = plan + .full_weights + .allocate_and_load_with_extra(&model.gguf, hybrid_decode_extra)?; + let (hybrid_decode_graph, hybrid_decode_prepared) = prepare_hybrid_decode_graph( + &mut hybrid_decode_loaded.ctx, + &hybrid_decode_loaded.tensor_ids, + &hybrid_decode_spec, + 1, + MetalDeviceFeatures::default(), + ) + .map_err(|err| std::io::Error::other(format!("prepare_hybrid_decode_graph: {err}")))?; + println!( + "qwen35moe.hybrid_decode_nodes: {}", + hybrid_decode_graph.graph.n_nodes() + ); + println!( + "qwen35moe.hybrid_decode_main_buffer_size: {}", + hybrid_decode_prepared.main_buffer_size + ); + println!( + "qwen35moe.hybrid_decode_tail_buffer_size: {}", + hybrid_decode_prepared.tail_buffer_size + ); + + match compile_attention_block_metal(&mut attention_loaded, &attention_spec, 1) { + Ok(compiled) => { + println!("attention_block_compiled.ok: true"); + match execute_attention_block_graph_metal_cached( + &compiled, + &attention_loaded, + LogitsProbeInput::TokenIds(&token_ids), + &positions, + ) { + Ok(run) => { + println!("attention_block_cached.hidden_size: {}", run.hidden_size); + println!("attention_block_cached.n_tokens: {}", run.n_tokens); + println!("attention_block_cached.hidden_len: {}", run.hidden.len()); + if let Some(first) = run.hidden.first() { + println!("attention_block_cached.hidden0: {}", first); + } + } + Err(err) => { + println!("attention_block_cached.error: {}", err); + } + } + } + Err(err) => { + println!("attention_block_compiled.error: {}", err); + } + } + + match compile_moe_ffn_metal(&mut moe_loaded, &moe_spec, 1) { + Ok(compiled) => { + println!("moe_ffn_compiled.ok: true"); + match execute_moe_ffn_graph_metal_cached( + &compiled, + &moe_loaded, + LogitsProbeInput::EmbeddingsF32 { + data: &moe_embed, + n_tokens: 1, + }, + ) { + Ok(run) => { + println!("moe_ffn.hidden_size: {}", run.hidden_size); + println!("moe_ffn.n_tokens: {}", run.n_tokens); + println!("moe_ffn.hidden_len: {}", run.hidden.len()); + if let Some(first) = run.hidden.first() { + println!("moe_ffn.hidden0: {}", first); + } + println!( + "moe_ffn.selected_experts: {:?}", + &run.selected_experts + [..run.expert_used_count.min(run.selected_experts.len())] + ); + } + Err(err) => { + println!("moe_ffn.error: {}", err); + } + } + } + Err(err) => { + println!("moe_ffn_compiled.error: {}", err); + } + } + + match compile_attention_decode_metal( + &mut attention_decode_loaded, + &attention_decode_spec, + 1, + ) { + Ok(compiled) => { + println!("attention_decode_compiled.ok: true"); + match execute_attention_decode_graph_metal_cached( + &compiled, + &mut attention_decode_loaded, + LogitsProbeInput::TokenIds(&[0_i32]), + &[0_i32], + 1, + ) { + Ok(run) => { + println!("attention_decode.step0.hidden_size: {}", run.hidden_size); + println!("attention_decode.step0.n_tokens: {}", run.n_tokens); + println!("attention_decode.step0.hidden_len: {}", run.hidden.len()); + if let Some(first) = run.hidden.first() { + println!("attention_decode.step0.hidden0: {}", first); + } + } + Err(err) => { + println!("attention_decode.step0.error: {}", err); + } + } + match execute_attention_decode_graph_metal_cached( + &compiled, + &mut attention_decode_loaded, + LogitsProbeInput::TokenIds(&[1_i32]), + &[1_i32], + 2, + ) { + Ok(run) => { + println!("attention_decode.step1.hidden_size: {}", run.hidden_size); + println!("attention_decode.step1.n_tokens: {}", run.n_tokens); + println!("attention_decode.step1.hidden_len: {}", run.hidden.len()); + if let Some(first) = run.hidden.first() { + println!("attention_decode.step1.hidden0: {}", first); + } + } + Err(err) => { + println!("attention_decode.step1.error: {}", err); + } + } + } + Err(err) => { + println!("attention_decode_compiled.error: {}", err); + } + } + + match compile_delta_net_recurrent_decode_metal( + &mut recurrent_decode_loaded, + &recurrent_decode_spec, + 1, + ) { + Ok(compiled) => { + println!("recurrent_decode_compiled.ok: true"); + match execute_delta_net_recurrent_decode_graph_metal_cached( + &compiled, + &mut recurrent_decode_loaded, + LogitsProbeInput::TokenIds(&[0_i32]), + ) { + Ok(run) => { + println!("recurrent_decode.step0.hidden_size: {}", run.hidden_size); + println!("recurrent_decode.step0.n_tokens: {}", run.n_tokens); + println!("recurrent_decode.step0.hidden_len: {}", run.hidden.len()); + if let Some(first) = run.hidden.first() { + println!("recurrent_decode.step0.hidden0: {}", first); + if first.is_nan() { + dump_recurrent_debug( + &model.gguf, + &recurrent_layout, + &recurrent_decode_spec, + &[0_i32], + &[ + "recur_decode.input_norm", + "recur_decode.beta", + "recur_decode.gate", + "recur_decode.conv_output", + "recur_decode.delta", + "recur_decode.output_view", + "recur_decode.output_rms", + "recur_decode.output_norm", + "recur_decode.z_silu", + "recur_decode.gated_output", + "recur_decode.final_output", + "recur_decode.output", + ], + )?; + } + } + } + Err(err) => { + println!("recurrent_decode.step0.error: {}", err); + } + } + match execute_delta_net_recurrent_decode_graph_metal_cached( + &compiled, + &mut recurrent_decode_loaded, + LogitsProbeInput::TokenIds(&[1_i32]), + ) { + Ok(run) => { + println!("recurrent_decode.step1.hidden_size: {}", run.hidden_size); + println!("recurrent_decode.step1.n_tokens: {}", run.n_tokens); + println!("recurrent_decode.step1.hidden_len: {}", run.hidden.len()); + if let Some(first) = run.hidden.first() { + println!("recurrent_decode.step1.hidden0: {}", first); + } + } + Err(err) => { + println!("recurrent_decode.step1.error: {}", err); + } + } + } + Err(err) => { + println!("recurrent_decode_compiled.error: {}", err); + } + } + + let hybrid_compile_started = Instant::now(); + match compile_hybrid_decode_metal(&mut hybrid_decode_loaded, &hybrid_decode_spec, 1) { + Ok(compiled) => { + println!("hybrid_decode_compiled.ok: true"); + println!( + "hybrid_decode.compile_ms: {}", + hybrid_compile_started.elapsed().as_millis() + ); + let step0_started = Instant::now(); + match execute_hybrid_decode_graph_metal_cached( + &compiled, + &mut hybrid_decode_loaded, + LogitsProbeInput::TokenIds(&[0_i32]), + &[0_i32], + 1, + ) { + Ok(run) => { + println!( + "hybrid_decode.step0.ms: {}", + step0_started.elapsed().as_millis() + ); + println!("hybrid_decode.step0.hidden_size: {}", run.hidden_size); + println!("hybrid_decode.step0.n_tokens: {}", run.n_tokens); + println!("hybrid_decode.step0.hidden_len: {}", run.hidden.len()); + println!("hybrid_decode.step0.vocab_size: {}", run.vocab_size); + println!("hybrid_decode.step0.logits_len: {}", run.logits.len()); + if let Some(first) = run.hidden.first() { + println!("hybrid_decode.step0.hidden0: {}", first); + } + if let Some(first) = run.logits.first() { + println!("hybrid_decode.step0.logit0: {}", first); + } + if let Some((layer, experts)) = run.selected_experts.first() { + println!("hybrid_decode.step0.layer{}_experts: {:?}", layer, experts); + } + } + Err(err) => { + println!("hybrid_decode.step0.error: {}", err); + } + } + let step1_started = Instant::now(); + match execute_hybrid_decode_graph_metal_cached( + &compiled, + &mut hybrid_decode_loaded, + LogitsProbeInput::TokenIds(&[1_i32]), + &[1_i32], + 2, + ) { + Ok(run) => { + println!( + "hybrid_decode.step1.ms: {}", + step1_started.elapsed().as_millis() + ); + println!("hybrid_decode.step1.hidden_size: {}", run.hidden_size); + println!("hybrid_decode.step1.n_tokens: {}", run.n_tokens); + println!("hybrid_decode.step1.hidden_len: {}", run.hidden.len()); + println!("hybrid_decode.step1.vocab_size: {}", run.vocab_size); + println!("hybrid_decode.step1.logits_len: {}", run.logits.len()); + if let Some(first) = run.hidden.first() { + println!("hybrid_decode.step1.hidden0: {}", first); + } + if let Some(first) = run.logits.first() { + println!("hybrid_decode.step1.logit0: {}", first); + } + if let Some((layer, experts)) = run.selected_experts.first() { + println!("hybrid_decode.step1.layer{}_experts: {:?}", layer, experts); + } + let bench_started = Instant::now(); + let mut bench_last_hidden0 = None; + let mut bench_last_logit0 = None; + let mut bench_steps = 0usize; + for step in 2_i32..8_i32 { + let run = execute_hybrid_decode_graph_metal_cached( + &compiled, + &mut hybrid_decode_loaded, + LogitsProbeInput::TokenIds(&[step]), + &[step], + usize::try_from(step + 1).unwrap(), + )?; + bench_steps += 1; + bench_last_hidden0 = run.hidden.first().copied(); + bench_last_logit0 = run.logits.first().copied(); + } + let bench_elapsed = bench_started.elapsed(); + println!("hybrid_decode.bench_steps: {}", bench_steps); + println!("hybrid_decode.bench_ms: {}", bench_elapsed.as_millis()); + println!( + "hybrid_decode.bench_tok_s: {:.3}", + bench_steps as f64 / bench_elapsed.as_secs_f64() + ); + if let Some(value) = bench_last_hidden0 { + println!("hybrid_decode.bench_last_hidden0: {}", value); + } + if let Some(value) = bench_last_logit0 { + println!("hybrid_decode.bench_last_logit0: {}", value); + } + } + Err(err) => { + println!("hybrid_decode.step1.error: {}", err); + } + } + } + Err(err) => { + println!("hybrid_decode_compiled.error: {}", err); + } + } + + match compile_logits_probe_metal( + &mut tail_probe_loaded, + &plan.tail_probe.spec, + GraphBatch { + n_tokens: 1, + n_outputs: 1, + }, + ) { + Ok(compiled) => { + println!("tail_probe_compiled.ok: true"); + match execute_logits_probe_graph_metal_cached( + &compiled, + &tail_probe_loaded, + LogitsProbeInput::EmbeddingsF32 { + data: &zero_embed, + n_tokens: 1, + }, + &[0], + ) { + Ok(run) => { + println!("tail_probe_cached.outputs: {}", run.n_outputs); + println!("tail_probe_cached.vocab_size: {}", run.vocab_size); + println!("tail_probe_cached.logits_len: {}", run.logits.len()); + if let Some(first) = run.logits.first() { + println!("tail_probe_cached.logit0: {}", first); + } + } + Err(err) => { + println!("tail_probe_cached.error: {}", err); + } + } + } + Err(err) => { + println!("tail_probe_compiled.error: {}", err); + } + } + + match execute_logits_probe_graph_metal( + &mut tail_probe_loaded, + &plan.tail_probe.spec, + LogitsProbeInput::EmbeddingsF32 { + data: &zero_embed, + n_tokens: 1, + }, + &[0], + ) { + Ok(run) => { + println!("tail_probe_graph.outputs: {}", run.n_outputs); + println!("tail_probe_graph.vocab_size: {}", run.vocab_size); + println!("tail_probe_graph.logits_len: {}", run.logits.len()); + if let Some(first) = run.logits.first() { + println!("tail_probe_graph.logit0: {}", first); + } + } + Err(err) => { + println!("tail_probe_graph.error: {}", err); + } + } + + match execute_logits_probe_metal( + &tail_probe_loaded, + &plan.tail_probe.spec, + LogitsProbeInput::EmbeddingsF32 { + data: &zero_embed, + n_tokens: 1, + }, + &[0], + ) { + Ok(run) => { + println!("tail_probe_eager.outputs: {}", run.n_outputs); + println!("tail_probe_eager.vocab_size: {}", run.vocab_size); + println!("tail_probe_eager.logits_len: {}", run.logits.len()); + if let Some(first) = run.logits.first() { + println!("tail_probe_eager.logit0: {}", first); + } + } + Err(err) => { + println!("tail_probe_eager.error: {}", err); + } + } + } + + Ok(()) +} + +fn qwen35moe_probe_layout( + qwen35moe: &makepad_llama::Qwen35MoeTensors, +) -> Result> { + let recurrent = qwen35moe + .layers + .iter() + .find_map(|layer| layer.recurrent.as_ref().map(|recurrent| (layer, recurrent))) + .ok_or_else(|| std::io::Error::other("qwen35moe probe could not find a recurrent layer"))?; + let attention = qwen35moe + .layers + .iter() + .find_map(|layer| layer.attention.as_ref().map(|attention| (layer, attention))) + .ok_or_else(|| { + std::io::Error::other("qwen35moe probe could not find an attention layer") + })?; + + Ok(makepad_llama::GgufWeightLayout::from_tensors(vec![ + qwen35moe.globals.output_norm.clone(), + recurrent.0.attn_norm.clone(), + recurrent.0.post_attention_norm.clone(), + recurrent.1.ssm_dt.clone(), + recurrent.1.ssm_norm.clone(), + attention.0.attn_norm.clone(), + attention.0.post_attention_norm.clone(), + attention.1.wq.clone(), + attention.1.attn_q_norm.clone(), + attention.1.wo.clone(), + ])?) +} + +fn dump_recurrent_debug( + gguf: &makepad_llama::GgufFile, + layout: &makepad_llama::GgufWeightLayout, + spec: &makepad_llama::DeltaNetRecurrentDecodeSpec, + token_ids: &[i32], + tensor_names: &[&str], +) -> Result<(), Box> { + let mut loaded = layout.allocate_and_load_with_extra(gguf, 64 << 20)?; + let (_, prepared) = prepare_delta_net_recurrent_decode_graph( + &mut loaded.ctx, + &loaded.tensor_ids, + spec, + token_ids.len(), + MetalDeviceFeatures::default(), + )?; + let runtime = MetalRuntime::new()?; + let compiled = compile_prepared_graph( + &runtime, + &loaded.ctx, + &prepared, + BufferStorageMode::Private, + BufferStorageMode::Private, + )?; + let input_id = loaded + .ctx + .get_tensor("recur_decode.inp_tokens") + .ok_or_else(|| std::io::Error::other("missing recur_decode.inp_tokens"))?; + let output_ids = tensor_names + .iter() + .filter_map(|name| { + loaded + .ctx + .get_tensor(name) + .map(|id| ((*name).to_string(), id)) + }) + .collect::>(); + + let execution = execute_compiled_graph( + &runtime, + &loaded.ctx, + &compiled, + &[MetalGraphTensorWrite { + tensor_id: input_id, + bytes: unsafe { + std::slice::from_raw_parts( + token_ids.as_ptr() as *const u8, + std::mem::size_of_val(token_ids), + ) + }, + }], + &output_ids.iter().map(|(_, id)| *id).collect::>(), + )?; + + for (name, id) in output_ids { + if let Some(tensor) = loaded.ctx.tensor(id) { + println!( + "recurrent_debug.{}.meta: ty={} rank={} ne={:?} nb={:?} contiguous={} view={}", + name, + tensor.desc.ty.name(), + tensor.desc.layout.rank(), + &tensor.ne[..tensor.desc.layout.rank()], + &tensor.nb[..tensor.desc.layout.rank()], + tensor.is_contiguous(), + tensor.is_view(), + ); + } + if let Some(bytes) = execution.outputs.get(&id) { + println!("recurrent_debug.{}.bytes: {}", name, bytes.len()); + let mut first = f32::NAN; + let mut nan_count = 0usize; + for (index, chunk) in bytes.chunks_exact(4).enumerate() { + let value = f32::from_le_bytes([chunk[0], chunk[1], chunk[2], chunk[3]]); + if index == 0 { + first = value; + } + if value.is_nan() { + nan_count += 1; + } + } + println!("recurrent_debug.{}.first: {}", name, first); + println!("recurrent_debug.{}.nan_count: {}", name, nan_count); + } + } + + Ok(()) +} diff --git a/libs/llama/src/error.rs b/libs/llama/src/error.rs new file mode 100644 index 0000000000..3ca04ab03c --- /dev/null +++ b/libs/llama/src/error.rs @@ -0,0 +1,38 @@ +use std::fmt::{Display, Formatter}; + +#[derive(Debug)] +pub enum LlamaError { + Io(std::io::Error), + Format(String), + Unsupported(String), +} + +impl LlamaError { + pub fn format(msg: impl Into) -> Self { + Self::Format(msg.into()) + } + + pub fn unsupported(msg: impl Into) -> Self { + Self::Unsupported(msg.into()) + } +} + +impl Display for LlamaError { + fn fmt(&self, f: &mut Formatter<'_>) -> std::fmt::Result { + match self { + Self::Io(err) => write!(f, "io error: {}", err), + Self::Format(msg) => write!(f, "gguf format error: {}", msg), + Self::Unsupported(msg) => write!(f, "unsupported: {}", msg), + } + } +} + +impl std::error::Error for LlamaError {} + +impl From for LlamaError { + fn from(value: std::io::Error) -> Self { + Self::Io(value) + } +} + +pub type Result = std::result::Result; diff --git a/libs/llama/src/gguf.rs b/libs/llama/src/gguf.rs new file mode 100644 index 0000000000..2355d9941f --- /dev/null +++ b/libs/llama/src/gguf.rs @@ -0,0 +1,706 @@ +use crate::error::{LlamaError, Result}; +use makepad_ggml::{block_elements, block_size, TensorType}; +use std::borrow::Cow; +use std::collections::HashMap; +use std::fmt::{Debug, Formatter}; +use std::fs::File; +use std::io::{BufReader, Read, Seek, SeekFrom}; +use std::path::{Path, PathBuf}; + +const GGUF_MAGIC: &[u8; 4] = b"GGUF"; +const GGUF_VERSION: u32 = 3; +const GGUF_DEFAULT_ALIGNMENT: u64 = 32; +const GGUF_MAX_STRING_LENGTH: u64 = 1024 * 1024 * 1024; +const GGUF_MAX_ARRAY_ELEMENTS: u64 = 1024 * 1024 * 1024; +const GGUF_KEY_GENERAL_ALIGNMENT: &str = "general.alignment"; + +#[repr(i32)] +#[derive(Clone, Copy, Debug, Eq, PartialEq)] +pub enum GgufType { + Uint8 = 0, + Int8 = 1, + Uint16 = 2, + Int16 = 3, + Uint32 = 4, + Int32 = 5, + Float32 = 6, + Bool = 7, + String = 8, + Array = 9, + Uint64 = 10, + Int64 = 11, + Float64 = 12, +} + +impl GgufType { + pub fn from_i32(value: i32) -> Option { + Some(match value { + 0 => Self::Uint8, + 1 => Self::Int8, + 2 => Self::Uint16, + 3 => Self::Int16, + 4 => Self::Uint32, + 5 => Self::Int32, + 6 => Self::Float32, + 7 => Self::Bool, + 8 => Self::String, + 9 => Self::Array, + 10 => Self::Uint64, + 11 => Self::Int64, + 12 => Self::Float64, + _ => return None, + }) + } + + pub fn name(self) -> &'static str { + match self { + Self::Uint8 => "u8", + Self::Int8 => "i8", + Self::Uint16 => "u16", + Self::Int16 => "i16", + Self::Uint32 => "u32", + Self::Int32 => "i32", + Self::Float32 => "f32", + Self::Bool => "bool", + Self::String => "str", + Self::Array => "arr", + Self::Uint64 => "u64", + Self::Int64 => "i64", + Self::Float64 => "f64", + } + } +} + +#[derive(Clone, Eq, Hash, PartialEq)] +pub struct GgufString(Vec); + +impl GgufString { + pub fn new(bytes: Vec) -> Self { + Self(bytes) + } + + pub fn as_bytes(&self) -> &[u8] { + &self.0 + } + + pub fn to_string_lossy(&self) -> Cow<'_, str> { + String::from_utf8_lossy(&self.0) + } + + pub fn try_utf8(&self) -> Result<&str> { + std::str::from_utf8(&self.0) + .map_err(|err| LlamaError::format(format!("invalid utf-8 gguf string: {}", err))) + } +} + +impl Debug for GgufString { + fn fmt(&self, f: &mut Formatter<'_>) -> std::fmt::Result { + write!(f, "{:?}", self.to_string_lossy()) + } +} + +#[derive(Clone, Debug)] +pub enum GgufArray { + Uint8(Vec), + Int8(Vec), + Uint16(Vec), + Int16(Vec), + Uint32(Vec), + Int32(Vec), + Float32(Vec), + Bool(Vec), + String(Vec), + Uint64(Vec), + Int64(Vec), + Float64(Vec), +} + +impl GgufArray { + pub fn len(&self) -> usize { + match self { + Self::Uint8(v) => v.len(), + Self::Int8(v) => v.len(), + Self::Uint16(v) => v.len(), + Self::Int16(v) => v.len(), + Self::Uint32(v) => v.len(), + Self::Int32(v) => v.len(), + Self::Float32(v) => v.len(), + Self::Bool(v) => v.len(), + Self::String(v) => v.len(), + Self::Uint64(v) => v.len(), + Self::Int64(v) => v.len(), + Self::Float64(v) => v.len(), + } + } + + pub fn element_type(&self) -> GgufType { + match self { + Self::Uint8(_) => GgufType::Uint8, + Self::Int8(_) => GgufType::Int8, + Self::Uint16(_) => GgufType::Uint16, + Self::Int16(_) => GgufType::Int16, + Self::Uint32(_) => GgufType::Uint32, + Self::Int32(_) => GgufType::Int32, + Self::Float32(_) => GgufType::Float32, + Self::Bool(_) => GgufType::Bool, + Self::String(_) => GgufType::String, + Self::Uint64(_) => GgufType::Uint64, + Self::Int64(_) => GgufType::Int64, + Self::Float64(_) => GgufType::Float64, + } + } + + pub fn as_u32_slice(&self) -> Option<&[u32]> { + match self { + Self::Uint32(v) => Some(v), + _ => None, + } + } +} + +#[derive(Clone, Debug)] +pub enum GgufValue { + Uint8(u8), + Int8(i8), + Uint16(u16), + Int16(i16), + Uint32(u32), + Int32(i32), + Float32(f32), + Bool(bool), + String(GgufString), + Array(GgufArray), + Uint64(u64), + Int64(i64), + Float64(f64), +} + +impl GgufValue { + pub fn value_type(&self) -> GgufType { + match self { + Self::Uint8(_) => GgufType::Uint8, + Self::Int8(_) => GgufType::Int8, + Self::Uint16(_) => GgufType::Uint16, + Self::Int16(_) => GgufType::Int16, + Self::Uint32(_) => GgufType::Uint32, + Self::Int32(_) => GgufType::Int32, + Self::Float32(_) => GgufType::Float32, + Self::Bool(_) => GgufType::Bool, + Self::String(_) => GgufType::String, + Self::Array(_) => GgufType::Array, + Self::Uint64(_) => GgufType::Uint64, + Self::Int64(_) => GgufType::Int64, + Self::Float64(_) => GgufType::Float64, + } + } + + pub fn as_u32(&self) -> Option { + match self { + Self::Uint32(v) => Some(*v), + _ => None, + } + } + + pub fn as_u64(&self) -> Option { + match self { + Self::Uint64(v) => Some(*v), + Self::Uint32(v) => Some((*v).into()), + _ => None, + } + } + + pub fn as_f32(&self) -> Option { + match self { + Self::Float32(v) => Some(*v), + _ => None, + } + } + + pub fn as_string(&self) -> Option<&GgufString> { + match self { + Self::String(v) => Some(v), + _ => None, + } + } + + pub fn as_array(&self) -> Option<&GgufArray> { + match self { + Self::Array(v) => Some(v), + _ => None, + } + } +} + +#[derive(Clone, Debug)] +pub struct GgufKeyValue { + pub key: String, + pub value: GgufValue, +} + +#[derive(Clone, Debug)] +pub struct GgufTensorInfo { + pub name: String, + pub dimensions: Vec, + pub tensor_type: TensorType, + pub offset: u64, + pub size_bytes: u64, +} + +impl GgufTensorInfo { + pub fn absolute_offset(&self, data_offset: u64) -> Result { + data_offset.checked_add(self.offset).ok_or_else(|| { + LlamaError::format(format!( + "overflow computing absolute tensor offset for '{}'", + self.name + )) + }) + } +} + +#[derive(Clone, Debug)] +pub struct GgufFile { + pub path: PathBuf, + pub file_size: u64, + pub version: u32, + pub alignment: u64, + pub data_offset: u64, + pub kv: Vec, + pub tensors: Vec, + kv_index: HashMap, + tensor_index: HashMap, +} + +impl GgufFile { + pub fn open(path: impl AsRef) -> Result { + let path = path.as_ref().to_path_buf(); + let file = File::open(&path)?; + let file_size = file.metadata()?.len(); + let mut reader = BufReader::new(file); + + let mut magic = [0u8; 4]; + reader.read_exact(&mut magic)?; + if &magic != GGUF_MAGIC { + return Err(LlamaError::format(format!( + "invalid magic {:?}, expected {:?}", + magic, GGUF_MAGIC + ))); + } + + let version = read_u32(&mut reader)?; + if version == 0 || version > GGUF_VERSION { + return Err(LlamaError::unsupported(format!( + "unsupported gguf version {}", + version + ))); + } + + let n_tensors = read_i64(&mut reader)?; + let n_kv = read_i64(&mut reader)?; + if n_tensors < 0 || n_kv < 0 { + return Err(LlamaError::format(format!( + "negative counts in header: n_tensors={}, n_kv={}", + n_tensors, n_kv + ))); + } + + let mut kv = Vec::with_capacity(n_kv as usize); + let mut kv_index = HashMap::with_capacity(n_kv as usize); + + for _ in 0..n_kv as usize { + let key = read_key_string(&mut reader)?; + let value_type_raw = read_i32(&mut reader)?; + let value_type = GgufType::from_i32(value_type_raw).ok_or_else(|| { + LlamaError::format(format!("invalid gguf value type {}", value_type_raw)) + })?; + + let value = read_value(&mut reader, value_type)?; + kv_index.insert(key.clone(), kv.len()); + kv.push(GgufKeyValue { key, value }); + } + + let alignment = kv + .iter() + .find(|entry| entry.key == GGUF_KEY_GENERAL_ALIGNMENT) + .and_then(|entry| entry.value.as_u32()) + .map(u64::from) + .unwrap_or(GGUF_DEFAULT_ALIGNMENT); + + if alignment == 0 { + return Err(LlamaError::format("invalid zero gguf alignment")); + } + + let mut tensors = Vec::with_capacity(n_tensors as usize); + let mut tensor_index = HashMap::with_capacity(n_tensors as usize); + + for _ in 0..n_tensors as usize { + let name = read_key_string(&mut reader)?; + let n_dims = read_u32(&mut reader)?; + if n_dims == 0 || n_dims > 4 { + return Err(LlamaError::format(format!( + "invalid tensor rank {} for '{}'", + n_dims, name + ))); + } + + let mut dimensions = Vec::with_capacity(n_dims as usize); + for _ in 0..n_dims as usize { + let dim = read_u64_from_i64(&mut reader)?; + dimensions.push(dim); + } + + let ggml_type = read_i32(&mut reader)?; + let tensor_type = TensorType::from_ggml_type(ggml_type as u32).ok_or_else(|| { + LlamaError::unsupported(format!( + "unsupported ggml tensor type {} for '{}'", + ggml_type, name + )) + })?; + let offset = read_u64(&mut reader)?; + let size_bytes = ggml_tensor_size_bytes(tensor_type, &dimensions)?; + + tensor_index.insert(name.clone(), tensors.len()); + tensors.push(GgufTensorInfo { + name, + dimensions, + tensor_type, + offset, + size_bytes, + }); + } + + let data_offset = align_up(reader.stream_position()?, alignment)?; + + for tensor in &tensors { + let start = tensor.absolute_offset(data_offset)?; + let end = start.checked_add(tensor.size_bytes).ok_or_else(|| { + LlamaError::format(format!( + "overflow computing tensor end for '{}'", + tensor.name + )) + })?; + if end > file_size { + return Err(LlamaError::format(format!( + "tensor '{}' extends beyond file bounds: end={} file_size={}", + tensor.name, end, file_size + ))); + } + } + + Ok(Self { + path, + file_size, + version, + alignment, + data_offset, + kv, + tensors, + kv_index, + tensor_index, + }) + } + + pub fn get_value(&self, key: &str) -> Option<&GgufValue> { + self.kv_index + .get(key) + .and_then(|idx| self.kv.get(*idx)) + .map(|entry| &entry.value) + } + + pub fn require_value(&self, key: &str) -> Result<&GgufValue> { + self.get_value(key) + .ok_or_else(|| LlamaError::format(format!("missing required gguf key '{}'", key))) + } + + pub fn get_tensor(&self, name: &str) -> Option<&GgufTensorInfo> { + self.tensor_index + .get(name) + .and_then(|idx| self.tensors.get(*idx)) + } + + pub fn require_tensor(&self, name: &str) -> Result<&GgufTensorInfo> { + self.get_tensor(name) + .ok_or_else(|| LlamaError::format(format!("missing tensor '{}'", name))) + } + + pub fn read_tensor_prefix(&self, name: &str, len: usize) -> Result> { + let tensor = self.require_tensor(name)?; + let mut file = File::open(&self.path)?; + let start = tensor.absolute_offset(self.data_offset)?; + let to_read = len.min(tensor.size_bytes as usize); + file.seek(SeekFrom::Start(start))?; + let mut out = vec![0u8; to_read]; + file.read_exact(&mut out)?; + Ok(out) + } + + pub fn read_tensor_bytes(&self, name: &str) -> Result> { + let tensor = self.require_tensor(name)?; + let len = usize::try_from(tensor.size_bytes).map_err(|_| { + LlamaError::format(format!( + "tensor '{}' size {} does not fit in usize", + name, tensor.size_bytes + )) + })?; + let mut out = vec![0u8; len]; + self.read_tensor_into(name, &mut out)?; + Ok(out) + } + + pub fn read_tensor_into(&self, name: &str, dst: &mut [u8]) -> Result<()> { + let tensor = self.require_tensor(name)?; + let expected = usize::try_from(tensor.size_bytes).map_err(|_| { + LlamaError::format(format!( + "tensor '{}' size {} does not fit in usize", + name, tensor.size_bytes + )) + })?; + if dst.len() != expected { + return Err(LlamaError::format(format!( + "tensor '{}' byte length mismatch: dst={} expected={}", + name, + dst.len(), + expected + ))); + } + + let mut file = File::open(&self.path)?; + let start = tensor.absolute_offset(self.data_offset)?; + file.seek(SeekFrom::Start(start))?; + file.read_exact(dst)?; + Ok(()) + } + + pub fn tensor_summary(&self, name: &str) -> Result { + let tensor = self.require_tensor(name)?; + Ok(format!( + "{} type={} dims={:?} size_bytes={} offset={}", + tensor.name, + tensor.tensor_type.name(), + tensor.dimensions, + tensor.size_bytes, + tensor.offset + )) + } +} + +fn align_up(value: u64, alignment: u64) -> Result { + let rem = value % alignment; + if rem == 0 { + return Ok(value); + } + value + .checked_add(alignment - rem) + .ok_or_else(|| LlamaError::format("overflow while aligning gguf data offset")) +} + +fn ggml_tensor_size_bytes(tensor_type: TensorType, dimensions: &[u64]) -> Result { + if dimensions.is_empty() { + return Err(LlamaError::format( + "tensor must have at least one dimension", + )); + } + + let block_elems = block_elements(tensor_type.ggml_type()) as u64; + let block_bytes = block_size(tensor_type.ggml_type()) as u64; + let row_blocks = dimensions[0] + .checked_add(block_elems - 1) + .ok_or_else(|| LlamaError::format("overflow computing row blocks"))? + / block_elems; + let row_size = row_blocks + .checked_mul(block_bytes) + .ok_or_else(|| LlamaError::format("overflow computing row size"))?; + + dimensions[1..].iter().try_fold(row_size, |acc, &dim| { + acc.checked_mul(dim) + .ok_or_else(|| LlamaError::format("overflow computing tensor size")) + }) +} + +fn read_value(reader: &mut impl Read, value_type: GgufType) -> Result { + Ok(match value_type { + GgufType::Uint8 => GgufValue::Uint8(read_u8(reader)?), + GgufType::Int8 => GgufValue::Int8(read_i8(reader)?), + GgufType::Uint16 => GgufValue::Uint16(read_u16(reader)?), + GgufType::Int16 => GgufValue::Int16(read_i16(reader)?), + GgufType::Uint32 => GgufValue::Uint32(read_u32(reader)?), + GgufType::Int32 => GgufValue::Int32(read_i32(reader)?), + GgufType::Float32 => GgufValue::Float32(read_f32(reader)?), + GgufType::Bool => GgufValue::Bool(read_bool(reader)?), + GgufType::String => GgufValue::String(read_value_string(reader)?), + GgufType::Array => { + let array_type_raw = read_i32(reader)?; + let array_type = GgufType::from_i32(array_type_raw).ok_or_else(|| { + LlamaError::format(format!("invalid gguf array type {}", array_type_raw)) + })?; + if array_type == GgufType::Array { + return Err(LlamaError::unsupported( + "nested gguf arrays are not supported", + )); + } + let count = read_u64(reader)?; + if count > GGUF_MAX_ARRAY_ELEMENTS { + return Err(LlamaError::format(format!( + "gguf array too large: {} elements", + count + ))); + } + GgufValue::Array(read_array(reader, array_type, count as usize)?) + } + GgufType::Uint64 => GgufValue::Uint64(read_u64(reader)?), + GgufType::Int64 => GgufValue::Int64(read_i64(reader)?), + GgufType::Float64 => GgufValue::Float64(read_f64(reader)?), + }) +} + +fn read_array(reader: &mut impl Read, array_type: GgufType, count: usize) -> Result { + Ok(match array_type { + GgufType::Uint8 => GgufArray::Uint8(read_n(reader, count, read_u8)?), + GgufType::Int8 => GgufArray::Int8(read_n(reader, count, read_i8)?), + GgufType::Uint16 => GgufArray::Uint16(read_n(reader, count, read_u16)?), + GgufType::Int16 => GgufArray::Int16(read_n(reader, count, read_i16)?), + GgufType::Uint32 => GgufArray::Uint32(read_n(reader, count, read_u32)?), + GgufType::Int32 => GgufArray::Int32(read_n(reader, count, read_i32)?), + GgufType::Float32 => GgufArray::Float32(read_n(reader, count, read_f32)?), + GgufType::Bool => GgufArray::Bool(read_n(reader, count, read_bool)?), + GgufType::String => GgufArray::String(read_n(reader, count, read_value_string)?), + GgufType::Uint64 => GgufArray::Uint64(read_n(reader, count, read_u64)?), + GgufType::Int64 => GgufArray::Int64(read_n(reader, count, read_i64)?), + GgufType::Float64 => GgufArray::Float64(read_n(reader, count, read_f64)?), + GgufType::Array => { + return Err(LlamaError::unsupported( + "nested gguf arrays are not supported", + )) + } + }) +} + +fn read_n( + reader: &mut R, + count: usize, + mut read_one: impl FnMut(&mut R) -> Result, +) -> Result> { + let mut out = Vec::with_capacity(count); + for _ in 0..count { + out.push(read_one(reader)?); + } + Ok(out) +} + +fn read_key_string(reader: &mut impl Read) -> Result { + let bytes = read_value_string(reader)?; + bytes + .try_utf8() + .map(|s| s.to_owned()) + .map_err(|err| LlamaError::format(format!("invalid utf-8 gguf key: {}", err))) +} + +fn read_value_string(reader: &mut impl Read) -> Result { + let len = read_u64(reader)?; + if len > GGUF_MAX_STRING_LENGTH { + return Err(LlamaError::format(format!( + "gguf string too large: {} bytes", + len + ))); + } + let mut bytes = vec![0u8; len as usize]; + reader.read_exact(&mut bytes)?; + Ok(GgufString::new(bytes)) +} + +fn read_bool(reader: &mut impl Read) -> Result { + Ok(read_i8(reader)? != 0) +} + +fn read_u8(reader: &mut impl Read) -> Result { + let mut buf = [0u8; 1]; + reader.read_exact(&mut buf)?; + Ok(buf[0]) +} + +fn read_i8(reader: &mut impl Read) -> Result { + Ok(read_u8(reader)? as i8) +} + +fn read_u16(reader: &mut impl Read) -> Result { + let mut buf = [0u8; 2]; + reader.read_exact(&mut buf)?; + Ok(u16::from_le_bytes(buf)) +} + +fn read_i16(reader: &mut impl Read) -> Result { + let mut buf = [0u8; 2]; + reader.read_exact(&mut buf)?; + Ok(i16::from_le_bytes(buf)) +} + +fn read_u32(reader: &mut impl Read) -> Result { + let mut buf = [0u8; 4]; + reader.read_exact(&mut buf)?; + Ok(u32::from_le_bytes(buf)) +} + +fn read_i32(reader: &mut impl Read) -> Result { + let mut buf = [0u8; 4]; + reader.read_exact(&mut buf)?; + Ok(i32::from_le_bytes(buf)) +} + +fn read_f32(reader: &mut impl Read) -> Result { + let mut buf = [0u8; 4]; + reader.read_exact(&mut buf)?; + Ok(f32::from_le_bytes(buf)) +} + +fn read_u64(reader: &mut impl Read) -> Result { + let mut buf = [0u8; 8]; + reader.read_exact(&mut buf)?; + Ok(u64::from_le_bytes(buf)) +} + +fn read_i64(reader: &mut impl Read) -> Result { + let mut buf = [0u8; 8]; + reader.read_exact(&mut buf)?; + Ok(i64::from_le_bytes(buf)) +} + +fn read_u64_from_i64(reader: &mut impl Read) -> Result { + let value = read_i64(reader)?; + if value < 0 { + return Err(LlamaError::format(format!( + "negative tensor dimension {}", + value + ))); + } + Ok(value as u64) +} + +fn read_f64(reader: &mut impl Read) -> Result { + let mut buf = [0u8; 8]; + reader.read_exact(&mut buf)?; + Ok(f64::from_le_bytes(buf)) +} + +#[cfg(test)] +mod tests { + use super::*; + use makepad_ggml::ggml_type_name; + + #[test] + fn ggml_tensor_size_for_q5_k_matches_known_shape() { + let ty = TensorType::Q5K; + let dims = [2048_u64, 512_u64]; + let size = ggml_tensor_size_bytes(ty, &dims).unwrap(); + assert_eq!(size, 720_896); + } + + #[test] + fn align_up_rounds_to_multiple() { + assert_eq!(align_up(33, 32).unwrap(), 64); + assert_eq!(align_up(64, 32).unwrap(), 64); + } + + #[test] + fn tensor_type_names_are_available() { + assert_eq!(ggml_type_name(TensorType::Q5K.ggml_type()), "q5_K"); + } +} diff --git a/libs/llama/src/lib.rs b/libs/llama/src/lib.rs new file mode 100644 index 0000000000..0452200890 --- /dev/null +++ b/libs/llama/src/lib.rs @@ -0,0 +1,67 @@ +mod error; +pub mod gguf; +pub mod model; +pub mod plan; +pub mod qwen35moe; +pub mod qwen35moe_runtime; +pub mod runtime; +pub mod session; +pub mod vocab; +pub mod weights; + +pub use error::{LlamaError, Result}; +pub use gguf::{ + GgufArray, GgufFile, GgufKeyValue, GgufString, GgufTensorInfo, GgufType, GgufValue, +}; +pub use model::{LlamaArchitecture, LlamaModel, ModelGeneral, Qwen35MoeConfig}; +pub use plan::{ + ModelExecutionPlan, ModelLayerInventory, ModelLayerRole, ModelTailProbePlan, + ModelTensorInventory, +}; +pub use qwen35moe::{ + Qwen35MoeAttentionScales, Qwen35MoeAttentionTensors, Qwen35MoeGlobalTensors, + Qwen35MoeLayerKind, Qwen35MoeLayerTensors, Qwen35MoeMoeScales, Qwen35MoeMoeTensors, + Qwen35MoeRecurrentScales, Qwen35MoeRecurrentTensors, Qwen35MoeTensors, +}; +pub use qwen35moe_runtime::{ + qwen35moe_attention_block_layout, qwen35moe_attention_block_spec, + qwen35moe_attention_decode_spec, qwen35moe_delta_net_recurrent_decode_spec, + qwen35moe_embedding_logits_probe_spec, qwen35moe_execution_plan, + qwen35moe_first_attention_block_spec, qwen35moe_first_moe_ffn_spec, + qwen35moe_first_recurrent_block_spec, qwen35moe_hybrid_cache_spec, + qwen35moe_hybrid_cache_template, qwen35moe_hybrid_decode_spec, qwen35moe_moe_ffn_layout, + qwen35moe_moe_ffn_spec, qwen35moe_recurrent_block_layout, qwen35moe_recurrent_block_spec, + qwen35moe_token_logits_probe_spec, Qwen35MoeDims, +}; +pub use runtime::{ + build_attention_block_graph, build_attention_decode_graph, + build_delta_net_recurrent_decode_graph, build_logits_probe_graph, build_moe_ffn_graph, + compile_attention_block_metal, compile_attention_decode_metal, + compile_delta_net_recurrent_decode_metal, compile_hybrid_decode_metal, + compile_logits_probe_metal, compile_moe_ffn_metal, execute_attention_block_graph_metal, + execute_attention_block_graph_metal_cached, execute_attention_decode_graph_metal, + execute_attention_decode_graph_metal_cached, execute_delta_net_recurrent_decode_graph_metal, + execute_delta_net_recurrent_decode_graph_metal_cached, execute_hybrid_decode_graph_metal, + execute_hybrid_decode_graph_metal_cached, execute_logits_probe_graph_metal, + execute_logits_probe_graph_metal_cached, execute_logits_probe_metal, + execute_moe_ffn_graph_metal, execute_moe_ffn_graph_metal_cached, + execute_prepared_attention_block_metal, execute_prepared_attention_decode_metal, + execute_prepared_delta_net_recurrent_decode_metal, execute_prepared_hybrid_decode_metal, + execute_prepared_logits_probe_metal, execute_prepared_moe_ffn_metal, + prepare_attention_block_graph, prepare_attention_decode_graph, + prepare_delta_net_recurrent_decode_graph, prepare_hybrid_decode_graph, + prepare_logits_probe_graph, prepare_moe_ffn_graph, AttentionBlockGraph, AttentionBlockRun, + AttentionBlockSpec, AttentionDecodeGraph, AttentionDecodeSpec, AttentionKvCacheSpec, + AttentionQueryLayout, AttentionRopeSpec, CompiledAttentionBlockMetal, + CompiledAttentionDecodeMetal, CompiledDeltaNetRecurrentDecodeMetal, CompiledHybridDecodeMetal, + CompiledLogitsProbeMetal, CompiledMoeFfnMetal, DeltaNetRecurrentBlockSpec, + DeltaNetRecurrentDecodeGraph, DeltaNetRecurrentDecodeSpec, DeltaNetRecurrentStateSpec, + DenseGatedFfnSpec, ExpertGatingFunc, GraphBatch, HybridAttentionCacheView, HybridCacheLayout, + HybridCacheShape, HybridCacheSpec, HybridCacheTemplate, HybridCacheTypes, HybridDecodeGraph, + HybridDecodeRun, HybridDecodeSpec, HybridLayerSpec, LogitsProbeGraph, LogitsProbeInput, + LogitsProbeRun, LogitsProbeSpec, MoeFfnGraph, MoeFfnRun, MoeFfnSpec, MoeSharedExpertSpec, + ProbeInputKind, RmsNormSpec, +}; +pub use session::{LlamaGeneration, LlamaSession, LlamaSessionConfig, LlamaStopReason}; +pub use vocab::{LlamaTextDecoder, LlamaTokenizerKind, LlamaVocab}; +pub use weights::{GgufWeightLayout, LoadedGgufWeights}; diff --git a/libs/llama/src/model.rs b/libs/llama/src/model.rs new file mode 100644 index 0000000000..deffdee2fa --- /dev/null +++ b/libs/llama/src/model.rs @@ -0,0 +1,288 @@ +use crate::error::{LlamaError, Result}; +use crate::gguf::{GgufArray, GgufFile, GgufString, GgufTensorInfo, GgufValue}; +use crate::plan::ModelExecutionPlan; +use crate::qwen35moe::Qwen35MoeTensors; +use crate::qwen35moe_runtime::qwen35moe_execution_plan; +use crate::weights::GgufWeightLayout; +use std::path::Path; + +#[derive(Clone, Debug, Eq, PartialEq)] +pub enum LlamaArchitecture { + Qwen35, + Qwen35Moe, + Unknown(String), +} + +impl LlamaArchitecture { + pub fn from_str(value: &str) -> Self { + match value { + "qwen35" => Self::Qwen35, + "qwen35moe" => Self::Qwen35Moe, + other => Self::Unknown(other.to_owned()), + } + } + + pub fn name(&self) -> &str { + match self { + Self::Qwen35 => "qwen35", + Self::Qwen35Moe => "qwen35moe", + Self::Unknown(name) => name.as_str(), + } + } +} + +#[derive(Clone, Debug)] +pub struct ModelGeneral { + pub architecture: String, + pub model_type: Option, + pub name: Option, + pub file_type: Option, + pub quantization_version: Option, +} + +#[derive(Clone, Debug)] +pub struct Qwen35MoeConfig { + pub block_count: u32, + pub context_length: u32, + pub embedding_length: u32, + pub attention_head_count: u32, + pub attention_head_count_kv: u32, + pub attention_key_length: u32, + pub attention_value_length: u32, + pub rope_dimension_count: u32, + pub rope_dimension_sections: Vec, + pub rope_freq_base: f32, + pub attention_layer_norm_rms_epsilon: f32, + pub expert_count: u32, + pub expert_used_count: u32, + pub expert_feed_forward_length: u32, + pub expert_shared_feed_forward_length: u32, + pub ssm_conv_kernel: u32, + pub ssm_state_size: u32, + pub ssm_group_count: u32, + pub ssm_time_step_rank: u32, + pub ssm_inner_size: u32, + pub full_attention_interval: u32, +} + +impl Qwen35MoeConfig { + pub fn from_gguf(gguf: &GgufFile) -> Result { + Ok(Self { + block_count: required_u32(gguf, "qwen35moe.block_count")?, + context_length: required_u32(gguf, "qwen35moe.context_length")?, + embedding_length: required_u32(gguf, "qwen35moe.embedding_length")?, + attention_head_count: required_u32(gguf, "qwen35moe.attention.head_count")?, + attention_head_count_kv: required_u32(gguf, "qwen35moe.attention.head_count_kv")?, + attention_key_length: required_u32(gguf, "qwen35moe.attention.key_length")?, + attention_value_length: required_u32(gguf, "qwen35moe.attention.value_length")?, + rope_dimension_count: required_u32(gguf, "qwen35moe.rope.dimension_count")?, + rope_dimension_sections: required_u32_array(gguf, "qwen35moe.rope.dimension_sections")?, + rope_freq_base: required_f32(gguf, "qwen35moe.rope.freq_base")?, + attention_layer_norm_rms_epsilon: required_f32( + gguf, + "qwen35moe.attention.layer_norm_rms_epsilon", + )?, + expert_count: required_u32(gguf, "qwen35moe.expert_count")?, + expert_used_count: required_u32(gguf, "qwen35moe.expert_used_count")?, + expert_feed_forward_length: required_u32(gguf, "qwen35moe.expert_feed_forward_length")?, + expert_shared_feed_forward_length: required_u32( + gguf, + "qwen35moe.expert_shared_feed_forward_length", + )?, + ssm_conv_kernel: required_u32(gguf, "qwen35moe.ssm.conv_kernel")?, + ssm_state_size: required_u32(gguf, "qwen35moe.ssm.state_size")?, + ssm_group_count: required_u32(gguf, "qwen35moe.ssm.group_count")?, + ssm_time_step_rank: required_u32(gguf, "qwen35moe.ssm.time_step_rank")?, + ssm_inner_size: required_u32(gguf, "qwen35moe.ssm.inner_size")?, + full_attention_interval: required_u32(gguf, "qwen35moe.full_attention_interval")?, + }) + } +} + +#[derive(Clone, Debug)] +pub struct LlamaModel { + pub gguf: GgufFile, + pub architecture: LlamaArchitecture, + pub general: ModelGeneral, + pub qwen35moe: Option, +} + +impl LlamaModel { + pub fn load(path: impl AsRef) -> Result { + let gguf = GgufFile::open(path)?; + let architecture = required_utf8_string(&gguf, "general.architecture")?; + let architecture_kind = LlamaArchitecture::from_str(&architecture); + + let general = ModelGeneral { + architecture, + model_type: optional_utf8_string(&gguf, "general.type")?, + name: optional_utf8_string(&gguf, "general.name")?, + file_type: optional_u32(&gguf, "general.file_type"), + quantization_version: optional_u32(&gguf, "general.quantization_version"), + }; + + let qwen35moe = match architecture_kind { + LlamaArchitecture::Qwen35Moe => Some(Qwen35MoeConfig::from_gguf(&gguf)?), + _ => None, + }; + + Ok(Self { + gguf, + architecture: architecture_kind, + general, + qwen35moe, + }) + } + + pub fn tensor(&self, name: &str) -> Option<&GgufTensorInfo> { + self.gguf.get_tensor(name) + } + + pub fn require_qwen35moe(&self) -> Result<&Qwen35MoeConfig> { + self.qwen35moe.as_ref().ok_or_else(|| { + LlamaError::unsupported(format!( + "model architecture '{}' is not qwen35moe", + self.architecture.name() + )) + }) + } + + pub fn qwen35moe_tensors(&self) -> Result { + Qwen35MoeTensors::from_model(self) + } + + pub fn qwen35moe_weight_layout(&self) -> Result { + self.qwen35moe_tensors()?.weight_layout() + } + + pub fn execution_plan(&self) -> Result { + match self.architecture { + LlamaArchitecture::Qwen35Moe => qwen35moe_execution_plan(self), + _ => Err(LlamaError::unsupported(format!( + "execution plan builder is not implemented for architecture '{}'", + self.architecture.name() + ))), + } + } + + pub fn validate_layout(&self) -> Result<()> { + let plan = self.execution_plan()?; + if plan.inventory.layers.is_empty() { + return Err(LlamaError::format(format!( + "model '{}' produced an empty layer inventory", + self.architecture.name() + ))); + } + Ok(()) + } + + pub fn validate_qwen35moe_layout(&self) -> Result<()> { + self.validate_layout() + } +} + +fn required_u32(gguf: &GgufFile, key: &str) -> Result { + let value = gguf.require_value(key)?; + value_to_u32(value).ok_or_else(|| { + LlamaError::format(format!( + "gguf key '{}' has type {}, expected integral scalar", + key, + value.value_type().name() + )) + }) +} + +fn required_f32(gguf: &GgufFile, key: &str) -> Result { + gguf.require_value(key)?.as_f32().ok_or_else(|| { + LlamaError::format(format!( + "gguf key '{}' has type {}, expected f32", + key, + gguf.require_value(key).unwrap().value_type().name() + )) + }) +} + +fn required_u32_array(gguf: &GgufFile, key: &str) -> Result> { + let value = gguf.require_value(key)?; + match value { + GgufValue::Array(values) => array_to_u32_vec(values).ok_or_else(|| { + LlamaError::format(format!( + "gguf key '{}' has type {}, expected integral array", + key, + value.value_type().name() + )) + }), + other => Err(LlamaError::format(format!( + "gguf key '{}' has type {}, expected u32 array", + key, + other.value_type().name() + ))), + } +} + +fn required_utf8_string(gguf: &GgufFile, key: &str) -> Result { + match gguf.require_value(key)? { + GgufValue::String(value) => value.try_utf8().map(|s| s.to_owned()), + other => Err(LlamaError::format(format!( + "gguf key '{}' has type {}, expected string", + key, + other.value_type().name() + ))), + } +} + +fn optional_utf8_string(gguf: &GgufFile, key: &str) -> Result> { + match gguf.get_value(key) { + None => Ok(None), + Some(GgufValue::String(value)) => value.try_utf8().map(|s| Some(s.to_owned())), + Some(other) => Err(LlamaError::format(format!( + "gguf key '{}' has type {}, expected string", + key, + other.value_type().name() + ))), + } +} + +fn optional_u32(gguf: &GgufFile, key: &str) -> Option { + gguf.get_value(key).and_then(value_to_u32) +} + +#[allow(dead_code)] +fn optional_string<'a>(gguf: &'a GgufFile, key: &str) -> Option<&'a GgufString> { + gguf.get_value(key).and_then(GgufValue::as_string) +} + +fn value_to_u32(value: &GgufValue) -> Option { + match value { + GgufValue::Uint32(v) => Some(*v), + GgufValue::Uint64(v) => u32::try_from(*v).ok(), + GgufValue::Int32(v) => u32::try_from(*v).ok(), + GgufValue::Int64(v) => u32::try_from(*v).ok(), + _ => None, + } +} + +fn array_to_u32_vec(value: &GgufArray) -> Option> { + match value { + GgufArray::Uint32(values) => Some(values.clone()), + GgufArray::Int32(values) => values + .iter() + .copied() + .map(u32::try_from) + .collect::, _>>() + .ok(), + GgufArray::Uint64(values) => values + .iter() + .copied() + .map(u32::try_from) + .collect::, _>>() + .ok(), + GgufArray::Int64(values) => values + .iter() + .copied() + .map(u32::try_from) + .collect::, _>>() + .ok(), + _ => None, + } +} diff --git a/libs/llama/src/plan.rs b/libs/llama/src/plan.rs new file mode 100644 index 0000000000..d534ad09be --- /dev/null +++ b/libs/llama/src/plan.rs @@ -0,0 +1,143 @@ +use std::collections::BTreeMap; + +use crate::gguf::GgufTensorInfo; +use crate::model::LlamaArchitecture; +use crate::runtime::{HybridCacheTemplate, LogitsProbeSpec}; +use crate::weights::GgufWeightLayout; +use crate::Result; + +#[derive(Clone, Copy, Debug, Eq, PartialEq)] +pub enum ModelLayerRole { + Attention, + Recurrent, + Unknown, +} + +impl ModelLayerRole { + pub fn name(self) -> &'static str { + match self { + Self::Attention => "attention", + Self::Recurrent => "recurrent", + Self::Unknown => "unknown", + } + } +} + +#[derive(Clone, Debug)] +pub struct ModelLayerInventory { + pub index: u32, + pub role: ModelLayerRole, + pub tensors: BTreeMap, +} + +#[derive(Clone, Debug, Default)] +pub struct ModelTensorInventory { + pub globals: BTreeMap, + pub layers: Vec, +} + +impl ModelTensorInventory { + pub fn unique_tensor_count(&self) -> usize { + self.unique_tensors().len() + } + + pub fn total_tensor_bytes(&self) -> u64 { + self.unique_tensors() + .into_iter() + .map(|tensor| tensor.size_bytes) + .sum() + } + + pub fn weight_layout(&self) -> Result { + GgufWeightLayout::from_tensors(self.unique_tensors()) + } + + pub fn count_layers_with_role(&self, role: ModelLayerRole) -> usize { + self.layers + .iter() + .filter(|layer| layer.role == role) + .count() + } + + pub fn unique_tensors(&self) -> Vec { + let mut tensors = BTreeMap::new(); + self.visit_tensors(|tensor| { + tensors + .entry(tensor.name.clone()) + .or_insert_with(|| tensor.clone()); + }); + tensors.into_values().collect() + } + + fn visit_tensors(&self, mut visit: impl FnMut(&GgufTensorInfo)) { + for tensor in self.globals.values() { + visit(tensor); + } + for layer in &self.layers { + for tensor in layer.tensors.values() { + visit(tensor); + } + } + } +} + +#[derive(Clone, Debug)] +pub struct ModelTailProbePlan { + pub spec: LogitsProbeSpec, + pub weights: GgufWeightLayout, + pub extra_activation_bytes: usize, +} + +#[derive(Clone, Debug)] +pub struct ModelExecutionPlan { + pub architecture: LlamaArchitecture, + pub embedding_length: u32, + pub vocab_size: Option, + pub inventory: ModelTensorInventory, + pub full_weights: GgufWeightLayout, + pub tail_probe: ModelTailProbePlan, + pub hybrid_cache: Option, +} + +impl ModelExecutionPlan { + pub fn layer_count(&self) -> usize { + self.inventory.layers.len() + } +} + +#[cfg(test)] +mod tests { + use super::{ModelLayerInventory, ModelLayerRole, ModelTensorInventory}; + use crate::gguf::GgufTensorInfo; + use makepad_ggml::TensorType; + use std::collections::BTreeMap; + + #[test] + fn inventory_deduplicates_by_tensor_name() { + let shared = GgufTensorInfo { + name: "shared.weight".to_owned(), + dimensions: vec![4, 4], + tensor_type: TensorType::F32, + offset: 0, + size_bytes: 64, + }; + + let mut globals = BTreeMap::new(); + globals.insert("shared".to_owned(), shared.clone()); + + let mut layer_tensors = BTreeMap::new(); + layer_tensors.insert("alias".to_owned(), shared); + + let inventory = ModelTensorInventory { + globals, + layers: vec![ModelLayerInventory { + index: 0, + role: ModelLayerRole::Attention, + tensors: layer_tensors, + }], + }; + + assert_eq!(inventory.unique_tensor_count(), 1); + assert_eq!(inventory.total_tensor_bytes(), 64); + } +} diff --git a/libs/llama/src/qwen35moe.rs b/libs/llama/src/qwen35moe.rs new file mode 100644 index 0000000000..6667ab78f8 --- /dev/null +++ b/libs/llama/src/qwen35moe.rs @@ -0,0 +1,410 @@ +use std::collections::BTreeMap; + +use crate::error::{LlamaError, Result}; +use crate::gguf::GgufTensorInfo; +use crate::model::LlamaModel; +use crate::weights::GgufWeightLayout; + +#[derive(Clone, Copy, Debug, Eq, PartialEq)] +pub enum Qwen35MoeLayerKind { + Attention, + Recurrent, +} + +impl Qwen35MoeLayerKind { + pub fn name(self) -> &'static str { + match self { + Self::Attention => "attention", + Self::Recurrent => "recurrent", + } + } +} + +#[derive(Clone, Debug)] +pub struct Qwen35MoeGlobalTensors { + pub token_embd: GgufTensorInfo, + pub output_norm: GgufTensorInfo, + pub output: GgufTensorInfo, +} + +#[derive(Clone, Debug, Default)] +pub struct Qwen35MoeAttentionScales { + pub wq: Option, + pub wk: Option, + pub wv: Option, + pub wo: Option, +} + +#[derive(Clone, Debug)] +pub struct Qwen35MoeAttentionTensors { + pub wq: GgufTensorInfo, + pub wk: GgufTensorInfo, + pub wv: GgufTensorInfo, + pub wo: GgufTensorInfo, + pub attn_q_norm: GgufTensorInfo, + pub attn_k_norm: GgufTensorInfo, + pub scales: Qwen35MoeAttentionScales, +} + +#[derive(Clone, Debug, Default)] +pub struct Qwen35MoeRecurrentScales { + pub wqkv: Option, + pub wqkv_gate: Option, + pub ssm_out: Option, + pub ssm_alpha: Option, + pub ssm_beta: Option, +} + +#[derive(Clone, Debug)] +pub struct Qwen35MoeRecurrentTensors { + pub wqkv: GgufTensorInfo, + pub wqkv_gate: GgufTensorInfo, + pub ssm_conv1d: GgufTensorInfo, + pub ssm_dt: GgufTensorInfo, + pub ssm_a: GgufTensorInfo, + pub ssm_beta: GgufTensorInfo, + pub ssm_alpha: GgufTensorInfo, + pub ssm_norm: GgufTensorInfo, + pub ssm_out: GgufTensorInfo, + pub scales: Qwen35MoeRecurrentScales, +} + +#[derive(Clone, Debug, Default)] +pub struct Qwen35MoeMoeScales { + pub ffn_gate_exps: Option, + pub ffn_up_exps: Option, + pub ffn_down_exps: Option, + pub ffn_gate_shexp: Option, + pub ffn_up_shexp: Option, + pub ffn_down_shexp: Option, +} + +#[derive(Clone, Debug)] +pub struct Qwen35MoeMoeTensors { + pub ffn_gate_inp: GgufTensorInfo, + pub ffn_gate_up_exps: Option, + pub ffn_gate_exps: Option, + pub ffn_up_exps: Option, + pub ffn_down_exps: GgufTensorInfo, + pub ffn_gate_inp_shexp: GgufTensorInfo, + pub ffn_gate_shexp: GgufTensorInfo, + pub ffn_up_shexp: GgufTensorInfo, + pub ffn_down_shexp: GgufTensorInfo, + pub scales: Qwen35MoeMoeScales, +} + +impl Qwen35MoeMoeTensors { + pub fn uses_merged_gate_up(&self) -> bool { + self.ffn_gate_up_exps.is_some() + } +} + +#[derive(Clone, Debug)] +pub struct Qwen35MoeLayerTensors { + pub index: u32, + pub kind: Qwen35MoeLayerKind, + pub attn_norm: GgufTensorInfo, + pub post_attention_norm: GgufTensorInfo, + pub attention: Option, + pub recurrent: Option, + pub moe: Qwen35MoeMoeTensors, +} + +#[derive(Clone, Debug)] +pub struct Qwen35MoeTensors { + pub globals: Qwen35MoeGlobalTensors, + pub layers: Vec, +} + +impl Qwen35MoeTensors { + pub fn from_model(model: &LlamaModel) -> Result { + let cfg = model.require_qwen35moe()?; + let token_embd = required_tensor(model, "token_embd.weight")?; + let output_norm = required_tensor(model, "output_norm.weight")?; + let output = optional_tensor(model, "output.weight").unwrap_or_else(|| token_embd.clone()); + + let mut layers = Vec::with_capacity(cfg.block_count as usize); + for index in 0..cfg.block_count { + let kind = layer_kind(index, cfg.full_attention_interval)?; + let attn_norm = required_tensor(model, &layer_name(index, "attn_norm", "weight"))?; + let post_attention_norm = + required_tensor(model, &layer_name(index, "post_attention_norm", "weight"))?; + + let attention = match kind { + Qwen35MoeLayerKind::Attention => Some(Qwen35MoeAttentionTensors { + wq: required_tensor(model, &layer_name(index, "attn_q", "weight"))?, + wk: required_tensor(model, &layer_name(index, "attn_k", "weight"))?, + wv: required_tensor(model, &layer_name(index, "attn_v", "weight"))?, + wo: required_tensor(model, &layer_name(index, "attn_output", "weight"))?, + attn_q_norm: required_tensor( + model, + &layer_name(index, "attn_q_norm", "weight"), + )?, + attn_k_norm: required_tensor( + model, + &layer_name(index, "attn_k_norm", "weight"), + )?, + scales: Qwen35MoeAttentionScales { + wq: optional_tensor(model, &layer_name(index, "attn_q", "scale")), + wk: optional_tensor(model, &layer_name(index, "attn_k", "scale")), + wv: optional_tensor(model, &layer_name(index, "attn_v", "scale")), + wo: optional_tensor(model, &layer_name(index, "attn_output", "scale")), + }, + }), + Qwen35MoeLayerKind::Recurrent => None, + }; + + let recurrent = match kind { + Qwen35MoeLayerKind::Attention => None, + Qwen35MoeLayerKind::Recurrent => Some(Qwen35MoeRecurrentTensors { + wqkv: required_tensor(model, &layer_name(index, "attn_qkv", "weight"))?, + wqkv_gate: required_tensor(model, &layer_name(index, "attn_gate", "weight"))?, + ssm_conv1d: required_tensor(model, &layer_name(index, "ssm_conv1d", "weight"))?, + ssm_dt: required_tensor(model, &layer_name(index, "ssm_dt", "bias"))?, + ssm_a: required_tensor(model, &layer_scalar_name(index, "ssm_a"))?, + ssm_beta: required_tensor(model, &layer_name(index, "ssm_beta", "weight"))?, + ssm_alpha: required_tensor(model, &layer_name(index, "ssm_alpha", "weight"))?, + ssm_norm: required_tensor(model, &layer_name(index, "ssm_norm", "weight"))?, + ssm_out: required_tensor(model, &layer_name(index, "ssm_out", "weight"))?, + scales: Qwen35MoeRecurrentScales { + wqkv: optional_tensor(model, &layer_name(index, "attn_qkv", "scale")), + wqkv_gate: optional_tensor(model, &layer_name(index, "attn_gate", "scale")), + ssm_out: optional_tensor(model, &layer_name(index, "ssm_out", "scale")), + ssm_alpha: optional_tensor(model, &layer_name(index, "ssm_alpha", "scale")), + ssm_beta: optional_tensor(model, &layer_name(index, "ssm_beta", "scale")), + }, + }), + }; + + let ffn_gate_up_exps = + optional_tensor(model, &layer_name(index, "ffn_gate_up_exps", "weight")); + let ffn_gate_exps = + optional_tensor(model, &layer_name(index, "ffn_gate_exps", "weight")); + let ffn_up_exps = optional_tensor(model, &layer_name(index, "ffn_up_exps", "weight")); + if ffn_gate_up_exps.is_none() && (ffn_gate_exps.is_none() || ffn_up_exps.is_none()) { + return Err(LlamaError::format(format!( + "layer {} is missing expert gate/up weights: expected either blk.{}.ffn_gate_up_exps.weight or both blk.{}.ffn_gate_exps.weight and blk.{}.ffn_up_exps.weight", + index, index, index, index + ))); + } + + layers.push(Qwen35MoeLayerTensors { + index, + kind, + attn_norm, + post_attention_norm, + attention, + recurrent, + moe: Qwen35MoeMoeTensors { + ffn_gate_inp: required_tensor( + model, + &layer_name(index, "ffn_gate_inp", "weight"), + )?, + ffn_gate_up_exps, + ffn_gate_exps, + ffn_up_exps, + ffn_down_exps: required_tensor( + model, + &layer_name(index, "ffn_down_exps", "weight"), + )?, + ffn_gate_inp_shexp: required_tensor( + model, + &layer_name(index, "ffn_gate_inp_shexp", "weight"), + )?, + ffn_gate_shexp: required_tensor( + model, + &layer_name(index, "ffn_gate_shexp", "weight"), + )?, + ffn_up_shexp: required_tensor( + model, + &layer_name(index, "ffn_up_shexp", "weight"), + )?, + ffn_down_shexp: required_tensor( + model, + &layer_name(index, "ffn_down_shexp", "weight"), + )?, + scales: Qwen35MoeMoeScales { + ffn_gate_exps: optional_tensor( + model, + &layer_name(index, "ffn_gate_exps", "scale"), + ), + ffn_up_exps: optional_tensor( + model, + &layer_name(index, "ffn_up_exps", "scale"), + ), + ffn_down_exps: optional_tensor( + model, + &layer_name(index, "ffn_down_exps", "scale"), + ), + ffn_gate_shexp: optional_tensor( + model, + &layer_name(index, "ffn_gate_shexp", "scale"), + ), + ffn_up_shexp: optional_tensor( + model, + &layer_name(index, "ffn_up_shexp", "scale"), + ), + ffn_down_shexp: optional_tensor( + model, + &layer_name(index, "ffn_down_shexp", "scale"), + ), + }, + }, + }); + } + + Ok(Self { + globals: Qwen35MoeGlobalTensors { + token_embd, + output_norm, + output, + }, + layers, + }) + } + + pub fn unique_tensor_count(&self) -> usize { + self.unique_tensors().len() + } + + pub fn total_tensor_bytes(&self) -> u64 { + self.unique_tensors() + .into_iter() + .map(|tensor| tensor.size_bytes) + .sum() + } + + pub fn weight_layout(&self) -> Result { + GgufWeightLayout::from_tensors(self.unique_tensors()) + } + + pub fn unique_tensors(&self) -> Vec { + let mut tensors = BTreeMap::new(); + self.visit_tensors(|tensor| { + tensors + .entry(tensor.name.clone()) + .or_insert_with(|| tensor.clone()); + }); + tensors.into_values().collect() + } + + fn visit_tensors(&self, mut visit: impl FnMut(&GgufTensorInfo)) { + visit(&self.globals.token_embd); + visit(&self.globals.output_norm); + visit(&self.globals.output); + + for layer in &self.layers { + visit(&layer.attn_norm); + visit(&layer.post_attention_norm); + + if let Some(attention) = &layer.attention { + visit(&attention.wq); + visit(&attention.wk); + visit(&attention.wv); + visit(&attention.wo); + visit(&attention.attn_q_norm); + visit(&attention.attn_k_norm); + visit_optional(&attention.scales.wq, &mut visit); + visit_optional(&attention.scales.wk, &mut visit); + visit_optional(&attention.scales.wv, &mut visit); + visit_optional(&attention.scales.wo, &mut visit); + } + + if let Some(recurrent) = &layer.recurrent { + visit(&recurrent.wqkv); + visit(&recurrent.wqkv_gate); + visit(&recurrent.ssm_conv1d); + visit(&recurrent.ssm_dt); + visit(&recurrent.ssm_a); + visit(&recurrent.ssm_beta); + visit(&recurrent.ssm_alpha); + visit(&recurrent.ssm_norm); + visit(&recurrent.ssm_out); + visit_optional(&recurrent.scales.wqkv, &mut visit); + visit_optional(&recurrent.scales.wqkv_gate, &mut visit); + visit_optional(&recurrent.scales.ssm_out, &mut visit); + visit_optional(&recurrent.scales.ssm_alpha, &mut visit); + visit_optional(&recurrent.scales.ssm_beta, &mut visit); + } + + visit(&layer.moe.ffn_gate_inp); + visit_optional(&layer.moe.ffn_gate_up_exps, &mut visit); + visit_optional(&layer.moe.ffn_gate_exps, &mut visit); + visit_optional(&layer.moe.ffn_up_exps, &mut visit); + visit(&layer.moe.ffn_down_exps); + visit(&layer.moe.ffn_gate_inp_shexp); + visit(&layer.moe.ffn_gate_shexp); + visit(&layer.moe.ffn_up_shexp); + visit(&layer.moe.ffn_down_shexp); + visit_optional(&layer.moe.scales.ffn_gate_exps, &mut visit); + visit_optional(&layer.moe.scales.ffn_up_exps, &mut visit); + visit_optional(&layer.moe.scales.ffn_down_exps, &mut visit); + visit_optional(&layer.moe.scales.ffn_gate_shexp, &mut visit); + visit_optional(&layer.moe.scales.ffn_up_shexp, &mut visit); + visit_optional(&layer.moe.scales.ffn_down_shexp, &mut visit); + } + } +} + +fn visit_optional<'a>( + tensor: &'a Option, + visit: &mut impl FnMut(&'a GgufTensorInfo), +) { + if let Some(tensor) = tensor.as_ref() { + visit(tensor); + } +} + +fn layer_kind(index: u32, full_attention_interval: u32) -> Result { + if full_attention_interval == 0 { + return Err(LlamaError::format( + "qwen35moe.full_attention_interval must be greater than zero", + )); + } + if (index + 1) % full_attention_interval == 0 { + Ok(Qwen35MoeLayerKind::Attention) + } else { + Ok(Qwen35MoeLayerKind::Recurrent) + } +} + +fn required_tensor(model: &LlamaModel, name: &str) -> Result { + model.gguf.require_tensor(name).cloned() +} + +fn optional_tensor(model: &LlamaModel, name: &str) -> Option { + model.gguf.get_tensor(name).cloned() +} + +fn layer_name(index: u32, stem: &str, suffix: &str) -> String { + format!("blk.{}.{}.{}", index, stem, suffix) +} + +fn layer_scalar_name(index: u32, stem: &str) -> String { + format!("blk.{}.{}", index, stem) +} + +#[cfg(test)] +mod tests { + use super::{layer_kind, Qwen35MoeLayerKind}; + + #[test] + fn layer_kind_matches_llama_cpp_interval_rule() { + let kinds = (0..8) + .map(|index| layer_kind(index, 4).unwrap()) + .collect::>(); + assert_eq!( + kinds, + vec![ + Qwen35MoeLayerKind::Recurrent, + Qwen35MoeLayerKind::Recurrent, + Qwen35MoeLayerKind::Recurrent, + Qwen35MoeLayerKind::Attention, + Qwen35MoeLayerKind::Recurrent, + Qwen35MoeLayerKind::Recurrent, + Qwen35MoeLayerKind::Recurrent, + Qwen35MoeLayerKind::Attention, + ] + ); + } +} diff --git a/libs/llama/src/qwen35moe_runtime.rs b/libs/llama/src/qwen35moe_runtime.rs new file mode 100644 index 0000000000..81bef96565 --- /dev/null +++ b/libs/llama/src/qwen35moe_runtime.rs @@ -0,0 +1,749 @@ +use std::collections::BTreeMap; + +use makepad_ggml::{TensorType, UnaryOp, GGML_ROPE_TYPE_IMROPE}; + +use crate::error::{LlamaError, Result}; +use crate::model::LlamaModel; +use crate::plan::{ + ModelExecutionPlan, ModelLayerInventory, ModelLayerRole, ModelTailProbePlan, + ModelTensorInventory, +}; +use crate::qwen35moe::{Qwen35MoeLayerKind, Qwen35MoeTensors}; +use crate::runtime::{ + AttentionBlockSpec, AttentionDecodeSpec, AttentionKvCacheSpec, AttentionQueryLayout, + AttentionRopeSpec, DeltaNetRecurrentBlockSpec, DeltaNetRecurrentDecodeSpec, + DeltaNetRecurrentStateSpec, DenseGatedFfnSpec, ExpertGatingFunc, HybridCacheShape, + HybridCacheSpec, HybridCacheTemplate, HybridCacheTypes, HybridDecodeSpec, HybridLayerSpec, + LogitsProbeSpec, MoeFfnSpec, MoeSharedExpertSpec, ProbeInputKind, RmsNormSpec, +}; +use crate::weights::GgufWeightLayout; + +#[derive(Clone, Debug)] +pub struct Qwen35MoeDims { + pub vocab_size: u32, + pub block_count: u32, + pub embedding_length: u32, + pub attention_head_count: u32, + pub attention_head_count_kv: u32, + pub attention_key_length: u32, + pub attention_value_length: u32, + pub expert_count: u32, + pub expert_used_count: u32, + pub ssm_conv_kernel: u32, + pub ssm_state_size: u32, + pub ssm_group_count: u32, + pub ssm_time_step_rank: u32, + pub ssm_inner_size: u32, + pub full_attention_interval: u32, +} + +impl Qwen35MoeDims { + pub fn from_model(model: &LlamaModel, tensors: &Qwen35MoeTensors) -> Result { + let cfg = model.require_qwen35moe()?; + let vocab_size = tensors + .globals + .token_embd + .dimensions + .get(1) + .copied() + .ok_or_else(|| LlamaError::format("token_embd.weight is missing vocab dimension"))?; + Ok(Self { + vocab_size: u32::try_from(vocab_size).map_err(|_| { + LlamaError::format(format!("vocab size {} does not fit in u32", vocab_size)) + })?, + block_count: cfg.block_count, + embedding_length: cfg.embedding_length, + attention_head_count: cfg.attention_head_count, + attention_head_count_kv: cfg.attention_head_count_kv, + attention_key_length: cfg.attention_key_length, + attention_value_length: cfg.attention_value_length, + expert_count: cfg.expert_count, + expert_used_count: cfg.expert_used_count, + ssm_conv_kernel: cfg.ssm_conv_kernel, + ssm_state_size: cfg.ssm_state_size, + ssm_group_count: cfg.ssm_group_count, + ssm_time_step_rank: cfg.ssm_time_step_rank, + ssm_inner_size: cfg.ssm_inner_size, + full_attention_interval: cfg.full_attention_interval, + }) + } + + pub fn recurrent_conv_width(&self) -> Result { + let conv_prefix = u64::from(self.ssm_conv_kernel.saturating_sub(1)); + let channels = u64::from(self.ssm_inner_size) + .checked_add( + 2_u64 + .checked_mul(u64::from(self.ssm_group_count)) + .and_then(|v| v.checked_mul(u64::from(self.ssm_state_size))) + .ok_or_else(|| { + LlamaError::format("overflow computing qwen35moe conv channels") + })?, + ) + .ok_or_else(|| LlamaError::format("overflow computing qwen35moe conv channels"))?; + conv_prefix + .checked_mul(channels) + .ok_or_else(|| LlamaError::format("overflow computing qwen35moe conv width")) + } + + pub fn recurrent_state_width(&self) -> Result { + u64::from(self.ssm_state_size) + .checked_mul(u64::from(self.ssm_inner_size)) + .ok_or_else(|| LlamaError::format("overflow computing qwen35moe recurrent state width")) + } + + pub fn attention_k_width(&self) -> u64 { + u64::from(self.attention_key_length) * u64::from(self.attention_head_count_kv) + } + + pub fn attention_v_width(&self) -> u64 { + u64::from(self.attention_value_length) * u64::from(self.attention_head_count_kv) + } +} + +pub fn qwen35moe_token_logits_probe_spec(model: &LlamaModel) -> Result { + let tensors = model.qwen35moe_tensors()?; + let cfg = model.require_qwen35moe()?; + Ok(LogitsProbeSpec { + input: ProbeInputKind::TokenIds { + token_embedding_name: tensors.globals.token_embd.name.clone(), + }, + output_norm_name: tensors.globals.output_norm.name.clone(), + output_name: tensors.globals.output.name.clone(), + rms_epsilon: cfg.attention_layer_norm_rms_epsilon, + }) +} + +pub fn qwen35moe_embedding_logits_probe_spec(model: &LlamaModel) -> Result { + let tensors = model.qwen35moe_tensors()?; + let dims = Qwen35MoeDims::from_model(model, &tensors)?; + let cfg = model.require_qwen35moe()?; + Ok(LogitsProbeSpec { + input: ProbeInputKind::Embeddings { + hidden_size: dims.embedding_length, + input_type: TensorType::F32, + }, + output_norm_name: tensors.globals.output_norm.name.clone(), + output_name: tensors.globals.output.name.clone(), + rms_epsilon: cfg.attention_layer_norm_rms_epsilon, + }) +} + +pub fn qwen35moe_attention_block_spec( + model: &LlamaModel, + layer_index: u32, +) -> Result { + let tensors = model.qwen35moe_tensors()?; + let dims = Qwen35MoeDims::from_model(model, &tensors)?; + let cfg = model.require_qwen35moe()?; + let layer = tensors + .layers + .iter() + .find(|layer| layer.index == layer_index) + .ok_or_else(|| LlamaError::format(format!("missing qwen35moe layer {}", layer_index)))?; + let attention = layer.attention.as_ref().ok_or_else(|| { + LlamaError::format(format!( + "qwen35moe layer {} is {} and does not expose full-attention tensors", + layer_index, + layer.kind.name() + )) + })?; + + Ok(AttentionBlockSpec { + input: ProbeInputKind::TokenIds { + token_embedding_name: tensors.globals.token_embd.name.clone(), + }, + input_norm_name: layer.attn_norm.name.clone(), + q_proj_name: attention.wq.name.clone(), + q_layout: AttentionQueryLayout::InterleavedQueryGate { + gate_activation: UnaryOp::Sigmoid, + }, + k_proj_name: attention.wk.name.clone(), + v_proj_name: attention.wv.name.clone(), + output_proj_name: attention.wo.name.clone(), + q_norm_name: Some(attention.attn_q_norm.name.clone()), + k_norm_name: Some(attention.attn_k_norm.name.clone()), + q_head_dim: dims.attention_key_length, + q_head_count: dims.attention_head_count, + k_head_dim: dims.attention_key_length, + kv_head_count: dims.attention_head_count_kv, + v_head_dim: dims.attention_value_length, + rms_epsilon: cfg.attention_layer_norm_rms_epsilon, + rope: Some(AttentionRopeSpec { + n_dims: i32::try_from(cfg.rope_dimension_count).map_err(|_| { + LlamaError::format(format!( + "rope_dimension_count {} does not fit in i32", + cfg.rope_dimension_count + )) + })?, + sections: rope_sections_i32(&cfg.rope_dimension_sections)?, + mode: GGML_ROPE_TYPE_IMROPE, + n_ctx_orig: i32::try_from(cfg.context_length).map_err(|_| { + LlamaError::format(format!( + "context_length {} does not fit in i32", + cfg.context_length + )) + })?, + freq_base: cfg.rope_freq_base, + freq_scale: 1.0, + ext_factor: 0.0, + attn_factor: 1.0, + beta_fast: 0.0, + beta_slow: 0.0, + }), + causal: true, + residual: true, + }) +} + +pub fn qwen35moe_first_attention_block_spec( + model: &LlamaModel, +) -> Result<(u32, AttentionBlockSpec)> { + let tensors = model.qwen35moe_tensors()?; + let layer = tensors + .layers + .iter() + .find(|layer| layer.kind == Qwen35MoeLayerKind::Attention) + .ok_or_else(|| LlamaError::format("qwen35moe model has no attention layers"))?; + Ok(( + layer.index, + qwen35moe_attention_block_spec(model, layer.index)?, + )) +} + +pub fn qwen35moe_attention_decode_spec( + model: &LlamaModel, + layer_index: u32, + max_context: u32, + max_sequences: u32, + k_type: TensorType, + v_type: TensorType, +) -> Result { + Ok(AttentionDecodeSpec { + block: qwen35moe_attention_block_spec(model, layer_index)?, + cache: AttentionKvCacheSpec { + max_context, + max_sequences, + k_type, + v_type, + }, + }) +} + +pub fn qwen35moe_recurrent_block_spec( + model: &LlamaModel, + layer_index: u32, +) -> Result { + let tensors = model.qwen35moe_tensors()?; + let dims = Qwen35MoeDims::from_model(model, &tensors)?; + let cfg = model.require_qwen35moe()?; + let layer = tensors + .layers + .iter() + .find(|layer| layer.index == layer_index) + .ok_or_else(|| LlamaError::format(format!("missing qwen35moe layer {}", layer_index)))?; + let recurrent = layer.recurrent.as_ref().ok_or_else(|| { + LlamaError::format(format!( + "qwen35moe layer {} is {} and does not expose recurrent tensors", + layer_index, + layer.kind.name() + )) + })?; + + let value_head_dim = dims + .ssm_inner_size + .checked_div(dims.ssm_time_step_rank) + .ok_or_else(|| LlamaError::format("qwen35moe ssm_time_step_rank must be non-zero"))?; + if value_head_dim * dims.ssm_time_step_rank != dims.ssm_inner_size { + return Err(LlamaError::format(format!( + "qwen35moe ssm_inner_size {} is not divisible by ssm_time_step_rank {}", + dims.ssm_inner_size, dims.ssm_time_step_rank + ))); + } + + Ok(DeltaNetRecurrentBlockSpec { + input: ProbeInputKind::TokenIds { + token_embedding_name: tensors.globals.token_embd.name.clone(), + }, + input_norm_name: layer.attn_norm.name.clone(), + qkv_proj_name: recurrent.wqkv.name.clone(), + z_proj_name: recurrent.wqkv_gate.name.clone(), + beta_proj_name: recurrent.ssm_beta.name.clone(), + alpha_proj_name: recurrent.ssm_alpha.name.clone(), + dt_bias_name: recurrent.ssm_dt.name.clone(), + a_name: recurrent.ssm_a.name.clone(), + conv_kernel_name: recurrent.ssm_conv1d.name.clone(), + norm_name: recurrent.ssm_norm.name.clone(), + output_proj_name: recurrent.ssm_out.name.clone(), + key_head_dim: dims.ssm_state_size, + key_head_count: dims.ssm_group_count, + value_head_dim, + value_head_count: dims.ssm_time_step_rank, + rms_epsilon: cfg.attention_layer_norm_rms_epsilon, + residual: true, + }) +} + +pub fn qwen35moe_first_recurrent_block_spec( + model: &LlamaModel, +) -> Result<(u32, DeltaNetRecurrentBlockSpec)> { + let tensors = model.qwen35moe_tensors()?; + let layer = tensors + .layers + .iter() + .find(|layer| layer.kind == Qwen35MoeLayerKind::Recurrent) + .ok_or_else(|| LlamaError::format("qwen35moe model has no recurrent layers"))?; + Ok(( + layer.index, + qwen35moe_recurrent_block_spec(model, layer.index)?, + )) +} + +pub fn qwen35moe_delta_net_recurrent_decode_spec( + model: &LlamaModel, + layer_index: u32, + max_sequences: u32, + r_type: TensorType, + s_type: TensorType, +) -> Result { + Ok(DeltaNetRecurrentDecodeSpec { + block: qwen35moe_recurrent_block_spec(model, layer_index)?, + cache: DeltaNetRecurrentStateSpec { + max_sequences, + r_type, + s_type, + }, + }) +} + +pub fn qwen35moe_moe_ffn_spec(model: &LlamaModel, layer_index: u32) -> Result { + let tensors = model.qwen35moe_tensors()?; + let dims = Qwen35MoeDims::from_model(model, &tensors)?; + let cfg = model.require_qwen35moe()?; + let layer = tensors + .layers + .iter() + .find(|layer| layer.index == layer_index) + .ok_or_else(|| LlamaError::format(format!("missing qwen35moe layer {}", layer_index)))?; + + Ok(MoeFfnSpec { + input: ProbeInputKind::Embeddings { + hidden_size: dims.embedding_length, + input_type: TensorType::F32, + }, + input_norm: Some(RmsNormSpec { + weight_name: layer.post_attention_norm.name.clone(), + epsilon: cfg.attention_layer_norm_rms_epsilon, + }), + router_proj_name: layer.moe.ffn_gate_inp.name.clone(), + expert_count: dims.expert_count, + expert_used_count: dims.expert_used_count, + gating_func: ExpertGatingFunc::SoftMax, + normalize_selected_weights: true, + weight_scale: 1.0, + merged_gate_up_proj_name: layer.moe.ffn_gate_up_exps.as_ref().map(|t| t.name.clone()), + gate_proj_name: layer.moe.ffn_gate_exps.as_ref().map(|t| t.name.clone()), + up_proj_name: layer + .moe + .ffn_up_exps + .as_ref() + .map(|t| t.name.clone()) + .unwrap_or_default(), + down_proj_name: layer.moe.ffn_down_exps.name.clone(), + activation: UnaryOp::Silu, + shared_expert: Some(MoeSharedExpertSpec { + ffn: DenseGatedFfnSpec { + gate_proj_name: layer.moe.ffn_gate_shexp.name.clone(), + up_proj_name: layer.moe.ffn_up_shexp.name.clone(), + down_proj_name: layer.moe.ffn_down_shexp.name.clone(), + gate_activation: UnaryOp::Silu, + }, + output_gate_name: Some(layer.moe.ffn_gate_inp_shexp.name.clone()), + output_gate_activation: UnaryOp::Sigmoid, + }), + }) +} + +pub fn qwen35moe_first_moe_ffn_spec(model: &LlamaModel) -> Result<(u32, MoeFfnSpec)> { + let tensors = model.qwen35moe_tensors()?; + let layer = tensors + .layers + .first() + .ok_or_else(|| LlamaError::format("qwen35moe model has no layers"))?; + Ok((layer.index, qwen35moe_moe_ffn_spec(model, layer.index)?)) +} + +pub fn qwen35moe_attention_block_layout( + model: &LlamaModel, + layer_index: u32, +) -> Result { + let tensors = model.qwen35moe_tensors()?; + let layer = tensors + .layers + .iter() + .find(|layer| layer.index == layer_index) + .ok_or_else(|| LlamaError::format(format!("missing qwen35moe layer {}", layer_index)))?; + let attention = layer.attention.as_ref().ok_or_else(|| { + LlamaError::format(format!( + "qwen35moe layer {} is {} and does not expose full-attention tensors", + layer_index, + layer.kind.name() + )) + })?; + + GgufWeightLayout::from_tensors(vec![ + tensors.globals.token_embd.clone(), + layer.attn_norm.clone(), + attention.wq.clone(), + attention.wk.clone(), + attention.wv.clone(), + attention.wo.clone(), + attention.attn_q_norm.clone(), + attention.attn_k_norm.clone(), + ]) +} + +pub fn qwen35moe_recurrent_block_layout( + model: &LlamaModel, + layer_index: u32, +) -> Result { + let tensors = model.qwen35moe_tensors()?; + let layer = tensors + .layers + .iter() + .find(|layer| layer.index == layer_index) + .ok_or_else(|| LlamaError::format(format!("missing qwen35moe layer {}", layer_index)))?; + let recurrent = layer.recurrent.as_ref().ok_or_else(|| { + LlamaError::format(format!( + "qwen35moe layer {} is {} and does not expose recurrent tensors", + layer_index, + layer.kind.name() + )) + })?; + + GgufWeightLayout::from_tensors(vec![ + tensors.globals.token_embd.clone(), + layer.attn_norm.clone(), + recurrent.wqkv.clone(), + recurrent.wqkv_gate.clone(), + recurrent.ssm_conv1d.clone(), + recurrent.ssm_dt.clone(), + recurrent.ssm_a.clone(), + recurrent.ssm_beta.clone(), + recurrent.ssm_alpha.clone(), + recurrent.ssm_norm.clone(), + recurrent.ssm_out.clone(), + ]) +} + +pub fn qwen35moe_moe_ffn_layout(model: &LlamaModel, layer_index: u32) -> Result { + let tensors = model.qwen35moe_tensors()?; + let layer = tensors + .layers + .iter() + .find(|layer| layer.index == layer_index) + .ok_or_else(|| LlamaError::format(format!("missing qwen35moe layer {}", layer_index)))?; + + let mut weights = vec![ + layer.post_attention_norm.clone(), + layer.moe.ffn_gate_inp.clone(), + layer.moe.ffn_down_exps.clone(), + layer.moe.ffn_gate_inp_shexp.clone(), + layer.moe.ffn_gate_shexp.clone(), + layer.moe.ffn_up_shexp.clone(), + layer.moe.ffn_down_shexp.clone(), + ]; + if let Some(tensor) = &layer.moe.ffn_gate_up_exps { + weights.push(tensor.clone()); + } + if let Some(tensor) = &layer.moe.ffn_gate_exps { + weights.push(tensor.clone()); + } + if let Some(tensor) = &layer.moe.ffn_up_exps { + weights.push(tensor.clone()); + } + + GgufWeightLayout::from_tensors(weights) +} + +pub fn qwen35moe_hybrid_cache_spec( + model: &LlamaModel, + n_ctx_seq: u32, + n_seq_max: u32, + attention_k_type: TensorType, + attention_v_type: TensorType, + recurrent_r_type: TensorType, + recurrent_s_type: TensorType, +) -> Result { + Ok(qwen35moe_hybrid_cache_template(model)?.materialize( + HybridCacheShape { + n_ctx_seq, + n_seq_max, + }, + HybridCacheTypes { + attention_k_type, + attention_v_type, + recurrent_r_type, + recurrent_s_type, + }, + )) +} + +pub fn qwen35moe_hybrid_cache_template(model: &LlamaModel) -> Result { + let tensors = model.qwen35moe_tensors()?; + let dims = Qwen35MoeDims::from_model(model, &tensors)?; + + let attention_layers = tensors + .layers + .iter() + .filter(|layer| layer.kind == Qwen35MoeLayerKind::Attention) + .map(|layer| layer.index) + .collect(); + let recurrent_layers = tensors + .layers + .iter() + .filter(|layer| layer.kind == Qwen35MoeLayerKind::Recurrent) + .map(|layer| layer.index) + .collect(); + + Ok(HybridCacheTemplate { + attention_layers, + recurrent_layers, + attention_k_width: dims.attention_k_width(), + attention_v_width: dims.attention_v_width(), + recurrent_r_width: dims.recurrent_conv_width()?, + recurrent_s_width: dims.recurrent_state_width()?, + }) +} + +pub fn qwen35moe_hybrid_decode_spec( + model: &LlamaModel, + max_context: u32, + max_sequences: u32, + attention_k_type: TensorType, + attention_v_type: TensorType, + recurrent_r_type: TensorType, + recurrent_s_type: TensorType, +) -> Result { + let tensors = model.qwen35moe_tensors()?; + let cfg = model.require_qwen35moe()?; + + let mut layers = Vec::with_capacity(tensors.layers.len()); + for layer in &tensors.layers { + let ffn = qwen35moe_moe_ffn_spec(model, layer.index)?; + match layer.kind { + Qwen35MoeLayerKind::Attention => layers.push(HybridLayerSpec::Attention { + layer_index: layer.index, + decode: qwen35moe_attention_decode_spec( + model, + layer.index, + max_context, + max_sequences, + attention_k_type, + attention_v_type, + )?, + ffn, + }), + Qwen35MoeLayerKind::Recurrent => layers.push(HybridLayerSpec::Recurrent { + layer_index: layer.index, + decode: qwen35moe_delta_net_recurrent_decode_spec( + model, + layer.index, + max_sequences, + recurrent_r_type, + recurrent_s_type, + )?, + ffn, + }), + } + } + + Ok(HybridDecodeSpec { + input: ProbeInputKind::TokenIds { + token_embedding_name: tensors.globals.token_embd.name.clone(), + }, + output_norm_name: tensors.globals.output_norm.name.clone(), + output_name: tensors.globals.output.name.clone(), + rms_epsilon: cfg.attention_layer_norm_rms_epsilon, + layers, + }) +} + +pub fn qwen35moe_execution_plan(model: &LlamaModel) -> Result { + let tensors = model.qwen35moe_tensors()?; + let dims = Qwen35MoeDims::from_model(model, &tensors)?; + let inventory = qwen35moe_inventory(&tensors); + + Ok(ModelExecutionPlan { + architecture: model.architecture.clone(), + embedding_length: dims.embedding_length, + vocab_size: Some(dims.vocab_size), + full_weights: inventory.weight_layout()?, + tail_probe: ModelTailProbePlan { + spec: qwen35moe_embedding_logits_probe_spec(model)?, + weights: GgufWeightLayout::from_tensors(qwen35moe_tail_probe_tensors(&tensors))?, + extra_activation_bytes: 8 << 20, + }, + hybrid_cache: Some(qwen35moe_hybrid_cache_template(model)?), + inventory, + }) +} + +fn qwen35moe_inventory(tensors: &Qwen35MoeTensors) -> ModelTensorInventory { + let mut globals = BTreeMap::new(); + insert_tensor(&mut globals, "token_embd", &tensors.globals.token_embd); + insert_tensor(&mut globals, "output_norm", &tensors.globals.output_norm); + insert_tensor(&mut globals, "output", &tensors.globals.output); + + let layers = tensors + .layers + .iter() + .map(|layer| { + let mut entries = BTreeMap::new(); + insert_tensor(&mut entries, "attn_norm", &layer.attn_norm); + insert_tensor( + &mut entries, + "post_attention_norm", + &layer.post_attention_norm, + ); + + if let Some(attention) = &layer.attention { + insert_tensor(&mut entries, "attn_q", &attention.wq); + insert_tensor(&mut entries, "attn_k", &attention.wk); + insert_tensor(&mut entries, "attn_v", &attention.wv); + insert_tensor(&mut entries, "attn_output", &attention.wo); + insert_tensor(&mut entries, "attn_q_norm", &attention.attn_q_norm); + insert_tensor(&mut entries, "attn_k_norm", &attention.attn_k_norm); + insert_optional_tensor(&mut entries, "attn_q.scale", &attention.scales.wq); + insert_optional_tensor(&mut entries, "attn_k.scale", &attention.scales.wk); + insert_optional_tensor(&mut entries, "attn_v.scale", &attention.scales.wv); + insert_optional_tensor(&mut entries, "attn_output.scale", &attention.scales.wo); + } + + if let Some(recurrent) = &layer.recurrent { + insert_tensor(&mut entries, "attn_qkv", &recurrent.wqkv); + insert_tensor(&mut entries, "attn_gate", &recurrent.wqkv_gate); + insert_tensor(&mut entries, "ssm_conv1d", &recurrent.ssm_conv1d); + insert_tensor(&mut entries, "ssm_dt", &recurrent.ssm_dt); + insert_tensor(&mut entries, "ssm_a", &recurrent.ssm_a); + insert_tensor(&mut entries, "ssm_beta", &recurrent.ssm_beta); + insert_tensor(&mut entries, "ssm_alpha", &recurrent.ssm_alpha); + insert_tensor(&mut entries, "ssm_norm", &recurrent.ssm_norm); + insert_tensor(&mut entries, "ssm_out", &recurrent.ssm_out); + insert_optional_tensor(&mut entries, "attn_qkv.scale", &recurrent.scales.wqkv); + insert_optional_tensor( + &mut entries, + "attn_gate.scale", + &recurrent.scales.wqkv_gate, + ); + insert_optional_tensor(&mut entries, "ssm_out.scale", &recurrent.scales.ssm_out); + insert_optional_tensor( + &mut entries, + "ssm_alpha.scale", + &recurrent.scales.ssm_alpha, + ); + insert_optional_tensor(&mut entries, "ssm_beta.scale", &recurrent.scales.ssm_beta); + } + + insert_tensor(&mut entries, "ffn_gate_inp", &layer.moe.ffn_gate_inp); + insert_optional_tensor( + &mut entries, + "ffn_gate_up_exps", + &layer.moe.ffn_gate_up_exps, + ); + insert_optional_tensor(&mut entries, "ffn_gate_exps", &layer.moe.ffn_gate_exps); + insert_optional_tensor(&mut entries, "ffn_up_exps", &layer.moe.ffn_up_exps); + insert_tensor(&mut entries, "ffn_down_exps", &layer.moe.ffn_down_exps); + insert_tensor( + &mut entries, + "ffn_gate_inp_shexp", + &layer.moe.ffn_gate_inp_shexp, + ); + insert_tensor(&mut entries, "ffn_gate_shexp", &layer.moe.ffn_gate_shexp); + insert_tensor(&mut entries, "ffn_up_shexp", &layer.moe.ffn_up_shexp); + insert_tensor(&mut entries, "ffn_down_shexp", &layer.moe.ffn_down_shexp); + insert_optional_tensor( + &mut entries, + "ffn_gate_exps.scale", + &layer.moe.scales.ffn_gate_exps, + ); + insert_optional_tensor( + &mut entries, + "ffn_up_exps.scale", + &layer.moe.scales.ffn_up_exps, + ); + insert_optional_tensor( + &mut entries, + "ffn_down_exps.scale", + &layer.moe.scales.ffn_down_exps, + ); + insert_optional_tensor( + &mut entries, + "ffn_gate_shexp.scale", + &layer.moe.scales.ffn_gate_shexp, + ); + insert_optional_tensor( + &mut entries, + "ffn_up_shexp.scale", + &layer.moe.scales.ffn_up_shexp, + ); + insert_optional_tensor( + &mut entries, + "ffn_down_shexp.scale", + &layer.moe.scales.ffn_down_shexp, + ); + + ModelLayerInventory { + index: layer.index, + role: layer_role(layer.kind), + tensors: entries, + } + }) + .collect(); + + ModelTensorInventory { globals, layers } +} + +fn qwen35moe_tail_probe_tensors(tensors: &Qwen35MoeTensors) -> Vec { + vec![ + tensors.globals.output_norm.clone(), + tensors.globals.output.clone(), + ] +} + +fn layer_role(kind: Qwen35MoeLayerKind) -> ModelLayerRole { + match kind { + Qwen35MoeLayerKind::Attention => ModelLayerRole::Attention, + Qwen35MoeLayerKind::Recurrent => ModelLayerRole::Recurrent, + } +} + +fn insert_tensor( + tensors: &mut BTreeMap, + key: &str, + tensor: &crate::GgufTensorInfo, +) { + tensors.insert(key.to_owned(), tensor.clone()); +} + +fn insert_optional_tensor( + tensors: &mut BTreeMap, + key: &str, + tensor: &Option, +) { + if let Some(tensor) = tensor { + insert_tensor(tensors, key, tensor); + } +} + +fn rope_sections_i32(sections: &[u32]) -> Result<[i32; 4]> { + let mut out = [0_i32; 4]; + for (index, slot) in out.iter_mut().enumerate() { + *slot = i32::try_from(*sections.get(index).unwrap_or(&0)).map_err(|_| { + LlamaError::format(format!( + "rope section {} does not fit in i32", + sections.get(index).copied().unwrap_or_default() + )) + })?; + } + Ok(out) +} diff --git a/libs/llama/src/runtime.rs b/libs/llama/src/runtime.rs new file mode 100644 index 0000000000..43003c67c2 --- /dev/null +++ b/libs/llama/src/runtime.rs @@ -0,0 +1,4797 @@ +use std::collections::BTreeMap; + +use makepad_ggml::{ + backend::metal::{ + execute_compiled_graph, prepare_graph, try_get_rows_ggml_bytes, try_matmul_nt_ggml_bytes, + try_rms_norm_mul_f32, BufferStorageMode, MetalCompiledGraph, MetalDeviceFeatures, + MetalGraphSession, MetalGraphTensorWrite, MetalPreparedGraph, MetalRuntime, + }, + f16_to_f32, f32_to_f16, ggml_row_size_for_type, BufferUsage, Context, Graph, Op, Prec, Tensor, + TensorId, TensorLayout, TensorType, UnaryOp, GGML_ROPE_TYPE_IMROPE, GGML_ROPE_TYPE_MROPE, +}; + +use crate::error::{LlamaError, Result}; +use crate::weights::LoadedGgufWeights; + +#[derive(Clone, Debug)] +pub enum ProbeInputKind { + TokenIds { + token_embedding_name: String, + }, + Embeddings { + hidden_size: u32, + input_type: TensorType, + }, +} + +#[derive(Clone, Debug)] +pub struct LogitsProbeSpec { + pub input: ProbeInputKind, + pub output_norm_name: String, + pub output_name: String, + pub rms_epsilon: f32, +} + +#[derive(Clone, Copy, Debug)] +pub struct GraphBatch { + pub n_tokens: usize, + pub n_outputs: usize, +} + +#[derive(Clone, Debug)] +pub struct LogitsProbeGraph { + pub graph: Graph, + pub input_primary: TensorId, + pub output_ids: TensorId, + pub input_embed: TensorId, + pub selected_embed: TensorId, + pub result_norm: TensorId, + pub result_output: TensorId, +} + +pub struct CompiledLogitsProbeMetal { + spec: LogitsProbeSpec, + probe: LogitsProbeGraph, + session: MetalGraphSession, +} + +impl CompiledLogitsProbeMetal { + pub fn runtime(&self) -> &MetalRuntime { + self.session.runtime() + } + + pub fn probe(&self) -> &LogitsProbeGraph { + &self.probe + } + + pub fn execute( + &self, + ctx: &Context, + input: LogitsProbeInput<'_>, + output_ids: &[i32], + ) -> Result { + execute_prepared_logits_probe_metal( + self.session.runtime(), + ctx, + &self.spec, + &self.probe, + self.session.compiled(), + input, + output_ids, + ) + } +} + +pub enum LogitsProbeInput<'a> { + TokenIds(&'a [i32]), + EmbeddingsF32 { data: &'a [f32], n_tokens: usize }, +} + +#[derive(Clone, Debug)] +pub struct LogitsProbeRun { + pub logits: Vec, + pub n_outputs: usize, + pub vocab_size: usize, +} + +#[derive(Clone, Debug)] +pub enum AttentionQueryLayout { + Plain, + InterleavedQueryGate { gate_activation: UnaryOp }, +} + +#[derive(Clone, Debug)] +pub struct AttentionRopeSpec { + pub n_dims: i32, + pub sections: [i32; 4], + pub mode: i32, + pub n_ctx_orig: i32, + pub freq_base: f32, + pub freq_scale: f32, + pub ext_factor: f32, + pub attn_factor: f32, + pub beta_fast: f32, + pub beta_slow: f32, +} + +fn rope_position_component_count(rope: &AttentionRopeSpec) -> usize { + if rope.mode == GGML_ROPE_TYPE_IMROPE || (rope.mode & GGML_ROPE_TYPE_MROPE) != 0 { + 4 + } else { + 1 + } +} + +fn rope_position_tensor_len(rope: &AttentionRopeSpec, n_tokens: usize) -> Result { + let n_positions = n_tokens + .checked_mul(rope_position_component_count(rope)) + .ok_or_else(|| LlamaError::format("overflow computing rope position tensor length"))?; + i64::try_from(n_positions) + .map_err(|_| LlamaError::format("rope position length does not fit in i64")) +} + +fn encode_rope_positions( + rope: &AttentionRopeSpec, + positions: &[i32], + n_tokens: usize, +) -> Result> { + let n_components = rope_position_component_count(rope); + if n_components == 1 { + if positions.len() != n_tokens { + return Err(LlamaError::format(format!( + "rope positions length mismatch: got {}, expected {}", + positions.len(), + n_tokens + ))); + } + return Ok(positions.to_vec()); + } + + let expanded_len = n_tokens + .checked_mul(n_components) + .ok_or_else(|| LlamaError::format("overflow computing expanded rope positions"))?; + if positions.len() == expanded_len { + return Ok(positions.to_vec()); + } + if positions.len() != n_tokens { + return Err(LlamaError::format(format!( + "mrope positions length mismatch: got {}, expected {} or {}", + positions.len(), + n_tokens, + expanded_len + ))); + } + + let mut expanded = vec![0_i32; expanded_len]; + expanded[..n_tokens].copy_from_slice(positions); + expanded[n_tokens..2 * n_tokens].copy_from_slice(positions); + expanded[2 * n_tokens..3 * n_tokens].copy_from_slice(positions); + Ok(expanded) +} + +fn causal_mask_f16_bytes(n_tokens: usize) -> Vec { + let mut bytes = Vec::with_capacity(n_tokens * n_tokens * std::mem::size_of::()); + let zero = f32_to_f16(0.0); + let neg_inf = f32_to_f16(f32::NEG_INFINITY); + for query in 0..n_tokens { + for key in 0..n_tokens { + let value = if key > query { neg_inf } else { zero }; + bytes.extend_from_slice(&value.to_le_bytes()); + } + } + bytes +} + +fn causal_mask_f32_bytes(n_tokens: usize) -> Vec { + let mut bytes = Vec::with_capacity(n_tokens * n_tokens * std::mem::size_of::()); + for query in 0..n_tokens { + for key in 0..n_tokens { + let value = if key > query { f32::NEG_INFINITY } else { 0.0 }; + bytes.extend_from_slice(&value.to_le_bytes()); + } + } + bytes +} + +fn position_causal_mask_f16_bytes(key_count: usize, positions: &[i32]) -> Result> { + let mut bytes = Vec::with_capacity( + positions + .len() + .checked_mul(key_count) + .and_then(|v| v.checked_mul(std::mem::size_of::())) + .ok_or_else(|| LlamaError::format("overflow computing attention decode mask bytes"))?, + ); + let zero = f32_to_f16(0.0); + let neg_inf = f32_to_f16(f32::NEG_INFINITY); + for &position in positions { + let position = usize::try_from(position) + .map_err(|_| LlamaError::format(format!("negative attention position {}", position)))?; + if position >= key_count { + return Err(LlamaError::format(format!( + "attention position {} exceeds key_count {}", + position, key_count + ))); + } + for key in 0..key_count { + let value = if key > position { neg_inf } else { zero }; + bytes.extend_from_slice(&value.to_le_bytes()); + } + } + Ok(bytes) +} + +fn position_causal_mask_f32_bytes(key_count: usize, positions: &[i32]) -> Result> { + let mut bytes = Vec::with_capacity( + positions + .len() + .checked_mul(key_count) + .and_then(|v| v.checked_mul(std::mem::size_of::())) + .ok_or_else(|| LlamaError::format("overflow computing attention decode mask bytes"))?, + ); + for &position in positions { + let position = usize::try_from(position) + .map_err(|_| LlamaError::format(format!("negative attention position {}", position)))?; + if position >= key_count { + return Err(LlamaError::format(format!( + "attention position {} exceeds key_count {}", + position, key_count + ))); + } + for key in 0..key_count { + let value = if key > position { + f32::NEG_INFINITY + } else { + 0.0 + }; + bytes.extend_from_slice(&value.to_le_bytes()); + } + } + Ok(bytes) +} + +fn should_use_flash_attention(n_tokens: usize) -> bool { + n_tokens == 1 +} + +fn attention_mask_tensor_type(n_tokens: usize) -> TensorType { + if should_use_flash_attention(n_tokens) { + TensorType::F16 + } else { + TensorType::F32 + } +} + +fn attention_mask_bytes_for_tensor( + ctx: &Context, + tensor_id: TensorId, + n_tokens: usize, +) -> Result> { + let tensor = require_tensor(ctx, tensor_id)?; + match tensor.desc.ty { + TensorType::F16 => Ok(causal_mask_f16_bytes(n_tokens)), + TensorType::F32 => Ok(causal_mask_f32_bytes(n_tokens)), + other => Err(LlamaError::unsupported(format!( + "unsupported attention block mask tensor type {}", + other.name() + ))), + } +} + +fn position_attention_mask_bytes_for_tensor( + ctx: &Context, + tensor_id: TensorId, + key_count: usize, + positions: &[i32], +) -> Result> { + let tensor = require_tensor(ctx, tensor_id)?; + match tensor.desc.ty { + TensorType::F16 => position_causal_mask_f16_bytes(key_count, positions), + TensorType::F32 => position_causal_mask_f32_bytes(key_count, positions), + other => Err(LlamaError::unsupported(format!( + "unsupported attention decode mask tensor type {}", + other.name() + ))), + } +} + +#[derive(Clone, Debug)] +pub struct AttentionBlockSpec { + pub input: ProbeInputKind, + pub input_norm_name: String, + pub q_proj_name: String, + pub q_layout: AttentionQueryLayout, + pub k_proj_name: String, + pub v_proj_name: String, + pub output_proj_name: String, + pub q_norm_name: Option, + pub k_norm_name: Option, + pub q_head_dim: u32, + pub q_head_count: u32, + pub k_head_dim: u32, + pub kv_head_count: u32, + pub v_head_dim: u32, + pub rms_epsilon: f32, + pub rope: Option, + pub causal: bool, + pub residual: bool, +} + +#[derive(Clone, Debug)] +pub struct AttentionBlockGraph { + pub graph: Graph, + pub input_primary: TensorId, + pub input_positions: Option, + pub input_mask: Option, + pub input_embed: TensorId, + pub result_output: TensorId, +} + +pub struct CompiledAttentionBlockMetal { + spec: AttentionBlockSpec, + block: AttentionBlockGraph, + session: MetalGraphSession, +} + +impl CompiledAttentionBlockMetal { + pub fn runtime(&self) -> &MetalRuntime { + self.session.runtime() + } + + pub fn block(&self) -> &AttentionBlockGraph { + &self.block + } + + pub fn execute( + &self, + ctx: &Context, + input: LogitsProbeInput<'_>, + positions: &[i32], + ) -> Result { + execute_prepared_attention_block_metal( + self.session.runtime(), + ctx, + &self.spec, + &self.block, + self.session.compiled(), + input, + positions, + ) + } +} + +#[derive(Clone, Debug)] +pub struct AttentionBlockRun { + pub hidden: Vec, + pub n_tokens: usize, + pub hidden_size: usize, +} + +#[derive(Clone, Debug)] +pub struct AttentionKvCacheSpec { + pub max_context: u32, + pub max_sequences: u32, + pub k_type: TensorType, + pub v_type: TensorType, +} + +#[derive(Clone, Debug)] +pub struct AttentionDecodeSpec { + pub block: AttentionBlockSpec, + pub cache: AttentionKvCacheSpec, +} + +#[derive(Clone, Debug)] +pub struct AttentionDecodeGraph { + pub graph: Graph, + pub input_primary: TensorId, + pub input_positions: TensorId, + pub input_rope_positions: Option, + pub input_mask: Option, + pub k_cache: TensorId, + pub v_cache: TensorId, + pub k_cache_view: TensorId, + pub v_cache_view: TensorId, + pub result_output: TensorId, +} + +pub struct CompiledAttentionDecodeMetal { + spec: AttentionDecodeSpec, + decode: AttentionDecodeGraph, + session: MetalGraphSession, +} + +impl CompiledAttentionDecodeMetal { + pub fn runtime(&self) -> &MetalRuntime { + self.session.runtime() + } + + pub fn decode(&self) -> &AttentionDecodeGraph { + &self.decode + } + + pub fn execute( + &self, + ctx: &mut Context, + input: LogitsProbeInput<'_>, + positions: &[i32], + cache_tokens: usize, + ) -> Result { + execute_prepared_attention_decode_metal( + self.session.runtime(), + ctx, + &self.spec, + &self.decode, + self.session.compiled(), + input, + positions, + cache_tokens, + ) + } +} + +#[derive(Clone, Debug)] +pub struct DeltaNetRecurrentBlockSpec { + pub input: ProbeInputKind, + pub input_norm_name: String, + pub qkv_proj_name: String, + pub z_proj_name: String, + pub beta_proj_name: String, + pub alpha_proj_name: String, + pub dt_bias_name: String, + pub a_name: String, + pub conv_kernel_name: String, + pub norm_name: String, + pub output_proj_name: String, + pub key_head_dim: u32, + pub key_head_count: u32, + pub value_head_dim: u32, + pub value_head_count: u32, + pub rms_epsilon: f32, + pub residual: bool, +} + +#[derive(Clone, Debug)] +pub struct DeltaNetRecurrentStateSpec { + pub max_sequences: u32, + pub r_type: TensorType, + pub s_type: TensorType, +} + +#[derive(Clone, Debug)] +pub struct DeltaNetRecurrentDecodeSpec { + pub block: DeltaNetRecurrentBlockSpec, + pub cache: DeltaNetRecurrentStateSpec, +} + +#[derive(Clone, Debug)] +pub struct DeltaNetRecurrentDecodeGraph { + pub graph: Graph, + pub input_primary: TensorId, + pub r_cache: TensorId, + pub s_cache: TensorId, + pub result_output: TensorId, +} + +pub struct CompiledDeltaNetRecurrentDecodeMetal { + spec: DeltaNetRecurrentDecodeSpec, + decode: DeltaNetRecurrentDecodeGraph, + session: MetalGraphSession, +} + +impl CompiledDeltaNetRecurrentDecodeMetal { + pub fn runtime(&self) -> &MetalRuntime { + self.session.runtime() + } + + pub fn decode(&self) -> &DeltaNetRecurrentDecodeGraph { + &self.decode + } + + pub fn execute( + &self, + ctx: &mut Context, + input: LogitsProbeInput<'_>, + ) -> Result { + execute_prepared_delta_net_recurrent_decode_metal( + self.session.runtime(), + ctx, + &self.spec, + &self.decode, + self.session.compiled(), + input, + ) + } +} + +#[derive(Clone, Debug)] +pub struct RmsNormSpec { + pub weight_name: String, + pub epsilon: f32, +} + +#[derive(Clone, Copy, Debug)] +pub enum ExpertGatingFunc { + SoftMax, + Sigmoid, + Identity, +} + +#[derive(Clone, Debug)] +pub struct DenseGatedFfnSpec { + pub gate_proj_name: String, + pub up_proj_name: String, + pub down_proj_name: String, + pub gate_activation: UnaryOp, +} + +#[derive(Clone, Debug)] +pub struct MoeSharedExpertSpec { + pub ffn: DenseGatedFfnSpec, + pub output_gate_name: Option, + pub output_gate_activation: UnaryOp, +} + +#[derive(Clone, Debug)] +pub struct MoeFfnSpec { + pub input: ProbeInputKind, + pub input_norm: Option, + pub router_proj_name: String, + pub expert_count: u32, + pub expert_used_count: u32, + pub gating_func: ExpertGatingFunc, + pub normalize_selected_weights: bool, + pub weight_scale: f32, + pub merged_gate_up_proj_name: Option, + pub gate_proj_name: Option, + pub up_proj_name: String, + pub down_proj_name: String, + pub activation: UnaryOp, + pub shared_expert: Option, +} + +#[derive(Clone, Debug)] +pub struct MoeFfnGraph { + pub graph: Graph, + pub input_primary: TensorId, + pub input_embed: TensorId, + pub selected_experts: TensorId, + pub result_output: TensorId, +} + +pub struct CompiledMoeFfnMetal { + spec: MoeFfnSpec, + block: MoeFfnGraph, + session: MetalGraphSession, +} + +impl CompiledMoeFfnMetal { + pub fn runtime(&self) -> &MetalRuntime { + self.session.runtime() + } + + pub fn block(&self) -> &MoeFfnGraph { + &self.block + } + + pub fn execute(&self, ctx: &Context, input: LogitsProbeInput<'_>) -> Result { + execute_prepared_moe_ffn_metal( + self.session.runtime(), + ctx, + &self.spec, + &self.block, + self.session.compiled(), + input, + ) + } +} + +#[derive(Clone, Debug)] +pub struct MoeFfnRun { + pub hidden: Vec, + pub n_tokens: usize, + pub hidden_size: usize, + pub selected_experts: Vec, + pub expert_used_count: usize, +} + +#[derive(Clone, Debug)] +pub enum HybridLayerSpec { + Attention { + layer_index: u32, + decode: AttentionDecodeSpec, + ffn: MoeFfnSpec, + }, + Recurrent { + layer_index: u32, + decode: DeltaNetRecurrentDecodeSpec, + ffn: MoeFfnSpec, + }, +} + +#[derive(Clone, Debug)] +pub struct HybridDecodeSpec { + pub input: ProbeInputKind, + pub output_norm_name: String, + pub output_name: String, + pub rms_epsilon: f32, + pub layers: Vec, +} + +#[derive(Clone, Debug)] +pub struct HybridAttentionCacheView { + pub layer_index: u32, + pub input_mask: Option, + pub k_cache_view: TensorId, + pub v_cache_view: TensorId, + pub k_head_dim: i64, + pub v_head_dim: i64, + pub kv_head_count: i64, + pub max_context: usize, + pub max_sequences: i64, +} + +#[derive(Clone, Debug)] +pub struct HybridMoeSelection { + pub layer_index: u32, + pub selected_experts: TensorId, + pub expert_used_count: usize, +} + +#[derive(Clone, Debug)] +pub struct HybridDecodeGraph { + pub graph: Graph, + pub input_primary: TensorId, + pub input_positions: Option, + pub input_rope_positions: Option, + pub attention_cache_views: Vec, + pub moe_selected_experts: Vec, + pub state_updates: Vec, + pub result_hidden: TensorId, + pub result_logits: TensorId, +} + +pub struct CompiledHybridDecodeMetal { + spec: HybridDecodeSpec, + decode: HybridDecodeGraph, + session: MetalGraphSession, +} + +impl CompiledHybridDecodeMetal { + pub fn runtime(&self) -> &MetalRuntime { + self.session.runtime() + } + + pub fn decode(&self) -> &HybridDecodeGraph { + &self.decode + } + + pub fn execute( + &self, + ctx: &mut Context, + input: LogitsProbeInput<'_>, + positions: &[i32], + cache_tokens: usize, + ) -> Result { + execute_prepared_hybrid_decode_metal( + self.session.runtime(), + ctx, + &self.spec, + &self.decode, + self.session.compiled(), + input, + positions, + cache_tokens, + ) + } +} + +#[derive(Clone, Debug)] +pub struct HybridDecodeRun { + pub hidden: Vec, + pub logits: Vec, + pub n_tokens: usize, + pub hidden_size: usize, + pub vocab_size: usize, + pub selected_experts: Vec<(u32, Vec)>, +} + +#[derive(Clone, Debug)] +pub struct HybridCacheSpec { + pub n_ctx_seq: u32, + pub n_seq_max: u32, + pub attention_layers: Vec, + pub recurrent_layers: Vec, + pub attention_k_width: u64, + pub attention_v_width: u64, + pub recurrent_r_width: u64, + pub recurrent_s_width: u64, + pub attention_k_type: TensorType, + pub attention_v_type: TensorType, + pub recurrent_r_type: TensorType, + pub recurrent_s_type: TensorType, +} + +#[derive(Clone, Copy, Debug)] +pub struct HybridCacheShape { + pub n_ctx_seq: u32, + pub n_seq_max: u32, +} + +#[derive(Clone, Copy, Debug)] +pub struct HybridCacheTypes { + pub attention_k_type: TensorType, + pub attention_v_type: TensorType, + pub recurrent_r_type: TensorType, + pub recurrent_s_type: TensorType, +} + +#[derive(Clone, Debug)] +pub struct HybridCacheTemplate { + pub attention_layers: Vec, + pub recurrent_layers: Vec, + pub attention_k_width: u64, + pub attention_v_width: u64, + pub recurrent_r_width: u64, + pub recurrent_s_width: u64, +} + +impl HybridCacheTemplate { + pub fn materialize(&self, shape: HybridCacheShape, types: HybridCacheTypes) -> HybridCacheSpec { + HybridCacheSpec { + n_ctx_seq: shape.n_ctx_seq, + n_seq_max: shape.n_seq_max, + attention_layers: self.attention_layers.clone(), + recurrent_layers: self.recurrent_layers.clone(), + attention_k_width: self.attention_k_width, + attention_v_width: self.attention_v_width, + recurrent_r_width: self.recurrent_r_width, + recurrent_s_width: self.recurrent_s_width, + attention_k_type: types.attention_k_type, + attention_v_type: types.attention_v_type, + recurrent_r_type: types.recurrent_r_type, + recurrent_s_type: types.recurrent_s_type, + } + } +} + +#[derive(Clone, Debug)] +pub struct HybridCacheLayout { + pub spec: HybridCacheSpec, + pub attention_k_bytes_per_layer: usize, + pub attention_v_bytes_per_layer: usize, + pub recurrent_r_bytes_per_layer: usize, + pub recurrent_s_bytes_per_layer: usize, + pub total_attention_k_bytes: usize, + pub total_attention_v_bytes: usize, + pub total_recurrent_r_bytes: usize, + pub total_recurrent_s_bytes: usize, + pub total_bytes: usize, +} + +impl HybridCacheLayout { + pub fn new(spec: HybridCacheSpec) -> Result { + let attention_k_bytes_per_layer = bytes_for_elements( + spec.attention_k_type, + spec.attention_k_width + .checked_mul(u64::from(spec.n_ctx_seq)) + .ok_or_else(|| LlamaError::format("overflow computing key-cache elements"))?, + )?; + let attention_v_bytes_per_layer = bytes_for_elements( + spec.attention_v_type, + spec.attention_v_width + .checked_mul(u64::from(spec.n_ctx_seq)) + .ok_or_else(|| LlamaError::format("overflow computing value-cache elements"))?, + )?; + let recurrent_r_bytes_per_layer = bytes_for_elements( + spec.recurrent_r_type, + spec.recurrent_r_width + .checked_mul(u64::from(spec.n_seq_max)) + .ok_or_else(|| LlamaError::format("overflow computing recurrent-r elements"))?, + )?; + let recurrent_s_bytes_per_layer = bytes_for_elements( + spec.recurrent_s_type, + spec.recurrent_s_width + .checked_mul(u64::from(spec.n_seq_max)) + .ok_or_else(|| LlamaError::format("overflow computing recurrent-s elements"))?, + )?; + + let total_attention_k_bytes = attention_k_bytes_per_layer + .checked_mul(spec.attention_layers.len()) + .ok_or_else(|| LlamaError::format("overflow computing total key-cache bytes"))?; + let total_attention_v_bytes = attention_v_bytes_per_layer + .checked_mul(spec.attention_layers.len()) + .ok_or_else(|| LlamaError::format("overflow computing total value-cache bytes"))?; + let total_recurrent_r_bytes = recurrent_r_bytes_per_layer + .checked_mul(spec.recurrent_layers.len()) + .ok_or_else(|| LlamaError::format("overflow computing total recurrent-r bytes"))?; + let total_recurrent_s_bytes = recurrent_s_bytes_per_layer + .checked_mul(spec.recurrent_layers.len()) + .ok_or_else(|| LlamaError::format("overflow computing total recurrent-s bytes"))?; + let total_bytes = total_attention_k_bytes + .checked_add(total_attention_v_bytes) + .and_then(|v| v.checked_add(total_recurrent_r_bytes)) + .and_then(|v| v.checked_add(total_recurrent_s_bytes)) + .ok_or_else(|| LlamaError::format("overflow computing total hybrid-cache bytes"))?; + + Ok(Self { + spec, + attention_k_bytes_per_layer, + attention_v_bytes_per_layer, + recurrent_r_bytes_per_layer, + recurrent_s_bytes_per_layer, + total_attention_k_bytes, + total_attention_v_bytes, + total_recurrent_r_bytes, + total_recurrent_s_bytes, + total_bytes, + }) + } +} + +pub fn build_logits_probe_graph( + ctx: &mut Context, + tensor_ids: &BTreeMap, + spec: &LogitsProbeSpec, + batch: GraphBatch, +) -> Result { + let input_primary = match &spec.input { + ProbeInputKind::TokenIds { .. } => ctx + .new_named_tensor( + "probe.inp_tokens", + TensorType::I32, + 1, + &[batch.n_tokens as i64], + BufferUsage::Activations, + ) + .map_err(LlamaError::format)?, + ProbeInputKind::Embeddings { + hidden_size, + input_type, + } => ctx + .new_named_tensor( + "probe.inp_embd", + *input_type, + 2, + &[i64::from(*hidden_size), batch.n_tokens as i64], + BufferUsage::Activations, + ) + .map_err(LlamaError::format)?, + }; + mark_input(ctx, input_primary)?; + + let output_ids = ctx + .new_named_tensor( + "probe.out_ids", + TensorType::I32, + 1, + &[batch.n_outputs as i64], + BufferUsage::Activations, + ) + .map_err(LlamaError::format)?; + mark_input(ctx, output_ids)?; + + let input_embed = match &spec.input { + ProbeInputKind::TokenIds { + token_embedding_name, + } => { + let token_embd = require_tensor_id(tensor_ids, token_embedding_name)?; + ctx.get_rows(token_embd, input_primary, BufferUsage::Activations) + .map_err(LlamaError::format)? + } + ProbeInputKind::Embeddings { .. } => input_primary, + }; + ctx.set_tensor_name(input_embed, "probe.input_embed") + .map_err(LlamaError::format)?; + + let selected_embed = ctx + .get_rows(input_embed, output_ids, BufferUsage::Activations) + .map_err(LlamaError::format)?; + ctx.set_tensor_name(selected_embed, "probe.selected_embed") + .map_err(LlamaError::format)?; + + let result_norm = ctx + .rms_norm_eps(selected_embed, spec.rms_epsilon, BufferUsage::Activations) + .map_err(LlamaError::format)?; + ctx.set_tensor_name(result_norm, "probe.result_norm") + .map_err(LlamaError::format)?; + + let output_norm = require_tensor_id(tensor_ids, &spec.output_norm_name)?; + let result_norm_scaled = ctx + .binary_like_a(Op::Mul, result_norm, output_norm, BufferUsage::Activations) + .map_err(LlamaError::format)?; + ctx.set_tensor_name(result_norm_scaled, "probe.result_norm_scaled") + .map_err(LlamaError::format)?; + + let output = require_tensor_id(tensor_ids, &spec.output_name)?; + let result_output = ctx + .mul_mat(output, result_norm_scaled, BufferUsage::Activations) + .map_err(LlamaError::format)?; + ctx.set_tensor_name(result_output, "probe.result_output") + .map_err(LlamaError::format)?; + mark_output(ctx, result_output)?; + + let mut graph = Graph::new(); + graph + .build_forward_expand(ctx, result_output) + .map_err(LlamaError::format)?; + + Ok(LogitsProbeGraph { + graph, + input_primary, + output_ids, + input_embed, + selected_embed, + result_norm, + result_output, + }) +} + +pub fn prepare_logits_probe_graph( + ctx: &mut Context, + tensor_ids: &BTreeMap, + spec: &LogitsProbeSpec, + batch: GraphBatch, + features: MetalDeviceFeatures, +) -> Result<(LogitsProbeGraph, MetalPreparedGraph)> { + let probe = build_logits_probe_graph(ctx, tensor_ids, spec, batch)?; + let prepared = prepare_graph(ctx, &probe.graph, features).map_err(LlamaError::format)?; + Ok((probe, prepared)) +} + +pub fn compile_logits_probe_metal( + weights: &mut LoadedGgufWeights, + spec: &LogitsProbeSpec, + batch: GraphBatch, +) -> Result { + let runtime = MetalRuntime::new().map_err(LlamaError::unsupported)?; + let (probe, prepared) = prepare_logits_probe_graph( + &mut weights.ctx, + &weights.tensor_ids, + spec, + batch, + runtime.features(), + )?; + let session = MetalGraphSession::from_runtime( + runtime, + &weights.ctx, + &prepared, + BufferStorageMode::Private, + BufferStorageMode::Private, + ) + .map_err(LlamaError::format)?; + + Ok(CompiledLogitsProbeMetal { + spec: spec.clone(), + probe, + session, + }) +} + +pub fn execute_prepared_logits_probe_metal( + runtime: &MetalRuntime, + ctx: &Context, + spec: &LogitsProbeSpec, + probe: &LogitsProbeGraph, + compiled: &MetalCompiledGraph, + input: LogitsProbeInput<'_>, + output_ids: &[i32], +) -> Result { + if output_ids.is_empty() { + return Err(LlamaError::format( + "logits probe requires at least one output id", + )); + } + + let input_primary = match (&spec.input, input) { + (ProbeInputKind::TokenIds { .. }, LogitsProbeInput::TokenIds(token_ids)) => { + i32_slice_as_bytes(token_ids).to_vec() + } + ( + ProbeInputKind::Embeddings { + hidden_size, + input_type, + }, + LogitsProbeInput::EmbeddingsF32 { data, n_tokens }, + ) => { + if *input_type != TensorType::F32 { + return Err(LlamaError::unsupported(format!( + "graph Metal probe currently expects F32 embeddings, got {}", + input_type.name() + ))); + } + let expected = (*hidden_size as usize) + .checked_mul(n_tokens) + .ok_or_else(|| LlamaError::format("overflow computing embedding input size"))?; + if data.len() != expected { + return Err(LlamaError::format(format!( + "embedding input length mismatch: got {}, expected {}", + data.len(), + expected + ))); + } + f32_slice_as_bytes(data).to_vec() + } + (ProbeInputKind::TokenIds { .. }, LogitsProbeInput::EmbeddingsF32 { .. }) => { + return Err(LlamaError::format( + "logits probe spec expects token ids but embeddings were provided", + )); + } + (ProbeInputKind::Embeddings { .. }, LogitsProbeInput::TokenIds(_)) => { + return Err(LlamaError::format( + "logits probe spec expects embeddings but token ids were provided", + )); + } + }; + + let execution = execute_compiled_graph( + runtime, + ctx, + compiled, + &[ + MetalGraphTensorWrite { + tensor_id: probe.input_primary, + bytes: &input_primary, + }, + MetalGraphTensorWrite { + tensor_id: probe.output_ids, + bytes: i32_slice_as_bytes(output_ids), + }, + ], + &[probe.result_output], + ) + .map_err(LlamaError::format)?; + + let result_bytes = execution + .outputs + .get(&probe.result_output) + .ok_or_else(|| LlamaError::format("compiled probe did not produce result_output bytes"))?; + let logits = f32_bytes_to_vec(result_bytes)?; + let output = ctx + .tensor(probe.result_output) + .ok_or_else(|| LlamaError::format("probe result_output tensor is invalid"))?; + let vocab_size = ne_usize(output, 0)?; + let n_outputs = ne_usize(output, 1)?; + + Ok(LogitsProbeRun { + logits, + n_outputs, + vocab_size, + }) +} + +pub fn execute_logits_probe_graph_metal( + weights: &mut LoadedGgufWeights, + spec: &LogitsProbeSpec, + input: LogitsProbeInput<'_>, + output_ids: &[i32], +) -> Result { + let compiled = compile_logits_probe_metal( + weights, + spec, + GraphBatch { + n_tokens: match &input { + LogitsProbeInput::TokenIds(token_ids) => token_ids.len(), + LogitsProbeInput::EmbeddingsF32 { n_tokens, .. } => *n_tokens, + }, + n_outputs: output_ids.len(), + }, + )?; + compiled.execute(&weights.ctx, input, output_ids) +} + +pub fn execute_logits_probe_graph_metal_cached( + compiled: &CompiledLogitsProbeMetal, + weights: &LoadedGgufWeights, + input: LogitsProbeInput<'_>, + output_ids: &[i32], +) -> Result { + compiled.execute(&weights.ctx, input, output_ids) +} + +fn build_attention_mha_output( + ctx: &mut Context, + q: TensorId, + k: TensorId, + v: TensorId, + input_mask: Option, + q_head_dim: u32, + n_tokens: usize, + prefix: &str, +) -> Result { + if should_use_flash_attention(n_tokens) { + let attn = ctx + .flash_attn_ext( + q, + k, + v, + input_mask, + 1.0f32 / (q_head_dim as f32).sqrt(), + 0.0, + 0.0, + BufferUsage::Activations, + ) + .map_err(LlamaError::format)?; + ctx.flash_attn_ext_set_prec(attn, Prec::F32) + .map_err(LlamaError::format)?; + let attn_tensor = require_tensor(ctx, attn)?.clone(); + let attn = ctx + .reshape( + attn, + &[ + attn_tensor.ne[0] * attn_tensor.ne[1], + attn_tensor.ne[2] * attn_tensor.ne[3], + ], + ) + .map_err(LlamaError::format)?; + ctx.set_tensor_name(attn, format!("{prefix}.attn_flat")) + .map_err(LlamaError::format)?; + return Ok(attn); + } + + let mut kq = ctx + .mul_mat(k, q, BufferUsage::Activations) + .map_err(LlamaError::format)?; + ctx.set_tensor_name(kq, format!("{prefix}.kq")) + .map_err(LlamaError::format)?; + kq = ctx + .scale( + kq, + 1.0f32 / (q_head_dim as f32).sqrt(), + BufferUsage::Activations, + ) + .map_err(LlamaError::format)?; + ctx.set_tensor_name(kq, format!("{prefix}.kq_scaled")) + .map_err(LlamaError::format)?; + if let Some(input_mask) = input_mask { + kq = ctx + .binary_like_a(Op::Add, kq, input_mask, BufferUsage::Activations) + .map_err(LlamaError::format)?; + ctx.set_tensor_name(kq, format!("{prefix}.kq_masked")) + .map_err(LlamaError::format)?; + } + kq = ctx + .soft_max(kq, BufferUsage::Activations) + .map_err(LlamaError::format)?; + ctx.set_tensor_name(kq, format!("{prefix}.kq_soft_max")) + .map_err(LlamaError::format)?; + + let v_transposed = ctx.transpose(v).map_err(LlamaError::format)?; + let v_for_matmul = ctx.cont(v_transposed).map_err(LlamaError::format)?; + ctx.set_tensor_name(v_for_matmul, format!("{prefix}.v_cont")) + .map_err(LlamaError::format)?; + + let kqv = ctx + .mul_mat(v_for_matmul, kq, BufferUsage::Activations) + .map_err(LlamaError::format)?; + ctx.set_tensor_name(kqv, format!("{prefix}.kqv")) + .map_err(LlamaError::format)?; + let attn = ctx.permute(kqv, [0, 2, 1, 3]).map_err(LlamaError::format)?; + let attn_tensor = require_tensor(ctx, attn)?.clone(); + let attn = ctx + .cont_2d( + attn, + attn_tensor.ne[0] * attn_tensor.ne[1], + attn_tensor.ne[2] * attn_tensor.ne[3], + ) + .map_err(LlamaError::format)?; + ctx.set_tensor_name(attn, format!("{prefix}.attn_flat")) + .map_err(LlamaError::format)?; + Ok(attn) +} + +pub fn build_attention_block_graph( + ctx: &mut Context, + tensor_ids: &BTreeMap, + spec: &AttentionBlockSpec, + n_tokens: usize, +) -> Result { + if n_tokens == 0 { + return Err(LlamaError::format( + "attention block graph requires at least one token", + )); + } + + let input_primary = match &spec.input { + ProbeInputKind::TokenIds { .. } => ctx + .new_named_tensor( + "attn.inp_tokens", + TensorType::I32, + 1, + &[n_tokens as i64], + BufferUsage::Activations, + ) + .map_err(LlamaError::format)?, + ProbeInputKind::Embeddings { + hidden_size, + input_type, + } => ctx + .new_named_tensor( + "attn.inp_embd", + *input_type, + 2, + &[i64::from(*hidden_size), n_tokens as i64], + BufferUsage::Activations, + ) + .map_err(LlamaError::format)?, + }; + mark_input(ctx, input_primary)?; + + let input_positions = if spec.rope.is_some() { + let rope = spec.rope.as_ref().unwrap(); + let positions = ctx + .new_named_tensor( + "attn.inp_pos", + TensorType::I32, + 1, + &[rope_position_tensor_len(rope, n_tokens)?], + BufferUsage::Activations, + ) + .map_err(LlamaError::format)?; + mark_input(ctx, positions)?; + Some(positions) + } else { + None + }; + let input_mask = if spec.causal { + let mask = ctx + .new_named_tensor( + "attn.kq_mask", + attention_mask_tensor_type(n_tokens), + 4, + &[n_tokens as i64, n_tokens as i64, 1, 1], + BufferUsage::Activations, + ) + .map_err(LlamaError::format)?; + mark_input(ctx, mask)?; + Some(mask) + } else { + None + }; + + let input_embed = match &spec.input { + ProbeInputKind::TokenIds { + token_embedding_name, + } => { + let token_embd = require_tensor_id(tensor_ids, token_embedding_name)?; + ctx.get_rows(token_embd, input_primary, BufferUsage::Activations) + .map_err(LlamaError::format)? + } + ProbeInputKind::Embeddings { .. } => input_primary, + }; + ctx.set_tensor_name(input_embed, "attn.input_embed") + .map_err(LlamaError::format)?; + + let input_norm = build_rms_norm_mul( + ctx, + tensor_ids, + input_embed, + spec.rms_epsilon, + &spec.input_norm_name, + "attn.input_norm", + )?; + + let q_weight = require_tensor_id(tensor_ids, &spec.q_proj_name)?; + let q_proj = ctx + .mul_mat(q_weight, input_norm, BufferUsage::Activations) + .map_err(LlamaError::format)?; + ctx.set_tensor_name(q_proj, "attn.q_proj") + .map_err(LlamaError::format)?; + + let q_gate_stride = usize::try_from(spec.q_head_dim) + .ok() + .and_then(|v| v.checked_mul(2)) + .and_then(|v| v.checked_mul(std::mem::size_of::())) + .ok_or_else(|| LlamaError::format("overflow computing q/gate stride"))?; + let q_token_stride = q_gate_stride + .checked_mul(usize::try_from(spec.q_head_count).map_err(|_| { + LlamaError::format(format!( + "q_head_count {} does not fit in usize", + spec.q_head_count + )) + })?) + .ok_or_else(|| LlamaError::format("overflow computing q/gate token stride"))?; + let gate_offset = usize::try_from(spec.q_head_dim) + .ok() + .and_then(|v| v.checked_mul(std::mem::size_of::())) + .ok_or_else(|| LlamaError::format("overflow computing gate offset"))?; + + let (mut q_states, gate) = match spec.q_layout { + AttentionQueryLayout::Plain => { + let q = ctx + .reshape( + q_proj, + &[ + i64::from(spec.q_head_dim), + i64::from(spec.q_head_count), + n_tokens as i64, + ], + ) + .map_err(LlamaError::format)?; + (q, None) + } + AttentionQueryLayout::InterleavedQueryGate { gate_activation } => { + let q = ctx + .view_3d( + q_proj, + i64::from(spec.q_head_dim), + i64::from(spec.q_head_count), + n_tokens as i64, + q_gate_stride, + q_token_stride, + 0, + ) + .map_err(LlamaError::format)?; + let gate_view = ctx + .view_3d( + q_proj, + i64::from(spec.q_head_dim), + i64::from(spec.q_head_count), + n_tokens as i64, + q_gate_stride, + q_token_stride, + gate_offset, + ) + .map_err(LlamaError::format)?; + let gate_cont = ctx + .cont_2d( + gate_view, + i64::from(spec.q_head_dim) * i64::from(spec.q_head_count), + n_tokens as i64, + ) + .map_err(LlamaError::format)?; + let gate = ctx + .unary(gate_cont, gate_activation, BufferUsage::Activations) + .map_err(LlamaError::format)?; + ctx.set_tensor_name(gate, "attn.gate") + .map_err(LlamaError::format)?; + (q, Some(gate)) + } + }; + ctx.set_tensor_name(q_states, "attn.q_states") + .map_err(LlamaError::format)?; + let q_pre_store = ctx + .view_2d( + q_states, + i64::from(spec.q_head_dim) * i64::from(spec.q_head_count), + n_tokens as i64, + ctx.tensor(q_states) + .ok_or_else(|| LlamaError::format("invalid q_states tensor"))? + .nb[2], + 0, + ) + .map_err(LlamaError::format)?; + ctx.set_tensor_name(q_pre_store, "attn.q_pre_store") + .map_err(LlamaError::format)?; + + if let Some(q_norm_name) = &spec.q_norm_name { + q_states = build_rms_norm_mul( + ctx, + tensor_ids, + q_states, + spec.rms_epsilon, + q_norm_name, + "attn.q_norm", + )?; + } + let q_norm_store = ctx + .view_2d( + q_states, + i64::from(spec.q_head_dim) * i64::from(spec.q_head_count), + n_tokens as i64, + ctx.tensor(q_states) + .ok_or_else(|| LlamaError::format("invalid q_states tensor"))? + .nb[2], + 0, + ) + .map_err(LlamaError::format)?; + ctx.set_tensor_name(q_norm_store, "attn.q_norm_store") + .map_err(LlamaError::format)?; + + let k_weight = require_tensor_id(tensor_ids, &spec.k_proj_name)?; + let mut k_states = ctx + .mul_mat(k_weight, input_norm, BufferUsage::Activations) + .map_err(LlamaError::format)?; + k_states = ctx + .reshape( + k_states, + &[ + i64::from(spec.k_head_dim), + i64::from(spec.kv_head_count), + n_tokens as i64, + ], + ) + .map_err(LlamaError::format)?; + ctx.set_tensor_name(k_states, "attn.k_states") + .map_err(LlamaError::format)?; + + if let Some(k_norm_name) = &spec.k_norm_name { + k_states = build_rms_norm_mul( + ctx, + tensor_ids, + k_states, + spec.rms_epsilon, + k_norm_name, + "attn.k_norm", + )?; + } + let k_norm_store = ctx + .view_2d( + k_states, + i64::from(spec.k_head_dim) * i64::from(spec.kv_head_count), + n_tokens as i64, + ctx.tensor(k_states) + .ok_or_else(|| LlamaError::format("invalid k_states tensor"))? + .nb[2], + 0, + ) + .map_err(LlamaError::format)?; + ctx.set_tensor_name(k_norm_store, "attn.k_norm_store") + .map_err(LlamaError::format)?; + + let v_weight = require_tensor_id(tensor_ids, &spec.v_proj_name)?; + let mut v_states = ctx + .mul_mat(v_weight, input_norm, BufferUsage::Activations) + .map_err(LlamaError::format)?; + v_states = ctx + .reshape( + v_states, + &[ + i64::from(spec.v_head_dim), + i64::from(spec.kv_head_count), + n_tokens as i64, + ], + ) + .map_err(LlamaError::format)?; + ctx.set_tensor_name(v_states, "attn.v_states") + .map_err(LlamaError::format)?; + + if let Some(rope) = &spec.rope { + let positions = input_positions.ok_or_else(|| { + LlamaError::format( + "attention block rope was requested without an input positions tensor", + ) + })?; + q_states = ctx + .rope_multi( + q_states, + positions, + None, + rope.n_dims, + rope.sections, + rope.mode, + rope.n_ctx_orig, + rope.freq_base, + rope.freq_scale, + rope.ext_factor, + rope.attn_factor, + rope.beta_fast, + rope.beta_slow, + BufferUsage::Activations, + ) + .map_err(LlamaError::format)?; + k_states = ctx + .rope_multi( + k_states, + positions, + None, + rope.n_dims, + rope.sections, + rope.mode, + rope.n_ctx_orig, + rope.freq_base, + rope.freq_scale, + rope.ext_factor, + rope.attn_factor, + rope.beta_fast, + rope.beta_slow, + BufferUsage::Activations, + ) + .map_err(LlamaError::format)?; + } + + let q_store = ctx + .view_2d( + q_states, + i64::from(spec.q_head_dim) * i64::from(spec.q_head_count), + n_tokens as i64, + ctx.tensor(q_states) + .ok_or_else(|| LlamaError::format("invalid q_states tensor"))? + .nb[2], + 0, + ) + .map_err(LlamaError::format)?; + ctx.set_tensor_name(q_store, "attn.q_store") + .map_err(LlamaError::format)?; + let k_store = ctx + .view_2d( + k_states, + i64::from(spec.k_head_dim) * i64::from(spec.kv_head_count), + n_tokens as i64, + ctx.tensor(k_states) + .ok_or_else(|| LlamaError::format("invalid k_states tensor"))? + .nb[2], + 0, + ) + .map_err(LlamaError::format)?; + ctx.set_tensor_name(k_store, "attn.k_store") + .map_err(LlamaError::format)?; + let v_store = ctx + .view_2d( + v_states, + i64::from(spec.v_head_dim) * i64::from(spec.kv_head_count), + n_tokens as i64, + ctx.tensor(v_states) + .ok_or_else(|| LlamaError::format("invalid v_states tensor"))? + .nb[2], + 0, + ) + .map_err(LlamaError::format)?; + ctx.set_tensor_name(v_store, "attn.v_store") + .map_err(LlamaError::format)?; + + q_states = ctx + .reshape( + q_states, + &[ + i64::from(spec.q_head_dim), + i64::from(spec.q_head_count), + n_tokens as i64, + 1, + ], + ) + .map_err(LlamaError::format)?; + ctx.set_tensor_name(q_states, "attn.q_states_4d") + .map_err(LlamaError::format)?; + k_states = ctx + .reshape( + k_states, + &[ + i64::from(spec.k_head_dim), + i64::from(spec.kv_head_count), + n_tokens as i64, + 1, + ], + ) + .map_err(LlamaError::format)?; + ctx.set_tensor_name(k_states, "attn.k_states_4d") + .map_err(LlamaError::format)?; + v_states = ctx + .reshape( + v_states, + &[ + i64::from(spec.v_head_dim), + i64::from(spec.kv_head_count), + n_tokens as i64, + 1, + ], + ) + .map_err(LlamaError::format)?; + ctx.set_tensor_name(v_states, "attn.v_states_4d") + .map_err(LlamaError::format)?; + + let q_attn = ctx + .permute(q_states, [0, 2, 1, 3]) + .map_err(LlamaError::format)?; + ctx.set_tensor_name(q_attn, "attn.q_attn") + .map_err(LlamaError::format)?; + let k_attn = ctx + .permute(k_states, [0, 2, 1, 3]) + .map_err(LlamaError::format)?; + ctx.set_tensor_name(k_attn, "attn.k_attn") + .map_err(LlamaError::format)?; + let v_attn = ctx + .permute(v_states, [0, 2, 1, 3]) + .map_err(LlamaError::format)?; + ctx.set_tensor_name(v_attn, "attn.v_attn") + .map_err(LlamaError::format)?; + + let mut attn = build_attention_mha_output( + ctx, + q_attn, + k_attn, + v_attn, + input_mask, + spec.q_head_dim, + n_tokens, + "attn", + )?; + + if let Some(gate) = gate { + attn = ctx + .binary_like_a(Op::Mul, attn, gate, BufferUsage::Activations) + .map_err(LlamaError::format)?; + ctx.set_tensor_name(attn, "attn.attn_gated") + .map_err(LlamaError::format)?; + } + + let output_weight = require_tensor_id(tensor_ids, &spec.output_proj_name)?; + let mut result_output = ctx + .mul_mat(output_weight, attn, BufferUsage::Activations) + .map_err(LlamaError::format)?; + ctx.set_tensor_name(result_output, "attn.output_proj") + .map_err(LlamaError::format)?; + + if spec.residual { + result_output = ctx + .binary_like_a( + Op::Add, + result_output, + input_embed, + BufferUsage::Activations, + ) + .map_err(LlamaError::format)?; + ctx.set_tensor_name(result_output, "attn.output_residual") + .map_err(LlamaError::format)?; + } + mark_output(ctx, result_output)?; + + let mut graph = Graph::new(); + graph + .build_forward_expand(ctx, result_output) + .map_err(LlamaError::format)?; + + Ok(AttentionBlockGraph { + graph, + input_primary, + input_positions, + input_mask, + input_embed, + result_output, + }) +} + +pub fn prepare_attention_block_graph( + ctx: &mut Context, + tensor_ids: &BTreeMap, + spec: &AttentionBlockSpec, + n_tokens: usize, + features: MetalDeviceFeatures, +) -> Result<(AttentionBlockGraph, MetalPreparedGraph)> { + let block = build_attention_block_graph(ctx, tensor_ids, spec, n_tokens)?; + let prepared = prepare_graph(ctx, &block.graph, features).map_err(LlamaError::format)?; + Ok((block, prepared)) +} + +pub fn compile_attention_block_metal( + weights: &mut LoadedGgufWeights, + spec: &AttentionBlockSpec, + n_tokens: usize, +) -> Result { + let runtime = MetalRuntime::new().map_err(LlamaError::unsupported)?; + let (block, prepared) = prepare_attention_block_graph( + &mut weights.ctx, + &weights.tensor_ids, + spec, + n_tokens, + runtime.features(), + )?; + let session = MetalGraphSession::from_runtime( + runtime, + &weights.ctx, + &prepared, + BufferStorageMode::Private, + BufferStorageMode::Private, + ) + .map_err(LlamaError::format)?; + + Ok(CompiledAttentionBlockMetal { + spec: spec.clone(), + block, + session, + }) +} + +pub fn execute_prepared_attention_block_metal( + runtime: &MetalRuntime, + ctx: &Context, + spec: &AttentionBlockSpec, + block: &AttentionBlockGraph, + compiled: &MetalCompiledGraph, + input: LogitsProbeInput<'_>, + positions: &[i32], +) -> Result { + let input_primary = match (&spec.input, input) { + (ProbeInputKind::TokenIds { .. }, LogitsProbeInput::TokenIds(token_ids)) => { + if token_ids.len() != positions.len() { + return Err(LlamaError::format(format!( + "attention block token/position length mismatch: {} vs {}", + token_ids.len(), + positions.len() + ))); + } + i32_slice_as_bytes(token_ids).to_vec() + } + ( + ProbeInputKind::Embeddings { + hidden_size, + input_type, + }, + LogitsProbeInput::EmbeddingsF32 { data, n_tokens }, + ) => { + if *input_type != TensorType::F32 { + return Err(LlamaError::unsupported(format!( + "attention block currently expects F32 embeddings, got {}", + input_type.name() + ))); + } + if n_tokens != positions.len() { + return Err(LlamaError::format(format!( + "attention block embedding/position length mismatch: {} vs {}", + n_tokens, + positions.len() + ))); + } + let expected = (*hidden_size as usize) + .checked_mul(n_tokens) + .ok_or_else(|| { + LlamaError::format("overflow computing attention embedding input size") + })?; + if data.len() != expected { + return Err(LlamaError::format(format!( + "attention embedding input length mismatch: got {}, expected {}", + data.len(), + expected + ))); + } + f32_slice_as_bytes(data).to_vec() + } + (ProbeInputKind::TokenIds { .. }, LogitsProbeInput::EmbeddingsF32 { .. }) => { + return Err(LlamaError::format( + "attention block spec expects token ids but embeddings were provided", + )); + } + (ProbeInputKind::Embeddings { .. }, LogitsProbeInput::TokenIds(_)) => { + return Err(LlamaError::format( + "attention block spec expects embeddings but token ids were provided", + )); + } + }; + + let mut writes = vec![MetalGraphTensorWrite { + tensor_id: block.input_primary, + bytes: &input_primary, + }]; + let n_tokens = positions.len(); + let rope_positions = spec + .rope + .as_ref() + .map(|rope| encode_rope_positions(rope, positions, n_tokens)) + .transpose()?; + if let Some(input_positions) = block.input_positions { + writes.push(MetalGraphTensorWrite { + tensor_id: input_positions, + bytes: i32_slice_as_bytes(rope_positions.as_deref().ok_or_else(|| { + LlamaError::format("attention block rope positions were not prepared") + })?), + }); + } else if !positions.is_empty() { + return Err(LlamaError::format( + "attention block received positions for a graph that does not use rope", + )); + } + let mask_bytes = block + .input_mask + .map(|input_mask| attention_mask_bytes_for_tensor(ctx, input_mask, n_tokens)) + .transpose()?; + if let Some(input_mask) = block.input_mask { + writes.push(MetalGraphTensorWrite { + tensor_id: input_mask, + bytes: mask_bytes.as_deref().ok_or_else(|| { + LlamaError::format("attention block causal mask was not prepared") + })?, + }); + } + + let execution = execute_compiled_graph(runtime, ctx, compiled, &writes, &[block.result_output]) + .map_err(LlamaError::format)?; + + let result_bytes = execution.outputs.get(&block.result_output).ok_or_else(|| { + LlamaError::format("compiled attention block did not produce result bytes") + })?; + let hidden = f32_bytes_to_vec(result_bytes)?; + let output = ctx + .tensor(block.result_output) + .ok_or_else(|| LlamaError::format("attention result_output tensor is invalid"))?; + + Ok(AttentionBlockRun { + hidden, + hidden_size: ne_usize(output, 0)?, + n_tokens: ne_usize(output, 1)?, + }) +} + +pub fn execute_attention_block_graph_metal( + weights: &mut LoadedGgufWeights, + spec: &AttentionBlockSpec, + input: LogitsProbeInput<'_>, + positions: &[i32], +) -> Result { + let compiled = compile_attention_block_metal(weights, spec, positions.len())?; + compiled.execute(&weights.ctx, input, positions) +} + +pub fn execute_attention_block_graph_metal_cached( + compiled: &CompiledAttentionBlockMetal, + weights: &LoadedGgufWeights, + input: LogitsProbeInput<'_>, + positions: &[i32], +) -> Result { + compiled.execute(&weights.ctx, input, positions) +} + +#[derive(Clone, Debug)] +struct BuiltAttentionDecode { + input_mask: Option, + k_cache: TensorId, + v_cache: TensorId, + k_cache_view: TensorId, + v_cache_view: TensorId, + result_output: TensorId, +} + +fn build_attention_decode_from_hidden( + ctx: &mut Context, + tensor_ids: &BTreeMap, + spec: &AttentionDecodeSpec, + input_embed: TensorId, + input_positions: TensorId, + input_rope_positions: Option, + n_tokens: usize, + prefix: &str, +) -> Result { + let block = &spec.block; + let n_tokens_i64 = + i64::try_from(n_tokens).map_err(|_| LlamaError::format("n_tokens does not fit in i64"))?; + + let input_norm = build_rms_norm_mul( + ctx, + tensor_ids, + input_embed, + block.rms_epsilon, + &block.input_norm_name, + &format!("{prefix}.input_norm"), + )?; + + let q_weight = require_tensor_id(tensor_ids, &block.q_proj_name)?; + let q_proj = ctx + .mul_mat(q_weight, input_norm, BufferUsage::Activations) + .map_err(LlamaError::format)?; + ctx.set_tensor_name(q_proj, format!("{prefix}.q_proj")) + .map_err(LlamaError::format)?; + + let q_gate_stride = usize::try_from(block.q_head_dim) + .ok() + .and_then(|v| v.checked_mul(2)) + .and_then(|v| v.checked_mul(std::mem::size_of::())) + .ok_or_else(|| LlamaError::format("overflow computing q/gate stride"))?; + let q_token_stride = q_gate_stride + .checked_mul(usize::try_from(block.q_head_count).map_err(|_| { + LlamaError::format(format!( + "q_head_count {} does not fit in usize", + block.q_head_count + )) + })?) + .ok_or_else(|| LlamaError::format("overflow computing q/gate token stride"))?; + let gate_offset = usize::try_from(block.q_head_dim) + .ok() + .and_then(|v| v.checked_mul(std::mem::size_of::())) + .ok_or_else(|| LlamaError::format("overflow computing gate offset"))?; + + let (mut q_states, gate) = match block.q_layout { + AttentionQueryLayout::Plain => { + let q = ctx + .reshape( + q_proj, + &[ + i64::from(block.q_head_dim), + i64::from(block.q_head_count), + n_tokens_i64, + ], + ) + .map_err(LlamaError::format)?; + (q, None) + } + AttentionQueryLayout::InterleavedQueryGate { gate_activation } => { + let q = ctx + .view_3d( + q_proj, + i64::from(block.q_head_dim), + i64::from(block.q_head_count), + n_tokens_i64, + q_gate_stride, + q_token_stride, + 0, + ) + .map_err(LlamaError::format)?; + let gate_view = ctx + .view_3d( + q_proj, + i64::from(block.q_head_dim), + i64::from(block.q_head_count), + n_tokens_i64, + q_gate_stride, + q_token_stride, + gate_offset, + ) + .map_err(LlamaError::format)?; + let gate_cont = ctx + .cont_2d( + gate_view, + i64::from(block.q_head_dim) * i64::from(block.q_head_count), + n_tokens_i64, + ) + .map_err(LlamaError::format)?; + let gate = ctx + .unary(gate_cont, gate_activation, BufferUsage::Activations) + .map_err(LlamaError::format)?; + ctx.set_tensor_name(gate, format!("{prefix}.gate")) + .map_err(LlamaError::format)?; + (q, Some(gate)) + } + }; + ctx.set_tensor_name(q_states, format!("{prefix}.q_states")) + .map_err(LlamaError::format)?; + let q_pre_store = ctx + .view_2d( + q_states, + i64::from(block.q_head_dim) * i64::from(block.q_head_count), + n_tokens_i64, + ctx.tensor(q_states) + .ok_or_else(|| LlamaError::format("invalid q_states tensor"))? + .nb[2], + 0, + ) + .map_err(LlamaError::format)?; + ctx.set_tensor_name(q_pre_store, format!("{prefix}.q_pre_store")) + .map_err(LlamaError::format)?; + + if let Some(q_norm_name) = &block.q_norm_name { + q_states = build_rms_norm_mul( + ctx, + tensor_ids, + q_states, + block.rms_epsilon, + q_norm_name, + &format!("{prefix}.q_norm"), + )?; + } + let q_norm_store = ctx + .view_2d( + q_states, + i64::from(block.q_head_dim) * i64::from(block.q_head_count), + n_tokens_i64, + ctx.tensor(q_states) + .ok_or_else(|| LlamaError::format("invalid q_states tensor"))? + .nb[2], + 0, + ) + .map_err(LlamaError::format)?; + ctx.set_tensor_name(q_norm_store, format!("{prefix}.q_norm_store")) + .map_err(LlamaError::format)?; + + let k_weight = require_tensor_id(tensor_ids, &block.k_proj_name)?; + let mut k_states = ctx + .mul_mat(k_weight, input_norm, BufferUsage::Activations) + .map_err(LlamaError::format)?; + k_states = ctx + .reshape( + k_states, + &[ + i64::from(block.k_head_dim), + i64::from(block.kv_head_count), + n_tokens_i64, + ], + ) + .map_err(LlamaError::format)?; + ctx.set_tensor_name(k_states, format!("{prefix}.k_states")) + .map_err(LlamaError::format)?; + + if let Some(k_norm_name) = &block.k_norm_name { + k_states = build_rms_norm_mul( + ctx, + tensor_ids, + k_states, + block.rms_epsilon, + k_norm_name, + &format!("{prefix}.k_norm"), + )?; + } + let k_norm_store = ctx + .view_2d( + k_states, + i64::from(block.k_head_dim) * i64::from(block.kv_head_count), + n_tokens_i64, + ctx.tensor(k_states) + .ok_or_else(|| LlamaError::format("invalid k_states tensor"))? + .nb[2], + 0, + ) + .map_err(LlamaError::format)?; + ctx.set_tensor_name(k_norm_store, format!("{prefix}.k_norm_store")) + .map_err(LlamaError::format)?; + + let v_weight = require_tensor_id(tensor_ids, &block.v_proj_name)?; + let mut v_states = ctx + .mul_mat(v_weight, input_norm, BufferUsage::Activations) + .map_err(LlamaError::format)?; + v_states = ctx + .reshape( + v_states, + &[ + i64::from(block.v_head_dim), + i64::from(block.kv_head_count), + n_tokens_i64, + ], + ) + .map_err(LlamaError::format)?; + ctx.set_tensor_name(v_states, format!("{prefix}.v_states")) + .map_err(LlamaError::format)?; + + let input_mask = if block.causal { + let mask = ctx + .new_named_tensor( + format!("{prefix}.kq_mask"), + attention_mask_tensor_type(n_tokens), + 4, + &[i64::from(spec.cache.max_context), n_tokens_i64, 1, 1], + BufferUsage::Activations, + ) + .map_err(LlamaError::format)?; + mark_input(ctx, mask)?; + Some(mask) + } else { + None + }; + + if let Some(rope) = &block.rope { + let rope_positions = input_rope_positions.ok_or_else(|| { + LlamaError::format("attention decode rope requested without a rope position tensor") + })?; + q_states = ctx + .rope_multi( + q_states, + rope_positions, + None, + rope.n_dims, + rope.sections, + rope.mode, + rope.n_ctx_orig, + rope.freq_base, + rope.freq_scale, + rope.ext_factor, + rope.attn_factor, + rope.beta_fast, + rope.beta_slow, + BufferUsage::Activations, + ) + .map_err(LlamaError::format)?; + k_states = ctx + .rope_multi( + k_states, + rope_positions, + None, + rope.n_dims, + rope.sections, + rope.mode, + rope.n_ctx_orig, + rope.freq_base, + rope.freq_scale, + rope.ext_factor, + rope.attn_factor, + rope.beta_fast, + rope.beta_slow, + BufferUsage::Activations, + ) + .map_err(LlamaError::format)?; + } + + let q_store = ctx + .view_2d( + q_states, + i64::from(block.q_head_dim) * i64::from(block.q_head_count), + n_tokens_i64, + ctx.tensor(q_states) + .ok_or_else(|| LlamaError::format("invalid q_states tensor"))? + .nb[2], + 0, + ) + .map_err(LlamaError::format)?; + ctx.set_tensor_name(q_store, format!("{prefix}.q_store")) + .map_err(LlamaError::format)?; + + let k_merged_width = i64::from(block.k_head_dim) * i64::from(block.kv_head_count); + let v_merged_width = i64::from(block.v_head_dim) * i64::from(block.kv_head_count); + let k_token_stride = ctx + .tensor(k_states) + .ok_or_else(|| LlamaError::format("invalid k_states tensor"))? + .nb[2]; + let v_token_stride = ctx + .tensor(v_states) + .ok_or_else(|| LlamaError::format("invalid v_states tensor"))? + .nb[2]; + + let k_store = ctx + .view_2d(k_states, k_merged_width, n_tokens_i64, k_token_stride, 0) + .map_err(LlamaError::format)?; + ctx.set_tensor_name(k_store, format!("{prefix}.k_store")) + .map_err(LlamaError::format)?; + let v_store = ctx + .view_2d(v_states, v_merged_width, n_tokens_i64, v_token_stride, 0) + .map_err(LlamaError::format)?; + ctx.set_tensor_name(v_store, format!("{prefix}.v_store")) + .map_err(LlamaError::format)?; + + q_states = ctx + .reshape( + q_states, + &[ + i64::from(block.q_head_dim), + i64::from(block.q_head_count), + n_tokens_i64, + 1, + ], + ) + .map_err(LlamaError::format)?; + ctx.set_tensor_name(q_states, format!("{prefix}.q_states_4d")) + .map_err(LlamaError::format)?; + + let k_cache = ctx + .new_named_tensor( + format!("{prefix}.k_cache"), + spec.cache.k_type, + 3, + &[ + k_merged_width, + i64::from(spec.cache.max_context), + i64::from(spec.cache.max_sequences), + ], + BufferUsage::State, + ) + .map_err(LlamaError::format)?; + let v_cache = ctx + .new_named_tensor( + format!("{prefix}.v_cache"), + spec.cache.v_type, + 3, + &[ + v_merged_width, + i64::from(spec.cache.max_context), + i64::from(spec.cache.max_sequences), + ], + BufferUsage::State, + ) + .map_err(LlamaError::format)?; + + let k_cache_written = ctx + .set_rows(k_cache, k_store, input_positions, BufferUsage::State) + .map_err(LlamaError::format)?; + let v_cache_written = ctx + .set_rows(v_cache, v_store, input_positions, BufferUsage::State) + .map_err(LlamaError::format)?; + + let k_cache_view = ctx + .view_4d( + k_cache_written, + i64::from(block.k_head_dim), + i64::from(spec.cache.max_context), + i64::from(block.kv_head_count), + i64::from(spec.cache.max_sequences), + row_size(spec.cache.k_type, k_merged_width)?, + row_size(spec.cache.k_type, i64::from(block.k_head_dim))?, + row_size( + spec.cache.k_type, + k_merged_width * i64::from(spec.cache.max_context), + )?, + 0, + ) + .map_err(LlamaError::format)?; + let v_cache_view = ctx + .view_4d( + v_cache_written, + i64::from(block.v_head_dim), + i64::from(spec.cache.max_context), + i64::from(block.kv_head_count), + i64::from(spec.cache.max_sequences), + row_size(spec.cache.v_type, v_merged_width)?, + row_size(spec.cache.v_type, i64::from(block.v_head_dim))?, + row_size( + spec.cache.v_type, + v_merged_width * i64::from(spec.cache.max_context), + )?, + 0, + ) + .map_err(LlamaError::format)?; + ctx.set_tensor_name(k_cache_view, format!("{prefix}.k_cache_view")) + .map_err(LlamaError::format)?; + ctx.set_tensor_name(v_cache_view, format!("{prefix}.v_cache_view")) + .map_err(LlamaError::format)?; + + let q_attn = ctx + .permute(q_states, [0, 2, 1, 3]) + .map_err(LlamaError::format)?; + ctx.set_tensor_name(q_attn, format!("{prefix}.q_attn")) + .map_err(LlamaError::format)?; + let mut attn = build_attention_mha_output( + ctx, + q_attn, + k_cache_view, + v_cache_view, + input_mask, + block.q_head_dim, + n_tokens, + prefix, + )?; + + if let Some(gate) = gate { + attn = ctx + .binary_like_a(Op::Mul, attn, gate, BufferUsage::Activations) + .map_err(LlamaError::format)?; + ctx.set_tensor_name(attn, format!("{prefix}.attn_gated")) + .map_err(LlamaError::format)?; + } + + let output_weight = require_tensor_id(tensor_ids, &block.output_proj_name)?; + let mut result_output = ctx + .mul_mat(output_weight, attn, BufferUsage::Activations) + .map_err(LlamaError::format)?; + ctx.set_tensor_name(result_output, format!("{prefix}.output_proj")) + .map_err(LlamaError::format)?; + + if block.residual { + result_output = ctx + .binary_like_a( + Op::Add, + result_output, + input_embed, + BufferUsage::Activations, + ) + .map_err(LlamaError::format)?; + ctx.set_tensor_name(result_output, format!("{prefix}.output_residual")) + .map_err(LlamaError::format)?; + } + + Ok(BuiltAttentionDecode { + input_mask, + k_cache, + v_cache, + k_cache_view, + v_cache_view, + result_output, + }) +} + +pub fn build_attention_decode_graph( + ctx: &mut Context, + tensor_ids: &BTreeMap, + spec: &AttentionDecodeSpec, + n_tokens: usize, +) -> Result { + if n_tokens == 0 { + return Err(LlamaError::format( + "attention decode graph requires at least one token", + )); + } + + let block = &spec.block; + let input_primary = match &block.input { + ProbeInputKind::TokenIds { .. } => ctx + .new_named_tensor( + "attn_decode.inp_tokens", + TensorType::I32, + 1, + &[n_tokens as i64], + BufferUsage::Activations, + ) + .map_err(LlamaError::format)?, + ProbeInputKind::Embeddings { + hidden_size, + input_type, + } => ctx + .new_named_tensor( + "attn_decode.inp_embd", + *input_type, + 2, + &[i64::from(*hidden_size), n_tokens as i64], + BufferUsage::Activations, + ) + .map_err(LlamaError::format)?, + }; + mark_input(ctx, input_primary)?; + + let input_positions = ctx + .new_named_tensor( + "attn_decode.inp_pos", + TensorType::I32, + 1, + &[n_tokens as i64], + BufferUsage::Activations, + ) + .map_err(LlamaError::format)?; + mark_input(ctx, input_positions)?; + let input_rope_positions = if let Some(rope) = &block.rope { + let positions = ctx + .new_named_tensor( + "attn_decode.inp_rope_pos", + TensorType::I32, + 1, + &[rope_position_tensor_len(rope, n_tokens)?], + BufferUsage::Activations, + ) + .map_err(LlamaError::format)?; + mark_input(ctx, positions)?; + Some(positions) + } else { + None + }; + + let input_embed = match &block.input { + ProbeInputKind::TokenIds { + token_embedding_name, + } => { + let token_embd = require_tensor_id(tensor_ids, token_embedding_name)?; + ctx.get_rows(token_embd, input_primary, BufferUsage::Activations) + .map_err(LlamaError::format)? + } + ProbeInputKind::Embeddings { .. } => input_primary, + }; + ctx.set_tensor_name(input_embed, "attn_decode.input_embed") + .map_err(LlamaError::format)?; + let built = build_attention_decode_from_hidden( + ctx, + tensor_ids, + spec, + input_embed, + input_positions, + input_rope_positions, + n_tokens, + "attn_decode", + )?; + let result_output = built.result_output; + mark_output(ctx, result_output)?; + + let mut graph = Graph::new(); + graph + .build_forward_expand(ctx, result_output) + .map_err(LlamaError::format)?; + + Ok(AttentionDecodeGraph { + graph, + input_primary, + input_positions, + input_rope_positions, + input_mask: built.input_mask, + k_cache: built.k_cache, + v_cache: built.v_cache, + k_cache_view: built.k_cache_view, + v_cache_view: built.v_cache_view, + result_output, + }) +} + +pub fn prepare_attention_decode_graph( + ctx: &mut Context, + tensor_ids: &BTreeMap, + spec: &AttentionDecodeSpec, + n_tokens: usize, + features: MetalDeviceFeatures, +) -> Result<(AttentionDecodeGraph, MetalPreparedGraph)> { + let decode = build_attention_decode_graph(ctx, tensor_ids, spec, n_tokens)?; + let prepared = prepare_graph(ctx, &decode.graph, features).map_err(LlamaError::format)?; + Ok((decode, prepared)) +} + +pub fn compile_attention_decode_metal( + weights: &mut LoadedGgufWeights, + spec: &AttentionDecodeSpec, + n_tokens: usize, +) -> Result { + let runtime = MetalRuntime::new().map_err(LlamaError::unsupported)?; + let (decode, prepared) = prepare_attention_decode_graph( + &mut weights.ctx, + &weights.tensor_ids, + spec, + n_tokens, + runtime.features(), + )?; + let session = MetalGraphSession::from_runtime( + runtime, + &weights.ctx, + &prepared, + BufferStorageMode::Private, + BufferStorageMode::Private, + ) + .map_err(LlamaError::format)?; + + Ok(CompiledAttentionDecodeMetal { + spec: spec.clone(), + decode, + session, + }) +} + +pub fn execute_prepared_attention_decode_metal( + runtime: &MetalRuntime, + ctx: &mut Context, + spec: &AttentionDecodeSpec, + decode: &AttentionDecodeGraph, + compiled: &MetalCompiledGraph, + input: LogitsProbeInput<'_>, + positions: &[i32], + cache_tokens: usize, +) -> Result { + if positions.is_empty() { + return Err(LlamaError::format( + "attention decode requires at least one position", + )); + } + let max_context = usize::try_from(spec.cache.max_context).map_err(|_| { + LlamaError::format(format!( + "max_context {} does not fit in usize", + spec.cache.max_context + )) + })?; + if cache_tokens == 0 || cache_tokens > max_context { + return Err(LlamaError::format(format!( + "attention decode cache_tokens {} is outside 1..={}", + cache_tokens, max_context + ))); + } + if positions.iter().copied().any(|pos| { + pos < 0 + || usize::try_from(pos) + .ok() + .map(|pos| pos >= cache_tokens) + .unwrap_or(true) + }) { + return Err(LlamaError::format(format!( + "attention decode positions {:?} exceed cache_tokens {}", + positions, cache_tokens + ))); + } + + let input_primary = match (&spec.block.input, input) { + (ProbeInputKind::TokenIds { .. }, LogitsProbeInput::TokenIds(token_ids)) => { + if token_ids.len() != positions.len() { + return Err(LlamaError::format(format!( + "attention decode token/position length mismatch: {} vs {}", + token_ids.len(), + positions.len() + ))); + } + i32_slice_as_bytes(token_ids).to_vec() + } + ( + ProbeInputKind::Embeddings { + hidden_size, + input_type, + }, + LogitsProbeInput::EmbeddingsF32 { data, n_tokens }, + ) => { + if *input_type != TensorType::F32 { + return Err(LlamaError::unsupported(format!( + "attention decode currently expects F32 embeddings, got {}", + input_type.name() + ))); + } + if n_tokens != positions.len() { + return Err(LlamaError::format(format!( + "attention decode embedding/position length mismatch: {} vs {}", + n_tokens, + positions.len() + ))); + } + let expected = (*hidden_size as usize) + .checked_mul(n_tokens) + .ok_or_else(|| { + LlamaError::format("overflow computing attention decode embedding size") + })?; + if data.len() != expected { + return Err(LlamaError::format(format!( + "attention decode embedding input length mismatch: got {}, expected {}", + data.len(), + expected + ))); + } + f32_slice_as_bytes(data).to_vec() + } + (ProbeInputKind::TokenIds { .. }, LogitsProbeInput::EmbeddingsF32 { .. }) => { + return Err(LlamaError::format( + "attention decode spec expects token ids but embeddings were provided", + )); + } + (ProbeInputKind::Embeddings { .. }, LogitsProbeInput::TokenIds(_)) => { + return Err(LlamaError::format( + "attention decode spec expects embeddings but token ids were provided", + )); + } + }; + + configure_attention_cache_view( + ctx, + decode.k_cache_view, + i64::from(spec.block.k_head_dim), + cache_tokens, + i64::from(spec.block.kv_head_count), + i64::from(spec.cache.max_sequences), + )?; + configure_attention_cache_view( + ctx, + decode.v_cache_view, + i64::from(spec.block.v_head_dim), + cache_tokens, + i64::from(spec.block.kv_head_count), + i64::from(spec.cache.max_sequences), + )?; + if let Some(input_mask) = decode.input_mask { + configure_attention_mask_view(ctx, input_mask, cache_tokens, positions.len())?; + } + + let rope_positions = spec + .block + .rope + .as_ref() + .map(|rope| encode_rope_positions(rope, positions, positions.len())) + .transpose()?; + let mut writes = vec![ + MetalGraphTensorWrite { + tensor_id: decode.input_primary, + bytes: &input_primary, + }, + MetalGraphTensorWrite { + tensor_id: decode.input_positions, + bytes: i32_slice_as_bytes(positions), + }, + ]; + if let Some(input_rope_positions) = decode.input_rope_positions { + writes.push(MetalGraphTensorWrite { + tensor_id: input_rope_positions, + bytes: i32_slice_as_bytes(rope_positions.as_deref().ok_or_else(|| { + LlamaError::format("attention decode rope positions were not prepared") + })?), + }); + } + let mask_bytes = decode + .input_mask + .map(|input_mask| { + position_attention_mask_bytes_for_tensor(ctx, input_mask, cache_tokens, positions) + }) + .transpose()?; + if let Some(input_mask) = decode.input_mask { + writes.push(MetalGraphTensorWrite { + tensor_id: input_mask, + bytes: mask_bytes.as_deref().ok_or_else(|| { + LlamaError::format("attention decode causal mask was not prepared") + })?, + }); + } + + let execution = + execute_compiled_graph(runtime, ctx, compiled, &writes, &[decode.result_output]) + .map_err(LlamaError::format)?; + + let result_bytes = execution + .outputs + .get(&decode.result_output) + .ok_or_else(|| { + LlamaError::format("compiled attention decode did not produce result bytes") + })?; + let hidden = f32_bytes_to_vec(result_bytes)?; + let output = ctx + .tensor(decode.result_output) + .ok_or_else(|| LlamaError::format("attention decode result tensor is invalid"))?; + + Ok(AttentionBlockRun { + hidden, + hidden_size: ne_usize(output, 0)?, + n_tokens: ne_usize(output, 1)?, + }) +} + +pub fn execute_attention_decode_graph_metal( + weights: &mut LoadedGgufWeights, + spec: &AttentionDecodeSpec, + n_tokens: usize, + input: LogitsProbeInput<'_>, + positions: &[i32], + cache_tokens: usize, +) -> Result { + let compiled = compile_attention_decode_metal(weights, spec, n_tokens)?; + compiled.execute(&mut weights.ctx, input, positions, cache_tokens) +} + +pub fn execute_attention_decode_graph_metal_cached( + compiled: &CompiledAttentionDecodeMetal, + weights: &mut LoadedGgufWeights, + input: LogitsProbeInput<'_>, + positions: &[i32], + cache_tokens: usize, +) -> Result { + compiled.execute(&mut weights.ctx, input, positions, cache_tokens) +} + +#[derive(Clone, Debug)] +struct BuiltDeltaNetRecurrentDecode { + r_cache: TensorId, + s_cache: TensorId, + r_cache_update: TensorId, + s_cache_update: TensorId, + result_output: TensorId, +} + +fn build_delta_net_recurrent_decode_from_hidden( + ctx: &mut Context, + tensor_ids: &BTreeMap, + spec: &DeltaNetRecurrentDecodeSpec, + input_embed: TensorId, + n_tokens: usize, + prefix: &str, +) -> Result { + let block = &spec.block; + let n_seqs = i64::from(spec.cache.max_sequences); + let n_tokens_i64 = + i64::try_from(n_tokens).map_err(|_| LlamaError::format("n_tokens does not fit in i64"))?; + let value_hidden_size = i64::from(block.value_head_dim) * i64::from(block.value_head_count); + let qkv_dim = i64::from(block.key_head_dim) + .checked_mul(i64::from(block.key_head_count)) + .and_then(|v| v.checked_mul(2)) + .and_then(|v| v.checked_add(value_hidden_size)) + .ok_or_else(|| LlamaError::format("overflow computing delta-net qkv width"))?; + + let input_norm = build_rms_norm_mul( + ctx, + tensor_ids, + input_embed, + block.rms_epsilon, + &block.input_norm_name, + &format!("{prefix}.input_norm"), + )?; + + let qkv_weight = require_tensor_id(tensor_ids, &block.qkv_proj_name)?; + let mut qkv_mixed = ctx + .mul_mat(qkv_weight, input_norm, BufferUsage::Activations) + .map_err(LlamaError::format)?; + qkv_mixed = ctx + .reshape(qkv_mixed, &[qkv_dim, n_tokens_i64, n_seqs]) + .map_err(LlamaError::format)?; + ctx.set_tensor_name(qkv_mixed, format!("{prefix}.qkv_mixed")) + .map_err(LlamaError::format)?; + + let z_weight = require_tensor_id(tensor_ids, &block.z_proj_name)?; + let z = ctx + .mul_mat(z_weight, input_norm, BufferUsage::Activations) + .map_err(LlamaError::format)?; + ctx.set_tensor_name(z, format!("{prefix}.z")) + .map_err(LlamaError::format)?; + + let beta_weight = require_tensor_id(tensor_ids, &block.beta_proj_name)?; + let mut beta = ctx + .mul_mat(beta_weight, input_norm, BufferUsage::Activations) + .map_err(LlamaError::format)?; + beta = ctx + .reshape( + beta, + &[1, i64::from(block.value_head_count), n_tokens_i64, n_seqs], + ) + .map_err(LlamaError::format)?; + beta = ctx + .unary(beta, UnaryOp::Sigmoid, BufferUsage::Activations) + .map_err(LlamaError::format)?; + ctx.set_tensor_name(beta, format!("{prefix}.beta")) + .map_err(LlamaError::format)?; + + let alpha_weight = require_tensor_id(tensor_ids, &block.alpha_proj_name)?; + let mut alpha = ctx + .mul_mat(alpha_weight, input_norm, BufferUsage::Activations) + .map_err(LlamaError::format)?; + alpha = ctx + .reshape( + alpha, + &[i64::from(block.value_head_count), n_tokens_i64, n_seqs], + ) + .map_err(LlamaError::format)?; + let dt_bias = require_tensor_id(tensor_ids, &block.dt_bias_name)?; + alpha = ctx + .binary_like_a(Op::Add, alpha, dt_bias, BufferUsage::Activations) + .map_err(LlamaError::format)?; + alpha = ctx + .unary(alpha, UnaryOp::SoftPlus, BufferUsage::Activations) + .map_err(LlamaError::format)?; + let ssm_a = require_tensor_id(tensor_ids, &block.a_name)?; + let mut gate = ctx + .binary_like_a(Op::Mul, alpha, ssm_a, BufferUsage::Activations) + .map_err(LlamaError::format)?; + gate = ctx + .reshape( + gate, + &[1, i64::from(block.value_head_count), n_tokens_i64, n_seqs], + ) + .map_err(LlamaError::format)?; + ctx.set_tensor_name(gate, format!("{prefix}.gate")) + .map_err(LlamaError::format)?; + + let conv_kernel_id = require_tensor_id(tensor_ids, &block.conv_kernel_name)?; + let conv_kernel = require_tensor(ctx, conv_kernel_id)?; + let conv_prefix = conv_kernel.ne[0] + .checked_sub(1) + .ok_or_else(|| LlamaError::format("delta-net conv kernel size underflow"))?; + if conv_kernel.ne[1] != qkv_dim { + return Err(LlamaError::format(format!( + "delta-net conv kernel width mismatch: kernel={}, expected={}", + conv_kernel.ne[1], qkv_dim + ))); + } + + let r_width = conv_prefix + .checked_mul(qkv_dim) + .ok_or_else(|| LlamaError::format("overflow computing recurrent-r width"))?; + let s_width = i64::from(block.value_head_dim) + .checked_mul(i64::from(block.value_head_dim)) + .and_then(|v| v.checked_mul(i64::from(block.value_head_count))) + .ok_or_else(|| LlamaError::format("overflow computing recurrent-s width"))?; + + let r_cache = ctx + .new_named_tensor( + format!("{prefix}.r_cache"), + spec.cache.r_type, + 2, + &[r_width, n_seqs], + BufferUsage::State, + ) + .map_err(LlamaError::format)?; + let s_cache = ctx + .new_named_tensor( + format!("{prefix}.s_cache"), + spec.cache.s_type, + 2, + &[s_width, n_seqs], + BufferUsage::State, + ) + .map_err(LlamaError::format)?; + + let conv_states = ctx + .view_3d( + r_cache, + conv_prefix, + qkv_dim, + n_seqs, + row_size(spec.cache.r_type, conv_prefix)?, + row_size(spec.cache.r_type, r_width)?, + 0, + ) + .map_err(LlamaError::format)?; + ctx.set_tensor_name(conv_states, format!("{prefix}.conv_states")) + .map_err(LlamaError::format)?; + + let qkv_mixed_t = ctx.transpose(qkv_mixed).map_err(LlamaError::format)?; + let conv_input = ctx + .concat(conv_states, qkv_mixed_t, 0, BufferUsage::Activations) + .map_err(LlamaError::format)?; + ctx.set_tensor_name(conv_input, format!("{prefix}.conv_input")) + .map_err(LlamaError::format)?; + + let conv_input_tensor = require_tensor(ctx, conv_input)?.clone(); + let last_conv_states = ctx + .view_3d( + conv_input, + conv_prefix, + qkv_dim, + n_seqs, + conv_input_tensor.nb[1], + conv_input_tensor.nb[2], + row_size(conv_input_tensor.desc.ty, n_tokens_i64)?, + ) + .map_err(LlamaError::format)?; + let last_conv_states_flat = ctx + .view_1d(last_conv_states, r_width * n_seqs, 0) + .map_err(LlamaError::format)?; + let r_cache_flat = ctx + .view_1d(r_cache, r_width * n_seqs, 0) + .map_err(LlamaError::format)?; + let r_cache_update = ctx + .cpy(last_conv_states_flat, r_cache_flat, BufferUsage::State) + .map_err(LlamaError::format)?; + + let state = ctx + .view_4d( + s_cache, + i64::from(block.value_head_dim), + i64::from(block.value_head_dim), + i64::from(block.value_head_count), + n_seqs, + row_size(spec.cache.s_type, i64::from(block.value_head_dim))?, + row_size( + spec.cache.s_type, + i64::from(block.value_head_dim) * i64::from(block.value_head_dim), + )?, + row_size(spec.cache.s_type, s_width)?, + 0, + ) + .map_err(LlamaError::format)?; + ctx.set_tensor_name(state, format!("{prefix}.state")) + .map_err(LlamaError::format)?; + + let conv_output = ctx + .ssm_conv(conv_input, conv_kernel_id, BufferUsage::Activations) + .map_err(LlamaError::format)?; + let conv_output = ctx + .unary(conv_output, UnaryOp::Silu, BufferUsage::Activations) + .map_err(LlamaError::format)?; + ctx.set_tensor_name(conv_output, format!("{prefix}.conv_output")) + .map_err(LlamaError::format)?; + + let conv_output_tensor = require_tensor(ctx, conv_output)?.clone(); + let qk_heads_width = i64::from(block.key_head_dim) * i64::from(block.key_head_count); + let q_conv = ctx + .view_4d( + conv_output, + i64::from(block.key_head_dim), + i64::from(block.key_head_count), + n_tokens_i64, + n_seqs, + row_size(conv_output_tensor.desc.ty, i64::from(block.key_head_dim))?, + row_size(conv_output_tensor.desc.ty, qkv_dim)?, + row_size(conv_output_tensor.desc.ty, qkv_dim * n_tokens_i64)?, + 0, + ) + .map_err(LlamaError::format)?; + ctx.set_tensor_name(q_conv, format!("{prefix}.q_conv")) + .map_err(LlamaError::format)?; + let k_conv = ctx + .view_4d( + conv_output, + i64::from(block.key_head_dim), + i64::from(block.key_head_count), + n_tokens_i64, + n_seqs, + row_size(conv_output_tensor.desc.ty, i64::from(block.key_head_dim))?, + row_size(conv_output_tensor.desc.ty, qkv_dim)?, + row_size(conv_output_tensor.desc.ty, qkv_dim * n_tokens_i64)?, + row_size(conv_output_tensor.desc.ty, qk_heads_width)?, + ) + .map_err(LlamaError::format)?; + ctx.set_tensor_name(k_conv, format!("{prefix}.k_conv")) + .map_err(LlamaError::format)?; + let v_conv = ctx + .view_4d( + conv_output, + i64::from(block.value_head_dim), + i64::from(block.value_head_count), + n_tokens_i64, + n_seqs, + row_size(conv_output_tensor.desc.ty, i64::from(block.value_head_dim))?, + row_size(conv_output_tensor.desc.ty, qkv_dim)?, + row_size(conv_output_tensor.desc.ty, qkv_dim * n_tokens_i64)?, + row_size(conv_output_tensor.desc.ty, qk_heads_width * 2)?, + ) + .map_err(LlamaError::format)?; + ctx.set_tensor_name(v_conv, format!("{prefix}.v_conv")) + .map_err(LlamaError::format)?; + + let q_conv = ctx + .l2_norm_eps(q_conv, block.rms_epsilon, BufferUsage::Activations) + .map_err(LlamaError::format)?; + ctx.set_tensor_name(q_conv, format!("{prefix}.q_conv_predelta")) + .map_err(LlamaError::format)?; + let k_conv = ctx + .l2_norm_eps(k_conv, block.rms_epsilon, BufferUsage::Activations) + .map_err(LlamaError::format)?; + ctx.set_tensor_name(k_conv, format!("{prefix}.k_conv_predelta")) + .map_err(LlamaError::format)?; + + let delta = ctx + .gated_delta_net( + q_conv, + k_conv, + v_conv, + gate, + beta, + state, + BufferUsage::Activations, + ) + .map_err(LlamaError::format)?; + ctx.set_tensor_name(delta, format!("{prefix}.delta")) + .map_err(LlamaError::format)?; + + let output = ctx + .view_4d( + delta, + i64::from(block.value_head_dim), + i64::from(block.value_head_count), + n_tokens_i64, + n_seqs, + row_size(TensorType::F32, i64::from(block.value_head_dim))?, + row_size(TensorType::F32, value_hidden_size)?, + row_size(TensorType::F32, value_hidden_size * n_tokens_i64)?, + 0, + ) + .map_err(LlamaError::format)?; + ctx.set_tensor_name(output, format!("{prefix}.output_view")) + .map_err(LlamaError::format)?; + let new_state = ctx + .view_4d( + delta, + i64::from(block.value_head_dim), + i64::from(block.value_head_dim), + i64::from(block.value_head_count), + n_seqs, + row_size(TensorType::F32, i64::from(block.value_head_dim))?, + row_size( + TensorType::F32, + i64::from(block.value_head_dim) * i64::from(block.value_head_dim), + )?, + row_size(TensorType::F32, s_width)?, + row_size(TensorType::F32, value_hidden_size * n_tokens_i64 * n_seqs)?, + ) + .map_err(LlamaError::format)?; + + let new_state_flat = ctx + .view_1d(new_state, s_width * n_seqs, 0) + .map_err(LlamaError::format)?; + let s_cache_flat = ctx + .view_1d(s_cache, s_width * n_seqs, 0) + .map_err(LlamaError::format)?; + let s_cache_update = ctx + .cpy(new_state_flat, s_cache_flat, BufferUsage::State) + .map_err(LlamaError::format)?; + + let z = ctx + .reshape( + z, + &[ + i64::from(block.value_head_dim), + i64::from(block.value_head_count), + n_tokens_i64, + n_seqs, + ], + ) + .map_err(LlamaError::format)?; + ctx.set_tensor_name(z, format!("{prefix}.z_4d")) + .map_err(LlamaError::format)?; + let output_rms = ctx + .rms_norm_eps(output, block.rms_epsilon, BufferUsage::Activations) + .map_err(LlamaError::format)?; + ctx.set_tensor_name(output_rms, format!("{prefix}.output_rms")) + .map_err(LlamaError::format)?; + let norm_weight = require_tensor_id(tensor_ids, &block.norm_name)?; + let output_norm = ctx + .binary_like_a(Op::Mul, output_rms, norm_weight, BufferUsage::Activations) + .map_err(LlamaError::format)?; + ctx.set_tensor_name(output_norm, format!("{prefix}.output_norm")) + .map_err(LlamaError::format)?; + let z_silu = ctx + .unary(z, UnaryOp::Silu, BufferUsage::Activations) + .map_err(LlamaError::format)?; + ctx.set_tensor_name(z_silu, format!("{prefix}.z_silu")) + .map_err(LlamaError::format)?; + let gated_output = ctx + .binary_like_a(Op::Mul, output_norm, z_silu, BufferUsage::Activations) + .map_err(LlamaError::format)?; + ctx.set_tensor_name(gated_output, format!("{prefix}.gated_output")) + .map_err(LlamaError::format)?; + + let final_output = ctx + .reshape(gated_output, &[value_hidden_size, n_tokens_i64 * n_seqs]) + .map_err(LlamaError::format)?; + ctx.set_tensor_name(final_output, format!("{prefix}.final_output")) + .map_err(LlamaError::format)?; + let output_weight = require_tensor_id(tensor_ids, &block.output_proj_name)?; + let mut result_output = ctx + .mul_mat(output_weight, final_output, BufferUsage::Activations) + .map_err(LlamaError::format)?; + if block.residual { + result_output = ctx + .binary_like_a( + Op::Add, + result_output, + input_embed, + BufferUsage::Activations, + ) + .map_err(LlamaError::format)?; + } + ctx.set_tensor_name(result_output, format!("{prefix}.output")) + .map_err(LlamaError::format)?; + + Ok(BuiltDeltaNetRecurrentDecode { + r_cache, + s_cache, + r_cache_update, + s_cache_update, + result_output, + }) +} + +pub fn build_delta_net_recurrent_decode_graph( + ctx: &mut Context, + tensor_ids: &BTreeMap, + spec: &DeltaNetRecurrentDecodeSpec, + n_tokens: usize, +) -> Result { + if n_tokens == 0 { + return Err(LlamaError::format( + "delta-net recurrent decode graph requires at least one token", + )); + } + if spec.cache.max_sequences != 1 { + return Err(LlamaError::unsupported(format!( + "delta-net recurrent decode currently requires max_sequences=1, got {}", + spec.cache.max_sequences + ))); + } + if spec.cache.r_type != TensorType::F32 || spec.cache.s_type != TensorType::F32 { + return Err(LlamaError::unsupported(format!( + "delta-net recurrent decode currently requires F32 caches, got r={} s={}", + spec.cache.r_type.name(), + spec.cache.s_type.name() + ))); + } + + let block = &spec.block; + let n_tokens_i64 = + i64::try_from(n_tokens).map_err(|_| LlamaError::format("n_tokens does not fit in i64"))?; + + let input_primary = match &block.input { + ProbeInputKind::TokenIds { .. } => ctx + .new_named_tensor( + "recur_decode.inp_tokens", + TensorType::I32, + 1, + &[n_tokens_i64], + BufferUsage::Activations, + ) + .map_err(LlamaError::format)?, + ProbeInputKind::Embeddings { + hidden_size, + input_type, + } => ctx + .new_named_tensor( + "recur_decode.inp_embd", + *input_type, + 2, + &[i64::from(*hidden_size), n_tokens_i64], + BufferUsage::Activations, + ) + .map_err(LlamaError::format)?, + }; + mark_input(ctx, input_primary)?; + + let input_embed = match &block.input { + ProbeInputKind::TokenIds { + token_embedding_name, + } => { + let token_embd = require_tensor_id(tensor_ids, token_embedding_name)?; + ctx.get_rows(token_embd, input_primary, BufferUsage::Activations) + .map_err(LlamaError::format)? + } + ProbeInputKind::Embeddings { .. } => input_primary, + }; + ctx.set_tensor_name(input_embed, "recur_decode.input_embed") + .map_err(LlamaError::format)?; + let built = build_delta_net_recurrent_decode_from_hidden( + ctx, + tensor_ids, + spec, + input_embed, + n_tokens, + "recur_decode", + )?; + let result_output = built.result_output; + mark_output(ctx, result_output)?; + + let mut graph = Graph::new(); + graph + .build_forward_expand(ctx, built.r_cache_update) + .map_err(LlamaError::format)?; + graph + .build_forward_expand(ctx, built.s_cache_update) + .map_err(LlamaError::format)?; + graph + .build_forward_expand(ctx, result_output) + .map_err(LlamaError::format)?; + + Ok(DeltaNetRecurrentDecodeGraph { + graph, + input_primary, + r_cache: built.r_cache, + s_cache: built.s_cache, + result_output, + }) +} + +pub fn prepare_delta_net_recurrent_decode_graph( + ctx: &mut Context, + tensor_ids: &BTreeMap, + spec: &DeltaNetRecurrentDecodeSpec, + n_tokens: usize, + features: MetalDeviceFeatures, +) -> Result<(DeltaNetRecurrentDecodeGraph, MetalPreparedGraph)> { + let decode = build_delta_net_recurrent_decode_graph(ctx, tensor_ids, spec, n_tokens)?; + let prepared = prepare_graph(ctx, &decode.graph, features).map_err(LlamaError::format)?; + Ok((decode, prepared)) +} + +pub fn compile_delta_net_recurrent_decode_metal( + weights: &mut LoadedGgufWeights, + spec: &DeltaNetRecurrentDecodeSpec, + n_tokens: usize, +) -> Result { + let runtime = MetalRuntime::new().map_err(LlamaError::unsupported)?; + let (decode, prepared) = prepare_delta_net_recurrent_decode_graph( + &mut weights.ctx, + &weights.tensor_ids, + spec, + n_tokens, + runtime.features(), + )?; + let session = MetalGraphSession::from_runtime( + runtime, + &weights.ctx, + &prepared, + BufferStorageMode::Private, + BufferStorageMode::Private, + ) + .map_err(LlamaError::format)?; + + Ok(CompiledDeltaNetRecurrentDecodeMetal { + spec: spec.clone(), + decode, + session, + }) +} + +pub fn execute_prepared_delta_net_recurrent_decode_metal( + runtime: &MetalRuntime, + ctx: &mut Context, + spec: &DeltaNetRecurrentDecodeSpec, + decode: &DeltaNetRecurrentDecodeGraph, + compiled: &MetalCompiledGraph, + input: LogitsProbeInput<'_>, +) -> Result { + let input_primary = match (&spec.block.input, input) { + (ProbeInputKind::TokenIds { .. }, LogitsProbeInput::TokenIds(token_ids)) => { + i32_slice_as_bytes(token_ids).to_vec() + } + ( + ProbeInputKind::Embeddings { + hidden_size, + input_type, + }, + LogitsProbeInput::EmbeddingsF32 { data, n_tokens }, + ) => { + if *input_type != TensorType::F32 { + return Err(LlamaError::unsupported(format!( + "delta-net recurrent decode currently expects F32 embeddings, got {}", + input_type.name() + ))); + } + let expected = (*hidden_size as usize) + .checked_mul(n_tokens) + .ok_or_else(|| { + LlamaError::format("overflow computing delta-net embedding input size") + })?; + if data.len() != expected { + return Err(LlamaError::format(format!( + "delta-net recurrent embedding input length mismatch: got {}, expected {}", + data.len(), + expected + ))); + } + f32_slice_as_bytes(data).to_vec() + } + (ProbeInputKind::TokenIds { .. }, LogitsProbeInput::EmbeddingsF32 { .. }) => { + return Err(LlamaError::format( + "delta-net recurrent spec expects token ids but embeddings were provided", + )); + } + (ProbeInputKind::Embeddings { .. }, LogitsProbeInput::TokenIds(_)) => { + return Err(LlamaError::format( + "delta-net recurrent spec expects embeddings but token ids were provided", + )); + } + }; + + let execution = execute_compiled_graph( + runtime, + ctx, + compiled, + &[MetalGraphTensorWrite { + tensor_id: decode.input_primary, + bytes: &input_primary, + }], + &[decode.result_output], + ) + .map_err(LlamaError::format)?; + + let result_bytes = execution + .outputs + .get(&decode.result_output) + .ok_or_else(|| { + LlamaError::format("compiled delta-net recurrent decode did not produce result bytes") + })?; + let hidden = f32_bytes_to_vec(result_bytes)?; + let output = ctx + .tensor(decode.result_output) + .ok_or_else(|| LlamaError::format("delta-net recurrent result tensor is invalid"))?; + + Ok(AttentionBlockRun { + hidden, + hidden_size: ne_usize(output, 0)?, + n_tokens: ne_usize(output, 1)?, + }) +} + +pub fn execute_delta_net_recurrent_decode_graph_metal( + weights: &mut LoadedGgufWeights, + spec: &DeltaNetRecurrentDecodeSpec, + n_tokens: usize, + input: LogitsProbeInput<'_>, +) -> Result { + let compiled = compile_delta_net_recurrent_decode_metal(weights, spec, n_tokens)?; + compiled.execute(&mut weights.ctx, input) +} + +pub fn execute_delta_net_recurrent_decode_graph_metal_cached( + compiled: &CompiledDeltaNetRecurrentDecodeMetal, + weights: &mut LoadedGgufWeights, + input: LogitsProbeInput<'_>, +) -> Result { + compiled.execute(&mut weights.ctx, input) +} + +#[derive(Clone, Debug)] +struct BuiltMoeFfn { + selected_experts: TensorId, + result_output: TensorId, +} + +fn build_moe_ffn_from_hidden( + ctx: &mut Context, + tensor_ids: &BTreeMap, + spec: &MoeFfnSpec, + input_embed: TensorId, + n_tokens: usize, + prefix: &str, +) -> Result { + let input_hidden = if let Some(norm) = &spec.input_norm { + build_rms_norm_mul( + ctx, + tensor_ids, + input_embed, + norm.epsilon, + &norm.weight_name, + &format!("{prefix}.input_norm"), + )? + } else { + input_embed + }; + + let router_weight = require_tensor_id(tensor_ids, &spec.router_proj_name)?; + let logits = ctx + .mul_mat(router_weight, input_hidden, BufferUsage::Activations) + .map_err(LlamaError::format)?; + ctx.set_tensor_name(logits, format!("{prefix}.router_logits")) + .map_err(LlamaError::format)?; + + let probs = match spec.gating_func { + ExpertGatingFunc::SoftMax => ctx + .soft_max(logits, BufferUsage::Activations) + .map_err(LlamaError::format)?, + ExpertGatingFunc::Sigmoid => ctx + .unary(logits, UnaryOp::Sigmoid, BufferUsage::Activations) + .map_err(LlamaError::format)?, + ExpertGatingFunc::Identity => logits, + }; + ctx.set_tensor_name(probs, format!("{prefix}.router_probs")) + .map_err(LlamaError::format)?; + + let selected_experts = ctx + .top_k( + probs, + i64::from(spec.expert_used_count), + BufferUsage::Activations, + ) + .map_err(LlamaError::format)?; + ctx.set_tensor_name(selected_experts, format!("{prefix}.selected_experts")) + .map_err(LlamaError::format)?; + + let probs_3d = ctx + .reshape( + probs, + &[ + 1, + i64::from(spec.expert_count), + i64::try_from(n_tokens) + .map_err(|_| LlamaError::format("n_tokens does not fit in i64"))?, + ], + ) + .map_err(LlamaError::format)?; + let weights_3d = ctx + .get_rows(probs_3d, selected_experts, BufferUsage::Activations) + .map_err(LlamaError::format)?; + let mut weights = ctx + .reshape( + weights_3d, + &[ + i64::from(spec.expert_used_count), + i64::try_from(n_tokens) + .map_err(|_| LlamaError::format("n_tokens does not fit in i64"))?, + ], + ) + .map_err(LlamaError::format)?; + ctx.set_tensor_name(weights, format!("{prefix}.selected_weights")) + .map_err(LlamaError::format)?; + + if matches!(spec.gating_func, ExpertGatingFunc::Identity) { + weights = ctx + .soft_max(weights, BufferUsage::Activations) + .map_err(LlamaError::format)?; + ctx.set_tensor_name(weights, format!("{prefix}.selected_weights_softmax")) + .map_err(LlamaError::format)?; + } + + if spec.normalize_selected_weights { + let mut weights_sum = ctx + .sum_rows(weights, BufferUsage::Activations) + .map_err(LlamaError::format)?; + weights_sum = ctx + .clamp( + weights_sum, + 6.103_515_6e-5, + f32::INFINITY, + BufferUsage::Activations, + ) + .map_err(LlamaError::format)?; + weights = ctx + .binary_like_a(Op::Div, weights, weights_sum, BufferUsage::Activations) + .map_err(LlamaError::format)?; + ctx.set_tensor_name(weights, format!("{prefix}.selected_weights_norm")) + .map_err(LlamaError::format)?; + } + + let mut weights = ctx + .reshape( + weights, + &[ + 1, + i64::from(spec.expert_used_count), + i64::try_from(n_tokens) + .map_err(|_| LlamaError::format("n_tokens does not fit in i64"))?, + ], + ) + .map_err(LlamaError::format)?; + + if spec.weight_scale != 1.0 { + weights = ctx + .scale(weights, spec.weight_scale, BufferUsage::Activations) + .map_err(LlamaError::format)?; + ctx.set_tensor_name(weights, format!("{prefix}.selected_weights_scaled")) + .map_err(LlamaError::format)?; + } + + let input_3d = ctx + .reshape( + input_hidden, + &[ + ctx.tensor(input_hidden) + .ok_or_else(|| LlamaError::format("invalid moe ffn input tensor"))? + .ne[0], + 1, + i64::try_from(n_tokens) + .map_err(|_| LlamaError::format("n_tokens does not fit in i64"))?, + ], + ) + .map_err(LlamaError::format)?; + + let (gate, up) = if let Some(merged_name) = &spec.merged_gate_up_proj_name { + let merged_weight = require_tensor_id(tensor_ids, merged_name)?; + let gate_up = ctx + .mul_mat_id( + merged_weight, + input_3d, + selected_experts, + BufferUsage::Activations, + ) + .map_err(LlamaError::format)?; + ctx.set_tensor_name(gate_up, format!("{prefix}.gate_up")) + .map_err(LlamaError::format)?; + let gate_up_tensor = ctx + .tensor(gate_up) + .ok_or_else(|| LlamaError::format("invalid merged gate/up tensor"))? + .clone(); + if gate_up_tensor.ne[0] % 2 != 0 { + return Err(LlamaError::format(format!( + "merged gate/up width {} is not divisible by 2", + gate_up_tensor.ne[0] + ))); + } + let ff_width = gate_up_tensor.ne[0] / 2; + let gate = ctx + .view_3d( + gate_up, + ff_width, + gate_up_tensor.ne[1], + gate_up_tensor.ne[2], + gate_up_tensor.nb[1], + gate_up_tensor.nb[2], + 0, + ) + .map_err(LlamaError::format)?; + let up = ctx + .view_3d( + gate_up, + ff_width, + gate_up_tensor.ne[1], + gate_up_tensor.ne[2], + gate_up_tensor.nb[1], + gate_up_tensor.nb[2], + usize::try_from(ff_width) + .ok() + .and_then(|ff_width| ff_width.checked_mul(gate_up_tensor.nb[0])) + .ok_or_else(|| { + LlamaError::format("overflow computing merged gate/up offset") + })?, + ) + .map_err(LlamaError::format)?; + (Some(gate), up) + } else { + let up_weight = require_tensor_id(tensor_ids, &spec.up_proj_name)?; + let up = ctx + .mul_mat_id( + up_weight, + input_3d, + selected_experts, + BufferUsage::Activations, + ) + .map_err(LlamaError::format)?; + ctx.set_tensor_name(up, format!("{prefix}.up")) + .map_err(LlamaError::format)?; + let gate = if let Some(name) = &spec.gate_proj_name { + let gate_weight = require_tensor_id(tensor_ids, name)?; + let gate = ctx + .mul_mat_id( + gate_weight, + input_3d, + selected_experts, + BufferUsage::Activations, + ) + .map_err(LlamaError::format)?; + ctx.set_tensor_name(gate, format!("{prefix}.gate")) + .map_err(LlamaError::format)?; + Some(gate) + } else { + None + }; + (gate, up) + }; + + let activated = if let Some(gate) = gate { + let gate = ctx + .unary(gate, spec.activation, BufferUsage::Activations) + .map_err(LlamaError::format)?; + ctx.set_tensor_name(gate, format!("{prefix}.gate_act")) + .map_err(LlamaError::format)?; + ctx.binary_like_a(Op::Mul, gate, up, BufferUsage::Activations) + .map_err(LlamaError::format)? + } else { + ctx.unary(up, spec.activation, BufferUsage::Activations) + .map_err(LlamaError::format)? + }; + ctx.set_tensor_name(activated, format!("{prefix}.hidden")) + .map_err(LlamaError::format)?; + + let down_weight = require_tensor_id(tensor_ids, &spec.down_proj_name)?; + let experts = ctx + .mul_mat_id( + down_weight, + activated, + selected_experts, + BufferUsage::Activations, + ) + .map_err(LlamaError::format)?; + ctx.set_tensor_name(experts, format!("{prefix}.down")) + .map_err(LlamaError::format)?; + + let experts = ctx + .binary_like_a(Op::Mul, experts, weights, BufferUsage::Activations) + .map_err(LlamaError::format)?; + ctx.set_tensor_name(experts, format!("{prefix}.weighted")) + .map_err(LlamaError::format)?; + + let experts_tensor = ctx + .tensor(experts) + .ok_or_else(|| LlamaError::format("invalid weighted expert tensor"))? + .clone(); + let mut moe_out = ctx + .view_2d( + experts, + experts_tensor.ne[0], + experts_tensor.ne[2], + experts_tensor.nb[2], + 0, + ) + .map_err(LlamaError::format)?; + for slot in 1..usize::try_from(spec.expert_used_count) + .map_err(|_| LlamaError::format("expert_used_count does not fit in usize"))? + { + let view = ctx + .view_2d( + experts, + experts_tensor.ne[0], + experts_tensor.ne[2], + experts_tensor.nb[2], + slot.checked_mul(experts_tensor.nb[1]).ok_or_else(|| { + LlamaError::format("overflow computing routed expert view offset") + })?, + ) + .map_err(LlamaError::format)?; + moe_out = ctx + .binary_like_a(Op::Add, moe_out, view, BufferUsage::Activations) + .map_err(LlamaError::format)?; + } + if spec.expert_used_count == 1 { + moe_out = ctx + .cont_2d(moe_out, experts_tensor.ne[0], experts_tensor.ne[2]) + .map_err(LlamaError::format)?; + } + ctx.set_tensor_name(moe_out, format!("{prefix}.moe_out")) + .map_err(LlamaError::format)?; + + let result_output = if let Some(shared) = &spec.shared_expert { + let mut shared_out = build_dense_gated_ffn( + ctx, + tensor_ids, + input_hidden, + &shared.ffn, + &format!("{prefix}.shared"), + )?; + if let Some(output_gate_name) = &shared.output_gate_name { + let gate_weight = require_tensor_id(tensor_ids, output_gate_name)?; + let gate = ctx + .mul_mat(gate_weight, input_hidden, BufferUsage::Activations) + .map_err(LlamaError::format)?; + let gate = ctx + .unary( + gate, + shared.output_gate_activation, + BufferUsage::Activations, + ) + .map_err(LlamaError::format)?; + shared_out = ctx + .binary_like_a(Op::Mul, shared_out, gate, BufferUsage::Activations) + .map_err(LlamaError::format)?; + } + ctx.binary_like_a(Op::Add, moe_out, shared_out, BufferUsage::Activations) + .map_err(LlamaError::format)? + } else { + moe_out + }; + ctx.set_tensor_name(result_output, format!("{prefix}.result_output")) + .map_err(LlamaError::format)?; + + Ok(BuiltMoeFfn { + selected_experts, + result_output, + }) +} + +pub fn build_moe_ffn_graph( + ctx: &mut Context, + tensor_ids: &BTreeMap, + spec: &MoeFfnSpec, + n_tokens: usize, +) -> Result { + if n_tokens == 0 { + return Err(LlamaError::format( + "moe ffn graph requires at least one token", + )); + } + if spec.expert_used_count == 0 { + return Err(LlamaError::format( + "moe ffn graph requires at least one routed expert", + )); + } + if spec.expert_used_count > spec.expert_count { + return Err(LlamaError::format(format!( + "moe ffn expert_used_count {} exceeds expert_count {}", + spec.expert_used_count, spec.expert_count + ))); + } + + let input_primary = match &spec.input { + ProbeInputKind::TokenIds { .. } => ctx + .new_named_tensor( + "moe_ffn.inp_tokens", + TensorType::I32, + 1, + &[n_tokens as i64], + BufferUsage::Activations, + ) + .map_err(LlamaError::format)?, + ProbeInputKind::Embeddings { + hidden_size, + input_type, + } => ctx + .new_named_tensor( + "moe_ffn.inp_embd", + *input_type, + 2, + &[i64::from(*hidden_size), n_tokens as i64], + BufferUsage::Activations, + ) + .map_err(LlamaError::format)?, + }; + mark_input(ctx, input_primary)?; + + let input_embed = match &spec.input { + ProbeInputKind::TokenIds { + token_embedding_name, + } => { + let token_embd = require_tensor_id(tensor_ids, token_embedding_name)?; + ctx.get_rows(token_embd, input_primary, BufferUsage::Activations) + .map_err(LlamaError::format)? + } + ProbeInputKind::Embeddings { .. } => input_primary, + }; + ctx.set_tensor_name(input_embed, "moe_ffn.input_embed") + .map_err(LlamaError::format)?; + + let built = build_moe_ffn_from_hidden(ctx, tensor_ids, spec, input_embed, n_tokens, "moe_ffn")?; + let result_output = built.result_output; + mark_output(ctx, result_output)?; + + let mut graph = Graph::new(); + graph + .build_forward_expand(ctx, result_output) + .map_err(LlamaError::format)?; + + Ok(MoeFfnGraph { + graph, + input_primary, + input_embed, + selected_experts: built.selected_experts, + result_output, + }) +} + +pub fn prepare_moe_ffn_graph( + ctx: &mut Context, + tensor_ids: &BTreeMap, + spec: &MoeFfnSpec, + n_tokens: usize, + features: MetalDeviceFeatures, +) -> Result<(MoeFfnGraph, MetalPreparedGraph)> { + let block = build_moe_ffn_graph(ctx, tensor_ids, spec, n_tokens)?; + let prepared = prepare_graph(ctx, &block.graph, features).map_err(LlamaError::format)?; + Ok((block, prepared)) +} + +pub fn compile_moe_ffn_metal( + weights: &mut LoadedGgufWeights, + spec: &MoeFfnSpec, + n_tokens: usize, +) -> Result { + let runtime = MetalRuntime::new().map_err(LlamaError::unsupported)?; + let (block, prepared) = prepare_moe_ffn_graph( + &mut weights.ctx, + &weights.tensor_ids, + spec, + n_tokens, + runtime.features(), + )?; + let session = MetalGraphSession::from_runtime( + runtime, + &weights.ctx, + &prepared, + BufferStorageMode::Private, + BufferStorageMode::Private, + ) + .map_err(LlamaError::format)?; + + Ok(CompiledMoeFfnMetal { + spec: spec.clone(), + block, + session, + }) +} + +pub fn execute_prepared_moe_ffn_metal( + runtime: &MetalRuntime, + ctx: &Context, + spec: &MoeFfnSpec, + block: &MoeFfnGraph, + compiled: &MetalCompiledGraph, + input: LogitsProbeInput<'_>, +) -> Result { + let input_primary = match (&spec.input, input) { + (ProbeInputKind::TokenIds { .. }, LogitsProbeInput::TokenIds(token_ids)) => { + i32_slice_as_bytes(token_ids).to_vec() + } + ( + ProbeInputKind::Embeddings { + hidden_size, + input_type, + }, + LogitsProbeInput::EmbeddingsF32 { data, n_tokens }, + ) => { + if *input_type != TensorType::F32 { + return Err(LlamaError::unsupported(format!( + "moe ffn currently expects F32 embeddings, got {}", + input_type.name() + ))); + } + let expected = (*hidden_size as usize) + .checked_mul(n_tokens) + .ok_or_else(|| LlamaError::format("overflow computing moe ffn embedding size"))?; + if data.len() != expected { + return Err(LlamaError::format(format!( + "moe ffn embedding input length mismatch: got {}, expected {}", + data.len(), + expected + ))); + } + f32_slice_as_bytes(data).to_vec() + } + (ProbeInputKind::TokenIds { .. }, LogitsProbeInput::EmbeddingsF32 { .. }) => { + return Err(LlamaError::format( + "moe ffn spec expects token ids but embeddings were provided", + )); + } + (ProbeInputKind::Embeddings { .. }, LogitsProbeInput::TokenIds(_)) => { + return Err(LlamaError::format( + "moe ffn spec expects embeddings but token ids were provided", + )); + } + }; + + let execution = execute_compiled_graph( + runtime, + ctx, + compiled, + &[MetalGraphTensorWrite { + tensor_id: block.input_primary, + bytes: &input_primary, + }], + &[block.result_output, block.selected_experts], + ) + .map_err(LlamaError::format)?; + + let result_bytes = execution + .outputs + .get(&block.result_output) + .ok_or_else(|| LlamaError::format("compiled moe ffn did not produce result bytes"))?; + let hidden = f32_bytes_to_vec(result_bytes)?; + let selected_experts = execution + .outputs + .get(&block.selected_experts) + .ok_or_else(|| LlamaError::format("compiled moe ffn did not produce selected experts"))?; + let output = ctx + .tensor(block.result_output) + .ok_or_else(|| LlamaError::format("moe ffn result_output tensor is invalid"))?; + + Ok(MoeFfnRun { + hidden, + hidden_size: ne_usize(output, 0)?, + n_tokens: ne_usize(output, 1)?, + selected_experts: selected_experts + .chunks_exact(std::mem::size_of::()) + .map(|chunk| i32::from_le_bytes(chunk.try_into().unwrap())) + .collect(), + expert_used_count: usize::try_from(spec.expert_used_count) + .map_err(|_| LlamaError::format("expert_used_count does not fit in usize"))?, + }) +} + +pub fn execute_moe_ffn_graph_metal( + weights: &mut LoadedGgufWeights, + spec: &MoeFfnSpec, + n_tokens: usize, + input: LogitsProbeInput<'_>, +) -> Result { + let compiled = compile_moe_ffn_metal(weights, spec, n_tokens)?; + compiled.execute(&weights.ctx, input) +} + +pub fn execute_moe_ffn_graph_metal_cached( + compiled: &CompiledMoeFfnMetal, + weights: &LoadedGgufWeights, + input: LogitsProbeInput<'_>, +) -> Result { + compiled.execute(&weights.ctx, input) +} + +pub fn build_hybrid_decode_graph( + ctx: &mut Context, + tensor_ids: &BTreeMap, + spec: &HybridDecodeSpec, + n_tokens: usize, +) -> Result { + if n_tokens == 0 { + return Err(LlamaError::format( + "hybrid decode graph requires at least one token", + )); + } + + let input_primary = match &spec.input { + ProbeInputKind::TokenIds { .. } => ctx + .new_named_tensor( + "hybrid_decode.inp_tokens", + TensorType::I32, + 1, + &[n_tokens as i64], + BufferUsage::Activations, + ) + .map_err(LlamaError::format)?, + ProbeInputKind::Embeddings { + hidden_size, + input_type, + } => ctx + .new_named_tensor( + "hybrid_decode.inp_embd", + *input_type, + 2, + &[i64::from(*hidden_size), n_tokens as i64], + BufferUsage::Activations, + ) + .map_err(LlamaError::format)?, + }; + mark_input(ctx, input_primary)?; + + let has_attention = spec + .layers + .iter() + .any(|layer| matches!(layer, HybridLayerSpec::Attention { .. })); + let input_positions = if has_attention { + let positions = ctx + .new_named_tensor( + "hybrid_decode.inp_pos", + TensorType::I32, + 1, + &[n_tokens as i64], + BufferUsage::Activations, + ) + .map_err(LlamaError::format)?; + mark_input(ctx, positions)?; + Some(positions) + } else { + None + }; + let rope_position_components = spec + .layers + .iter() + .filter_map(|layer| match layer { + HybridLayerSpec::Attention { decode, .. } => decode.block.rope.as_ref(), + HybridLayerSpec::Recurrent { .. } => None, + }) + .map(rope_position_component_count) + .max() + .unwrap_or(0); + let input_rope_positions = if rope_position_components > 0 { + let positions = ctx + .new_named_tensor( + "hybrid_decode.inp_rope_pos", + TensorType::I32, + 1, + &[ + i64::try_from(n_tokens.checked_mul(rope_position_components).ok_or_else( + || { + LlamaError::format( + "overflow computing hybrid decode rope position length", + ) + }, + )?) + .map_err(|_| { + LlamaError::format("hybrid rope position length does not fit in i64") + })?, + ], + BufferUsage::Activations, + ) + .map_err(LlamaError::format)?; + mark_input(ctx, positions)?; + Some(positions) + } else { + None + }; + + let mut hidden = match &spec.input { + ProbeInputKind::TokenIds { + token_embedding_name, + } => { + let token_embd = require_tensor_id(tensor_ids, token_embedding_name)?; + ctx.get_rows(token_embd, input_primary, BufferUsage::Activations) + .map_err(LlamaError::format)? + } + ProbeInputKind::Embeddings { .. } => input_primary, + }; + ctx.set_tensor_name(hidden, "hybrid_decode.input_embed") + .map_err(LlamaError::format)?; + + let mut attention_cache_views = Vec::new(); + let mut moe_selected_experts = Vec::new(); + let mut state_updates = Vec::new(); + + for layer in &spec.layers { + match layer { + HybridLayerSpec::Attention { + layer_index, + decode, + ffn, + } => { + let prefix = format!("hybrid_decode.layer{layer_index}"); + let positions = input_positions.ok_or_else(|| { + LlamaError::format(format!( + "attention layer {} requires position input", + layer_index + )) + })?; + let attn = build_attention_decode_from_hidden( + ctx, + tensor_ids, + decode, + hidden, + positions, + input_rope_positions, + n_tokens, + &format!("{prefix}.attn"), + )?; + attention_cache_views.push(HybridAttentionCacheView { + layer_index: *layer_index, + input_mask: attn.input_mask, + k_cache_view: attn.k_cache_view, + v_cache_view: attn.v_cache_view, + k_head_dim: i64::from(decode.block.k_head_dim), + v_head_dim: i64::from(decode.block.v_head_dim), + kv_head_count: i64::from(decode.block.kv_head_count), + max_context: usize::try_from(decode.cache.max_context).map_err(|_| { + LlamaError::format(format!( + "attention layer {} max_context {} does not fit in usize", + layer_index, decode.cache.max_context + )) + })?, + max_sequences: i64::from(decode.cache.max_sequences), + }); + let residual = attn.result_output; + let moe = build_moe_ffn_from_hidden( + ctx, + tensor_ids, + ffn, + residual, + n_tokens, + &format!("{prefix}.moe"), + )?; + moe_selected_experts.push(HybridMoeSelection { + layer_index: *layer_index, + selected_experts: moe.selected_experts, + expert_used_count: usize::try_from(ffn.expert_used_count).map_err(|_| { + LlamaError::format("expert_used_count does not fit in usize") + })?, + }); + hidden = ctx + .binary_like_a( + Op::Add, + moe.result_output, + residual, + BufferUsage::Activations, + ) + .map_err(LlamaError::format)?; + ctx.set_tensor_name(hidden, format!("{prefix}.post_moe")) + .map_err(LlamaError::format)?; + } + HybridLayerSpec::Recurrent { + layer_index, + decode, + ffn, + } => { + let prefix = format!("hybrid_decode.layer{layer_index}"); + let recur = build_delta_net_recurrent_decode_from_hidden( + ctx, + tensor_ids, + decode, + hidden, + n_tokens, + &format!("{prefix}.recur"), + )?; + state_updates.push(recur.r_cache_update); + state_updates.push(recur.s_cache_update); + let residual = recur.result_output; + let moe = build_moe_ffn_from_hidden( + ctx, + tensor_ids, + ffn, + residual, + n_tokens, + &format!("{prefix}.moe"), + )?; + moe_selected_experts.push(HybridMoeSelection { + layer_index: *layer_index, + selected_experts: moe.selected_experts, + expert_used_count: usize::try_from(ffn.expert_used_count).map_err(|_| { + LlamaError::format("expert_used_count does not fit in usize") + })?, + }); + hidden = ctx + .binary_like_a( + Op::Add, + moe.result_output, + residual, + BufferUsage::Activations, + ) + .map_err(LlamaError::format)?; + ctx.set_tensor_name(hidden, format!("{prefix}.post_moe")) + .map_err(LlamaError::format)?; + } + } + } + + let result_hidden = hidden; + ctx.set_tensor_name(result_hidden, "hybrid_decode.result_hidden") + .map_err(LlamaError::format)?; + mark_output(ctx, result_hidden)?; + + let result_norm = build_rms_norm_mul( + ctx, + tensor_ids, + result_hidden, + spec.rms_epsilon, + &spec.output_norm_name, + "hybrid_decode.result_norm", + )?; + let output_weight = require_tensor_id(tensor_ids, &spec.output_name)?; + let result_logits = ctx + .mul_mat(output_weight, result_norm, BufferUsage::Activations) + .map_err(LlamaError::format)?; + ctx.set_tensor_name(result_logits, "hybrid_decode.result_logits") + .map_err(LlamaError::format)?; + mark_output(ctx, result_logits)?; + + let mut graph = Graph::new(); + for &update in &state_updates { + graph + .build_forward_expand(ctx, update) + .map_err(LlamaError::format)?; + } + graph + .build_forward_expand(ctx, result_hidden) + .map_err(LlamaError::format)?; + graph + .build_forward_expand(ctx, result_logits) + .map_err(LlamaError::format)?; + + Ok(HybridDecodeGraph { + graph, + input_primary, + input_positions, + input_rope_positions, + attention_cache_views, + moe_selected_experts, + state_updates, + result_hidden, + result_logits, + }) +} + +pub fn prepare_hybrid_decode_graph( + ctx: &mut Context, + tensor_ids: &BTreeMap, + spec: &HybridDecodeSpec, + n_tokens: usize, + features: MetalDeviceFeatures, +) -> Result<(HybridDecodeGraph, MetalPreparedGraph)> { + let decode = build_hybrid_decode_graph(ctx, tensor_ids, spec, n_tokens)?; + let prepared = prepare_graph(ctx, &decode.graph, features).map_err(LlamaError::format)?; + Ok((decode, prepared)) +} + +pub fn compile_hybrid_decode_metal( + weights: &mut LoadedGgufWeights, + spec: &HybridDecodeSpec, + n_tokens: usize, +) -> Result { + let runtime = MetalRuntime::new().map_err(LlamaError::unsupported)?; + let (decode, prepared) = prepare_hybrid_decode_graph( + &mut weights.ctx, + &weights.tensor_ids, + spec, + n_tokens, + runtime.features(), + )?; + let session = MetalGraphSession::from_runtime( + runtime, + &weights.ctx, + &prepared, + BufferStorageMode::Private, + BufferStorageMode::Private, + ) + .map_err(LlamaError::format)?; + + Ok(CompiledHybridDecodeMetal { + spec: spec.clone(), + decode, + session, + }) +} + +pub fn execute_prepared_hybrid_decode_metal( + runtime: &MetalRuntime, + ctx: &mut Context, + spec: &HybridDecodeSpec, + decode: &HybridDecodeGraph, + compiled: &MetalCompiledGraph, + input: LogitsProbeInput<'_>, + positions: &[i32], + cache_tokens: usize, +) -> Result { + if decode.input_positions.is_some() { + if positions.is_empty() { + return Err(LlamaError::format( + "hybrid decode requires at least one position", + )); + } + for cache_view in &decode.attention_cache_views { + if cache_tokens == 0 || cache_tokens > cache_view.max_context { + return Err(LlamaError::format(format!( + "hybrid decode cache_tokens {} is outside 1..={} for attention layer {}", + cache_tokens, cache_view.max_context, cache_view.layer_index + ))); + } + } + if positions.iter().copied().any(|pos| { + pos < 0 + || usize::try_from(pos) + .ok() + .map(|pos| pos >= cache_tokens) + .unwrap_or(true) + }) { + return Err(LlamaError::format(format!( + "hybrid decode positions {:?} exceed cache_tokens {}", + positions, cache_tokens + ))); + } + } else if !positions.is_empty() { + return Err(LlamaError::format( + "hybrid decode received positions for a graph without attention layers", + )); + } + + let input_primary = match (&spec.input, input) { + (ProbeInputKind::TokenIds { .. }, LogitsProbeInput::TokenIds(token_ids)) => { + if decode.input_positions.is_some() && token_ids.len() != positions.len() { + return Err(LlamaError::format(format!( + "hybrid decode token/position length mismatch: {} vs {}", + token_ids.len(), + positions.len() + ))); + } + i32_slice_as_bytes(token_ids).to_vec() + } + ( + ProbeInputKind::Embeddings { + hidden_size, + input_type, + }, + LogitsProbeInput::EmbeddingsF32 { data, n_tokens }, + ) => { + if *input_type != TensorType::F32 { + return Err(LlamaError::unsupported(format!( + "hybrid decode currently expects F32 embeddings, got {}", + input_type.name() + ))); + } + if decode.input_positions.is_some() && n_tokens != positions.len() { + return Err(LlamaError::format(format!( + "hybrid decode embedding/position length mismatch: {} vs {}", + n_tokens, + positions.len() + ))); + } + let expected = (*hidden_size as usize) + .checked_mul(n_tokens) + .ok_or_else(|| { + LlamaError::format("overflow computing hybrid decode embedding size") + })?; + if data.len() != expected { + return Err(LlamaError::format(format!( + "hybrid decode embedding input length mismatch: got {}, expected {}", + data.len(), + expected + ))); + } + f32_slice_as_bytes(data).to_vec() + } + (ProbeInputKind::TokenIds { .. }, LogitsProbeInput::EmbeddingsF32 { .. }) => { + return Err(LlamaError::format( + "hybrid decode spec expects token ids but embeddings were provided", + )); + } + (ProbeInputKind::Embeddings { .. }, LogitsProbeInput::TokenIds(_)) => { + return Err(LlamaError::format( + "hybrid decode spec expects embeddings but token ids were provided", + )); + } + }; + + for cache_view in &decode.attention_cache_views { + configure_attention_cache_view( + ctx, + cache_view.k_cache_view, + cache_view.k_head_dim, + cache_tokens, + cache_view.kv_head_count, + cache_view.max_sequences, + )?; + configure_attention_cache_view( + ctx, + cache_view.v_cache_view, + cache_view.v_head_dim, + cache_tokens, + cache_view.kv_head_count, + cache_view.max_sequences, + )?; + if let Some(input_mask) = cache_view.input_mask { + configure_attention_mask_view(ctx, input_mask, cache_tokens, positions.len())?; + } + } + + let mut writes = vec![MetalGraphTensorWrite { + tensor_id: decode.input_primary, + bytes: &input_primary, + }]; + if let Some(input_positions) = decode.input_positions { + writes.push(MetalGraphTensorWrite { + tensor_id: input_positions, + bytes: i32_slice_as_bytes(positions), + }); + } + let hybrid_rope_positions = if decode.input_rope_positions.is_some() { + let rope = spec + .layers + .iter() + .find_map(|layer| match layer { + HybridLayerSpec::Attention { decode, .. } => decode.block.rope.as_ref(), + HybridLayerSpec::Recurrent { .. } => None, + }) + .ok_or_else(|| { + LlamaError::format( + "hybrid decode has rope position input without an attention rope spec", + ) + })?; + Some(encode_rope_positions(rope, positions, positions.len())?) + } else { + None + }; + if let Some(input_rope_positions) = decode.input_rope_positions { + writes.push(MetalGraphTensorWrite { + tensor_id: input_rope_positions, + bytes: i32_slice_as_bytes(hybrid_rope_positions.as_deref().ok_or_else(|| { + LlamaError::format("hybrid decode rope positions were not prepared") + })?), + }); + } + let attention_mask_bytes = decode + .attention_cache_views + .iter() + .filter_map(|cache_view| { + cache_view.input_mask.map(|input_mask| { + position_attention_mask_bytes_for_tensor(ctx, input_mask, cache_tokens, positions) + }) + }) + .collect::>>()?; + let mut attention_mask_index = 0usize; + for cache_view in &decode.attention_cache_views { + if cache_view.input_mask.is_some() { + let bytes = attention_mask_bytes + .get(attention_mask_index) + .ok_or_else(|| { + LlamaError::format("hybrid decode mask storage is unexpectedly empty") + })?; + attention_mask_index += 1; + writes.push(MetalGraphTensorWrite { + tensor_id: cache_view.input_mask.ok_or_else(|| { + LlamaError::format(format!( + "hybrid decode attention layer {} lost its input mask", + cache_view.layer_index + )) + })?, + bytes, + }); + } + } + + let mut outputs = vec![decode.result_hidden, decode.result_logits]; + outputs.extend( + decode + .moe_selected_experts + .iter() + .map(|sel| sel.selected_experts), + ); + let execution = execute_compiled_graph(runtime, ctx, compiled, &writes, &outputs) + .map_err(LlamaError::format)?; + + let hidden = execution + .outputs + .get(&decode.result_hidden) + .ok_or_else(|| LlamaError::format("hybrid decode did not produce hidden bytes"))?; + let hidden = f32_bytes_to_vec(hidden)?; + let logits = execution + .outputs + .get(&decode.result_logits) + .ok_or_else(|| LlamaError::format("hybrid decode did not produce logits bytes"))?; + let logits = f32_bytes_to_vec(logits)?; + let hidden_tensor = ctx + .tensor(decode.result_hidden) + .ok_or_else(|| LlamaError::format("hybrid decode result_hidden tensor is invalid"))?; + let logits_tensor = ctx + .tensor(decode.result_logits) + .ok_or_else(|| LlamaError::format("hybrid decode result_logits tensor is invalid"))?; + + let mut selected_experts = Vec::with_capacity(decode.moe_selected_experts.len()); + for selection in &decode.moe_selected_experts { + let bytes = execution + .outputs + .get(&selection.selected_experts) + .ok_or_else(|| { + LlamaError::format(format!( + "hybrid decode did not produce selected experts for layer {}", + selection.layer_index + )) + })?; + let experts = bytes + .chunks_exact(std::mem::size_of::()) + .map(|chunk| i32::from_le_bytes(chunk.try_into().unwrap())) + .collect::>(); + selected_experts.push((selection.layer_index, experts)); + } + + Ok(HybridDecodeRun { + hidden, + logits, + n_tokens: ne_usize(hidden_tensor, 1)?, + hidden_size: ne_usize(hidden_tensor, 0)?, + vocab_size: ne_usize(logits_tensor, 0)?, + selected_experts, + }) +} + +pub fn execute_hybrid_decode_graph_metal( + weights: &mut LoadedGgufWeights, + spec: &HybridDecodeSpec, + n_tokens: usize, + input: LogitsProbeInput<'_>, + positions: &[i32], + cache_tokens: usize, +) -> Result { + let compiled = compile_hybrid_decode_metal(weights, spec, n_tokens)?; + compiled.execute(&mut weights.ctx, input, positions, cache_tokens) +} + +pub fn execute_hybrid_decode_graph_metal_cached( + compiled: &CompiledHybridDecodeMetal, + weights: &mut LoadedGgufWeights, + input: LogitsProbeInput<'_>, + positions: &[i32], + cache_tokens: usize, +) -> Result { + compiled.execute(&mut weights.ctx, input, positions, cache_tokens) +} + +pub fn execute_logits_probe_metal( + weights: &LoadedGgufWeights, + spec: &LogitsProbeSpec, + input: LogitsProbeInput<'_>, + output_ids: &[i32], +) -> Result { + if output_ids.is_empty() { + return Err(LlamaError::format( + "logits probe requires at least one output id", + )); + } + + let input_embed = match (&spec.input, input) { + ( + ProbeInputKind::TokenIds { + token_embedding_name, + }, + LogitsProbeInput::TokenIds(token_ids), + ) => { + let token_embd_id = weights.require_tensor_id(token_embedding_name)?; + let token_embd = require_tensor(&weights.ctx, token_embd_id)?; + let hidden_size = ne_usize(token_embd, 0)?; + let vocab_size = ne_usize(token_embd, 1)?; + try_get_rows_ggml_bytes( + weights + .ctx + .tensor_data(token_embd_id) + .map_err(LlamaError::format)?, + token_embd.desc.ty.ggml_type(), + hidden_size, + vocab_size, + token_ids, + ) + .ok_or_else(|| { + LlamaError::unsupported(format!( + "Metal get_rows is unavailable or unsupported for {}", + token_embd.desc.ty.name() + )) + })? + } + ( + ProbeInputKind::Embeddings { + hidden_size, + input_type, + }, + LogitsProbeInput::EmbeddingsF32 { data, n_tokens }, + ) => { + if *input_type != TensorType::F32 { + return Err(LlamaError::unsupported(format!( + "eager Metal probe currently expects F32 embeddings, got {}", + input_type.name() + ))); + } + let expected = (*hidden_size as usize) + .checked_mul(n_tokens) + .ok_or_else(|| LlamaError::format("overflow computing embedding input size"))?; + if data.len() != expected { + return Err(LlamaError::format(format!( + "embedding input length mismatch: got {}, expected {}", + data.len(), + expected + ))); + } + data.to_vec() + } + (ProbeInputKind::TokenIds { .. }, LogitsProbeInput::EmbeddingsF32 { .. }) => { + return Err(LlamaError::format( + "logits probe spec expects token ids but embeddings were provided", + )); + } + (ProbeInputKind::Embeddings { .. }, LogitsProbeInput::TokenIds(_)) => { + return Err(LlamaError::format( + "logits probe spec expects embeddings but token ids were provided", + )); + } + }; + + let hidden_size = match &spec.input { + ProbeInputKind::TokenIds { + token_embedding_name, + } => { + let token_embd_id = weights.require_tensor_id(token_embedding_name)?; + let token_embd = require_tensor(&weights.ctx, token_embd_id)?; + ne_usize(token_embd, 0)? + } + ProbeInputKind::Embeddings { hidden_size, .. } => *hidden_size as usize, + }; + let n_tokens = input_embed + .len() + .checked_div(hidden_size) + .ok_or_else(|| LlamaError::format("invalid hidden size for probe input"))?; + + for &row in output_ids { + let row_ok = usize::try_from(row).ok().is_some_and(|row| row < n_tokens); + if !row_ok { + return Err(LlamaError::format(format!( + "probe output row {} is out of range {}", + row, n_tokens + ))); + } + } + + let selected = try_get_rows_ggml_bytes( + f32_slice_as_bytes(&input_embed), + TensorType::F32.ggml_type(), + hidden_size, + n_tokens, + output_ids, + ) + .ok_or_else(|| LlamaError::unsupported("Metal F32 get_rows is unavailable".to_string()))?; + + let output_norm_id = require_tensor_id(tensor_ids(weights), &spec.output_norm_name)?; + let output_norm = read_tensor_as_f32(weights, output_norm_id)?; + let normed = try_rms_norm_mul_f32( + &selected, + &[output_ids.len(), hidden_size], + &output_norm, + &[hidden_size], + spec.rms_epsilon, + ) + .ok_or_else(|| LlamaError::unsupported("Metal rms_norm_mul is unavailable".to_string()))?; + + let output_id = require_tensor_id(tensor_ids(weights), &spec.output_name)?; + let output = require_tensor(&weights.ctx, output_id)?; + let vocab_size = ne_usize(output, 1)?; + let logits = try_matmul_nt_ggml_bytes( + &normed, + weights + .ctx + .tensor_data(output_id) + .map_err(LlamaError::format)?, + output.desc.ty.ggml_type(), + output_ids.len(), + hidden_size, + vocab_size, + ) + .ok_or_else(|| { + LlamaError::unsupported(format!( + "Metal quantized matmul is unavailable or unsupported for {}", + output.desc.ty.name() + )) + })?; + + Ok(LogitsProbeRun { + logits, + n_outputs: output_ids.len(), + vocab_size, + }) +} + +fn build_dense_gated_ffn( + ctx: &mut Context, + tensor_ids: &BTreeMap, + input: TensorId, + spec: &DenseGatedFfnSpec, + prefix: &str, +) -> Result { + let gate_weight = require_tensor_id(tensor_ids, &spec.gate_proj_name)?; + let gate = ctx + .mul_mat(gate_weight, input, BufferUsage::Activations) + .map_err(LlamaError::format)?; + ctx.set_tensor_name(gate, &format!("{prefix}.gate")) + .map_err(LlamaError::format)?; + + let up_weight = require_tensor_id(tensor_ids, &spec.up_proj_name)?; + let up = ctx + .mul_mat(up_weight, input, BufferUsage::Activations) + .map_err(LlamaError::format)?; + ctx.set_tensor_name(up, &format!("{prefix}.up")) + .map_err(LlamaError::format)?; + + let gate = ctx + .unary(gate, spec.gate_activation, BufferUsage::Activations) + .map_err(LlamaError::format)?; + ctx.set_tensor_name(gate, &format!("{prefix}.gate_act")) + .map_err(LlamaError::format)?; + + let hidden = ctx + .binary_like_a(Op::Mul, gate, up, BufferUsage::Activations) + .map_err(LlamaError::format)?; + ctx.set_tensor_name(hidden, &format!("{prefix}.hidden")) + .map_err(LlamaError::format)?; + + let down_weight = require_tensor_id(tensor_ids, &spec.down_proj_name)?; + let output = ctx + .mul_mat(down_weight, hidden, BufferUsage::Activations) + .map_err(LlamaError::format)?; + ctx.set_tensor_name(output, &format!("{prefix}.output")) + .map_err(LlamaError::format)?; + + Ok(output) +} + +fn build_rms_norm_mul( + ctx: &mut Context, + tensor_ids: &BTreeMap, + src: TensorId, + epsilon: f32, + weight_name: &str, + tensor_name: &str, +) -> Result { + let norm = ctx + .rms_norm_eps(src, epsilon, BufferUsage::Activations) + .map_err(LlamaError::format)?; + let weight = require_tensor_id(tensor_ids, weight_name)?; + let scaled = ctx + .binary_like_a(Op::Mul, norm, weight, BufferUsage::Activations) + .map_err(LlamaError::format)?; + ctx.set_tensor_name(scaled, tensor_name) + .map_err(LlamaError::format)?; + Ok(scaled) +} + +fn configure_attention_cache_view( + ctx: &mut Context, + tensor_id: TensorId, + ne0: i64, + ne1: usize, + ne2: i64, + ne3: i64, +) -> Result<()> { + let tensor = ctx + .tensor(tensor_id) + .ok_or_else(|| LlamaError::format(format!("invalid tensor id {}", tensor_id)))?; + let strides = tensor.nb; + let layout = TensorLayout::from_parts( + 4, + &[ + ne0, + i64::try_from(ne1).map_err(|_| { + LlamaError::format(format!("cache length {} does not fit in i64", ne1)) + })?, + ne2, + ne3, + ], + &strides, + ) + .map_err(LlamaError::format)?; + ctx.set_tensor_layout(tensor_id, layout) + .map_err(LlamaError::format) +} + +fn configure_attention_mask_view( + ctx: &mut Context, + tensor_id: TensorId, + key_count: usize, + query_count: usize, +) -> Result<()> { + let tensor = ctx + .tensor(tensor_id) + .ok_or_else(|| LlamaError::format(format!("invalid tensor id {}", tensor_id)))?; + let strides = tensor.nb; + let layout = TensorLayout::from_parts( + 4, + &[ + i64::try_from(key_count).map_err(|_| { + LlamaError::format(format!("mask key count {} does not fit in i64", key_count)) + })?, + i64::try_from(query_count).map_err(|_| { + LlamaError::format(format!( + "mask query count {} does not fit in i64", + query_count + )) + })?, + tensor.ne[2], + tensor.ne[3], + ], + &strides, + ) + .map_err(LlamaError::format)?; + ctx.set_tensor_layout(tensor_id, layout) + .map_err(LlamaError::format) +} + +fn row_size(ty: TensorType, ne: i64) -> Result { + ggml_row_size_for_type(ty, ne).map_err(LlamaError::format) +} + +fn bytes_for_elements(ty: TensorType, elements: u64) -> Result { + let scalar_size = ty.scalar_size_bytes().ok_or_else(|| { + LlamaError::unsupported(format!( + "cache layout requires scalar tensor types, got {}", + ty.name() + )) + })?; + usize::try_from(elements) + .ok() + .and_then(|elements| elements.checked_mul(scalar_size)) + .ok_or_else(|| { + LlamaError::format(format!( + "overflow computing byte size for {} elements of {}", + elements, + ty.name() + )) + }) +} + +fn mark_input(ctx: &mut Context, id: TensorId) -> Result<()> { + ctx.tensor_mut(id) + .ok_or_else(|| LlamaError::format(format!("invalid input tensor {}", id)))? + .set_input(); + Ok(()) +} + +fn mark_output(ctx: &mut Context, id: TensorId) -> Result<()> { + ctx.tensor_mut(id) + .ok_or_else(|| LlamaError::format(format!("invalid output tensor {}", id)))? + .set_output(); + Ok(()) +} + +fn require_tensor_id(tensor_ids: &BTreeMap, name: &str) -> Result { + tensor_ids + .get(name) + .copied() + .ok_or_else(|| LlamaError::format(format!("missing resident tensor '{}'", name))) +} + +fn tensor_ids(weights: &LoadedGgufWeights) -> &BTreeMap { + &weights.tensor_ids +} + +fn require_tensor(ctx: &Context, id: TensorId) -> Result<&Tensor> { + ctx.tensor(id) + .ok_or_else(|| LlamaError::format(format!("invalid tensor id {}", id))) +} + +fn ne_usize(tensor: &Tensor, dim: usize) -> Result { + usize::try_from(tensor.ne[dim]).map_err(|_| { + LlamaError::format(format!( + "tensor '{}' dimension {} does not fit in usize", + tensor.name().unwrap_or(""), + tensor.ne[dim] + )) + }) +} + +fn read_tensor_as_f32(weights: &LoadedGgufWeights, id: TensorId) -> Result> { + let tensor = require_tensor(&weights.ctx, id)?; + let bytes = weights.ctx.tensor_data(id).map_err(LlamaError::format)?; + match tensor.desc.ty { + TensorType::F32 => bytes + .chunks_exact(4) + .map(|chunk| Ok(f32::from_le_bytes([chunk[0], chunk[1], chunk[2], chunk[3]]))) + .collect(), + TensorType::F16 => bytes + .chunks_exact(2) + .map(|chunk| Ok(f16_to_f32(u16::from_le_bytes([chunk[0], chunk[1]])))) + .collect(), + TensorType::BF16 => bytes + .chunks_exact(2) + .map(|chunk| { + let bits = u32::from(u16::from_le_bytes([chunk[0], chunk[1]])) << 16; + Ok(f32::from_bits(bits)) + }) + .collect(), + other => Err(LlamaError::unsupported(format!( + "expected scalar tensor for eager probe, got {}", + other.name() + ))), + } +} + +fn f32_slice_as_bytes(slice: &[f32]) -> &[u8] { + unsafe { std::slice::from_raw_parts(slice.as_ptr() as *const u8, std::mem::size_of_val(slice)) } +} + +fn i32_slice_as_bytes(slice: &[i32]) -> &[u8] { + unsafe { std::slice::from_raw_parts(slice.as_ptr() as *const u8, std::mem::size_of_val(slice)) } +} + +fn f32_bytes_to_vec(bytes: &[u8]) -> Result> { + if bytes.len() % std::mem::size_of::() != 0 { + return Err(LlamaError::format(format!( + "f32 output byte length {} is not divisible by {}", + bytes.len(), + std::mem::size_of::() + ))); + } + + bytes + .chunks_exact(4) + .map(|chunk| Ok(f32::from_le_bytes([chunk[0], chunk[1], chunk[2], chunk[3]]))) + .collect() +} + +#[cfg(test)] +mod tests { + use super::{HybridCacheShape, HybridCacheTemplate, HybridCacheTypes}; + use makepad_ggml::TensorType; + + #[test] + fn hybrid_cache_template_materializes_types_and_shape() { + let template = HybridCacheTemplate { + attention_layers: vec![3, 7], + recurrent_layers: vec![0, 1, 2], + attention_k_width: 128, + attention_v_width: 64, + recurrent_r_width: 4096, + recurrent_s_width: 8192, + }; + + let spec = template.materialize( + HybridCacheShape { + n_ctx_seq: 4096, + n_seq_max: 8, + }, + HybridCacheTypes { + attention_k_type: TensorType::F16, + attention_v_type: TensorType::F16, + recurrent_r_type: TensorType::F32, + recurrent_s_type: TensorType::F32, + }, + ); + + assert_eq!(spec.n_ctx_seq, 4096); + assert_eq!(spec.n_seq_max, 8); + assert_eq!(spec.attention_layers, vec![3, 7]); + assert_eq!(spec.recurrent_layers, vec![0, 1, 2]); + assert_eq!(spec.attention_k_type, TensorType::F16); + assert_eq!(spec.recurrent_s_type, TensorType::F32); + } +} diff --git a/libs/llama/src/session.rs b/libs/llama/src/session.rs new file mode 100644 index 0000000000..1dd178d647 --- /dev/null +++ b/libs/llama/src/session.rs @@ -0,0 +1,403 @@ +use std::collections::BTreeMap; +use std::path::Path; + +use makepad_ggml::TensorType; + +use crate::error::{LlamaError, Result}; +use crate::model::LlamaModel; +use crate::plan::ModelExecutionPlan; +use crate::qwen35moe_runtime::qwen35moe_hybrid_decode_spec; +use crate::runtime::{ + compile_hybrid_decode_metal, execute_hybrid_decode_graph_metal_cached, + CompiledHybridDecodeMetal, HybridCacheLayout, HybridCacheShape, HybridCacheTypes, + HybridDecodeRun, HybridDecodeSpec, LogitsProbeInput, +}; +use crate::vocab::LlamaVocab; +use crate::weights::LoadedGgufWeights; + +const DEFAULT_EXTRA_ACTIVATION_BYTES: usize = 512 << 20; +const DEFAULT_PREFILL_BATCH_SIZE: usize = 1; + +#[derive(Clone, Copy, Debug)] +pub struct LlamaSessionConfig { + pub max_context: Option, + pub max_sequences: u32, + pub prefill_batch_size: usize, + pub attention_k_type: TensorType, + pub attention_v_type: TensorType, + pub recurrent_r_type: TensorType, + pub recurrent_s_type: TensorType, + pub extra_activation_bytes: usize, +} + +impl Default for LlamaSessionConfig { + fn default() -> Self { + Self { + max_context: None, + max_sequences: 1, + prefill_batch_size: DEFAULT_PREFILL_BATCH_SIZE, + attention_k_type: TensorType::F16, + attention_v_type: TensorType::F16, + recurrent_r_type: TensorType::F32, + recurrent_s_type: TensorType::F32, + extra_activation_bytes: DEFAULT_EXTRA_ACTIVATION_BYTES, + } + } +} + +#[derive(Clone, Copy, Debug, Eq, PartialEq)] +pub enum LlamaStopReason { + MaxNewTokens, + EndOfSequence, + PaddingToken, +} + +#[derive(Clone, Debug)] +pub struct LlamaGeneration { + pub token_ids: Vec, + pub text: String, + pub stop_reason: LlamaStopReason, +} + +pub struct LlamaSession { + model: LlamaModel, + vocab: LlamaVocab, + plan: ModelExecutionPlan, + spec: HybridDecodeSpec, + config: LlamaSessionConfig, + max_context: usize, + context_extra_bytes: usize, + weights: LoadedGgufWeights, + compiled_by_batch: BTreeMap, + token_ids: Vec, + last_run: Option, +} + +impl LlamaSession { + pub fn load(path: impl AsRef, config: LlamaSessionConfig) -> Result { + Self::from_owned_model(LlamaModel::load(path)?, config) + } + + pub fn from_model(model: &LlamaModel, config: LlamaSessionConfig) -> Result { + Self::from_owned_model(model.clone(), config) + } + + pub fn model(&self) -> &LlamaModel { + &self.model + } + + pub fn vocab(&self) -> &LlamaVocab { + &self.vocab + } + + pub fn config(&self) -> &LlamaSessionConfig { + &self.config + } + + pub fn token_ids(&self) -> &[i32] { + &self.token_ids + } + + pub fn token_count(&self) -> usize { + self.token_ids.len() + } + + pub fn max_context(&self) -> usize { + self.max_context + } + + pub fn remaining_context(&self) -> usize { + self.max_context.saturating_sub(self.token_ids.len()) + } + + pub fn last_run(&self) -> Option<&HybridDecodeRun> { + self.last_run.as_ref() + } + + pub fn last_logits(&self) -> Option<&[f32]> { + self.last_run.as_ref().map(|run| run.logits.as_slice()) + } + + pub fn reset(&mut self) -> Result<()> { + let (weights, compiled_by_batch) = build_runtime_state( + &self.model, + &self.plan, + &self.spec, + self.context_extra_bytes, + )?; + self.weights = weights; + self.compiled_by_batch = compiled_by_batch; + self.token_ids.clear(); + self.last_run = None; + Ok(()) + } + + pub fn append_token(&mut self, token_id: i32) -> Result<()> { + self.append_tokens(std::slice::from_ref(&token_id)) + } + + pub fn append_tokens(&mut self, token_ids: &[i32]) -> Result<()> { + if token_ids.is_empty() { + return Ok(()); + } + self.ensure_capacity(token_ids.len())?; + let prefill_batch_size = self.config.prefill_batch_size.max(1); + let mut offset = 0; + while offset < token_ids.len() { + let batch_size = (token_ids.len() - offset).min(prefill_batch_size); + self.append_token_batch(&token_ids[offset..offset + batch_size])?; + offset += batch_size; + } + Ok(()) + } + + pub fn next_greedy_token(&mut self) -> Result> { + let next_token = self.greedy_candidate()?; + if self.stop_reason_for(next_token).is_some() { + return Ok(None); + } + self.append_token(next_token)?; + Ok(Some(next_token)) + } + + pub fn continue_greedy(&mut self, max_new_tokens: usize) -> Result { + let mut token_ids = Vec::with_capacity(max_new_tokens); + let mut stop_reason = LlamaStopReason::MaxNewTokens; + + for _ in 0..max_new_tokens { + let next_token = self.greedy_candidate()?; + if let Some(reason) = self.stop_reason_for(next_token) { + stop_reason = reason; + break; + } + self.append_token(next_token)?; + token_ids.push(next_token); + } + + Ok(LlamaGeneration { + text: self.vocab.decode_tokens(&token_ids)?, + token_ids, + stop_reason, + }) + } + + fn from_owned_model(model: LlamaModel, config: LlamaSessionConfig) -> Result { + model.validate_layout()?; + if config.max_sequences == 0 { + return Err(LlamaError::format( + "session max_sequences must be at least 1", + )); + } + + let vocab = LlamaVocab::from_model(&model)?; + let plan = model.execution_plan()?; + let max_context = resolve_max_context(&model, config)?; + let cache_shape = HybridCacheShape { + n_ctx_seq: max_context, + n_seq_max: config.max_sequences, + }; + let cache_types = HybridCacheTypes { + attention_k_type: config.attention_k_type, + attention_v_type: config.attention_v_type, + recurrent_r_type: config.recurrent_r_type, + recurrent_s_type: config.recurrent_s_type, + }; + let cache_bytes = plan + .hybrid_cache + .as_ref() + .map(|template| HybridCacheLayout::new(template.materialize(cache_shape, cache_types))) + .transpose()? + .map_or(0, |layout| layout.total_bytes); + let context_extra_bytes = cache_bytes + .checked_add(config.extra_activation_bytes) + .ok_or_else(|| LlamaError::format("overflow computing session activation bytes"))?; + let spec = qwen35moe_hybrid_decode_spec( + &model, + cache_shape.n_ctx_seq, + cache_shape.n_seq_max, + config.attention_k_type, + config.attention_v_type, + config.recurrent_r_type, + config.recurrent_s_type, + )?; + let (weights, compiled_by_batch) = + build_runtime_state(&model, &plan, &spec, context_extra_bytes)?; + + Ok(Self { + model, + vocab, + plan, + spec, + config, + max_context: usize::try_from(max_context) + .map_err(|_| LlamaError::format("session max_context does not fit in usize"))?, + context_extra_bytes, + weights, + compiled_by_batch, + token_ids: Vec::new(), + last_run: None, + }) + } + + fn ensure_capacity(&self, additional_tokens: usize) -> Result<()> { + let total = self + .token_ids + .len() + .checked_add(additional_tokens) + .ok_or_else(|| LlamaError::format("overflow computing total session tokens"))?; + if total > self.max_context { + return Err(LlamaError::format(format!( + "session context overflow: need {} tokens, max_context is {}", + total, self.max_context + ))); + } + Ok(()) + } + + fn greedy_candidate(&self) -> Result { + argmax_token_id(self.last_logits().ok_or_else(|| { + LlamaError::format("session has no logits yet; append context tokens before continuing") + })?) + } + + fn stop_reason_for(&self, token_id: i32) -> Option { + if Some(token_id) == self.vocab.eos_token_id() { + Some(LlamaStopReason::EndOfSequence) + } else if Some(token_id) == self.vocab.padding_token_id() { + Some(LlamaStopReason::PaddingToken) + } else { + None + } + } + + fn append_token_batch(&mut self, token_ids: &[i32]) -> Result<()> { + let batch_size = token_ids.len(); + self.ensure_compiled_batch(batch_size)?; + let start = self.token_ids.len(); + let positions = (start..start + batch_size) + .map(|position| { + i32::try_from(position) + .map_err(|_| LlamaError::format("token position does not fit in i32")) + }) + .collect::>>()?; + let cache_tokens = start + .checked_add(batch_size) + .ok_or_else(|| LlamaError::format("overflow computing session cache length"))?; + let run = { + let compiled = self + .compiled_by_batch + .get(&batch_size) + .ok_or_else(|| LlamaError::format("compiled batch graph was not cached"))?; + execute_hybrid_decode_graph_metal_cached( + compiled, + &mut self.weights, + LogitsProbeInput::TokenIds(token_ids), + &positions, + cache_tokens, + )? + }; + self.token_ids.extend_from_slice(token_ids); + self.last_run = Some(collapse_last_token_run(run)?); + Ok(()) + } + + fn ensure_compiled_batch(&mut self, batch_size: usize) -> Result<()> { + if self.compiled_by_batch.contains_key(&batch_size) { + return Ok(()); + } + let compiled = compile_hybrid_decode_metal(&mut self.weights, &self.spec, batch_size)?; + self.compiled_by_batch.insert(batch_size, compiled); + Ok(()) + } +} + +fn resolve_max_context(model: &LlamaModel, config: LlamaSessionConfig) -> Result { + let max_context = config + .max_context + .unwrap_or(model.require_qwen35moe()?.context_length); + if max_context == 0 { + return Err(LlamaError::format("session max_context must be at least 1")); + } + Ok(max_context) +} + +fn build_runtime_state( + model: &LlamaModel, + plan: &ModelExecutionPlan, + spec: &HybridDecodeSpec, + context_extra_bytes: usize, +) -> Result<( + LoadedGgufWeights, + BTreeMap, +)> { + let mut weights = plan + .full_weights + .allocate_and_load_with_extra(&model.gguf, context_extra_bytes)?; + let mut compiled_by_batch = BTreeMap::new(); + compiled_by_batch.insert(1, compile_hybrid_decode_metal(&mut weights, spec, 1)?); + Ok((weights, compiled_by_batch)) +} + +fn argmax_token_id(logits: &[f32]) -> Result { + let (index, _) = logits + .iter() + .copied() + .enumerate() + .max_by(|a, b| a.1.total_cmp(&b.1).then_with(|| a.0.cmp(&b.0))) + .ok_or_else(|| LlamaError::format("logit vector was empty"))?; + i32::try_from(index).map_err(|_| LlamaError::format("argmax index does not fit in i32")) +} + +fn collapse_last_token_run(run: HybridDecodeRun) -> Result { + if run.n_tokens <= 1 { + return Ok(run); + } + + if run.hidden.len() < run.hidden_size { + return Err(LlamaError::format(format!( + "hybrid decode hidden length mismatch: got {}, need at least {}", + run.hidden.len(), + run.hidden_size + ))); + } + + if run.logits.len() < run.vocab_size { + return Err(LlamaError::format(format!( + "hybrid decode logits length mismatch: got {}, need at least {}", + run.logits.len(), + run.vocab_size + ))); + } + + let inferred_tokens = if run.vocab_size > 0 && run.logits.len() % run.vocab_size == 0 { + run.logits.len() / run.vocab_size + } else if run.hidden_size > 0 && run.hidden.len() % run.hidden_size == 0 { + run.hidden.len() / run.hidden_size + } else { + run.n_tokens + }; + let hidden_start = run.hidden.len() - run.hidden_size; + let logits_start = run.logits.len() - run.vocab_size; + let selected_experts = run + .selected_experts + .into_iter() + .map(|(layer_index, experts)| { + let per_token = experts.len().checked_div(inferred_tokens).unwrap_or(0); + let experts = if per_token == 0 || per_token * inferred_tokens != experts.len() { + experts + } else { + experts[experts.len() - per_token..].to_vec() + }; + (layer_index, experts) + }) + .collect(); + + Ok(HybridDecodeRun { + hidden: run.hidden[hidden_start..].to_vec(), + logits: run.logits[logits_start..].to_vec(), + n_tokens: 1, + hidden_size: run.hidden_size, + vocab_size: run.vocab_size, + selected_experts, + }) +} diff --git a/libs/llama/src/vocab.rs b/libs/llama/src/vocab.rs new file mode 100644 index 0000000000..c03ffae769 --- /dev/null +++ b/libs/llama/src/vocab.rs @@ -0,0 +1,297 @@ +use crate::error::{LlamaError, Result}; +use crate::gguf::{GgufArray, GgufFile, GgufValue}; +use crate::model::LlamaModel; +use std::collections::HashMap; + +const GGUF_KEY_TOKENIZER_TOKENS: &str = "tokenizer.ggml.tokens"; +const GGUF_KEY_TOKENIZER_MODEL: &str = "tokenizer.ggml.model"; +const GGUF_KEY_TOKENIZER_PRE: &str = "tokenizer.ggml.pre"; +const GGUF_KEY_TOKENIZER_EOS_TOKEN_ID: &str = "tokenizer.ggml.eos_token_id"; +const GGUF_KEY_TOKENIZER_PADDING_TOKEN_ID: &str = "tokenizer.ggml.padding_token_id"; + +#[derive(Clone, Debug, Eq, PartialEq)] +pub enum LlamaTokenizerKind { + Plain, + Gpt2, +} + +#[derive(Clone, Debug)] +pub struct LlamaVocab { + pieces: Vec, + tokenizer_kind: LlamaTokenizerKind, + tokenizer_pre: Option, + eos_token_id: Option, + padding_token_id: Option, +} + +pub struct LlamaTextDecoder { + tokenizer_kind: LlamaTokenizerKind, + gpt2_byte_decoder: Option>, + pending_bytes: Vec, +} + +impl LlamaVocab { + pub fn from_model(model: &LlamaModel) -> Result { + Self::from_gguf(&model.gguf) + } + + pub fn from_gguf(gguf: &GgufFile) -> Result { + let value = gguf.get_value(GGUF_KEY_TOKENIZER_TOKENS).ok_or_else(|| { + LlamaError::format(format!( + "missing required gguf key '{}'", + GGUF_KEY_TOKENIZER_TOKENS + )) + })?; + let pieces = match value { + GgufValue::Array(GgufArray::String(tokens)) => tokens + .iter() + .map(|token| token.to_string_lossy().into_owned()) + .collect(), + other => { + return Err(LlamaError::format(format!( + "gguf key '{}' has type {}, expected string array", + GGUF_KEY_TOKENIZER_TOKENS, + other.value_type().name() + ))) + } + }; + + let tokenizer_kind = match optional_utf8_string(gguf, GGUF_KEY_TOKENIZER_MODEL)? { + Some(model) if model.eq_ignore_ascii_case("gpt2") => LlamaTokenizerKind::Gpt2, + _ => LlamaTokenizerKind::Plain, + }; + + Ok(Self { + pieces, + tokenizer_kind, + tokenizer_pre: optional_utf8_string(gguf, GGUF_KEY_TOKENIZER_PRE)?, + eos_token_id: optional_u32(gguf, GGUF_KEY_TOKENIZER_EOS_TOKEN_ID) + .and_then(|id| i32::try_from(id).ok()), + padding_token_id: optional_u32(gguf, GGUF_KEY_TOKENIZER_PADDING_TOKEN_ID) + .and_then(|id| i32::try_from(id).ok()), + }) + } + + pub fn len(&self) -> usize { + self.pieces.len() + } + + pub fn is_empty(&self) -> bool { + self.pieces.is_empty() + } + + pub fn piece(&self, token_id: i32) -> Option<&str> { + usize::try_from(token_id) + .ok() + .and_then(|index| self.pieces.get(index)) + .map(String::as_str) + } + + pub fn escaped_piece(&self, token_id: i32) -> Option { + self.piece(token_id) + .map(|piece| piece.chars().flat_map(|ch| ch.escape_default()).collect()) + } + + pub fn tokenizer_kind(&self) -> &LlamaTokenizerKind { + &self.tokenizer_kind + } + + pub fn tokenizer_pre(&self) -> Option<&str> { + self.tokenizer_pre.as_deref() + } + + pub fn eos_token_id(&self) -> Option { + self.eos_token_id + } + + pub fn padding_token_id(&self) -> Option { + self.padding_token_id + } + + pub fn text_decoder(&self) -> LlamaTextDecoder { + LlamaTextDecoder::new(self.tokenizer_kind.clone()) + } + + pub fn decode_tokens(&self, token_ids: &[i32]) -> Result { + let mut decoder = self.text_decoder(); + let mut text = String::new(); + for token_id in token_ids.iter().copied() { + let chunk = decoder.push_token(self, token_id).ok_or_else(|| { + LlamaError::format(format!("token id {} is outside the vocabulary", token_id)) + })?; + text.push_str(&chunk); + } + text.push_str(&decoder.finish()); + Ok(text) + } +} + +impl LlamaTextDecoder { + pub fn new(tokenizer_kind: LlamaTokenizerKind) -> Self { + let gpt2_byte_decoder = match tokenizer_kind { + LlamaTokenizerKind::Gpt2 => Some(gpt2_byte_decoder()), + LlamaTokenizerKind::Plain => None, + }; + Self { + tokenizer_kind, + gpt2_byte_decoder, + pending_bytes: Vec::new(), + } + } + + pub fn push_token(&mut self, vocab: &LlamaVocab, token_id: i32) -> Option { + Some(self.push_piece(vocab.piece(token_id)?)) + } + + pub fn push_piece(&mut self, piece: &str) -> String { + match self.tokenizer_kind { + LlamaTokenizerKind::Plain => piece.to_owned(), + LlamaTokenizerKind::Gpt2 => { + let decoder = self + .gpt2_byte_decoder + .as_ref() + .expect("gpt2 decoder missing"); + for ch in piece.chars() { + if let Some(byte) = decoder.get(&ch) { + self.pending_bytes.push(*byte); + } else { + let mut utf8 = [0_u8; 4]; + self.pending_bytes + .extend_from_slice(ch.encode_utf8(&mut utf8).as_bytes()); + } + } + drain_utf8_prefix(&mut self.pending_bytes) + } + } + } + + pub fn finish(mut self) -> String { + if self.pending_bytes.is_empty() { + String::new() + } else { + let text = String::from_utf8_lossy(&self.pending_bytes).into_owned(); + self.pending_bytes.clear(); + text + } + } +} + +fn optional_utf8_string(gguf: &GgufFile, key: &str) -> Result> { + match gguf.get_value(key) { + None => Ok(None), + Some(GgufValue::String(value)) => value.try_utf8().map(|s| Some(s.to_owned())), + Some(other) => Err(LlamaError::format(format!( + "gguf key '{}' has type {}, expected string", + key, + other.value_type().name() + ))), + } +} + +fn optional_u32(gguf: &GgufFile, key: &str) -> Option { + gguf.get_value(key).and_then(value_to_u32) +} + +fn value_to_u32(value: &GgufValue) -> Option { + match value { + GgufValue::Uint32(v) => Some(*v), + GgufValue::Uint64(v) => u32::try_from(*v).ok(), + GgufValue::Int32(v) => u32::try_from(*v).ok(), + GgufValue::Int64(v) => u32::try_from(*v).ok(), + _ => None, + } +} + +fn gpt2_byte_decoder() -> HashMap { + let mut used = [false; 256]; + let mut bs = Vec::new(); + + for value in 33_u16..=126 { + used[value as usize] = true; + bs.push(value); + } + for value in 161_u16..=172 { + used[value as usize] = true; + bs.push(value); + } + for value in 174_u16..=255 { + used[value as usize] = true; + bs.push(value); + } + + let mut cs = bs.clone(); + let mut extra = 0_u16; + for byte in 0_u16..=255 { + if !used[byte as usize] { + bs.push(byte); + cs.push(256 + extra); + extra += 1; + } + } + + bs.into_iter() + .zip(cs) + .map(|(byte, codepoint)| { + ( + char::from_u32(u32::from(codepoint)).expect("invalid gpt2 codepoint"), + byte as u8, + ) + }) + .collect() +} + +fn drain_utf8_prefix(pending_bytes: &mut Vec) -> String { + let mut out = String::new(); + loop { + match std::str::from_utf8(pending_bytes) { + Ok(valid) => { + out.push_str(valid); + pending_bytes.clear(); + return out; + } + Err(err) => { + let valid_up_to = err.valid_up_to(); + if valid_up_to > 0 { + out.push_str(&String::from_utf8_lossy(&pending_bytes[..valid_up_to])); + pending_bytes.drain(..valid_up_to); + continue; + } + match err.error_len() { + None => return out, + Some(invalid_len) => { + out.push('\u{FFFD}'); + pending_bytes.drain(..invalid_len); + } + } + } + } + } +} + +#[cfg(test)] +mod tests { + use super::{gpt2_byte_decoder, LlamaTextDecoder, LlamaTokenizerKind}; + + #[test] + fn gpt2_byte_decoder_maps_space_marker() { + let decoder = gpt2_byte_decoder(); + assert_eq!(decoder.get(&'Ġ'), Some(&b' ')); + assert_eq!(decoder.get(&'H'), Some(&b'H')); + } + + #[test] + fn gpt2_text_decoder_decodes_ascii_and_space() { + let mut decoder = LlamaTextDecoder::new(LlamaTokenizerKind::Gpt2); + assert_eq!(decoder.push_piece("Hello"), "Hello"); + assert_eq!(decoder.push_piece("Ġworld"), " world"); + assert_eq!(decoder.finish(), ""); + } + + #[test] + fn gpt2_text_decoder_handles_utf8_split_across_tokens() { + let mut decoder = LlamaTextDecoder::new(LlamaTokenizerKind::Gpt2); + assert_eq!(decoder.push_piece("Ã"), ""); + assert_eq!(decoder.push_piece("©"), "é"); + assert_eq!(decoder.finish(), ""); + } +} diff --git a/libs/llama/src/weights.rs b/libs/llama/src/weights.rs new file mode 100644 index 0000000000..04327ba5c3 --- /dev/null +++ b/libs/llama/src/weights.rs @@ -0,0 +1,144 @@ +use std::collections::BTreeMap; + +use makepad_ggml::{ggml_pad, BufferUsage, Context, InitParams, TensorId, GGML_MEM_ALIGN}; + +use crate::error::{LlamaError, Result}; +use crate::gguf::{GgufFile, GgufTensorInfo}; + +#[derive(Clone, Debug)] +pub struct GgufWeightLayout { + pub tensors: Vec, + pub total_bytes: usize, +} + +#[derive(Clone, Debug)] +pub struct LoadedGgufWeights { + pub ctx: Context, + pub tensor_ids: BTreeMap, +} + +impl LoadedGgufWeights { + pub fn tensor_id(&self, name: &str) -> Option { + self.tensor_ids.get(name).copied() + } + + pub fn require_tensor_id(&self, name: &str) -> Result { + self.tensor_id(name) + .ok_or_else(|| LlamaError::format(format!("missing resident tensor '{}'", name))) + } +} + +impl GgufWeightLayout { + pub fn from_tensors(tensors: impl IntoIterator) -> Result { + let mut dedup = BTreeMap::new(); + for tensor in tensors { + dedup.entry(tensor.name.clone()).or_insert(tensor); + } + + let tensors = dedup.into_values().collect::>(); + let total_bytes = padded_total_bytes(&tensors)?; + Ok(Self { + tensors, + total_bytes, + }) + } + + pub fn allocate_context(&self) -> Result { + self.allocate_context_with_extra(0) + } + + pub fn allocate_context_with_extra(&self, extra_bytes: usize) -> Result { + let mem_size = ggml_pad( + self.total_bytes + .checked_add(extra_bytes) + .ok_or_else(|| LlamaError::format("overflow computing gguf context size"))?, + GGML_MEM_ALIGN, + ); + let mut ctx = Context::new(InitParams { + mem_size, + mem_buffer: None, + no_alloc: false, + }); + let tensor_ids = allocate_tensor_ids(&mut ctx, &self.tensors)?; + Ok(LoadedGgufWeights { ctx, tensor_ids }) + } + + pub fn allocate_and_load(&self, gguf: &GgufFile) -> Result { + self.allocate_and_load_with_extra(gguf, 0) + } + + pub fn allocate_and_load_with_extra( + &self, + gguf: &GgufFile, + extra_bytes: usize, + ) -> Result { + let mut loaded = self.allocate_context_with_extra(extra_bytes)?; + for tensor in &self.tensors { + let tensor_id = loaded.tensor_id(&tensor.name).ok_or_else(|| { + LlamaError::format(format!("missing loaded tensor '{}'", tensor.name)) + })?; + let dst = loaded + .ctx + .tensor_data_mut(tensor_id) + .map_err(LlamaError::format)?; + gguf.read_tensor_into(&tensor.name, dst)?; + } + Ok(loaded) + } +} + +fn allocate_tensor_ids( + ctx: &mut Context, + tensors: &[GgufTensorInfo], +) -> Result> { + let mut tensor_ids = BTreeMap::new(); + for tensor in tensors { + let dims = tensor_extents_i64(tensor)?; + let id = ctx + .new_named_tensor( + tensor.name.clone(), + tensor.tensor_type, + dims.len(), + &dims, + BufferUsage::Weights, + ) + .map_err(LlamaError::format)?; + tensor_ids.insert(tensor.name.clone(), id); + } + Ok(tensor_ids) +} + +fn padded_total_bytes(tensors: &[GgufTensorInfo]) -> Result { + let mut total = 0usize; + for tensor in tensors { + total = ggml_pad(total, GGML_MEM_ALIGN); + let size_bytes = usize::try_from(tensor.size_bytes).map_err(|_| { + LlamaError::format(format!( + "tensor '{}' size {} does not fit in usize", + tensor.name, tensor.size_bytes + )) + })?; + total = total.checked_add(size_bytes).ok_or_else(|| { + LlamaError::format(format!( + "overflow computing total bytes for tensor '{}'", + tensor.name + )) + })?; + } + Ok(ggml_pad(total, GGML_MEM_ALIGN)) +} + +fn tensor_extents_i64(tensor: &GgufTensorInfo) -> Result> { + tensor + .dimensions + .iter() + .map(|&dim| { + i64::try_from(dim).map_err(|_| { + LlamaError::format(format!( + "tensor '{}' dimension {} does not fit in i64", + tensor.name, dim + )) + }) + }) + .collect() +} diff --git a/libs/lz4/Cargo.toml b/libs/lz4/Cargo.toml new file mode 100644 index 0000000000..1a24e7f4dd --- /dev/null +++ b/libs/lz4/Cargo.toml @@ -0,0 +1,11 @@ +[package] +name = "makepad-lz4" +version = "0.1.0" +edition = "2021" +license = "BSD-2-Clause" +description = "Rust port of the core LZ4 block compressor and decompressor" + +[features] +force-scalar = [] + +[dependencies] diff --git a/libs/lz4/src/lib.rs b/libs/lz4/src/lib.rs new file mode 100644 index 0000000000..adcdd058c6 --- /dev/null +++ b/libs/lz4/src/lib.rs @@ -0,0 +1,830 @@ +// Port of the core block codec from upstream LZ4 (`local/lz4/lib/lz4.c` and +// `local/lz4/doc/lz4_Block_format.md`). +// +// This first Rust version intentionally focuses on the block API surface: +// `compress_bound`, fast/default compression, and safe decompression. +// It does not yet expose the frame, HC, or streaming APIs. + +use std::{fmt, ptr}; + +#[cfg(all(target_arch = "aarch64", not(feature = "force-scalar")))] +use std::arch::aarch64::{vceqq_u8, vld1q_u8, vminvq_u8, vst1q_u8}; + +const MINMATCH: usize = 4; +const LASTLITERALS: usize = 5; +const MFLIMIT: usize = 12; +const MAX_DISTANCE: usize = 65_535; +const HASH_LOG: usize = 12; +const HASH_SIZE: usize = 1 << HASH_LOG; +const RUN_MASK: usize = 15; +const ML_MASK: usize = 15; +const SKIP_TRIGGER: usize = 6; +const INVALID_INDEX: u32 = u32::MAX; +const ACCELERATION_DEFAULT: usize = 1; +const ACCELERATION_MAX: usize = 65_537; +const FAST_INPUT_MARGIN: usize = 14 + 2; +const FAST_OUTPUT_MARGIN: usize = 14 + 18; +const INC32_TABLE: [usize; 8] = [0, 1, 2, 1, 0, 4, 4, 4]; +const DEC64_TABLE: [isize; 8] = [0, 0, 0, -1, -4, 1, 2, 3]; +pub const MAX_INPUT_SIZE: usize = 0x7E00_0000; + +#[cfg(any(target_arch = "x86", target_arch = "x86_64", target_arch = "aarch64"))] +const FAST_DEC_LOOP: bool = true; +#[cfg(not(any(target_arch = "x86", target_arch = "x86_64", target_arch = "aarch64")))] +const FAST_DEC_LOOP: bool = false; + +#[derive(Debug, Clone, Copy, PartialEq, Eq)] +pub enum CompressError { + InputTooLarge, + OutputTooSmall, +} + +impl fmt::Display for CompressError { + fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result { + match self { + CompressError::InputTooLarge => write!(f, "input is larger than LZ4 allows"), + CompressError::OutputTooSmall => write!(f, "output buffer is too small"), + } + } +} + +impl std::error::Error for CompressError {} + +#[derive(Debug, Clone, Copy, PartialEq, Eq)] +pub enum DecompressError { + MalformedInput, + OutputTooSmall, +} + +impl fmt::Display for DecompressError { + fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result { + match self { + DecompressError::MalformedInput => write!(f, "malformed lz4 block"), + DecompressError::OutputTooSmall => write!(f, "output buffer is too small"), + } + } +} + +impl std::error::Error for DecompressError {} + +#[inline] +pub const fn compress_bound(input_size: usize) -> usize { + if input_size > MAX_INPUT_SIZE { + 0 + } else { + input_size + input_size / 255 + 16 + } +} + +pub const fn implementation_name() -> &'static str { + #[cfg(all(target_arch = "aarch64", not(feature = "force-scalar")))] + { + "aarch64-neon" + } + #[cfg(all(target_arch = "aarch64", feature = "force-scalar"))] + { + "aarch64-scalar" + } + #[cfg(not(target_arch = "aarch64"))] + { + "scalar" + } +} + +pub fn compress_default_into(input: &[u8], output: &mut [u8]) -> Result { + compress_fast_into(input, output, ACCELERATION_DEFAULT) +} + +pub fn compress_default(input: &[u8]) -> Result, CompressError> { + compress_fast(input, ACCELERATION_DEFAULT) +} + +pub fn compress_fast(input: &[u8], acceleration: usize) -> Result, CompressError> { + let bound = compress_bound(input.len()); + if bound == 0 { + return Err(CompressError::InputTooLarge); + } + let mut output = vec![0u8; bound]; + let written = compress_fast_into(input, &mut output, acceleration)?; + output.truncate(written); + Ok(output) +} + +pub fn compress_fast_into( + input: &[u8], + output: &mut [u8], + acceleration: usize, +) -> Result { + if input.len() > MAX_INPUT_SIZE { + return Err(CompressError::InputTooLarge); + } + if input.is_empty() { + if output.is_empty() { + return Err(CompressError::OutputTooSmall); + } + output[0] = 0; + return Ok(1); + } + + let acceleration = acceleration.clamp(ACCELERATION_DEFAULT, ACCELERATION_MAX); + let mut table = [INVALID_INDEX; HASH_SIZE]; + let mut src: usize; + let mut anchor = 0usize; + let src_end = input.len(); + let match_limit = src_end.saturating_sub(LASTLITERALS); + let mf_limit = src_end.saturating_sub(MFLIMIT); + let mut dst = 0usize; + + if input.len() < 13 { + return emit_last_literals(input, anchor, output, dst); + } + + table[hash_at(input, 0)] = 0; + src = 1; + let mut forward = src; + let mut forward_hash = hash_at(input, forward); + + while forward <= mf_limit { + let mut step = 1usize; + let mut search_match_nb = acceleration << SKIP_TRIGGER; + let mut match_pos; + + loop { + let hash = forward_hash; + src = forward; + forward = forward.saturating_add(step); + step = (search_match_nb >> SKIP_TRIGGER).max(1); + search_match_nb = search_match_nb.saturating_add(1); + + let candidate = table[hash]; + table[hash] = src as u32; + + if forward > mf_limit { + return emit_last_literals(input, anchor, output, dst); + } + forward_hash = hash_at(input, forward); + + if candidate == INVALID_INDEX { + continue; + } + match_pos = candidate as usize; + if src <= match_pos || src - match_pos > MAX_DISTANCE { + continue; + } + if load_u32(input, match_pos) != load_u32(input, src) { + continue; + } + break; + } + + while src > anchor && match_pos > 0 && input[src - 1] == input[match_pos - 1] { + src -= 1; + match_pos -= 1; + } + + let match_len = + MINMATCH + count_match(input, src + MINMATCH, match_pos + MINMATCH, match_limit); + dst = emit_sequence(input, anchor, src, match_pos, match_len, output, dst)?; + src += match_len; + anchor = src; + + if src > mf_limit { + break; + } + + table[hash_at(input, src - 2)] = (src - 2) as u32; + let hash = hash_at(input, src); + let candidate = table[hash]; + table[hash] = src as u32; + if candidate != INVALID_INDEX { + let candidate = candidate as usize; + if src > candidate + && src - candidate <= MAX_DISTANCE + && load_u32(input, candidate) == load_u32(input, src) + { + let match_len = MINMATCH + + count_match(input, src + MINMATCH, candidate + MINMATCH, match_limit); + dst = emit_sequence(input, anchor, src, candidate, match_len, output, dst)?; + src += match_len; + anchor = src; + if src > mf_limit { + break; + } + table[hash_at(input, src - 2)] = (src - 2) as u32; + forward = src + 1; + if forward <= mf_limit { + forward_hash = hash_at(input, forward); + continue; + } + break; + } + } + + forward = src + 1; + if forward > mf_limit { + break; + } + forward_hash = hash_at(input, forward); + } + + emit_last_literals(input, anchor, output, dst) +} + +pub fn decompress_safe(input: &[u8], output: &mut [u8]) -> Result { + if output.is_empty() { + return if input == [0] { + Ok(0) + } else { + Err(DecompressError::MalformedInput) + }; + } + if input.is_empty() { + return Err(DecompressError::MalformedInput); + } + + let mut src = 0usize; + let mut dst = 0usize; + let short_input_end = input.len().saturating_sub(FAST_INPUT_MARGIN); + let short_output_end = output.len().saturating_sub(FAST_OUTPUT_MARGIN); + let input_ptr = input.as_ptr(); + let output_ptr = output.as_mut_ptr(); + + while src < input.len() { + let token = input[src]; + src += 1; + + let lit_token = token as usize >> 4; + let match_token = token as usize & ML_MASK; + let literal_len = read_length(lit_token, input, &mut src)?; + let mut offset = None; + + if FAST_DEC_LOOP + && lit_token != RUN_MASK + && src < short_input_end + && dst <= short_output_end + { + unsafe { + copy_16_bytes(output_ptr.add(dst), input_ptr.add(src)); + } + src += literal_len; + dst += literal_len; + + let fast_offset = unsafe { load_u16_le_ptr(input_ptr.add(src)) as usize }; + src += 2; + if fast_offset == 0 || fast_offset > dst { + return Err(DecompressError::MalformedInput); + } + offset = Some(fast_offset); + + if match_token != ML_MASK && fast_offset >= 8 { + let match_src = dst - fast_offset; + unsafe { + if fast_offset >= 16 { + copy_16_bytes(output_ptr.add(dst), output_ptr.add(match_src)); + } else { + copy_u64(output_ptr.add(dst), output_ptr.add(match_src)); + copy_u64(output_ptr.add(dst + 8), output_ptr.add(match_src + 8)); + } + ptr::copy_nonoverlapping( + output_ptr.add(match_src + 16), + output_ptr.add(dst + 16), + 2, + ); + } + dst += match_token + MINMATCH; + continue; + } + } else { + if src + literal_len > input.len() { + return Err(DecompressError::MalformedInput); + } + if dst + literal_len > output.len() { + return Err(DecompressError::OutputTooSmall); + } + unsafe { + copy_bytes(output_ptr.add(dst), input_ptr.add(src), literal_len); + } + src += literal_len; + dst += literal_len; + } + + if src == input.len() { + return Ok(dst); + } + + let offset = match offset { + Some(offset) => offset, + None => { + if src + 2 > input.len() { + return Err(DecompressError::MalformedInput); + } + let offset = unsafe { load_u16_le_ptr(input_ptr.add(src)) as usize }; + src += 2; + offset + } + }; + if offset == 0 || offset > dst { + return Err(DecompressError::MalformedInput); + } + + let match_len = MINMATCH + read_length(match_token, input, &mut src)?; + if dst + match_len > output.len() { + return Err(DecompressError::OutputTooSmall); + } + + unsafe { + copy_match(output_ptr, dst, offset, match_len); + } + dst += match_len; + } + + Err(DecompressError::MalformedInput) +} + +fn emit_sequence( + input: &[u8], + anchor: usize, + match_start: usize, + match_pos: usize, + match_len: usize, + output: &mut [u8], + mut dst: usize, +) -> Result { + let literal_len = match_start - anchor; + let literal_len_bytes = extra_len_bytes(literal_len); + let match_code = match_len - MINMATCH; + let match_len_bytes = extra_len_bytes(match_code); + let needed = 1 + literal_len_bytes + literal_len + 2 + match_len_bytes; + if dst + needed > output.len() { + return Err(CompressError::OutputTooSmall); + } + + let token_pos = dst; + output[token_pos] = 0; + dst += 1; + + let lit_nibble = literal_len.min(RUN_MASK); + output[token_pos] = (lit_nibble << 4) as u8; + dst = write_length_bytes(literal_len, RUN_MASK, output, dst)?; + + unsafe { + copy_bytes( + output.as_mut_ptr().add(dst), + input.as_ptr().add(anchor), + literal_len, + ); + } + dst += literal_len; + + let offset = match_start - match_pos; + output[dst..dst + 2].copy_from_slice(&(offset as u16).to_le_bytes()); + dst += 2; + + let match_nibble = match_code.min(ML_MASK); + output[token_pos] |= match_nibble as u8; + dst = write_length_bytes(match_code, ML_MASK, output, dst)?; + + Ok(dst) +} + +fn emit_last_literals( + input: &[u8], + anchor: usize, + output: &mut [u8], + mut dst: usize, +) -> Result { + let literal_len = input.len() - anchor; + let needed = 1 + extra_len_bytes(literal_len) + literal_len; + if dst + needed > output.len() { + return Err(CompressError::OutputTooSmall); + } + + let token_pos = dst; + output[token_pos] = 0; + dst += 1; + + let lit_nibble = literal_len.min(RUN_MASK); + output[token_pos] = (lit_nibble << 4) as u8; + dst = write_length_bytes(literal_len, RUN_MASK, output, dst)?; + unsafe { + copy_bytes( + output.as_mut_ptr().add(dst), + input.as_ptr().add(anchor), + literal_len, + ); + } + dst += literal_len; + Ok(dst) +} + +#[inline] +fn extra_len_bytes(len: usize) -> usize { + if len < RUN_MASK { + 0 + } else { + 1 + (len - RUN_MASK) / 255 + } +} + +fn write_length_bytes( + len: usize, + inline_limit: usize, + output: &mut [u8], + mut dst: usize, +) -> Result { + if len < inline_limit { + return Ok(dst); + } + let mut remaining = len - inline_limit; + while remaining >= 255 { + if dst >= output.len() { + return Err(CompressError::OutputTooSmall); + } + output[dst] = 255; + dst += 1; + remaining -= 255; + } + if dst >= output.len() { + return Err(CompressError::OutputTooSmall); + } + output[dst] = remaining as u8; + dst += 1; + Ok(dst) +} + +fn read_length(nibble: usize, input: &[u8], src: &mut usize) -> Result { + let mut len = nibble; + if nibble != 15 { + return Ok(len); + } + loop { + let byte = *input.get(*src).ok_or(DecompressError::MalformedInput)? as usize; + *src += 1; + len = len + .checked_add(byte) + .ok_or(DecompressError::MalformedInput)?; + if byte != 255 { + return Ok(len); + } + } +} + +#[inline] +fn hash_at(input: &[u8], pos: usize) -> usize { + let sequence = load_u32(input, pos); + ((sequence.wrapping_mul(2_654_435_761)) >> ((MINMATCH * 8 - HASH_LOG) as u32)) as usize + & (HASH_SIZE - 1) +} + +#[inline] +fn count_match(input: &[u8], a: usize, b: usize, limit: usize) -> usize { + #[cfg(all(target_arch = "aarch64", not(feature = "force-scalar")))] + { + let remaining = (limit - a).min(limit - b); + if remaining >= 16 { + unsafe { + return count_match_aarch64( + input.as_ptr().add(a), + input.as_ptr().add(b), + remaining, + ); + } + } + } + + count_match_scalar(input, a, b, limit) +} + +#[inline] +fn count_match_scalar(input: &[u8], mut a: usize, mut b: usize, limit: usize) -> usize { + let start = a; + while a + 8 <= limit && b + 8 <= limit { + let diff = load_u64(input, a) ^ load_u64(input, b); + if diff == 0 { + a += 8; + b += 8; + } else { + return (a - start) + (diff.trailing_zeros() as usize / 8); + } + } + while a < limit && b < limit && input[a] == input[b] { + a += 1; + b += 1; + } + a - start +} + +#[cfg(all(target_arch = "aarch64", not(feature = "force-scalar")))] +#[target_feature(enable = "neon")] +unsafe fn count_match_aarch64( + mut a_ptr: *const u8, + mut b_ptr: *const u8, + mut remaining: usize, +) -> usize { + let mut matched = 0usize; + + while remaining >= 16 { + let diff_index = first_diff_16(a_ptr, b_ptr); + if diff_index != 16 { + return matched + diff_index; + } + a_ptr = a_ptr.add(16); + b_ptr = b_ptr.add(16); + matched += 16; + remaining -= 16; + } + + while remaining >= 8 { + let diff = + ptr::read_unaligned(a_ptr as *const u64) ^ ptr::read_unaligned(b_ptr as *const u64); + if diff != 0 { + return matched + (diff.trailing_zeros() as usize / 8); + } + a_ptr = a_ptr.add(8); + b_ptr = b_ptr.add(8); + matched += 8; + remaining -= 8; + } + + while remaining > 0 && *a_ptr == *b_ptr { + a_ptr = a_ptr.add(1); + b_ptr = b_ptr.add(1); + matched += 1; + remaining -= 1; + } + + matched +} + +#[cfg(all(target_arch = "aarch64", not(feature = "force-scalar")))] +#[target_feature(enable = "neon")] +unsafe fn first_diff_16(a_ptr: *const u8, b_ptr: *const u8) -> usize { + let matches = vceqq_u8(vld1q_u8(a_ptr), vld1q_u8(b_ptr)); + if vminvq_u8(matches) == u8::MAX { + return 16; + } + + let mut mask = [0u8; 16]; + vst1q_u8(mask.as_mut_ptr(), matches); + mask.iter().position(|&byte| byte == 0).unwrap_or(16) +} + +#[inline] +fn load_u32(input: &[u8], pos: usize) -> u32 { + unsafe { load_u32_le_ptr(input.as_ptr().add(pos)) } +} + +#[inline] +fn load_u64(input: &[u8], pos: usize) -> u64 { + unsafe { load_u64_le_ptr(input.as_ptr().add(pos)) } +} + +#[inline] +unsafe fn load_u16_le_ptr(ptr: *const u8) -> u16 { + u16::from_le(ptr::read_unaligned(ptr as *const u16)) +} + +#[inline(always)] +unsafe fn load_u32_le_ptr(ptr: *const u8) -> u32 { + u32::from_le(ptr::read_unaligned(ptr as *const u32)) +} + +#[inline(always)] +unsafe fn load_u64_le_ptr(ptr: *const u8) -> u64 { + u64::from_le(ptr::read_unaligned(ptr as *const u64)) +} + +#[inline(always)] +unsafe fn copy_bytes(dst: *mut u8, src: *const u8, len: usize) { + if len != 0 { + ptr::copy_nonoverlapping(src, dst, len); + } +} + +#[inline(always)] +unsafe fn copy_u64(dst: *mut u8, src: *const u8) { + let value = ptr::read_unaligned(src as *const u64); + ptr::write_unaligned(dst as *mut u64, value); +} + +#[inline(always)] +unsafe fn copy_16_bytes(dst: *mut u8, src: *const u8) { + #[cfg(all(target_arch = "aarch64", not(feature = "force-scalar")))] + { + return copy_16_bytes_aarch64(dst, src); + } + + #[cfg(any(not(target_arch = "aarch64"), feature = "force-scalar"))] + { + ptr::copy_nonoverlapping(src, dst, 16); + } +} + +#[cfg(all(target_arch = "aarch64", not(feature = "force-scalar")))] +#[target_feature(enable = "neon")] +unsafe fn copy_16_bytes_aarch64(dst: *mut u8, src: *const u8) { + let bytes = vld1q_u8(src); + vst1q_u8(dst, bytes); +} + +#[inline(always)] +unsafe fn copy_match(output_ptr: *mut u8, dst: usize, offset: usize, match_len: usize) { + let dst_ptr = output_ptr.add(dst); + let src_ptr = dst_ptr.sub(offset) as *const u8; + + if offset < 8 { + copy_match_short_offset(dst_ptr, src_ptr, match_len, offset); + return; + } + + let mut copied = 0usize; + while copied + 16 <= match_len && offset >= 16 { + copy_16_bytes(dst_ptr.add(copied), src_ptr.add(copied)); + copied += 16; + } + while copied + 8 <= match_len { + copy_u64(dst_ptr.add(copied), src_ptr.add(copied)); + copied += 8; + } + while copied < match_len { + ptr::write(dst_ptr.add(copied), *src_ptr.add(copied)); + copied += 1; + } +} + +#[inline(always)] +unsafe fn copy_match_short_offset( + dst_ptr: *mut u8, + src_ptr: *const u8, + match_len: usize, + offset: usize, +) { + let mut copied = 0usize; + + match offset { + 1 | 2 | 4 => { + let mut pattern = [0u8; 8]; + for index in 0..8 { + pattern[index] = *src_ptr.add(index % offset); + } + let repeated = u64::from_ne_bytes(pattern); + while copied + 8 <= match_len { + ptr::write_unaligned(dst_ptr.add(copied) as *mut u64, repeated); + copied += 8; + } + while copied < match_len { + ptr::write(dst_ptr.add(copied), pattern[copied % offset]); + copied += 1; + } + } + _ => { + if match_len < 8 { + while copied < match_len { + ptr::write(dst_ptr.add(copied), *src_ptr.add(copied)); + copied += 1; + } + return; + } + + ptr::write(dst_ptr, *src_ptr); + ptr::write(dst_ptr.add(1), *src_ptr.add(1)); + ptr::write(dst_ptr.add(2), *src_ptr.add(2)); + ptr::write(dst_ptr.add(3), *src_ptr.add(3)); + let adjusted = src_ptr.add(INC32_TABLE[offset]); + ptr::copy_nonoverlapping(adjusted, dst_ptr.add(4), 4); + + copied = 8; + let mut tail_src = adjust_ptr(adjusted, -DEC64_TABLE[offset]); + while copied + 8 <= match_len { + copy_u64(dst_ptr.add(copied), tail_src); + copied += 8; + tail_src = tail_src.add(8); + } + while copied < match_len { + ptr::write(dst_ptr.add(copied), *tail_src); + copied += 1; + tail_src = tail_src.add(1); + } + } + } +} + +#[inline(always)] +unsafe fn adjust_ptr(ptr: *const u8, delta: isize) -> *const u8 { + if delta >= 0 { + ptr.add(delta as usize) + } else { + ptr.sub((-delta) as usize) + } +} + +#[cfg(test)] +mod tests { + use super::*; + + #[test] + fn decompresses_empty_block() { + let input = [0u8]; + let mut out = [0u8; 1]; + let decoded = decompress_safe(&input, &mut out).expect("empty block should decode"); + assert_eq!(decoded, 0); + } + + #[test] + fn decompresses_literal_only_block() { + let input = [0x50, b'h', b'e', b'l', b'l', b'o']; + let mut out = [0u8; 5]; + let decoded = decompress_safe(&input, &mut out).expect("literal-only block should decode"); + assert_eq!(decoded, 5); + assert_eq!(&out, b"hello"); + } + + #[test] + fn decompresses_overlap_match_block() { + let input = [0x14, b'a', 0x01, 0x00, 0x50, b'a', b'a', b'a', b'a', b'a']; + let mut out = [0u8; 14]; + let decoded = decompress_safe(&input, &mut out).expect("overlap match should decode"); + assert_eq!(decoded, 14); + assert_eq!(&out, b"aaaaaaaaaaaaaa"); + } + + #[test] + fn decompresses_offset_three_overlap_block() { + let input = [ + 0x37, b'a', b'b', b'c', 0x03, 0x00, 0x50, b'c', b'a', b'b', b'c', b'a', + ]; + let mut out = [0u8; 19]; + let decoded = + decompress_safe(&input, &mut out).expect("offset-three overlap should decode"); + assert_eq!(decoded, 19); + assert_eq!(&out, b"abcabcabcabcabcabca"); + } + + #[test] + fn decompresses_short_match_with_large_offset() { + let input = [ + 0x80, b'a', b'b', b'c', b'd', b'e', b'f', b'g', b'h', 0x08, 0x00, 0x50, b'e', b'f', + b'g', b'h', b'i', + ]; + let mut out = [0u8; 17]; + let decoded = + decompress_safe(&input, &mut out).expect("short large-offset match should decode"); + assert_eq!(decoded, 17); + assert_eq!(&out, b"abcdefghabcdefghi"); + } + + #[test] + fn decompresses_long_literal_length() { + let payload = *b"abcdefghijklmnopqrst"; + let mut input = vec![0xF0, 5]; + input.extend_from_slice(&payload); + let mut out = [0u8; 20]; + let decoded = decompress_safe(&input, &mut out).expect("long literal block should decode"); + assert_eq!(decoded, payload.len()); + assert_eq!(&out, &payload); + } + + #[test] + fn roundtrips_varied_inputs() { + let cases: Vec> = vec![ + vec![], + b"a".to_vec(), + b"hello".to_vec(), + b"aaaaaaaaaaaaaa".to_vec(), + b"abcabcabcabcabcabcabcabc".to_vec(), + (0..256).map(|n| n as u8).collect(), + (0..4096).map(|n| ((n * 17) & 255) as u8).collect(), + { + let mut data = Vec::new(); + for _ in 0..128 { + data.extend_from_slice(b"the quick brown fox jumps over the lazy dog "); + } + data + }, + ]; + + for case in cases { + let compressed = compress_default(&case).expect("compression should succeed"); + let mut roundtrip = vec![0u8; case.len()]; + let decoded = decompress_safe(&compressed, &mut roundtrip) + .expect("compressed block should decode"); + assert_eq!(decoded, case.len()); + assert_eq!(roundtrip, case); + } + } + + #[test] + fn rejects_output_that_is_too_small() { + let compressed = compress_default(b"hello hello hello").expect("compression should work"); + let mut out = [0u8; 4]; + let err = decompress_safe(&compressed, &mut out).expect_err("short output should fail"); + assert_eq!(err, DecompressError::OutputTooSmall); + } + + #[test] + fn compress_bound_matches_formula() { + assert_eq!(compress_bound(0), 16); + assert_eq!(compress_bound(1), 17); + assert_eq!(compress_bound(1024), 1024 + 1024 / 255 + 16); + } +} diff --git a/libs/math/src/math_f32.rs b/libs/math/src/math_f32.rs index f976ff8d4d..07f017bbdd 100644 --- a/libs/math/src/math_f32.rs +++ b/libs/math/src/math_f32.rs @@ -162,6 +162,10 @@ impl Pose { position: b.position, } } + + pub fn is_finite(&self) -> bool { + self.orientation.is_finite() && self.position.is_finite() + } } #[repr(C)] @@ -411,6 +415,10 @@ impl Vec3f { z: a.z.max(b.z), } } + + pub fn is_finite(&self) -> bool { + self.x.is_finite() && self.y.is_finite() && self.z.is_finite() + } } /* @@ -756,6 +764,10 @@ impl Quat { self.dot(self).sqrt() } + pub fn is_finite(&self) -> bool { + self.x.is_finite() && self.y.is_finite() && self.z.is_finite() && self.w.is_finite() + } + pub fn normalized(&mut self) -> Quat { let len = self.length(); Quat { diff --git a/libs/pdf_parse/src/document.rs b/libs/pdf_parse/src/document.rs index cd513e5b7d..d2df4f79e4 100644 --- a/libs/pdf_parse/src/document.rs +++ b/libs/pdf_parse/src/document.rs @@ -10,6 +10,7 @@ pub struct PdfDocument<'a> { data: &'a [u8], xref: XRefTable, cache: HashMap, + object_stream_cache: HashMap>, pages: Vec, } @@ -31,6 +32,7 @@ impl<'a> PdfDocument<'a> { data, xref, cache: HashMap::new(), + object_stream_cache: HashMap::new(), pages: Vec::new(), }; @@ -83,7 +85,13 @@ impl<'a> PdfDocument<'a> { .ok_or_else(|| PdfError::new(format!("object {} not in xref", num)))? .clone(); - let obj = parse_indirect_object_at(self.data, entry.offset)?.1; + let obj = match entry.location { + XRefLocation::Uncompressed { offset } => parse_indirect_object_at(self.data, offset)?.1, + XRefLocation::Compressed { + obj_stream_obj_num, + index, + } => self.resolve_compressed_object(num, obj_stream_obj_num, index)?, + }; self.cache.insert(num, obj.clone()); Ok(obj) } @@ -173,4 +181,166 @@ impl<'a> PdfDocument<'a> { _ => Err(PdfError::new("expected dict")), } } + + fn resolve_compressed_object( + &mut self, + obj_num: u32, + obj_stream_obj_num: u32, + index: usize, + ) -> PdfResult { + self.load_object_stream(obj_stream_obj_num)?; + + let stream_objects = self + .object_stream_cache + .get(&obj_stream_obj_num) + .ok_or_else(|| { + PdfError::new(format!( + "object stream {} was not cached after decoding", + obj_stream_obj_num + )) + })?; + + stream_objects.get(&obj_num).cloned().ok_or_else(|| { + PdfError::new(format!( + "compressed object {} missing from object stream {} at index {}", + obj_num, obj_stream_obj_num, index + )) + }) + } + + fn load_object_stream(&mut self, obj_stream_obj_num: u32) -> PdfResult<()> { + if self.object_stream_cache.contains_key(&obj_stream_obj_num) { + return Ok(()); + } + + let stream_obj = self.resolve_obj_num(obj_stream_obj_num)?; + let stream = stream_obj + .as_stream() + .ok_or_else(|| PdfError::new("object stream entry did not resolve to a stream"))?; + + if stream.dict.get_name("Type") != Some("ObjStm") { + return Err(PdfError::new(format!( + "xref compressed object points at non-object stream {}", + obj_stream_obj_num + ))); + } + + let count = stream + .dict + .get_int("N") + .ok_or_else(|| PdfError::new("object stream missing /N"))? as usize; + let first = stream + .dict + .get_int("First") + .ok_or_else(|| PdfError::new("object stream missing /First"))? + as usize; + + let decoded = self.decode_stream(stream)?; + let mut header_lex = Lexer::new(&decoded, 0); + let mut object_entries = Vec::with_capacity(count); + + for _ in 0..count { + let embedded_num = match header_lex.read_object()? { + PdfObj::Int(n) if n >= 0 => n as u32, + _ => { + return Err(PdfError::new( + "object stream header contained invalid embedded object number", + )) + } + }; + let relative_offset = match header_lex.read_object()? { + PdfObj::Int(n) if n >= 0 => n as usize, + _ => { + return Err(PdfError::new( + "object stream header contained invalid embedded object offset", + )) + } + }; + object_entries.push((embedded_num, relative_offset)); + } + + let mut objects = HashMap::with_capacity(count); + for (embedded_num, relative_offset) in object_entries { + let start = first + relative_offset; + if start >= decoded.len() { + return Err(PdfError::new(format!( + "object stream {} entry {} points past decoded data", + obj_stream_obj_num, embedded_num + ))); + } + let mut obj_lex = Lexer::new(&decoded, start); + let obj = obj_lex.read_object()?; + objects.insert(embedded_num, obj); + } + + self.object_stream_cache.insert(obj_stream_obj_num, objects); + Ok(()) + } +} + +#[cfg(test)] +mod tests { + use super::*; + use crate::parser::{XRefEntry, XRefLocation}; + + #[test] + fn resolves_compressed_objects_from_object_streams() { + let decoded = b"1 0 2 8 (Hello) 42"; + let object_stream = format!( + "10 0 obj << /Type /ObjStm /N 2 /First 8 /Length {} >> stream\n{}\ +\nendstream\nendobj\n", + decoded.len(), + std::str::from_utf8(decoded).unwrap() + ); + let data = object_stream.into_bytes(); + + let mut entries = HashMap::new(); + entries.insert( + 10, + XRefEntry { + location: XRefLocation::Uncompressed { offset: 0 }, + gen: 0, + in_use: true, + }, + ); + entries.insert( + 1, + XRefEntry { + location: XRefLocation::Compressed { + obj_stream_obj_num: 10, + index: 0, + }, + gen: 0, + in_use: true, + }, + ); + entries.insert( + 2, + XRefEntry { + location: XRefLocation::Compressed { + obj_stream_obj_num: 10, + index: 1, + }, + gen: 0, + in_use: true, + }, + ); + + let mut doc = PdfDocument { + data: &data, + xref: XRefTable { + entries, + trailer: PdfDict::new(), + }, + cache: HashMap::new(), + object_stream_cache: HashMap::new(), + pages: Vec::new(), + }; + + assert_eq!( + doc.resolve_obj_num(1).unwrap(), + PdfObj::Str(b"Hello".to_vec()) + ); + assert_eq!(doc.resolve_obj_num(2).unwrap(), PdfObj::Int(42)); + } } diff --git a/libs/pdf_parse/src/page.rs b/libs/pdf_parse/src/page.rs index 1719f35191..b76dd8b61a 100644 --- a/libs/pdf_parse/src/page.rs +++ b/libs/pdf_parse/src/page.rs @@ -67,20 +67,20 @@ impl PdfPage { .as_dict() .ok_or_else(|| PdfError::new("page object is not a dict"))?; + let inherited = collect_inherited_page_properties(doc, dict)?; + // MediaBox (required, may be inherited) - let media_box = parse_rect(dict.get("MediaBox"))?; - let crop_box = dict - .get("CropBox") - .and_then(|o| parse_rect(Some(o)).ok()) - .unwrap_or(media_box); - let rotate = dict.get_int("Rotate").unwrap_or(0) as i32; + let media_box = inherited + .media_box + .ok_or_else(|| PdfError::new("page missing MediaBox"))?; + let crop_box = inherited.crop_box.unwrap_or(media_box); + let rotate = inherited.rotate.unwrap_or(0); // Content streams let content_data = extract_content_data(doc, dict)?; // Resources (may be inherited from parent Pages node) - let resources_obj = dict.get("Resources"); - let resources = if let Some(r) = resources_obj { + let resources = if let Some(r) = inherited.resources.as_ref() { doc.resolve(r)? } else { PdfObj::Dict(PdfDict::new()) @@ -118,8 +118,79 @@ impl PdfPage { } } -fn parse_rect(obj: Option<&PdfObj>) -> PdfResult<[f64; 4]> { - let obj = obj.ok_or_else(|| PdfError::new("missing rectangle"))?; +#[derive(Default)] +struct InheritedPageProperties { + media_box: Option<[f64; 4]>, + crop_box: Option<[f64; 4]>, + rotate: Option, + resources: Option, +} + +fn collect_inherited_page_properties( + doc: &mut PdfDocument, + dict: &PdfDict, +) -> PdfResult { + let mut props = InheritedPageProperties { + media_box: resolve_rect_opt(doc, dict.get("MediaBox"))?, + crop_box: resolve_rect_opt(doc, dict.get("CropBox"))?, + rotate: dict.get_int("Rotate").map(|v| v as i32), + resources: dict.get("Resources").cloned(), + }; + + let mut parent = dict.get("Parent").cloned(); + let mut depth = 0usize; + + while let Some(parent_obj) = parent { + depth += 1; + if depth > 128 { + return Err(PdfError::new("page parent chain exceeded 128 levels")); + } + + let resolved_parent = doc.resolve(&parent_obj)?; + let parent_dict = resolved_parent + .as_dict() + .ok_or_else(|| PdfError::new("page parent is not a dictionary"))?; + + if props.media_box.is_none() { + props.media_box = resolve_rect_opt(doc, parent_dict.get("MediaBox"))?; + } + if props.crop_box.is_none() { + props.crop_box = resolve_rect_opt(doc, parent_dict.get("CropBox"))?; + } + if props.rotate.is_none() { + props.rotate = parent_dict.get_int("Rotate").map(|v| v as i32); + } + if props.resources.is_none() { + props.resources = parent_dict.get("Resources").cloned(); + } + + if props.media_box.is_some() + && props.crop_box.is_some() + && props.rotate.is_some() + && props.resources.is_some() + { + break; + } + + parent = parent_dict.get("Parent").cloned(); + } + + Ok(props) +} + +fn resolve_rect_opt(doc: &mut PdfDocument, obj: Option<&PdfObj>) -> PdfResult> { + match obj { + Some(obj) => Ok(Some(resolve_rect(doc, obj)?)), + None => Ok(None), + } +} + +fn resolve_rect(doc: &mut PdfDocument, obj: &PdfObj) -> PdfResult<[f64; 4]> { + let resolved = doc.resolve(obj)?; + parse_rect(&resolved) +} + +fn parse_rect(obj: &PdfObj) -> PdfResult<[f64; 4]> { let nums = obj .as_number_array() .ok_or_else(|| PdfError::new("rectangle must be a number array"))?; diff --git a/libs/pdf_parse/src/parser.rs b/libs/pdf_parse/src/parser.rs index 74a43dc766..660fbcac0d 100644 --- a/libs/pdf_parse/src/parser.rs +++ b/libs/pdf_parse/src/parser.rs @@ -6,11 +6,22 @@ use std::collections::HashMap; /// Parsed cross-reference entry. #[derive(Clone, Debug)] pub struct XRefEntry { - pub offset: usize, + pub location: XRefLocation, pub gen: u16, pub in_use: bool, } +#[derive(Clone, Debug)] +pub enum XRefLocation { + Uncompressed { + offset: usize, + }, + Compressed { + obj_stream_obj_num: u32, + index: usize, + }, +} + /// Parsed cross-reference table + trailer. #[derive(Clone, Debug)] pub struct XRefTable { @@ -96,7 +107,9 @@ fn parse_xref_table(data: &[u8], offset: usize) -> PdfResult { entries.insert( start_obj + i, XRefEntry { - offset: entry_offset, + location: XRefLocation::Uncompressed { + offset: entry_offset, + }, gen: entry_gen, in_use: true, }, @@ -206,20 +219,24 @@ fn parse_xref_stream(data: &[u8], offset: usize) -> PdfResult { entries.insert( obj_num, XRefEntry { - offset: field1 as usize, + location: XRefLocation::Uncompressed { + offset: field1 as usize, + }, gen: field2 as u16, in_use: true, }, ); } 2 => { - // Compressed object in object stream: field1=stream_obj_num, field2=index - // Store with a special marker - we'll handle these in document.rs + // Compressed object in object stream: field1=stream_obj_num, field2=index. entries.insert( obj_num, XRefEntry { - offset: field1 as usize, // obj stream number - gen: field2 as u16, // index within obj stream + location: XRefLocation::Compressed { + obj_stream_obj_num: field1 as u32, + index: field2 as usize, + }, + gen: 0, in_use: true, }, ); @@ -271,7 +288,7 @@ fn brute_force_xref(data: &[u8]) -> PdfResult { entries.insert( obj_num, XRefEntry { - offset: i, + location: XRefLocation::Uncompressed { offset: i }, gen, in_use: true, }, diff --git a/libs/pdf_parse/src/tests.rs b/libs/pdf_parse/src/tests.rs index a775fb5b1e..9315f784fd 100644 --- a/libs/pdf_parse/src/tests.rs +++ b/libs/pdf_parse/src/tests.rs @@ -445,6 +445,80 @@ fn test_char_width_base14() { assert!((w1 - w2).abs() < 0.01, "Courier should be monospaced"); } +#[test] +fn test_page_inherits_media_box_and_resources() { + fn push_obj(pdf: &mut Vec, offsets: &mut Vec, num: usize, body: &str) { + offsets.push(pdf.len()); + pdf.extend_from_slice(format!("{} 0 obj\n", num).as_bytes()); + pdf.extend_from_slice(body.as_bytes()); + pdf.extend_from_slice(b"\nendobj\n"); + } + + let mut pdf = b"%PDF-1.4\n".to_vec(); + let mut offsets = vec![0usize]; + + push_obj( + &mut pdf, + &mut offsets, + 1, + "<< /Type /Catalog /Pages 2 0 R >>", + ); + push_obj( + &mut pdf, + &mut offsets, + 2, + "<< /Type /Pages /Count 1 /Kids [3 0 R] /MediaBox [0 0 300 200] /Resources << /Font << /F1 4 0 R >> >> >>", + ); + push_obj( + &mut pdf, + &mut offsets, + 3, + "<< /Type /Page /Parent 2 0 R /Contents 5 0 R >>", + ); + push_obj( + &mut pdf, + &mut offsets, + 4, + "<< /Type /Font /Subtype /Type1 /BaseFont /Helvetica >>", + ); + + let content = "BT /F1 12 Tf 10 10 Td (Hello) Tj ET"; + push_obj( + &mut pdf, + &mut offsets, + 5, + &format!( + "<< /Length {} >>\nstream\n{}\nendstream", + content.len(), + content + ), + ); + + let xref_offset = pdf.len(); + pdf.extend_from_slice(format!("xref\n0 {}\n", offsets.len()).as_bytes()); + pdf.extend_from_slice(b"0000000000 65535 f \n"); + for offset in offsets.iter().skip(1) { + pdf.extend_from_slice(format!("{:010} 00000 n \n", offset).as_bytes()); + } + pdf.extend_from_slice( + format!( + "trailer\n<< /Size {} /Root 1 0 R >>\nstartxref\n{}\n%%EOF\n", + offsets.len(), + xref_offset + ) + .as_bytes(), + ); + + let mut doc = PdfDocument::parse(&pdf).unwrap(); + assert_eq!(doc.page_count(), 1); + + let page = doc.page(0).unwrap(); + assert_eq!(page.media_box, [0.0, 0.0, 300.0, 200.0]); + assert_eq!(page.crop_box, [0.0, 0.0, 300.0, 200.0]); + assert!(page.fonts.contains_key("F1")); + assert!(!page.content_data.is_empty()); +} + // ============================================================ // Stress test // ============================================================ diff --git a/libs/rapier/crates/rapier3d/Cargo.toml b/libs/rapier/crates/rapier3d/Cargo.toml index 86441c0b89..804947d5ae 100644 --- a/libs/rapier/crates/rapier3d/Cargo.toml +++ b/libs/rapier/crates/rapier3d/Cargo.toml @@ -54,7 +54,7 @@ required-features = ["dim3", "f32"] vec_map = { version = "0.8", path = "../../vendor/vec_map" } num-traits = { version = "0.2", path = "../../vendor/num-traits" } nalgebra = { version = "0.34", default-features = false, features = ["std"], path = "../../vendor/nalgebra" } -parry3d = { version = "0.26.0", default-features = false, features = ["alloc", "dim3", "f32", "required-features", "std", "enhanced-determinism"], path = "../../vendor/parry3d" } +parry3d = { version = "0.26.0", default-features = false, features = ["alloc", "dim3", "f32", "required-features", "std"], path = "../../vendor/parry3d" } simba = { version = "0.9.1", path = "../../vendor/simba" } bitflags = { version = "2", path = "../../../../libs/bitflags" } static_assertions = { version = "1", path = "../../vendor/static_assertions" } diff --git a/libs/rapier/vendor/parry3d/Cargo.toml b/libs/rapier/vendor/parry3d/Cargo.toml index 49038f7dbc..08a144793e 100644 --- a/libs/rapier/vendor/parry3d/Cargo.toml +++ b/libs/rapier/vendor/parry3d/Cargo.toml @@ -124,6 +124,14 @@ path = "../glamx" version = "2" optional = true path = "../../../../libs/vulkan/indexmap" +[dependencies.foldhash] +version = "0.2.0" +default-features = false +path = "../../../../libs/vulkan/foldhash" +[dependencies.hashbrown] +version = "0.16.1" +default-features = false +path = "../../../../libs/vulkan/hashbrown" [dependencies.log] package = "makepad-error-log" path = "../../../../libs/error_log" diff --git a/libs/rapier/vendor/parry3d/src/lib.rs b/libs/rapier/vendor/parry3d/src/lib.rs index a7fc7492e6..dc219b8d17 100644 --- a/libs/rapier/vendor/parry3d/src/lib.rs +++ b/libs/rapier/vendor/parry3d/src/lib.rs @@ -63,6 +63,10 @@ extern crate std; #[cfg(feature = "alloc")] #[cfg_attr(test, macro_use)] extern crate alloc; +#[cfg(not(feature = "enhanced-determinism"))] +extern crate foldhash; +#[cfg(not(feature = "enhanced-determinism"))] +extern crate hashbrown; #[cfg(feature = "serde")] #[macro_use] diff --git a/libs/terminal_core/src/terminal.rs b/libs/terminal_core/src/terminal.rs index db7046375e..b2067a3e56 100644 --- a/libs/terminal_core/src/terminal.rs +++ b/libs/terminal_core/src/terminal.rs @@ -53,6 +53,7 @@ pub struct Terminal { pub cursor_color: Option, pub title: String, + bell_pending: bool, parser: Parser, actions_buf: Vec, outbound: Vec, @@ -76,6 +77,7 @@ impl Terminal { default_bg: Rgb::new(0x1d, 0x1f, 0x21), cursor_color: None, title: String::new(), + bell_pending: false, parser: Parser::new(), actions_buf: Vec::with_capacity(64), outbound: Vec::with_capacity(64), @@ -126,6 +128,10 @@ impl Terminal { std::mem::take(&mut self.outbound) } + pub fn take_bell(&mut self) -> bool { + std::mem::take(&mut self.bell_pending) + } + fn push_outbound(&mut self, bytes: &[u8]) { self.outbound.extend_from_slice(bytes); } @@ -161,6 +167,7 @@ impl Terminal { match byte { 0x07 => { // BEL — bell/notification + self.bell_pending = true; } 0x08 => { // BS — backspace @@ -1138,6 +1145,22 @@ mod tests { assert_eq!(terminal.default_fg.b, 0xef); } + #[test] + fn bell_sets_pending_flag() { + let mut terminal = Terminal::new(80, 24); + terminal.process_bytes(b"\x07"); + assert!(terminal.take_bell()); + assert!(!terminal.take_bell()); + } + + #[test] + fn osc_bel_terminator_does_not_ring_bell() { + let mut terminal = Terminal::new(80, 24); + terminal.process_bytes(b"\x1b]2;build log\x07"); + assert_eq!(terminal.title, "build log"); + assert!(!terminal.take_bell()); + } + #[test] fn dcs_with_c1_st_terminator_does_not_block_following_output() { let mut terminal = Terminal::new(80, 24); @@ -1631,8 +1654,14 @@ mod tests { let before_row_0 = grid_row_text(&t, 0); let before_row_1 = grid_row_text(&t, 1); - assert!(!before_row_0.is_empty(), "expected first prompt row to have text"); - assert!(!before_row_1.is_empty(), "expected wrapped continuation row to have text"); + assert!( + !before_row_0.is_empty(), + "expected first prompt row to have text" + ); + assert!( + !before_row_1.is_empty(), + "expected wrapped continuation row to have text" + ); assert!( t.screen().grid.line_wrapped[0], "expected prompt to wrap before resize" diff --git a/libs/tsdf/Cargo.toml b/libs/tsdf/Cargo.toml new file mode 100644 index 0000000000..5e5050c5af --- /dev/null +++ b/libs/tsdf/Cargo.toml @@ -0,0 +1,10 @@ +[package] +name = "makepad-tsdf" +version = "0.1.0" +edition = "2021" +license = "MIT OR Apache-2.0" +description = "Shared TSDF storage and integration utilities" + +[dependencies] +makepad-math = { path = "../math", version = "1.0.0" } +makepad-micro-serde = { path = "../micro_serde", version = "1.0.0" } diff --git a/libs/tsdf/src/depth_integration.rs b/libs/tsdf/src/depth_integration.rs new file mode 100644 index 0000000000..200e77f5a9 --- /dev/null +++ b/libs/tsdf/src/depth_integration.rs @@ -0,0 +1,2538 @@ +use crate::{ + empty_bounds, ChunkKey, SparseTsdGridReadSnapshot, SparseTsdReadChunk, TsdfPublishedSnapshot, + XrDepthAlignHeightMap, +}; +use makepad_math::{vec2f, vec3f, vec4f, Mat4f, Vec3f, Vec4f}; +use std::{ + collections::{HashMap, HashSet, VecDeque}, + sync::Arc, + time::Instant, +}; + +pub const DEPTH_VOXEL_EYE_INDEX: usize = 0; +const DEPTH_TSD_CHUNK_EDGE_VOXELS: i32 = 4; +const DEPTH_TSD_CHUNK_VOLUME: usize = (DEPTH_TSD_CHUNK_EDGE_VOXELS as usize) + * (DEPTH_TSD_CHUNK_EDGE_VOXELS as usize) + * (DEPTH_TSD_CHUNK_EDGE_VOXELS as usize); +const DEPTH_TSD_CHUNK_VALID_WORDS: usize = + (DEPTH_TSD_CHUNK_VOLUME + u64::BITS as usize - 1) / u64::BITS as usize; +const DEPTH_IMAGE_EDGE_MARGIN_PIXELS: usize = 32; +pub const DEPTH_VOXEL_MIN_DISTANCE_METERS: f32 = 0.08; +pub const DEPTH_VOXEL_MAX_DISTANCE_METERS: f32 = 6.0; +const DEPTH_TSD_MIN_UPDATE_DISTANCE_METERS: f32 = 0.5; +pub const DEPTH_TSD_TARGET_INTEGRATION_INTERVAL_MILLIS: u64 = 1000; +const DEPTH_TSD_SUBMIT_SAMPLE_GRID_X: usize = 6; +const DEPTH_TSD_SUBMIT_SAMPLE_GRID_Y: usize = 4; +const DEPTH_TSD_SUBMIT_SAMPLE_NDC_MARGIN: f32 = 0.82; +const DEPTH_TSD_SUBMIT_FRONTIER_HORIZON_METERS: f32 = 3.0; +const DEPTH_TSD_SUBMIT_FRONTIER_SURFACE_ABS_DISTANCE: f32 = 0.4; +const DEPTH_TSD_SUBMIT_MEDIUM_NOVELTY_SCORE: f32 = 3.0; +const DEPTH_TSD_SUBMIT_STRONG_NOVELTY_SCORE: f32 = 7.0; +const DEPTH_TSD_SUBMIT_MEDIUM_INTERVAL_MILLIS: u64 = 500; +const DEPTH_TSD_SUBMIT_STRONG_INTERVAL_MILLIS: u64 = 250; +const DEPTH_TSD_SUBMIT_VERTICAL_BOUNDS_PADDING_METERS: f32 = 1.25; +const DEPTH_TSD_NOVELTY_SAMPLE_GRID_X: usize = 16; +const DEPTH_TSD_NOVELTY_SAMPLE_GRID_Y: usize = 12; +const DEPTH_TSD_NOVELTY_BOUNDS_PADDING_VOXELS: i32 = 2; +const DEPTH_TSD_NOVELTY_UNKNOWN_WEIGHT: f32 = 1.0; +const DEPTH_TSD_NOVELTY_LOW_CONFIDENCE_WEIGHT: f32 = 0.35; +const DEPTH_TSD_NOVELTY_SURFACE_MISMATCH_WEIGHT: f32 = 0.2; +const DEPTH_TSD_NOVELTY_SURFACE_MISMATCH_ABS_DISTANCE: f32 = 0.35; +const DEPTH_TSD_NOVELTY_OUTSIDE_BOUNDS_WEIGHT: f32 = 2.0; +pub const DEPTH_VOXEL_MIN_DEPTH_VALUE: f32 = 1.0 / 65535.0; +pub const DEPTH_VOXEL_MAX_DEPTH_VALUE: f32 = 0.9995; +const DEPTH_TSD_MIN_NORMAL_DOT: f32 = 0.3; +const DEPTH_TSD_APPLY_DELTA_EPSILON: f32 = 0.01; +const DEPTH_TSD_MAX_CONFIDENCE: u8 = 32; +const DEPTH_TSD_STABLE_CONFIDENCE: u8 = 8; +const DEPTH_PLAYER_CLEAR_MAX_CONFIDENCE: u8 = 2; +const DEPTH_PLAYER_EXCLUDE_RADIUS_METERS: f32 = 0.32; +// Clear the body band below the headset while leaving floor and ceiling connected. +const DEPTH_PLAYER_EXCLUDE_MIN_RELATIVE_Y_METERS: f32 = -0.95; +const DEPTH_PLAYER_EXCLUDE_MAX_RELATIVE_Y_METERS: f32 = 0.0; +const DEPTH_MESH_UPDATE_DISTANCE_METERS: f32 = 4.0; +pub const DEPTH_PUBLISHED_HEIGHT_MAP_INTERVAL_MILLIS: u64 = 1000; +pub const DEPTH_PROJECTED_HEIGHT_REFRESH_INTERVAL_MILLIS: u64 = 33; +const DEPTH_ALIGN_HEIGHT_MAP_BOUNDS_PADDING_METERS: f32 = 0.45; +const DEPTH_ALIGN_HEIGHT_MAP_FLOOR_BIN_METERS: f32 = 0.04; +const DEPTH_ALIGN_HEIGHT_MAP_FLOOR_MIN_SUPPORT_RATIO: f32 = 0.005; +const DEPTH_ALIGN_HEIGHT_MAP_FLOOR_MIN_SUPPORT_CELLS: usize = 24; +const DEPTH_ALIGN_HEIGHT_MAP_FLOOR_SUPPORT_WINDOW_BINS: usize = 2; +const DEPTH_ALIGN_VECTOR_SLICE_TOP_Y_METERS: f32 = 2.00; +const DEPTH_ALIGN_VECTOR_SLICE_ISO_HEIGHT_METERS: f32 = 0.50; +const DEPTH_ALIGN_VECTOR_SLICE_PLAYER_CUTOUT_RADIUS_METERS: f32 = + DEPTH_PLAYER_EXCLUDE_RADIUS_METERS + 0.12; +const DEPTH_ALIGN_PROJECTED_HEIGHT_SAMPLES_PER_TICK: usize = 512; +const DEPTH_ALIGN_PROJECTED_HEIGHT_MAX_SAMPLES_PER_SLICE: usize = + DEPTH_ALIGN_PROJECTED_HEIGHT_SAMPLES_PER_TICK * 8; +pub const DEPTH_ALIGN_PROJECTED_HEIGHT_MAX_SLICE_CREDITS: usize = 4; +const DEPTH_ALIGN_PROJECTED_HEIGHT_DIRTY_SWIZZLE: usize = 8; + +const fn depth_tsd_distance_meters(voxel_size_meters: f32) -> f32 { + voxel_size_meters * 2.0 +} + +const fn depth_tsd_refresh_clearance_meters(voxel_size_meters: f32) -> f32 { + voxel_size_meters * 1.5 +} + +const fn depth_normal_neighbor_max_distance_delta_meters(voxel_size_meters: f32) -> f32 { + voxel_size_meters * 2.5 +} + +const fn depth_carve_neighbor_max_distance_delta_meters(voxel_size_meters: f32) -> f32 { + voxel_size_meters * 1.5 +} + +#[derive(Copy, Clone, Debug, Default, PartialEq, Eq, Hash, Ord, PartialOrd)] +pub struct VoxelCoord { + pub x: i32, + pub y: i32, + pub z: i32, +} + +impl VoxelCoord { + pub const fn new(x: i32, y: i32, z: i32) -> Self { + Self { x, y, z } + } +} + +impl core::ops::Add for VoxelCoord { + type Output = Self; + + fn add(self, rhs: Self) -> Self::Output { + Self::new(self.x + rhs.x, self.y + rhs.y, self.z + rhs.z) + } +} + +impl core::ops::Sub for VoxelCoord { + type Output = Self; + + fn sub(self, rhs: Self) -> Self::Output { + Self::new(self.x - rhs.x, self.y - rhs.y, self.z - rhs.z) + } +} + +#[derive(Clone, Copy, Debug, Default)] +pub struct DepthFrameNovelty { + pub score: f32, + pub valid_samples: usize, +} + +#[derive(Clone, Copy, Debug, Default)] +pub struct SubmitDepthFrameNovelty { + pub score: f32, + pub valid_rays: usize, + pub force_readback: bool, +} + +fn point_inside_bounds_xz(point: Vec3f, min: Vec3f, max: Vec3f) -> bool { + point.x >= min.x && point.x <= max.x && point.z >= min.z && point.z <= max.z +} + +fn ray_aabb_exit_distance(origin: Vec3f, direction: Vec3f, min: Vec3f, max: Vec3f) -> Option { + let mut t_min = f32::NEG_INFINITY; + let mut t_max = f32::INFINITY; + for (origin_axis, direction_axis, min_axis, max_axis) in [ + (origin.x, direction.x, min.x, max.x), + (origin.y, direction.y, min.y, max.y), + (origin.z, direction.z, min.z, max.z), + ] { + if direction_axis.abs() <= 1.0e-6 { + if origin_axis < min_axis || origin_axis > max_axis { + return None; + } + continue; + } + let inv_direction = 1.0 / direction_axis; + let t0 = (min_axis - origin_axis) * inv_direction; + let t1 = (max_axis - origin_axis) * inv_direction; + t_min = t_min.max(t0.min(t1)); + t_max = t_max.min(t0.max(t1)); + if t_max < t_min { + return None; + } + } + (t_max.is_finite() && t_max >= 0.0).then_some(t_max.max(0.0)) +} + +fn depth_ndc_to_view_with_inv_proj( + inv_depth_proj: Mat4f, + ndc_x: f32, + ndc_y: f32, + ndc_z: f32, +) -> Option { + let view = inv_depth_proj.transform_vec4(vec4f(ndc_x, ndc_y, ndc_z, 1.0)); + if !view.w.is_finite() || view.w.abs() < 1.0e-6 { + return None; + } + let inv_w = 1.0 / view.w; + let point = vec3f(view.x * inv_w, view.y * inv_w, view.z * inv_w); + (point.x.is_finite() && point.y.is_finite() && point.z.is_finite()).then_some(point) +} + +pub fn depth_ndc_to_world_ray( + inv_depth_proj: Mat4f, + world_from_depth_view: Mat4f, + ndc_x: f32, + ndc_y: f32, +) -> Option { + let view = depth_ndc_to_view_with_inv_proj(inv_depth_proj, ndc_x, ndc_y, 1.0)?; + let view_direction = view.normalize(); + if view_direction.length() <= 1.0e-4 { + return None; + } + let world_direction = world_from_depth_view.transform_vec4(vec4f( + view_direction.x, + view_direction.y, + view_direction.z, + 0.0, + )); + let direction = vec3f(world_direction.x, world_direction.y, world_direction.z).normalize(); + (direction.length() > 1.0e-4).then_some(direction) +} + +pub fn score_submit_depth_frame_novelty( + grid: &SparseTsdGridReadSnapshot, + camera_world: Vec3f, + inv_depth_proj: Mat4f, + world_from_depth_view: Mat4f, +) -> SubmitDepthFrameNovelty { + let Some((mut bounds_min, mut bounds_max)) = + grid.world_bounds(DEPTH_TSD_NOVELTY_BOUNDS_PADDING_VOXELS) + else { + return SubmitDepthFrameNovelty { + force_readback: true, + ..Default::default() + }; + }; + bounds_min.y -= DEPTH_TSD_SUBMIT_VERTICAL_BOUNDS_PADDING_METERS; + bounds_max.y += DEPTH_TSD_SUBMIT_VERTICAL_BOUNDS_PADDING_METERS; + if !point_inside_bounds_xz(camera_world, bounds_min, bounds_max) { + return SubmitDepthFrameNovelty { + force_readback: true, + ..Default::default() + }; + } + + let mut novelty = SubmitDepthFrameNovelty::default(); + let lookahead_distance = (grid.voxel_size * 2.0).max(0.1); + for y in 0..DEPTH_TSD_SUBMIT_SAMPLE_GRID_Y { + let y_t = if DEPTH_TSD_SUBMIT_SAMPLE_GRID_Y == 1 { + 0.5 + } else { + y as f32 / (DEPTH_TSD_SUBMIT_SAMPLE_GRID_Y - 1) as f32 + }; + let ndc_y = + -DEPTH_TSD_SUBMIT_SAMPLE_NDC_MARGIN + y_t * DEPTH_TSD_SUBMIT_SAMPLE_NDC_MARGIN * 2.0; + for x in 0..DEPTH_TSD_SUBMIT_SAMPLE_GRID_X { + let x_t = if DEPTH_TSD_SUBMIT_SAMPLE_GRID_X == 1 { + 0.5 + } else { + x as f32 / (DEPTH_TSD_SUBMIT_SAMPLE_GRID_X - 1) as f32 + }; + let ndc_x = -DEPTH_TSD_SUBMIT_SAMPLE_NDC_MARGIN + + x_t * DEPTH_TSD_SUBMIT_SAMPLE_NDC_MARGIN * 2.0; + let Some(ray_direction) = + depth_ndc_to_world_ray(inv_depth_proj, world_from_depth_view, ndc_x, ndc_y) + else { + continue; + }; + novelty.valid_rays += 1; + let Some(exit_distance) = + ray_aabb_exit_distance(camera_world, ray_direction, bounds_min, bounds_max) + else { + continue; + }; + if exit_distance > DEPTH_TSD_SUBMIT_FRONTIER_HORIZON_METERS { + continue; + } + let boundary_distance = (exit_distance - grid.voxel_size).max(0.0); + let boundary_world = camera_world + ray_direction.scale(boundary_distance); + let (boundary_x, boundary_y, boundary_z) = grid.world_to_voxel_xyz(boundary_world); + let boundary_distance_value = + grid.normalized_distance_xyz(boundary_x, boundary_y, boundary_z); + let boundary_confidence = grid.confidence_xyz(boundary_x, boundary_y, boundary_z); + let known_frontier_surface = boundary_distance_value.is_some_and(|distance| { + boundary_confidence >= DEPTH_TSD_STABLE_CONFIDENCE + && distance.abs() <= DEPTH_TSD_SUBMIT_FRONTIER_SURFACE_ABS_DISTANCE + }); + if known_frontier_surface { + continue; + } + novelty.score += + 1.0 - (exit_distance / DEPTH_TSD_SUBMIT_FRONTIER_HORIZON_METERS).clamp(0.0, 1.0); + if boundary_distance_value.is_none() { + novelty.score += 0.75; + } else { + let confidence_gap = DEPTH_TSD_STABLE_CONFIDENCE.saturating_sub(boundary_confidence) + as f32 + / DEPTH_TSD_STABLE_CONFIDENCE as f32; + novelty.score += 0.35 * confidence_gap; + } + let beyond_frontier_world = + camera_world + ray_direction.scale(exit_distance + lookahead_distance); + let (beyond_x, beyond_y, beyond_z) = grid.world_to_voxel_xyz(beyond_frontier_world); + if grid + .normalized_distance_xyz(beyond_x, beyond_y, beyond_z) + .is_none() + { + novelty.score += 0.5; + } + } + } + novelty +} + +fn submit_depth_readback_min_interval_millis(novelty: SubmitDepthFrameNovelty) -> u64 { + if novelty.force_readback { + DEPTH_TSD_SUBMIT_STRONG_INTERVAL_MILLIS + } else if novelty.score >= DEPTH_TSD_SUBMIT_STRONG_NOVELTY_SCORE { + DEPTH_TSD_SUBMIT_STRONG_INTERVAL_MILLIS + } else if novelty.score >= DEPTH_TSD_SUBMIT_MEDIUM_NOVELTY_SCORE { + DEPTH_TSD_SUBMIT_MEDIUM_INTERVAL_MILLIS + } else { + DEPTH_TSD_TARGET_INTEGRATION_INTERVAL_MILLIS + } +} + +pub fn submit_should_readback_depth_frame( + snapshot: Option<&TsdfPublishedSnapshot>, + camera_world: Vec3f, + inv_depth_proj: Mat4f, + world_from_depth_view: Mat4f, + now: Instant, + last_depth_readback_at: Option, +) -> bool { + let Some(snapshot) = snapshot else { + return last_depth_readback_at.is_none_or(|last| { + now.duration_since(last) + >= std::time::Duration::from_millis(DEPTH_TSD_SUBMIT_STRONG_INTERVAL_MILLIS) + }); + }; + if snapshot.grid.is_empty() { + return last_depth_readback_at.is_none_or(|last| { + now.duration_since(last) + >= std::time::Duration::from_millis(DEPTH_TSD_SUBMIT_STRONG_INTERVAL_MILLIS) + }); + } + let novelty = score_submit_depth_frame_novelty( + &snapshot.grid, + camera_world, + inv_depth_proj, + world_from_depth_view, + ); + let min_interval_millis = submit_depth_readback_min_interval_millis(novelty); + last_depth_readback_at.is_none_or(|last| { + now.duration_since(last) >= std::time::Duration::from_millis(min_interval_millis) + }) +} + +fn tsd_chunk_key_and_local_id(coord: VoxelCoord) -> (VoxelCoord, u16) { + let chunk_key = VoxelCoord::new( + coord.x.div_euclid(DEPTH_TSD_CHUNK_EDGE_VOXELS), + coord.y.div_euclid(DEPTH_TSD_CHUNK_EDGE_VOXELS), + coord.z.div_euclid(DEPTH_TSD_CHUNK_EDGE_VOXELS), + ); + let lx = coord.x.rem_euclid(DEPTH_TSD_CHUNK_EDGE_VOXELS) as usize; + let ly = coord.y.rem_euclid(DEPTH_TSD_CHUNK_EDGE_VOXELS) as usize; + let lz = coord.z.rem_euclid(DEPTH_TSD_CHUNK_EDGE_VOXELS) as usize; + let edge = DEPTH_TSD_CHUNK_EDGE_VOXELS as usize; + let local_id = lx + ly * edge + lz * edge * edge; + (chunk_key, local_id as u16) +} + +fn tsd_voxel_coord_from_chunk_key_and_local_id(chunk_key: VoxelCoord, local_id: u16) -> VoxelCoord { + let edge = DEPTH_TSD_CHUNK_EDGE_VOXELS as usize; + let local_id = local_id as usize; + let lx = local_id % edge; + let ly = (local_id / edge) % edge; + let lz = local_id / (edge * edge); + VoxelCoord::new( + chunk_key.x * DEPTH_TSD_CHUNK_EDGE_VOXELS + lx as i32, + chunk_key.y * DEPTH_TSD_CHUNK_EDGE_VOXELS + ly as i32, + chunk_key.z * DEPTH_TSD_CHUNK_EDGE_VOXELS + lz as i32, + ) +} + +#[inline(always)] +fn ray_voxel_axis_step( + point_axis: f32, + direction_axis: f32, + voxel_size: f32, + coord_axis: i32, + start_distance: f32, +) -> (i32, f32, f32) { + if direction_axis > 1.0e-6 { + let next_boundary = (coord_axis + 1) as f32 * voxel_size; + ( + 1, + start_distance + (next_boundary - point_axis) / direction_axis, + voxel_size / direction_axis, + ) + } else if direction_axis < -1.0e-6 { + let next_boundary = coord_axis as f32 * voxel_size; + ( + -1, + start_distance + (next_boundary - point_axis) / direction_axis, + -voxel_size / direction_axis, + ) + } else { + (0, f32::INFINITY, f32::INFINITY) + } +} + +#[derive(Clone, Debug)] +struct SparseTsdChunk { + data: Arc, + live_count: usize, +} + +#[derive(Clone, Copy, Debug, Default)] +pub struct SparseTsdWriteResult { + pub state_changed: bool, + pub value_changed: bool, + pub became_live: bool, +} + +impl SparseTsdChunk { + fn new(chunk_volume: usize) -> Self { + Self { + data: Arc::new(SparseTsdReadChunk::new(chunk_volume)), + live_count: 0, + } + } + + fn is_valid(&self, id: usize) -> bool { + SparseTsdReadChunk::is_valid_index(&self.data.valid_bits, id) + } + + fn value(&self, id: usize) -> Option { + if !self.is_valid(id) { + None + } else { + Some(SparseTsdReadChunk::decode_normalized_distance( + self.data.values[id], + )) + } + } + + fn confidence(&self, id: usize) -> u8 { + if !self.is_valid(id) { + 0 + } else { + SparseTsdReadChunk::decode_metadata_confidence(self.data.metadata[id]) + } + } + + fn accumulate(&mut self, id: usize, value: f32, generation: u64) -> SparseTsdWriteResult { + let previous_valid = self.is_valid(id); + let previous_meta = self.data.metadata[id]; + let previous = self.value(id); + let value = value.clamp(-1.0, 1.0); + let next_value = if let Some(previous) = previous { + let delta = (previous - value).abs(); + let mut confidence = + SparseTsdReadChunk::decode_metadata_confidence(previous_meta).max(1); + if delta < 0.08 { + confidence = confidence.saturating_add(2).min(DEPTH_TSD_MAX_CONFIDENCE); + } else if delta > 0.35 { + confidence = confidence.saturating_sub(2).max(1); + } + let confidence = confidence as f32; + previous + (value - previous) / (confidence + 1.0) + } else { + value + } + .clamp(-1.0, 1.0); + let changed = previous + .map(|previous| (previous - next_value).abs() > 1.0e-4) + .unwrap_or(true); + let data = Arc::make_mut(&mut self.data); + data.values[id] = SparseTsdReadChunk::encode_normalized_distance(next_value); + SparseTsdReadChunk::set_valid_index(&mut data.valid_bits, id); + let next_confidence = if let Some(previous) = previous { + let delta = (previous - value).abs(); + let mut confidence = + SparseTsdReadChunk::decode_metadata_confidence(previous_meta).max(1); + if delta < 0.08 { + confidence = confidence.saturating_add(2).min(DEPTH_TSD_MAX_CONFIDENCE); + } else if delta < 0.18 { + confidence = confidence.saturating_add(1).min(DEPTH_TSD_MAX_CONFIDENCE); + } else if delta > 0.35 { + confidence = confidence.saturating_sub(2).max(1); + } else { + confidence = confidence.saturating_sub(1).max(1); + } + confidence + } else { + 1 + }; + data.metadata[id] = SparseTsdReadChunk::encode_metadata(next_confidence, generation); + if previous.is_none() { + self.live_count += 1; + } + SparseTsdWriteResult { + state_changed: !previous_valid || changed || previous_meta != data.metadata[id], + value_changed: changed, + became_live: previous.is_none(), + } + } + + fn overwrite(&mut self, id: usize, value: f32, generation: u64) -> SparseTsdWriteResult { + let previous_valid = self.is_valid(id); + let previous_meta = self.data.metadata[id]; + let previous = self.value(id); + let value = value.clamp(-1.0, 1.0); + let changed = previous + .map(|previous| (previous - value).abs() > 1.0e-4) + .unwrap_or(true); + let data = Arc::make_mut(&mut self.data); + data.values[id] = SparseTsdReadChunk::encode_normalized_distance(value); + SparseTsdReadChunk::set_valid_index(&mut data.valid_bits, id); + data.metadata[id] = + SparseTsdReadChunk::encode_metadata(DEPTH_TSD_MAX_CONFIDENCE, generation); + if previous.is_none() { + self.live_count += 1; + } + SparseTsdWriteResult { + state_changed: !previous_valid || changed || previous_meta != data.metadata[id], + value_changed: changed, + became_live: previous.is_none(), + } + } +} + +#[derive(Clone, Debug)] +pub struct SparseTsdGrid { + voxel_size: f32, + chunk_edge: i32, + chunk_volume: usize, + chunks: HashMap, + active_value_count: usize, +} + +impl SparseTsdGrid { + pub fn new(voxel_size: f32, chunk_edge: i32) -> Self { + Self { + voxel_size, + chunk_edge, + chunk_volume: (chunk_edge as usize).pow(3), + chunks: HashMap::new(), + active_value_count: 0, + } + } + + pub fn normalized_distance(&self, coord: VoxelCoord) -> Option { + let (chunk_key, local_id) = self.chunk_key_and_id(coord); + let chunk = self.chunks.get(&chunk_key)?; + chunk.value(local_id) + } + + fn confidence(&self, coord: VoxelCoord) -> u8 { + let (chunk_key, local_id) = self.chunk_key_and_id(coord); + self.chunks + .get(&chunk_key) + .map(|chunk| chunk.confidence(local_id)) + .unwrap_or(0) + } + + pub fn overwrite_normalized_distance( + &mut self, + coord: VoxelCoord, + value: f32, + generation: u64, + ) -> SparseTsdWriteResult { + let (chunk_key, local_id) = self.chunk_key_and_id(coord); + let chunk = self + .chunks + .entry(chunk_key) + .or_insert_with(|| SparseTsdChunk::new(self.chunk_volume)); + let result = chunk.overwrite(local_id, value, generation); + if result.became_live { + self.active_value_count += 1; + } + result + } + + pub fn world_to_voxel_coord(&self, point: Vec3f) -> VoxelCoord { + VoxelCoord::new( + (point.x / self.voxel_size).floor() as i32, + (point.y / self.voxel_size).floor() as i32, + (point.z / self.voxel_size).floor() as i32, + ) + } + + pub fn voxel_center_world(&self, coord: VoxelCoord) -> Vec3f { + vec3f( + (coord.x as f32 + 0.5) * self.voxel_size, + (coord.y as f32 + 0.5) * self.voxel_size, + (coord.z as f32 + 0.5) * self.voxel_size, + ) + } + + pub fn world_bounds(&self, padding_voxels: i32) -> Option<(Vec3f, Vec3f)> { + if self.chunks.is_empty() { + return None; + } + let mut min = VoxelCoord::new(i32::MAX, i32::MAX, i32::MAX); + let mut max = VoxelCoord::new(i32::MIN, i32::MIN, i32::MIN); + for (key, chunk) in &self.chunks { + if chunk.live_count == 0 { + continue; + } + min.x = min.x.min(key.x * self.chunk_edge); + min.y = min.y.min(key.y * self.chunk_edge); + min.z = min.z.min(key.z * self.chunk_edge); + max.x = max.x.max((key.x + 1) * self.chunk_edge); + max.y = max.y.max((key.y + 1) * self.chunk_edge); + max.z = max.z.max((key.z + 1) * self.chunk_edge); + } + if min.x == i32::MAX { + return None; + } + Some(( + vec3f( + (min.x - padding_voxels) as f32 * self.voxel_size, + (min.y - padding_voxels) as f32 * self.voxel_size, + (min.z - padding_voxels) as f32 * self.voxel_size, + ), + vec3f( + (max.x + padding_voxels) as f32 * self.voxel_size, + (max.y + padding_voxels) as f32 * self.voxel_size, + (max.z + padding_voxels) as f32 * self.voxel_size, + ), + )) + } + + fn chunk_key_and_id(&self, coord: VoxelCoord) -> (VoxelCoord, usize) { + let cx = coord.x.div_euclid(self.chunk_edge); + let cy = coord.y.div_euclid(self.chunk_edge); + let cz = coord.z.div_euclid(self.chunk_edge); + let lx = coord.x.rem_euclid(self.chunk_edge) as usize; + let ly = coord.y.rem_euclid(self.chunk_edge) as usize; + let lz = coord.z.rem_euclid(self.chunk_edge) as usize; + let edge = self.chunk_edge as usize; + let id = lx + ly * edge + lz * edge * edge; + (VoxelCoord::new(cx, cy, cz), id) + } + + fn build_read_snapshot(&self) -> SparseTsdGridReadSnapshot { + let mut chunks = HashMap::with_capacity(self.chunks.len()); + for (&chunk_key, chunk) in &self.chunks { + if chunk.live_count == 0 { + continue; + } + let read_chunk_key = voxel_coord_to_chunk_key(chunk_key); + chunks.insert(read_chunk_key, chunk.data.clone()); + } + let chunk_edge_shift = self + .chunk_edge + .is_positive() + .then_some(self.chunk_edge as u32) + .filter(|edge| edge.is_power_of_two()) + .map(|edge| edge.trailing_zeros() as u8); + SparseTsdGridReadSnapshot { + voxel_size: self.voxel_size, + chunk_edge: self.chunk_edge, + chunk_edge_shift, + chunk_edge_mask: if chunk_edge_shift.is_some() { + self.chunk_edge - 1 + } else { + 0 + }, + chunk_volume: self.chunk_volume, + active_value_count: self.active_value_count, + active_bounds: self.world_bounds(0), + chunks, + } + } +} + +fn voxel_coord_to_chunk_key(coord: VoxelCoord) -> ChunkKey { + ChunkKey::new(coord.x, coord.y, coord.z) +} + +#[derive(Debug)] +pub struct DepthMeshVolume { + generation: u64, + latest_topology_generation: u64, + eye_index: usize, + image_width: u32, + image_height: u32, + voxel_size_meters: f32, + bounds_min: Vec3f, + bounds_max: Vec3f, + mesh_grid: SparseTsdGrid, + dirty_tsdf_chunk_keys: HashSet, + update_sequence: u64, + latest_camera_world: Option, + projected_height_field: Option, + projected_height_publish_pending: bool, + published_height_map: Option, + pending_projected_height_dirty_samples: HashSet, + pending_projected_height_sample_queue: VecDeque, +} + +#[derive(Clone, Copy, Debug, PartialEq)] +struct ProjectedHeightFieldLayout { + origin_x: f32, + origin_z: f32, + cell_size_meters: f32, + size_x: usize, + size_z: usize, + top_y_meters: f32, + bottom_y_meters: f32, +} + +#[derive(Clone, Debug)] +struct ProjectedHeightField { + layout: ProjectedHeightFieldLayout, + player_cutout_center: Option, + heights_meters: Vec, + valid: Vec, +} + +impl ProjectedHeightField { + fn new(layout: ProjectedHeightFieldLayout, player_cutout_center: Option) -> Self { + let sample_count = (layout.size_x + 1) * (layout.size_z + 1); + Self { + layout, + player_cutout_center, + heights_meters: vec![0.0; sample_count], + valid: vec![0; sample_count], + } + } + + fn sample_size_x(&self) -> usize { + self.layout.size_x + 1 + } + + fn sample_size_z(&self) -> usize { + self.layout.size_z + 1 + } +} + +impl DepthMeshVolume { + pub fn new(voxel_size_meters: f32) -> Self { + Self { + generation: 0, + latest_topology_generation: 0, + eye_index: 0, + image_width: 0, + image_height: 0, + voxel_size_meters, + bounds_min: vec3f(0.0, 0.0, 0.0), + bounds_max: vec3f(0.0, 0.0, 0.0), + mesh_grid: SparseTsdGrid::new(voxel_size_meters, DEPTH_TSD_CHUNK_EDGE_VOXELS), + dirty_tsdf_chunk_keys: HashSet::new(), + update_sequence: 0, + latest_camera_world: None, + projected_height_field: None, + projected_height_publish_pending: false, + published_height_map: None, + pending_projected_height_dirty_samples: HashSet::new(), + pending_projected_height_sample_queue: VecDeque::new(), + } + } + + pub fn voxel_size_meters(&self) -> f32 { + self.voxel_size_meters + } + + pub fn pending_projected_height_sample_count(&self) -> usize { + self.pending_projected_height_sample_queue.len() + } + + pub fn projected_height_publish_pending(&self) -> bool { + self.projected_height_publish_pending + } + + pub fn set_projected_height_publish_pending(&mut self, pending: bool) { + self.projected_height_publish_pending = pending; + } + + pub fn has_published_height_map(&self) -> bool { + self.published_height_map.is_some() + } + + fn update_bounds(&mut self) { + if let Some((min, max)) = self.mesh_grid.world_bounds(0) { + self.bounds_min = min; + self.bounds_max = max; + } else { + (self.bounds_min, self.bounds_max) = empty_bounds(); + } + } + + pub fn published_tsdf_snapshot( + &self, + previous: Option<&TsdfPublishedSnapshot>, + ) -> TsdfPublishedSnapshot { + let grid = if self.dirty_tsdf_chunk_keys.is_empty() { + previous + .map(|previous| previous.grid.clone()) + .unwrap_or_else(|| Arc::new(self.mesh_grid.build_read_snapshot())) + } else { + Arc::new(self.mesh_grid.build_read_snapshot()) + }; + TsdfPublishedSnapshot { + generation: self.generation, + latest_topology_generation: self.latest_topology_generation, + update_sequence: self.update_sequence, + grid, + height_map: self.published_height_map.clone(), + } + } + + pub fn clear_published_tsdf_dirty_state(&mut self) { + self.dirty_tsdf_chunk_keys.clear(); + } + + pub fn clear_published_height_map(&mut self) -> bool { + if self.published_height_map.is_none() { + return false; + } + self.published_height_map = None; + self.update_sequence = self.update_sequence.saturating_add(1); + true + } + + pub fn discard_obsolete_surface_state(&mut self) {} +} + +#[derive(Clone, Debug)] +pub struct DepthMeshJob { + pub reset_generation: u64, + pub generation: u64, + pub eye_index: usize, + pub width: u32, + pub height: u32, + pub voxel_size_meters: f32, + pub camera_world: Vec3f, + pub depth_proj: Mat4f, + pub inv_depth_proj: Mat4f, + pub depth_view_from_world: Mat4f, + pub world_from_depth_view: Mat4f, + pub depth: Vec, +} + +pub struct PreparedDepthMeshJob { + reset_generation: u64, + generation: u64, + eye_index: usize, + width: u32, + height: u32, + voxel_size_meters: f32, + camera_world: Vec3f, + depth_proj: Mat4f, + inv_depth_proj: Mat4f, + depth_view_from_world: Mat4f, + visible_world_min: Vec3f, + visible_world_max: Vec3f, + depth: Vec, +} + +impl PreparedDepthMeshJob { + pub fn reset_generation(&self) -> u64 { + self.reset_generation + } + + pub fn voxel_size_meters(&self) -> f32 { + self.voxel_size_meters + } +} + +#[derive(Clone, Copy, Debug, Default)] +struct FrameTsdSample { + chunk_key: VoxelCoord, + local_id: u16, + normalized: f32, +} + +#[derive(Clone, Copy, Debug)] +struct DepthGridSample { + world: Vec3f, + normal: Vec3f, + ray_distance: f32, + valid: bool, + has_normal: bool, +} + +#[derive(Default)] +pub struct DepthPreprocessWorkerState { + sampled_depth: Vec, + reduced_tsd_samples: Vec, + frame_tsd_chunks: HashMap, + frame_tsd_dirty_chunks: Vec, + staged_tsd_sample_visits: usize, + depth_width: usize, + depth_height: usize, +} + +struct FrameTsdAccumChunk { + sums: Box<[f32; DEPTH_TSD_CHUNK_VOLUME]>, + counts: Box<[u16; DEPTH_TSD_CHUNK_VOLUME]>, + touched_bits: [u64; DEPTH_TSD_CHUNK_VALID_WORDS], + touched_count: usize, +} + +impl FrameTsdAccumChunk { + fn new() -> Self { + Self { + sums: Box::new([0.0; DEPTH_TSD_CHUNK_VOLUME]), + counts: Box::new([0; DEPTH_TSD_CHUNK_VOLUME]), + touched_bits: [0; DEPTH_TSD_CHUNK_VALID_WORDS], + touched_count: 0, + } + } + + #[inline(always)] + fn record(&mut self, local_id: usize, normalized: f32) { + let (word_index, bit_mask) = SparseTsdReadChunk::valid_bit_parts(local_id); + if (self.touched_bits[word_index] & bit_mask) == 0 { + self.touched_bits[word_index] |= bit_mask; + self.touched_count += 1; + } + self.sums[local_id] += normalized; + self.counts[local_id] = self.counts[local_id].saturating_add(1); + } + + fn clear_touched(&mut self) { + for word_index in 0..self.touched_bits.len() { + let mut pending_bits = self.touched_bits[word_index]; + while pending_bits != 0 { + let bit_index = pending_bits.trailing_zeros() as usize; + pending_bits &= pending_bits - 1; + let local_id = word_index * u64::BITS as usize + bit_index; + if local_id < DEPTH_TSD_CHUNK_VOLUME { + self.sums[local_id] = 0.0; + self.counts[local_id] = 0; + } + } + } + self.touched_bits.fill(0); + self.touched_count = 0; + } + + fn push_reduced_samples( + &self, + chunk_key: VoxelCoord, + reduced_tsd_samples: &mut Vec, + ) { + for word_index in 0..self.touched_bits.len() { + let mut pending_bits = self.touched_bits[word_index]; + while pending_bits != 0 { + let bit_index = pending_bits.trailing_zeros() as usize; + pending_bits &= pending_bits - 1; + let local_id = word_index * u64::BITS as usize + bit_index; + if local_id >= DEPTH_TSD_CHUNK_VOLUME { + continue; + } + let count = self.counts[local_id].max(1) as f32; + reduced_tsd_samples.push(FrameTsdSample { + chunk_key, + local_id: local_id as u16, + normalized: self.sums[local_id] / count, + }); + } + } + } +} + +impl DepthPreprocessWorkerState { + pub fn staged_tsd_sample_visits(&self) -> usize { + self.staged_tsd_sample_visits + } + + pub fn reduced_tsd_sample_count(&self) -> usize { + self.reduced_tsd_samples.len() + } + + fn clear_frame_tsd_accum(&mut self) { + for chunk_key in self.frame_tsd_dirty_chunks.drain(..) { + if let Some(chunk) = self.frame_tsd_chunks.get_mut(&chunk_key) { + chunk.clear_touched(); + } + } + self.staged_tsd_sample_visits = 0; + self.reduced_tsd_samples.clear(); + } + + #[inline(always)] + fn record_tsd_sample(&mut self, coord: VoxelCoord, normalized: f32) { + let (chunk_key, local_id) = tsd_chunk_key_and_local_id(coord); + let chunk = self + .frame_tsd_chunks + .entry(chunk_key) + .or_insert_with(FrameTsdAccumChunk::new); + if chunk.touched_count == 0 { + self.frame_tsd_dirty_chunks.push(chunk_key); + } + chunk.record(local_id as usize, normalized); + self.staged_tsd_sample_visits += 1; + } + + fn build_reduced_tsd_samples(&mut self) { + let capacity = self + .frame_tsd_dirty_chunks + .iter() + .filter_map(|chunk_key| self.frame_tsd_chunks.get(chunk_key)) + .map(|chunk| chunk.touched_count) + .sum(); + self.reduced_tsd_samples.reserve(capacity); + for &chunk_key in &self.frame_tsd_dirty_chunks { + if let Some(chunk) = self.frame_tsd_chunks.get(&chunk_key) { + chunk.push_reduced_samples(chunk_key, &mut self.reduced_tsd_samples); + } + } + self.reduced_tsd_samples.sort_unstable_by(|left, right| { + left.chunk_key + .cmp(&right.chunk_key) + .then(left.local_id.cmp(&right.local_id)) + }); + } +} + +#[inline(always)] +fn append_tsd_samples_along_ray( + worker_state: &mut DepthPreprocessWorkerState, + camera_world: Vec3f, + ray_dir: Vec3f, + surface_distance: f32, + start_distance: f32, + end_distance: f32, + voxel_size_meters: f32, + tsd_distance_meters: f32, +) { + debug_assert!(end_distance >= start_distance); + let start_distance = start_distance.max(0.0); + let end_distance = end_distance.max(start_distance); + if !start_distance.is_finite() || !end_distance.is_finite() { + return; + } + + let segment_bias = (voxel_size_meters * 1.0e-4).min(1.0e-4); + let traversal_start = (start_distance + segment_bias).min(end_distance); + let start_point = camera_world + ray_dir.scale(traversal_start); + let mut coord = VoxelCoord::new( + (start_point.x / voxel_size_meters).floor() as i32, + (start_point.y / voxel_size_meters).floor() as i32, + (start_point.z / voxel_size_meters).floor() as i32, + ); + let (step_x, mut t_max_x, t_delta_x) = ray_voxel_axis_step( + start_point.x, + ray_dir.x, + voxel_size_meters, + coord.x, + traversal_start, + ); + let (step_y, mut t_max_y, t_delta_y) = ray_voxel_axis_step( + start_point.y, + ray_dir.y, + voxel_size_meters, + coord.y, + traversal_start, + ); + let (step_z, mut t_max_z, t_delta_z) = ray_voxel_axis_step( + start_point.z, + ray_dir.z, + voxel_size_meters, + coord.z, + traversal_start, + ); + const CROSSING_EPSILON: f32 = 1.0e-5; + + loop { + let voxel_world = vec3f( + voxel_center_axis(voxel_size_meters, coord.x), + voxel_center_axis(voxel_size_meters, coord.y), + voxel_center_axis(voxel_size_meters, coord.z), + ); + if !point_inside_player_exclusion(camera_world, voxel_world) { + let voxel_distance = (voxel_world - camera_world).dot(ray_dir); + if voxel_distance.is_finite() { + let normalized = + ((surface_distance - voxel_distance) / tsd_distance_meters).clamp(-1.0, 1.0); + worker_state.record_tsd_sample(coord, normalized); + } + } + + let next_crossing = t_max_x.min(t_max_y.min(t_max_z)); + if next_crossing > end_distance { + break; + } + if t_max_x <= next_crossing + CROSSING_EPSILON { + coord.x += step_x; + t_max_x += t_delta_x; + } + if t_max_y <= next_crossing + CROSSING_EPSILON { + coord.y += step_y; + t_max_y += t_delta_y; + } + if t_max_z <= next_crossing + CROSSING_EPSILON { + coord.z += step_z; + t_max_z += t_delta_z; + } + } +} + +pub fn score_depth_job_novelty(volume: &DepthMeshVolume, job: &DepthMeshJob) -> DepthFrameNovelty { + let width = job.width as usize; + let height = job.height as usize; + if width == 0 || height == 0 { + return DepthFrameNovelty::default(); + } + let inner_width = width + .saturating_sub(DEPTH_IMAGE_EDGE_MARGIN_PIXELS * 2) + .max(1); + let inner_height = height + .saturating_sub(DEPTH_IMAGE_EDGE_MARGIN_PIXELS * 2) + .max(1); + let x_step = inner_width.div_ceil(DEPTH_TSD_NOVELTY_SAMPLE_GRID_X).max(1); + let y_step = inner_height + .div_ceil(DEPTH_TSD_NOVELTY_SAMPLE_GRID_Y) + .max(1); + let padded_bounds = volume + .mesh_grid + .world_bounds(DEPTH_TSD_NOVELTY_BOUNDS_PADDING_VOXELS); + let mut novelty = DepthFrameNovelty::default(); + + for y in (0..height).step_by(y_step) { + for x in (0..width).step_by(x_step) { + if !depth_pixel_inside_margin(width, height, x, y) { + continue; + } + let Some(world) = depth_pixel_to_world(job, x as u32, y as u32) else { + continue; + }; + if point_inside_player_exclusion(job.camera_world, world) { + continue; + } + novelty.valid_samples += 1; + + if padded_bounds.is_some_and(|(min, max)| { + world.x < min.x + || world.x > max.x + || world.y < min.y + || world.y > max.y + || world.z < min.z + || world.z > max.z + }) { + novelty.score += DEPTH_TSD_NOVELTY_OUTSIDE_BOUNDS_WEIGHT; + continue; + } + + let coord = volume.mesh_grid.world_to_voxel_coord(world); + if let Some(distance) = volume.mesh_grid.normalized_distance(coord) { + let confidence = volume.mesh_grid.confidence(coord); + let confidence_gap = DEPTH_TSD_STABLE_CONFIDENCE.saturating_sub(confidence) as f32 + / DEPTH_TSD_STABLE_CONFIDENCE as f32; + novelty.score += DEPTH_TSD_NOVELTY_LOW_CONFIDENCE_WEIGHT * confidence_gap; + let distance_excess = + (distance.abs() - DEPTH_TSD_NOVELTY_SURFACE_MISMATCH_ABS_DISTANCE).max(0.0); + if distance_excess > 0.0 { + novelty.score += DEPTH_TSD_NOVELTY_SURFACE_MISMATCH_WEIGHT + * (distance_excess + / (1.0 - DEPTH_TSD_NOVELTY_SURFACE_MISMATCH_ABS_DISTANCE)); + } + } else { + novelty.score += DEPTH_TSD_NOVELTY_UNKNOWN_WEIGHT; + } + } + } + + novelty +} + +pub fn preprocess_depth_mesh( + job: DepthMeshJob, + worker_state: &mut DepthPreprocessWorkerState, +) -> Result { + rebuild_sampled_depth_grid(&job, worker_state); + + let voxel_size_meters = job.voxel_size_meters; + let tsd_distance_meters = depth_tsd_distance_meters(voxel_size_meters); + worker_state.clear_frame_tsd_accum(); + let mut observed_world_min = vec3f(f32::INFINITY, f32::INFINITY, f32::INFINITY); + let mut observed_world_max = vec3f(f32::NEG_INFINITY, f32::NEG_INFINITY, f32::NEG_INFINITY); + for y in 0..worker_state.depth_height { + for x in 0..worker_state.depth_width { + if !depth_pixel_inside_margin(worker_state.depth_width, worker_state.depth_height, x, y) + { + continue; + } + let sample = worker_state.sampled_depth[y * worker_state.depth_width + x]; + if !sample.valid || !sample.has_normal { + continue; + } + if point_inside_player_exclusion(job.camera_world, sample.world) { + continue; + } + + let surface_ray = sample.world - job.camera_world; + let surface_distance = sample.ray_distance; + if !surface_distance.is_finite() + || !(DEPTH_TSD_MIN_UPDATE_DISTANCE_METERS..=DEPTH_VOXEL_MAX_DISTANCE_METERS) + .contains(&surface_distance) + { + continue; + } + + let ray_dir = surface_ray.normalize(); + let norm_dot = if sample.has_normal { + (-sample.normal.dot(ray_dir)).clamp(0.0, 1.0) + } else { + 1.0 + }; + if sample.has_normal && norm_dot <= DEPTH_TSD_MIN_NORMAL_DOT { + continue; + } + + observed_world_min = Vec3f::min_componentwise(observed_world_min, sample.world); + observed_world_max = Vec3f::max_componentwise(observed_world_max, sample.world); + + let start_distance = + (surface_distance - tsd_distance_meters).max(DEPTH_TSD_MIN_UPDATE_DISTANCE_METERS); + let end_distance = + (surface_distance + tsd_distance_meters).min(DEPTH_VOXEL_MAX_DISTANCE_METERS); + append_tsd_samples_along_ray( + worker_state, + job.camera_world, + ray_dir, + surface_distance, + start_distance, + end_distance, + voxel_size_meters, + tsd_distance_meters, + ); + } + } + + worker_state.build_reduced_tsd_samples(); + + let (visible_world_min, visible_world_max) = if observed_world_min.x.is_finite() + && observed_world_min.y.is_finite() + && observed_world_min.z.is_finite() + && observed_world_max.x.is_finite() + && observed_world_max.y.is_finite() + && observed_world_max.z.is_finite() + { + let padding = vec3f( + tsd_distance_meters, + tsd_distance_meters, + tsd_distance_meters, + ); + (observed_world_min - padding, observed_world_max + padding) + } else { + depth_visible_world_bounds(&job).unwrap_or((vec3f(0.0, 0.0, 0.0), vec3f(0.0, 0.0, 0.0))) + }; + + Ok(PreparedDepthMeshJob { + reset_generation: job.reset_generation, + generation: job.generation, + eye_index: job.eye_index, + width: job.width, + height: job.height, + voxel_size_meters, + camera_world: job.camera_world, + depth_proj: job.depth_proj, + inv_depth_proj: job.inv_depth_proj, + depth_view_from_world: job.depth_view_from_world, + visible_world_min, + visible_world_max, + depth: job.depth, + }) +} + +pub fn apply_preprocessed_depth_mesh( + job: PreparedDepthMeshJob, + worker_state: &DepthPreprocessWorkerState, + volume: &mut DepthMeshVolume, +) { + volume.generation = job.generation; + volume.eye_index = job.eye_index; + volume.image_width = job.width; + volume.image_height = job.height; + volume.latest_camera_world = Some(job.camera_world); + + let mut topology_changes = apply_tsd_samples(volume, &worker_state.reduced_tsd_samples); + topology_changes += refresh_visible_free_space(volume, &job); + topology_changes += clear_player_exclusion_volume(volume, job.camera_world); + if topology_changes != 0 { + volume.latest_topology_generation = job.generation; + } + volume.update_bounds(); +} + +pub fn update_published_height_map(volume: &mut DepthMeshVolume) -> bool { + let next_height_map = build_projected_height_map(volume); + if volume.published_height_map == next_height_map { + return false; + } + volume.published_height_map = next_height_map; + volume.update_sequence = volume.update_sequence.saturating_add(1); + true +} + +pub fn projected_height_refresh_budget( + pending_samples: usize, + now: Instant, + next_publish_at: Instant, + slice_credits: usize, +) -> usize { + if pending_samples == 0 || slice_credits == 0 { + return 0; + } + let remaining_millis = next_publish_at.saturating_duration_since(now).as_millis() as usize; + let slice_interval_millis = DEPTH_PROJECTED_HEIGHT_REFRESH_INTERVAL_MILLIS.max(1) as usize; + let remaining_slices = remaining_millis.div_ceil(slice_interval_millis).max(1); + let samples_per_slice = pending_samples.div_ceil(remaining_slices).clamp( + DEPTH_ALIGN_PROJECTED_HEIGHT_SAMPLES_PER_TICK, + DEPTH_ALIGN_PROJECTED_HEIGHT_MAX_SAMPLES_PER_SLICE, + ); + samples_per_slice.saturating_mul(slice_credits).min( + DEPTH_ALIGN_PROJECTED_HEIGHT_MAX_SAMPLES_PER_SLICE + * DEPTH_ALIGN_PROJECTED_HEIGHT_MAX_SLICE_CREDITS, + ) +} + +pub fn sync_projected_height_field_layout(volume: &mut DepthMeshVolume) { + let Some(layout) = projected_height_field_layout(volume) else { + volume.projected_height_field = None; + volume.projected_height_publish_pending = false; + volume.pending_projected_height_dirty_samples.clear(); + volume.pending_projected_height_sample_queue.clear(); + return; + }; + let needs_rebuild = volume + .projected_height_field + .as_ref() + .map(|field| field.layout != layout) + .unwrap_or(true); + if !needs_rebuild { + return; + } + let previous_field = volume.projected_height_field.take(); + let previous_dirty_samples = volume + .pending_projected_height_sample_queue + .iter() + .copied() + .collect::>(); + let mut next_field = ProjectedHeightField::new(layout, volume.latest_camera_world); + let copied_overlap = previous_field.as_ref().is_some_and(|previous_field| { + copy_projected_height_field_overlap(previous_field, &mut next_field) + }); + let remapped_dirty_samples = previous_field + .as_ref() + .map(|previous_field| { + remap_projected_height_dirty_samples( + previous_field, + &next_field, + &previous_dirty_samples, + ) + }) + .unwrap_or_default(); + let missing_sample_indices = next_field + .valid + .iter() + .enumerate() + .filter_map(|(sample_index, valid)| (*valid == 0).then_some(sample_index)) + .collect::>(); + volume.projected_height_field = Some(next_field); + volume.projected_height_publish_pending = true; + volume.pending_projected_height_dirty_samples.clear(); + volume.pending_projected_height_sample_queue.clear(); + if copied_overlap { + for sample_index in remapped_dirty_samples { + enqueue_projected_height_sample_dirty(volume, sample_index); + } + for sample_index in missing_sample_indices { + enqueue_projected_height_sample_dirty(volume, sample_index); + } + } else { + mark_all_projected_height_samples_dirty(volume); + } +} + +pub fn sync_projected_height_field_player_cutout(volume: &mut DepthMeshVolume) { + let Some(previous_cutout_center) = volume + .projected_height_field + .as_ref() + .map(|field| field.player_cutout_center) + else { + return; + }; + let next_cutout_center = volume.latest_camera_world; + if projected_height_cutout_center_matches(previous_cutout_center, next_cutout_center) { + return; + } + let dirty_radius = + DEPTH_ALIGN_VECTOR_SLICE_PLAYER_CUTOUT_RADIUS_METERS + volume.voxel_size_meters; + if let Some(center) = previous_cutout_center { + mark_projected_height_samples_dirty_around_point(volume, center, dirty_radius); + } + if let Some(center) = next_cutout_center { + mark_projected_height_samples_dirty_around_point(volume, center, dirty_radius); + } + if let Some(field) = volume.projected_height_field.as_mut() { + field.player_cutout_center = next_cutout_center; + } + volume.projected_height_publish_pending = true; +} + +pub fn refresh_projected_height_field(volume: &mut DepthMeshVolume, max_samples: usize) -> bool { + let flush_all = max_samples == usize::MAX; + let mut processed = 0usize; + let mut changed = false; + loop { + if !flush_all && processed >= max_samples { + break; + } + let Some(sample_index) = volume.pending_projected_height_sample_queue.pop_front() else { + break; + }; + volume + .pending_projected_height_dirty_samples + .remove(&sample_index); + let Some((layout, cutout_center, sample_size_x, current_height, current_valid)) = + volume.projected_height_field.as_ref().map(|field| { + ( + field.layout, + field.player_cutout_center, + field.sample_size_x(), + field.heights_meters[sample_index], + field.valid[sample_index] != 0, + ) + }) + else { + break; + }; + let sample_x = sample_index % sample_size_x; + let sample_z = sample_index / sample_size_x; + let point = vector_slice_corner_world( + layout.origin_x, + layout.origin_z, + layout.cell_size_meters, + DEPTH_ALIGN_VECTOR_SLICE_ISO_HEIGHT_METERS, + sample_x, + sample_z, + ); + let (next_valid, next_height) = if cutout_center.is_some_and(|camera_world| { + vector_slice_point_inside_player_cutout(camera_world, point) + }) { + (false, 0.0) + } else if let Some(height) = query_depth_grid_projected_column_height( + volume, + point.x, + point.z, + layout.top_y_meters, + layout.bottom_y_meters, + ) { + (true, height) + } else { + (false, 0.0) + }; + if current_valid != next_valid + || (next_valid && (current_height - next_height).abs() > 1.0e-4) + || (!next_valid && current_height != 0.0) + { + if let Some(field) = volume.projected_height_field.as_mut() { + field.valid[sample_index] = next_valid as u8; + field.heights_meters[sample_index] = next_height; + } + changed = true; + } + processed += 1; + } + changed +} + +fn projected_height_field_layout(volume: &DepthMeshVolume) -> Option { + let (bounds_min, bounds_max) = volume.mesh_grid.world_bounds(1)?; + let cell_size_meters = volume.voxel_size_meters.max(1.0e-5); + let padding_cells = + (DEPTH_ALIGN_HEIGHT_MAP_BOUNDS_PADDING_METERS / cell_size_meters).ceil() as i32; + let min_cell_x = (bounds_min.x / cell_size_meters).floor() as i32 - padding_cells; + let max_cell_x = (bounds_max.x / cell_size_meters).ceil() as i32 + padding_cells; + let min_cell_z = (bounds_min.z / cell_size_meters).floor() as i32 - padding_cells; + let max_cell_z = (bounds_max.z / cell_size_meters).ceil() as i32 + padding_cells; + let size_x = (max_cell_x - min_cell_x).max(1) as usize; + let size_z = (max_cell_z - min_cell_z).max(1) as usize; + if size_x > u16::MAX as usize || size_z > u16::MAX as usize { + return None; + } + let origin_x = min_cell_x as f32 * cell_size_meters; + let origin_z = min_cell_z as f32 * cell_size_meters; + let top_y_meters = vector_slice_projection_top_y(volume)?; + let bottom_y_meters = vector_slice_projection_bottom_y(volume, top_y_meters)?; + Some(ProjectedHeightFieldLayout { + origin_x, + origin_z, + cell_size_meters, + size_x, + size_z, + top_y_meters, + bottom_y_meters, + }) +} + +fn projected_height_cutout_center_matches(a: Option, b: Option) -> bool { + match (a, b) { + (None, None) => true, + (Some(a), Some(b)) => (a - b).length() <= 1.0e-4, + _ => false, + } +} + +fn enqueue_projected_height_sample_dirty(volume: &mut DepthMeshVolume, sample_index: usize) { + if volume + .pending_projected_height_dirty_samples + .insert(sample_index) + { + volume + .pending_projected_height_sample_queue + .push_back(sample_index); + } +} + +fn projected_height_sample_index(sample_size_x: usize, x: usize, z: usize) -> usize { + x + z * sample_size_x +} + +fn mark_all_projected_height_samples_dirty(volume: &mut DepthMeshVolume) { + let Some((sample_size_x, sample_size_z)) = volume + .projected_height_field + .as_ref() + .map(|field| (field.sample_size_x(), field.sample_size_z())) + else { + return; + }; + let swizzle = DEPTH_ALIGN_PROJECTED_HEIGHT_DIRTY_SWIZZLE.max(1); + for z_phase in 0..swizzle { + for x_phase in 0..swizzle { + for z in (z_phase..sample_size_z).step_by(swizzle) { + for x in (x_phase..sample_size_x).step_by(swizzle) { + enqueue_projected_height_sample_dirty( + volume, + projected_height_sample_index(sample_size_x, x, z), + ); + } + } + } + } +} + +fn projected_height_layout_sample_offset( + source: ProjectedHeightFieldLayout, + target: ProjectedHeightFieldLayout, +) -> Option<(isize, isize)> { + let cell_size = source.cell_size_meters.max(1.0e-5); + if (source.cell_size_meters - target.cell_size_meters).abs() > 1.0e-5 { + return None; + } + let offset_x = (source.origin_x - target.origin_x) / cell_size; + let offset_z = (source.origin_z - target.origin_z) / cell_size; + let snapped_x = offset_x.round(); + let snapped_z = offset_z.round(); + ((offset_x - snapped_x).abs() <= 1.0e-4 && (offset_z - snapped_z).abs() <= 1.0e-4) + .then_some((snapped_x as isize, snapped_z as isize)) +} + +fn copy_projected_height_field_overlap( + previous: &ProjectedHeightField, + next: &mut ProjectedHeightField, +) -> bool { + let Some((offset_x, offset_z)) = + projected_height_layout_sample_offset(previous.layout, next.layout) + else { + return false; + }; + let previous_size_x = previous.sample_size_x(); + let previous_size_z = previous.sample_size_z(); + let next_size_x = next.sample_size_x(); + let next_size_z = next.sample_size_z(); + for previous_z in 0..previous_size_z { + let next_z = previous_z as isize + offset_z; + if next_z < 0 || next_z >= next_size_z as isize { + continue; + } + for previous_x in 0..previous_size_x { + let next_x = previous_x as isize + offset_x; + if next_x < 0 || next_x >= next_size_x as isize { + continue; + } + let previous_index = + projected_height_sample_index(previous_size_x, previous_x, previous_z); + let next_index = + projected_height_sample_index(next_size_x, next_x as usize, next_z as usize); + next.valid[next_index] = previous.valid[previous_index]; + next.heights_meters[next_index] = previous.heights_meters[previous_index]; + } + } + true +} + +fn remap_projected_height_dirty_samples( + previous: &ProjectedHeightField, + next: &ProjectedHeightField, + previous_dirty_samples: &[usize], +) -> Vec { + let Some((offset_x, offset_z)) = + projected_height_layout_sample_offset(previous.layout, next.layout) + else { + return Vec::new(); + }; + let previous_size_x = previous.sample_size_x(); + let next_size_x = next.sample_size_x(); + let next_size_z = next.sample_size_z(); + let mut remapped = Vec::new(); + for previous_index in previous_dirty_samples { + let previous_x = previous_index % previous_size_x; + let previous_z = previous_index / previous_size_x; + let next_x = previous_x as isize + offset_x; + let next_z = previous_z as isize + offset_z; + if next_x < 0 + || next_z < 0 + || next_x >= next_size_x as isize + || next_z >= next_size_z as isize + { + continue; + } + remapped.push(projected_height_sample_index( + next_size_x, + next_x as usize, + next_z as usize, + )); + } + remapped +} + +fn mark_projected_height_samples_dirty_world_rect( + volume: &mut DepthMeshVolume, + min_x: f32, + max_x: f32, + min_z: f32, + max_z: f32, +) { + let Some((origin_x, origin_z, cell_size_meters, sample_size_x, sample_size_z)) = + volume.projected_height_field.as_ref().map(|field| { + ( + field.layout.origin_x, + field.layout.origin_z, + field.layout.cell_size_meters, + field.sample_size_x(), + field.sample_size_z(), + ) + }) + else { + return; + }; + let max_sample_x = sample_size_x as isize - 1; + let max_sample_z = sample_size_z as isize - 1; + let raw_min_x = ((min_x - origin_x) / cell_size_meters.max(1.0e-5)).floor() as isize; + let raw_max_x = ((max_x - origin_x) / cell_size_meters.max(1.0e-5)).ceil() as isize; + let raw_min_z = ((min_z - origin_z) / cell_size_meters.max(1.0e-5)).floor() as isize; + let raw_max_z = ((max_z - origin_z) / cell_size_meters.max(1.0e-5)).ceil() as isize; + if raw_max_x < 0 || raw_max_z < 0 || raw_min_x > max_sample_x || raw_min_z > max_sample_z { + return; + } + let sample_min_x = raw_min_x.clamp(0, max_sample_x) as usize; + let sample_max_x = raw_max_x.clamp(0, max_sample_x) as usize; + let sample_min_z = raw_min_z.clamp(0, max_sample_z) as usize; + let sample_max_z = raw_max_z.clamp(0, max_sample_z) as usize; + for z in sample_min_z..=sample_max_z { + for x in sample_min_x..=sample_max_x { + enqueue_projected_height_sample_dirty(volume, x + z * sample_size_x); + } + } +} + +fn mark_projected_height_samples_dirty_around_voxel( + volume: &mut DepthMeshVolume, + voxel: VoxelCoord, +) { + if volume.projected_height_field.is_none() { + return; + } + let center = volume.mesh_grid.voxel_center_world(voxel); + let radius = volume.voxel_size_meters; + mark_projected_height_samples_dirty_world_rect( + volume, + center.x - radius, + center.x + radius, + center.z - radius, + center.z + radius, + ); +} + +fn mark_projected_height_samples_dirty_around_point( + volume: &mut DepthMeshVolume, + point: Vec3f, + radius: f32, +) { + mark_projected_height_samples_dirty_world_rect( + volume, + point.x - radius, + point.x + radius, + point.z - radius, + point.z + radius, + ); +} + +fn vector_slice_projection_top_y(volume: &DepthMeshVolume) -> Option { + let (bounds_min, bounds_max) = volume.mesh_grid.world_bounds(1)?; + let min_y = bounds_min.y + volume.voxel_size_meters; + let max_y = bounds_max.y - volume.voxel_size_meters; + if max_y <= min_y { + return None; + } + Some(DEPTH_ALIGN_VECTOR_SLICE_TOP_Y_METERS.clamp(min_y, max_y)) +} + +fn vector_slice_projection_bottom_y(volume: &DepthMeshVolume, top_y: f32) -> Option { + let (bounds_min, _) = volume.mesh_grid.world_bounds(1)?; + let min_y = bounds_min.y + volume.voxel_size_meters; + let max_y = top_y - volume.voxel_size_meters; + (max_y > min_y).then_some(min_y) +} + +fn vector_slice_corner_world( + origin_x: f32, + origin_z: f32, + cell_size: f32, + y: f32, + x: usize, + z: usize, +) -> Vec3f { + vec3f( + origin_x + x as f32 * cell_size, + y, + origin_z + z as f32 * cell_size, + ) +} + +fn vector_slice_point_inside_player_cutout(camera_world: Vec3f, point: Vec3f) -> bool { + let dx = point.x - camera_world.x; + let dz = point.z - camera_world.z; + dx * dx + dz * dz + <= DEPTH_ALIGN_VECTOR_SLICE_PLAYER_CUTOUT_RADIUS_METERS + * DEPTH_ALIGN_VECTOR_SLICE_PLAYER_CUTOUT_RADIUS_METERS +} + +fn vector_slice_cell_inside_player_cutout( + camera_world: Vec3f, + origin_x: f32, + origin_z: f32, + cell_size: f32, + x: usize, + z: usize, +) -> bool { + let center = vec3f( + origin_x + (x as f32 + 0.5) * cell_size, + DEPTH_ALIGN_VECTOR_SLICE_ISO_HEIGHT_METERS, + origin_z + (z as f32 + 0.5) * cell_size, + ); + let expanded_radius = DEPTH_ALIGN_VECTOR_SLICE_PLAYER_CUTOUT_RADIUS_METERS + cell_size * 0.75; + let dx = center.x - camera_world.x; + let dz = center.z - camera_world.z; + dx * dx + dz * dz <= expanded_radius * expanded_radius +} + +fn query_depth_grid_projected_column_height( + volume: &DepthMeshVolume, + sample_x: f32, + sample_z: f32, + top_y: f32, + bottom_y: f32, +) -> Option { + let top_coord = volume + .mesh_grid + .world_to_voxel_coord(vec3f(sample_x, top_y, sample_z)) + .y; + let bottom_coord = volume + .mesh_grid + .world_to_voxel_coord(vec3f(sample_x, bottom_y, sample_z)) + .y; + if top_coord <= bottom_coord { + return None; + } + let mut had_data = false; + let mut topmost_valid_distance = None::; + for y_coord in (bottom_coord + 1..=top_coord).rev() { + let Some(above) = query_grid_bilinear_distance_at_y(volume, sample_x, sample_z, y_coord) + else { + continue; + }; + let Some(below) = + query_grid_bilinear_distance_at_y(volume, sample_x, sample_z, y_coord - 1) + else { + continue; + }; + had_data = true; + topmost_valid_distance.get_or_insert(above); + if above <= 0.0 || below > 0.0 { + continue; + } + + let denom = above - below; + let blend = if denom.abs() > 1.0e-6 { + (above / denom).clamp(0.0, 1.0) + } else { + 0.5 + }; + let y_above = voxel_center_axis(volume.voxel_size_meters, y_coord); + let y_below = voxel_center_axis(volume.voxel_size_meters, y_coord - 1); + return Some(y_above + (y_below - y_above) * blend); + } + if topmost_valid_distance.is_some_and(|distance| distance <= 0.0) { + Some(top_y) + } else if had_data { + Some(bottom_y) + } else { + None + } +} + +fn estimate_projected_height_floor_y(valid_cell_heights: &[f32], bottom_y: f32, top_y: f32) -> f32 { + let heights = valid_cell_heights + .iter() + .copied() + .filter(|value| value.is_finite()) + .collect::>(); + if heights.is_empty() { + return bottom_y; + } + let span = (top_y - bottom_y).max(1.0e-3); + let bin_size = DEPTH_ALIGN_HEIGHT_MAP_FLOOR_BIN_METERS + .max(span / 256.0) + .min(span); + let bin_count = ((span / bin_size).ceil() as usize).max(1) + 1; + let mut bins = vec![0usize; bin_count]; + for height in &heights { + let bin = (((*height - bottom_y) / bin_size).floor() as isize) + .clamp(0, bin_count.saturating_sub(1) as isize) as usize; + bins[bin] += 1; + } + + let min_support = DEPTH_ALIGN_HEIGHT_MAP_FLOOR_MIN_SUPPORT_CELLS.max( + (heights.len() as f32 * DEPTH_ALIGN_HEIGHT_MAP_FLOOR_MIN_SUPPORT_RATIO).ceil() as usize, + ); + let support_window_bins = DEPTH_ALIGN_HEIGHT_MAP_FLOOR_SUPPORT_WINDOW_BINS.max(1); + for start_bin in 0..bin_count { + let end_bin = (start_bin + support_window_bins).min(bin_count); + let support = bins[start_bin..end_bin].iter().copied().sum::(); + if support < min_support { + continue; + } + let low = bottom_y + start_bin as f32 * bin_size; + let high = bottom_y + end_bin as f32 * bin_size; + let mut sum = 0.0; + let mut count = 0usize; + for height in &heights { + if *height >= low && *height <= high { + sum += *height; + count += 1; + } + } + if count > 0 { + return (sum / count as f32).clamp(bottom_y, top_y); + } + } + + let mut sorted = heights; + sorted.sort_by(|left, right| left.total_cmp(right)); + let fallback_count = ((sorted.len() as f32 * 0.01).ceil() as usize).clamp(1, 32); + let fallback = &sorted[..fallback_count]; + (fallback.iter().copied().sum::() / fallback.len() as f32).clamp(bottom_y, top_y) +} + +fn build_projected_height_map(volume: &mut DepthMeshVolume) -> Option { + sync_projected_height_field_layout(volume); + sync_projected_height_field_player_cutout(volume); + let field = volume.projected_height_field.as_ref()?; + let origin_x = field.layout.origin_x; + let origin_z = field.layout.origin_z; + let cell_size = field.layout.cell_size_meters; + let size_x = field.layout.size_x; + let size_z = field.layout.size_z; + let sample_size_x = field.sample_size_x(); + let heights = &field.heights_meters; + let valid = &field.valid; + let player_cutout_center = field.player_cutout_center; + let mut height_map = XrDepthAlignHeightMap { + origin_x, + origin_z, + cell_size_meters: cell_size, + size_x: size_x as u16, + size_z: size_z as u16, + bottom_y_meters: field.layout.bottom_y_meters, + top_y_meters: field.layout.top_y_meters, + floor_y_meters: field.layout.bottom_y_meters, + player_cutout_center: player_cutout_center.map(|center| vec2f(center.x, center.z)), + player_cutout_radius_meters: DEPTH_ALIGN_VECTOR_SLICE_PLAYER_CUTOUT_RADIUS_METERS, + heights_meters: vec![f32::NAN; size_x * size_z], + }; + let mut valid_cell_heights = Vec::::with_capacity(size_x.saturating_mul(size_z)); + for z in 0..size_z { + for x in 0..size_x { + if player_cutout_center.is_some_and(|camera_world| { + vector_slice_cell_inside_player_cutout( + camera_world, + origin_x, + origin_z, + cell_size, + x, + z, + ) + }) { + continue; + } + let corner_indices = [ + x + z * sample_size_x, + x + 1 + z * sample_size_x, + x + 1 + (z + 1) * sample_size_x, + x + (z + 1) * sample_size_x, + ]; + let preview_index = height_map.cell_index(x, z); + let mut height_sum = 0.0; + let mut height_count = 0usize; + for corner_index in corner_indices { + if valid[corner_index] == 0 { + continue; + } + height_sum += heights[corner_index]; + height_count += 1; + } + if height_count != 0 { + let height = height_sum / height_count as f32; + height_map.heights_meters[preview_index] = height; + valid_cell_heights.push(height); + } + } + } + height_map.floor_y_meters = estimate_projected_height_floor_y( + &valid_cell_heights, + field.layout.bottom_y_meters, + field.layout.top_y_meters, + ); + Some(height_map) +} + +fn rebuild_sampled_depth_grid(job: &DepthMeshJob, worker_state: &mut DepthPreprocessWorkerState) { + let normal_neighbor_max_distance_delta_meters = + depth_normal_neighbor_max_distance_delta_meters(job.voxel_size_meters); + let width = job.width as usize; + let height = job.height as usize; + worker_state.depth_width = width; + worker_state.depth_height = height; + worker_state.sampled_depth.clear(); + worker_state.sampled_depth.resize( + width * height, + DepthGridSample { + world: vec3f(0.0, 0.0, 0.0), + normal: vec3f(0.0, 1.0, 0.0), + ray_distance: 0.0, + valid: false, + has_normal: false, + }, + ); + + for y in 0..height { + for x in 0..width { + if !depth_pixel_inside_margin(width, height, x, y) { + continue; + } + let raw_depth = job.depth[y * width + x] as f32 / u16::MAX as f32; + if !(DEPTH_VOXEL_MIN_DEPTH_VALUE..DEPTH_VOXEL_MAX_DEPTH_VALUE).contains(&raw_depth) { + continue; + } + let Some(world) = depth_pixel_to_world(job, x as u32, y as u32) else { + continue; + }; + worker_state.sampled_depth[y * width + x] = DepthGridSample { + world, + normal: vec3f(0.0, 1.0, 0.0), + ray_distance: (world - job.camera_world).length(), + valid: true, + has_normal: false, + }; + } + } + + for y in 0..height { + for x in 0..width { + let index = y * width + x; + if !worker_state.sampled_depth[index].valid { + continue; + } + let world = worker_state.sampled_depth[index].world; + let sample_x = + sampled_depth_at_pixel(worker_state, (x + 2).min(width - 1) as u32, y as u32); + let sample_y = + sampled_depth_at_pixel(worker_state, x as u32, (y + 2).min(height - 1) as u32); + if !sample_x.valid || !sample_y.valid { + continue; + } + let ray_distance = worker_state.sampled_depth[index].ray_distance; + if (sample_x.ray_distance - ray_distance).abs() + > normal_neighbor_max_distance_delta_meters + || (sample_y.ray_distance - ray_distance).abs() + > normal_neighbor_max_distance_delta_meters + { + continue; + } + let h_deriv = sample_x.world - world; + let v_deriv = sample_y.world - world; + if h_deriv.length() <= 1.0e-4 || v_deriv.length() <= 1.0e-4 { + continue; + } + let mut normal = Vec3f::cross(h_deriv, v_deriv).normalize().scale(-1.0); + if normal.length() <= 1.0e-4 { + continue; + } + let view_dir = (job.camera_world - world).normalize(); + if normal.dot(view_dir) < 0.0 { + normal = normal.scale(-1.0); + } + worker_state.sampled_depth[index].normal = normal; + worker_state.sampled_depth[index].has_normal = true; + } + } +} + +fn sampled_depth_at_pixel( + worker_state: &DepthPreprocessWorkerState, + pixel_x: u32, + pixel_y: u32, +) -> DepthGridSample { + if worker_state.depth_width == 0 || worker_state.depth_height == 0 { + return DepthGridSample { + world: vec3f(0.0, 0.0, 0.0), + normal: vec3f(0.0, 1.0, 0.0), + ray_distance: 0.0, + valid: false, + has_normal: false, + }; + } + let x = pixel_x.min(worker_state.depth_width.saturating_sub(1) as u32) as usize; + let y = pixel_y.min(worker_state.depth_height.saturating_sub(1) as u32) as usize; + worker_state.sampled_depth[y * worker_state.depth_width + x] +} + +fn depth_pixel_inside_margin(width: usize, height: usize, x: usize, y: usize) -> bool { + let margin = DEPTH_IMAGE_EDGE_MARGIN_PIXELS; + if width <= margin * 2 || height <= margin * 2 { + return true; + } + x >= margin && y >= margin && x + margin < width && y + margin < height +} + +fn depth_ndc_to_view(job: &DepthMeshJob, ndc_x: f32, ndc_y: f32, ndc_z: f32) -> Option { + let view = job + .inv_depth_proj + .transform_vec4(vec4f(ndc_x, ndc_y, ndc_z, 1.0)); + if !view.w.is_finite() || view.w.abs() < 1.0e-6 { + return None; + } + let inv_w = 1.0 / view.w; + let point = vec3f(view.x * inv_w, view.y * inv_w, view.z * inv_w); + (point.x.is_finite() && point.y.is_finite() && point.z.is_finite()).then_some(point) +} + +fn depth_view_to_world(job: &DepthMeshJob, view: Vec3f) -> Option { + let world = job + .world_from_depth_view + .transform_vec4(vec4f(view.x, view.y, view.z, 1.0)); + if !world.w.is_finite() || world.w.abs() < 1.0e-6 { + return None; + } + let inv_w = 1.0 / world.w; + let point = vec3f(world.x * inv_w, world.y * inv_w, world.z * inv_w); + (point.x.is_finite() && point.y.is_finite() && point.z.is_finite()).then_some(point) +} + +fn depth_pixel_to_world(job: &DepthMeshJob, x: u32, y: u32) -> Option { + let view = depth_pixel_to_view( + &job.depth, + job.width, + job.height, + job.inv_depth_proj, + x as usize, + y as usize, + )?; + let world = job.world_from_depth_view.transform_vec4(view); + if !world.w.is_finite() || world.w.abs() < 1.0e-6 { + return None; + } + let inv_w = 1.0 / world.w; + let point = vec3f(world.x * inv_w, world.y * inv_w, world.z * inv_w); + (point.x.is_finite() && point.y.is_finite() && point.z.is_finite()).then_some(point) +} + +fn depth_pixel_to_view( + depth: &[u16], + width: u32, + height: u32, + inv_depth_proj: Mat4f, + x: usize, + y: usize, +) -> Option { + let raw_depth = *depth.get(y * width as usize + x)? as f32 / u16::MAX as f32; + if !(DEPTH_VOXEL_MIN_DEPTH_VALUE..DEPTH_VOXEL_MAX_DEPTH_VALUE).contains(&raw_depth) { + return None; + } + let uv_x = (x as f32 + 0.5) / width as f32; + let uv_y = (y as f32 + 0.5) / height as f32; + let clip = vec4f( + uv_x * 2.0 - 1.0, + uv_y * 2.0 - 1.0, + raw_depth * 2.0 - 1.0, + 1.0, + ); + let view = inv_depth_proj.transform_vec4(clip); + if !view.w.is_finite() || view.w.abs() < 1.0e-6 { + return None; + } + let view = vec4f(view.x / view.w, view.y / view.w, view.z / view.w, 1.0); + let distance = view.to_vec3f().length(); + if !distance.is_finite() + || !(DEPTH_VOXEL_MIN_DISTANCE_METERS..=DEPTH_VOXEL_MAX_DISTANCE_METERS).contains(&distance) + { + return None; + } + Some(view) +} + +fn depth_world_to_pixel( + world: Vec3f, + width: u32, + height: u32, + depth_view_from_world: Mat4f, + depth_proj: Mat4f, +) -> Option<(usize, usize, f32)> { + let view = depth_view_from_world.transform_vec4(vec4f(world.x, world.y, world.z, 1.0)); + if !view.w.is_finite() || view.w.abs() < 1.0e-6 { + return None; + } + let view = vec4f(view.x / view.w, view.y / view.w, view.z / view.w, 1.0); + let view_pos = view.to_vec3f(); + let ray_distance = view_pos.length(); + if !ray_distance.is_finite() + || !(DEPTH_VOXEL_MIN_DISTANCE_METERS..=DEPTH_VOXEL_MAX_DISTANCE_METERS) + .contains(&ray_distance) + { + return None; + } + let clip = depth_proj.transform_vec4(view); + if !clip.w.is_finite() || clip.w.abs() < 1.0e-6 { + return None; + } + let ndc_x = clip.x / clip.w; + let ndc_y = clip.y / clip.w; + let ndc_z = clip.z / clip.w; + if !ndc_x.is_finite() || !ndc_y.is_finite() || !ndc_z.is_finite() { + return None; + } + if !(-1.0..=1.0).contains(&ndc_x) + || !(-1.0..=1.0).contains(&ndc_y) + || !(-1.0..=1.0).contains(&ndc_z) + { + return None; + } + let pixel_x = ((ndc_x * 0.5 + 0.5) * width as f32).floor() as isize; + let pixel_y = ((ndc_y * 0.5 + 0.5) * height as f32).floor() as isize; + if pixel_x < 0 || pixel_y < 0 || pixel_x >= width as isize || pixel_y >= height as isize { + return None; + } + Some((pixel_x as usize, pixel_y as usize, ray_distance)) +} + +fn depth_pixel_is_reliable_for_carve( + job: &PreparedDepthMeshJob, + pixel_x: usize, + pixel_y: usize, + observed_distance: f32, +) -> bool { + let carve_neighbor_max_distance_delta_meters = + depth_carve_neighbor_max_distance_delta_meters(job.voxel_size_meters); + let width = job.width as usize; + let height = job.height as usize; + if !depth_pixel_inside_margin(width, height, pixel_x, pixel_y) { + return false; + } + + let mut agreeing_neighbors = 0u8; + for (nx, ny) in [ + (pixel_x.saturating_sub(1), pixel_y), + ((pixel_x + 1).min(width.saturating_sub(1)), pixel_y), + (pixel_x, pixel_y.saturating_sub(1)), + (pixel_x, (pixel_y + 1).min(height.saturating_sub(1))), + ] { + if nx == pixel_x && ny == pixel_y { + continue; + } + let Some(neighbor_view) = depth_pixel_to_view( + &job.depth, + job.width, + job.height, + job.inv_depth_proj, + nx, + ny, + ) else { + continue; + }; + let neighbor_distance = neighbor_view.to_vec3f().length(); + if (neighbor_distance - observed_distance).abs() <= carve_neighbor_max_distance_delta_meters + { + agreeing_neighbors = agreeing_neighbors.saturating_add(1); + } + } + agreeing_neighbors >= 2 +} + +fn apply_tsd_samples(volume: &mut DepthMeshVolume, frame_tsd_samples: &[FrameTsdSample]) -> usize { + debug_assert_eq!(volume.mesh_grid.chunk_edge, DEPTH_TSD_CHUNK_EDGE_VOXELS); + let mut changed = 0; + let mut changed_coords = Vec::new(); + let mut index = 0usize; + while index < frame_tsd_samples.len() { + let chunk_key = frame_tsd_samples[index].chunk_key; + let mut chunk_dirty = false; + let mut became_live = 0usize; + changed_coords.clear(); + { + let chunk = volume + .mesh_grid + .chunks + .entry(chunk_key) + .or_insert_with(|| SparseTsdChunk::new(volume.mesh_grid.chunk_volume)); + while index < frame_tsd_samples.len() && frame_tsd_samples[index].chunk_key == chunk_key + { + let sample = frame_tsd_samples[index]; + let local_id = sample.local_id as usize; + let previous = chunk.value(local_id).unwrap_or(2.0); + let update = chunk.accumulate(local_id, sample.normalized, volume.generation); + if update.became_live { + became_live += 1; + } + chunk_dirty |= update.state_changed; + if update.value_changed { + let current = chunk.value(local_id).unwrap_or(previous); + if (previous - current).abs() >= DEPTH_TSD_APPLY_DELTA_EPSILON { + changed_coords.push(tsd_voxel_coord_from_chunk_key_and_local_id( + chunk_key, + sample.local_id, + )); + } + } + index += 1; + } + } + volume.mesh_grid.active_value_count += became_live; + if chunk_dirty { + volume.dirty_tsdf_chunk_keys.insert(chunk_key); + } + for coord in changed_coords.drain(..) { + mark_projected_height_samples_dirty_around_voxel(volume, coord); + changed += 1; + } + } + changed +} + +fn refresh_visible_free_space(volume: &mut DepthMeshVolume, job: &PreparedDepthMeshJob) -> usize { + let tsd_distance_meters = depth_tsd_distance_meters(job.voxel_size_meters); + let tsd_refresh_clearance_meters = depth_tsd_refresh_clearance_meters(job.voxel_size_meters); + let min_coord = volume.mesh_grid.world_to_voxel_coord(job.visible_world_min); + let max_coord = volume.mesh_grid.world_to_voxel_coord(job.visible_world_max); + let (min_chunk_key, _) = tsd_chunk_key_and_local_id(min_coord); + let (max_chunk_key, _) = tsd_chunk_key_and_local_id(max_coord); + let visible_chunk_keys = volume + .mesh_grid + .chunks + .keys() + .copied() + .filter(|chunk_key| { + chunk_key.x >= min_chunk_key.x + && chunk_key.x <= max_chunk_key.x + && chunk_key.y >= min_chunk_key.y + && chunk_key.y <= max_chunk_key.y + && chunk_key.z >= min_chunk_key.z + && chunk_key.z <= max_chunk_key.z + }) + .collect::>(); + let mut changed = 0; + + for chunk_key in visible_chunk_keys { + let mut chunk_dirty = false; + let mut changed_coords = Vec::new(); + { + let Some(chunk) = volume.mesh_grid.chunks.get_mut(&chunk_key) else { + continue; + }; + if chunk.live_count == 0 { + continue; + } + for word_index in 0..chunk.data.valid_bits.len() { + let mut pending_bits = chunk.data.valid_bits[word_index]; + while pending_bits != 0 { + let bit_index = pending_bits.trailing_zeros() as usize; + pending_bits &= pending_bits - 1; + let local_id = word_index * u64::BITS as usize + bit_index; + if local_id >= volume.mesh_grid.chunk_volume { + continue; + } + + let coord = + tsd_voxel_coord_from_chunk_key_and_local_id(chunk_key, local_id as u16); + if coord.x < min_coord.x + || coord.x > max_coord.x + || coord.y < min_coord.y + || coord.y > max_coord.y + || coord.z < min_coord.z + || coord.z > max_coord.z + { + continue; + } + + let Some(previous) = chunk.value(local_id) else { + continue; + }; + if previous >= 1.0 - 1.0e-4 { + continue; + } + + let voxel_world = vec3f( + voxel_center_axis(job.voxel_size_meters, coord.x), + voxel_center_axis(job.voxel_size_meters, coord.y), + voxel_center_axis(job.voxel_size_meters, coord.z), + ); + let Some((pixel_x, pixel_y, voxel_distance)) = depth_world_to_pixel( + voxel_world, + job.width, + job.height, + job.depth_view_from_world, + job.depth_proj, + ) else { + continue; + }; + if !depth_pixel_inside_margin( + job.width as usize, + job.height as usize, + pixel_x, + pixel_y, + ) { + continue; + } + let Some(observed_view) = depth_pixel_to_view( + &job.depth, + job.width, + job.height, + job.inv_depth_proj, + pixel_x, + pixel_y, + ) else { + continue; + }; + let observed_distance = observed_view.to_vec3f().length(); + if !depth_pixel_is_reliable_for_carve(job, pixel_x, pixel_y, observed_distance) + { + continue; + } + let clearance = observed_distance - voxel_distance; + if !observed_distance.is_finite() || clearance < tsd_refresh_clearance_meters { + continue; + } + let confidence = chunk.confidence(local_id); + if confidence >= DEPTH_TSD_STABLE_CONFIDENCE + && previous <= 0.25 + && clearance < tsd_distance_meters + { + continue; + } + let update = chunk.accumulate(local_id, 1.0, volume.generation); + chunk_dirty |= update.state_changed; + if update.value_changed { + let current = chunk.value(local_id).unwrap_or(previous); + if (previous - current).abs() >= DEPTH_TSD_APPLY_DELTA_EPSILON { + changed_coords.push(coord); + } + } + } + } + } + if chunk_dirty { + volume.dirty_tsdf_chunk_keys.insert(chunk_key); + } + for coord in changed_coords.drain(..) { + mark_projected_height_samples_dirty_around_voxel(volume, coord); + changed += 1; + } + } + + changed +} + +fn clear_player_exclusion_volume(volume: &mut DepthMeshVolume, camera_world: Vec3f) -> usize { + let min_world = vec3f( + camera_world.x - DEPTH_PLAYER_EXCLUDE_RADIUS_METERS, + camera_world.y + DEPTH_PLAYER_EXCLUDE_MIN_RELATIVE_Y_METERS, + camera_world.z - DEPTH_PLAYER_EXCLUDE_RADIUS_METERS, + ); + let max_world = vec3f( + camera_world.x + DEPTH_PLAYER_EXCLUDE_RADIUS_METERS, + camera_world.y + DEPTH_PLAYER_EXCLUDE_MAX_RELATIVE_Y_METERS, + camera_world.z + DEPTH_PLAYER_EXCLUDE_RADIUS_METERS, + ); + let min_coord = volume.mesh_grid.world_to_voxel_coord(min_world); + let max_coord = volume.mesh_grid.world_to_voxel_coord(max_world); + let mut changed = 0; + for z in min_coord.z..=max_coord.z { + for y in min_coord.y..=max_coord.y { + for x in min_coord.x..=max_coord.x { + let coord = VoxelCoord::new(x, y, z); + let center = volume.mesh_grid.voxel_center_world(coord); + if !point_inside_player_exclusion(camera_world, center) { + continue; + } + let Some(previous) = volume.mesh_grid.normalized_distance(coord) else { + continue; + }; + if volume.mesh_grid.confidence(coord) > DEPTH_PLAYER_CLEAR_MAX_CONFIDENCE { + continue; + } + if previous >= 1.0 - 1.0e-4 { + continue; + } + let update = + volume + .mesh_grid + .overwrite_normalized_distance(coord, 1.0, volume.generation); + if update.state_changed { + mark_tsdf_chunk_dirty(volume, coord); + } + if update.value_changed { + mark_projected_height_samples_dirty_around_voxel(volume, coord); + changed += 1; + } + } + } + } + changed +} + +fn mark_tsdf_chunk_dirty(volume: &mut DepthMeshVolume, voxel: VoxelCoord) { + let (chunk_key, _) = volume.mesh_grid.chunk_key_and_id(voxel); + volume.dirty_tsdf_chunk_keys.insert(chunk_key); +} + +fn depth_visible_world_bounds(job: &DepthMeshJob) -> Option<(Vec3f, Vec3f)> { + let Some(bottom_left) = depth_ndc_to_view(job, -1.0, -1.0, 0.0) else { + return None; + }; + let Some(bottom_right) = depth_ndc_to_view(job, 1.0, -1.0, 0.0) else { + return None; + }; + let Some(top_right) = depth_ndc_to_view(job, 1.0, 1.0, 0.0) else { + return None; + }; + let Some(top_left) = depth_ndc_to_view(job, -1.0, 1.0, 0.0) else { + return None; + }; + let mut corners = [bottom_left, bottom_right, top_right, top_left]; + for corner in &mut corners { + let scale = DEPTH_MESH_UPDATE_DISTANCE_METERS / (-corner.z).max(1.0e-6); + *corner = corner.scale(scale); + } + + let mut world_min = vec3f(f32::INFINITY, f32::INFINITY, f32::INFINITY); + let mut world_max = vec3f(f32::NEG_INFINITY, f32::NEG_INFINITY, f32::NEG_INFINITY); + for corner in corners { + let Some(world) = depth_view_to_world(job, corner) else { + continue; + }; + world_min = Vec3f::min_componentwise(world_min, world); + world_max = Vec3f::max_componentwise(world_max, world); + } + (world_min.x.is_finite() + && world_min.y.is_finite() + && world_min.z.is_finite() + && world_max.x.is_finite() + && world_max.y.is_finite() + && world_max.z.is_finite()) + .then_some((world_min, world_max)) +} + +fn point_inside_player_exclusion(camera_world: Vec3f, world: Vec3f) -> bool { + let dx = world.x - camera_world.x; + let dz = world.z - camera_world.z; + let horizontal_sq = dx * dx + dz * dz; + let relative_y = world.y - camera_world.y; + horizontal_sq <= DEPTH_PLAYER_EXCLUDE_RADIUS_METERS * DEPTH_PLAYER_EXCLUDE_RADIUS_METERS + && relative_y >= DEPTH_PLAYER_EXCLUDE_MIN_RELATIVE_Y_METERS + && relative_y <= DEPTH_PLAYER_EXCLUDE_MAX_RELATIVE_Y_METERS +} + +#[cfg(test)] +mod tests { + use super::*; + + #[test] + fn player_exclusion_band_leaves_floor_and_ceiling_intact() { + let camera_world = vec3f(0.0, 1.6, 0.0); + + assert!(point_inside_player_exclusion( + camera_world, + vec3f(0.0, 1.0, 0.0) + )); + assert!(!point_inside_player_exclusion( + camera_world, + vec3f(0.0, 0.0, 0.0) + )); + assert!(!point_inside_player_exclusion( + camera_world, + vec3f(0.0, 2.3, 0.0) + )); + assert!(!point_inside_player_exclusion( + camera_world, + vec3f(0.5, 1.0, 0.0) + )); + } +} + +fn voxel_center_axis(voxel_size: f32, coord: i32) -> f32 { + (coord as f32 + 0.5) * voxel_size +} + +fn query_grid_bilinear_distance_at_y( + volume: &DepthMeshVolume, + sample_x: f32, + sample_z: f32, + y_coord: i32, +) -> Option { + let voxel_size = volume.voxel_size_meters; + let grid_x = sample_x / voxel_size - 0.5; + let grid_z = sample_z / voxel_size - 0.5; + let x0 = grid_x.floor() as i32; + let z0 = grid_z.floor() as i32; + let tx = grid_x - x0 as f32; + let tz = grid_z - z0 as f32; + + let v00 = volume + .mesh_grid + .normalized_distance(VoxelCoord::new(x0, y_coord, z0))?; + let v10 = volume + .mesh_grid + .normalized_distance(VoxelCoord::new(x0 + 1, y_coord, z0))?; + let v01 = volume + .mesh_grid + .normalized_distance(VoxelCoord::new(x0, y_coord, z0 + 1))?; + let v11 = volume + .mesh_grid + .normalized_distance(VoxelCoord::new(x0 + 1, y_coord, z0 + 1))?; + + let vx0 = v00 + (v10 - v00) * tx; + let vx1 = v01 + (v11 - v01) * tx; + Some(vx0 + (vx1 - vx0) * tz) +} diff --git a/libs/tsdf/src/lib.rs b/libs/tsdf/src/lib.rs new file mode 100644 index 0000000000..7aac6ec189 --- /dev/null +++ b/libs/tsdf/src/lib.rs @@ -0,0 +1,594 @@ +pub use makepad_math; + +use makepad_math::{vec3f, Vec2f, Vec3f}; +use makepad_micro_serde::*; +use std::{ + collections::HashMap, + sync::{ + atomic::{AtomicBool, AtomicU32, AtomicU64, Ordering}, + Arc, Mutex, OnceLock, RwLock, + }, + time::Instant, +}; + +pub const XR_TSDF_DEFAULT_VOXEL_SIZE_METERS: f32 = 0.03; + +#[derive(Clone, Copy, Debug, Default, PartialEq, Eq, Hash)] +pub struct ChunkKey { + pub x: i32, + pub y: i32, + pub z: i32, +} + +impl ChunkKey { + pub const fn new(x: i32, y: i32, z: i32) -> Self { + Self { x, y, z } + } +} + +#[derive(Clone, Debug, Default)] +pub struct XrTsdfStats { + pub frames_seen: u64, + pub frames_meshed: u64, + pub frames_dropped: u64, +} + +#[derive(Clone, Debug, Default)] +pub struct XrTsdfState { + pub stats: XrTsdfStats, + pub last_error: Option, +} + +#[derive(Clone, Debug, Default)] +pub struct SparseTsdReadChunk { + pub values: Vec, + pub valid_bits: Vec, + pub metadata: Vec, +} + +impl SparseTsdReadChunk { + const NORMALIZED_DISTANCE_ENCODE_SCALE: f32 = i16::MAX as f32; + const CONFIDENCE_BITS: u16 = 6; + const CONFIDENCE_MASK: u16 = (1u16 << Self::CONFIDENCE_BITS) - 1; + const GENERATION_MASK: u16 = (1u16 << (u16::BITS as u16 - Self::CONFIDENCE_BITS)) - 1; + const GENERATION_WRAP: u64 = Self::GENERATION_MASK as u64 + 1; + + pub fn new(chunk_volume: usize) -> Self { + Self { + values: vec![0; chunk_volume], + valid_bits: vec![0; Self::valid_word_count(chunk_volume)], + metadata: vec![0; chunk_volume], + } + } + + pub fn valid_word_count(chunk_volume: usize) -> usize { + (chunk_volume + u64::BITS as usize - 1) / u64::BITS as usize + } + + pub fn valid_bit_parts(id: usize) -> (usize, u64) { + let word_index = id / u64::BITS as usize; + let bit_mask = 1u64 << (id % u64::BITS as usize); + (word_index, bit_mask) + } + + pub fn is_valid_index(valid_bits: &[u64], id: usize) -> bool { + let (word_index, bit_mask) = Self::valid_bit_parts(id); + valid_bits + .get(word_index) + .copied() + .is_some_and(|word| (word & bit_mask) != 0) + } + + pub fn set_valid_index(valid_bits: &mut [u64], id: usize) -> bool { + let (word_index, bit_mask) = Self::valid_bit_parts(id); + let Some(word) = valid_bits.get_mut(word_index) else { + return false; + }; + let was_valid = (*word & bit_mask) != 0; + *word |= bit_mask; + was_valid + } + + pub fn encode_normalized_distance(value: f32) -> i16 { + (value.clamp(-1.0, 1.0) * Self::NORMALIZED_DISTANCE_ENCODE_SCALE).round() as i16 + } + + pub fn decode_normalized_distance(value: i16) -> f32 { + value as f32 / Self::NORMALIZED_DISTANCE_ENCODE_SCALE + } + + pub fn encode_generation_tag(generation: u64) -> u16 { + generation as u16 & Self::GENERATION_MASK + } + + pub fn decode_generation_tag(tag: u16, current_generation: u64) -> u64 { + let generation_mask = Self::GENERATION_MASK as u64; + let base = current_generation & !generation_mask; + let candidate = base | tag as u64; + if candidate > current_generation { + candidate.saturating_sub(Self::GENERATION_WRAP) + } else { + candidate + } + } + + pub(crate) fn encode_metadata(confidence: u8, observed_generation: u64) -> u16 { + let confidence = confidence.min(Self::CONFIDENCE_MASK as u8) as u16; + let generation = Self::encode_generation_tag(observed_generation); + (generation << Self::CONFIDENCE_BITS) | confidence + } + + pub(crate) fn decode_metadata_confidence(metadata: u16) -> u8 { + (metadata & Self::CONFIDENCE_MASK) as u8 + } + + pub(crate) fn decode_metadata_generation_tag(metadata: u16) -> u16 { + metadata >> Self::CONFIDENCE_BITS + } + + pub fn value(&self, id: usize) -> Option { + if !Self::is_valid_index(&self.valid_bits, id) { + None + } else { + self.values + .get(id) + .copied() + .map(Self::decode_normalized_distance) + } + } + + pub fn confidence(&self, id: usize) -> u8 { + if !Self::is_valid_index(&self.valid_bits, id) { + 0 + } else { + self.metadata + .get(id) + .copied() + .map(Self::decode_metadata_confidence) + .unwrap_or(0) + } + } + + pub fn observed_generation(&self, id: usize, current_generation: u64) -> u64 { + let tag = self + .metadata + .get(id) + .copied() + .map(Self::decode_metadata_generation_tag) + .unwrap_or(0); + Self::decode_generation_tag(tag, current_generation) + } + + pub fn set_value(&mut self, id: usize, value: f32, confidence: u8, observed_generation: u64) { + if id >= self.values.len() || id >= self.metadata.len() { + return; + } + self.values[id] = Self::encode_normalized_distance(value); + Self::set_valid_index(&mut self.valid_bits, id); + self.metadata[id] = Self::encode_metadata(confidence, observed_generation); + } + + pub fn heap_bytes(&self) -> u64 { + (self.values.capacity() * std::mem::size_of::() + + self.valid_bits.capacity() * std::mem::size_of::() + + self.metadata.capacity() * std::mem::size_of::()) as u64 + } +} + +#[derive(Clone, Debug, Default)] +pub struct SparseTsdGridReadSnapshot { + pub voxel_size: f32, + pub chunk_edge: i32, + pub chunk_edge_shift: Option, + pub chunk_edge_mask: i32, + pub chunk_volume: usize, + pub active_value_count: usize, + pub active_bounds: Option<(Vec3f, Vec3f)>, + pub chunks: HashMap>, +} + +impl SparseTsdGridReadSnapshot { + pub fn is_empty(&self) -> bool { + self.active_value_count == 0 + } + + pub fn chunk_count(&self) -> usize { + self.chunks.len() + } + + pub fn world_bounds(&self, padding_voxels: i32) -> Option<(Vec3f, Vec3f)> { + let (min, max) = self.active_bounds?; + let padding = padding_voxels as f32 * self.voxel_size; + Some(( + vec3f(min.x - padding, min.y - padding, min.z - padding), + vec3f(max.x + padding, max.y + padding, max.z + padding), + )) + } + + pub fn world_to_voxel_xyz(&self, point: Vec3f) -> (i32, i32, i32) { + ( + (point.x / self.voxel_size).floor() as i32, + (point.y / self.voxel_size).floor() as i32, + (point.z / self.voxel_size).floor() as i32, + ) + } + + pub fn voxel_center_world_xyz(&self, x: i32, y: i32, z: i32) -> Vec3f { + vec3f( + (x as f32 + 0.5) * self.voxel_size, + (y as f32 + 0.5) * self.voxel_size, + (z as f32 + 0.5) * self.voxel_size, + ) + } + + #[inline] + pub fn chunk_axis_and_local(&self, value: i32) -> (i32, usize) { + if let Some(shift) = self.chunk_edge_shift { + let mask = self.chunk_edge_mask; + (value >> shift, (value & mask.max(0)) as usize) + } else { + ( + value.div_euclid(self.chunk_edge), + value.rem_euclid(self.chunk_edge) as usize, + ) + } + } + + #[inline] + pub fn chunk_key_and_local_index_xyz(&self, x: i32, y: i32, z: i32) -> (ChunkKey, usize) { + let (chunk_x, lx) = self.chunk_axis_and_local(x); + let (chunk_y, ly) = self.chunk_axis_and_local(y); + let (chunk_z, lz) = self.chunk_axis_and_local(z); + let edge = self.chunk_edge as usize; + let local_id = lx + ly * edge + lz * edge * edge; + (ChunkKey::new(chunk_x, chunk_y, chunk_z), local_id) + } + + pub fn normalized_distance_xyz(&self, x: i32, y: i32, z: i32) -> Option { + let (chunk_key, local_id) = self.chunk_key_and_local_index_xyz(x, y, z); + let chunk = self.chunks.get(&chunk_key)?; + chunk.value(local_id) + } + + pub fn confidence_xyz(&self, x: i32, y: i32, z: i32) -> u8 { + let (chunk_key, local_id) = self.chunk_key_and_local_index_xyz(x, y, z); + self.chunks + .get(&chunk_key) + .map(|chunk| chunk.confidence(local_id)) + .unwrap_or(0) + } + + pub fn heap_bytes(&self) -> u64 { + let chunk_bytes = self + .chunks + .values() + .map(|chunk| chunk.heap_bytes()) + .sum::(); + let table_bytes = self.chunks.capacity() as u64 + * std::mem::size_of::<(ChunkKey, Arc)>() as u64; + chunk_bytes + table_bytes + } +} + +#[derive(Clone, Debug, Default, PartialEq, SerBin, DeBin)] +pub struct XrDepthAlignHeightMap { + pub origin_x: f32, + pub origin_z: f32, + pub cell_size_meters: f32, + pub size_x: u16, + pub size_z: u16, + pub bottom_y_meters: f32, + pub top_y_meters: f32, + pub floor_y_meters: f32, + pub player_cutout_center: Option, + pub player_cutout_radius_meters: f32, + pub heights_meters: Vec, +} + +impl XrDepthAlignHeightMap { + pub fn cell_count(&self) -> usize { + self.size_x as usize * self.size_z as usize + } + + pub fn size_x_usize(&self) -> usize { + self.size_x as usize + } + + pub fn size_z_usize(&self) -> usize { + self.size_z as usize + } + + pub fn extent_x_meters(&self) -> f32 { + self.cell_size_meters * self.size_x.max(1) as f32 + } + + pub fn extent_z_meters(&self) -> f32 { + self.cell_size_meters * self.size_z.max(1) as f32 + } + + pub fn cell_index(&self, x: usize, z: usize) -> usize { + x + z * self.size_x_usize() + } + + pub fn is_empty(&self) -> bool { + self.size_x == 0 + || self.size_z == 0 + || self.heights_meters.len() != self.cell_count() + || self.heights_meters.iter().all(|value| !value.is_finite()) + } +} + +#[derive(Clone, Debug, Default)] +pub struct TsdfPublishedSnapshot { + pub generation: u64, + pub latest_topology_generation: u64, + pub update_sequence: u64, + pub grid: Arc, + pub height_map: Option, +} + +#[derive(Clone, Copy, Debug, Default, PartialEq, Eq)] +pub struct XrTsdfCooperativeStepResult { + pub did_work: bool, + pub has_more_work: bool, + pub completed_cycle: bool, +} + +#[derive(Clone, Copy, Debug, Default, PartialEq, Eq)] +pub struct XrTsdfCooperativeStepStats { + pub callback_registered: bool, + pub has_more_work: bool, + pub last_step_micros: u64, + pub average_step_micros: u64, + pub last_cycle_compute_micros: u64, + pub average_cycle_compute_micros: u64, + pub last_cycle_micros: u64, + pub average_cycle_micros: u64, + pub current_cycle_steps: u32, + pub completed_cycles: u64, +} + +type XrTsdfCooperativeStepCallback = + Box XrTsdfCooperativeStepResult + Send + 'static>; + +#[derive(Default)] +struct XrTsdfCooperativeStepSlot { + callback: Option, + stats: XrTsdfCooperativeStepStats, + current_cycle_compute_micros: u64, + current_cycle_started_at: Option, +} + +impl XrTsdfCooperativeStepSlot { + fn reset_cycle(&mut self) { + self.current_cycle_compute_micros = 0; + self.current_cycle_started_at = None; + self.stats.current_cycle_steps = 0; + self.stats.has_more_work = false; + } +} + +#[derive(Clone)] +pub struct XrTsdfStore { + state: Arc>, + published_snapshot: Arc>>>, + reset_generation: Arc, + surface_analysis_enabled: Arc, + voxel_size_meters_bits: Arc, + cooperative_step: Arc>, + cooperative_step_stats_cache: Arc>, +} + +impl Default for XrTsdfStore { + fn default() -> Self { + Self { + state: Arc::new(RwLock::new(XrTsdfState::default())), + published_snapshot: Arc::new(RwLock::new(None)), + reset_generation: Arc::new(AtomicU64::new(0)), + surface_analysis_enabled: Arc::new(AtomicBool::new(false)), + voxel_size_meters_bits: Arc::new(AtomicU32::new( + XR_TSDF_DEFAULT_VOXEL_SIZE_METERS.to_bits(), + )), + cooperative_step: Arc::new(Mutex::new(XrTsdfCooperativeStepSlot::default())), + cooperative_step_stats_cache: Arc::new(RwLock::new( + XrTsdfCooperativeStepStats::default(), + )), + } + } +} + +#[allow(dead_code)] +impl XrTsdfStore { + pub fn set_cooperative_step_callback(&self, callback: Option) { + if let Ok(mut slot) = self.cooperative_step.lock() { + slot.callback = callback; + slot.stats.callback_registered = slot.callback.is_some(); + if slot.callback.is_none() { + slot.reset_cycle(); + slot.stats.last_step_micros = 0; + slot.stats.average_step_micros = 0; + slot.stats.last_cycle_compute_micros = 0; + slot.stats.average_cycle_compute_micros = 0; + slot.stats.last_cycle_micros = 0; + slot.stats.average_cycle_micros = 0; + slot.stats.completed_cycles = 0; + } + if let Ok(mut stats_cache) = self.cooperative_step_stats_cache.write() { + *stats_cache = slot.stats; + } + } + } + + pub fn cooperative_step_stats(&self) -> XrTsdfCooperativeStepStats { + self.cooperative_step_stats_cache + .read() + .map(|stats| *stats) + .unwrap_or_default() + } + + pub fn set_voxel_size_meters(&self, voxel_size_meters: f32) -> f32 { + let voxel_size_meters = voxel_size_meters.clamp(0.03, 0.10); + let previous = self + .voxel_size_meters_bits + .swap(voxel_size_meters.to_bits(), Ordering::AcqRel); + if previous != voxel_size_meters.to_bits() { + self.clear(); + } + voxel_size_meters + } + + pub fn voxel_size_meters(&self) -> f32 { + f32::from_bits(self.voxel_size_meters_bits.load(Ordering::Acquire)) + } + + pub fn reset_generation(&self) -> u64 { + self.reset_generation.load(Ordering::Acquire) + } + + pub fn request_reset(&self) -> u64 { + let generation = self.reset_generation.fetch_add(1, Ordering::AcqRel) + 1; + self.clear(); + generation + } + + pub fn set_surface_analysis_enabled(&self, enabled: bool) { + self.surface_analysis_enabled + .store(enabled, Ordering::Release); + } + + pub fn surface_analysis_enabled(&self) -> bool { + self.surface_analysis_enabled.load(Ordering::Acquire) + } + + pub fn state(&self) -> Arc> { + self.state.clone() + } + + pub fn latest_tsdf_snapshot(&self) -> Option> { + self.published_snapshot + .read() + .ok() + .and_then(|snapshot| snapshot.clone()) + } + + pub fn record_seen(&self) { + if let Ok(mut state) = self.state.write() { + state.stats.frames_seen += 1; + } + } + + pub fn record_drop(&self) { + if let Ok(mut state) = self.state.write() { + state.stats.frames_dropped += 1; + } + } + + pub fn set_error(&self, error: String) { + if let Ok(mut state) = self.state.write() { + state.last_error = Some(error); + } + } + + pub fn publish_tsdf_snapshot(&self, snapshot: TsdfPublishedSnapshot) { + if let Ok(mut published_snapshot) = self.published_snapshot.write() { + *published_snapshot = Some(Arc::new(snapshot)); + } + if let Ok(mut state) = self.state.write() { + state.stats.frames_meshed += 1; + state.last_error = None; + } + } + + pub fn clear(&self) { + if let Ok(mut state) = self.state.write() { + state.last_error = None; + state.stats = XrTsdfStats::default(); + } + if let Ok(mut published_snapshot) = self.published_snapshot.write() { + *published_snapshot = None; + } + if let Ok(mut slot) = self.cooperative_step.lock() { + slot.reset_cycle(); + if let Ok(mut stats_cache) = self.cooperative_step_stats_cache.write() { + *stats_cache = slot.stats; + } + } + } + + pub fn run_cooperative_step(&self) -> Option { + let mut slot = self.cooperative_step.lock().ok()?; + let callback = slot.callback.as_mut()?; + let started = Instant::now(); + let result = callback(); + let elapsed_micros = started.elapsed().as_micros().min(u64::MAX as u128) as u64; + slot.stats.callback_registered = true; + slot.stats.last_step_micros = elapsed_micros; + slot.stats.average_step_micros = + ema_u64(slot.stats.average_step_micros, elapsed_micros, 1, 4); + if result.did_work { + slot.current_cycle_started_at.get_or_insert(started); + slot.current_cycle_compute_micros = slot + .current_cycle_compute_micros + .saturating_add(elapsed_micros); + slot.stats.current_cycle_steps = slot.stats.current_cycle_steps.saturating_add(1); + } else if !result.has_more_work { + slot.reset_cycle(); + } + slot.stats.has_more_work = result.has_more_work; + if result.completed_cycle { + let cycle_wall_micros = slot + .current_cycle_started_at + .map(|cycle_started| { + cycle_started.elapsed().as_micros().min(u64::MAX as u128) as u64 + }) + .unwrap_or(slot.current_cycle_compute_micros); + slot.stats.last_cycle_compute_micros = slot.current_cycle_compute_micros; + slot.stats.average_cycle_compute_micros = ema_u64( + slot.stats.average_cycle_compute_micros, + slot.current_cycle_compute_micros, + 1, + 4, + ); + slot.stats.last_cycle_micros = cycle_wall_micros; + slot.stats.average_cycle_micros = + ema_u64(slot.stats.average_cycle_micros, cycle_wall_micros, 1, 4); + slot.stats.completed_cycles = slot.stats.completed_cycles.saturating_add(1); + slot.reset_cycle(); + } + if let Ok(mut stats_cache) = self.cooperative_step_stats_cache.write() { + *stats_cache = slot.stats; + } + Some(result) + } +} + +fn ema_u64(current: u64, sample: u64, numerator: u64, denominator: u64) -> u64 { + if denominator == 0 { + return sample; + } + if current == 0 { + sample + } else { + current + .saturating_mul(denominator.saturating_sub(numerator)) + .saturating_add(sample.saturating_mul(numerator)) + / denominator + } +} + +pub fn xr_tsdf_store() -> XrTsdfStore { + static STORE: OnceLock = OnceLock::new(); + STORE.get_or_init(XrTsdfStore::default).clone() +} + +#[allow(dead_code)] +pub fn empty_bounds() -> (Vec3f, Vec3f) { + (vec3f(0.0, 0.0, 0.0), vec3f(0.0, 0.0, 0.0)) +} + +pub mod depth_integration; +pub use depth_integration::*; + +#[cfg(test)] +#[path = "synthetic_bench.rs"] +mod synthetic_bench; diff --git a/libs/tsdf/src/synthetic_bench.rs b/libs/tsdf/src/synthetic_bench.rs new file mode 100644 index 0000000000..c0e6bd535e --- /dev/null +++ b/libs/tsdf/src/synthetic_bench.rs @@ -0,0 +1,291 @@ +use crate::{ + apply_preprocessed_depth_mesh, depth_ndc_to_world_ray, makepad_math, preprocess_depth_mesh, + DepthMeshJob, DepthMeshVolume, DepthPreprocessWorkerState, TsdfPublishedSnapshot, + DEPTH_VOXEL_MAX_DEPTH_VALUE, DEPTH_VOXEL_MAX_DISTANCE_METERS, DEPTH_VOXEL_MIN_DEPTH_VALUE, + DEPTH_VOXEL_MIN_DISTANCE_METERS, +}; +use makepad_math::{vec3f, vec4f, CameraFov, Mat4f, Vec3f}; +use std::sync::Arc; + +#[derive(Clone, Copy, Debug)] +struct SyntheticTsdfBenchStats { + voxel_size_meters: f32, + frame_count: usize, + build_ms_per_frame: f64, + preprocess_ms_per_frame: f64, + apply_ms_per_frame: f64, + publish_ms_per_frame: f64, + staged_samples_per_frame: f64, + reduced_samples_per_frame: f64, + chunk_count: usize, + active_voxel_count: usize, + heap_mib: f64, +} + +#[derive(Clone, Copy)] +struct SyntheticBox { + min: Vec3f, + max: Vec3f, +} + +impl SyntheticBox { + const fn new(min: Vec3f, max: Vec3f) -> Self { + Self { min, max } + } + + fn hit_distance(self, origin: Vec3f, direction: Vec3f) -> Option { + let mut t_min = f32::NEG_INFINITY; + let mut t_max = f32::INFINITY; + for (origin_axis, direction_axis, min_axis, max_axis) in [ + (origin.x, direction.x, self.min.x, self.max.x), + (origin.y, direction.y, self.min.y, self.max.y), + (origin.z, direction.z, self.min.z, self.max.z), + ] { + if direction_axis.abs() <= 1.0e-6 { + if origin_axis < min_axis || origin_axis > max_axis { + return None; + } + continue; + } + let inv_direction = direction_axis.recip(); + let t0 = (min_axis - origin_axis) * inv_direction; + let t1 = (max_axis - origin_axis) * inv_direction; + t_min = t_min.max(t0.min(t1)); + t_max = t_max.min(t0.max(t1)); + if t_max < t_min { + return None; + } + } + if !t_max.is_finite() || t_max < 0.0 { + return None; + } + let entry = t_min.max(0.0); + (entry.is_finite() && entry <= t_max).then_some(entry) + } +} + +fn synthetic_scene_boxes() -> Vec { + vec![ + SyntheticBox::new(vec3f(-2.60, -0.10, -5.20), vec3f(2.60, 0.0, 0.70)), + SyntheticBox::new(vec3f(-2.60, 2.45, -5.20), vec3f(2.60, 2.55, 0.70)), + SyntheticBox::new(vec3f(-2.60, 0.0, -5.20), vec3f(-2.50, 2.55, 0.70)), + SyntheticBox::new(vec3f(2.50, 0.0, -5.20), vec3f(2.60, 2.55, 0.70)), + SyntheticBox::new(vec3f(-2.60, 0.0, -5.20), vec3f(2.60, 2.55, -5.10)), + SyntheticBox::new(vec3f(-0.45, 0.0, -2.65), vec3f(0.45, 1.15, -1.85)), + SyntheticBox::new(vec3f(1.15, 0.0, -3.55), vec3f(1.85, 1.65, -2.95)), + SyntheticBox::new(vec3f(-1.85, 0.0, -4.20), vec3f(-1.25, 0.95, -3.55)), + ] +} + +fn synthetic_camera_world(frame_index: usize, frame_count: usize) -> Vec3f { + let t = if frame_count <= 1 { + 0.0 + } else { + frame_index as f32 / (frame_count - 1) as f32 + }; + let x = (t * std::f32::consts::TAU * 1.35).sin() * 0.28; + let y = 1.48 + (t * std::f32::consts::TAU * 2.10).sin() * 0.03; + let z = 0.18 - t * 0.72; + vec3f(x, y, z) +} + +fn synthetic_depth_job( + scene_boxes: &[SyntheticBox], + width: u32, + height: u32, + generation: u64, + voxel_size_meters: f32, + camera_world: Vec3f, +) -> DepthMeshJob { + let depth_proj = Mat4f::from_camera_fov( + &CameraFov { + angle_left: -0.92, + angle_right: 0.92, + angle_up: 0.72, + angle_down: -0.72, + }, + 0.05, + 8.0, + ); + let inv_depth_proj = depth_proj.invert(); + let world_from_depth_view = Mat4f::translation(camera_world); + let depth_view_from_world = world_from_depth_view.invert(); + let depth = render_synthetic_depth( + scene_boxes, + width, + height, + camera_world, + depth_proj, + inv_depth_proj, + world_from_depth_view, + depth_view_from_world, + ); + DepthMeshJob { + reset_generation: 0, + generation, + eye_index: 0, + width, + height, + voxel_size_meters, + camera_world, + depth_proj, + inv_depth_proj, + depth_view_from_world, + world_from_depth_view, + depth, + } +} + +fn render_synthetic_depth( + scene_boxes: &[SyntheticBox], + width: u32, + height: u32, + camera_world: Vec3f, + depth_proj: Mat4f, + inv_depth_proj: Mat4f, + world_from_depth_view: Mat4f, + depth_view_from_world: Mat4f, +) -> Vec { + let mut depth = vec![0u16; width as usize * height as usize]; + for y in 0..height as usize { + for x in 0..width as usize { + let uv_x = (x as f32 + 0.5) / width as f32; + let uv_y = (y as f32 + 0.5) / height as f32; + let ndc_x = uv_x * 2.0 - 1.0; + let ndc_y = uv_y * 2.0 - 1.0; + let Some(ray_direction) = + depth_ndc_to_world_ray(inv_depth_proj, world_from_depth_view, ndc_x, ndc_y) + else { + continue; + }; + let nearest = scene_boxes + .iter() + .filter_map(|wall| wall.hit_distance(camera_world, ray_direction)) + .filter(|distance| { + distance.is_finite() + && *distance >= DEPTH_VOXEL_MIN_DISTANCE_METERS + && *distance <= DEPTH_VOXEL_MAX_DISTANCE_METERS + }) + .min_by(|left, right| left.partial_cmp(right).unwrap_or(std::cmp::Ordering::Equal)); + let Some(hit_distance) = nearest else { + continue; + }; + let world = camera_world + ray_direction.scale(hit_distance); + let view = depth_view_from_world.transform_vec4(vec4f(world.x, world.y, world.z, 1.0)); + if !view.w.is_finite() || view.w.abs() < 1.0e-6 { + continue; + } + let view = vec4f(view.x / view.w, view.y / view.w, view.z / view.w, 1.0); + let clip = depth_proj.transform_vec4(view); + if !clip.w.is_finite() || clip.w.abs() < 1.0e-6 { + continue; + } + let ndc_z = clip.z / clip.w; + let raw_depth = (ndc_z * 0.5 + 0.5).clamp( + DEPTH_VOXEL_MIN_DEPTH_VALUE, + DEPTH_VOXEL_MAX_DEPTH_VALUE - 1.0 / u16::MAX as f32, + ); + depth[y * width as usize + x] = (raw_depth * u16::MAX as f32).round() as u16; + } + } + depth +} + +fn run_synthetic_tsdf_benchmark_case( + voxel_size_meters: f32, + width: u32, + height: u32, + frame_count: usize, +) -> SyntheticTsdfBenchStats { + let scene_boxes = synthetic_scene_boxes(); + let mut worker_state = DepthPreprocessWorkerState::default(); + let mut volume = DepthMeshVolume::new(voxel_size_meters); + let mut previous_snapshot = None::>; + + let mut depth_build_total = std::time::Duration::ZERO; + let mut preprocess_total = std::time::Duration::ZERO; + let mut apply_total = std::time::Duration::ZERO; + let mut publish_total = std::time::Duration::ZERO; + let mut total_staged_samples = 0usize; + let mut total_reduced_samples = 0usize; + + for frame_index in 0..frame_count { + let camera_world = synthetic_camera_world(frame_index, frame_count); + + let started = std::time::Instant::now(); + let job = synthetic_depth_job( + &scene_boxes, + width, + height, + frame_index as u64 + 1, + voxel_size_meters, + camera_world, + ); + depth_build_total += started.elapsed(); + + let started = std::time::Instant::now(); + let prepared = preprocess_depth_mesh(job, &mut worker_state) + .expect("synthetic depth preprocess should succeed"); + preprocess_total += started.elapsed(); + total_staged_samples += worker_state.staged_tsd_sample_visits(); + total_reduced_samples += worker_state.reduced_tsd_sample_count(); + + let started = std::time::Instant::now(); + apply_preprocessed_depth_mesh(prepared, &worker_state, &mut volume); + volume.discard_obsolete_surface_state(); + apply_total += started.elapsed(); + + let started = std::time::Instant::now(); + let snapshot = volume.published_tsdf_snapshot(previous_snapshot.as_deref()); + publish_total += started.elapsed(); + volume.clear_published_tsdf_dirty_state(); + previous_snapshot = Some(Arc::new(snapshot)); + } + + let snapshot = previous_snapshot.expect("synthetic benchmark should publish a snapshot"); + let frames = frame_count as f64; + SyntheticTsdfBenchStats { + voxel_size_meters, + frame_count, + build_ms_per_frame: depth_build_total.as_secs_f64() * 1000.0 / frames, + preprocess_ms_per_frame: preprocess_total.as_secs_f64() * 1000.0 / frames, + apply_ms_per_frame: apply_total.as_secs_f64() * 1000.0 / frames, + publish_ms_per_frame: publish_total.as_secs_f64() * 1000.0 / frames, + staged_samples_per_frame: total_staged_samples as f64 / frames, + reduced_samples_per_frame: total_reduced_samples as f64 / frames, + chunk_count: snapshot.grid.chunk_count(), + active_voxel_count: snapshot.grid.active_value_count, + heap_mib: snapshot.grid.heap_bytes() as f64 / (1024.0 * 1024.0), + } +} + +#[test] +#[ignore = "Synthetic TSDF perf benchmark; run with cargo test --release -- --ignored --nocapture"] +fn synthetic_depthmap_tsdf_perf_release() { + let width = 1280; + let height = 1280; + let frame_count = 6usize; + + for voxel_size_meters in [0.03, 0.05, 0.10] { + let stats = + run_synthetic_tsdf_benchmark_case(voxel_size_meters, width, height, frame_count); + eprintln!( + "synthetic_tsdf voxel={:.02} frames={} build_ms/frame={:.3} preprocess_ms/frame={:.3} apply_ms/frame={:.3} publish_ms/frame={:.3} staged/frame={:.0} reduced/frame={:.0} chunks={} active_voxels={} heap_mib={:.3}", + stats.voxel_size_meters, + stats.frame_count, + stats.build_ms_per_frame, + stats.preprocess_ms_per_frame, + stats.apply_ms_per_frame, + stats.publish_ms_per_frame, + stats.staged_samples_per_frame, + stats.reduced_samples_per_frame, + stats.chunk_count, + stats.active_voxel_count, + stats.heap_mib, + ); + + assert!(stats.active_voxel_count > 0); + assert!(stats.chunk_count > 0); + assert!(stats.reduced_samples_per_frame > 256.0); + } +} diff --git a/libs/voice2/Cargo.toml b/libs/voice2/Cargo.toml new file mode 100644 index 0000000000..9c3ad8262d --- /dev/null +++ b/libs/voice2/Cargo.toml @@ -0,0 +1,26 @@ +[package] +name = "makepad-voice2" +version = "0.1.0" +edition = "2021" +description = "Baseline copy of makepad-voice for ggml migration" +license = "MIT" + +[features] +default = [] +metal-precompile = [] + +[dependencies] +makepad-ggml = { path = "../ggml" } + +[target.'cfg(any(target_os = "macos", target_os = "ios"))'.dependencies] +makepad-objc-sys = { path = "../objc-sys", version = "1.0.0" } + +[workspace] + +[[bin]] +name = "whisper-test" +path = "src/bin/whisper_test.rs" + +[[bin]] +name = "apple-speech-test" +path = "src/bin/apple_speech_test.rs" diff --git a/libs/voice2/build.rs b/libs/voice2/build.rs new file mode 100644 index 0000000000..6b6e8a0939 --- /dev/null +++ b/libs/voice2/build.rs @@ -0,0 +1,352 @@ +use std::env; +use std::fs; +use std::process::Command; + +const IOS_DEPLOYMENT_TARGET_DEFAULT: &str = "26.0"; + +fn main() { + let target_os = env::var("CARGO_CFG_TARGET_OS").unwrap_or_default(); + let host = env::var("HOST").unwrap_or_default(); + let is_apple_host = host.contains("apple-darwin"); + let mut force_whisper = env::var("MAKEPAD") + .ok() + .is_some_and(|configs| configs.split(['+', ',']).any(|config| config == "whisper")); + + println!("cargo:rustc-check-cfg=cfg(force_whisper)"); + println!("cargo:rerun-if-env-changed=MAKEPAD"); + println!("cargo:rerun-if-env-changed=MAKEPAD_VOICE_METAL_PRECOMPILE"); + println!("cargo:rerun-if-env-changed=IPHONEOS_DEPLOYMENT_TARGET"); + println!("cargo:rerun-if-env-changed=IPHONESIMULATOR_DEPLOYMENT_TARGET"); + + if target_os == "macos" { + build_whisper_metallib(); + } + + if !force_whisper && is_apple_host && (target_os == "macos" || target_os == "ios") { + if !build_speech_bridge(&target_os) { + force_whisper = true; + println!( + "cargo:warning=swift speech bridge unavailable; building makepad-voice2 with Whisper fallback" + ); + } + } + + if force_whisper { + println!("cargo:rustc-cfg=force_whisper"); + } +} + +fn build_whisper_metallib() { + let precompile_default = env::var_os("CARGO_FEATURE_METAL_PRECOMPILE").is_some(); + let precompile_enabled = env::var("MAKEPAD_VOICE_METAL_PRECOMPILE") + .ok() + .map(|v| { + let v = v.trim().to_ascii_lowercase(); + !(v.is_empty() || v == "0" || v == "false" || v == "no" || v == "off") + }) + .unwrap_or(precompile_default); + + let out_dir = env::var("OUT_DIR").unwrap(); + let manifest_dir = env::var("CARGO_MANIFEST_DIR").unwrap(); + + let ggml_src_dir = format!("{}/src/metal/ggml", manifest_dir); + let ggml_metal_dir = format!("{}/src/metal/ggml", manifest_dir); + + let metal_src = format!("{}/ggml-metal.metal", ggml_metal_dir); + let common_h = format!("{}/ggml-common.h", ggml_src_dir); + let impl_h = format!("{}/ggml-metal-impl.h", ggml_metal_dir); + + println!("cargo:rerun-if-changed={}", metal_src); + println!("cargo:rerun-if-changed={}", common_h); + println!("cargo:rerun-if-changed={}", impl_h); + + let _ = fs::create_dir_all(&out_dir); + let air_path = format!("{}/ggml-metal.air", out_dir); + let metallib_path = format!("{}/ggml-default.metallib", out_dir); + + if !precompile_enabled { + let _ = fs::write(&metallib_path, []); + println!( + "cargo:rustc-env=MAKEPAD_VOICE2_GGML_METALLIB={}", + metallib_path + ); + return; + } + + let metal_status = Command::new("xcrun") + .args([ + "--sdk", + "macosx", + "metal", + "-O3", + "-c", + &metal_src, + "-I", + &ggml_src_dir, + "-I", + &ggml_metal_dir, + "-o", + &air_path, + ]) + .status(); + + let ok = metal_status.as_ref().is_ok_and(|s| s.success()); + if !ok { + println!("cargo:warning=failed to compile ggml-metal.metal to AIR; runtime source compile will be used"); + let _ = fs::write(&metallib_path, []); + println!( + "cargo:rustc-env=MAKEPAD_VOICE2_GGML_METALLIB={}", + metallib_path + ); + return; + } + + let metallib_status = Command::new("xcrun") + .args([ + "--sdk", + "macosx", + "metallib", + &air_path, + "-o", + &metallib_path, + ]) + .status(); + + let ok = metallib_status.as_ref().is_ok_and(|s| s.success()); + if !ok { + println!("cargo:warning=failed to build ggml default metallib; runtime source compile will be used"); + let _ = fs::write(&metallib_path, []); + } + + println!( + "cargo:rustc-env=MAKEPAD_VOICE2_GGML_METALLIB={}", + metallib_path + ); +} + +fn build_speech_bridge(target_os: &str) -> bool { + let out_dir = env::var("OUT_DIR").unwrap(); + let manifest_dir = env::var("CARGO_MANIFEST_DIR").unwrap(); + let swift_src = format!("{}/swift/speech_bridge.swift", manifest_dir); + let swift_module_cache = format!("{}/swift_module_cache", out_dir); + + println!("cargo:rerun-if-changed=swift/speech_bridge.swift"); + let _ = fs::create_dir_all(&swift_module_cache); + + // Compile Swift -> static library + let output_lib = format!("{}/libspeech_bridge.a", out_dir); + let mut swift_args = vec![ + "-emit-library".to_string(), + "-static".to_string(), + "-parse-as-library".to_string(), + "-module-name".to_string(), + "speech_bridge".to_string(), + "-module-cache-path".to_string(), + swift_module_cache.clone(), + ]; + if target_os == "ios" { + if let Some((swift_target, sdk_path)) = ios_swift_target_and_sdk() { + swift_args.push("-target".to_string()); + swift_args.push(swift_target); + swift_args.push("-sdk".to_string()); + swift_args.push(sdk_path); + } + } + swift_args.push("-o".to_string()); + swift_args.push(output_lib.clone()); + swift_args.push(swift_src); + + let status = match Command::new("swiftc").args(swift_args).status() { + Ok(status) => status, + Err(err) => { + println!("cargo:warning=failed to run swiftc for speech bridge: {}", err); + return false; + } + }; + + if !status.success() { + println!("cargo:warning=swiftc compilation failed for speech bridge"); + return false; + } + + // Fix the @rpath issue with libswift_Concurrency.dylib BEFORE emitting any + // link-search paths, so our override directory appears first in -L order. + // + // Background: The macOS SDK's libswift_Concurrency.tbd contains $ld$previous + // entries that tell the linker to record "@rpath/libswift_Concurrency.dylib" + // as the install name when MACOSX_DEPLOYMENT_TARGET < 15.0. Rust defaults to + // MACOSX_DEPLOYMENT_TARGET=11.0, which triggers this behavior. The resulting + // binary then fails at runtime with "dyld: Library not loaded: @rpath/...". + // + // The fix: create modified copies of the .tbd files with $ld$previous entries + // stripped, and add them to the linker search path before the SDK paths. + // Since cargo:rustc-link-search propagates from library crates to dependent + // binaries, the final binary's link step will find our clean .tbd first and + // use the absolute install name "/usr/lib/swift/libswift_Concurrency.dylib". + if target_os == "macos" { + fix_swift_rpath_tbds(&out_dir); + } + + // Link the static library + println!("cargo:rustc-link-search=native={}", out_dir); + println!("cargo:rustc-link-lib=static=speech_bridge"); + + // On iOS, ensure the linker uses the same deployment target as the Swift objects. + // Without this, Rust's default target triple (arm64-apple-ios10.0.0) causes a + // mismatch and missing symbols like ___chkstk_darwin from the Swift async runtime. + if target_os == "ios" { + let deployment_key = { + let abi = env::var("CARGO_CFG_TARGET_ABI").unwrap_or_default(); + let arch = env::var("CARGO_CFG_TARGET_ARCH").unwrap_or_default(); + if abi == "sim" || arch == "x86_64" { + "IPHONESIMULATOR_DEPLOYMENT_TARGET" + } else { + "IPHONEOS_DEPLOYMENT_TARGET" + } + }; + let deployment = + env::var(deployment_key).unwrap_or_else(|_| IOS_DEPLOYMENT_TARGET_DEFAULT.to_string()); + println!("cargo:rustc-link-arg=-miphoneos-version-min={}", deployment); + } + + // Link Apple frameworks + println!("cargo:rustc-link-lib=framework=Speech"); + println!("cargo:rustc-link-lib=framework=Foundation"); + println!("cargo:rustc-link-lib=framework=AVFoundation"); + println!("cargo:rustc-link-lib=framework=CoreMedia"); + + // Add Swift runtime library search paths so the linker can resolve symbols. + let target_info = match Command::new("swiftc").args(["-print-target-info"]).output() { + Ok(output) => output, + Err(err) => { + println!("cargo:warning=failed to get swift target info: {}", err); + return true; + } + }; + + if target_info.status.success() { + let info_str = String::from_utf8_lossy(&target_info.stdout); + for line in info_str.lines() { + let trimmed = line.trim().trim_matches('"').trim_end_matches(','); + if trimmed.starts_with("/") && trimmed.contains("lib/swift") { + println!("cargo:rustc-link-search=native={}", trimmed); + } + } + } + + true +} + +fn ios_swift_target_and_sdk() -> Option<(String, String)> { + let arch = env::var("CARGO_CFG_TARGET_ARCH").ok()?; + let abi = env::var("CARGO_CFG_TARGET_ABI").unwrap_or_default(); + let is_simulator = abi == "sim" || arch == "x86_64"; + let swift_arch = match arch.as_str() { + "aarch64" => "arm64", + "x86_64" => "x86_64", + _ => return None, + }; + let deployment_key = if is_simulator { + "IPHONESIMULATOR_DEPLOYMENT_TARGET" + } else { + "IPHONEOS_DEPLOYMENT_TARGET" + }; + let deployment = + env::var(deployment_key).unwrap_or_else(|_| IOS_DEPLOYMENT_TARGET_DEFAULT.to_string()); + let swift_target = if is_simulator { + format!("{swift_arch}-apple-ios{deployment}-simulator") + } else { + format!("{swift_arch}-apple-ios{deployment}") + }; + let sdk_name = if is_simulator { + "iphonesimulator" + } else { + "iphoneos" + }; + let sdk_path = Command::new("xcrun") + .args(["--sdk", sdk_name, "--show-sdk-path"]) + .output() + .ok() + .filter(|o| o.status.success()) + .map(|o| String::from_utf8_lossy(&o.stdout).trim().to_string())?; + Some((swift_target, sdk_path)) +} + +/// Create modified copies of Swift runtime .tbd files without $ld$previous entries. +/// +/// The $ld$previous entries in the SDK's .tbd files cause the linker to use +/// @rpath/ as the install name for deployment targets below certain thresholds. +/// By stripping these entries and placing our modified .tbd in a search directory +/// that comes before the SDK, the linker will use the actual absolute install +/// names (e.g. "/usr/lib/swift/libswift_Concurrency.dylib") regardless of the +/// deployment target. +fn fix_swift_rpath_tbds(out_dir: &str) { + let sdk_path = Command::new("xcrun") + .args(&["--show-sdk-path"]) + .output() + .ok() + .filter(|o| o.status.success()) + .map(|o| String::from_utf8_lossy(&o.stdout).trim().to_string()); + + let sdk_path = match sdk_path { + Some(p) => p, + None => return, + }; + + let override_dir = format!("{}/swift_tbd_override", out_dir); + if fs::create_dir_all(&override_dir).is_err() { + return; + } + + // Only patch the specific Swift runtime .tbd files our static library depends on. + let swift_tbd_dir = format!("{}/usr/lib/swift", sdk_path); + let tbds_to_fix = [ + "libswift_Concurrency.tbd", + "libswiftCore.tbd", + "libswiftFoundation.tbd", + "libswift_StringProcessing.tbd", + "libswift_RegexParser.tbd", + ]; + + for name in &tbds_to_fix { + let tbd_path = format!("{}/{}", swift_tbd_dir, name); + let content = match fs::read_to_string(&tbd_path) { + Ok(c) => c, + Err(_) => continue, + }; + if !content.contains("$ld$previous$@rpath/") { + continue; + } + let fixed = strip_ld_previous_rpath(&content); + let _ = fs::write(format!("{}/{}", override_dir, name), &fixed); + } + + // Emit this search path BEFORE any other Swift library search paths. + // cargo:rustc-link-search propagates from library crates to dependent binaries, + // so the final binary's linker will find our modified .tbd files first. + println!("cargo:rustc-link-search=native={}", override_dir); +} + +/// Strip $ld$previous entries that reference @rpath/ from a .tbd file's content. +fn strip_ld_previous_rpath(content: &str) -> String { + let mut result = content.to_string(); + + // $ld$previous entries appear as quoted strings in YAML: + // '$ld$previous$@rpath/libswift_Concurrency.dylib$$1$10.9$12.0$$' + // They may be followed by comma + whitespace in a YAML sequence. + while let Some(start) = result.find("'$ld$previous$@rpath/") { + if let Some(end_quote_offset) = result[start + 1..].find('\'') { + let end = start + 1 + end_quote_offset + 1; // past closing quote + // Skip trailing comma and whitespace/newlines + let rest = &result[end..]; + let trimmed = + rest.trim_start_matches(|c: char| c == ',' || c == ' ' || c == '\n' || c == '\r'); + let skip = rest.len() - trimmed.len(); + result = format!("{}{}", &result[..start], &result[end + skip..]); + } else { + break; + } + } + + result +} diff --git a/libs/voice2/src/apple/speech.rs b/libs/voice2/src/apple/speech.rs new file mode 100644 index 0000000000..8336eb9435 --- /dev/null +++ b/libs/voice2/src/apple/speech.rs @@ -0,0 +1,90 @@ +use crate::decode_loop::{Segment, WhisperParams}; +use std::ffi::{c_void, CStr, CString}; +use std::os::raw::c_char; + +/// C-compatible segment layout matching Swift CSegment struct. +/// Must stay in sync with speech_bridge.swift. +#[repr(C)] +struct CSegment { + text: *mut c_char, + start_ms: i64, + end_ms: i64, +} + +extern "C" { + /// Swift @_cdecl uses OpaquePointer for the segments pointer, + /// which maps to void* in C. We cast on the Rust side. + fn apple_speech_transcribe( + samples: *const f32, + sample_count: i64, + lang: *const c_char, + out_count: *mut i32, + out_segments: *mut *mut c_void, + ) -> i32; + + fn apple_speech_free_segments(ptr: *mut c_void, count: i32); + + fn apple_speech_ensure_model(lang: *const c_char) -> i32; +} + +/// Transcribe PCM audio (f32, 16kHz, mono) using Apple SpeechAnalyzer. +/// Returns segments with timestamps, matching the same `Segment` type as the CPU Whisper backend. +pub fn transcribe(samples: &[f32], params: &WhisperParams) -> Vec { + let lang = + CString::new(params.language.as_str()).unwrap_or_else(|_| CString::new("en").unwrap()); + let mut count: i32 = 0; + let mut raw_ptr: *mut c_void = std::ptr::null_mut(); + + let ret = unsafe { + apple_speech_transcribe( + samples.as_ptr(), + samples.len() as i64, + lang.as_ptr(), + &mut count, + &mut raw_ptr, + ) + }; + + if ret != 0 || count <= 0 || raw_ptr.is_null() { + return Vec::new(); + } + + let ptr = raw_ptr as *mut CSegment; + + let segments = unsafe { + (0..count as usize) + .map(|i| { + let cs = &*ptr.add(i); + let text = if cs.text.is_null() { + String::new() + } else { + CStr::from_ptr(cs.text).to_string_lossy().into_owned() + }; + Segment { + start_ms: cs.start_ms, + end_ms: cs.end_ms, + text, + } + }) + .collect() + }; + + unsafe { + apple_speech_free_segments(raw_ptr, count); + } + + segments +} + +/// Ensure the speech model for a language is downloaded. +/// Call this before the first transcription for a given language. +/// Returns Ok(()) if the model is ready, Err(()) on failure. +pub fn ensure_model(language: &str) -> Result<(), ()> { + let lang = CString::new(language).unwrap_or_else(|_| CString::new("en").unwrap()); + let ret = unsafe { apple_speech_ensure_model(lang.as_ptr()) }; + if ret == 0 { + Ok(()) + } else { + Err(()) + } +} diff --git a/libs/voice2/src/bin/apple_speech_test.rs b/libs/voice2/src/bin/apple_speech_test.rs new file mode 100644 index 0000000000..9ff292dd15 --- /dev/null +++ b/libs/voice2/src/bin/apple_speech_test.rs @@ -0,0 +1,116 @@ +#[cfg(all(any(target_os = "macos", target_os = "ios"), not(force_whisper)))] +fn main() { + use std::io::{Read, Seek, SeekFrom}; + + let wav_path = std::env::args() + .nth(1) + .unwrap_or_else(|| "local/whisper.cpp/samples/jfk.wav".into()); + + eprintln!("loading audio: {}", wav_path); + let samples = read_wav_pcm_f32(&wav_path); + + let params = makepad_voice2::WhisperParams::default(); + eprintln!( + "params: language='{}', translate={}", + params.language, params.translate + ); + + // Try ensure_model first + eprintln!("ensuring model for '{}'...", params.language); + match makepad_voice2::apple_speech::ensure_model(¶ms.language) { + Ok(()) => eprintln!("model ready"), + Err(()) => eprintln!("WARNING: ensure_model failed (may still work)"), + } + + eprintln!( + "transcribing {} samples ({:.2}s)...", + samples.len(), + samples.len() as f64 / 16000.0 + ); + let t0 = std::time::Instant::now(); + let segments = makepad_voice2::apple_speech::transcribe(&samples, ¶ms); + let elapsed = t0.elapsed().as_secs_f64(); + eprintln!( + "transcription done in {:.2}s, got {} segments", + elapsed, + segments.len() + ); + + if segments.is_empty() { + eprintln!("WARNING: no segments returned!"); + } + + for seg in &segments { + let t0 = seg.start_ms as f64 / 1000.0; + let t1 = seg.end_ms as f64 / 1000.0; + println!("[{:.2} --> {:.2}] {}", t0, t1, seg.text); + } + + fn read_wav_pcm_f32(path: &str) -> Vec { + let mut f = std::fs::File::open(path).expect("failed to open wav"); + let mut riff_header = [0u8; 12]; + f.read_exact(&mut riff_header) + .expect("failed to read RIFF header"); + assert_eq!(&riff_header[0..4], b"RIFF"); + assert_eq!(&riff_header[8..12], b"WAVE"); + + let mut channels = 1u16; + let mut _sample_rate = 16000u32; + let mut bits_per_sample = 16u16; + let mut audio_data = Vec::new(); + + loop { + let mut chunk_header = [0u8; 8]; + if f.read_exact(&mut chunk_header).is_err() { + break; + } + let chunk_id = &chunk_header[0..4]; + let chunk_size = u32::from_le_bytes([ + chunk_header[4], + chunk_header[5], + chunk_header[6], + chunk_header[7], + ]) as usize; + + if chunk_id == b"fmt " { + let mut fmt = vec![0u8; chunk_size]; + f.read_exact(&mut fmt).expect("failed to read fmt"); + channels = u16::from_le_bytes([fmt[2], fmt[3]]); + _sample_rate = u32::from_le_bytes([fmt[4], fmt[5], fmt[6], fmt[7]]); + bits_per_sample = u16::from_le_bytes([fmt[14], fmt[15]]); + } else if chunk_id == b"data" { + audio_data = vec![0u8; chunk_size]; + f.read_exact(&mut audio_data).expect("failed to read data"); + break; + } else { + f.seek(SeekFrom::Current(chunk_size as i64)).expect("skip"); + } + } + + eprintln!( + "wav: {} Hz, {} ch, {} bit", + _sample_rate, channels, bits_per_sample + ); + + let n_samples = audio_data.len() / 2; + let mut samples = Vec::with_capacity(n_samples); + for i in 0..n_samples { + let s = i16::from_le_bytes([audio_data[i * 2], audio_data[i * 2 + 1]]); + samples.push(s as f32 / 32768.0); + } + if channels == 2 { + samples = samples.iter().step_by(2).copied().collect(); + } + eprintln!( + "wav: {} samples ({:.1}s)", + samples.len(), + samples.len() as f64 / 16000.0 + ); + samples + } +} + +#[cfg(not(all(any(target_os = "macos", target_os = "ios"), not(force_whisper))))] +fn main() { + eprintln!("apple-speech path unavailable on this target/config"); +} diff --git a/libs/voice2/src/bin/metal_strip_unused.rs b/libs/voice2/src/bin/metal_strip_unused.rs new file mode 100644 index 0000000000..6fdc45b075 --- /dev/null +++ b/libs/voice2/src/bin/metal_strip_unused.rs @@ -0,0 +1,441 @@ +use std::collections::HashSet; +use std::fs; +use std::io; +use std::path::Path; + +const METAL_PATH: &str = "libs/voice2/src/metal/ggml/ggml-metal.metal"; +const METAL_BACKUP_PATH: &str = "libs/voice2/src/metal/ggml/ggml-metal.metal.bak"; +const COMMON_H_PATH: &str = "libs/voice2/src/metal/ggml/ggml-common.h"; +const METAL_IMPL_H_PATH: &str = "libs/voice2/src/metal/ggml/ggml-metal-impl.h"; +const REMOVED_REPORT_PATH: &str = "/tmp/metal_strip_removed_kernels.txt"; +const TURBO_KERNEL_KEEP: &[&str] = &[ + "kernel_unary_f32_f32_4", + "kernel_bin_fuse_f32_f32_f32_4", + "kernel_cpy_f32_f16", + "kernel_im2col_f32", + "kernel_mul_mm_f16_f32", + "kernel_mul_mv_ext_f16_f32_r1_2", + "kernel_mul_mv_ext_f16_f32_r1_3", + "kernel_mul_mv_ext_f16_f32_r1_4", + "kernel_mul_mv_ext_f16_f32_r1_5", + "kernel_mul_mv_ext_f32_f32_r1_2", + "kernel_mul_mv_ext_f32_f32_r1_3", + "kernel_mul_mv_ext_f32_f32_r1_4", + "kernel_mul_mv_ext_f32_f32_r1_5", + "kernel_mul_mv_f16_f32_4", + "kernel_mul_mv_f32_f32_4", + "kernel_norm_mul_add_f32_4", + "kernel_flash_attn_ext_pad", + "kernel_flash_attn_ext_blk", + "kernel_flash_attn_ext_vec_reduce", + "kernel_flash_attn_ext_f16_dk64_dv64", + "kernel_flash_attn_ext_vec_f32_dk64_dv64", +]; + +#[derive(Clone, Debug)] +struct KernelBlock { + start: usize, + end: usize, + name: String, +} + +#[derive(Clone, Debug)] +struct TableBlock { + start: usize, + end: usize, + name: String, +} + +fn line_start(src: &str, idx: usize) -> usize { + src[..idx].rfind('\n').map(|p| p + 1).unwrap_or(0) +} + +fn prev_line_start(src: &str, current_start: usize) -> Option { + if current_start == 0 { + return None; + } + let prev_end = current_start.saturating_sub(1); + Some(src[..prev_end].rfind('\n').map(|p| p + 1).unwrap_or(0)) +} + +fn line_end(src: &str, start: usize) -> usize { + src[start..] + .find('\n') + .map(|o| start + o) + .unwrap_or(src.len()) +} + +fn find_matching_brace(src: &str, open_idx: usize) -> Option { + let b = src.as_bytes(); + let mut i = open_idx; + let mut depth = 0usize; + while i < b.len() { + match b[i] { + b'{' => depth += 1, + b'}' => { + if depth == 0 { + return None; + } + depth -= 1; + if depth == 0 { + return Some(i); + } + } + _ => {} + } + i += 1; + } + None +} + +fn parse_export_name(block_src: &str) -> String { + if let Some(pos) = block_src.find("host_name(\"") { + let start = pos + "host_name(\"".len(); + if let Some(end_rel) = block_src[start..].find('"') { + return block_src[start..start + end_rel].to_string(); + } + } + + if let Some(pos) = block_src.find("kernel void") { + let mut s = &block_src[pos + "kernel void".len()..]; + s = s.trim_start(); + let mut out = String::new(); + for ch in s.chars() { + if ch.is_whitespace() || ch == '(' || ch == '<' { + break; + } + out.push(ch); + } + if !out.is_empty() { + return out; + } + } + + "unknown_kernel".to_string() +} + +fn find_kernel_blocks(src: &str) -> io::Result> { + let mut blocks = Vec::new(); + let mut search_from = 0usize; + + while let Some(rel) = src[search_from..].find("kernel void") { + let idx = search_from + rel; + let mut start = line_start(src, idx); + + while let Some(prev_start) = prev_line_start(src, start) { + let prev_end = line_end(src, prev_start); + let prev = src[prev_start..prev_end].trim(); + if prev.starts_with("template") || prev.starts_with("[[") { + start = prev_start; + continue; + } + break; + } + + let open_rel = src[idx..].find('{'); + let semi_rel = src[idx..].find(';'); + if match (open_rel, semi_rel) { + (None, Some(_)) => true, + (Some(open), Some(semi)) if semi < open => true, + _ => false, + } { + // Declaration-only kernel entry. + search_from = idx + "kernel void".len(); + continue; + } + let open_rel = + open_rel.ok_or_else(|| io::Error::other("failed to find kernel opening brace"))?; + let open_idx = idx + open_rel; + let close_idx = find_matching_brace(src, open_idx) + .ok_or_else(|| io::Error::other("failed to match kernel closing brace"))?; + let end = close_idx + 1; + + let name = parse_export_name(&src[start..end]); + blocks.push(KernelBlock { start, end, name }); + search_from = end; + } + + blocks.sort_by_key(|b| b.start); + Ok(blocks) +} + +fn render_with_keep(original: &str, blocks: &[KernelBlock], keep: &[bool]) -> String { + let mut out = String::with_capacity(original.len()); + let mut cursor = 0usize; + + for (idx, block) in blocks.iter().enumerate() { + if block.start > cursor { + out.push_str(&original[cursor..block.start]); + } + if keep[idx] { + out.push_str(&original[block.start..block.end]); + } + cursor = block.end; + } + + if cursor < original.len() { + out.push_str(&original[cursor..]); + } + + out +} + +fn is_used_by_backend_turbo(name: &str) -> bool { + TURBO_KERNEL_KEEP.contains(&name) +} + +fn parse_host_name(line: &str) -> Option<&str> { + let pos = line.find("host_name(\"")?; + let start = pos + "host_name(\"".len(); + let end_rel = line[start..].find('"')?; + Some(&line[start..start + end_rel]) +} + +fn strip_unused_template_host_lines( + src: &str, + keep_names: &HashSet, +) -> (String, Vec) { + let mut out = String::with_capacity(src.len()); + let mut removed = Vec::new(); + let lines: Vec<&str> = src.split_inclusive('\n').collect(); + let mut i = 0usize; + while i < lines.len() { + let line = lines[i]; + if line.contains("template") { + if let Some(name) = parse_host_name(line) { + if !keep_names.contains(name) { + removed.push(name.to_string()); + i += 1; + while i < lines.len() && !lines[i - 1].contains(';') { + i += 1; + } + continue; + } + } + } + out.push_str(line); + i += 1; + } + + (out, removed) +} + +fn parse_table_name(begin_line: &str) -> Option { + let pos = begin_line.find("GGML_TABLE_BEGIN(")?; + let mut s = &begin_line[pos + "GGML_TABLE_BEGIN(".len()..]; + let c0 = s.find(',')?; + s = &s[c0 + 1..]; + let c1 = s.find(',')?; + let name = s[..c1].trim(); + if name.is_empty() { + None + } else { + Some(name.to_string()) + } +} + +fn find_table_blocks(src: &str) -> io::Result> { + let mut blocks = Vec::new(); + let mut search_from = 0usize; + while let Some(rel) = src[search_from..].find("GGML_TABLE_BEGIN(") { + let idx = search_from + rel; + let start = line_start(src, idx); + let begin_end = line_end(src, start); + let name = parse_table_name(&src[start..begin_end]) + .ok_or_else(|| io::Error::other("failed to parse GGML_TABLE_BEGIN name"))?; + + let end_marker_rel = src[idx..] + .find("GGML_TABLE_END()") + .ok_or_else(|| io::Error::other("failed to find GGML_TABLE_END for table"))?; + let end_marker_idx = idx + end_marker_rel; + let end_line = line_end(src, end_marker_idx); + let end = if end_line < src.len() { + end_line + 1 + } else { + end_line + }; + + blocks.push(TableBlock { start, end, name }); + search_from = end; + } + Ok(blocks) +} + +fn render_table_keep(original: &str, blocks: &[TableBlock], keep: &[bool]) -> String { + let mut out = String::with_capacity(original.len()); + let mut cursor = 0usize; + + for (idx, block) in blocks.iter().enumerate() { + if block.start > cursor { + out.push_str(&original[cursor..block.start]); + } + if keep[idx] { + out.push_str(&original[block.start..block.end]); + } + cursor = block.end; + } + + if cursor < original.len() { + out.push_str(&original[cursor..]); + } + + out +} + +fn contains_ident(src: &str, ident: &str) -> bool { + let mut offset = 0usize; + while let Some(rel) = src[offset..].find(ident) { + let idx = offset + rel; + let before_ok = idx == 0 + || !src.as_bytes()[idx - 1].is_ascii_alphanumeric() && src.as_bytes()[idx - 1] != b'_'; + let after_idx = idx + ident.len(); + let after_ok = after_idx >= src.len() + || !src.as_bytes()[after_idx].is_ascii_alphanumeric() + && src.as_bytes()[after_idx] != b'_'; + if before_ok && after_ok { + return true; + } + offset = idx + ident.len(); + } + false +} + +fn main() -> Result<(), Box> { + if !Path::new(METAL_PATH).exists() { + return Err(format!("metal source not found at {}", METAL_PATH).into()); + } + + if !Path::new(METAL_BACKUP_PATH).exists() { + fs::copy(METAL_PATH, METAL_BACKUP_PATH)?; + } + + let original = fs::read_to_string(METAL_PATH)?; + let before_bytes = original.len(); + let blocks = find_kernel_blocks(&original)?; + if blocks.is_empty() { + return Err("no kernel blocks found in metal source".into()); + } + + let mut keep: Vec = blocks + .iter() + .map(|b| is_used_by_backend_turbo(&b.name)) + .collect(); + + // Keep dependency closure: if a removed kernel symbol is still referenced by + // remaining source text, keep that block to preserve compile/link integrity. + loop { + let candidate = render_with_keep(&original, &blocks, &keep); + let mut changed = false; + for (i, block) in blocks.iter().enumerate() { + if keep[i] { + continue; + } + let has_ref = candidate.contains(&format!("{}(", block.name)) + || candidate.contains(&format!("{}<", block.name)); + if !block.name.is_empty() && has_ref { + keep[i] = true; + changed = true; + } + } + if !changed { + break; + } + } + + let stripped_blocks = render_with_keep(&original, &blocks, &keep); + let mut keep_names: HashSet = TURBO_KERNEL_KEEP + .iter() + .map(|name| (*name).to_string()) + .collect(); + for (block, keep_block) in blocks.iter().zip(keep.iter()) { + if *keep_block { + keep_names.insert(block.name.clone()); + } + } + let (stripped, removed_template_hosts) = + strip_unused_template_host_lines(&stripped_blocks, &keep_names); + let after_bytes = stripped.len(); + fs::write(METAL_PATH, &stripped)?; + + let removed_kernel_blocks: Vec = blocks + .iter() + .zip(keep.iter()) + .filter_map(|(b, k)| if *k { None } else { Some(b.name.clone()) }) + .collect(); + let mut removed: Vec = removed_kernel_blocks.clone(); + for name in removed_template_hosts { + removed.push(format!("template:{}", name)); + } + removed.sort(); + removed.dedup(); + // Strip unreferenced quantization tables from ggml-common.h based on the + // final stripped metal source. + let common_original = fs::read_to_string(COMMON_H_PATH)?; + let common_before_bytes = common_original.len(); + let table_blocks = find_table_blocks(&common_original)?; + let impl_h_source = fs::read_to_string(METAL_IMPL_H_PATH)?; + let table_usage_source = format!("{}\n{}", stripped, impl_h_source); + let table_keep: Vec = table_blocks + .iter() + .map(|tb| { + let used_by_metal = contains_ident(&table_usage_source, &tb.name); + let used_before = contains_ident(&common_original[..tb.start], &tb.name); + let used_after = contains_ident(&common_original[tb.end..], &tb.name); + used_by_metal || used_before || used_after + }) + .collect(); + let common_stripped = render_table_keep(&common_original, &table_blocks, &table_keep); + let common_after_bytes = common_stripped.len(); + fs::write(COMMON_H_PATH, common_stripped)?; + + let removed_tables: Vec = table_blocks + .iter() + .zip(table_keep.iter()) + .filter_map(|(tb, keep_tb)| { + if *keep_tb { + None + } else { + Some(format!("table:{}", tb.name)) + } + }) + .collect(); + removed.extend(removed_tables.clone()); + removed.sort(); + removed.dedup(); + fs::write(REMOVED_REPORT_PATH, removed.join("\n"))?; + + eprintln!( + "[metal-strip] kernels total={} kept={} removed={}", + blocks.len(), + blocks.len().saturating_sub(removed_kernel_blocks.len()), + removed_kernel_blocks.len() + ); + eprintln!( + "[metal-strip] templates removed={}", + removed.len().saturating_sub(removed_kernel_blocks.len()) + ); + eprintln!( + "[metal-strip] bytes before={} after={} delta={} ({:.1}%)", + before_bytes, + after_bytes, + (before_bytes as isize - after_bytes as isize), + 100.0 * (before_bytes.saturating_sub(after_bytes) as f64) / before_bytes.max(1) as f64 + ); + eprintln!( + "[metal-strip] ggml-common tables total={} kept={} removed={}", + table_blocks.len(), + table_keep.iter().filter(|k| **k).count(), + removed_tables.len() + ); + eprintln!( + "[metal-strip] ggml-common bytes before={} after={} delta={} ({:.1}%)", + common_before_bytes, + common_after_bytes, + (common_before_bytes as isize - common_after_bytes as isize), + 100.0 * (common_before_bytes.saturating_sub(common_after_bytes) as f64) + / common_before_bytes.max(1) as f64 + ); + eprintln!("[metal-strip] removed list: {}", REMOVED_REPORT_PATH); + + Ok(()) +} diff --git a/libs/voice2/src/bin/whisper_test.rs b/libs/voice2/src/bin/whisper_test.rs new file mode 100644 index 0000000000..f6fc96a8e8 --- /dev/null +++ b/libs/voice2/src/bin/whisper_test.rs @@ -0,0 +1,159 @@ +use makepad_voice2::{WhisperModel, WhisperParams, WhisperState}; +use std::io::{Read, Seek, SeekFrom}; + +fn read_wav_pcm_f32(path: &str) -> Vec { + let mut f = std::fs::File::open(path).expect("failed to open wav"); + + // Read RIFF header (12 bytes) + let mut riff_header = [0u8; 12]; + f.read_exact(&mut riff_header) + .expect("failed to read RIFF header"); + assert_eq!(&riff_header[0..4], b"RIFF", "not a RIFF file"); + assert_eq!(&riff_header[8..12], b"WAVE", "not a WAVE file"); + + let mut channels = 1u16; + let mut sample_rate = 16000u32; + let mut bits_per_sample = 16u16; + let mut audio_data = Vec::new(); + + // Parse chunks + loop { + let mut chunk_header = [0u8; 8]; + if f.read_exact(&mut chunk_header).is_err() { + break; + } + let chunk_id = &chunk_header[0..4]; + let chunk_size = u32::from_le_bytes([ + chunk_header[4], + chunk_header[5], + chunk_header[6], + chunk_header[7], + ]) as usize; + + if chunk_id == b"fmt " { + let mut fmt = vec![0u8; chunk_size]; + f.read_exact(&mut fmt).expect("failed to read fmt chunk"); + channels = u16::from_le_bytes([fmt[2], fmt[3]]); + sample_rate = u32::from_le_bytes([fmt[4], fmt[5], fmt[6], fmt[7]]); + bits_per_sample = u16::from_le_bytes([fmt[14], fmt[15]]); + } else if chunk_id == b"data" { + audio_data = vec![0u8; chunk_size]; + f.read_exact(&mut audio_data) + .expect("failed to read data chunk"); + break; + } else { + // Skip unknown chunks + f.seek(SeekFrom::Current(chunk_size as i64)) + .expect("failed to skip chunk"); + } + } + + eprintln!( + "wav: {} Hz, {} ch, {} bit", + sample_rate, channels, bits_per_sample + ); + assert_eq!(sample_rate, 16000, "expected 16kHz"); + assert_eq!(bits_per_sample, 16, "expected 16-bit PCM"); + + // Convert i16 -> f32 + let n_samples = audio_data.len() / 2; + let mut samples = Vec::with_capacity(n_samples); + for i in 0..n_samples { + let s = i16::from_le_bytes([audio_data[i * 2], audio_data[i * 2 + 1]]); + samples.push(s as f32 / 32768.0); + } + + // If stereo, take left channel only + if channels == 2 { + samples = samples.iter().step_by(2).copied().collect(); + } + + eprintln!( + "wav: {} samples ({:.1}s)", + samples.len(), + samples.len() as f64 / 16000.0 + ); + samples +} + +fn main() { + let model_path = std::env::args() + .nth(1) + .unwrap_or_else(|| "ggml-large-v3-turbo.bin".into()); + let wav_path = std::env::args() + .nth(2) + .unwrap_or_else(|| "local/whisper.cpp/samples/jfk.wav".into()); + + eprintln!("loading model: {}", model_path); + if let Ok(threads) = std::env::var("MAKEPAD_VOICE_THREADS") { + eprintln!("using MAKEPAD_VOICE_THREADS={}", threads); + } + let t0 = std::time::Instant::now(); + let model = WhisperModel::load_file(&model_path).expect("failed to load model"); + eprintln!("model loaded in {:.1}s", t0.elapsed().as_secs_f64()); + + let max_sec: f64 = std::env::args() + .nth(3) + .and_then(|s| s.parse().ok()) + .unwrap_or(0.0); + let max_tokens: Option = std::env::args() + .nth(4) + .and_then(|s| s.parse().ok()) + .or_else(|| { + std::env::var("MAKEPAD_VOICE_MAX_TOKENS") + .ok() + .and_then(|s| s.parse().ok()) + }); + + eprintln!("loading audio: {}", wav_path); + let mut samples = read_wav_pcm_f32(&wav_path); + if max_sec > 0.0 { + let max_samples = (max_sec * 16000.0) as usize; + samples.truncate(max_samples); + eprintln!("truncated to {:.1}s ({} samples)", max_sec, samples.len()); + } + + let mut params = WhisperParams::default(); + if let Some(max_tokens) = max_tokens { + params.max_tokens = max_tokens; + eprintln!("using max_tokens={}", max_tokens); + } + + let bench_repeat = std::env::args() + .nth(5) + .and_then(|s| s.parse::().ok()) + .or_else(|| { + std::env::var("MAKEPAD_VOICE_BENCH_REPEAT") + .ok() + .and_then(|s| s.parse::().ok()) + }) + .unwrap_or(1) + .max(1); + if bench_repeat > 1 { + eprintln!("using MAKEPAD_VOICE_BENCH_REPEAT={}", bench_repeat); + } + + for run in 0..bench_repeat { + let mut state = WhisperState::new(&model); + eprintln!("transcribing run {}/{}...", run + 1, bench_repeat); + makepad_voice2::reset_profiling(); + let t0 = std::time::Instant::now(); + let segments = state.transcribe(&model, &samples, ¶ms); + let total = t0.elapsed().as_secs_f64(); + eprintln!( + "transcription run {}/{} done in {:.1}s", + run + 1, + bench_repeat, + total + ); + makepad_voice2::print_profiling(); + + if run + 1 == bench_repeat { + for seg in &segments { + let t0 = seg.start_ms as f64 / 1000.0; + let t1 = seg.end_ms as f64 / 1000.0; + println!("[{:.2} --> {:.2}] {}", t0, t1, seg.text); + } + } + } +} diff --git a/libs/voice2/src/cpu/decode_loop.rs b/libs/voice2/src/cpu/decode_loop.rs new file mode 100644 index 0000000000..fff85150ec --- /dev/null +++ b/libs/voice2/src/cpu/decode_loop.rs @@ -0,0 +1,515 @@ +use crate::decoder::{self, KvCache}; +use crate::encoder; +use crate::mel; +use crate::model::WhisperModel; + +/// A transcribed text segment with timestamps. +#[derive(Debug, Clone)] +pub struct Segment { + pub start_ms: i64, + pub end_ms: i64, + pub text: String, +} + +/// Parameters for transcription. +#[derive(Debug, Clone)] +pub struct WhisperParams { + pub language: String, + pub translate: bool, + pub no_timestamps: bool, + pub single_segment: bool, + pub temperature: f32, + pub max_tokens: usize, + pub no_speech_thold: f32, + pub suppress_blank: bool, +} + +impl Default for WhisperParams { + fn default() -> Self { + WhisperParams { + language: "en".into(), + translate: false, + no_timestamps: false, + single_segment: false, + temperature: 0.0, + max_tokens: 0, + no_speech_thold: 0.6, + suppress_blank: true, + } + } +} + +/// Mutable state for the transcription process. +pub struct WhisperState { + kv_cache: KvCache, +} + +impl WhisperState { + pub fn new(model: &WhisperModel) -> Self { + let n_layers = model.hparams.n_text_layer as usize; + WhisperState { + kv_cache: KvCache::new(n_layers), + } + } + + /// Transcribe PCM audio (f32, 16kHz, mono) and return text segments. + pub fn transcribe( + &mut self, + model: &WhisperModel, + samples: &[f32], + params: &WhisperParams, + ) -> Vec { + let n_ctx = model.hparams.n_audio_ctx as usize; + let n_mels = model.hparams.n_mels as usize; + + // 1. Compute mel spectrogram + let _t = std::time::Instant::now(); + let (mel_data, _n_mel, n_mel_len, n_mel_len_org) = + mel::log_mel_spectrogram(samples, &model.filters, 1); + crate::PROF_MEL.fetch_add( + _t.elapsed().as_nanos() as u64, + std::sync::atomic::Ordering::Relaxed, + ); + + let mut segments = Vec::new(); + let mut seek = 0usize; // in mel frames + + // Use the original (non-30s-padded) mel length as the decode endpoint. + let mel_end = n_mel_len_org.min(n_mel_len); + + while seek < mel_end { + // 2. Extract mel chunk: [n_mels, 2*n_ctx] starting at seek + let chunk_len = 2 * n_ctx; + let is_final_chunk = seek + chunk_len >= mel_end; + let mut mel_chunk = vec![0.0f32; n_mels * chunk_len]; + for j in 0..n_mels { + for i in 0..chunk_len { + let mel_idx = seek + i; + if mel_idx < n_mel_len { + mel_chunk[j * chunk_len + i] = mel_data[j * n_mel_len + mel_idx]; + } + // else: zero (already) + } + } + + // 3. Encode + let _t = std::time::Instant::now(); + let encoder_out = encoder::encode(model, &mel_chunk, n_ctx); + crate::PROF_ENCODER.fetch_add( + _t.elapsed().as_nanos() as u64, + std::sync::atomic::Ordering::Relaxed, + ); + + // 4. Pre-compute cross-attention KV + let _t = std::time::Instant::now(); + let cross_kv = encoder::compute_cross_kv(model, &encoder_out); + crate::PROF_CROSS_KV.fetch_add( + _t.elapsed().as_nanos() as u64, + std::sync::atomic::Ordering::Relaxed, + ); + + // 5. Decode tokens + self.kv_cache.clear(); + + let vocab = &model.vocab; + + // Build initial prompt: [SOT, lang_id, task, (notimestamps?)] + let mut prompt_tokens: Vec = Vec::new(); + prompt_tokens.push(vocab.token_sot); + + // Language token (SOT + 1 + lang_id) + if vocab.is_multilingual() { + let lang_id = language_id(¶ms.language); + prompt_tokens.push(vocab.token_sot + 1 + lang_id); + if params.translate { + prompt_tokens.push(vocab.token_translate); + } else { + prompt_tokens.push(vocab.token_transcribe); + } + } + + if params.no_timestamps { + prompt_tokens.push(vocab.token_not); + } + + let n_prompt = prompt_tokens.len(); + let positions: Vec = (0..n_prompt as i32).collect(); + + // Decode prompt + let _t = std::time::Instant::now(); + let logits = decoder::decode( + model, + &prompt_tokens, + &positions, + &mut self.kv_cache, + &cross_kv, + ); + crate::PROF_DECODER.fetch_add( + _t.elapsed().as_nanos() as u64, + std::sync::atomic::Ordering::Relaxed, + ); + crate::PROF_DECODER_CALLS.fetch_add(1, std::sync::atomic::Ordering::Relaxed); + decoder::advance_kv_cache(&mut self.kv_cache, n_prompt); + + // Check no-speech probability + let n_vocab = model.hparams.n_vocab as usize; + let last_logits = &logits.data[(n_prompt - 1) * n_vocab..n_prompt * n_vocab]; + let no_speech_prob = softmax_single(last_logits, vocab.token_nosp as usize); + + if no_speech_prob > params.no_speech_thold { + // Skip this chunk - no speech detected + seek += n_ctx * 2; + continue; + } + + // 6. Auto-regressive token generation (greedy) + let mut result_tokens: Vec = Vec::new(); + let max_tokens = if params.max_tokens > 0 { + params.max_tokens + } else { + model.hparams.n_text_ctx as usize / 2 + } + .min((model.hparams.n_text_ctx as usize).saturating_sub(n_prompt)); + + let mut has_ts = false; + let mut seek_delta: usize = n_ctx * 2; // default: advance full chunk + + for _i in 0..max_tokens { + if self.kv_cache.n_past >= model.hparams.n_text_ctx as usize { + break; + } + // Get logits for last token + let prev_token = if result_tokens.is_empty() { + // Use last logits from prompt decode + let token = + sample_greedy(last_logits, vocab, params, &result_tokens, has_ts, 0); + result_tokens.push(token); + if token.id == vocab.token_eot { + break; + } + if token.id >= vocab.token_beg { + has_ts = true; + seek_delta = 2 * (token.id - vocab.token_beg) as usize; + } + continue; + } else { + result_tokens.last().unwrap().id + }; + + let pos = (self.kv_cache.n_past) as i32; + let _t = std::time::Instant::now(); + let logits = + decoder::decode(model, &[prev_token], &[pos], &mut self.kv_cache, &cross_kv); + crate::PROF_DECODER.fetch_add( + _t.elapsed().as_nanos() as u64, + std::sync::atomic::Ordering::Relaxed, + ); + crate::PROF_DECODER_CALLS.fetch_add(1, std::sync::atomic::Ordering::Relaxed); + decoder::advance_kv_cache(&mut self.kv_cache, 1); + + let cur_logits = &logits.data[0..n_vocab]; + let seek_delta_cur = if has_ts { seek_delta } else { 0 }; + let token = sample_greedy( + cur_logits, + vocab, + params, + &result_tokens, + has_ts, + seek_delta_cur, + ); + + if token.id == vocab.token_eot { + break; + } + + if token.id >= vocab.token_beg { + let new_delta = 2 * (token.id - vocab.token_beg) as usize; + if has_ts && new_delta < seek_delta { + // Don't go backwards + break; + } + seek_delta = new_delta; + has_ts = true; + } + + result_tokens.push(token); + } + + // Mirror whisper.cpp behavior: if decoding ends with a single trailing timestamp, + // advance by the whole chunk to avoid re-decoding the same tail window. + let single_timestamp_ending = result_tokens.len() > 1 + && result_tokens[result_tokens.len() - 2].id < vocab.token_beg + && result_tokens[result_tokens.len() - 1].id > vocab.token_beg; + if single_timestamp_ending { + seek_delta = std::cmp::min(mel_end - seek, n_ctx * 2); + } + if is_final_chunk { + seek_delta = mel_end - seek; + } + + // 7. Convert tokens to segments + let chunk_start_ms = (seek as f64 * 10.0) as i64; // mel frames to ms (each frame = 10ms) + + if params.no_timestamps || params.single_segment { + // Single segment for entire chunk + let text: String = result_tokens + .iter() + .filter(|t| t.id < vocab.token_eot) + .map(|t| vocab.token_to_str(t.id)) + .collect(); + if !text.is_empty() { + let end_ms = chunk_start_ms + (seek_delta as f64 * 10.0) as i64; + segments.push(Segment { + start_ms: chunk_start_ms, + end_ms, + text, + }); + } + } else { + // Split on timestamp tokens + let mut seg_start = chunk_start_ms; + let mut seg_text = String::new(); + let mut _seg_start_idx = 0; + + for (idx, token) in result_tokens.iter().enumerate() { + if token.id >= vocab.token_beg { + let ts_ms = chunk_start_ms + (2 * (token.id - vocab.token_beg) as i64) * 10; + + if !seg_text.is_empty() { + segments.push(Segment { + start_ms: seg_start, + end_ms: ts_ms, + text: seg_text.clone(), + }); + } + seg_start = ts_ms; + seg_text.clear(); + _seg_start_idx = idx + 1; + } else if token.id < vocab.token_eot { + seg_text.push_str(vocab.token_to_str(token.id)); + } + } + + // Remaining text after last timestamp + if !seg_text.is_empty() { + let end_ms = chunk_start_ms + (seek_delta as f64 * 10.0) as i64; + segments.push(Segment { + start_ms: seg_start, + end_ms, + text: seg_text, + }); + } + } + + // Advance seek + let seek_step = std::cmp::min(seek_delta, mel_end.saturating_sub(seek)); + if seek_step == 0 { + break; + } + seek += seek_step; + } + + segments + } +} + +#[derive(Debug, Clone, Copy)] +struct TokenData { + id: i32, +} + +fn sample_greedy( + logits: &[f32], + vocab: &crate::model::Vocab, + params: &WhisperParams, + prev_tokens: &[TokenData], + has_ts: bool, + seek_delta: usize, +) -> TokenData { + let n_vocab = logits.len(); + let mut logits = logits.to_vec(); + + // Suppress special tokens + logits[vocab.token_not as usize] = f32::NEG_INFINITY; + logits[vocab.token_sot as usize] = f32::NEG_INFINITY; + logits[vocab.token_nosp as usize] = f32::NEG_INFINITY; + logits[vocab.token_translate as usize] = f32::NEG_INFINITY; + logits[vocab.token_transcribe as usize] = f32::NEG_INFINITY; + logits[vocab.token_prev as usize] = f32::NEG_INFINITY; + logits[vocab.token_solm as usize] = f32::NEG_INFINITY; + + // Suppress blank at start + if params.suppress_blank && prev_tokens.is_empty() { + logits[vocab.token_eot as usize] = f32::NEG_INFINITY; + if let Some(&space_id) = vocab.token_to_id.get(" ") { + logits[space_id as usize] = f32::NEG_INFINITY; + } + } + + // Timestamp pairing constraints + if !prev_tokens.is_empty() { + let last = prev_tokens.last().unwrap().id; + let last_was_ts = last >= vocab.token_beg; + + if last_was_ts { + let penultimate_was_ts = + prev_tokens.len() < 2 || prev_tokens[prev_tokens.len() - 2].id >= vocab.token_beg; + if penultimate_was_ts { + // Two timestamps in a row: suppress all timestamp tokens + for i in vocab.token_beg as usize..n_vocab { + logits[i] = f32::NEG_INFINITY; + } + } else { + // After text+timestamp: suppress all text tokens + for i in 0..vocab.token_eot as usize { + logits[i] = f32::NEG_INFINITY; + } + } + } + } + + // Enforce monotonic timestamps + if has_ts && !params.no_timestamps { + let tid0 = seek_delta / 2; + for i in vocab.token_beg as usize..vocab.token_beg as usize + tid0 { + if i < n_vocab { + logits[i] = f32::NEG_INFINITY; + } + } + } + + // Temperature scaling + if params.temperature > 0.0 { + for l in &mut logits { + *l /= params.temperature; + } + } + + // Greedy: pick argmax + let mut best_id = 0; + let mut best_val = f32::NEG_INFINITY; + for (i, &v) in logits.iter().enumerate() { + if v > best_val { + best_val = v; + best_id = i; + } + } + + TokenData { id: best_id as i32 } +} + +/// Compute softmax probability for a single token. +fn softmax_single(logits: &[f32], idx: usize) -> f32 { + let max_val = logits.iter().cloned().fold(f32::NEG_INFINITY, f32::max); + let mut sum = 0.0f32; + for &v in logits { + sum += (v - max_val).exp(); + } + (logits[idx] - max_val).exp() / sum +} + +fn language_id(lang: &str) -> i32 { + // Common language codes. For a full implementation this would be a complete table. + match lang { + "en" => 0, + "zh" => 1, + "de" => 2, + "es" => 3, + "ru" => 4, + "ko" => 5, + "fr" => 6, + "ja" => 7, + "pt" => 8, + "tr" => 9, + "pl" => 10, + "ca" => 11, + "nl" => 12, + "ar" => 13, + "sv" => 14, + "it" => 15, + "id" => 16, + "hi" => 17, + "fi" => 18, + "vi" => 19, + "he" => 20, + "uk" => 21, + "el" => 22, + "ms" => 23, + "cs" => 24, + "ro" => 25, + "da" => 26, + "hu" => 27, + "ta" => 28, + "no" => 29, + "th" => 30, + "ur" => 31, + "hr" => 32, + "bg" => 33, + "lt" => 34, + "la" => 35, + "mi" => 36, + "ml" => 37, + "cy" => 38, + "sk" => 39, + "te" => 40, + "fa" => 41, + "lv" => 42, + "bn" => 43, + "sr" => 44, + "az" => 45, + "sl" => 46, + "kn" => 47, + "et" => 48, + "mk" => 49, + "br" => 50, + "eu" => 51, + "is" => 52, + "hy" => 53, + "ne" => 54, + "mn" => 55, + "bs" => 56, + "kk" => 57, + "sq" => 58, + "sw" => 59, + "gl" => 60, + "mr" => 61, + "pa" => 62, + "si" => 63, + "km" => 64, + "sn" => 65, + "yo" => 66, + "so" => 67, + "af" => 68, + "oc" => 69, + "ka" => 70, + "be" => 71, + "tg" => 72, + "sd" => 73, + "gu" => 74, + "am" => 75, + "yi" => 76, + "lo" => 77, + "uz" => 78, + "fo" => 79, + "ht" => 80, + "ps" => 81, + "tk" => 82, + "nn" => 83, + "mt" => 84, + "sa" => 85, + "lb" => 86, + "my" => 87, + "bo" => 88, + "tl" => 89, + "mg" => 90, + "as" => 91, + "tt" => 92, + "haw" => 93, + "ln" => 94, + "ha" => 95, + "ba" => 96, + "jw" => 97, + "su" => 98, + _ => 0, // default to english + } +} diff --git a/libs/voice2/src/cpu/decoder.rs b/libs/voice2/src/cpu/decoder.rs new file mode 100644 index 0000000000..efbe4da6fb --- /dev/null +++ b/libs/voice2/src/cpu/decoder.rs @@ -0,0 +1,604 @@ +use crate::model::WhisperModel; +use crate::tensor::{parallel_for, SendPtr, Tensor}; + +const EPS: f32 = 1e-5; + +fn pad_to(v: usize, align: usize) -> usize { + if align == 0 { + return v; + } + let rem = v % align; + if rem == 0 { + v + } else { + v + (align - rem) + } +} + +/// KV cache for the decoder self-attention. +/// Stores K and V for each layer, growing as tokens are generated. +pub struct KvCache { + /// Per-layer K cache: each is [n_past, n_state] + pub k: Vec>, + /// Per-layer V cache: each is [n_past, n_state] + pub v: Vec>, + pub n_past: usize, +} + +impl KvCache { + pub fn new(n_layers: usize) -> Self { + KvCache { + k: vec![Vec::new(); n_layers], + v: vec![Vec::new(); n_layers], + n_past: 0, + } + } + + pub fn clear(&mut self) { + for k in &mut self.k { + k.clear(); + } + for v in &mut self.v { + v.clear(); + } + self.n_past = 0; + crate::metal_backend::clear_decoder_kv_cache(); + } + + /// Append K, V rows for a layer + fn append(&mut self, layer: usize, k_row: &[f32], v_row: &[f32]) { + self.k[layer].extend_from_slice(k_row); + self.v[layer].extend_from_slice(v_row); + } + + /// Get cached K for a layer as [n_past + n_new, n_state] shaped data + fn k_data(&self, layer: usize) -> &[f32] { + &self.k[layer] + } + fn v_data(&self, layer: usize) -> &[f32] { + &self.v[layer] + } +} + +/// Run one decoder step. +/// tokens: token IDs for this step (can be >1 for prompt processing) +/// kv_cache: self-attention KV cache (mutated) +/// cross_kv: pre-computed cross-attention K, V per layer +/// Returns logits: [n_tokens, n_vocab] +pub fn decode( + model: &WhisperModel, + tokens: &[i32], + positions: &[i32], + kv_cache: &mut KvCache, + cross_kv: &[(Tensor, Tensor)], +) -> Tensor { + let n_tokens = tokens.len(); + let n_state = model.hparams.n_text_state as usize; + let n_head = model.hparams.n_text_head as usize; + let n_state_head = n_state / n_head; + + // Token embedding + positional embedding + let token_embd = Tensor::get_rows(&model.d_te, tokens); + let pos_embd = Tensor::get_rows(&model.d_pe, positions); + let mut cur = Tensor::add(&token_embd, &pos_embd); + + let n_past = kv_cache.n_past; + let n_kv = n_past + n_tokens; // total KV length after this step + + for (il, layer) in model.decoder_layers.iter().enumerate() { + // === Self-Attention === + let residual = cur.clone(); + + // Q, K, V projections. + let (q, k_new, v_new) = if crate::metal_backend::is_requested() && n_tokens == 1 { + if let Some((q_data, k_data, v_data)) = + crate::metal_backend::try_decoder_self_qkv_step_f32( + &cur.data, + n_state, + &layer.attn_ln_0_w.data, + &layer.attn_ln_0_b.data, + &layer.attn_q_w.data, + layer.attn_q_w.ggml_type, + &layer.attn_q_b.data, + &layer.attn_k_w.data, + layer.attn_k_w.ggml_type, + &layer.attn_v_w.data, + layer.attn_v_w.ggml_type, + &layer.attn_v_b.data, + ) + { + ( + Tensor { + data: q_data, + shape: vec![1, n_state], + }, + Tensor { + data: k_data, + shape: vec![1, n_state], + }, + Tensor { + data: v_data, + shape: vec![1, n_state], + }, + ) + } else { + let normed = + Tensor::layer_norm_mul_add(&cur, &layer.attn_ln_0_w, &layer.attn_ln_0_b, EPS); + ( + Tensor::linear_raw(&normed, &layer.attn_q_w, &layer.attn_q_b), + Tensor::matmul_raw(&normed, &layer.attn_k_w), + Tensor::linear_raw(&normed, &layer.attn_v_w, &layer.attn_v_b), + ) + } + } else { + let normed = + Tensor::layer_norm_mul_add(&cur, &layer.attn_ln_0_w, &layer.attn_ln_0_b, EPS); + ( + Tensor::linear_raw(&normed, &layer.attn_q_w, &layer.attn_q_b), + Tensor::matmul_raw(&normed, &layer.attn_k_w), + Tensor::linear_raw(&normed, &layer.attn_v_w, &layer.attn_v_b), + ) + }; + + // Append new K, V to cache + kv_cache.append(il, &k_new.data, &v_new.data); + + // Self-attention with causal mask + let k_all = kv_cache.k_data(il); + let v_all = kv_cache.v_data(il); + + let (ref k_cross, ref v_cross) = cross_kv[il]; + let n_audio_ctx = k_cross.shape[0]; + let n_audio_ctx_flash = if crate::metal_backend::is_requested() && n_tokens == 1 { + pad_to(n_audio_ctx, 256) + } else { + n_audio_ctx + }; + + let scale = 1.0 / (n_state_head as f32).sqrt(); + if crate::metal_backend::is_requested() && n_tokens == 1 { + if let Some(out) = crate::metal_backend::try_decoder_self_cross_ffn_step_f32( + il, + &residual.data, + &q.data, + k_all, + v_all, + n_kv, + n_state, + n_head, + &k_cross.data, + &v_cross.data, + n_audio_ctx_flash, + layer, + ) { + cur = Tensor { + data: out, + shape: vec![n_tokens, n_state], + }; + continue; + } + } + + let attn_out = if crate::metal_backend::is_requested() && n_tokens == 1 { + crate::metal_backend::try_flash_attn_f32_self_kv_cache( + il, + &q.data, + k_all, + v_all, + n_kv, + n_head, + n_state_head, + scale, + ) + .or_else(|| { + crate::metal_backend::try_flash_attn_f32_packed( + &q.data, + k_all, + v_all, + n_tokens, + n_kv, + n_head, + n_state_head, + scale, + ) + }) + .unwrap_or_else(|| { + let mut attn_out = vec![0.0f32; n_tokens * n_state]; + let out_ptr = SendPtr::new(attn_out.as_mut_ptr()); + let q_data = &q.data; + parallel_for(n_head, |h| { + let h_off = h * n_state_head; + let mut qh = vec![0.0f32; n_tokens * n_state_head]; + let mut kh = vec![0.0f32; n_kv * n_state_head]; + let mut vh = vec![0.0f32; n_kv * n_state_head]; + for i in 0..n_tokens { + qh[i * n_state_head..(i + 1) * n_state_head].copy_from_slice( + &q_data[i * n_state + h_off..i * n_state + h_off + n_state_head], + ); + } + for j in 0..n_kv { + kh[j * n_state_head..(j + 1) * n_state_head].copy_from_slice( + &k_all[j * n_state + h_off..j * n_state + h_off + n_state_head], + ); + vh[j * n_state_head..(j + 1) * n_state_head].copy_from_slice( + &v_all[j * n_state + h_off..j * n_state + h_off + n_state_head], + ); + } + + // Q @ K^T with causal mask + let mut scores = vec![f32::NEG_INFINITY; n_tokens * n_kv]; + for i in 0..n_tokens { + let q_row = &qh[i * n_state_head..(i + 1) * n_state_head]; + let q_pos = n_past + i; + for j in 0..n_kv.min(q_pos + 1) { + let k_row = &kh[j * n_state_head..(j + 1) * n_state_head]; + scores[i * n_kv + j] = Tensor::dot_f32(q_row, k_row) * scale; + } + } + + // Softmax per query + for i in 0..n_tokens { + let row = &mut scores[i * n_kv..(i + 1) * n_kv]; + let max_val = row.iter().cloned().fold(f32::NEG_INFINITY, f32::max); + let mut sum = 0.0f32; + for v in row.iter_mut() { + if *v > f32::NEG_INFINITY { + *v = (*v - max_val).exp(); + } else { + *v = 0.0; + } + sum += *v; + } + if sum > 0.0 { + let inv = 1.0 / sum; + for v in row.iter_mut() { + *v *= inv; + } + } + } + + // scores @ V with transposed V + let mut vh_t = vec![0.0f32; n_state_head * n_kv]; + for j in 0..n_kv { + for d in 0..n_state_head { + vh_t[d * n_kv + j] = vh[j * n_state_head + d]; + } + } + for i in 0..n_tokens { + let s_row = &scores[i * n_kv..(i + 1) * n_kv]; + for d in 0..n_state_head { + let v_col = &vh_t[d * n_kv..(d + 1) * n_kv]; + let out_idx = i * n_state + h_off + d; + debug_assert!(out_idx < n_tokens * n_state); + unsafe { + // Safe: each parallel head writes to a disjoint [h_off, h_off+n_state_head) + // slice in every token row, and out_idx is bounds-checked above. + *out_ptr.ptr().add(out_idx) = Tensor::dot_f32(s_row, v_col); + } + } + } + }); + attn_out + }) + } else { + let mut attn_out = vec![0.0f32; n_tokens * n_state]; + let out_ptr = SendPtr::new(attn_out.as_mut_ptr()); + let q_data = &q.data; + parallel_for(n_head, |h| { + let h_off = h * n_state_head; + let mut qh = vec![0.0f32; n_tokens * n_state_head]; + let mut kh = vec![0.0f32; n_kv * n_state_head]; + let mut vh = vec![0.0f32; n_kv * n_state_head]; + for i in 0..n_tokens { + qh[i * n_state_head..(i + 1) * n_state_head].copy_from_slice( + &q_data[i * n_state + h_off..i * n_state + h_off + n_state_head], + ); + } + for j in 0..n_kv { + kh[j * n_state_head..(j + 1) * n_state_head].copy_from_slice( + &k_all[j * n_state + h_off..j * n_state + h_off + n_state_head], + ); + vh[j * n_state_head..(j + 1) * n_state_head].copy_from_slice( + &v_all[j * n_state + h_off..j * n_state + h_off + n_state_head], + ); + } + + // Q @ K^T with causal mask + let mut scores = vec![f32::NEG_INFINITY; n_tokens * n_kv]; + for i in 0..n_tokens { + let q_row = &qh[i * n_state_head..(i + 1) * n_state_head]; + let q_pos = n_past + i; + for j in 0..n_kv.min(q_pos + 1) { + let k_row = &kh[j * n_state_head..(j + 1) * n_state_head]; + scores[i * n_kv + j] = Tensor::dot_f32(q_row, k_row) * scale; + } + } + + // Softmax per query + for i in 0..n_tokens { + let row = &mut scores[i * n_kv..(i + 1) * n_kv]; + let max_val = row.iter().cloned().fold(f32::NEG_INFINITY, f32::max); + let mut sum = 0.0f32; + for v in row.iter_mut() { + if *v > f32::NEG_INFINITY { + *v = (*v - max_val).exp(); + } else { + *v = 0.0; + } + sum += *v; + } + if sum > 0.0 { + let inv = 1.0 / sum; + for v in row.iter_mut() { + *v *= inv; + } + } + } + + // scores @ V with transposed V + let mut vh_t = vec![0.0f32; n_state_head * n_kv]; + for j in 0..n_kv { + for d in 0..n_state_head { + vh_t[d * n_kv + j] = vh[j * n_state_head + d]; + } + } + for i in 0..n_tokens { + let s_row = &scores[i * n_kv..(i + 1) * n_kv]; + for d in 0..n_state_head { + let v_col = &vh_t[d * n_kv..(d + 1) * n_kv]; + let out_idx = i * n_state + h_off + d; + debug_assert!(out_idx < n_tokens * n_state); + unsafe { + // Safe: per-head writes are disjoint; index bounds asserted above. + *out_ptr.ptr().add(out_idx) = Tensor::dot_f32(s_row, v_col); + } + } + } + }); + attn_out + }; + + let attn_result = Tensor { + data: attn_out, + shape: vec![n_tokens, n_state], + }; + let projected = Tensor::linear_raw(&attn_result, &layer.attn_ln_1_w, &layer.attn_ln_1_b); + cur = Tensor::add(&projected, &residual); + + if crate::metal_backend::is_requested() && n_tokens == 1 { + if let Some(out) = crate::metal_backend::try_decoder_cross_ffn_step_f32( + il, + &cur.data, + n_state, + n_head, + &k_cross.data, + &v_cross.data, + n_audio_ctx_flash, + layer, + ) { + cur = Tensor { + data: out, + shape: vec![n_tokens, n_state], + }; + continue; + } + } + + // === Cross-Attention === + let residual = cur.clone(); + + let normed = Tensor::layer_norm_mul_add( + &cur, + &layer.cross_attn_ln_0_w, + &layer.cross_attn_ln_0_b, + EPS, + ); + + let q = Tensor::linear_raw(&normed, &layer.cross_attn_q_w, &layer.cross_attn_q_b); + + let scale = 1.0 / (n_state_head as f32).sqrt(); + + let attn_out = if crate::metal_backend::is_requested() && n_tokens == 1 { + crate::metal_backend::try_flash_attn_f32_cross_kv_cache( + il, + &q.data, + &k_cross.data, + &v_cross.data, + n_tokens, + n_audio_ctx_flash, + n_head, + n_state_head, + scale, + ) + .or_else(|| { + crate::metal_backend::try_flash_attn_f32_packed( + &q.data, + &k_cross.data, + &v_cross.data, + n_tokens, + n_audio_ctx, + n_head, + n_state_head, + scale, + ) + }) + .unwrap_or_else(|| { + let mut attn_out = vec![0.0f32; n_tokens * n_state]; + let out_ptr = SendPtr::new(attn_out.as_mut_ptr()); + let q_data = &q.data; + let k_cross_data = &k_cross.data; + let v_cross_data = &v_cross.data; + parallel_for(n_head, |h| { + let h_off = h * n_state_head; + let mut qh = vec![0.0f32; n_tokens * n_state_head]; + let mut kh = vec![0.0f32; n_audio_ctx * n_state_head]; + let mut vh = vec![0.0f32; n_audio_ctx * n_state_head]; + for i in 0..n_tokens { + qh[i * n_state_head..(i + 1) * n_state_head].copy_from_slice( + &q_data[i * n_state + h_off..i * n_state + h_off + n_state_head], + ); + } + for j in 0..n_audio_ctx { + kh[j * n_state_head..(j + 1) * n_state_head].copy_from_slice( + &k_cross_data[j * n_state + h_off..j * n_state + h_off + n_state_head], + ); + vh[j * n_state_head..(j + 1) * n_state_head].copy_from_slice( + &v_cross_data[j * n_state + h_off..j * n_state + h_off + n_state_head], + ); + } + + // Q @ K_cross^T + let mut scores = vec![0.0f32; n_tokens * n_audio_ctx]; + for i in 0..n_tokens { + let q_row = &qh[i * n_state_head..(i + 1) * n_state_head]; + for j in 0..n_audio_ctx { + let k_row = &kh[j * n_state_head..(j + 1) * n_state_head]; + scores[i * n_audio_ctx + j] = Tensor::dot_f32(q_row, k_row) * scale; + } + } + + // Softmax + for i in 0..n_tokens { + let row = &mut scores[i * n_audio_ctx..(i + 1) * n_audio_ctx]; + let max_val = row.iter().cloned().fold(f32::NEG_INFINITY, f32::max); + let mut sum = 0.0f32; + for v in row.iter_mut() { + *v = (*v - max_val).exp(); + sum += *v; + } + let inv = 1.0 / sum; + for v in row.iter_mut() { + *v *= inv; + } + } + + // scores @ V_cross with transposed V + let mut vh_t = vec![0.0f32; n_state_head * n_audio_ctx]; + for j in 0..n_audio_ctx { + for d in 0..n_state_head { + vh_t[d * n_audio_ctx + j] = vh[j * n_state_head + d]; + } + } + for i in 0..n_tokens { + let s_row = &scores[i * n_audio_ctx..(i + 1) * n_audio_ctx]; + for d in 0..n_state_head { + let v_col = &vh_t[d * n_audio_ctx..(d + 1) * n_audio_ctx]; + let out_idx = i * n_state + h_off + d; + debug_assert!(out_idx < n_tokens * n_state); + unsafe { + // Safe: per-head writes are disjoint; index bounds asserted above. + *out_ptr.ptr().add(out_idx) = Tensor::dot_f32(s_row, v_col); + } + } + } + }); + attn_out + }) + } else { + let mut attn_out = vec![0.0f32; n_tokens * n_state]; + let out_ptr = SendPtr::new(attn_out.as_mut_ptr()); + let q_data = &q.data; + let k_cross_data = &k_cross.data; + let v_cross_data = &v_cross.data; + parallel_for(n_head, |h| { + let h_off = h * n_state_head; + let mut qh = vec![0.0f32; n_tokens * n_state_head]; + let mut kh = vec![0.0f32; n_audio_ctx * n_state_head]; + let mut vh = vec![0.0f32; n_audio_ctx * n_state_head]; + for i in 0..n_tokens { + qh[i * n_state_head..(i + 1) * n_state_head].copy_from_slice( + &q_data[i * n_state + h_off..i * n_state + h_off + n_state_head], + ); + } + for j in 0..n_audio_ctx { + kh[j * n_state_head..(j + 1) * n_state_head].copy_from_slice( + &k_cross_data[j * n_state + h_off..j * n_state + h_off + n_state_head], + ); + vh[j * n_state_head..(j + 1) * n_state_head].copy_from_slice( + &v_cross_data[j * n_state + h_off..j * n_state + h_off + n_state_head], + ); + } + + // Q @ K_cross^T + let mut scores = vec![0.0f32; n_tokens * n_audio_ctx]; + for i in 0..n_tokens { + let q_row = &qh[i * n_state_head..(i + 1) * n_state_head]; + for j in 0..n_audio_ctx { + let k_row = &kh[j * n_state_head..(j + 1) * n_state_head]; + scores[i * n_audio_ctx + j] = Tensor::dot_f32(q_row, k_row) * scale; + } + } + + // Softmax + for i in 0..n_tokens { + let row = &mut scores[i * n_audio_ctx..(i + 1) * n_audio_ctx]; + let max_val = row.iter().cloned().fold(f32::NEG_INFINITY, f32::max); + let mut sum = 0.0f32; + for v in row.iter_mut() { + *v = (*v - max_val).exp(); + sum += *v; + } + let inv = 1.0 / sum; + for v in row.iter_mut() { + *v *= inv; + } + } + + // scores @ V_cross with transposed V + let mut vh_t = vec![0.0f32; n_state_head * n_audio_ctx]; + for j in 0..n_audio_ctx { + for d in 0..n_state_head { + vh_t[d * n_audio_ctx + j] = vh[j * n_state_head + d]; + } + } + for i in 0..n_tokens { + let s_row = &scores[i * n_audio_ctx..(i + 1) * n_audio_ctx]; + for d in 0..n_state_head { + let v_col = &vh_t[d * n_audio_ctx..(d + 1) * n_audio_ctx]; + let out_idx = i * n_state + h_off + d; + debug_assert!(out_idx < n_tokens * n_state); + unsafe { + // Safe: per-head writes are disjoint; index bounds asserted above. + *out_ptr.ptr().add(out_idx) = Tensor::dot_f32(s_row, v_col); + } + } + } + }); + attn_out + }; + + let attn_result = Tensor { + data: attn_out, + shape: vec![n_tokens, n_state], + }; + let projected = Tensor::linear_raw( + &attn_result, + &layer.cross_attn_ln_1_w, + &layer.cross_attn_ln_1_b, + ); + cur = Tensor::add(&projected, &residual); + + // === Feed-Forward === + let residual = cur.clone(); + + let normed = Tensor::layer_norm_mul_add(&cur, &layer.mlp_ln_w, &layer.mlp_ln_b, EPS); + + let mut ff = Tensor::linear_raw(&normed, &layer.mlp_0_w, &layer.mlp_0_b); + ff = Tensor::gelu(&ff); + ff = Tensor::linear_raw(&ff, &layer.mlp_1_w, &layer.mlp_1_b); + + cur = Tensor::add(&ff, &residual); + } + + // Final layer norm + let cur = Tensor::layer_norm_mul_add(&cur, &model.d_ln_w, &model.d_ln_b, EPS); + + // Project to vocab logits: cur [n_tokens, n_state] @ d_te^T [n_state, n_vocab] -> [n_tokens, n_vocab] + // d_te is [n_vocab, n_state], so we compute cur @ d_te^T + Tensor::matmul_t(&cur, &model.d_te) +} + +/// Increment KV cache position counter after a decode step. +pub fn advance_kv_cache(kv_cache: &mut KvCache, n_tokens: usize) { + kv_cache.n_past += n_tokens; +} diff --git a/libs/voice2/src/cpu/encoder.rs b/libs/voice2/src/cpu/encoder.rs new file mode 100644 index 0000000000..66313d92c6 --- /dev/null +++ b/libs/voice2/src/cpu/encoder.rs @@ -0,0 +1,357 @@ +use crate::model::WhisperModel; +use crate::tensor::{parallel_for, RawTensor, Tensor}; + +const EPS: f32 = 1e-5; + +/// Multi-head self-attention with parallel head computation. +/// x: [seq_len, n_state] +/// Returns: [seq_len, n_state] +fn multi_head_attention( + x: &Tensor, + q_w: &RawTensor, + q_b: &Tensor, + k_w: &RawTensor, + v_w: &RawTensor, + v_b: &Tensor, + out_w: &RawTensor, + out_b: &Tensor, + n_head: usize, +) -> Tensor { + let seq_len = x.shape[0]; + let n_state = x.shape[1]; + let n_state_head = n_state / n_head; + let scale = 1.0 / (n_state_head as f32).sqrt(); + + let xq = if Tensor::activation_q8_enabled() && seq_len > 2 && n_state % 32 == 0 { + Some(Tensor::prequantize_rows_q8(x, n_state)) + } else { + None + }; + + // Q, K, V projections: [seq_len, n_state] + let q = Tensor::linear_raw_with_prequant(x, q_w, xq.as_deref(), q_b); + let k = Tensor::matmul_raw_with_prequant(x, k_w, xq.as_deref()); + let v = Tensor::linear_raw_with_prequant(x, v_w, xq.as_deref(), v_b); + + if crate::metal_backend::is_requested() { + if let Some(out) = crate::metal_backend::try_flash_attn_f32_packed( + &q.data, + &k.data, + &v.data, + seq_len, + seq_len, + n_head, + n_state_head, + scale, + ) { + let attn_out = Tensor { + data: out, + shape: vec![seq_len, n_state], + }; + + return Tensor::linear_raw(&attn_out, out_w, out_b); + } + } + + let mut out = vec![0.0f32; seq_len * n_state]; + let out_ptr = crate::tensor::SendPtr::new(out.as_mut_ptr()); + let q_data = &q.data; + let k_data = &k.data; + let v_data = &v.data; + + parallel_for(n_head, |h| { + let h_off = h * n_state_head; + let out_data = unsafe { std::slice::from_raw_parts_mut(out_ptr.ptr(), seq_len * n_state) }; + + // Extract contiguous per-head Q, K, V for SIMD-friendly access + let mut qh = vec![0.0f32; seq_len * n_state_head]; + let mut kh = vec![0.0f32; seq_len * n_state_head]; + let mut vh = vec![0.0f32; seq_len * n_state_head]; + for i in 0..seq_len { + qh[i * n_state_head..(i + 1) * n_state_head] + .copy_from_slice(&q_data[i * n_state + h_off..i * n_state + h_off + n_state_head]); + kh[i * n_state_head..(i + 1) * n_state_head] + .copy_from_slice(&k_data[i * n_state + h_off..i * n_state + h_off + n_state_head]); + vh[i * n_state_head..(i + 1) * n_state_head] + .copy_from_slice(&v_data[i * n_state + h_off..i * n_state + h_off + n_state_head]); + } + + // Transpose V head to [n_state_head, seq_len] for SIMD dot + let mut vh_t = vec![0.0f32; n_state_head * seq_len]; + for i in 0..seq_len { + for d in 0..n_state_head { + vh_t[d * seq_len + i] = vh[i * n_state_head + d]; + } + } + + // Row-streamed attention: avoids allocating a full [seq_len, seq_len] score matrix. + let mut scores_row = vec![0.0f32; seq_len]; + for i in 0..seq_len { + let q_row = &qh[i * n_state_head..(i + 1) * n_state_head]; + + let mut max_val = f32::NEG_INFINITY; + for j in 0..seq_len { + let k_row = &kh[j * n_state_head..(j + 1) * n_state_head]; + let s = Tensor::dot_f32(q_row, k_row) * scale; + scores_row[j] = s; + if s > max_val { + max_val = s; + } + } + + let mut sum = 0.0f32; + for v in scores_row.iter_mut() { + *v = (*v - max_val).exp(); + sum += *v; + } + let inv = 1.0 / sum; + for v in scores_row.iter_mut() { + *v *= inv; + } + + for d in 0..n_state_head { + let v_col = &vh_t[d * seq_len..(d + 1) * seq_len]; + out_data[i * n_state + h_off + d] = Tensor::dot_f32(&scores_row, v_col); + } + } + }); + + let attn_out = Tensor { + data: out, + shape: vec![seq_len, n_state], + }; + + // Output projection + Tensor::linear_raw(&attn_out, out_w, out_b) +} + +/// Run the whisper encoder on a mel spectrogram chunk. +/// mel_data: slice of mel spectrogram for this chunk, shape [n_mels, 2*n_ctx] +/// Returns encoder output: [n_ctx, n_state] +pub fn encode(model: &WhisperModel, mel_data: &[f32], n_ctx: usize) -> Tensor { + let n_mels = model.hparams.n_mels as usize; + let n_state = model.hparams.n_audio_state as usize; + let n_head = model.hparams.n_audio_head as usize; + + // mel input: [n_mels, 2*n_ctx] + let mel = Tensor { + data: mel_data.to_vec(), + shape: vec![n_mels, 2 * n_ctx], + }; + + // Conv1 + GELU: [n_mels, 2*n_ctx] -> [n_state, 2*n_ctx] + let _tc = std::time::Instant::now(); + let mut cur = Tensor::conv1d_raw(&mel, &model.e_conv_1_w, &model.e_conv_1_b, 1); + cur = Tensor::gelu(&cur); + + // Conv2 + GELU: [n_state, 2*n_ctx] -> [n_state, n_ctx] (stride 2) + cur = Tensor::conv1d_raw(&cur, &model.e_conv_2_w, &model.e_conv_2_b, 2); + cur = Tensor::gelu(&cur); + crate::PROF_ENC_CONV.fetch_add( + _tc.elapsed().as_nanos() as u64, + std::sync::atomic::Ordering::Relaxed, + ); + + // cur is [n_state, n_ctx], transpose to [n_ctx, n_state] + cur = Tensor::transpose_2d(&cur); + + // Add positional embedding: [n_ctx, n_state] + // e_pe is [n_audio_ctx, n_state], take first n_ctx rows + let pe_data: Vec = model.e_pe.data[..n_ctx * n_state].to_vec(); + let pe = Tensor { + data: pe_data, + shape: vec![n_ctx, n_state], + }; + cur = Tensor::add(&cur, &pe); + + if crate::metal_backend::is_requested() { + let t_stack = std::time::Instant::now(); + if let Some(out) = crate::metal_backend::try_encoder_stack_f32( + &cur.data, + cur.shape[0], + cur.shape[1], + n_head, + &model.encoder_layers, + &model.e_ln_w.data, + &model.e_ln_b.data, + ) { + let dt = t_stack.elapsed().as_nanos() as u64; + crate::PROF_ENC_ATTN.fetch_add(dt / 2, std::sync::atomic::Ordering::Relaxed); + crate::PROF_ENC_ELEM.fetch_add(dt - dt / 2, std::sync::atomic::Ordering::Relaxed); + return Tensor { + data: out, + shape: cur.shape.clone(), + }; + } + } + + // Transformer encoder blocks + for layer in &model.encoder_layers { + if crate::metal_backend::is_requested() { + let t_layer = std::time::Instant::now(); + if let Some(out) = crate::metal_backend::try_encoder_layer_f32( + &cur.data, + cur.shape[0], + cur.shape[1], + n_head, + &layer.attn_ln_0_w.data, + &layer.attn_ln_0_b.data, + &layer.attn_q_w.data, + layer.attn_q_w.ggml_type, + &layer.attn_q_b.data, + &layer.attn_k_w.data, + layer.attn_k_w.ggml_type, + &layer.attn_v_w.data, + layer.attn_v_w.ggml_type, + &layer.attn_v_b.data, + &layer.attn_ln_1_w.data, + layer.attn_ln_1_w.ggml_type, + &layer.attn_ln_1_b.data, + &layer.mlp_ln_w.data, + &layer.mlp_ln_b.data, + &layer.mlp_0_w.data, + layer.mlp_0_w.ggml_type, + &layer.mlp_0_b.data, + &layer.mlp_1_w.data, + layer.mlp_1_w.ggml_type, + &layer.mlp_1_b.data, + ) { + cur = Tensor { + data: out, + shape: cur.shape.clone(), + }; + let dt = t_layer.elapsed().as_nanos() as u64; + crate::PROF_ENC_ATTN.fetch_add(dt / 2, std::sync::atomic::Ordering::Relaxed); + crate::PROF_ENC_ELEM.fetch_add(dt - dt / 2, std::sync::atomic::Ordering::Relaxed); + continue; + } + } + + // Self-attention block + let t_attn = std::time::Instant::now(); + let mut attn_done = false; + if crate::metal_backend::is_requested() { + if let Some(out) = crate::metal_backend::try_encoder_attn_block_f32( + &cur.data, + cur.shape[0], + cur.shape[1], + n_head, + &layer.attn_ln_0_w.data, + &layer.attn_ln_0_b.data, + &layer.attn_q_w.data, + layer.attn_q_w.ggml_type, + &layer.attn_q_b.data, + &layer.attn_k_w.data, + layer.attn_k_w.ggml_type, + &layer.attn_v_w.data, + layer.attn_v_w.ggml_type, + &layer.attn_v_b.data, + &layer.attn_ln_1_w.data, + layer.attn_ln_1_w.ggml_type, + &layer.attn_ln_1_b.data, + ) { + cur = Tensor { + data: out, + shape: cur.shape.clone(), + }; + attn_done = true; + } + } + if !attn_done { + let residual = cur.clone(); + + // Layer norm + let normed = + Tensor::layer_norm_mul_add(&cur, &layer.attn_ln_0_w, &layer.attn_ln_0_b, EPS); + + // Multi-head self-attention + let attn_out = multi_head_attention( + &normed, + &layer.attn_q_w, + &layer.attn_q_b, + &layer.attn_k_w, + &layer.attn_v_w, + &layer.attn_v_b, + &layer.attn_ln_1_w, + &layer.attn_ln_1_b, + n_head, + ); + + cur = Tensor::add(&attn_out, &residual); + } + crate::PROF_ENC_ATTN.fetch_add( + t_attn.elapsed().as_nanos() as u64, + std::sync::atomic::Ordering::Relaxed, + ); + + // Feed-forward block + let t_elem = std::time::Instant::now(); + let mut ffn_done = false; + if crate::metal_backend::is_requested() { + if let Some(out) = crate::metal_backend::try_encoder_ffn_block_f32( + &cur.data, + cur.shape[0], + cur.shape[1], + &layer.mlp_ln_w.data, + &layer.mlp_ln_b.data, + &layer.mlp_0_w.data, + layer.mlp_0_w.ggml_type, + &layer.mlp_0_b.data, + &layer.mlp_1_w.data, + layer.mlp_1_w.ggml_type, + &layer.mlp_1_b.data, + ) { + cur = Tensor { + data: out, + shape: cur.shape.clone(), + }; + ffn_done = true; + } + } + if !ffn_done { + let residual = cur.clone(); + + let normed = Tensor::layer_norm_mul_add(&cur, &layer.mlp_ln_w, &layer.mlp_ln_b, EPS); + + // MLP: linear -> gelu -> linear + let mut ff = Tensor::linear_raw(&normed, &layer.mlp_0_w, &layer.mlp_0_b); + ff = Tensor::gelu(&ff); + ff = Tensor::linear_raw(&ff, &layer.mlp_1_w, &layer.mlp_1_b); + + cur = Tensor::add(&ff, &residual); + } + crate::PROF_ENC_ELEM.fetch_add( + t_elem.elapsed().as_nanos() as u64, + std::sync::atomic::Ordering::Relaxed, + ); + } + + // Final layer norm + Tensor::layer_norm_mul_add(&cur, &model.e_ln_w, &model.e_ln_b, EPS) +} + +/// Pre-compute cross-attention K and V for all decoder layers. +/// encoder_out: [n_ctx, n_state] +/// Returns: Vec of (K, V) pairs, one per decoder layer. +/// K: [n_ctx, n_state], V: [n_ctx, n_state] +pub fn compute_cross_kv(model: &WhisperModel, encoder_out: &Tensor) -> Vec<(Tensor, Tensor)> { + let mut cross_kv = Vec::new(); + let n_state = encoder_out.shape[1]; + let batch = encoder_out.shape[0]; + let xq = if Tensor::activation_q8_enabled() && batch > 2 && n_state % 32 == 0 { + Some(Tensor::prequantize_rows_q8(encoder_out, n_state)) + } else { + None + }; + for layer in &model.decoder_layers { + let k = Tensor::matmul_raw_with_prequant(encoder_out, &layer.cross_attn_k_w, xq.as_deref()); + let v = Tensor::linear_raw_with_prequant( + encoder_out, + &layer.cross_attn_v_w, + xq.as_deref(), + &layer.cross_attn_v_b, + ); + cross_kv.push((k, v)); + } + cross_kv +} diff --git a/libs/voice2/src/cpu/mel.rs b/libs/voice2/src/cpu/mel.rs new file mode 100644 index 0000000000..aad42f2286 --- /dev/null +++ b/libs/voice2/src/cpu/mel.rs @@ -0,0 +1,233 @@ +use crate::model::MelFilters; + +pub const WHISPER_SAMPLE_RATE: usize = 16000; +pub const WHISPER_N_FFT: usize = 400; +pub const WHISPER_HOP_LENGTH: usize = 160; +pub const WHISPER_CHUNK_SIZE: usize = 30; // seconds + +/// Precomputed sine/cosine tables and Hann window for FFT. +pub struct MelCache { + sin_vals: [f32; WHISPER_N_FFT], + cos_vals: [f32; WHISPER_N_FFT], + hann: [f32; WHISPER_N_FFT], +} + +impl MelCache { + pub fn new() -> Self { + let mut sin_vals = [0.0f32; WHISPER_N_FFT]; + let mut cos_vals = [0.0f32; WHISPER_N_FFT]; + let mut hann = [0.0f32; WHISPER_N_FFT]; + + for i in 0..WHISPER_N_FFT { + let theta = 2.0 * std::f64::consts::PI * i as f64 / WHISPER_N_FFT as f64; + sin_vals[i] = theta.sin() as f32; + cos_vals[i] = theta.cos() as f32; + } + + // Periodic Hann window + for i in 0..WHISPER_N_FFT { + hann[i] = (0.5 + * (1.0 - (2.0 * std::f64::consts::PI * i as f64 / WHISPER_N_FFT as f64).cos())) + as f32; + } + + MelCache { + sin_vals, + cos_vals, + hann, + } + } +} + +/// Naive DFT for non-power-of-2 sizes (fallback). +fn dft(input: &[f32], n: usize, out: &mut [f32], cache: &MelCache) { + let step = WHISPER_N_FFT / n; + for k in 0..n { + let mut re = 0.0f32; + let mut im = 0.0f32; + for j in 0..n { + let idx = (k * j * step) % WHISPER_N_FFT; + re += input[j] * cache.cos_vals[idx]; + im -= input[j] * cache.sin_vals[idx]; + } + out[k * 2] = re; + out[k * 2 + 1] = im; + } +} + +/// Cooley-Tukey FFT. Input is real-valued, output is complex (interleaved re, im). +/// `work` must have space for at least 4*N floats beyond `input`. +fn fft(input: &mut [f32], n: usize, out: &mut [f32], cache: &MelCache) { + if n == 1 { + out[0] = input[0]; + out[1] = 0.0; + return; + } + + let half = n / 2; + if n - half * 2 == 1 { + // Odd size, fallback to DFT + dft(input, n, out, cache); + return; + } + + // Split even/odd into temp space after input + let input_ptr = input.as_mut_ptr(); + unsafe { + let even_ptr = input_ptr.add(n); + for i in 0..half { + *even_ptr.add(i) = *input_ptr.add(2 * i); + } + let even_fft_ptr = out.as_mut_ptr().add(2 * n); + let even_slice = std::slice::from_raw_parts_mut(even_ptr, half + 4 * half); + let even_out = std::slice::from_raw_parts_mut(even_fft_ptr, 2 * half + 4 * half); + fft(even_slice, half, even_out, cache); + + let odd_ptr = even_ptr; // reuse + for i in 0..half { + *odd_ptr.add(i) = *input_ptr.add(2 * i + 1); + } + let odd_fft_ptr = even_fft_ptr.add(n); // after even_fft + let odd_slice = std::slice::from_raw_parts_mut(odd_ptr, half + 4 * half); + let odd_out = std::slice::from_raw_parts_mut(odd_fft_ptr, 2 * half + 4 * half); + fft(odd_slice, half, odd_out, cache); + + // Combine + let step = WHISPER_N_FFT / n; + for k in 0..half { + let idx = k * step; + let re = cache.cos_vals[idx]; + let im = -cache.sin_vals[idx]; + + let re_odd = *odd_fft_ptr.add(2 * k); + let im_odd = *odd_fft_ptr.add(2 * k + 1); + + let out_ptr = out.as_mut_ptr(); + *out_ptr.add(2 * k) = *even_fft_ptr.add(2 * k) + re * re_odd - im * im_odd; + *out_ptr.add(2 * k + 1) = *even_fft_ptr.add(2 * k + 1) + re * im_odd + im * re_odd; + + *out_ptr.add(2 * (k + half)) = *even_fft_ptr.add(2 * k) - re * re_odd + im * im_odd; + *out_ptr.add(2 * (k + half) + 1) = + *even_fft_ptr.add(2 * k + 1) - re * im_odd - im * re_odd; + } + } +} + +/// Compute log mel spectrogram from PCM samples. +/// Returns mel data in shape [n_mel, n_len]. +pub fn log_mel_spectrogram( + samples: &[f32], + filters: &MelFilters, + n_threads: usize, +) -> (Vec, usize, usize, usize) { + let cache = MelCache::new(); + let n_samples = samples.len(); + let frame_size = WHISPER_N_FFT; + let frame_step = WHISPER_HOP_LENGTH; + let n_mel = filters.n_mel as usize; + let n_fft = filters.n_fft as usize; // should be 1 + WHISPER_N_FFT/2 = 201 + + // Padding: 30 seconds of zeros + reflective pad at edges + let stage_1_pad = WHISPER_SAMPLE_RATE * WHISPER_CHUNK_SIZE; // 480000 + let stage_2_pad = frame_size / 2; // 200 + + let padded_len = n_samples + stage_1_pad + stage_2_pad * 2; + let mut padded = vec![0.0f32; padded_len]; + + // Copy audio with stage_2_pad offset + padded[stage_2_pad..stage_2_pad + n_samples].copy_from_slice(samples); + + // Reflective pad at beginning + for i in 0..stage_2_pad.min(n_samples) { + padded[stage_2_pad - 1 - i] = samples[i + 1.min(n_samples - 1)]; + } + + // Zero pad the rest (already zeroed) + + let n_len = (padded_len - frame_size) / frame_step; + let n_len_org = 1 + (n_samples + stage_2_pad - frame_size) / frame_step; + + let mut mel_data = vec![0.0f32; n_mel * n_len]; + + // Process each frame (single-threaded for now, n_threads ignored) + let _ = n_threads; + let n_samples_padded = n_samples + stage_2_pad; + + // Work buffers for FFT + let mut fft_in = vec![0.0f32; frame_size * 4]; // extra space for FFT recursion + let mut fft_out = vec![0.0f32; frame_size * 8]; + + for i in 0..n_len { + let offset = i * frame_step; + + // Apply Hann window + let end = (n_samples_padded).min(offset + frame_size); + for j in 0..frame_size { + if offset + j < end { + fft_in[j] = cache.hann[j] * padded[offset + j]; + } else { + fft_in[j] = 0.0; + } + } + // Clear extra space + for j in frame_size..fft_in.len() { + fft_in[j] = 0.0; + } + + // FFT + fft(&mut fft_in, frame_size, &mut fft_out, &cache); + + // Magnitude squared + for j in 0..n_fft { + fft_out[j] = fft_out[2 * j] * fft_out[2 * j] + fft_out[2 * j + 1] * fft_out[2 * j + 1]; + } + + // Apply mel filterbank + for j in 0..n_mel { + let mut sum = 0.0f64; + let filter_row = &filters.data[j * n_fft..(j + 1) * n_fft]; + // Unrolled by 4 + let mut k = 0; + while k + 3 < n_fft { + sum += fft_out[k] as f64 * filter_row[k] as f64 + + fft_out[k + 1] as f64 * filter_row[k + 1] as f64 + + fft_out[k + 2] as f64 * filter_row[k + 2] as f64 + + fft_out[k + 3] as f64 * filter_row[k + 3] as f64; + k += 4; + } + while k < n_fft { + sum += fft_out[k] as f64 * filter_row[k] as f64; + k += 1; + } + mel_data[j * n_len + i] = sum.max(1e-10).log10() as f32; + } + } + + // Fill remaining frames (beyond audio) with log(1e-10) + let log_min = (1e-10f64).log10() as f32; + for i in 0..n_len { + if i * frame_step >= n_samples_padded { + for j in 0..n_mel { + mel_data[j * n_len + i] = log_min; + } + } + } + + // Clamping and normalization + let mut max_val = f32::NEG_INFINITY; + for &v in &mel_data { + if v > max_val { + max_val = v; + } + } + max_val -= 8.0; + + for v in &mut mel_data { + if *v < max_val { + *v = max_val; + } + *v = (*v + 4.0) / 4.0; + } + + (mel_data, n_mel, n_len, n_len_org) +} diff --git a/libs/voice2/src/cpu/model.rs b/libs/voice2/src/cpu/model.rs new file mode 100644 index 0000000000..fe8b1f83c5 --- /dev/null +++ b/libs/voice2/src/cpu/model.rs @@ -0,0 +1,495 @@ +use crate::quant::*; +use crate::tensor::{RawTensor, Tensor}; +use std::collections::HashMap; +use std::io::{self, Read, Seek}; + +const GGML_FILE_MAGIC: u32 = 0x67676d6c; + +fn should_preserve_raw_weight_type() -> bool { + crate::settings::PRESERVE_RAW_WEIGHT_TYPE +} + +#[derive(Debug, Clone)] +pub struct WhisperHparams { + pub n_vocab: i32, + pub n_audio_ctx: i32, + pub n_audio_state: i32, + pub n_audio_head: i32, + pub n_audio_layer: i32, + pub n_text_ctx: i32, + pub n_text_state: i32, + pub n_text_head: i32, + pub n_text_layer: i32, + pub n_mels: i32, + pub ftype: i32, +} + +#[derive(Debug, Clone)] +pub struct MelFilters { + pub n_mel: i32, + pub n_fft: i32, + pub data: Vec, +} + +#[derive(Debug, Clone)] +pub struct Vocab { + pub n_vocab: i32, + pub token_to_id: HashMap, + pub id_to_token: HashMap, + // special tokens + pub token_eot: i32, + pub token_sot: i32, + pub token_translate: i32, + pub token_transcribe: i32, + pub token_solm: i32, + pub token_prev: i32, + pub token_nosp: i32, + pub token_not: i32, + pub token_beg: i32, +} + +impl Vocab { + pub fn is_multilingual(&self) -> bool { + self.n_vocab >= 51865 + } + + pub fn num_languages(&self) -> i32 { + self.n_vocab - 51765 - if self.is_multilingual() { 1 } else { 0 } + } + + pub fn token_to_str(&self, id: i32) -> &str { + self.id_to_token.get(&id).map(|s| s.as_str()).unwrap_or("") + } +} + +pub struct EncoderLayer { + pub attn_ln_0_w: Tensor, // layer norm scale (element-wise) + pub attn_ln_0_b: Tensor, + pub attn_ln_1_w: RawTensor, // attn.out.weight (matmul) + pub attn_ln_1_b: Tensor, // attn.out.bias + pub attn_q_w: RawTensor, + pub attn_q_b: Tensor, + pub attn_k_w: RawTensor, + pub attn_v_w: RawTensor, + pub attn_v_b: Tensor, + pub mlp_ln_w: Tensor, // layer norm scale (element-wise) + pub mlp_ln_b: Tensor, + pub mlp_0_w: RawTensor, + pub mlp_0_b: Tensor, + pub mlp_1_w: RawTensor, + pub mlp_1_b: Tensor, +} + +pub struct DecoderLayer { + // self-attention + pub attn_ln_0_w: Tensor, // layer norm (element-wise) + pub attn_ln_0_b: Tensor, + pub attn_ln_1_w: RawTensor, // attn.out.weight (matmul) + pub attn_ln_1_b: Tensor, + pub attn_q_w: RawTensor, + pub attn_q_b: Tensor, + pub attn_k_w: RawTensor, + pub attn_v_w: RawTensor, + pub attn_v_b: Tensor, + // cross-attention + pub cross_attn_ln_0_w: Tensor, // layer norm (element-wise) + pub cross_attn_ln_0_b: Tensor, + pub cross_attn_ln_1_w: RawTensor, // cross_attn.out.weight (matmul) + pub cross_attn_ln_1_b: Tensor, + pub cross_attn_q_w: RawTensor, + pub cross_attn_q_b: Tensor, + pub cross_attn_k_w: RawTensor, + pub cross_attn_v_w: RawTensor, + pub cross_attn_v_b: Tensor, + // feed-forward + pub mlp_ln_w: Tensor, // layer norm (element-wise) + pub mlp_ln_b: Tensor, + pub mlp_0_w: RawTensor, + pub mlp_0_b: Tensor, + pub mlp_1_w: RawTensor, + pub mlp_1_b: Tensor, +} + +pub struct WhisperModel { + pub hparams: WhisperHparams, + pub filters: MelFilters, + pub vocab: Vocab, + pub wtype: u32, + + // encoder + pub e_pe: Tensor, // positional embedding [n_audio_ctx, n_audio_state] + pub e_conv_1_w: RawTensor, // conv1.weight [n_audio_state, n_mels, 3] + pub e_conv_1_b: Tensor, // conv1.bias [n_audio_state] + pub e_conv_2_w: RawTensor, // conv2.weight [n_audio_state, n_audio_state, 3] + pub e_conv_2_b: Tensor, // conv2.bias [n_audio_state] + pub e_ln_w: Tensor, // ln_post.weight [n_audio_state] + pub e_ln_b: Tensor, // ln_post.bias [n_audio_state] + pub encoder_layers: Vec, + + // decoder + pub d_pe: Tensor, // positional embedding [n_text_ctx, n_text_state] + pub d_te: Tensor, // token embedding [n_vocab, n_text_state] + pub d_ln_w: Tensor, // ln.weight [n_text_state] + pub d_ln_b: Tensor, // ln.bias [n_text_state] + pub decoder_layers: Vec, +} + +fn read_i32(r: &mut R) -> io::Result { + let mut buf = [0u8; 4]; + r.read_exact(&mut buf)?; + Ok(i32::from_le_bytes(buf)) +} + +fn read_u32(r: &mut R) -> io::Result { + let mut buf = [0u8; 4]; + r.read_exact(&mut buf)?; + Ok(u32::from_le_bytes(buf)) +} + +fn read_f32_vec(r: &mut R, n: usize) -> io::Result> { + let mut buf = vec![0u8; n * 4]; + r.read_exact(&mut buf)?; + let data: Vec = buf + .chunks_exact(4) + .map(|c| f32::from_le_bytes([c[0], c[1], c[2], c[3]])) + .collect(); + Ok(data) +} + +fn read_string(r: &mut R, len: usize) -> io::Result { + let mut buf = vec![0u8; len]; + r.read_exact(&mut buf)?; + Ok(String::from_utf8_lossy(&buf).to_string()) +} + +fn ggml_type_for_ftype(ftype: i32) -> u32 { + // ftype encodes the quantization format for the weight tensors + match ftype % 16384 { + // The quantization version factor + 0 => GGML_TYPE_F32, + 1 => GGML_TYPE_F16, + 2 => GGML_TYPE_Q4_0, + 3 => GGML_TYPE_Q4_1, + 6 => GGML_TYPE_Q5_0, + 7 => GGML_TYPE_Q5_1, + 8 => GGML_TYPE_Q8_0, + _ => GGML_TYPE_F16, // fallback + } +} + +impl WhisperModel { + pub fn load(r: &mut R) -> io::Result { + // 1. Magic + let magic = read_u32(r)?; + if magic != GGML_FILE_MAGIC { + return Err(io::Error::new( + io::ErrorKind::InvalidData, + format!("bad magic: 0x{:x}", magic), + )); + } + + // 2. Hyperparameters + let hparams = WhisperHparams { + n_vocab: read_i32(r)?, + n_audio_ctx: read_i32(r)?, + n_audio_state: read_i32(r)?, + n_audio_head: read_i32(r)?, + n_audio_layer: read_i32(r)?, + n_text_ctx: read_i32(r)?, + n_text_state: read_i32(r)?, + n_text_head: read_i32(r)?, + n_text_layer: read_i32(r)?, + n_mels: read_i32(r)?, + ftype: read_i32(r)?, + }; + + let wtype = ggml_type_for_ftype(hparams.ftype); + + // 3. Mel filters + let n_mel = read_i32(r)?; + let n_fft = read_i32(r)?; + let mel_data = read_f32_vec(r, (n_mel * n_fft) as usize)?; + let filters = MelFilters { + n_mel, + n_fft, + data: mel_data, + }; + + // 4. Vocabulary + let n_vocab_file = read_i32(r)?; + let mut token_to_id = HashMap::new(); + let mut id_to_token = HashMap::new(); + + for i in 0..n_vocab_file { + let len = read_u32(r)? as usize; + let word = if len > 0 { + read_string(r, len)? + } else { + String::new() + }; + token_to_id.insert(word.clone(), i); + id_to_token.insert(i, word); + } + + let mut vocab = Vocab { + n_vocab: hparams.n_vocab, + token_to_id, + id_to_token, + token_eot: 50256, + token_sot: 50257, + token_translate: 50357, + token_transcribe: 50358, + token_solm: 50359, + token_prev: 50360, + token_nosp: 50361, + token_not: 50362, + token_beg: 50363, + }; + + // Adjust for multilingual models + if vocab.is_multilingual() { + vocab.token_eot += 1; + vocab.token_sot += 1; + let dt = vocab.num_languages() - 98; + vocab.token_translate += dt; + vocab.token_transcribe += dt; + vocab.token_solm += dt; + vocab.token_prev += dt; + vocab.token_nosp += dt; + vocab.token_not += dt; + vocab.token_beg += dt; + } + + // Add extra tokens if needed + if n_vocab_file < hparams.n_vocab { + for i in n_vocab_file..hparams.n_vocab { + let word = if i > vocab.token_beg { + format!("[_TT_{}]", i - vocab.token_beg) + } else if i == vocab.token_eot { + "[_EOT_]".into() + } else if i == vocab.token_sot { + "[_SOT_]".into() + } else if i == vocab.token_beg { + "[_BEG_]".into() + } else { + format!("[_extra_token_{}]", i) + }; + vocab.token_to_id.insert(word.clone(), i); + vocab.id_to_token.insert(i, word); + } + } + + // 5. Load tensors + let mut tensors: HashMap = HashMap::new(); + + loop { + let n_dims = match read_i32(r) { + Ok(v) => v, + Err(_) => break, + }; + let name_len = match read_i32(r) { + Ok(v) => v, + Err(_) => break, + }; + let ttype = match read_i32(r) { + Ok(v) => v as u32, + Err(_) => break, + }; + + let mut ne = vec![0i32; n_dims as usize]; + for i in 0..n_dims as usize { + ne[i] = read_i32(r)?; + } + + let name = read_string(r, name_len as usize)?; + + let nelements: usize = ne.iter().map(|&x| x as usize).product(); + let be = block_elements(ttype); + let bs = block_size(ttype); + let nbytes = (nelements / be) * bs; + + let mut data = vec![0u8; nbytes]; + r.read_exact(&mut data)?; + + // Keep GGML shape order (dim 0 = innermost/contiguous dimension). + // For 2D: shape[0] = columns (contiguous), shape[1] = rows + // For matmul weights [in, out]: row i of out_features has in_features contiguous elements + let shape: Vec = ne.iter().map(|&x| x as usize).collect(); + + tensors.insert( + name, + RawTensor { + data, + shape, + ggml_type: ttype, + }, + ); + } + + // 6. Build model struct from loaded tensors + fn take_f32(tensors: &mut HashMap, name: &str) -> Tensor { + tensors + .remove(name) + .unwrap_or_else(|| panic!("missing tensor: {}", name)) + .to_f32() + } + let preserve_wtype = should_preserve_raw_weight_type(); + fn take_raw( + tensors: &mut HashMap, + name: &str, + preserve_wtype: bool, + ) -> RawTensor { + let t = tensors + .remove(name) + .unwrap_or_else(|| panic!("missing tensor: {}", name)); + if preserve_wtype { + t + } else { + t.to_q8_0() + } + } + + // Encoder global tensors + let e_pe = take_f32(&mut tensors, "encoder.positional_embedding"); + let e_conv_1_w = take_raw(&mut tensors, "encoder.conv1.weight", preserve_wtype); + let e_conv_1_b = take_f32(&mut tensors, "encoder.conv1.bias"); + let e_conv_2_w = take_raw(&mut tensors, "encoder.conv2.weight", preserve_wtype); + let e_conv_2_b = take_f32(&mut tensors, "encoder.conv2.bias"); + let e_ln_w = take_f32(&mut tensors, "encoder.ln_post.weight"); + let e_ln_b = take_f32(&mut tensors, "encoder.ln_post.bias"); + + // Encoder layers + let mut encoder_layers = Vec::new(); + for i in 0..hparams.n_audio_layer { + let p = format!("encoder.blocks.{}", i); + encoder_layers.push(EncoderLayer { + attn_ln_0_w: take_f32(&mut tensors, &format!("{}.attn_ln.weight", p)), + attn_ln_0_b: take_f32(&mut tensors, &format!("{}.attn_ln.bias", p)), + attn_ln_1_w: take_raw( + &mut tensors, + &format!("{}.attn.out.weight", p), + preserve_wtype, + ), + attn_ln_1_b: take_f32(&mut tensors, &format!("{}.attn.out.bias", p)), + attn_q_w: take_raw( + &mut tensors, + &format!("{}.attn.query.weight", p), + preserve_wtype, + ), + attn_q_b: take_f32(&mut tensors, &format!("{}.attn.query.bias", p)), + attn_k_w: take_raw( + &mut tensors, + &format!("{}.attn.key.weight", p), + preserve_wtype, + ), + attn_v_w: take_raw( + &mut tensors, + &format!("{}.attn.value.weight", p), + preserve_wtype, + ), + attn_v_b: take_f32(&mut tensors, &format!("{}.attn.value.bias", p)), + mlp_ln_w: take_f32(&mut tensors, &format!("{}.mlp_ln.weight", p)), + mlp_ln_b: take_f32(&mut tensors, &format!("{}.mlp_ln.bias", p)), + mlp_0_w: take_raw(&mut tensors, &format!("{}.mlp.0.weight", p), preserve_wtype), + mlp_0_b: take_f32(&mut tensors, &format!("{}.mlp.0.bias", p)), + mlp_1_w: take_raw(&mut tensors, &format!("{}.mlp.2.weight", p), preserve_wtype), + mlp_1_b: take_f32(&mut tensors, &format!("{}.mlp.2.bias", p)), + }); + } + + // Decoder global tensors + let d_pe = take_f32(&mut tensors, "decoder.positional_embedding"); + let d_te = take_f32(&mut tensors, "decoder.token_embedding.weight"); + let d_ln_w = take_f32(&mut tensors, "decoder.ln.weight"); + let d_ln_b = take_f32(&mut tensors, "decoder.ln.bias"); + + // Decoder layers + let mut decoder_layers = Vec::new(); + for i in 0..hparams.n_text_layer { + let p = format!("decoder.blocks.{}", i); + decoder_layers.push(DecoderLayer { + attn_ln_0_w: take_f32(&mut tensors, &format!("{}.attn_ln.weight", p)), + attn_ln_0_b: take_f32(&mut tensors, &format!("{}.attn_ln.bias", p)), + attn_ln_1_w: take_raw( + &mut tensors, + &format!("{}.attn.out.weight", p), + preserve_wtype, + ), + attn_ln_1_b: take_f32(&mut tensors, &format!("{}.attn.out.bias", p)), + attn_q_w: take_raw( + &mut tensors, + &format!("{}.attn.query.weight", p), + preserve_wtype, + ), + attn_q_b: take_f32(&mut tensors, &format!("{}.attn.query.bias", p)), + attn_k_w: take_raw( + &mut tensors, + &format!("{}.attn.key.weight", p), + preserve_wtype, + ), + attn_v_w: take_raw( + &mut tensors, + &format!("{}.attn.value.weight", p), + preserve_wtype, + ), + attn_v_b: take_f32(&mut tensors, &format!("{}.attn.value.bias", p)), + cross_attn_ln_0_w: take_f32(&mut tensors, &format!("{}.cross_attn_ln.weight", p)), + cross_attn_ln_0_b: take_f32(&mut tensors, &format!("{}.cross_attn_ln.bias", p)), + cross_attn_ln_1_w: take_raw( + &mut tensors, + &format!("{}.cross_attn.out.weight", p), + preserve_wtype, + ), + cross_attn_ln_1_b: take_f32(&mut tensors, &format!("{}.cross_attn.out.bias", p)), + cross_attn_q_w: take_raw( + &mut tensors, + &format!("{}.cross_attn.query.weight", p), + preserve_wtype, + ), + cross_attn_q_b: take_f32(&mut tensors, &format!("{}.cross_attn.query.bias", p)), + cross_attn_k_w: take_raw( + &mut tensors, + &format!("{}.cross_attn.key.weight", p), + preserve_wtype, + ), + cross_attn_v_w: take_raw( + &mut tensors, + &format!("{}.cross_attn.value.weight", p), + preserve_wtype, + ), + cross_attn_v_b: take_f32(&mut tensors, &format!("{}.cross_attn.value.bias", p)), + mlp_ln_w: take_f32(&mut tensors, &format!("{}.mlp_ln.weight", p)), + mlp_ln_b: take_f32(&mut tensors, &format!("{}.mlp_ln.bias", p)), + mlp_0_w: take_raw(&mut tensors, &format!("{}.mlp.0.weight", p), preserve_wtype), + mlp_0_b: take_f32(&mut tensors, &format!("{}.mlp.0.bias", p)), + mlp_1_w: take_raw(&mut tensors, &format!("{}.mlp.2.weight", p), preserve_wtype), + mlp_1_b: take_f32(&mut tensors, &format!("{}.mlp.2.bias", p)), + }); + } + + Ok(WhisperModel { + hparams, + filters, + vocab, + wtype, + e_pe, + e_conv_1_w, + e_conv_1_b, + e_conv_2_w, + e_conv_2_b, + e_ln_w, + e_ln_b, + encoder_layers, + d_pe, + d_te, + d_ln_w, + d_ln_b, + decoder_layers, + }) + } + + pub fn load_file(path: &str) -> io::Result { + let mut f = std::fs::File::open(path)?; + Self::load(&mut f) + } +} diff --git a/libs/voice2/src/cpu/quant.rs b/libs/voice2/src/cpu/quant.rs new file mode 100644 index 0000000000..164c5bb186 --- /dev/null +++ b/libs/voice2/src/cpu/quant.rs @@ -0,0 +1 @@ +pub use makepad_ggml::quant::*; diff --git a/libs/voice2/src/cpu/tensor.rs b/libs/voice2/src/cpu/tensor.rs new file mode 100644 index 0000000000..d8137b0aaa --- /dev/null +++ b/libs/voice2/src/cpu/tensor.rs @@ -0,0 +1,1768 @@ +use crate::quant::*; +use std::cell::RefCell; +use std::sync::atomic::{AtomicUsize, Ordering}; +use std::sync::{Condvar, Mutex}; + +thread_local! { + static Q8_ACT_SCRATCH: RefCell> = RefCell::new(Vec::new()); +} + +fn quant_gpu_enabled() -> bool { + crate::settings::ENABLE_METAL_QUANT +} + +/// Wrapper to pass raw *mut f32 across thread boundaries. +/// Stores the pointer as usize which is always Send+Sync. +/// Safety: callers must ensure non-overlapping writes and valid lifetime. +#[derive(Clone, Copy)] +pub struct SendPtr(usize); + +impl SendPtr { + pub fn new(p: *mut f32) -> Self { + SendPtr(p as usize) + } + #[inline] + pub fn ptr(&self) -> *mut f32 { + self.0 as *mut f32 + } +} + +#[derive(Clone, Copy)] +struct SendPtrU8(usize); + +impl SendPtrU8 { + fn new(p: *mut u8) -> Self { + SendPtrU8(p as usize) + } + #[inline] + fn ptr(&self) -> *mut u8 { + self.0 as *mut u8 + } +} + +/// A simple persistent thread pool. +/// Workers park on a Condvar and wake when work is submitted. +struct ThreadPool { + n_workers: usize, + // Work descriptor — protected by mutex for epoch synchronization + work: Mutex, + wake: Condvar, + // Atomic work counter — accessed without mutex during work-steal + counter: AtomicUsize, + // Completion tracking + done_count: AtomicUsize, + done_mutex: Mutex, + done_cv: Condvar, +} + +struct WorkDesc { + fn_data: usize, + fn_vtable: usize, + total: usize, + epoch: usize, +} + +impl ThreadPool { + fn new(n: usize) -> &'static Self { + let pool = Box::leak(Box::new(ThreadPool { + n_workers: n, + work: Mutex::new(WorkDesc { + fn_data: 0, + fn_vtable: 0, + total: 0, + epoch: 0, + }), + wake: Condvar::new(), + counter: AtomicUsize::new(0), + done_count: AtomicUsize::new(0), + done_mutex: Mutex::new(false), + done_cv: Condvar::new(), + })); + + for _ in 0..n { + let pool_ptr = pool as *const ThreadPool as usize; + std::thread::spawn(move || { + let pool = unsafe { &*(pool_ptr as *const ThreadPool) }; + let mut last_epoch = 0; + loop { + // Wait for new work + let (fn_data, fn_vtable, total) = { + let mut w = pool.work.lock().unwrap(); + while w.epoch == last_epoch { + w = pool.wake.wait(w).unwrap(); + } + last_epoch = w.epoch; + (w.fn_data, w.fn_vtable, w.total) + }; + + // Reconstruct the &dyn Fn(usize) from raw parts + let f: &dyn Fn(usize) = unsafe { std::mem::transmute([fn_data, fn_vtable]) }; + + // Work-steal loop + loop { + let i = pool.counter.fetch_add(1, Ordering::Relaxed); + if i >= total { + break; + } + f(i); + } + + // Signal completion + if pool.done_count.fetch_add(1, Ordering::AcqRel) + 1 == pool.n_workers { + let mut done = pool.done_mutex.lock().unwrap(); + *done = true; + pool.done_cv.notify_one(); + } + } + }); + } + + pool + } + + /// Submit work and block until complete. + /// SAFETY: `f` must remain valid until all workers are done (guaranteed by blocking). + fn run(&self, total: usize, f: &dyn Fn(usize)) { + // Encode the fat pointer as two usizes + let raw: [usize; 2] = unsafe { std::mem::transmute(f) }; + + // Reset completion and counter + self.done_count.store(0, Ordering::Relaxed); + self.counter.store(0, Ordering::Relaxed); + *self.done_mutex.lock().unwrap() = false; + + // Set up work and wake all workers + { + let mut w = self.work.lock().unwrap(); + w.fn_data = raw[0]; + w.fn_vtable = raw[1]; + w.total = total; + w.epoch += 1; + } + self.wake.notify_all(); + + // Wait for all workers to finish + let mut done = self.done_mutex.lock().unwrap(); + while !*done { + done = self.done_cv.wait(done).unwrap(); + } + } +} + +fn get_pool() -> &'static ThreadPool { + static POOL: std::sync::OnceLock<&'static ThreadPool> = std::sync::OnceLock::new(); + *POOL.get_or_init(|| { + let hw = std::thread::available_parallelism() + .map(|n| n.get()) + .unwrap_or(1); + let max_threads = hw.max(1); + let default_threads = if cfg!(any( + target_os = "macos", + target_os = "windows", + target_os = "linux" + )) { + hw.min(4).max(1) + } else { + hw.min(8).max(4).min(max_threads) + }; + let n = default_threads; + ThreadPool::new(n) + }) +} + +#[inline] +fn use_act_q8() -> bool { + crate::settings::ENABLE_ACT_Q8 +} + +/// Run `n_jobs` tasks in parallel, each receiving its job index. +#[inline] +pub fn parallel_for(n_jobs: usize, f: impl Fn(usize) + Send + Sync) { + if n_jobs == 0 { + return; + } + if n_jobs == 1 { + f(0); + return; + } + get_pool().run(n_jobs, &f); +} + +/// Work-stealing parallel loop: distribute `total` work items across threads. +#[inline] +pub fn parallel_work_steal(total: usize, f: impl Fn(usize) + Send + Sync) { + if total == 0 { + return; + } + if total < 64 || get_pool().n_workers <= 1 { + for i in 0..total { + f(i); + } + return; + } + get_pool().run(total, &f); +} + +// SIMD dot product: compute dot product of two f32 slices +#[inline] +fn dot_f32(a: &[f32], b: &[f32]) -> f32 { + dot_f32_simd(a, b) +} + +#[cfg(target_arch = "aarch64")] +#[inline] +fn dot_f32_simd(a: &[f32], b: &[f32]) -> f32 { + use std::arch::aarch64::*; + let n = a.len().min(b.len()); + let mut i = 0; + unsafe { + let mut sum0 = vdupq_n_f32(0.0); + let mut sum1 = vdupq_n_f32(0.0); + let mut sum2 = vdupq_n_f32(0.0); + let mut sum3 = vdupq_n_f32(0.0); + // Process 16 elements per iteration (4 NEON registers x 4 floats) + while i + 15 < n { + let a0 = vld1q_f32(a.as_ptr().add(i)); + let b0 = vld1q_f32(b.as_ptr().add(i)); + sum0 = vfmaq_f32(sum0, a0, b0); + let a1 = vld1q_f32(a.as_ptr().add(i + 4)); + let b1 = vld1q_f32(b.as_ptr().add(i + 4)); + sum1 = vfmaq_f32(sum1, a1, b1); + let a2 = vld1q_f32(a.as_ptr().add(i + 8)); + let b2 = vld1q_f32(b.as_ptr().add(i + 8)); + sum2 = vfmaq_f32(sum2, a2, b2); + let a3 = vld1q_f32(a.as_ptr().add(i + 12)); + let b3 = vld1q_f32(b.as_ptr().add(i + 12)); + sum3 = vfmaq_f32(sum3, a3, b3); + i += 16; + } + // Process 4 elements at a time + while i + 3 < n { + let va = vld1q_f32(a.as_ptr().add(i)); + let vb = vld1q_f32(b.as_ptr().add(i)); + sum0 = vfmaq_f32(sum0, va, vb); + i += 4; + } + sum0 = vaddq_f32(sum0, sum1); + sum2 = vaddq_f32(sum2, sum3); + sum0 = vaddq_f32(sum0, sum2); + let mut result = vaddvq_f32(sum0); + // Scalar tail + while i < n { + result += a[i] * b[i]; + i += 1; + } + result + } +} + +#[cfg(target_arch = "x86_64")] +#[inline] +fn dot_f32_simd(a: &[f32], b: &[f32]) -> f32 { + if is_x86_feature_detected!("avx") && is_x86_feature_detected!("fma") { + unsafe { dot_f32_avx_fma(a, b) } + } else { + dot_f32_scalar(a, b) + } +} + +#[cfg(target_arch = "x86_64")] +#[target_feature(enable = "avx,fma")] +unsafe fn dot_f32_avx_fma(a: &[f32], b: &[f32]) -> f32 { + use std::arch::x86_64::*; + let n = a.len().min(b.len()); + let mut i = 0; + let mut sum0 = _mm256_setzero_ps(); + let mut sum1 = _mm256_setzero_ps(); + let mut sum2 = _mm256_setzero_ps(); + let mut sum3 = _mm256_setzero_ps(); + // Process 32 elements per iteration (4 AVX registers x 8 floats) + while i + 31 < n { + let a0 = _mm256_loadu_ps(a.as_ptr().add(i)); + let b0 = _mm256_loadu_ps(b.as_ptr().add(i)); + sum0 = _mm256_fmadd_ps(a0, b0, sum0); + let a1 = _mm256_loadu_ps(a.as_ptr().add(i + 8)); + let b1 = _mm256_loadu_ps(b.as_ptr().add(i + 8)); + sum1 = _mm256_fmadd_ps(a1, b1, sum1); + let a2 = _mm256_loadu_ps(a.as_ptr().add(i + 16)); + let b2 = _mm256_loadu_ps(b.as_ptr().add(i + 16)); + sum2 = _mm256_fmadd_ps(a2, b2, sum2); + let a3 = _mm256_loadu_ps(a.as_ptr().add(i + 24)); + let b3 = _mm256_loadu_ps(b.as_ptr().add(i + 24)); + sum3 = _mm256_fmadd_ps(a3, b3, sum3); + i += 32; + } + // Process 8 elements at a time + while i + 7 < n { + let va = _mm256_loadu_ps(a.as_ptr().add(i)); + let vb = _mm256_loadu_ps(b.as_ptr().add(i)); + sum0 = _mm256_fmadd_ps(va, vb, sum0); + i += 8; + } + sum0 = _mm256_add_ps(sum0, sum1); + sum2 = _mm256_add_ps(sum2, sum3); + sum0 = _mm256_add_ps(sum0, sum2); + // Horizontal sum of 8 floats + let hi = _mm256_extractf128_ps(sum0, 1); + let lo = _mm256_castps256_ps128(sum0); + let sum128 = _mm_add_ps(lo, hi); + let shuf = _mm_movehdup_ps(sum128); + let sums = _mm_add_ps(sum128, shuf); + let shuf2 = _mm_movehl_ps(sums, sums); + let sums2 = _mm_add_ss(sums, shuf2); + let mut result = _mm_cvtss_f32(sums2); + // Scalar tail + while i < n { + result += a[i] * b[i]; + i += 1; + } + result +} + +#[cfg(not(any(target_arch = "aarch64", target_arch = "x86_64")))] +#[inline] +fn dot_f32_simd(a: &[f32], b: &[f32]) -> f32 { + dot_f32_scalar(a, b) +} + +#[inline] +#[cfg(not(target_arch = "aarch64"))] +fn dot_f32_scalar(a: &[f32], b: &[f32]) -> f32 { + let n = a.len().min(b.len()); + let mut sum = 0.0f32; + let mut i = 0; + while i + 3 < n { + sum += a[i] * b[i] + a[i + 1] * b[i + 1] + a[i + 2] * b[i + 2] + a[i + 3] * b[i + 3]; + i += 4; + } + while i < n { + sum += a[i] * b[i]; + i += 1; + } + sum +} + +// ---- f16 × f32 dot product (widening FMA, no pre-dequant) ---- + +/// Dot product of f16 weights (raw bytes, 2 bytes per element) with f32 activations. +/// `w_f16` is &[u8] with length n*2, `x_f32` is &[f32] with length n. +#[inline] +fn dot_f16_f32(w_f16: &[u8], x_f32: &[f32]) -> f32 { + dot_f16_f32_simd(w_f16, x_f32) +} + +#[cfg(target_arch = "aarch64")] +#[inline] +fn dot_f16_f32_simd(w_f16: &[u8], x_f32: &[f32]) -> f32 { + use std::arch::aarch64::*; + let n = x_f32.len(); + let w_ptr = w_f16.as_ptr() as *const u16; + let x_ptr = x_f32.as_ptr(); + let mut i = 0; + unsafe { + let mut sum0 = vdupq_n_f32(0.0); + let mut sum1 = vdupq_n_f32(0.0); + let mut sum2 = vdupq_n_f32(0.0); + let mut sum3 = vdupq_n_f32(0.0); + + // Use inline asm for FCVTL (f16->f32 widening) since the intrinsic is unstable + // FCVTL converts 4 f16 in Vn.4H (lower half of 64-bit D register) to 4 f32 in Vd.4S + #[inline(always)] + unsafe fn f16x4_to_f32x4(h: uint16x4_t) -> float32x4_t { + let out: float32x4_t; + std::arch::asm!( + "fcvtl {out:v}.4s, {inp:v}.4h", + inp = in(vreg) h, + out = out(vreg) out, + options(pure, nomem, nostack) + ); + out + } + + // Process 16 elements per iteration + while i + 15 < n { + let h0 = vld1_u16(w_ptr.add(i)); + let w0 = f16x4_to_f32x4(h0); + let x0 = vld1q_f32(x_ptr.add(i)); + sum0 = vfmaq_f32(sum0, w0, x0); + + let h1 = vld1_u16(w_ptr.add(i + 4)); + let w1 = f16x4_to_f32x4(h1); + let x1 = vld1q_f32(x_ptr.add(i + 4)); + sum1 = vfmaq_f32(sum1, w1, x1); + + let h2 = vld1_u16(w_ptr.add(i + 8)); + let w2 = f16x4_to_f32x4(h2); + let x2 = vld1q_f32(x_ptr.add(i + 8)); + sum2 = vfmaq_f32(sum2, w2, x2); + + let h3 = vld1_u16(w_ptr.add(i + 12)); + let w3 = f16x4_to_f32x4(h3); + let x3 = vld1q_f32(x_ptr.add(i + 12)); + sum3 = vfmaq_f32(sum3, w3, x3); + + i += 16; + } + while i + 3 < n { + let h = vld1_u16(w_ptr.add(i)); + let w = f16x4_to_f32x4(h); + let x = vld1q_f32(x_ptr.add(i)); + sum0 = vfmaq_f32(sum0, w, x); + i += 4; + } + sum0 = vaddq_f32(sum0, sum1); + sum2 = vaddq_f32(sum2, sum3); + sum0 = vaddq_f32(sum0, sum2); + let mut result = vaddvq_f32(sum0); + while i < n { + result += f16_to_f32(*w_ptr.add(i)) * x_f32[i]; + i += 1; + } + result + } +} + +#[cfg(not(target_arch = "aarch64"))] +#[inline] +fn dot_f16_f32_simd(w_f16: &[u8], x_f32: &[f32]) -> f32 { + let n = x_f32.len(); + let mut sum = 0.0f32; + for i in 0..n { + let h = u16::from_le_bytes([w_f16[i * 2], w_f16[i * 2 + 1]]); + sum += f16_to_f32(h) * x_f32[i]; + } + sum +} + +/// A tensor storing f32 data with shape information. +/// All computation happens in f32. Quantized weights are dequantized on the fly during matmul. +#[derive(Clone)] +pub struct Tensor { + pub data: Vec, + pub shape: Vec, // e.g. [rows, cols] for 2D +} + +/// Raw weight tensor - stores quantized (or f16/f32) bytes as loaded from the model file. +/// Dequantized on the fly during matrix multiply. +pub struct RawTensor { + pub data: Vec, + pub shape: Vec, + pub ggml_type: u32, +} + +impl RawTensor { + /// Dequantize the entire tensor to f32. + pub fn to_f32(&self) -> Tensor { + let n = self.shape.iter().product::(); + let mut out = vec![0.0f32; n]; + match self.ggml_type { + GGML_TYPE_F32 => { + for i in 0..n { + let off = i * 4; + out[i] = f32::from_le_bytes([ + self.data[off], + self.data[off + 1], + self.data[off + 2], + self.data[off + 3], + ]); + } + } + GGML_TYPE_F16 => { + for i in 0..n { + let off = i * 2; + out[i] = f16_to_f32(u16::from_le_bytes([self.data[off], self.data[off + 1]])); + } + } + GGML_TYPE_Q4_0 => { + let bs = block_size(GGML_TYPE_Q4_0); + let mut tmp = [0.0f32; QK]; + let nb = n / QK; + for b in 0..nb { + dequantize_q4_0(&self.data[b * bs..], &mut tmp); + out[b * QK..(b + 1) * QK].copy_from_slice(&tmp); + } + } + GGML_TYPE_Q4_1 => { + let bs = block_size(GGML_TYPE_Q4_1); + let mut tmp = [0.0f32; QK]; + let nb = n / QK; + for b in 0..nb { + dequantize_q4_1(&self.data[b * bs..], &mut tmp); + out[b * QK..(b + 1) * QK].copy_from_slice(&tmp); + } + } + GGML_TYPE_Q5_0 => { + let bs = block_size(GGML_TYPE_Q5_0); + let mut tmp = [0.0f32; QK]; + let nb = n / QK; + for b in 0..nb { + dequantize_q5_0(&self.data[b * bs..], &mut tmp); + out[b * QK..(b + 1) * QK].copy_from_slice(&tmp); + } + } + GGML_TYPE_Q5_1 => { + let bs = block_size(GGML_TYPE_Q5_1); + let mut tmp = [0.0f32; QK]; + let nb = n / QK; + for b in 0..nb { + dequantize_q5_1(&self.data[b * bs..], &mut tmp); + out[b * QK..(b + 1) * QK].copy_from_slice(&tmp); + } + } + GGML_TYPE_Q8_0 => { + let bs = block_size(GGML_TYPE_Q8_0); + let mut tmp = [0.0f32; QK]; + let nb = n / QK; + for b in 0..nb { + dequantize_q8_0(&self.data[b * bs..], &mut tmp); + out[b * QK..(b + 1) * QK].copy_from_slice(&tmp); + } + } + t => panic!("unsupported dequantize type {}", t), + } + // Reverse shape from GGML column-major to row-major order. + let mut shape = self.shape.clone(); + shape.reverse(); + Tensor { data: out, shape } + } + + /// Number of rows (product of all dims except dim0). + /// In GGML layout, dim0 is contiguous. Rows = dim1 * dim2 * ... + pub fn n_rows(&self) -> usize { + if self.shape.len() <= 1 { + 1 + } else { + self.shape[1..].iter().product() + } + } + + /// Number of columns (dim0 = contiguous/inner dimension in GGML layout) + pub fn n_cols(&self) -> usize { + self.shape.first().copied().unwrap_or(1) + } + + /// Convert this RawTensor to Q8_0 quantization (from F16 or F32). + /// If already Q8_0 or another quantized type, returns self unchanged. + pub fn to_q8_0(self) -> RawTensor { + match self.ggml_type { + GGML_TYPE_F16 => { + let n_elements: usize = self.shape.iter().product(); + let data = quantize_f16_to_q8_0(&self.data, n_elements); + RawTensor { + data, + shape: self.shape, + ggml_type: GGML_TYPE_Q8_0, + } + } + GGML_TYPE_F32 => { + let n_elements: usize = self.shape.iter().product(); + // First convert to f32 vec, then quantize + let mut f32_data = vec![0.0f32; n_elements]; + for i in 0..n_elements { + let off = i * 4; + f32_data[i] = f32::from_le_bytes([ + self.data[off], + self.data[off + 1], + self.data[off + 2], + self.data[off + 3], + ]); + } + let data = quantize_f32_to_q8_0(&f32_data); + RawTensor { + data, + shape: self.shape, + ggml_type: GGML_TYPE_Q8_0, + } + } + _ => self, // already quantized, leave as-is + } + } + + /// Bytes per row + pub fn row_bytes(&self) -> usize { + let cols = self.n_cols(); + let be = block_elements(self.ggml_type); + let bs = block_size(self.ggml_type); + (cols / be) * bs + } + + /// Get raw bytes for a specific row + pub fn row_data(&self, row: usize) -> &[u8] { + let rb = self.row_bytes(); + &self.data[row * rb..(row + 1) * rb] + } +} + +impl Tensor { + pub fn zeros(shape: &[usize]) -> Self { + let n: usize = shape.iter().product(); + Tensor { + data: vec![0.0; n], + shape: shape.to_vec(), + } + } + + /// SIMD-accelerated dot product of two f32 slices. + #[inline] + pub fn dot_f32(a: &[f32], b: &[f32]) -> f32 { + dot_f32(a, b) + } + + pub fn numel(&self) -> usize { + self.shape.iter().product() + } + + /// Reshape (no copy, just changes shape metadata). Panics if numel differs. + pub fn reshape(&self, new_shape: &[usize]) -> Tensor { + let n: usize = new_shape.iter().product(); + assert_eq!( + n, + self.numel(), + "reshape: element count mismatch {} vs {}", + n, + self.numel() + ); + Tensor { + data: self.data.clone(), + shape: new_shape.to_vec(), + } + } + + /// 2D shape helpers + pub fn rows(&self) -> usize { + if self.shape.len() == 1 { + 1 + } else { + self.shape[0] + } + } + pub fn cols(&self) -> usize { + *self.shape.last().unwrap() + } + + // ---- Element-wise operations ---- + + /// out = a + b (broadcasting b if it's smaller) + pub fn add(a: &Tensor, b: &Tensor) -> Tensor { + if let Some(out) = crate::metal_backend::try_add_f32(&a.data, &a.shape, &b.data, &b.shape) { + return Tensor { + data: out, + shape: a.shape.clone(), + }; + } + + if a.numel() == b.numel() { + let data: Vec = a.data.iter().zip(&b.data).map(|(x, y)| x + y).collect(); + return Tensor { + data, + shape: a.shape.clone(), + }; + } + // broadcast b along leading dims + let bn = b.numel(); + let data: Vec = a + .data + .iter() + .enumerate() + .map(|(i, x)| x + b.data[i % bn]) + .collect(); + Tensor { + data, + shape: a.shape.clone(), + } + } + + /// out = a + b, in-place on a + pub fn add_inplace(a: &mut Tensor, b: &Tensor) { + let bn = b.numel(); + for (i, x) in a.data.iter_mut().enumerate() { + *x += b.data[i % bn]; + } + } + + /// out = a * b element-wise (broadcasting b) + pub fn mul(a: &Tensor, b: &Tensor) -> Tensor { + if let Some(out) = crate::metal_backend::try_mul_f32(&a.data, &a.shape, &b.data, &b.shape) { + return Tensor { + data: out, + shape: a.shape.clone(), + }; + } + + if a.numel() == b.numel() { + let data: Vec = a.data.iter().zip(&b.data).map(|(x, y)| x * y).collect(); + return Tensor { + data, + shape: a.shape.clone(), + }; + } + let bn = b.numel(); + let data: Vec = a + .data + .iter() + .enumerate() + .map(|(i, x)| x * b.data[i % bn]) + .collect(); + Tensor { + data, + shape: a.shape.clone(), + } + } + + /// out = a * scalar + pub fn scale(a: &Tensor, s: f32) -> Tensor { + let data: Vec = a.data.iter().map(|x| x * s).collect(); + Tensor { + data, + shape: a.shape.clone(), + } + } + + pub fn scale_inplace(a: &mut Tensor, s: f32) { + for x in &mut a.data { + *x *= s; + } + } + + /// GELU activation: x * 0.5 * (1 + tanh(sqrt(2/pi) * (x + 0.044715 * x^3))) + pub fn gelu(a: &Tensor) -> Tensor { + if let Some(out) = crate::metal_backend::try_gelu_f32(&a.data, &a.shape) { + return Tensor { + data: out, + shape: a.shape.clone(), + }; + } + + const SQRT_2_PI: f32 = 0.7978845608; + let data: Vec = a + .data + .iter() + .map(|&x| { + let t = (SQRT_2_PI * (x + 0.044715 * x * x * x)).tanh(); + 0.5 * x * (1.0 + t) + }) + .collect(); + Tensor { + data, + shape: a.shape.clone(), + } + } + + /// Layer normalization along the last dimension. + /// out[i] = (x[i] - mean) / sqrt(var + eps) + pub fn layer_norm(x: &Tensor, eps: f32) -> Tensor { + if let Some(out) = crate::metal_backend::try_layer_norm_f32(&x.data, &x.shape, eps) { + return Tensor { + data: out, + shape: x.shape.clone(), + }; + } + + let n = *x.shape.last().unwrap(); + let batches = x.numel() / n; + let mut out = vec![0.0f32; x.numel()]; + + for b in 0..batches { + let off = b * n; + let slice = &x.data[off..off + n]; + + let mean: f32 = slice.iter().sum::() / n as f32; + let var: f32 = slice.iter().map(|v| (v - mean) * (v - mean)).sum::() / n as f32; + let inv_std = 1.0 / (var + eps).sqrt(); + + for i in 0..n { + out[off + i] = (slice[i] - mean) * inv_std; + } + } + + Tensor { + data: out, + shape: x.shape.clone(), + } + } + + /// Layer normalization followed by affine transform. + /// out = layer_norm(x, eps) * mul + add + pub fn layer_norm_mul_add(x: &Tensor, mul: &Tensor, add: &Tensor, eps: f32) -> Tensor { + if let Some(out) = crate::metal_backend::try_layer_norm_mul_add_f32( + &x.data, &x.shape, &mul.data, &mul.shape, &add.data, &add.shape, eps, + ) { + return Tensor { + data: out, + shape: x.shape.clone(), + }; + } + + let norm = Self::layer_norm(x, eps); + Self::add(&Self::mul(&norm, mul), add) + } + + /// Softmax along last dimension, with optional additive mask and scale. + /// out[i] = exp((x[i]*scale + mask[i]) - max) / sum + pub fn softmax(x: &Tensor, mask: Option<&Tensor>, scale: f32) -> Tensor { + let n = *x.shape.last().unwrap(); + let batches = x.numel() / n; + let mut out = vec![0.0f32; x.numel()]; + + for b in 0..batches { + let off = b * n; + let slice = &x.data[off..off + n]; + + let mut max_val = f32::NEG_INFINITY; + for i in 0..n { + let mut v = slice[i] * scale; + if let Some(m) = mask { + v += m.data[off % m.numel() + i % m.data.len().min(n)]; + } + if v > max_val { + max_val = v; + } + } + + let mut sum = 0.0f32; + for i in 0..n { + let mut v = slice[i] * scale; + if let Some(m) = mask { + v += m.data[off % m.numel() + i % m.data.len().min(n)]; + } + let e = (v - max_val).exp(); + out[off + i] = e; + sum += e; + } + + let inv_sum = 1.0 / sum; + for i in 0..n { + out[off + i] *= inv_sum; + } + } + + Tensor { + data: out, + shape: x.shape.clone(), + } + } + + /// Softmax with a mask tensor that has shape [n_kv] applied per-row, + /// for the decoder attention mask pattern. + pub fn softmax_masked(x: &Tensor, mask: &[f32], scale: f32, n_kv: usize) -> Tensor { + let n = *x.shape.last().unwrap(); + assert_eq!(n, n_kv); + let batches = x.numel() / n; + let mut out = vec![0.0f32; x.numel()]; + + for b in 0..batches { + let off = b * n; + let mask_off = (b % (mask.len() / n)) * n; + + let mut max_val = f32::NEG_INFINITY; + for i in 0..n { + let v = x.data[off + i] * scale + mask[mask_off + i]; + if v > max_val { + max_val = v; + } + } + + let mut sum = 0.0f32; + for i in 0..n { + let v = x.data[off + i] * scale + mask[mask_off + i]; + let e = (v - max_val).exp(); + out[off + i] = e; + sum += e; + } + + if sum > 0.0 { + let inv = 1.0 / sum; + for i in 0..n { + out[off + i] *= inv; + } + } + } + + Tensor { + data: out, + shape: x.shape.clone(), + } + } + + /// Transpose a 2D tensor: [rows, cols] -> [cols, rows] + pub fn transpose_2d(a: &Tensor) -> Tensor { + assert_eq!(a.shape.len(), 2); + let (r, c) = (a.shape[0], a.shape[1]); + let mut out = vec![0.0f32; r * c]; + for i in 0..r { + for j in 0..c { + out[j * r + i] = a.data[i * c + j]; + } + } + Tensor { + data: out, + shape: vec![c, r], + } + } + + /// Get rows (embedding lookup): weight[indices[i]] for each index + /// weight: [n_vocab, dim], indices: list of i32 -> out: [n_tokens, dim] + pub fn get_rows(weight: &Tensor, indices: &[i32]) -> Tensor { + let dim = weight.shape[weight.shape.len() - 1]; + let n = indices.len(); + let mut out = vec![0.0f32; n * dim]; + for (i, &idx) in indices.iter().enumerate() { + let src_off = idx as usize * dim; + out[i * dim..(i + 1) * dim].copy_from_slice(&weight.data[src_off..src_off + dim]); + } + Tensor { + data: out, + shape: vec![n, dim], + } + } + + // ---- Matrix multiply ---- + + /// Matrix multiply: a[M, K] @ b[K, N] -> out[M, N] + /// This is the f32 x f32 path. Transposes b for SIMD-friendly dot products. + pub fn matmul(a: &Tensor, b: &Tensor) -> Tensor { + assert_eq!(a.shape.len(), 2); + assert_eq!(b.shape.len(), 2); + let m = a.shape[0]; + let k = a.shape[1]; + assert_eq!(b.shape[0], k); + let n = b.shape[1]; + + if let Some(out) = crate::metal_backend::try_matmul_nn_f32(&a.data, &b.data, m, k, n) { + return Tensor { + data: out, + shape: vec![m, n], + }; + } + + // Transpose b to [N, K] so each output column is a contiguous row + let mut bt = vec![0.0f32; n * k]; + for i in 0..k { + for j in 0..n { + bt[j * k + i] = b.data[i * n + j]; + } + } + + let mut out = vec![0.0f32; m * n]; + for i in 0..m { + let a_row = &a.data[i * k..(i + 1) * k]; + for j in 0..n { + let b_row = &bt[j * k..(j + 1) * k]; + out[i * n + j] = dot_f32(a_row, b_row); + } + } + Tensor { + data: out, + shape: vec![m, n], + } + } + + /// Matrix multiply: x[M, K] @ w^T where w is [N, K] (transposed weight). + /// Both x and w are f32 Tensors. w has layout [out_features, in_features] + /// where each row is a contiguous weight vector (ideal for SIMD dot products). + /// Result is [M, N]. Uses multiple threads for large matmuls. + pub fn matmul_t(x: &Tensor, w: &Tensor) -> Tensor { + let _t = std::time::Instant::now(); + let in_features = w.cols(); + let out_features = w.rows(); + let batch = x.numel() / in_features; + assert_eq!(x.numel(), batch * in_features, "matmul_t: x size mismatch"); + + if let Some(out) = crate::metal_backend::try_matmul_nt_f32( + &x.data, + &w.data, + batch, + in_features, + out_features, + ) { + crate::PROF_MATMUL_T.fetch_add( + _t.elapsed().as_nanos() as u64, + std::sync::atomic::Ordering::Relaxed, + ); + crate::PROF_MATMUL_T_CALLS.fetch_add(1, std::sync::atomic::Ordering::Relaxed); + return Tensor { + data: out, + shape: vec![batch, out_features], + }; + } + + let mut out = vec![0.0f32; batch * out_features]; + let out_ptr = SendPtr::new(out.as_mut_ptr()); + let x_data = &x.data; + let w_data = &w.data; + parallel_work_steal(out_features, |o| { + let w_row = &w_data[o * in_features..(o + 1) * in_features]; + for b in 0..batch { + let x_row = &x_data[b * in_features..(b + 1) * in_features]; + unsafe { + *out_ptr.ptr().add(b * out_features + o) = dot_f32(x_row, w_row); + } + } + }); + + crate::PROF_MATMUL_T.fetch_add( + _t.elapsed().as_nanos() as u64, + std::sync::atomic::Ordering::Relaxed, + ); + crate::PROF_MATMUL_T_CALLS.fetch_add(1, std::sync::atomic::Ordering::Relaxed); + Tensor { + data: out, + shape: vec![batch, out_features], + } + } + + #[inline] + pub fn activation_q8_enabled() -> bool { + use_act_q8() + } + + /// Quantize all activation rows of x (shape [batch, in_features]) into GGML Q8_0 blocks. + pub fn prequantize_rows_q8(x: &Tensor, in_features: usize) -> Vec { + assert_eq!( + in_features % QK, + 0, + "prequantize_rows_q8: K must be multiple of {}", + QK + ); + let batch = x.numel() / in_features; + assert_eq!( + x.numel(), + batch * in_features, + "prequantize_rows_q8: x size mismatch" + ); + let bs = block_size(GGML_TYPE_Q8_0); + let nb = in_features / QK; + let rb = nb * bs; + let mut out = vec![0u8; batch * rb]; + let x_data = &x.data; + let out_ptr = SendPtrU8::new(out.as_mut_ptr()); + parallel_work_steal(batch, |b| { + let x_row = &x_data[b * in_features..(b + 1) * in_features]; + let out_row = unsafe { std::slice::from_raw_parts_mut(out_ptr.ptr().add(b * rb), rb) }; + for blk in 0..nb { + quantize_q8_0_block( + &x_row[blk * QK..(blk + 1) * QK], + &mut out_row[blk * bs..(blk + 1) * bs], + ); + } + }); + out + } + + /// Matrix multiply with a quantized weight matrix. + /// weight (RawTensor) is [out_features, in_features] (row-major quantized) + /// x is [batch, in_features] (f32) + /// result is [batch, out_features] (f32) + /// This computes x @ weight^T (each weight row dots with each x row) + pub fn matmul_raw(x: &Tensor, weight: &RawTensor) -> Tensor { + Self::matmul_raw_with_prequant(x, weight, None) + } + + /// Fused linear layer: x @ weight^T + bias. + /// Uses fused Metal matmul+add when available. + pub fn linear_raw(x: &Tensor, weight: &RawTensor, bias: &Tensor) -> Tensor { + let in_features = weight.n_cols(); + let out_features = weight.n_rows(); + let batch = x.numel() / in_features; + assert_eq!( + x.numel(), + batch * in_features, + "linear_raw: x size mismatch" + ); + + if bias.numel() == out_features { + if let Some(out) = crate::metal_backend::try_matmul_nt_ggml_bytes_add_bias( + &x.data, + &weight.data, + weight.ggml_type, + batch, + in_features, + out_features, + &bias.data, + ) { + return Tensor { + data: out, + shape: vec![batch, out_features], + }; + } + } + + Tensor::add(&Self::matmul_raw(x, weight), bias) + } + + /// Same as linear_raw but allows pre-quantized activation rows for CPU fallback path. + pub fn linear_raw_with_prequant( + x: &Tensor, + weight: &RawTensor, + xq_rows: Option<&[u8]>, + bias: &Tensor, + ) -> Tensor { + let in_features = weight.n_cols(); + let out_features = weight.n_rows(); + let batch = x.numel() / in_features; + assert_eq!( + x.numel(), + batch * in_features, + "linear_raw_with_prequant: x size mismatch" + ); + + if bias.numel() == out_features { + if let Some(out) = crate::metal_backend::try_matmul_nt_ggml_bytes_add_bias( + &x.data, + &weight.data, + weight.ggml_type, + batch, + in_features, + out_features, + &bias.data, + ) { + return Tensor { + data: out, + shape: vec![batch, out_features], + }; + } + } + + Tensor::add(&Self::matmul_raw_with_prequant(x, weight, xq_rows), bias) + } + + /// Same as matmul_raw but optionally accepts pre-quantized activation rows in Q8_0 layout. + /// `xq_rows` is [batch, row_bytes] where row_bytes = (in_features/QK)*34. + pub fn matmul_raw_with_prequant( + x: &Tensor, + weight: &RawTensor, + xq_rows: Option<&[u8]>, + ) -> Tensor { + let _t = std::time::Instant::now(); + let in_features = weight.n_cols(); + let out_features = weight.n_rows(); + let batch = x.numel() / in_features; + assert_eq!( + x.numel(), + batch * in_features, + "matmul_raw: x size mismatch" + ); + + if weight.ggml_type == GGML_TYPE_F32 { + if let Some(out) = crate::metal_backend::try_matmul_nt_f32_bytes( + &x.data, + &weight.data, + batch, + in_features, + out_features, + ) { + crate::PROF_MATMUL_RAW.fetch_add( + _t.elapsed().as_nanos() as u64, + std::sync::atomic::Ordering::Relaxed, + ); + crate::PROF_MATMUL_RAW_CALLS.fetch_add(1, std::sync::atomic::Ordering::Relaxed); + return Tensor { + data: out, + shape: vec![batch, out_features], + }; + } + } else if weight.ggml_type == GGML_TYPE_F16 { + if let Some(out) = crate::metal_backend::try_matmul_nt_f16_bytes( + &x.data, + &weight.data, + batch, + in_features, + out_features, + ) { + crate::PROF_MATMUL_RAW.fetch_add( + _t.elapsed().as_nanos() as u64, + std::sync::atomic::Ordering::Relaxed, + ); + crate::PROF_MATMUL_RAW_CALLS.fetch_add(1, std::sync::atomic::Ordering::Relaxed); + return Tensor { + data: out, + shape: vec![batch, out_features], + }; + } + } else if quant_gpu_enabled() { + match weight.ggml_type { + GGML_TYPE_Q4_0 | GGML_TYPE_Q4_1 | GGML_TYPE_Q5_0 | GGML_TYPE_Q5_1 + | GGML_TYPE_Q8_0 => { + if let Some(out) = crate::metal_backend::try_matmul_nt_ggml_bytes( + &x.data, + &weight.data, + weight.ggml_type, + batch, + in_features, + out_features, + ) { + crate::PROF_MATMUL_RAW.fetch_add( + _t.elapsed().as_nanos() as u64, + std::sync::atomic::Ordering::Relaxed, + ); + crate::PROF_MATMUL_RAW_CALLS + .fetch_add(1, std::sync::atomic::Ordering::Relaxed); + return Tensor { + data: out, + shape: vec![batch, out_features], + }; + } + } + _ => {} + } + } + + let mut out = vec![0.0f32; batch * out_features]; + + match weight.ggml_type { + GGML_TYPE_F32 => { + let w = weight.to_f32(); + for b in 0..batch { + let x_row = &x.data[b * in_features..(b + 1) * in_features]; + for o in 0..out_features { + let w_row = &w.data[o * in_features..(o + 1) * in_features]; + out[b * out_features + o] = dot_f32(x_row, w_row); + } + } + } + GGML_TYPE_F16 => { + // Direct f16×f32 dot product — no pre-dequant, half the bandwidth + let row_bytes = in_features * 2; // 2 bytes per f16 + let out_ptr = SendPtr::new(out.as_mut_ptr()); + let x_data = &x.data; + let w_data = &weight.data; + parallel_work_steal(out_features, |o| { + let w_row = &w_data[o * row_bytes..(o + 1) * row_bytes]; + for b in 0..batch { + let x_row = &x_data[b * in_features..(b + 1) * in_features]; + unsafe { + *out_ptr.ptr().add(b * out_features + o) = dot_f16_f32(w_row, x_row); + } + } + }); + } + GGML_TYPE_Q4_0 => { + let bs = block_size(GGML_TYPE_Q4_0); + let nb = in_features / QK; + let rb = nb * bs; + for b in 0..batch { + let x_row = &x.data[b * in_features..(b + 1) * in_features]; + for o in 0..out_features { + let w_row = &weight.data[o * rb..]; + let mut sum = 0.0f32; + for blk in 0..nb { + sum += vec_dot_q4_0_f32(&w_row[blk * bs..], &x_row[blk * QK..]); + } + out[b * out_features + o] = sum; + } + } + } + GGML_TYPE_Q5_0 => { + let bs = block_size(GGML_TYPE_Q5_0); + let nb = in_features / QK; + let rb = nb * bs; + let out_ptr = SendPtr::new(out.as_mut_ptr()); + let x_data = &x.data; + let w_data = &weight.data; + let use_q8_act = use_act_q8() && in_features % QK == 0 && nb > 0; + + if use_q8_act && batch > 2 { + let xq_bs = block_size(GGML_TYPE_Q8_0); + let xq_rb = nb * xq_bs; + + if let Some(xq_all) = xq_rows { + assert!( + xq_all.len() >= batch * xq_rb, + "matmul_raw_with_prequant: xq_rows too small: {} < {}", + xq_all.len(), + batch * xq_rb + ); + + parallel_work_steal(batch, |b| { + let xq = &xq_all[b * xq_rb..(b + 1) * xq_rb]; + let out_row_ptr = unsafe { out_ptr.ptr().add(b * out_features) }; + for o in 0..out_features { + let w_row = &w_data[o * rb..(o + 1) * rb]; + let mut sum = 0.0f32; + let mut blk = 0; + while blk + 3 < nb { + sum += + vec_dot_q5_0_q8_0(&w_row[blk * bs..], &xq[blk * xq_bs..]); + sum += vec_dot_q5_0_q8_0( + &w_row[(blk + 1) * bs..], + &xq[(blk + 1) * xq_bs..], + ); + sum += vec_dot_q5_0_q8_0( + &w_row[(blk + 2) * bs..], + &xq[(blk + 2) * xq_bs..], + ); + sum += vec_dot_q5_0_q8_0( + &w_row[(blk + 3) * bs..], + &xq[(blk + 3) * xq_bs..], + ); + blk += 4; + } + while blk < nb { + sum += + vec_dot_q5_0_q8_0(&w_row[blk * bs..], &xq[blk * xq_bs..]); + blk += 1; + } + unsafe { + *out_row_ptr.add(o) = sum; + } + } + }); + } else { + parallel_work_steal(batch, |b| { + let x_row = &x_data[b * in_features..(b + 1) * in_features]; + let out_row_ptr = unsafe { out_ptr.ptr().add(b * out_features) }; + Q8_ACT_SCRATCH.with(|scratch| { + let mut scratch = scratch.borrow_mut(); + if scratch.len() < xq_rb { + scratch.resize(xq_rb, 0); + } + let xq = &mut scratch[..xq_rb]; + for blk in 0..nb { + quantize_q8_0_block( + &x_row[blk * QK..(blk + 1) * QK], + &mut xq[blk * xq_bs..(blk + 1) * xq_bs], + ); + } + + for o in 0..out_features { + let w_row = &w_data[o * rb..(o + 1) * rb]; + let mut sum = 0.0f32; + let mut blk = 0; + while blk + 3 < nb { + sum += vec_dot_q5_0_q8_0( + &w_row[blk * bs..], + &xq[blk * xq_bs..], + ); + sum += vec_dot_q5_0_q8_0( + &w_row[(blk + 1) * bs..], + &xq[(blk + 1) * xq_bs..], + ); + sum += vec_dot_q5_0_q8_0( + &w_row[(blk + 2) * bs..], + &xq[(blk + 2) * xq_bs..], + ); + sum += vec_dot_q5_0_q8_0( + &w_row[(blk + 3) * bs..], + &xq[(blk + 3) * xq_bs..], + ); + blk += 4; + } + while blk < nb { + sum += vec_dot_q5_0_q8_0( + &w_row[blk * bs..], + &xq[blk * xq_bs..], + ); + blk += 1; + } + unsafe { + *out_row_ptr.add(o) = sum; + } + } + }); + }); + } + } else if batch > 2 { + parallel_work_steal(batch, |b| { + let x_row = &x_data[b * in_features..(b + 1) * in_features]; + let out_row_ptr = unsafe { out_ptr.ptr().add(b * out_features) }; + for o in 0..out_features { + let w_row = &w_data[o * rb..(o + 1) * rb]; + let mut sum = 0.0f32; + let mut blk = 0; + while blk + 3 < nb { + sum += vec_dot_q5_0_f32(&w_row[blk * bs..], &x_row[blk * QK..]); + sum += vec_dot_q5_0_f32( + &w_row[(blk + 1) * bs..], + &x_row[(blk + 1) * QK..], + ); + sum += vec_dot_q5_0_f32( + &w_row[(blk + 2) * bs..], + &x_row[(blk + 2) * QK..], + ); + sum += vec_dot_q5_0_f32( + &w_row[(blk + 3) * bs..], + &x_row[(blk + 3) * QK..], + ); + blk += 4; + } + while blk < nb { + sum += vec_dot_q5_0_f32(&w_row[blk * bs..], &x_row[blk * QK..]); + blk += 1; + } + unsafe { + *out_row_ptr.add(o) = sum; + } + } + }); + } else { + parallel_work_steal(out_features, |o| { + let w_row = &w_data[o * rb..(o + 1) * rb]; + for b in 0..batch { + let x_row = &x_data[b * in_features..(b + 1) * in_features]; + let mut sum = 0.0f32; + let mut blk = 0; + while blk + 3 < nb { + sum += vec_dot_q5_0_f32(&w_row[blk * bs..], &x_row[blk * QK..]); + sum += vec_dot_q5_0_f32( + &w_row[(blk + 1) * bs..], + &x_row[(blk + 1) * QK..], + ); + sum += vec_dot_q5_0_f32( + &w_row[(blk + 2) * bs..], + &x_row[(blk + 2) * QK..], + ); + sum += vec_dot_q5_0_f32( + &w_row[(blk + 3) * bs..], + &x_row[(blk + 3) * QK..], + ); + blk += 4; + } + while blk < nb { + sum += vec_dot_q5_0_f32(&w_row[blk * bs..], &x_row[blk * QK..]); + blk += 1; + } + unsafe { + *out_ptr.ptr().add(b * out_features + o) = sum; + } + } + }); + } + } + GGML_TYPE_Q8_0 => { + let bs = block_size(GGML_TYPE_Q8_0); + let nb = in_features / QK; + let rb = nb * bs; + let out_ptr = SendPtr::new(out.as_mut_ptr()); + let x_data = &x.data; + let w_data = &weight.data; + let use_q8_act = use_act_q8() && in_features % QK == 0 && nb > 0; + if use_q8_act && batch > 2 { + if let Some(xq_all) = xq_rows { + assert!( + xq_all.len() >= batch * rb, + "matmul_raw_with_prequant: xq_rows too small: {} < {}", + xq_all.len(), + batch * rb + ); + parallel_work_steal(batch, |b| { + let xq = &xq_all[b * rb..(b + 1) * rb]; + let out_row_ptr = unsafe { out_ptr.ptr().add(b * out_features) }; + for o in 0..out_features { + let w_row = &w_data[o * rb..(o + 1) * rb]; + let mut sum = 0.0f32; + let mut blk = 0; + while blk + 3 < nb { + sum += vec_dot_q8_0_q8_0(&w_row[blk * bs..], &xq[blk * bs..]); + sum += vec_dot_q8_0_q8_0( + &w_row[(blk + 1) * bs..], + &xq[(blk + 1) * bs..], + ); + sum += vec_dot_q8_0_q8_0( + &w_row[(blk + 2) * bs..], + &xq[(blk + 2) * bs..], + ); + sum += vec_dot_q8_0_q8_0( + &w_row[(blk + 3) * bs..], + &xq[(blk + 3) * bs..], + ); + blk += 4; + } + while blk < nb { + sum += vec_dot_q8_0_q8_0(&w_row[blk * bs..], &xq[blk * bs..]); + blk += 1; + } + unsafe { + *out_row_ptr.add(o) = sum; + } + } + }); + } else { + // Aggressive CPU fast path: quantize activations to Q8 per-row once, then Q8xQ8 dots. + parallel_work_steal(batch, |b| { + let x_row = &x_data[b * in_features..(b + 1) * in_features]; + let out_row_ptr = unsafe { out_ptr.ptr().add(b * out_features) }; + Q8_ACT_SCRATCH.with(|scratch| { + let mut scratch = scratch.borrow_mut(); + if scratch.len() < rb { + scratch.resize(rb, 0); + } + let xq = &mut scratch[..rb]; + for blk in 0..nb { + quantize_q8_0_block( + &x_row[blk * QK..(blk + 1) * QK], + &mut xq[blk * bs..(blk + 1) * bs], + ); + } + for o in 0..out_features { + let w_row = &w_data[o * rb..(o + 1) * rb]; + let mut sum = 0.0f32; + let mut blk = 0; + while blk + 3 < nb { + sum += + vec_dot_q8_0_q8_0(&w_row[blk * bs..], &xq[blk * bs..]); + sum += vec_dot_q8_0_q8_0( + &w_row[(blk + 1) * bs..], + &xq[(blk + 1) * bs..], + ); + sum += vec_dot_q8_0_q8_0( + &w_row[(blk + 2) * bs..], + &xq[(blk + 2) * bs..], + ); + sum += vec_dot_q8_0_q8_0( + &w_row[(blk + 3) * bs..], + &xq[(blk + 3) * bs..], + ); + blk += 4; + } + while blk < nb { + sum += + vec_dot_q8_0_q8_0(&w_row[blk * bs..], &xq[blk * bs..]); + blk += 1; + } + unsafe { + *out_row_ptr.add(o) = sum; + } + } + }); + }); + } + } else if batch > 2 { + // For larger batches, keep each x row hot in cache and sweep all output rows. + parallel_work_steal(batch, |b| { + let x_row = &x_data[b * in_features..(b + 1) * in_features]; + let out_row_ptr = unsafe { out_ptr.ptr().add(b * out_features) }; + for o in 0..out_features { + let w_row = &w_data[o * rb..(o + 1) * rb]; + let mut sum = 0.0f32; + let mut blk = 0; + while blk + 3 < nb { + sum += vec_dot_q8_0_f32(&w_row[blk * bs..], &x_row[blk * QK..]); + sum += vec_dot_q8_0_f32( + &w_row[(blk + 1) * bs..], + &x_row[(blk + 1) * QK..], + ); + sum += vec_dot_q8_0_f32( + &w_row[(blk + 2) * bs..], + &x_row[(blk + 2) * QK..], + ); + sum += vec_dot_q8_0_f32( + &w_row[(blk + 3) * bs..], + &x_row[(blk + 3) * QK..], + ); + blk += 4; + } + while blk < nb { + sum += vec_dot_q8_0_f32(&w_row[blk * bs..], &x_row[blk * QK..]); + blk += 1; + } + unsafe { + *out_row_ptr.add(o) = sum; + } + } + }); + } else { + // For tiny batches (decoder path), parallelize over output rows. + parallel_work_steal(out_features, |o| { + let w_row = &w_data[o * rb..(o + 1) * rb]; + for b in 0..batch { + let x_row = &x_data[b * in_features..(b + 1) * in_features]; + let mut sum = 0.0f32; + let mut blk = 0; + while blk + 3 < nb { + sum += vec_dot_q8_0_f32(&w_row[blk * bs..], &x_row[blk * QK..]); + sum += vec_dot_q8_0_f32( + &w_row[(blk + 1) * bs..], + &x_row[(blk + 1) * QK..], + ); + sum += vec_dot_q8_0_f32( + &w_row[(blk + 2) * bs..], + &x_row[(blk + 2) * QK..], + ); + sum += vec_dot_q8_0_f32( + &w_row[(blk + 3) * bs..], + &x_row[(blk + 3) * QK..], + ); + blk += 4; + } + while blk < nb { + sum += vec_dot_q8_0_f32(&w_row[blk * bs..], &x_row[blk * QK..]); + blk += 1; + } + unsafe { + *out_ptr.ptr().add(b * out_features + o) = sum; + } + } + }); + } + } + _t => { + // Fallback: dequantize the whole weight and do f32 matmul + let w = weight.to_f32(); + for b in 0..batch { + let x_row = &x.data[b * in_features..(b + 1) * in_features]; + for o in 0..out_features { + let w_row = &w.data[o * in_features..(o + 1) * in_features]; + out[b * out_features + o] = dot_f32(x_row, w_row); + } + } + } + } + + crate::PROF_MATMUL_RAW.fetch_add( + _t.elapsed().as_nanos() as u64, + std::sync::atomic::Ordering::Relaxed, + ); + crate::PROF_MATMUL_RAW_CALLS.fetch_add(1, std::sync::atomic::Ordering::Relaxed); + Tensor { + data: out, + shape: vec![batch, out_features], + } + } + + /// 1D convolution with padding=kernel_size/2 (same padding), given stride. + /// input: [channels_in, length] + /// weight: [channels_out, channels_in, kernel_size] (stored as RawTensor) + /// bias: [channels_out] (f32 Tensor, shaped [1, channels_out]) + /// output: [channels_out, length / stride] + pub fn conv1d(input: &Tensor, weight: &Tensor, bias: &Tensor, stride: usize) -> Tensor { + let ch_out = weight.shape[0]; + let ch_in = weight.shape[1]; + let ksize = weight.shape[2]; + let in_len = input.shape[1]; + let pad = ksize / 2; + let out_len = (in_len + 2 * pad - ksize) / stride + 1; + + if let Some(im2col) = + crate::metal_backend::try_im2col_1d_f32(&input.data, ch_in, in_len, ksize, stride, pad) + { + let k = ch_in * ksize; + if let Some(mm_out) = + crate::metal_backend::try_matmul_nt_f32(&im2col, &weight.data, out_len, k, ch_out) + { + let mut out = vec![0.0f32; ch_out * out_len]; + let out_ptr = SendPtr::new(out.as_mut_ptr()); + let b_data = &bias.data; + parallel_for(ch_out, |co| { + let b = b_data[co]; + for t in 0..out_len { + unsafe { + *out_ptr.ptr().add(co * out_len + t) = mm_out[t * ch_out + co] + b; + } + } + }); + + return Tensor { + data: out, + shape: vec![ch_out, out_len], + }; + } + } + + let mut out = vec![0.0f32; ch_out * out_len]; + let out_ptr = SendPtr::new(out.as_mut_ptr()); + let in_data = &input.data; + let w_data = &weight.data; + let b_data = &bias.data; + parallel_for(ch_out, |co| { + let b = b_data[co]; + let w_co = &w_data[co * ch_in * ksize..(co + 1) * ch_in * ksize]; + for t in 0..out_len { + let mut sum = b; + let in_start = t * stride; + for ci in 0..ch_in { + let in_row_off = ci * in_len; + let w_ci = &w_co[ci * ksize..(ci + 1) * ksize]; + for k in 0..ksize { + let in_pos = in_start + k; + let in_pos = in_pos as isize - pad as isize; + if in_pos >= 0 && (in_pos as usize) < in_len { + sum += in_data[in_row_off + in_pos as usize] * w_ci[k]; + } + } + } + unsafe { + *out_ptr.ptr().add(co * out_len + t) = sum; + } + } + }); + + Tensor { + data: out, + shape: vec![ch_out, out_len], + } + } + + /// Same as conv1d but with RawTensor weight (dequantize first) + pub fn conv1d_raw(input: &Tensor, weight: &RawTensor, bias: &Tensor, stride: usize) -> Tensor { + if weight.shape.len() == 3 { + let ksize = weight.shape[0]; + let ch_in = weight.shape[1]; + let ch_out = weight.shape[2]; + let in_len = input.shape[1]; + let pad = ksize / 2; + let out_len = (in_len + 2 * pad - ksize) / stride + 1; + + if let Some(im2col) = crate::metal_backend::try_im2col_1d_f32( + &input.data, + ch_in, + in_len, + ksize, + stride, + pad, + ) { + let k = ch_in * ksize; + let mm_out = match weight.ggml_type { + GGML_TYPE_F32 => crate::metal_backend::try_matmul_nt_f32_bytes( + &im2col, + &weight.data, + out_len, + k, + ch_out, + ), + GGML_TYPE_F16 => crate::metal_backend::try_matmul_nt_f16_bytes( + &im2col, + &weight.data, + out_len, + k, + ch_out, + ), + GGML_TYPE_Q4_0 | GGML_TYPE_Q4_1 | GGML_TYPE_Q5_0 | GGML_TYPE_Q5_1 + | GGML_TYPE_Q8_0 + if quant_gpu_enabled() => + { + crate::metal_backend::try_matmul_nt_ggml_bytes( + &im2col, + &weight.data, + weight.ggml_type, + out_len, + k, + ch_out, + ) + } + _ => None, + }; + + if let Some(mm_out) = mm_out { + let mut out = vec![0.0f32; ch_out * out_len]; + let out_ptr = SendPtr::new(out.as_mut_ptr()); + let b_data = &bias.data; + parallel_for(ch_out, |co| { + let b = b_data[co]; + for t in 0..out_len { + unsafe { + *out_ptr.ptr().add(co * out_len + t) = mm_out[t * ch_out + co] + b; + } + } + }); + return Tensor { + data: out, + shape: vec![ch_out, out_len], + }; + } + } + } + + let w = weight.to_f32(); + Self::conv1d(input, &w, bias, stride) + } +} diff --git a/libs/voice2/src/lib.rs b/libs/voice2/src/lib.rs new file mode 100644 index 0000000000..e63e0655c2 --- /dev/null +++ b/libs/voice2/src/lib.rs @@ -0,0 +1,85 @@ +#[path = "cpu/decode_loop.rs"] +mod decode_loop; +#[path = "cpu/decoder.rs"] +mod decoder; +#[path = "cpu/encoder.rs"] +mod encoder; +#[path = "cpu/mel.rs"] +mod mel; +#[path = "metal/backend.rs"] +mod metal_backend; +#[path = "cpu/model.rs"] +mod model; +#[path = "cpu/quant.rs"] +mod quant; +mod settings; +#[path = "cpu/tensor.rs"] +mod tensor; +mod transcriber; + +#[cfg(all(any(target_os = "macos", target_os = "ios"), not(force_whisper)))] +#[path = "apple/speech.rs"] +pub mod apple_speech; + +pub use decode_loop::{Segment, WhisperParams, WhisperState}; +pub use model::WhisperModel; +pub use transcriber::{ + NativeAppleTranscriber, VoiceBackendKind, VoiceTranscribeError, VoiceTranscribeParams, + VoiceTranscriber, WhisperTranscriber, +}; + +use std::sync::atomic::{AtomicU64, Ordering}; + +pub(crate) static PROF_MATMUL_RAW: AtomicU64 = AtomicU64::new(0); +pub(crate) static PROF_MATMUL_RAW_CALLS: AtomicU64 = AtomicU64::new(0); +pub(crate) static PROF_MATMUL_T: AtomicU64 = AtomicU64::new(0); +pub(crate) static PROF_MATMUL_T_CALLS: AtomicU64 = AtomicU64::new(0); +pub(crate) static PROF_ENCODER: AtomicU64 = AtomicU64::new(0); +pub(crate) static PROF_ENC_ATTN: AtomicU64 = AtomicU64::new(0); +pub(crate) static PROF_ENC_CONV: AtomicU64 = AtomicU64::new(0); +pub(crate) static PROF_ENC_ELEM: AtomicU64 = AtomicU64::new(0); +pub(crate) static PROF_DECODER: AtomicU64 = AtomicU64::new(0); +pub(crate) static PROF_DECODER_CALLS: AtomicU64 = AtomicU64::new(0); +pub(crate) static PROF_CROSS_KV: AtomicU64 = AtomicU64::new(0); +pub(crate) static PROF_MEL: AtomicU64 = AtomicU64::new(0); + +pub fn reset_profiling() { + PROF_MATMUL_RAW.store(0, Ordering::Relaxed); + PROF_MATMUL_RAW_CALLS.store(0, Ordering::Relaxed); + PROF_MATMUL_T.store(0, Ordering::Relaxed); + PROF_MATMUL_T_CALLS.store(0, Ordering::Relaxed); + PROF_ENCODER.store(0, Ordering::Relaxed); + PROF_ENC_ATTN.store(0, Ordering::Relaxed); + PROF_ENC_CONV.store(0, Ordering::Relaxed); + PROF_ENC_ELEM.store(0, Ordering::Relaxed); + PROF_DECODER.store(0, Ordering::Relaxed); + PROF_DECODER_CALLS.store(0, Ordering::Relaxed); + PROF_CROSS_KV.store(0, Ordering::Relaxed); + PROF_MEL.store(0, Ordering::Relaxed); +} + +pub fn print_profiling() { + let to_ms = |v: &AtomicU64| v.load(Ordering::Relaxed) as f64 / 1_000_000.0; + eprintln!("--- profiling ---"); + eprintln!(" mel: {:.1}ms", to_ms(&PROF_MEL)); + eprintln!(" encoder: {:.1}ms", to_ms(&PROF_ENCODER)); + eprintln!(" enc_conv: {:.1}ms", to_ms(&PROF_ENC_CONV)); + eprintln!(" enc_attn: {:.1}ms", to_ms(&PROF_ENC_ATTN)); + eprintln!(" enc_elem: {:.1}ms", to_ms(&PROF_ENC_ELEM)); + eprintln!(" cross_kv: {:.1}ms", to_ms(&PROF_CROSS_KV)); + eprintln!( + " decoder: {:.1}ms ({} calls)", + to_ms(&PROF_DECODER), + PROF_DECODER_CALLS.load(Ordering::Relaxed) + ); + eprintln!( + " matmul_raw: {:.1}ms ({} calls)", + to_ms(&PROF_MATMUL_RAW), + PROF_MATMUL_RAW_CALLS.load(Ordering::Relaxed) + ); + eprintln!( + " matmul_t: {:.1}ms ({} calls)", + to_ms(&PROF_MATMUL_T), + PROF_MATMUL_T_CALLS.load(Ordering::Relaxed) + ); +} diff --git a/libs/voice2/src/metal/backend.rs b/libs/voice2/src/metal/backend.rs new file mode 100644 index 0000000000..7d963f6bcf --- /dev/null +++ b/libs/voice2/src/metal/backend.rs @@ -0,0 +1,7073 @@ +#[allow(dead_code)] +fn log_mul_mat_requested() -> bool { + crate::settings::LOG_METAL_MUL_MAT +} + +fn metal_requested() -> bool { + crate::settings::USE_METAL_BACKEND +} + +pub(crate) fn try_matmul_nn_f32( + a: &[f32], + b: &[f32], + m: usize, + k: usize, + n: usize, +) -> Option> { + if !metal_requested() { + return None; + } + makepad_ggml::backend::metal::try_matmul_nn_f32(a, b, m, k, n) +} + +pub(crate) fn try_matmul_nt_f32( + a: &[f32], + bt: &[f32], + m: usize, + k: usize, + n: usize, +) -> Option> { + if !metal_requested() { + return None; + } + makepad_ggml::backend::metal::try_matmul_nt_f32(a, bt, m, k, n) +} + +pub(crate) fn try_matmul_nt_f32_bytes( + a: &[f32], + bt_bytes: &[u8], + m: usize, + k: usize, + n: usize, +) -> Option> { + if !metal_requested() { + return None; + } + makepad_ggml::backend::metal::try_matmul_nt_f32_bytes(a, bt_bytes, m, k, n) +} + +pub(crate) fn try_matmul_nt_f16_bytes( + a: &[f32], + bt_f16_bytes: &[u8], + m: usize, + k: usize, + n: usize, +) -> Option> { + if !metal_requested() { + return None; + } + makepad_ggml::backend::metal::try_matmul_nt_f16_bytes(a, bt_f16_bytes, m, k, n) +} + +pub(crate) fn try_matmul_nt_ggml_bytes( + a: &[f32], + bt_bytes: &[u8], + bt_ggml_type: u32, + m: usize, + k: usize, + n: usize, +) -> Option> { + if !metal_requested() { + return None; + } + makepad_ggml::backend::metal::try_matmul_nt_ggml_bytes(a, bt_bytes, bt_ggml_type, m, k, n) +} + +pub(crate) fn try_matmul_nt_ggml_bytes_add_bias( + a: &[f32], + bt_bytes: &[u8], + bt_ggml_type: u32, + m: usize, + k: usize, + n: usize, + bias: &[f32], +) -> Option> { + if !metal_requested() { + return None; + } + makepad_ggml::backend::metal::try_matmul_nt_ggml_bytes_add_bias( + a, + bt_bytes, + bt_ggml_type, + m, + k, + n, + bias, + ) +} + +pub(crate) fn try_flash_attn_f32_packed( + q: &[f32], + k: &[f32], + v: &[f32], + n_q: usize, + n_kv: usize, + n_head: usize, + d: usize, + scale: f32, +) -> Option> { + if !metal_requested() { + return None; + } + makepad_ggml::backend::metal::try_flash_attn_f32_packed(q, k, v, n_q, n_kv, n_head, d, scale) +} + +pub(crate) fn clear_decoder_kv_cache() { + if !metal_requested() { + return; + } + makepad_ggml::backend::metal::clear_decoder_kv_cache(); +} + +#[allow(clippy::too_many_arguments)] +pub(crate) fn try_flash_attn_f32_self_kv_cache( + layer: usize, + q: &[f32], + k_all: &[f32], + v_all: &[f32], + n_kv: usize, + n_head: usize, + d: usize, + scale: f32, +) -> Option> { + if !metal_requested() { + return None; + } + makepad_ggml::backend::metal::try_flash_attn_f32_self_kv_cache( + layer, q, k_all, v_all, n_kv, n_head, d, scale, + ) +} + +#[allow(clippy::too_many_arguments)] +pub(crate) fn try_flash_attn_f32_cross_kv_cache( + layer: usize, + q: &[f32], + k_cross: &[f32], + v_cross: &[f32], + n_q: usize, + n_kv: usize, + n_head: usize, + d: usize, + scale: f32, +) -> Option> { + if !metal_requested() { + return None; + } + makepad_ggml::backend::metal::try_flash_attn_f32_cross_kv_cache( + layer, q, k_cross, v_cross, n_q, n_kv, n_head, d, scale, + ) +} + +pub(crate) fn try_add_f32( + a: &[f32], + a_shape: &[usize], + b: &[f32], + b_shape: &[usize], +) -> Option> { + if !metal_requested() { + return None; + } + makepad_ggml::backend::metal::try_add_f32(a, a_shape, b, b_shape) +} + +pub(crate) fn try_mul_f32( + a: &[f32], + a_shape: &[usize], + b: &[f32], + b_shape: &[usize], +) -> Option> { + if !metal_requested() { + return None; + } + makepad_ggml::backend::metal::try_mul_f32(a, a_shape, b, b_shape) +} + +pub(crate) fn try_gelu_f32(a: &[f32], shape: &[usize]) -> Option> { + if !metal_requested() { + return None; + } + makepad_ggml::backend::metal::try_gelu_f32(a, shape) +} + +pub(crate) fn try_layer_norm_f32(x: &[f32], shape: &[usize], eps: f32) -> Option> { + if !metal_requested() { + return None; + } + makepad_ggml::backend::metal::try_layer_norm_f32(x, shape, eps) +} + +pub(crate) fn try_layer_norm_mul_add_f32( + x: &[f32], + x_shape: &[usize], + mul: &[f32], + mul_shape: &[usize], + add: &[f32], + add_shape: &[usize], + eps: f32, +) -> Option> { + if !metal_requested() { + return None; + } + makepad_ggml::backend::metal::try_layer_norm_mul_add_f32( + x, + x_shape, + mul, + mul_shape, + add, + add_shape, + eps, + ) +} + +pub(crate) fn try_im2col_1d_f32( + input: &[f32], + ic: usize, + iw: usize, + kw: usize, + stride: usize, + pad: usize, +) -> Option> { + if !metal_requested() { + return None; + } + makepad_ggml::backend::metal::try_im2col_1d_f32(input, ic, iw, kw, stride, pad) +} + +#[allow(clippy::too_many_arguments)] +pub(crate) fn try_encoder_attn_block_f32( + x: &[f32], + seq_len: usize, + n_state: usize, + n_head: usize, + ln_w: &[f32], + ln_b: &[f32], + q_w_bytes: &[u8], + q_w_ggml_type: u32, + q_b: &[f32], + k_w_bytes: &[u8], + k_w_ggml_type: u32, + v_w_bytes: &[u8], + v_w_ggml_type: u32, + v_b: &[f32], + out_w_bytes: &[u8], + out_w_ggml_type: u32, + out_b: &[f32], +) -> Option> { + if !metal_requested() { + return None; + } + imp::try_encoder_attn_block_f32( + x, + seq_len, + n_state, + n_head, + ln_w, + ln_b, + q_w_bytes, + q_w_ggml_type, + q_b, + k_w_bytes, + k_w_ggml_type, + v_w_bytes, + v_w_ggml_type, + v_b, + out_w_bytes, + out_w_ggml_type, + out_b, + ) +} + +#[allow(clippy::too_many_arguments)] +pub(crate) fn try_encoder_ffn_block_f32( + x: &[f32], + seq_len: usize, + n_state: usize, + ln_w: &[f32], + ln_b: &[f32], + w0_bytes: &[u8], + w0_ggml_type: u32, + b0: &[f32], + w1_bytes: &[u8], + w1_ggml_type: u32, + b1: &[f32], +) -> Option> { + if !metal_requested() { + return None; + } + imp::try_encoder_ffn_block_f32( + x, + seq_len, + n_state, + ln_w, + ln_b, + w0_bytes, + w0_ggml_type, + b0, + w1_bytes, + w1_ggml_type, + b1, + ) +} + +#[allow(clippy::too_many_arguments)] +pub(crate) fn try_encoder_layer_f32( + x: &[f32], + seq_len: usize, + n_state: usize, + n_head: usize, + attn_ln_w: &[f32], + attn_ln_b: &[f32], + q_w_bytes: &[u8], + q_w_ggml_type: u32, + q_b: &[f32], + k_w_bytes: &[u8], + k_w_ggml_type: u32, + v_w_bytes: &[u8], + v_w_ggml_type: u32, + v_b: &[f32], + out_w_bytes: &[u8], + out_w_ggml_type: u32, + out_b: &[f32], + mlp_ln_w: &[f32], + mlp_ln_b: &[f32], + w0_bytes: &[u8], + w0_ggml_type: u32, + b0: &[f32], + w1_bytes: &[u8], + w1_ggml_type: u32, + b1: &[f32], +) -> Option> { + if !metal_requested() { + return None; + } + imp::try_encoder_layer_f32( + x, + seq_len, + n_state, + n_head, + attn_ln_w, + attn_ln_b, + q_w_bytes, + q_w_ggml_type, + q_b, + k_w_bytes, + k_w_ggml_type, + v_w_bytes, + v_w_ggml_type, + v_b, + out_w_bytes, + out_w_ggml_type, + out_b, + mlp_ln_w, + mlp_ln_b, + w0_bytes, + w0_ggml_type, + b0, + w1_bytes, + w1_ggml_type, + b1, + ) +} + +pub(crate) fn try_encoder_stack_f32( + x: &[f32], + seq_len: usize, + n_state: usize, + n_head: usize, + layers: &[crate::model::EncoderLayer], + final_ln_w: &[f32], + final_ln_b: &[f32], +) -> Option> { + if !metal_requested() { + return None; + } + imp::try_encoder_stack_f32(x, seq_len, n_state, n_head, layers, final_ln_w, final_ln_b) +} + +#[allow(clippy::too_many_arguments)] +pub(crate) fn try_decoder_self_qkv_step_f32( + x: &[f32], + n_state: usize, + attn_ln_w: &[f32], + attn_ln_b: &[f32], + q_w_bytes: &[u8], + q_w_ggml_type: u32, + q_b: &[f32], + k_w_bytes: &[u8], + k_w_ggml_type: u32, + v_w_bytes: &[u8], + v_w_ggml_type: u32, + v_b: &[f32], +) -> Option<(Vec, Vec, Vec)> { + if !metal_requested() { + return None; + } + imp::try_decoder_self_qkv_step_f32( + x, + n_state, + attn_ln_w, + attn_ln_b, + q_w_bytes, + q_w_ggml_type, + q_b, + k_w_bytes, + k_w_ggml_type, + v_w_bytes, + v_w_ggml_type, + v_b, + ) +} + +#[allow(clippy::too_many_arguments)] +pub(crate) fn try_decoder_cross_ffn_step_f32( + layer_idx: usize, + x: &[f32], + n_state: usize, + n_head: usize, + k_cross: &[f32], + v_cross: &[f32], + n_audio_ctx: usize, + layer: &crate::model::DecoderLayer, +) -> Option> { + if !metal_requested() { + return None; + } + imp::try_decoder_cross_ffn_step_f32( + layer_idx, + x, + n_state, + n_head, + k_cross, + v_cross, + n_audio_ctx, + layer, + ) +} + +#[allow(clippy::too_many_arguments)] +pub(crate) fn try_decoder_self_cross_ffn_step_f32( + layer_idx: usize, + x: &[f32], + q_self: &[f32], + k_all: &[f32], + v_all: &[f32], + n_kv: usize, + n_state: usize, + n_head: usize, + k_cross: &[f32], + v_cross: &[f32], + n_audio_ctx: usize, + layer: &crate::model::DecoderLayer, +) -> Option> { + if !metal_requested() { + return None; + } + imp::try_decoder_self_cross_ffn_step_f32( + layer_idx, + x, + q_self, + k_all, + v_all, + n_kv, + n_state, + n_head, + k_cross, + v_cross, + n_audio_ctx, + layer, + ) +} + +pub(crate) fn is_requested() -> bool { + metal_requested() +} + +#[cfg(not(target_os = "macos"))] +mod imp { + pub(super) fn try_matmul_nn_f32( + _a: &[f32], + _b: &[f32], + _m: usize, + _k: usize, + _n: usize, + ) -> Option> { + None + } + + pub(super) fn try_matmul_nt_f32( + _a: &[f32], + _bt: &[f32], + _m: usize, + _k: usize, + _n: usize, + ) -> Option> { + None + } + + pub(super) fn try_matmul_nt_f32_bytes( + _a: &[f32], + _bt_bytes: &[u8], + _m: usize, + _k: usize, + _n: usize, + ) -> Option> { + None + } + + pub(super) fn try_matmul_nt_f16_bytes( + _a: &[f32], + _bt_f16_bytes: &[u8], + _m: usize, + _k: usize, + _n: usize, + ) -> Option> { + None + } + + pub(super) fn try_matmul_nt_ggml_bytes( + _a: &[f32], + _bt_bytes: &[u8], + _bt_ggml_type: u32, + _m: usize, + _k: usize, + _n: usize, + ) -> Option> { + None + } + + pub(super) fn try_matmul_nt_ggml_bytes_add_bias( + _a: &[f32], + _bt_bytes: &[u8], + _bt_ggml_type: u32, + _m: usize, + _k: usize, + _n: usize, + _bias: &[f32], + ) -> Option> { + None + } + + pub(super) fn try_flash_attn_f32_packed( + _q: &[f32], + _k: &[f32], + _v: &[f32], + _n_q: usize, + _n_kv: usize, + _n_head: usize, + _d: usize, + _scale: f32, + ) -> Option> { + None + } + + pub(super) fn clear_decoder_kv_cache() {} + + #[allow(clippy::too_many_arguments)] + pub(super) fn try_flash_attn_f32_self_kv_cache( + _layer: usize, + _q: &[f32], + _k_all: &[f32], + _v_all: &[f32], + _n_kv: usize, + _n_head: usize, + _d: usize, + _scale: f32, + ) -> Option> { + None + } + + #[allow(clippy::too_many_arguments)] + pub(super) fn try_flash_attn_f32_cross_kv_cache( + _layer: usize, + _q: &[f32], + _k_cross: &[f32], + _v_cross: &[f32], + _n_q: usize, + _n_kv: usize, + _n_head: usize, + _d: usize, + _scale: f32, + ) -> Option> { + None + } + + pub(super) fn try_add_f32( + _a: &[f32], + _a_shape: &[usize], + _b: &[f32], + _b_shape: &[usize], + ) -> Option> { + None + } + + pub(super) fn try_mul_f32( + _a: &[f32], + _a_shape: &[usize], + _b: &[f32], + _b_shape: &[usize], + ) -> Option> { + None + } + + pub(super) fn try_gelu_f32(_a: &[f32], _shape: &[usize]) -> Option> { + None + } + + pub(super) fn try_layer_norm_f32(_x: &[f32], _shape: &[usize], _eps: f32) -> Option> { + None + } + + pub(super) fn try_layer_norm_mul_add_f32( + _x: &[f32], + _x_shape: &[usize], + _mul: &[f32], + _mul_shape: &[usize], + _add: &[f32], + _add_shape: &[usize], + _eps: f32, + ) -> Option> { + None + } + + pub(super) fn try_im2col_1d_f32( + _input: &[f32], + _ic: usize, + _iw: usize, + _kw: usize, + _stride: usize, + _pad: usize, + ) -> Option> { + None + } + + #[allow(clippy::too_many_arguments)] + pub(super) fn try_encoder_attn_block_f32( + _x: &[f32], + _seq_len: usize, + _n_state: usize, + _n_head: usize, + _ln_w: &[f32], + _ln_b: &[f32], + _q_w_bytes: &[u8], + _q_w_ggml_type: u32, + _q_b: &[f32], + _k_w_bytes: &[u8], + _k_w_ggml_type: u32, + _v_w_bytes: &[u8], + _v_w_ggml_type: u32, + _v_b: &[f32], + _out_w_bytes: &[u8], + _out_w_ggml_type: u32, + _out_b: &[f32], + ) -> Option> { + None + } + + #[allow(clippy::too_many_arguments)] + pub(super) fn try_encoder_ffn_block_f32( + _x: &[f32], + _seq_len: usize, + _n_state: usize, + _ln_w: &[f32], + _ln_b: &[f32], + _w0_bytes: &[u8], + _w0_ggml_type: u32, + _b0: &[f32], + _w1_bytes: &[u8], + _w1_ggml_type: u32, + _b1: &[f32], + ) -> Option> { + None + } + + #[allow(clippy::too_many_arguments)] + pub(super) fn try_encoder_layer_f32( + _x: &[f32], + _seq_len: usize, + _n_state: usize, + _n_head: usize, + _attn_ln_w: &[f32], + _attn_ln_b: &[f32], + _q_w_bytes: &[u8], + _q_w_ggml_type: u32, + _q_b: &[f32], + _k_w_bytes: &[u8], + _k_w_ggml_type: u32, + _v_w_bytes: &[u8], + _v_w_ggml_type: u32, + _v_b: &[f32], + _out_w_bytes: &[u8], + _out_w_ggml_type: u32, + _out_b: &[f32], + _mlp_ln_w: &[f32], + _mlp_ln_b: &[f32], + _w0_bytes: &[u8], + _w0_ggml_type: u32, + _b0: &[f32], + _w1_bytes: &[u8], + _w1_ggml_type: u32, + _b1: &[f32], + ) -> Option> { + None + } + + pub(super) fn try_encoder_stack_f32( + _x: &[f32], + _seq_len: usize, + _n_state: usize, + _n_head: usize, + _layers: &[crate::model::EncoderLayer], + _final_ln_w: &[f32], + _final_ln_b: &[f32], + ) -> Option> { + None + } + + #[allow(clippy::too_many_arguments)] + pub(super) fn try_decoder_self_qkv_step_f32( + _x: &[f32], + _n_state: usize, + _attn_ln_w: &[f32], + _attn_ln_b: &[f32], + _q_w_bytes: &[u8], + _q_w_ggml_type: u32, + _q_b: &[f32], + _k_w_bytes: &[u8], + _k_w_ggml_type: u32, + _v_w_bytes: &[u8], + _v_w_ggml_type: u32, + _v_b: &[f32], + ) -> Option<(Vec, Vec, Vec)> { + None + } + + #[allow(clippy::too_many_arguments)] + pub(super) fn try_decoder_cross_ffn_step_f32( + _layer_idx: usize, + _x: &[f32], + _n_state: usize, + _n_head: usize, + _k_cross: &[f32], + _v_cross: &[f32], + _n_audio_ctx: usize, + _layer: &crate::model::DecoderLayer, + ) -> Option> { + None + } + + #[allow(clippy::too_many_arguments)] + pub(super) fn try_decoder_self_cross_ffn_step_f32( + _layer_idx: usize, + _x: &[f32], + _q_self: &[f32], + _k_all: &[f32], + _v_all: &[f32], + _n_kv: usize, + _n_state: usize, + _n_head: usize, + _k_cross: &[f32], + _v_cross: &[f32], + _n_audio_ctx: usize, + _layer: &crate::model::DecoderLayer, + ) -> Option> { + None + } +} + +#[cfg(target_os = "macos")] +mod imp { + use crate::model::{DecoderLayer, EncoderLayer}; + use crate::quant::{ + block_size, f32_to_f16, GGML_TYPE_F16, GGML_TYPE_F32, GGML_TYPE_Q4_0, GGML_TYPE_Q4_1, + GGML_TYPE_Q5_0, GGML_TYPE_Q5_1, GGML_TYPE_Q8_0, + }; + use makepad_objc_sys::runtime::{nil, ObjcId, Object, YES}; + use makepad_objc_sys::{class, msg_send, sel, sel_impl}; + use std::cell::RefCell; + use std::collections::HashMap; + use std::ffi::{c_char, c_void, CStr}; + use std::ptr::NonNull; + use std::sync::OnceLock; + + const UTF8_ENCODING: u64 = 4; + const MTL_RESOURCE_STORAGE_MODE_SHARED: u64 = 0; + const MTL_RESOURCE_OPTIONS_STORAGE_MODE_PRIVATE: u64 = 32; + const MTL_STORAGE_MODE_PRIVATE: u64 = 2; + const MTL_GPU_FAMILY_APPLE6: u64 = 1006; + const MTL_GPU_FAMILY_METAL3: u64 = 5001; + const MTL_GPU_FAMILY_METAL4: u64 = 5002; + + const MTL_DATA_TYPE_INT: u64 = 29; + const MTL_DATA_TYPE_SHORT: u64 = 37; + const MTL_DATA_TYPE_BOOL: u64 = 53; + + const FC_FLASH_ATTN_EXT_PAD: i32 = 100; + const FC_FLASH_ATTN_EXT_BLK: i32 = 200; + const FC_FLASH_ATTN_EXT: i32 = 300; + const FC_FLASH_ATTN_EXT_VEC: i32 = 400; + const FC_FLASH_ATTN_EXT_VEC_REDUCE: i32 = 500; + const FC_MUL_MV: i32 = 600; + const FC_MUL_MM: i32 = 700; + const FC_UNARY: i32 = 1200; + const FC_BIN: i32 = 1300; + const OP_FLASH_ATTN_EXT_NQPSG: i32 = 8; + const OP_FLASH_ATTN_EXT_NCPSG: i32 = 64; + const OP_FLASH_ATTN_EXT_VEC_NQPSG: i32 = 1; + const OP_FLASH_ATTN_EXT_VEC_NCPSG: i32 = 32; + const OP_UNARY_NUM_GELU: i16 = 103; + const SCRATCH_FLASH_PAD: u8 = 1; + const SCRATCH_FLASH_BLK: u8 = 2; + const SCRATCH_FLASH_TMP: u8 = 3; + const SCRATCH_FLASH_OUT: u8 = 4; + const SCRATCH_FLASH_MASK: u8 = 5; + const SCRATCH_ENC_NORM0: u8 = 10; + const SCRATCH_ENC_NORM1: u8 = 11; + const SCRATCH_DEC_NORM0: u8 = 12; + const SCRATCH_DEC_NORM1: u8 = 13; + const SCRATCH_ENC_FLASH_K_F16: u8 = 14; + const SCRATCH_ENC_FLASH_V_F16: u8 = 15; + + const N_R0_Q4_0: i32 = 4; + const N_SG_Q4_0: i32 = 2; + + const N_R0_Q4_1: i32 = 4; + const N_SG_Q4_1: i32 = 2; + + const N_R0_Q5_0: i32 = 4; + const N_SG_Q5_0: i32 = 2; + + const N_R0_Q5_1: i32 = 4; + const N_SG_Q5_1: i32 = 2; + + const N_R0_Q8_0: i32 = 2; + const N_SG_Q8_0: i32 = 4; + + const _GGML_METAL_SOURCE_RAW: &str = include_str!("ggml/ggml-metal.metal"); + const _GGML_COMMON_H: &str = include_str!("ggml/ggml-common.h"); + const _GGML_METAL_IMPL_H: &str = include_str!("ggml/ggml-metal-impl.h"); + const _GGML_METALLIB_BYTES: &[u8] = include_bytes!(env!("MAKEPAD_VOICE2_GGML_METALLIB")); + + #[link(name = "Metal", kind = "framework")] + extern "C" { + fn MTLCreateSystemDefaultDevice() -> ObjcId; + fn MTLCopyAllDevices() -> ObjcId; + } + + #[link(name = "Foundation", kind = "framework")] + extern "C" {} + + #[repr(C)] + #[derive(Copy, Clone)] + struct MTLSize { + width: u64, + height: u64, + depth: u64, + } + + #[derive(Clone, Copy, Debug, Eq, Hash, PartialEq)] + struct BufferKey { + ptr: usize, + len: usize, + tag: u8, + } + + #[derive(Clone, Copy, Debug, Eq, Hash, PartialEq)] + enum Src0Type { + F32, + F16, + Q4_0, + Q4_1, + Q5_0, + Q5_1, + Q8_0, + } + + #[derive(Clone, Copy, Debug)] + enum FunctionConstantValue { + Int32(i32), + Int16(i16), + Bool(bool), + } + + #[derive(Clone, Copy, Debug)] + struct FunctionConstant { + idx: i32, + value: FunctionConstantValue, + } + + #[repr(C)] + #[derive(Copy, Clone)] + struct KArgsMulMm { + ne00: i32, + ne02: i32, + nb01: u64, + nb02: u64, + nb03: u64, + ne12: i32, + nb10: u64, + nb11: u64, + nb12: u64, + nb13: u64, + ne0: i32, + ne1: i32, + r2: i16, + r3: i16, + } + + #[repr(C)] + #[derive(Copy, Clone)] + struct KArgsMulMv { + ne00: i32, + ne01: i32, + ne02: i32, + nb00: u64, + nb01: u64, + nb02: u64, + nb03: u64, + ne10: i32, + ne11: i32, + ne12: i32, + nb10: u64, + nb11: u64, + nb12: u64, + nb13: u64, + ne0: i32, + ne1: i32, + nr0: i32, + r2: i16, + r3: i16, + } + + #[repr(C)] + #[derive(Copy, Clone)] + struct KArgsMulMvExt { + ne00: i32, + ne01: i32, + ne02: i32, + nb00: u64, + nb01: u64, + nb02: u64, + nb03: u64, + ne10: i32, + ne11: i32, + ne12: i32, + nb10: u64, + nb11: u64, + nb12: u64, + nb13: u64, + ne0: i32, + ne1: i32, + r2: i16, + r3: i16, + } + + #[repr(C)] + #[derive(Copy, Clone)] + struct KArgsCpy { + nk0: i64, + ne00: i64, + ne01: i64, + ne02: i64, + ne03: i64, + nb00: u64, + nb01: u64, + nb02: u64, + nb03: u64, + ne0: i64, + ne1: i64, + ne2: i64, + ne3: i64, + nb0: u64, + nb1: u64, + nb2: u64, + nb3: u64, + } + + #[repr(C)] + #[derive(Copy, Clone)] + struct KArgsFlashAttnExtPad { + ne11: i32, + ne_12_2: i32, + ne_12_3: i32, + nb11: u64, + nb12: u64, + nb13: u64, + nb21: u64, + nb22: u64, + nb23: u64, + ne31: i32, + ne32: i32, + ne33: i32, + nb31: u64, + nb32: u64, + nb33: u64, + } + + #[repr(C)] + #[derive(Copy, Clone)] + struct KArgsFlashAttnExtBlk { + ne01: i32, + ne30: i32, + ne31: i32, + ne32: i32, + ne33: i32, + nb31: u64, + nb32: u64, + nb33: u64, + } + + #[repr(C)] + #[derive(Copy, Clone)] + struct KArgsFlashAttnExt { + ne01: i32, + ne02: i32, + ne03: i32, + nb01: u64, + nb02: u64, + nb03: u64, + ne11: i32, + ne_12_2: i32, + ne_12_3: i32, + ns10: i32, + nb11: u64, + nb12: u64, + nb13: u64, + ns20: i32, + nb21: u64, + nb22: u64, + nb23: u64, + ne31: i32, + ne32: i32, + ne33: i32, + nb31: u64, + nb32: u64, + nb33: u64, + ne1: i32, + ne2: i32, + ne3: i32, + scale: f32, + max_bias: f32, + m0: f32, + m1: f32, + n_head_log2: i32, + logit_softcap: f32, + } + + #[repr(C)] + #[derive(Copy, Clone)] + struct KArgsFlashAttnExtVec { + ne01: i32, + ne02: i32, + ne03: i32, + nb01: u64, + nb02: u64, + nb03: u64, + ne11: i32, + ne_12_2: i32, + ne_12_3: i32, + ns10: i32, + nb11: u64, + nb12: u64, + nb13: u64, + ns20: i32, + nb21: u64, + nb22: u64, + nb23: u64, + ne31: i32, + ne32: i32, + ne33: i32, + nb31: u64, + nb32: u64, + nb33: u64, + ne1: i32, + ne2: i32, + ne3: i32, + scale: f32, + max_bias: f32, + m0: f32, + m1: f32, + n_head_log2: i32, + logit_softcap: f32, + } + + #[repr(C)] + #[derive(Copy, Clone)] + struct KArgsFlashAttnExtVecReduce { + nrows: i32, + } + + #[repr(C)] + #[derive(Copy, Clone)] + struct KArgsUnary { + ne00: i32, + ne01: i32, + ne02: i32, + ne03: i32, + nb00: u64, + nb01: u64, + nb02: u64, + nb03: u64, + ne0: i32, + ne1: i32, + ne2: i32, + ne3: i32, + nb0: u64, + nb1: u64, + nb2: u64, + nb3: u64, + slope: f32, + scale: f32, + bias: f32, + val: f32, + min: f32, + max: f32, + } + + #[repr(C)] + #[derive(Copy, Clone)] + struct KArgsBin { + ne00: i32, + ne01: i32, + ne02: i32, + ne03: i32, + nb00: u64, + nb01: u64, + nb02: u64, + nb03: u64, + ne10: i32, + ne11: i32, + ne12: i32, + ne13: i32, + nb10: u64, + nb11: u64, + nb12: u64, + nb13: u64, + ne0: i32, + ne1: i32, + ne2: i32, + ne3: i32, + nb0: u64, + nb1: u64, + nb2: u64, + nb3: u64, + offs: u64, + o1: [u64; 8], + } + + #[repr(C)] + #[derive(Copy, Clone)] + struct KArgsNorm { + ne00: i32, + ne00_t: i32, + nb1: u64, + nb2: u64, + nb3: u64, + eps: f32, + nef1: [i32; 3], + nef2: [i32; 3], + nef3: [i32; 3], + nbf1: [u64; 3], + nbf2: [u64; 3], + nbf3: [u64; 3], + } + + #[repr(C)] + #[derive(Copy, Clone)] + struct KArgsIm2Col { + ofs0: u64, + ofs1: u64, + iw: i32, + ih: i32, + chw: i32, + s0: i32, + s1: i32, + p0: i32, + p1: i32, + d0: i32, + d1: i32, + n: i32, + kh: i32, + kw: i32, + khw: i32, + } + + #[derive(Copy, Clone)] + struct Shape4 { + ne: [i32; 4], + nb: [u64; 4], + numel: usize, + } + + struct StrongId(NonNull); + + impl StrongId { + unsafe fn from_owned(id: ObjcId) -> Option { + NonNull::new(id).map(Self) + } + + unsafe fn from_unowned(id: ObjcId) -> Option { + if id.is_null() { + return None; + } + let _: () = msg_send![id, retain]; + NonNull::new(id).map(Self) + } + + fn as_id(&self) -> ObjcId { + self.0.as_ptr() + } + } + + impl Drop for StrongId { + fn drop(&mut self) { + unsafe { + let _: () = msg_send![self.0.as_ptr(), release]; + } + } + } + + struct AutoreleasePool(ObjcId); + + impl AutoreleasePool { + fn new() -> Self { + let pool: ObjcId = unsafe { msg_send![class!(NSAutoreleasePool), new] }; + Self(pool) + } + } + + impl Drop for AutoreleasePool { + fn drop(&mut self) { + if !self.0.is_null() { + unsafe { + let _: () = msg_send![self.0, release]; + } + } + } + } + + fn nsstring_to_string(ns_string: ObjcId) -> String { + if ns_string.is_null() { + return String::new(); + } + unsafe { + let utf8_ptr: *const c_char = msg_send![ns_string, UTF8String]; + if utf8_ptr.is_null() { + return String::new(); + } + CStr::from_ptr(utf8_ptr).to_string_lossy().into_owned() + } + } + + fn str_to_nsstring_owned(s: &str) -> ObjcId { + unsafe { + let ns_string: ObjcId = msg_send![class!(NSString), alloc]; + if ns_string.is_null() { + return nil; + } + msg_send![ + ns_string, + initWithBytes: s.as_ptr() as *const c_void + length: s.len() as u64 + encoding: UTF8_ENCODING + ] + } + } + + fn ns_error_to_string(error: ObjcId) -> String { + if error.is_null() { + return "unknown Metal error".to_string(); + } + unsafe { + let desc: ObjcId = msg_send![error, localizedDescription]; + nsstring_to_string(desc) + } + } + + fn device_supports_family(device: ObjcId, family: u64) -> bool { + unsafe { msg_send![device, supportsFamily: family] } + } + + fn metal_compile_feature_macros(device: ObjcId) -> (bool, bool) { + let mut has_bfloat = device_supports_family(device, MTL_GPU_FAMILY_METAL3) + || device_supports_family(device, MTL_GPU_FAMILY_APPLE6); + if crate::settings::DISABLE_GGML_METAL_BF16 { + has_bfloat = false; + } + + let mut has_tensor = device_supports_family(device, MTL_GPU_FAMILY_METAL4); + if crate::settings::DISABLE_GGML_METAL_TENSOR { + has_tensor = false; + } + + if !crate::settings::FORCE_ENABLE_GGML_METAL_TENSOR && has_tensor { + let dev_name_obj: ObjcId = unsafe { msg_send![device, name] }; + let dev_name = nsstring_to_string(dev_name_obj); + let tensor_whitelisted = dev_name.contains("M5") + || dev_name.contains("M6") + || dev_name.contains("A19") + || dev_name.contains("A20"); + if !tensor_whitelisted { + has_tensor = false; + } + } + + (has_bfloat, has_tensor) + } + + fn read_text_with_fallback(paths: &[&str], fallback: &str) -> String { + for path in paths { + if let Ok(text) = std::fs::read_to_string(path) { + return text; + } + } + fallback.to_string() + } + + fn build_ggml_source() -> String { + // Development path: prefer source files from disk so shader-pruning tools can + // iterate without rebuilding the Rust crate every edit. + let mut src = read_text_with_fallback( + &[ + "libs/voice2/src/ggml-metal/ggml-metal.metal", + "src/ggml-metal/ggml-metal.metal", + "libs/voice2/src/metal/ggml/ggml-metal.metal", + "src/metal/ggml/ggml-metal.metal", + ], + _GGML_METAL_SOURCE_RAW, + ); + let common_h = read_text_with_fallback( + &[ + "libs/voice2/src/ggml-metal/ggml-common.h", + "src/ggml-metal/ggml-common.h", + "libs/voice2/src/metal/ggml/ggml-common.h", + "src/metal/ggml/ggml-common.h", + ], + _GGML_COMMON_H, + ); + let impl_h = read_text_with_fallback( + &[ + "libs/voice2/src/ggml-metal/ggml-metal-impl.h", + "src/ggml-metal/ggml-metal-impl.h", + "libs/voice2/src/metal/ggml/ggml-metal-impl.h", + "src/metal/ggml/ggml-metal-impl.h", + ], + _GGML_METAL_IMPL_H, + ); + src = src.replace("__embed_ggml-common.h__", &common_h); + src = src.replace("#include \"ggml-common.h\"", &common_h); + src = src.replace("#include \"ggml-metal-impl.h\"", &impl_h); + src + } + + fn src0_type_from_ggml(t: u32) -> Option { + match t { + GGML_TYPE_F32 => Some(Src0Type::F32), + GGML_TYPE_F16 => Some(Src0Type::F16), + GGML_TYPE_Q4_0 => Some(Src0Type::Q4_0), + GGML_TYPE_Q4_1 => Some(Src0Type::Q4_1), + GGML_TYPE_Q5_0 => Some(Src0Type::Q5_0), + GGML_TYPE_Q5_1 => Some(Src0Type::Q5_1), + GGML_TYPE_Q8_0 => Some(Src0Type::Q8_0), + _ => None, + } + } + + fn src0_type_name(t: Src0Type) -> &'static str { + match t { + Src0Type::F32 => "f32", + Src0Type::F16 => "f16", + Src0Type::Q4_0 => "q4_0", + Src0Type::Q4_1 => "q4_1", + Src0Type::Q5_0 => "q5_0", + Src0Type::Q5_1 => "q5_1", + Src0Type::Q8_0 => "q8_0", + } + } + + fn src0_layout_bytes_per_row(t: Src0Type, k: usize) -> Result<(usize, u64), String> { + match t { + Src0Type::F32 => Ok(( + k.checked_mul(4) + .ok_or_else(|| "overflow computing f32 row bytes".to_string())?, + 4, + )), + Src0Type::F16 => Ok(( + k.checked_mul(2) + .ok_or_else(|| "overflow computing f16 row bytes".to_string())?, + 2, + )), + Src0Type::Q4_0 | Src0Type::Q4_1 | Src0Type::Q5_0 | Src0Type::Q5_1 | Src0Type::Q8_0 => { + if k % 32 != 0 { + return Err(format!( + "quantized kernel requires K multiple of 32, got {}", + k + )); + } + let ggml_type = match t { + Src0Type::Q4_0 => GGML_TYPE_Q4_0, + Src0Type::Q4_1 => GGML_TYPE_Q4_1, + Src0Type::Q5_0 => GGML_TYPE_Q5_0, + Src0Type::Q5_1 => GGML_TYPE_Q5_1, + Src0Type::Q8_0 => GGML_TYPE_Q8_0, + _ => unreachable!(), + }; + let bs = block_size(ggml_type); + let row = (k / 32) + .checked_mul(bs) + .ok_or_else(|| "overflow computing quantized row bytes".to_string())?; + Ok((row, bs as u64)) + } + } + } + + fn shape4_from_row_major(shape: &[usize], elem_bytes: u64) -> Result { + if shape.is_empty() { + return Err("shape must be non-empty".to_string()); + } + if shape.len() > 4 { + return Err(format!( + "shape rank > 4 is unsupported in metal elementwise path: {:?}", + shape + )); + } + let mut ne = [1i32; 4]; + for (i, &d) in shape.iter().rev().enumerate() { + ne[i] = i32::try_from(d).map_err(|_| format!("shape dim too large: {}", d))?; + } + let mut nb = [0u64; 4]; + nb[0] = elem_bytes; + for i in 1..4 { + nb[i] = nb[i - 1] + .checked_mul(ne[i - 1] as u64) + .ok_or_else(|| "overflow computing strides".to_string())?; + } + let numel = shape.iter().try_fold(1usize, |acc, &d| { + acc.checked_mul(d) + .ok_or_else(|| "overflow computing tensor numel".to_string()) + })?; + Ok(Shape4 { ne, nb, numel }) + } + + fn nrows(s: &Shape4) -> usize { + (s.ne[1] as usize) + .saturating_mul(s.ne[2] as usize) + .saturating_mul(s.ne[3] as usize) + } + + fn f32_slice_as_bytes(s: &[f32]) -> &[u8] { + unsafe { + std::slice::from_raw_parts( + s.as_ptr() as *const u8, + s.len() * std::mem::size_of::(), + ) + } + } + + fn flash_attn_supported_head_dim(d: usize) -> bool { + matches!( + d, + 32 | 40 | 48 | 64 | 72 | 80 | 96 | 112 | 128 | 192 | 256 | 576 + ) + } + + fn flash_attn_use_vec(n_q: usize, d: usize) -> bool { + n_q < 20 && d % 32 == 0 + } + + fn flash_attn_kv_elem_bytes(src_type: Src0Type) -> Result { + match src_type { + Src0Type::F32 => Ok(std::mem::size_of::()), + Src0Type::F16 => Ok(std::mem::size_of::()), + _ => Err(format!( + "unsupported flash-attn kv type for Metal path: {:?}", + src_type + )), + } + } + + #[derive(Clone, Copy, Default)] + struct FlashAttnExtParams { + has_mask: bool, + has_sinks: bool, + max_bias: f32, + logit_softcap: f32, + } + + fn pad_to(v: usize, align: usize) -> usize { + if align == 0 { + return v; + } + let rem = v % align; + if rem == 0 { + v + } else { + v + (align - rem) + } + } + + fn flash_attn_smem_bytes(dk: usize, dv: usize, _nsg: i32) -> usize { + let nqptg = OP_FLASH_ATTN_EXT_NQPSG as usize; + let ncpsg = OP_FLASH_ATTN_EXT_NCPSG as usize; + + // Matches ggml-metal FATTN_SMEM() for non-quantized f32 K/V. + let words = nqptg.saturating_mul(dk + 2 * pad_to(dv, 64) + 2 * (2 * ncpsg)); + pad_to(words.saturating_mul(std::mem::size_of::() / 2), 16) + } + + fn flash_attn_vec_smem_bytes(dk: usize, dv: usize, nsg: i32) -> usize { + let ncpsg = OP_FLASH_ATTN_EXT_VEC_NCPSG as usize; + let words = + (pad_to(dk, 128) + 4 * ncpsg + 2 * pad_to(dv, 128)).saturating_mul(nsg.max(1) as usize); + pad_to(words.saturating_mul(std::mem::size_of::() / 2), 16) + } + + fn flash_attn_ext_extra_pad_bytes( + n_q: usize, + n_kv: usize, + n_head: usize, + d: usize, + kv_elem_bytes: usize, + has_mask: bool, + use_vec: bool, + ) -> Result { + // Match ggml-metal: reserve non-vec sized padding space, but gate by the active kernel kvpad. + let reserve_ncpsg = OP_FLASH_ATTN_EXT_NCPSG as usize; + let active_ncpsg = if use_vec { + OP_FLASH_ATTN_EXT_VEC_NCPSG as usize + } else { + OP_FLASH_ATTN_EXT_NCPSG as usize + }; + let has_kvpad = n_kv % active_ncpsg != 0; + if !has_kvpad { + return Ok(0); + } + + let n_state = n_head + .checked_mul(d) + .ok_or_else(|| "overflow computing flash n_state".to_string())?; + let nb11 = n_state + .checked_mul(kv_elem_bytes) + .ok_or_else(|| "overflow computing flash nb11".to_string())?; + let nb21 = nb11; + + let k_term = nb11 + .checked_mul(n_head) + .ok_or_else(|| "overflow computing flash extra pad K bytes".to_string())?; + let v_term = nb21 + .checked_mul(n_head) + .ok_or_else(|| "overflow computing flash extra pad V bytes".to_string())?; + let mask_term = if has_mask { + std::mem::size_of::() + .checked_mul(n_q) + .ok_or_else(|| "overflow computing flash extra pad mask bytes".to_string())? + } else { + 0 + }; + + reserve_ncpsg + .checked_mul( + k_term + .checked_add(v_term) + .and_then(|v| v.checked_add(mask_term)) + .ok_or_else(|| "overflow computing flash extra pad size".to_string())?, + ) + .ok_or_else(|| "overflow computing flash extra pad size".to_string()) + } + + fn flash_attn_ext_extra_blk_bytes( + n_q: usize, + n_kv: usize, + has_mask: bool, + use_vec: bool, + ) -> Result { + if !has_mask { + return Ok(0); + } + + let nqptg = if use_vec { + OP_FLASH_ATTN_EXT_VEC_NQPSG as usize + } else { + OP_FLASH_ATTN_EXT_NQPSG as usize + }; + let ncpsg = if use_vec { + OP_FLASH_ATTN_EXT_VEC_NCPSG as usize + } else { + OP_FLASH_ATTN_EXT_NCPSG as usize + }; + + let ne1 = (n_q + nqptg - 1) / nqptg; + let ne0 = (n_kv + ncpsg - 1) / ncpsg; + let raw = ne0 + .checked_mul(ne1) + .ok_or_else(|| "overflow computing flash extra blk size".to_string())?; + + Ok(pad_to(raw, 32)) + } + + fn flash_attn_ext_extra_tmp_bytes( + n_q: usize, + n_head: usize, + d: usize, + nwg: usize, + ) -> Result { + let ne01_max = n_q.min(32); + std::mem::size_of::() + .checked_mul(ne01_max) + .and_then(|v| v.checked_mul(n_head)) + .and_then(|v| v.checked_mul(nwg)) + .and_then(|v| v.checked_mul(d + 2)) + .ok_or_else(|| "overflow computing flash extra tmp size".to_string()) + } + + fn can_use_mul_mv_ext(src0: Src0Type, ne00: i32, ne11: i32) -> bool { + if ne00 % 128 != 0 { + return false; + } + if !(2..=8).contains(&ne11) { + return false; + } + matches!( + src0, + Src0Type::F32 + | Src0Type::F16 + | Src0Type::Q4_0 + | Src0Type::Q4_1 + | Src0Type::Q5_0 + | Src0Type::Q5_1 + | Src0Type::Q8_0 + ) + } + + struct PipelineState { + obj: StrongId, + smem: usize, + nsg: i32, + nr0: i32, + nr1: i32, + } + + struct DecoderKvLayer { + k: StrongId, + v: StrongId, + n_state: usize, + cap_rows: usize, + len_rows: usize, + } + + struct CrossKvLayer { + k: StrongId, + v: StrongId, + n_state: usize, + n_rows: usize, + src_k_ptr: usize, + src_v_ptr: usize, + src_k_len: usize, + src_v_len: usize, + } + + struct ScratchBuffer { + buf: StrongId, + cap_bytes: usize, + is_private: bool, + } + + struct MetalContext { + device: StrongId, + command_queue: StrongId, + library: StrongId, + pipeline_cache: HashMap, + cached_weight_buffers: HashMap, + scratch_buffers: HashMap, + matmul_out_buffers: HashMap, + decoder_kv_layers: HashMap, + cross_kv_layers: HashMap, + batch_depth: usize, + batch_command_buffer: Option, + batch_encoder: Option, + last_command_buffer: Option, + } + + impl MetalContext { + fn create_device() -> Option { + unsafe { + let dev = MTLCreateSystemDefaultDevice(); + if let Some(dev) = StrongId::from_owned(dev) { + return Some(dev); + } + + let all = MTLCopyAllDevices(); + if all.is_null() { + return None; + } + + let count: u64 = msg_send![all, count]; + let first: ObjcId = if count > 0 { + msg_send![all, objectAtIndex: 0u64] + } else { + nil + }; + let _: () = msg_send![all, release]; + + StrongId::from_unowned(first) + } + } + + fn new() -> Result { + let _pool = AutoreleasePool::new(); + + let device = Self::create_device().ok_or_else(|| { + "unable to create Metal device (MTLCreateSystemDefaultDevice and MTLCopyAllDevices returned nil)" + .to_string() + })?; + + let command_queue_obj: ObjcId = unsafe { msg_send![device.as_id(), newCommandQueue] }; + let command_queue = unsafe { StrongId::from_owned(command_queue_obj) } + .ok_or_else(|| "newCommandQueue returned nil".to_string())?; + + let library = match Self::load_library_from_metallib(device.as_id()) { + Ok(Some(lib)) => lib, + Ok(None) => { + let source = build_ggml_source(); + Self::compile_library(device.as_id(), &source)? + } + Err(err) => { + eprintln!( + "[voice2][metal] precompiled metallib load failed, compiling source: {}", + err + ); + let source = build_ggml_source(); + Self::compile_library(device.as_id(), &source)? + } + }; + + eprintln!("[voice2][metal] backend initialized (ggml kernels)"); + + Ok(Self { + device, + command_queue, + library, + pipeline_cache: HashMap::new(), + cached_weight_buffers: HashMap::new(), + scratch_buffers: HashMap::new(), + matmul_out_buffers: HashMap::new(), + decoder_kv_layers: HashMap::new(), + cross_kv_layers: HashMap::new(), + batch_depth: 0, + batch_command_buffer: None, + batch_encoder: None, + last_command_buffer: None, + }) + } + + fn load_library_from_metallib(device: ObjcId) -> Result, String> { + if _GGML_METALLIB_BYTES.is_empty() { + return Ok(None); + } + + let _pool = AutoreleasePool::new(); + + let data_obj: ObjcId = unsafe { + msg_send![ + class!(NSData), + dataWithBytes: _GGML_METALLIB_BYTES.as_ptr() as *const c_void + length: _GGML_METALLIB_BYTES.len() as u64 + ] + }; + if data_obj.is_null() { + return Err("NSData::dataWithBytes returned nil".to_string()); + } + + let mut error: ObjcId = nil; + let library_obj: ObjcId = + unsafe { msg_send![device, newLibraryWithData: data_obj error: &mut error] }; + if library_obj.is_null() { + return Err(format!( + "newLibraryWithData failed: {}", + ns_error_to_string(error) + )); + } + + let library = unsafe { StrongId::from_owned(library_obj) } + .ok_or_else(|| "newLibraryWithData returned nil".to_string())?; + Ok(Some(library)) + } + + fn compile_library(device: ObjcId, source: &str) -> Result { + let _pool = AutoreleasePool::new(); + + let options_obj: ObjcId = unsafe { msg_send![class!(MTLCompileOptions), new] }; + let options = unsafe { StrongId::from_owned(options_obj) } + .ok_or_else(|| "MTLCompileOptions::new returned nil".to_string())?; + unsafe { + let _: () = msg_send![options.as_id(), setFastMathEnabled: YES]; + } + + let (has_bfloat, has_tensor) = metal_compile_feature_macros(device); + if has_bfloat || has_tensor { + let prep_obj: ObjcId = + unsafe { msg_send![class!(NSMutableDictionary), dictionary] }; + if !prep_obj.is_null() { + if has_bfloat { + let key_obj = str_to_nsstring_owned("GGML_METAL_HAS_BF16"); + let val_obj = str_to_nsstring_owned("1"); + let key = unsafe { StrongId::from_owned(key_obj) } + .ok_or_else(|| "failed to build metal macro key".to_string())?; + let val = unsafe { StrongId::from_owned(val_obj) } + .ok_or_else(|| "failed to build metal macro value".to_string())?; + unsafe { + let _: () = + msg_send![prep_obj, setObject: val.as_id() forKey: key.as_id()]; + } + } + if has_tensor { + let key_obj = str_to_nsstring_owned("GGML_METAL_HAS_TENSOR"); + let val_obj = str_to_nsstring_owned("1"); + let key = unsafe { StrongId::from_owned(key_obj) } + .ok_or_else(|| "failed to build metal macro key".to_string())?; + let val = unsafe { StrongId::from_owned(val_obj) } + .ok_or_else(|| "failed to build metal macro value".to_string())?; + unsafe { + let _: () = + msg_send![prep_obj, setObject: val.as_id() forKey: key.as_id()]; + } + } + unsafe { + let _: () = msg_send![options.as_id(), setPreprocessorMacros: prep_obj]; + } + } + } + + let source_obj = str_to_nsstring_owned(source); + let source_obj = unsafe { StrongId::from_owned(source_obj) } + .ok_or_else(|| "failed to create NSString for Metal source".to_string())?; + + let mut error: ObjcId = nil; + let library_obj: ObjcId = unsafe { + msg_send![ + device, + newLibraryWithSource: source_obj.as_id() + options: options.as_id() + error: &mut error + ] + }; + + unsafe { StrongId::from_owned(library_obj) }.ok_or_else(|| { + format!("newLibraryWithSource failed: {}", ns_error_to_string(error)) + }) + } + + fn new_buffer_with_bytes(&self, bytes: &[u8]) -> Result { + let obj: ObjcId = unsafe { + msg_send![ + self.device.as_id(), + newBufferWithBytes: bytes.as_ptr() as *const c_void + length: bytes.len() as u64 + options: MTL_RESOURCE_STORAGE_MODE_SHARED + ] + }; + unsafe { StrongId::from_owned(obj) } + .ok_or_else(|| format!("newBufferWithBytes failed for {} bytes", bytes.len())) + } + + fn new_buffer_with_length(&self, byte_len: usize) -> Result { + let obj: ObjcId = unsafe { + msg_send![ + self.device.as_id(), + newBufferWithLength: byte_len as u64 + options: MTL_RESOURCE_STORAGE_MODE_SHARED + ] + }; + unsafe { StrongId::from_owned(obj) } + .ok_or_else(|| format!("newBufferWithLength failed for {} bytes", byte_len)) + } + + fn new_buffer_with_length_private(&self, byte_len: usize) -> Result { + let obj: ObjcId = unsafe { + msg_send![ + self.device.as_id(), + newBufferWithLength: byte_len as u64 + options: MTL_RESOURCE_OPTIONS_STORAGE_MODE_PRIVATE + ] + }; + unsafe { StrongId::from_owned(obj) }.ok_or_else(|| { + format!("newBufferWithLength(private) failed for {} bytes", byte_len) + }) + } + + fn get_or_create_scratch_buffer( + &mut self, + kind: u8, + need_bytes: usize, + ) -> Result { + let need_bytes = need_bytes.max(1); + let is_private = matches!( + kind, + SCRATCH_FLASH_PAD + | SCRATCH_FLASH_BLK + | SCRATCH_FLASH_TMP + | SCRATCH_FLASH_OUT + | SCRATCH_ENC_NORM0 + | SCRATCH_ENC_NORM1 + | SCRATCH_DEC_NORM0 + | SCRATCH_DEC_NORM1 + ); + if let Some(entry) = self.scratch_buffers.get(&kind) { + if entry.cap_bytes >= need_bytes && entry.is_private == is_private { + return Ok(entry.buf.as_id()); + } + } + + let buf = if is_private { + self.new_buffer_with_length_private(need_bytes)? + } else { + self.new_buffer_with_length(need_bytes)? + }; + self.scratch_buffers.insert( + kind, + ScratchBuffer { + buf, + cap_bytes: need_bytes, + is_private, + }, + ); + + Ok(self.scratch_buffers.get(&kind).unwrap().buf.as_id()) + } + + fn get_or_create_matmul_out_buffer( + &mut self, + tag: u8, + need_bytes: usize, + ) -> Result { + let need_bytes = need_bytes.max(1); + if let Some(entry) = self.matmul_out_buffers.get(&tag) { + if entry.cap_bytes >= need_bytes { + return Ok(entry.buf.as_id()); + } + } + + // Mirror ggml-metal backend allocation for compute intermediates: + // matrix outputs are GPU-only private buffers. + let buf = self.new_buffer_with_length_private(need_bytes)?; + self.matmul_out_buffers.insert( + tag, + ScratchBuffer { + buf, + cap_bytes: need_bytes, + is_private: true, + }, + ); + + Ok(self.matmul_out_buffers.get(&tag).unwrap().buf.as_id()) + } + + fn buffer_length_bytes(&self, buffer: ObjcId) -> Result { + let len_u64: u64 = unsafe { msg_send![buffer, length] }; + usize::try_from(len_u64).map_err(|_| format!("buffer length too large: {}", len_u64)) + } + + fn zero_buffer_range( + &self, + buffer: ObjcId, + offset_bytes: usize, + len_bytes: usize, + ) -> Result<(), String> { + if len_bytes == 0 { + return Ok(()); + } + let buf_len = self.buffer_length_bytes(buffer)?; + let end = offset_bytes + .checked_add(len_bytes) + .ok_or_else(|| "overflow computing zero range end".to_string())?; + if end > buf_len { + return Err(format!( + "zero range out of bounds: offset={}, len={}, buffer_len={}", + offset_bytes, len_bytes, buf_len + )); + } + let ptr: *mut u8 = unsafe { msg_send![buffer, contents] }; + if ptr.is_null() { + return Err("buffer contents returned null".to_string()); + } + unsafe { + std::ptr::write_bytes(ptr.add(offset_bytes), 0u8, len_bytes); + } + Ok(()) + } + + fn prepare_decoder_self_mask_f16( + &mut self, + n_valid: usize, + n_total: usize, + ) -> Result { + if n_total == 0 || n_valid > n_total { + return Err(format!( + "invalid decoder self mask sizes: n_valid={}, n_total={}", + n_valid, n_total + )); + } + + let mask_bytes = n_total + .checked_mul(std::mem::size_of::()) + .ok_or_else(|| "overflow computing decoder self mask bytes".to_string())?; + let mask_id = self.get_or_create_scratch_buffer(SCRATCH_FLASH_MASK, mask_bytes)?; + let mask_cap = self.buffer_length_bytes(mask_id)?; + if mask_bytes > mask_cap { + return Err(format!( + "decoder self mask write exceeds buffer: need={}, cap={}", + mask_bytes, mask_cap + )); + } + let ptr: *mut u16 = unsafe { msg_send![mask_id, contents] }; + if ptr.is_null() { + return Err("decoder self mask buffer contents returned null".to_string()); + } + + let neg_inf_h = f32_to_f16(f32::NEG_INFINITY); + unsafe { + let dst = std::slice::from_raw_parts_mut(ptr, n_total); + for v in dst.iter_mut().take(n_valid) { + *v = 0; + } + for v in dst.iter_mut().skip(n_valid) { + *v = neg_inf_h; + } + } + + Ok(mask_id) + } + + fn copy_f32_buffer_contents( + &self, + buffer: ObjcId, + elems: usize, + ) -> Result, String> { + let byte_len = elems + .checked_mul(std::mem::size_of::()) + .ok_or_else(|| "overflow computing f32 copy byte length".to_string())?; + let cap = self.buffer_length_bytes(buffer)?; + if byte_len > cap { + return Err(format!( + "f32 read exceeds buffer: need={} bytes, cap={} bytes", + byte_len, cap + )); + } + let out_ptr: *const c_void = unsafe { msg_send![buffer, contents] }; + if out_ptr.is_null() { + return Err("output buffer contents returned null".to_string()); + } + + let mut out = vec![0.0f32; elems]; + unsafe { + std::ptr::copy_nonoverlapping(out_ptr as *const f32, out.as_mut_ptr(), elems); + } + Ok(out) + } + + fn copy_buffer_to_shared_staging( + &self, + src_buffer: ObjcId, + len_bytes: usize, + ) -> Result { + let len_bytes = len_bytes.max(1); + let src_len = self.buffer_length_bytes(src_buffer)?; + if len_bytes > src_len { + return Err(format!( + "staging copy exceeds source buffer: need={} bytes, src_len={} bytes", + len_bytes, src_len + )); + } + let dst_buffer = self.new_buffer_with_length(len_bytes)?; + + let command_buffer_obj: ObjcId = + unsafe { msg_send![self.command_queue.as_id(), commandBuffer] }; + let command_buffer = unsafe { StrongId::from_unowned(command_buffer_obj) } + .ok_or_else(|| "commandBuffer returned nil".to_string())?; + + let blit_encoder_obj: ObjcId = + unsafe { msg_send![command_buffer.as_id(), blitCommandEncoder] }; + let blit_encoder = unsafe { StrongId::from_unowned(blit_encoder_obj) } + .ok_or_else(|| "blitCommandEncoder returned nil".to_string())?; + + unsafe { + let _: () = msg_send![ + blit_encoder.as_id(), + copyFromBuffer: src_buffer + sourceOffset: 0u64 + toBuffer: dst_buffer.as_id() + destinationOffset: 0u64 + size: len_bytes as u64 + ]; + let _: () = msg_send![blit_encoder.as_id(), endEncoding]; + let _: () = msg_send![command_buffer.as_id(), commit]; + let _: () = msg_send![command_buffer.as_id(), waitUntilCompleted]; + } + + let status: u64 = unsafe { msg_send![command_buffer.as_id(), status] }; + if status == 5 { + let error: ObjcId = unsafe { msg_send![command_buffer.as_id(), error] }; + return Err(format!( + "Metal command buffer error (buffer staging copy): {}", + ns_error_to_string(error) + )); + } + + Ok(dst_buffer) + } + + fn copy_f32_buffer_contents_readable( + &self, + buffer: ObjcId, + elems: usize, + ) -> Result, String> { + let byte_len = elems + .checked_mul(std::mem::size_of::()) + .ok_or_else(|| "overflow computing readback byte length".to_string())?; + let storage_mode: u64 = unsafe { msg_send![buffer, storageMode] }; + if storage_mode == MTL_STORAGE_MODE_PRIVATE { + let staging = self.copy_buffer_to_shared_staging(buffer, byte_len)?; + self.copy_f32_buffer_contents(staging.as_id(), elems) + } else { + self.copy_f32_buffer_contents(buffer, elems) + } + } + + fn read_f32_buffer(&self, buffer: ObjcId, elems: usize) -> Result, String> { + self.wait_queue_idle()?; + self.copy_f32_buffer_contents_readable(buffer, elems) + } + + fn read_f32_buffers3( + &self, + b0: ObjcId, + b1: ObjcId, + b2: ObjcId, + elems: usize, + ) -> Result<(Vec, Vec, Vec), String> { + self.wait_queue_idle()?; + let o0 = self.copy_f32_buffer_contents_readable(b0, elems)?; + let o1 = self.copy_f32_buffer_contents_readable(b1, elems)?; + let o2 = self.copy_f32_buffer_contents_readable(b2, elems)?; + Ok((o0, o1, o2)) + } + + fn get_or_create_cached_f32_buffer( + &mut self, + data: &[f32], + tag: u8, + ) -> Result { + let bytes = f32_slice_as_bytes(data); + let key = BufferKey { + ptr: data.as_ptr() as usize, + len: bytes.len(), + tag, + }; + self.get_or_create_weight_buffer(key, bytes) + } + + fn get_or_create_weight_buffer( + &mut self, + key: BufferKey, + bytes: &[u8], + ) -> Result { + if !self.cached_weight_buffers.contains_key(&key) { + let buf = self.new_buffer_with_bytes(bytes)?; + self.cached_weight_buffers.insert(key, buf); + } + Ok(self.cached_weight_buffers.get(&key).unwrap().as_id()) + } + + fn clear_decoder_kv_cache(&mut self) { + self.decoder_kv_layers.clear(); + self.cross_kv_layers.clear(); + } + + fn ensure_decoder_kv_layer( + &mut self, + layer: usize, + n_state: usize, + need_rows: usize, + ) -> Result<(ObjcId, ObjcId), String> { + let need_rows = need_rows.max(1); + + if let Some(entry) = self.decoder_kv_layers.get(&layer) { + if entry.n_state == n_state && entry.cap_rows >= need_rows { + return Ok((entry.k.as_id(), entry.v.as_id())); + } + } + + let row_bytes = n_state + .checked_mul(std::mem::size_of::()) + .ok_or_else(|| "overflow computing decoder kv row bytes".to_string())?; + + let old = self.decoder_kv_layers.remove(&layer); + let cap_rows = if let Some(ref old) = old { + if old.n_state == n_state { + old.cap_rows.saturating_mul(2).max(need_rows).max(32) + } else { + need_rows.max(32) + } + } else { + need_rows.max(32) + }; + let total_bytes = cap_rows + .checked_mul(row_bytes) + .ok_or_else(|| "overflow computing decoder kv bytes".to_string())?; + + let new_k = self.new_buffer_with_length(total_bytes)?; + let new_v = self.new_buffer_with_length(total_bytes)?; + + let mut len_rows = 0usize; + if let Some(old) = old { + if old.n_state == n_state && old.len_rows > 0 { + let copy_rows = old.len_rows.min(cap_rows); + let copy_bytes = copy_rows + .checked_mul(row_bytes) + .ok_or_else(|| "overflow computing decoder kv copy bytes".to_string())?; + let old_k_ptr: *const u8 = unsafe { msg_send![old.k.as_id(), contents] }; + let old_v_ptr: *const u8 = unsafe { msg_send![old.v.as_id(), contents] }; + let new_k_ptr: *mut u8 = unsafe { msg_send![new_k.as_id(), contents] }; + let new_v_ptr: *mut u8 = unsafe { msg_send![new_v.as_id(), contents] }; + if old_k_ptr.is_null() + || old_v_ptr.is_null() + || new_k_ptr.is_null() + || new_v_ptr.is_null() + { + return Err("decoder kv buffer contents returned null".to_string()); + } + unsafe { + std::ptr::copy_nonoverlapping(old_k_ptr, new_k_ptr, copy_bytes); + std::ptr::copy_nonoverlapping(old_v_ptr, new_v_ptr, copy_bytes); + } + len_rows = copy_rows; + } + } + + self.decoder_kv_layers.insert( + layer, + DecoderKvLayer { + k: new_k, + v: new_v, + n_state, + cap_rows, + len_rows, + }, + ); + let entry = self + .decoder_kv_layers + .get(&layer) + .ok_or_else(|| "decoder kv layer insertion failed".to_string())?; + Ok((entry.k.as_id(), entry.v.as_id())) + } + + fn ensure_cross_kv_layer( + &mut self, + layer: usize, + n_state: usize, + n_rows: usize, + k_cross: &[f32], + v_cross: &[f32], + ) -> Result<(ObjcId, ObjcId), String> { + if n_state == 0 || n_rows == 0 { + return Err(format!( + "invalid cross kv dimensions: n_state={}, n_rows={}", + n_state, n_rows + )); + } + if k_cross.len() != v_cross.len() { + return Err(format!( + "cross kv k/v len mismatch: k={}, v={}", + k_cross.len(), + v_cross.len(), + )); + } + if k_cross.len() % n_state != 0 { + return Err(format!( + "cross kv len not divisible by n_state: len={}, n_state={}", + k_cross.len(), + n_state + )); + } + + let src_rows = k_cross.len() / n_state; + if src_rows == 0 { + return Err("cross kv has zero rows".to_string()); + } + if src_rows > n_rows { + return Err(format!( + "cross kv source rows exceed requested rows: src_rows={}, n_rows={}", + src_rows, n_rows + )); + } + + let src_k_ptr = k_cross.as_ptr() as usize; + let src_v_ptr = v_cross.as_ptr() as usize; + let src_k_len = k_cross.len(); + let src_v_len = v_cross.len(); + + if let Some(entry) = self.cross_kv_layers.get(&layer) { + if entry.n_state == n_state + && entry.n_rows == n_rows + && entry.src_k_ptr == src_k_ptr + && entry.src_v_ptr == src_v_ptr + && entry.src_k_len == src_k_len + && entry.src_v_len == src_v_len + { + return Ok((entry.k.as_id(), entry.v.as_id())); + } + } + + let row_bytes = n_state + .checked_mul(std::mem::size_of::()) + .ok_or_else(|| "overflow computing cross kv row bytes".to_string())?; + let total_bytes = n_rows + .checked_mul(row_bytes) + .ok_or_else(|| "overflow computing cross kv total bytes".to_string())?; + let copy_bytes = src_rows + .checked_mul(row_bytes) + .ok_or_else(|| "overflow computing cross kv copy bytes".to_string())?; + + let (k_buf, v_buf) = if src_rows == n_rows { + ( + self.new_buffer_with_bytes(f32_slice_as_bytes(k_cross))?, + self.new_buffer_with_bytes(f32_slice_as_bytes(v_cross))?, + ) + } else { + let k_buf = self.new_buffer_with_length(total_bytes)?; + let v_buf = self.new_buffer_with_length(total_bytes)?; + + let dst_k: *mut u8 = unsafe { msg_send![k_buf.as_id(), contents] }; + let dst_v: *mut u8 = unsafe { msg_send![v_buf.as_id(), contents] }; + if dst_k.is_null() || dst_v.is_null() { + return Err("cross kv buffer contents returned null".to_string()); + } + unsafe { + std::ptr::write_bytes(dst_k, 0u8, total_bytes); + std::ptr::write_bytes(dst_v, 0u8, total_bytes); + std::ptr::copy_nonoverlapping(k_cross.as_ptr() as *const u8, dst_k, copy_bytes); + std::ptr::copy_nonoverlapping(v_cross.as_ptr() as *const u8, dst_v, copy_bytes); + } + + (k_buf, v_buf) + }; + self.cross_kv_layers.insert( + layer, + CrossKvLayer { + k: k_buf, + v: v_buf, + n_state, + n_rows, + src_k_ptr, + src_v_ptr, + src_k_len, + src_v_len, + }, + ); + + let entry = self + .cross_kv_layers + .get(&layer) + .ok_or_else(|| "cross kv layer insertion failed".to_string())?; + Ok((entry.k.as_id(), entry.v.as_id())) + } + + fn compile_pipeline( + &self, + base_name: &str, + constants: &[FunctionConstant], + ) -> Result { + let _pool = AutoreleasePool::new(); + + let base_obj = str_to_nsstring_owned(base_name); + let base_obj = unsafe { StrongId::from_owned(base_obj) } + .ok_or_else(|| format!("failed to create NSString for function '{}'", base_name))?; + + let mut error: ObjcId = nil; + let func_obj: ObjcId = if constants.is_empty() { + unsafe { msg_send![self.library.as_id(), newFunctionWithName: base_obj.as_id()] } + } else { + let cv_obj: ObjcId = unsafe { msg_send![class!(MTLFunctionConstantValues), new] }; + let cv = unsafe { StrongId::from_owned(cv_obj) } + .ok_or_else(|| "MTLFunctionConstantValues::new returned nil".to_string())?; + + for c in constants { + unsafe { + match c.value { + FunctionConstantValue::Int32(v) => { + let _: () = msg_send![ + cv.as_id(), + setConstantValue: &v as *const i32 as *const c_void + type: MTL_DATA_TYPE_INT + atIndex: c.idx as u64 + ]; + } + FunctionConstantValue::Int16(v) => { + let _: () = msg_send![ + cv.as_id(), + setConstantValue: &v as *const i16 as *const c_void + type: MTL_DATA_TYPE_SHORT + atIndex: c.idx as u64 + ]; + } + FunctionConstantValue::Bool(v) => { + let b: u8 = if v { 1 } else { 0 }; + let _: () = msg_send![ + cv.as_id(), + setConstantValue: &b as *const u8 as *const c_void + type: MTL_DATA_TYPE_BOOL + atIndex: c.idx as u64 + ]; + } + } + } + } + + unsafe { + msg_send![ + self.library.as_id(), + newFunctionWithName: base_obj.as_id() + constantValues: cv.as_id() + error: &mut error + ] + } + }; + + let func = unsafe { StrongId::from_owned(func_obj) }.ok_or_else(|| { + format!( + "newFunctionWithName('{}') failed: {}", + base_name, + ns_error_to_string(error) + ) + })?; + + let mut error: ObjcId = nil; + let pipeline_obj: ObjcId = unsafe { + msg_send![ + self.device.as_id(), + newComputePipelineStateWithFunction: func.as_id() + error: &mut error + ] + }; + + unsafe { StrongId::from_owned(pipeline_obj) }.ok_or_else(|| { + format!( + "newComputePipelineStateWithFunction('{}') failed: {}", + base_name, + ns_error_to_string(error) + ) + }) + } + + fn get_or_compile_cached_pipeline( + &mut self, + cache_name: String, + base_name: &str, + constants: &[FunctionConstant], + smem: usize, + nr0: i32, + nr1: i32, + nsg: i32, + ) -> Result<(ObjcId, usize, i32, i32, i32), String> { + if !self.pipeline_cache.contains_key(&cache_name) { + if crate::settings::LOG_METAL_PIPELINES { + eprintln!("[voice2][metal] compile_pipeline base={}", base_name); + } + let compiled = self.compile_pipeline(base_name, constants)?; + self.pipeline_cache.insert( + cache_name.clone(), + PipelineState { + obj: compiled, + smem, + nsg, + nr0, + nr1, + }, + ); + } + + let p = self.pipeline_cache.get(&cache_name).unwrap(); + Ok((p.obj.as_id(), p.smem, p.nr0, p.nr1, p.nsg)) + } + + fn pipeline_max_threads(pipeline: ObjcId) -> u64 { + unsafe { msg_send![pipeline, maxTotalThreadsPerThreadgroup] } + } + + fn create_compute_encoder(&self, command_buffer: ObjcId) -> Result { + let encoder_obj: ObjcId = unsafe { msg_send![command_buffer, computeCommandEncoder] }; + + unsafe { StrongId::from_unowned(encoder_obj) } + .ok_or_else(|| "computeCommandEncoder returned nil".to_string()) + } + + fn begin_batch(&mut self) -> Result<(), String> { + if self.batch_depth == 0 { + let command_buffer_obj: ObjcId = + unsafe { msg_send![self.command_queue.as_id(), commandBuffer] }; + let command_buffer = unsafe { StrongId::from_unowned(command_buffer_obj) } + .ok_or_else(|| "commandBuffer returned nil".to_string())?; + + let encoder = self.create_compute_encoder(command_buffer.as_id())?; + + self.batch_command_buffer = Some(command_buffer); + self.batch_encoder = Some(encoder); + } + self.batch_depth += 1; + Ok(()) + } + + fn end_batch(&mut self) -> Result<(), String> { + if self.batch_depth == 0 { + return Err("end_batch called with no active batch".to_string()); + } + + self.batch_depth -= 1; + if self.batch_depth == 0 { + let command_buffer = self + .batch_command_buffer + .take() + .ok_or_else(|| "batch command buffer missing".to_string())?; + let encoder = self + .batch_encoder + .take() + .ok_or_else(|| "batch encoder missing".to_string())?; + + unsafe { + let _: () = msg_send![encoder.as_id(), endEncoding]; + let _: () = msg_send![command_buffer.as_id(), commit]; + } + + self.last_command_buffer = Some(command_buffer); + } + + Ok(()) + } + + fn with_batch(&mut self, f: F) -> Result + where + F: FnOnce(&mut Self) -> Result, + { + self.begin_batch()?; + let out = f(self); + let end_res = self.end_batch(); + match (out, end_res) { + (Ok(v), Ok(())) => Ok(v), + (Err(e), Ok(())) => Err(e), + (Ok(_), Err(e)) => Err(e), + (Err(e), Err(_)) => Err(e), + } + } + + fn begin_command_encoder( + &self, + ) -> Result<(ObjcId, ObjcId, Option<(StrongId, StrongId)>), String> { + if self.batch_depth > 0 { + let command_buffer = self + .batch_command_buffer + .as_ref() + .ok_or_else(|| "batch command buffer missing".to_string())?; + let encoder = self + .batch_encoder + .as_ref() + .ok_or_else(|| "batch encoder missing".to_string())?; + return Ok((command_buffer.as_id(), encoder.as_id(), None)); + } + + let command_buffer_obj: ObjcId = + unsafe { msg_send![self.command_queue.as_id(), commandBuffer] }; + let command_buffer = unsafe { StrongId::from_unowned(command_buffer_obj) } + .ok_or_else(|| "commandBuffer returned nil".to_string())?; + + let encoder = self.create_compute_encoder(command_buffer.as_id())?; + + Ok(( + command_buffer.as_id(), + encoder.as_id(), + Some((command_buffer, encoder)), + )) + } + + fn wait_queue_idle(&self) -> Result<(), String> { + if self.batch_depth > 0 { + return Err("wait_queue_idle called while command batch is active".to_string()); + } + + if let Some(command_buffer) = self.last_command_buffer.as_ref() { + let command_buffer_id = command_buffer.as_id(); + unsafe { + let _: () = msg_send![command_buffer_id, waitUntilCompleted]; + } + let status: u64 = unsafe { msg_send![command_buffer_id, status] }; + if status == 5 { + let error: ObjcId = unsafe { msg_send![command_buffer_id, error] }; + return Err(format!( + "Metal command buffer error (queue idle wait): {}", + ns_error_to_string(error) + )); + } + return Ok(()); + } + + let command_buffer_obj: ObjcId = + unsafe { msg_send![self.command_queue.as_id(), commandBuffer] }; + let command_buffer = unsafe { StrongId::from_unowned(command_buffer_obj) } + .ok_or_else(|| "commandBuffer returned nil".to_string())?; + unsafe { + let _: () = msg_send![command_buffer.as_id(), commit]; + let _: () = msg_send![command_buffer.as_id(), waitUntilCompleted]; + } + let status: u64 = unsafe { msg_send![command_buffer.as_id(), status] }; + if status == 5 { + let error: ObjcId = unsafe { msg_send![command_buffer.as_id(), error] }; + return Err(format!( + "Metal command buffer error (queue idle wait): {}", + ns_error_to_string(error) + )); + } + Ok(()) + } + + fn end_command_encoder( + &mut self, + handles: Option<(StrongId, StrongId)>, + ) -> Result<(), String> { + let Some((command_buffer, encoder)) = handles else { + return Ok(()); + }; + + unsafe { + let _: () = msg_send![encoder.as_id(), endEncoding]; + let _: () = msg_send![command_buffer.as_id(), commit]; + } + + self.last_command_buffer = Some(command_buffer); + + Ok(()) + } + + fn dispatch_mul_mv_ext( + &mut self, + src0: Src0Type, + src0_id: ObjcId, + src1_id: ObjcId, + dst_id: ObjcId, + ne00: i32, + ne01: i32, + ne10: i32, + ne11: i32, + nb00: u64, + nb01: u64, + nb10: u64, + nb11: u64, + ne0: i32, + ne1: i32, + ) -> Result<(), String> { + static LOG_ONCE: OnceLock<()> = OnceLock::new(); + if LOG_ONCE.set(()).is_ok() { + eprintln!("[voice2][metal] mul_mat dispatch: mul_mv_ext"); + } + + let nsg = 2i32; + let nxpsg = if ne00 % 256 == 0 && ne11 < 3 { + 16i32 + } else if ne00 % 128 == 0 { + 8i32 + } else { + 4i32 + }; + let nypsg = 32 / nxpsg; + let r0ptg = nypsg * nsg; + let r1ptg = match ne11 { + 2 => 2, + 3 | 6 => 3, + 4 | 7 | 8 => 4, + 5 => 5, + _ => return Err(format!("unsupported ne11 for mul_mv_ext: {}", ne11)), + }; + + let base = format!( + "kernel_mul_mv_ext_{}_{}_r1_{}", + src0_type_name(src0), + "f32", + r1ptg + ); + let name = format!("{}_nsg={}_nxpsg={}", base, nsg, nxpsg); + + let constants = [ + FunctionConstant { + idx: FC_MUL_MV + 0, + value: FunctionConstantValue::Int16(nsg as i16), + }, + FunctionConstant { + idx: FC_MUL_MV + 1, + value: FunctionConstantValue::Int16(nxpsg as i16), + }, + ]; + + let (pipeline, _smem, _nr0, _nr1, _pnsg) = + self.get_or_compile_cached_pipeline(name, &base, &constants, 0, 0, 0, nsg)?; + + let args = KArgsMulMvExt { + ne00, + ne01, + ne02: 1, + nb00, + nb01, + nb02: nb01 * ne01 as u64, + nb03: nb01 * ne01 as u64, + ne10, + ne11, + ne12: 1, + nb10, + nb11, + nb12: nb11 * ne11 as u64, + nb13: nb11 * ne11 as u64, + ne0, + ne1, + r2: 1, + r3: 1, + }; + + let (_command_buffer, encoder, encoder_handles) = self.begin_command_encoder()?; + unsafe { + let _: () = msg_send![encoder, setComputePipelineState: pipeline]; + let _: () = msg_send![ + encoder, + setBytes: &args as *const KArgsMulMvExt as *const c_void + length: std::mem::size_of::() as u64 + atIndex: 0u64 + ]; + let _: () = msg_send![encoder, setBuffer: src0_id offset: 0u64 atIndex: 1u64]; + let _: () = msg_send![encoder, setBuffer: src1_id offset: 0u64 atIndex: 2u64]; + let _: () = msg_send![encoder, setBuffer: dst_id offset: 0u64 atIndex: 3u64]; + + let tgs = MTLSize { + width: ((ne01 + r0ptg - 1) / r0ptg) as u64, + height: ((ne11 + r1ptg - 1) / r1ptg) as u64, + depth: 1, + }; + let tpg = MTLSize { + width: 32, + height: nsg as u64, + depth: 1, + }; + let _: () = msg_send![ + encoder, + dispatchThreadgroups: tgs + threadsPerThreadgroup: tpg + ]; + } + + self.end_command_encoder(encoder_handles) + } + + fn dispatch_mul_mm( + &mut self, + src0: Src0Type, + src0_id: ObjcId, + src1_id: ObjcId, + dst_id: ObjcId, + ne00: i32, + ne01: i32, + nb01: u64, + ne12: i32, + nb10: u64, + nb11: u64, + ne0: i32, + ne1: i32, + ) -> Result<(), String> { + static LOG_ONCE: OnceLock<()> = OnceLock::new(); + if LOG_ONCE.set(()).is_ok() { + eprintln!("[voice2][metal] mul_mat dispatch: mul_mm"); + } + + let bc_inp = ne00 % 32 != 0; + let bc_out = ne0 % 64 != 0 || ne1 % 32 != 0; + + let base = format!("kernel_mul_mm_{}_{}", src0_type_name(src0), "f32"); + let name = format!("{}_bci={}_bco={}", base, bc_inp as i32, bc_out as i32); + + let smem = if bc_out { + 8192usize + } else { + 4096usize + 2048usize + }; + let constants = [ + FunctionConstant { + idx: FC_MUL_MM + 0, + value: FunctionConstantValue::Bool(bc_inp), + }, + FunctionConstant { + idx: FC_MUL_MM + 1, + value: FunctionConstantValue::Bool(bc_out), + }, + ]; + + let (pipeline, pipeline_smem, _nr0, _nr1, _nsg) = + self.get_or_compile_cached_pipeline(name, &base, &constants, smem, 0, 0, 0)?; + + let args = KArgsMulMm { + ne00, + ne02: 1, + nb01, + nb02: nb01 * ne01 as u64, + nb03: nb01 * ne01 as u64, + ne12, + nb10, + nb11, + nb12: nb11 * ne1 as u64, + nb13: nb11 * ne1 as u64, + ne0, + ne1, + r2: 1, + r3: 1, + }; + + let (_command_buffer, encoder, encoder_handles) = self.begin_command_encoder()?; + unsafe { + let _: () = msg_send![encoder, setComputePipelineState: pipeline]; + let _: () = msg_send![ + encoder, + setBytes: &args as *const KArgsMulMm as *const c_void + length: std::mem::size_of::() as u64 + atIndex: 0u64 + ]; + let _: () = msg_send![encoder, setBuffer: src0_id offset: 0u64 atIndex: 1u64]; + let _: () = msg_send![encoder, setBuffer: src1_id offset: 0u64 atIndex: 2u64]; + let _: () = msg_send![encoder, setBuffer: dst_id offset: 0u64 atIndex: 3u64]; + let _: () = msg_send![ + encoder, + setThreadgroupMemoryLength: pipeline_smem as u64 + atIndex: 0u64 + ]; + + let tgs = MTLSize { + width: ((ne1 + 31) / 32) as u64, + height: ((ne01 + 63) / 64) as u64, + depth: ne12 as u64, + }; + let tpg = MTLSize { + width: 128, + height: 1, + depth: 1, + }; + let _: () = msg_send![ + encoder, + dispatchThreadgroups: tgs + threadsPerThreadgroup: tpg + ]; + } + + self.end_command_encoder(encoder_handles) + } + + fn dispatch_mul_mv( + &mut self, + src0: Src0Type, + src0_id: ObjcId, + src1_id: ObjcId, + dst_id: ObjcId, + ne00: i32, + ne01: i32, + ne10: i32, + ne11: i32, + nb00: u64, + nb01: u64, + nb10: u64, + nb11: u64, + ne0: i32, + ne1: i32, + ) -> Result<(), String> { + static LOG_ONCE: OnceLock<()> = OnceLock::new(); + if LOG_ONCE.set(()).is_ok() { + eprintln!("[voice2][metal] mul_mat dispatch: mul_mv"); + } + + let (nsg, nr0, nr1, smem, suffix) = match src0 { + Src0Type::F32 | Src0Type::F16 => { + if ne00 < 32 { + (1, 32, 1, 0usize, "_short") + } else { + let nsg = ((ne00 + 127) / 128).min(4); + let nr0 = 2; + let smem = 32usize * std::mem::size_of::() * nr0 as usize; + let suffix = if ne00 % 4 == 0 { "_4" } else { "" }; + (nsg, nr0, 1, smem, suffix) + } + } + Src0Type::Q4_0 => (N_SG_Q4_0, N_R0_Q4_0, 1, 0usize, ""), + Src0Type::Q4_1 => (N_SG_Q4_1, N_R0_Q4_1, 1, 0usize, ""), + Src0Type::Q5_0 => (N_SG_Q5_0, N_R0_Q5_0, 1, 0usize, ""), + Src0Type::Q5_1 => (N_SG_Q5_1, N_R0_Q5_1, 1, 0usize, ""), + Src0Type::Q8_0 => ( + N_SG_Q8_0, + N_R0_Q8_0, + 1, + 32usize * std::mem::size_of::() * N_R0_Q8_0 as usize, + "", + ), + }; + + let base = format!("kernel_mul_mv_{}_{}{}", src0_type_name(src0), "f32", suffix); + let name = format!("{}_nsg={}", base, nsg); + let constants = [FunctionConstant { + idx: FC_MUL_MV + 0, + value: FunctionConstantValue::Int16(nsg as i16), + }]; + + let (pipeline, _pipeline_smem, pn0, pn1, pnsg) = + self.get_or_compile_cached_pipeline(name, &base, &constants, smem, nr0, nr1, nsg)?; + + let args = KArgsMulMv { + ne00, + ne01, + ne02: 1, + nb00, + nb01, + nb02: nb01 * ne01 as u64, + nb03: nb01 * ne01 as u64, + ne10, + ne11, + ne12: 1, + nb10, + nb11, + nb12: nb11 * ne11 as u64, + nb13: nb11 * ne11 as u64, + ne0, + ne1, + nr0: pn0, + r2: 1, + r3: 1, + }; + + let (_command_buffer, encoder, encoder_handles) = self.begin_command_encoder()?; + unsafe { + let _: () = msg_send![encoder, setComputePipelineState: pipeline]; + let _: () = msg_send![ + encoder, + setBytes: &args as *const KArgsMulMv as *const c_void + length: std::mem::size_of::() as u64 + atIndex: 0u64 + ]; + let _: () = msg_send![encoder, setBuffer: src0_id offset: 0u64 atIndex: 1u64]; + let _: () = msg_send![encoder, setBuffer: src1_id offset: 0u64 atIndex: 2u64]; + let _: () = msg_send![encoder, setBuffer: dst_id offset: 0u64 atIndex: 3u64]; + + if smem > 0 { + let _: () = msg_send![ + encoder, + setThreadgroupMemoryLength: smem as u64 + atIndex: 0u64 + ]; + } + + let tg_x = if matches!(src0, Src0Type::F32 | Src0Type::F16 | Src0Type::Q8_0) { + (ne01 + pn0 - 1) / pn0 + } else { + (ne01 + pn0 * pnsg - 1) / (pn0 * pnsg) + }; + let tg_y = (ne11 + pn1 - 1) / pn1; + + let tgs = MTLSize { + width: tg_x as u64, + height: tg_y as u64, + depth: 1, + }; + let tpg = MTLSize { + width: 32, + height: pnsg as u64, + depth: 1, + }; + let _: () = msg_send![ + encoder, + dispatchThreadgroups: tgs + threadsPerThreadgroup: tpg + ]; + } + + self.end_command_encoder(encoder_handles) + } + + fn dispatch_unary_f32( + &mut self, + op_num: i16, + src0_id: ObjcId, + dst_id: ObjcId, + shape: &Shape4, + ) -> Result<(), String> { + let is_c4 = shape.ne[0] % 4 == 0; + let is_cnt = shape.numel < 32768; + + let base = if is_c4 { + "kernel_unary_f32_f32_4" + } else { + "kernel_unary_f32_f32" + }; + let name = format!("{}_op={}_cnt={}", base, op_num, is_cnt as i32); + + let constants = [ + FunctionConstant { + idx: FC_UNARY + 0, + value: FunctionConstantValue::Int16(op_num), + }, + FunctionConstant { + idx: FC_UNARY + 1, + value: FunctionConstantValue::Bool(is_cnt), + }, + ]; + + let (pipeline, _smem, _nr0, _nr1, _nsg) = + self.get_or_compile_cached_pipeline(name, base, &constants, 0, 0, 0, 0)?; + + let mut args = KArgsUnary { + ne00: shape.ne[0], + ne01: shape.ne[1], + ne02: shape.ne[2], + ne03: shape.ne[3], + nb00: shape.nb[0], + nb01: shape.nb[1], + nb02: shape.nb[2], + nb03: shape.nb[3], + ne0: shape.ne[0], + ne1: shape.ne[1], + ne2: shape.ne[2], + ne3: shape.ne[3], + nb0: shape.nb[0], + nb1: shape.nb[1], + nb2: shape.nb[2], + nb3: shape.nb[3], + slope: 0.0, + scale: 0.0, + bias: 0.0, + val: 0.0, + min: 0.0, + max: 0.0, + }; + + if is_c4 { + args.ne00 /= 4; + args.ne0 /= 4; + } + + let (_command_buffer, encoder, encoder_handles) = self.begin_command_encoder()?; + unsafe { + let _: () = msg_send![encoder, setComputePipelineState: pipeline]; + let _: () = msg_send![ + encoder, + setBytes: &args as *const KArgsUnary as *const c_void + length: std::mem::size_of::() as u64 + atIndex: 0u64 + ]; + let _: () = msg_send![encoder, setBuffer: src0_id offset: 0u64 atIndex: 1u64]; + let _: () = msg_send![encoder, setBuffer: dst_id offset: 0u64 atIndex: 2u64]; + + if is_cnt { + let n = if is_c4 { shape.numel / 4 } else { shape.numel }; + let tgs = MTLSize { + width: n as u64, + height: 1, + depth: 1, + }; + let tpg = MTLSize { + width: 1, + height: 1, + depth: 1, + }; + let _: () = msg_send![ + encoder, + dispatchThreadgroups: tgs + threadsPerThreadgroup: tpg + ]; + } else { + let nth_max = + std::cmp::min(256u64, Self::pipeline_max_threads(pipeline)).max(1u64); + let nth = std::cmp::min(args.ne00 as u64, nth_max).max(1u64); + let nk0 = ((args.ne00 as u64) + nth - 1) / nth; + + let tgs = MTLSize { + width: nk0.saturating_mul(shape.ne[1] as u64), + height: shape.ne[2] as u64, + depth: shape.ne[3] as u64, + }; + let tpg = MTLSize { + width: nth, + height: 1, + depth: 1, + }; + let _: () = msg_send![ + encoder, + dispatchThreadgroups: tgs + threadsPerThreadgroup: tpg + ]; + } + } + + self.end_command_encoder(encoder_handles) + } + + fn dispatch_cpy_f32_to_f16( + &mut self, + src_id: ObjcId, + dst_id: ObjcId, + ne00: usize, + ne01: usize, + nb00: u64, + nb01: u64, + nb0: u64, + nb1: u64, + ) -> Result<(), String> { + let base = "kernel_cpy_f32_f16"; + let (pipeline, _smem, _nr0, _nr1, _nsg) = + self.get_or_compile_cached_pipeline(base.to_string(), base, &[], 0, 0, 0, 0)?; + + let ne00_i64 = i64::try_from(ne00).map_err(|_| format!("ne00 too large: {}", ne00))?; + let ne01_i64 = i64::try_from(ne01).map_err(|_| format!("ne01 too large: {}", ne01))?; + let nb02 = nb01 + .checked_mul(ne01 as u64) + .ok_or_else(|| "overflow computing cpy nb02".to_string())?; + let nb2 = nb1 + .checked_mul(ne01 as u64) + .ok_or_else(|| "overflow computing cpy nb2".to_string())?; + let args = KArgsCpy { + nk0: ne00_i64, + ne00: ne00_i64, + ne01: ne01_i64, + ne02: 1, + ne03: 1, + nb00, + nb01, + nb02, + nb03: nb02, + ne0: ne00_i64, + ne1: ne01_i64, + ne2: 1, + ne3: 1, + nb0, + nb1, + nb2, + nb3: nb2, + }; + + let max_threads = Self::pipeline_max_threads(pipeline).max(1); + let nth = std::cmp::min(ne00.max(1) as u64, max_threads).max(1); + let nw0 = ((ne00.max(1) as u64) + nth - 1) / nth; + + let (_command_buffer, encoder, encoder_handles) = self.begin_command_encoder()?; + unsafe { + let _: () = msg_send![encoder, setComputePipelineState: pipeline]; + let _: () = msg_send![ + encoder, + setBytes: &args as *const KArgsCpy as *const c_void + length: std::mem::size_of::() as u64 + atIndex: 0u64 + ]; + let _: () = msg_send![encoder, setBuffer: src_id offset: 0u64 atIndex: 1u64]; + let _: () = msg_send![encoder, setBuffer: dst_id offset: 0u64 atIndex: 2u64]; + + let tgs = MTLSize { + width: nw0 + .checked_mul(ne01 as u64) + .ok_or_else(|| "overflow computing cpy threadgroups width".to_string())?, + height: 1, + depth: 1, + }; + let tpg = MTLSize { + width: nth, + height: 1, + depth: 1, + }; + let _: () = msg_send![ + encoder, + dispatchThreadgroups: tgs + threadsPerThreadgroup: tpg + ]; + } + + self.end_command_encoder(encoder_handles) + } + + fn dispatch_bin_f32( + &mut self, + op_num: i16, + src0_id: ObjcId, + src1_id: ObjcId, + dst_id: ObjcId, + src0_shape: &Shape4, + src1_shape: &Shape4, + ) -> Result<(), String> { + for d in 0..4 { + let b = src1_shape.ne[d]; + let a = src0_shape.ne[d]; + if b != 1 && b != a { + return Err(format!( + "binary broadcast mismatch at dim {}: lhs={}, rhs={}", + d, a, b + )); + } + } + + let is_c4 = src0_shape.ne[0] % 4 == 0 && src1_shape.ne[0] % 4 == 0; + let is_rb = nrows(src1_shape) == 1 && src0_shape.numel < 65536; + + let base = if is_c4 { + "kernel_bin_fuse_f32_f32_f32_4" + } else { + "kernel_bin_fuse_f32_f32_f32" + }; + let name = format!("{}_op={}_nf=1_rb={}", base, op_num, is_rb as i32); + + let constants = [ + FunctionConstant { + idx: FC_BIN + 0, + value: FunctionConstantValue::Int16(op_num), + }, + FunctionConstant { + idx: FC_BIN + 1, + value: FunctionConstantValue::Int16(1), + }, + FunctionConstant { + idx: FC_BIN + 2, + value: FunctionConstantValue::Bool(is_rb), + }, + ]; + + let (pipeline, _smem, _nr0, _nr1, _nsg) = + self.get_or_compile_cached_pipeline(name, base, &constants, 0, 0, 0, 0)?; + + let mut args = KArgsBin { + ne00: src0_shape.ne[0], + ne01: src0_shape.ne[1], + ne02: src0_shape.ne[2], + ne03: src0_shape.ne[3], + nb00: src0_shape.nb[0], + nb01: src0_shape.nb[1], + nb02: src0_shape.nb[2], + nb03: src0_shape.nb[3], + ne10: src1_shape.ne[0], + ne11: src1_shape.ne[1], + ne12: src1_shape.ne[2], + ne13: src1_shape.ne[3], + nb10: src1_shape.nb[0], + nb11: src1_shape.nb[1], + nb12: src1_shape.nb[2], + nb13: src1_shape.nb[3], + ne0: src0_shape.ne[0], + ne1: src0_shape.ne[1], + ne2: src0_shape.ne[2], + ne3: src0_shape.ne[3], + nb0: src0_shape.nb[0], + nb1: src0_shape.nb[1], + nb2: src0_shape.nb[2], + nb3: src0_shape.nb[3], + offs: 0, + o1: [0u64; 8], + }; + + if is_c4 { + args.ne00 /= 4; + args.ne10 /= 4; + args.ne0 /= 4; + } + + let (_command_buffer, encoder, encoder_handles) = self.begin_command_encoder()?; + unsafe { + let _: () = msg_send![encoder, setComputePipelineState: pipeline]; + let _: () = msg_send![ + encoder, + setBytes: &args as *const KArgsBin as *const c_void + length: std::mem::size_of::() as u64 + atIndex: 0u64 + ]; + let _: () = msg_send![encoder, setBuffer: src0_id offset: 0u64 atIndex: 1u64]; + let _: () = msg_send![encoder, setBuffer: src1_id offset: 0u64 atIndex: 2u64]; + let _: () = msg_send![encoder, setBuffer: dst_id offset: 0u64 atIndex: 3u64]; + + if is_rb { + let n = if is_c4 { + src0_shape.numel / 4 + } else { + src0_shape.numel + }; + let tgs = MTLSize { + width: n as u64, + height: 1, + depth: 1, + }; + let tpg = MTLSize { + width: 1, + height: 1, + depth: 1, + }; + let _: () = msg_send![ + encoder, + dispatchThreadgroups: tgs + threadsPerThreadgroup: tpg + ]; + } else { + let nth_max = + std::cmp::min(256u64, Self::pipeline_max_threads(pipeline)).max(1u64); + let mut nth = 1u64; + while 2 * nth < args.ne0 as u64 && nth < nth_max { + nth *= 2; + } + let tgs = MTLSize { + width: src0_shape.ne[1] as u64, + height: src0_shape.ne[2] as u64, + depth: src0_shape.ne[3] as u64, + }; + let tpg = MTLSize { + width: nth, + height: 1, + depth: 1, + }; + let _: () = msg_send![ + encoder, + dispatchThreadgroups: tgs + threadsPerThreadgroup: tpg + ]; + } + } + + self.end_command_encoder(encoder_handles) + } + + #[allow(clippy::too_many_arguments)] + fn dispatch_norm_f32( + &mut self, + src0_id: ObjcId, + src1_0_id: ObjcId, + src1_1_id: ObjcId, + dst_id: ObjcId, + src0_shape: &Shape4, + src1_0_shape: &Shape4, + src1_1_shape: &Shape4, + eps: f32, + n_fuse: i32, + ) -> Result<(), String> { + if src0_shape.ne[0] <= 0 { + return Err("norm ne0 must be positive".to_string()); + } + + let is_c4 = src0_shape.ne[0] % 4 == 0; + let suffix = if is_c4 { "_4" } else { "" }; + let base = match n_fuse { + 1 => format!("kernel_norm_f32{}", suffix), + 2 => format!("kernel_norm_mul_f32{}", suffix), + 3 => format!("kernel_norm_mul_add_f32{}", suffix), + _ => return Err(format!("unsupported norm fuse level: {}", n_fuse)), + }; + + let (pipeline, pipeline_smem, _nr0, _nr1, _nsg) = + self.get_or_compile_cached_pipeline(base.clone(), &base, &[], 32 * 4, 0, 0, 0)?; + + let ne00_t = if is_c4 { + src0_shape.ne[0] / 4 + } else { + src0_shape.ne[0] + }; + let args = KArgsNorm { + ne00: src0_shape.ne[0], + ne00_t, + nb1: src0_shape.nb[1], + nb2: src0_shape.nb[2], + nb3: src0_shape.nb[3], + eps, + nef1: [src0_shape.ne[1], src1_0_shape.ne[1], src1_1_shape.ne[1]], + nef2: [src0_shape.ne[2], src1_0_shape.ne[2], src1_1_shape.ne[2]], + nef3: [src0_shape.ne[3], src1_0_shape.ne[3], src1_1_shape.ne[3]], + nbf1: [src0_shape.nb[1], src1_0_shape.nb[1], src1_1_shape.nb[1]], + nbf2: [src0_shape.nb[2], src1_0_shape.nb[2], src1_1_shape.nb[2]], + nbf3: [src0_shape.nb[3], src1_0_shape.nb[3], src1_1_shape.nb[3]], + }; + + let mut nth = 32u64; + let nth_max = Self::pipeline_max_threads(pipeline).max(1u64); + while nth < args.ne00_t as u64 && nth < nth_max { + nth *= 2; + } + nth = std::cmp::min(nth, nth_max); + nth = std::cmp::min(nth, args.ne00_t.max(1) as u64); + + let (_command_buffer, encoder, encoder_handles) = self.begin_command_encoder()?; + unsafe { + let _: () = msg_send![encoder, setComputePipelineState: pipeline]; + let _: () = msg_send![ + encoder, + setBytes: &args as *const KArgsNorm as *const c_void + length: std::mem::size_of::() as u64 + atIndex: 0u64 + ]; + let _: () = msg_send![encoder, setBuffer: src0_id offset: 0u64 atIndex: 1u64]; + let _: () = msg_send![encoder, setBuffer: src1_0_id offset: 0u64 atIndex: 2u64]; + let _: () = msg_send![encoder, setBuffer: src1_1_id offset: 0u64 atIndex: 3u64]; + let _: () = msg_send![encoder, setBuffer: dst_id offset: 0u64 atIndex: 4u64]; + let _: () = msg_send![ + encoder, + setThreadgroupMemoryLength: pipeline_smem as u64 + atIndex: 0u64 + ]; + + let tgs = MTLSize { + width: src0_shape.ne[1] as u64, + height: src0_shape.ne[2] as u64, + depth: src0_shape.ne[3] as u64, + }; + let tpg = MTLSize { + width: nth, + height: 1, + depth: 1, + }; + let _: () = msg_send![ + encoder, + dispatchThreadgroups: tgs + threadsPerThreadgroup: tpg + ]; + } + + self.end_command_encoder(encoder_handles) + } + + fn dispatch_im2col_1d_f32( + &mut self, + src_id: ObjcId, + dst_id: ObjcId, + ic: usize, + iw: usize, + kw: usize, + stride: usize, + pad: usize, + ow: usize, + ) -> Result<(), String> { + let base = "kernel_im2col_f32"; + let (pipeline, _smem, _nr0, _nr1, _nsg) = + self.get_or_compile_cached_pipeline(base.to_string(), base, &[], 0, 0, 0, 0)?; + + let ic_i32 = i32::try_from(ic).map_err(|_| format!("ic too large: {}", ic))?; + let iw_i32 = i32::try_from(iw).map_err(|_| format!("iw too large: {}", iw))?; + let kw_i32 = i32::try_from(kw).map_err(|_| format!("kw too large: {}", kw))?; + let ow_i32 = i32::try_from(ow).map_err(|_| format!("ow too large: {}", ow))?; + let stride_i32 = + i32::try_from(stride).map_err(|_| format!("stride too large: {}", stride))?; + let pad_i32 = i32::try_from(pad).map_err(|_| format!("pad too large: {}", pad))?; + + let chw = ic + .checked_mul(kw) + .ok_or_else(|| "overflow computing im2col CHW".to_string())?; + let ofs0 = ic + .checked_mul(iw) + .ok_or_else(|| "overflow computing im2col ofs0".to_string())?; + + let args = KArgsIm2Col { + ofs0: ofs0 as u64, + ofs1: iw as u64, + iw: iw_i32, + ih: 1, + chw: i32::try_from(chw).map_err(|_| format!("CHW too large: {}", chw))?, + s0: stride_i32, + s1: 1, + p0: pad_i32, + p1: 0, + d0: 1, + d1: 1, + n: 1, + kh: 1, + kw: kw_i32, + khw: kw_i32, + }; + + let max_threads = Self::pipeline_max_threads(pipeline); + let khkw = kw_i32 as u64; + if khkw == 0 || khkw > max_threads { + return Err(format!( + "invalid im2col thread shape: kh*kw={} max={}", + khkw, max_threads + )); + } + let ntptg0 = (max_threads / khkw).min(1).max(1); + + let (_command_buffer, encoder, encoder_handles) = self.begin_command_encoder()?; + unsafe { + let _: () = msg_send![encoder, setComputePipelineState: pipeline]; + let _: () = msg_send![ + encoder, + setBytes: &args as *const KArgsIm2Col as *const c_void + length: std::mem::size_of::() as u64 + atIndex: 0u64 + ]; + let _: () = msg_send![encoder, setBuffer: src_id offset: 0u64 atIndex: 1u64]; + let _: () = msg_send![encoder, setBuffer: dst_id offset: 0u64 atIndex: 2u64]; + + let tgs = MTLSize { + width: ic_i32 as u64, + height: 1, + depth: ow_i32 as u64, + }; + let tpg = MTLSize { + width: ntptg0, + height: 1, + depth: kw_i32 as u64, + }; + let _: () = msg_send![ + encoder, + dispatchThreadgroups: tgs + threadsPerThreadgroup: tpg + ]; + } + + self.end_command_encoder(encoder_handles) + } + + #[allow(clippy::too_many_arguments)] + fn dispatch_flash_attn_ext_pad( + &mut self, + k_id: ObjcId, + v_id: ObjcId, + mask_id: ObjcId, + pad_id: ObjcId, + has_mask: bool, + ncpsg: i32, + ne11: i32, + ne_12_2: i32, + ne_12_3: i32, + nb11: u64, + nb12: u64, + nb13: u64, + nb21: u64, + nb22: u64, + nb23: u64, + ne31: i32, + ne32: i32, + ne33: i32, + nb31: u64, + nb32: u64, + nb33: u64, + ) -> Result<(), String> { + let base = "kernel_flash_attn_ext_pad"; + let name = format!("{}_mask={}_ncpsg={}", base, has_mask as i32, ncpsg); + let constants = [ + FunctionConstant { + idx: FC_FLASH_ATTN_EXT_PAD + 0, + value: FunctionConstantValue::Bool(has_mask), + }, + FunctionConstant { + idx: FC_FLASH_ATTN_EXT_PAD + 25, + value: FunctionConstantValue::Int32(ncpsg), + }, + ]; + let (pipeline, _smem, _nr0, _nr1, _nsg) = + self.get_or_compile_cached_pipeline(name, base, &constants, 0, 0, 0, 0)?; + + let args = KArgsFlashAttnExtPad { + ne11, + ne_12_2, + ne_12_3, + nb11, + nb12, + nb13, + nb21, + nb22, + nb23, + ne31, + ne32, + ne33, + nb31, + nb32, + nb33, + }; + + let (_command_buffer, encoder, encoder_handles) = self.begin_command_encoder()?; + unsafe { + let _: () = msg_send![encoder, setComputePipelineState: pipeline]; + let _: () = msg_send![ + encoder, + setBytes: &args as *const KArgsFlashAttnExtPad as *const c_void + length: std::mem::size_of::() as u64 + atIndex: 0u64 + ]; + let _: () = msg_send![encoder, setBuffer: k_id offset: 0u64 atIndex: 1u64]; + let _: () = msg_send![encoder, setBuffer: v_id offset: 0u64 atIndex: 2u64]; + let _: () = msg_send![encoder, setBuffer: mask_id offset: 0u64 atIndex: 3u64]; + let _: () = msg_send![encoder, setBuffer: pad_id offset: 0u64 atIndex: 4u64]; + + let tgs = MTLSize { + width: ncpsg as u64, + height: ne_12_2.max(ne32) as u64, + depth: ne_12_3.max(ne33) as u64, + }; + let tpg = MTLSize { + width: 32, + height: 1, + depth: 1, + }; + let _: () = msg_send![ + encoder, + dispatchThreadgroups: tgs + threadsPerThreadgroup: tpg + ]; + } + + self.end_command_encoder(encoder_handles) + } + + #[allow(clippy::too_many_arguments)] + fn dispatch_flash_attn_ext_blk( + &mut self, + mask_id: ObjcId, + blk_id: ObjcId, + n_q: usize, + n_kv: usize, + ne31: i32, + ne32: i32, + ne33: i32, + nb31: u64, + nb32: u64, + nb33: u64, + nqptg: i32, + ncpsg: i32, + ) -> Result<(), String> { + let base = "kernel_flash_attn_ext_blk"; + let name = format!("{}_nqptg={}_ncpsg={}", base, nqptg, ncpsg); + let constants = [ + FunctionConstant { + idx: FC_FLASH_ATTN_EXT_BLK + 24, + value: FunctionConstantValue::Int32(nqptg), + }, + FunctionConstant { + idx: FC_FLASH_ATTN_EXT_BLK + 25, + value: FunctionConstantValue::Int32(ncpsg), + }, + ]; + let (pipeline, _smem, _nr0, _nr1, _nsg) = + self.get_or_compile_cached_pipeline(name, base, &constants, 0, 0, 0, 0)?; + + let ne01 = i32::try_from(n_q).map_err(|_| format!("n_q too large: {}", n_q))?; + let ne30 = i32::try_from(n_kv).map_err(|_| format!("n_kv too large: {}", n_kv))?; + let args = KArgsFlashAttnExtBlk { + ne01, + ne30, + ne31, + ne32, + ne33, + nb31, + nb32, + nb33, + }; + + let (_command_buffer, encoder, encoder_handles) = self.begin_command_encoder()?; + unsafe { + let _: () = msg_send![encoder, setComputePipelineState: pipeline]; + let _: () = msg_send![ + encoder, + setBytes: &args as *const KArgsFlashAttnExtBlk as *const c_void + length: std::mem::size_of::() as u64 + atIndex: 0u64 + ]; + let _: () = msg_send![encoder, setBuffer: mask_id offset: 0u64 atIndex: 1u64]; + let _: () = msg_send![encoder, setBuffer: blk_id offset: 0u64 atIndex: 2u64]; + + let nblk1 = ((ne01 + nqptg - 1) / nqptg) as u64; + let nblk0 = ((ne30 + ncpsg - 1) / ncpsg) as u64; + let tgs = MTLSize { + width: nblk0, + height: nblk1, + depth: (ne32 * ne33) as u64, + }; + let tpg = MTLSize { + width: 32, + height: 1, + depth: 1, + }; + let _: () = msg_send![ + encoder, + dispatchThreadgroups: tgs + threadsPerThreadgroup: tpg + ]; + } + + self.end_command_encoder(encoder_handles) + } + + fn dispatch_flash_attn_ext_f32( + &mut self, + q_id: ObjcId, + k_id: ObjcId, + v_id: ObjcId, + mask_id: ObjcId, + sinks_id: ObjcId, + pad_id: ObjcId, + blk_id: ObjcId, + dst_id: ObjcId, + n_q: usize, + n_kv: usize, + n_head: usize, + d: usize, + kv_type: Src0Type, + scale: f32, + has_mask: bool, + has_sinks: bool, + max_bias: f32, + logit_softcap: f32, + ) -> Result<(), String> { + if !flash_attn_supported_head_dim(d) { + return Err(format!( + "unsupported flash-attn head dim for kv kernel: {}", + d + )); + } + if d % 4 != 0 { + return Err(format!( + "flash-attn requires head dim divisible by 4 (float4 store), got {}", + d + )); + } + + let nsg = if d >= 512 { 8 } else { 4 }; + let nqptg = OP_FLASH_ATTN_EXT_NQPSG; + let ncpsg = OP_FLASH_ATTN_EXT_NCPSG; + let has_kvpad = n_kv % (ncpsg as usize) != 0; + let has_bias = max_bias != 0.0; + let has_scap = logit_softcap != 0.0; + + let ne01 = i32::try_from(n_q).map_err(|_| format!("n_q too large: {}", n_q))?; + let ne11 = i32::try_from(n_kv).map_err(|_| format!("n_kv too large: {}", n_kv))?; + let ne02 = + i32::try_from(n_head).map_err(|_| format!("n_head too large: {}", n_head))?; + let n_state = n_head + .checked_mul(d) + .ok_or_else(|| "overflow computing flash n_state".to_string())?; + let kv_elem_bytes = flash_attn_kv_elem_bytes(kv_type)? as u64; + + let nb01 = (n_state as u64) + .checked_mul(4) + .ok_or_else(|| "overflow computing flash nb01".to_string())?; + let nb02 = (d as u64) + .checked_mul(4) + .ok_or_else(|| "overflow computing flash nb02".to_string())?; + let nb03 = nb01 + .checked_mul(n_q as u64) + .ok_or_else(|| "overflow computing flash nb03".to_string())?; + + let nb11 = (n_state as u64) + .checked_mul(kv_elem_bytes) + .ok_or_else(|| "overflow computing flash nb11".to_string())?; + let nb12 = (d as u64) + .checked_mul(kv_elem_bytes) + .ok_or_else(|| "overflow computing flash nb12".to_string())?; + let nb13 = nb11 + .checked_mul(n_kv as u64) + .ok_or_else(|| "overflow computing flash nb13".to_string())?; + + let nb21 = nb11; + let nb22 = nb12; + let nb23 = nb13; + + let ne31 = ne01; + let ne32 = 1i32; + let ne33 = 1i32; + let nb31 = (n_kv as u64) + .checked_mul(2) + .ok_or_else(|| "overflow computing flash nb31".to_string())?; + let nb32 = nb31 + .checked_mul(n_q as u64) + .ok_or_else(|| "overflow computing flash nb32".to_string())?; + let nb33 = nb32; + + let n_head_log2 = if n_head <= 1 { + 1i32 + } else { + let p = (usize::BITS - 1) - (n_head as u32).leading_zeros(); + (1u32 << p) as i32 + }; + let m0 = (2.0f32).powf(-(max_bias) / (n_head_log2 as f32)); + let m1 = (2.0f32).powf(-(max_bias / 2.0) / (n_head_log2 as f32)); + let scale_k = if has_scap { + scale / logit_softcap + } else { + scale + }; + let bc_mask = has_mask && (ne31 % 8 != 0); + let ns10 = i32::try_from(nb11 / kv_elem_bytes) + .map_err(|_| "overflow computing flash ns10".to_string())?; + let ns20 = i32::try_from(nb21 / kv_elem_bytes) + .map_err(|_| "overflow computing flash ns20".to_string())?; + + if has_kvpad { + self.dispatch_flash_attn_ext_pad( + k_id, v_id, mask_id, pad_id, has_mask, ncpsg, ne11, ne02, 1, nb11, nb12, nb13, + nb21, nb22, nb23, ne31, ne32, ne33, nb31, nb32, nb33, + )?; + } + if has_mask { + self.dispatch_flash_attn_ext_blk( + mask_id, blk_id, n_q, n_kv, ne31, ne32, ne33, nb31, nb32, nb33, nqptg, ncpsg, + )?; + } + + let base = format!( + "kernel_flash_attn_ext_{}_dk{}_dv{}", + src0_type_name(kv_type), + d, + d + ); + let name = format!( + "{}_mask={}_sinks={}_bias={}_scap={}_kvpad={}_bcm={}_ns10={}_ns20={}_nsg={}", + base, + has_mask as i32, + has_sinks as i32, + has_bias as i32, + has_scap as i32, + has_kvpad as i32, + bc_mask as i32, + ns10, + ns20, + nsg + ); + let constants = [ + FunctionConstant { + idx: FC_FLASH_ATTN_EXT + 0, + value: FunctionConstantValue::Bool(has_mask), + }, + FunctionConstant { + idx: FC_FLASH_ATTN_EXT + 1, + value: FunctionConstantValue::Bool(has_sinks), + }, + FunctionConstant { + idx: FC_FLASH_ATTN_EXT + 2, + value: FunctionConstantValue::Bool(has_bias), + }, + FunctionConstant { + idx: FC_FLASH_ATTN_EXT + 3, + value: FunctionConstantValue::Bool(has_scap), + }, + FunctionConstant { + idx: FC_FLASH_ATTN_EXT + 4, + value: FunctionConstantValue::Bool(has_kvpad), + }, + FunctionConstant { + idx: FC_FLASH_ATTN_EXT + 10, + value: FunctionConstantValue::Bool(bc_mask), + }, + FunctionConstant { + idx: FC_FLASH_ATTN_EXT + 20, + value: FunctionConstantValue::Int32(ns10), + }, + FunctionConstant { + idx: FC_FLASH_ATTN_EXT + 21, + value: FunctionConstantValue::Int32(ns20), + }, + FunctionConstant { + idx: FC_FLASH_ATTN_EXT + 22, + value: FunctionConstantValue::Int32(nsg), + }, + ]; + + let smem = flash_attn_smem_bytes(d, d, nsg); + let (pipeline, pipeline_smem, _nr0, _nr1, _pnsg) = + self.get_or_compile_cached_pipeline(name, &base, &constants, smem, 0, 0, nsg)?; + + let args = KArgsFlashAttnExt { + ne01, + ne02, + ne03: 1, + nb01, + nb02, + nb03, + ne11, + ne_12_2: ne02, + ne_12_3: 1, + ns10, + nb11, + nb12, + nb13, + ns20, + nb21, + nb22, + nb23, + ne31, + ne32, + ne33, + nb31, + nb32, + nb33, + ne1: ne02, + ne2: ne01, + ne3: 1, + scale: scale_k, + max_bias, + m0, + m1, + n_head_log2, + logit_softcap, + }; + + let (_command_buffer, encoder, encoder_handles) = self.begin_command_encoder()?; + unsafe { + let _: () = msg_send![encoder, setComputePipelineState: pipeline]; + let _: () = msg_send![ + encoder, + setBytes: &args as *const KArgsFlashAttnExt as *const c_void + length: std::mem::size_of::() as u64 + atIndex: 0u64 + ]; + let _: () = msg_send![encoder, setBuffer: q_id offset: 0u64 atIndex: 1u64]; + let _: () = msg_send![encoder, setBuffer: k_id offset: 0u64 atIndex: 2u64]; + let _: () = msg_send![encoder, setBuffer: v_id offset: 0u64 atIndex: 3u64]; + let _: () = msg_send![encoder, setBuffer: mask_id offset: 0u64 atIndex: 4u64]; + let _: () = msg_send![encoder, setBuffer: sinks_id offset: 0u64 atIndex: 5u64]; + let _: () = msg_send![encoder, setBuffer: pad_id offset: 0u64 atIndex: 6u64]; + let _: () = msg_send![encoder, setBuffer: blk_id offset: 0u64 atIndex: 7u64]; + let _: () = msg_send![encoder, setBuffer: dst_id offset: 0u64 atIndex: 8u64]; + let _: () = msg_send![ + encoder, + setThreadgroupMemoryLength: pipeline_smem as u64 + atIndex: 0u64 + ]; + + let tgs = MTLSize { + width: ((n_q as i32 + nqptg - 1) / nqptg) as u64, + height: n_head as u64, + depth: 1, + }; + let tpg = MTLSize { + width: 32, + height: nsg as u64, + depth: 1, + }; + let _: () = msg_send![ + encoder, + dispatchThreadgroups: tgs + threadsPerThreadgroup: tpg + ]; + } + + self.end_command_encoder(encoder_handles) + } + + fn dispatch_flash_attn_ext_vec_reduce_f32( + &mut self, + tmp_id: ObjcId, + dst_id: ObjcId, + nrows: usize, + d: usize, + nwg: i32, + ) -> Result<(), String> { + let base = "kernel_flash_attn_ext_vec_reduce"; + let name = format!("{}_dv={}_nwg={}", base, d, nwg); + let d_i32 = + i32::try_from(d).map_err(|_| format!("d too large for vec reduce: {}", d))?; + let constants = [ + FunctionConstant { + idx: FC_FLASH_ATTN_EXT_VEC_REDUCE + 0, + value: FunctionConstantValue::Int32(d_i32), + }, + FunctionConstant { + idx: FC_FLASH_ATTN_EXT_VEC_REDUCE + 1, + value: FunctionConstantValue::Int32(nwg), + }, + ]; + let (pipeline, _smem, _nr0, _nr1, _pnsg) = + self.get_or_compile_cached_pipeline(name, base, &constants, 0, 0, 0, 0)?; + + let nrows_i32 = i32::try_from(nrows) + .map_err(|_| format!("nrows too large for vec reduce: {}", nrows))?; + let args = KArgsFlashAttnExtVecReduce { nrows: nrows_i32 }; + + let tpg_width = (32i32) + .checked_mul(nwg) + .ok_or_else(|| "overflow computing vec reduce tpg width".to_string())?; + let max_threads = Self::pipeline_max_threads(pipeline); + if tpg_width as u64 > max_threads { + return Err(format!( + "vec reduce threadsPerThreadgroup={} exceeds max={}", + tpg_width, max_threads + )); + } + + let (_command_buffer, encoder, encoder_handles) = self.begin_command_encoder()?; + unsafe { + let _: () = msg_send![encoder, setComputePipelineState: pipeline]; + let _: () = msg_send![ + encoder, + setBytes: &args as *const KArgsFlashAttnExtVecReduce as *const c_void + length: std::mem::size_of::() as u64 + atIndex: 0u64 + ]; + let _: () = msg_send![encoder, setBuffer: tmp_id offset: 0u64 atIndex: 1u64]; + let _: () = msg_send![encoder, setBuffer: dst_id offset: 0u64 atIndex: 2u64]; + + let tgs = MTLSize { + width: nrows as u64, + height: 1, + depth: 1, + }; + let tpg = MTLSize { + width: tpg_width as u64, + height: 1, + depth: 1, + }; + let _: () = msg_send![ + encoder, + dispatchThreadgroups: tgs + threadsPerThreadgroup: tpg + ]; + } + + self.end_command_encoder(encoder_handles) + } + + fn dispatch_flash_attn_ext_vec_f32( + &mut self, + q_id: ObjcId, + k_id: ObjcId, + v_id: ObjcId, + mask_id: ObjcId, + sinks_id: ObjcId, + pad_id: ObjcId, + tmp_id: Option, + dst_id: ObjcId, + n_q: usize, + n_kv: usize, + n_head: usize, + d: usize, + kv_type: Src0Type, + scale: f32, + has_mask: bool, + has_sinks: bool, + max_bias: f32, + logit_softcap: f32, + ) -> Result<(), String> { + if d % 32 != 0 { + return Err(format!( + "flash-attn vec requires head dim divisible by 32, got {}", + d + )); + } + if !flash_attn_supported_head_dim(d) { + return Err(format!( + "unsupported flash-attn vec head dim for kv kernel: {}", + d + )); + } + + let nqptg = OP_FLASH_ATTN_EXT_VEC_NQPSG; + let ncpsg = OP_FLASH_ATTN_EXT_VEC_NCPSG; + let nhptg = 1i32; + let has_kvpad = n_kv % (ncpsg as usize) != 0; + let has_bias = max_bias != 0.0; + let has_scap = logit_softcap != 0.0; + + let nwg = 32i32; + let mut nsg = 1i32; + while (2i64) + .saturating_mul(nwg as i64) + .saturating_mul(nsg as i64) + .saturating_mul(ncpsg as i64) + < n_kv as i64 + && nsg < 4 + { + nsg *= 2; + } + + let ne01 = i32::try_from(n_q).map_err(|_| format!("n_q too large: {}", n_q))?; + let ne11 = i32::try_from(n_kv).map_err(|_| format!("n_kv too large: {}", n_kv))?; + let ne02 = + i32::try_from(n_head).map_err(|_| format!("n_head too large: {}", n_head))?; + let n_state = n_head + .checked_mul(d) + .ok_or_else(|| "overflow computing flash n_state".to_string())?; + let kv_elem_bytes = flash_attn_kv_elem_bytes(kv_type)? as u64; + + let nb01 = (n_state as u64) + .checked_mul(4) + .ok_or_else(|| "overflow computing flash nb01".to_string())?; + let nb02 = (d as u64) + .checked_mul(4) + .ok_or_else(|| "overflow computing flash nb02".to_string())?; + let nb03 = nb01 + .checked_mul(n_q as u64) + .ok_or_else(|| "overflow computing flash nb03".to_string())?; + + let nb11 = (n_state as u64) + .checked_mul(kv_elem_bytes) + .ok_or_else(|| "overflow computing flash nb11".to_string())?; + let nb12 = (d as u64) + .checked_mul(kv_elem_bytes) + .ok_or_else(|| "overflow computing flash nb12".to_string())?; + let nb13 = nb11 + .checked_mul(n_kv as u64) + .ok_or_else(|| "overflow computing flash nb13".to_string())?; + + let nb21 = nb11; + let nb22 = nb12; + let nb23 = nb13; + + let ne31 = ne01; + let ne32 = 1i32; + let ne33 = 1i32; + let nb31 = (n_kv as u64) + .checked_mul(2) + .ok_or_else(|| "overflow computing flash nb31".to_string())?; + let nb32 = nb31 + .checked_mul(n_q as u64) + .ok_or_else(|| "overflow computing flash nb32".to_string())?; + let nb33 = nb32; + + let n_head_log2 = if n_head <= 1 { + 1i32 + } else { + let p = (usize::BITS - 1) - (n_head as u32).leading_zeros(); + (1u32 << p) as i32 + }; + let m0 = (2.0f32).powf(-(max_bias) / (n_head_log2 as f32)); + let m1 = (2.0f32).powf(-(max_bias / 2.0) / (n_head_log2 as f32)); + let scale_k = if has_scap { + scale / logit_softcap + } else { + scale + }; + let ns10 = i32::try_from(nb11 / kv_elem_bytes) + .map_err(|_| "overflow computing flash vec ns10".to_string())?; + let ns20 = i32::try_from(nb21 / kv_elem_bytes) + .map_err(|_| "overflow computing flash vec ns20".to_string())?; + + if has_kvpad { + self.dispatch_flash_attn_ext_pad( + k_id, v_id, mask_id, pad_id, has_mask, ncpsg, ne11, ne02, 1, nb11, nb12, nb13, + nb21, nb22, nb23, ne31, ne32, ne33, nb31, nb32, nb33, + )?; + } + + let base = format!( + "kernel_flash_attn_ext_vec_{}_dk{}_dv{}", + src0_type_name(kv_type), + d, + d + ); + let name = format!( + "{}_mask={}_sink={}_bias={}_scap={}_kvpad={}_ns10={}_ns20={}_nsg={}_nwg={}", + base, + has_mask as i32, + has_sinks as i32, + has_bias as i32, + has_scap as i32, + has_kvpad as i32, + ns10, + ns20, + nsg, + nwg + ); + let constants = [ + FunctionConstant { + idx: FC_FLASH_ATTN_EXT_VEC + 0, + value: FunctionConstantValue::Bool(has_mask), + }, + FunctionConstant { + idx: FC_FLASH_ATTN_EXT_VEC + 1, + value: FunctionConstantValue::Bool(has_sinks), + }, + FunctionConstant { + idx: FC_FLASH_ATTN_EXT_VEC + 2, + value: FunctionConstantValue::Bool(has_bias), + }, + FunctionConstant { + idx: FC_FLASH_ATTN_EXT_VEC + 3, + value: FunctionConstantValue::Bool(has_scap), + }, + FunctionConstant { + idx: FC_FLASH_ATTN_EXT_VEC + 4, + value: FunctionConstantValue::Bool(has_kvpad), + }, + FunctionConstant { + idx: FC_FLASH_ATTN_EXT_VEC + 20, + value: FunctionConstantValue::Int32(ns10), + }, + FunctionConstant { + idx: FC_FLASH_ATTN_EXT_VEC + 21, + value: FunctionConstantValue::Int32(ns20), + }, + FunctionConstant { + idx: FC_FLASH_ATTN_EXT_VEC + 22, + value: FunctionConstantValue::Int32(nsg), + }, + FunctionConstant { + idx: FC_FLASH_ATTN_EXT_VEC + 23, + value: FunctionConstantValue::Int32(nwg), + }, + ]; + + let smem = flash_attn_vec_smem_bytes(d, d, nsg); + let (pipeline, pipeline_smem, _nr0, _nr1, _pnsg) = + self.get_or_compile_cached_pipeline(name, &base, &constants, smem, 0, 0, nsg)?; + let max_threads = Self::pipeline_max_threads(pipeline); + let thread_width = (32i32) + .checked_mul(nsg) + .ok_or_else(|| "overflow computing vec thread width".to_string())?; + if thread_width as u64 > max_threads { + return Err(format!( + "flash-attn vec threadsPerThreadgroup={} exceeds max={}", + thread_width, max_threads + )); + } + + let args = KArgsFlashAttnExtVec { + ne01, + ne02, + ne03: 1, + nb01, + nb02, + nb03, + ne11, + ne_12_2: ne02, + ne_12_3: 1, + ns10, + nb11, + nb12, + nb13, + ns20, + nb21, + nb22, + nb23, + ne31, + ne32, + ne33, + nb31, + nb32, + nb33, + ne1: ne02, + ne2: ne01, + ne3: 1, + scale: scale_k, + max_bias, + m0, + m1, + n_head_log2, + logit_softcap, + }; + + let nrows = n_q + .checked_mul(n_head) + .ok_or_else(|| "overflow computing flash vec nrows".to_string())?; + + let out_id = if nwg == 1 { + dst_id + } else { + tmp_id + .ok_or_else(|| "flash-attn vec requires tmp buffer when nwg > 1".to_string())? + }; + + let (_command_buffer, encoder, encoder_handles) = self.begin_command_encoder()?; + unsafe { + let _: () = msg_send![encoder, setComputePipelineState: pipeline]; + let _: () = msg_send![ + encoder, + setBytes: &args as *const KArgsFlashAttnExtVec as *const c_void + length: std::mem::size_of::() as u64 + atIndex: 0u64 + ]; + let _: () = msg_send![encoder, setBuffer: q_id offset: 0u64 atIndex: 1u64]; + let _: () = msg_send![encoder, setBuffer: k_id offset: 0u64 atIndex: 2u64]; + let _: () = msg_send![encoder, setBuffer: v_id offset: 0u64 atIndex: 3u64]; + let _: () = msg_send![encoder, setBuffer: mask_id offset: 0u64 atIndex: 4u64]; + let _: () = msg_send![encoder, setBuffer: sinks_id offset: 0u64 atIndex: 5u64]; + let _: () = msg_send![encoder, setBuffer: pad_id offset: 0u64 atIndex: 6u64]; + let _: () = msg_send![encoder, setBuffer: out_id offset: 0u64 atIndex: 7u64]; + + let _: () = msg_send![ + encoder, + setThreadgroupMemoryLength: pipeline_smem as u64 + atIndex: 0u64 + ]; + + let tgs = MTLSize { + width: ((n_q as i32 + nqptg - 1) / nqptg) as u64, + height: ((n_head as i32 + nhptg - 1) / nhptg) as u64, + depth: nwg as u64, + }; + let tpg = MTLSize { + width: 32, + height: nsg as u64, + depth: 1, + }; + let _: () = msg_send![ + encoder, + dispatchThreadgroups: tgs + threadsPerThreadgroup: tpg + ]; + } + + self.end_command_encoder(encoder_handles)?; + + if nwg > 1 { + let tmp_id = tmp_id + .ok_or_else(|| "flash-attn vec requires tmp buffer when nwg > 1".to_string())?; + self.dispatch_flash_attn_ext_vec_reduce_f32(tmp_id, dst_id, nrows, d, nwg)?; + } + + Ok(()) + } + + fn flash_attn_f32_packed( + &mut self, + q: &[f32], + k: &[f32], + v: &[f32], + n_q: usize, + n_kv: usize, + n_head: usize, + d: usize, + scale: f32, + ) -> Result, String> { + if n_q == 0 || n_kv == 0 || n_head == 0 || d == 0 { + return Ok(Vec::new()); + } + + let q_need = n_q + .checked_mul(n_head) + .and_then(|v| v.checked_mul(d)) + .ok_or_else(|| "overflow computing flash q size".to_string())?; + if q.len() != q_need { + return Err(format!( + "flash q len mismatch: got {}, expected {}", + q.len(), + q_need + )); + } + + let kv_need = n_kv + .checked_mul(n_head) + .and_then(|v| v.checked_mul(d)) + .ok_or_else(|| "overflow computing flash kv size".to_string())?; + if k.len() != kv_need { + return Err(format!( + "flash k len mismatch: got {}, expected {}", + k.len(), + kv_need + )); + } + if v.len() != kv_need { + return Err(format!( + "flash v len mismatch: got {}, expected {}", + v.len(), + kv_need + )); + } + + let out_elems = n_q + .checked_mul(n_head) + .and_then(|v| v.checked_mul(d)) + .ok_or_else(|| "overflow computing flash output size".to_string())?; + let out_bytes = out_elems + .checked_mul(std::mem::size_of::()) + .ok_or_else(|| "overflow computing flash output bytes".to_string())?; + + let q_bytes = unsafe { + std::slice::from_raw_parts( + q.as_ptr() as *const u8, + q.len() * std::mem::size_of::(), + ) + }; + let k_bytes = unsafe { + std::slice::from_raw_parts( + k.as_ptr() as *const u8, + k.len() * std::mem::size_of::(), + ) + }; + let v_bytes = unsafe { + std::slice::from_raw_parts( + v.as_ptr() as *const u8, + v.len() * std::mem::size_of::(), + ) + }; + + let q_buf = self.new_buffer_with_bytes(q_bytes)?; + let k_buf = self.new_buffer_with_bytes(k_bytes)?; + let v_buf = self.new_buffer_with_bytes(v_bytes)?; + let dst_id = self.get_or_create_scratch_buffer(SCRATCH_FLASH_OUT, out_bytes)?; + + let params = FlashAttnExtParams::default(); + let kv_type = Src0Type::F32; + let kv_elem_bytes = flash_attn_kv_elem_bytes(kv_type)?; + let use_vec = flash_attn_use_vec(n_q, d); + let pad_bytes = flash_attn_ext_extra_pad_bytes( + n_q, + n_kv, + n_head, + d, + kv_elem_bytes, + params.has_mask, + use_vec, + )?; + let pad_id = self.get_or_create_scratch_buffer(SCRATCH_FLASH_PAD, pad_bytes)?; + + if use_vec { + let tmp_bytes = flash_attn_ext_extra_tmp_bytes(n_q, n_head, d, 32)?; + let tmp_id = self.get_or_create_scratch_buffer(SCRATCH_FLASH_TMP, tmp_bytes)?; + + self.dispatch_flash_attn_ext_vec_f32( + q_buf.as_id(), + k_buf.as_id(), + v_buf.as_id(), + q_buf.as_id(), // unused when has_mask=false + q_buf.as_id(), // unused when has_sinks=false + pad_id, + Some(tmp_id), + dst_id, + n_q, + n_kv, + n_head, + d, + kv_type, + scale, + params.has_mask, + params.has_sinks, + params.max_bias, + params.logit_softcap, + )?; + } else { + let blk_bytes = flash_attn_ext_extra_blk_bytes(n_q, n_kv, params.has_mask, false)?; + let blk_id = self.get_or_create_scratch_buffer(SCRATCH_FLASH_BLK, blk_bytes)?; + + self.dispatch_flash_attn_ext_f32( + q_buf.as_id(), + k_buf.as_id(), + v_buf.as_id(), + q_buf.as_id(), // unused when has_mask=false + q_buf.as_id(), // unused when has_sinks=false + pad_id, + blk_id, + dst_id, + n_q, + n_kv, + n_head, + d, + kv_type, + scale, + params.has_mask, + params.has_sinks, + params.max_bias, + params.logit_softcap, + )?; + } + + self.read_f32_buffer(dst_id, out_elems) + } + + #[allow(clippy::too_many_arguments)] + fn flash_attn_f32_self_kv_cache( + &mut self, + layer: usize, + q: &[f32], + k_all: &[f32], + v_all: &[f32], + n_kv: usize, + n_head: usize, + d: usize, + scale: f32, + ) -> Result, String> { + if n_kv == 0 || n_head == 0 || d == 0 { + return Ok(Vec::new()); + } + let n_state = n_head + .checked_mul(d) + .ok_or_else(|| "overflow computing n_state".to_string())?; + let kv_need = n_kv + .checked_mul(n_state) + .ok_or_else(|| "overflow computing decoder self kv size".to_string())?; + if q.len() != n_state || k_all.len() != kv_need || v_all.len() != kv_need { + return Err(format!( + "decoder self kv len mismatch: q={}, k_all={}, v_all={}, expected q={}, kv={}", + q.len(), + k_all.len(), + v_all.len(), + n_state, + kv_need + )); + } + + // Mirror whisper.cpp Metal path: self-KV cache length is padded to 32. + let n_kv_flash = pad_to(n_kv, 32); + let (k_id, v_id) = self.ensure_decoder_kv_layer(layer, n_state, n_kv_flash)?; + let row_bytes = n_state + .checked_mul(std::mem::size_of::()) + .ok_or_else(|| "overflow computing decoder kv row bytes".to_string())?; + let start_row = self + .decoder_kv_layers + .get(&layer) + .map(|e| e.len_rows.min(n_kv)) + .unwrap_or(0); + if start_row < n_kv { + let copy_rows = n_kv - start_row; + let copy_bytes = copy_rows + .checked_mul(row_bytes) + .ok_or_else(|| "overflow computing decoder kv copy bytes".to_string())?; + let offset = start_row + .checked_mul(row_bytes) + .ok_or_else(|| "overflow computing decoder kv copy offset".to_string())?; + let src_k = f32_slice_as_bytes(&k_all[start_row * n_state..n_kv * n_state]); + let src_v = f32_slice_as_bytes(&v_all[start_row * n_state..n_kv * n_state]); + let dst_k: *mut u8 = unsafe { msg_send![k_id, contents] }; + let dst_v: *mut u8 = unsafe { msg_send![v_id, contents] }; + if dst_k.is_null() || dst_v.is_null() { + return Err("decoder kv buffer contents returned null".to_string()); + } + unsafe { + std::ptr::copy_nonoverlapping(src_k.as_ptr(), dst_k.add(offset), copy_bytes); + std::ptr::copy_nonoverlapping(src_v.as_ptr(), dst_v.add(offset), copy_bytes); + } + if let Some(entry) = self.decoder_kv_layers.get_mut(&layer) { + entry.len_rows = n_kv; + } + } + + if n_kv < n_kv_flash { + let tail_off = n_kv + .checked_mul(row_bytes) + .ok_or_else(|| "overflow computing decoder kv tail offset".to_string())?; + let tail_len = (n_kv_flash - n_kv) + .checked_mul(row_bytes) + .ok_or_else(|| "overflow computing decoder kv tail bytes".to_string())?; + self.zero_buffer_range(k_id, tail_off, tail_len)?; + self.zero_buffer_range(v_id, tail_off, tail_len)?; + } + + let mask_id = self.prepare_decoder_self_mask_f16(n_kv, n_kv_flash)?; + let q_buf = self.new_buffer_with_bytes(f32_slice_as_bytes(q))?; + let out_buf = self.flash_attn_f32_from_buffers_with_params( + q_buf.as_id(), + k_id, + v_id, + 1, + n_kv_flash, + n_head, + d, + scale, + FlashAttnExtParams { + has_mask: true, + ..FlashAttnExtParams::default() + }, + Some(mask_id), + )?; + self.read_f32_buffer(out_buf.as_id(), n_state) + } + + #[allow(clippy::too_many_arguments)] + fn flash_attn_f32_cross_kv_cache( + &mut self, + layer: usize, + q: &[f32], + k_cross: &[f32], + v_cross: &[f32], + n_q: usize, + n_kv: usize, + n_head: usize, + d: usize, + scale: f32, + ) -> Result, String> { + if n_q == 0 || n_kv == 0 || n_head == 0 || d == 0 { + return Ok(Vec::new()); + } + let n_state = n_head + .checked_mul(d) + .ok_or_else(|| "overflow computing n_state".to_string())?; + let q_need = n_q + .checked_mul(n_state) + .ok_or_else(|| "overflow computing cross q size".to_string())?; + if q.len() != q_need { + return Err(format!( + "cross q len mismatch: got {}, expected {}", + q.len(), + q_need + )); + } + + let (k_id, v_id) = + self.ensure_cross_kv_layer(layer, n_state, n_kv, k_cross, v_cross)?; + let q_buf = self.new_buffer_with_bytes(f32_slice_as_bytes(q))?; + let out_buf = self.flash_attn_f32_from_buffers( + q_buf.as_id(), + k_id, + v_id, + n_q, + n_kv, + n_head, + d, + scale, + )?; + self.read_f32_buffer(out_buf.as_id(), q_need) + } + + fn flash_attn_f32_from_buffers( + &mut self, + q_id: ObjcId, + k_id: ObjcId, + v_id: ObjcId, + n_q: usize, + n_kv: usize, + n_head: usize, + d: usize, + scale: f32, + ) -> Result { + self.flash_attn_f32_from_buffers_with_params( + q_id, + k_id, + v_id, + n_q, + n_kv, + n_head, + d, + scale, + FlashAttnExtParams::default(), + None, + ) + } + + #[allow(clippy::too_many_arguments)] + fn flash_attn_f32_from_buffers_with_params( + &mut self, + q_id: ObjcId, + k_id: ObjcId, + v_id: ObjcId, + n_q: usize, + n_kv: usize, + n_head: usize, + d: usize, + scale: f32, + params: FlashAttnExtParams, + mask_id: Option, + ) -> Result { + self.flash_attn_f32_from_buffers_with_params_typed( + q_id, + k_id, + v_id, + n_q, + n_kv, + n_head, + d, + scale, + params, + mask_id, + Src0Type::F32, + ) + } + + #[allow(clippy::too_many_arguments)] + fn flash_attn_f32_from_buffers_with_params_typed( + &mut self, + q_id: ObjcId, + k_id: ObjcId, + v_id: ObjcId, + n_q: usize, + n_kv: usize, + n_head: usize, + d: usize, + scale: f32, + params: FlashAttnExtParams, + mask_id: Option, + kv_type: Src0Type, + ) -> Result { + let out_elems = n_q + .checked_mul(n_head) + .and_then(|v| v.checked_mul(d)) + .ok_or_else(|| "overflow computing flash output size".to_string())?; + let out_bytes = out_elems + .checked_mul(std::mem::size_of::()) + .ok_or_else(|| "overflow computing flash output bytes".to_string())?; + let kv_elem_bytes = flash_attn_kv_elem_bytes(kv_type)?; + + let dst_id = self.get_or_create_scratch_buffer(SCRATCH_FLASH_OUT, out_bytes)?; + + let use_vec = flash_attn_use_vec(n_q, d); + let pad_bytes = flash_attn_ext_extra_pad_bytes( + n_q, + n_kv, + n_head, + d, + kv_elem_bytes, + params.has_mask, + use_vec, + )?; + let pad_id = self.get_or_create_scratch_buffer(SCRATCH_FLASH_PAD, pad_bytes)?; + let mask_buf_id = if params.has_mask { + mask_id.ok_or_else(|| { + "flash-attn requested mask but no mask buffer was provided".to_string() + })? + } else { + q_id + }; + + if use_vec { + let tmp_bytes = flash_attn_ext_extra_tmp_bytes(n_q, n_head, d, 32)?; + let tmp_id = self.get_or_create_scratch_buffer(SCRATCH_FLASH_TMP, tmp_bytes)?; + + self.dispatch_flash_attn_ext_vec_f32( + q_id, + k_id, + v_id, + mask_buf_id, + q_id, // unused when has_sinks=false + pad_id, + Some(tmp_id), + dst_id, + n_q, + n_kv, + n_head, + d, + kv_type, + scale, + params.has_mask, + params.has_sinks, + params.max_bias, + params.logit_softcap, + )?; + } else { + let blk_bytes = flash_attn_ext_extra_blk_bytes(n_q, n_kv, params.has_mask, false)?; + let blk_id = self.get_or_create_scratch_buffer(SCRATCH_FLASH_BLK, blk_bytes)?; + + self.dispatch_flash_attn_ext_f32( + q_id, + k_id, + v_id, + mask_buf_id, + q_id, // unused when has_sinks=false + pad_id, + blk_id, + dst_id, + n_q, + n_kv, + n_head, + d, + kv_type, + scale, + params.has_mask, + params.has_sinks, + params.max_bias, + params.logit_softcap, + )?; + } + + unsafe { StrongId::from_unowned(dst_id) } + .ok_or_else(|| "flash-attn output buffer returned nil".to_string()) + } + + fn encoder_flash_kv_f16_buffer( + &mut self, + src_f32_id: ObjcId, + seq_len: usize, + n_kv_flash: usize, + n_state: usize, + scratch_kind: u8, + ) -> Result { + let row_bytes_f32 = n_state + .checked_mul(std::mem::size_of::()) + .ok_or_else(|| "overflow computing encoder f32 row bytes".to_string())?; + let row_bytes_f16 = n_state + .checked_mul(std::mem::size_of::()) + .ok_or_else(|| "overflow computing encoder f16 row bytes".to_string())?; + let total_bytes = n_kv_flash + .checked_mul(row_bytes_f16) + .ok_or_else(|| "overflow computing encoder f16 kv bytes".to_string())?; + let dst_id = self.get_or_create_scratch_buffer(scratch_kind, total_bytes)?; + + self.dispatch_cpy_f32_to_f16( + src_f32_id, + dst_id, + n_state, + seq_len, + std::mem::size_of::() as u64, + row_bytes_f32 as u64, + std::mem::size_of::() as u64, + row_bytes_f16 as u64, + )?; + + if seq_len < n_kv_flash { + let tail_off = seq_len + .checked_mul(row_bytes_f16) + .ok_or_else(|| "overflow computing encoder kv tail offset".to_string())?; + let tail_bytes = (n_kv_flash - seq_len) + .checked_mul(row_bytes_f16) + .ok_or_else(|| "overflow computing encoder kv tail bytes".to_string())?; + self.zero_buffer_range(dst_id, tail_off, tail_bytes)?; + } + + Ok(dst_id) + } + + #[allow(clippy::too_many_arguments)] + fn linear_from_src_buffer( + &mut self, + src_id: ObjcId, + m: usize, + k: usize, + w_bytes: &[u8], + w_ggml_type: u32, + n: usize, + bias: Option<&[f32]>, + weight_tag: u8, + bias_tag: u8, + ) -> Result { + let dst = self.matmul_nt_ggml_from_src1_buffer( + src_id, + w_bytes, + w_ggml_type, + m, + k, + n, + Some(weight_tag), + )?; + + if let Some(bias) = bias { + if bias.len() != n { + return Err(format!( + "linear bias len mismatch: got {}, expected {}", + bias.len(), + n + )); + } + let bias_id = self.get_or_create_cached_f32_buffer(bias, bias_tag)?; + let dst_shape = shape4_from_row_major(&[m, n], 4)?; + let bias_shape = shape4_from_row_major(&[n], 4)?; + self.dispatch_bin_f32( + 0, + dst.as_id(), + bias_id, + dst.as_id(), + &dst_shape, + &bias_shape, + )?; + } + + Ok(dst) + } + + #[allow(clippy::too_many_arguments)] + fn encoder_attn_block_f32( + &mut self, + x: &[f32], + seq_len: usize, + n_state: usize, + n_head: usize, + ln_w: &[f32], + ln_b: &[f32], + q_w_bytes: &[u8], + q_w_ggml_type: u32, + q_b: &[f32], + k_w_bytes: &[u8], + k_w_ggml_type: u32, + v_w_bytes: &[u8], + v_w_ggml_type: u32, + v_b: &[f32], + out_w_bytes: &[u8], + out_w_ggml_type: u32, + out_b: &[f32], + ) -> Result, String> { + if n_state == 0 || seq_len == 0 || n_head == 0 || n_state % n_head != 0 { + return Err(format!( + "invalid attn dimensions: seq_len={}, n_state={}, n_head={}", + seq_len, n_state, n_head + )); + } + let x_need = seq_len + .checked_mul(n_state) + .ok_or_else(|| "overflow computing x size".to_string())?; + if x.len() != x_need { + return Err(format!( + "x len mismatch: got {}, expected {}", + x.len(), + x_need + )); + } + if ln_w.len() != n_state || ln_b.len() != n_state { + return Err("layernorm affine size mismatch".to_string()); + } + if q_b.len() != n_state || v_b.len() != n_state || out_b.len() != n_state { + return Err("attention bias size mismatch".to_string()); + } + + let x_buf = self.new_buffer_with_bytes(f32_slice_as_bytes(x))?; + let x_shape = shape4_from_row_major(&[seq_len, n_state], 4)?; + let ln_shape = shape4_from_row_major(&[n_state], 4)?; + let ln_w_id = self.get_or_create_cached_f32_buffer(ln_w, 110)?; + let ln_b_id = self.get_or_create_cached_f32_buffer(ln_b, 111)?; + + let norm_buf = self.new_buffer_with_length(x_need * std::mem::size_of::())?; + self.dispatch_norm_f32( + x_buf.as_id(), + ln_w_id, + ln_b_id, + norm_buf.as_id(), + &x_shape, + &ln_shape, + &ln_shape, + 1e-5f32, + 3, + )?; + + let q_buf = self.linear_from_src_buffer( + norm_buf.as_id(), + seq_len, + n_state, + q_w_bytes, + q_w_ggml_type, + n_state, + Some(q_b), + 112, + 113, + )?; + let k_buf = self.linear_from_src_buffer( + norm_buf.as_id(), + seq_len, + n_state, + k_w_bytes, + k_w_ggml_type, + n_state, + None, + 114, + 0, + )?; + let v_buf = self.linear_from_src_buffer( + norm_buf.as_id(), + seq_len, + n_state, + v_w_bytes, + v_w_ggml_type, + n_state, + Some(v_b), + 115, + 116, + )?; + + let d = n_state / n_head; + let scale = 1.0f32 / (d as f32).sqrt(); + let attn_buf = self.flash_attn_f32_from_buffers( + q_buf.as_id(), + k_buf.as_id(), + v_buf.as_id(), + seq_len, + seq_len, + n_head, + d, + scale, + )?; + + let proj_buf = self.linear_from_src_buffer( + attn_buf.as_id(), + seq_len, + n_state, + out_w_bytes, + out_w_ggml_type, + n_state, + Some(out_b), + 117, + 118, + )?; + + self.dispatch_bin_f32( + 0, + proj_buf.as_id(), + x_buf.as_id(), + proj_buf.as_id(), + &x_shape, + &x_shape, + )?; + + self.read_f32_buffer(proj_buf.as_id(), x_need) + } + + #[allow(clippy::too_many_arguments)] + fn encoder_ffn_block_f32( + &mut self, + x: &[f32], + seq_len: usize, + n_state: usize, + ln_w: &[f32], + ln_b: &[f32], + w0_bytes: &[u8], + w0_ggml_type: u32, + b0: &[f32], + w1_bytes: &[u8], + w1_ggml_type: u32, + b1: &[f32], + ) -> Result, String> { + if n_state == 0 || seq_len == 0 { + return Err(format!( + "invalid ffn dimensions: seq_len={}, n_state={}", + seq_len, n_state + )); + } + let x_need = seq_len + .checked_mul(n_state) + .ok_or_else(|| "overflow computing x size".to_string())?; + if x.len() != x_need { + return Err(format!( + "x len mismatch: got {}, expected {}", + x.len(), + x_need + )); + } + if ln_w.len() != n_state || ln_b.len() != n_state { + return Err("layernorm affine size mismatch".to_string()); + } + let n_ff = b0.len(); + if n_ff == 0 || b1.len() != n_state { + return Err("ffn bias size mismatch".to_string()); + } + + let x_buf = self.new_buffer_with_bytes(f32_slice_as_bytes(x))?; + let x_shape = shape4_from_row_major(&[seq_len, n_state], 4)?; + let ln_shape = shape4_from_row_major(&[n_state], 4)?; + let ln_w_id = self.get_or_create_cached_f32_buffer(ln_w, 120)?; + let ln_b_id = self.get_or_create_cached_f32_buffer(ln_b, 121)?; + + let norm_buf = self.new_buffer_with_length(x_need * std::mem::size_of::())?; + self.dispatch_norm_f32( + x_buf.as_id(), + ln_w_id, + ln_b_id, + norm_buf.as_id(), + &x_shape, + &ln_shape, + &ln_shape, + 1e-5f32, + 3, + )?; + + let ff0_buf = self.linear_from_src_buffer( + norm_buf.as_id(), + seq_len, + n_state, + w0_bytes, + w0_ggml_type, + n_ff, + Some(b0), + 122, + 123, + )?; + + let ff0_shape = shape4_from_row_major(&[seq_len, n_ff], 4)?; + self.dispatch_unary_f32( + OP_UNARY_NUM_GELU, + ff0_buf.as_id(), + ff0_buf.as_id(), + &ff0_shape, + )?; + + let ff1_buf = self.linear_from_src_buffer( + ff0_buf.as_id(), + seq_len, + n_ff, + w1_bytes, + w1_ggml_type, + n_state, + Some(b1), + 124, + 125, + )?; + + self.dispatch_bin_f32( + 0, + ff1_buf.as_id(), + x_buf.as_id(), + ff1_buf.as_id(), + &x_shape, + &x_shape, + )?; + + self.read_f32_buffer(ff1_buf.as_id(), x_need) + } + + #[allow(clippy::too_many_arguments)] + fn encoder_layer_from_buffer_f32( + &mut self, + x_id: ObjcId, + seq_len: usize, + n_state: usize, + n_head: usize, + attn_ln_w: &[f32], + attn_ln_b: &[f32], + q_w_bytes: &[u8], + q_w_ggml_type: u32, + q_b: &[f32], + k_w_bytes: &[u8], + k_w_ggml_type: u32, + v_w_bytes: &[u8], + v_w_ggml_type: u32, + v_b: &[f32], + out_w_bytes: &[u8], + out_w_ggml_type: u32, + out_b: &[f32], + mlp_ln_w: &[f32], + mlp_ln_b: &[f32], + w0_bytes: &[u8], + w0_ggml_type: u32, + b0: &[f32], + w1_bytes: &[u8], + w1_ggml_type: u32, + b1: &[f32], + tag_base: u8, + ) -> Result { + if n_state == 0 || seq_len == 0 || n_head == 0 || n_state % n_head != 0 { + return Err(format!( + "invalid encoder layer dimensions: seq_len={}, n_state={}, n_head={}", + seq_len, n_state, n_head + )); + } + if attn_ln_w.len() != n_state + || attn_ln_b.len() != n_state + || mlp_ln_w.len() != n_state + || mlp_ln_b.len() != n_state + { + return Err("layernorm affine size mismatch".to_string()); + } + if q_b.len() != n_state || v_b.len() != n_state || out_b.len() != n_state { + return Err("attention bias size mismatch".to_string()); + } + let n_ff = b0.len(); + if n_ff == 0 || b1.len() != n_state { + return Err("ffn bias size mismatch".to_string()); + } + + let x_shape = shape4_from_row_major(&[seq_len, n_state], 4)?; + let ln_shape = shape4_from_row_major(&[n_state], 4)?; + + // Attention sub-block + let attn_ln_w_id = + self.get_or_create_cached_f32_buffer(attn_ln_w, tag_base.wrapping_add(0))?; + let attn_ln_b_id = + self.get_or_create_cached_f32_buffer(attn_ln_b, tag_base.wrapping_add(1))?; + let norm_bytes = x_shape + .numel + .checked_mul(std::mem::size_of::()) + .ok_or_else(|| "overflow computing encoder norm buffer bytes".to_string())?; + let norm0_id = self.get_or_create_scratch_buffer(SCRATCH_ENC_NORM0, norm_bytes)?; + self.dispatch_norm_f32( + x_id, + attn_ln_w_id, + attn_ln_b_id, + norm0_id, + &x_shape, + &ln_shape, + &ln_shape, + 1e-5f32, + 3, + )?; + + let q_buf = self.linear_from_src_buffer( + norm0_id, + seq_len, + n_state, + q_w_bytes, + q_w_ggml_type, + n_state, + Some(q_b), + tag_base.wrapping_add(2), + tag_base.wrapping_add(3), + )?; + + // Match whisper.cpp encoder flash-attn usage: keep KV rows padded. + let n_kv_flash = pad_to(seq_len, 256); + + let k_buf_f32 = self.linear_from_src_buffer( + norm0_id, + seq_len, + n_state, + k_w_bytes, + k_w_ggml_type, + n_state, + None, + tag_base.wrapping_add(4), + 0, + )?; + let v_buf_f32 = self.linear_from_src_buffer( + norm0_id, + seq_len, + n_state, + v_w_bytes, + v_w_ggml_type, + n_state, + Some(v_b), + tag_base.wrapping_add(5), + tag_base.wrapping_add(6), + )?; + let k_buf_f16 = self.encoder_flash_kv_f16_buffer( + k_buf_f32.as_id(), + seq_len, + n_kv_flash, + n_state, + SCRATCH_ENC_FLASH_K_F16, + )?; + let v_buf_f16 = self.encoder_flash_kv_f16_buffer( + v_buf_f32.as_id(), + seq_len, + n_kv_flash, + n_state, + SCRATCH_ENC_FLASH_V_F16, + )?; + + let d = n_state / n_head; + let scale = 1.0f32 / (d as f32).sqrt(); + let attn_buf = self.flash_attn_f32_from_buffers_with_params_typed( + q_buf.as_id(), + k_buf_f16, + v_buf_f16, + seq_len, + n_kv_flash, + n_head, + d, + scale, + FlashAttnExtParams::default(), + None, + Src0Type::F16, + )?; + let attn_res_buf = self.linear_from_src_buffer( + attn_buf.as_id(), + seq_len, + n_state, + out_w_bytes, + out_w_ggml_type, + n_state, + Some(out_b), + tag_base.wrapping_add(7), + tag_base.wrapping_add(8), + )?; + self.dispatch_bin_f32( + 0, + attn_res_buf.as_id(), + x_id, + attn_res_buf.as_id(), + &x_shape, + &x_shape, + )?; + + // FFN sub-block + let mlp_ln_w_id = + self.get_or_create_cached_f32_buffer(mlp_ln_w, tag_base.wrapping_add(9))?; + let mlp_ln_b_id = + self.get_or_create_cached_f32_buffer(mlp_ln_b, tag_base.wrapping_add(10))?; + let norm1_id = self.get_or_create_scratch_buffer(SCRATCH_ENC_NORM1, norm_bytes)?; + self.dispatch_norm_f32( + attn_res_buf.as_id(), + mlp_ln_w_id, + mlp_ln_b_id, + norm1_id, + &x_shape, + &ln_shape, + &ln_shape, + 1e-5f32, + 3, + )?; + + let ff0_buf = self.linear_from_src_buffer( + norm1_id, + seq_len, + n_state, + w0_bytes, + w0_ggml_type, + n_ff, + Some(b0), + tag_base.wrapping_add(11), + tag_base.wrapping_add(12), + )?; + let ff0_shape = shape4_from_row_major(&[seq_len, n_ff], 4)?; + self.dispatch_unary_f32( + OP_UNARY_NUM_GELU, + ff0_buf.as_id(), + ff0_buf.as_id(), + &ff0_shape, + )?; + let ff1_buf = self.linear_from_src_buffer( + ff0_buf.as_id(), + seq_len, + n_ff, + w1_bytes, + w1_ggml_type, + n_state, + Some(b1), + tag_base.wrapping_add(13), + tag_base.wrapping_add(14), + )?; + self.dispatch_bin_f32( + 0, + ff1_buf.as_id(), + attn_res_buf.as_id(), + ff1_buf.as_id(), + &x_shape, + &x_shape, + )?; + + Ok(ff1_buf) + } + + #[allow(clippy::too_many_arguments)] + fn encoder_layer_f32( + &mut self, + x: &[f32], + seq_len: usize, + n_state: usize, + n_head: usize, + attn_ln_w: &[f32], + attn_ln_b: &[f32], + q_w_bytes: &[u8], + q_w_ggml_type: u32, + q_b: &[f32], + k_w_bytes: &[u8], + k_w_ggml_type: u32, + v_w_bytes: &[u8], + v_w_ggml_type: u32, + v_b: &[f32], + out_w_bytes: &[u8], + out_w_ggml_type: u32, + out_b: &[f32], + mlp_ln_w: &[f32], + mlp_ln_b: &[f32], + w0_bytes: &[u8], + w0_ggml_type: u32, + b0: &[f32], + w1_bytes: &[u8], + w1_ggml_type: u32, + b1: &[f32], + ) -> Result, String> { + let x_need = seq_len + .checked_mul(n_state) + .ok_or_else(|| "overflow computing x size".to_string())?; + if x.len() != x_need { + return Err(format!( + "x len mismatch: got {}, expected {}", + x.len(), + x_need + )); + } + let x_buf = self.new_buffer_with_bytes(f32_slice_as_bytes(x))?; + + let ff1_buf = self.encoder_layer_from_buffer_f32( + x_buf.as_id(), + seq_len, + n_state, + n_head, + attn_ln_w, + attn_ln_b, + q_w_bytes, + q_w_ggml_type, + q_b, + k_w_bytes, + k_w_ggml_type, + v_w_bytes, + v_w_ggml_type, + v_b, + out_w_bytes, + out_w_ggml_type, + out_b, + mlp_ln_w, + mlp_ln_b, + w0_bytes, + w0_ggml_type, + b0, + w1_bytes, + w1_ggml_type, + b1, + 130, + )?; + self.read_f32_buffer(ff1_buf.as_id(), x_need) + } + + fn encoder_stack_f32( + &mut self, + x: &[f32], + seq_len: usize, + n_state: usize, + n_head: usize, + layers: &[EncoderLayer], + final_ln_w: &[f32], + final_ln_b: &[f32], + ) -> Result, String> { + if n_state == 0 || seq_len == 0 || n_head == 0 || n_state % n_head != 0 { + return Err(format!( + "invalid encoder stack dimensions: seq_len={}, n_state={}, n_head={}", + seq_len, n_state, n_head + )); + } + let x_need = seq_len + .checked_mul(n_state) + .ok_or_else(|| "overflow computing encoder stack x size".to_string())?; + if x.len() != x_need { + return Err(format!( + "encoder stack x len mismatch: got {}, expected {}", + x.len(), + x_need + )); + } + if final_ln_w.len() != n_state || final_ln_b.len() != n_state { + return Err("encoder stack final layernorm affine size mismatch".to_string()); + } + + let x_shape = shape4_from_row_major(&[seq_len, n_state], 4)?; + let ln_shape = shape4_from_row_major(&[n_state], 4)?; + + let out_buf = self.with_batch(|ctx| { + let mut cur_buf = ctx.new_buffer_with_bytes(f32_slice_as_bytes(x))?; + + // Reuse the same intermediate-output tag set across encoder layers. + // Layers execute sequentially, so per-layer unique output tags only inflate + // persistent Metal intermediate buffers without improving correctness. + for layer in layers.iter() { + let tag_base = 160u8; + let cur_id = cur_buf.as_id(); + cur_buf = ctx.encoder_layer_from_buffer_f32( + cur_id, + seq_len, + n_state, + n_head, + &layer.attn_ln_0_w.data, + &layer.attn_ln_0_b.data, + &layer.attn_q_w.data, + layer.attn_q_w.ggml_type, + &layer.attn_q_b.data, + &layer.attn_k_w.data, + layer.attn_k_w.ggml_type, + &layer.attn_v_w.data, + layer.attn_v_w.ggml_type, + &layer.attn_v_b.data, + &layer.attn_ln_1_w.data, + layer.attn_ln_1_w.ggml_type, + &layer.attn_ln_1_b.data, + &layer.mlp_ln_w.data, + &layer.mlp_ln_b.data, + &layer.mlp_0_w.data, + layer.mlp_0_w.ggml_type, + &layer.mlp_0_b.data, + &layer.mlp_1_w.data, + layer.mlp_1_w.ggml_type, + &layer.mlp_1_b.data, + tag_base, + )?; + } + + let ln_w_id = ctx.get_or_create_cached_f32_buffer(final_ln_w, 120)?; + let ln_b_id = ctx.get_or_create_cached_f32_buffer(final_ln_b, 121)?; + let out_buf = ctx.new_buffer_with_length(x_need * std::mem::size_of::())?; + ctx.dispatch_norm_f32( + cur_buf.as_id(), + ln_w_id, + ln_b_id, + out_buf.as_id(), + &x_shape, + &ln_shape, + &ln_shape, + 1e-5f32, + 3, + )?; + Ok(out_buf) + })?; + + self.read_f32_buffer(out_buf.as_id(), x_need) + } + + #[allow(clippy::too_many_arguments)] + fn decoder_self_qkv_step_f32( + &mut self, + x: &[f32], + n_state: usize, + attn_ln_w: &[f32], + attn_ln_b: &[f32], + q_w_bytes: &[u8], + q_w_ggml_type: u32, + q_b: &[f32], + k_w_bytes: &[u8], + k_w_ggml_type: u32, + v_w_bytes: &[u8], + v_w_ggml_type: u32, + v_b: &[f32], + ) -> Result<(Vec, Vec, Vec), String> { + if n_state == 0 { + return Err("decoder qkv: n_state is zero".to_string()); + } + if x.len() != n_state + || attn_ln_w.len() != n_state + || attn_ln_b.len() != n_state + || q_b.len() != n_state + || v_b.len() != n_state + { + return Err("decoder qkv: input/affine size mismatch".to_string()); + } + + let x_shape = shape4_from_row_major(&[1, n_state], 4)?; + let ln_shape = shape4_from_row_major(&[n_state], 4)?; + + let (q_buf, k_buf, v_buf) = self.with_batch(|ctx| { + let x_buf = ctx.new_buffer_with_bytes(f32_slice_as_bytes(x))?; + + let ln_w_id = ctx.get_or_create_cached_f32_buffer(attn_ln_w, 240)?; + let ln_b_id = ctx.get_or_create_cached_f32_buffer(attn_ln_b, 241)?; + let norm_bytes = n_state + .checked_mul(std::mem::size_of::()) + .ok_or_else(|| "overflow computing decoder qkv norm bytes".to_string())?; + let norm_id = ctx.get_or_create_scratch_buffer(SCRATCH_DEC_NORM0, norm_bytes)?; + ctx.dispatch_norm_f32( + x_buf.as_id(), + ln_w_id, + ln_b_id, + norm_id, + &x_shape, + &ln_shape, + &ln_shape, + 1e-5f32, + 3, + )?; + + let q_buf = ctx.linear_from_src_buffer( + norm_id, + 1, + n_state, + q_w_bytes, + q_w_ggml_type, + n_state, + Some(q_b), + 242, + 243, + )?; + let k_buf = ctx.linear_from_src_buffer( + norm_id, + 1, + n_state, + k_w_bytes, + k_w_ggml_type, + n_state, + None, + 244, + 0, + )?; + let v_buf = ctx.linear_from_src_buffer( + norm_id, + 1, + n_state, + v_w_bytes, + v_w_ggml_type, + n_state, + Some(v_b), + 245, + 246, + )?; + + Ok((q_buf, k_buf, v_buf)) + })?; + + self.read_f32_buffers3(q_buf.as_id(), k_buf.as_id(), v_buf.as_id(), n_state) + } + + #[allow(clippy::too_many_arguments)] + fn decoder_self_cross_ffn_step_f32( + &mut self, + layer_idx: usize, + x: &[f32], + q_self: &[f32], + k_all: &[f32], + v_all: &[f32], + n_kv: usize, + n_state: usize, + n_head: usize, + k_cross: &[f32], + v_cross: &[f32], + n_audio_ctx: usize, + layer: &DecoderLayer, + ) -> Result, String> { + if n_state == 0 || n_head == 0 || n_state % n_head != 0 { + return Err(format!( + "invalid decoder dimensions: n_state={}, n_head={}", + n_state, n_head + )); + } + if x.len() != n_state || q_self.len() != n_state { + return Err(format!( + "decoder self-cross x/q len mismatch: x={}, q={}, expected={}", + x.len(), + q_self.len(), + n_state + )); + } + let kv_need = n_kv + .checked_mul(n_state) + .ok_or_else(|| "overflow computing decoder self kv size".to_string())?; + if k_all.len() != kv_need || v_all.len() != kv_need { + return Err(format!( + "decoder self kv len mismatch: k_all={}, v_all={}, expected={}", + k_all.len(), + v_all.len(), + kv_need + )); + } + if layer.attn_ln_1_b.data.len() != n_state + || layer.cross_attn_ln_0_w.data.len() != n_state + || layer.cross_attn_ln_0_b.data.len() != n_state + || layer.cross_attn_q_b.data.len() != n_state + || layer.cross_attn_ln_1_b.data.len() != n_state + || layer.mlp_ln_w.data.len() != n_state + || layer.mlp_ln_b.data.len() != n_state + || layer.mlp_1_b.data.len() != n_state + { + return Err("decoder self/cross/ffn affine size mismatch".to_string()); + } + + let n_ff = layer.mlp_0_b.data.len(); + if n_ff == 0 { + return Err("decoder ffn hidden size is zero".to_string()); + } + + let d = n_state / n_head; + let scale = 1.0f32 / (d as f32).sqrt(); + + let out_buf = self.with_batch(|ctx| { + let x_shape = shape4_from_row_major(&[1, n_state], 4)?; + let ln_shape = shape4_from_row_major(&[n_state], 4)?; + + let x_buf = ctx.new_buffer_with_bytes(f32_slice_as_bytes(x))?; + + let n_kv_flash = pad_to(n_kv, 32); + let (k_self_id, v_self_id) = + ctx.ensure_decoder_kv_layer(layer_idx, n_state, n_kv_flash)?; + let row_bytes = n_state + .checked_mul(std::mem::size_of::()) + .ok_or_else(|| "overflow computing decoder kv row bytes".to_string())?; + let start_row = ctx + .decoder_kv_layers + .get(&layer_idx) + .map(|e| e.len_rows.min(n_kv)) + .unwrap_or(0); + if start_row < n_kv { + let copy_rows = n_kv - start_row; + let copy_bytes = copy_rows + .checked_mul(row_bytes) + .ok_or_else(|| "overflow computing decoder kv copy bytes".to_string())?; + let offset = start_row + .checked_mul(row_bytes) + .ok_or_else(|| "overflow computing decoder kv copy offset".to_string())?; + let src_k = f32_slice_as_bytes(&k_all[start_row * n_state..n_kv * n_state]); + let src_v = f32_slice_as_bytes(&v_all[start_row * n_state..n_kv * n_state]); + let dst_k: *mut u8 = unsafe { msg_send![k_self_id, contents] }; + let dst_v: *mut u8 = unsafe { msg_send![v_self_id, contents] }; + if dst_k.is_null() || dst_v.is_null() { + return Err("decoder kv buffer contents returned null".to_string()); + } + unsafe { + std::ptr::copy_nonoverlapping( + src_k.as_ptr(), + dst_k.add(offset), + copy_bytes, + ); + std::ptr::copy_nonoverlapping( + src_v.as_ptr(), + dst_v.add(offset), + copy_bytes, + ); + } + if let Some(entry) = ctx.decoder_kv_layers.get_mut(&layer_idx) { + entry.len_rows = n_kv; + } + } + + if n_kv < n_kv_flash { + let tail_off = n_kv + .checked_mul(row_bytes) + .ok_or_else(|| "overflow computing decoder kv tail offset".to_string())?; + let tail_len = (n_kv_flash - n_kv) + .checked_mul(row_bytes) + .ok_or_else(|| "overflow computing decoder kv tail bytes".to_string())?; + ctx.zero_buffer_range(k_self_id, tail_off, tail_len)?; + ctx.zero_buffer_range(v_self_id, tail_off, tail_len)?; + } + + let mask_id = ctx.prepare_decoder_self_mask_f16(n_kv, n_kv_flash)?; + let q_self_buf = ctx.new_buffer_with_bytes(f32_slice_as_bytes(q_self))?; + let self_attn_buf = ctx.flash_attn_f32_from_buffers_with_params( + q_self_buf.as_id(), + k_self_id, + v_self_id, + 1, + n_kv_flash, + n_head, + d, + scale, + FlashAttnExtParams { + has_mask: true, + ..FlashAttnExtParams::default() + }, + Some(mask_id), + )?; + + let self_proj_buf = ctx.linear_from_src_buffer( + self_attn_buf.as_id(), + 1, + n_state, + &layer.attn_ln_1_w.data, + layer.attn_ln_1_w.ggml_type, + n_state, + Some(&layer.attn_ln_1_b.data), + 212, + 213, + )?; + + ctx.dispatch_bin_f32( + 0, + self_proj_buf.as_id(), + x_buf.as_id(), + self_proj_buf.as_id(), + &x_shape, + &x_shape, + )?; + + let (k_cross_id, v_cross_id) = + ctx.ensure_cross_kv_layer(layer_idx, n_state, n_audio_ctx, k_cross, v_cross)?; + + let cross_ln_w_id = + ctx.get_or_create_cached_f32_buffer(&layer.cross_attn_ln_0_w.data, 214)?; + let cross_ln_b_id = + ctx.get_or_create_cached_f32_buffer(&layer.cross_attn_ln_0_b.data, 215)?; + let norm_bytes = n_state + .checked_mul(std::mem::size_of::()) + .ok_or_else(|| "overflow computing decoder norm bytes".to_string())?; + let norm0_id = ctx.get_or_create_scratch_buffer(SCRATCH_DEC_NORM0, norm_bytes)?; + ctx.dispatch_norm_f32( + self_proj_buf.as_id(), + cross_ln_w_id, + cross_ln_b_id, + norm0_id, + &x_shape, + &ln_shape, + &ln_shape, + 1e-5f32, + 3, + )?; + + let q_cross_buf = ctx.linear_from_src_buffer( + norm0_id, + 1, + n_state, + &layer.cross_attn_q_w.data, + layer.cross_attn_q_w.ggml_type, + n_state, + Some(&layer.cross_attn_q_b.data), + 216, + 217, + )?; + + let cross_attn_buf = ctx.flash_attn_f32_from_buffers( + q_cross_buf.as_id(), + k_cross_id, + v_cross_id, + 1, + n_audio_ctx, + n_head, + d, + scale, + )?; + + let cross_proj_buf = ctx.linear_from_src_buffer( + cross_attn_buf.as_id(), + 1, + n_state, + &layer.cross_attn_ln_1_w.data, + layer.cross_attn_ln_1_w.ggml_type, + n_state, + Some(&layer.cross_attn_ln_1_b.data), + 218, + 219, + )?; + + ctx.dispatch_bin_f32( + 0, + cross_proj_buf.as_id(), + self_proj_buf.as_id(), + cross_proj_buf.as_id(), + &x_shape, + &x_shape, + )?; + + let mlp_ln_w_id = ctx.get_or_create_cached_f32_buffer(&layer.mlp_ln_w.data, 220)?; + let mlp_ln_b_id = ctx.get_or_create_cached_f32_buffer(&layer.mlp_ln_b.data, 221)?; + let norm1_id = ctx.get_or_create_scratch_buffer(SCRATCH_DEC_NORM1, norm_bytes)?; + ctx.dispatch_norm_f32( + cross_proj_buf.as_id(), + mlp_ln_w_id, + mlp_ln_b_id, + norm1_id, + &x_shape, + &ln_shape, + &ln_shape, + 1e-5f32, + 3, + )?; + + let ff0_buf = ctx.linear_from_src_buffer( + norm1_id, + 1, + n_state, + &layer.mlp_0_w.data, + layer.mlp_0_w.ggml_type, + n_ff, + Some(&layer.mlp_0_b.data), + 222, + 223, + )?; + + let ff0_shape = shape4_from_row_major(&[1, n_ff], 4)?; + ctx.dispatch_unary_f32( + OP_UNARY_NUM_GELU, + ff0_buf.as_id(), + ff0_buf.as_id(), + &ff0_shape, + )?; + + let ff1_buf = ctx.linear_from_src_buffer( + ff0_buf.as_id(), + 1, + n_ff, + &layer.mlp_1_w.data, + layer.mlp_1_w.ggml_type, + n_state, + Some(&layer.mlp_1_b.data), + 224, + 225, + )?; + + ctx.dispatch_bin_f32( + 0, + ff1_buf.as_id(), + cross_proj_buf.as_id(), + ff1_buf.as_id(), + &x_shape, + &x_shape, + )?; + + Ok(ff1_buf) + })?; + + self.read_f32_buffer(out_buf.as_id(), n_state) + } + + #[allow(clippy::too_many_arguments)] + fn decoder_cross_ffn_step_f32( + &mut self, + layer_idx: usize, + x: &[f32], + n_state: usize, + n_head: usize, + k_cross: &[f32], + v_cross: &[f32], + n_audio_ctx: usize, + layer: &DecoderLayer, + ) -> Result, String> { + if n_state == 0 || n_head == 0 || n_state % n_head != 0 { + return Err(format!( + "invalid decoder dimensions: n_state={}, n_head={}", + n_state, n_head + )); + } + if x.len() != n_state { + return Err(format!( + "decoder x len mismatch: got {}, expected {}", + x.len(), + n_state + )); + } + if layer.cross_attn_ln_0_w.data.len() != n_state + || layer.cross_attn_ln_0_b.data.len() != n_state + || layer.cross_attn_q_b.data.len() != n_state + || layer.cross_attn_ln_1_b.data.len() != n_state + || layer.mlp_ln_w.data.len() != n_state + || layer.mlp_ln_b.data.len() != n_state + || layer.mlp_1_b.data.len() != n_state + { + return Err("decoder cross/ffn affine size mismatch".to_string()); + } + + let n_ff = layer.mlp_0_b.data.len(); + if n_ff == 0 { + return Err("decoder ffn hidden size is zero".to_string()); + } + + let d = n_state / n_head; + let scale = 1.0f32 / (d as f32).sqrt(); + + let out_buf = self.with_batch(|ctx| { + let x_shape = shape4_from_row_major(&[1, n_state], 4)?; + let ln_shape = shape4_from_row_major(&[n_state], 4)?; + + let x_buf = ctx.new_buffer_with_bytes(f32_slice_as_bytes(x))?; + let (k_cross_id, v_cross_id) = + ctx.ensure_cross_kv_layer(layer_idx, n_state, n_audio_ctx, k_cross, v_cross)?; + + // Cross-attention block + let cross_ln_w_id = + ctx.get_or_create_cached_f32_buffer(&layer.cross_attn_ln_0_w.data, 200)?; + let cross_ln_b_id = + ctx.get_or_create_cached_f32_buffer(&layer.cross_attn_ln_0_b.data, 201)?; + let norm_bytes = n_state + .checked_mul(std::mem::size_of::()) + .ok_or_else(|| "overflow computing decoder norm bytes".to_string())?; + let norm0_id = ctx.get_or_create_scratch_buffer(SCRATCH_DEC_NORM0, norm_bytes)?; + ctx.dispatch_norm_f32( + x_buf.as_id(), + cross_ln_w_id, + cross_ln_b_id, + norm0_id, + &x_shape, + &ln_shape, + &ln_shape, + 1e-5f32, + 3, + )?; + + let q_buf = ctx.linear_from_src_buffer( + norm0_id, + 1, + n_state, + &layer.cross_attn_q_w.data, + layer.cross_attn_q_w.ggml_type, + n_state, + Some(&layer.cross_attn_q_b.data), + 202, + 203, + )?; + + let attn_buf = ctx.flash_attn_f32_from_buffers( + q_buf.as_id(), + k_cross_id, + v_cross_id, + 1, + n_audio_ctx, + n_head, + d, + scale, + )?; + + let cross_proj_buf = ctx.linear_from_src_buffer( + attn_buf.as_id(), + 1, + n_state, + &layer.cross_attn_ln_1_w.data, + layer.cross_attn_ln_1_w.ggml_type, + n_state, + Some(&layer.cross_attn_ln_1_b.data), + 204, + 205, + )?; + + ctx.dispatch_bin_f32( + 0, + cross_proj_buf.as_id(), + x_buf.as_id(), + cross_proj_buf.as_id(), + &x_shape, + &x_shape, + )?; + + // FFN block + let mlp_ln_w_id = ctx.get_or_create_cached_f32_buffer(&layer.mlp_ln_w.data, 206)?; + let mlp_ln_b_id = ctx.get_or_create_cached_f32_buffer(&layer.mlp_ln_b.data, 207)?; + let norm1_id = ctx.get_or_create_scratch_buffer(SCRATCH_DEC_NORM1, norm_bytes)?; + ctx.dispatch_norm_f32( + cross_proj_buf.as_id(), + mlp_ln_w_id, + mlp_ln_b_id, + norm1_id, + &x_shape, + &ln_shape, + &ln_shape, + 1e-5f32, + 3, + )?; + + let ff0_buf = ctx.linear_from_src_buffer( + norm1_id, + 1, + n_state, + &layer.mlp_0_w.data, + layer.mlp_0_w.ggml_type, + n_ff, + Some(&layer.mlp_0_b.data), + 208, + 209, + )?; + + let ff0_shape = shape4_from_row_major(&[1, n_ff], 4)?; + ctx.dispatch_unary_f32( + OP_UNARY_NUM_GELU, + ff0_buf.as_id(), + ff0_buf.as_id(), + &ff0_shape, + )?; + + let ff1_buf = ctx.linear_from_src_buffer( + ff0_buf.as_id(), + 1, + n_ff, + &layer.mlp_1_w.data, + layer.mlp_1_w.ggml_type, + n_state, + Some(&layer.mlp_1_b.data), + 210, + 211, + )?; + + ctx.dispatch_bin_f32( + 0, + ff1_buf.as_id(), + cross_proj_buf.as_id(), + ff1_buf.as_id(), + &x_shape, + &x_shape, + )?; + + Ok(ff1_buf) + })?; + + self.read_f32_buffer(out_buf.as_id(), n_state) + } + + fn bin_f32( + &mut self, + op_num: i16, + a: &[f32], + a_shape: &[usize], + b: &[f32], + b_shape: &[usize], + ) -> Result, String> { + let a_s = shape4_from_row_major(a_shape, 4)?; + let b_s = shape4_from_row_major(b_shape, 4)?; + if a.len() != a_s.numel { + return Err(format!( + "lhs len mismatch: got {}, expected {}", + a.len(), + a_s.numel + )); + } + if b.len() != b_s.numel { + return Err(format!( + "rhs len mismatch: got {}, expected {}", + b.len(), + b_s.numel + )); + } + + let a_bytes = unsafe { + std::slice::from_raw_parts( + a.as_ptr() as *const u8, + a.len() * std::mem::size_of::(), + ) + }; + let b_bytes = unsafe { + std::slice::from_raw_parts( + b.as_ptr() as *const u8, + b.len() * std::mem::size_of::(), + ) + }; + + let a_buf = self.new_buffer_with_bytes(a_bytes)?; + let b_buf = self.new_buffer_with_bytes(b_bytes)?; + let dst_buf = self.new_buffer_with_length(a_s.numel * std::mem::size_of::())?; + + self.dispatch_bin_f32( + op_num, + a_buf.as_id(), + b_buf.as_id(), + dst_buf.as_id(), + &a_s, + &b_s, + )?; + self.read_f32_buffer(dst_buf.as_id(), a_s.numel) + } + + fn unary_gelu_f32(&mut self, a: &[f32], shape: &[usize]) -> Result, String> { + let s = shape4_from_row_major(shape, 4)?; + if a.len() != s.numel { + return Err(format!( + "unary len mismatch: got {}, expected {}", + a.len(), + s.numel + )); + } + + let a_bytes = unsafe { + std::slice::from_raw_parts( + a.as_ptr() as *const u8, + a.len() * std::mem::size_of::(), + ) + }; + + let a_buf = self.new_buffer_with_bytes(a_bytes)?; + let dst_buf = self.new_buffer_with_length(s.numel * std::mem::size_of::())?; + self.dispatch_unary_f32(OP_UNARY_NUM_GELU, a_buf.as_id(), dst_buf.as_id(), &s)?; + self.read_f32_buffer(dst_buf.as_id(), s.numel) + } + + fn norm_f32(&mut self, x: &[f32], x_shape: &[usize], eps: f32) -> Result, String> { + let s = shape4_from_row_major(x_shape, 4)?; + if x.len() != s.numel { + return Err(format!( + "norm len mismatch: got {}, expected {}", + x.len(), + s.numel + )); + } + + let x_bytes = unsafe { + std::slice::from_raw_parts( + x.as_ptr() as *const u8, + x.len() * std::mem::size_of::(), + ) + }; + + let x_buf = self.new_buffer_with_bytes(x_bytes)?; + let dst_buf = self.new_buffer_with_length(s.numel * std::mem::size_of::())?; + self.dispatch_norm_f32( + x_buf.as_id(), + x_buf.as_id(), + x_buf.as_id(), + dst_buf.as_id(), + &s, + &s, + &s, + eps, + 1, + )?; + self.read_f32_buffer(dst_buf.as_id(), s.numel) + } + + #[allow(clippy::too_many_arguments)] + fn norm_mul_add_f32( + &mut self, + x: &[f32], + x_shape: &[usize], + mul: &[f32], + mul_shape: &[usize], + add: &[f32], + add_shape: &[usize], + eps: f32, + ) -> Result, String> { + let x_s = shape4_from_row_major(x_shape, 4)?; + let m_s = shape4_from_row_major(mul_shape, 4)?; + let a_s = shape4_from_row_major(add_shape, 4)?; + + if x.len() != x_s.numel { + return Err(format!( + "norm src len mismatch: got {}, expected {}", + x.len(), + x_s.numel + )); + } + if mul.len() != m_s.numel { + return Err(format!( + "norm mul len mismatch: got {}, expected {}", + mul.len(), + m_s.numel + )); + } + if add.len() != a_s.numel { + return Err(format!( + "norm add len mismatch: got {}, expected {}", + add.len(), + a_s.numel + )); + } + + if m_s.ne[0] != x_s.ne[0] || a_s.ne[0] != x_s.ne[0] { + return Err(format!( + "norm fuse ne0 mismatch: x={} mul={} add={}", + x_s.ne[0], m_s.ne[0], a_s.ne[0] + )); + } + for d in 1..4 { + if (m_s.ne[d] != 1 && m_s.ne[d] != x_s.ne[d]) + || (a_s.ne[d] != 1 && a_s.ne[d] != x_s.ne[d]) + { + return Err("norm fuse broadcast mismatch".to_string()); + } + } + + let x_bytes = unsafe { + std::slice::from_raw_parts( + x.as_ptr() as *const u8, + x.len() * std::mem::size_of::(), + ) + }; + let mul_bytes = unsafe { + std::slice::from_raw_parts( + mul.as_ptr() as *const u8, + mul.len() * std::mem::size_of::(), + ) + }; + let add_bytes = unsafe { + std::slice::from_raw_parts( + add.as_ptr() as *const u8, + add.len() * std::mem::size_of::(), + ) + }; + + let x_buf = self.new_buffer_with_bytes(x_bytes)?; + let mul_buf = self.new_buffer_with_bytes(mul_bytes)?; + let add_buf = self.new_buffer_with_bytes(add_bytes)?; + let dst_buf = self.new_buffer_with_length(x_s.numel * std::mem::size_of::())?; + + self.dispatch_norm_f32( + x_buf.as_id(), + mul_buf.as_id(), + add_buf.as_id(), + dst_buf.as_id(), + &x_s, + &m_s, + &a_s, + eps, + 3, + )?; + self.read_f32_buffer(dst_buf.as_id(), x_s.numel) + } + + fn im2col_1d_f32( + &mut self, + input: &[f32], + ic: usize, + iw: usize, + kw: usize, + stride: usize, + pad: usize, + ) -> Result, String> { + if kw == 0 || stride == 0 { + return Err("im2col requires kw>0 and stride>0".to_string()); + } + let expect = ic + .checked_mul(iw) + .ok_or_else(|| "overflow computing input size".to_string())?; + if input.len() != expect { + return Err(format!( + "im2col input len mismatch: got {}, expected {}", + input.len(), + expect + )); + } + let num = iw + .checked_add(pad.saturating_mul(2)) + .ok_or_else(|| "overflow computing im2col output numerator".to_string())?; + if num < kw { + return Ok(Vec::new()); + } + let ow = (num - kw) / stride + 1; + let chw = ic + .checked_mul(kw) + .ok_or_else(|| "overflow computing im2col CHW".to_string())?; + let out_elems = ow + .checked_mul(chw) + .ok_or_else(|| "overflow computing im2col output size".to_string())?; + + let input_bytes = unsafe { + std::slice::from_raw_parts( + input.as_ptr() as *const u8, + input.len() * std::mem::size_of::(), + ) + }; + let src_buf = self.new_buffer_with_bytes(input_bytes)?; + let dst_buf = self.new_buffer_with_length(out_elems * std::mem::size_of::())?; + self.dispatch_im2col_1d_f32( + src_buf.as_id(), + dst_buf.as_id(), + ic, + iw, + kw, + stride, + pad, + ow, + )?; + self.read_f32_buffer(dst_buf.as_id(), out_elems) + } + + fn matmul_nt_ggml_from_src1_buffer( + &mut self, + src1_id: ObjcId, + bt_bytes: &[u8], + bt_ggml_type: u32, + m: usize, + k: usize, + n: usize, + weight_cache_tag: Option, + ) -> Result { + if m == 0 || k == 0 || n == 0 { + return self + .new_buffer_with_length(m.saturating_mul(n) * std::mem::size_of::()); + } + + let src0 = src0_type_from_ggml(bt_ggml_type).ok_or_else(|| { + format!( + "unsupported src0 ggml_type for metal matmul: {}", + bt_ggml_type + ) + })?; + let (src0_row_bytes, nb00) = src0_layout_bytes_per_row(src0, k)?; + let expected_src0 = n + .checked_mul(src0_row_bytes) + .ok_or_else(|| "matmul overflow computing src0 bytes".to_string())?; + if bt_bytes.len() != expected_src0 { + return Err(format!( + "rhs len mismatch: got {}, expected {} (type {:?}, k={}, n={})", + bt_bytes.len(), + expected_src0, + src0, + k, + n + )); + } + + let ne00 = i32::try_from(k).map_err(|_| format!("k too large: {}", k))?; + let ne01 = i32::try_from(n).map_err(|_| format!("n too large: {}", n))?; + let ne10 = i32::try_from(k).map_err(|_| format!("k too large: {}", k))?; + let ne11 = i32::try_from(m).map_err(|_| format!("m too large: {}", m))?; + let ne0 = ne01; + let ne1 = ne11; + let nb01 = src0_row_bytes as u64; + let nb10 = 4u64; + let nb11 = (k as u64) + .checked_mul(4) + .ok_or_else(|| "overflow computing nb11".to_string())?; + let mn = m + .checked_mul(n) + .ok_or_else(|| "matmul overflow computing m*n".to_string())?; + + let mut src0_temp = None; + let src0_id = if let Some(tag) = weight_cache_tag { + let key = BufferKey { + ptr: bt_bytes.as_ptr() as usize, + len: bt_bytes.len(), + tag, + }; + self.get_or_create_weight_buffer(key, bt_bytes)? + } else { + let b = self.new_buffer_with_bytes(bt_bytes)?; + let id = b.as_id(); + src0_temp = Some(b); + id + }; + + let dst_bytes = mn + .checked_mul(std::mem::size_of::()) + .ok_or_else(|| "matmul overflow computing dst bytes".to_string())?; + let mut dst_temp = None; + let dst_id = if let Some(tag) = weight_cache_tag { + self.get_or_create_matmul_out_buffer(tag, dst_bytes)? + } else { + let b = self.new_buffer_with_length(dst_bytes)?; + let id = b.as_id(); + dst_temp = Some(b); + id + }; + let used_mul_mv_ext = can_use_mul_mv_ext(src0, ne00, ne11); + let used_mul_mm = ne00 >= 64 && ne11 > 8; + + let (kernel, compute_res) = if used_mul_mv_ext { + ( + "mul_mv_ext", + self.dispatch_mul_mv_ext( + src0, src0_id, src1_id, dst_id, ne00, ne01, ne10, ne11, nb00, nb01, nb10, + nb11, ne0, ne1, + ), + ) + } else if used_mul_mm { + match self.dispatch_mul_mm( + src0, src0_id, src1_id, dst_id, ne00, ne01, nb01, 1, nb10, nb11, ne0, ne1, + ) { + Ok(()) => ("mul_mm", Ok(())), + Err(e) => ("mul_mv", { + eprintln!( + "[voice2][metal] mul_mm failed for type {:?}, falling back to mul_mv: {}", + src0, e + ); + self.dispatch_mul_mv( + src0, src0_id, src1_id, dst_id, ne00, ne01, ne10, ne11, nb00, nb01, + nb10, nb11, ne0, ne1, + ) + }), + } + } else { + ( + "mul_mv", + self.dispatch_mul_mv( + src0, src0_id, src1_id, dst_id, ne00, ne01, ne10, ne11, nb00, nb01, nb10, + nb11, ne0, ne1, + ), + ) + }; + compute_res?; + if super::log_mul_mat_requested() { + eprintln!( + "[voice2][metal] mul_mat kernel={} src0={} src1=f32 ne00={} ne01={} ne11={} ne12=1 nb01={} nb11={}", + kernel, + src0_type_name(src0), + ne00, + ne01, + ne11, + nb01, + nb11 + ); + } + drop(src0_temp); + if let Some(dst_buffer) = dst_temp { + Ok(dst_buffer) + } else { + unsafe { StrongId::from_unowned(dst_id) } + .ok_or_else(|| "matmul scratch output buffer returned nil".to_string()) + } + } + + fn matmul_nt_ggml_bytes_impl( + &mut self, + a: &[f32], + bt_bytes: &[u8], + bt_ggml_type: u32, + m: usize, + k: usize, + n: usize, + weight_cache_tag: Option, + ) -> Result<(StrongId, usize, usize), String> { + let mk = m + .checked_mul(k) + .ok_or_else(|| "matmul overflow computing m*k".to_string())?; + if a.len() != mk { + return Err(format!( + "lhs len mismatch: got {}, expected {}", + a.len(), + mk + )); + } + + let a_bytes = unsafe { + std::slice::from_raw_parts( + a.as_ptr() as *const u8, + a.len() * std::mem::size_of::(), + ) + }; + let src1_buffer = self.new_buffer_with_bytes(a_bytes)?; + let dst_buffer = self.matmul_nt_ggml_from_src1_buffer( + src1_buffer.as_id(), + bt_bytes, + bt_ggml_type, + m, + k, + n, + weight_cache_tag, + )?; + + Ok((dst_buffer, m, n)) + } + + fn matmul_nn_f32( + &mut self, + a: &[f32], + b: &[f32], + m: usize, + k: usize, + n: usize, + ) -> Result, String> { + let mk = m + .checked_mul(k) + .ok_or_else(|| "overflow computing m*k".to_string())?; + let kn = k + .checked_mul(n) + .ok_or_else(|| "overflow computing k*n".to_string())?; + if a.len() != mk { + return Err(format!( + "lhs len mismatch: got {}, expected {}", + a.len(), + mk + )); + } + if b.len() != kn { + return Err(format!( + "rhs len mismatch: got {}, expected {}", + b.len(), + kn + )); + } + + let mut bt = vec![0.0f32; n * k]; + for i in 0..k { + for j in 0..n { + bt[j * k + i] = b[i * n + j]; + } + } + + let bt_bytes = unsafe { + std::slice::from_raw_parts( + bt.as_ptr() as *const u8, + bt.len() * std::mem::size_of::(), + ) + }; + + let (dst, mr, nr) = + self.matmul_nt_ggml_bytes_impl(a, bt_bytes, GGML_TYPE_F32, m, k, n, None)?; + self.read_f32_buffer(dst.as_id(), mr * nr) + } + + fn matmul_nt_f32( + &mut self, + a: &[f32], + bt: &[f32], + m: usize, + k: usize, + n: usize, + ) -> Result, String> { + let bt_bytes = unsafe { + std::slice::from_raw_parts( + bt.as_ptr() as *const u8, + bt.len() * std::mem::size_of::(), + ) + }; + let cache_tag = Some(1u8); + let (dst, mr, nr) = + self.matmul_nt_ggml_bytes_impl(a, bt_bytes, GGML_TYPE_F32, m, k, n, cache_tag)?; + self.read_f32_buffer(dst.as_id(), mr * nr) + } + + fn matmul_nt_f32_bytes( + &mut self, + a: &[f32], + bt_bytes: &[u8], + m: usize, + k: usize, + n: usize, + ) -> Result, String> { + let (dst, mr, nr) = + self.matmul_nt_ggml_bytes_impl(a, bt_bytes, GGML_TYPE_F32, m, k, n, Some(2u8))?; + self.read_f32_buffer(dst.as_id(), mr * nr) + } + + fn matmul_nt_f16_bytes( + &mut self, + a: &[f32], + bt_f16_bytes: &[u8], + m: usize, + k: usize, + n: usize, + ) -> Result, String> { + let (dst, mr, nr) = + self.matmul_nt_ggml_bytes_impl(a, bt_f16_bytes, GGML_TYPE_F16, m, k, n, Some(3u8))?; + self.read_f32_buffer(dst.as_id(), mr * nr) + } + + fn matmul_nt_ggml_bytes( + &mut self, + a: &[f32], + bt_bytes: &[u8], + bt_ggml_type: u32, + m: usize, + k: usize, + n: usize, + ) -> Result, String> { + let tag = match bt_ggml_type { + GGML_TYPE_F32 => 2u8, + GGML_TYPE_F16 => 3u8, + GGML_TYPE_Q4_0 => 4u8, + GGML_TYPE_Q4_1 => 5u8, + GGML_TYPE_Q5_0 => 6u8, + GGML_TYPE_Q5_1 => 7u8, + GGML_TYPE_Q8_0 => 8u8, + _ => 0u8, + }; + let (dst, mr, nr) = + self.matmul_nt_ggml_bytes_impl(a, bt_bytes, bt_ggml_type, m, k, n, Some(tag))?; + self.read_f32_buffer(dst.as_id(), mr * nr) + } + + fn matmul_nt_ggml_bytes_add_bias( + &mut self, + a: &[f32], + bt_bytes: &[u8], + bt_ggml_type: u32, + m: usize, + k: usize, + n: usize, + bias: &[f32], + ) -> Result, String> { + if bias.len() != n { + return Err(format!( + "bias len mismatch for matmul+add: got {}, expected {}", + bias.len(), + n + )); + } + + let tag = match bt_ggml_type { + GGML_TYPE_F32 => 2u8, + GGML_TYPE_F16 => 3u8, + GGML_TYPE_Q4_0 => 4u8, + GGML_TYPE_Q4_1 => 5u8, + GGML_TYPE_Q5_0 => 6u8, + GGML_TYPE_Q5_1 => 7u8, + GGML_TYPE_Q8_0 => 8u8, + _ => 0u8, + }; + + let (dst, mr, nr) = + self.matmul_nt_ggml_bytes_impl(a, bt_bytes, bt_ggml_type, m, k, n, Some(tag))?; + + let bias_shape = shape4_from_row_major(&[n], 4)?; + let dst_shape = shape4_from_row_major(&[m, n], 4)?; + let bias_bytes = unsafe { + std::slice::from_raw_parts( + bias.as_ptr() as *const u8, + bias.len() * std::mem::size_of::(), + ) + }; + let bias_buf = self.new_buffer_with_bytes(bias_bytes)?; + self.dispatch_bin_f32( + 0, + dst.as_id(), + bias_buf.as_id(), + dst.as_id(), + &dst_shape, + &bias_shape, + )?; + + self.read_f32_buffer(dst.as_id(), mr * nr) + } + } + + fn with_context(f: impl FnOnce(&mut MetalContext) -> Result) -> Option { + enum ContextState { + Uninitialized, + Disabled, + Ready(MetalContext), + } + + thread_local! { + static CONTEXT: RefCell = const { RefCell::new(ContextState::Uninitialized) }; + } + + CONTEXT.with(|ctx| { + let mut ctx = ctx.borrow_mut(); + + if matches!(&*ctx, ContextState::Uninitialized) { + *ctx = match MetalContext::new() { + Ok(created) => ContextState::Ready(created), + Err(err) => { + eprintln!("[voice2][metal] backend disabled: {}", err); + ContextState::Disabled + } + }; + } + + let ctx = match &mut *ctx { + ContextState::Ready(ctx) => ctx, + ContextState::Disabled | ContextState::Uninitialized => return None, + }; + match f(ctx) { + Ok(v) => Some(v), + Err(err) => { + eprintln!("[voice2][metal] compute failed: {}", err); + None + } + } + }) + } + + pub(super) fn try_matmul_nn_f32( + a: &[f32], + b: &[f32], + m: usize, + k: usize, + n: usize, + ) -> Option> { + with_context(|ctx| ctx.matmul_nn_f32(a, b, m, k, n)) + } + + pub(super) fn try_matmul_nt_f32( + a: &[f32], + bt: &[f32], + m: usize, + k: usize, + n: usize, + ) -> Option> { + with_context(|ctx| ctx.matmul_nt_f32(a, bt, m, k, n)) + } + + pub(super) fn try_matmul_nt_f32_bytes( + a: &[f32], + bt_bytes: &[u8], + m: usize, + k: usize, + n: usize, + ) -> Option> { + with_context(|ctx| ctx.matmul_nt_f32_bytes(a, bt_bytes, m, k, n)) + } + + pub(super) fn try_matmul_nt_f16_bytes( + a: &[f32], + bt_f16_bytes: &[u8], + m: usize, + k: usize, + n: usize, + ) -> Option> { + with_context(|ctx| ctx.matmul_nt_f16_bytes(a, bt_f16_bytes, m, k, n)) + } + + pub(super) fn try_matmul_nt_ggml_bytes( + a: &[f32], + bt_bytes: &[u8], + bt_ggml_type: u32, + m: usize, + k: usize, + n: usize, + ) -> Option> { + with_context(|ctx| ctx.matmul_nt_ggml_bytes(a, bt_bytes, bt_ggml_type, m, k, n)) + } + + pub(super) fn try_matmul_nt_ggml_bytes_add_bias( + a: &[f32], + bt_bytes: &[u8], + bt_ggml_type: u32, + m: usize, + k: usize, + n: usize, + bias: &[f32], + ) -> Option> { + with_context(|ctx| { + ctx.matmul_nt_ggml_bytes_add_bias(a, bt_bytes, bt_ggml_type, m, k, n, bias) + }) + } + + pub(super) fn try_flash_attn_f32_packed( + q: &[f32], + k: &[f32], + v: &[f32], + n_q: usize, + n_kv: usize, + n_head: usize, + d: usize, + scale: f32, + ) -> Option> { + with_context(|ctx| ctx.flash_attn_f32_packed(q, k, v, n_q, n_kv, n_head, d, scale)) + } + + pub(super) fn clear_decoder_kv_cache() { + let _ = with_context(|ctx| { + ctx.clear_decoder_kv_cache(); + Ok(()) + }); + } + + #[allow(clippy::too_many_arguments)] + pub(super) fn try_flash_attn_f32_self_kv_cache( + layer: usize, + q: &[f32], + k_all: &[f32], + v_all: &[f32], + n_kv: usize, + n_head: usize, + d: usize, + scale: f32, + ) -> Option> { + with_context(|ctx| { + ctx.flash_attn_f32_self_kv_cache(layer, q, k_all, v_all, n_kv, n_head, d, scale) + }) + } + + #[allow(clippy::too_many_arguments)] + pub(super) fn try_flash_attn_f32_cross_kv_cache( + layer: usize, + q: &[f32], + k_cross: &[f32], + v_cross: &[f32], + n_q: usize, + n_kv: usize, + n_head: usize, + d: usize, + scale: f32, + ) -> Option> { + with_context(|ctx| { + ctx.flash_attn_f32_cross_kv_cache( + layer, q, k_cross, v_cross, n_q, n_kv, n_head, d, scale, + ) + }) + } + + pub(super) fn try_add_f32( + a: &[f32], + a_shape: &[usize], + b: &[f32], + b_shape: &[usize], + ) -> Option> { + with_context(|ctx| ctx.bin_f32(0, a, a_shape, b, b_shape)) + } + + pub(super) fn try_mul_f32( + a: &[f32], + a_shape: &[usize], + b: &[f32], + b_shape: &[usize], + ) -> Option> { + with_context(|ctx| ctx.bin_f32(2, a, a_shape, b, b_shape)) + } + + pub(super) fn try_gelu_f32(a: &[f32], shape: &[usize]) -> Option> { + with_context(|ctx| ctx.unary_gelu_f32(a, shape)) + } + + pub(super) fn try_layer_norm_f32(x: &[f32], shape: &[usize], eps: f32) -> Option> { + with_context(|ctx| ctx.norm_f32(x, shape, eps)) + } + + pub(super) fn try_layer_norm_mul_add_f32( + x: &[f32], + x_shape: &[usize], + mul: &[f32], + mul_shape: &[usize], + add: &[f32], + add_shape: &[usize], + eps: f32, + ) -> Option> { + with_context(|ctx| ctx.norm_mul_add_f32(x, x_shape, mul, mul_shape, add, add_shape, eps)) + } + + pub(super) fn try_im2col_1d_f32( + input: &[f32], + ic: usize, + iw: usize, + kw: usize, + stride: usize, + pad: usize, + ) -> Option> { + with_context(|ctx| ctx.im2col_1d_f32(input, ic, iw, kw, stride, pad)) + } + + #[allow(clippy::too_many_arguments)] + pub(super) fn try_encoder_attn_block_f32( + x: &[f32], + seq_len: usize, + n_state: usize, + n_head: usize, + ln_w: &[f32], + ln_b: &[f32], + q_w_bytes: &[u8], + q_w_ggml_type: u32, + q_b: &[f32], + k_w_bytes: &[u8], + k_w_ggml_type: u32, + v_w_bytes: &[u8], + v_w_ggml_type: u32, + v_b: &[f32], + out_w_bytes: &[u8], + out_w_ggml_type: u32, + out_b: &[f32], + ) -> Option> { + with_context(|ctx| { + ctx.encoder_attn_block_f32( + x, + seq_len, + n_state, + n_head, + ln_w, + ln_b, + q_w_bytes, + q_w_ggml_type, + q_b, + k_w_bytes, + k_w_ggml_type, + v_w_bytes, + v_w_ggml_type, + v_b, + out_w_bytes, + out_w_ggml_type, + out_b, + ) + }) + } + + #[allow(clippy::too_many_arguments)] + pub(super) fn try_encoder_ffn_block_f32( + x: &[f32], + seq_len: usize, + n_state: usize, + ln_w: &[f32], + ln_b: &[f32], + w0_bytes: &[u8], + w0_ggml_type: u32, + b0: &[f32], + w1_bytes: &[u8], + w1_ggml_type: u32, + b1: &[f32], + ) -> Option> { + with_context(|ctx| { + ctx.encoder_ffn_block_f32( + x, + seq_len, + n_state, + ln_w, + ln_b, + w0_bytes, + w0_ggml_type, + b0, + w1_bytes, + w1_ggml_type, + b1, + ) + }) + } + + #[allow(clippy::too_many_arguments)] + pub(super) fn try_encoder_layer_f32( + x: &[f32], + seq_len: usize, + n_state: usize, + n_head: usize, + attn_ln_w: &[f32], + attn_ln_b: &[f32], + q_w_bytes: &[u8], + q_w_ggml_type: u32, + q_b: &[f32], + k_w_bytes: &[u8], + k_w_ggml_type: u32, + v_w_bytes: &[u8], + v_w_ggml_type: u32, + v_b: &[f32], + out_w_bytes: &[u8], + out_w_ggml_type: u32, + out_b: &[f32], + mlp_ln_w: &[f32], + mlp_ln_b: &[f32], + w0_bytes: &[u8], + w0_ggml_type: u32, + b0: &[f32], + w1_bytes: &[u8], + w1_ggml_type: u32, + b1: &[f32], + ) -> Option> { + with_context(|ctx| { + ctx.encoder_layer_f32( + x, + seq_len, + n_state, + n_head, + attn_ln_w, + attn_ln_b, + q_w_bytes, + q_w_ggml_type, + q_b, + k_w_bytes, + k_w_ggml_type, + v_w_bytes, + v_w_ggml_type, + v_b, + out_w_bytes, + out_w_ggml_type, + out_b, + mlp_ln_w, + mlp_ln_b, + w0_bytes, + w0_ggml_type, + b0, + w1_bytes, + w1_ggml_type, + b1, + ) + }) + } + + pub(super) fn try_encoder_stack_f32( + x: &[f32], + seq_len: usize, + n_state: usize, + n_head: usize, + layers: &[EncoderLayer], + final_ln_w: &[f32], + final_ln_b: &[f32], + ) -> Option> { + with_context(|ctx| { + ctx.encoder_stack_f32(x, seq_len, n_state, n_head, layers, final_ln_w, final_ln_b) + }) + } + + #[allow(clippy::too_many_arguments)] + pub(super) fn try_decoder_self_qkv_step_f32( + x: &[f32], + n_state: usize, + attn_ln_w: &[f32], + attn_ln_b: &[f32], + q_w_bytes: &[u8], + q_w_ggml_type: u32, + q_b: &[f32], + k_w_bytes: &[u8], + k_w_ggml_type: u32, + v_w_bytes: &[u8], + v_w_ggml_type: u32, + v_b: &[f32], + ) -> Option<(Vec, Vec, Vec)> { + with_context(|ctx| { + ctx.decoder_self_qkv_step_f32( + x, + n_state, + attn_ln_w, + attn_ln_b, + q_w_bytes, + q_w_ggml_type, + q_b, + k_w_bytes, + k_w_ggml_type, + v_w_bytes, + v_w_ggml_type, + v_b, + ) + }) + } + + #[allow(clippy::too_many_arguments)] + pub(super) fn try_decoder_cross_ffn_step_f32( + layer_idx: usize, + x: &[f32], + n_state: usize, + n_head: usize, + k_cross: &[f32], + v_cross: &[f32], + n_audio_ctx: usize, + layer: &DecoderLayer, + ) -> Option> { + with_context(|ctx| { + ctx.decoder_cross_ffn_step_f32( + layer_idx, + x, + n_state, + n_head, + k_cross, + v_cross, + n_audio_ctx, + layer, + ) + }) + } + + #[allow(clippy::too_many_arguments)] + pub(super) fn try_decoder_self_cross_ffn_step_f32( + layer_idx: usize, + x: &[f32], + q_self: &[f32], + k_all: &[f32], + v_all: &[f32], + n_kv: usize, + n_state: usize, + n_head: usize, + k_cross: &[f32], + v_cross: &[f32], + n_audio_ctx: usize, + layer: &DecoderLayer, + ) -> Option> { + with_context(|ctx| { + ctx.decoder_self_cross_ffn_step_f32( + layer_idx, + x, + q_self, + k_all, + v_all, + n_kv, + n_state, + n_head, + k_cross, + v_cross, + n_audio_ctx, + layer, + ) + }) + } +} diff --git a/libs/voice2/src/metal/ggml/LICENSE b/libs/voice2/src/metal/ggml/LICENSE new file mode 100644 index 0000000000..3714c93e66 --- /dev/null +++ b/libs/voice2/src/metal/ggml/LICENSE @@ -0,0 +1,23 @@ +Source snapshot commit: 364c77f4ca2737e3287652e0e8a8c6dce3231bba + +MIT License + +Copyright (c) 2023-2026 The ggml authors + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in all +copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +SOFTWARE. diff --git a/libs/voice2/src/metal/ggml/ggml-common.h b/libs/voice2/src/metal/ggml/ggml-common.h new file mode 100644 index 0000000000..9e9b18ffcf --- /dev/null +++ b/libs/voice2/src/metal/ggml/ggml-common.h @@ -0,0 +1,1324 @@ +#ifndef GGML_COMMON_DECL + +#if defined(GGML_COMMON_DECL_C) +#include + +typedef uint16_t ggml_half; +typedef uint32_t ggml_half2; + +#define GGML_COMMON_AGGR_U +#define GGML_COMMON_AGGR_S + +#define GGML_COMMON_DECL +#elif defined(GGML_COMMON_DECL_CPP) +#include + +typedef uint16_t ggml_half; +typedef uint32_t ggml_half2; + +// std-c++ allow anonymous unions but some compiler warn on it +#define GGML_COMMON_AGGR_U data +// std-c++ do not allow it. +#define GGML_COMMON_AGGR_S data + +#define GGML_COMMON_DECL +#elif defined(GGML_COMMON_DECL_METAL) +#include + +typedef half ggml_half; +typedef half2 ggml_half2; + +#define GGML_COMMON_AGGR_U +#define GGML_COMMON_AGGR_S + +#define GGML_COMMON_DECL +#elif defined(GGML_COMMON_DECL_CUDA) +#if defined(GGML_COMMON_DECL_MUSA) +#include +#else +#include +#endif +#include + +typedef half ggml_half; +typedef half2 ggml_half2; + +#define GGML_COMMON_AGGR_U +#define GGML_COMMON_AGGR_S data + +#define GGML_COMMON_DECL +#elif defined(GGML_COMMON_DECL_HIP) +#include +#include + +typedef half ggml_half; +typedef half2 ggml_half2; + +#define GGML_COMMON_AGGR_U +#define GGML_COMMON_AGGR_S data + +#define GGML_COMMON_DECL +#elif defined(GGML_COMMON_DECL_SYCL) +#include +#include + +typedef sycl::half ggml_half; +typedef sycl::half2 ggml_half2; + +#define GGML_COMMON_AGGR_U +#define GGML_COMMON_AGGR_S data + +#define GGML_COMMON_DECL +#endif + +#if defined(GGML_COMMON_DECL) + +#ifndef __cplusplus +#ifndef static_assert +#if defined(__STDC_VERSION__) && (__STDC_VERSION__ >= 201100L) +#define static_assert(cond, msg) _Static_assert(cond, msg) +#else +#define static_assert(cond, msg) struct global_scope_noop_trick +#endif +#endif +#endif // __cplusplus + +// QK = number of values after dequantization +// QK_K = super-block size + +#define QK_K 256 +#define K_SCALE_SIZE 12 + +#if defined(GGML_COMMON_DECL_CUDA) || defined(GGML_COMMON_DECL_HIP) || defined(GGML_COMMON_DECL_SYCL) +// QR = QK / number of values before dequantization +// QI = number of 32 bit integers before dequantization + +#define QI4_0 (QK4_0 / (4 * QR4_0)) +#define QR4_0 2 + +#define QI4_1 (QK4_1 / (4 * QR4_1)) +#define QR4_1 2 + +#define QI_MXFP4 (QK_MXFP4 / (4 * QR_MXFP4)) +#define QR_MXFP4 2 + +#define QI5_0 (QK5_0 / (4 * QR5_0)) +#define QR5_0 2 + +#define QI5_1 (QK5_1 / (4 * QR5_1)) +#define QR5_1 2 + +#define QI8_0 (QK8_0 / (4 * QR8_0)) +#define QR8_0 1 + +#define QI8_1 (QK8_1 / (4 * QR8_1)) +#define QR8_1 1 + +#define QI2_K (QK_K / (4*QR2_K)) +#define QR2_K 4 + +#define QI3_K (QK_K / (4*QR3_K)) +#define QR3_K 4 + +#define QI4_K (QK_K / (4*QR4_K)) +#define QR4_K 2 + +#define QI5_K (QK_K / (4*QR5_K)) +#define QR5_K 2 + +#define QI6_K (QK_K / (4*QR6_K)) +#define QR6_K 2 + +#define QI2_XXS (QK_K / (4*QR2_XXS)) +#define QR2_XXS 4 + +#define QI2_XS (QK_K / (4*QR2_XS)) +#define QR2_XS 4 + +#define QI2_S (QK_K / (4*QR2_S)) +#define QR2_S 4 + +#define QI3_XXS (QK_K / (4*QR3_XXS)) +#define QR3_XXS 4 + +#define QI3_XS (QK_K / (4*QR3_XS)) +#define QR3_XS 4 + +#define QI1_S (QK_K / (4*QR1_S)) +#define QR1_S 8 + +#define QI1_M (QK_K / (4*QR1_M)) +#define QR1_M 8 + +#define QI4_NL (QK4_NL / (4*QR4_NL)) +#define QR4_NL 2 + +#define QI4_XS (QK_K / (4*QR4_XS)) +#define QR4_XS 2 + +#define QI3_S (QK_K / (4*QR3_S)) +#define QR3_S 4 + +#endif // GGML_COMMON_DECL_CUDA || GGML_COMMON_DECL_HIP + +#ifdef _MSC_VER +#define GGML_EXTENSION +#else // _MSC_VER +#define GGML_EXTENSION __extension__ +#endif // _MSC_VER + +#define QK4_0 32 +typedef struct { + ggml_half d; // delta + uint8_t qs[QK4_0 / 2]; // nibbles / quants +} block_q4_0; +static_assert(sizeof(block_q4_0) == sizeof(ggml_half) + QK4_0 / 2, "wrong q4_0 block size/padding"); + +#define QK4_1 32 +typedef struct { + GGML_EXTENSION union { + struct { + ggml_half d; // delta + ggml_half m; // min + } GGML_COMMON_AGGR_S; + ggml_half2 dm; + } GGML_COMMON_AGGR_U; + uint8_t qs[QK4_1 / 2]; // nibbles / quants +} block_q4_1; +static_assert(sizeof(block_q4_1) == 2 * sizeof(ggml_half) + QK4_1 / 2, "wrong q4_1 block size/padding"); + +#define QK_MXFP4 32 +typedef struct { + uint8_t e; // E8M0 + uint8_t qs[QK_MXFP4/2]; +} block_mxfp4; +static_assert(sizeof(block_mxfp4) == sizeof(uint8_t) + QK_MXFP4/2, "wrong mxfp4 block size/padding"); + +#define QK5_0 32 +typedef struct { + ggml_half d; // delta + uint8_t qh[4]; // 5-th bit of quants + uint8_t qs[QK5_0 / 2]; // nibbles / quants +} block_q5_0; +static_assert(sizeof(block_q5_0) == sizeof(ggml_half) + sizeof(uint32_t) + QK5_0 / 2, "wrong q5_0 block size/padding"); + +#define QK5_1 32 +typedef struct { + GGML_EXTENSION union { + struct { + ggml_half d; // delta + ggml_half m; // min + } GGML_COMMON_AGGR_S; + ggml_half2 dm; + } GGML_COMMON_AGGR_U; + uint8_t qh[4]; // 5-th bit of quants + uint8_t qs[QK5_1 / 2]; // nibbles / quants +} block_q5_1; +static_assert(sizeof(block_q5_1) == 2 * sizeof(ggml_half) + sizeof(uint32_t) + QK5_1 / 2, "wrong q5_1 block size/padding"); + +#define QK8_0 32 +typedef struct { + ggml_half d; // delta + int8_t qs[QK8_0]; // quants +} block_q8_0; +static_assert(sizeof(block_q8_0) == sizeof(ggml_half) + QK8_0, "wrong q8_0 block size/padding"); + +#define QK8_1 32 +typedef struct { + GGML_EXTENSION union { + struct { + ggml_half d; // delta + ggml_half s; // d * sum(qs[i]) + } GGML_COMMON_AGGR_S; + ggml_half2 ds; + } GGML_COMMON_AGGR_U; + int8_t qs[QK8_1]; // quants +} block_q8_1; +static_assert(sizeof(block_q8_1) == 2*sizeof(ggml_half) + QK8_1, "wrong q8_1 block size/padding"); + +// +// Ternary quantization +// + +// 1.6875 bpw +typedef struct { + uint8_t qs[(QK_K - 4 * QK_K / 64) / 5]; // 5 elements per byte (3^5 = 243 < 256) + uint8_t qh[QK_K/64]; // 4 elements per byte + ggml_half d; +} block_tq1_0; +static_assert(sizeof(block_tq1_0) == sizeof(ggml_half) + QK_K / 64 + (QK_K - 4 * QK_K / 64) / 5, "wrong tq1_0 block size/padding"); + +// 2.0625 bpw +typedef struct { + uint8_t qs[QK_K/4]; // 2 bits per element + ggml_half d; +} block_tq2_0; +static_assert(sizeof(block_tq2_0) == sizeof(ggml_half) + QK_K / 4, "wrong tq2_0 block size/padding"); + +// +// Super-block quantization structures +// + +// 2-bit quantization +// weight is represented as x = a * q + b +// 16 blocks of 16 elements each +// Effectively 2.625 bits per weight +typedef struct { + uint8_t scales[QK_K/16]; // scales and mins, quantized with 4 bits + uint8_t qs[QK_K/4]; // quants + GGML_EXTENSION union { + struct { + ggml_half d; // super-block scale for quantized scales + ggml_half dmin; // super-block scale for quantized mins + } GGML_COMMON_AGGR_S; + ggml_half2 dm; + } GGML_COMMON_AGGR_U; +} block_q2_K; +static_assert(sizeof(block_q2_K) == 2*sizeof(ggml_half) + QK_K/16 + QK_K/4, "wrong q2_K block size/padding"); + +// 3-bit quantization +// weight is represented as x = a * q +// 16 blocks of 16 elements each +// Effectively 3.4375 bits per weight +typedef struct { + uint8_t hmask[QK_K/8]; // quants - high bit + uint8_t qs[QK_K/4]; // quants - low 2 bits + uint8_t scales[12]; // scales, quantized with 6 bits + ggml_half d; // super-block scale +} block_q3_K; +static_assert(sizeof(block_q3_K) == sizeof(ggml_half) + QK_K / 4 + QK_K / 8 + 12, "wrong q3_K block size/padding"); + +// 4-bit quantization +// 8 blocks of 32 elements each +// weight is represented as x = a * q + b +// Effectively 4.5 bits per weight +typedef struct { + GGML_EXTENSION union { + struct { + ggml_half d; // super-block scale for quantized scales + ggml_half dmin; // super-block scale for quantized mins + } GGML_COMMON_AGGR_S; + ggml_half2 dm; + } GGML_COMMON_AGGR_U; + uint8_t scales[K_SCALE_SIZE]; // scales and mins, quantized with 6 bits + uint8_t qs[QK_K/2]; // 4--bit quants +} block_q4_K; +static_assert(sizeof(block_q4_K) == 2*sizeof(ggml_half) + K_SCALE_SIZE + QK_K/2, "wrong q4_K block size/padding"); + +// 5-bit quantization +// 8 blocks of 32 elements each +// weight is represented as x = a * q + b +// Effectively 5.5 bits per weight +typedef struct { + GGML_EXTENSION union { + struct { + ggml_half d; // super-block scale for quantized scales + ggml_half dmin; // super-block scale for quantized mins + } GGML_COMMON_AGGR_S; + ggml_half2 dm; + } GGML_COMMON_AGGR_U; + uint8_t scales[K_SCALE_SIZE]; // scales and mins, quantized with 6 bits + uint8_t qh[QK_K/8]; // quants, high bit + uint8_t qs[QK_K/2]; // quants, low 4 bits +} block_q5_K; +static_assert(sizeof(block_q5_K) == 2*sizeof(ggml_half) + K_SCALE_SIZE + QK_K/2 + QK_K/8, "wrong q5_K block size/padding"); + +// 6-bit quantization +// weight is represented as x = a * q +// 16 blocks of 16 elements each +// Effectively 6.5625 bits per weight +typedef struct { + uint8_t ql[QK_K/2]; // quants, lower 4 bits + uint8_t qh[QK_K/4]; // quants, upper 2 bits + int8_t scales[QK_K/16]; // scales, quantized with 8 bits + ggml_half d; // super-block scale +} block_q6_K; +static_assert(sizeof(block_q6_K) == sizeof(ggml_half) + QK_K / 16 + 3*QK_K/4, "wrong q6_K block size/padding"); + +// This is only used for intermediate quantization and dot products +typedef struct { + float d; // delta + int8_t qs[QK_K]; // quants + int16_t bsums[QK_K/16]; // sum of quants in groups of 16 +} block_q8_K; +static_assert(sizeof(block_q8_K) == sizeof(float) + QK_K + QK_K/16*sizeof(int16_t), "wrong q8_K block size/padding"); + +// (Almost) "true" 2-bit quantization. +// Due to the need to use blocks as per ggml design, it ends up using +// 2.0625 bpw because of the 16-bit scale for each block of 256. +typedef struct { + ggml_half d; + uint16_t qs[QK_K/8]; +} block_iq2_xxs; +static_assert(sizeof(block_iq2_xxs) == sizeof(ggml_half) + QK_K/8*sizeof(uint16_t), "wrong iq2_xxs block size/padding"); + +// 2.3125 bpw quants +typedef struct { + ggml_half d; + uint16_t qs[QK_K/8]; + uint8_t scales[QK_K/32]; +} block_iq2_xs; +static_assert(sizeof(block_iq2_xs) == sizeof(ggml_half) + QK_K/8*sizeof(uint16_t) + QK_K/32, "wrong iq2_xs block size/padding"); + +// 2.5625 bpw quants +typedef struct { + ggml_half d; + uint8_t qs[QK_K/4]; + uint8_t qh[QK_K/32]; + uint8_t scales[QK_K/32]; +} block_iq2_s; +static_assert(sizeof(block_iq2_s) == sizeof(ggml_half) + QK_K/4 + QK_K/16, "wrong iq2_s block size/padding"); + +// (Almost) "true" 3-bit quantization. +// Due to the need to use blocks as per ggml design, it ends up using +// 3.0625 bpw because of the 16-bit scale for each block of 256. +typedef struct { + ggml_half d; + uint8_t qs[3*QK_K/8]; +} block_iq3_xxs; +static_assert(sizeof(block_iq3_xxs) == sizeof(ggml_half) + 3*(QK_K/8), "wrong iq3_xxs block size/padding"); + +// 3.4375 bpw +#define IQ3S_N_SCALE QK_K/64 +typedef struct { + ggml_half d; + uint8_t qs[QK_K/4]; + uint8_t qh[QK_K/32]; + uint8_t signs[QK_K/8]; + uint8_t scales[IQ3S_N_SCALE]; +} block_iq3_s; +static_assert(sizeof(block_iq3_s) == sizeof(ggml_half) + 13*(QK_K/32) + IQ3S_N_SCALE, "wrong iq3_s block size/padding"); + +// 1.5625 bpw +typedef struct { + ggml_half d; + uint8_t qs[QK_K/8]; + uint16_t qh[QK_K/32]; +} block_iq1_s; +static_assert(sizeof(block_iq1_s) == sizeof(ggml_half) + QK_K/8 + QK_K/16, "wrong iq1_s block size/padding"); + +// 1.75 bpw +typedef struct { + uint8_t qs[QK_K/8]; // grid index, low 8 bits + uint8_t qh[QK_K/16]; // grid index, high 3 bits + grid shift bit (for two groups of 8) + uint8_t scales[QK_K/32]; // 3-bit block scales (4-bit if QK_K == 64) +} block_iq1_m; +static_assert(sizeof(block_iq1_m) == QK_K/8 + QK_K/16 + QK_K/32, "wrong iq1_m block size/padding"); + +// Used by IQ1_M quants +typedef union { + ggml_half f16; + uint16_t u16; +} iq1m_scale_t; + +// Non-linear quants +#define QK4_NL 32 +typedef struct { + ggml_half d; + uint8_t qs[QK4_NL/2]; +} block_iq4_nl; +static_assert(sizeof(block_iq4_nl) == sizeof(ggml_half) + QK4_NL/2, "wrong iq4_nl block size/padding"); + +typedef struct { + ggml_half d; + uint16_t scales_h; + uint8_t scales_l[QK_K/64]; + uint8_t qs[QK_K/2]; +} block_iq4_xs; +static_assert(sizeof(block_iq4_xs) == sizeof(ggml_half) + sizeof(uint16_t) + QK_K/64 + QK_K/2, "wrong iq4_xs block size/padding"); + +#endif // GGML_COMMON_DECL +#endif // GGML_COMMON_DECL + +//////////////////////////////////////////////////////////////////////////////// + +#ifndef GGML_COMMON_IMPL + +#if defined(GGML_COMMON_IMPL_C) +#include + +#define GGML_TABLE_BEGIN(type, name, size) static const type name[size] = { +#define GGML_TABLE_END() }; + +#define GGML_COMMON_IMPL +#elif defined(GGML_COMMON_IMPL_CPP) +#include + +#define GGML_TABLE_BEGIN(type, name, size) static const type name[size] = { +#define GGML_TABLE_END() }; + +#define GGML_COMMON_IMPL +#elif defined(GGML_COMMON_IMPL_METAL) +#include + +#define GGML_TABLE_BEGIN(type, name, size) static const constant type name[size] = { +#define GGML_TABLE_END() }; + +#define GGML_COMMON_IMPL +#elif defined(GGML_COMMON_IMPL_CUDA) || defined(GGML_COMMON_IMPL_HIP) || defined(GGML_COMMON_IMPL_MUSA) +#include + +#define GGML_TABLE_BEGIN(type, name, size) static const __device__ type name[size] = { +#define GGML_TABLE_END() }; + +#define GGML_COMMON_IMPL +#elif defined(GGML_COMMON_IMPL_SYCL) + +#include + +#define GGML_TABLE_BEGIN(type, name, size) static const type name[size] = { +#define GGML_TABLE_END() }; + +#define GGML_COMMON_IMPL +#endif + +#if defined(GGML_COMMON_IMPL) + +GGML_TABLE_BEGIN(uint8_t, kmask_iq2xs, 8) + 1, 2, 4, 8, 16, 32, 64, 128 +GGML_TABLE_END() + +GGML_TABLE_BEGIN(uint8_t, ksigns_iq2xs, 128) + 0, 129, 130, 3, 132, 5, 6, 135, 136, 9, 10, 139, 12, 141, 142, 15, + 144, 17, 18, 147, 20, 149, 150, 23, 24, 153, 154, 27, 156, 29, 30, 159, + 160, 33, 34, 163, 36, 165, 166, 39, 40, 169, 170, 43, 172, 45, 46, 175, + 48, 177, 178, 51, 180, 53, 54, 183, 184, 57, 58, 187, 60, 189, 190, 63, + 192, 65, 66, 195, 68, 197, 198, 71, 72, 201, 202, 75, 204, 77, 78, 207, + 80, 209, 210, 83, 212, 85, 86, 215, 216, 89, 90, 219, 92, 221, 222, 95, + 96, 225, 226, 99, 228, 101, 102, 231, 232, 105, 106, 235, 108, 237, 238, 111, + 240, 113, 114, 243, 116, 245, 246, 119, 120, 249, 250, 123, 252, 125, 126, 255, +GGML_TABLE_END() + + + +GGML_TABLE_BEGIN(uint64_t, iq2xxs_grid, 256) + 0x0808080808080808, 0x080808080808082b, 0x0808080808081919, 0x0808080808082b08, + 0x0808080808082b2b, 0x0808080808190819, 0x0808080808191908, 0x08080808082b0808, + 0x08080808082b082b, 0x08080808082b2b08, 0x08080808082b2b2b, 0x0808080819080819, + 0x0808080819081908, 0x0808080819190808, 0x0808080819192b08, 0x08080808192b0819, + 0x08080808192b1908, 0x080808082b080808, 0x080808082b08082b, 0x080808082b082b2b, + 0x080808082b2b082b, 0x0808081908080819, 0x0808081908081908, 0x0808081908190808, + 0x0808081908191919, 0x0808081919080808, 0x080808192b081908, 0x080808192b192b08, + 0x0808082b08080808, 0x0808082b0808082b, 0x0808082b082b082b, 0x0808082b2b08082b, + 0x0808190808080819, 0x0808190808081908, 0x0808190808190808, 0x08081908082b0819, + 0x08081908082b1908, 0x0808190819080808, 0x080819081908082b, 0x0808190819082b08, + 0x08081908192b0808, 0x080819082b080819, 0x080819082b081908, 0x080819082b190808, + 0x080819082b2b1908, 0x0808191908080808, 0x080819190808082b, 0x0808191908082b08, + 0x08081919082b0808, 0x080819191908192b, 0x08081919192b2b19, 0x080819192b080808, + 0x080819192b190819, 0x0808192b08082b19, 0x0808192b08190808, 0x0808192b19080808, + 0x0808192b2b081908, 0x0808192b2b2b1908, 0x08082b0808080808, 0x08082b0808081919, + 0x08082b0808082b08, 0x08082b0808191908, 0x08082b08082b2b08, 0x08082b0819080819, + 0x08082b0819081908, 0x08082b0819190808, 0x08082b081919082b, 0x08082b082b082b08, + 0x08082b1908081908, 0x08082b1919080808, 0x08082b2b0808082b, 0x08082b2b08191908, + 0x0819080808080819, 0x0819080808081908, 0x0819080808190808, 0x08190808082b0819, + 0x0819080819080808, 0x08190808192b0808, 0x081908082b081908, 0x081908082b190808, + 0x081908082b191919, 0x0819081908080808, 0x0819081908082b08, 0x08190819082b0808, + 0x0819081919190808, 0x0819081919192b2b, 0x081908192b080808, 0x0819082b082b1908, + 0x0819082b19081919, 0x0819190808080808, 0x0819190808082b08, 0x08191908082b0808, + 0x08191908082b1919, 0x0819190819082b19, 0x081919082b080808, 0x0819191908192b08, + 0x08191919192b082b, 0x0819192b08080808, 0x0819192b0819192b, 0x08192b0808080819, + 0x08192b0808081908, 0x08192b0808190808, 0x08192b0819080808, 0x08192b082b080819, + 0x08192b1908080808, 0x08192b1908081919, 0x08192b192b2b0808, 0x08192b2b19190819, + 0x082b080808080808, 0x082b08080808082b, 0x082b080808082b2b, 0x082b080819081908, + 0x082b0808192b0819, 0x082b08082b080808, 0x082b08082b08082b, 0x082b0819082b2b19, + 0x082b081919082b08, 0x082b082b08080808, 0x082b082b0808082b, 0x082b190808080819, + 0x082b190808081908, 0x082b190808190808, 0x082b190819080808, 0x082b19081919192b, + 0x082b191908080808, 0x082b191919080819, 0x082b1919192b1908, 0x082b192b2b190808, + 0x082b2b0808082b08, 0x082b2b08082b0808, 0x082b2b082b191908, 0x082b2b2b19081908, + 0x1908080808080819, 0x1908080808081908, 0x1908080808190808, 0x1908080808192b08, + 0x19080808082b0819, 0x19080808082b1908, 0x1908080819080808, 0x1908080819082b08, + 0x190808081919192b, 0x19080808192b0808, 0x190808082b080819, 0x190808082b081908, + 0x190808082b190808, 0x1908081908080808, 0x19080819082b0808, 0x19080819192b0819, + 0x190808192b080808, 0x190808192b081919, 0x1908082b08080819, 0x1908082b08190808, + 0x1908082b19082b08, 0x1908082b1919192b, 0x1908082b192b2b08, 0x1908190808080808, + 0x1908190808082b08, 0x19081908082b0808, 0x190819082b080808, 0x190819082b192b19, + 0x190819190819082b, 0x19081919082b1908, 0x1908192b08080808, 0x19082b0808080819, + 0x19082b0808081908, 0x19082b0808190808, 0x19082b0819080808, 0x19082b0819081919, + 0x19082b1908080808, 0x19082b1919192b08, 0x19082b19192b0819, 0x19082b192b08082b, + 0x19082b2b19081919, 0x19082b2b2b190808, 0x1919080808080808, 0x1919080808082b08, + 0x1919080808190819, 0x1919080808192b19, 0x19190808082b0808, 0x191908082b080808, + 0x191908082b082b08, 0x1919081908081908, 0x191908191908082b, 0x191908192b2b1908, + 0x1919082b2b190819, 0x191919082b190808, 0x191919082b19082b, 0x1919191908082b2b, + 0x1919192b08080819, 0x1919192b19191908, 0x19192b0808080808, 0x19192b0808190819, + 0x19192b0808192b19, 0x19192b08192b1908, 0x19192b1919080808, 0x19192b2b08082b08, + 0x192b080808081908, 0x192b080808190808, 0x192b080819080808, 0x192b0808192b2b08, + 0x192b081908080808, 0x192b081919191919, 0x192b082b08192b08, 0x192b082b192b0808, + 0x192b190808080808, 0x192b190808081919, 0x192b191908190808, 0x192b19190819082b, + 0x192b19192b081908, 0x192b2b081908082b, 0x2b08080808080808, 0x2b0808080808082b, + 0x2b08080808082b2b, 0x2b08080819080819, 0x2b0808082b08082b, 0x2b08081908081908, + 0x2b08081908192b08, 0x2b08081919080808, 0x2b08082b08190819, 0x2b08190808080819, + 0x2b08190808081908, 0x2b08190808190808, 0x2b08190808191919, 0x2b08190819080808, + 0x2b081908192b0808, 0x2b08191908080808, 0x2b0819191908192b, 0x2b0819192b191908, + 0x2b08192b08082b19, 0x2b08192b19080808, 0x2b08192b192b0808, 0x2b082b080808082b, + 0x2b082b1908081908, 0x2b082b2b08190819, 0x2b19080808081908, 0x2b19080808190808, + 0x2b190808082b1908, 0x2b19080819080808, 0x2b1908082b2b0819, 0x2b1908190819192b, + 0x2b1908192b080808, 0x2b19082b19081919, 0x2b19190808080808, 0x2b191908082b082b, + 0x2b19190819081908, 0x2b19191919190819, 0x2b192b082b080819, 0x2b192b19082b0808, + 0x2b2b08080808082b, 0x2b2b080819190808, 0x2b2b08082b081919, 0x2b2b081908082b19, + 0x2b2b082b08080808, 0x2b2b190808192b08, 0x2b2b2b0819190808, 0x2b2b2b1908081908, +GGML_TABLE_END() + +GGML_TABLE_BEGIN(uint64_t, iq2xs_grid, 512) + 0x0808080808080808, 0x080808080808082b, 0x0808080808081919, 0x0808080808082b08, + 0x0808080808082b2b, 0x0808080808190819, 0x0808080808191908, 0x080808080819192b, + 0x0808080808192b19, 0x08080808082b0808, 0x08080808082b082b, 0x08080808082b1919, + 0x08080808082b2b08, 0x0808080819080819, 0x0808080819081908, 0x080808081908192b, + 0x0808080819082b19, 0x0808080819190808, 0x080808081919082b, 0x0808080819191919, + 0x0808080819192b08, 0x08080808192b0819, 0x08080808192b1908, 0x080808082b080808, + 0x080808082b08082b, 0x080808082b081919, 0x080808082b082b08, 0x080808082b190819, + 0x080808082b191908, 0x080808082b192b19, 0x080808082b2b0808, 0x0808081908080819, + 0x0808081908081908, 0x080808190808192b, 0x0808081908082b19, 0x0808081908190808, + 0x080808190819082b, 0x0808081908191919, 0x0808081908192b08, 0x0808081908192b2b, + 0x08080819082b0819, 0x08080819082b1908, 0x0808081919080808, 0x080808191908082b, + 0x0808081919081919, 0x0808081919082b08, 0x0808081919190819, 0x0808081919191908, + 0x08080819192b0808, 0x08080819192b2b08, 0x080808192b080819, 0x080808192b081908, + 0x080808192b190808, 0x0808082b08080808, 0x0808082b0808082b, 0x0808082b08081919, + 0x0808082b08082b08, 0x0808082b08190819, 0x0808082b08191908, 0x0808082b082b0808, + 0x0808082b19080819, 0x0808082b19081908, 0x0808082b19190808, 0x0808082b19191919, + 0x0808082b2b080808, 0x0808082b2b082b2b, 0x0808190808080819, 0x0808190808081908, + 0x080819080808192b, 0x0808190808082b19, 0x0808190808190808, 0x080819080819082b, + 0x0808190808191919, 0x0808190808192b08, 0x08081908082b0819, 0x08081908082b1908, + 0x0808190819080808, 0x080819081908082b, 0x0808190819081919, 0x0808190819082b08, + 0x0808190819190819, 0x0808190819191908, 0x080819081919192b, 0x08081908192b0808, + 0x080819082b080819, 0x080819082b081908, 0x080819082b190808, 0x0808191908080808, + 0x080819190808082b, 0x0808191908081919, 0x0808191908082b08, 0x0808191908190819, + 0x0808191908191908, 0x08081919082b0808, 0x0808191919080819, 0x0808191919081908, + 0x0808191919190808, 0x08081919192b0819, 0x080819192b080808, 0x0808192b08080819, + 0x0808192b08081908, 0x0808192b08190808, 0x0808192b082b192b, 0x0808192b19080808, + 0x0808192b1908082b, 0x0808192b2b081908, 0x08082b0808080808, 0x08082b080808082b, + 0x08082b0808081919, 0x08082b0808082b08, 0x08082b0808082b2b, 0x08082b0808190819, + 0x08082b0808191908, 0x08082b08082b0808, 0x08082b08082b1919, 0x08082b0819080819, + 0x08082b0819081908, 0x08082b0819190808, 0x08082b0819192b08, 0x08082b082b080808, + 0x08082b082b2b0808, 0x08082b082b2b2b2b, 0x08082b1908080819, 0x08082b1908081908, + 0x08082b1908190808, 0x08082b1919080808, 0x08082b192b080819, 0x08082b192b082b19, + 0x08082b2b08080808, 0x08082b2b082b0808, 0x08082b2b082b2b08, 0x08082b2b2b19192b, + 0x08082b2b2b2b0808, 0x0819080808080819, 0x0819080808081908, 0x081908080808192b, + 0x0819080808082b19, 0x0819080808190808, 0x081908080819082b, 0x0819080808191919, + 0x0819080808192b08, 0x08190808082b0819, 0x08190808082b1908, 0x0819080819080808, + 0x081908081908082b, 0x0819080819081919, 0x0819080819082b08, 0x0819080819190819, + 0x0819080819191908, 0x08190808192b0808, 0x08190808192b2b2b, 0x081908082b080819, + 0x081908082b081908, 0x081908082b190808, 0x0819081908080808, 0x081908190808082b, + 0x0819081908081919, 0x0819081908082b08, 0x0819081908190819, 0x0819081908191908, + 0x08190819082b0808, 0x0819081919080819, 0x0819081919081908, 0x0819081919190808, + 0x081908192b080808, 0x081908192b191908, 0x081908192b19192b, 0x0819082b08080819, + 0x0819082b08081908, 0x0819082b0808192b, 0x0819082b08190808, 0x0819082b19080808, + 0x0819082b192b0808, 0x0819190808080808, 0x081919080808082b, 0x0819190808081919, + 0x0819190808082b08, 0x0819190808190819, 0x0819190808191908, 0x08191908082b0808, + 0x0819190819080819, 0x0819190819081908, 0x0819190819082b19, 0x0819190819190808, + 0x08191908192b1908, 0x081919082b080808, 0x0819191908080819, 0x0819191908081908, + 0x0819191908190808, 0x0819191919080808, 0x0819192b08080808, 0x0819192b08191908, + 0x0819192b19082b19, 0x08192b0808080819, 0x08192b0808081908, 0x08192b0808190808, + 0x08192b080819082b, 0x08192b0819080808, 0x08192b0819191908, 0x08192b082b08192b, + 0x08192b1908080808, 0x08192b1908081919, 0x08192b19192b192b, 0x08192b2b19190819, + 0x08192b2b2b2b2b19, 0x082b080808080808, 0x082b08080808082b, 0x082b080808081919, + 0x082b080808082b08, 0x082b080808082b2b, 0x082b080808190819, 0x082b080808191908, + 0x082b0808082b0808, 0x082b080819080819, 0x082b080819081908, 0x082b080819190808, + 0x082b08082b080808, 0x082b08082b2b0808, 0x082b081908080819, 0x082b081908081908, + 0x082b081908190808, 0x082b081919080808, 0x082b081919082b08, 0x082b0819192b1919, + 0x082b082b08080808, 0x082b082b082b082b, 0x082b082b2b080808, 0x082b082b2b2b2b08, + 0x082b190808080819, 0x082b190808081908, 0x082b190808190808, 0x082b1908082b2b19, + 0x082b190819080808, 0x082b191908080808, 0x082b191919080819, 0x082b19191919082b, + 0x082b19192b192b19, 0x082b192b08080819, 0x082b192b08192b2b, 0x082b192b2b2b192b, + 0x082b2b0808080808, 0x082b2b0808082b08, 0x082b2b0808082b2b, 0x082b2b08082b0808, + 0x082b2b0819191919, 0x082b2b082b082b08, 0x082b2b082b2b082b, 0x082b2b19192b2b08, + 0x082b2b192b190808, 0x082b2b2b08082b08, 0x082b2b2b082b0808, 0x082b2b2b2b08082b, + 0x082b2b2b2b082b08, 0x082b2b2b2b082b2b, 0x1908080808080819, 0x1908080808081908, + 0x190808080808192b, 0x1908080808082b19, 0x1908080808190808, 0x190808080819082b, + 0x1908080808191919, 0x1908080808192b08, 0x19080808082b0819, 0x19080808082b1908, + 0x1908080819080808, 0x190808081908082b, 0x1908080819081919, 0x1908080819082b08, + 0x1908080819082b2b, 0x1908080819190819, 0x1908080819191908, 0x19080808192b0808, + 0x19080808192b1919, 0x190808082b080819, 0x190808082b081908, 0x190808082b190808, + 0x1908081908080808, 0x190808190808082b, 0x1908081908081919, 0x1908081908082b08, + 0x1908081908190819, 0x1908081908191908, 0x19080819082b0808, 0x1908081919080819, + 0x1908081919081908, 0x1908081919190808, 0x190808192b080808, 0x190808192b081919, + 0x190808192b2b082b, 0x1908082b08080819, 0x1908082b08081908, 0x1908082b08190808, + 0x1908082b0819082b, 0x1908082b082b2b19, 0x1908082b19080808, 0x1908190808080808, + 0x190819080808082b, 0x1908190808081919, 0x1908190808082b08, 0x1908190808190819, + 0x1908190808191908, 0x1908190808192b19, 0x19081908082b0808, 0x1908190819080819, + 0x1908190819081908, 0x1908190819190808, 0x190819082b080808, 0x190819082b191908, + 0x1908191908080819, 0x1908191908081908, 0x1908191908190808, 0x19081919082b1908, + 0x1908191919080808, 0x190819192b192b2b, 0x1908192b08080808, 0x1908192b08082b2b, + 0x1908192b19081908, 0x1908192b19190808, 0x19082b0808080819, 0x19082b0808081908, + 0x19082b0808190808, 0x19082b0819080808, 0x19082b0819081919, 0x19082b0819191908, + 0x19082b08192b082b, 0x19082b1908080808, 0x19082b1908190819, 0x19082b1919081908, + 0x19082b1919190808, 0x19082b19192b2b19, 0x19082b2b08081908, 0x1919080808080808, + 0x191908080808082b, 0x1919080808081919, 0x1919080808082b08, 0x1919080808190819, + 0x1919080808191908, 0x19190808082b0808, 0x19190808082b2b08, 0x1919080819080819, + 0x1919080819081908, 0x1919080819190808, 0x191908082b080808, 0x1919081908080819, + 0x1919081908081908, 0x1919081908190808, 0x1919081908191919, 0x1919081919080808, + 0x191908191908082b, 0x1919082b08080808, 0x1919082b19081908, 0x1919082b2b2b2b2b, + 0x1919190808080819, 0x1919190808081908, 0x1919190808190808, 0x19191908082b0819, + 0x1919190819080808, 0x19191908192b0808, 0x191919082b080819, 0x191919082b2b0819, + 0x1919191908080808, 0x1919191908082b08, 0x191919192b080808, 0x191919192b082b08, + 0x1919192b082b0819, 0x1919192b192b2b08, 0x1919192b2b2b0819, 0x19192b0808080808, + 0x19192b0808191908, 0x19192b0819080819, 0x19192b0819190808, 0x19192b082b192b19, + 0x19192b1908192b2b, 0x19192b1919080808, 0x19192b191908082b, 0x19192b2b2b081919, + 0x192b080808080819, 0x192b080808081908, 0x192b080808190808, 0x192b080819080808, + 0x192b080819191908, 0x192b0808192b082b, 0x192b08082b08192b, 0x192b08082b2b2b19, + 0x192b081908080808, 0x192b082b082b1908, 0x192b082b19082b2b, 0x192b082b2b19082b, + 0x192b190808080808, 0x192b19080819192b, 0x192b191908190808, 0x192b191919080808, + 0x192b191919081919, 0x192b19192b2b1908, 0x192b2b0808080819, 0x192b2b08192b2b2b, + 0x192b2b19082b1919, 0x192b2b2b0808192b, 0x192b2b2b19191908, 0x192b2b2b192b082b, + 0x2b08080808080808, 0x2b0808080808082b, 0x2b08080808081919, 0x2b08080808082b08, + 0x2b08080808190819, 0x2b08080808191908, 0x2b080808082b0808, 0x2b080808082b2b2b, + 0x2b08080819080819, 0x2b08080819081908, 0x2b08080819190808, 0x2b0808082b080808, + 0x2b0808082b08082b, 0x2b0808082b2b2b08, 0x2b0808082b2b2b2b, 0x2b08081908080819, + 0x2b08081908081908, 0x2b0808190808192b, 0x2b08081908190808, 0x2b08081919080808, + 0x2b08081919190819, 0x2b08081919192b19, 0x2b08082b08080808, 0x2b08082b082b0808, + 0x2b08082b2b080808, 0x2b08082b2b08082b, 0x2b08082b2b2b0808, 0x2b08082b2b2b2b08, + 0x2b08190808080819, 0x2b08190808081908, 0x2b08190808190808, 0x2b0819080819082b, + 0x2b08190808191919, 0x2b08190819080808, 0x2b081908192b0808, 0x2b0819082b082b19, + 0x2b08191908080808, 0x2b08191919081908, 0x2b0819192b2b1919, 0x2b08192b08192b08, + 0x2b08192b192b2b2b, 0x2b082b0808080808, 0x2b082b0808082b08, 0x2b082b08082b1919, + 0x2b082b0819192b2b, 0x2b082b082b080808, 0x2b082b082b08082b, 0x2b082b082b2b2b08, + 0x2b082b190808192b, 0x2b082b2b082b082b, 0x2b082b2b2b080808, 0x2b082b2b2b082b08, + 0x2b082b2b2b19192b, 0x2b082b2b2b2b2b08, 0x2b19080808080819, 0x2b19080808081908, + 0x2b19080808190808, 0x2b19080819080808, 0x2b1908081919192b, 0x2b1908082b081908, + 0x2b19081908080808, 0x2b190819082b082b, 0x2b190819192b1908, 0x2b19082b1919192b, + 0x2b19082b2b082b19, 0x2b19190808080808, 0x2b19190808081919, 0x2b19190819081908, + 0x2b19190819190808, 0x2b19190819192b08, 0x2b191919082b2b19, 0x2b1919192b190808, + 0x2b1919192b19082b, 0x2b19192b19080819, 0x2b192b0819190819, 0x2b192b082b2b192b, + 0x2b192b1919082b19, 0x2b192b2b08191919, 0x2b192b2b192b0808, 0x2b2b080808080808, + 0x2b2b08080808082b, 0x2b2b080808082b08, 0x2b2b080808082b2b, 0x2b2b0808082b0808, + 0x2b2b0808082b2b2b, 0x2b2b08082b2b0808, 0x2b2b081919190819, 0x2b2b081919192b19, + 0x2b2b08192b2b192b, 0x2b2b082b08080808, 0x2b2b082b0808082b, 0x2b2b082b08082b08, + 0x2b2b082b082b2b2b, 0x2b2b082b2b080808, 0x2b2b082b2b2b0808, 0x2b2b190819080808, + 0x2b2b19082b191919, 0x2b2b192b192b1919, 0x2b2b192b2b192b08, 0x2b2b2b0808082b2b, + 0x2b2b2b08082b0808, 0x2b2b2b08082b082b, 0x2b2b2b08082b2b08, 0x2b2b2b082b2b0808, + 0x2b2b2b082b2b2b08, 0x2b2b2b1908081908, 0x2b2b2b192b081908, 0x2b2b2b192b08192b, + 0x2b2b2b2b082b2b08, 0x2b2b2b2b082b2b2b, 0x2b2b2b2b2b190819, 0x2b2b2b2b2b2b2b2b, +GGML_TABLE_END() + +GGML_TABLE_BEGIN(uint64_t, iq2s_grid, 1024) + 0x0808080808080808, 0x080808080808082b, 0x0808080808081919, 0x0808080808082b08, + 0x0808080808082b2b, 0x0808080808190819, 0x0808080808191908, 0x080808080819192b, + 0x0808080808192b19, 0x08080808082b0808, 0x08080808082b082b, 0x08080808082b1919, + 0x08080808082b2b08, 0x0808080819080819, 0x0808080819081908, 0x080808081908192b, + 0x0808080819082b19, 0x0808080819190808, 0x080808081919082b, 0x0808080819191919, + 0x0808080819192b08, 0x08080808192b0819, 0x08080808192b1908, 0x08080808192b192b, + 0x08080808192b2b19, 0x080808082b080808, 0x080808082b08082b, 0x080808082b081919, + 0x080808082b082b08, 0x080808082b190819, 0x080808082b191908, 0x080808082b2b0808, + 0x080808082b2b1919, 0x080808082b2b2b2b, 0x0808081908080819, 0x0808081908081908, + 0x080808190808192b, 0x0808081908082b19, 0x0808081908190808, 0x080808190819082b, + 0x0808081908191919, 0x0808081908192b08, 0x08080819082b0819, 0x08080819082b1908, + 0x0808081919080808, 0x080808191908082b, 0x0808081919081919, 0x0808081919082b08, + 0x0808081919190819, 0x0808081919191908, 0x080808191919192b, 0x0808081919192b19, + 0x08080819192b0808, 0x08080819192b1919, 0x08080819192b2b08, 0x080808192b080819, + 0x080808192b081908, 0x080808192b190808, 0x080808192b19082b, 0x080808192b191919, + 0x080808192b2b0819, 0x080808192b2b1908, 0x0808082b08080808, 0x0808082b0808082b, + 0x0808082b08081919, 0x0808082b08082b08, 0x0808082b08190819, 0x0808082b08191908, + 0x0808082b082b0808, 0x0808082b082b2b2b, 0x0808082b19080819, 0x0808082b19081908, + 0x0808082b1908192b, 0x0808082b19082b19, 0x0808082b19190808, 0x0808082b19191919, + 0x0808082b2b080808, 0x0808082b2b081919, 0x0808082b2b082b2b, 0x0808082b2b191908, + 0x0808082b2b2b082b, 0x0808190808080819, 0x0808190808081908, 0x080819080808192b, + 0x0808190808082b19, 0x0808190808190808, 0x080819080819082b, 0x0808190808191919, + 0x0808190808192b08, 0x08081908082b0819, 0x08081908082b1908, 0x08081908082b192b, + 0x08081908082b2b19, 0x0808190819080808, 0x080819081908082b, 0x0808190819081919, + 0x0808190819082b08, 0x0808190819082b2b, 0x0808190819190819, 0x0808190819191908, + 0x080819081919192b, 0x0808190819192b19, 0x08081908192b0808, 0x08081908192b082b, + 0x08081908192b1919, 0x080819082b080819, 0x080819082b081908, 0x080819082b08192b, + 0x080819082b082b19, 0x080819082b190808, 0x080819082b191919, 0x080819082b192b08, + 0x080819082b2b0819, 0x080819082b2b1908, 0x0808191908080808, 0x080819190808082b, + 0x0808191908081919, 0x0808191908082b08, 0x0808191908082b2b, 0x0808191908190819, + 0x0808191908191908, 0x080819190819192b, 0x0808191908192b19, 0x08081919082b0808, + 0x08081919082b1919, 0x08081919082b2b08, 0x0808191919080819, 0x0808191919081908, + 0x080819191908192b, 0x0808191919082b19, 0x0808191919190808, 0x080819191919082b, + 0x0808191919191919, 0x0808191919192b08, 0x08081919192b0819, 0x08081919192b1908, + 0x080819192b080808, 0x080819192b08082b, 0x080819192b081919, 0x080819192b082b08, + 0x080819192b190819, 0x080819192b191908, 0x080819192b2b0808, 0x0808192b08080819, + 0x0808192b08081908, 0x0808192b0808192b, 0x0808192b08082b19, 0x0808192b08190808, + 0x0808192b08191919, 0x0808192b19080808, 0x0808192b19081919, 0x0808192b19082b08, + 0x0808192b19190819, 0x0808192b19191908, 0x0808192b192b0808, 0x0808192b2b080819, + 0x0808192b2b081908, 0x0808192b2b190808, 0x08082b0808080808, 0x08082b080808082b, + 0x08082b0808081919, 0x08082b0808082b08, 0x08082b0808190819, 0x08082b0808191908, + 0x08082b080819192b, 0x08082b0808192b19, 0x08082b08082b0808, 0x08082b08082b1919, + 0x08082b08082b2b2b, 0x08082b0819080819, 0x08082b0819081908, 0x08082b081908192b, + 0x08082b0819082b19, 0x08082b0819190808, 0x08082b081919082b, 0x08082b0819191919, + 0x08082b0819192b08, 0x08082b08192b0819, 0x08082b08192b1908, 0x08082b082b080808, + 0x08082b082b081919, 0x08082b082b191908, 0x08082b082b2b2b2b, 0x08082b1908080819, + 0x08082b1908081908, 0x08082b1908190808, 0x08082b190819082b, 0x08082b1908191919, + 0x08082b1908192b08, 0x08082b19082b0819, 0x08082b1919080808, 0x08082b1919081919, + 0x08082b1919082b08, 0x08082b1919190819, 0x08082b1919191908, 0x08082b19192b0808, + 0x08082b192b080819, 0x08082b192b190808, 0x08082b2b08080808, 0x08082b2b08190819, + 0x08082b2b08191908, 0x08082b2b082b082b, 0x08082b2b082b2b08, 0x08082b2b082b2b2b, + 0x08082b2b19190808, 0x08082b2b2b192b19, 0x0819080808080819, 0x0819080808081908, + 0x081908080808192b, 0x0819080808082b19, 0x0819080808190808, 0x081908080819082b, + 0x0819080808191919, 0x0819080808192b08, 0x08190808082b0819, 0x08190808082b1908, + 0x08190808082b192b, 0x0819080819080808, 0x081908081908082b, 0x0819080819081919, + 0x0819080819082b08, 0x0819080819190819, 0x0819080819191908, 0x081908081919192b, + 0x0819080819192b19, 0x08190808192b0808, 0x08190808192b082b, 0x08190808192b1919, + 0x08190808192b2b08, 0x081908082b080819, 0x081908082b081908, 0x081908082b08192b, + 0x081908082b190808, 0x081908082b191919, 0x081908082b192b08, 0x081908082b2b0819, + 0x081908082b2b1908, 0x0819081908080808, 0x081908190808082b, 0x0819081908081919, + 0x0819081908082b08, 0x0819081908082b2b, 0x0819081908190819, 0x0819081908191908, + 0x081908190819192b, 0x0819081908192b19, 0x08190819082b0808, 0x08190819082b082b, + 0x08190819082b1919, 0x08190819082b2b08, 0x0819081919080819, 0x0819081919081908, + 0x081908191908192b, 0x0819081919082b19, 0x0819081919190808, 0x081908191919082b, + 0x0819081919191919, 0x0819081919192b08, 0x08190819192b0819, 0x08190819192b1908, + 0x081908192b080808, 0x081908192b08082b, 0x081908192b081919, 0x081908192b082b08, + 0x081908192b190819, 0x081908192b191908, 0x0819082b08080819, 0x0819082b08081908, + 0x0819082b08082b19, 0x0819082b08190808, 0x0819082b08191919, 0x0819082b082b0819, + 0x0819082b082b1908, 0x0819082b19080808, 0x0819082b19081919, 0x0819082b19190819, + 0x0819082b19191908, 0x0819082b2b080819, 0x0819082b2b081908, 0x0819082b2b190808, + 0x0819190808080808, 0x081919080808082b, 0x0819190808081919, 0x0819190808082b08, + 0x0819190808190819, 0x0819190808191908, 0x081919080819192b, 0x0819190808192b19, + 0x08191908082b0808, 0x08191908082b1919, 0x08191908082b2b08, 0x0819190819080819, + 0x0819190819081908, 0x081919081908192b, 0x0819190819082b19, 0x0819190819190808, + 0x081919081919082b, 0x0819190819191919, 0x0819190819192b08, 0x08191908192b0819, + 0x08191908192b1908, 0x081919082b080808, 0x081919082b08082b, 0x081919082b081919, + 0x081919082b082b08, 0x081919082b190819, 0x081919082b191908, 0x081919082b2b0808, + 0x0819191908080819, 0x0819191908081908, 0x081919190808192b, 0x0819191908082b19, + 0x0819191908190808, 0x081919190819082b, 0x0819191908191919, 0x0819191908192b08, + 0x08191919082b0819, 0x08191919082b1908, 0x0819191919080808, 0x081919191908082b, + 0x0819191919081919, 0x0819191919082b08, 0x0819191919190819, 0x0819191919191908, + 0x08191919192b0808, 0x081919192b080819, 0x081919192b081908, 0x081919192b190808, + 0x0819192b08080808, 0x0819192b08081919, 0x0819192b08082b08, 0x0819192b08190819, + 0x0819192b08191908, 0x0819192b082b0808, 0x0819192b19080819, 0x0819192b19081908, + 0x0819192b19190808, 0x0819192b2b080808, 0x0819192b2b2b2b2b, 0x08192b0808080819, + 0x08192b0808081908, 0x08192b080808192b, 0x08192b0808082b19, 0x08192b0808190808, + 0x08192b0808191919, 0x08192b0808192b08, 0x08192b08082b0819, 0x08192b0819080808, + 0x08192b081908082b, 0x08192b0819081919, 0x08192b0819082b08, 0x08192b0819190819, + 0x08192b0819191908, 0x08192b08192b0808, 0x08192b082b080819, 0x08192b082b081908, + 0x08192b1908080808, 0x08192b190808082b, 0x08192b1908081919, 0x08192b1908082b08, + 0x08192b1908190819, 0x08192b1908191908, 0x08192b19082b0808, 0x08192b1919080819, + 0x08192b1919081908, 0x08192b1919190808, 0x08192b19192b2b19, 0x08192b192b2b082b, + 0x08192b2b08081908, 0x08192b2b08190808, 0x08192b2b19080808, 0x08192b2b1919192b, + 0x082b080808080808, 0x082b08080808082b, 0x082b080808081919, 0x082b080808082b08, + 0x082b080808190819, 0x082b080808191908, 0x082b08080819192b, 0x082b080808192b19, + 0x082b0808082b0808, 0x082b0808082b1919, 0x082b0808082b2b2b, 0x082b080819080819, + 0x082b080819081908, 0x082b080819190808, 0x082b08081919082b, 0x082b080819191919, + 0x082b0808192b1908, 0x082b08082b080808, 0x082b08082b082b2b, 0x082b08082b191908, + 0x082b08082b2b2b2b, 0x082b081908080819, 0x082b081908081908, 0x082b081908190808, + 0x082b08190819082b, 0x082b081908191919, 0x082b0819082b0819, 0x082b081919080808, + 0x082b08191908082b, 0x082b081919081919, 0x082b081919190819, 0x082b081919191908, + 0x082b0819192b0808, 0x082b08192b080819, 0x082b08192b081908, 0x082b08192b190808, + 0x082b082b08080808, 0x082b082b08082b2b, 0x082b082b082b082b, 0x082b082b082b2b08, + 0x082b082b082b2b2b, 0x082b082b19081908, 0x082b082b19190808, 0x082b082b2b082b08, + 0x082b082b2b082b2b, 0x082b082b2b2b2b08, 0x082b190808080819, 0x082b190808081908, + 0x082b19080808192b, 0x082b190808082b19, 0x082b190808190808, 0x082b190808191919, + 0x082b190808192b08, 0x082b1908082b0819, 0x082b1908082b1908, 0x082b190819080808, + 0x082b19081908082b, 0x082b190819081919, 0x082b190819082b08, 0x082b190819190819, + 0x082b190819191908, 0x082b1908192b0808, 0x082b19082b080819, 0x082b19082b081908, + 0x082b19082b190808, 0x082b191908080808, 0x082b191908081919, 0x082b191908082b08, + 0x082b191908190819, 0x082b191908191908, 0x082b1919082b0808, 0x082b191919080819, + 0x082b191919081908, 0x082b191919190808, 0x082b1919192b192b, 0x082b19192b080808, + 0x082b192b08080819, 0x082b192b08081908, 0x082b192b08190808, 0x082b192b19080808, + 0x082b192b19192b19, 0x082b2b0808080808, 0x082b2b0808081919, 0x082b2b0808190819, + 0x082b2b0808191908, 0x082b2b0819080819, 0x082b2b0819081908, 0x082b2b0819190808, + 0x082b2b082b082b2b, 0x082b2b082b2b2b2b, 0x082b2b1908080819, 0x082b2b1908081908, + 0x082b2b1908190808, 0x082b2b192b191919, 0x082b2b2b08082b2b, 0x082b2b2b082b082b, + 0x082b2b2b192b1908, 0x082b2b2b2b082b08, 0x082b2b2b2b082b2b, 0x1908080808080819, + 0x1908080808081908, 0x190808080808192b, 0x1908080808082b19, 0x1908080808190808, + 0x190808080819082b, 0x1908080808191919, 0x1908080808192b08, 0x1908080808192b2b, + 0x19080808082b0819, 0x19080808082b1908, 0x19080808082b192b, 0x1908080819080808, + 0x190808081908082b, 0x1908080819081919, 0x1908080819082b08, 0x1908080819082b2b, + 0x1908080819190819, 0x1908080819191908, 0x190808081919192b, 0x1908080819192b19, + 0x19080808192b0808, 0x19080808192b082b, 0x19080808192b1919, 0x190808082b080819, + 0x190808082b081908, 0x190808082b190808, 0x190808082b191919, 0x190808082b192b08, + 0x190808082b2b0819, 0x190808082b2b1908, 0x1908081908080808, 0x190808190808082b, + 0x1908081908081919, 0x1908081908082b08, 0x1908081908190819, 0x1908081908191908, + 0x190808190819192b, 0x1908081908192b19, 0x19080819082b0808, 0x19080819082b082b, + 0x19080819082b1919, 0x1908081919080819, 0x1908081919081908, 0x190808191908192b, + 0x1908081919082b19, 0x1908081919190808, 0x190808191919082b, 0x1908081919191919, + 0x1908081919192b08, 0x19080819192b0819, 0x19080819192b1908, 0x190808192b080808, + 0x190808192b08082b, 0x190808192b081919, 0x190808192b082b08, 0x190808192b190819, + 0x190808192b191908, 0x190808192b2b0808, 0x1908082b08080819, 0x1908082b08081908, + 0x1908082b08190808, 0x1908082b0819082b, 0x1908082b08191919, 0x1908082b08192b08, + 0x1908082b082b1908, 0x1908082b19080808, 0x1908082b19081919, 0x1908082b19082b08, + 0x1908082b19190819, 0x1908082b19191908, 0x1908082b192b0808, 0x1908082b2b080819, + 0x1908082b2b081908, 0x1908190808080808, 0x190819080808082b, 0x1908190808081919, + 0x1908190808082b08, 0x1908190808082b2b, 0x1908190808190819, 0x1908190808191908, + 0x190819080819192b, 0x1908190808192b19, 0x19081908082b0808, 0x19081908082b082b, + 0x19081908082b1919, 0x19081908082b2b08, 0x1908190819080819, 0x1908190819081908, + 0x190819081908192b, 0x1908190819082b19, 0x1908190819190808, 0x190819081919082b, + 0x1908190819191919, 0x1908190819192b08, 0x19081908192b0819, 0x19081908192b1908, + 0x190819082b080808, 0x190819082b08082b, 0x190819082b081919, 0x190819082b082b08, + 0x190819082b190819, 0x190819082b191908, 0x190819082b2b0808, 0x1908191908080819, + 0x1908191908081908, 0x190819190808192b, 0x1908191908082b19, 0x1908191908190808, + 0x190819190819082b, 0x1908191908191919, 0x1908191908192b08, 0x19081919082b0819, + 0x19081919082b1908, 0x1908191919080808, 0x190819191908082b, 0x1908191919081919, + 0x1908191919082b08, 0x1908191919190819, 0x1908191919191908, 0x19081919192b0808, + 0x19081919192b2b2b, 0x190819192b080819, 0x190819192b081908, 0x190819192b190808, + 0x1908192b08080808, 0x1908192b0808082b, 0x1908192b08081919, 0x1908192b08082b08, + 0x1908192b08190819, 0x1908192b08191908, 0x1908192b082b0808, 0x1908192b19080819, + 0x1908192b19081908, 0x1908192b19190808, 0x1908192b2b080808, 0x1908192b2b2b1919, + 0x19082b0808080819, 0x19082b0808081908, 0x19082b0808082b19, 0x19082b0808190808, + 0x19082b080819082b, 0x19082b0808191919, 0x19082b0808192b08, 0x19082b08082b0819, + 0x19082b08082b1908, 0x19082b0819080808, 0x19082b081908082b, 0x19082b0819081919, + 0x19082b0819082b08, 0x19082b0819190819, 0x19082b0819191908, 0x19082b08192b0808, + 0x19082b082b081908, 0x19082b082b190808, 0x19082b1908080808, 0x19082b190808082b, + 0x19082b1908081919, 0x19082b1908082b08, 0x19082b1908190819, 0x19082b1908191908, + 0x19082b19082b0808, 0x19082b1919080819, 0x19082b1919081908, 0x19082b1919190808, + 0x19082b192b080808, 0x19082b192b19192b, 0x19082b2b08080819, 0x19082b2b08081908, + 0x19082b2b08190808, 0x19082b2b19080808, 0x1919080808080808, 0x191908080808082b, + 0x1919080808081919, 0x1919080808082b08, 0x1919080808190819, 0x1919080808191908, + 0x191908080819192b, 0x1919080808192b19, 0x19190808082b0808, 0x19190808082b082b, + 0x19190808082b1919, 0x19190808082b2b08, 0x1919080819080819, 0x1919080819081908, + 0x191908081908192b, 0x1919080819082b19, 0x1919080819190808, 0x191908081919082b, + 0x1919080819191919, 0x1919080819192b08, 0x19190808192b0819, 0x19190808192b1908, + 0x191908082b080808, 0x191908082b08082b, 0x191908082b081919, 0x191908082b082b08, + 0x191908082b190819, 0x191908082b191908, 0x1919081908080819, 0x1919081908081908, + 0x191908190808192b, 0x1919081908082b19, 0x1919081908190808, 0x191908190819082b, + 0x1919081908191919, 0x1919081908192b08, 0x19190819082b0819, 0x19190819082b1908, + 0x1919081919080808, 0x191908191908082b, 0x1919081919081919, 0x1919081919082b08, + 0x1919081919190819, 0x1919081919191908, 0x19190819192b0808, 0x191908192b080819, + 0x191908192b081908, 0x191908192b190808, 0x1919082b08080808, 0x1919082b08081919, + 0x1919082b08082b08, 0x1919082b08190819, 0x1919082b08191908, 0x1919082b082b0808, + 0x1919082b19080819, 0x1919082b19081908, 0x1919082b19190808, 0x1919082b192b2b19, + 0x1919082b2b080808, 0x1919190808080819, 0x1919190808081908, 0x191919080808192b, + 0x1919190808082b19, 0x1919190808190808, 0x191919080819082b, 0x1919190808191919, + 0x1919190808192b08, 0x19191908082b0819, 0x19191908082b1908, 0x1919190819080808, + 0x191919081908082b, 0x1919190819081919, 0x1919190819082b08, 0x1919190819190819, + 0x1919190819191908, 0x19191908192b0808, 0x191919082b080819, 0x191919082b081908, + 0x191919082b190808, 0x1919191908080808, 0x191919190808082b, 0x1919191908081919, + 0x1919191908082b08, 0x1919191908190819, 0x1919191908191908, 0x19191919082b0808, + 0x1919191919080819, 0x1919191919081908, 0x1919191919190808, 0x191919192b080808, + 0x1919192b08080819, 0x1919192b08081908, 0x1919192b08190808, 0x1919192b082b192b, + 0x1919192b19080808, 0x19192b0808080808, 0x19192b080808082b, 0x19192b0808081919, + 0x19192b0808082b08, 0x19192b0808190819, 0x19192b0808191908, 0x19192b08082b0808, + 0x19192b0819080819, 0x19192b0819081908, 0x19192b0819190808, 0x19192b0819192b2b, + 0x19192b082b080808, 0x19192b1908080819, 0x19192b1908081908, 0x19192b1908190808, + 0x19192b1919080808, 0x19192b2b08080808, 0x19192b2b08192b19, 0x19192b2b2b081919, + 0x19192b2b2b2b2b08, 0x192b080808080819, 0x192b080808081908, 0x192b08080808192b, + 0x192b080808190808, 0x192b08080819082b, 0x192b080808191919, 0x192b080808192b08, + 0x192b0808082b0819, 0x192b0808082b1908, 0x192b080819080808, 0x192b080819081919, + 0x192b080819082b08, 0x192b080819190819, 0x192b080819191908, 0x192b0808192b0808, + 0x192b08082b081908, 0x192b08082b190808, 0x192b081908080808, 0x192b08190808082b, + 0x192b081908081919, 0x192b081908082b08, 0x192b081908190819, 0x192b081908191908, + 0x192b0819082b0808, 0x192b081919080819, 0x192b081919081908, 0x192b081919190808, + 0x192b08192b080808, 0x192b08192b192b19, 0x192b082b08081908, 0x192b082b08190808, + 0x192b082b19080808, 0x192b082b1919192b, 0x192b082b2b2b0819, 0x192b190808080808, + 0x192b190808081919, 0x192b190808082b08, 0x192b190808190819, 0x192b190808191908, + 0x192b1908082b0808, 0x192b190819080819, 0x192b190819081908, 0x192b190819190808, + 0x192b19082b080808, 0x192b191908080819, 0x192b191908081908, 0x192b191908190808, + 0x192b191919080808, 0x192b191919082b2b, 0x192b1919192b2b08, 0x192b19192b19082b, + 0x192b192b08080808, 0x192b192b2b191908, 0x192b2b0808080819, 0x192b2b0808081908, + 0x192b2b0808190808, 0x192b2b08192b1919, 0x192b2b082b192b08, 0x192b2b1908080808, + 0x192b2b19082b2b2b, 0x192b2b2b1908082b, 0x192b2b2b2b2b0819, 0x2b08080808080808, + 0x2b0808080808082b, 0x2b08080808081919, 0x2b08080808082b08, 0x2b08080808190819, + 0x2b08080808191908, 0x2b08080808192b19, 0x2b080808082b0808, 0x2b080808082b1919, + 0x2b08080819080819, 0x2b08080819081908, 0x2b08080819190808, 0x2b0808081919082b, + 0x2b08080819191919, 0x2b08080819192b08, 0x2b080808192b0819, 0x2b0808082b080808, + 0x2b0808082b081919, 0x2b0808082b190819, 0x2b0808082b191908, 0x2b08081908080819, + 0x2b08081908081908, 0x2b08081908082b19, 0x2b08081908190808, 0x2b0808190819082b, + 0x2b08081908191919, 0x2b08081908192b08, 0x2b080819082b0819, 0x2b080819082b1908, + 0x2b08081919080808, 0x2b0808191908082b, 0x2b08081919081919, 0x2b08081919082b08, + 0x2b08081919190819, 0x2b08081919191908, 0x2b0808192b080819, 0x2b0808192b081908, + 0x2b0808192b190808, 0x2b0808192b2b2b19, 0x2b08082b08080808, 0x2b08082b08081919, + 0x2b08082b08082b2b, 0x2b08082b08190819, 0x2b08082b08191908, 0x2b08082b19080819, + 0x2b08082b19081908, 0x2b08082b19190808, 0x2b08190808080819, 0x2b08190808081908, + 0x2b0819080808192b, 0x2b08190808082b19, 0x2b08190808190808, 0x2b0819080819082b, + 0x2b08190808191919, 0x2b08190808192b08, 0x2b081908082b0819, 0x2b08190819080808, + 0x2b0819081908082b, 0x2b08190819081919, 0x2b08190819082b08, 0x2b08190819190819, + 0x2b08190819191908, 0x2b081908192b0808, 0x2b0819082b080819, 0x2b0819082b081908, + 0x2b0819082b190808, 0x2b08191908080808, 0x2b0819190808082b, 0x2b08191908081919, + 0x2b08191908082b08, 0x2b08191908190819, 0x2b08191908191908, 0x2b081919082b0808, + 0x2b08191919080819, 0x2b08191919081908, 0x2b08191919190808, 0x2b0819192b080808, + 0x2b0819192b082b2b, 0x2b08192b08080819, 0x2b08192b08081908, 0x2b08192b08190808, + 0x2b08192b082b2b19, 0x2b08192b19080808, 0x2b082b0808080808, 0x2b082b0808081919, + 0x2b082b0808190819, 0x2b082b0808191908, 0x2b082b0819080819, 0x2b082b0819081908, + 0x2b082b0819190808, 0x2b082b082b2b082b, 0x2b082b1908080819, 0x2b082b1908081908, + 0x2b082b1919080808, 0x2b082b19192b1919, 0x2b082b2b082b082b, 0x2b082b2b19192b08, + 0x2b082b2b19192b2b, 0x2b082b2b2b08082b, 0x2b082b2b2b2b082b, 0x2b19080808080819, + 0x2b19080808081908, 0x2b19080808082b19, 0x2b19080808190808, 0x2b1908080819082b, + 0x2b19080808191919, 0x2b19080808192b08, 0x2b190808082b1908, 0x2b19080819080808, + 0x2b1908081908082b, 0x2b19080819081919, 0x2b19080819082b08, 0x2b19080819190819, + 0x2b19080819191908, 0x2b190808192b0808, 0x2b1908082b080819, 0x2b1908082b081908, + 0x2b1908082b190808, 0x2b19081908080808, 0x2b19081908081919, 0x2b19081908190819, + 0x2b19081908191908, 0x2b19081919080819, 0x2b19081919081908, 0x2b19081919190808, + 0x2b19081919192b2b, 0x2b19082b08080819, 0x2b19082b08081908, 0x2b19082b08190808, + 0x2b19082b19080808, 0x2b19082b2b2b192b, 0x2b19190808080808, 0x2b1919080808082b, + 0x2b19190808081919, 0x2b19190808082b08, 0x2b19190808190819, 0x2b19190808191908, + 0x2b191908082b0808, 0x2b19190819080819, 0x2b19190819081908, 0x2b19190819190808, + 0x2b1919082b080808, 0x2b1919082b19192b, 0x2b19191908080819, 0x2b19191908081908, + 0x2b19191908190808, 0x2b19191919080808, 0x2b1919192b192b08, 0x2b1919192b2b0819, + 0x2b19192b08080808, 0x2b19192b1908192b, 0x2b19192b192b1908, 0x2b192b0808080819, + 0x2b192b0808081908, 0x2b192b0808190808, 0x2b192b08082b192b, 0x2b192b0819080808, + 0x2b192b082b2b2b19, 0x2b192b1908080808, 0x2b192b1919082b19, 0x2b192b191919082b, + 0x2b192b2b2b190808, 0x2b2b080808080808, 0x2b2b080808081919, 0x2b2b080808082b2b, + 0x2b2b080808191908, 0x2b2b0808082b082b, 0x2b2b0808082b2b2b, 0x2b2b080819080819, + 0x2b2b080819081908, 0x2b2b080819190808, 0x2b2b08082b2b082b, 0x2b2b08082b2b2b2b, + 0x2b2b081919080808, 0x2b2b0819192b1919, 0x2b2b082b0808082b, 0x2b2b082b08082b2b, + 0x2b2b082b082b082b, 0x2b2b082b082b2b08, 0x2b2b082b082b2b2b, 0x2b2b082b2b08082b, + 0x2b2b082b2b082b08, 0x2b2b082b2b082b2b, 0x2b2b082b2b2b2b08, 0x2b2b190808080819, + 0x2b2b190808081908, 0x2b2b190808190808, 0x2b2b190819080808, 0x2b2b19082b082b19, + 0x2b2b19082b2b1908, 0x2b2b191908080808, 0x2b2b191908192b19, 0x2b2b192b19190819, + 0x2b2b2b0808082b2b, 0x2b2b2b08082b2b08, 0x2b2b2b082b2b082b, 0x2b2b2b1919191908, + 0x2b2b2b192b08192b, 0x2b2b2b2b08082b08, 0x2b2b2b2b08082b2b, 0x2b2b2b2b082b0808, + 0x2b2b2b2b082b082b, 0x2b2b2b2b082b2b08, 0x2b2b2b2b2b082b08, 0x2b2b2b2b2b2b2b2b, +GGML_TABLE_END() + +GGML_TABLE_BEGIN(uint32_t, iq3xxs_grid, 256) + 0x04040404, 0x04040414, 0x04040424, 0x04040c0c, 0x04040c1c, 0x04040c3e, 0x04041404, 0x04041414, + 0x04041c0c, 0x04042414, 0x04043e1c, 0x04043e2c, 0x040c040c, 0x040c041c, 0x040c0c04, 0x040c0c14, + 0x040c140c, 0x040c142c, 0x040c1c04, 0x040c1c14, 0x040c240c, 0x040c2c24, 0x040c3e04, 0x04140404, + 0x04140414, 0x04140424, 0x04140c0c, 0x04141404, 0x04141414, 0x04141c0c, 0x04141c1c, 0x04141c3e, + 0x04142c0c, 0x04142c3e, 0x04143e2c, 0x041c040c, 0x041c043e, 0x041c0c04, 0x041c0c14, 0x041c142c, + 0x041c3e04, 0x04240c1c, 0x04241c3e, 0x04242424, 0x04242c3e, 0x04243e1c, 0x04243e2c, 0x042c040c, + 0x042c043e, 0x042c1c14, 0x042c2c14, 0x04341c2c, 0x04343424, 0x043e0c04, 0x043e0c24, 0x043e0c34, + 0x043e241c, 0x043e340c, 0x0c04040c, 0x0c04041c, 0x0c040c04, 0x0c040c14, 0x0c04140c, 0x0c04141c, + 0x0c041c04, 0x0c041c14, 0x0c041c24, 0x0c04243e, 0x0c042c04, 0x0c0c0404, 0x0c0c0414, 0x0c0c0c0c, + 0x0c0c1404, 0x0c0c1414, 0x0c14040c, 0x0c14041c, 0x0c140c04, 0x0c140c14, 0x0c14140c, 0x0c141c04, + 0x0c143e14, 0x0c1c0404, 0x0c1c0414, 0x0c1c1404, 0x0c1c1c0c, 0x0c1c2434, 0x0c1c3434, 0x0c24040c, + 0x0c24042c, 0x0c242c04, 0x0c2c1404, 0x0c2c1424, 0x0c2c2434, 0x0c2c3e0c, 0x0c34042c, 0x0c3e1414, + 0x0c3e2404, 0x14040404, 0x14040414, 0x14040c0c, 0x14040c1c, 0x14041404, 0x14041414, 0x14041434, + 0x14041c0c, 0x14042414, 0x140c040c, 0x140c041c, 0x140c042c, 0x140c0c04, 0x140c0c14, 0x140c140c, + 0x140c1c04, 0x140c341c, 0x140c343e, 0x140c3e04, 0x14140404, 0x14140414, 0x14140c0c, 0x14140c3e, + 0x14141404, 0x14141414, 0x14141c3e, 0x14142404, 0x14142c2c, 0x141c040c, 0x141c0c04, 0x141c0c24, + 0x141c3e04, 0x141c3e24, 0x14241c2c, 0x14242c1c, 0x142c041c, 0x142c143e, 0x142c240c, 0x142c3e24, + 0x143e040c, 0x143e041c, 0x143e0c34, 0x143e242c, 0x1c04040c, 0x1c040c04, 0x1c040c14, 0x1c04140c, + 0x1c04141c, 0x1c042c04, 0x1c04342c, 0x1c043e14, 0x1c0c0404, 0x1c0c0414, 0x1c0c1404, 0x1c0c1c0c, + 0x1c0c2424, 0x1c0c2434, 0x1c14040c, 0x1c14041c, 0x1c140c04, 0x1c14142c, 0x1c142c14, 0x1c143e14, + 0x1c1c0c0c, 0x1c1c1c1c, 0x1c241c04, 0x1c24243e, 0x1c243e14, 0x1c2c0404, 0x1c2c0434, 0x1c2c1414, + 0x1c2c2c2c, 0x1c340c24, 0x1c341c34, 0x1c34341c, 0x1c3e1c1c, 0x1c3e3404, 0x24040424, 0x24040c3e, + 0x24041c2c, 0x24041c3e, 0x24042c1c, 0x24042c3e, 0x240c3e24, 0x24141404, 0x24141c3e, 0x24142404, + 0x24143404, 0x24143434, 0x241c043e, 0x241c242c, 0x24240424, 0x24242c0c, 0x24243424, 0x242c142c, + 0x242c241c, 0x242c3e04, 0x243e042c, 0x243e0c04, 0x243e0c14, 0x243e1c04, 0x2c040c14, 0x2c04240c, + 0x2c043e04, 0x2c0c0404, 0x2c0c0434, 0x2c0c1434, 0x2c0c2c2c, 0x2c140c24, 0x2c141c14, 0x2c143e14, + 0x2c1c0414, 0x2c1c2c1c, 0x2c240c04, 0x2c24141c, 0x2c24143e, 0x2c243e14, 0x2c2c0414, 0x2c2c1c0c, + 0x2c342c04, 0x2c3e1424, 0x2c3e2414, 0x34041424, 0x34042424, 0x34042434, 0x34043424, 0x340c140c, + 0x340c340c, 0x34140c3e, 0x34143424, 0x341c1c04, 0x341c1c34, 0x34242424, 0x342c042c, 0x342c2c14, + 0x34341c1c, 0x343e041c, 0x343e140c, 0x3e04041c, 0x3e04042c, 0x3e04043e, 0x3e040c04, 0x3e041c14, + 0x3e042c14, 0x3e0c1434, 0x3e0c2404, 0x3e140c14, 0x3e14242c, 0x3e142c14, 0x3e1c0404, 0x3e1c0c2c, + 0x3e1c1c1c, 0x3e1c3404, 0x3e24140c, 0x3e24240c, 0x3e2c0404, 0x3e2c0414, 0x3e2c1424, 0x3e341c04, +GGML_TABLE_END() + +GGML_TABLE_BEGIN(uint32_t, iq3s_grid, 512) + 0x01010101, 0x01010103, 0x01010105, 0x0101010b, 0x0101010f, 0x01010301, 0x01010303, 0x01010305, + 0x01010309, 0x0101030d, 0x01010501, 0x01010503, 0x0101050b, 0x01010707, 0x01010901, 0x01010905, + 0x0101090b, 0x0101090f, 0x01010b03, 0x01010b07, 0x01010d01, 0x01010d05, 0x01010f03, 0x01010f09, + 0x01010f0f, 0x01030101, 0x01030103, 0x01030105, 0x01030109, 0x01030301, 0x01030303, 0x0103030b, + 0x01030501, 0x01030507, 0x0103050f, 0x01030703, 0x0103070b, 0x01030909, 0x01030d03, 0x01030d0b, + 0x01030f05, 0x01050101, 0x01050103, 0x0105010b, 0x0105010f, 0x01050301, 0x01050307, 0x0105030d, + 0x01050503, 0x0105050b, 0x01050701, 0x01050709, 0x01050905, 0x0105090b, 0x0105090f, 0x01050b03, + 0x01050b07, 0x01050f01, 0x01050f07, 0x01070107, 0x01070303, 0x0107030b, 0x01070501, 0x01070505, + 0x01070703, 0x01070707, 0x0107070d, 0x01070909, 0x01070b01, 0x01070b05, 0x01070d0f, 0x01070f03, + 0x01070f0b, 0x01090101, 0x01090307, 0x0109030f, 0x01090503, 0x01090509, 0x01090705, 0x01090901, + 0x01090907, 0x01090b03, 0x01090f01, 0x010b0105, 0x010b0109, 0x010b0501, 0x010b0505, 0x010b050d, + 0x010b0707, 0x010b0903, 0x010b090b, 0x010b090f, 0x010b0d0d, 0x010b0f07, 0x010d010d, 0x010d0303, + 0x010d0307, 0x010d0703, 0x010d0b05, 0x010d0f03, 0x010f0101, 0x010f0105, 0x010f0109, 0x010f0501, + 0x010f0505, 0x010f050d, 0x010f0707, 0x010f0b01, 0x010f0b09, 0x03010101, 0x03010103, 0x03010105, + 0x03010109, 0x03010301, 0x03010303, 0x03010307, 0x0301030b, 0x0301030f, 0x03010501, 0x03010505, + 0x03010703, 0x03010709, 0x0301070d, 0x03010b09, 0x03010b0d, 0x03010d03, 0x03010f05, 0x03030101, + 0x03030103, 0x03030107, 0x0303010d, 0x03030301, 0x03030309, 0x03030503, 0x03030701, 0x03030707, + 0x03030903, 0x03030b01, 0x03030b05, 0x03030f01, 0x03030f0d, 0x03050101, 0x03050305, 0x0305030b, + 0x0305030f, 0x03050501, 0x03050509, 0x03050705, 0x03050901, 0x03050907, 0x03050b0b, 0x03050d01, + 0x03050f05, 0x03070103, 0x03070109, 0x0307010f, 0x03070301, 0x03070307, 0x03070503, 0x0307050f, + 0x03070701, 0x03070709, 0x03070903, 0x03070d05, 0x03070f01, 0x03090107, 0x0309010b, 0x03090305, + 0x03090309, 0x03090703, 0x03090707, 0x03090905, 0x0309090d, 0x03090b01, 0x03090b09, 0x030b0103, + 0x030b0301, 0x030b0307, 0x030b0503, 0x030b0701, 0x030b0705, 0x030b0b03, 0x030d0501, 0x030d0509, + 0x030d050f, 0x030d0909, 0x030d090d, 0x030f0103, 0x030f0107, 0x030f0301, 0x030f0305, 0x030f0503, + 0x030f070b, 0x030f0903, 0x030f0d05, 0x030f0f01, 0x05010101, 0x05010103, 0x05010107, 0x0501010b, + 0x0501010f, 0x05010301, 0x05010305, 0x05010309, 0x0501030d, 0x05010503, 0x05010507, 0x0501050f, + 0x05010701, 0x05010705, 0x05010903, 0x05010907, 0x0501090b, 0x05010b01, 0x05010b05, 0x05010d0f, + 0x05010f01, 0x05010f07, 0x05010f0b, 0x05030101, 0x05030105, 0x05030301, 0x05030307, 0x0503030f, + 0x05030505, 0x0503050b, 0x05030703, 0x05030709, 0x05030905, 0x05030b03, 0x05050103, 0x05050109, + 0x0505010f, 0x05050503, 0x05050507, 0x05050701, 0x0505070f, 0x05050903, 0x05050b07, 0x05050b0f, + 0x05050f03, 0x05050f09, 0x05070101, 0x05070105, 0x0507010b, 0x05070303, 0x05070505, 0x05070509, + 0x05070703, 0x05070707, 0x05070905, 0x05070b01, 0x05070d0d, 0x05090103, 0x0509010f, 0x05090501, + 0x05090507, 0x05090705, 0x0509070b, 0x05090903, 0x05090f05, 0x05090f0b, 0x050b0109, 0x050b0303, + 0x050b0505, 0x050b070f, 0x050b0901, 0x050b0b07, 0x050b0f01, 0x050d0101, 0x050d0105, 0x050d010f, + 0x050d0503, 0x050d0b0b, 0x050d0d03, 0x050f010b, 0x050f0303, 0x050f050d, 0x050f0701, 0x050f0907, + 0x050f0b01, 0x07010105, 0x07010303, 0x07010307, 0x0701030b, 0x0701030f, 0x07010505, 0x07010703, + 0x07010707, 0x0701070b, 0x07010905, 0x07010909, 0x0701090f, 0x07010b03, 0x07010d07, 0x07010f03, + 0x07030103, 0x07030107, 0x0703010b, 0x07030309, 0x07030503, 0x07030507, 0x07030901, 0x07030d01, + 0x07030f05, 0x07030f0d, 0x07050101, 0x07050305, 0x07050501, 0x07050705, 0x07050709, 0x07050b01, + 0x07070103, 0x07070301, 0x07070309, 0x07070503, 0x07070507, 0x0707050f, 0x07070701, 0x07070903, + 0x07070907, 0x0707090f, 0x07070b0b, 0x07070f07, 0x07090107, 0x07090303, 0x0709030d, 0x07090505, + 0x07090703, 0x07090b05, 0x07090d01, 0x07090d09, 0x070b0103, 0x070b0301, 0x070b0305, 0x070b050b, + 0x070b0705, 0x070b0909, 0x070b0b0d, 0x070b0f07, 0x070d030d, 0x070d0903, 0x070f0103, 0x070f0107, + 0x070f0501, 0x070f0505, 0x070f070b, 0x09010101, 0x09010109, 0x09010305, 0x09010501, 0x09010509, + 0x0901050f, 0x09010705, 0x09010903, 0x09010b01, 0x09010f01, 0x09030105, 0x0903010f, 0x09030303, + 0x09030307, 0x09030505, 0x09030701, 0x0903070b, 0x09030907, 0x09030b03, 0x09030b0b, 0x09050103, + 0x09050107, 0x09050301, 0x0905030b, 0x09050503, 0x09050707, 0x09050901, 0x09050b0f, 0x09050d05, + 0x09050f01, 0x09070109, 0x09070303, 0x09070307, 0x09070501, 0x09070505, 0x09070703, 0x0907070b, + 0x09090101, 0x09090105, 0x09090509, 0x0909070f, 0x09090901, 0x09090f03, 0x090b010b, 0x090b010f, + 0x090b0503, 0x090b0d05, 0x090d0307, 0x090d0709, 0x090d0d01, 0x090f0301, 0x090f030b, 0x090f0701, + 0x090f0907, 0x090f0b03, 0x0b010105, 0x0b010301, 0x0b010309, 0x0b010505, 0x0b010901, 0x0b010909, + 0x0b01090f, 0x0b010b05, 0x0b010d0d, 0x0b010f09, 0x0b030103, 0x0b030107, 0x0b03010b, 0x0b030305, + 0x0b030503, 0x0b030705, 0x0b030f05, 0x0b050101, 0x0b050303, 0x0b050507, 0x0b050701, 0x0b05070d, + 0x0b050b07, 0x0b070105, 0x0b07010f, 0x0b070301, 0x0b07050f, 0x0b070909, 0x0b070b03, 0x0b070d0b, + 0x0b070f07, 0x0b090103, 0x0b090109, 0x0b090501, 0x0b090705, 0x0b09090d, 0x0b0b0305, 0x0b0b050d, + 0x0b0b0b03, 0x0b0b0b07, 0x0b0d0905, 0x0b0f0105, 0x0b0f0109, 0x0b0f0505, 0x0d010303, 0x0d010307, + 0x0d01030b, 0x0d010703, 0x0d010707, 0x0d010d01, 0x0d030101, 0x0d030501, 0x0d03050f, 0x0d030d09, + 0x0d050305, 0x0d050709, 0x0d050905, 0x0d050b0b, 0x0d050d05, 0x0d050f01, 0x0d070101, 0x0d070309, + 0x0d070503, 0x0d070901, 0x0d09050b, 0x0d090907, 0x0d090d05, 0x0d0b0101, 0x0d0b0107, 0x0d0b0709, + 0x0d0b0d01, 0x0d0d010b, 0x0d0d0901, 0x0d0f0303, 0x0d0f0307, 0x0f010101, 0x0f010109, 0x0f01010f, + 0x0f010501, 0x0f010505, 0x0f01070d, 0x0f010901, 0x0f010b09, 0x0f010d05, 0x0f030105, 0x0f030303, + 0x0f030509, 0x0f030907, 0x0f03090b, 0x0f050103, 0x0f050109, 0x0f050301, 0x0f05030d, 0x0f050503, + 0x0f050701, 0x0f050b03, 0x0f070105, 0x0f070705, 0x0f07070b, 0x0f070b07, 0x0f090103, 0x0f09010b, + 0x0f090307, 0x0f090501, 0x0f090b01, 0x0f0b0505, 0x0f0b0905, 0x0f0d0105, 0x0f0d0703, 0x0f0f0101, +GGML_TABLE_END() + +// TODO: fix name to kvalues_iq4_nl + +// e2m1 values (doubled) +// ref: https://www.opencompute.org/documents/ocp-microscaling-formats-mx-v1-0-spec-final-pdf + +#define NGRID_IQ1S 2048 +#define IQ1S_DELTA 0.125f +#define IQ1M_DELTA 0.125f +#if defined(GGML_COMMON_IMPL_C) +#else +GGML_TABLE_BEGIN(uint32_t, iq1s_grid_gpu, NGRID_IQ1S) + 0x00000000, 0x00000002, 0x00000101, 0x00000200, 0x00000202, 0x00010001, 0x00010101, 0x00020000, + 0x00020002, 0x00020200, 0x00020202, 0x01000101, 0x01010001, 0x01010100, 0x01010102, 0x01020101, + 0x02000000, 0x02000002, 0x02000200, 0x02000202, 0x02010101, 0x02020000, 0x02020002, 0x02020200, + 0x02020202, 0x00000110, 0x00000111, 0x00010011, 0x00010110, 0x00010112, 0x00010211, 0x00010212, + 0x00020111, 0x01000011, 0x01000112, 0x01000211, 0x01010012, 0x01010111, 0x01010212, 0x01020011, + 0x01020110, 0x01020112, 0x01020210, 0x02000111, 0x02010011, 0x02010110, 0x02010112, 0x02020111, + 0x00000020, 0x00000022, 0x00000220, 0x00000222, 0x00010121, 0x00020020, 0x00020022, 0x00020220, + 0x00020222, 0x01000121, 0x01010021, 0x01010221, 0x01020120, 0x01020221, 0x02000020, 0x02000022, + 0x02000220, 0x02000222, 0x02010021, 0x02010121, 0x02010221, 0x02020020, 0x02020022, 0x02020220, + 0x02020222, 0x00011001, 0x00011100, 0x00011102, 0x00021101, 0x01001001, 0x01001201, 0x01011101, + 0x01011202, 0x01021100, 0x01021101, 0x02011001, 0x02011201, 0x02021101, 0x00001011, 0x00001110, + 0x00001111, 0x00001112, 0x00011111, 0x00011210, 0x00011212, 0x00021211, 0x01001010, 0x01001111, + 0x01001212, 0x01011010, 0x01011011, 0x01011110, 0x01011111, 0x01011112, 0x01011211, 0x01021010, + 0x01021012, 0x01021111, 0x01021210, 0x01021212, 0x02001011, 0x02011011, 0x02011111, 0x02011210, + 0x02011212, 0x02021011, 0x02021110, 0x02021111, 0x02021112, 0x02021211, 0x00011120, 0x00011221, + 0x01001021, 0x01001120, 0x01011020, 0x01011022, 0x01011121, 0x01011220, 0x01021020, 0x01021021, + 0x01021122, 0x01021221, 0x02001121, 0x02011021, 0x02011120, 0x02011221, 0x00002000, 0x00002002, + 0x00002200, 0x00002202, 0x00012101, 0x00022000, 0x00022002, 0x00022200, 0x00022202, 0x01002101, + 0x01012001, 0x01012102, 0x01022101, 0x02002000, 0x02002002, 0x02002200, 0x02002202, 0x02012101, + 0x02022000, 0x02022002, 0x02022200, 0x02022202, 0x00002111, 0x00012011, 0x00012110, 0x00012211, + 0x00022110, 0x00022111, 0x01002011, 0x01012010, 0x01012011, 0x01012111, 0x01022011, 0x01022110, + 0x01022211, 0x02012011, 0x02012110, 0x02012112, 0x02012211, 0x02022111, 0x00002020, 0x00002022, + 0x00002220, 0x00002222, 0x00012121, 0x00022020, 0x00022022, 0x00022220, 0x00022222, 0x01002121, + 0x01012021, 0x01012221, 0x01022021, 0x01022121, 0x02002020, 0x02002022, 0x02002121, 0x02002220, + 0x02002222, 0x02012121, 0x02022020, 0x02022022, 0x02022220, 0x02022222, 0x00110000, 0x00110001, + 0x00110100, 0x00110201, 0x00120100, 0x00120101, 0x01100001, 0x01100100, 0x01110000, 0x01110101, + 0x01110200, 0x01120001, 0x01120100, 0x01120101, 0x01120201, 0x02110001, 0x02110100, 0x02110102, + 0x02120001, 0x02120101, 0x00100011, 0x00100110, 0x00100112, 0x00100211, 0x00110010, 0x00110012, + 0x00110111, 0x00110210, 0x00120011, 0x00120110, 0x00120211, 0x01100111, 0x01100212, 0x01110010, + 0x01110011, 0x01110012, 0x01110110, 0x01110111, 0x01110112, 0x01110211, 0x01120010, 0x01120111, + 0x02100110, 0x02110012, 0x02110111, 0x02120011, 0x02120110, 0x00110021, 0x00110120, 0x00110122, + 0x00120121, 0x01100020, 0x01100122, 0x01100221, 0x01110022, 0x01110121, 0x01110220, 0x01110222, + 0x01120120, 0x01120122, 0x02100121, 0x02110021, 0x02110120, 0x02110122, 0x02120121, 0x00101001, + 0x00101102, 0x00101201, 0x00111100, 0x00111101, 0x00111200, 0x00111201, 0x00121001, 0x00121102, + 0x01101001, 0x01101101, 0x01101102, 0x01101200, 0x01101202, 0x01111001, 0x01111100, 0x01111101, + 0x01111102, 0x01111201, 0x01121002, 0x01121101, 0x01121200, 0x02101100, 0x02101201, 0x02111000, + 0x02111100, 0x02111101, 0x02111200, 0x02111201, 0x02111202, 0x02121001, 0x02121100, 0x02121101, + 0x02121201, 0x00101012, 0x00101111, 0x00101212, 0x00111011, 0x00111110, 0x00111111, 0x00111112, + 0x00111211, 0x00121010, 0x00121012, 0x00121111, 0x00121210, 0x00121212, 0x01101011, 0x01101110, + 0x01101111, 0x01101112, 0x01111011, 0x01111012, 0x01111110, 0x01111111, 0x01111112, 0x01111211, + 0x01111212, 0x01121011, 0x01121110, 0x01121111, 0x01121112, 0x01121211, 0x02101010, 0x02101012, + 0x02101110, 0x02101111, 0x02101210, 0x02101212, 0x02111010, 0x02111011, 0x02111110, 0x02111111, + 0x02111112, 0x02111211, 0x02111212, 0x02121010, 0x02121012, 0x02121111, 0x00101021, 0x00101120, + 0x00101121, 0x00101122, 0x00111121, 0x00111122, 0x00111220, 0x00111222, 0x00121021, 0x00121122, + 0x01101020, 0x01101022, 0x01101120, 0x01101121, 0x01101220, 0x01101222, 0x01111021, 0x01111121, + 0x01111122, 0x01111220, 0x01111221, 0x01121021, 0x01121120, 0x01121121, 0x01121220, 0x01121221, + 0x01121222, 0x02101122, 0x02101222, 0x02111022, 0x02111121, 0x02121120, 0x02121221, 0x00112001, + 0x00112102, 0x00122101, 0x01102001, 0x01102100, 0x01102102, 0x01102201, 0x01112000, 0x01112101, + 0x01112200, 0x01112202, 0x01122000, 0x01122001, 0x01122100, 0x01122102, 0x01122201, 0x02102101, + 0x02112001, 0x02112100, 0x02122101, 0x00112010, 0x00112012, 0x00112111, 0x00112212, 0x00122011, + 0x00122111, 0x01102012, 0x01102110, 0x01102111, 0x01102210, 0x01112011, 0x01112110, 0x01112111, + 0x01112112, 0x01112211, 0x01112212, 0x01122010, 0x01122111, 0x01122212, 0x02102211, 0x02112011, + 0x02112012, 0x02112111, 0x02112210, 0x02122011, 0x02122112, 0x02122211, 0x00102221, 0x00112122, + 0x00122120, 0x00122122, 0x01102120, 0x01102122, 0x01102221, 0x01112020, 0x01112022, 0x01112121, + 0x01112220, 0x01122021, 0x01122122, 0x01122221, 0x02102121, 0x02112021, 0x02112122, 0x02112222, + 0x00200000, 0x00200002, 0x00200200, 0x00200202, 0x00210101, 0x00220000, 0x00220002, 0x00220101, + 0x00220200, 0x00220202, 0x01200101, 0x01210001, 0x01210201, 0x01220001, 0x01220101, 0x02200000, + 0x02200002, 0x02200200, 0x02200202, 0x02210101, 0x02220000, 0x02220002, 0x02220101, 0x02220200, + 0x02220202, 0x00200111, 0x00210011, 0x00210110, 0x00210211, 0x00220111, 0x01200012, 0x01200110, + 0x01200211, 0x01210111, 0x01210210, 0x01210212, 0x01220011, 0x01220110, 0x01220111, 0x01220112, + 0x02200111, 0x02210010, 0x02210112, 0x02210211, 0x02220111, 0x00200021, 0x00200220, 0x00200222, + 0x00210021, 0x00210121, 0x00220020, 0x00220022, 0x00220220, 0x00220222, 0x01200121, 0x01210021, + 0x01210122, 0x01210221, 0x01220121, 0x02200021, 0x02200220, 0x02200222, 0x02210021, 0x02210121, + 0x02220020, 0x02220022, 0x02220220, 0x02220222, 0x00201101, 0x00211100, 0x00211102, 0x00211201, + 0x00221101, 0x01201100, 0x01201101, 0x01201102, 0x01201201, 0x01211002, 0x01211101, 0x01211200, + 0x01211202, 0x01221102, 0x02201101, 0x02211001, 0x02211100, 0x02211201, 0x02221001, 0x02221101, + 0x00201211, 0x00211111, 0x00221011, 0x00221211, 0x01201010, 0x01201111, 0x01201210, 0x01211011, + 0x01211110, 0x01211111, 0x01211211, 0x01221012, 0x01221111, 0x01221210, 0x02201211, 0x02211010, + 0x02211110, 0x02211111, 0x02211210, 0x02211212, 0x02221011, 0x02221110, 0x02221112, 0x02221211, + 0x00201121, 0x00211020, 0x00211022, 0x00211221, 0x00221121, 0x01201021, 0x01201221, 0x01211121, + 0x01221020, 0x01221021, 0x01221221, 0x02201120, 0x02201122, 0x02211020, 0x02211222, 0x00202000, + 0x00202002, 0x00202200, 0x00202202, 0x00212101, 0x00222000, 0x00222002, 0x00222200, 0x00222202, + 0x01202101, 0x01212001, 0x01212100, 0x01222101, 0x02202000, 0x02202002, 0x02202200, 0x02202202, + 0x02222000, 0x02222002, 0x02222200, 0x02222202, 0x00202211, 0x00212011, 0x00212110, 0x00212211, + 0x00222111, 0x01202112, 0x01202211, 0x01212012, 0x01212111, 0x01222011, 0x01222110, 0x01222112, + 0x01222211, 0x02202111, 0x02212010, 0x02212112, 0x02212211, 0x02222110, 0x02222111, 0x00202020, + 0x00202022, 0x00202220, 0x00202222, 0x00222020, 0x00222022, 0x00222220, 0x00222222, 0x01202121, + 0x01212021, 0x01212122, 0x01212221, 0x01222121, 0x02202020, 0x02202022, 0x02202220, 0x02202222, + 0x02212121, 0x02222020, 0x02222022, 0x02222220, 0x02222222, 0x10000101, 0x10010001, 0x10010102, + 0x10020101, 0x11000201, 0x11010002, 0x11010101, 0x11010200, 0x11010202, 0x11020001, 0x11020100, + 0x11020102, 0x12010100, 0x12010201, 0x12020001, 0x12020102, 0x10000010, 0x10000011, 0x10000110, + 0x10000112, 0x10000211, 0x10010012, 0x10010111, 0x10010112, 0x10010210, 0x10010212, 0x10020011, + 0x10020112, 0x10020211, 0x11000111, 0x11000210, 0x11000212, 0x11010011, 0x11010110, 0x11010111, + 0x11010112, 0x11010211, 0x11010212, 0x11020111, 0x11020210, 0x11020212, 0x12000011, 0x12000110, + 0x12000112, 0x12010010, 0x12010012, 0x12010111, 0x12020010, 0x12020011, 0x12020012, 0x10000121, + 0x10010021, 0x10010120, 0x10010122, 0x10020121, 0x11000021, 0x11010022, 0x11010121, 0x11010222, + 0x11020120, 0x11020221, 0x12000221, 0x12010120, 0x12020121, 0x10001001, 0x10011101, 0x10011201, + 0x10021201, 0x11001101, 0x11001200, 0x11001202, 0x11011001, 0x11011100, 0x11011101, 0x11011102, + 0x11021001, 0x11021002, 0x11021101, 0x11021200, 0x11021202, 0x12001001, 0x12001102, 0x12001201, + 0x12011000, 0x12011002, 0x12011101, 0x12021000, 0x12021001, 0x12021201, 0x10001011, 0x10001012, + 0x10001111, 0x10001212, 0x10011011, 0x10011110, 0x10011111, 0x10011112, 0x10011211, 0x10021010, + 0x10021111, 0x10021212, 0x11001011, 0x11001110, 0x11001111, 0x11001112, 0x11001211, 0x11011010, + 0x11011011, 0x11011110, 0x11011111, 0x11011112, 0x11011210, 0x11011211, 0x11021011, 0x11021110, + 0x11021111, 0x11021112, 0x11021211, 0x12001012, 0x12001110, 0x12001111, 0x12001210, 0x12011011, + 0x12011110, 0x12011111, 0x12011112, 0x12011211, 0x12011212, 0x12021111, 0x12021210, 0x12021212, + 0x10001021, 0x10001121, 0x10001221, 0x10011120, 0x10011121, 0x10011220, 0x10011222, 0x10021021, + 0x10021120, 0x10021221, 0x11001020, 0x11001022, 0x11001121, 0x11001220, 0x11011020, 0x11011021, + 0x11011022, 0x11011121, 0x11011122, 0x11011221, 0x11021022, 0x11021121, 0x11021220, 0x12001021, + 0x12001121, 0x12001222, 0x12011120, 0x12011121, 0x12021021, 0x12021120, 0x12021122, 0x10002101, + 0x10012001, 0x10012101, 0x10012202, 0x10022101, 0x11002002, 0x11002201, 0x11012000, 0x11012101, + 0x11012200, 0x11022001, 0x11022100, 0x11022102, 0x11022201, 0x12002101, 0x12012001, 0x12012100, + 0x12012102, 0x12012201, 0x12022101, 0x10002011, 0x10002111, 0x10002112, 0x10002212, 0x10012010, + 0x10012110, 0x10012111, 0x10012210, 0x10022011, 0x10022110, 0x10022112, 0x11002010, 0x11002111, + 0x11002212, 0x11012011, 0x11012012, 0x11012110, 0x11012111, 0x11012112, 0x11012211, 0x11022010, + 0x11022012, 0x11022111, 0x11022112, 0x11022212, 0x12002112, 0x12002211, 0x12012012, 0x12012111, + 0x12012112, 0x12012210, 0x12022011, 0x12022110, 0x12022112, 0x12022211, 0x10012122, 0x11002120, + 0x11002122, 0x11002221, 0x11012121, 0x11012220, 0x11012222, 0x11022120, 0x11022221, 0x12012120, + 0x12022121, 0x10100001, 0x10100100, 0x10100101, 0x10100102, 0x10100201, 0x10110002, 0x10110101, + 0x10110202, 0x10120001, 0x10120100, 0x10120201, 0x11100000, 0x11100101, 0x11100200, 0x11110001, + 0x11110100, 0x11110101, 0x11110102, 0x11110201, 0x11120101, 0x11120200, 0x12100102, 0x12100201, + 0x12110101, 0x12110200, 0x12120000, 0x12120001, 0x12120102, 0x12120201, 0x10100111, 0x10100210, + 0x10100211, 0x10100212, 0x10110011, 0x10110110, 0x10110111, 0x10110112, 0x10110210, 0x10110211, + 0x10120010, 0x10120111, 0x10120112, 0x10120210, 0x10120212, 0x11100011, 0x11100110, 0x11100111, + 0x11100112, 0x11100211, 0x11110010, 0x11110011, 0x11110012, 0x11110110, 0x11110111, 0x11110112, + 0x11110210, 0x11110211, 0x11110212, 0x11120011, 0x11120110, 0x11120111, 0x11120112, 0x11120211, + 0x12100012, 0x12100111, 0x12110011, 0x12110110, 0x12110111, 0x12110112, 0x12110211, 0x12120010, + 0x12120111, 0x12120212, 0x10100021, 0x10100122, 0x10110022, 0x10110121, 0x10110222, 0x10120021, + 0x10120120, 0x11100022, 0x11100121, 0x11100222, 0x11110021, 0x11110120, 0x11110121, 0x11110122, + 0x11110221, 0x11120022, 0x11120121, 0x12100121, 0x12110020, 0x12110022, 0x12110121, 0x12110221, + 0x12110222, 0x12120120, 0x10101100, 0x10101101, 0x10111001, 0x10111100, 0x10111101, 0x10111102, + 0x10111200, 0x10111201, 0x10121001, 0x10121101, 0x10121200, 0x10121202, 0x11101001, 0x11101100, + 0x11101101, 0x11101102, 0x11101201, 0x11101202, 0x11111000, 0x11111001, 0x11111100, 0x11111101, + 0x11111102, 0x11111200, 0x11111201, 0x11111202, 0x11121001, 0x11121002, 0x11121100, 0x11121101, + 0x11121102, 0x11121201, 0x12101000, 0x12101200, 0x12101202, 0x12111001, 0x12111100, 0x12111101, + 0x12111102, 0x12111201, 0x12121001, 0x12121100, 0x12121101, 0x12121202, 0x10101011, 0x10101012, + 0x10101110, 0x10101111, 0x10101112, 0x10101211, 0x10111010, 0x10111011, 0x10111012, 0x10111110, + 0x10111111, 0x10111112, 0x10111211, 0x10111212, 0x10121011, 0x10121110, 0x10121111, 0x10121112, + 0x10121211, 0x11101010, 0x11101011, 0x11101012, 0x11101110, 0x11101111, 0x11101112, 0x11101210, + 0x11101211, 0x11111010, 0x11111011, 0x11111012, 0x11111110, 0x11111111, 0x11111112, 0x11111210, + 0x11111211, 0x11111212, 0x11121010, 0x11121011, 0x11121110, 0x11121111, 0x11121112, 0x11121210, + 0x11121211, 0x11121212, 0x12101011, 0x12101110, 0x12101111, 0x12101211, 0x12101212, 0x12111010, + 0x12111011, 0x12111110, 0x12111111, 0x12111112, 0x12111210, 0x12111211, 0x12121011, 0x12121110, + 0x12121111, 0x12121112, 0x12121211, 0x10101020, 0x10101021, 0x10101022, 0x10101120, 0x10101122, + 0x10101220, 0x10101221, 0x10111021, 0x10111120, 0x10111121, 0x10111220, 0x10111221, 0x10121020, + 0x10121021, 0x10121022, 0x10121120, 0x10121121, 0x10121122, 0x10121220, 0x10121221, 0x11101021, + 0x11101121, 0x11101122, 0x11101220, 0x11101221, 0x11101222, 0x11111020, 0x11111021, 0x11111022, + 0x11111120, 0x11111121, 0x11111122, 0x11111220, 0x11111221, 0x11111222, 0x11121021, 0x11121120, + 0x11121121, 0x11121221, 0x12101022, 0x12101121, 0x12101122, 0x12101220, 0x12101221, 0x12101222, + 0x12111021, 0x12111121, 0x12111222, 0x12121022, 0x12121121, 0x12121122, 0x12121220, 0x12121221, + 0x10102100, 0x10102101, 0x10102102, 0x10102201, 0x10112000, 0x10112101, 0x10112200, 0x10122001, + 0x10122202, 0x11102101, 0x11102200, 0x11102202, 0x11112001, 0x11112100, 0x11112101, 0x11112102, + 0x11112200, 0x11112201, 0x11122000, 0x11122002, 0x11122100, 0x11122101, 0x12102002, 0x12102201, + 0x12112000, 0x12112002, 0x12112101, 0x12112200, 0x12122001, 0x12122201, 0x10102011, 0x10102012, + 0x10102111, 0x10102212, 0x10112011, 0x10112110, 0x10112111, 0x10112112, 0x10112211, 0x10122111, + 0x11102011, 0x11102110, 0x11102111, 0x11102112, 0x11102211, 0x11112010, 0x11112011, 0x11112012, + 0x11112110, 0x11112111, 0x11112112, 0x11112210, 0x11112211, 0x11112212, 0x11122011, 0x11122110, + 0x11122111, 0x11122112, 0x11122211, 0x12102011, 0x12102111, 0x12102211, 0x12112011, 0x12112110, + 0x12112111, 0x12112112, 0x12112210, 0x12112211, 0x12122111, 0x10102120, 0x10102220, 0x10112121, + 0x10112222, 0x10122020, 0x10122121, 0x10122122, 0x10122221, 0x11102121, 0x11102220, 0x11102221, + 0x11112021, 0x11112121, 0x11112122, 0x11112220, 0x11112221, 0x11122022, 0x11122121, 0x11122220, + 0x11122222, 0x12102021, 0x12102222, 0x12112022, 0x12112121, 0x12112122, 0x12112220, 0x12112222, + 0x12122021, 0x10200101, 0x10210100, 0x10210102, 0x10210201, 0x10220101, 0x11200100, 0x11210000, + 0x11210101, 0x11210102, 0x11210200, 0x11210202, 0x11220001, 0x11220100, 0x11220102, 0x11220201, + 0x12200001, 0x12210102, 0x12220101, 0x10200011, 0x10200110, 0x10200112, 0x10200211, 0x10210012, + 0x10210111, 0x10220011, 0x10220012, 0x10220112, 0x10220211, 0x11200111, 0x11200211, 0x11210011, + 0x11210111, 0x11210112, 0x11210211, 0x11220111, 0x11220112, 0x11220212, 0x12200110, 0x12200212, + 0x12210012, 0x12210111, 0x12220011, 0x12220112, 0x12220211, 0x10210021, 0x10210122, 0x10210221, + 0x11200020, 0x11200021, 0x11200122, 0x11210121, 0x11210122, 0x11210220, 0x11220020, 0x12200121, + 0x12210021, 0x12210122, 0x12220121, 0x10211001, 0x10211002, 0x10211101, 0x10211102, 0x10211202, + 0x10221001, 0x10221102, 0x10221201, 0x11201000, 0x11201002, 0x11201101, 0x11201200, 0x11201202, + 0x11211001, 0x11211100, 0x11211101, 0x11211102, 0x11211201, 0x11211202, 0x11221000, 0x11221002, + 0x11221101, 0x12201100, 0x12201101, 0x12201201, 0x12211000, 0x12211002, 0x12211100, 0x12211101, + 0x12211102, 0x12211200, 0x12211202, 0x12221001, 0x12221100, 0x12221201, 0x10201111, 0x10201210, + 0x10201212, 0x10211011, 0x10211111, 0x10211112, 0x10211211, 0x11201110, 0x11201111, 0x11201112, + 0x11201211, 0x11211010, 0x11211011, 0x11211110, 0x11211111, 0x11211112, 0x11211211, 0x11221011, + 0x11221110, 0x11221111, 0x11221112, 0x11221211, 0x12201112, 0x12201211, 0x12201212, 0x12211011, + 0x12211111, 0x12211112, 0x12211211, 0x12211212, 0x12221012, 0x12221111, 0x12221112, 0x12221210, + 0x10201022, 0x10201221, 0x10211121, 0x10221020, 0x10221122, 0x10221220, 0x10221221, 0x11201020, + 0x11201121, 0x11201220, 0x11201222, 0x11211021, 0x11211120, 0x11211121, 0x11211122, 0x11211220, + 0x11211222, 0x11221020, 0x11221121, 0x11221220, 0x12201020, 0x12201022, 0x12201121, 0x12201222, + 0x12211120, 0x12211122, 0x12211220, 0x12211221, 0x12221020, 0x12221120, 0x12221122, 0x12221222, + 0x10212102, 0x10212201, 0x10222101, 0x11202001, 0x11212002, 0x11212101, 0x11212202, 0x11222001, + 0x11222201, 0x12202101, 0x12212001, 0x12212200, 0x12222102, 0x10202011, 0x10202110, 0x10212010, + 0x10212111, 0x10222011, 0x10222110, 0x10222112, 0x10222211, 0x11202010, 0x11202011, 0x11202111, + 0x11202112, 0x11202210, 0x11212011, 0x11212110, 0x11212111, 0x11212112, 0x11212211, 0x11222010, + 0x11222111, 0x11222212, 0x12202012, 0x12202110, 0x12202212, 0x12212111, 0x12222011, 0x12222110, + 0x12222111, 0x12222211, 0x10212021, 0x10212122, 0x10212220, 0x11202021, 0x11202120, 0x11202221, + 0x11212020, 0x11212121, 0x11212220, 0x11212222, 0x11222120, 0x11222121, 0x11222221, 0x12202122, + 0x12212120, 0x12212220, 0x12212222, 0x12222122, 0x20000000, 0x20000002, 0x20000200, 0x20000202, + 0x20020000, 0x20020002, 0x20020200, 0x20020202, 0x21000101, 0x21010000, 0x21010001, 0x21010100, + 0x21010102, 0x21010201, 0x21020101, 0x22000000, 0x22000002, 0x22000200, 0x22000202, 0x22010101, + 0x22020000, 0x22020002, 0x22020200, 0x22020202, 0x20000111, 0x20010011, 0x20010110, 0x20010112, + 0x20010211, 0x20020111, 0x21000011, 0x21000110, 0x21000211, 0x21010010, 0x21010012, 0x21010111, + 0x21010112, 0x21010210, 0x21010211, 0x21020110, 0x21020112, 0x21020211, 0x22000111, 0x22000211, + 0x22010110, 0x22010112, 0x22010211, 0x22020111, 0x20000020, 0x20000022, 0x20000220, 0x20000222, + 0x20010121, 0x20020020, 0x20020022, 0x20020220, 0x20020222, 0x21010021, 0x21010120, 0x21010221, + 0x21020121, 0x22000020, 0x22000022, 0x22000220, 0x22000222, 0x22010121, 0x22020020, 0x22020022, + 0x22020220, 0x22020222, 0x20011100, 0x20011201, 0x21001001, 0x21001100, 0x21011001, 0x21011101, + 0x21011202, 0x21021001, 0x21021100, 0x21021201, 0x22011100, 0x22011201, 0x20001011, 0x20001211, + 0x20011012, 0x20011111, 0x20011212, 0x20021112, 0x20021211, 0x21001010, 0x21001011, 0x21001111, + 0x21001210, 0x21011011, 0x21011110, 0x21011111, 0x21011112, 0x21011211, 0x21011212, 0x21021111, + 0x21021112, 0x21021210, 0x21021212, 0x22001011, 0x22001110, 0x22001112, 0x22001211, 0x22011010, + 0x22011012, 0x22011111, 0x22011210, 0x22021112, 0x20011021, 0x20011122, 0x20011221, 0x20021121, + 0x21001021, 0x21001120, 0x21001221, 0x21001222, 0x21011020, 0x21011121, 0x21011221, 0x21011222, + 0x21021021, 0x21021122, 0x21021222, 0x22001121, 0x22011021, 0x22011222, 0x22021120, 0x20002000, + 0x20002002, 0x20002200, 0x20002202, 0x20012101, 0x20022000, 0x20022002, 0x20022200, 0x20022202, + 0x21002001, 0x21002101, 0x21012001, 0x21012100, 0x21012201, 0x21022101, 0x21022201, 0x22002000, + 0x22002002, 0x22002200, 0x22002202, 0x22012101, 0x22022000, 0x22022002, 0x22022200, 0x22022202, + 0x20002111, 0x20002112, 0x20012011, 0x20012110, 0x20012112, 0x20022111, 0x21002011, 0x21002110, + 0x21002112, 0x21002211, 0x21012010, 0x21012012, 0x21012111, 0x21012212, 0x21022011, 0x21022110, + 0x22002111, 0x22012112, 0x22012211, 0x22022111, 0x20002020, 0x20002022, 0x20002220, 0x20002222, + 0x20012121, 0x20022020, 0x20022022, 0x20022220, 0x20022222, 0x21002121, 0x21012021, 0x21012120, + 0x21012122, 0x22002020, 0x22002022, 0x22002220, 0x22002222, 0x22012121, 0x22022020, 0x22022022, + 0x22022220, 0x22022222, 0x20100101, 0x20110001, 0x20110102, 0x20110200, 0x20110201, 0x20120101, + 0x21100001, 0x21100102, 0x21100201, 0x21110101, 0x21110200, 0x21110202, 0x21120201, 0x21120202, + 0x22100101, 0x22110001, 0x22110100, 0x22110102, 0x22110201, 0x22120101, 0x20100011, 0x20100110, + 0x20100112, 0x20100211, 0x20110010, 0x20110111, 0x20110210, 0x20110212, 0x20120011, 0x20120110, + 0x20120112, 0x20120211, 0x21100010, 0x21100111, 0x21110010, 0x21110011, 0x21110110, 0x21110111, + 0x21110112, 0x21110211, 0x21120012, 0x21120111, 0x22100110, 0x22100112, 0x22110012, 0x22110111, + 0x22110210, 0x22120011, 0x22120110, 0x22120112, 0x22120211, 0x20100121, 0x20110021, 0x20110120, + 0x20110221, 0x20120121, 0x21100120, 0x21100122, 0x21100221, 0x21110020, 0x21110022, 0x21110121, + 0x21110220, 0x21120122, 0x21120221, 0x22100121, 0x22110120, 0x22110122, 0x22120221, 0x20101001, + 0x20101100, 0x20101102, 0x20111000, 0x20111101, 0x20111200, 0x20121102, 0x21101000, 0x21101202, + 0x21111001, 0x21111100, 0x21111101, 0x21111102, 0x21111200, 0x21111201, 0x21121000, 0x21121001, + 0x21121002, 0x21121101, 0x22101100, 0x22101102, 0x22111002, 0x22111100, 0x22111101, 0x22111200, + 0x22121001, 0x22121201, 0x20101010, 0x20101111, 0x20101210, 0x20101212, 0x20111010, 0x20111011, + 0x20111110, 0x20111111, 0x20111112, 0x20111211, 0x20121011, 0x20121111, 0x20121211, 0x20121212, + 0x21101011, 0x21101110, 0x21101111, 0x21101112, 0x21101211, 0x21111010, 0x21111011, 0x21111012, + 0x21111110, 0x21111111, 0x21111112, 0x21111210, 0x21111211, 0x21111212, 0x21121011, 0x21121110, + 0x21121111, 0x21121112, 0x21121211, 0x22101011, 0x22101111, 0x22101210, 0x22111011, 0x22111012, + 0x22111110, 0x22111111, 0x22111112, 0x22111211, 0x22111212, 0x22121010, 0x22121012, 0x22121111, + 0x22121210, 0x22121212, 0x20101021, 0x20101120, 0x20111020, 0x20111121, 0x20111221, 0x20121020, + 0x20121122, 0x20121221, 0x21101121, 0x21101220, 0x21101221, 0x21111021, 0x21111022, 0x21111121, + 0x21111122, 0x21111221, 0x21121121, 0x21121220, 0x22101022, 0x22101120, 0x22101221, 0x22101222, + 0x22111022, 0x22111120, 0x22111121, 0x22121120, 0x22121122, 0x22121221, 0x20102101, 0x20112102, + 0x20112201, 0x20122101, 0x21102001, 0x21102102, 0x21112000, 0x21112002, 0x21112101, 0x21112102, + 0x21112202, 0x21122100, 0x21122101, 0x22102101, 0x22112001, 0x22112102, 0x22112201, 0x22122101, + 0x20102110, 0x20102112, 0x20102211, 0x20112010, 0x20112012, 0x20112111, 0x20112210, 0x20112212, + 0x20122010, 0x20122011, 0x20122110, 0x20122112, 0x21102010, 0x21102012, 0x21102111, 0x21102210, + 0x21102212, 0x21112011, 0x21112110, 0x21112111, 0x21112112, 0x21112211, 0x21122012, 0x21122111, + 0x21122112, 0x21122212, 0x22102011, 0x22102110, 0x22112010, 0x22112012, 0x22112111, 0x22112212, + 0x22122011, 0x22122112, 0x20102121, 0x20112121, 0x20122121, 0x21102120, 0x21102122, 0x21102221, + 0x21112020, 0x21112121, 0x21112220, 0x21122021, 0x22102121, 0x22112021, 0x22112120, 0x22112121, + 0x22112122, 0x20200000, 0x20200002, 0x20200200, 0x20200202, 0x20210101, 0x20220000, 0x20220002, + 0x20220200, 0x20220202, 0x21200101, 0x21210001, 0x21210100, 0x21210102, 0x21210201, 0x22200000, + 0x22200002, 0x22200200, 0x22200202, 0x22210101, 0x22220000, 0x22220002, 0x22220200, 0x22220202, + 0x20200111, 0x20200211, 0x20210011, 0x20210110, 0x20210112, 0x20210211, 0x20210212, 0x21200112, + 0x21200211, 0x21210011, 0x21210111, 0x21210210, 0x21210212, 0x21220011, 0x21220110, 0x22200111, + 0x22210010, 0x22210012, 0x22210112, 0x22210211, 0x20200022, 0x20200220, 0x20200222, 0x20210020, + 0x20210221, 0x20220022, 0x20220220, 0x20220222, 0x21200121, 0x21210021, 0x21210122, 0x21210221, + 0x21220121, 0x22200020, 0x22200022, 0x22200220, 0x22200222, 0x22210121, 0x22220020, 0x22220022, + 0x22220220, 0x22220222, 0x20211201, 0x20221101, 0x21201001, 0x21201100, 0x21211000, 0x21211100, + 0x21211101, 0x21211200, 0x21211202, 0x21221001, 0x21221101, 0x21221102, 0x21221200, 0x21221201, + 0x22201101, 0x20201112, 0x20201211, 0x20211010, 0x20211012, 0x20211111, 0x20211210, 0x20221112, + 0x20221211, 0x21201012, 0x21201111, 0x21211011, 0x21211110, 0x21211111, 0x21211112, 0x21211211, + 0x21221111, 0x21221212, 0x22201011, 0x22201110, 0x22201111, 0x22201112, 0x22201211, 0x22211012, + 0x22211111, 0x22211210, 0x20201121, 0x20211021, 0x20211122, 0x20211222, 0x20221021, 0x20221121, + 0x21201120, 0x21201122, 0x21201222, 0x21211022, 0x21211121, 0x21211122, 0x21211220, 0x21221020, + 0x21221022, 0x22201122, 0x22211020, 0x22211121, 0x22211122, 0x22211221, 0x22221021, 0x22221120, + 0x22221122, 0x20202000, 0x20202002, 0x20202200, 0x20202202, 0x20222000, 0x20222002, 0x20222200, + 0x20222202, 0x21212001, 0x21212100, 0x21212102, 0x21212201, 0x22202000, 0x22202002, 0x22202200, + 0x22202202, 0x22212101, 0x22222000, 0x22222002, 0x22222200, 0x22222202, 0x20202111, 0x20212110, + 0x20212211, 0x20222011, 0x20222111, 0x21202011, 0x21212010, 0x21212111, 0x21212212, 0x21222011, + 0x21222112, 0x21222211, 0x22212010, 0x22212112, 0x20202020, 0x20202022, 0x20202220, 0x20202222, + 0x20222020, 0x20222022, 0x20222220, 0x20222222, 0x21212021, 0x21212120, 0x21212122, 0x22202020, + 0x22202022, 0x22202220, 0x22202222, 0x22212121, 0x22222020, 0x22222022, 0x22222220, 0x22222222, +GGML_TABLE_END() +#endif + +#endif // GGML_COMMON_IMPL +#endif // GGML_COMMON_IMPL diff --git a/libs/voice2/src/metal/ggml/ggml-metal-impl.h b/libs/voice2/src/metal/ggml/ggml-metal-impl.h new file mode 100644 index 0000000000..383e0d6e93 --- /dev/null +++ b/libs/voice2/src/metal/ggml/ggml-metal-impl.h @@ -0,0 +1,1054 @@ +#ifndef GGML_METAL_IMPL +#define GGML_METAL_IMPL + +// kernel parameters for mat-vec threadgroups +// +// N_R0: number of src0 rows to process per simdgroup +// N_SG: number of simdgroups per threadgroup +// +// TODO: for optimal performance, become function of the device and work size + +#define N_R0_Q4_0 4 +#define N_SG_Q4_0 2 + +#define N_R0_Q4_1 4 +#define N_SG_Q4_1 2 + +#define N_R0_Q5_0 4 +#define N_SG_Q5_0 2 + +#define N_R0_Q5_1 4 +#define N_SG_Q5_1 2 + +#define N_R0_Q8_0 2 +#define N_SG_Q8_0 4 + +#define N_R0_MXFP4 2 +#define N_SG_MXFP4 2 + +#define N_R0_Q2_K 4 +#define N_SG_Q2_K 2 + +#define N_R0_Q3_K 2 +#define N_SG_Q3_K 2 + +#define N_R0_Q4_K 2 +#define N_SG_Q4_K 2 + +#define N_R0_Q5_K 2 +#define N_SG_Q5_K 2 + +#define N_R0_Q6_K 2 +#define N_SG_Q6_K 2 + +#define N_R0_IQ1_S 4 +#define N_SG_IQ1_S 2 + +#define N_R0_IQ1_M 4 +#define N_SG_IQ1_M 2 + +#define N_R0_IQ2_XXS 4 +#define N_SG_IQ2_XXS 2 + +#define N_R0_IQ2_XS 4 +#define N_SG_IQ2_XS 2 + +#define N_R0_IQ2_S 4 +#define N_SG_IQ2_S 2 + +#define N_R0_IQ3_XXS 4 +#define N_SG_IQ3_XXS 2 + +#define N_R0_IQ3_S 4 +#define N_SG_IQ3_S 2 + +#define N_R0_IQ4_NL 2 +#define N_SG_IQ4_NL 2 + +#define N_R0_IQ4_XS 2 +#define N_SG_IQ4_XS 2 + +// function constants offsets +#define FC_FLASH_ATTN_EXT_PAD 100 +#define FC_FLASH_ATTN_EXT_BLK 200 +#define FC_FLASH_ATTN_EXT 300 +#define FC_FLASH_ATTN_EXT_VEC 400 +#define FC_FLASH_ATTN_EXT_VEC_REDUCE 500 +#define FC_MUL_MV 600 +#define FC_MUL_MM 700 +#define FC_ROPE 800 +#define FC_SSM_CONV 900 +#define FC_SOLVE_TRI 1000 +#define FC_COUNT_EQUAL 1100 +#define FC_UNARY 1200 +#define FC_BIN 1300 +#define FC_SUM_ROWS 1400 + +// op-specific constants +#define OP_FLASH_ATTN_EXT_NQPSG 8 +#define OP_FLASH_ATTN_EXT_NCPSG 64 + +#define OP_FLASH_ATTN_EXT_VEC_NQPSG 1 +#define OP_FLASH_ATTN_EXT_VEC_NCPSG 32 + +#define OP_UNARY_NUM_SCALE 10 +#define OP_UNARY_NUM_FILL 11 +#define OP_UNARY_NUM_CLAMP 12 +#define OP_UNARY_NUM_SQR 13 +#define OP_UNARY_NUM_SQRT 14 +#define OP_UNARY_NUM_SIN 15 +#define OP_UNARY_NUM_COS 16 +#define OP_UNARY_NUM_LOG 17 +#define OP_UNARY_NUM_LEAKY_RELU 18 + +#define OP_UNARY_NUM_TANH 100 +#define OP_UNARY_NUM_RELU 101 +#define OP_UNARY_NUM_SIGMOID 102 +#define OP_UNARY_NUM_GELU 103 +#define OP_UNARY_NUM_GELU_ERF 104 +#define OP_UNARY_NUM_GELU_QUICK 105 +#define OP_UNARY_NUM_SILU 106 +#define OP_UNARY_NUM_ELU 107 +#define OP_UNARY_NUM_NEG 108 +#define OP_UNARY_NUM_ABS 109 +#define OP_UNARY_NUM_SGN 110 +#define OP_UNARY_NUM_STEP 111 +#define OP_UNARY_NUM_HARDSWISH 112 +#define OP_UNARY_NUM_HARDSIGMOID 113 +#define OP_UNARY_NUM_EXP 114 +#define OP_UNARY_NUM_SOFTPLUS 115 +#define OP_UNARY_NUM_EXPM1 116 + +#define OP_SUM_ROWS_NUM_SUM_ROWS 10 +#define OP_SUM_ROWS_NUM_MEAN 11 + +// kernel argument structs +// +// - element counters (e.g. ne00) typically use int32_t to reduce register usage +// however, be careful from int overflows when using those in the kernel implementation +// +// - strides (e.g. nb00) use uint64_t + +typedef struct { + int32_t ne00; + int32_t ne01; + int32_t ne02; + int32_t ne03; + uint64_t nb00; + uint64_t nb01; + uint64_t nb02; + uint64_t nb03; + int32_t ne10; + int32_t ne11; + int32_t ne12; + int32_t ne13; + uint64_t nb10; + uint64_t nb11; + uint64_t nb12; + uint64_t nb13; + int32_t ne0; + int32_t ne1; + int32_t ne2; + int32_t ne3; + uint64_t nb0; + uint64_t nb1; + uint64_t nb2; + uint64_t nb3; + int32_t dim; +} ggml_metal_kargs_concat; + +typedef struct { + int32_t ne00; + int32_t ne01; + int32_t ne02; + int32_t ne03; + uint64_t nb00; + uint64_t nb01; + uint64_t nb02; + uint64_t nb03; + int32_t ne0; + int32_t ne1; + int32_t ne2; + int32_t ne3; + uint64_t nb0; + uint64_t nb1; + uint64_t nb2; + uint64_t nb3; + float slope; + float scale; + float bias; + float val; + float min; + float max; +} ggml_metal_kargs_unary; + +typedef struct { + int32_t ne00; + int32_t ne01; + int32_t ne02; + int32_t ne03; + uint64_t nb00; + uint64_t nb01; + uint64_t nb02; + uint64_t nb03; + int32_t ne10; + int32_t ne11; + int32_t ne12; + int32_t ne13; + uint64_t nb10; + uint64_t nb11; + uint64_t nb12; + uint64_t nb13; + int32_t ne0; + int32_t ne1; + int32_t ne2; + int32_t ne3; + uint64_t nb0; + uint64_t nb1; + uint64_t nb2; + uint64_t nb3; + uint64_t offs; + uint64_t o1[8]; +} ggml_metal_kargs_bin; + +typedef struct { + int64_t ne0; + int64_t ne1; + size_t nb01; + size_t nb02; + size_t nb11; + size_t nb21; +} ggml_metal_kargs_add_id; + +typedef struct { + int32_t ne00; + int32_t ne01; + int32_t ne02; + int32_t ne03; + uint64_t nb00; + uint64_t nb01; + uint64_t nb02; + uint64_t nb03; + int32_t ne0; + int32_t ne1; + int32_t ne2; + int32_t ne3; + uint64_t nb0; + uint64_t nb1; + uint64_t nb2; + uint64_t nb3; +} ggml_metal_kargs_repeat; + +typedef struct { + int64_t nk0; + int64_t ne00; + int64_t ne01; + int64_t ne02; + int64_t ne03; + uint64_t nb00; + uint64_t nb01; + uint64_t nb02; + uint64_t nb03; + int64_t ne0; + int64_t ne1; + int64_t ne2; + int64_t ne3; + uint64_t nb0; + uint64_t nb1; + uint64_t nb2; + uint64_t nb3; +} ggml_metal_kargs_cpy; + +typedef struct { + int64_t ne10; + int64_t ne11; + int64_t ne12; + uint64_t nb10; + uint64_t nb11; + uint64_t nb12; + uint64_t nb13; + uint64_t nb1; + uint64_t nb2; + uint64_t nb3; + uint64_t offs; + bool inplace; +} ggml_metal_kargs_set; + +typedef struct { + int32_t ne00; + int32_t ne01; + int32_t ne02; + int32_t ne03; + uint64_t nb00; + uint64_t nb01; + uint64_t nb02; + uint64_t nb03; + int32_t ne0; + int32_t ne1; + int32_t ne2; + int32_t ne3; + uint64_t nb0; + uint64_t nb1; + uint64_t nb2; + uint64_t nb3; + int32_t n_past; + int32_t n_dims; + int32_t n_ctx_orig; + float freq_base; + float freq_scale; + float ext_factor; + float attn_factor; + float beta_fast; + float beta_slow; + int32_t sect_0; + int32_t sect_1; + int32_t sect_2; + int32_t sect_3; + bool src2; +} ggml_metal_kargs_rope; + +typedef struct { + int32_t ne11; + int32_t ne_12_2; // assume K and V are same shape + int32_t ne_12_3; + uint64_t nb11; + uint64_t nb12; + uint64_t nb13; + uint64_t nb21; + uint64_t nb22; + uint64_t nb23; + int32_t ne31; + int32_t ne32; + int32_t ne33; + uint64_t nb31; + uint64_t nb32; + uint64_t nb33; +} ggml_metal_kargs_flash_attn_ext_pad; + +typedef struct { + int32_t ne01; + int32_t ne30; + int32_t ne31; + int32_t ne32; + int32_t ne33; + uint64_t nb31; + uint64_t nb32; + uint64_t nb33; +} ggml_metal_kargs_flash_attn_ext_blk; + +typedef struct { + int32_t ne01; + int32_t ne02; + int32_t ne03; + uint64_t nb01; + uint64_t nb02; + uint64_t nb03; + int32_t ne11; + int32_t ne_12_2; // assume K and V are same shape + int32_t ne_12_3; + int32_t ns10; + uint64_t nb11; + uint64_t nb12; + uint64_t nb13; + int32_t ns20; + uint64_t nb21; + uint64_t nb22; + uint64_t nb23; + int32_t ne31; + int32_t ne32; + int32_t ne33; + uint64_t nb31; + uint64_t nb32; + uint64_t nb33; + int32_t ne1; + int32_t ne2; + int32_t ne3; + float scale; + float max_bias; + float m0; + float m1; + int32_t n_head_log2; + float logit_softcap; +} ggml_metal_kargs_flash_attn_ext; + +typedef struct { + int32_t ne01; + int32_t ne02; + int32_t ne03; + uint64_t nb01; + uint64_t nb02; + uint64_t nb03; + int32_t ne11; + int32_t ne_12_2; // assume K and V are same shape + int32_t ne_12_3; + int32_t ns10; + uint64_t nb11; + uint64_t nb12; + uint64_t nb13; + int32_t ns20; + uint64_t nb21; + uint64_t nb22; + uint64_t nb23; + int32_t ne31; + int32_t ne32; + int32_t ne33; + uint64_t nb31; + uint64_t nb32; + uint64_t nb33; + int32_t ne1; + int32_t ne2; + int32_t ne3; + float scale; + float max_bias; + float m0; + float m1; + int32_t n_head_log2; + float logit_softcap; +} ggml_metal_kargs_flash_attn_ext_vec; + +typedef struct { + int32_t nrows; +} ggml_metal_kargs_flash_attn_ext_vec_reduce; + +typedef struct { + int32_t ne00; + int32_t ne02; + uint64_t nb01; + uint64_t nb02; + uint64_t nb03; + int32_t ne12; + uint64_t nb10; + uint64_t nb11; + uint64_t nb12; + uint64_t nb13; + int32_t ne0; + int32_t ne1; + int16_t r2; + int16_t r3; +} ggml_metal_kargs_mul_mm; + +typedef struct { + int32_t ne00; + int32_t ne01; + int32_t ne02; + uint64_t nb00; + uint64_t nb01; + uint64_t nb02; + uint64_t nb03; + int32_t ne10; + int32_t ne11; + int32_t ne12; + uint64_t nb10; + uint64_t nb11; + uint64_t nb12; + uint64_t nb13; + int32_t ne0; + int32_t ne1; + int32_t nr0; + int16_t r2; + int16_t r3; +} ggml_metal_kargs_mul_mv; + +typedef struct { + int32_t ne00; + int32_t ne01; + int32_t ne02; + uint64_t nb00; + uint64_t nb01; + uint64_t nb02; + uint64_t nb03; + int32_t ne10; + int32_t ne11; + int32_t ne12; + uint64_t nb10; + uint64_t nb11; + uint64_t nb12; + uint64_t nb13; + int32_t ne0; + int32_t ne1; + int16_t r2; + int16_t r3; +} ggml_metal_kargs_mul_mv_ext; + +typedef struct { + int32_t ne02; + int32_t ne10; + int32_t ne11; // n_expert_used (bcast) + uint64_t nb11; + uint64_t nb12; + int32_t ne21; // n_tokens + int32_t ne20; // n_expert_used + uint64_t nb21; +} ggml_metal_kargs_mul_mm_id_map0; + +typedef struct { + int32_t ne00; + int32_t ne02; + uint64_t nb01; + uint64_t nb02; + uint64_t nb03; + int32_t ne11; + uint64_t nb10; + uint64_t nb11; + uint64_t nb12; + uint64_t nb13; + int32_t ne20; + int32_t ne21; + int32_t ne0; + int32_t ne1; + int16_t r2; + int16_t r3; +} ggml_metal_kargs_mul_mm_id; + +typedef struct { + int32_t nei0; + int32_t nei1; + uint64_t nbi1; + int32_t ne00; + int32_t ne01; + int32_t ne02; + uint64_t nb00; + uint64_t nb01; + uint64_t nb02; + int32_t ne10; + int32_t ne11; + int32_t ne12; + int32_t ne13; + uint64_t nb10; + uint64_t nb11; + uint64_t nb12; + int32_t ne0; + int32_t ne1; + uint64_t nb1; + int32_t nr0; +} ggml_metal_kargs_mul_mv_id; + +// NORM +// RMS_NORM +typedef struct { + int32_t ne00; + int32_t ne00_t; + uint64_t nb1; + uint64_t nb2; + uint64_t nb3; + float eps; + int32_t nef1[3]; + int32_t nef2[3]; + int32_t nef3[3]; + uint64_t nbf1[3]; + uint64_t nbf2[3]; + uint64_t nbf3[3]; +} ggml_metal_kargs_norm; + +typedef struct { + int32_t ne00; + int32_t ne01; + int32_t ne02; + int32_t ne03; + uint64_t nb00; + uint64_t nb01; + uint64_t nb02; + uint64_t nb03; + int32_t ne0; + int32_t ne1; + int32_t ne2; + int32_t ne3; + uint64_t nb0; + uint64_t nb1; + uint64_t nb2; + uint64_t nb3; + float eps; +} ggml_metal_kargs_l2_norm; + +typedef struct { + int64_t ne00; + int64_t ne01; + int64_t ne02; + uint64_t nb00; + uint64_t nb01; + uint64_t nb02; + int32_t ngrp; + float eps; +} ggml_metal_kargs_group_norm; + +typedef struct { + int32_t IC; + int32_t IL; + int32_t K; + int32_t s0; + uint64_t nb0; + uint64_t nb1; +} ggml_metal_kargs_conv_transpose_1d; + +typedef struct { + int32_t IC; + int32_t IH; + int32_t IW; + int32_t KH; + int32_t KW; + int32_t OC; + int32_t s0; + uint64_t nb0; + uint64_t nb1; + uint64_t nb2; +} ggml_metal_kargs_conv_transpose_2d; + +typedef struct { + uint64_t nb00; + uint64_t nb01; + uint64_t nb02; + uint64_t nb03; + uint64_t nb10; + uint64_t nb11; + uint64_t nb12; + uint64_t nb13; + uint64_t nb0; + uint64_t nb1; + uint64_t nb2; + uint64_t nb3; + int32_t IW; + int32_t IH; + int32_t KW; + int32_t KH; + int32_t IC; + int32_t OC; + int32_t OW; + int32_t OH; + int32_t N; + int32_t s0; + int32_t s1; + int32_t p0; + int32_t p1; + int32_t d0; + int32_t d1; +} ggml_metal_kargs_conv_2d; + +typedef struct { + uint64_t ofs0; + uint64_t ofs1; + int32_t IW; + int32_t IH; + int32_t CHW; + int32_t s0; + int32_t s1; + int32_t p0; + int32_t p1; + int32_t d0; + int32_t d1; + int32_t N; + int32_t KH; + int32_t KW; + int32_t KHW; // KH * KW, pre-computed on CPU to save GPU resources +} ggml_metal_kargs_im2col; + +typedef struct{ + int32_t ne00; + uint64_t nb01; + int32_t ne10; + uint64_t nb11; + int32_t ne0; + uint64_t nb1; + int32_t i00; + int32_t i10; + float alpha; + float limit; +} ggml_metal_kargs_glu; + +typedef struct { + uint64_t np; +} ggml_metal_kargs_sum; + +typedef struct { + int64_t ne00; + int64_t ne01; + int64_t ne02; + int64_t ne03; + uint64_t nb00; + uint64_t nb01; + uint64_t nb02; + uint64_t nb03; + int64_t ne0; + int64_t ne1; + int64_t ne2; + int64_t ne3; + uint64_t nb0; + uint64_t nb1; + uint64_t nb2; + uint64_t nb3; +} ggml_metal_kargs_sum_rows; + +typedef struct { + int64_t ne00; + int64_t ne01; + int64_t ne02; + int64_t ne03; + uint64_t nb00; + uint64_t nb01; + uint64_t nb02; + uint64_t nb03; + int64_t net0; + int64_t net1; + int64_t net2; + int64_t net3; + uint64_t nbt0; + uint64_t nbt1; + uint64_t nbt2; + uint64_t nbt3; + bool outb; +} ggml_metal_kargs_cumsum_blk; + +typedef struct { + int64_t ne00; + int64_t ne01; + int64_t ne02; + int64_t ne03; + uint64_t nb00; + uint64_t nb01; + uint64_t nb02; + uint64_t nb03; + int64_t net0; + int64_t net1; + int64_t net2; + int64_t net3; + uint64_t nbt0; + uint64_t nbt1; + uint64_t nbt2; + uint64_t nbt3; +} ggml_metal_kargs_cumsum_add; + +typedef struct { + int32_t ne00; + int32_t ne01; + int32_t ne02; + uint64_t nb01; + uint64_t nb02; + uint64_t nb03; + int32_t ne11; + int32_t ne12; + int32_t ne13; + uint64_t nb11; + uint64_t nb12; + uint64_t nb13; + uint64_t nb1; + uint64_t nb2; + uint64_t nb3; + float scale; + float max_bias; + float m0; + float m1; + int32_t n_head_log2; +} ggml_metal_kargs_soft_max; + +typedef struct { + int64_t ne00; + int64_t ne01; + int64_t ne02; + uint64_t nb00; + uint64_t nb01; + uint64_t nb02; + int64_t ne10; + int64_t ne11; + uint64_t nb10; + uint64_t nb11; + int64_t ne0; + int64_t ne1; + int64_t ne2; + uint64_t nb0; + uint64_t nb1; + uint64_t nb2; +} ggml_metal_kargs_ssm_conv; + +typedef struct { + int64_t d_state; + int64_t d_inner; + int64_t n_head; + int64_t n_group; + int64_t n_seq_tokens; + int64_t n_seqs; + uint64_t s_off; + uint64_t nb00; + uint64_t nb01; + uint64_t nb02; + uint64_t nb03; + uint64_t nb10; + uint64_t nb11; + uint64_t nb12; + uint64_t ns12; + uint64_t nb13; + uint64_t nb20; + uint64_t nb21; + uint64_t ns21; + uint64_t nb22; + int64_t ne30; + uint64_t nb31; + uint64_t nb41; + uint64_t nb42; + uint64_t ns42; + uint64_t nb43; + uint64_t nb51; + uint64_t nb52; + uint64_t ns52; + uint64_t nb53; + uint64_t nb0; +} ggml_metal_kargs_ssm_scan; + +typedef struct { + int32_t ne00; + int32_t ne01; + int32_t ne02; + int32_t ne03; + uint64_t nb00; + uint64_t nb01; + uint64_t nb02; + uint64_t nb03; + int32_t ne10; + int32_t ne11; + int32_t ne12; + int32_t ne13; + uint64_t nb10; + uint64_t nb11; + uint64_t nb12; + uint64_t nb13; + int32_t ne0; + int32_t ne1; + int32_t ne2; + int32_t ne3; + uint64_t nb0; + uint64_t nb1; + uint64_t nb2; + uint64_t nb3; +} ggml_metal_kargs_solve_tri; + +typedef struct { + int32_t ne00t; + int32_t ne00; + uint64_t nb01; + uint64_t nb02; + uint64_t nb03; + int32_t ne10; + uint64_t nb10; + uint64_t nb11; + uint64_t nb12; + uint64_t nb1; + uint64_t nb2; + uint64_t nb3; +} ggml_metal_kargs_get_rows; + +typedef struct { + int32_t nk0; + int32_t ne01; + uint64_t nb01; + uint64_t nb02; + uint64_t nb03; + int32_t ne11; + int32_t ne12; + uint64_t nb10; + uint64_t nb11; + uint64_t nb12; + uint64_t nb1; + uint64_t nb2; + uint64_t nb3; +} ggml_metal_kargs_set_rows; + +typedef struct { + int32_t ne00; + int32_t ne01; + int32_t ne02; + int32_t ne03; + uint64_t nb00; + uint64_t nb01; + uint64_t nb02; + uint64_t nb03; + int32_t ne0; + int32_t ne1; + int32_t ne2; + int32_t ne3; + uint64_t nb0; + uint64_t nb1; + uint64_t nb2; + uint64_t nb3; +} ggml_metal_kargs_diag; + +typedef struct { + int64_t ne00; + int64_t ne01; + int64_t ne02; + int64_t ne03; + uint64_t nb00; + uint64_t nb01; + uint64_t nb02; + uint64_t nb03; + int64_t ne0; + int64_t ne1; + int64_t ne2; + int64_t ne3; + uint64_t nb0; + uint64_t nb1; + uint64_t nb2; + uint64_t nb3; + float sf0; + float sf1; + float sf2; + float sf3; +} ggml_metal_kargs_upscale; + +typedef struct { + int64_t ne00; + int64_t ne01; + int64_t ne02; + int64_t ne03; + uint64_t nb00; + uint64_t nb01; + uint64_t nb02; + uint64_t nb03; + int64_t ne0; + int64_t ne1; + int64_t ne2; + int64_t ne3; + uint64_t nb0; + uint64_t nb1; + uint64_t nb2; + uint64_t nb3; +} ggml_metal_kargs_pad; + +typedef struct { + int64_t ne00; + int64_t ne01; + int64_t ne02; + int64_t ne03; + uint64_t nb00; + uint64_t nb01; + uint64_t nb02; + uint64_t nb03; + int64_t ne0; + int64_t ne1; + int64_t ne2; + int64_t ne3; + uint64_t nb0; + uint64_t nb1; + uint64_t nb2; + uint64_t nb3; + int32_t p0; + int32_t p1; +} ggml_metal_kargs_pad_reflect_1d; + +typedef struct { + uint64_t nb1; + int dim; + int max_period; +} ggml_metal_kargs_timestep_embedding; + +typedef struct { + int32_t ne00; + int32_t ne01; + int32_t ne02; + int32_t ne03; + uint64_t nb00; + uint64_t nb01; + uint64_t nb02; + uint64_t nb03; + int32_t ne0; + int32_t ne1; + int32_t ne2; + int32_t ne3; + uint64_t nb0; + uint64_t nb1; + uint64_t nb2; + uint64_t nb3; +} ggml_metal_kargs_tri; + +typedef struct { + int32_t ne00; + int32_t ne01; + int32_t ne02; + int32_t ne03; + uint64_t nb00; + uint64_t nb01; + uint64_t nb02; + uint64_t nb03; + int32_t ne0; + int32_t ne1; + int32_t ne2; + int32_t ne3; + int32_t top_k; +} ggml_metal_kargs_argsort; + +typedef struct { + int64_t ne00; + int64_t ne01; + int64_t ne02; + int64_t ne03; + uint64_t nb00; + uint64_t nb01; + uint64_t nb02; + uint64_t nb03; + int32_t ne0; + int32_t ne1; + int32_t ne2; + int32_t ne3; + int32_t top_k; + int32_t len; +} ggml_metal_kargs_argsort_merge; + +typedef struct { + int64_t ne0; + float start; + float step; +} ggml_metal_kargs_arange; + +typedef struct { + int64_t val; +} ggml_metal_kargs_memset; + +typedef struct { + int32_t ne00; + int32_t ne01; + int32_t ne02; + int32_t ne03; + uint64_t nb00; + uint64_t nb01; + uint64_t nb02; + uint64_t nb03; + uint64_t nb10; + uint64_t nb11; + uint64_t nb12; + uint64_t nb13; +} ggml_metal_kargs_count_equal; + +typedef struct { + int32_t k0; + int32_t k1; + int32_t s0; + int32_t s1; + int32_t p0; + int32_t p1; + int64_t IH; + int64_t IW; + int64_t OH; + int64_t OW; + int64_t np; +} ggml_metal_kargs_pool_2d; + +typedef struct { + int32_t k0; + int32_t s0; + int32_t p0; + int64_t IW; + int64_t OW; + int64_t np; +} ggml_metal_kargs_pool_1d; + +typedef struct { + int64_t ne00; + uint64_t nb01; +} ggml_metal_kargs_argmax; + +typedef struct { + int64_t np; +} ggml_metal_kargs_opt_step_adamw; + +typedef struct { + int64_t np; +} ggml_metal_kargs_opt_step_sgd; + +#endif // GGML_METAL_IMPL diff --git a/libs/voice2/src/metal/ggml/ggml-metal.metal b/libs/voice2/src/metal/ggml/ggml-metal.metal new file mode 100644 index 0000000000..3620bc0dfb --- /dev/null +++ b/libs/voice2/src/metal/ggml/ggml-metal.metal @@ -0,0 +1,7887 @@ +#define GGML_COMMON_DECL_METAL +#define GGML_COMMON_IMPL_METAL +#if defined(GGML_METAL_EMBED_LIBRARY) +__embed_ggml-common.h__ +#else +#include "ggml-common.h" +#endif +#include "ggml-metal-impl.h" + +#include + +#ifdef GGML_METAL_HAS_TENSOR +#include + +#include +#endif + +using namespace metal; + +#define MAX(x, y) ((x) > (y) ? (x) : (y)) +#define MIN(x, y) ((x) < (y) ? (x) : (y)) +#define SWAP(x, y) { auto tmp = (x); (x) = (y); (y) = tmp; } + +#define PAD2(x, n) (((x) + (n) - 1) & ~((n) - 1)) + +#define FOR_UNROLL(x) _Pragma("clang loop unroll(full)") for (x) + +#define N_SIMDWIDTH 32 // assuming SIMD group size is 32 + +// ref: https://developer.apple.com/metal/Metal-Shading-Language-Specification.pdf +// +// cmd: +// .../usr/bin/metal -dM -E -c ggml/src/ggml-metal/ggml-metal.metal +// .../usr/bin/metal -dM -E -c -target air64-apple-ios14.0 ggml/src/ggml-metal/ggml-metal.metal +// +#if __METAL_VERSION__ < 310 && defined(GGML_METAL_HAS_BF16) +#undef GGML_METAL_HAS_BF16 +#endif + +#if defined(GGML_METAL_HAS_BF16) +typedef matrix bfloat4x4; +typedef matrix bfloat2x4; +#endif + +constexpr constant static float kvalues_iq4nl_f[16] = { + -127.f, -104.f, -83.f, -65.f, -49.f, -35.f, -22.f, -10.f, 1.f, 13.f, 25.f, 38.f, 53.f, 69.f, 89.f, 113.f +}; + +constexpr constant static float kvalues_mxfp4_f[16] = { + 0, .5f, 1.f, 1.5f, 2.f, 3.f, 4.f, 6.f, -0, -.5f, -1.f, -1.5f, -2.f, -3.f, -4.f, -6.f +}; + +static inline int best_index_int8(int n, constant float * val, float x) { + if (x <= val[0]) return 0; + if (x >= val[n-1]) return n-1; + int ml = 0, mu = n-1; + while (mu-ml > 1) { + int mav = (ml+mu)/2; + if (x < val[mav]) mu = mav; else ml = mav; + } + return x - val[mu-1] < val[mu] - x ? mu-1 : mu; +} + +static inline float e8m0_to_fp32(uint8_t x) { + uint32_t bits; + + if (x == 0) { + bits = 0x00400000; + } else { + bits = (uint32_t) x << 23; + } + + return as_type(bits); +} + +static inline float dot(float x, float y) { + return x*y; +} + +static inline float sum(float x) { + return x; +} + +static inline float sum(float4 x) { + return x[0] + x[1] + x[2] + x[3]; +} + +// NOTE: this is not dequantizing - we are simply fitting the template +template +void dequantize_f32(device const float4x4 * src, short il, thread type4x4 & reg) { + reg = (type4x4)(*src); +} + +template +void dequantize_f32_t4(device const float4 * src, short il, thread type4 & reg) { + reg = (type4)(*src); +} + +template +void dequantize_f16(device const half4x4 * src, short il, thread type4x4 & reg) { + reg = (type4x4)(*src); +} + +template +void dequantize_f16_t4(device const half4 * src, short il, thread type4 & reg) { + reg = (type4)(*(src)); +} + +#if defined(GGML_METAL_HAS_BF16) +template +void dequantize_bf16(device const bfloat4x4 * src, short il, thread type4x4 & reg) { + reg = (type4x4)(*src); +} + +template +void dequantize_bf16_t4(device const bfloat4 * src, short il, thread type4 & reg) { + reg = (type4)(*(src)); +} +#endif + +template +void dequantize_q4_0(device const block_q4_0 * xb, short il, thread type4x4 & reg) { + device const uint16_t * qs = ((device const uint16_t *)xb + 1); + const float d1 = il ? (xb->d / 16.h) : xb->d; + const float d2 = d1 / 256.f; + const float md = -8.h * xb->d; + const ushort mask0 = il ? 0x00F0 : 0x000F; + const ushort mask1 = mask0 << 8; + + float4x4 reg_f; + + for (int i = 0; i < 8; i++) { + reg_f[i/2][2*(i%2) + 0] = d1 * (qs[i] & mask0) + md; + reg_f[i/2][2*(i%2) + 1] = d2 * (qs[i] & mask1) + md; + } + + reg = (type4x4) reg_f; +} + +template +void dequantize_q4_0_t4(device const block_q4_0 * xb, short il, thread type4 & reg) { + device const uint16_t * qs = ((device const uint16_t *)xb + 1); + const float d1 = (il/4) ? (xb->d / 16.h) : xb->d; + const float d2 = d1 / 256.f; + const float md = -8.h * xb->d; + const ushort mask0 = (il/4) ? 0x00F0 : 0x000F; + const ushort mask1 = mask0 << 8; + + for (int i = 0; i < 2; i++) { + reg[2*i + 0] = d1 * (qs[2*(il%4) + i] & mask0) + md; + reg[2*i + 1] = d2 * (qs[2*(il%4) + i] & mask1) + md; + } +} + +void quantize_q4_0(device const float * src, device block_q4_0 & dst) { +#pragma METAL fp math_mode(safe) + float amax = 0.0f; // absolute max + float max = 0.0f; + + for (int j = 0; j < QK4_0; j++) { + const float v = src[j]; + if (amax < fabs(v)) { + amax = fabs(v); + max = v; + } + } + + const float d = max / -8; + const float id = d ? 1.0f/d : 0.0f; + + dst.d = d; + + for (int j = 0; j < QK4_0/2; ++j) { + const float x0 = src[0 + j]*id; + const float x1 = src[QK4_0/2 + j]*id; + + const uint8_t xi0 = MIN(15, (int8_t)(x0 + 8.5f)); + const uint8_t xi1 = MIN(15, (int8_t)(x1 + 8.5f)); + + dst.qs[j] = xi0; + dst.qs[j] |= xi1 << 4; + } +} + +void quantize_q4_1(device const float * src, device block_q4_1 & dst) { +#pragma METAL fp math_mode(safe) + float min = FLT_MAX; + float max = -FLT_MAX; + + for (int j = 0; j < QK4_1; j++) { + const float v = src[j]; + if (min > v) min = v; + if (max < v) max = v; + } + + const float d = (max - min) / ((1 << 4) - 1); + const float id = d ? 1.0f/d : 0.0f; + + dst.d = d; + dst.m = min; + + for (int j = 0; j < QK4_1/2; ++j) { + const float x0 = (src[0 + j] - min)*id; + const float x1 = (src[QK4_1/2 + j] - min)*id; + + const uint8_t xi0 = MIN(15, (int8_t)(x0 + 0.5f)); + const uint8_t xi1 = MIN(15, (int8_t)(x1 + 0.5f)); + + dst.qs[j] = xi0; + dst.qs[j] |= xi1 << 4; + } +} + +void quantize_q5_0(device const float * src, device block_q5_0 & dst) { +#pragma METAL fp math_mode(safe) + float amax = 0.0f; // absolute max + float max = 0.0f; + + for (int j = 0; j < QK5_0; j++) { + const float v = src[j]; + if (amax < fabs(v)) { + amax = fabs(v); + max = v; + } + } + + const float d = max / -16; + const float id = d ? 1.0f/d : 0.0f; + + dst.d = d; + + uint32_t qh = 0; + for (int j = 0; j < QK5_0/2; ++j) { + const float x0 = src[0 + j]*id; + const float x1 = src[QK5_0/2 + j]*id; + + const uint8_t xi0 = MIN(31, (int8_t)(x0 + 16.5f)); + const uint8_t xi1 = MIN(31, (int8_t)(x1 + 16.5f)); + + dst.qs[j] = (xi0 & 0xf) | ((xi1 & 0xf) << 4); + qh |= ((xi0 & 0x10u) >> 4) << (j + 0); + qh |= ((xi1 & 0x10u) >> 4) << (j + QK5_0/2); + } + + thread const uint8_t * qh8 = (thread const uint8_t *)&qh; + + for (int j = 0; j < 4; ++j) { + dst.qh[j] = qh8[j]; + } +} + +void quantize_q5_1(device const float * src, device block_q5_1 & dst) { +#pragma METAL fp math_mode(safe) + float max = src[0]; + float min = src[0]; + + for (int j = 1; j < QK5_1; j++) { + const float v = src[j]; + min = v < min ? v : min; + max = v > max ? v : max; + } + + const float d = (max - min) / 31; + const float id = d ? 1.0f/d : 0.0f; + + dst.d = d; + dst.m = min; + + uint32_t qh = 0; + for (int j = 0; j < QK5_1/2; ++j) { + const float x0 = (src[0 + j] - min)*id; + const float x1 = (src[QK5_1/2 + j] - min)*id; + + const uint8_t xi0 = (uint8_t)(x0 + 0.5f); + const uint8_t xi1 = (uint8_t)(x1 + 0.5f); + + dst.qs[j] = (xi0 & 0xf) | ((xi1 & 0xf) << 4); + qh |= ((xi0 & 0x10u) >> 4) << (j + 0); + qh |= ((xi1 & 0x10u) >> 4) << (j + QK5_1/2); + } + + thread const uint8_t * qh8 = (thread const uint8_t *)&qh; + + for (int j = 0; j < 4; ++j) { + dst.qh[j] = qh8[j]; + } +} + +void quantize_q8_0(device const float * src, device block_q8_0 & dst) { +#pragma METAL fp math_mode(safe) + float amax = 0.0f; // absolute max + + for (int j = 0; j < QK8_0; j++) { + const float v = src[j]; + amax = MAX(amax, fabs(v)); + } + + const float d = amax / ((1 << 7) - 1); + const float id = d ? 1.0f/d : 0.0f; + + dst.d = d; + + for (int j = 0; j < QK8_0; ++j) { + const float x0 = src[j]*id; + + dst.qs[j] = round(x0); + } +} + +void quantize_iq4_nl(device const float * src, device block_iq4_nl & dst) { +#pragma METAL fp math_mode(safe) + float amax = 0.0f; // absolute max + float max = 0.0f; + + for (int j = 0; j < QK4_NL; j++) { + const float v = src[j]; + if (amax < fabs(v)) { + amax = fabs(v); + max = v; + } + } + + const float d = max / kvalues_iq4nl_f[0]; + const float id = d ? 1.0f/d : 0.0f; + + float sumqx = 0, sumq2 = 0; + for (int j = 0; j < QK4_NL/2; ++j) { + const float x0 = src[0 + j]*id; + const float x1 = src[QK4_NL/2 + j]*id; + + const uint8_t xi0 = best_index_int8(16, kvalues_iq4nl_f, x0); + const uint8_t xi1 = best_index_int8(16, kvalues_iq4nl_f, x1); + + dst.qs[j] = xi0 | (xi1 << 4); + + const float v0 = kvalues_iq4nl_f[xi0]; + const float v1 = kvalues_iq4nl_f[xi1]; + const float w0 = src[0 + j]*src[0 + j]; + const float w1 = src[QK4_NL/2 + j]*src[QK4_NL/2 + j]; + sumqx += w0*v0*src[j] + w1*v1*src[QK4_NL/2 + j]; + sumq2 += w0*v0*v0 + w1*v1*v1; + + } + + dst.d = sumq2 > 0 ? sumqx/sumq2 : d; +} + +template +void dequantize_q4_1(device const block_q4_1 * xb, short il, thread type4x4 & reg) { + device const uint16_t * qs = ((device const uint16_t *)xb + 2); + const float d1 = il ? (xb->d / 16.h) : xb->d; + const float d2 = d1 / 256.f; + const float m = xb->m; + const ushort mask0 = il ? 0x00F0 : 0x000F; + const ushort mask1 = mask0 << 8; + + float4x4 reg_f; + + for (int i = 0; i < 8; i++) { + reg_f[i/2][2*(i%2) + 0] = ((qs[i] & mask0) * d1) + m; + reg_f[i/2][2*(i%2) + 1] = ((qs[i] & mask1) * d2) + m; + } + + reg = (type4x4) reg_f; +} + +template +void dequantize_q4_1_t4(device const block_q4_1 * xb, short il, thread type4 & reg) { + device const uint16_t * qs = ((device const uint16_t *)xb + 2); + const float d1 = (il/4) ? (xb->d / 16.h) : xb->d; + const float d2 = d1 / 256.f; + const float m = xb->m; + const ushort mask0 = (il/4) ? 0x00F0 : 0x000F; + const ushort mask1 = mask0 << 8; + + for (int i = 0; i < 2; i++) { + reg[2*i + 0] = d1 * (qs[2*(il%4) + i] & mask0) + m; + reg[2*i + 1] = d2 * (qs[2*(il%4) + i] & mask1) + m; + } +} + +template +void dequantize_q5_0(device const block_q5_0 * xb, short il, thread type4x4 & reg) { + device const uint16_t * qs = ((device const uint16_t *)xb + 3); + const float d = xb->d; + const float md = -16.h * xb->d; + const ushort mask = il ? 0x00F0 : 0x000F; + + const uint32_t qh = *((device const uint32_t *)xb->qh); + + const int x_mv = il ? 4 : 0; + + const int gh_mv = il ? 12 : 0; + const int gh_bk = il ? 0 : 4; + + float4x4 reg_f; + + for (int i = 0; i < 8; i++) { + // extract the 5-th bits for x0 and x1 + const uint8_t xh_0 = ((qh >> (gh_mv + 2*i )) << gh_bk) & 0x10; + const uint8_t xh_1 = ((qh >> (gh_mv + 2*i+1)) << gh_bk) & 0x10; + + // combine the 4-bits from qs with the 5th bit + const int32_t x0 = ((((qs[i] ) & mask) >> x_mv) | xh_0); + const int32_t x1 = ((((qs[i] >> 8) & mask) >> x_mv) | xh_1); + + reg_f[i/2][2*(i%2) + 0] = d * x0 + md; + reg_f[i/2][2*(i%2) + 1] = d * x1 + md; + } + + reg = (type4x4) reg_f; +} + +template +void dequantize_q5_0_t4(device const block_q5_0 * xb, short il, thread type4 & reg) { + device const uint16_t * qs = ((device const uint16_t *)xb + 3); + const float d = xb->d; + const float md = -16.h * xb->d; + const ushort mask = (il/4) ? 0x00F0 : 0x000F; + + const uint32_t qh = *((device const uint32_t *)xb->qh); + + const int x_mv = (il/4) ? 4 : 0; + + const int gh_mv = (il/4) ? 12 : 0; + const int gh_bk = (il/4) ? 0 : 4; + + for (int ii = 0; ii < 2; ii++) { + int i = 2*(il%4) + ii; + + // extract the 5-th bits for x0 and x1 + const uint8_t xh_0 = ((qh >> (gh_mv + 2*i )) << gh_bk) & 0x10; + const uint8_t xh_1 = ((qh >> (gh_mv + 2*i+1)) << gh_bk) & 0x10; + + // combine the 4-bits from qs with the 5th bit + const int32_t x0 = ((((qs[i] ) & mask) >> x_mv) | xh_0); + const int32_t x1 = ((((qs[i] >> 8) & mask) >> x_mv) | xh_1); + + reg[2*ii + 0] = d * x0 + md; + reg[2*ii + 1] = d * x1 + md; + } +} + +template +void dequantize_q5_1(device const block_q5_1 * xb, short il, thread type4x4 & reg) { + device const uint16_t * qs = ((device const uint16_t *)xb + 4); + const float d = xb->d; + const float m = xb->m; + const ushort mask = il ? 0x00F0 : 0x000F; + + const uint32_t qh = *((device const uint32_t *)xb->qh); + + const int x_mv = il ? 4 : 0; + + const int gh_mv = il ? 12 : 0; + const int gh_bk = il ? 0 : 4; + + float4x4 reg_f; + + for (int i = 0; i < 8; i++) { + // extract the 5-th bits for x0 and x1 + const uint8_t xh_0 = ((qh >> (gh_mv + 2*i )) << gh_bk) & 0x10; + const uint8_t xh_1 = ((qh >> (gh_mv + 2*i+1)) << gh_bk) & 0x10; + + // combine the 4-bits from qs with the 5th bit + const int32_t x0 = ((((qs[i] ) & mask) >> x_mv) | xh_0); + const int32_t x1 = ((((qs[i] >> 8) & mask) >> x_mv) | xh_1); + + reg_f[i/2][2*(i%2) + 0] = d * x0 + m; + reg_f[i/2][2*(i%2) + 1] = d * x1 + m; + } + + reg = (type4x4) reg_f; +} + +template +void dequantize_q5_1_t4(device const block_q5_1 * xb, short il, thread type4 & reg) { + device const uint16_t * qs = ((device const uint16_t *)xb + 4); + const float d = xb->d; + const float m = xb->m; + const ushort mask = (il/4) ? 0x00F0 : 0x000F; + + const uint32_t qh = *((device const uint32_t *)xb->qh); + + const int x_mv = (il/4) ? 4 : 0; + + const int gh_mv = (il/4) ? 12 : 0; + const int gh_bk = (il/4) ? 0 : 4; + + for (int ii = 0; ii < 2; ii++) { + int i = 2*(il%4) + ii; + + // extract the 5-th bits for x0 and x1 + const uint8_t xh_0 = ((qh >> (gh_mv + 2*i )) << gh_bk) & 0x10; + const uint8_t xh_1 = ((qh >> (gh_mv + 2*i+1)) << gh_bk) & 0x10; + + // combine the 4-bits from qs with the 5th bit + const int32_t x0 = ((((qs[i] ) & mask) >> x_mv) | xh_0); + const int32_t x1 = ((((qs[i] >> 8) & mask) >> x_mv) | xh_1); + + reg[2*ii + 0] = d * x0 + m; + reg[2*ii + 1] = d * x1 + m; + } +} + +template +void dequantize_q8_0(device const block_q8_0 *xb, short il, thread type4x4 & reg) { + device const int8_t * qs = ((device const int8_t *)xb->qs); + const float d = xb->d; + + float4x4 reg_f; + + for (int i = 0; i < 16; i++) { + reg_f[i/4][i%4] = (qs[i + 16*il] * d); + } + + reg = (type4x4) reg_f; +} + +template +void dequantize_q8_0_t4(device const block_q8_0 *xb, short il, thread type4 & reg) { + device const int8_t * qs = ((device const int8_t *)xb->qs); + const float d = xb->d; + + for (int i = 0; i < 4; i++) { + reg[i] = (qs[4*(il%4) + i + 16*(il/4)] * d); + } +} + +template +void dequantize_mxfp4(device const block_mxfp4 * xb, short il, thread type4x4 & reg) { + device const uint8_t * q2 = (device const uint8_t *)xb->qs; + + const float d = e8m0_to_fp32(xb->e); + const uint8_t shr = il >= 1 ? 4 : 0; + + for (int i = 0; i < 4; ++i) { + reg[i][0] = d * kvalues_mxfp4_f[(q2[4*i + 0] >> shr) & 0x0F]; + reg[i][1] = d * kvalues_mxfp4_f[(q2[4*i + 1] >> shr) & 0x0F]; + reg[i][2] = d * kvalues_mxfp4_f[(q2[4*i + 2] >> shr) & 0x0F]; + reg[i][3] = d * kvalues_mxfp4_f[(q2[4*i + 3] >> shr) & 0x0F]; + } +} + +template +void dequantize_mxfp4_t4(device const block_mxfp4 * xb, short il, thread type4 & reg) { + device const uint8_t * q2 = (device const uint8_t *)xb->qs; + + const float d = e8m0_to_fp32(xb->e); + const short il4 = il%4; + + const uint8_t shr = il >= 4 ? 4 : 0; + + reg[0] = d * kvalues_mxfp4_f[(q2[4*il4 + 0] >> shr) & 0x0F]; + reg[1] = d * kvalues_mxfp4_f[(q2[4*il4 + 1] >> shr) & 0x0F]; + reg[2] = d * kvalues_mxfp4_f[(q2[4*il4 + 2] >> shr) & 0x0F]; + reg[3] = d * kvalues_mxfp4_f[(q2[4*il4 + 3] >> shr) & 0x0F]; +} + +template +void dequantize_q2_K(device const block_q2_K *xb, short il, thread type4x4 & reg) { + const float d = xb->d; + const float min = xb->dmin; + device const uint8_t * q = (device const uint8_t *)xb->qs; + float dl, ml; + uint8_t sc = xb->scales[il]; + + q = q + 32*(il/8) + 16*(il&1); + il = (il/2)%4; + + half coef = il>1 ? (il>2 ? 1/64.h : 1/16.h) : (il>0 ? 1/4.h : 1.h); + uchar mask = il>1 ? (il>2 ? 192 : 48) : (il>0 ? 12 : 3); + dl = d * (sc & 0xF) * coef, ml = min * (sc >> 4); + for (int i = 0; i < 16; ++i) { + reg[i/4][i%4] = dl * (q[i] & mask) - ml; + } +} + +template +void dequantize_q3_K(device const block_q3_K *xb, short il, thread type4x4 & reg) { + const half d_all = xb->d; + device const uint8_t * q = (device const uint8_t *)xb->qs; + device const uint8_t * h = (device const uint8_t *)xb->hmask; + device const int8_t * scales = (device const int8_t *)xb->scales; + + q = q + 32 * (il/8) + 16 * (il&1); + h = h + 16 * (il&1); + uint8_t m = 1 << (il/2); + uint16_t kmask1 = (il/4)>1 ? ((il/4)>2 ? 192 : 48) : \ + ((il/4)>0 ? 12 : 3); + uint16_t kmask2 = il/8 ? 0xF0 : 0x0F; + uint16_t scale_2 = scales[il%8], scale_1 = scales[8 + il%4]; + int16_t dl_int = (il/4)&1 ? (scale_2&kmask2) | ((scale_1&kmask1) << 2) + : (scale_2&kmask2) | ((scale_1&kmask1) << 4); + float dl = il<8 ? d_all * (dl_int - 32.f) : d_all * (dl_int / 16.f - 32.f); + const float ml = 4.f * dl; + + il = (il/2) & 3; + const half coef = il>1 ? (il>2 ? 1/64.h : 1/16.h) : (il>0 ? 1/4.h : 1.h); + const uint8_t mask = il>1 ? (il>2 ? 192 : 48) : (il>0 ? 12 : 3); + dl *= coef; + + for (int i = 0; i < 16; ++i) { + reg[i/4][i%4] = dl * (q[i] & mask) - (h[i] & m ? 0 : ml); + } +} + +static inline uchar2 get_scale_min_k4_just2(int j, int k, device const uchar * q) { + return j < 4 ? uchar2{uchar(q[j+0+k] & 63), uchar(q[j+4+k] & 63)} + : uchar2{uchar((q[j+4+k] & 0xF) | ((q[j-4+k] & 0xc0) >> 2)), uchar((q[j+4+k] >> 4) | ((q[j-0+k] & 0xc0) >> 2))}; +} + +template +void dequantize_q4_K(device const block_q4_K * xb, short il, thread type4x4 & reg) { + device const uchar * q = xb->qs; + + short is = (il/4) * 2; + q = q + (il/4) * 32 + 16 * (il&1); + il = il & 3; + const uchar2 sc = get_scale_min_k4_just2(is, il/2, xb->scales); + const float d = il < 2 ? xb->d : xb->d / 16.h; + const float min = xb->dmin; + const float dl = d * sc[0]; + const float ml = min * sc[1]; + + const ushort mask = il < 2 ? 0x0F : 0xF0; + for (int i = 0; i < 16; ++i) { + reg[i/4][i%4] = dl * (q[i] & mask) - ml; + } +} + +template +void dequantize_q5_K(device const block_q5_K *xb, short il, thread type4x4 & reg) { + device const uint8_t * q = xb->qs; + device const uint8_t * qh = xb->qh; + + short is = (il/4) * 2; + q = q + 32 * (il/4) + 16 * (il&1); + qh = qh + 16 * (il&1); + uint8_t ul = 1 << (il/2); + il = il & 3; + const uchar2 sc = get_scale_min_k4_just2(is, il/2, xb->scales); + const float d = il < 2 ? xb->d : xb->d / 16.f; + const float min = xb->dmin; + const float dl = d * sc[0]; + const float ml = min * sc[1]; + + const ushort mask = il<2 ? 0x0F : 0xF0; + const float qh_val = il<2 ? 16.f : 256.f; + for (int i = 0; i < 16; ++i) { + reg[i/4][i%4] = dl * ((q[i] & mask) + (qh[i] & ul ? qh_val : 0)) - ml; + } +} + +template +void dequantize_q6_K(device const block_q6_K *xb, short il, thread type4x4 & reg) { + const half d_all = xb->d; + device const uint16_t * ql = (device const uint16_t *)xb->ql; + device const uint16_t * qh = (device const uint16_t *)xb->qh; + device const int8_t * scales = (device const int8_t *)xb->scales; + + ql = ql + 32*(il/8) + 16*((il/2)&1) + 8*(il&1); + qh = qh + 16*(il/8) + 8*(il&1); + float sc = scales[(il%2) + 2 * ((il/2))]; + il = (il/2) & 3; + + const uint32_t kmask1 = il>1 ? (il>2 ? 0xC0C0C0C0 : 0x30303030) : (il>0 ? 0x0C0C0C0C : 0x03030303); + const uint32_t kmask2 = il>1 ? 0xF0F0F0F0 : 0x0F0F0F0F; + const float ml = d_all * sc * 32.f; + const float dl0 = d_all * sc; + const float dl1 = dl0 / 256.f; + const float dl2 = dl0 / (256.f * 256.f); + const float dl3 = dl0 / (256.f * 256.f * 256.f); + const uint8_t shr_h = il>2 ? 2 : 0; + const uint8_t shl_h = il>1 ? 0 : (il>0 ? 2 : 4); + const uint8_t shr_l = il>1 ? 4 : 0; + for (int i = 0; i < 4; ++i) { + const uint32_t low = (ql[2*i] | (uint32_t)(ql[2*i+1] << 16)) & kmask2; + const uint32_t high = (qh[2*i] | (uint32_t)(qh[2*i+1] << 16)) & kmask1; + const uint32_t q = ((high << shl_h) >> shr_h) | (low >> shr_l); + reg[i][0] = dl0 * ((half)(q & 0xFF)) - ml; + reg[i][1] = dl1 * ((float)(q & 0xFF00)) - ml; + reg[i][2] = dl2 * ((float)(q & 0xFF0000)) - ml; + reg[i][3] = dl3 * ((float)(q & 0xFF000000)) - ml; + } +} + +template +void dequantize_iq2_xxs(device const block_iq2_xxs * xb, short il, thread type4x4 & reg) { + // il is 0...15 for QK_K = 256 => index of block of 32 is il/2 + const float d = xb->d; + const int ib32 = il/2; + il = il%2; + // il = 0 or 1. il = 0 processes the first 16 quants in a block of 32, il = 1 the second 16 + // each block of 32 needs 2 uint32_t's for the quants & scale, so 4 uint16_t's. + device const uint16_t * q2 = xb->qs + 4*ib32; + const uint32_t aux32_g = q2[0] | (q2[1] << 16); + const uint32_t aux32_s = q2[2] | (q2[3] << 16); + thread const uint8_t * aux8 = (thread const uint8_t *)&aux32_g; + const float dl = d * (0.5f + (aux32_s >> 28)) * 0.25f; + constant uint8_t * grid = (constant uint8_t *)(iq2xxs_grid + aux8[2*il+0]); + uint8_t signs = ksigns_iq2xs[(aux32_s >> 14*il) & 127]; + for (int i = 0; i < 8; ++i) { + reg[i/4][i%4] = dl * grid[i] * (signs & kmask_iq2xs[i] ? -1.f : 1.f); + } + grid = (constant uint8_t *)(iq2xxs_grid + aux8[2*il+1]); + signs = ksigns_iq2xs[(aux32_s >> (14*il+7)) & 127]; + for (int i = 0; i < 8; ++i) { + reg[2+i/4][i%4] = dl * grid[i] * (signs & kmask_iq2xs[i] ? -1.f : 1.f); + } +} + +template +void dequantize_iq2_xs(device const block_iq2_xs * xb, short il, thread type4x4 & reg) { + // il is 0...15 for QK_K = 256 => index of block of 32 is il/2 + const float d = xb->d; + const int ib32 = il/2; + il = il%2; + // il = 0 or 1. il = 0 processes the first 16 quants in a block of 32, il = 1 the second 16 + device const uint16_t * q2 = xb->qs + 4*ib32; + const float dl = d * (0.5f + ((xb->scales[ib32] >> 4*il) & 0xf)) * 0.25f; + constant uint8_t * grid = (constant uint8_t *)(iq2xs_grid + (q2[2*il+0] & 511)); + uint8_t signs = ksigns_iq2xs[q2[2*il+0] >> 9]; + for (int i = 0; i < 8; ++i) { + reg[i/4][i%4] = dl * grid[i] * (signs & kmask_iq2xs[i] ? -1.f : 1.f); + } + grid = (constant uint8_t *)(iq2xs_grid + (q2[2*il+1] & 511)); + signs = ksigns_iq2xs[q2[2*il+1] >> 9]; + for (int i = 0; i < 8; ++i) { + reg[2+i/4][i%4] = dl * grid[i] * (signs & kmask_iq2xs[i] ? -1.f : 1.f); + } +} + +template +void dequantize_iq3_xxs(device const block_iq3_xxs * xb, short il, thread type4x4 & reg) { + // il is 0...15 for QK_K = 256 => index of block of 32 is il/2 + const float d = xb->d; + const int ib32 = il/2; + il = il%2; + // il = 0 or 1. il = 0 processes the first 16 quants in a block of 32, il = 1 the second 16 + device const uint8_t * q3 = xb->qs + 8*ib32; + device const uint16_t * gas = (device const uint16_t *)(xb->qs + QK_K/4) + 2*ib32; + const uint32_t aux32 = gas[0] | (gas[1] << 16); + const float dl = d * (0.5f + (aux32 >> 28)) * 0.5f; + constant uint8_t * grid1 = (constant uint8_t *)(iq3xxs_grid + q3[4*il+0]); + constant uint8_t * grid2 = (constant uint8_t *)(iq3xxs_grid + q3[4*il+1]); + uint8_t signs = ksigns_iq2xs[(aux32 >> 14*il) & 127]; + for (int i = 0; i < 4; ++i) { + reg[0][i] = dl * grid1[i] * (signs & kmask_iq2xs[i+0] ? -1.f : 1.f); + reg[1][i] = dl * grid2[i] * (signs & kmask_iq2xs[i+4] ? -1.f : 1.f); + } + grid1 = (constant uint8_t *)(iq3xxs_grid + q3[4*il+2]); + grid2 = (constant uint8_t *)(iq3xxs_grid + q3[4*il+3]); + signs = ksigns_iq2xs[(aux32 >> (14*il+7)) & 127]; + for (int i = 0; i < 4; ++i) { + reg[2][i] = dl * grid1[i] * (signs & kmask_iq2xs[i+0] ? -1.f : 1.f); + reg[3][i] = dl * grid2[i] * (signs & kmask_iq2xs[i+4] ? -1.f : 1.f); + } +} + +template +void dequantize_iq3_s(device const block_iq3_s * xb, short il, thread type4x4 & reg) { + // il is 0...15 for QK_K = 256 => index of block of 32 is il/2 + const float d = xb->d; + const int ib32 = il/2; + il = il%2; + // il = 0 or 1. il = 0 processes the first 16 quants in a block of 32, il = 1 the second 16 + device const uint8_t * qs = xb->qs + 8*ib32; + device const uint8_t * signs = xb->signs + 4*ib32 + 2*il; + const uint8_t qh = xb->qh[ib32] >> 4*il; + const float dl = d * (1 + 2*((xb->scales[ib32/2] >> 4*(ib32%2)) & 0xf)); + constant uint8_t * grid1 = (constant uint8_t *)(iq3s_grid + (qs[4*il+0] | ((qh << 8) & 256))); + constant uint8_t * grid2 = (constant uint8_t *)(iq3s_grid + (qs[4*il+1] | ((qh << 7) & 256))); + for (int i = 0; i < 4; ++i) { + reg[0][i] = dl * grid1[i] * select(1, -1, signs[0] & kmask_iq2xs[i+0]); + reg[1][i] = dl * grid2[i] * select(1, -1, signs[0] & kmask_iq2xs[i+4]); + } + grid1 = (constant uint8_t *)(iq3s_grid + (qs[4*il+2] | ((qh << 6) & 256))); + grid2 = (constant uint8_t *)(iq3s_grid + (qs[4*il+3] | ((qh << 5) & 256))); + for (int i = 0; i < 4; ++i) { + reg[2][i] = dl * grid1[i] * select(1, -1, signs[1] & kmask_iq2xs[i+0]); + reg[3][i] = dl * grid2[i] * select(1, -1, signs[1] & kmask_iq2xs[i+4]); + } +} + +template +void dequantize_iq2_s(device const block_iq2_s * xb, short il, thread type4x4 & reg) { + // il is 0...15 for QK_K = 256 => index of block of 32 is il/2 + const float d = xb->d; + const int ib32 = il/2; + il = il%2; + // il = 0 or 1. il = 0 processes the first 16 quants in a block of 32, il = 1 the second 16 + device const uint8_t * qs = xb->qs + 4*ib32 + 2*il; + device const uint8_t * signs = qs + QK_K/8; + const uint8_t qh = xb->qh[ib32] >> 4*il; + const float dl = d * (0.5f + ((xb->scales[ib32] >> 4*il) & 0xf)) * 0.25f; + constant uint8_t * grid1 = (constant uint8_t *)(iq2s_grid + (qs[0] | ((qh << 8) & 0x300))); + constant uint8_t * grid2 = (constant uint8_t *)(iq2s_grid + (qs[1] | ((qh << 6) & 0x300))); + for (int i = 0; i < 8; ++i) { + reg[i/4+0][i%4] = dl * grid1[i] * select(1, -1, signs[0] & kmask_iq2xs[i]); + reg[i/4+2][i%4] = dl * grid2[i] * select(1, -1, signs[1] & kmask_iq2xs[i]); + } +} + +template +void dequantize_iq1_s(device const block_iq1_s * xb, short il, thread type4x4 & reg) { + // il is 0...15 for QK_K = 256 => index of block of 32 is il/2 + const int ib32 = il/2; + il = il%2; + const float d = xb->d; + device const uint8_t * qs = xb->qs + 4*ib32 + 2*il; + device const uint16_t * qh = xb->qh; + const float dl = d * (2*((qh[ib32] >> 12) & 7) + 1); + const float ml = dl * (qh[ib32] & 0x8000 ? -1 - IQ1S_DELTA : -1 + IQ1S_DELTA); + const uint16_t h = qh[ib32] >> 6*il; + constant uint8_t * grid1 = (constant uint8_t *)(iq1s_grid_gpu + (qs[0] | ((h << 8) & 0x700))); + constant uint8_t * grid2 = (constant uint8_t *)(iq1s_grid_gpu + (qs[1] | ((h << 5) & 0x700))); + for (int i = 0; i < 4; ++i) { + reg[0][i] = dl * (grid1[i] & 0xf) + ml; + reg[1][i] = dl * (grid1[i] >> 4) + ml; + reg[2][i] = dl * (grid2[i] & 0xf) + ml; + reg[3][i] = dl * (grid2[i] >> 4) + ml; + } +} + +template +void dequantize_iq1_m(device const block_iq1_m * xb, short il, thread type4x4 & reg) { + // il is 0...15 for QK_K = 256 => index of block of 32 is il/2 + const int ib32 = il/2; + il = il%2; + device const uint16_t * sc = (device const uint16_t *)xb->scales; + + iq1m_scale_t scale; + scale.u16 = (sc[0] >> 12) | ((sc[1] >> 8) & 0x00f0) | ((sc[2] >> 4) & 0x0f00) | (sc[3] & 0xf000); + const float d = scale.f16; + + device const uint8_t * qs = xb->qs + 4*ib32 + 2*il; + device const uint8_t * qh = xb->qh + 2*ib32 + il; + + const float dl = d * (2*((sc[ib32/2] >> (6*(ib32%2)+3*il)) & 7) + 1); + const float ml1 = dl * (qh[0] & 0x08 ? -1 - IQ1M_DELTA : -1 + IQ1M_DELTA); + const float ml2 = dl * (qh[0] & 0x80 ? -1 - IQ1M_DELTA : -1 + IQ1M_DELTA); + constant uint8_t * grid1 = (constant uint8_t *)(iq1s_grid_gpu + (qs[0] | ((qh[0] << 8) & 0x700))); + constant uint8_t * grid2 = (constant uint8_t *)(iq1s_grid_gpu + (qs[1] | ((qh[0] << 4) & 0x700))); + for (int i = 0; i < 4; ++i) { + reg[0][i] = dl * (grid1[i] & 0xf) + ml1; + reg[1][i] = dl * (grid1[i] >> 4) + ml1; + reg[2][i] = dl * (grid2[i] & 0xf) + ml2; + reg[3][i] = dl * (grid2[i] >> 4) + ml2; + } +} + +template +void dequantize_iq4_nl(device const block_iq4_nl * xb, short il, thread type4x4 & reg) { + device const uint16_t * q4 = (device const uint16_t *)xb->qs; + const float d = xb->d; + uint32_t aux32; + thread const uint8_t * q8 = (thread const uint8_t *)&aux32; + for (int i = 0; i < 4; ++i) { + aux32 = ((q4[2*i] | (q4[2*i+1] << 16)) >> 4*il) & 0x0f0f0f0f; + reg[i][0] = d * kvalues_iq4nl_f[q8[0]]; + reg[i][1] = d * kvalues_iq4nl_f[q8[1]]; + reg[i][2] = d * kvalues_iq4nl_f[q8[2]]; + reg[i][3] = d * kvalues_iq4nl_f[q8[3]]; + } +} + +template +void dequantize_iq4_nl_t4(device const block_iq4_nl * xb, short il, thread type4 & reg) { + device const uint16_t * q4 = (device const uint16_t *)xb->qs; + const float d = xb->d; + uint32_t aux32; + thread const uint8_t * q8 = (thread const uint8_t *)&aux32; + aux32 = ((q4[2*(il%4)] | (q4[2*(il%4)+1] << 16)) >> 4*(il/4)) & 0x0f0f0f0f; + reg[0] = d * kvalues_iq4nl_f[q8[0]]; + reg[1] = d * kvalues_iq4nl_f[q8[1]]; + reg[2] = d * kvalues_iq4nl_f[q8[2]]; + reg[3] = d * kvalues_iq4nl_f[q8[3]]; +} + +template +void dequantize_iq4_xs(device const block_iq4_xs * xb, short il, thread type4x4 & reg) { + // il is 0...15 for QK_K = 256 => index of block of 32 is il/2 + const int ib32 = il/2; + il = il%2; + // il = 0 or 1. il = 0 processes the first 16 quants in a block of 32, il = 1 the second 16 + device const uint32_t * q4 = (device const uint32_t *)xb->qs + 4*ib32; + const int ls = ((xb->scales_l[ib32/2] >> 4*(ib32%2)) & 0xf) | (((xb->scales_h >> 2*ib32) & 3) << 4); + const float d = (float)xb->d * (ls - 32); + uint32_t aux32; + thread const uint8_t * q8 = (thread const uint8_t *)&aux32; + for (int i = 0; i < 4; ++i) { + aux32 = (q4[i] >> 4*il) & 0x0f0f0f0f; + reg[i][0] = d * kvalues_iq4nl_f[q8[0]]; + reg[i][1] = d * kvalues_iq4nl_f[q8[1]]; + reg[i][2] = d * kvalues_iq4nl_f[q8[2]]; + reg[i][3] = d * kvalues_iq4nl_f[q8[3]]; + } +} + +enum ggml_sort_order { + GGML_SORT_ORDER_ASC, + GGML_SORT_ORDER_DESC, +}; + +constant float GELU_COEF_A = 0.044715f; +constant float GELU_QUICK_COEF = -1.702f; +constant float SQRT_2_OVER_PI = 0.79788456080286535587989211986876f; +constant float SQRT_2_INV = 0.70710678118654752440084436210484f; + +// based on Abramowitz and Stegun formula 7.1.26 or similar Hastings' approximation +// ref: https://www.johndcook.com/blog/python_erf/ +constant float p_erf = 0.3275911f; +constant float a1_erf = 0.254829592f; +constant float a2_erf = -0.284496736f; +constant float a3_erf = 1.421413741f; +constant float a4_erf = -1.453152027f; +constant float a5_erf = 1.061405429f; + +template +inline T erf_approx(T x) { + T sign_x = sign(x); + x = fabs(x); + T t = 1.0f / (1.0f + p_erf * x); + T y = 1.0f - (((((a5_erf * t + a4_erf) * t) + a3_erf) * t + a2_erf) * t + a1_erf) * t * exp(-x * x); + return sign_x * y; +} + +template T elu_approx(T x); + +template<> inline float elu_approx(float x) { + return (x > 0.f) ? x : (exp(x) - 1); +} + +template<> inline float4 elu_approx(float4 x) { + float4 res; + + res[0] = (x[0] > 0.0f) ? x[0] : (exp(x[0]) - 1.0f); + res[1] = (x[1] > 0.0f) ? x[1] : (exp(x[1]) - 1.0f); + res[2] = (x[2] > 0.0f) ? x[2] : (exp(x[2]) - 1.0f); + res[3] = (x[3] > 0.0f) ? x[3] : (exp(x[3]) - 1.0f); + + return res; +} + +constant short FC_unary_op [[function_constant(FC_UNARY + 0)]]; +constant bool FC_unary_cnt[[function_constant(FC_UNARY + 1)]]; + +template +kernel void kernel_unary_impl( + constant ggml_metal_kargs_unary & args, + device const char * src0, + device char * dst, + uint3 tgpig[[threadgroup_position_in_grid]], + ushort3 tpitg[[thread_position_in_threadgroup]], + ushort3 ntg[[threads_per_threadgroup]]) { +#define FC_OP FC_unary_op +#define FC_CNT FC_unary_cnt + + device const T0 * src0_ptr; + device T * dst_ptr; + + int i0; + + if (FC_CNT) { + i0 = tgpig.x; + + src0_ptr = (device const T0 *) (src0); + dst_ptr = (device T *) (dst); + } else { + const int i03 = tgpig.z; + const int i02 = tgpig.y; + const int k0 = tgpig.x/args.ne01; + const int i01 = tgpig.x - k0*args.ne01; + + i0 = k0*ntg.x + tpitg.x; + + src0_ptr = (device const T0 *) (src0 + i03*args.nb03 + i02*args.nb02 + i01*args.nb01); + dst_ptr = (device T *) (dst + i03*args.nb3 + i02*args.nb2 + i01*args.nb1 ); + } + + { + //threadgroup_barrier(mem_flags::mem_none); + + if (!FC_CNT) { + if (i0 >= args.ne0) { + return; + } + } + + const TC x = (TC) src0_ptr[i0]; + + if (FC_OP == OP_UNARY_NUM_SCALE) { + dst_ptr[i0] = (T) (args.scale * x + args.bias); + } + + if (FC_OP == OP_UNARY_NUM_FILL) { + dst_ptr[i0] = (T) args.val; + } + + if (FC_OP == OP_UNARY_NUM_CLAMP) { + dst_ptr[i0] = (T) clamp(x, args.min, args.max); + } + + if (FC_OP == OP_UNARY_NUM_SQR) { + dst_ptr[i0] = (T) (x * x); + } + + if (FC_OP == OP_UNARY_NUM_SQRT) { + dst_ptr[i0] = (T) sqrt(x); + } + + if (FC_OP == OP_UNARY_NUM_SIN) { + dst_ptr[i0] = (T) sin(x); + } + + if (FC_OP == OP_UNARY_NUM_COS) { + dst_ptr[i0] = (T) cos(x); + } + + if (FC_OP == OP_UNARY_NUM_LOG) { + dst_ptr[i0] = (T) log(x); + } + + if (FC_OP == OP_UNARY_NUM_LEAKY_RELU) { + dst_ptr[i0] = (T) (TC(x > 0)*x + TC(x <= 0)*(x * args.slope)); + } + + if (FC_OP == OP_UNARY_NUM_TANH) { + dst_ptr[i0] = (T) precise::tanh(x); + } + + if (FC_OP == OP_UNARY_NUM_RELU) { + dst_ptr[i0] = (T) fmax(0, x); + } + + if (FC_OP == OP_UNARY_NUM_SIGMOID) { + dst_ptr[i0] = (T) (1 / (1 + exp(-x))); + } + + if (FC_OP == OP_UNARY_NUM_GELU) { + dst_ptr[i0] = (T) (0.5*x*(1 + precise::tanh(SQRT_2_OVER_PI*x*(1 + GELU_COEF_A*x*x)))); + } + + if (FC_OP == OP_UNARY_NUM_GELU_ERF) { + dst_ptr[i0] = (T) (0.5*x*(1 + erf_approx(SQRT_2_INV*x))); + } + + if (FC_OP == OP_UNARY_NUM_GELU_QUICK) { + dst_ptr[i0] = (T) (x * (1/(1 + exp(GELU_QUICK_COEF*x)))); + } + + if (FC_OP == OP_UNARY_NUM_SILU) { + dst_ptr[i0] = (T) (x / (1 + exp(-x))); + } + + if (FC_OP == OP_UNARY_NUM_ELU) { + dst_ptr[i0] = (T) elu_approx(x); + } + + if (FC_OP == OP_UNARY_NUM_NEG) { + dst_ptr[i0] = (T) -x; + } + + if (FC_OP == OP_UNARY_NUM_ABS) { + dst_ptr[i0] = (T) fabs(x); + } + + if (FC_OP == OP_UNARY_NUM_SGN) { + dst_ptr[i0] = T(x > 0) - T(x < 0); + } + + if (FC_OP == OP_UNARY_NUM_STEP) { + dst_ptr[i0] = T(x > 0); + } + + if (FC_OP == OP_UNARY_NUM_HARDSWISH) { + dst_ptr[i0] = (T) (x * fmax(0, fmin(1, x/6 + 0.5))); + } + + if (FC_OP == OP_UNARY_NUM_HARDSIGMOID) { + dst_ptr[i0] = (T) fmax(0, fmin(1, x/6 + 0.5)); + } + + if (FC_OP == OP_UNARY_NUM_EXP) { + dst_ptr[i0] = (T) exp(x); + } + + if (FC_OP == OP_UNARY_NUM_SOFTPLUS) { + dst_ptr[i0] = (T) select(log(1 + exp(x)), x, x > 20); + } + + if (FC_OP == OP_UNARY_NUM_EXPM1) { + // TODO: precise implementation + dst_ptr[i0] = (T) (exp(x) - 1); + } + } + +#undef FC_OP +#undef FC_CNT +} + +typedef decltype(kernel_unary_impl) kernel_unary_t; + +template [[host_name("kernel_unary_f32_f32_4")]] kernel kernel_unary_t kernel_unary_impl; + +// OP: 0 - add, 1 - sub, 2 - mul, 3 - div +constant short FC_bin_op [[function_constant(FC_BIN + 0)]]; +constant short FC_bin_f [[function_constant(FC_BIN + 1)]]; +constant bool FC_bin_rb [[function_constant(FC_BIN + 2)]]; + +template +kernel void kernel_bin_fuse_impl( + constant ggml_metal_kargs_bin & args, + device const char * src0, + device const char * src1, + device char * dst, + uint3 tgpig[[threadgroup_position_in_grid]], + ushort3 tpitg[[thread_position_in_threadgroup]], + ushort3 ntg[[threads_per_threadgroup]]) { +#define FC_OP FC_bin_op +#define FC_F FC_bin_f +#define FC_RB FC_bin_rb + + if (FC_RB) { + // row broadcast + const uint i0 = tgpig.x; + const uint i1 = i0%args.ne10; + + device const T0 * src0_row = (device const T0 *) (src0); + device T * dst_row = (device T *) (dst); + + if (FC_F == 1) { + device const T1 * src1_row = (device const T1 *) (src1 + args.o1[0]); + + if (FC_OP == 0) { + dst_row[i0] = src0_row[i0] + src1_row[i1]; + } + + if (FC_OP == 1) { + dst_row[i0] = src0_row[i0] - src1_row[i1]; + } + + if (FC_OP == 2) { + dst_row[i0] = src0_row[i0] * src1_row[i1]; + } + + if (FC_OP == 3) { + dst_row[i0] = src0_row[i0] / src1_row[i1]; + } + } else { + T0 res = src0_row[i0]; + + if (FC_OP == 0) { + FOR_UNROLL (short j = 0; j < FC_F; ++j) { + res += ((device const T1 *) (src1 + args.o1[j]))[i1]; + } + } + + if (FC_OP == 1) { + FOR_UNROLL (short j = 0; j < FC_F; ++j) { + res -= ((device const T1 *) (src1 + args.o1[j]))[i1]; + } + } + + if (FC_OP == 2) { + FOR_UNROLL (short j = 0; j < FC_F; ++j) { + res *= ((device const T1 *) (src1 + args.o1[j]))[i1]; + } + } + + if (FC_OP == 3) { + FOR_UNROLL (short j = 0; j < FC_F; ++j) { + res /= ((device const T1 *) (src1 + args.o1[j]))[i1]; + } + } + + dst_row[i0] = res; + } + } else { + const int i03 = tgpig.z; + const int i02 = tgpig.y; + const int i01 = tgpig.x; + + if (i01 >= args.ne01) { + return; + } + + const int i13 = i03%args.ne13; + const int i12 = i02%args.ne12; + const int i11 = i01%args.ne11; + + device const T0 * src0_ptr = (device const T0 *) (src0 + i03*args.nb03 + i02*args.nb02 + i01*args.nb01 + args.offs); + device T * dst_ptr = (device T *) (dst + i03*args.nb3 + i02*args.nb2 + i01*args.nb1 + args.offs); + + if (FC_F == 1) { + device const T1 * src1_ptr = (device const T1 *) (src1 + args.o1[0] + i13*args.nb13 + i12*args.nb12 + i11*args.nb11); + + for (int i0 = tpitg.x; i0 < args.ne0; i0 += ntg.x) { + const int i10 = i0%args.ne10; + + if (FC_OP == 0) { + dst_ptr[i0] = src0_ptr[i0] + src1_ptr[i10]; + } + + if (FC_OP == 1) { + dst_ptr[i0] = src0_ptr[i0] - src1_ptr[i10]; + } + + if (FC_OP == 2) { + dst_ptr[i0] = src0_ptr[i0] * src1_ptr[i10]; + } + + if (FC_OP == 3) { + dst_ptr[i0] = src0_ptr[i0] / src1_ptr[i10]; + } + } + } else { + device const T1 * src1_ptr[8]; + FOR_UNROLL (short j = 0; j < FC_F; ++j) { + src1_ptr[j] = (device const T1 *) (src1 + args.o1[j] + i13*args.nb13 + i12*args.nb12 + i11*args.nb11); + } + + for (int i0 = tpitg.x; i0 < args.ne0; i0 += ntg.x) { + const int i10 = i0%args.ne10; + + T res = src0_ptr[i0]; + + if (FC_OP == 0) { + FOR_UNROLL (short j = 0; j < FC_F; ++j) { + res += src1_ptr[j][i10]; + } + } + + if (FC_OP == 1) { + FOR_UNROLL (short j = 0; j < FC_F; ++j) { + res -= src1_ptr[j][i10]; + } + } + + if (FC_OP == 2) { + FOR_UNROLL (short j = 0; j < FC_F; ++j) { + res *= src1_ptr[j][i10]; + } + } + + if (FC_OP == 3) { + FOR_UNROLL (short j = 0; j < FC_F; ++j) { + res /= src1_ptr[j][i10]; + } + } + + dst_ptr[i0] = res; + } + } + } + +#undef FC_OP +#undef FC_F +#undef FC_RB +} + +typedef decltype(kernel_bin_fuse_impl) kernel_bin_fuse_t; + +template [[host_name("kernel_bin_fuse_f32_f32_f32_4")]] kernel kernel_bin_fuse_t kernel_bin_fuse_impl; + + + +template +kernel void kernel_repeat( + constant ggml_metal_kargs_repeat & args, + device const char * src0, + device char * dst, + uint3 tgpig[[threadgroup_position_in_grid]], + ushort3 tpitg[[thread_position_in_threadgroup]], + ushort3 ntg[[threads_per_threadgroup]]) { + const int i3 = tgpig.z; + const int i2 = tgpig.y; + const int i1 = tgpig.x; + + const int i03 = i3%args.ne03; + const int i02 = i2%args.ne02; + const int i01 = i1%args.ne01; + + device const char * src0_ptr = src0 + i03*args.nb03 + i02*args.nb02 + i01*args.nb01; + device char * dst_ptr = dst + i3*args.nb3 + i2*args.nb2 + i1*args.nb1; + + for (int i0 = tpitg.x; i0 < args.ne0; i0 += ntg.x) { + const int i00 = i0%args.ne00; + *((device T *)(dst_ptr + i0*args.nb0)) = *((device T *)(src0_ptr + i00*args.nb00)); + } +} + +typedef decltype(kernel_repeat) kernel_repeat_t; + + + + + + + + + + + + + + + + +constant short FC_sum_rows_op [[function_constant(FC_SUM_ROWS + 0)]]; + +template +kernel void kernel_sum_rows_impl( + constant ggml_metal_kargs_sum_rows & args, + device const char * src0, + device char * dst, + threadgroup char * shmem [[threadgroup(0)]], + uint3 tgpig[[threadgroup_position_in_grid]], + ushort3 tpitg[[thread_position_in_threadgroup]], + ushort sgitg[[simdgroup_index_in_threadgroup]], + ushort tiisg[[thread_index_in_simdgroup]], + ushort3 ntg[[threads_per_threadgroup]]) { +#define FC_OP FC_sum_rows_op + + const int i3 = tgpig.z; + const int i2 = tgpig.y; + const int i1 = tgpig.x; + + threadgroup T0 * shmem_t = (threadgroup T0 *) shmem; + + if (sgitg == 0) { + shmem_t[tiisg] = 0.0f; + } + + device const T0 * src_row = (device const T0 *) (src0 + i1*args.nb01 + i2*args.nb02 + i3*args.nb03); + device T * dst_row = (device T *) (dst + i1*args.nb1 + i2*args.nb2 + i3*args.nb3); + + T0 sumf = T0(0.0f); + + for (int64_t i0 = tpitg.x; i0 < args.ne00; i0 += ntg.x) { + sumf += src_row[i0]; + } + + sumf = simd_sum(sumf); + + threadgroup_barrier(mem_flags::mem_threadgroup); + + if (tiisg == 0) { + shmem_t[sgitg] = sumf; + } + + threadgroup_barrier(mem_flags::mem_threadgroup); + + sumf = shmem_t[tiisg]; + sumf = simd_sum(sumf); + + if (tpitg.x == 0) { + if (FC_OP == OP_SUM_ROWS_NUM_MEAN) { + if (is_same::value) { + dst_row[0] = sum(sumf) / (4*args.ne00); + } else { + dst_row[0] = sum(sumf) / args.ne00; + } + } else { + dst_row[0] = sum(sumf); + } + } + +#undef FC_OP +} + +typedef decltype(kernel_sum_rows_impl) kernel_sum_rows_t; + + +template +kernel void kernel_cumsum_blk( + constant ggml_metal_kargs_cumsum_blk & args, + device const char * src0, + device char * tmp, + device char * dst, + threadgroup char * shmem [[threadgroup(0)]], + uint3 tgpig[[threadgroup_position_in_grid]], + ushort3 tpitg[[thread_position_in_threadgroup]], + ushort sgitg[[simdgroup_index_in_threadgroup]], + ushort tiisg[[thread_index_in_simdgroup]], + ushort3 ntg[[threads_per_threadgroup]]) { + const int ib = tgpig[0]/args.ne01; + + const int i00 = ib*ntg.x; + const int i01 = tgpig[0]%args.ne01; + const int i02 = tgpig[1]; + const int i03 = tgpig[2]; + + device const float * src0_row = (device const float *) (src0 + + args.nb01*i01 + + args.nb02*i02 + + args.nb03*i03); + + threadgroup float * shmem_f32 = (threadgroup float *) shmem; + + float v = 0.0f; + + if (i00 + tpitg.x < args.ne00) { + v = src0_row[i00 + tpitg.x]; + } + + float s = simd_prefix_inclusive_sum(v); + + if (tiisg == N_SIMDWIDTH - 1) { + shmem_f32[sgitg] = s; + } + + threadgroup_barrier(mem_flags::mem_threadgroup); + + if (sgitg == 0) { + shmem_f32[tiisg] = simd_prefix_exclusive_sum(shmem_f32[tiisg]); + } + + threadgroup_barrier(mem_flags::mem_threadgroup); + + s += shmem_f32[sgitg]; + + device float * dst_row = (device float *) dst + + args.ne00*i01 + + args.ne00*args.ne01*i02 + + args.ne00*args.ne01*args.ne02*i03; + + if (i00 + tpitg.x < args.ne00) { + dst_row[i00 + tpitg.x] = s; + } + + if (args.outb && tpitg.x == ntg.x - 1) { + device float * tmp_row = (device float *) tmp + + args.net0*i01 + + args.net0*args.net1*i02 + + args.net0*args.net1*args.net2*i03; + + tmp_row[ib] = s; + } +} + +typedef decltype(kernel_cumsum_blk) kernel_cumsum_blk_t; + + +template +kernel void kernel_cumsum_add( + constant ggml_metal_kargs_cumsum_add & args, + device const char * tmp, + device char * dst, + uint3 tgpig[[threadgroup_position_in_grid]], + ushort3 tpitg[[thread_position_in_threadgroup]], + ushort sgitg[[simdgroup_index_in_threadgroup]], + ushort tiisg[[thread_index_in_simdgroup]], + ushort3 ntg[[threads_per_threadgroup]]) { + const int ib = tgpig[0]/args.ne01; + + if (ib == 0) { + return; + } + + const int i00 = ib*ntg.x; + const int i01 = tgpig[0]%args.ne01; + const int i02 = tgpig[1]; + const int i03 = tgpig[2]; + + device const float * tmp_row = (device const float *) (tmp + + args.nbt1*i01 + + args.nbt2*i02 + + args.nbt3*i03); + + device float * dst_row = (device float *) dst + + args.ne00*i01 + + args.ne00*args.ne01*i02 + + args.ne00*args.ne01*args.ne02*i03; + + if (i00 + tpitg.x < args.ne00) { + dst_row[i00 + tpitg.x] += tmp_row[ib - 1]; + } +} + +typedef decltype(kernel_cumsum_add) kernel_cumsum_add_t; + + + +template +bool _ggml_vec_tri_cmp(const int i, const int r); + +template<> +bool _ggml_vec_tri_cmp(const int i, const int r) { + return i < r; +} + +template<> +bool _ggml_vec_tri_cmp(const int i, const int r) { + return i <= r; +} + +template<> +bool _ggml_vec_tri_cmp(const int i, const int r) { + return i > r; +} + +template<> +bool _ggml_vec_tri_cmp(const int i, const int r) { + return i >= r; +} + +template +kernel void kernel_tri( + constant ggml_metal_kargs_tri & args, + device const char * src0, + device const char * dst, + uint3 tgpig[[threadgroup_position_in_grid]], + ushort3 tpitg[[thread_position_in_threadgroup]], + ushort3 ntg[[threads_per_threadgroup]]) { + const int i3 = tgpig.z; + const int i2 = tgpig.y; + const int i1 = tgpig.x; + + if (i3 >= args.ne03 || i2 >= args.ne02 || i1 >= args.ne01) { + return; + } + + device const T * src_row = (device const T *) ((device const char *) src0 + i1*args.nb01 + i2*args.nb02 + i3*args.nb03); + device T * dst_row = (device T *) ((device char *) dst + i1*args.nb1 + i2*args.nb2 + i3*args.nb3); + + // Each thread is a single element of the row if ne00 < max threads per + // threadgroup, so this will loop once for each index that this thread is + // responsible for + for (int64_t i0 = tpitg.x; i0 < args.ne00; i0 += ntg.x) { + // Use the comparison as a mask for branchless + dst_row[i0] = static_cast(_ggml_vec_tri_cmp(i0, i1)) * src_row[i0]; + } +} + +typedef decltype(kernel_tri) kernel_tri_t; + +#if defined(GGML_METAL_HAS_BF16) +#endif + +template +kernel void kernel_soft_max( + constant ggml_metal_kargs_soft_max & args, + device const char * src0, + device const char * src1, + device const char * src2, + device char * dst, + threadgroup float * buf [[threadgroup(0)]], + uint3 tgpig[[threadgroup_position_in_grid]], + uint3 tpitg[[thread_position_in_threadgroup]], + uint sgitg[[simdgroup_index_in_threadgroup]], + uint tiisg[[thread_index_in_simdgroup]], + uint3 tptg[[threads_per_threadgroup]]) { + const int32_t i03 = tgpig.z; + const int32_t i02 = tgpig.y; + const int32_t i01 = tgpig.x; + + const int32_t i13 = i03%args.ne13; + const int32_t i12 = i02%args.ne12; + const int32_t i11 = i01; + + device const float * psrc0 = (device const float *) (src0 + i01*args.nb01 + i02*args.nb02 + i03*args.nb03); + device const T * pmask = src1 != src0 ? (device const T * ) (src1 + i11*args.nb11 + i12*args.nb12 + i13*args.nb13) : nullptr; + device const float * psrc2 = src2 != src0 ? (device const float *) (src2) : nullptr; + device float * pdst = (device float *) (dst + i01*args.nb1 + i02*args.nb2 + i03*args.nb3); + + float slope = 1.0f; + + // ALiBi + if (args.max_bias > 0.0f) { + const int32_t h = i02; + + const float base = h < args.n_head_log2 ? args.m0 : args.m1; + const int exp = h < args.n_head_log2 ? h + 1 : 2*(h - args.n_head_log2) + 1; + + slope = pow(base, exp); + } + + // parallel max + float lmax = psrc2 ? psrc2[i02] : -INFINITY; + + for (int i00 = tpitg.x; i00 < args.ne00; i00 += tptg.x) { + lmax = MAX(lmax, psrc0[i00]*args.scale + (pmask ? slope*pmask[i00] : 0.0f)); + } + + // find the max value in the block + float max_val = simd_max(lmax); + if (tptg.x > N_SIMDWIDTH) { + if (sgitg == 0) { + buf[tiisg] = -INFINITY; + } + + threadgroup_barrier(mem_flags::mem_threadgroup); + + if (tiisg == 0) { + buf[sgitg] = max_val; + } + + threadgroup_barrier(mem_flags::mem_threadgroup); + + max_val = buf[tiisg]; + max_val = simd_max(max_val); + } + + // parallel sum + float lsum = 0.0f; + for (int i00 = tpitg.x; i00 < args.ne00; i00 += tptg.x) { + const float exp_psrc0 = exp((psrc0[i00]*args.scale + (pmask ? slope*pmask[i00] : 0.0f)) - max_val); + lsum += exp_psrc0; + pdst[i00] = exp_psrc0; + } + + // This barrier fixes a failing test + // ref: https://github.com/ggml-org/ggml/pull/621#discussion_r1425156335 + threadgroup_barrier(mem_flags::mem_none); + + float sum = simd_sum(lsum); + + if (tptg.x > N_SIMDWIDTH) { + if (sgitg == 0) { + buf[tiisg] = 0.0f; + } + + threadgroup_barrier(mem_flags::mem_threadgroup); + + if (tiisg == 0) { + buf[sgitg] = sum; + } + + threadgroup_barrier(mem_flags::mem_threadgroup); + + sum = buf[tiisg]; + sum = simd_sum(sum); + } + + if (psrc2) { + sum += exp(psrc2[i02] - max_val); + } + + const float inv_sum = 1.0f/sum; + + for (int i00 = tpitg.x; i00 < args.ne00; i00 += tptg.x) { + pdst[i00] *= inv_sum; + } +} + +template +kernel void kernel_soft_max_4( + constant ggml_metal_kargs_soft_max & args, + device const char * src0, + device const char * src1, + device const char * src2, + device char * dst, + threadgroup float * buf [[threadgroup(0)]], + uint3 tgpig[[threadgroup_position_in_grid]], + uint3 tpitg[[thread_position_in_threadgroup]], + uint sgitg[[simdgroup_index_in_threadgroup]], + uint tiisg[[thread_index_in_simdgroup]], + uint3 tptg[[threads_per_threadgroup]]) { + const int32_t i03 = tgpig.z; + const int32_t i02 = tgpig.y; + const int32_t i01 = tgpig.x; + + const int32_t i13 = i03%args.ne13; + const int32_t i12 = i02%args.ne12; + const int32_t i11 = i01; + + device const float4 * psrc4 = (device const float4 *) (src0 + i01*args.nb01 + i02*args.nb02 + i03*args.nb03); + device const T * pmask = src1 != src0 ? (device const T * ) (src1 + i11*args.nb11 + i12*args.nb12 + i13*args.nb13) : nullptr; + device const float * psrc2 = src2 != src0 ? (device const float * ) (src2) : nullptr; + device float4 * pdst4 = (device float4 *) (dst + i01*args.nb1 + i02*args.nb2 + i03*args.nb3); + + float slope = 1.0f; + + if (args.max_bias > 0.0f) { + const int32_t h = i02; + + const float base = h < args.n_head_log2 ? args.m0 : args.m1; + const int exp = h < args.n_head_log2 ? h + 1 : 2*(h - args.n_head_log2) + 1; + + slope = pow(base, exp); + } + + // parallel max + float4 lmax4 = psrc2 ? psrc2[i02] : -INFINITY; + + for (int i00 = tpitg.x; i00 < args.ne00/4; i00 += tptg.x) { + lmax4 = fmax(lmax4, psrc4[i00]*args.scale + (float4)((pmask ? slope*pmask[i00] : 0.0f))); + } + + const float lmax = MAX(MAX(lmax4[0], lmax4[1]), MAX(lmax4[2], lmax4[3])); + + float max_val = simd_max(lmax); + if (tptg.x > N_SIMDWIDTH) { + if (sgitg == 0) { + buf[tiisg] = -INFINITY; + } + + threadgroup_barrier(mem_flags::mem_threadgroup); + + if (tiisg == 0) { + buf[sgitg] = max_val; + } + + threadgroup_barrier(mem_flags::mem_threadgroup); + + max_val = buf[tiisg]; + max_val = simd_max(max_val); + } + + // parallel sum + float4 lsum4 = 0.0f; + for (int i00 = tpitg.x; i00 < args.ne00/4; i00 += tptg.x) { + const float4 exp_psrc4 = exp((psrc4[i00]*args.scale + (float4)((pmask ? slope*pmask[i00] : 0.0f))) - max_val); + lsum4 += exp_psrc4; + pdst4[i00] = exp_psrc4; + } + + const float lsum = lsum4[0] + lsum4[1] + lsum4[2] + lsum4[3]; + + // This barrier fixes a failing test + // ref: https://github.com/ggml-org/ggml/pull/621#discussion_r1425156335 + threadgroup_barrier(mem_flags::mem_none); + + float sum = simd_sum(lsum); + + if (tptg.x > N_SIMDWIDTH) { + if (sgitg == 0) { + buf[tiisg] = 0.0f; + } + + threadgroup_barrier(mem_flags::mem_threadgroup); + + if (tiisg == 0) { + buf[sgitg] = sum; + } + + threadgroup_barrier(mem_flags::mem_threadgroup); + + sum = buf[tiisg]; + sum = simd_sum(sum); + } + + if (psrc2) { + sum += exp(psrc2[i02] - max_val); + } + + const float inv_sum = 1.0f/sum; + + for (int i00 = tpitg.x; i00 < args.ne00/4; i00 += tptg.x) { + pdst4[i00] *= inv_sum; + } +} + +typedef decltype(kernel_soft_max) kernel_soft_max_t; +typedef decltype(kernel_soft_max_4) kernel_soft_max_4_t; + + +// ref: ggml.c:ggml_compute_forward_ssm_conv_f32 + + + + +constant short FC_ssm_conv_bs [[function_constant(FC_SSM_CONV + 0)]]; + +// Batched version: each threadgroup processes multiple tokens for better efficiency +// Thread layout: each thread handles one token, threadgroup covers BATCH_SIZE tokens + + + + +// ref: ggml.c:ggml_compute_forward_ssm_scan_f32, Mamba-2 part +// Optimized version: reduces redundant memory loads by having one thread load shared values + + + + + + +constant short FC_solve_tri_nsg [[function_constant(FC_SOLVE_TRI + 0)]]; +constant short FC_solve_tri_n [[function_constant(FC_SOLVE_TRI + 1)]]; +constant short FC_solve_tri_k [[function_constant(FC_SOLVE_TRI + 2)]]; + + + + + +// F == 1 : norm (no fuse) +// F == 2 : norm + mul +// F == 3 : norm + mul + add +template +kernel void kernel_norm_fuse_impl( + constant ggml_metal_kargs_norm & args, + device const char * src0, + device const char * src1_0, + device const char * src1_1, + device char * dst, + threadgroup float * shmem_f32 [[threadgroup(0)]], + uint3 tgpig[[threadgroup_position_in_grid]], + ushort3 tpitg[[thread_position_in_threadgroup]], + ushort sgitg[[simdgroup_index_in_threadgroup]], + ushort tiisg[[thread_index_in_simdgroup]], + ushort3 ntg[[threads_per_threadgroup]]) { + if (sgitg == 0) { + shmem_f32[tiisg] = 0.0f; + } + + const int i01 = tgpig.x; + const int i02 = tgpig.y; + const int i03 = tgpig.z; + + device const T * x = (device const T *) (src0 + i03*args.nbf3[0] + i02*args.nbf2[0] + i01*args.nbf1[0]); + + device const T * f0 = (device const T *) (src1_0 + (i03%args.nef3[1])*args.nbf3[1] + (i02%args.nef2[1])*args.nbf2[1] + (i01%args.nef1[1])*args.nbf1[1]); + device const T * f1 = (device const T *) (src1_1 + (i03%args.nef3[2])*args.nbf3[2] + (i02%args.nef2[2])*args.nbf2[2] + (i01%args.nef1[2])*args.nbf1[2]); + + T sumft(0.0f); + + float sumf = 0.0f; + + for (int i00 = tpitg.x; i00 < args.ne00_t; i00 += ntg.x) { + sumft += x[i00]; + } + sumf = dot(sumft, T(1.0f)); + sumf = simd_sum(sumf); + + threadgroup_barrier(mem_flags::mem_threadgroup); + + if (tiisg == 0) { + shmem_f32[sgitg] = sumf; + } + + threadgroup_barrier(mem_flags::mem_threadgroup); + + sumf = shmem_f32[tiisg]; + sumf = simd_sum(sumf); + + const float mean = sumf/args.ne00; + + device T * y = (device T *) (dst + i03*args.nb3 + i02*args.nb2 + i01*args.nb1); + + sumf = 0.0f; + for (int i00 = tpitg.x; i00 < args.ne00_t; i00 += ntg.x) { + y[i00] = x[i00] - mean; + sumf += dot(y[i00], y[i00]); + } + sumf = simd_sum(sumf); + + threadgroup_barrier(mem_flags::mem_threadgroup); + + if (tiisg == 0) { + shmem_f32[sgitg] = sumf; + } + + threadgroup_barrier(mem_flags::mem_threadgroup); + + sumf = shmem_f32[tiisg]; + sumf = simd_sum(sumf); + + const float variance = sumf/args.ne00; + + const float scale = 1.0f/sqrt(variance + args.eps); + for (int i00 = tpitg.x; i00 < args.ne00_t; i00 += ntg.x) { + if (F == 1) { + y[i00] = (y[i00]*scale); + } + if (F == 2) { + y[i00] = (y[i00]*scale)*f0[i00]; + } + if (F == 3) { + y[i00] = (y[i00]*scale)*f0[i00] + f1[i00]; + } + } +} + +typedef decltype(kernel_norm_fuse_impl) kernel_norm_fuse_t; + + +template [[host_name("kernel_norm_mul_add_f32_4")]] kernel kernel_norm_fuse_t kernel_norm_fuse_impl; + +// F == 1 : rms_norm (no fuse) +// F == 2 : rms_norm + mul +// F == 3 : rms_norm + mul + add +template +kernel void kernel_rms_norm_fuse_impl( + constant ggml_metal_kargs_norm & args, + device const char * src0, + device const char * src1_0, + device const char * src1_1, + device char * dst, + threadgroup float * shmem_f32 [[threadgroup(0)]], + uint3 tgpig[[threadgroup_position_in_grid]], + ushort3 tpitg[[thread_position_in_threadgroup]], + ushort sgitg[[simdgroup_index_in_threadgroup]], + ushort tiisg[[thread_index_in_simdgroup]], + ushort3 ntg[[threads_per_threadgroup]]) { + if (sgitg == 0) { + shmem_f32[tiisg] = 0.0f; + } + + const int i01 = tgpig.x; + const int i02 = tgpig.y; + const int i03 = tgpig.z; + + device const T * x = (device const T *) (src0 + i03*args.nbf3[0] + i02*args.nbf2[0] + i01*args.nbf1[0]); + + device const T * f0 = (device const T *) (src1_0 + (i03%args.nef3[1])*args.nbf3[1] + (i02%args.nef2[1])*args.nbf2[1] + (i01%args.nef1[1])*args.nbf1[1]); + device const T * f1 = (device const T *) (src1_1 + (i03%args.nef3[2])*args.nbf3[2] + (i02%args.nef2[2])*args.nbf2[2] + (i01%args.nef1[2])*args.nbf1[2]); + + float sumf = 0.0f; + + // parallel sum + for (int i00 = tpitg.x; i00 < args.ne00_t; i00 += ntg.x) { + sumf += dot(x[i00], x[i00]); + } + sumf = simd_sum(sumf); + + threadgroup_barrier(mem_flags::mem_threadgroup); + + if (tiisg == 0) { + shmem_f32[sgitg] = sumf; + } + + threadgroup_barrier(mem_flags::mem_threadgroup); + + sumf = shmem_f32[tiisg]; + sumf = simd_sum(sumf); + + const float mean = sumf/args.ne00; + const float scale = 1.0f/sqrt(mean + args.eps); + + device T * y = (device T *) (dst + i03*args.nb3 + i02*args.nb2 + i01*args.nb1); + for (int i00 = tpitg.x; i00 < args.ne00_t; i00 += ntg.x) { + if (F == 1) { + y[i00] = (x[i00]*scale); + } + if (F == 2) { + y[i00] = (x[i00]*scale)*f0[i00]; + } + if (F == 3) { + y[i00] = (x[i00]*scale)*f0[i00] + f1[i00]; + } + } +} + +typedef decltype(kernel_rms_norm_fuse_impl) kernel_rms_norm_fuse_t; + + + +template +kernel void kernel_l2_norm_impl( + constant ggml_metal_kargs_l2_norm & args, + device const char * src0, + device char * dst, + threadgroup float * shmem_f32 [[threadgroup(0)]], + uint3 tgpig[[threadgroup_position_in_grid]], + ushort3 tpitg[[thread_position_in_threadgroup]], + ushort sgitg[[simdgroup_index_in_threadgroup]], + ushort tiisg[[thread_index_in_simdgroup]], + ushort3 ntg[[threads_per_threadgroup]]) { + const int i03 = tgpig.z; + const int i02 = tgpig.y; + const int i01 = tgpig.x; + + if (sgitg == 0) { + shmem_f32[tiisg] = 0.0f; + } + + device const T0 * x = (device const T0 *) (src0 + i03*args.nb03 + i02*args.nb02 + i01*args.nb01); + device T * y = (device T *) (dst + i03*args.nb3 + i02*args.nb2 + i01*args.nb1); + + float sumf = 0.0f; + + // parallel sum + for (int i00 = tpitg.x; i00 < args.ne00; i00 += ntg.x) { + sumf += dot(x[i00], x[i00]); + } + sumf = simd_sum(sumf); + + threadgroup_barrier(mem_flags::mem_threadgroup); + + if (tiisg == 0) { + shmem_f32[sgitg] = sumf; + } + + threadgroup_barrier(mem_flags::mem_threadgroup); + + sumf = shmem_f32[tiisg]; + sumf = simd_sum(sumf); + + const float scale = 1.0f/sqrt(max(sumf, args.eps)); + + for (int i00 = tpitg.x; i00 < args.ne00; i00 += ntg.x) { + y[i00] = x[i00] * scale; + } +} + +typedef decltype(kernel_l2_norm_impl) kernel_l2_norm_t; + + + + +// function for calculate inner product between half a q4_0 block and 16 floats (yl), sumy is SUM(yl[i]) +// il indicates where the q4 quants begin (0 or QK4_0/4) +// we assume that the yl's have been multiplied with the appropriate scale factor +// that corresponds to the missing bit shifts (1, 1/16, 1/256, 1/4096) +inline float block_q_n_dot_y(device const block_q4_0 * qb_curr, float sumy, thread float * yl, int il) { + float d = qb_curr->d; + + float acc[4] = { 0.0f, 0.0f, 0.0f, 0.0f }; + + device const uint16_t * qs = ((device const uint16_t *) qb_curr + 1 + il/2); + + for (int i = 0; i < 8; i += 2) { + acc[0] += yl[i + 0] * (qs[i / 2] & 0x000F); + acc[1] += yl[i + 1] * (qs[i / 2] & 0x0F00); + acc[2] += yl[i + 8] * (qs[i / 2] & 0x00F0); + acc[3] += yl[i + 9] * (qs[i / 2] & 0xF000); + } + + return d * (sumy * -8.f + acc[0] + acc[1] + acc[2] + acc[3]); +} + +// function for calculate inner product between half a q4_1 block and 16 floats (yl), sumy is SUM(yl[i]) +// il indicates where the q4 quants begin (0 or QK4_0/4) +// we assume that the yl's have been multiplied with the appropriate scale factor +// that corresponds to the missing bit shifts (1, 1/16, 1/256, 1/4096) +inline float block_q_n_dot_y(device const block_q4_1 * qb_curr, float sumy, thread float * yl, int il) { + float d = qb_curr->d; + float m = qb_curr->m; + + float acc[4] = { 0.0f, 0.0f, 0.0f, 0.0f }; + + device const uint16_t * qs = ((device const uint16_t *) qb_curr + 2 + il/2); + + for (int i = 0; i < 8; i+=2) { + acc[0] += yl[i + 0] * (qs[i / 2] & 0x000F); + acc[1] += yl[i + 1] * (qs[i / 2] & 0x0F00); + acc[2] += yl[i + 8] * (qs[i / 2] & 0x00F0); + acc[3] += yl[i + 9] * (qs[i / 2] & 0xF000); + } + + return d * (acc[0] + acc[1] + acc[2] + acc[3]) + sumy * m; +} + +// function for calculate inner product between half a q5_0 block and 16 floats (yl), sumy is SUM(yl[i]) +// il indicates where the q5 quants begin (0 or QK5_0/4) +// we assume that the yl's have been multiplied with the appropriate scale factor +// that corresponds to the missing bit shifts (1, 1/16, 1/256, 1/4096) +inline float block_q_n_dot_y(device const block_q5_0 * qb_curr, float sumy, thread float * yl, int il) { + float d = qb_curr->d; + + float acc[4] = { 0.0f, 0.0f, 0.0f, 0.0f }; + + device const uint16_t * qs = ((device const uint16_t *)qb_curr + 3 + il/2); + const uint32_t qh = *((device const uint32_t *)qb_curr->qh); + + for (int i = 0; i < 8; i+=2) { + acc[0] += yl[i + 0] * ((qs[i / 2] & 0x000F) | ((qh >> (i+0+il ) << 4 ) & 0x00010)); + acc[1] += yl[i + 1] * ((qs[i / 2] & 0x0F00) | ((qh >> (i+1+il ) << 12) & 0x01000)); + acc[2] += yl[i + 8] * ((qs[i / 2] & 0x00F0) | ((qh >> (i+0+il+QK5_0/2) << 8 ) & 0x00100)); + acc[3] += yl[i + 9] * ((qs[i / 2] & 0xF000) | ((qh >> (i+1+il+QK5_0/2) << 16) & 0x10000)); + } + + return d * (sumy * -16.f + acc[0] + acc[1] + acc[2] + acc[3]); +} + +// function for calculate inner product between half a q5_1 block and 16 floats (yl), sumy is SUM(yl[i]) +// il indicates where the q5 quants begin (0 or QK5_1/4) +// we assume that the yl's have been multiplied with the appropriate scale factor +// that corresponds to the missing bit shifts (1, 1/16, 1/256, 1/4096) +inline float block_q_n_dot_y(device const block_q5_1 * qb_curr, float sumy, thread float * yl, int il) { + float d = qb_curr->d; + float m = qb_curr->m; + + float acc[4] = { 0.0f, 0.0f, 0.0f, 0.0f }; + + device const uint16_t * qs = ((device const uint16_t *)qb_curr + 4 + il/2); + const uint32_t qh = *((device const uint32_t *)qb_curr->qh); + + for (int i = 0; i < 8; i+=2) { + acc[0] += yl[i + 0] * ((qs[i / 2] & 0x000F) | ((qh >> (i+0+il ) << 4 ) & 0x00010)); + acc[1] += yl[i + 1] * ((qs[i / 2] & 0x0F00) | ((qh >> (i+1+il ) << 12) & 0x01000)); + acc[2] += yl[i + 8] * ((qs[i / 2] & 0x00F0) | ((qh >> (i+0+il+QK5_0/2) << 8 ) & 0x00100)); + acc[3] += yl[i + 9] * ((qs[i / 2] & 0xF000) | ((qh >> (i+1+il+QK5_0/2) << 16) & 0x10000)); + } + + return d * (acc[0] + acc[1] + acc[2] + acc[3]) + sumy * m; +} + +template +static inline void helper_mv_reduce_and_write( + device float * dst_f32, + float sumf[NR0], + const int r0, + const int ne01, + ushort tiisg, + ushort sgitg, + threadgroup char * shmem) { + constexpr short NW = N_SIMDWIDTH; + + threadgroup float * shmem_f32[NR0]; + + for (short row = 0; row < NR0; ++row) { + shmem_f32[row] = (threadgroup float *) shmem + NW*row; + + if (sgitg == 0) { + shmem_f32[row][tiisg] = 0.0f; + } + + sumf[row] = simd_sum(sumf[row]); + } + + threadgroup_barrier(mem_flags::mem_threadgroup); + + for (short row = 0; row < NR0; ++row) { + if (tiisg == 0) { + shmem_f32[row][sgitg] = sumf[row]; + } + } + + threadgroup_barrier(mem_flags::mem_threadgroup); + + for (short row = 0; row < NR0 && r0 + row < ne01; ++row) { + float tot = simd_sum(shmem_f32[row][tiisg]); + + if (tiisg == 0 && sgitg == 0) { + dst_f32[r0 + row] = tot; + } + } +} + +constant short FC_mul_mv_nsg [[function_constant(FC_MUL_MV + 0)]]; +constant short FC_mul_mv_nxpsg [[function_constant(FC_MUL_MV + 1)]]; + +template +void mul_vec_q_n_f32_impl( + args_t args, + device const char * src0, + device const char * src1, + device char * dst, + threadgroup char * shmem, + uint3 tgpig, + ushort tiisg, + ushort sgitg) { + const short NSG = FC_mul_mv_nsg; + + constexpr short NW = N_SIMDWIDTH; + constexpr short NQ = 16; + + const int nb = args.ne00/QK4_0; + + const int r0 = (tgpig.x*NSG + sgitg)*NR0; + //const int r0 = tgpig.x*NR0; + const int r1 = tgpig.y; + const int im = tgpig.z; + + const uint i12 = im%args.ne12; + const uint i13 = im/args.ne12; + + //const uint64_t offset0 = r0*args.nb01 + (i12/args.r2)*args.nb02 + (i13/args.r3)*args.nb03; + const uint64_t offset1 = r1*args.nb11 + (i12 )*args.nb12 + (i13 )*args.nb13; + + //device const block_q_type * x = (device const block_q_type *) (src0 + offset0); + device const float * y = (device const float *) (src1 + offset1); + + // pointers to src0 rows + device const block_q_type * ax[NR0]; + FOR_UNROLL (int row = 0; row < NR0; ++row) { + const uint64_t offset0 = (r0 + row)*args.nb01 + (i12/args.r2)*args.nb02 + (i13/args.r3)*args.nb03; + + ax[row] = (device const block_q_type *) ((device char *) src0 + offset0); + } + + float sumf[NR0] = {0.f}; + + const short ix = (tiisg/(NW/NQ)); + const short il = (tiisg%(NW/NQ))*8; + + //const int ib0 = sgitg*NQ + ix; + const int ib0 = ix; + + float yl[16]; // src1 vector cache + + //device const float * yb = y + ix*QK4_0 + il; + device const float * yb = y + ib0*QK4_0 + il; + + // each thread in a SIMD group deals with half a block. + //for (int ib = ib0; ib < nb; ib += NSG*NQ) { + for (int ib = ib0; ib < nb; ib += NQ) { + float sumy[2] = { 0.f, 0.f }; + + FOR_UNROLL (short i = 0; i < 8; i += 2) { + sumy[0] += yb[i + 0] + yb[i + 1]; + yl[i + 0] = yb[i + 0]; + yl[i + 1] = yb[i + 1]/256.f; + + sumy[1] += yb[i + 16] + yb[i + 17]; + yl[i + 8] = yb[i + 16]/16.f; + yl[i + 9] = yb[i + 17]/4096.f; + } + + FOR_UNROLL (short row = 0; row < NR0; row++) { + sumf[row] += block_q_n_dot_y(ax[row] + ib, sumy[0] + sumy[1], yl, il); + } + + yb += QK4_0 * 16; + //yb += NSG*NQ*QK4_0; + } + + device float * dst_f32 = (device float *) dst + im*args.ne0*args.ne1 + r1*args.ne0; + + //helper_mv_reduce_and_write(dst_f32, sumf, r0, args.ne01, tiisg, sgitg, shmem); + + for (int row = 0; row < NR0; ++row) { + const float tot = simd_sum(sumf[row]); + + if (tiisg == 0 && r0 + row < args.ne01) { + dst_f32[r0 + row] = tot; + } + } +} + + + + + + + + + +template +void kernel_mul_mv_q8_0_f32_impl( + args_t args, + device const char * src0, + device const char * src1, + device char * dst, + threadgroup char * shmem, + uint3 tgpig, + ushort tiisg, + ushort sgitg) { + const short NSG = FC_mul_mv_nsg; + + constexpr short NW = N_SIMDWIDTH; + constexpr short NQ = 8; + + const int nb = args.ne00/QK8_0; + + const int r0 = tgpig.x*NR0; + const int r1 = tgpig.y; + const int im = tgpig.z; + + const uint i12 = im%args.ne12; + const uint i13 = im/args.ne12; + + //const uint64_t offset0 = r0*args.nb01 + (i12/args.r2)*args.nb02 + (i13/args.r3)*args.nb03; + const uint64_t offset1 = r1*args.nb11 + (i12 )*args.nb12 + (i13 )*args.nb13; + + //device const block_q8_0 * x = (device const block_q8_0 *) (src0 + offset0); + device const float * y = (device const float *) (src1 + offset1); + + // pointers to src0 rows + device const block_q8_0 * ax[NR0]; + FOR_UNROLL (short row = 0; row < NR0; ++row) { + const uint64_t offset0 = (r0 + row)*args.nb01 + (i12/args.r2)*args.nb02 + (i13/args.r3)*args.nb03; + + ax[row] = (device const block_q8_0 *) ((device char *) src0 + offset0); + } + + float sumf[NR0] = { 0.f }; + + const short ix = tiisg/(NW/NQ); + const short il = tiisg%(NW/NQ); + + const int ib0 = sgitg*NQ + ix; + + float yl[NQ]; + + device const float * yb = y + ib0*QK8_0 + il*NQ; + + // each thread in a SIMD group deals with NQ quants at a time + for (int ib = ib0; ib < nb; ib += NSG*NQ) { + for (short i = 0; i < NQ; ++i) { + yl[i] = yb[i]; + } + + for (short row = 0; row < NR0; row++) { + device const int8_t * qs = ax[row][ib].qs + il*NQ; + + float sumq = 0.f; + FOR_UNROLL (short i = 0; i < NQ; ++i) { + sumq += qs[i] * yl[i]; + } + + sumf[row] += sumq*ax[row][ib].d; + } + + yb += NSG*NQ*QK8_0; + } + + device float * dst_f32 = (device float *) dst + (uint64_t)im*args.ne0*args.ne1 + (uint64_t)r1*args.ne0; + + helper_mv_reduce_and_write(dst_f32, sumf, r0, args.ne01, tiisg, sgitg, shmem); +} + + + +// mat-vec kernel processing in chunks of float4 +// chpb - chunks per quantization block +template +void kernel_mul_mv_ext_q4_f32_impl( + constant ggml_metal_kargs_mul_mv_ext & args, + device const char * src0, + device const char * src1, + device char * dst, + uint3 tgpig[[threadgroup_position_in_grid]], + ushort tiisg[[thread_index_in_simdgroup]], + ushort sgitg[[simdgroup_index_in_threadgroup]]) { + const short NSG = FC_mul_mv_nsg; + const short nxpsg = FC_mul_mv_nxpsg; + + const short chpt = 4; // chunks per thread + + //const short nxpsg = (32); + const short nypsg = (32/nxpsg); + + const short tx = tiisg%nxpsg; + const short ty = tiisg/nxpsg; + + const int i01 = tgpig.x*(nypsg*NSG) + nypsg*sgitg + ty; + const int i11 = tgpig.y*r1ptg; + const int i1m = tgpig.z; + + const int i12 = i1m%args.ne12; + const int i13 = i1m/args.ne12; + + const uint64_t offset0 = i01*args.nb01 + (i12/args.r2)*args.nb02 + (i13/args.r3)*args.nb03; + const uint64_t offset1 = i11*args.nb11 + (i12 )*args.nb12 + (i13 )*args.nb13; + + device const q_t * xq = (i01 < args.ne01) ? (device const q_t *) (src0 + offset0) + tx/chpb : (device const q_t *) src0; + + device const float4 * y4[r1ptg]; + + for (int ir1 = 0; ir1 < r1ptg; ++ir1) { + y4[ir1] = (i11 + ir1 < args.ne11) ? (device const float4 *) (src1 + offset1 + ir1*args.nb11) + tx : (device const float4 *) src1; + } + + float sumf[r1ptg] = { [ 0 ... r1ptg - 1 ] = 0.0f }; + + short cch = tx%chpb; // current chunk index + + for (int ich = tx; 4*ich < args.ne00; ich += chpt*nxpsg) { + float4 lx[chpt]; + +#pragma unroll(chpt) + for (short ch = 0; ch < chpt; ++ch) { + deq_t4(xq, cch, lx[ch]); + + cch += nxpsg; + if (cch >= chpb) { + xq += cch/chpb; + cch %= chpb; + } + } + +#pragma unroll(chpt) + for (short ch = 0; ch < chpt; ++ch) { +#pragma unroll(r1ptg) + for (short ir1 = 0; ir1 < r1ptg; ++ir1) { + sumf[ir1] += dot(lx[ch], y4[ir1][ch*nxpsg]); + } + } + +#pragma unroll(r1ptg) + for (short ir1 = 0; ir1 < r1ptg; ++ir1) { + y4[ir1] += chpt*nxpsg; + } + } + + // reduce only the threads in each row + for (short ir1 = 0; ir1 < r1ptg; ++ir1) { + if (nxpsg >= 32) { + sumf[ir1] += simd_shuffle_down(sumf[ir1], 16); + } + if (nxpsg >= 16) { + sumf[ir1] += simd_shuffle_down(sumf[ir1], 8); + } + if (nxpsg >= 8) { + sumf[ir1] += simd_shuffle_down(sumf[ir1], 4); + } + if (nxpsg >= 4) { + sumf[ir1] += simd_shuffle_down(sumf[ir1], 2); + } + if (nxpsg >= 2) { + sumf[ir1] += simd_shuffle_down(sumf[ir1], 1); + } + + //sumf[ir1] = simd_sum(sumf[ir1]); + } + + if (tx == 0) { + for (short ir1 = 0; ir1 < r1ptg && i11 + ir1 < args.ne11; ++ir1) { + device float * dst_f32 = (device float *) dst + (uint64_t)i1m*args.ne0*args.ne1 + (uint64_t)(i11 + ir1)*args.ne0; + + if (i01 < args.ne01) { + dst_f32[i01] = sumf[ir1]; + } + } + } +} + +// mat-vec kernel processing in chunks of float4x4 +template +void kernel_mul_mv_ext_q4x4_f32_impl( + constant ggml_metal_kargs_mul_mv_ext & args, + device const char * src0, + device const char * src1, + device char * dst, + uint3 tgpig[[threadgroup_position_in_grid]], + ushort tiisg[[thread_index_in_simdgroup]], + ushort sgitg[[simdgroup_index_in_threadgroup]]) { + const short NSG = FC_mul_mv_nsg; + const short nxpsg = FC_mul_mv_nxpsg; + + const short chpt = 1; + + //const short nxpsg = (32); + const short nypsg = (32/nxpsg); + + const short tx = tiisg%nxpsg; + const short ty = tiisg/nxpsg; + + const int i01 = tgpig.x*(nypsg*NSG) + nypsg*sgitg + ty; + const int i11 = tgpig.y*r1ptg; + const int i1m = tgpig.z; + + const int i12 = i1m%args.ne12; + const int i13 = i1m/args.ne12; + + const uint64_t offset0 = i01*args.nb01 + (i12/args.r2)*args.nb02 + (i13/args.r3)*args.nb03; + const uint64_t offset1 = i11*args.nb11 + (i12 )*args.nb12 + (i13 )*args.nb13; + + device const q_t * xq = (i01 < args.ne01) ? (device const q_t *) (src0 + offset0) + tx/chpb : (device const q_t *) src0; + + device const float4x4 * y4x4[r1ptg]; + + for (int ir1 = 0; ir1 < r1ptg; ++ir1) { + y4x4[ir1] = (i11 + ir1 < args.ne11) ? (device const float4x4 *) (src1 + offset1 + ir1*args.nb11) + tx : (device const float4x4 *) src1; + } + + float sumf[r1ptg] = { [ 0 ... r1ptg - 1 ] = 0.0f }; + + short cch = tx%chpb; + + for (int ich = tx; 16*ich < args.ne00; ich += chpt*nxpsg) { + float4x4 lx[chpt]; + +#pragma unroll(chpt) + for (short ch = 0; ch < chpt; ++ch) { + deq_t4x4(xq, cch, lx[ch]); + + cch += nxpsg; + if (cch >= chpb) { + xq += cch/chpb; + cch %= chpb; + } + } + +#pragma unroll(chpt) + for (short ch = 0; ch < chpt; ++ch) { +#pragma unroll(r1ptg) + for (short ir1 = 0; ir1 < r1ptg; ++ir1) { + sumf[ir1] += + dot(lx[ch][0], y4x4[ir1][ch*nxpsg][0]) + + dot(lx[ch][1], y4x4[ir1][ch*nxpsg][1]) + + dot(lx[ch][2], y4x4[ir1][ch*nxpsg][2]) + + dot(lx[ch][3], y4x4[ir1][ch*nxpsg][3]); + + } + } + +#pragma unroll(r1ptg) + for (short ir1 = 0; ir1 < r1ptg; ++ir1) { + y4x4[ir1] += chpt*nxpsg; + } + } + + for (short ir1 = 0; ir1 < r1ptg; ++ir1) { + if (nxpsg >= 32) { + sumf[ir1] += simd_shuffle_down(sumf[ir1], 16); + } + if (nxpsg >= 16) { + sumf[ir1] += simd_shuffle_down(sumf[ir1], 8); + } + if (nxpsg >= 8) { + sumf[ir1] += simd_shuffle_down(sumf[ir1], 4); + } + if (nxpsg >= 4) { + sumf[ir1] += simd_shuffle_down(sumf[ir1], 2); + } + if (nxpsg >= 2) { + sumf[ir1] += simd_shuffle_down(sumf[ir1], 1); + } + + //sumf[ir1] = simd_sum(sumf[ir1]); + } + + if (tx == 0) { + for (short ir1 = 0; ir1 < r1ptg && i11 + ir1 < args.ne11; ++ir1) { + device float * dst_f32 = (device float *) dst + (uint64_t)i1m*args.ne0*args.ne1 + (uint64_t)(i11 + ir1)*args.ne0; + + if (i01 < args.ne01) { + dst_f32[i01] = sumf[ir1]; + } + } + } +} + +// dispatchers needed for compile-time nxpsg +// epb - elements per quantization block +template +kernel void kernel_mul_mv_ext_q4_f32_disp( + constant ggml_metal_kargs_mul_mv_ext & args, + device const char * src0, + device const char * src1, + device char * dst, + uint3 tgpig[[threadgroup_position_in_grid]], + ushort tiisg[[thread_index_in_simdgroup]], + ushort sgitg[[simdgroup_index_in_threadgroup]]) { + kernel_mul_mv_ext_q4_f32_impl(args, src0, src1, dst, tgpig, tiisg, sgitg); +} + +template +kernel void kernel_mul_mv_ext_q4x4_f32_disp( + constant ggml_metal_kargs_mul_mv_ext & args, + device const char * src0, + device const char * src1, + device char * dst, + uint3 tgpig[[threadgroup_position_in_grid]], + ushort tiisg[[thread_index_in_simdgroup]], + ushort sgitg[[simdgroup_index_in_threadgroup]]) { + kernel_mul_mv_ext_q4x4_f32_impl(args, src0, src1, dst, tgpig, tiisg, sgitg); +} + +typedef decltype(kernel_mul_mv_ext_q4_f32_disp <2, block_q8_0, 32, dequantize_q8_0_t4>) mul_mv_ext_q4_f32_t; +typedef decltype(kernel_mul_mv_ext_q4x4_f32_disp<2, block_q4_K, 256, dequantize_q4_K>) mul_mv_ext_q4x4_f32_t; + +template [[host_name("kernel_mul_mv_ext_f32_f32_r1_2")]] kernel mul_mv_ext_q4_f32_t kernel_mul_mv_ext_q4_f32_disp<2, float4, 4, dequantize_f32_t4>; +template [[host_name("kernel_mul_mv_ext_f32_f32_r1_3")]] kernel mul_mv_ext_q4_f32_t kernel_mul_mv_ext_q4_f32_disp<3, float4, 4, dequantize_f32_t4>; +template [[host_name("kernel_mul_mv_ext_f32_f32_r1_4")]] kernel mul_mv_ext_q4_f32_t kernel_mul_mv_ext_q4_f32_disp<4, float4, 4, dequantize_f32_t4>; +template [[host_name("kernel_mul_mv_ext_f32_f32_r1_5")]] kernel mul_mv_ext_q4_f32_t kernel_mul_mv_ext_q4_f32_disp<5, float4, 4, dequantize_f32_t4>; + +template [[host_name("kernel_mul_mv_ext_f16_f32_r1_2")]] kernel mul_mv_ext_q4_f32_t kernel_mul_mv_ext_q4_f32_disp<2, half4, 4, dequantize_f16_t4>; +template [[host_name("kernel_mul_mv_ext_f16_f32_r1_3")]] kernel mul_mv_ext_q4_f32_t kernel_mul_mv_ext_q4_f32_disp<3, half4, 4, dequantize_f16_t4>; +template [[host_name("kernel_mul_mv_ext_f16_f32_r1_4")]] kernel mul_mv_ext_q4_f32_t kernel_mul_mv_ext_q4_f32_disp<4, half4, 4, dequantize_f16_t4>; +template [[host_name("kernel_mul_mv_ext_f16_f32_r1_5")]] kernel mul_mv_ext_q4_f32_t kernel_mul_mv_ext_q4_f32_disp<5, half4, 4, dequantize_f16_t4>; + + + + + + + + + + + +template +void kernel_mul_mv_t_t_impl( + args_t args, + device const char * src0, + device const char * src1, + device char * dst, + threadgroup char * shmem, + uint3 tgpig, + ushort tiisg, + ushort sgitg) { + const short NSG = FC_mul_mv_nsg; + + constexpr short NW = N_SIMDWIDTH; + constexpr short NB = 32; + constexpr short NF = 8; + + const int nb = args.ne00/NB; + + const int r0 = tgpig.x*NR0; + const int r1 = tgpig.y; + const int im = tgpig.z; + + const uint i12 = im%args.ne12; + const uint i13 = im/args.ne12; + + //const uint64_t offset0 = r0*args.nb01 + (i12/args.r2)*args.nb02 + (i13/args.r3)*args.nb03; + const uint64_t offset1 = r1*args.nb11 + (i12 )*args.nb12 + (i13 )*args.nb13; + + //device const T0 * x = (device const T0 *) (src0 + offset0); + device const T1 * y = (device const T1 *) (src1 + offset1); + + // pointers to src0 rows + device const T0 * ax [NR0]; + FOR_UNROLL (short row = 0; row < NR0; ++row) { + const uint64_t offset0 = (r0 + row)*args.nb01 + (i12/args.r2)*args.nb02 + (i13/args.r3)*args.nb03; + + ax[row] = (device const T0 *) ((device char *) src0 + offset0); + } + + float sumf[NR0] = { 0.f }; + + const short ix = tiisg/(NW/NF); + const short il = tiisg%(NW/NF); + + const int ib0 = sgitg*NF + ix; + + T1 yl[NF]; + + device const T1 * yb = y + (ib0*NB + il*NF); + + for (int ib = ib0; ib < nb; ib += NSG*NF) { + for (short i = 0; i < NF; ++i) { + yl[i] = yb[i]; + } + + for (short row = 0; row < NR0; row++) { + device const T0 * xb = ax[row] + (ib*NB + il*NF); + + float sumq = 0.f; + FOR_UNROLL (short i = 0; i < NF; ++i) { + sumq += xb[i] * yl[i]; + } + + sumf[row] += sumq; + } + + yb += NSG*NF*NW; + } + + for (int i = nb*NB + sgitg*NW + tiisg; i < args.ne00; i += NW*NSG) { + for (short row = 0; row < NR0; row++) { + sumf[row] += ax[row][i] * y[i]; + } + } + + device float * dst_f32 = (device float *) dst + (uint64_t)im*args.ne0*args.ne1 + (uint64_t)r1*args.ne0; + + helper_mv_reduce_and_write(dst_f32, sumf, r0, args.ne01, tiisg, sgitg, shmem); +} + +template +void kernel_mul_mv_t_t_disp( + args_t args, + device const char * src0, + device const char * src1, + device char * dst, + threadgroup char * shmem, + uint3 tgpig, + ushort tiisg, + ushort sgitg) { + switch (args.nr0) { + //case 1: kernel_mul_mv_t_t_impl(args, src0, src1, dst, shmem, tgpig, tiisg, sgitg); break; + case 2: kernel_mul_mv_t_t_impl(args, src0, src1, dst, shmem, tgpig, tiisg, sgitg); break; + //case 3: kernel_mul_mv_t_t_impl(args, src0, src1, dst, shmem, tgpig, tiisg, sgitg); break; + //case 4: kernel_mul_mv_t_t_impl(args, src0, src1, dst, shmem, tgpig, tiisg, sgitg); break; + } +} + +template +kernel void kernel_mul_mv_t_t( + constant ggml_metal_kargs_mul_mv & args, + device const char * src0, + device const char * src1, + device char * dst, + threadgroup char * shmem [[threadgroup(0)]], + uint3 tgpig[[threadgroup_position_in_grid]], + ushort tiisg[[thread_index_in_simdgroup]], + ushort sgitg[[simdgroup_index_in_threadgroup]]) { + kernel_mul_mv_t_t_disp(args, src0, src1, dst, shmem, tgpig, tiisg, sgitg); +} + +typedef decltype(kernel_mul_mv_t_t) mul_mv_t_t; + +#if defined(GGML_METAL_HAS_BF16) +#endif + +template +void kernel_mul_mv_t_t_4_impl( + args_t args, + device const char * src0, + device const char * src1, + device char * dst, + threadgroup char * shmem, + uint3 tgpig, + ushort tiisg, + ushort sgitg) { + const short NSG = FC_mul_mv_nsg; + + constexpr short NW = N_SIMDWIDTH; + constexpr short NB = 32; + constexpr short NF = 16; + constexpr short NF4 = NF/4; + + const int nb = args.ne00/NB; + + const int r0 = tgpig.x*NR0; + const int r1 = tgpig.y; + const int im = tgpig.z; + + const uint i12 = im%args.ne12; + const uint i13 = im/args.ne12; + + //const uint64_t offset0 = r0*args.nb01 + (i12/args.r2)*args.nb02 + (i13/args.r3)*args.nb03; + const uint64_t offset1 = r1*args.nb11 + (i12 )*args.nb12 + (i13 )*args.nb13; + + device const T1 * y = (device const T1 *) (src1 + offset1); + device const T14 * y4 = (device const T14 *) (src1 + offset1); + + // pointers to src0 rows + device const T0 * ax [NR0]; + device const T04 * ax4[NR0]; + FOR_UNROLL (short row = 0; row < NR0; ++row) { + const uint64_t offset0 = (r0 + row)*args.nb01 + (i12/args.r2)*args.nb02 + (i13/args.r3)*args.nb03; + + ax [row] = (device const T0 *) ((device char *) src0 + offset0); + ax4[row] = (device const T04 *) ((device char *) src0 + offset0); + } + + float sumf[NR0] = { 0.f }; + + const short ix = tiisg/(NW/NF); + const short il = tiisg%(NW/NF); + + const int ib0 = sgitg*NF + ix; + + T14 yl4[NF4]; + + device const T14 * yb4 = y4 + (ib0*NB + il*NF)/4; + + for (int ib = ib0; ib < nb; ib += NSG*NF) { + for (short i = 0; i < NF4; ++i) { + yl4[i] = yb4[i]; + } + + for (short row = 0; row < NR0; row++) { + device const T04 * xb4 = ax4[row] + (ib*NB + il*NF)/4; + + float sumq = 0.f; + FOR_UNROLL (short i = 0; i < NF4; ++i) { + sumq += dot(float4(xb4[i]), float4(yl4[i])); + } + + sumf[row] += sumq; + } + + yb4 += NSG*NF*NW/4; + } + + for (int i = nb*NB + sgitg*NW + tiisg; i < args.ne00; i += NW*NSG) { + for (short row = 0; row < NR0; row++) { + sumf[row] += ax[row][i] * y[i]; + } + } + + device float * dst_f32 = (device float *) dst + (uint64_t)im*args.ne0*args.ne1 + (uint64_t)r1*args.ne0; + + helper_mv_reduce_and_write(dst_f32, sumf, r0, args.ne01, tiisg, sgitg, shmem); +} + +template +void kernel_mul_mv_t_t_4_disp( + args_t args, + device const char * src0, + device const char * src1, + device char * dst, + threadgroup char * shmem, + uint3 tgpig, + ushort tiisg, + ushort sgitg) { + switch (args.nr0) { + //case 1: kernel_mul_mv_t_t_4_impl(args, src0, src1, dst, shmem, tgpig, tiisg, sgitg); break; + case 2: kernel_mul_mv_t_t_4_impl(args, src0, src1, dst, shmem, tgpig, tiisg, sgitg); break; + //case 3: kernel_mul_mv_t_t_4_impl(args, src0, src1, dst, shmem, tgpig, tiisg, sgitg); break; + //case 4: kernel_mul_mv_t_t_4_impl(args, src0, src1, dst, shmem, tgpig, tiisg, sgitg); break; + }; +} + +template +kernel void kernel_mul_mv_t_t_4( + constant ggml_metal_kargs_mul_mv & args, + device const char * src0, + device const char * src1, + device char * dst, + threadgroup char * shmem [[threadgroup(0)]], + uint3 tgpig[[threadgroup_position_in_grid]], + ushort tiisg[[thread_index_in_simdgroup]], + ushort sgitg[[simdgroup_index_in_threadgroup]]) { + kernel_mul_mv_t_t_4_disp(args, src0, src1, dst, shmem, tgpig, tiisg, sgitg); +} + +typedef decltype(kernel_mul_mv_t_t_4) mul_mv_t_t_4; + +template [[host_name("kernel_mul_mv_f32_f32_4")]] kernel mul_mv_t_t_4 kernel_mul_mv_t_t_4; +template [[host_name("kernel_mul_mv_f16_f32_4")]] kernel mul_mv_t_t_4 kernel_mul_mv_t_t_4; +#if defined(GGML_METAL_HAS_BF16) +#endif + +template +void kernel_mul_mv_t_t_short_impl( + args_t args, + device const char * src0, + device const char * src1, + device char * dst, + uint3 tgpig, + ushort tiisg) { + const int r0 = tgpig.x*32 + tiisg; + const int r1 = tgpig.y; + const int im = tgpig.z; + + if (r0 >= args.ne01) { + return; + } + + const uint i12 = im%args.ne12; + const uint i13 = im/args.ne12; + + const uint64_t offset0 = r0*args.nb01 + (i12/args.r2)*args.nb02 + (i13/args.r3)*args.nb03; + + device const T0 * x = (device const T0 *) (src0 + offset0); + + device float * dst_f32 = (device float *) dst + (uint64_t)im*args.ne0*args.ne1; + + const uint64_t offset1 = r1*args.nb11 + (i12 )*args.nb12 + (i13 )*args.nb13; + + device const T1 * y = (device const T1 *) (src1 + offset1); + + float res = 0.0f; + + for (int i = 0; i < args.ne00; ++i) { + res += (float) x[i] * (float) y[i]; + } + + dst_f32[(uint64_t)r1*args.ne0 + r0] = res; +} + +template +kernel void kernel_mul_mv_t_t_short( + constant ggml_metal_kargs_mul_mv & args, + device const char * src0, + device const char * src1, + device char * dst, + uint3 tgpig[[threadgroup_position_in_grid]], + ushort tiisg[[thread_index_in_simdgroup]]) { + kernel_mul_mv_t_t_short_impl( + args, + src0, + src1, + dst, + tgpig, + tiisg); +} + +typedef decltype(kernel_mul_mv_t_t_short) mul_mv_t_t_short_t; + +#if defined(GGML_METAL_HAS_BF16) +#endif + +constant bool FC_rope_is_imrope [[function_constant(FC_ROPE + 0)]]; + +static float rope_yarn_ramp(const float low, const float high, const int i0) { + const float y = (i0 / 2 - low) / max(0.001f, high - low); + return 1.0f - min(1.0f, max(0.0f, y)); +} + +// YaRN algorithm based on LlamaYaRNScaledRotaryEmbedding.py from https://github.com/jquesnelle/yarn +// MIT licensed. Copyright (c) 2023 Jeffrey Quesnelle and Bowen Peng. +static void rope_yarn( + float theta_extrap, float freq_scale, float corr_dims[2], int i0, float ext_factor, float mscale, + thread float * cos_theta, thread float * sin_theta) { + // Get n-d rotational scaling corrected for extrapolation + float theta_interp = freq_scale * theta_extrap; + float theta = theta_interp; + if (ext_factor != 0.0f) { + float ramp_mix = rope_yarn_ramp(corr_dims[0], corr_dims[1], i0) * ext_factor; + theta = theta_interp * (1 - ramp_mix) + theta_extrap * ramp_mix; + + // Get n-d magnitude scaling corrected for interpolation + mscale *= 1.0f + 0.1f * log(1.0f / freq_scale); + } + *cos_theta = cos(theta) * mscale; + *sin_theta = sin(theta) * mscale; +} + +// Apparently solving `n_rot = 2pi * x * base^((2 * max_pos_emb) / n_dims)` for x, we get +// `corr_fac(n_rot) = n_dims * log(max_pos_emb / (n_rot * 2pi)) / (2 * log(base))` +static float rope_yarn_corr_factor(int n_dims, int n_ctx_orig, float n_rot, float base) { + return n_dims * log(n_ctx_orig / (n_rot * 2 * M_PI_F)) / (2 * log(base)); +} + +static void rope_yarn_corr_dims( + int n_dims, int n_ctx_orig, float freq_base, float beta_fast, float beta_slow, float dims[2] +) { + // start and end correction dims + dims[0] = max(0.0f, floor(rope_yarn_corr_factor(n_dims, n_ctx_orig, beta_fast, freq_base))); + dims[1] = min(n_dims - 1.0f, ceil(rope_yarn_corr_factor(n_dims, n_ctx_orig, beta_slow, freq_base))); +} + +template +kernel void kernel_rope_norm( + constant ggml_metal_kargs_rope & args, + device const char * src0, + device const char * src1, + device const char * src2, + device char * dst, + ushort tiitg[[thread_index_in_threadgroup]], + ushort3 tptg [[threads_per_threadgroup]], + uint3 tgpig[[threadgroup_position_in_grid]]) { + const int i3 = tgpig[2]; + const int i2 = tgpig[1]; + const int i1 = tgpig[0]; + + float corr_dims[2]; + rope_yarn_corr_dims(args.n_dims, args.n_ctx_orig, args.freq_base, args.beta_fast, args.beta_slow, corr_dims); + + device const int32_t * pos = (device const int32_t *) src1; + + const float theta_base = (float) pos[i2]; + const float inv_ndims = -1.f/args.n_dims; + + float cos_theta; + float sin_theta; + + for (int i0 = 2*tiitg; i0 < args.ne0; i0 += 2*tptg.x) { + if (i0 < args.n_dims) { + const int ic = i0/2; + + const float theta = theta_base * pow(args.freq_base, inv_ndims*i0); + + const float freq_factor = args.src2 ? ((device const float *) src2)[ic] : 1.0f; + + rope_yarn(theta/freq_factor, args.freq_scale, corr_dims, i0, args.ext_factor, args.attn_factor, &cos_theta, &sin_theta); + + device const T * const src = (device T *)(src0 + i3*args.nb03 + i2*args.nb02 + i1*args.nb01 + i0*args.nb00); + device T * dst_data = (device T *)( dst + i3*args.nb3 + i2*args.nb2 + i1*args.nb1 + i0*args.nb0); + + const float x0 = src[0]; + const float x1 = src[1]; + + dst_data[0] = x0*cos_theta - x1*sin_theta; + dst_data[1] = x0*sin_theta + x1*cos_theta; + } else { + device const T * const src = (device T *)(src0 + i3*args.nb03 + i2*args.nb02 + i1*args.nb01 + i0*args.nb00); + device T * dst_data = (device T *)( dst + i3*args.nb3 + i2*args.nb2 + i1*args.nb1 + i0*args.nb0); + + dst_data[0] = src[0]; + dst_data[1] = src[1]; + } + } +} + +template +kernel void kernel_rope_neox( + constant ggml_metal_kargs_rope & args, + device const char * src0, + device const char * src1, + device const char * src2, + device char * dst, + ushort tiitg[[thread_index_in_threadgroup]], + ushort3 tptg [[threads_per_threadgroup]], + uint3 tgpig[[threadgroup_position_in_grid]]) { + const int i3 = tgpig[2]; + const int i2 = tgpig[1]; + const int i1 = tgpig[0]; + + float corr_dims[2]; + rope_yarn_corr_dims(args.n_dims, args.n_ctx_orig, args.freq_base, args.beta_fast, args.beta_slow, corr_dims); + + device const int32_t * pos = (device const int32_t *) src1; + + const float theta_base = (float) pos[i2]; + const float inv_ndims = -1.f/args.n_dims; + + float cos_theta; + float sin_theta; + + for (int i0 = 2*tiitg; i0 < args.ne0; i0 += 2*tptg.x) { + if (i0 < args.n_dims) { + const int ic = i0/2; + + const float theta = theta_base * pow(args.freq_base, inv_ndims*i0); + + const float freq_factor = args.src2 ? ((device const float *) src2)[ic] : 1.0f; + + rope_yarn(theta/freq_factor, args.freq_scale, corr_dims, i0, args.ext_factor, args.attn_factor, &cos_theta, &sin_theta); + + device const T * const src = (device T *)(src0 + i3*args.nb03 + i2*args.nb02 + i1*args.nb01 + ic*args.nb00); + device T * dst_data = (device T *)( dst + i3*args.nb3 + i2*args.nb2 + i1*args.nb1 + ic*args.nb0); + + const float x0 = src[0]; + const float x1 = src[args.n_dims/2]; + + dst_data[0] = x0*cos_theta - x1*sin_theta; + dst_data[args.n_dims/2] = x0*sin_theta + x1*cos_theta; + } else { + device const T * const src = (device T *)(src0 + i3*args.nb03 + i2*args.nb02 + i1*args.nb01 + i0*args.nb00); + device T * dst_data = (device T *)( dst + i3*args.nb3 + i2*args.nb2 + i1*args.nb1 + i0*args.nb0); + + dst_data[0] = src[0]; + dst_data[1] = src[1]; + } + } +} + +template +kernel void kernel_rope_multi( + constant ggml_metal_kargs_rope & args, + device const char * src0, + device const char * src1, + device const char * src2, + device char * dst, + ushort tiitg[[thread_index_in_threadgroup]], + ushort3 tptg [[threads_per_threadgroup]], + uint3 tgpig[[threadgroup_position_in_grid]]) { + const int i3 = tgpig[2]; + const int i2 = tgpig[1]; + const int i1 = tgpig[0]; + + float corr_dims[2]; + rope_yarn_corr_dims(args.n_dims, args.n_ctx_orig, args.freq_base, args.beta_fast, args.beta_slow, corr_dims); + + device const int32_t * pos = (device const int32_t *) src1; + + const float inv_ndims = -1.f/args.n_dims; + + float cos_theta; + float sin_theta; + + for (int i0 = 2*tiitg; i0 < args.ne0; i0 += 2*tptg.x) { + if (i0 < args.n_dims) { + const int ic = i0/2; + + // mrope theta calculations + // note: the rest is the same as kernel_rope_neox + const int sect_dims = args.sect_0 + args.sect_1 + args.sect_2 + args.sect_3; + const int sec_w01 = args.sect_0 + args.sect_1; // end of section 1 + const int sec_w012 = args.sect_0 + args.sect_1 + args.sect_2; // end of section 2 + const int sector = ic % sect_dims; + + float theta_base; + if (FC_rope_is_imrope) { + if (sector % 3 == 1 && sector < 3 * args.sect_1) { // h + theta_base = (float) pos[i2 + args.ne02 * 1]; + } else if (sector % 3 == 2 && sector < 3 * args.sect_2) { // w + theta_base = (float) pos[i2 + args.ne02 * 2]; + } else if (sector % 3 == 0 && sector < 3 * args.sect_0) { // t + theta_base = (float) pos[i2 + args.ne02 * 0]; + } else { // e + theta_base = (float) pos[i2 + args.ne02 * 3]; + } + } else { + if (sector < args.sect_0) { + theta_base = (float) pos[i2]; + } else if (sector < sec_w01) { + theta_base = (float) pos[i2 + args.ne02 * 1]; + } else if (sector < sec_w012) { + theta_base = (float) pos[i2 + args.ne02 * 2]; + } else { + theta_base = (float) pos[i2 + args.ne02 * 3]; + } + } + // end of mrope + + const float theta = theta_base * pow(args.freq_base, inv_ndims*i0); + + const float freq_factor = args.src2 ? ((device const float *) src2)[ic] : 1.0f; + + rope_yarn(theta/freq_factor, args.freq_scale, corr_dims, i0, args.ext_factor, args.attn_factor, &cos_theta, &sin_theta); + + device const T * const src = (device T *)(src0 + i3*args.nb03 + i2*args.nb02 + i1*args.nb01 + ic*args.nb00); + device T * dst_data = (device T *)( dst + i3*args.nb3 + i2*args.nb2 + i1*args.nb1 + ic*args.nb0); + + const float x0 = src[0]; + const float x1 = src[args.n_dims/2]; + + dst_data[0] = x0*cos_theta - x1*sin_theta; + dst_data[args.n_dims/2] = x0*sin_theta + x1*cos_theta; + } else { + device const T * const src = (device T *)(src0 + i3*args.nb03 + i2*args.nb02 + i1*args.nb01 + i0*args.nb00); + device T * dst_data = (device T *)( dst + i3*args.nb3 + i2*args.nb2 + i1*args.nb1 + i0*args.nb0); + + dst_data[0] = src[0]; + dst_data[1] = src[1]; + } + } +} + +template +kernel void kernel_rope_vision( + constant ggml_metal_kargs_rope & args, + device const char * src0, + device const char * src1, + device const char * src2, + device char * dst, + ushort tiitg[[thread_index_in_threadgroup]], + ushort3 tptg [[threads_per_threadgroup]], + uint3 tgpig[[threadgroup_position_in_grid]]) { + const int i3 = tgpig[2]; + const int i2 = tgpig[1]; + const int i1 = tgpig[0]; + + float corr_dims[2]; + rope_yarn_corr_dims(args.n_dims, args.n_ctx_orig, args.freq_base, args.beta_fast, args.beta_slow, corr_dims); + + device const int32_t * pos = (device const int32_t *) src1; + + const float inv_ndims = -1.f/args.n_dims; + + float cos_theta; + float sin_theta; + + for (int i0 = 2*tiitg; i0 < args.ne0; i0 += 2*tptg.x) { + if (i0 < 2*args.n_dims) { // different from kernel_rope_multi + const int ic = i0/2; + + // mrope theta calculations (only support 2 dimensions) + const int sect_dims = args.sect_0 + args.sect_1; + const int sector = ic % sect_dims; + + float p; + float theta_base; + if (sector < args.sect_1) { + p = (float) sector; + theta_base = (float) pos[i2]; + } else { + p = (float) sector - args.sect_0; + theta_base = (float) pos[i2 + args.ne02]; + } + + const float theta = theta_base * pow(args.freq_base, 2.0f * inv_ndims * p); + // end of mrope + + const float freq_factor = args.src2 ? ((device const float *) src2)[ic] : 1.0f; + + rope_yarn(theta/freq_factor, args.freq_scale, corr_dims, i0, args.ext_factor, args.attn_factor, &cos_theta, &sin_theta); + + device const T * const src = (device T *)(src0 + i3*args.nb03 + i2*args.nb02 + i1*args.nb01 + ic*args.nb00); + device T * dst_data = (device T *)( dst + i3*args.nb3 + i2*args.nb2 + i1*args.nb1 + ic*args.nb0); + + const float x0 = src[0]; + const float x1 = src[args.n_dims]; // different from kernel_rope_multi + + dst_data[0] = x0*cos_theta - x1*sin_theta; + dst_data[args.n_dims] = x0*sin_theta + x1*cos_theta; // different from kernel_rope_multi + } else { + device const T * const src = (device T *)(src0 + i3*args.nb03 + i2*args.nb02 + i1*args.nb01 + i0*args.nb00); + device T * dst_data = (device T *)( dst + i3*args.nb3 + i2*args.nb2 + i1*args.nb1 + i0*args.nb0); + + dst_data[0] = src[0]; + dst_data[1] = src[1]; + } + } +} + +typedef decltype(kernel_rope_norm) kernel_rope_norm_t; +typedef decltype(kernel_rope_neox) kernel_rope_neox_t; +typedef decltype(kernel_rope_multi) kernel_rope_multi_t; +typedef decltype(kernel_rope_vision) kernel_rope_vision_t; + + + + + +typedef void (im2col_t)( + constant ggml_metal_kargs_im2col & args, + device const float * x, + device char * dst, + uint3 tgpig[[threadgroup_position_in_grid]], + uint3 tgpg[[threadgroups_per_grid]], + uint3 tpitg[[thread_position_in_threadgroup]], + uint3 ntg[[threads_per_threadgroup]]); + +template +kernel void kernel_im2col( + constant ggml_metal_kargs_im2col & args, + device const float * x, + device char * dst, + uint3 tgpig[[threadgroup_position_in_grid]], + uint3 tgpg[[threadgroups_per_grid]], + uint3 tpitg[[thread_position_in_threadgroup]], + uint3 ntg[[threads_per_threadgroup]]) { +// const int64_t IC = tgpg[0]; + const int64_t OH = tgpg[1]; + const int64_t OW = tgpg[2]; + + const int64_t KH = ntg[1]; + const int64_t KW = ntg[2]; + + int64_t in = tpitg[0]; + const int64_t ikh = tpitg[1]; + const int64_t ikw = tpitg[2]; + + const int64_t iic = tgpig[0]; + const int64_t ioh = tgpig[1]; + const int64_t iow = tgpig[2]; + + const int64_t iiw = iow*args.s0 + ikw*args.d0 - args.p0; + const int64_t iih = ioh*args.s1 + ikh*args.d1 - args.p1; + + int64_t offset_dst = (in*OH*OW + ioh*OW + iow)*args.CHW + (iic*(KH*KW) + ikh*KW + ikw); + + device T * pdst = (device T *) (dst); + + if (iih < 0 || iih >= args.IH || iiw < 0 || iiw >= args.IW) { + while (in < args.N) { + pdst[offset_dst] = 0.0f; + offset_dst += ntg[0]*args.CHW*OH*OW; + + in += ntg[0]; + } + } else { + int64_t offset_src = in*args.ofs0 + iic*args.ofs1 + iih*args.IW + iiw; + + while (in < args.N) { + pdst[offset_dst] = x[offset_src]; + + offset_dst += ntg[0]*args.CHW*OH*OW; + offset_src += ntg[0]*args.ofs0; + + in += ntg[0]; + } + } +} + +template [[host_name("kernel_im2col_f32")]] kernel im2col_t kernel_im2col; + +// TODO: obolete -- remove +//typedef void (im2col_ext_t)( +// constant ggml_metal_kargs_im2col & args, +// device const float * x, +// device char * dst, +// uint3 tgpig[[threadgroup_position_in_grid]], +// uint3 tgpg[[threadgroups_per_grid]], +// uint3 tpitg[[thread_position_in_threadgroup]], +// uint3 ntg[[threads_per_threadgroup]]); +// +//template +//kernel void kernel_im2col_ext( +// constant ggml_metal_kargs_im2col & args, +// device const float * x, +// device char * dst, +// uint3 tgpig[[threadgroup_position_in_grid]], +// uint3 tgpg[[threadgroups_per_grid]], // tgpg[0] = D x IC x KH x KW, CHW = IC x KH x KW +// uint3 tpitg[[thread_position_in_threadgroup]], +// uint3 ntg[[threads_per_threadgroup]]) { // [M, 1, 1] +// const int64_t KHW = (int64_t)args.KHW; +// +// const int64_t d = tgpig[0] / args.CHW; +// const int64_t chw = tgpig[0] % args.CHW; +// const int64_t tgpig_0 = chw / KHW; // 0 ~ (IC - 1) +// const int64_t HW = tgpig[0] % KHW; +// +// const int64_t tpitg_0 = (d * ntg[0]) + tpitg[0]; +// if (tpitg_0 >= args.N) { +// return; +// } +// +// const int64_t tpitg_1 = HW / args.KW; +// const int64_t tpitg_2 = HW % args.KW; +// +// const int64_t iiw = tgpig[2] * args.s0 + tpitg_2 * args.d0 - args.p0; +// const int64_t iih = tgpig[1] * args.s1 + tpitg_1 * args.d1 - args.p1; +// +// const int64_t offset_dst = +// (tpitg_0 * tgpg[1] * tgpg[2] + tgpig[1] * tgpg[2] + tgpig[2]) * args.CHW + +// (tgpig_0 * KHW + tpitg_1 * args.KW + tpitg_2); +// +// device T * pdst = (device T *) (dst); +// +// if (iih < 0 || iih >= args.IH || iiw < 0 || iiw >= args.IW) { +// pdst[offset_dst] = 0.0f; +// } else { +// const int64_t offset_src = tpitg_0 * args.ofs0 + tgpig_0 * args.ofs1; +// pdst[offset_dst] = x[offset_src + iih * args.IW + iiw]; +// } +//} +// + +template +kernel void kernel_conv_2d( + constant ggml_metal_kargs_conv_2d & args, + device const char * weights, + device const char * src, + device char * dst, + uint3 tgpig[[threadgroup_position_in_grid]], + uint3 tgpg[[threadgroups_per_grid]], + uint3 tpitg[[thread_position_in_threadgroup]], + uint3 ntg[[threads_per_threadgroup]]) { + + const uint threads_per_tg = ntg.x * ntg.y * ntg.z; + const uint tg_index = (tgpig.z * tgpg.y + tgpig.y) * tgpg.x + tgpig.x; + const uint local_thread = tpitg.z * (ntg.x * ntg.y) + tpitg.y * ntg.x + tpitg.x; + const uint thread_index = tg_index * threads_per_tg + local_thread; + const uint64_t total_threads = (uint64_t) threads_per_tg * tgpg.x * tgpg.y * tgpg.z; + const uint64_t total_outputs = (uint64_t) args.N * args.OC * args.OH * args.OW; + + for (uint64_t index = thread_index; index < total_outputs; index += total_threads) { + uint64_t tmp = index; + + const int32_t ow = tmp % args.OW; tmp /= args.OW; + const int32_t oh = tmp % args.OH; tmp /= args.OH; + const int32_t oc = tmp % args.OC; tmp /= args.OC; + const int32_t n = tmp; + + float acc = 0.0f; + + const int32_t base_x = ow*args.s0 - args.p0; + const int32_t base_y = oh*args.s1 - args.p1; + + int32_t ky_start = 0; + if (base_y < 0) { + ky_start = (-base_y + args.d1 - 1)/args.d1; + } + int32_t ky_end = args.KH; + const int32_t y_max = args.IH - 1 - base_y; + if (y_max < 0) { + ky_end = ky_start; + } else if (base_y + (args.KH - 1)*args.d1 >= args.IH) { + ky_end = min(ky_end, y_max/args.d1 + 1); + } + + int32_t kx_start = 0; + if (base_x < 0) { + kx_start = (-base_x + args.d0 - 1)/args.d0; + } + int32_t kx_end = args.KW; + const int32_t x_max = args.IW - 1 - base_x; + if (x_max < 0) { + kx_end = kx_start; + } else if (base_x + (args.KW - 1)*args.d0 >= args.IW) { + kx_end = min(kx_end, x_max/args.d0 + 1); + } + + if (ky_start < ky_end && kx_start < kx_end) { + const uint64_t src_base_n = (uint64_t) n * args.nb13; + const uint64_t w_base_oc = (uint64_t) oc * args.nb03; + + for (int32_t ic = 0; ic < args.IC; ++ic) { + const uint64_t src_base_nc = src_base_n + (uint64_t) ic * args.nb12; + const uint64_t w_base_ocic = w_base_oc + (uint64_t) ic * args.nb02; + + for (int32_t ky = ky_start; ky < ky_end; ++ky) { + const int32_t iy = base_y + ky*args.d1; + const uint64_t src_base_row = src_base_nc + (uint64_t) iy * args.nb11; + const uint64_t w_base_row = w_base_ocic + (uint64_t) ky * args.nb01; + + for (int32_t kx = kx_start; kx < kx_end; ++kx) { + const int32_t ix = base_x + kx*args.d0; + const uint64_t src_offs = src_base_row + (uint64_t) ix * args.nb10; + const uint64_t w_offs = w_base_row + (uint64_t) kx * args.nb00; + + const float x = *(device const float *)(src + src_offs); + const float w = (float) (*(device const TK *)(weights + w_offs)); + + acc += x * w; + } + } + } + } + + const uint64_t dst_offs = + (uint64_t) n * args.nb3 + + (uint64_t) oc * args.nb2 + + (uint64_t) oh * args.nb1 + + (uint64_t) ow * args.nb0; + + *(device float *)(dst + dst_offs) = acc; + } +} + + + +typedef void (conv_transpose_1d_t)( + constant ggml_metal_kargs_conv_transpose_1d & args, + device const float * src0, + device const float * src1, + device char * dst, + uint3 tgpig[[threadgroup_position_in_grid]], + uint3 tgpg[[threadgroups_per_grid]]); + +template +kernel void kernel_conv_transpose_1d( + constant ggml_metal_kargs_conv_transpose_1d & args, + device const T * src0, + device const float * src1, + device char * dst, + uint3 tgpig[[threadgroup_position_in_grid]], + uint3 tgpg[[threadgroups_per_grid]]) { + + float v = 0.0f; + + for (int64_t c = 0; c < args.IC; c++) { + const int32_t kernel_offset = c * tgpg[1] * args.K + args.K * tgpig[1]; + const int32_t input_offset = c * args.IL; + + for (int64_t i = 0; i < args.IL; i++) { + if (tgpig[0] >= i * args.s0 && tgpig[0] < i * args.s0 + args.K) { + v += src0[kernel_offset + tgpig[0] - i * args.s0] * src1[input_offset + i]; + } + } + } + + device float * dst_ptr = (device float *) (dst + tgpig[0] * args.nb0 + tgpig[1] * args.nb1); + + dst_ptr[0] = v; +} + + + + +typedef void (conv_transpose_2d_t)( + constant ggml_metal_kargs_conv_transpose_2d & args, + device const float * src0, + device const float * src1, + device char * dst, + uint3 tgpig[[threadgroup_position_in_grid]], + uint3 tgpg[[threadgroups_per_grid]]); + +template +kernel void kernel_conv_transpose_2d( + constant ggml_metal_kargs_conv_transpose_2d & args, + device const T * src0, + device const float * src1, + device char * dst, + threadgroup float * shared_sum [[threadgroup(0)]], + uint3 tgpig[[threadgroup_position_in_grid]], + uint3 tpitg[[thread_position_in_threadgroup]], + uint3 ntg[[threads_per_threadgroup]]) { + + const int64_t out_x = tgpig[0]; + const int64_t out_y = tgpig[1]; + const int64_t out_c = tgpig[2]; + + const int64_t kw = tpitg[0]; + const int64_t kh = tpitg[1]; + + float v = 0.0f; + + for (int64_t in_c = 0; in_c < args.IC; in_c++) { + int64_t in_y = out_y - kh; + + if (in_y < 0 || in_y % args.s0) continue; + + in_y /= args.s0; + + if (in_y >= args.IH) continue; + + int64_t in_x = out_x - kw; + + if (in_x < 0 || in_x % args.s0) continue; + + in_x /= args.s0; + + if (in_x >= args.IW) continue; + + const int64_t input_idx = (args.IW * args.IH) * in_c + (args.IW) * in_y + in_x; + const int64_t kernel_idx = (args.KH * args.KW * args.OC) * in_c + (args.KH * args.KW) * out_c + (args.KW) * kh + kw; + + v += (float)src0[kernel_idx] * src1[input_idx]; + } + + const uint tid = tpitg.y * ntg.x + tpitg.x; + shared_sum[tid] = v; + + threadgroup_barrier(mem_flags::mem_threadgroup); + + if (tid == 0) { + float total = 0.0f; + const uint num_threads = ntg.x * ntg.y; + for (uint i = 0; i < num_threads; i++) { + total += shared_sum[i]; + } + + device float * dst_ptr = (device float *) (dst + out_x*args.nb0 + out_y * args.nb1 + out_c*args.nb2); + dst_ptr[0] = total; + } +} + + + + + + + + + + + + + +// bitonic sort implementation following the CUDA kernels as reference +typedef void (argsort_t)( + constant ggml_metal_kargs_argsort & args, + device const char * src0, + device int32_t * dst, + threadgroup int32_t * shmem_i32 [[threadgroup(0)]], + uint3 tgpig[[threadgroup_position_in_grid]], + ushort3 tpitg[[thread_position_in_threadgroup]], + ushort3 ntg[[threads_per_threadgroup]]); + +template +kernel void kernel_argsort_f32_i32( + constant ggml_metal_kargs_argsort & args, + device const char * src0, + device int32_t * dst, + threadgroup int32_t * shmem_i32 [[threadgroup(0)]], + uint3 tgpig[[threadgroup_position_in_grid]], + ushort3 tpitg[[thread_position_in_threadgroup]], + ushort3 ntg[[threads_per_threadgroup]]) { + // bitonic sort + const int col = tpitg[0]; + const int ib = tgpig[0] / args.ne01; + + const int i00 = ib*ntg.x; + const int i01 = tgpig[0] % args.ne01; + const int i02 = tgpig[1]; + const int i03 = tgpig[2]; + + device const float * src0_row = (device const float *) (src0 + args.nb01*i01 + args.nb02*i02 + args.nb03*i03); + + // initialize indices + shmem_i32[col] = i00 + col; + + threadgroup_barrier(mem_flags::mem_threadgroup); + + for (int k = 2; k <= ntg.x; k *= 2) { + for (int j = k / 2; j > 0; j /= 2) { + int ixj = col ^ j; + if (ixj > col) { + if ((col & k) == 0) { + if (shmem_i32[col] >= args.ne00 || + (shmem_i32[ixj] < args.ne00 && (order == GGML_SORT_ORDER_ASC ? + src0_row[shmem_i32[col]] > src0_row[shmem_i32[ixj]] : + src0_row[shmem_i32[col]] < src0_row[shmem_i32[ixj]])) + ) { + SWAP(shmem_i32[col], shmem_i32[ixj]); + } + } else { + if (shmem_i32[ixj] >= args.ne00 || + (shmem_i32[col] < args.ne00 && (order == GGML_SORT_ORDER_ASC ? + src0_row[shmem_i32[col]] < src0_row[shmem_i32[ixj]] : + src0_row[shmem_i32[col]] > src0_row[shmem_i32[ixj]])) + ) { + SWAP(shmem_i32[col], shmem_i32[ixj]); + } + } + } + + threadgroup_barrier(mem_flags::mem_threadgroup); + } + } + + const int64_t i0 = ib*args.top_k; + + // copy the result to dst without the padding + if (i0 + col < args.ne0 && col < args.top_k) { + dst += i0 + args.ne0*i01 + args.ne0*args.ne1*i02 + args.ne0*args.ne1*args.ne2*i03; + + dst[col] = shmem_i32[col]; + } +} + + +typedef void (argsort_merge_t)( + constant ggml_metal_kargs_argsort_merge & args, + device const char * src0, + device const int32_t * tmp, + device int32_t * dst, + uint3 tgpig[[threadgroup_position_in_grid]], + ushort3 tpitg[[thread_position_in_threadgroup]], + ushort3 ntg[[threads_per_threadgroup]]); + +template +kernel void kernel_argsort_merge_f32_i32( + constant ggml_metal_kargs_argsort_merge & args, + device const char * src0, + device const int32_t * tmp, + device int32_t * dst, + uint3 tgpig[[threadgroup_position_in_grid]], + ushort3 tpitg[[thread_position_in_threadgroup]], + ushort3 ntg[[threads_per_threadgroup]]) { + + const int im = tgpig[0] / args.ne01; + const int i01 = tgpig[0] % args.ne01; + const int i02 = tgpig[1]; + const int i03 = tgpig[2]; + + const int start = im * (2 * args.len); + + const int len0 = MIN(args.len, MAX(0, args.ne0 - (int)(start))); + const int len1 = MIN(args.len, MAX(0, args.ne0 - (int)(start + args.len))); + + const int total = len0 + len1; + + device const int32_t * tmp0 = tmp + start + + i01*args.ne0 + + i02*args.ne0*args.ne01 + + i03*args.ne0*args.ne01*args.ne02; + + device const int32_t * tmp1 = tmp0 + args.len; + + dst += start + + i01*args.top_k + + i02*args.top_k*args.ne01 + + i03*args.top_k*args.ne01*args.ne02; + + device const float * src0_row = (device const float *)(src0 + + args.nb01*i01 + + args.nb02*i02 + + args.nb03*i03); + + if (total == 0) { + return; + } + + const int chunk = (total + ntg.x - 1) / ntg.x; + + const int k0 = tpitg.x * chunk; + const int k1 = MIN(MIN(k0 + chunk, total), args.top_k); + + if (k0 >= args.top_k) { + return; + } + + if (k0 >= total) { + return; + } + + int low = k0 > len1 ? k0 - len1 : 0; + int high = MIN(k0, len0); + + // binary-search partition (i, j) such that i + j = k + while (low < high) { + const int mid = (low + high) >> 1; + + const int32_t idx0 = tmp0[mid]; + const int32_t idx1 = tmp1[k0 - mid - 1]; + + const float val0 = src0_row[idx0]; + const float val1 = src0_row[idx1]; + + bool take_left; + if (order == GGML_SORT_ORDER_ASC) { + take_left = (val0 <= val1); + } else { + take_left = (val0 >= val1); + } + + if (take_left) { + low = mid + 1; + } else { + high = mid; + } + } + + int i = low; + int j = k0 - i; + + // keep the merge fronts into registers + int32_t idx0 = 0; + float val0 = 0.0f; + if (i < len0) { + idx0 = tmp0[i]; + val0 = src0_row[idx0]; + } + + int32_t idx1 = 0; + float val1 = 0.0f; + if (j < len1) { + idx1 = tmp1[j]; + val1 = src0_row[idx1]; + } + + for (int k = k0; k < k1; ++k) { + int32_t out_idx; + + if (i >= len0) { + while (k < k1) { + dst[k++] = tmp1[j++]; + } + break; + } else if (j >= len1) { + while (k < k1) { + dst[k++] = tmp0[i++]; + } + break; + } else { + bool take_left; + + if (order == GGML_SORT_ORDER_ASC) { + take_left = (val0 <= val1); + } else { + take_left = (val0 >= val1); + } + + if (take_left) { + out_idx = idx0; + ++i; + if (i < len0) { + idx0 = tmp0[i]; + val0 = src0_row[idx0]; + } + } else { + out_idx = idx1; + ++j; + if (j < len1) { + idx1 = tmp1[j]; + val1 = src0_row[idx1]; + } + } + } + + dst[k] = out_idx; + } +} + + +constant bool FC_flash_attn_ext_pad_has_mask [[function_constant(FC_FLASH_ATTN_EXT_PAD + 0)]]; + +constant int32_t FC_flash_attn_ext_pad_ncpsg [[function_constant(FC_FLASH_ATTN_EXT_PAD + 25)]]; + +// pad the last chunk of C elements of k and v into a an extra pad buffer +kernel void kernel_flash_attn_ext_pad( + constant ggml_metal_kargs_flash_attn_ext_pad & args, + device const char * k, + device const char * v, + device const char * mask, + device char * dst, + uint3 tgpig[[threadgroup_position_in_grid]], + ushort tiitg[[thread_index_in_threadgroup]], + ushort3 ntg[[threads_per_threadgroup]]) { + const int32_t C = FC_flash_attn_ext_pad_ncpsg; + + device char * k_pad = dst; + device char * v_pad = k_pad + args.nb11*C*args.ne_12_2*args.ne_12_3; + device char * mask_pad = v_pad + args.nb21*C*args.ne_12_2*args.ne_12_3; + + const int32_t icp = args.ne11 % C; + const int32_t ic0 = args.ne11 - icp; + + const int32_t i1 = tgpig[0]; + const int32_t i2 = tgpig[1]; + const int32_t i3 = tgpig[2]; + + if (i2 < args.ne_12_2 && i3 < args.ne_12_3) { + device const char * k_src = k + args.nb11*(ic0 + i1) + args.nb12*i2 + args.nb13*i3; + device const char * v_src = v + args.nb21*(ic0 + i1) + args.nb22*i2 + args.nb23*i3; + + device char * k_dst = k_pad + args.nb11*i1 + args.nb11*C*i2 + args.nb11*C*args.ne_12_2*i3; + device char * v_dst = v_pad + args.nb21*i1 + args.nb21*C*i2 + args.nb21*C*args.ne_12_2*i3; + + if (i1 >= icp) { + // here it is not important the exact value that will be used as we rely on masking out the scores in the attention + for (uint64_t i = tiitg; i < args.nb11; i += ntg.x) { + k_dst[i] = 0; + } + for (uint64_t i = tiitg; i < args.nb21; i += ntg.x) { + v_dst[i] = 0; + } + } else { + for (uint64_t i = tiitg; i < args.nb11; i += ntg.x) { + k_dst[i] = k_src[i]; + } + for (uint64_t i = tiitg; i < args.nb21; i += ntg.x) { + v_dst[i] = v_src[i]; + } + } + } + + if (FC_flash_attn_ext_pad_has_mask) { + if (i2 < args.ne32 && i3 < args.ne33) { + for (int ib = i1; ib < args.ne31; ib += C) { + device const half * mask_src = (device const half *)(mask + args.nb31*ib + args.nb32*i2 + args.nb33*i3) + ic0; + device half * mask_dst = (device half *)(mask_pad) + C*ib + C*args.ne31*i2 + C*args.ne31*args.ne32*i3; + + for (int i = tiitg; i < C; i += ntg.x) { + if (i >= icp) { + mask_dst[i] = -MAXHALF; + } else { + mask_dst[i] = mask_src[i]; + } + } + } + } + } +} + +constant int32_t FC_flash_attn_ext_blk_nqptg [[function_constant(FC_FLASH_ATTN_EXT_BLK + 24)]]; +constant int32_t FC_flash_attn_ext_blk_ncpsg [[function_constant(FC_FLASH_ATTN_EXT_BLK + 25)]]; + +// scan the blocks of the mask that are not masked +// 0 - masked (i.e. full of -INF, skip) +// 1 - not masked (i.e. at least one element of the mask is not -INF) +// 2 - all zero +kernel void kernel_flash_attn_ext_blk( + constant ggml_metal_kargs_flash_attn_ext_blk & args, + device const char * mask, + device char * dst, + uint3 tgpig[[threadgroup_position_in_grid]], + ushort tiisg[[thread_index_in_simdgroup]]) { + // block size C x Q + const int32_t Q = FC_flash_attn_ext_blk_nqptg; + const int32_t C = FC_flash_attn_ext_blk_ncpsg; + + constexpr short NW = N_SIMDWIDTH; + + const int32_t i3 = tgpig[2]/args.ne32; + const int32_t i2 = tgpig[2]%args.ne32; + const int32_t i1 = tgpig[1]; + const int32_t i0 = tgpig[0]; + + char res = i0*C + C > args.ne30 ? 1 : 0; + + device const half * mask_src = (device const half *) (mask + (i1*Q)*args.nb31 + i2*args.nb32 + i3*args.nb33) + i0*C + tiisg; + + // detailed check of the elements of the block + if ((C > NW || Q > 1) && res == 0) { + half mmin = MAXHALF; + half mmax = -MAXHALF; + + FOR_UNROLL (short j = 0; j < Q; ++j) { + FOR_UNROLL (short ii = 0; ii < C/NW; ++ii) { + mmin = min(mmin, mask_src[ii*NW]); + mmax = max(mmax, mask_src[ii*NW]); + } + + mask_src += args.nb31/2; + } + + mmin = simd_min(mmin); + mmax = simd_max(mmax); + + if (mmax > -MAXHALF) { + if (mmin == 0.0 && mmax == 0.0) { + res = 2; + } else { + res = 1; + } + } + } + + const int32_t nblk1 = ((args.ne01 + Q - 1)/Q); + const int32_t nblk0 = ((args.ne30 + C - 1)/C); + + if (tiisg == 0) { + dst[((i3*args.ne32 + i2)*nblk1 + i1)*nblk0 + i0] = res; + } +} + +constant bool FC_flash_attn_ext_has_mask [[function_constant(FC_FLASH_ATTN_EXT + 0)]]; +constant bool FC_flash_attn_ext_has_sinks [[function_constant(FC_FLASH_ATTN_EXT + 1)]]; +constant bool FC_flash_attn_ext_has_bias [[function_constant(FC_FLASH_ATTN_EXT + 2)]]; +constant bool FC_flash_attn_ext_has_scap [[function_constant(FC_FLASH_ATTN_EXT + 3)]]; +constant bool FC_flash_attn_ext_has_kvpad [[function_constant(FC_FLASH_ATTN_EXT + 4)]]; + +constant bool FC_flash_attn_ext_bc_mask [[function_constant(FC_FLASH_ATTN_EXT + 10)]]; + +//constant float FC_flash_attn_ext_scale [[function_constant(FC_FLASH_ATTN_EXT + 10)]]; +//constant float FC_flash_attn_ext_max_bias [[function_constant(FC_FLASH_ATTN_EXT + 11)]]; +//constant float FC_flash_attn_ext_logit_softcap [[function_constant(FC_FLASH_ATTN_EXT + 12)]]; + +constant int32_t FC_flash_attn_ext_ns10 [[function_constant(FC_FLASH_ATTN_EXT + 20)]]; +constant int32_t FC_flash_attn_ext_ns20 [[function_constant(FC_FLASH_ATTN_EXT + 21)]]; +constant int32_t FC_flash_attn_ext_nsg [[function_constant(FC_FLASH_ATTN_EXT + 22)]]; + +// ref: https://arxiv.org/pdf/2307.08691.pdf +template< + typename q_t, // query types in shared memory + typename q4_t, + typename q8x8_t, + typename k_t, // key types in shared memory + typename k4x4_t, + typename k8x8_t, + typename v_t, // value types in shared memory + typename v4x4_t, + typename v8x8_t, + typename qk_t, // Q*K types + typename qk8x8_t, + typename s_t, // soft-max types + typename s2_t, + typename s8x8_t, + typename o_t, // attention accumulation types + typename o4_t, + typename o8x8_t, + typename kd4x4_t, // key type in device memory + short nl_k, + void (*deq_k)(device const kd4x4_t *, short, thread k4x4_t &), + typename vd4x4_t, // value type in device memory + short nl_v, + void (*deq_v)(device const vd4x4_t *, short, thread v4x4_t &), + short DK, // K head size + short DV, // V head size + short Q, // queries per threadgroup + short C, // cache items per threadgroup + short NSG> // number of simd groups +void kernel_flash_attn_ext_impl( + constant ggml_metal_kargs_flash_attn_ext & args, + device const char * q, + device const char * k, + device const char * v, + device const char * mask, + device const char * sinks, + device const char * pad, + device const char * blk, + device char * dst, + threadgroup half * shmem_f16, + uint3 tgpig, + ushort tiisg, + ushort sgitg) { + const ushort iq3 = tgpig[2]; + const ushort iq2 = tgpig[1]; + const ushort iq1 = tgpig[0]*Q; + +#define NS10 (FC_flash_attn_ext_ns10) +#define NS20 (FC_flash_attn_ext_ns20) + + // note: I had some concerns that using this instead of the ugly macros above was affecting performance + // need to re-check carefully and if no regressions are observerd - remove the macros + // the concerns is that maybe using const variables requires extra registers? but not sure if the compiler + // is clever enough to avoid this. unfortunately, using constexpr is not possible with FC + //const short NS10 = FC_flash_attn_ext_ns10; + //const short NS20 = FC_flash_attn_ext_ns20; + + constexpr short KV = 8; + + constexpr short DK4 = DK/4; + constexpr short DK8 = DK/8; + constexpr short DK16 = DK/16; + constexpr short DV4 = DV/4; + //constexpr short DV8 = DV/8; + constexpr short DV16 = DV/16; + + constexpr short PV = PAD2(DV, 64); + constexpr short PV4 = PV/4; + constexpr short PV8 = PV/8; + //constexpr short PV16 = PV/16; + + constexpr short NW = N_SIMDWIDTH; + constexpr short NQ = Q/NSG; + constexpr short SH = 2*C; // shared memory per simdgroup (s_t == float) + + constexpr short TS = 2*SH; + constexpr short T = DK + 2*PV; // shared memory size per query in (half) + + threadgroup q_t * sq = (threadgroup q_t *) (shmem_f16 + 0*T); // holds the query data + threadgroup q4_t * sq4 = (threadgroup q4_t *) (shmem_f16 + 0*T); // same as above but in q4_t + threadgroup o_t * so = (threadgroup o_t *) (shmem_f16 + 0*T + Q*DK); // the result for all queries in 8x8 matrices (the O matrix from the paper) + threadgroup o4_t * so4 = (threadgroup o4_t *) (shmem_f16 + 0*T + Q*DK); + threadgroup s_t * ss = (threadgroup s_t *) (shmem_f16 + Q*T); // scratch buffer for attention, mask and diagonal matrix + threadgroup s2_t * ss2 = (threadgroup s2_t *) (shmem_f16 + Q*T); // same as above but in s2_t + + threadgroup k_t * sk = (threadgroup k_t *) (shmem_f16 + sgitg*(4*16*KV) + Q*T + Q*TS); // scratch buffer to load K in shared memory + threadgroup k4x4_t * sk4x4 = (threadgroup k4x4_t *) (shmem_f16 + sgitg*(4*16*KV) + Q*T + Q*TS); // same as above but in k4x4_t + + threadgroup v_t * sv = (threadgroup v_t *) (shmem_f16 + sgitg*(4*16*KV) + Q*T + Q*TS); // scratch buffer to load V in shared memory + threadgroup v4x4_t * sv4x4 = (threadgroup v4x4_t *) (shmem_f16 + sgitg*(4*16*KV) + Q*T + Q*TS); // same as above but in v4x4_t + + // mask storage in shared mem + threadgroup half2 * sm2 = (threadgroup half2 *) (shmem_f16 + Q*T + 2*C); + + // per-query mask pointers + device const half2 * pm2[NQ]; + + FOR_UNROLL (short jj = 0; jj < NQ; ++jj) { + const short j = jj*NSG + sgitg; + + pm2[jj] = (device const half2 *) ((device const char *) mask + (iq1 + j)*args.nb31 + (iq2%args.ne32)*args.nb32 + (iq3%args.ne33)*args.nb33); + } + + { + const int32_t nblk1 = ((args.ne01 + Q - 1)/Q); + const int32_t nblk0 = ((args.ne11 + C - 1)/C); + + blk += (((iq3%args.ne33)*args.ne32 + (iq2%args.ne32))*nblk1 + iq1/Q)*nblk0; + } + + { + q += iq1*args.nb01 + iq2*args.nb02 + iq3*args.nb03; + + const short ikv2 = iq2/(args.ne02/args.ne_12_2); + const short ikv3 = iq3/(args.ne03/args.ne_12_3); + + k += ikv2*args.nb12 + ikv3*args.nb13; + v += ikv2*args.nb22 + ikv3*args.nb23; + } + + // load heads from Q to shared memory + FOR_UNROLL (short jj = 0; jj < NQ; ++jj) { + const short j = jj*NSG + sgitg; + + device const float4 * q4 = (device const float4 *) ((device const char *) q + j*args.nb01); + + for (short i = tiisg; i < DK4; i += NW) { + if (iq1 + j < args.ne01) { + sq4[j*DK4 + i] = (q4_t) q4[i]; + } else { + sq4[j*DK4 + i] = 0; + } + } + } + + // zero out + FOR_UNROLL (short jj = 0; jj < NQ; ++jj) { + const short j = jj*NSG + sgitg; + + for (short i = tiisg; i < DV4; i += NW) { + so4[j*PV4 + i] = 0; + } + + for (short i = tiisg; i < SH; i += NW) { + ss[j*SH + i] = 0.0f; + } + } + + threadgroup_barrier(mem_flags::mem_threadgroup); + + float S[NQ] = { [0 ... NQ-1] = 0.0f }; + + { + float M[NQ] = { [0 ... NQ-1] = -FLT_MAX/2 }; + + float slope = 1.0f; + + // ALiBi + if (FC_flash_attn_ext_has_bias) { + const short h = iq2; + + const float base = h < args.n_head_log2 ? args.m0 : args.m1; + const short exph = h < args.n_head_log2 ? h + 1 : 2*(h - args.n_head_log2) + 1; + + slope = pow(base, exph); + } + + // loop over the KV cache + // each simdgroup handles blocks of Q rows and C columns + for (int ic0 = 0; ; ++ic0) { + int ic = ic0*C; + if (ic >= args.ne11) { + break; + } + + // the last partial chunk uses the pad buffer as source + if (FC_flash_attn_ext_has_kvpad && ic + C > args.ne11) { + k = pad; + v = k + args.nb11*C*args.ne_12_2*args.ne_12_3; + mask = v + args.nb21*C*args.ne_12_2*args.ne_12_3; + + const short ikv2 = iq2/(args.ne02/args.ne_12_2); + const short ikv3 = iq3/(args.ne03/args.ne_12_3); + + k += (ikv2 + ikv3*args.ne_12_2)*args.nb11*C; + v += (ikv2 + ikv3*args.ne_12_2)*args.nb21*C; + + if (!FC_flash_attn_ext_has_mask) { + threadgroup half * sm = (threadgroup half *) (sm2); + + FOR_UNROLL (short jj = 0; jj < NQ; ++jj) { + const short j = jj*NSG + sgitg; + + for (short i = tiisg; i < C; i += NW) { + if (ic + i >= args.ne11) { + sm[2*j*SH + i] = -MAXHALF; + } + } + } + } else { + FOR_UNROLL (short jj = 0; jj < NQ; ++jj) { + const short j = jj*NSG + sgitg; + + pm2[jj] = (device const half2 *) ((device const half *) mask + + (iq1 + j)*C + + (iq2%args.ne32)*(C*args.ne31) + + (iq3%args.ne33)*(C*args.ne31*args.ne32)); + } + } + + ic = 0; + } + + char blk_cur = 1; + + // read the mask into shared mem + if (FC_flash_attn_ext_has_mask) { + blk_cur = blk[ic0]; + + if (blk_cur == 0) { + FOR_UNROLL (short jj = 0; jj < NQ; ++jj) { + pm2[jj] += NW; + } + + continue; + } + + if (blk_cur == 1) { + FOR_UNROLL (short jj = 0; jj < NQ; ++jj) { + const short j = jj*NSG + sgitg; + + if (FC_flash_attn_ext_bc_mask) { + sm2[j*SH + tiisg] = (iq1 + j) < args.ne31 ? pm2[jj][tiisg] : half2(-MAXHALF, -MAXHALF); + } else { + sm2[j*SH + tiisg] = pm2[jj][tiisg]; + } + + pm2[jj] += NW; + } + } else if (blk_cur == 2) { + FOR_UNROLL (short jj = 0; jj < NQ; ++jj) { + pm2[jj] += NW; + } + } + +#if 0 + // note: old -INF block optimization - obsoleted by pre-computing non-masked blocks + + threadgroup_barrier(mem_flags::mem_threadgroup); + + // used to detect blocks full of -INF + // skip only when the entire threadgroup is masked + half2 smax2(-MAXHALF/2, -MAXHALF/2); + + FOR_UNROLL (short j = 0; j < Q; ++j) { + smax2 = max(smax2, sm2[j*SH + tiisg]); + } + + smax2 = simd_max(smax2); + + if (max(smax2[0], smax2[1]) <= -MAXHALF/2) { + // this barrier is important + threadgroup_barrier(mem_flags::mem_threadgroup); + + continue; + } +#endif + } + + // Q*K^T + // this is compile-time check, so it does not have runtime overhead + if (is_same::value) { + // we can read directly from global memory + device const k_t * pk = (device const k_t *) (k + ic*args.nb11); + threadgroup const q_t * pq = sq; + threadgroup s_t * ps = ss; + + pk += sgitg*(8*NS10); + ps += sgitg*(8*1); + + static_assert((C/8) % NSG == 0, ""); + + constexpr short NC = (C/8)/NSG; + + FOR_UNROLL (short cc = 0; cc < NC; ++cc) { + qk8x8_t mqk = make_filled_simdgroup_matrix((qk_t) 0.0f); + + if (DK % 16 != 0) { + k8x8_t mk; + q8x8_t mq; + + FOR_UNROLL (short i = 0; i < DK8; ++i) { + simdgroup_barrier(mem_flags::mem_none); + + simdgroup_load(mk, pk + 8*i, NS10, 0, true); + simdgroup_load(mq, pq + 8*i, DK); + + simdgroup_barrier(mem_flags::mem_none); + + simdgroup_multiply_accumulate(mqk, mq, mk, mqk); + } + } else { + k8x8_t mk[2]; + q8x8_t mq[2]; + + // note: too much unroll can tank the performance for large heads + #pragma unroll (MIN(DK8/2, 4*NSG)) + for (short i = 0; i < DK8/2; ++i) { + simdgroup_barrier(mem_flags::mem_none); + + simdgroup_load(mq[0], pq + 0*8 + 16*i, DK); + simdgroup_load(mq[1], pq + 1*8 + 16*i, DK); + + simdgroup_load(mk[0], pk + 0*8 + 16*i, NS10, 0, true); + simdgroup_load(mk[1], pk + 1*8 + 16*i, NS10, 0, true); + + simdgroup_barrier(mem_flags::mem_none); + + simdgroup_multiply_accumulate(mqk, mq[0], mk[0], mqk); + simdgroup_multiply_accumulate(mqk, mq[1], mk[1], mqk); + } + } + + simdgroup_store(mqk, ps, SH, 0, false); + + pk += 8*(NSG*NS10); + ps += 8*(NSG); + } + } else { + // TODO: this is the quantized K cache branch - not optimized yet + for (short ccc = 0; ccc < (C/8)/NSG; ++ccc) { + const short cc = ccc*NSG + sgitg; + + const short tx = tiisg%4; + const short ty = tiisg/4; + + qk8x8_t mqk = make_filled_simdgroup_matrix((qk_t) 0.0f); + + for (short ii = 0; ii < DK16; ii += 4) { + device const kd4x4_t * pk4x4 = (device const kd4x4_t *) (k + ((ic + 8*cc + ty)*args.nb11)); + + if (DK16%4 == 0) { + // the head is evenly divisible by 4*16 = 64, so no need for bound checks + { + k4x4_t tmp; + deq_k(pk4x4 + (ii + tx)/nl_k, (ii + tx)%nl_k, tmp); + sk4x4[4*ty + tx] = tmp; + } + + simdgroup_barrier(mem_flags::mem_threadgroup); + + FOR_UNROLL (short k = 0; k < 4; ++k) { + k8x8_t mk; + q8x8_t mq; + + simdgroup_load(mk, sk + 16*k + 0*8, 4*16, 0, true); // transpose + simdgroup_load(mq, sq + (2*(ii + k) + 0)*8, DK); + simdgroup_multiply_accumulate(mqk, mq, mk, mqk); + + simdgroup_load(mk, sk + 16*k + 1*8, 4*16, 0, true); // transpose + simdgroup_load(mq, sq + (2*(ii + k) + 1)*8, DK); + simdgroup_multiply_accumulate(mqk, mq, mk, mqk); + } + } else { + if (ii + tx < DK16) { + k4x4_t tmp; + deq_k(pk4x4 + (ii + tx)/nl_k, (ii + tx)%nl_k, tmp); + sk4x4[4*ty + tx] = tmp; + } + + simdgroup_barrier(mem_flags::mem_threadgroup); + + for (short k = 0; k < 4 && ii + k < DK16; ++k) { + k8x8_t mk; + q8x8_t mq; + + simdgroup_load(mk, sk + 16*k + 0*8, 4*16, 0, true); // transpose + simdgroup_load(mq, sq + (2*(ii + k) + 0)*8, DK); + simdgroup_multiply_accumulate(mqk, mq, mk, mqk); + + simdgroup_load(mk, sk + 16*k + 1*8, 4*16, 0, true); // transpose + simdgroup_load(mq, sq + (2*(ii + k) + 1)*8, DK); + simdgroup_multiply_accumulate(mqk, mq, mk, mqk); + } + } + } + + simdgroup_store(mqk, ss + 8*cc, SH, 0, false); + } + } + + threadgroup_barrier(mem_flags::mem_threadgroup); + + // online softmax + FOR_UNROLL (short jj = 0; jj < NQ; ++jj) { + const short j = jj*NSG + sgitg; + + const float m = M[jj]; + + // scale and apply the logitcap / mask + float2 s2 = ss2[j*SH/2 + tiisg]*args.scale; + + if (FC_flash_attn_ext_has_scap) { + s2 = args.logit_softcap*precise::tanh(s2); + } + + // mqk = mqk + slope*mask + if (blk_cur != 2) { + if (FC_flash_attn_ext_has_bias) { + s2 += s2_t(sm2[j*SH + tiisg])*slope; + } else { + s2 += s2_t(sm2[j*SH + tiisg]); + } + } + + M[jj] = simd_max(max(M[jj], max(s2[0], s2[1]))); + + const float ms = exp(m - M[jj]); + const float2 vs2 = exp(s2 - M[jj]); + + S[jj] = S[jj]*ms + simd_sum(vs2[0] + vs2[1]); + + // the P matrix from the paper (Q rows, C columns) + ss2[j*SH/2 + tiisg] = vs2; + + if (DV4 % NW == 0) { + FOR_UNROLL (short ii = 0; ii < DV4/NW; ++ii) { + const short i = ii*NW + tiisg; + + so4[j*PV4 + i] *= ms; + } + } else { + for (short i = tiisg; i < DV4; i += NW) { + so4[j*PV4 + i] *= ms; + } + } + } + + threadgroup_barrier(mem_flags::mem_threadgroup); + + // O = O + (Q*K^T)*V + { + // we can read directly from global memory + if (is_same::value) { + static_assert(PV8 % NSG == 0, ""); + + constexpr short NO = PV8/NSG; + + o8x8_t lo[NO]; + + { + auto sot = so + 8*sgitg; + + FOR_UNROLL (short ii = 0; ii < NO; ++ii) { + simdgroup_load(lo[ii], sot, PV, 0, false); + + sot += 8*NSG; + } + } + + { + device const v_t * pv = (device const v_t *) (v + ic*args.nb21); + + pv += 8*sgitg; + + if (DV <= 64) { + FOR_UNROLL (short cc = 0; cc < C/8; ++cc) { + s8x8_t vs; + simdgroup_load(vs, ss + 8*cc, SH, 0, false); + + FOR_UNROLL (short ii = 0; ii < NO/2; ++ii) { + v8x8_t mv[2]; + + simdgroup_load(mv[0], pv + 0*NSG + 16*ii*NSG, NS20, 0, false); + simdgroup_load(mv[1], pv + 8*NSG + 16*ii*NSG, NS20, 0, false); + + simdgroup_multiply_accumulate(lo[2*ii + 0], vs, mv[0], lo[2*ii + 0]); + simdgroup_multiply_accumulate(lo[2*ii + 1], vs, mv[1], lo[2*ii + 1]); + } + + pv += 8*NS20; + } + } else { + constexpr short NC = (C/8)/2; + + FOR_UNROLL (short cc = 0; cc < NC; ++cc) { + s8x8_t vs[2]; + + simdgroup_load(vs[0], ss + 16*cc + 0, SH, 0, false); + simdgroup_load(vs[1], ss + 16*cc + 8, SH, 0, false); + + FOR_UNROLL (short ii = 0; ii < NO/2; ++ii) { + v8x8_t mv[4]; + + simdgroup_load(mv[0], pv + 0*NSG + 16*ii*NSG + 0*8*NS20, NS20, 0, false); + simdgroup_load(mv[1], pv + 8*NSG + 16*ii*NSG + 0*8*NS20, NS20, 0, false); + simdgroup_load(mv[2], pv + 0*NSG + 16*ii*NSG + 1*8*NS20, NS20, 0, false); + simdgroup_load(mv[3], pv + 8*NSG + 16*ii*NSG + 1*8*NS20, NS20, 0, false); + + simdgroup_multiply_accumulate(lo[2*ii + 0], vs[0], mv[0], lo[2*ii + 0]); + simdgroup_multiply_accumulate(lo[2*ii + 1], vs[0], mv[1], lo[2*ii + 1]); + simdgroup_multiply_accumulate(lo[2*ii + 0], vs[1], mv[2], lo[2*ii + 0]); + simdgroup_multiply_accumulate(lo[2*ii + 1], vs[1], mv[3], lo[2*ii + 1]); + } + + pv += 2*8*NS20; + } + } + } + + { + auto sot = so + 8*sgitg; + + FOR_UNROLL (short ii = 0; ii < NO; ++ii) { + simdgroup_store(lo[ii], sot, PV, 0, false); + + sot += 8*NSG; + } + } + } else { + // TODO: this is the quantized V cache branch - not optimized yet + + const short tx = tiisg%4; + const short ty = tiisg/4; + + for (short cc = 0; cc < C/8; ++cc) { + s8x8_t vs; + simdgroup_load(vs, ss + 8*cc, SH, 0, false); + + for (short ii = 4*sgitg; ii < DV16; ii += 4*NSG) { + device const vd4x4_t * pv4x4 = (device const vd4x4_t *) (v + ((ic + 8*cc + ty)*args.nb21)); + + if (DV16%4 == 0) { + // no need for bound checks + { + v4x4_t tmp; + deq_v(pv4x4 + (ii + tx)/nl_v, (ii + tx)%nl_v, tmp); + sv4x4[4*ty + tx] = tmp; + } + + simdgroup_barrier(mem_flags::mem_threadgroup); + + FOR_UNROLL (short k = 0; k < 4; ++k) { + v8x8_t mv[2]; + o8x8_t lo[2]; + + simdgroup_load(mv[0], sv + 16*k + 0*8, 4*16, 0, false); + simdgroup_load(mv[1], sv + 16*k + 1*8, 4*16, 0, false); + simdgroup_load(lo[0], so + 8*(2*(ii + k) + 0), PV, 0, false); + simdgroup_load(lo[1], so + 8*(2*(ii + k) + 1), PV, 0, false); + + simdgroup_multiply_accumulate(lo[0], vs, mv[0], lo[0]); + simdgroup_multiply_accumulate(lo[1], vs, mv[1], lo[1]); + + simdgroup_store(lo[0], so + 8*(2*(ii + k) + 0), PV, 0, false); + simdgroup_store(lo[1], so + 8*(2*(ii + k) + 1), PV, 0, false); + } + } else { + if (ii + tx < DV16) { + v4x4_t tmp; + deq_v(pv4x4 + (ii + tx)/nl_v, (ii + tx)%nl_v, tmp); + sv4x4[4*ty + tx] = tmp; + } + + simdgroup_barrier(mem_flags::mem_threadgroup); + + for (short k = 0; k < 4 && ii + k < DV16; ++k) { + v8x8_t mv[2]; + o8x8_t lo[2]; + + simdgroup_load(mv[0], sv + 16*k + 0*8, 4*16, 0, false); + simdgroup_load(mv[1], sv + 16*k + 1*8, 4*16, 0, false); + simdgroup_load(lo[0], so + 8*(2*(ii + k) + 0), PV, 0, false); + simdgroup_load(lo[1], so + 8*(2*(ii + k) + 1), PV, 0, false); + + simdgroup_multiply_accumulate(lo[0], vs, mv[0], lo[0]); + simdgroup_multiply_accumulate(lo[1], vs, mv[1], lo[1]); + + simdgroup_store(lo[0], so + 8*(2*(ii + k) + 0), PV, 0, false); + simdgroup_store(lo[1], so + 8*(2*(ii + k) + 1), PV, 0, false); + } + } + } + } + } + } + + threadgroup_barrier(mem_flags::mem_threadgroup); + } + + if (FC_flash_attn_ext_has_sinks) { + FOR_UNROLL (short jj = 0; jj < NQ; ++jj) { + const short j = jj*NSG + sgitg; + + const float m = M[jj]; + const float s = tiisg == 0 ? ((device const float *) sinks)[iq2] : -FLT_MAX/2; + + M[jj] = simd_max(max(M[jj], s)); + + const float ms = exp(m - M[jj]); + const float vs = exp(s - M[jj]); + + S[jj] = S[jj]*ms + simd_sum(vs); + + for (short i = tiisg; i < DV4; i += NW) { + so4[j*PV4 + i] *= ms; + } + } + } + } + + // store to global memory + for (short jj = 0; jj < NQ; ++jj) { + const short j = jj*NSG + sgitg; + if (iq1 + j >= args.ne01) { + break; + } + + device float4 * dst4 = (device float4 *) dst + ((uint64_t)iq3*args.ne2*args.ne1 + iq2 + (uint64_t)(iq1 + j)*args.ne1)*DV4; + + const float scale = S[jj] == 0.0 ? 0.0f : 1.0f/S[jj]; + + if (DV4 % NW == 0) { + FOR_UNROLL (short ii = 0; ii < DV4/NW; ++ii) { + const short i = ii*NW + tiisg; + + dst4[i] = (float4) so4[j*PV4 + i]*scale; + } + } else { + for (short i = tiisg; i < DV4; i += NW) { + dst4[i] = (float4) so4[j*PV4 + i]*scale; + } + } + } + +#undef NS10 +#undef NS20 +} + +template< + typename q_t, // query types in shared memory + typename q4_t, + typename q8x8_t, + typename k_t, // key types in shared memory + typename k4x4_t, + typename k8x8_t, + typename v_t, // value types in shared memory + typename v4x4_t, + typename v8x8_t, + typename qk_t, // Q*K types + typename qk8x8_t, + typename s_t, // soft-max types + typename s2_t, + typename s8x8_t, + typename o_t, // attention accumulation types + typename o4_t, + typename o8x8_t, + typename kd4x4_t, // key type in device memory + short nl_k, + void (*deq_k)(device const kd4x4_t *, short, thread k4x4_t &), + typename vd4x4_t, // value type in device memory + short nl_v, + void (*deq_v)(device const vd4x4_t *, short, thread v4x4_t &), + short DK, // K head size + short DV, // V head size + short Q = OP_FLASH_ATTN_EXT_NQPSG, // queries per threadgroup + short C = OP_FLASH_ATTN_EXT_NCPSG> // cache items per threadgroup +kernel void kernel_flash_attn_ext( + constant ggml_metal_kargs_flash_attn_ext & args, + device const char * q, + device const char * k, + device const char * v, + device const char * mask, + device const char * sinks, + device const char * pad, + device const char * blk, + device char * dst, + threadgroup half * shmem_f16 [[threadgroup(0)]], + uint3 tgpig[[threadgroup_position_in_grid]], + ushort tiisg[[thread_index_in_simdgroup]], + ushort sgitg[[simdgroup_index_in_threadgroup]]) { +#define FWD_TMPL q_t, q4_t, q8x8_t, k_t, k4x4_t, k8x8_t, v_t, v4x4_t, v8x8_t, qk_t, qk8x8_t, s_t, s2_t, s8x8_t, o_t, o4_t, o8x8_t, kd4x4_t, nl_k, deq_k, vd4x4_t, nl_v, deq_v, DK, DV, Q, C +#define FWD_ARGS args, q, k, v, mask, sinks, pad, blk, dst, shmem_f16, tgpig, tiisg, sgitg + switch (FC_flash_attn_ext_nsg) { + // note: disabled cases to reduce library load time + //case 1: kernel_flash_attn_ext_impl(FWD_ARGS); break; + //case 2: kernel_flash_attn_ext_impl(FWD_ARGS); break; + case 4: kernel_flash_attn_ext_impl(FWD_ARGS); break; + case 8: kernel_flash_attn_ext_impl(FWD_ARGS); break; + } +#undef FWD_TMPL +#undef FWD_ARGS +} + +// TODO: this is quite ugly. in the future these types will be hardcoded in the kernel, but for now keep them as +// template to be able to explore different combinations +// +#define FA_TYPES \ + half, half4, simdgroup_half8x8, \ + half, half4x4, simdgroup_half8x8, \ + half, half4x4, simdgroup_half8x8, \ + float, simdgroup_float8x8, \ + float, float2, simdgroup_float8x8, \ + float, float4, simdgroup_float8x8 + //half, half4, simdgroup_half8x8 + +#define FA_TYPES_BF \ + bfloat, bfloat4, simdgroup_bfloat8x8, \ + bfloat, bfloat4x4, simdgroup_bfloat8x8, \ + bfloat, bfloat4x4, simdgroup_bfloat8x8, \ + float, simdgroup_float8x8, \ + float, float2, simdgroup_float8x8, \ + half, half4, simdgroup_half8x8 + //float, float4, simdgroup_float8x8 + +#define FA_TYPES_F32 \ + half, half4, simdgroup_half8x8, \ + float, float4x4, simdgroup_float8x8, \ + float, float4x4, simdgroup_float8x8, \ + float, simdgroup_float8x8, \ + float, float2, simdgroup_float8x8, \ + float, float4, simdgroup_float8x8 + //half, half4, simdgroup_half8x8 + +typedef decltype(kernel_flash_attn_ext) flash_attn_ext_t; + + +template [[host_name("kernel_flash_attn_ext_f16_dk64_dv64" )]] kernel flash_attn_ext_t kernel_flash_attn_ext; + +#if defined(GGML_METAL_HAS_BF16) +#endif + + + + + + +#undef FA_TYPES +#undef FA_TYPES_BF +#undef FA_TYPES_F32 + +constant bool FC_flash_attn_ext_vec_has_mask [[function_constant(FC_FLASH_ATTN_EXT_VEC + 0)]]; +constant bool FC_flash_attn_ext_vec_has_sinks [[function_constant(FC_FLASH_ATTN_EXT_VEC + 1)]]; +constant bool FC_flash_attn_ext_vec_has_bias [[function_constant(FC_FLASH_ATTN_EXT_VEC + 2)]]; +constant bool FC_flash_attn_ext_vec_has_scap [[function_constant(FC_FLASH_ATTN_EXT_VEC + 3)]]; +constant bool FC_flash_attn_ext_vec_has_kvpad [[function_constant(FC_FLASH_ATTN_EXT_VEC + 4)]]; + +//constant float FC_flash_attn_ext_vec_scale [[function_constant(FC_FLASH_ATTN_EXT_VEC + 10)]]; +//constant float FC_flash_attn_ext_vec_max_bias [[function_constant(FC_FLASH_ATTN_EXT_VEC + 11)]]; +//constant float FC_flash_attn_ext_vec_logit_softcap [[function_constant(FC_FLASH_ATTN_EXT_VEC + 12)]]; + +constant int32_t FC_flash_attn_ext_vec_ns10 [[function_constant(FC_FLASH_ATTN_EXT_VEC + 20)]]; +constant int32_t FC_flash_attn_ext_vec_ns20 [[function_constant(FC_FLASH_ATTN_EXT_VEC + 21)]]; +constant int32_t FC_flash_attn_ext_vec_nsg [[function_constant(FC_FLASH_ATTN_EXT_VEC + 22)]]; +constant int32_t FC_flash_attn_ext_vec_nwg [[function_constant(FC_FLASH_ATTN_EXT_VEC + 23)]]; + +template< + typename q4_t, // query types in shared memory + typename k4_t, // key types in shared memory + typename v4_t, // value types in shared memory + typename qk_t, // Q*K types + typename s_t, // soft-max types + typename s4_t, + typename o4_t, // attention accumulation types + typename kd4_t, // key type in device memory + short nl_k, + void (*deq_k_t4)(device const kd4_t *, short, thread k4_t &), + typename vd4_t, // value type in device memory + short nl_v, + void (*deq_v_t4)(device const vd4_t *, short, thread v4_t &), + short DK, // K head size + short DV, // V head size + short NE = 4, // head elements per thread + short Q = OP_FLASH_ATTN_EXT_VEC_NQPSG, // queries per threadgroup + short C = OP_FLASH_ATTN_EXT_VEC_NCPSG> // cache items per threadgroup +kernel void kernel_flash_attn_ext_vec( + constant ggml_metal_kargs_flash_attn_ext_vec & args, + device const char * q, + device const char * k, + device const char * v, + device const char * mask, + device const char * sinks, + device const char * pad, + device char * dst, + threadgroup half * shmem_f16 [[threadgroup(0)]], + uint3 tgpig[[threadgroup_position_in_grid]], + ushort tiisg[[thread_index_in_simdgroup]], + ushort sgitg[[simdgroup_index_in_threadgroup]]) { + static_assert(DK % 32 == 0, "DK must be divisible by 32"); + static_assert(DV % 32 == 0, "DV must be divisible by 32"); + +#define NWG (FC_flash_attn_ext_vec_nwg) +#define NSG (FC_flash_attn_ext_vec_nsg) + +#define NS10 (FC_flash_attn_ext_vec_ns10) +#define NS20 (FC_flash_attn_ext_vec_ns20) + + const short iwg = tgpig[2]%NWG; + + const ushort iq3 = tgpig[2]/NWG; + const ushort iq2 = tgpig[1]; + const ushort iq1 = tgpig[0]; + + constexpr short DK4 = DK/4; + constexpr short DV4 = DV/4; + + constexpr short PK = PAD2(DK, 128); + constexpr short PK4 = PK/4; + + constexpr short PV = PAD2(DV, 128); + constexpr short PV4 = PV/4; + + constexpr short NW = N_SIMDWIDTH; + constexpr short NL = NW/NE; // note: this can be adjusted to support different head sizes and simdgroup work loads + constexpr short SH = 4*C; // shared memory per simdgroup + + static_assert(DK4 % NL == 0, "DK4 must be divisible by NL"); + static_assert(DV4 % NL == 0, "DV4 must be divisible by NL"); + + //const short T = PK + NSG*SH; // shared memory size per query in (half) + + //threadgroup q_t * sq = (threadgroup q_t *) (shmem_f16 + 0*PK); // holds the query data + threadgroup q4_t * sq4 = (threadgroup q4_t *) (shmem_f16 + 0*PK); // same as above but in q4_t + threadgroup s_t * ss = (threadgroup s_t *) (shmem_f16 + sgitg*SH + NSG*PK); // scratch buffer for attention + threadgroup s4_t * ss4 = (threadgroup s4_t *) (shmem_f16 + sgitg*SH + NSG*PK); // same as above but in s4_t + threadgroup half * sm = (threadgroup half *) (shmem_f16 + sgitg*SH + 2*C + NSG*PK); // scratch buffer for mask + threadgroup o4_t * so4 = (threadgroup o4_t *) (shmem_f16 + 2*sgitg*PV + NSG*PK + NSG*SH); // scratch buffer for the results + + // store the result for all queries in shared memory (the O matrix from the paper) + so4 += tiisg; + + { + q += iq1*args.nb01 + iq2*args.nb02 + iq3*args.nb03; + + const short ikv2 = iq2/(args.ne02/args.ne_12_2); + const short ikv3 = iq3/(args.ne03/args.ne_12_3); + + k += ikv2*args.nb12 + ikv3*args.nb13; + v += ikv2*args.nb22 + ikv3*args.nb23; + } + + // load heads from Q to shared memory + device const float4 * q4 = (device const float4 *) ((device const char *) q); + + if (iq1 < args.ne01) { + for (short i = tiisg; i < PK4; i += NW) { + if (i < DK4) { + sq4[i] = (q4_t) q4[i]; + } else { + sq4[i] = (q4_t) 0.0f; + } + } + } + + // zero out so + for (short i = 0; i < DV4/NL; ++i) { + so4[i*NL] = (o4_t) 0.0f; + } + + // zero out shared memory SH + for (short i = tiisg; i < SH/4; i += NW) { + ss4[i] = (s4_t) 0.0f; + } + + threadgroup_barrier(mem_flags::mem_threadgroup); + + { + float S = 0.0f; + float M = -FLT_MAX/2; + + // thread indices inside the simdgroup + const short tx = tiisg%NL; + const short ty = tiisg/NL; + + // pointer to the mask + device const half * pm = (device const half *) (mask + iq1*args.nb31 + (iq2%args.ne32)*args.nb32 + (iq3%args.ne33)*args.nb33); + + float slope = 1.0f; + + // ALiBi + if (FC_flash_attn_ext_vec_has_bias) { + const short h = iq2; + + const float base = h < args.n_head_log2 ? args.m0 : args.m1; + const short exph = h < args.n_head_log2 ? h + 1 : 2*(h - args.n_head_log2) + 1; + + slope = pow(base, exph); + } + + // loop over the KV cache + // each simdgroup handles blocks of Q rows and C columns + for (int ic0 = iwg*NSG + sgitg; ; ic0 += NWG*NSG) { + int ic = ic0*C; + if (ic >= args.ne11) { + break; + } + + // the last partial chunk uses the pad buffer as source + if (FC_flash_attn_ext_vec_has_kvpad && ic + C > args.ne11) { + k = pad; + v = k + args.nb11*C*args.ne_12_2*args.ne_12_3; + mask = v + args.nb21*C*args.ne_12_2*args.ne_12_3; + + const short ikv2 = iq2/(args.ne02/args.ne_12_2); + const short ikv3 = iq3/(args.ne03/args.ne_12_3); + + k += (ikv2 + ikv3*args.ne_12_2)*args.nb11*C; + v += (ikv2 + ikv3*args.ne_12_2)*args.nb21*C; + + if (!FC_flash_attn_ext_vec_has_mask) { + if (ic + tiisg >= args.ne11) { + sm[tiisg] = -MAXHALF; + } + } else { + pm = (device const half *) (mask) + + iq1*C + + (iq2%args.ne32)*(C*args.ne31) + + (iq3%args.ne33)*(C*args.ne31*args.ne32); + } + + ic = 0; + } + + if (FC_flash_attn_ext_vec_has_mask) { + sm[tiisg] = pm[ic + tiisg]; + } + + // skip -INF blocks + if (simd_max(sm[tiisg]) <= -MAXHALF) { + continue; + } + + // Q*K^T + { + device const k4_t * pk4 = (device const k4_t *) (k + ic*args.nb11); + threadgroup const q4_t * pq4 = sq4; + + pk4 += ty*NS10/4 + tx; + pq4 += tx; + + qk_t mqk[C/NE] = { [ 0 ... C/NE - 1] = 0.0f }; + + // each simdgroup processes 1 query and NE (NW/NL) cache elements + FOR_UNROLL (short cc = 0; cc < C/NE; ++cc) { + if (is_same::value) { + FOR_UNROLL (short ii = 0; ii < DK4/NL; ++ii) { + mqk[cc] += dot((float4) pk4[cc*NE*NS10/4 + ii*NL], (float4) pq4[ii*NL]); + } + } else { + device const kd4_t * pk = (device const kd4_t *) (k + ((ic + NE*cc + ty)*args.nb11)); + + k4_t mk; + + FOR_UNROLL (short ii = 0; ii < DK4/NL; ++ii) { + const short i = ii*NL + tx; + + deq_k_t4(pk + i/nl_k, i%nl_k, mk); + + mqk[cc] += dot((float4) mk, (float4) sq4[i]); + } + } + + if (NE == 1) { + mqk[cc] = simd_sum(mqk[cc]); + } else { + // simdgroup reduce (NE = 4) + // [ 0 .. 7] -> [ 0] + // [ 8 .. 15] -> [ 8] + // [16 .. 23] -> [16] + // [24 .. 31] -> [24] + if (NE <= 1) { + mqk[cc] += simd_shuffle_down(mqk[cc], 16); + } + if (NE <= 2) { + mqk[cc] += simd_shuffle_down(mqk[cc], 8); + } + if (NE <= 4) { + mqk[cc] += simd_shuffle_down(mqk[cc], 4); + } + if (NE <= 8) { + mqk[cc] += simd_shuffle_down(mqk[cc], 2); + } + if (NE <= 16) { + mqk[cc] += simd_shuffle_down(mqk[cc], 1); + } + + // broadcast + mqk[cc] = simd_shuffle(mqk[cc], NL*ty); + } + } + + if (FC_flash_attn_ext_vec_has_mask && + !FC_flash_attn_ext_vec_has_scap && + !FC_flash_attn_ext_vec_has_bias) { + ss[NE*tx + ty] = fma(mqk[tx], args.scale, (qk_t) sm[NE*tx + ty]); + } else { + mqk[tx] *= args.scale; + + if (FC_flash_attn_ext_vec_has_scap) { + mqk[tx] = args.logit_softcap*precise::tanh(mqk[tx]); + } + + if (FC_flash_attn_ext_vec_has_bias) { + mqk[tx] += (qk_t) sm[NE*tx + ty]*slope; + } else { + mqk[tx] += (qk_t) sm[NE*tx + ty]; + } + + ss[NE*tx + ty] = mqk[tx]; + } + } + + simdgroup_barrier(mem_flags::mem_threadgroup); + + // online softmax + { + const float m = M; + const float s = ss[tiisg]; + + M = simd_max(max(M, s)); + + const float ms = exp(m - M); + const float vs = exp(s - M); + + S = S*ms + simd_sum(vs); + + // the P matrix from the paper (Q rows, C columns) + ss[tiisg] = vs; + + // O = diag(ms)*O + if ((DV4/NL % NW == 0) || ty == 0) { + FOR_UNROLL (short ii = 0; ii < DV4/NL; ++ii) { + so4[ii*NL] *= ms; + } + } + } + + simdgroup_barrier(mem_flags::mem_threadgroup); + + // O = O + (Q*K^T)*V + { + o4_t lo[DV4/NL]; + FOR_UNROLL (short ii = 0; ii < DV4/NL; ++ii) { + lo[ii] = 0.0f; + } + + if (is_same::value) { + device const v4_t * pv4 = (device const v4_t *) (v + ic*args.nb21); + + pv4 += ty*NS20/4 + tx; + + const auto sst = ss + ty; + + FOR_UNROLL (short cc = 0; cc < C/NE; ++cc) { + FOR_UNROLL (short ii = 0; ii < DV4/NL; ++ii) { + lo[ii] += o4_t(float4(pv4[cc*NE*NS20/4 + ii*NL])*float4(sst[cc*NE])); + } + } + } else { + FOR_UNROLL (short cc = 0; cc < C/NE; ++cc) { + device const vd4_t * pv4 = (device const vd4_t *) (v + ((ic + NE*cc + ty)*args.nb21)); + + FOR_UNROLL (short ii = 0; ii < DV4/NL; ++ii) { + const short i = ii*NL + tx; + + v4_t mv; + deq_v_t4(pv4 + i/nl_v, i%nl_v, mv); + + lo[ii] += o4_t(float4(mv)*float4(ss[NE*cc + ty])); + } + } + } + + FOR_UNROLL (short ii = 0; ii < DV4/NL; ++ii) { + if (NE > 1) { + lo[ii][0] += simd_shuffle_down(lo[ii][0], 16); + lo[ii][1] += simd_shuffle_down(lo[ii][1], 16); + lo[ii][2] += simd_shuffle_down(lo[ii][2], 16); + lo[ii][3] += simd_shuffle_down(lo[ii][3], 16); + } + + if (NE > 2) { + lo[ii][0] += simd_shuffle_down(lo[ii][0], 8); + lo[ii][1] += simd_shuffle_down(lo[ii][1], 8); + lo[ii][2] += simd_shuffle_down(lo[ii][2], 8); + lo[ii][3] += simd_shuffle_down(lo[ii][3], 8); + } + + if (NE > 4) { + lo[ii][0] += simd_shuffle_down(lo[ii][0], 4); + lo[ii][1] += simd_shuffle_down(lo[ii][1], 4); + lo[ii][2] += simd_shuffle_down(lo[ii][2], 4); + lo[ii][3] += simd_shuffle_down(lo[ii][3], 4); + } + + if (NE > 8) { + lo[ii][0] += simd_shuffle_down(lo[ii][0], 2); + lo[ii][1] += simd_shuffle_down(lo[ii][1], 2); + lo[ii][2] += simd_shuffle_down(lo[ii][2], 2); + lo[ii][3] += simd_shuffle_down(lo[ii][3], 2); + } + + if (NE > 16) { + lo[ii][0] += simd_shuffle_down(lo[ii][0], 1); + lo[ii][1] += simd_shuffle_down(lo[ii][1], 1); + lo[ii][2] += simd_shuffle_down(lo[ii][2], 1); + lo[ii][3] += simd_shuffle_down(lo[ii][3], 1); + } + } + + if ((DV4/NL % NW == 0) || ty == 0) { + FOR_UNROLL (short ii = 0; ii < DV4/NL; ++ii) { + so4[ii*NL] += lo[ii]; + } + } + } + } + + if (FC_flash_attn_ext_vec_has_sinks && sgitg == 0 && iwg == 0) { + const float m = M; + const float s = tiisg == 0 ? ((device const float *) sinks)[iq2] : -FLT_MAX/2; + + M = simd_max(max(M, s)); + + const float ms = exp(m - M); + const float vs = exp(s - M); + + S = S*ms + simd_sum(vs); + + if ((DV4/NL % NW == 0) || ty == 0) { + FOR_UNROLL (short ii = 0; ii < DV4/NL; ++ii) { + so4[ii*NL] *= ms; + } + } + } + + // these are needed for reducing the results from the simdgroups (reuse the ss buffer) + if (tiisg == 0) { + ss[0] = (s_t) S; + ss[1] = (s_t) M; + } + } + + so4 -= tiisg; + + threadgroup_barrier(mem_flags::mem_threadgroup); + + // parallel reduce + for (short r = NSG/2; r > 0; r >>= 1) { + if (sgitg < r) { + const float S0 = ss[ 0]; + const float S1 = ss[r*(SH/2) + 0]; + + const float M0 = ss[ 1]; + const float M1 = ss[r*(SH/2) + 1]; + + const float M = max(M0, M1); + + const float ms0 = exp(M0 - M); + const float ms1 = exp(M1 - M); + + const float S = S0*ms0 + S1*ms1; + + if (tiisg == 0) { + ss[0] = S; + ss[1] = M; + } + + // O_0 = diag(ms0)*O_0 + diag(ms1)*O_1 + for (short i = tiisg; i < DV4; i += NW) { + so4[i] = so4[i]*ms0 + so4[i + r*PV4]*ms1; + } + } + + threadgroup_barrier(mem_flags::mem_threadgroup); + } + + // final rescale with 1/S and store to global memory + if (sgitg == 0) { + const int64_t nrows = args.ne3*args.ne2*args.ne1; + const int64_t rid = iq3*args.ne2*args.ne1 + iq2 + iq1*args.ne1; + + device float4 * dst4 = (device float4 *) dst; + device float * dst1 = (device float *) dst + nrows*DV*NWG; // the S and M are stored after the results + + const float S = NWG == 1 ? (ss[0] == 0.0f ? 0.0f : 1.0f/ss[0]) : 1.0f; + + // interleave the workgroup data + for (short i = tiisg; i < DV4; i += NW) { + dst4[rid*DV4*NWG + NWG*i + iwg] = (float4) so4[i]*S; + } + + // store S and M + if (NWG > 1) { + if (tiisg == 0) { + dst1[rid*(2*NWG) + 2*iwg + 0] = ss[0]; + dst1[rid*(2*NWG) + 2*iwg + 1] = ss[1]; + } + } + } + +#undef NWG +#undef NSG +#undef NS10 +#undef NS20 +} + +// note: I think the s_t can be half instead of float, because the Q*K scaling is done before storing to shared mem +// in the other (non-vec) kernel, we need s_t to also be float because we scale during the soft_max +// +#define FA_TYPES \ + half4, \ + half4, \ + half4, \ + float, \ + float, float4, \ + float4 + +#define FA_TYPES_F32 \ + half4, \ + float4, \ + float4, \ + float, \ + float, float4, \ + float4 + +typedef decltype(kernel_flash_attn_ext_vec) flash_attn_ext_vec_t; + +#if defined(GGML_METAL_HAS_BF16) +#endif + +template [[host_name("kernel_flash_attn_ext_vec_f32_dk64_dv64")]] kernel flash_attn_ext_vec_t kernel_flash_attn_ext_vec; +#if defined(GGML_METAL_HAS_BF16) +#endif + +#if defined(GGML_METAL_HAS_BF16) +#endif + +#if defined(GGML_METAL_HAS_BF16) +#endif + +#if defined(GGML_METAL_HAS_BF16) +#endif + +#if defined(GGML_METAL_HAS_BF16) +#endif + +#if defined(GGML_METAL_HAS_BF16) +#endif + +#if defined(GGML_METAL_HAS_BF16) +#endif + +#undef FA_TYPES +#undef FA_TYPES_F32 + +constant int32_t FC_flash_attn_ext_vec_reduce_DV [[function_constant(FC_FLASH_ATTN_EXT_VEC_REDUCE + 0)]]; +constant int32_t FC_flash_attn_ext_vec_reduce_NWG [[function_constant(FC_FLASH_ATTN_EXT_VEC_REDUCE + 1)]]; + +kernel void kernel_flash_attn_ext_vec_reduce( + constant ggml_metal_kargs_flash_attn_ext_vec_reduce & args, + device const char * htmp, + device char * dst, + uint tgpig[[threadgroup_position_in_grid]], + ushort tiisg[[thread_index_in_simdgroup]], + ushort sgitg[[simdgroup_index_in_threadgroup]]) { +#define NWG (FC_flash_attn_ext_vec_reduce_NWG) +#define DV (FC_flash_attn_ext_vec_reduce_DV) + + const uint64_t rid = tgpig; + + const short iwg = tiisg; + + device const float * ss = (device const float *) htmp + (uint64_t)args.nrows*DV*NWG; + + float S = ss[rid*(2*NWG) + 2*iwg + 0]; + float M = ss[rid*(2*NWG) + 2*iwg + 1]; + + const float m = simd_max(M); + const float ms = exp(M - m); + + S = simd_sum(S*ms); + S = S == 0.0f ? 0.0f : 1.0f/S; + + const short DV4 = DV/4; + + device const float4 * htmp4 = (device const float4 *) htmp + rid*DV4*NWG; + device float4 * dst4 = (device float4 *) dst + rid*DV4; + + for (short i = sgitg; i < DV4; i += NWG) { + const float4 v = simd_sum(htmp4[i*NWG + iwg]*ms); + + if (iwg == 0) { + dst4[i] = v*S; + } + } + +#undef NWG +#undef DV +} + +template +kernel void kernel_cpy_t_t( + constant ggml_metal_kargs_cpy & args, + device const char * src0, + device char * dst, + uint3 tgpig[[threadgroup_position_in_grid]], + ushort tiitg[[thread_index_in_threadgroup]], + ushort3 ntg[[threads_per_threadgroup]]) { + const int i03 = tgpig[2]; + const int i02 = tgpig[1]; + const int i01 = ntg[1] == 1 ? tgpig[0]%args.ne01 : tgpig[0]*ntg[1] + tiitg/ntg[0]; + const int iw0 = ntg[1] == 1 ? tgpig[0]/args.ne01 : 0; + + const int64_t n = i03*args.ne02*args.ne01*args.ne00 + i02*args.ne01*args.ne00 + i01*args.ne00; + + const int64_t i3 = n/(args.ne2*args.ne1*args.ne0); + const int64_t i2 = (n - i3*args.ne2*args.ne1*args.ne0)/(args.ne1*args.ne0); + const int64_t i1 = (n - i3*args.ne2*args.ne1*args.ne0 - i2*args.ne1*args.ne0)/args.ne0; + const int64_t i0 = (n - i3*args.ne2*args.ne1*args.ne0 - i2*args.ne1*args.ne0 - i1*args.ne0); + + device T1 * dst_data = (device T1 *) (dst + i3*args.nb3 + i2*args.nb2 + i1*args.nb1 + i0*args.nb0); + + for (int64_t i00 = iw0*ntg[0] + tiitg%ntg[0]; i00 < args.ne00; ) { + device const T0 * src = (device T0 *)(src0 + i03*args.nb03 + i02*args.nb02 + i01*args.nb01 + i00*args.nb00); + dst_data[i00] = (T1) src[0]; + break; + } +} + +typedef decltype(kernel_cpy_t_t) kernel_cpy_t; + +template [[host_name("kernel_cpy_f32_f16")]] kernel kernel_cpy_t kernel_cpy_t_t; +#if defined(GGML_METAL_HAS_BF16) +#endif +#if defined(GGML_METAL_HAS_BF16) +#endif + +template +kernel void kernel_cpy_f32_q( + constant ggml_metal_kargs_cpy & args, + device const char * src0, + device char * dst, + uint3 tgpig[[threadgroup_position_in_grid]], + ushort tiitg[[thread_index_in_threadgroup]], + ushort3 ntg[[threads_per_threadgroup]]) { + const int i03 = tgpig[2]; + const int i02 = tgpig[1]; + const int i01 = ntg[1] == 1 ? tgpig[0]%args.ne01 : tgpig[0]*ntg[1] + tiitg/ntg[0]; + const int iw0 = ntg[1] == 1 ? tgpig[0]/args.ne01 : 0; + + const int64_t n = i03*args.ne02*args.ne01*args.ne00 + i02*args.ne01*args.ne00 + i01*args.ne00; + + const int64_t i3 = n / (args.ne2*args.ne1*args.ne0); + const int64_t i2 = (n - i3*args.ne2*args.ne1*args.ne0) / (args.ne1*args.ne0); + const int64_t i1 = (n - i3*args.ne2*args.ne1*args.ne0 - i2*args.ne1*args.ne0) / args.ne0; + const int64_t i0 = (n - i3*args.ne2*args.ne1*args.ne0 - i2*args.ne1*args.ne0 - i1*args.ne0)/QK; + + device block_q * dst_data = (device block_q *)(dst + i3*args.nb3 + i2*args.nb2 + i1*args.nb1 + i0*args.nb0); + + for (int64_t i00 = iw0*ntg[0] + tiitg%ntg[0]; i00 < args.nk0; ) { + device const float * src = (device const float *)(src0 + i03*args.nb03 + i02*args.nb02 + i01*args.nb01 + (i00*QK)*args.nb00); + + quantize_func(src, dst_data[i00]); + + break; + } +} + +typedef decltype(kernel_cpy_f32_q) cpy_f_q_t; + + +template +kernel void kernel_cpy_q_f32( + constant ggml_metal_kargs_cpy & args, + device const char * src0, + device char * dst, + uint3 tgpig[[threadgroup_position_in_grid]], + ushort tiitg[[thread_index_in_threadgroup]], + ushort3 ntg[[threads_per_threadgroup]]) { + const int i03 = tgpig[2]; + const int i02 = tgpig[1]; + const int i01 = ntg[1] == 1 ? tgpig[0]%args.ne01 : tgpig[0]*ntg[1] + tiitg/ntg[0]; + const int iw0 = ntg[1] == 1 ? tgpig[0]/args.ne01 : 0; + + const int64_t n = i03*args.ne02*args.ne01*args.ne00 + i02*args.ne01*args.ne00 + i01*args.ne00; + + const int64_t i3 = n/(args.ne2*args.ne1*args.ne0); + const int64_t i2 = (n - i3*args.ne2*args.ne1*args.ne0)/(args.ne1*args.ne0); + const int64_t i1 = (n - i3*args.ne2*args.ne1*args.ne0 - i2*args.ne1*args.ne0)/args.ne0; + const int64_t i0 = (n - i3*args.ne2*args.ne1*args.ne0 - i2*args.ne1*args.ne0 - i1*args.ne0); + + device const block_q * src_data = (device const block_q *)(src0 + i03*args.nb03 + i02*args.nb02 + i01*args.nb01); + device T4x4 * dst_data = (device T4x4 *)(dst + i3*args.nb3 + i2*args.nb2 + i1*args.nb1 + i0*args.nb0); + + for (int64_t i00 = iw0*ntg[0] + tiitg%ntg[0]; i00 < args.nk0; ) { + T4x4 temp; + dequantize_func(src_data + i00/nl, i00%nl, temp); + dst_data[i00] = temp; + + break; + } +} + +typedef decltype(kernel_cpy_q_f32) cpy_q_f_t; + + + + + +template +void kernel_mul_mv_q2_K_f32_impl( + args_t args, + device const char * src0, + device const char * src1, + device char * dst, + threadgroup char * shmem, + uint3 tgpig, + ushort tiisg, + ushort sgitg) { + const short NSG = FC_mul_mv_nsg; + + const int nb = args.ne00/QK_K; + + const int r0 = tgpig.x; + const int r1 = tgpig.y; + const int im = tgpig.z; + + const int first_row = (r0 * NSG + sgitg) * nr0; + + const uint i12 = im%args.ne12; + const uint i13 = im/args.ne12; + + const uint64_t offset0 = first_row*args.nb01 + (i12/args.r2)*args.nb02 + (i13/args.r3)*args.nb03; + const uint64_t offset1 = r1*args.nb11 + (i12 )*args.nb12 + (i13 )*args.nb13; + + device const block_q2_K * x = (device const block_q2_K *) (src0 + offset0); + device const float * y = (device const float *) (src1 + offset1); + + float yl[32]; + float sumf[nr0]={0.f}; + + const short ix = tiisg/8; // 0...3 + const short it = tiisg%8; // 0...7 + const short iq = it/4; // 0 or 1 + const short ir = it%4; // 0...3 + const short is = (8*ir)/16;// 0 or 1 + + device const float * y4 = y + ix * QK_K + 128 * iq + 8 * ir; + + for (int ib = ix; ib < nb; ib += 4) { + float4 sumy = {0.f, 0.f, 0.f, 0.f}; + for (short i = 0; i < 8; ++i) { + yl[i+ 0] = y4[i+ 0]; sumy[0] += yl[i+ 0]; + yl[i+ 8] = y4[i+32]; sumy[1] += yl[i+ 8]; + yl[i+16] = y4[i+64]; sumy[2] += yl[i+16]; + yl[i+24] = y4[i+96]; sumy[3] += yl[i+24]; + } + + device const uint8_t * sc = (device const uint8_t *)x[ib].scales + 8*iq + is; + device const uint16_t * qs = (device const uint16_t *)x[ib].qs + 16 * iq + 4 * ir; + device const half * dh = &x[ib].d; + + for (short row = 0; row < nr0; row++) { + float4 acc1 = {0.f, 0.f, 0.f, 0.f}; + float4 acc2 = {0.f, 0.f, 0.f, 0.f}; + for (int i = 0; i < 8; i += 2) { + acc1[0] += yl[i+ 0] * (qs[i/2] & 0x0003); + acc2[0] += yl[i+ 1] * (qs[i/2] & 0x0300); + acc1[1] += yl[i+ 8] * (qs[i/2] & 0x000c); + acc2[1] += yl[i+ 9] * (qs[i/2] & 0x0c00); + acc1[2] += yl[i+16] * (qs[i/2] & 0x0030); + acc2[2] += yl[i+17] * (qs[i/2] & 0x3000); + acc1[3] += yl[i+24] * (qs[i/2] & 0x00c0); + acc2[3] += yl[i+25] * (qs[i/2] & 0xc000); + } + float dall = dh[0]; + float dmin = dh[1] * 1.f/16.f; + sumf[row] += dall * ((acc1[0] + 1.f/256.f * acc2[0]) * (sc[0] & 0xF) * 1.f/ 1.f + + (acc1[1] + 1.f/256.f * acc2[1]) * (sc[2] & 0xF) * 1.f/ 4.f + + (acc1[2] + 1.f/256.f * acc2[2]) * (sc[4] & 0xF) * 1.f/16.f + + (acc1[3] + 1.f/256.f * acc2[3]) * (sc[6] & 0xF) * 1.f/64.f) - + dmin * (sumy[0] * (sc[0] & 0xF0) + sumy[1] * (sc[2] & 0xF0) + sumy[2] * (sc[4] & 0xF0) + sumy[3] * (sc[6] & 0xF0)); + + qs += args.nb01/2; + sc += args.nb01; + dh += args.nb01/2; + } + + y4 += 4 * QK_K; + } + + device float * dst_f32 = (device float *) dst + (uint64_t)im*args.ne0*args.ne1 + (uint64_t)r1*args.ne0; + + for (int row = 0; row < nr0 && first_row + row < args.ne0; ++row) { + float sum_all = simd_sum(sumf[row]); + if (tiisg == 0) { + dst_f32[first_row + row] = sum_all; + } + } +} + + + +template +void kernel_mul_mv_q3_K_f32_impl( + args_t args, + device const char * src0, + device const char * src1, + device char * dst, + threadgroup char * shmem, + uint3 tgpig, + ushort tiisg, + ushort sgitg) { + const short NSG = FC_mul_mv_nsg; + + const int nb = args.ne00/QK_K; + + const int r0 = tgpig.x; + const int r1 = tgpig.y; + const int im = tgpig.z; + + const int first_row = (r0 * NSG + sgitg) * nr0; + + const uint i12 = im%args.ne12; + const uint i13 = im/args.ne12; + + const uint64_t offset0 = first_row*args.nb01 + (i12/args.r2)*args.nb02 + (i13/args.r3)*args.nb03; + const uint64_t offset1 = r1*args.nb11 + (i12 )*args.nb12 + (i13 )*args.nb13; + + device const block_q3_K * x = (device const block_q3_K *) (src0 + offset0); + device const float * yy = (device const float *) (src1 + offset1); + + float yl[32]; + + //const uint16_t kmask1 = 0x3030; + //const uint16_t kmask2 = 0x0f0f; + + const short tid = tiisg/4; + const short ix = tiisg%4; + const short ip = tid/4; // 0 or 1 + const short il = 2*((tid%4)/2); // 0 or 2 + const short ir = tid%2; + const short l0 = 8*ir; + + // One would think that the Metal compiler would figure out that ip and il can only have + // 4 possible states, and optimize accordingly. Well, no. It needs help, and we do it + // with these two tales. + // + // Possible masks for the high bit + const ushort4 mm[4] = {{0x0001, 0x0100, 0x0002, 0x0200}, // ip = 0, il = 0 + {0x0004, 0x0400, 0x0008, 0x0800}, // ip = 0, il = 2 + {0x0010, 0x1000, 0x0020, 0x2000}, // ip = 1, il = 0 + {0x0040, 0x4000, 0x0080, 0x8000}}; // ip = 1, il = 2 + + // Possible masks for the low 2 bits + const int4 qm[2] = {{0x0003, 0x0300, 0x000c, 0x0c00}, {0x0030, 0x3000, 0x00c0, 0xc000}}; + + const ushort4 hm = mm[2*ip + il/2]; + + const short shift = 2*il; + + const float v1 = il == 0 ? 4.f : 64.f; + const float v2 = 4.f * v1; + + const uint16_t s_shift1 = 4*ip; + const uint16_t s_shift2 = s_shift1 + il; + + const short q_offset = 32*ip + l0; + const short y_offset = 128*ip + 32*il + l0; + + device const float * y1 = yy + ix*QK_K + y_offset; + + uint32_t scales32, aux32; + thread uint16_t * scales16 = (thread uint16_t *)&scales32; + thread const int8_t * scales = (thread const int8_t *)&scales32; + + float sumf1[nr0] = {0.f}; + float sumf2[nr0] = {0.f}; + + for (int i = ix; i < nb; i += 4) { + for (short l = 0; l < 8; ++l) { + yl[l+ 0] = y1[l+ 0]; + yl[l+ 8] = y1[l+16]; + yl[l+16] = y1[l+32]; + yl[l+24] = y1[l+48]; + } + + device const uint16_t * q = (device const uint16_t *)(x[i].qs + q_offset); + device const uint16_t * h = (device const uint16_t *)(x[i].hmask + l0); + device const uint16_t * a = (device const uint16_t *)(x[i].scales); + device const half * dh = &x[i].d; + + for (short row = 0; row < nr0; ++row) { + const float d_all = (float)dh[0]; + + scales16[0] = a[4]; + scales16[1] = a[5]; + aux32 = ((scales32 >> s_shift2) << 4) & 0x30303030; + scales16[0] = a[il+0]; + scales16[1] = a[il+1]; + scales32 = ((scales32 >> s_shift1) & 0x0f0f0f0f) | aux32; + + float s1 = 0, s2 = 0, s3 = 0, s4 = 0, s5 = 0, s6 = 0; + for (short l = 0; l < 8; l += 2) { + const int32_t qs = q[l/2]; + s1 += yl[l+0] * (qs & qm[il/2][0]); + s2 += yl[l+1] * (qs & qm[il/2][1]); + s3 += ((h[l/2] & hm[0]) ? 0.f : yl[l+0]) + ((h[l/2] & hm[1]) ? 0.f : yl[l+1]); + s4 += yl[l+16] * (qs & qm[il/2][2]); + s5 += yl[l+17] * (qs & qm[il/2][3]); + s6 += ((h[l/2] & hm[2]) ? 0.f : yl[l+16]) + ((h[l/2] & hm[3]) ? 0.f : yl[l+17]); + } + float d1 = d_all * (s1 + 1.f/256.f * s2 - s3*v1); + float d2 = d_all * (s4 + 1.f/256.f * s5 - s6*v2); + sumf1[row] += d1 * (scales[0] - 32); + sumf2[row] += d2 * (scales[2] - 32); + + s1 = s2 = s3 = s4 = s5 = s6 = 0; + for (short l = 0; l < 8; l += 2) { + const int32_t qs = q[l/2+8]; + s1 += yl[l+8] * (qs & qm[il/2][0]); + s2 += yl[l+9] * (qs & qm[il/2][1]); + s3 += ((h[l/2+8] & hm[0]) ? 0.f : yl[l+8]) + ((h[l/2+8] & hm[1]) ? 0.f : yl[l+9]); + s4 += yl[l+24] * (qs & qm[il/2][2]); + s5 += yl[l+25] * (qs & qm[il/2][3]); + s6 += ((h[l/2+8] & hm[2]) ? 0.f : yl[l+24]) + ((h[l/2+8] & hm[3]) ? 0.f : yl[l+25]); + } + d1 = d_all * (s1 + 1.f/256.f * s2 - s3*v1); + d2 = d_all * (s4 + 1.f/256.f * s5 - s6*v2); + sumf1[row] += d1 * (scales[1] - 32); + sumf2[row] += d2 * (scales[3] - 32); + + q += args.nb01/2; + h += args.nb01/2; + a += args.nb01/2; + dh += args.nb01/2; + } + + y1 += 4 * QK_K; + } + + for (int row = 0; row < nr0; ++row) { + const float sumf = (sumf1[row] + 0.25f * sumf2[row]) / (1 << shift); + sumf1[row] = simd_sum(sumf); + } + + device float * dst_f32 = (device float *) dst + (uint64_t)im*args.ne0*args.ne1 + (uint64_t)r1*args.ne0; + + if (tiisg == 0) { + for (int row = 0; row < nr0 && first_row + row < args.ne0; ++row) { + dst_f32[first_row + row] = sumf1[row]; + } + } +} + + + +template +void kernel_mul_mv_q4_K_f32_impl( + args_t args, + device const char * src0, + device const char * src1, + device char * dst, + threadgroup char * shmem, + uint3 tgpig, + ushort tiisg, + ushort sgitg) { + const short NSG = FC_mul_mv_nsg; + + constexpr uint16_t kmask1 = 0x3f3f; + constexpr uint16_t kmask2 = 0x0f0f; + constexpr uint16_t kmask3 = 0xc0c0; + + const short ix = tiisg/8; // 0...3 + const short it = tiisg%8; // 0...7 + const short iq = it/4; // 0 or 1 + const short ir = it%4; // 0...3 + + const int nb = args.ne00/QK_K; + + const int r0 = tgpig.x; + const int r1 = tgpig.y; + const int im = tgpig.z; + + const int first_row = (r0 * NSG + sgitg) * nr0; + + const uint i12 = im%args.ne12; + const uint i13 = im/args.ne12; + + const uint64_t offset0 = first_row*args.nb01 + (i12/args.r2)*args.nb02 + (i13/args.r3)*args.nb03; + const uint64_t offset1 = r1*args.nb11 + (i12 )*args.nb12 + (i13 )*args.nb13; + + device const block_q4_K * x = (device const block_q4_K *) (src0 + offset0); + device const float * y = (device const float *) (src1 + offset1); + + float yl[16]; + float yh[16]; + + float sumf[nr0]={0.f}; + + device const float * y4 = y + ix * QK_K + 64 * iq + 8 * ir; + + uint16_t sc16[4]; + thread const uint8_t * sc8 = (thread const uint8_t *)sc16; + + for (int ib = ix; ib < nb; ib += 4) { + float4 sumy = {0.f, 0.f, 0.f, 0.f}; + + for (short i = 0; i < 8; ++i) { + yl[i+0] = y4[i+ 0]; sumy[0] += yl[i+0]; + yl[i+8] = y4[i+ 32]; sumy[1] += yl[i+8]; + yh[i+0] = y4[i+128]; sumy[2] += yh[i+0]; + yh[i+8] = y4[i+160]; sumy[3] += yh[i+8]; + } + + device const uint16_t * sc = (device const uint16_t *)x[ib].scales + iq; + device const uint16_t * q1 = (device const uint16_t *)x[ib].qs + 16 * iq + 4 * ir; + device const half * dh = &x[ib].d; + + for (short row = 0; row < nr0; row++) { + sc16[0] = sc[0] & kmask1; + sc16[1] = sc[2] & kmask1; + sc16[2] = ((sc[4] >> 0) & kmask2) | ((sc[0] & kmask3) >> 2); + sc16[3] = ((sc[4] >> 4) & kmask2) | ((sc[2] & kmask3) >> 2); + + device const uint16_t * q2 = q1 + 32; + + float4 acc1 = {0.f, 0.f, 0.f, 0.f}; + float4 acc2 = {0.f, 0.f, 0.f, 0.f}; + + FOR_UNROLL (short i = 0; i < 4; ++i) { + acc1[0] += yl[2*i + 0] * (q1[i] & 0x000F); + acc1[1] += yl[2*i + 1] * (q1[i] & 0x0F00); + acc1[2] += yl[2*i + 8] * (q1[i] & 0x00F0); + acc1[3] += yl[2*i + 9] * (q1[i] & 0xF000); + acc2[0] += yh[2*i + 0] * (q2[i] & 0x000F); + acc2[1] += yh[2*i + 1] * (q2[i] & 0x0F00); + acc2[2] += yh[2*i + 8] * (q2[i] & 0x00F0); + acc2[3] += yh[2*i + 9] * (q2[i] & 0xF000); + } + + sumf[row] += dh[0] * ((acc1[0] + 1.f/256.f * acc1[1]) * sc8[0] + + (acc1[2] + 1.f/256.f * acc1[3]) * sc8[1] * 1.f/16.f + + (acc2[0] + 1.f/256.f * acc2[1]) * sc8[4] + + (acc2[2] + 1.f/256.f * acc2[3]) * sc8[5] * 1.f/16.f) - + dh[1] * (sumy[0] * sc8[2] + sumy[1] * sc8[3] + sumy[2] * sc8[6] + sumy[3] * sc8[7]); + + q1 += args.nb01/2; + sc += args.nb01/2; + dh += args.nb01/2; + } + + y4 += 4 * QK_K; + } + + device float * dst_f32 = (device float *) dst + (int64_t)im*args.ne0*args.ne1 + (int64_t)r1*args.ne0; + + for (int row = 0; row < nr0 && first_row + row < args.ne0; ++row) { + float sum_all = simd_sum(sumf[row]); + if (tiisg == 0) { + dst_f32[first_row + row] = sum_all; + } + } +} + + + +template +void kernel_mul_mv_q5_K_f32_impl( + args_t args, + device const char * src0, + device const char * src1, + device char * dst, + threadgroup char * shmem, + uint3 tgpig, + ushort tiisg, + ushort sgitg) { + const short NSG = FC_mul_mv_nsg; + + const int nb = args.ne00/QK_K; + + const int r0 = tgpig.x; + const int r1 = tgpig.y; + const int im = tgpig.z; + + const int first_row = (r0 * NSG + sgitg) * nr0; + + const uint i12 = im%args.ne12; + const uint i13 = im/args.ne12; + + const uint64_t offset0 = first_row*args.nb01 + (i12/args.r2)*args.nb02 + (i13/args.r3)*args.nb03; + const uint64_t offset1 = r1*args.nb11 + (i12 )*args.nb12 + (i13 )*args.nb13; + + device const block_q5_K * x = (device const block_q5_K *) (src0 + offset0); + device const float * yy = (device const float *) (src1 + offset1); + + float sumf[nr0]={0.f}; + + float yl[16], yh[16]; + + constexpr uint16_t kmask1 = 0x3f3f; + constexpr uint16_t kmask2 = 0x0f0f; + constexpr uint16_t kmask3 = 0xc0c0; + + const short tid = tiisg/4; + const short ix = tiisg%4; + const short iq = tid/4; + const short ir = tid%4; + + const short l0 = 8*ir; + const short q_offset = 32*iq + l0; + const short y_offset = 64*iq + l0; + + const uint8_t hm1 = 1u << (2*iq); + const uint8_t hm2 = hm1 << 1; + const uint8_t hm3 = hm1 << 4; + const uint8_t hm4 = hm2 << 4; + + uint16_t sc16[4]; + thread const uint8_t * sc8 = (thread const uint8_t *)sc16; + + device const float * y1 = yy + ix*QK_K + y_offset; + + for (int i = ix; i < nb; i += 4) { + device const uint8_t * q1 = x[i].qs + q_offset; + device const uint8_t * qh = x[i].qh + l0; + device const half * dh = &x[i].d; + device const uint16_t * a = (device const uint16_t *)x[i].scales + iq; + + device const float * y2 = y1 + 128; + float4 sumy = {0.f, 0.f, 0.f, 0.f}; + for (short l = 0; l < 8; ++l) { + yl[l+0] = y1[l+ 0]; sumy[0] += yl[l+0]; + yl[l+8] = y1[l+32]; sumy[1] += yl[l+8]; + yh[l+0] = y2[l+ 0]; sumy[2] += yh[l+0]; + yh[l+8] = y2[l+32]; sumy[3] += yh[l+8]; + } + + for (short row = 0; row < nr0; ++row) { + device const uint8_t * q2 = q1 + 64; + + sc16[0] = a[0] & kmask1; + sc16[1] = a[2] & kmask1; + sc16[2] = ((a[4] >> 0) & kmask2) | ((a[0] & kmask3) >> 2); + sc16[3] = ((a[4] >> 4) & kmask2) | ((a[2] & kmask3) >> 2); + + float4 acc1 = {0.f}; + float4 acc2 = {0.f}; + FOR_UNROLL (short l = 0; l < 8; ++l) { + uint8_t h = qh[l]; + acc1[0] += yl[l+0] * (q1[l] & 0x0F); + acc1[1] += yl[l+8] * (q1[l] & 0xF0); + acc1[2] += yh[l+0] * (q2[l] & 0x0F); + acc1[3] += yh[l+8] * (q2[l] & 0xF0); + acc2[0] += h & hm1 ? yl[l+0] : 0.f; + acc2[1] += h & hm2 ? yl[l+8] : 0.f; + acc2[2] += h & hm3 ? yh[l+0] : 0.f; + acc2[3] += h & hm4 ? yh[l+8] : 0.f; + } + + sumf[row] += dh[0] * (sc8[0] * (acc1[0] + 16.f*acc2[0]) + + sc8[1] * (acc1[1]/16.f + 16.f*acc2[1]) + + sc8[4] * (acc1[2] + 16.f*acc2[2]) + + sc8[5] * (acc1[3]/16.f + 16.f*acc2[3])) - + dh[1] * (sumy[0] * sc8[2] + sumy[1] * sc8[3] + sumy[2] * sc8[6] + sumy[3] * sc8[7]); + + q1 += args.nb01; + qh += args.nb01; + dh += args.nb01/2; + a += args.nb01/2; + } + + y1 += 4 * QK_K; + } + + device float * dst_f32 = (device float *) dst + (uint64_t)im*args.ne0*args.ne1 + (uint64_t)r1*args.ne0; + + for (int row = 0; row < nr0 && first_row + row < args.ne0; ++row) { + const float tot = simd_sum(sumf[row]); + if (tiisg == 0) { + dst_f32[first_row + row] = tot; + } + } +} + + + +template +void kernel_mul_mv_q6_K_f32_impl( + args_t args, + device const char * src0, + device const char * src1, + device char * dst, + threadgroup char * shmem, + uint3 tgpig, + ushort tiisg, + ushort sgitg) { + const short NSG = FC_mul_mv_nsg; + + constexpr uint8_t kmask1 = 0x03; + constexpr uint8_t kmask2 = 0x0C; + constexpr uint8_t kmask3 = 0x30; + constexpr uint8_t kmask4 = 0xC0; + + const int nb = args.ne00/QK_K; + + const int r0 = tgpig.x; + const int r1 = tgpig.y; + const int im = tgpig.z; + + const int first_row = (r0 * NSG + sgitg) * nr0; + + const uint i12 = im%args.ne12; + const uint i13 = im/args.ne12; + + const uint64_t offset0 = first_row*args.nb01 + (i12/args.r2)*args.nb02 + (i13/args.r3)*args.nb03; + const uint64_t offset1 = r1*args.nb11 + (i12 )*args.nb12 + (i13 )*args.nb13; + + device const block_q6_K * x = (device const block_q6_K *) (src0 + offset0); + device const float * yy = (device const float *) (src1 + offset1); + + float sumf[nr0] = { 0.f }; + + float yl[16]; + + const short tid = tiisg/2; + const short ix = tiisg%2; + const short ip = tid/8; // 0 or 1 + const short il = tid%8; + const short l0 = 4*il; + const short is = 8*ip + l0/16; + + const short y_offset = 128*ip + l0; + const short q_offset_l = 64*ip + l0; + const short q_offset_h = 32*ip + l0; + + for (int i = ix; i < nb; i += 2) { + device const uint8_t * q1 = x[i].ql + q_offset_l; + device const uint8_t * q2 = q1 + 32; + device const uint8_t * qh = x[i].qh + q_offset_h; + device const int8_t * sc = x[i].scales + is; + device const half * dh = &x[i].d; + + device const float * y = yy + i * QK_K + y_offset; + + for (short l = 0; l < 4; ++l) { + yl[4*l + 0] = y[l + 0]; + yl[4*l + 1] = y[l + 32]; + yl[4*l + 2] = y[l + 64]; + yl[4*l + 3] = y[l + 96]; + } + + for (short row = 0; row < nr0; ++row) { + float4 sums = {0.f, 0.f, 0.f, 0.f}; + + FOR_UNROLL (short l = 0; l < 4; ++l) { + sums[0] += yl[4*l + 0] * ((int8_t)((q1[l] & 0xF) | ((qh[l] & kmask1) << 4)) - 32); + sums[1] += yl[4*l + 1] * ((int8_t)((q2[l] & 0xF) | ((qh[l] & kmask2) << 2)) - 32); + sums[2] += yl[4*l + 2] * ((int8_t)((q1[l] >> 4) | ((qh[l] & kmask3) << 0)) - 32); + sums[3] += yl[4*l + 3] * ((int8_t)((q2[l] >> 4) | ((qh[l] & kmask4) >> 2)) - 32); + } + + sumf[row] += dh[0] * (sums[0] * sc[0] + sums[1] * sc[2] + sums[2] * sc[4] + sums[3] * sc[6]); + + q1 += args.nb01; + q2 += args.nb01; + qh += args.nb01; + sc += args.nb01; + dh += args.nb01/2; + } + } + + device float * dst_f32 = (device float *) dst + (uint64_t)im*args.ne0*args.ne1 + (uint64_t)r1*args.ne0; + + for (int row = 0; row < nr0 && first_row + row < args.ne0; ++row) { + float sum_all = simd_sum(sumf[row]); + if (tiisg == 0) { + dst_f32[first_row + row] = sum_all; + } + } +} + + + +// ======================= "True" 2-bit + +template +void kernel_mul_mv_iq2_xxs_f32_impl( + args_t args, + device const char * src0, + device const char * src1, + device char * dst, + threadgroup char * shmem, + uint3 tgpig, + ushort tiisg, + ushort sgitg) { + const short NSG = FC_mul_mv_nsg; + + const int nb = args.ne00/QK_K; + + const int r0 = tgpig.x; + const int r1 = tgpig.y; + const int im = tgpig.z; + + const int first_row = (r0 * NSG + sgitg) * nr0; + + const uint i12 = im%args.ne12; + const uint i13 = im/args.ne12; + + const uint64_t offset0 = first_row*args.nb01 + (i12/args.r2)*args.nb02 + (i13/args.r3)*args.nb03; + const uint64_t offset1 = r1*args.nb11 + (i12 )*args.nb12 + (i13 )*args.nb13; + + device const block_iq2_xxs * x = (device const block_iq2_xxs *) (src0 + offset0); + device const float * y = (device const float *) (src1 + offset1); + + float yl[32]; + float sumf[nr0]={0.f}; + + const int nb32 = nb * (QK_K / 32); + + threadgroup uint64_t * svalues = (threadgroup uint64_t *)(shmem); + threadgroup uint8_t * ssigns = (threadgroup uint8_t *)(svalues + 256); + { + int nval = 4; + int pos = (32*sgitg + tiisg)*nval; + for (int i = 0; i < nval; ++i) svalues[pos + i] = iq2xxs_grid[pos + i]; + nval = 2; + pos = (32*sgitg + tiisg)*nval; + for (int i = 0; i < nval; ++i) ssigns[pos+i] = ksigns_iq2xs[pos+i]; + threadgroup_barrier(mem_flags::mem_threadgroup); + } + + const int ix = tiisg; + + device const float * y4 = y + 32 * ix; + + for (int ib32 = ix; ib32 < nb32; ib32 += 32) { + for (short i = 0; i < 32; ++i) { + yl[i] = y4[i]; + } + + const int ibl = ib32 / (QK_K / 32); + const int ib = ib32 % (QK_K / 32); + + device const block_iq2_xxs * xr = x + ibl; + device const uint16_t * q2 = xr->qs + 4 * ib; + device const half * dh = &xr->d; + + for (short row = 0; row < nr0; row++) { + const float db = dh[0]; + device const uint8_t * aux8 = (device const uint8_t *)q2; + const uint32_t aux32 = q2[2] | (q2[3] << 16); + const float d = db * (0.5f + (aux32 >> 28)); + + float sum = 0; + for (short l = 0; l < 4; ++l) { + const threadgroup uint8_t * grid = (const threadgroup uint8_t *)(svalues + aux8[l]); + const uint8_t signs = ssigns[(aux32 >> 7*l) & 127]; + for (short j = 0; j < 8; ++j) { + sum += yl[8*l + j] * grid[j] * (signs & kmask_iq2xs[j] ? -1.f : 1.f); + } + } + sumf[row] += d * sum; + + dh += args.nb01/2; + q2 += args.nb01/2; + } + + y4 += 32 * 32; + } + + device float * dst_f32 = (device float *) dst + (uint64_t)im*args.ne0*args.ne1 + (uint64_t)r1*args.ne0; + + for (int row = 0; row < nr0 && first_row + row < args.ne0; ++row) { + float sum_all = simd_sum(sumf[row]); + if (tiisg == 0) { + dst_f32[first_row + row] = sum_all * 0.25f; + } + } +} + + + +template +void kernel_mul_mv_iq2_xs_f32_impl( + args_t args, + device const char * src0, + device const char * src1, + device char * dst, + threadgroup char * shmem, + uint3 tgpig, + ushort tiisg, + ushort sgitg) { + const short NSG = FC_mul_mv_nsg; + + const int nb = args.ne00/QK_K; + + const int r0 = tgpig.x; + const int r1 = tgpig.y; + const int im = tgpig.z; + + const int first_row = (r0 * NSG + sgitg) * nr0; + + const uint i12 = im%args.ne12; + const uint i13 = im/args.ne12; + + const uint64_t offset0 = first_row*args.nb01 + (i12/args.r2)*args.nb02 + (i13/args.r3)*args.nb03; + const uint64_t offset1 = r1*args.nb11 + (i12 )*args.nb12 + (i13 )*args.nb13; + + device const block_iq2_xs * x = (device const block_iq2_xs *) (src0 + offset0); + device const float * y = (device const float *) (src1 + offset1); + + float yl[32]; + float sumf[nr0]={0.f}; + + const int nb32 = nb * (QK_K / 32); + + threadgroup uint64_t * svalues = (threadgroup uint64_t *)(shmem); + threadgroup uint8_t * ssigns = (threadgroup uint8_t *)(svalues + 512); + { + int nval = 8; + int pos = (32*sgitg + tiisg)*nval; + for (int i = 0; i < nval; ++i) svalues[pos + i] = iq2xs_grid[pos + i]; + nval = 2; + pos = (32*sgitg + tiisg)*nval; + for (int i = 0; i < nval; ++i) ssigns[pos+i] = ksigns_iq2xs[pos+i]; + threadgroup_barrier(mem_flags::mem_threadgroup); + } + + const int ix = tiisg; + + device const float * y4 = y + 32 * ix; + + for (int ib32 = ix; ib32 < nb32; ib32 += 32) { + for (short i = 0; i < 32; ++i) { + yl[i] = y4[i]; + } + + const int ibl = ib32 / (QK_K / 32); + const int ib = ib32 % (QK_K / 32); + + device const block_iq2_xs * xr = x + ibl; + device const uint16_t * q2 = xr->qs + 4 * ib; + device const uint8_t * sc = xr->scales + ib; + device const half * dh = &xr->d; + + for (short row = 0; row < nr0; row++) { + const float db = dh[0]; + const uint8_t ls1 = sc[0] & 0xf; + const uint8_t ls2 = sc[0] >> 4; + const float d1 = db * (0.5f + ls1); + const float d2 = db * (0.5f + ls2); + + float sum1 = 0, sum2 = 0; + for (short l = 0; l < 2; ++l) { + const threadgroup uint8_t * grid = (const threadgroup uint8_t *)(svalues + (q2[l] & 511)); + const uint8_t signs = ssigns[(q2[l] >> 9)]; + for (short j = 0; j < 8; ++j) { + sum1 += yl[8*l + j] * grid[j] * (signs & kmask_iq2xs[j] ? -1.f : 1.f); + } + } + for (short l = 2; l < 4; ++l) { + const threadgroup uint8_t * grid = (const threadgroup uint8_t *)(svalues + (q2[l] & 511)); + const uint8_t signs = ssigns[(q2[l] >> 9)]; + for (short j = 0; j < 8; ++j) { + sum2 += yl[8*l + j] * grid[j] * (signs & kmask_iq2xs[j] ? -1.f : 1.f); + } + } + sumf[row] += d1 * sum1 + d2 * sum2; + + dh += args.nb01/2; + q2 += args.nb01/2; + sc += args.nb01; + } + + y4 += 32 * 32; + } + + device float * dst_f32 = (device float *) dst + (uint64_t)im*args.ne0*args.ne1 + (uint64_t)r1*args.ne0; + + for (int row = 0; row < nr0 && first_row + row < args.ne0; ++row) { + float sum_all = simd_sum(sumf[row]); + if (tiisg == 0) { + dst_f32[first_row + row] = sum_all * 0.25f; + } + } +} + + + +template +void kernel_mul_mv_iq3_xxs_f32_impl( + args_t args, + device const char * src0, + device const char * src1, + device char * dst, + threadgroup char * shmem, + uint3 tgpig, + ushort tiisg, + ushort sgitg) { + const short NSG = FC_mul_mv_nsg; + + const int nb = args.ne00/QK_K; + + const int r0 = tgpig.x; + const int r1 = tgpig.y; + const int im = tgpig.z; + + const int first_row = (r0 * NSG + sgitg) * nr0; + + const uint i12 = im%args.ne12; + const uint i13 = im/args.ne12; + + const uint64_t offset0 = first_row*args.nb01 + (i12/args.r2)*args.nb02 + (i13/args.r3)*args.nb03; + const uint64_t offset1 = r1*args.nb11 + (i12 )*args.nb12 + (i13 )*args.nb13; + + device const block_iq3_xxs * x = (device const block_iq3_xxs *) (src0 + offset0); + device const float * y = (device const float *) (src1 + offset1); + + float yl[32]; + float sumf[nr0]={0.f}; + + const int nb32 = nb * (QK_K / 32); + + threadgroup uint32_t * svalues = (threadgroup uint32_t *)(shmem); + threadgroup uint8_t * ssigns = (threadgroup uint8_t *)(svalues + 256); + { + int nval = 4; + int pos = (32*sgitg + tiisg)*nval; + for (int i = 0; i < nval; ++i) svalues[pos + i] = iq3xxs_grid[pos + i]; + nval = 2; + pos = (32*sgitg + tiisg)*nval; + for (int i = 0; i < nval; ++i) ssigns[pos+i] = ksigns_iq2xs[pos+i]; + threadgroup_barrier(mem_flags::mem_threadgroup); + } + + const int ix = tiisg; + + device const float * y4 = y + 32 * ix; + + for (int ib32 = ix; ib32 < nb32; ib32 += 32) { + for (short i = 0; i < 32; ++i) { + yl[i] = y4[i]; + } + + const int ibl = ib32 / (QK_K / 32); + const int ib = ib32 % (QK_K / 32); + + device const block_iq3_xxs * xr = x + ibl; + device const uint8_t * q3 = xr->qs + 8 * ib; + device const uint16_t * gas = (device const uint16_t *)(xr->qs + QK_K/4) + 2 * ib; + device const half * dh = &xr->d; + + for (short row = 0; row < nr0; row++) { + const float db = dh[0]; + const uint32_t aux32 = gas[0] | (gas[1] << 16); + const float d = db * (0.5f + (aux32 >> 28)); + + float2 sum = {0}; + for (short l = 0; l < 4; ++l) { + const threadgroup uint8_t * grid1 = (const threadgroup uint8_t *)(svalues + q3[2*l+0]); + const threadgroup uint8_t * grid2 = (const threadgroup uint8_t *)(svalues + q3[2*l+1]); + const uint8_t signs = ssigns[(aux32 >> 7*l) & 127]; + for (short j = 0; j < 4; ++j) { + sum[0] += yl[8*l + j + 0] * grid1[j] * (signs & kmask_iq2xs[j+0] ? -1.f : 1.f); + sum[1] += yl[8*l + j + 4] * grid2[j] * (signs & kmask_iq2xs[j+4] ? -1.f : 1.f); + } + } + sumf[row] += d * (sum[0] + sum[1]); + + dh += args.nb01/2; + q3 += args.nb01; + gas += args.nb01/2; + } + + y4 += 32 * 32; + } + + device float * dst_f32 = (device float *) dst + (uint64_t)im*args.ne0*args.ne1 + (uint64_t)r1*args.ne0; + + for (int row = 0; row < nr0 && first_row + row < args.ne0; ++row) { + float sum_all = simd_sum(sumf[row]); + if (tiisg == 0) { + dst_f32[first_row + row] = sum_all * 0.5f; + } + } +} + + + +template +void kernel_mul_mv_iq3_s_f32_impl( + args_t args, + device const char * src0, + device const char * src1, + device char * dst, + threadgroup char * shmem, + uint3 tgpig, + ushort tiisg, + ushort sgitg) { + const short NSG = FC_mul_mv_nsg; + + const int nb = args.ne00/QK_K; + + const int r0 = tgpig.x; + const int r1 = tgpig.y; + const int im = tgpig.z; + + const int first_row = (r0 * NSG + sgitg) * nr0; + + const uint i12 = im%args.ne12; + const uint i13 = im/args.ne12; + + const uint64_t offset0 = first_row*args.nb01 + (i12/args.r2)*args.nb02 + (i13/args.r3)*args.nb03; + const uint64_t offset1 = r1*args.nb11 + (i12 )*args.nb12 + (i13 )*args.nb13; + + device const block_iq3_s * x = (device const block_iq3_s *) (src0 + offset0); + device const float * y = (device const float *) (src1 + offset1); + + float yl[32]; + float sumf[nr0]={0.f}; + + const int nb32 = nb * (QK_K / 32); + + threadgroup uint32_t * svalues = (threadgroup uint32_t *) shmem; + { + int nval = 8; + int pos = (32*sgitg + tiisg)*nval; + for (int i = 0; i < nval; ++i) svalues[pos + i] = iq3s_grid[pos + i]; + threadgroup_barrier(mem_flags::mem_threadgroup); + } + + const int ix = tiisg; + + device const float * y4 = y + 32 * ix; + + for (int ib32 = ix; ib32 < nb32; ib32 += 32) { + for (short i = 0; i < 32; ++i) { + yl[i] = y4[i]; + } + + const int ibl = ib32 / (QK_K / 32); + const int ib = ib32 % (QK_K / 32); + + device const block_iq3_s * xr = x + ibl; + device const uint8_t * qs = xr->qs + 8 * ib; + device const uint8_t * qh = xr->qh + ib; + device const uint8_t * sc = xr->scales + (ib/2); + device const uint8_t * signs = xr->signs + 4 * ib; + device const half * dh = &xr->d; + + for (short row = 0; row < nr0; row++) { + const float db = dh[0]; + const float d = db * (1 + 2*((sc[0] >> 4*(ib%2)) & 0xf)); + + float2 sum = {0}; + for (short l = 0; l < 4; ++l) { + const threadgroup uint32_t * table1 = qh[0] & kmask_iq2xs[2*l+0] ? svalues + 256 : svalues; + const threadgroup uint32_t * table2 = qh[0] & kmask_iq2xs[2*l+1] ? svalues + 256 : svalues; + const threadgroup uint8_t * grid1 = (const threadgroup uint8_t *)(table1 + qs[2*l+0]); + const threadgroup uint8_t * grid2 = (const threadgroup uint8_t *)(table2 + qs[2*l+1]); + for (short j = 0; j < 4; ++j) { + sum[0] += yl[8*l + j + 0] * grid1[j] * select(1, -1, signs[l] & kmask_iq2xs[j+0]); + sum[1] += yl[8*l + j + 4] * grid2[j] * select(1, -1, signs[l] & kmask_iq2xs[j+4]); + } + } + sumf[row] += d * (sum[0] + sum[1]); + + dh += args.nb01/2; + qs += args.nb01; + qh += args.nb01; + sc += args.nb01; + signs += args.nb01; + } + + y4 += 32 * 32; + } + + device float * dst_f32 = (device float *) dst + (uint64_t)im*args.ne0*args.ne1 + (uint64_t)r1*args.ne0; + + for (int row = 0; row < nr0 && first_row + row < args.ne0; ++row) { + float sum_all = simd_sum(sumf[row]); + if (tiisg == 0) { + dst_f32[first_row + row] = sum_all; + } + } +} + + + +template +void kernel_mul_mv_iq2_s_f32_impl( + args_t args, + device const char * src0, + device const char * src1, + device char * dst, + threadgroup char * shmem, + uint3 tgpig, + ushort tiisg, + ushort sgitg) { + const short NSG = FC_mul_mv_nsg; + + const int nb = args.ne00/QK_K; + + const int r0 = tgpig.x; + const int r1 = tgpig.y; + const int im = tgpig.z; + + const int first_row = (r0 * NSG + sgitg) * nr0; + + const uint i12 = im%args.ne12; + const uint i13 = im/args.ne12; + + const uint64_t offset0 = first_row*args.nb01 + (i12/args.r2)*args.nb02 + (i13/args.r3)*args.nb03; + const uint64_t offset1 = r1*args.nb11 + (i12 )*args.nb12 + (i13 )*args.nb13; + + device const block_iq2_s * x = (device const block_iq2_s *) (src0 + offset0); + device const float * y = (device const float *) (src1 + offset1); + + float yl[32]; + float sumf[nr0]={0.f}; + + const int nb32 = nb * (QK_K / 32); + + //threadgroup uint64_t * svalues = (threadgroup uint64_t *) shmem; + //{ + // int nval = 32; + // int pos = (32*sgitg + tiisg)*nval; + // for (int i = 0; i < nval; ++i) svalues[pos + i] = iq2s_grid[pos + i]; + // threadgroup_barrier(mem_flags::mem_threadgroup); + //} + + const short ix = tiisg; + + device const float * y4 = y + 32 * ix; + + for (int ib32 = ix; ib32 < nb32; ib32 += 32) { + for (short i = 0; i < 32; ++i) { + yl[i] = y4[i]; + } + + const int ibl = ib32 / (QK_K / 32); + const int ib = ib32 % (QK_K / 32); + + device const block_iq2_s * xr = x + ibl; + device const uint8_t * qs = xr->qs + 4 * ib; + device const uint8_t * qh = xr->qh + ib; + device const uint8_t * sc = xr->scales + ib; + device const uint8_t * signs = qs + QK_K/8; + device const half * dh = &xr->d; + + for (short row = 0; row < nr0; row++) { + const float db = dh[0]; + const float d1 = db * (0.5f + (sc[0] & 0xf)); + const float d2 = db * (0.5f + (sc[0] >> 4)); + + float2 sum = {0}; + for (short l = 0; l < 2; ++l) { + //const threadgroup uint8_t * grid1 = (const threadgroup uint8_t *)(svalues + (qs[l+0] | ((qh[0] << (8-2*l)) & 0x300))); + //const threadgroup uint8_t * grid2 = (const threadgroup uint8_t *)(svalues + (qs[l+2] | ((qh[0] << (4-2*l)) & 0x300))); + constant uint8_t * grid1 = (constant uint8_t *)(iq2s_grid + (qs[l+0] | ((qh[0] << (8-2*l)) & 0x300))); + constant uint8_t * grid2 = (constant uint8_t *)(iq2s_grid + (qs[l+2] | ((qh[0] << (4-2*l)) & 0x300))); + for (short j = 0; j < 8; ++j) { + sum[0] += yl[8*l + j + 0] * grid1[j] * select(1, -1, signs[l+0] & kmask_iq2xs[j]); + sum[1] += yl[8*l + j + 16] * grid2[j] * select(1, -1, signs[l+2] & kmask_iq2xs[j]); + } + } + sumf[row] += d1 * sum[0] + d2 * sum[1]; + + dh += args.nb01/2; + qs += args.nb01; + qh += args.nb01; + sc += args.nb01; + signs += args.nb01; + } + + y4 += 32 * 32; + } + + device float * dst_f32 = (device float *) dst + (uint64_t)im*args.ne0*args.ne1 + (uint64_t)r1*args.ne0; + + for (int row = 0; row < nr0 && first_row + row < args.ne0; ++row) { + float sum_all = simd_sum(sumf[row]); + if (tiisg == 0) { + dst_f32[first_row + row] = sum_all * 0.25f; + } + } +} + + + +template +void kernel_mul_mv_iq1_s_f32_impl( + args_t args, + device const char * src0, + device const char * src1, + device char * dst, + threadgroup char * shmem, + uint3 tgpig, + ushort tiisg, + ushort sgitg) { + const short NSG = FC_mul_mv_nsg; + + const int nb = args.ne00/QK_K; + + const int r0 = tgpig.x; + const int r1 = tgpig.y; + const int im = tgpig.z; + + const int first_row = (r0 * NSG + sgitg) * nr0; + + const uint i12 = im%args.ne12; + const uint i13 = im/args.ne12; + + const uint64_t offset0 = first_row*args.nb01 + (i12/args.r2)*args.nb02 + (i13/args.r3)*args.nb03; + const uint64_t offset1 = r1*args.nb11 + (i12 )*args.nb12 + (i13 )*args.nb13; + + device const block_iq1_s * x = (device const block_iq1_s *) (src0 + offset0); + device const float * y = (device const float *) (src1 + offset1); + + float yl[32]; + float sumf[nr0]={0.f}; + + const int nb32 = nb * (QK_K / 32); + + const short ix = tiisg; + + device const float * y4 = y + 32 * ix; + + for (int ib32 = ix; ib32 < nb32; ib32 += 32) { + float sumy = 0; + for (short i = 0; i < 32; ++i) { + yl[i] = y4[i]; + sumy += yl[i]; + } + + const int ibl = ib32 / (QK_K / 32); + const int ib = ib32 % (QK_K / 32); + + device const block_iq1_s * xr = x + ibl; + device const uint8_t * qs = xr->qs + 4 * ib; + device const uint16_t * qh = xr->qh + ib; + device const half * dh = &xr->d; + + for (short row = 0; row < nr0; row++) { + constant uint8_t * grid1 = (constant uint8_t *)(iq1s_grid_gpu + (qs[0] | ((qh[0] << 8) & 0x700))); + constant uint8_t * grid2 = (constant uint8_t *)(iq1s_grid_gpu + (qs[1] | ((qh[0] << 5) & 0x700))); + constant uint8_t * grid3 = (constant uint8_t *)(iq1s_grid_gpu + (qs[2] | ((qh[0] << 2) & 0x700))); + constant uint8_t * grid4 = (constant uint8_t *)(iq1s_grid_gpu + (qs[3] | ((qh[0] >> 1) & 0x700))); + + float sum = 0; + for (short j = 0; j < 4; ++j) { + sum += yl[j+ 0] * (grid1[j] & 0xf) + yl[j+ 4] * (grid1[j] >> 4) + + yl[j+ 8] * (grid2[j] & 0xf) + yl[j+12] * (grid2[j] >> 4) + + yl[j+16] * (grid3[j] & 0xf) + yl[j+20] * (grid3[j] >> 4) + + yl[j+24] * (grid4[j] & 0xf) + yl[j+28] * (grid4[j] >> 4); + } + sumf[row] += (float)dh[0] * (sum + sumy * (qh[0] & 0x8000 ? -1 - IQ1S_DELTA : -1 + IQ1S_DELTA)) * (2*((qh[0] >> 12) & 7) + 1); + + dh += args.nb01/2; + qs += args.nb01; + qh += args.nb01/2; + } + + y4 += 32 * 32; + } + + device float * dst_f32 = (device float *) dst + (uint64_t)im*args.ne0*args.ne1 + (uint64_t)r1*args.ne0; + + for (int row = 0; row < nr0 && first_row + row < args.ne0; ++row) { + float sum_all = simd_sum(sumf[row]); + if (tiisg == 0) { + dst_f32[first_row + row] = sum_all; + } + } +} + + + +template +void kernel_mul_mv_iq1_m_f32_impl( + args_t args, + device const char * src0, + device const char * src1, + device char * dst, + threadgroup char * shmem, + uint3 tgpig, + ushort tiisg, + ushort sgitg) { + const short NSG = FC_mul_mv_nsg; + + const int nb = args.ne00/QK_K; + + const int r0 = tgpig.x; + const int r1 = tgpig.y; + const int im = tgpig.z; + + const int first_row = (r0 * NSG + sgitg) * nr0; + + const uint i12 = im%args.ne12; + const uint i13 = im/args.ne12; + + const uint64_t offset0 = first_row*args.nb01 + (i12/args.r2)*args.nb02 + (i13/args.r3)*args.nb03; + const uint64_t offset1 = r1*args.nb11 + (i12 )*args.nb12 + (i13 )*args.nb13; + + device const block_iq1_m * x = (device const block_iq1_m *) (src0 + offset0); + device const float * y = (device const float *) (src1 + offset1); + + float yl[32]; + float sumf[nr0]={0.f}; + + const int nb32 = nb * (QK_K / 32); + + const short ix = tiisg; + + device const float * y4 = y + 32 * ix; + + iq1m_scale_t scale; + + for (int ib32 = ix; ib32 < nb32; ib32 += 32) { + float4 sumy = {0.f}; + for (short i = 0; i < 8; ++i) { + yl[i+ 0] = y4[i+ 0]; sumy[0] += yl[i+ 0]; + yl[i+ 8] = y4[i+ 8]; sumy[1] += yl[i+ 8]; + yl[i+16] = y4[i+16]; sumy[2] += yl[i+16]; + yl[i+24] = y4[i+24]; sumy[3] += yl[i+24]; + } + + const int ibl = ib32 / (QK_K / 32); + const int ib = ib32 % (QK_K / 32); + + device const block_iq1_m * xr = x + ibl; + device const uint8_t * qs = xr->qs + 4 * ib; + device const uint8_t * qh = xr->qh + 2 * ib; + device const uint16_t * sc = (device const uint16_t *)xr->scales; + + for (short row = 0; row < nr0; row++) { + scale.u16 = (sc[0] >> 12) | ((sc[1] >> 8) & 0x00f0) | ((sc[2] >> 4) & 0x0f00) | (sc[3] & 0xf000); + + constant uint8_t * grid1 = (constant uint8_t *)(iq1s_grid_gpu + (qs[0] | ((qh[0] << 8) & 0x700))); + constant uint8_t * grid2 = (constant uint8_t *)(iq1s_grid_gpu + (qs[1] | ((qh[0] << 4) & 0x700))); + constant uint8_t * grid3 = (constant uint8_t *)(iq1s_grid_gpu + (qs[2] | ((qh[1] << 8) & 0x700))); + constant uint8_t * grid4 = (constant uint8_t *)(iq1s_grid_gpu + (qs[3] | ((qh[1] << 4) & 0x700))); + + float2 sum = {0.f}; + for (short j = 0; j < 4; ++j) { + sum[0] += yl[j+ 0] * (grid1[j] & 0xf) + yl[j+ 4] * (grid1[j] >> 4) + + yl[j+ 8] * (grid2[j] & 0xf) + yl[j+12] * (grid2[j] >> 4); + sum[1] += yl[j+16] * (grid3[j] & 0xf) + yl[j+20] * (grid3[j] >> 4) + + yl[j+24] * (grid4[j] & 0xf) + yl[j+28] * (grid4[j] >> 4); + } + const float delta1 = sumy[0] * (qh[0] & 0x08 ? -1 - IQ1M_DELTA : -1 + IQ1M_DELTA) + sumy[1] * (qh[0] & 0x80 ? -1 - IQ1M_DELTA : -1 + IQ1M_DELTA); + const float delta2 = sumy[2] * (qh[1] & 0x08 ? -1 - IQ1M_DELTA : -1 + IQ1M_DELTA) + sumy[3] * (qh[1] & 0x80 ? -1 - IQ1M_DELTA : -1 + IQ1M_DELTA); + + sumf[row] += (float)scale.f16 * ((sum[0] + delta1) * (2*((sc[ib/2] >> (6*(ib%2)+0)) & 7) + 1) + + (sum[1] + delta2) * (2*((sc[ib/2] >> (6*(ib%2)+3)) & 7) + 1)); + + sc += args.nb01/2; + qs += args.nb01; + qh += args.nb01; + } + + y4 += 32 * 32; + } + + device float * dst_f32 = (device float *) dst + (uint64_t)im*args.ne0*args.ne1 + (uint64_t)r1*args.ne0; + + for (int row = 0; row < nr0 && first_row + row < args.ne0; ++row) { + float sum_all = simd_sum(sumf[row]); + if (tiisg == 0) { + dst_f32[first_row + row] = sum_all; + } + } +} + + + +template +void kernel_mul_mv_iq4_nl_f32_impl( + args_t args, + device const char * src0, + device const char * src1, + device char * dst, + threadgroup char * shmem, + uint3 tgpig, + ushort tiisg, + ushort sgitg) { + const short NSG = FC_mul_mv_nsg; + + threadgroup float * shmem_f32 = (threadgroup float *) shmem; + + const int r0 = tgpig.x; + const int r1 = tgpig.y; + const int im = tgpig.z; + + const int first_row = (r0 * NSG + sgitg) * NR0; + + const uint i12 = im%args.ne12; + const uint i13 = im/args.ne12; + + const uint64_t offset0 = first_row*args.nb01 + (i12/args.r2)*args.nb02 + (i13/args.r3)*args.nb03; + const uint64_t offset1 = r1*args.nb11 + (i12 )*args.nb12 + (i13 )*args.nb13; + + device const block_iq4_nl * x = (device const block_iq4_nl *) (src0 + offset0); + device const float * y = (device const float *) (src1 + offset1); + + const int nb = args.ne00/QK4_NL; + const int ns01 = args.nb01/args.nb00; + + const short ix = tiisg/2; // 0...15 + const short it = tiisg%2; // 0 or 1 + + shmem_f32[tiisg] = kvalues_iq4nl_f[tiisg%16]; + threadgroup_barrier(mem_flags::mem_threadgroup); + + float4 yl[4]; + float sumf[NR0]={0.f}; + + device const float * yb = y + ix*QK4_NL + it*8; + + uint32_t aux32[2]; + thread const uint8_t * q8 = (thread const uint8_t *)aux32; + + float4 qf1, qf2; + + // [TAG_MUL_MV_WEIRD] + for (int ib = ix; ib < nb && ib < ns01; ib += 16) { + device const float4 * y4 = (device const float4 *)yb; + yl[0] = y4[0]; + yl[1] = y4[4]; + yl[2] = y4[1]; + yl[3] = y4[5]; + + for (short row = 0; row < NR0; row++) { + device const block_iq4_nl & xb = x[row*ns01 + ib]; + device const uint16_t * q4 = (device const uint16_t *)(xb.qs + 8*it); + + float4 acc1 = {0.f}, acc2 = {0.f}; + + aux32[0] = q4[0] | (q4[1] << 16); + aux32[1] = (aux32[0] >> 4) & 0x0f0f0f0f; + aux32[0] &= 0x0f0f0f0f; + qf1 = {shmem_f32[q8[0]], shmem_f32[q8[1]], shmem_f32[q8[2]], shmem_f32[q8[3]]}; + qf2 = {shmem_f32[q8[4]], shmem_f32[q8[5]], shmem_f32[q8[6]], shmem_f32[q8[7]]}; + acc1 += yl[0] * qf1; + acc2 += yl[1] * qf2; + + aux32[0] = q4[2] | (q4[3] << 16); + aux32[1] = (aux32[0] >> 4) & 0x0f0f0f0f; + aux32[0] &= 0x0f0f0f0f; + qf1 = {shmem_f32[q8[0]], shmem_f32[q8[1]], shmem_f32[q8[2]], shmem_f32[q8[3]]}; + qf2 = {shmem_f32[q8[4]], shmem_f32[q8[5]], shmem_f32[q8[6]], shmem_f32[q8[7]]}; + acc1 += yl[2] * qf1; + acc2 += yl[3] * qf2; + + acc1 += acc2; + + sumf[row] += (float)xb.d * (acc1[0] + acc1[1] + acc1[2] + acc1[3]); + } + + yb += 16 * QK4_NL; + } + + device float * dst_f32 = (device float *) dst + (uint64_t)im*args.ne0*args.ne1 + (uint64_t)r1*args.ne0; + + for (int row = 0; row < NR0 && first_row + row < args.ne0; ++row) { + float sum_all = simd_sum(sumf[row]); + if (tiisg == 0) { + dst_f32[first_row + row] = sum_all; + } + } +} + + + +template +void kernel_mul_mv_iq4_xs_f32_impl( + args_t args, + device const char * src0, + device const char * src1, + device char * dst, + threadgroup char * shmem, + uint3 tgpig, + ushort tiisg, + ushort sgitg) { + const short NSG = FC_mul_mv_nsg; + + threadgroup float * shmem_f32 = (threadgroup float *) shmem; + + const int r0 = tgpig.x; + const int r1 = tgpig.y; + const int im = tgpig.z; + const int first_row = (r0 * NSG + sgitg) * NR0; + + const uint i12 = im%args.ne12; + const uint i13 = im/args.ne12; + + const uint64_t offset0 = first_row*args.nb01 + (i12/args.r2)*args.nb02 + (i13/args.r3)*args.nb03; + const uint64_t offset1 = r1*args.nb11 + (i12 )*args.nb12 + (i13 )*args.nb13; + + device const block_iq4_xs * x = (device const block_iq4_xs *) (src0 + offset0); + device const float * y = (device const float *) (src1 + offset1); + + const int nb = args.ne00/QK_K; + const int ns01 = args.nb01/args.nb00; + + const short ix = tiisg/16; // 0 or 1 + const short it = tiisg%16; // 0...15 + const short ib = it/2; + const short il = it%2; + + shmem_f32[tiisg] = kvalues_iq4nl_f[tiisg%16]; + threadgroup_barrier(mem_flags::mem_threadgroup); + + float4 yl[4]; + float sumf[NR0]={0.f}; + + device const float * yb = y + ix * QK_K + ib * 32 + il * 8; + + uint32_t aux32[2]; + thread const uint8_t * q8 = (thread const uint8_t *)aux32; + + float4 qf1, qf2; + + // [TAG_MUL_MV_WEIRD] + for (int ibl = ix; ibl < nb && ibl < ns01; ibl += 2) { + device const float4 * y4 = (device const float4 *)yb; + yl[0] = y4[0]; + yl[1] = y4[4]; + yl[2] = y4[1]; + yl[3] = y4[5]; + + for (short row = 0; row < NR0; ++row) { + device const block_iq4_xs & xb = x[row*ns01 + ibl]; + device const uint32_t * q4 = (device const uint32_t *)(xb.qs + 16*ib + 8*il); + + float4 acc1 = {0.f}, acc2 = {0.f}; + + aux32[0] = (q4[0] ) & 0x0f0f0f0f; + aux32[1] = (q4[0] >> 4) & 0x0f0f0f0f; + qf1 = {shmem_f32[q8[0]], shmem_f32[q8[1]], shmem_f32[q8[2]], shmem_f32[q8[3]]}; + qf2 = {shmem_f32[q8[4]], shmem_f32[q8[5]], shmem_f32[q8[6]], shmem_f32[q8[7]]}; + acc1 += yl[0] * qf1; + acc2 += yl[1] * qf2; + + aux32[0] = (q4[1] ) & 0x0f0f0f0f; + aux32[1] = (q4[1] >> 4) & 0x0f0f0f0f; + qf1 = {shmem_f32[q8[0]], shmem_f32[q8[1]], shmem_f32[q8[2]], shmem_f32[q8[3]]}; + qf2 = {shmem_f32[q8[4]], shmem_f32[q8[5]], shmem_f32[q8[6]], shmem_f32[q8[7]]}; + acc1 += yl[2] * qf1; + acc2 += yl[3] * qf2; + + acc1 += acc2; + + const int ls = (((xb.scales_l[ib/2] >> 4*(ib%2)) & 0xf) | (((xb.scales_h >> 2*ib) & 3) << 4)) - 32; + sumf[row] += (float)xb.d * ls * (acc1[0] + acc1[1] + acc1[2] + acc1[3]); + } + + yb += 2 * QK_K; + } + + device float * dst_f32 = (device float *) dst + (uint64_t)im*args.ne0*args.ne1 + (uint64_t)r1*args.ne0; + + for (int row = 0; row < NR0 && first_row + row < args.ne0; ++row) { + float sum_all = simd_sum(sumf[row]); + if (tiisg == 0) { + dst_f32[first_row + row] = sum_all; + } + } +} + + + +template +void kernel_mul_mv_mxfp4_f32_impl( + args_t args, + device const char * src0, + device const char * src1, + device char * dst, + threadgroup char * shmem, + uint3 tgpig, + ushort tiisg, + ushort sgitg) { + const short NSG = FC_mul_mv_nsg; + + threadgroup float * shmem_f32 = (threadgroup float *) shmem; + + const int r0 = tgpig.x; + const int r1 = tgpig.y; + const int im = tgpig.z; + + const int first_row = (r0 * NSG + sgitg) * NR0; + + const uint i12 = im%args.ne12; + const uint i13 = im/args.ne12; + + const uint64_t offset0 = first_row*args.nb01 + (i12/args.r2)*args.nb02 + (i13/args.r3)*args.nb03; + const uint64_t offset1 = r1*args.nb11 + (i12 )*args.nb12 + (i13 )*args.nb13; + + device const block_mxfp4 * x = (device const block_mxfp4 *) (src0 + offset0); + device const float * y = (device const float *) (src1 + offset1); + + const int nb = args.ne00/QK_MXFP4; + const int ns01 = args.nb01/args.nb00; // this can be larger than nb for permuted src0 tensors + + const short ix = tiisg/2; // 0...15 + const short it = tiisg%2; // 0 or 1 + + shmem_f32[tiisg] = kvalues_mxfp4_f[tiisg%16]; + threadgroup_barrier(mem_flags::mem_threadgroup); + + float4 yl[4]; + float sumf[NR0]={0.f}; + + device const float * yb = y + ix*QK_MXFP4 + it*8; + + // note: just the check `ib < nb` is enough, but adding the redundant `&& ib < ns01` check makes the kernel a bit faster + // no idea why that is - needs some deeper investigation [TAG_MUL_MV_WEIRD] + for (int ib = ix; ib < nb && ib < ns01; ib += 16) { + device const float4 * y4 = (device const float4 *) yb; + + yl[0] = y4[0]; + yl[1] = y4[4]; + yl[2] = y4[1]; + yl[3] = y4[5]; + + FOR_UNROLL (short row = 0; row < NR0; row++) { + device const block_mxfp4 & xb = x[row*ns01 + ib]; + device const uint8_t * q2 = (device const uint8_t *)(xb.qs + 8*it); + + float4 acc1 = yl[0]*float4(shmem_f32[q2[0] & 0x0F], shmem_f32[q2[1] & 0x0F], shmem_f32[q2[2] & 0x0F], shmem_f32[q2[3] & 0x0F]); + float4 acc2 = yl[1]*float4(shmem_f32[q2[0] >> 4 ], shmem_f32[q2[1] >> 4 ], shmem_f32[q2[2] >> 4 ], shmem_f32[q2[3] >> 4 ]); + float4 acc3 = yl[2]*float4(shmem_f32[q2[4] & 0x0F], shmem_f32[q2[5] & 0x0F], shmem_f32[q2[6] & 0x0F], shmem_f32[q2[7] & 0x0F]); + float4 acc4 = yl[3]*float4(shmem_f32[q2[4] >> 4 ], shmem_f32[q2[5] >> 4 ], shmem_f32[q2[6] >> 4 ], shmem_f32[q2[7] >> 4 ]); + + acc1 = (acc1 + acc3) + (acc2 + acc4); + + sumf[row] += e8m0_to_fp32(xb.e) * ((acc1[0] + acc1[1]) + (acc1[2] + acc1[3])); + } + + yb += 16 * QK_MXFP4; + } + + device float * dst_f32 = (device float *) dst + (uint64_t)im*args.ne0*args.ne1 + (uint64_t)r1*args.ne0; + + for (int row = 0; row < NR0 && first_row + row < args.ne0; ++row) { + float sum_all = simd_sum(sumf[row]); + if (tiisg == 0) { + dst_f32[first_row + row] = sum_all; + } + } +} + + + +template +kernel void kernel_get_rows_q( + constant ggml_metal_kargs_get_rows & args, + device const void * src0, + device const void * src1, + device void * dst, + uint3 tgpig[[threadgroup_position_in_grid]], + ushort tiitg[[thread_index_in_threadgroup]], + ushort3 ntg [[threads_per_threadgroup]]) { + const int32_t iw0 = tgpig.x/args.ne10; + const int32_t i10 = tgpig.x%args.ne10; + const int32_t i11 = tgpig.y; + const int32_t i12 = tgpig.z; + + const int32_t r = ((const device int32_t *) ((const device char *) src1 + i12*args.nb12 + i11*args.nb11 + i10*args.nb10))[0]; + + const int32_t i02 = i11; + const int32_t i03 = i12; + + auto psrc = (device const block_q *) ((const device char *) src0 + i03*args.nb03 + i02*args.nb02 + r*args.nb01); + auto pdst = (device float4x4 *) (( device char *) dst + i12*args.nb3 + i11*args.nb2 + i10*args.nb1); + + for (int ind = iw0*ntg.x + tiitg; ind < args.ne00t;) { + float4x4 temp; + dequantize_func(psrc + ind/nl, ind%nl, temp); + pdst[ind] = temp; + + break; + } +} + +template +kernel void kernel_get_rows_f( + constant ggml_metal_kargs_get_rows & args, + device const void * src0, + device const void * src1, + device void * dst, + uint3 tgpig[[threadgroup_position_in_grid]], + ushort tiitg[[thread_index_in_threadgroup]], + ushort3 ntg [[threads_per_threadgroup]]) { + const int32_t iw0 = tgpig.x/args.ne10; + const int32_t i10 = tgpig.x%args.ne10; + const int32_t i11 = tgpig.y; + const int32_t i12 = tgpig.z; + + const int32_t r = ((const device int32_t *) ((const device char *) src1 + i12*args.nb12 + i11*args.nb11 + i10*args.nb10))[0]; + + const int32_t i02 = i11; + const int32_t i03 = i12; + + auto psrc = (const device T0 *) ((const device char *) src0 + i03*args.nb03 + i02*args.nb02 + r*args.nb01); + auto pdst = ( device T *) (( device char *) dst + i12*args.nb3 + i11*args.nb2 + i10*args.nb1); + + for (int ind = iw0*ntg.x + tiitg; ind < args.ne00t;) { + pdst[ind] = psrc[ind]; + + break; + } +} + +template +kernel void kernel_set_rows_q32( + constant ggml_metal_kargs_set_rows & args, + device const void * src0, + device const void * src1, + device float * dst, + uint3 tgpig[[threadgroup_position_in_grid]], + uint tiitg[[thread_index_in_threadgroup]], + uint3 tptg [[threads_per_threadgroup]]) { + const int32_t i03 = tgpig.z; + const int32_t i02 = tgpig.y; + + const int32_t i12 = i03%args.ne12; + const int32_t i11 = i02%args.ne11; + + const int32_t i01 = tgpig.x*tptg.y + tiitg/tptg.x; + if (i01 >= args.ne01) { + return; + } + + const int32_t i10 = i01; + const TI i1 = ((const device TI *) ((const device char *) src1 + i10*args.nb10 + i11*args.nb11 + i12*args.nb12))[0]; + + device block_q * dst_row = ( device block_q *) (( device char *) dst + i1*args.nb1 + i02*args.nb2 + i03*args.nb3); + const device float * src_row = (const device float *) ((const device char *) src0 + i01*args.nb01 + i02*args.nb02 + i03*args.nb03); + + for (int ind = tiitg%tptg.x; ind < args.nk0; ind += tptg.x) { + quantize_func(src_row + 32*ind, dst_row[ind]); + } +} + +template +kernel void kernel_set_rows_f( + constant ggml_metal_kargs_set_rows & args, + device const void * src0, + device const void * src1, + device float * dst, + uint3 tgpig[[threadgroup_position_in_grid]], + uint tiitg[[thread_index_in_threadgroup]], + uint3 tptg [[threads_per_threadgroup]]) { + const int32_t i03 = tgpig.z; + const int32_t i02 = tgpig.y; + + const int32_t i12 = i03%args.ne12; + const int32_t i11 = i02%args.ne11; + + const int32_t i01 = tgpig.x*tptg.y + tiitg/tptg.x; + if (i01 >= args.ne01) { + return; + } + + const int32_t i10 = i01; + const TI i1 = ((const device TI *) ((const device char *) src1 + i10*args.nb10 + i11*args.nb11 + i12*args.nb12))[0]; + + device T * dst_row = ( device T *) (( device char *) dst + i1*args.nb1 + i02*args.nb2 + i03*args.nb3); + const device float * src_row = (const device float *) ((const device char *) src0 + i01*args.nb01 + i02*args.nb02 + i03*args.nb03); + + for (int ind = tiitg%tptg.x; ind < args.nk0; ind += tptg.x) { + dst_row[ind] = (T) src_row[ind]; + } +} + + + +constant bool FC_mul_mm_bc_inp [[function_constant(FC_MUL_MM + 0)]]; +constant bool FC_mul_mm_bc_out [[function_constant(FC_MUL_MM + 1)]]; + +// each block_q contains 16*nl weights +template +kernel void kernel_mul_mm( + constant ggml_metal_kargs_mul_mm & args, + device const char * src0, + device const char * src1, + device char * dst, + threadgroup char * shmem [[threadgroup(0)]], + uint3 tgpig[[threadgroup_position_in_grid]], + ushort tiitg[[thread_index_in_threadgroup]], + ushort sgitg[[simdgroup_index_in_threadgroup]]) { + + threadgroup S0 * sa = (threadgroup S0 *)(shmem); + threadgroup S1 * sb = (threadgroup S1 *)(shmem + 4096); + +#ifdef GGML_METAL_HAS_TENSOR + threadgroup float * sc = (threadgroup float *)(shmem); +#endif + + constexpr int NR0 = 64; + constexpr int NR1 = 32; + + constexpr int NK = 32; + constexpr int NL0 = NK/16; + constexpr int NL1 = NK/8; + + const int im = tgpig.z; + const int r0 = tgpig.y*NR0; + const int r1 = tgpig.x*NR1; + + // if this block is of 64x32 shape or smaller + const short nr0 = (args.ne0 - r0 < NR0) ? (args.ne0 - r0) : NR0; + const short nr1 = (args.ne1 - r1 < NR1) ? (args.ne1 - r1) : NR1; + + // a thread shouldn't load data outside of the matrix + const short lr0 = ((short)tiitg/NL0) < nr0 ? ((short)tiitg/NL0) : nr0 - 1; // 0 .. 63 + const short lr1 = ((short)tiitg/NL1) < nr1 ? ((short)tiitg/NL1) : nr1 - 1; // 0 .. 31 + + const short il0 = (tiitg % NL0); + + short il = il0; + + const int i12 = im%args.ne12; + const int i13 = im/args.ne12; + + const uint64_t offset0 = (i12/args.r2)*args.nb02 + (i13/args.r3)*args.nb03; + const short offset1 = il0/nl; + + device const block_q * x = (device const block_q *)(src0 + args.nb01*(r0 + lr0) + offset0) + offset1; + + const short iy = 8*(tiitg % NL1); + + device const T1 * y = (device const T1 *)(src1 + + args.nb13*i13 + + args.nb12*i12 + + args.nb11*(r1 + lr1) + + args.nb10*iy); + +#ifndef GGML_METAL_HAS_TENSOR + S0_8x8 ma[4]; + S1_8x8 mb[2]; + + simdgroup_float8x8 mc[8]; + + for (short i = 0; i < 8; i++){ + mc[i] = make_filled_simdgroup_matrix(0.f); + } +#else + auto tA = tensor, tensor_inline>(sa, dextents(NK, NR0)); + auto tB = tensor, tensor_inline>(sb, dextents(NR1, NK )); + + mpp::tensor_ops::matmul2d< + mpp::tensor_ops::matmul2d_descriptor(NR1, NR0, NK, false, true, false, mpp::tensor_ops::matmul2d_descriptor::mode::multiply_accumulate), + execution_simdgroups<4>> mm; + + auto cT = mm.get_destination_cooperative_tensor(); +#endif + + for (int loop_k = 0; loop_k < args.ne00; loop_k += NK) { +#ifndef GGML_METAL_HAS_TENSOR + // load data and store to threadgroup memory + if (is_same::value && FC_mul_mm_bc_inp) { + threadgroup_barrier(mem_flags::mem_threadgroup); + + // no need for dequantization + for (short i = 0; i < 16; i++) { + const short sx = 2*il0 + i/8; + const short sy = (tiitg/NL0)/8; + + //const short lx = i%8; + //const short ly = (tiitg/NL0)%8; + const short lx = (tiitg/NL0)%8; + const short ly = i%8; + + const short ib = 8*sx + sy; + + *(sa + 64*ib + 8*ly + lx) = loop_k + 16*il + i < args.ne00 ? *((device T0 *) x + i) : 0; + } + } else { + S0_4x4 temp_a; + dequantize_func(x, il, temp_a); + + threadgroup_barrier(mem_flags::mem_threadgroup); + + FOR_UNROLL (short i = 0; i < 16; i++) { + const short sx = 2*il0 + i/8; + const short sy = (tiitg/NL0)/8; + + //const short lx = i%8; + //const short ly = (tiitg/NL0)%8; + const short lx = (tiitg/NL0)%8; + const short ly = i%8; + + const short ib = 8*sx + sy; + + // NOTE: this is massively slower.. WTF? + //sa[64*ib + 8*ly + lx] = temp_a[i/4][i%4]; + + *(sa + 64*ib + 8*ly + lx) = temp_a[i/4][i%4]; + } + } + + if (FC_mul_mm_bc_inp) { + for (short i = 0; i < 8; ++i) { + const short sx = (tiitg%NL1); + const short sy = (tiitg/NL1)/8; + + const short lx = i; + const short ly = (tiitg/NL1)%8; + //const short lx = (tiitg/NL1)%8; + //const short ly = i; + + const short ib = 4*sx + sy; + + *(sb + 64*ib + 8*ly + lx) = loop_k + iy + i < args.ne00 ? (S1) *((device T1 *) y + i) : 0; + } + } else { + const short sx = (tiitg%NL1); + const short sy = (tiitg/NL1)/8; + + //const short dx = sx; + //const short dy = sy; + + const short ly = (tiitg/NL1)%8; + + const short ib = 4*sx + sy; + + *(threadgroup S1_2x4 *)(sb + 64*ib + 8*ly) = (S1_2x4)(*((device T1_2x4 *) y)); + } +#else + // load data and store to threadgroup memory + if (is_same::value && FC_mul_mm_bc_inp) { + threadgroup_barrier(mem_flags::mem_threadgroup); + + // no need for dequantization + for (short i = 0; i < 16; i++) { + const short sx = 2*il0 + i/8; + const short sy = (tiitg/NL0)/8; + + const short lx = i%8; + const short ly = (tiitg/NL0)%8; + //const short lx = (tiitg/NL0)%8; + //const short ly = i%8; + + *(sa + NK*(8*sy + ly) + 8*sx + lx) = loop_k + 16*il + i < args.ne00 ? *((device T0 *) x + i) : 0; + } + } else { + S0_4x4 temp_a; + dequantize_func(x, il, temp_a); + + threadgroup_barrier(mem_flags::mem_threadgroup); + + FOR_UNROLL (short i = 0; i < 16; i++) { + const short sx = 2*il0 + i/8; + const short sy = (tiitg/NL0)/8; + + const short lx = i%8; + const short ly = (tiitg/NL0)%8; + //const short lx = (tiitg/NL0)%8; + //const short ly = i%8; + + *(sa + NK*(8*sy + ly) + 8*sx + lx) = temp_a[i/4][i%4]; + } + } + + if (FC_mul_mm_bc_inp) { + for (short i = 0; i < 8; ++i) { + const short sx = (tiitg%NL1); + const short sy = (tiitg/NL1)/8; + + const short lx = i; + const short ly = (tiitg/NL1)%8; + //const short lx = (tiitg/NL1)%8; + //const short ly = i; + + *(sb + NK*(8*sy + ly) + 8*sx + lx) = loop_k + iy + i < args.ne00 ? (S1) *((device T1 *) y + i) : 0; + } + } else { + const short sx = (tiitg%NL1); + const short sy = (tiitg/NL1)/8; + + //const short lx = i; + const short ly = (tiitg/NL1)%8; + //const short lx = (tiitg/NL1)%8; + //const short ly = i; + + *(threadgroup S1_2x4 *)(sb + NK*(8*sy + ly) + 8*sx) = (S1_2x4)(*((device T1_2x4 *) y)); + } +#endif + + il = (il + 2 < nl) ? il + 2 : il % 2; + x = (il < 2) ? x + (2 + nl - 1)/nl : x; + + y += NK; + + threadgroup_barrier(mem_flags::mem_threadgroup); + +#ifndef GGML_METAL_HAS_TENSOR + // load matrices from threadgroup memory and conduct outer products + threadgroup const S0 * lsma = (sa + 4*64*(sgitg%2)); + threadgroup const S1 * lsmb = (sb + 2*64*(sgitg/2)); + + FOR_UNROLL (short ik = 0; ik < NK/8; ik++) { + simdgroup_barrier(mem_flags::mem_none); + + FOR_UNROLL (short i = 0; i < 4; i++) { + simdgroup_load(ma[i], lsma + 64*i, 8, 0, false); + } + + simdgroup_barrier(mem_flags::mem_none); + + FOR_UNROLL (short i = 0; i < 2; i++) { + simdgroup_load(mb[i], lsmb + 64*i, 8, 0, false); + } + + simdgroup_barrier(mem_flags::mem_none); + + FOR_UNROLL (short i = 0; i < 8; i++){ + simdgroup_multiply_accumulate(mc[i], mb[i/4], ma[i%4], mc[i]); + } + + lsma += 8*64; + lsmb += 4*64; + } +#else + auto sA = tA.slice(0, 0); + auto sB = tB.slice(0, 0); + + mm.run(sB, sA, cT); +#endif + } + + if (!FC_mul_mm_bc_out || (r0 + NR0 <= args.ne0 && r1 + NR1 <= args.ne1)) { + // if no bounds checks on the output are needed, we can directly write to device memory +#ifdef GGML_METAL_HAS_TENSOR + device float * C = (device float *) dst + + r0 + \ + r1 * args.ne0 + im*args.ne1*args.ne0; + + auto tC = tensor, tensor_inline>(C, dextents(args.ne0, NR1)); + cT.store(tC); +#else + device float * C = (device float *) dst + + (r0 + 32*(sgitg & 1)) + \ + (r1 + 16*(sgitg >> 1)) * args.ne0 + im*args.ne1*args.ne0; + + for (short i = 0; i < 8; i++) { + simdgroup_store(mc[i], C + 8*(i%4) + 8*args.ne0*(i/4), args.ne0, 0, false); + } +#endif + } else { + // block is smaller than 64x32, we should avoid writing data outside of the matrix + threadgroup_barrier(mem_flags::mem_threadgroup); + + threadgroup float * temp_str = ((threadgroup float *) shmem) + 32*(sgitg&1) + (16*(sgitg >> 1))*NR0; + +#ifdef GGML_METAL_HAS_TENSOR + auto tC = tensor, tensor_inline>(sc, dextents(NR0, NR1)); + cT.store(tC); +#else + for (short i = 0; i < 8; i++) { + simdgroup_store(mc[i], temp_str + 8*(i%4) + 8*NR0*(i/4), NR0, 0, false); + } +#endif + + threadgroup_barrier(mem_flags::mem_threadgroup); + + if (sgitg == 0) { + for (int j = tiitg; j < nr1; j += NR1) { + device float * D = (device float *) dst + r0 + (r1 + j)*args.ne0 + im*args.ne1*args.ne0; + device float4 * D4 = (device float4 *) D; + + threadgroup float * C = temp_str + (j*NR0); + threadgroup float4 * C4 = (threadgroup float4 *) C; + + int i = 0; + for (; i < nr0/4; i++) { + *(D4 + i) = *(C4 + i); + } + + i *= 4; + for (; i < nr0; i++) { + *(D + i) = *(C + i); + } + } + } + } +} + +template // n_expert_used +kernel void kernel_mul_mm_id_map0( + constant ggml_metal_kargs_mul_mm_id_map0 & args, + device const char * src2, + device char * htpe, + device char * hids, + threadgroup char * shmem [[threadgroup(0)]], + ushort tpitg[[thread_position_in_threadgroup]], + ushort ntg[[threads_per_threadgroup]]) { + const short ide = tpitg; // expert id + + uint32_t n_all = 0; + + device int32_t * ids_i32 = (device int32_t *) hids + ide*args.ne21; + + for (int i21 = 0; i21 < args.ne21; i21 += ntg) { // n_tokens + if (i21 + tpitg < args.ne21) { + device const int32_t * src2_i32 = (device const int32_t *) (src2 + (i21 + tpitg)*args.nb21); + + threadgroup uint16_t * sids = (threadgroup uint16_t *) shmem + tpitg*ne20; + + #pragma unroll(ne20) + for (short i20 = 0; i20 < ne20; i20++) { + sids[i20] = src2_i32[i20]; + } + } + + threadgroup_barrier(mem_flags::mem_threadgroup); + + for (short t = 0; t < ntg; t++) { + if (i21 + t >= args.ne21) { + break; + } + + threadgroup const uint16_t * sids = (threadgroup const uint16_t *) shmem + t*ne20; + + short sel = 0; + #pragma unroll(ne20) + for (short i20 = 0; i20 < ne20; i20++) { + sel += (sids[i20] == ide)*(i20 + 1); + } + + ids_i32[n_all] = (i21 + t)*ne20 + sel - 1; + + n_all += sel > 0; + } + + threadgroup_barrier(mem_flags::mem_threadgroup); + } + + device uint32_t * tpe_u32 = (device uint32_t *) (htpe); + tpe_u32[ide] = n_all; +} + +typedef decltype(kernel_mul_mm_id_map0<1>) kernel_mul_mm_id_map0_t; + + +template +kernel void kernel_mul_mm_id( + constant ggml_metal_kargs_mul_mm_id & args, + device const char * src0, + device const char * src1, + device const char * htpe, + device const char * hids, + device char * dst, + threadgroup char * shmem [[threadgroup(0)]], + uint3 tgpig[[threadgroup_position_in_grid]], + ushort tiitg[[thread_index_in_threadgroup]], + ushort tiisg[[thread_index_in_simdgroup]], + ushort sgitg[[simdgroup_index_in_threadgroup]]) { + threadgroup S0 * sa = (threadgroup S0 *)(shmem); + threadgroup S1 * sb = (threadgroup S1 *)(shmem + 4096); + +#ifdef GGML_METAL_HAS_TENSOR + threadgroup float * sc = (threadgroup float *)(shmem); +#endif + + constexpr int NR0 = 64; + constexpr int NR1 = 32; + + constexpr int NK = 32; + constexpr int NL0 = NK/16; + constexpr int NL1 = NK/8; + + const int im = tgpig.z; // expert + const int r0 = tgpig.y*NR0; + const int r1 = tgpig.x*NR1; + + device const uint32_t * tpe_u32 = (device const uint32_t *) (htpe); + device const int32_t * ids_i32 = (device const int32_t *) (hids); + + const int32_t neh1 = tpe_u32[im]; + + if (r1 >= neh1) { + return; + } + + // if this block is of 64x32 shape or smaller + const short nr0 = (args.ne0 - r0 < NR0) ? (args.ne0 - r0) : NR0; + const short nr1 = ( neh1 - r1 < NR1) ? ( neh1 - r1) : NR1; + + // a thread shouldn't load data outside of the matrix + const short lr0 = ((short)tiitg/NL0) < nr0 ? ((short)tiitg/NL0) : nr0 - 1; // 0 .. 63 + const short lr1 = ((short)tiitg/NL1) < nr1 ? ((short)tiitg/NL1) : nr1 - 1; // 0 .. 31 + + const short il0 = (tiitg % NL0); + + short il = il0; + + const int id = ids_i32[im*args.ne21 + r1 + lr1]; + + const short i11 = (id % args.ne20) % args.ne11; + const short i12 = (id / args.ne20); + const short i13 = 0; + + const uint64_t offset0 = im*args.nb02 + i13*args.nb03; + const short offset1 = il0/nl; + + device const block_q * x = (device const block_q *)(src0 + args.nb01*(r0 + lr0) + offset0) + offset1; + + const short iy = 8*(tiitg % NL1); + + device const T1 * y = (device const T1 *)(src1 + + args.nb13*i13 + + args.nb12*i12 + + args.nb11*i11 + + args.nb10*iy); + +#ifndef GGML_METAL_HAS_TENSOR + S0_8x8 ma[4]; + S1_8x8 mb[2]; + + simdgroup_float8x8 mc[8]; + + for (short i = 0; i < 8; i++){ + mc[i] = make_filled_simdgroup_matrix(0.f); + } +#else + auto tA = tensor, tensor_inline>(sa, dextents(NK, NR0)); + auto tB = tensor, tensor_inline>(sb, dextents(NR1, NK )); + + mpp::tensor_ops::matmul2d< + mpp::tensor_ops::matmul2d_descriptor(NR1, NR0, NK, false, true, false, mpp::tensor_ops::matmul2d_descriptor::mode::multiply_accumulate), + execution_simdgroups<4>> mm; + + auto cT = mm.get_destination_cooperative_tensor(); +#endif + + for (int loop_k = 0; loop_k < args.ne00; loop_k += NK) { +#ifndef GGML_METAL_HAS_TENSOR + // load data and store to threadgroup memory + if (is_same::value && FC_mul_mm_bc_inp) { + threadgroup_barrier(mem_flags::mem_threadgroup); + + // no need for dequantization + for (short i = 0; i < 16; i++) { + const short sx = 2*il0 + i/8; + const short sy = (tiitg/NL0)/8; + + //const short lx = i%8; + //const short ly = (tiitg/NL0)%8; + const short lx = (tiitg/NL0)%8; + const short ly = i%8; + + const short ib = 8*sx + sy; + + *(sa + 64*ib + 8*ly + lx) = loop_k + 16*il + i < args.ne00 ? *((device T0 *) x + i) : 0; + } + } else { + S0_4x4 temp_a; + dequantize_func(x, il, temp_a); + + threadgroup_barrier(mem_flags::mem_threadgroup); + + FOR_UNROLL (short i = 0; i < 16; i++) { + const short sx = 2*il0 + i/8; + const short sy = (tiitg/NL0)/8; + + //const short lx = i%8; + //const short ly = (tiitg/NL0)%8; + const short lx = (tiitg/NL0)%8; + const short ly = i%8; + + const short ib = 8*sx + sy; + + // NOTE: this is massively slower.. WTF? + //sa[64*ib + 8*ly + lx] = temp_a[i/4][i%4]; + + *(sa + 64*ib + 8*ly + lx) = temp_a[i/4][i%4]; + } + } + + if (FC_mul_mm_bc_inp) { + for (short i = 0; i < 8; ++i) { + const short sx = (tiitg%NL1); + const short sy = (tiitg/NL1)/8; + + const short lx = i; + const short ly = (tiitg/NL1)%8; + //const short lx = (tiitg/NL1)%8; + //const short ly = i; + + const short ib = 4*sx + sy; + + *(sb + 64*ib + 8*ly + lx) = loop_k + iy + i < args.ne00 ? (S1) *((device T1 *) y + i) : 0; + } + } else { + const short sx = (tiitg%NL1); + const short sy = (tiitg/NL1)/8; + + //const short dx = sx; + //const short dy = sy; + + const short ly = (tiitg/NL1)%8; + + const short ib = 4*sx + sy; + + *(threadgroup S1_2x4 *)(sb + 64*ib + 8*ly) = (S1_2x4)(*((device T1_2x4 *) y)); + } +#else + // load data and store to threadgroup memory + if (is_same::value && FC_mul_mm_bc_inp) { + threadgroup_barrier(mem_flags::mem_threadgroup); + + // no need for dequantization + for (short i = 0; i < 16; i++) { + const short sx = 2*il0 + i/8; + const short sy = (tiitg/NL0)/8; + + const short lx = i%8; + const short ly = (tiitg/NL0)%8; + //const short lx = (tiitg/NL0)%8; + //const short ly = i%8; + + *(sa + NK*(8*sy + ly) + 8*sx + lx) = loop_k + 16*il + i < args.ne00 ? *((device T0 *) x + i) : 0; + } + } else { + S0_4x4 temp_a; + dequantize_func(x, il, temp_a); + + threadgroup_barrier(mem_flags::mem_threadgroup); + + FOR_UNROLL (short i = 0; i < 16; i++) { + const short sx = 2*il0 + i/8; + const short sy = (tiitg/NL0)/8; + + const short lx = i%8; + const short ly = (tiitg/NL0)%8; + //const short lx = (tiitg/NL0)%8; + //const short ly = i%8; + + *(sa + NK*(8*sy + ly) + 8*sx + lx) = temp_a[i/4][i%4]; + } + } + + if (FC_mul_mm_bc_inp) { + for (short i = 0; i < 8; ++i) { + const short sx = (tiitg%NL1); + const short sy = (tiitg/NL1)/8; + + const short lx = i; + const short ly = (tiitg/NL1)%8; + //const short lx = (tiitg/NL1)%8; + //const short ly = i; + + *(sb + NK*(8*sy + ly) + 8*sx + lx) = loop_k + iy + i < args.ne00 ? (S1) *((device T1 *) y + i) : 0; + } + } else { + const short sx = (tiitg%NL1); + const short sy = (tiitg/NL1)/8; + + //const short lx = i; + const short ly = (tiitg/NL1)%8; + //const short lx = (tiitg/NL1)%8; + //const short ly = i; + + *(threadgroup S1_2x4 *)(sb + NK*(8*sy + ly) + 8*sx) = (S1_2x4)(*((device T1_2x4 *) y)); + } +#endif + + il = (il + 2 < nl) ? il + 2 : il % 2; + x = (il < 2) ? x + (2 + nl - 1)/nl : x; + + y += NK; + + threadgroup_barrier(mem_flags::mem_threadgroup); + +#ifndef GGML_METAL_HAS_TENSOR + // load matrices from threadgroup memory and conduct outer products + threadgroup const S0 * lsma = (sa + 4*64*(sgitg%2)); + threadgroup const S1 * lsmb = (sb + 2*64*(sgitg/2)); + + FOR_UNROLL (short ik = 0; ik < NK/8; ik++) { + simdgroup_barrier(mem_flags::mem_none); + + FOR_UNROLL (short i = 0; i < 4; i++) { + simdgroup_load(ma[i], lsma + 64*i, 8, 0, false); + } + + simdgroup_barrier(mem_flags::mem_none); + + FOR_UNROLL (short i = 0; i < 2; i++) { + simdgroup_load(mb[i], lsmb + 64*i, 8, 0, false); + } + + simdgroup_barrier(mem_flags::mem_none); + + FOR_UNROLL (short i = 0; i < 8; i++){ + simdgroup_multiply_accumulate(mc[i], mb[i/4], ma[i%4], mc[i]); + } + + lsma += 8*64; + lsmb += 4*64; + } +#else + auto sA = tA.slice(0, 0); + auto sB = tB.slice(0, 0); + + mm.run(sB, sA, cT); +#endif + } + + // block is smaller than 64x32, we should avoid writing data outside of the matrix + threadgroup_barrier(mem_flags::mem_threadgroup); + +#ifdef GGML_METAL_HAS_TENSOR + auto tC = tensor, tensor_inline>(sc, dextents(NR0, NR1)); + cT.store(tC); +#else + threadgroup float * temp_str = ((threadgroup float *) shmem) + 32*(sgitg&1) + (16*(sgitg >> 1))*NR0; + + for (short i = 0; i < 8; i++) { + simdgroup_store(mc[i], temp_str + 8*(i%4) + 8*NR0*(i/4), NR0, 0, false); + } +#endif + + threadgroup_barrier(mem_flags::mem_threadgroup); + + for (short j = sgitg; j < nr1; j += 4) { + const int id = ids_i32[im*args.ne21 + r1 + j]; + + const short ide = id % args.ne20; + const short idt = id / args.ne20; + + device float * D = (device float *) dst + r0 + ide*args.ne0 + idt*args.ne1*args.ne0; + device float4 * D4 = (device float4 *) D; + + threadgroup float * C = (threadgroup float *) shmem + j*NR0; + threadgroup float4 * C4 = (threadgroup float4 *) C; + + int i = tiisg; + for (; i < nr0/4; i += 32) { + *(D4 + i) = *(C4 + i); + } + + i = (4*(nr0/4)) + tiisg; + for (; i < nr0; i += 32) { + *(D + i) = *(C + i); + } + } +} + +#define QK_NL 16 + +// +// get rows +// + +typedef decltype(kernel_get_rows_f) get_rows_f_t; + +#if defined(GGML_METAL_HAS_BF16) +#endif + +typedef decltype(kernel_get_rows_q) get_rows_q_t; + + +// +// set rows +// + +typedef decltype(kernel_set_rows_f) set_rows_f_t; + +#if defined(GGML_METAL_HAS_BF16) +#endif + +typedef decltype(kernel_set_rows_q32) set_rows_q32_t; + + +// +// matrix-matrix multiplication +// + +typedef decltype(kernel_mul_mm) mul_mm_t; + +template [[host_name("kernel_mul_mm_f16_f32")]] kernel mul_mm_t kernel_mul_mm; +#if defined(GGML_METAL_HAS_BF16) +#endif + + +// +// indirect matrix-matrix multiplication +// + +typedef decltype(kernel_mul_mm_id) mul_mm_id; + +#if defined(GGML_METAL_HAS_BF16) +#endif + + +// +// matrix-vector multiplication +// + +typedef void (kernel_mul_mv_disp_t)( + ggml_metal_kargs_mul_mv args, + device const char * src0, + device const char * src1, + device char * dst, + uint3 tgpig, + ushort tiisg); + +typedef void (kernel_mul_mv2_disp_t)( + ggml_metal_kargs_mul_mv args, + device const char * src0, + device const char * src1, + device char * dst, + threadgroup char * shmem, + uint3 tgpig, + ushort tiisg, + ushort sgitg); + +template +void mmv_fn( + ggml_metal_kargs_mul_mv args, + device const char * src0, + device const char * src1, + device char * dst, + threadgroup char * shmem, + uint3 tgpig, + ushort tiitg, + ushort tiisg, + ushort sgitg) { + disp_fn(args, src0, src1, dst, tgpig, tiisg); +} + +template +void mmv_fn( + ggml_metal_kargs_mul_mv args, + device const char * src0, + device const char * src1, + device char * dst, + threadgroup char * shmem, + uint3 tgpig, + ushort tiitg, + ushort tiisg, + ushort sgitg) { + disp_fn(args, src0, src1, dst, shmem, tgpig, tiisg, sgitg); +} + +typedef decltype(mmv_fn>) mul_mv_disp_fn_t; + +template +kernel void kernel_mul_mv_id( + constant ggml_metal_kargs_mul_mv_id & args, + device const char * src0s, + device const char * src1, + device char * dst, + device const char * ids, + threadgroup char * shmem [[threadgroup(0)]], + uint3 tgpig[[threadgroup_position_in_grid]], + ushort tiitg[[thread_index_in_threadgroup]], + ushort tiisg[[thread_index_in_simdgroup]], + ushort sgitg[[simdgroup_index_in_threadgroup]]) { + const int iid1 = tgpig.z/args.nei0; + const int idx = tgpig.z%args.nei0; + + tgpig.z = 0; + + const int32_t i02 = ((device const int32_t *) (ids + iid1*args.nbi1))[idx]; + + const int64_t i11 = idx % args.ne11; + const int64_t i12 = iid1; + + const int64_t i1 = idx; + const int64_t i2 = i12; + + device const char * src0_cur = src0s + i02*args.nb02; + device const char * src1_cur = src1 + i11*args.nb11 + i12*args.nb12; + + device char * dst_cur = dst + (i1*args.ne0 + i2*args.ne1*args.ne0)*sizeof(float); + + ggml_metal_kargs_mul_mv args0 = { + /*.ne00 =*/ args.ne00, + /*.ne01 =*/ args.ne01, + /*.ne02 =*/ 1, // args.ne02, + /*.nb00 =*/ args.nb00, + /*.nb01 =*/ args.nb01, + /*.nb02 =*/ args.nb02, + /*.nb03 =*/ args.nb02, // args.ne02 == 1 + /*.ne10 =*/ args.ne10, + /*.ne11 =*/ 1, // args.ne11, + /*.ne12 =*/ 1, // args.ne12, + /*.nb10 =*/ args.nb10, + /*.nb11 =*/ args.nb11, + /*.nb12 =*/ args.nb12, + /*.nb13 =*/ args.nb12, // ne12 == 1 + /*.ne0 =*/ args.ne0, + /*.ne1 =*/ 1, // args.ne1, + /*.nr0 =*/ args.nr0, + /*.r2 =*/ 1, + /*.r3 =*/ 1, + }; + + disp_fn( + args0, + /* src0 */ src0_cur, + /* src1 */ src1_cur, + /* dst */ dst_cur, + shmem, + tgpig, + tiitg, + tiisg, + sgitg); +} + +typedef decltype(kernel_mul_mv_id>>) kernel_mul_mv_id_t; + +typedef decltype(kernel_mul_mv_id>>) kernel_mul_mv_id_4_t; + +#if defined(GGML_METAL_HAS_BF16) +#endif +#if defined(GGML_METAL_HAS_BF16) +#endif + + + + + + + + + + + + + + + + + + +template +kernel void kernel_memset( + constant ggml_metal_kargs_memset & args, + device T * dst, + uint tpig[[thread_position_in_grid]]) { + dst[tpig] = args.val; +} + +typedef decltype(kernel_memset) kernel_memset_t; + + +constant short FC_count_equal_nsg [[function_constant(FC_COUNT_EQUAL + 0)]]; + +template +kernel void kernel_count_equal( + constant ggml_metal_kargs_count_equal & args, + device const char * src0, + device const char * src1, + device atomic_int * dst, + threadgroup int32_t * shmem_i32 [[threadgroup(0)]], + uint3 tgpig[[threadgroup_position_in_grid]], + ushort3 tpitg[[thread_position_in_threadgroup]], + ushort sgitg[[simdgroup_index_in_threadgroup]], + ushort tiisg[[thread_index_in_simdgroup]], + ushort3 ntg[[threads_per_threadgroup]]) { + const short NSG = FC_count_equal_nsg; + + const int i3 = tgpig.z; + const int i2 = tgpig.y; + const int i1 = tgpig.x; + + if (i3 >= args.ne03 || i2 >= args.ne02 || i1 >= args.ne01) { + return; + } + + int sum = 0; + + device const char * base0 = src0 + i1*args.nb01 + i2*args.nb02 + i3*args.nb03; + device const char * base1 = src1 + i1*args.nb11 + i2*args.nb12 + i3*args.nb13; + + for (int64_t i0 = tpitg.x; i0 < args.ne00; i0 += ntg.x) { + const T v0 = *(device const T *)(base0 + i0*args.nb00); + const T v1 = *(device const T *)(base1 + i0*args.nb10); + sum += (v0 == v1); + } + + sum = simd_sum(sum); + + if (tiisg == 0) { + shmem_i32[sgitg] = sum; + } + + threadgroup_barrier(mem_flags::mem_threadgroup); + + if (sgitg == 0) { + float v = 0.0f; + if (tpitg.x < NSG) { + v = shmem_i32[tpitg.x]; + } + + float total = simd_sum(v); + if (tpitg.x == 0) { + atomic_fetch_add_explicit(dst, (int32_t) total, memory_order_relaxed); + } + } +} + +typedef decltype(kernel_count_equal) kernel_count_equal_t; + diff --git a/libs/voice2/src/settings.rs b/libs/voice2/src/settings.rs new file mode 100644 index 0000000000..64ec0704f4 --- /dev/null +++ b/libs/voice2/src/settings.rs @@ -0,0 +1,26 @@ +// Runtime settings for the turbo-focused path. +// Keep these as in-code constants (no runtime env toggles). + +// Prefer Metal on Apple platforms, CPU elsewhere. +pub(crate) const USE_METAL_BACKEND: bool = cfg!(target_os = "macos"); + +// Preserve raw ggml weight types so Metal kernels can consume packed formats directly. +pub(crate) const PRESERVE_RAW_WEIGHT_TYPE: bool = USE_METAL_BACKEND; + +// Encoder / matmul fast paths. +pub(crate) const ENABLE_METAL_QUANT: bool = true; +pub(crate) const ENABLE_ACT_Q8: bool = cfg!(target_arch = "aarch64"); + +// Debug logging gates. +#[allow(dead_code)] +pub(crate) const LOG_METAL_MUL_MAT: bool = false; +#[allow(dead_code)] +pub(crate) const LOG_METAL_PIPELINES: bool = false; + +// Keep ggml-metal feature gates at default behavior. +#[allow(dead_code)] +pub(crate) const DISABLE_GGML_METAL_BF16: bool = false; +#[allow(dead_code)] +pub(crate) const DISABLE_GGML_METAL_TENSOR: bool = false; +#[allow(dead_code)] +pub(crate) const FORCE_ENABLE_GGML_METAL_TENSOR: bool = false; diff --git a/libs/voice2/src/transcriber.rs b/libs/voice2/src/transcriber.rs new file mode 100644 index 0000000000..5a0f96f559 --- /dev/null +++ b/libs/voice2/src/transcriber.rs @@ -0,0 +1,298 @@ +use crate::{Segment, WhisperModel, WhisperParams, WhisperState}; +#[allow(unused_imports)] +use std::path::Path; + +const DEFAULT_MODEL_PATH: &str = "ggml-large-v3-turbo.bin"; + +#[derive(Clone, Copy, Debug, Eq, PartialEq)] +pub enum VoiceBackendKind { + Whisper, + NativeApple, +} + +impl VoiceBackendKind { + pub fn default_for_platform() -> Self { + #[cfg(any( + all(target_os = "ios", not(force_whisper)), + all(target_os = "macos", not(force_whisper)) + ))] + { + Self::NativeApple + } + #[cfg(not(any( + all(target_os = "ios", not(force_whisper)), + all(target_os = "macos", not(force_whisper)) + )))] + { + Self::Whisper + } + } + + pub fn from_makepad_env() -> Self { + #[cfg(all(target_os = "ios", not(force_whisper)))] + { + return Self::NativeApple; + } + + if std::env::var("MAKEPAD").ok().is_some_and(|configs| { + configs + .split(['+', ',']) + .any(|config| config.eq_ignore_ascii_case("whisper")) + }) { + return Self::Whisper; + } + Self::default_for_platform() + } +} + +#[derive(Clone, Debug)] +pub struct VoiceTranscribeParams { + pub language: String, + pub translate: bool, + pub include_timestamps: bool, + pub single_segment: bool, + pub max_tokens: usize, + pub silence_threshold: f32, + pub suppress_blank: bool, + pub temperature: f32, +} + +impl Default for VoiceTranscribeParams { + fn default() -> Self { + let whisper = WhisperParams::default(); + Self { + language: whisper.language, + translate: whisper.translate, + include_timestamps: !whisper.no_timestamps, + single_segment: whisper.single_segment, + max_tokens: whisper.max_tokens, + silence_threshold: whisper.no_speech_thold, + suppress_blank: whisper.suppress_blank, + temperature: whisper.temperature, + } + } +} + +impl VoiceTranscribeParams { + pub fn for_live_dictation() -> Self { + let mut out = Self::default(); + out.include_timestamps = false; + out.single_segment = true; + out.max_tokens = 48; + out.silence_threshold = 0.65; + out.suppress_blank = true; + out.temperature = 0.0; + out + } + + fn to_whisper_params(&self) -> WhisperParams { + let mut out = WhisperParams::default(); + out.language = self.language.clone(); + out.translate = self.translate; + out.no_timestamps = !self.include_timestamps; + out.single_segment = self.single_segment; + out.max_tokens = self.max_tokens; + out.no_speech_thold = self.silence_threshold; + out.suppress_blank = self.suppress_blank; + out.temperature = self.temperature; + out + } +} + +#[derive(Debug)] +pub enum VoiceTranscribeError { + BackendUnavailable(&'static str), + ModelLoadFailed(String), + BackendFailed(&'static str), +} + +pub struct WhisperTranscriber { + model_path: String, + model: Option, + state: Option, + model_load_failed: bool, +} + +impl WhisperTranscriber { + fn model_path_from_env() -> String { + std::env::var("MAKEPAD_VOICE_MODEL").unwrap_or_else(|_| DEFAULT_MODEL_PATH.to_string()) + } + + pub fn new_from_env() -> Self { + Self { + model_path: Self::model_path_from_env(), + model: None, + state: None, + model_load_failed: false, + } + } + + fn ensure_loaded(&mut self) -> Result<(), VoiceTranscribeError> { + if self.model.is_some() && self.state.is_some() { + return Ok(()); + } + if self.model_load_failed { + return Err(VoiceTranscribeError::ModelLoadFailed( + self.model_path.clone(), + )); + } + match WhisperModel::load_file(&self.model_path) { + Ok(model) => { + self.state = Some(WhisperState::new(&model)); + self.model = Some(model); + Ok(()) + } + Err(_) => { + self.model_load_failed = true; + Err(VoiceTranscribeError::ModelLoadFailed( + self.model_path.clone(), + )) + } + } + } + + pub fn preload(&mut self, _params: &VoiceTranscribeParams) -> Result<(), VoiceTranscribeError> { + self.ensure_loaded() + } + + pub fn transcribe( + &mut self, + samples: &[f32], + params: &VoiceTranscribeParams, + ) -> Result, VoiceTranscribeError> { + self.ensure_loaded()?; + let whisper = params.to_whisper_params(); + match (self.model.as_ref(), self.state.as_mut()) { + (Some(model), Some(state)) => Ok(state.transcribe(model, samples, &whisper)), + _ => Err(VoiceTranscribeError::BackendFailed( + "whisper backend state unavailable", + )), + } + } +} + +pub struct NativeAppleTranscriber; + +#[cfg(any( + all(target_os = "ios", not(force_whisper)), + all(target_os = "macos", not(force_whisper)) +))] +impl NativeAppleTranscriber { + pub fn new() -> Self { + Self + } + + pub fn preload(&mut self, params: &VoiceTranscribeParams) -> Result<(), VoiceTranscribeError> { + crate::apple_speech::ensure_model(¶ms.language) + .map_err(|_| VoiceTranscribeError::BackendFailed("apple ensure_model failed")) + } + + pub fn transcribe( + &mut self, + samples: &[f32], + params: &VoiceTranscribeParams, + ) -> Result, VoiceTranscribeError> { + Ok(crate::apple_speech::transcribe( + samples, + ¶ms.to_whisper_params(), + )) + } +} + +#[cfg(not(any( + all(target_os = "ios", not(force_whisper)), + all(target_os = "macos", not(force_whisper)) +)))] +impl NativeAppleTranscriber { + pub fn new() -> Self { + Self + } + + pub fn preload(&mut self, _params: &VoiceTranscribeParams) -> Result<(), VoiceTranscribeError> { + Err(VoiceTranscribeError::BackendUnavailable( + "native apple backend unavailable", + )) + } + + pub fn transcribe( + &mut self, + _samples: &[f32], + _params: &VoiceTranscribeParams, + ) -> Result, VoiceTranscribeError> { + Err(VoiceTranscribeError::BackendUnavailable( + "native apple backend unavailable", + )) + } +} + +pub enum VoiceTranscriber { + Whisper(WhisperTranscriber), + NativeApple(NativeAppleTranscriber), +} + +impl VoiceTranscriber { + pub fn new(kind: VoiceBackendKind) -> Self { + match kind { + VoiceBackendKind::Whisper => Self::Whisper(WhisperTranscriber::new_from_env()), + VoiceBackendKind::NativeApple => Self::NativeApple(NativeAppleTranscriber::new()), + } + } + + pub fn from_makepad_env() -> Self { + #[cfg(all(target_os = "ios", not(force_whisper)))] + { + return Self::NativeApple(NativeAppleTranscriber::new()); + } + + let kind = VoiceBackendKind::from_makepad_env(); + + #[cfg(all(target_os = "macos", not(force_whisper)))] + { + let model_path = WhisperTranscriber::model_path_from_env(); + let model_exists = Path::new(&model_path).exists(); + + if kind == VoiceBackendKind::Whisper { + if !model_exists { + eprintln!( + "[voice] whisper model not found at '{}', using native apple backend", + model_path + ); + return Self::NativeApple(NativeAppleTranscriber::new()); + } + return Self::Whisper(WhisperTranscriber::new_from_env()); + } + + // On Apple platforms, auto-prefer Whisper when the model is available. + if model_exists { + return Self::Whisper(WhisperTranscriber::new_from_env()); + } + } + + Self::new(kind) + } + + pub fn kind(&self) -> VoiceBackendKind { + match self { + Self::Whisper(_) => VoiceBackendKind::Whisper, + Self::NativeApple(_) => VoiceBackendKind::NativeApple, + } + } + + pub fn preload(&mut self, params: &VoiceTranscribeParams) -> Result<(), VoiceTranscribeError> { + match self { + Self::Whisper(inner) => inner.preload(params), + Self::NativeApple(inner) => inner.preload(params), + } + } + + pub fn transcribe( + &mut self, + samples: &[f32], + params: &VoiceTranscribeParams, + ) -> Result, VoiceTranscribeError> { + match self { + Self::Whisper(inner) => inner.transcribe(samples, params), + Self::NativeApple(inner) => inner.transcribe(samples, params), + } + } +} diff --git a/libs/voice2/swift/speech_bridge.swift b/libs/voice2/swift/speech_bridge.swift new file mode 100644 index 0000000000..1f3e8413a1 --- /dev/null +++ b/libs/voice2/swift/speech_bridge.swift @@ -0,0 +1,169 @@ +import Foundation +import Speech +import AVFoundation +import CoreMedia + +struct CSegment { + var text: UnsafeMutablePointer? + var start_ms: Int64 + var end_ms: Int64 +} + +func resolveLocale(_ langStr: String) -> Locale { + let defaults: [String: String] = [ + "en": "en-US", "fr": "fr-FR", "de": "de-DE", "es": "es-ES", + "it": "it-IT", "pt": "pt-BR", "zh": "zh-CN", "ja": "ja-JP", + "ko": "ko-KR", "yue": "yue-CN", + ] + return Locale(identifier: defaults[langStr] ?? langStr) +} + +func runAsyncSync(_ body: @escaping @Sendable () async throws -> T) throws -> T { + let sem = DispatchSemaphore(value: 0) + let box = UnsafeMutablePointer>.allocate(capacity: 1) + box.initialize(to: .failure(NSError(domain: "uninit", code: 0))) + DispatchQueue.global(qos: .userInitiated).async { + Task { + do { box.pointee = .success(try await body()) } + catch { box.pointee = .failure(error) } + sem.signal() + } + } + sem.wait() + let r = box.pointee + box.deallocate() + switch r { + case .success(let v): return v + case .failure(let e): throw e + } +} + +@_cdecl("apple_speech_transcribe") +func transcribe( + _ samples: UnsafePointer, + _ sampleCount: Int64, + _ lang: UnsafePointer, + _ outCount: UnsafeMutablePointer, + _ outSegments: UnsafeMutablePointer +) -> Int32 { + let langStr = String(cString: lang) + let requestedLocale = resolveLocale(langStr) + let count = Int(sampleCount) + + if count == 0 { + outCount.pointee = 0 + outSegments.pointee = nil + return 0 + } + + let samplesCopy = Array(UnsafeBufferPointer(start: samples, count: count)) + + do { + let segments: [(String, Int64, Int64)] = try runAsyncSync { + let locale = await SpeechTranscriber.supportedLocale(equivalentTo: requestedLocale) + ?? requestedLocale + let preset = SpeechTranscriber.Preset( + transcriptionOptions: [], + reportingOptions: [], + attributeOptions: [.audioTimeRange] + ) + let transcriber = SpeechTranscriber(locale: locale, preset: preset) + let analyzer = SpeechAnalyzer(modules: [transcriber]) + + let inputFormat = AVAudioFormat( + commonFormat: .pcmFormatFloat32, + sampleRate: 16000, channels: 1, interleaved: false + )! + + let tempURL = URL(fileURLWithPath: NSTemporaryDirectory()) + .appendingPathComponent("makepad_speech_\(UUID().uuidString).caf") + defer { try? FileManager.default.removeItem(at: tempURL) } + + let outFile = try AVAudioFile(forWriting: tempURL, settings: inputFormat.settings) + let allFrames = AVAudioFrameCount(count) + let outBuf = AVAudioPCMBuffer(pcmFormat: inputFormat, frameCapacity: allFrames)! + outBuf.frameLength = allFrames + samplesCopy.withUnsafeBufferPointer { buf in + memcpy(outBuf.floatChannelData![0], buf.baseAddress!, count * MemoryLayout.size) + } + try outFile.write(from: outBuf) + + let inFile = try AVAudioFile(forReading: tempURL) + try await analyzer.start(inputAudioFile: inFile, finishAfterFile: true) + var segs: [(String, Int64, Int64)] = [] + for try await response in transcriber.results { + let text = String(response.text.characters) + let isFinal = response.isFinal + if isFinal { + let range = response.range + let startMs = Int64(CMTimeGetSeconds(range.start) * 1000) + let endMs = Int64(CMTimeGetSeconds( + CMTimeAdd(range.start, range.duration)) * 1000) + if !text.isEmpty { segs.append((text, startMs, endMs)) } + } + } + return segs + } + + let n = Int32(segments.count) + outCount.pointee = n + if segments.isEmpty { + outSegments.pointee = nil + return 0 + } + let ptr = UnsafeMutablePointer.allocate(capacity: segments.count) + for (i, (text, startMs, endMs)) in segments.enumerated() { + ptr[i] = CSegment(text: strdup(text), start_ms: startMs, end_ms: endMs) + } + outSegments.pointee = OpaquePointer(ptr) + return 0 + } catch { + NSLog("[speech_bridge] transcribe ERROR: %@", error.localizedDescription) + outCount.pointee = 0 + outSegments.pointee = nil + return -1 + } +} + +@_cdecl("apple_speech_free_segments") +func freeSegments(_ ptr: OpaquePointer?, _ count: Int32) { + guard let rawPtr = ptr else { return } + let typed = UnsafeMutablePointer(rawPtr) + for i in 0..) -> Int32 { + let langStr = String(cString: lang) + let requestedLocale = resolveLocale(langStr) + + do { + try runAsyncSync { + let locale = await SpeechTranscriber.supportedLocale(equivalentTo: requestedLocale) + ?? requestedLocale + let supported = await SpeechTranscriber.supportedLocales + let bcp47 = locale.identifier(.bcp47) + guard supported.contains(where: { $0.identifier(.bcp47) == bcp47 }) else { + throw NSError(domain: "speech_bridge", code: -2, + userInfo: [NSLocalizedDescriptionKey: "Unsupported: \(bcp47)"]) + } + let installed = await SpeechTranscriber.installedLocales + if !installed.contains(where: { $0.identifier(.bcp47) == bcp47 }) { + let preset = SpeechTranscriber.Preset( + transcriptionOptions: [], reportingOptions: [], attributeOptions: []) + let t = SpeechTranscriber(locale: locale, preset: preset) + if let dl = try await AssetInventory.assetInstallationRequest(supporting: [t]) { + try await dl.downloadAndInstall() + } + } + } + return 0 + } catch let e as NSError where e.code == -2 { return -2 } + catch { + NSLog("[speech_bridge] ensureModel error: %@", error.localizedDescription) + return -1 + } +} diff --git a/makepad.splash b/makepad.splash index 2be16aae2e..fa285604c4 100644 --- a/makepad.splash +++ b/makepad.splash @@ -78,6 +78,7 @@ let RunStudioExt = RunItem{ hub.set_run_items([ RunStudioRelease{name: "makepad-example-browser"} RunStudioRelease{name: "makepad-example-charts"} + RunStudioRelease{name: "makepad-example-splash"} RunAndroid{name: "makepad-example-pdf"} RunStudioRelease{name: "makepad-example-map"} RunStudioRelease{name: "makepad-example-todo"} diff --git a/platform/Cargo.toml b/platform/Cargo.toml index d2262f889d..84bc4b7bd8 100644 --- a/platform/Cargo.toml +++ b/platform/Cargo.toml @@ -18,6 +18,7 @@ makepad-network = { path = "./network", version = "1.0.0", features = ["script"] makepad-script = { path="./script", version="1.0.0"} makepad-live-reload-core = { path = "../libs/live_reload_core", version = "0.1.0" } makepad-zune-png = { path = "../libs/zune/zune-png", version = "0.5.1" } +makepad-tsdf = { path = "../libs/tsdf", version = "0.1.0" } smallvec = { version = "1.15", path = "../libs/smallvec" } bitflags = { version = "2.10", path = "../libs/bitflags" } diff --git a/platform/script/src/mod_shader.rs b/platform/script/src/mod_shader.rs index 303d74e44c..3dfe5185a0 100644 --- a/platform/script/src/mod_shader.rs +++ b/platform/script/src/mod_shader.rs @@ -394,4 +394,93 @@ pub fn define_shader_module(heap: &mut ScriptHeap, native: &mut ScriptNative) { NIL }, ); + + native.add_method( + heap, + shader, + id_lut!(test_compile_draw_contains), + script_args!(io_self = NIL, needle = NIL), + |vm, args| { + let io_self = script_value!(vm, args.io_self); + let needle_val = script_value!(vm, args.needle); + let Some(needle) = vm.bx.heap.string_with(needle_val, |_heap, s| s.to_string()) else { + return ScriptValue::from_bool(false); + }; + + if let Some(io_self) = io_self.as_object() { + let mut output = ShaderOutput::default(); + output.backend = ShaderBackend::Metal; + output.use_vulkan = false; + + output.pre_collect_rust_instance_io(vm, io_self); + output.pre_collect_shader_io(vm, io_self); + + if let Some(fnobj) = vm + .bx + .heap + .object_method( + io_self, + id!(vertex).into(), + vm.bx.threads.cur_ref().trap.pass(), + ) + .as_object() + { + output.mode = ShaderMode::Vertex; + ShaderFnCompiler::compile_shader_def( + vm, + &mut output, + NoTrap, + id!(vertex), + fnobj, + ShaderType::IoSelf(io_self), + vec![], + ); + } + if let Some(fnobj) = vm + .bx + .heap + .object_method( + io_self, + id!(fragment).into(), + vm.bx.threads.cur_ref().trap.pass(), + ) + .as_object() + { + output.mode = ShaderMode::Fragment; + ShaderFnCompiler::compile_shader_def( + vm, + &mut output, + NoTrap, + id!(fragment), + fnobj, + ShaderType::IoSelf(io_self), + vec![], + ); + } + + output.assign_uniform_buffer_indices(&vm.bx.heap, 3); + + let mut out = String::new(); + output.create_struct_defs(vm, &mut out); + output.metal_create_instance_struct(vm, &mut out); + output.metal_create_uniform_struct(vm, &mut out); + output.metal_create_scope_uniform_struct(vm, &mut out); + output.metal_create_io_struct(vm, &mut out); + output.metal_create_varying_struct(vm, &mut out); + output.metal_create_vertex_buffer_struct(vm, &mut out); + output.metal_create_sampler_decls(&mut out); + output.metal_create_helpers(&mut out); + output.create_functions(&mut out); + output.metal_create_io_vertex_struct(vm, &mut out); + output.metal_create_io_framebuffer_struct(vm, &mut out); + output.metal_create_io_fragment_struct(vm, &mut out); + output.metal_create_vertex_fn(vm, &mut out); + output.metal_create_fragment_main_fn(vm, &mut out); + + return ScriptValue::from_bool(out.contains(&needle)); + } + + ScriptValue::from_bool(false) + }, + ); } diff --git a/platform/script/src/shader.rs b/platform/script/src/shader.rs index d5fbbb2124..79beab7070 100644 --- a/platform/script/src/shader.rs +++ b/platform/script/src/shader.rs @@ -1105,8 +1105,8 @@ impl ShaderFnCompiler { Opcode::MOD => self.handle_arithmetic(vm, output, opargs, "%", false), Opcode::ADD => self.handle_arithmetic(vm, output, opargs, "+", false), Opcode::SUB => self.handle_arithmetic(vm, output, opargs, "-", false), - Opcode::SHL => self.handle_arithmetic(vm, output, opargs, ">>", true), - Opcode::SHR => self.handle_arithmetic(vm, output, opargs, "<<", true), + Opcode::SHL => self.handle_arithmetic(vm, output, opargs, "<<", true), + Opcode::SHR => self.handle_arithmetic(vm, output, opargs, ">>", true), Opcode::AND => self.handle_arithmetic(vm, output, opargs, "&", true), Opcode::OR => self.handle_arithmetic(vm, output, opargs, "|", true), Opcode::XOR => self.handle_arithmetic(vm, output, opargs, "^", true), @@ -1138,10 +1138,10 @@ impl ShaderFnCompiler { self.handle_arithmetic_assign(vm, output, opargs, "^=", true); } Opcode::ASSIGN_SHL => { - self.handle_arithmetic_assign(vm, output, opargs, ">>=", true); + self.handle_arithmetic_assign(vm, output, opargs, "<<=", true); } Opcode::ASSIGN_SHR => { - self.handle_arithmetic_assign(vm, output, opargs, "<<=", true); + self.handle_arithmetic_assign(vm, output, opargs, ">>=", true); } Opcode::ASSIGN_IFNIL => { script_err_not_impl!(self.trap, "ASSIGN_IFNIL: null-coalescing assignment `x ??= default` not supported in shaders"); @@ -1173,10 +1173,10 @@ impl ShaderFnCompiler { self.handle_arithmetic_field_assign(vm, output, opargs, "^=", true); } Opcode::ASSIGN_FIELD_SHL => { - self.handle_arithmetic_field_assign(vm, output, opargs, ">>=", true); + self.handle_arithmetic_field_assign(vm, output, opargs, "<<=", true); } Opcode::ASSIGN_FIELD_SHR => { - self.handle_arithmetic_field_assign(vm, output, opargs, "<<=", true); + self.handle_arithmetic_field_assign(vm, output, opargs, ">>=", true); } Opcode::ASSIGN_FIELD_IFNIL => { script_err_not_impl!(self.trap, "ASSIGN_FIELD_IFNIL: null-coalescing field assignment `obj.x ??= default` not supported in shaders"); @@ -1208,10 +1208,10 @@ impl ShaderFnCompiler { self.handle_arithmetic_index_assign(vm, output, opargs, "^=", true); } Opcode::ASSIGN_INDEX_SHL => { - self.handle_arithmetic_index_assign(vm, output, opargs, ">>=", true); + self.handle_arithmetic_index_assign(vm, output, opargs, "<<=", true); } Opcode::ASSIGN_INDEX_SHR => { - self.handle_arithmetic_index_assign(vm, output, opargs, "<<=", true); + self.handle_arithmetic_index_assign(vm, output, opargs, ">>=", true); } Opcode::ASSIGN_INDEX_IFNIL => { script_err_not_impl!(self.trap, "ASSIGN_INDEX_IFNIL: null-coalescing index assignment `arr[i] ??= default` not supported in shaders"); diff --git a/platform/script/src/shader_builtins.rs b/platform/script/src/shader_builtins.rs index ec502f9657..eb6628f8b5 100644 --- a/platform/script/src/shader_builtins.rs +++ b/platform/script/src/shader_builtins.rs @@ -557,6 +557,84 @@ pub fn define_shader_builtins( |_vm, _args| ScriptValue::NIL, ); + // Bitcast helpers for shader code. These are primarily intended for shader use, + // but we provide scalar runtime behavior so expressions can still evaluate. + native.add_method( + heap, + math, + id_lut!(asuint), + script_args!(x = 0.0), + |vm, args| { + let x_val = vm + .bx + .heap + .value(args, id!(x).into(), vm.bx.threads.cur_ref().trap.pass()); + if let Some(v) = x_val.as_u32() { + return ScriptValue::from_u32(v); + } + if let Some(v) = x_val.as_i32() { + return ScriptValue::from_u32(v as u32); + } + if let Some(v) = x_val.as_f32() { + return ScriptValue::from_u32(v.to_bits()); + } + if let Some(v) = x_val.as_f16() { + return ScriptValue::from_u32(v.to_bits()); + } + let f = vm.bx.heap.cast_to_f64(x_val, vm.bx.threads.cur_ref().trap.ip) as f32; + ScriptValue::from_u32(f.to_bits()) + }, + ); + native.add_method( + heap, + math, + id_lut!(asint), + script_args!(x = 0.0), + |vm, args| { + let x_val = vm + .bx + .heap + .value(args, id!(x).into(), vm.bx.threads.cur_ref().trap.pass()); + if let Some(v) = x_val.as_i32() { + return ScriptValue::from_i32(v); + } + if let Some(v) = x_val.as_u32() { + return ScriptValue::from_i32(v as i32); + } + if let Some(v) = x_val.as_f32() { + return ScriptValue::from_i32(v.to_bits() as i32); + } + if let Some(v) = x_val.as_f16() { + return ScriptValue::from_i32(v.to_bits() as i32); + } + let f = vm.bx.heap.cast_to_f64(x_val, vm.bx.threads.cur_ref().trap.ip) as f32; + ScriptValue::from_i32(f.to_bits() as i32) + }, + ); + native.add_method( + heap, + math, + id_lut!(asfloat), + script_args!(x = 0.0), + |vm, args| { + let x_val = vm + .bx + .heap + .value(args, id!(x).into(), vm.bx.threads.cur_ref().trap.pass()); + if let Some(v) = x_val.as_f32() { + return ScriptValue::from_f32(v); + } + if let Some(v) = x_val.as_u32() { + return ScriptValue::from_f32(f32::from_bits(v)); + } + if let Some(v) = x_val.as_i32() { + return ScriptValue::from_f32(f32::from_bits(v as u32)); + } + let f = vm.bx.heap.cast_to_f64(x_val, vm.bx.threads.cur_ref().trap.ip) as f32; + ScriptValue::from_f32(f) + }, + ); + // 2 argument functions - support f64, Vec2f, Vec3f, Vec4f, Color native.add_method( heap, @@ -1026,6 +1104,69 @@ pub fn type_table_builtin( let is_any_int = |t| is_int(t) || is_vec_int(t); match name { + id!(asuint) => { + if args.len() != 1 { + script_err_invalid_args!( + trap, + "shader builtin 'asuint' requires 1 arg, got {}", + args.len() + ); + return builtins.pod_void; + } + return match args[0] { + t if t == f32_t || t == f16_t || t == u32_t => u32_t, + t => { + script_err_type_mismatch!( + trap, + "shader builtin 'asuint' requires scalar float arg, got {}", + fmt_ty(t) + ); + builtins.pod_void + } + }; + } + id!(asint) => { + if args.len() != 1 { + script_err_invalid_args!( + trap, + "shader builtin 'asint' requires 1 arg, got {}", + args.len() + ); + return builtins.pod_void; + } + return match args[0] { + t if t == f32_t || t == f16_t || t == i32_t => i32_t, + t => { + script_err_type_mismatch!( + trap, + "shader builtin 'asint' requires scalar float arg, got {}", + fmt_ty(t) + ); + builtins.pod_void + } + }; + } + id!(asfloat) => { + if args.len() != 1 { + script_err_invalid_args!( + trap, + "shader builtin 'asfloat' requires 1 arg, got {}", + args.len() + ); + return builtins.pod_void; + } + return match args[0] { + t if t == u32_t || t == i32_t || t == f32_t => f32_t, + t => { + script_err_type_mismatch!( + trap, + "shader builtin 'asfloat' requires scalar int arg, got {}", + fmt_ty(t) + ); + builtins.pod_void + } + }; + } // Float only 1 argument id!(acos) | id!(acosh) diff --git a/platform/script/src/shader_calls.rs b/platform/script/src/shader_calls.rs index 6b2392a520..05634031cf 100644 --- a/platform/script/src/shader_calls.rs +++ b/platform/script/src/shader_calls.rs @@ -1098,6 +1098,119 @@ impl ShaderFnCompiler { return; } + if name == id!(asuint) || name == id!(asint) || name == id!(asfloat) { + let mut concrete_args = Vec::new(); + let mut formatted_args = Vec::new(); + for (ty, s) in args { + concrete_args.push(ty.make_concrete(builtins).unwrap_or(builtins.pod_void)); + formatted_args.push(s); + } + + let arg = formatted_args.first().cloned().unwrap_or_default(); + let arg_ty = concrete_args.first().copied().unwrap_or(builtins.pod_void); + let mut out = self.stack.new_string(); + match name { + id!(asuint) => match output.backend { + ShaderBackend::Glsl => { + if arg_ty == builtins.pod_u32 { + write!(out, "{arg}").ok(); + } else { + write!(out, "floatBitsToUint({arg})").ok(); + } + } + ShaderBackend::Wgsl => { + if arg_ty == builtins.pod_u32 { + write!(out, "{arg}").ok(); + } else { + write!(out, "bitcast({arg})").ok(); + } + } + ShaderBackend::Hlsl => { + write!(out, "asuint({arg})").ok(); + } + ShaderBackend::Metal => { + write!(out, "as_type({arg})").ok(); + } + ShaderBackend::Rust => { + if arg_ty == builtins.pod_u32 { + write!(out, "{arg}").ok(); + } else { + write!(out, "({arg}).to_bits()").ok(); + } + } + }, + id!(asint) => match output.backend { + ShaderBackend::Glsl => { + if arg_ty == builtins.pod_i32 { + write!(out, "{arg}").ok(); + } else { + write!(out, "floatBitsToInt({arg})").ok(); + } + } + ShaderBackend::Wgsl => { + if arg_ty == builtins.pod_i32 { + write!(out, "{arg}").ok(); + } else { + write!(out, "bitcast({arg})").ok(); + } + } + ShaderBackend::Hlsl => { + write!(out, "asint({arg})").ok(); + } + ShaderBackend::Metal => { + write!(out, "as_type({arg})").ok(); + } + ShaderBackend::Rust => { + if arg_ty == builtins.pod_i32 { + write!(out, "{arg}").ok(); + } else { + write!(out, "(({arg}).to_bits() as i32)").ok(); + } + } + }, + id!(asfloat) => match output.backend { + ShaderBackend::Glsl => { + if arg_ty == builtins.pod_f32 || arg_ty == builtins.pod_f16 { + write!(out, "{arg}").ok(); + } else if arg_ty == builtins.pod_i32 { + write!(out, "intBitsToFloat({arg})").ok(); + } else { + write!(out, "uintBitsToFloat({arg})").ok(); + } + } + ShaderBackend::Wgsl => { + if arg_ty == builtins.pod_f32 || arg_ty == builtins.pod_f16 { + write!(out, "{arg}").ok(); + } else { + write!(out, "bitcast({arg})").ok(); + } + } + ShaderBackend::Hlsl => { + write!(out, "asfloat({arg})").ok(); + } + ShaderBackend::Metal => { + write!(out, "as_type({arg})").ok(); + } + ShaderBackend::Rust => { + if arg_ty == builtins.pod_f32 || arg_ty == builtins.pod_f16 { + write!(out, "{arg}").ok(); + } else { + write!(out, "f32::from_bits(({arg}) as u32)").ok(); + } + } + }, + _ => {} + } + + for s in formatted_args { + self.stack.free_string(s); + } + + let ret = type_table_builtin(name, &concrete_args, builtins, self.trap.pass()); + self.stack.push(self.trap.pass(), ShaderType::Pod(ret), out); + return; + } + // Check if any arg is a float type - if so, abstract ints should be floats let has_float = args.iter().any(|(ty, _)| match ty { ShaderType::Pod(pt) => vm.bx.heap.pod_types[pt.index as usize].ty.is_float_type(), diff --git a/platform/script/test/src/main.rs b/platform/script/test/src/main.rs index b5d595271e..4fbb1a88f4 100644 --- a/platform/script/test/src/main.rs +++ b/platform/script/test/src/main.rs @@ -1984,6 +1984,12 @@ pub fn main() { let ucount = self.u_count let uflags = self.u_flags let utime = self.uniforms.time + let bits_u = asuint(utime) + let bits_i = asint(utime) + let shifted_left = u32(1) << u32(3) + let shifted_bits = (bits_u >> u32(31)) | (bits_u & u32(7)) + let back_from_u = asfloat(bits_u) + let back_from_i = asfloat(bits_i) // ---- Texture sampling ---- let diffuse = self.tex_diffuse.sample(uv) @@ -2033,6 +2039,8 @@ pub fn main() { } } shader.test_compile_draw(shader_all_features) + assert(shader.test_compile_draw_contains(shader_all_features, "<< uint(3)")) + assert(shader.test_compile_draw_contains(shader_all_features, ">> uint(31)")) }; diff --git a/platform/src/app_main.rs b/platform/src/app_main.rs index a98840cbb7..6937f32572 100644 --- a/platform/src/app_main.rs +++ b/platform/src/app_main.rs @@ -16,6 +16,31 @@ pub fn should_run_stdin_loop_from_env() -> bool { }) } +#[cfg(headless)] +pub(crate) fn should_disable_headless_draw_from_args() -> bool { + std::env::args().any(|v| v == "--no-draw") +} + +#[cfg(headless)] +pub(crate) fn headless_draw_cycles_from_args() -> Option { + let mut args = std::env::args(); + while let Some(arg) = args.next() { + if let Some(value) = arg.strip_prefix("--draws=") { + if let Ok(draws) = value.parse::() { + return Some(draws.max(1)); + } + } + if arg == "--draws" { + if let Some(value) = args.next() { + if let Ok(draws) = value.parse::() { + return Some(draws.max(1)); + } + } + } + } + None +} + fn normalize_studio_http_from_studio_var(studio: &str) -> String { let studio = studio.trim().trim_end_matches('/'); if studio.is_empty() { diff --git a/platform/src/cx.rs b/platform/src/cx.rs index 9c5d3076ba..73617d15db 100644 --- a/platform/src/cx.rs +++ b/platform/src/cx.rs @@ -125,6 +125,10 @@ pub struct Cx { /// Display context for the main window, used by AdaptiveView pub display_context: DisplayContext, + /// When true, a script re-apply (LiveEdit) will be triggered on the next + /// event loop iteration to pick up changed dynamic values like safe area insets. + pub pending_script_reapply: bool, + pub debug: Debug, #[allow(dead_code)] @@ -431,6 +435,7 @@ impl Cx { performance_stats: Default::default(), display_context: Default::default(), + pending_script_reapply: false, widget_tree_dump_requests: Default::default(), widget_snapshot_requests: Default::default(), diff --git a/platform/src/cx_api.rs b/platform/src/cx_api.rs index 332ae6bb01..e5a600ca7c 100644 --- a/platform/src/cx_api.rs +++ b/platform/src/cx_api.rs @@ -11,7 +11,8 @@ use { event::keyboard::CharOffset, event::xr::XrAnchor, event::{ - video_playback::CameraPreviewMode, DragItem, NextFrame, Timer, Trigger, VideoSource, + video_playback::CameraPreviewMode, DragItem, NextFrame, Timer, Trigger, + VideoSource, }, gpu_info::GpuInfo, ime::TextInputConfig, @@ -37,6 +38,15 @@ pub enum OpenUrlInPlace { No, } +#[derive(Clone, Copy, Debug, Default, PartialEq, Eq, Hash)] +pub enum CxThreadPriority { + #[default] + Normal, + Utility, + Background, + Idle, +} + #[derive(Clone, Copy, Debug, PartialEq, Eq, Hash)] pub struct SystemBrowserId(pub LiveId); @@ -426,12 +436,34 @@ impl Cx { self.in_draw_event } + /// Updates the `mod.widgets.SAFE_INSET_PAD_*` values on the script heap + /// so that Splash code can reference them in widget definitions. + /// Requests a deferred re-application of all script/Splash widget definitions, + /// causing widgets to pick up updated values from the script heap. + /// The re-apply happens on the next event loop iteration (not synchronously), + /// to avoid re-entrancy issues when called from within an event handler. + pub fn request_script_reapply(&mut self) { + self.pending_script_reapply = true; + } + + pub fn update_safe_inset_script_values(&mut self, insets: crate::event::SafeAreaInsets) { + use makepad_script::trap::NoTrap; + let Some(vm) = self.script_vm.as_mut() else { + return; + }; + let widgets = vm.heap.module(id!(widgets)); + vm.heap.set_value(widgets, id!(SAFE_INSET_PAD_TOP).into(), insets.top.into(), NoTrap); + vm.heap.set_value(widgets, id!(SAFE_INSET_PAD_BOTTOM).into(), insets.bottom.into(), NoTrap); + vm.heap.set_value(widgets, id!(SAFE_INSET_PAD_LEFT).into(), insets.left.into(), NoTrap); + vm.heap.set_value(widgets, id!(SAFE_INSET_PAD_RIGHT).into(), insets.right.into(), NoTrap); + } + pub fn xr_capabilities(&self) -> &XrCapabilities { &self.xr_capabilities } - pub fn xr_depth_mesh(&self) -> crate::xr_depth_mesh::XrDepthMeshStore { - crate::xr_depth_mesh::xr_depth_mesh_store() + pub fn xr_tsdf(&self) -> crate::xr_tsdf::XrTsdfStore { + crate::xr_tsdf::xr_tsdf_store() } pub fn xr_render_scale(&self) -> Option { @@ -450,6 +482,14 @@ impl Cx { ::xr_effective_frame_rate_hz(self) } + pub fn set_thread_priority(priority: CxThreadPriority) { + #[cfg(target_os = "android")] + crate::os::linux::android::android::set_current_thread_priority(priority); + + #[cfg(not(target_os = "android"))] + let _ = priority; + } + pub fn get_ref(&self) -> CxRef { CxRef(self.self_ref.clone().unwrap()) } diff --git a/platform/src/display_context.rs b/platform/src/display_context.rs index 0960584bea..3add3b34cd 100644 --- a/platform/src/display_context.rs +++ b/platform/src/display_context.rs @@ -1,3 +1,4 @@ +use crate::event::SafeAreaInsets; use crate::Vec2d; const DEFAULT_MIN_DESKTOP_WIDTH: f64 = 860.; @@ -10,6 +11,9 @@ pub struct DisplayContext { pub updated_on_event_id: u64, /// The current screen size pub screen_size: Vec2d, + /// Safe area insets for the current window (non-zero on devices with notches, + /// rounded corners, home indicators, etc.) + pub safe_area_insets: SafeAreaInsets, } impl DisplayContext { diff --git a/platform/src/event/window.rs b/platform/src/event/window.rs index e4beedddde..9035277452 100644 --- a/platform/src/event/window.rs +++ b/platform/src/event/window.rs @@ -4,6 +4,18 @@ use { std::rc::Rc, }; + +/// Safe area insets describing regions of the screen that should not contain +/// interactive content (e.g., notch/Dynamic Island, home indicator, rounded corners). +/// Values are in logical points (not physical pixels). +#[derive(Clone, Copy, Debug, Default, PartialEq)] +pub struct SafeAreaInsets { + pub top: f64, + pub right: f64, + pub bottom: f64, + pub left: f64, +} + #[derive(Clone, Debug, Default, PartialEq)] pub struct WindowGeom { pub dpi_factor: f64, @@ -14,6 +26,34 @@ pub struct WindowGeom { pub position: Vec2d, pub inner_size: Vec2d, pub outer_size: Vec2d, + /// Safe area insets for this window (non-zero on devices with notches, + /// rounded corners, home indicators, etc.) + pub safe_area_insets: SafeAreaInsets, + /// Bounding box of the window-chrome buttons drawn by this window, in logical + /// pixels with a top-left origin at the top-left corner of the content view + /// (Y increases downward — Makepad's standard coordinate system). + /// + /// **Per-platform values:** + /// - **macOS** — bounding box of the three traffic-light buttons (close / + /// miniaturize / zoom), queried live from the OS via `standardWindowButton:`. + /// Buttons sit on the left side of the title bar. + /// - **Windows** — bounding box of the three Makepad-drawn caption buttons + /// (minimize / maximize / close), each 46 × 29 logical px, right-aligned at + /// the top of the caption bar. + /// - **Linux / Wayland with `custom_window_chrome`** — same button layout as + /// Windows (right-aligned, 138 × 29 logical px). + /// - **All other platforms** (X11 with WM decorations, LinuxDirect, Android, + /// iOS, Web, …) — zero rect, because the platform either provides its own + /// chrome outside the content area or has no title bar at all. + /// + /// **How to use this:** + /// When drawing custom content inside a caption bar (e.g., a title label, + /// search field, or toolbar), use this rect to determine which region is + /// occupied by chrome buttons so you can apply the necessary margins and avoid + /// overdrawing the buttons. On macOS the occupied region is on the left; on + /// Windows / Wayland it is on the right. A zero rect means there are no + /// chrome buttons to avoid. + pub window_chrome_buttons: Rect, } #[derive(Clone, Debug)] diff --git a/platform/src/event/xr.rs b/platform/src/event/xr.rs index 1472ff0bc1..10a232a367 100644 --- a/platform/src/event/xr.rs +++ b/platform/src/event/xr.rs @@ -7,6 +7,13 @@ use { std::{cell::Cell, rc::Rc}, }; +pub const XR_TOUCH_DOWN_FRONT: f32 = 6.0; +pub const XR_TOUCH_DOWN_BACK: f32 = -12.0; +pub const XR_TOUCH_CAPTURE_FRONT: f32 = 11.0; +// About 2cm behind the panel at XrView's default logical-pixel scale. +pub const XR_TOUCH_CAPTURE_BACK: f32 = -16.0; +pub const XR_TOUCH_REARM_FRONT: f32 = 28.0; + #[derive(Clone, Debug, Default, SerBin, DeBin)] pub struct XrController { pub grip_pose: Pose, @@ -93,7 +100,41 @@ pub struct XrHand { pub pinch: [u8; 4], } +#[derive(Clone, Copy, Debug, Default)] +pub struct XrFingerFistDebug { + pub bend_degrees: Option, + pub forward_extension_ratio: Option, + pub bend_ok: bool, + pub extension_ok: bool, + pub passes: bool, +} + +#[derive(Clone, Copy, Debug, Default)] +pub struct XrPalmDownDebug { + pub back_of_hand_up_angle_degrees: Option, + pub along_hand_vertical_degrees: Option, + pub across_hand_vertical_degrees: Option, + pub passes: bool, +} + impl XrHand { + pub const GRAB_ACTIVE: u8 = 1 << 5; + pub const GRAB_ACTIVE_THRESHOLD: f32 = 0.55; + pub const FIST_MIN_FINGER_BEND_DEGREES: f32 = 40.0; + pub const FIST_MAX_FINGER_FORWARD_EXTENSION_RATIO: f32 = 1.80; + pub const OPEN_MAX_FINGER_BEND_DEGREES: f32 = 30.0; + pub const OPEN_MAX_AVERAGE_FINGER_BEND_DEGREES: f32 = 17.0; + pub const OPEN_SYNC_MAX_UP_ANGLE_DEGREES: f32 = 60.0; + pub const OPEN_SYNC_MAX_ACROSS_VERTICAL_DEGREES: f32 = 80.0; + pub const PALM_DOWN_MAX_BACK_OF_HAND_UP_ANGLE_DEGREES: f32 = 95.0; + pub const PALM_DOWN_MAX_ALONG_HAND_VERTICAL_DEGREES: f32 = 70.0; + pub const PALM_DOWN_MAX_ACROSS_HAND_VERTICAL_DEGREES: f32 = 70.0; + const MIN_PALM_SPAN_METERS: f32 = 0.01; + const MAX_PALM_SPAN_METERS: f32 = 0.22; + const MAX_JOINT_DISTANCE_FROM_PALM_METERS: f32 = 0.28; + const MAX_FINGER_SEGMENT_LENGTH_METERS: f32 = 0.12; + const MAX_TIP_LENGTH_METERS: f32 = 0.10; + pub fn in_view(&self) -> bool { self.flags & Self::IN_VIEW != 0 } @@ -154,6 +195,403 @@ impl XrHand { self.pinch[Self::PINCH_STRENGTH_LITTLE] as f32 / u8::MAX as f32 } + pub fn grabbing(&self) -> bool { + self.tips_active & Self::GRAB_ACTIVE != 0 + } + + fn finite_joint_pose(&self, joint: usize) -> Option { + let pose = self.joints[joint]; + pose.is_finite().then_some(pose) + } + + pub fn tracking_pose(&self) -> Option { + if !self.in_view() { + return None; + } + let center = self.finite_joint_pose(Self::CENTER)?; + let wrist = self.finite_joint_pose(Self::WRIST)?.position; + let palm_span = (center.position - wrist).length(); + if !palm_span.is_finite() + || !(Self::MIN_PALM_SPAN_METERS..=Self::MAX_PALM_SPAN_METERS).contains(&palm_span) + { + return None; + } + Some(Pose::new( + center.orientation, + center.position * 0.78 + wrist * 0.22, + )) + } + + pub fn joint_pose_checked(&self, joint: usize) -> Option { + let pose = self.finite_joint_pose(joint)?; + if joint == Self::CENTER || joint == Self::WRIST { + self.tracking_pose()?; + return Some(pose); + } + let palm_center = self.tracking_pose()?.position; + let distance = (pose.position - palm_center).length(); + (distance.is_finite() && distance <= Self::MAX_JOINT_DISTANCE_FROM_PALM_METERS) + .then_some(pose) + } + + fn joint_position_near_palm(&self, joint: usize, palm_center: Vec3f) -> Option { + let position = self.finite_joint_pose(joint)?.position; + let distance = (position - palm_center).length(); + (distance.is_finite() && distance <= Self::MAX_JOINT_DISTANCE_FROM_PALM_METERS) + .then_some(position) + } + + fn tip_length_checked(&self, tip: usize) -> Option { + let length = self.tips[tip]; + (length.is_finite() && (0.0..=Self::MAX_TIP_LENGTH_METERS).contains(&length)) + .then_some(length) + } + + pub fn tip_pos_checked(&self, tip: usize) -> Option { + let palm_center = self.tracking_pose()?.position; + let knuckle = Self::END_KNUCKLES.get(tip).copied()?; + let knuckle_pose = self.finite_joint_pose(knuckle)?; + let tip_length = self.tip_length_checked(tip)?; + let tip_position = knuckle_pose + .to_mat4() + .transform_vec4(vec4(0.0, 0.0, -tip_length, 1.0)) + .to_vec3f(); + let distance = (tip_position - palm_center).length(); + (tip_position.is_finite() + && distance.is_finite() + && distance <= Self::MAX_JOINT_DISTANCE_FROM_PALM_METERS) + .then_some(tip_position) + } + + pub fn joint_chain_positions(&self, chain: &[usize]) -> Option> { + if chain.is_empty() { + return None; + } + let palm_center = self.tracking_pose()?.position; + let mut points = SmallVec::<[Vec3f; 4]>::new(); + for &joint in chain { + points.push(self.joint_position_near_palm(joint, palm_center)?); + } + for pair in points.windows(2) { + let segment = pair[1] - pair[0]; + let length = segment.length(); + if !length.is_finite() + || length <= 0.0001 + || length > Self::MAX_FINGER_SEGMENT_LENGTH_METERS + { + return None; + } + } + Some(points) + } + + pub fn finger_chain_positions_partial(&self, tip: usize) -> Option> { + let chain = Self::finger_chain(tip); + if chain.is_empty() { + return None; + } + let palm_center = self.tracking_pose()?.position; + let mut points = SmallVec::<[Vec3f; 4]>::new(); + for &joint in chain { + if let Some(position) = self.joint_position_near_palm(joint, palm_center) { + points.push(position); + } + } + if points.len() < 3 { + return None; + } + for pair in points.windows(2) { + let segment = pair[1] - pair[0]; + let length = segment.length(); + if !length.is_finite() + || length <= 0.0001 + || length > Self::MAX_FINGER_SEGMENT_LENGTH_METERS + { + return None; + } + } + Some(points) + } + + pub fn finger_chain_positions(&self, tip: usize) -> Option> { + let mut points = SmallVec::<[Vec3f; 5]>::new(); + points.extend(self.joint_chain_positions(Self::finger_chain(tip))?); + let tip_position = self.tip_pos_checked(tip)?; + let tail_length = (tip_position - *points.last()?).length(); + if !tail_length.is_finite() + || tail_length <= 0.0001 + || tail_length > Self::MAX_FINGER_SEGMENT_LENGTH_METERS + { + return None; + } + points.push(tip_position); + Some(points) + } + + pub fn finger_end_joint_position(&self, tip: usize) -> Option { + let preferred_joint = Self::END_KNUCKLES.get(tip).copied()?; + if let Some(position) = self + .joint_pose_checked(preferred_joint) + .map(|pose| pose.position) + { + return Some(position); + } + self.finger_chain_positions_partial(tip)?.last().copied() + } + + pub fn finger_max_bend_angle_degrees_joint_only(&self, tip: usize) -> Option { + let points = self.finger_chain_positions_partial(tip)?; + let mut max_angle = 0.0f32; + for triple in points.windows(3) { + let in_dir = Self::normalized_segment_direction(triple[0], triple[1])?; + let out_dir = Self::normalized_segment_direction(triple[1], triple[2])?; + let angle = in_dir.dot(out_dir).clamp(-1.0, 1.0).acos().to_degrees(); + max_angle = max_angle.max(angle); + } + Some(max_angle) + } + + pub fn finger_bend_degrees(&self, tip: usize) -> Option { + self.finger_max_bend_angle_degrees_joint_only(tip) + .map(|bend| bend.max(0.0)) + } + + fn finger_base_joint(tip: usize) -> Option { + match tip { + Self::INDEX_TIP => Some(Self::INDEX_BASE), + Self::MIDDLE_TIP => Some(Self::MIDDLE_BASE), + Self::RING_TIP => Some(Self::RING_BASE), + Self::LITTLE_TIP => Some(Self::LITTLE_BASE), + _ => None, + } + } + + fn fist_along_direction(&self) -> Option { + let center = self.joint_pose_checked(Self::CENTER)?.position; + let wrist = self.joint_pose_checked(Self::WRIST)?.position; + let along_hand = center - wrist; + (along_hand.length() > 1.0e-5).then_some(along_hand.normalize()) + } + + fn fist_palm_width(&self) -> Option { + let index_base = self.joint_pose_checked(Self::INDEX_BASE)?.position; + let little_base = self.joint_pose_checked(Self::LITTLE_BASE)?.position; + let width = (index_base - little_base).length(); + (width > 1.0e-5).then_some(width) + } + + fn fist_across_direction(&self) -> Option { + let index_base = self.joint_pose_checked(Self::INDEX_BASE)?.position; + let little_base = self.joint_pose_checked(Self::LITTLE_BASE)?.position; + let across_hand = index_base - little_base; + (across_hand.length() > 1.0e-5).then_some(across_hand.normalize()) + } + + fn back_of_hand_normal_for_palm_down(&self, is_left: bool) -> Option { + let along_hand = self.fist_along_direction()?; + let across_hand = if is_left { + self.joint_pose_checked(Self::INDEX_BASE)?.position + - self.joint_pose_checked(Self::LITTLE_BASE)?.position + } else { + self.joint_pose_checked(Self::LITTLE_BASE)?.position + - self.joint_pose_checked(Self::INDEX_BASE)?.position + }; + if across_hand.length() <= 1.0e-5 { + return None; + } + let back_of_hand = Vec3f::cross(across_hand.normalize(), along_hand); + (back_of_hand.length() > 1.0e-5).then_some(back_of_hand.normalize()) + } + + fn axis_vertical_degrees(axis: Vec3f) -> Option { + (axis.length() > 1.0e-5).then_some(axis.y.abs().clamp(0.0, 1.0).asin().to_degrees()) + } + + pub fn back_of_hand_up_angle_degrees(&self, is_left: bool) -> Option { + let back_of_hand = self.back_of_hand_normal_for_palm_down(is_left)?; + let up = vec3f(0.0, 1.0, 0.0); + Some(back_of_hand.dot(up).clamp(-1.0, 1.0).acos().to_degrees()) + } + + pub fn finger_fist_debug(&self, tip: usize) -> XrFingerFistDebug { + let Some(base_joint) = Self::finger_base_joint(tip) else { + return XrFingerFistDebug::default(); + }; + let (Some(along_hand), Some(palm_width)) = + (self.fist_along_direction(), self.fist_palm_width()) + else { + return XrFingerFistDebug::default(); + }; + let bend_degrees = self.finger_bend_degrees(tip); + let forward_extension_ratio = + self.finger_end_joint_position(tip) + .and_then(|end_position| { + self.joint_pose_checked(base_joint).map(|pose| { + let forward_extension = (end_position - pose.position).dot(along_hand); + forward_extension / palm_width + }) + }); + let bend_ok = bend_degrees.is_some_and(|bend| bend >= Self::FIST_MIN_FINGER_BEND_DEGREES); + let extension_ok = forward_extension_ratio + .is_some_and(|ext| ext <= Self::FIST_MAX_FINGER_FORWARD_EXTENSION_RATIO); + XrFingerFistDebug { + bend_degrees, + forward_extension_ratio, + bend_ok, + extension_ok, + passes: bend_ok && extension_ok, + } + } + + fn finger_passes_fist_gate(&self, tip: usize) -> bool { + self.finger_fist_debug(tip).passes + } + + pub fn is_fist(&self) -> bool { + if !self.in_view() { + return false; + } + [ + Self::INDEX_TIP, + Self::MIDDLE_TIP, + Self::RING_TIP, + Self::LITTLE_TIP, + ] + .into_iter() + .all(|tip| self.finger_passes_fist_gate(tip)) + } + + pub fn average_open_finger_bend_degrees(&self) -> Option { + let bends = [ + self.finger_bend_degrees(Self::INDEX_TIP)?, + self.finger_bend_degrees(Self::MIDDLE_TIP)?, + self.finger_bend_degrees(Self::RING_TIP)?, + self.finger_bend_degrees(Self::LITTLE_TIP)?, + ]; + Some(bends.into_iter().sum::() / bends.len() as f32) + } + + pub fn is_open(&self) -> bool { + if !self.in_view() { + return false; + } + let bends = [ + self.finger_bend_degrees(Self::INDEX_TIP), + self.finger_bend_degrees(Self::MIDDLE_TIP), + self.finger_bend_degrees(Self::RING_TIP), + self.finger_bend_degrees(Self::LITTLE_TIP), + ]; + if !bends.iter().all(|bend| { + bend.is_some_and(|bend| bend <= Self::OPEN_MAX_FINGER_BEND_DEGREES) + }) { + return false; + } + self.average_open_finger_bend_degrees().is_some_and(|average| { + average <= Self::OPEN_MAX_AVERAGE_FINGER_BEND_DEGREES + }) + } + + pub fn along_hand_up_angle_degrees(&self) -> Option { + let along_hand = self.fist_along_direction()?; + let up = vec3f(0.0, 1.0, 0.0); + Some(along_hand.dot(up).clamp(-1.0, 1.0).acos().to_degrees()) + } + + pub fn is_upright_for_box_sync(&self) -> bool { + self.along_hand_up_angle_degrees().is_some_and(|up_angle| { + self.fist_across_direction() + .and_then(Self::axis_vertical_degrees) + .is_some_and(|across_vertical| { + up_angle <= Self::OPEN_SYNC_MAX_UP_ANGLE_DEGREES + && across_vertical <= Self::OPEN_SYNC_MAX_ACROSS_VERTICAL_DEGREES + }) + }) + } + + pub fn palm_down_debug(&self, is_left: bool) -> XrPalmDownDebug { + let back_of_hand_up_angle_degrees = self.back_of_hand_up_angle_degrees(is_left); + let along_hand_vertical_degrees = self + .fist_along_direction() + .and_then(Self::axis_vertical_degrees); + let across_hand_vertical_degrees = self + .fist_across_direction() + .and_then(Self::axis_vertical_degrees); + let passes = back_of_hand_up_angle_degrees.is_some_and(|back_angle| { + along_hand_vertical_degrees.is_some_and(|along_angle| { + across_hand_vertical_degrees.is_some_and(|across_angle| { + back_angle <= Self::PALM_DOWN_MAX_BACK_OF_HAND_UP_ANGLE_DEGREES + && along_angle <= Self::PALM_DOWN_MAX_ALONG_HAND_VERTICAL_DEGREES + && across_angle <= Self::PALM_DOWN_MAX_ACROSS_HAND_VERTICAL_DEGREES + }) + }) + }); + XrPalmDownDebug { + back_of_hand_up_angle_degrees, + along_hand_vertical_degrees, + across_hand_vertical_degrees, + passes, + } + } + + pub fn is_palm_down(&self, is_left: bool) -> bool { + self.palm_down_debug(is_left).passes + } + + fn index_grab_release_metrics(&self) -> Option<(f32, f32, f32)> { + let points = self.finger_chain_positions(Self::INDEX_TIP)?; + let [base, knuckle1, knuckle2, knuckle3, tip] = + [points[0], points[1], points[2], points[3], points[4]]; + let seg0 = Self::normalized_segment_direction(base, knuckle1)?; + let seg1 = Self::normalized_segment_direction(knuckle1, knuckle2)?; + let seg2 = Self::normalized_segment_direction(knuckle2, knuckle3)?; + let seg3 = Self::normalized_segment_direction(knuckle3, tip)?; + + let chain_length = (knuckle1 - base).length() + + (knuckle2 - knuckle1).length() + + (knuckle3 - knuckle2).length() + + (tip - knuckle3).length(); + if chain_length <= 0.0001 { + return None; + } + + let direct_length = (tip - base).length(); + let segment_dots = [seg0.dot(seg1), seg1.dot(seg2), seg2.dot(seg3)]; + let max_bend_angle_degrees = segment_dots + .into_iter() + .map(|dot| dot.clamp(-1.0, 1.0).acos().to_degrees()) + .fold(0.0, f32::max); + let straightness = segment_dots.into_iter().fold(1.0, f32::min); + let extension_ratio = direct_length / chain_length; + Some((max_bend_angle_degrees, straightness, extension_ratio)) + } + + pub fn grab_intent(&self) -> bool { + if !self.grabbing() { + return false; + } + let Some((max_bend_angle_degrees, straightness, extension_ratio)) = + self.index_grab_release_metrics() + else { + return true; + }; + !(max_bend_angle_degrees <= 36.0 && straightness >= 0.78 && extension_ratio >= 0.90) + } + + pub fn pinch_anchor_pose(&self) -> Option { + let pinch_active = + self.pinch_index() || self.pinch_strength_index() >= Self::GRAB_ACTIVE_THRESHOLD; + if !pinch_active { + return None; + } + let palm_pose = self.tracking_pose()?; + let thumb_tip = self.tip_pos_checked(Self::THUMB_TIP)?; + let index_tip = self.tip_pos_checked(Self::INDEX_TIP)?; + Some(Pose::new( + palm_pose.orientation, + (thumb_tip + index_tip) * 0.5, + )) + } + pub const IN_VIEW: u8 = 1 << 0; pub const AIM_VALID: u8 = 1 << 1; pub const PINCH_INDEX: u8 = 1 << 2; @@ -310,21 +748,15 @@ impl XrHand { if !self.tip_active(tip) { return None; } - let chain = Self::finger_chain(tip); - if chain.len() < 2 { + let points = self.finger_chain_positions(tip)?; + if points.len() < 3 { return None; } let mut directions = SmallVec::<[Vec3f; 4]>::new(); - for pair in chain.windows(2) { - let a = self.joints[pair[0]].position; - let b = self.joints[pair[1]].position; - directions.push(Self::normalized_segment_direction(a, b)?); + for pair in points.windows(2) { + directions.push(Self::normalized_segment_direction(pair[0], pair[1])?); } - directions.push(Self::normalized_segment_direction( - self.joints[*chain.last()?].position, - self.tip_pos_for_index(tip), - )?); directions.windows(2).fold(None, |max_angle, pair| { let dot = pair[0].dot(pair[1]).clamp(-1.0, 1.0); @@ -342,6 +774,118 @@ impl XrHand { } } +#[cfg(test)] +mod local_event_tests { + use super::*; + + fn point_pose(base: Vec3f, z: f32) -> Pose { + Pose::new(Quat::default(), base + vec3f(0.0, 0.0, z)) + } + + fn make_pointing_hand() -> XrHand { + let base = vec3f(0.20, 1.22, -0.22); + let mut hand = XrHand::default(); + hand.flags = XrHand::IN_VIEW | XrHand::AIM_VALID; + hand.tips_active = XrHand::GRAB_ACTIVE | (1 << XrHand::INDEX_TIP); + hand.tips[XrHand::INDEX_TIP] = 0.038; + hand.joints[XrHand::CENTER] = Pose::new(Quat::default(), base + vec3f(0.0, -0.03, 0.03)); + hand.joints[XrHand::WRIST] = Pose::new(Quat::default(), base + vec3f(0.0, -0.05, 0.08)); + hand.joints[XrHand::INDEX_BASE] = point_pose(base, 0.0); + hand.joints[XrHand::INDEX_KNUCKLE1] = point_pose(base, -0.041); + hand.joints[XrHand::INDEX_KNUCKLE2] = point_pose(base + vec3f(0.001, 0.002, 0.0), -0.082); + hand.joints[XrHand::INDEX_KNUCKLE3] = point_pose(base + vec3f(0.002, 0.004, 0.0), -0.122); + hand + } + + fn make_index_pinch_hand() -> XrHand { + let base = vec3f(0.20, 1.22, -0.22); + let mut hand = XrHand::default(); + hand.flags = XrHand::IN_VIEW | XrHand::AIM_VALID | XrHand::PINCH_INDEX; + hand.tips_active = (1 << XrHand::THUMB_TIP) | (1 << XrHand::INDEX_TIP); + hand.tips[XrHand::THUMB_TIP] = 0.028; + hand.tips[XrHand::INDEX_TIP] = 0.034; + hand.pinch[XrHand::PINCH_STRENGTH_INDEX] = 220; + hand.joints[XrHand::CENTER] = Pose::new(Quat::default(), base + vec3f(0.0, -0.03, 0.03)); + hand.joints[XrHand::WRIST] = Pose::new(Quat::default(), base + vec3f(0.0, -0.05, 0.08)); + hand.joints[XrHand::THUMB_BASE] = + Pose::new(Quat::default(), base + vec3f(-0.020, -0.008, -0.010)); + hand.joints[XrHand::THUMB_KNUCKLE1] = + Pose::new(Quat::default(), base + vec3f(-0.010, 0.000, -0.030)); + hand.joints[XrHand::THUMB_KNUCKLE2] = + Pose::new(Quat::default(), base + vec3f(0.000, 0.008, -0.040)); + hand.joints[XrHand::INDEX_BASE] = point_pose(base + vec3f(0.008, 0.0, 0.0), -0.005); + hand.joints[XrHand::INDEX_KNUCKLE1] = point_pose(base + vec3f(0.010, 0.004, 0.0), -0.032); + hand.joints[XrHand::INDEX_KNUCKLE2] = point_pose(base + vec3f(0.012, 0.010, 0.0), -0.054); + hand.joints[XrHand::INDEX_KNUCKLE3] = point_pose(base + vec3f(0.013, 0.018, 0.0), -0.068); + hand + } + + fn make_curled_grab_hand() -> XrHand { + let base = vec3f(0.20, 1.22, -0.22); + let mut hand = XrHand::default(); + hand.flags = XrHand::IN_VIEW | XrHand::AIM_VALID; + hand.tips_active = XrHand::GRAB_ACTIVE | (1 << XrHand::INDEX_TIP); + hand.tips[XrHand::INDEX_TIP] = 0.030; + hand.joints[XrHand::CENTER] = Pose::new(Quat::default(), base + vec3f(0.0, -0.03, 0.03)); + hand.joints[XrHand::WRIST] = Pose::new(Quat::default(), base + vec3f(0.0, -0.05, 0.08)); + hand.joints[XrHand::INDEX_BASE] = point_pose(base, 0.0); + hand.joints[XrHand::INDEX_KNUCKLE1] = point_pose(base, -0.030); + hand.joints[XrHand::INDEX_KNUCKLE2] = + Pose::new(Quat::default(), base + vec3f(0.018, -0.012, -0.040)); + hand.joints[XrHand::INDEX_KNUCKLE3] = + Pose::new(Quat::default(), base + vec3f(0.034, -0.030, -0.032)); + hand + } + + fn make_sparse_tracking_hand() -> XrHand { + let base = vec3f(0.20, 1.22, -0.22); + let mut hand = XrHand::default(); + hand.flags = XrHand::IN_VIEW | XrHand::AIM_VALID; + hand.tips_active = XrHand::GRAB_ACTIVE; + hand.joints[XrHand::CENTER] = Pose::new(Quat::default(), base + vec3f(0.0, -0.03, 0.03)); + hand.joints[XrHand::WRIST] = Pose::new(Quat::default(), base + vec3f(0.0, -0.05, 0.08)); + hand.aim_pose = Pose::new(Quat::default(), base + vec3f(0.0, 0.0, -0.12)); + hand + } + + #[test] + fn grab_intent_releases_for_clearly_pointing_index_even_when_raw_grab_bit_is_set() { + let hand = make_pointing_hand(); + assert!(hand.grabbing()); + assert!(!hand.grab_intent()); + } + + #[test] + fn grab_intent_keeps_curled_hand_as_active_grab() { + let hand = make_curled_grab_hand(); + assert!(hand.grabbing()); + assert!(hand.grab_intent()); + } + + #[test] + fn finger_chain_positions_reject_sparse_default_joint_sample() { + let hand = make_sparse_tracking_hand(); + assert!(hand.tracking_pose().is_some()); + assert!(hand.finger_chain_positions(XrHand::INDEX_TIP).is_none()); + } + + #[test] + fn pinch_anchor_pose_uses_thumb_index_midpoint_instead_of_palm_center() { + let hand = make_index_pinch_hand(); + let pinch_pose = hand.pinch_anchor_pose().expect("pinch anchor should exist"); + let palm_pose = hand.tracking_pose().expect("tracking pose should exist"); + let thumb_tip = hand + .tip_pos_checked(XrHand::THUMB_TIP) + .expect("thumb tip should be valid"); + let index_tip = hand + .tip_pos_checked(XrHand::INDEX_TIP) + .expect("index tip should be valid"); + let expected_midpoint = (thumb_tip + index_tip) * 0.5; + assert!((pinch_pose.position - expected_midpoint).length() < 0.001); + assert!((pinch_pose.position - palm_pose.position).length() > 0.01); + } +} + #[derive(Clone, Debug)] pub struct XrFingerTip { pub index: usize, @@ -371,6 +915,13 @@ pub struct XrAnchor { } impl XrAnchor { + pub fn mirrored(&self) -> Self { + Self { + left: self.right, + right: self.left, + } + } + pub fn to_quat(&self) -> Quat { let mut forward = self.right - self.left; forward.y = 0.0; @@ -396,18 +947,34 @@ impl XrAnchor { pub fn mapping_to(&self, other: &XrAnchor) -> Mat4f { Mat4f::mul( - &self.to_pose().to_mat4().invert(), &other.to_pose().to_mat4(), + &self.to_pose().to_mat4().invert(), ) } } +#[derive(Clone, Copy, Debug, Default, SerBin, DeBin, PartialEq)] +pub enum XrSyncAnchorExtrema { + #[default] + Low, + High, +} + +#[derive(Clone, Copy, Debug, Default, SerBin, DeBin, PartialEq)] +pub struct XrSyncAnchor { + pub id: u32, + pub captured_at: f64, + pub extrema: XrSyncAnchorExtrema, + pub anchor: XrAnchor, +} + #[derive(Clone, Debug, Default, SerBin, DeBin)] pub struct XrState { pub time: f64, pub head_pose: Pose, pub order_counter: u8, pub anchor: Option, + pub sync_anchor: Option, pub left_controller: XrController, pub right_controller: XrController, pub left_hand: XrHand, @@ -420,6 +987,7 @@ impl XrState { time: (b.time - a.time) * f as f64 + a.time, head_pose: Pose::from_lerp(a.head_pose, b.head_pose, f), anchor: b.anchor, + sync_anchor: b.sync_anchor, left_controller: b.left_controller.clone(), right_controller: b.right_controller.clone(), left_hand: b.left_hand.clone(), @@ -435,12 +1003,7 @@ impl XrState { } pub fn scene_anchor_pose(&self) -> Option { - if let Some(_anchor) = &self.anchor { - // lets construct a pose from 2 positions - None - } else { - None - } + self.anchor.map(|anchor| anchor.to_pose()) } pub fn hands(&self) -> [&XrHand; 2] { @@ -487,11 +1050,6 @@ impl XrUpdateEvent { } impl XrLocalEvent { - const XR_TOUCH_DOWN_FRONT: f32 = 6.0; - const XR_TOUCH_DOWN_BACK: f32 = -12.0; - const XR_TOUCH_RELEASE_FRONT: f32 = 11.0; - const XR_TOUCH_RELEASE_BACK: f32 = -20.0; - const XR_TOUCH_REARM_FRONT: f32 = 28.0; const XR_TOUCH_ACTIVE_FINGER_MAX_BEND_DEGREES: f32 = 70.0; fn fingertip_slot(is_left: bool, index: usize) -> usize { @@ -537,9 +1095,10 @@ impl XrLocalEvent { if !hand.tip_active(index) { continue; } - let pos = inv - .transform_vec4(hand.tip_pos_for_index(index).to_vec4()) - .to_vec3f(); + let Some(tip_position) = hand.tip_pos_checked(index) else { + continue; + }; + let pos = inv.transform_vec4(tip_position.to_vec4()).to_vec3f(); finger_tips.push(XrFingerTip { index, is_left, @@ -557,15 +1116,11 @@ impl XrLocalEvent { } fn tip_is_touching_for_down(tip: &XrFingerTip) -> bool { - tip.active - && tip.touch_z <= Self::XR_TOUCH_DOWN_FRONT - && tip.touch_z >= Self::XR_TOUCH_DOWN_BACK + tip.active && tip.touch_z <= XR_TOUCH_DOWN_FRONT && tip.touch_z >= XR_TOUCH_DOWN_BACK } fn tip_is_touching_for_capture(tip: &XrFingerTip) -> bool { - tip.active - && tip.touch_z <= Self::XR_TOUCH_RELEASE_FRONT - && tip.touch_z >= Self::XR_TOUCH_RELEASE_BACK + tip.touch_z <= XR_TOUCH_CAPTURE_FRONT && tip.touch_z >= XR_TOUCH_CAPTURE_BACK } pub fn from_update_event(e: &XrUpdateEvent, mat: &Mat4f) -> XrLocalEvent { @@ -593,10 +1148,10 @@ impl XrLocalEvent { .iter() .find(|tip| tip.is_left == is_left && tip.index == index) { - if tip.touch_z > Self::XR_TOUCH_REARM_FRONT { + if tip.touch_z > XR_TOUCH_REARM_FRONT { cx.fingers.xr_poke_unlock(digit_id); } - if tip.active { + if tip.active || Self::tip_is_touching_for_capture(tip) { cx.fingers.cycle_hover_area(digit_id); } else { cx.fingers.remove_hover(digit_id); @@ -770,3 +1325,37 @@ impl XrLocalEvent { return Hit::Nothing; } } + +#[cfg(test)] +mod tests { + use super::*; + + fn tip(active: bool, touch_z: f32) -> XrFingerTip { + XrFingerTip { + index: XrHand::INDEX_TIP, + is_left: false, + active, + interactive: true, + pos: vec3f(0.0, 0.0, touch_z), + ray_dir: vec3f(0.0, 0.0, -1.0), + touch_z, + handled: Cell::new(Area::Empty), + } + } + + #[test] + fn xr_touch_down_still_requires_active_finger() { + assert!(XrLocalEvent::tip_is_touching_for_down(&tip(true, 0.0))); + assert!(!XrLocalEvent::tip_is_touching_for_down(&tip(false, 0.0))); + } + + #[test] + fn xr_touch_capture_allows_push_through_without_active_finger() { + assert!(XrLocalEvent::tip_is_touching_for_capture(&tip( + false, -15.0 + ))); + assert!(!XrLocalEvent::tip_is_touching_for_capture(&tip( + false, -18.0 + ))); + } +} diff --git a/platform/src/lib.rs b/platform/src/lib.rs index 037f353f86..fa983ea131 100644 --- a/platform/src/lib.rs +++ b/platform/src/lib.rs @@ -51,7 +51,7 @@ pub mod permission; mod texture; mod uniform_buffer; mod window; -mod xr_depth_mesh; +mod xr_tsdf; pub mod web_socket; @@ -73,6 +73,9 @@ pub mod display_context; mod app_main; pub use crate::app_main::{resolve_studio_http, should_run_stdin_loop_from_env}; pub use crate::cx_api::{can_play_type, CxSystemBrowser, SystemBrowserId}; +pub use crate::xr_tsdf::{ + XrDepthAlignHeightMap, XrTsdfCooperativeStepResult, XrTsdfCooperativeStepStats, +}; #[cfg(target_arch = "wasm32")] pub use makepad_wasm_bridge; @@ -102,8 +105,8 @@ pub use { audio::*, component::{ComponentInfo, ComponentRegistries, ComponentRegistry}, cursor::MouseCursor, - cx::{Cx, CxRef, OsType}, - cx_api::{AccessibilityUpdatePayload, CxOsApi, CxOsOp, OpenUrlInPlace}, + cx::{Cx, CxRef, LinuxWindowParams, OsType}, + cx_api::{AccessibilityUpdatePayload, CxOsApi, CxOsOp, CxThreadPriority, OpenUrlInPlace}, draw_list::{CxDrawCall, CxDrawItem, CxDrawListPool, CxRectArea, DrawList, DrawListId}, draw_matrix::DrawMatrix, draw_pass::{ @@ -161,8 +164,10 @@ pub use { VirtualKeyboardEvent, WindowCloseRequestedEvent, WindowClosedEvent, + SafeAreaInsets, WindowDragQueryEvent, WindowDragQueryResponse, + WindowGeom, WindowGeomChangeEvent, WindowMovedEvent, XrAnchor, @@ -216,20 +221,9 @@ pub use { ScriptWindowHandle, WindowBackdrop, WindowHandle, WindowIcon, WindowIconBuffer, WindowId, WindowVisuals, }, - xr_depth_mesh::{ - xr_depth_align_analyze_remote_to_local, - xr_depth_align_build_wall_feature_normal_histogram, - xr_depth_align_build_wall_normal_histogram, xr_depth_align_loopback_preview_solution, - xr_depth_align_solve_remote_to_local, xr_depth_align_test_markers, - xr_depth_align_transform_descriptor, ChunkKey, XrDepthAlignDebug, - XrDepthAlignDescriptor, XrDepthAlignPreview, XrDepthAlignSample, - XrDepthAlignSampleKind, XrDepthAlignSolution, XrDepthAlignSolveDiagnostic, - XrDepthAlignSolveOutcome, XrDepthAlignState, XrDepthAlignWallFeature, XrDepthMesh, - XrDepthMeshChunk, XrDepthMeshQuery, XrDepthMeshQueryCollider, - XrDepthMeshQueryColliderGeometry, XrDepthMeshQueryColliderRole, XrDepthMeshQueryHit, - XrDepthMeshQueryResolvedSurface, XrDepthMeshQueryResult, XrDepthMeshQuerySupportPlane, - XrDepthMeshState, XrDepthMeshStats, XrDepthMeshStore, XrDepthPlaneKind, - XrDepthPlanePatch, + xr_tsdf::{ + ChunkKey, SparseTsdGridReadSnapshot, SparseTsdReadChunk, TsdfPublishedSnapshot, + XrTsdfState, XrTsdfStats, XrTsdfStore, }, }, app_main::*, diff --git a/platform/src/os/apple/ios/ios.rs b/platform/src/os/apple/ios/ios.rs index 649be20f7c..003b2bfd6e 100644 --- a/platform/src/os/apple/ios/ios.rs +++ b/platform/src/os/apple/ios/ios.rs @@ -555,6 +555,12 @@ impl Cx { IosEvent::Init => { with_ios_app(|app| app.start_timer(0, 0.008, true)); self.start_studio_websocket_delayed(); + // Populate display_context and script heap with safe area insets + // BEFORE Startup, so app script_mod! definitions can use them. + let geom = with_ios_app(|app| app.last_window_geom.clone()); + self.display_context.screen_size = geom.inner_size; + self.display_context.safe_area_insets = geom.safe_area_insets; + self.update_safe_inset_script_values(geom.safe_area_insets); self.call_event_handler(&Event::Startup); self.redraw_all(); } @@ -567,7 +573,6 @@ impl Cx { self.call_event_handler(&Event::WindowLostFocus(window_id)); } IosEvent::WindowGeomChange(re) => { - // do this here because mac let window_id = CxWindowPool::id_zero(); let window = &mut self.windows[window_id]; window.window_geom = re.new_geom.clone(); @@ -793,6 +798,14 @@ impl Cx { } } + // If a script re-apply was requested (e.g., safe area insets changed + // on rotation), fire LiveEdit now that all event handlers have returned. + if self.pending_script_reapply { + self.pending_script_reapply = false; + self.call_event_handler(&Event::LiveEdit); + self.redraw_all(); + } + if self.any_passes_dirty() || self.need_redrawing() || self.new_next_frames.len() != 0 diff --git a/platform/src/os/apple/ios/ios_app.rs b/platform/src/os/apple/ios/ios_app.rs index aabc42c74f..dbfd3597fd 100644 --- a/platform/src/os/apple/ios/ios_app.rs +++ b/platform/src/os/apple/ios/ios_app.rs @@ -259,6 +259,10 @@ impl IosApp { let () = msg_send![mtk_view_obj, setUserInteractionEnabled: YES]; let () = msg_send![mtk_view_obj, setAutoResizeDrawable: YES]; let () = msg_send![mtk_view_obj, setMultipleTouchEnabled: YES]; + // UIViewAutoresizingFlexibleWidth (2) | UIViewAutoresizingFlexibleHeight (16) + // Ensures the view resizes with the window on rotation, which is + // required for safeAreaInsets to update correctly. + let () = msg_send![mtk_view_obj, setAutoresizingMask: 18u64]; let text_input_view: ObjcId = msg_send![get_ios_class_global().text_input_view, alloc]; let text_input_view: ObjcId = msg_send![text_input_view, initWithFrame: NSRect { @@ -377,8 +381,9 @@ impl IosApp { // Store the delegate for cleanup self.keyboard_observer_delegate = Some(textfield_dlg); - let () = msg_send![window_obj, addSubview: mtk_view_obj]; - + // Don't manually addSubview — setRootViewController manages the + // view controller's view in the window hierarchy, which is required + // for proper safe area inset propagation on device rotation. let () = msg_send![window_obj, setRootViewController: view_ctrl_obj]; self.view_controller = Some(view_ctrl_obj); let () = msg_send![window_obj, makeKeyAndVisible]; @@ -454,6 +459,24 @@ impl IosApp { screen_rect.size.height as f64, ); + // Query safe area insets from the MTKView (accounts for notch/Dynamic Island, + // home indicator, rounded corners, etc.) + let safe_area_insets = with_ios_app(|app| { + if let Some(mtk_view) = app.mtk_view { + unsafe { + let insets: UIEdgeInsets = msg_send![mtk_view, safeAreaInsets]; + crate::event::SafeAreaInsets { + top: insets.top, + right: insets.right, + bottom: insets.bottom, + left: insets.left, + } + } + } else { + crate::event::SafeAreaInsets::default() + } + }); + let new_geom = WindowGeom { xr_is_presenting: false, is_topmost: false, @@ -463,6 +486,7 @@ impl IosApp { outer_size: new_size, dpi_factor, position: dvec2(0.0, 0.0), + safe_area_insets, }; let first_draw = with_ios_app(|app| app.first_draw); diff --git a/platform/src/os/apple/ios/ios_delegates.rs b/platform/src/os/apple/ios/ios_delegates.rs index e2e2b44556..89f5f5ecaa 100644 --- a/platform/src/os/apple/ios/ios_delegates.rs +++ b/platform/src/os/apple/ios/ios_delegates.rs @@ -50,6 +50,21 @@ pub fn define_makepad_view_controller() -> *const Class { unsafe { *this.get_ivar("_prefersHomeIndicatorAutoHidden") } } + // Called by iOS when the safe area insets change (e.g., device rotation). + // At this point the view's safeAreaInsets reflect the new orientation, + // unlike draw_size_will_change which may fire before the update. + extern "C" fn view_safe_area_insets_did_change(_this: &Object, _: Sel) { + let should_call = IOS_APP + .try_with(|app| match app.try_borrow_mut() { + Ok(app_ref) => app_ref.is_some(), + Err(_) => false, + }) + .unwrap_or(false); + if should_call { + IosApp::check_window_geom(); + } + } + unsafe { decl.add_method( sel!(prefersStatusBarHidden), @@ -59,6 +74,10 @@ pub fn define_makepad_view_controller() -> *const Class { sel!(prefersHomeIndicatorAutoHidden), prefers_home_indicator_auto_hidden as extern "C" fn(&Object, Sel) -> BOOL, ); + decl.add_method( + sel!(viewSafeAreaInsetsDidChange), + view_safe_area_insets_did_change as extern "C" fn(&Object, Sel), + ); } decl.register() diff --git a/platform/src/os/apple/macos/macos_delegates.rs b/platform/src/os/apple/macos/macos_delegates.rs index 471a170284..6486003a27 100644 --- a/platform/src/os/apple/macos/macos_delegates.rs +++ b/platform/src/os/apple/macos/macos_delegates.rs @@ -218,26 +218,38 @@ pub fn define_macos_window_delegate() -> *const Class { //WindowDelegate::emit_event(state, WindowEvent::HoveredFileCancelled); } - // Invoked when entered fullscreen - extern "C" fn window_did_enter_fullscreen(this: &Object, _: Sel, _: ObjcId) { + // Fullscreen lifecycle: set `is_fullscreen` in the `will` callbacks so the + // widget layer can show/hide the caption bar at the START of the animation, + // not after it finishes. The `did` callbacks fire a second change event so + // the final geometry (after the animation) is also picked up. + + extern "C" fn window_will_enter_fullscreen(this: &Object, _: Sel, _: ObjcId) { let cw = get_cocoa_window(this); cw.is_fullscreen = true; cw.send_change_event(); } - // Invoked when before enter fullscreen - extern "C" fn window_will_enter_fullscreen(this: &Object, _: Sel, _: ObjcId) { - let _cw = get_cocoa_window(this); + extern "C" fn window_did_enter_fullscreen(this: &Object, _: Sel, _: ObjcId) { + let cw = get_cocoa_window(this); + cw.send_change_event(); } - // Invoked when exited fullscreen - extern "C" fn window_did_exit_fullscreen(this: &Object, _: Sel, _: ObjcId) { + extern "C" fn window_will_exit_fullscreen(this: &Object, _: Sel, _: ObjcId) { let cw = get_cocoa_window(this); cw.is_fullscreen = false; cw.send_change_event(); } - extern "C" fn window_did_fail_to_enter_fullscreen(_this: &Object, _: Sel, _: ObjcId) {} + extern "C" fn window_did_exit_fullscreen(this: &Object, _: Sel, _: ObjcId) { + let cw = get_cocoa_window(this); + cw.send_change_event(); + } + + extern "C" fn window_did_fail_to_enter_fullscreen(this: &Object, _: Sel, _: ObjcId) { + let cw = get_cocoa_window(this); + cw.is_fullscreen = false; + cw.send_change_event(); + } let superclass = class!(NSObject); let mut decl = ClassDecl::new("RenderWindowDelegate", superclass).unwrap(); @@ -317,6 +329,10 @@ pub fn define_macos_window_delegate() -> *const Class { sel!(windowWillEnterFullScreen:), window_will_enter_fullscreen as extern "C" fn(&Object, Sel, ObjcId), ); + decl.add_method( + sel!(windowWillExitFullScreen:), + window_will_exit_fullscreen as extern "C" fn(&Object, Sel, ObjcId), + ); decl.add_method( sel!(windowDidExitFullScreen:), window_did_exit_fullscreen as extern "C" fn(&Object, Sel, ObjcId), diff --git a/platform/src/os/apple/macos/macos_window.rs b/platform/src/os/apple/macos/macos_window.rs index 19ab126f10..94a34cc903 100644 --- a/platform/src/os/apple/macos/macos_window.rs +++ b/platform/src/os/apple/macos/macos_window.rs @@ -7,7 +7,7 @@ use { WindowClosedEvent, WindowDragQueryEvent, WindowDragQueryResponse, WindowGeom, WindowGeomChangeEvent, }, - makepad_math::Vec2d, + makepad_math::{Rect, Vec2d}, os::{ apple::apple_sys::*, apple::apple_util::str_to_nsstring, @@ -568,6 +568,67 @@ impl MacosWindow { with_macos_app(|app| app.time_now()) } + /// Returns the bounding box of all three macOS traffic-light buttons + /// (close / miniaturize / zoom) in Makepad's coordinate system + /// (top-left origin, Y increases downward). + /// + /// Returns `None` if any button is missing (e.g., borderless popup windows). + pub fn traffic_lights_geom(&self) -> Option { + unsafe { + let close: ObjcId = msg_send![self.window, standardWindowButton: 0u64]; + let miniaturize: ObjcId = msg_send![self.window, standardWindowButton: 1u64]; + let zoom: ObjcId = msg_send![self.window, standardWindowButton: 2u64]; + if close.is_null() || miniaturize.is_null() || zoom.is_null() { + return None; + } + let content_view: ObjcId = msg_send![self.window, contentView]; + let content_frame: NSRect = msg_send![content_view, frame]; + let h = content_frame.size.height; + + // Convert a button's NSRect from its superview's coord space to the + // content view's coord space, then flip to Makepad's top-left origin. + let ns_to_rect = |btn: ObjcId| -> (f64, f64, f64, f64) { + let superview: ObjcId = msg_send![btn, superview]; + let ns_frame: NSRect = msg_send![btn, frame]; + let ns_frame: NSRect = msg_send![ + content_view, + convertRect: ns_frame + fromView: superview + ]; + let top = h - (ns_frame.origin.y + ns_frame.size.height); + let left = ns_frame.origin.x; + let right = ns_frame.origin.x + ns_frame.size.width; + let bottom = h - ns_frame.origin.y; + (top, left, right, bottom) + }; + + let (t0, l0, r0, b0) = ns_to_rect(close); + let (t1, l1, r1, b1) = ns_to_rect(miniaturize); + let (t2, l2, r2, b2) = ns_to_rect(zoom); + + let top = t0.min(t1).min(t2); + let left = l0.min(l1).min(l2); + let right = r0.max(r1).max(r2); + let bottom = b0.max(b1).max(b2); + + // During a fullscreen transition the content view resizes before + // the buttons' superview repositions, so the Y-flip can place the + // buttons near the bottom of the view. Traffic-light buttons are + // always near the top of the window, so discard bogus geometry + // where they appear in the lower half of the content area. + if top > h * 0.5 { + return None; + } + + let rect = Rect { + pos: Vec2d { x: left, y: top }, + size: Vec2d { x: right - left, y: bottom - top }, + }; + + Some(rect) + } + } + pub fn get_window_geom(&self) -> WindowGeom { WindowGeom { xr_is_presenting: false, @@ -578,6 +639,8 @@ impl MacosWindow { outer_size: self.get_outer_size(), dpi_factor: self.get_dpi_factor(), position: self.get_position(), + window_chrome_buttons: self.traffic_lights_geom().unwrap_or_default(), + ..Default::default() } } diff --git a/platform/src/os/apple/tvos/tvos_app.rs b/platform/src/os/apple/tvos/tvos_app.rs index 3618e4b0a0..c56e7baac5 100644 --- a/platform/src/os/apple/tvos/tvos_app.rs +++ b/platform/src/os/apple/tvos/tvos_app.rs @@ -197,6 +197,7 @@ impl TvosApp { outer_size: new_size, dpi_factor, position: dvec2(0.0, 0.0), + ..Default::default() }; if get_tvos_app_global().first_draw { diff --git a/platform/src/os/headless/event_loop.rs b/platform/src/os/headless/event_loop.rs index cf72b876b9..07c871b856 100644 --- a/platform/src/os/headless/event_loop.rs +++ b/platform/src/os/headless/event_loop.rs @@ -16,6 +16,8 @@ use std::{ io::{self, BufRead, BufReader, Write}, path::PathBuf, rc::Rc, + thread, + time::Duration, time::Instant, }; @@ -51,8 +53,28 @@ impl Cx { cx.borrow_mut().in_makepad_studio = true; cx.borrow_mut().stdin_event_loop(); } else { - cx.borrow_mut().headless_single_frame(); + let draw_cycles = cx.borrow().os.draw_cycles; + if let Some(draw_cycles) = draw_cycles { + cx.borrow_mut().headless_bounded_loop(draw_cycles); + } else { + cx.borrow_mut().headless_single_frame(); + } + } + } + + pub fn headless_event_loop_for_draw_cycles(cx: Rc>, draw_cycles: usize) { + cx.borrow_mut().self_ref = Some(cx.clone()); + cx.borrow_mut().headless_bounded_loop(draw_cycles.max(1)); + } + + pub fn headless_no_draw_event_loop_for_draw_cycles(cx: Rc>, draw_cycles: usize) { + cx.borrow_mut().self_ref = Some(cx.clone()); + { + let mut cx_ref = cx.borrow_mut(); + cx_ref.os.no_draw = true; + cx_ref.os.no_draw_initialized = false; } + cx.borrow_mut().headless_bounded_loop(draw_cycles.max(1)); } fn headless_single_frame(&mut self) { @@ -73,10 +95,82 @@ impl Cx { if !self.new_next_frames.is_empty() { self.call_next_frame_event(time_now); } - if self.need_redrawing() { + if self.os.no_draw || self.need_redrawing() { + let _ = self.headless_process_draw_cycle(&mut windows, false, time_now); + } + } + + fn headless_bounded_loop(&mut self, draw_cycles: usize) { + let mut windows = Vec::new(); + self.call_event_handler(&Event::Startup); + let mut running = self.headless_handle_platform_ops(&mut windows, false); + if windows.is_empty() { + windows.push(HeadlessWindowState { + created: true, + width: 1280, + height: 720, + dpi_factor: 1.0, + frame_id: 0, + presentable_id: None, + }); + } + + let mut completed_cycles = 0usize; + while running && completed_cycles < draw_cycles { + if SignalToUI::check_and_clear_ui_signal() { + self.handle_script_signals(); + self.call_event_handler(&Event::Signal); + } + if SignalToUI::check_and_clear_action_signal() { + self.handle_action_receiver(); + } + self.dispatch_network_runtime_events(); + + let timer_events = self.os.stdin_timers.get_dispatch(); + for event in timer_events { + self.handle_script_timer(&event); + self.call_event_handler(&Event::Timer(event)); + } + + running = self.headless_handle_platform_ops(&mut windows, false); + if !running { + break; + } + + let time_now = self.os.stdin_timers.time_now(); + if !self.new_next_frames.is_empty() { + self.call_next_frame_event(time_now); + } + if self.os.no_draw || self.need_redrawing() { + let _ = self.headless_process_draw_cycle(&mut windows, false, time_now); + } + completed_cycles += 1; + + if !running { + break; + } + thread::sleep(Duration::from_millis(1)); + } + } + + fn headless_process_draw_cycle( + &mut self, + windows: &mut Vec, + send_protocol: bool, + time_now: f64, + ) -> bool { + if self.os.no_draw { self.call_draw_event(time_now); - self.headless_compile_shaders(); - self.headless_emit_frames(&mut windows, false, time_now); + self.os.no_draw_initialized = true; + return false; + } + self.call_draw_event(time_now); + self.headless_compile_shaders(); + if send_protocol && self.screenshot_requests.is_empty() { + self.headless_render_all_passes(time_now); + true + } else { + self.headless_emit_frames(windows, send_protocol, time_now) } } @@ -110,14 +204,8 @@ impl Cx { let mut running = self.headless_handle_platform_ops(&mut windows, true); if running { let time_now = self.seconds_since_app_start(); - if self.need_redrawing() { - self.call_draw_event(time_now); - self.headless_compile_shaders(); - if self.screenshot_requests.is_empty() { - self.headless_render_all_passes(time_now); - } else { - let _ = self.headless_emit_frames(&mut windows, true, time_now); - } + if self.os.no_draw || self.need_redrawing() { + let _ = self.headless_process_draw_cycle(&mut windows, true, time_now); } } write_stdout_msg(&AppToStudio::AfterStartup); @@ -305,20 +393,17 @@ impl Cx { self.call_next_frame_event(time_now); } - let mut rendered = false; - if self.need_redrawing() { - self.call_draw_event(time_now); - self.headless_compile_shaders(); - if self.screenshot_requests.is_empty() { - self.headless_render_all_passes(time_now); - rendered = true; - } else { - rendered = self.headless_emit_frames(&mut windows, true, time_now); - } - } + if self.os.no_draw || self.need_redrawing() { + let rendered = + self.headless_process_draw_cycle(&mut windows, true, time_now); - if rendered - || !self.os.stdin_timers.timers.is_empty() + if rendered + || !self.os.stdin_timers.timers.is_empty() + || !self.new_next_frames.is_empty() + { + write_stdout_msg(&AppToStudio::RequestAnimationFrame); + } + } else if !self.os.stdin_timers.timers.is_empty() || !self.new_next_frames.is_empty() { write_stdout_msg(&AppToStudio::RequestAnimationFrame); @@ -339,20 +424,17 @@ impl Cx { self.call_next_frame_event(time_now); } - let mut rendered = false; - if self.need_redrawing() { - self.call_draw_event(time_now); - self.headless_compile_shaders(); - if self.screenshot_requests.is_empty() { - self.headless_render_all_passes(time_now); - rendered = true; - } else { - rendered = self.headless_emit_frames(&mut windows, true, time_now); - } - } + if self.os.no_draw || self.need_redrawing() { + let rendered = + self.headless_process_draw_cycle(&mut windows, true, time_now); - if rendered - || !self.os.stdin_timers.timers.is_empty() + if rendered + || !self.os.stdin_timers.timers.is_empty() + || !self.new_next_frames.is_empty() + { + write_stdout_msg(&AppToStudio::RequestAnimationFrame); + } + } else if !self.os.stdin_timers.timers.is_empty() || !self.new_next_frames.is_empty() { write_stdout_msg(&AppToStudio::RequestAnimationFrame); @@ -593,6 +675,8 @@ impl Cx { impl CxOsApi for Cx { fn init_cx_os(&mut self) { self.os.start_time = Some(Instant::now()); + self.os.no_draw = crate::app_main::should_disable_headless_draw_from_args(); + self.os.draw_cycles = crate::app_main::headless_draw_cycles_from_args(); if let Some(item) = std::option_env!("MAKEPAD_PACKAGE_DIR") { self.package_root = Some(item.to_string()); } diff --git a/platform/src/os/headless/mod.rs b/platform/src/os/headless/mod.rs index 18de73bf69..dfdc905b6f 100644 --- a/platform/src/os/headless/mod.rs +++ b/platform/src/os/headless/mod.rs @@ -64,6 +64,9 @@ pub struct CxOs { pub(crate) start_time: Option, pub(crate) shader_jit: jit::HeadlessShaderJit, pub(crate) frame_dir: Option, + pub(crate) no_draw: bool, + pub(crate) no_draw_initialized: bool, + pub(crate) draw_cycles: Option, pub(crate) render_pool: Option>, pub(crate) render_pool_threads: usize, } @@ -75,6 +78,9 @@ impl Default for CxOs { start_time: None, shader_jit: Default::default(), frame_dir: None, + no_draw: false, + no_draw_initialized: false, + draw_cycles: None, render_pool: None, render_pool_threads: 0, } diff --git a/platform/src/os/headless/shader.rs b/platform/src/os/headless/shader.rs index 5cac672562..eb4ea0e0b3 100644 --- a/platform/src/os/headless/shader.rs +++ b/platform/src/os/headless/shader.rs @@ -94,10 +94,22 @@ impl DrawVars { output.assign_uniform_buffer_indices(&vm.bx.heap, 3); - let gen_result = generate_headless_rust_shader_module(&mut output, vm, io_self); - let varying_total_slots = gen_result.varying_total_slots; - let code = CxDrawShaderCode::Combined { - code: gen_result.source.clone(), + let no_draw = vm.host.cx().os.no_draw; + let (varying_total_slots, code) = if no_draw { + ( + count_varying_slots(&output, vm), + CxDrawShaderCode::Combined { + code: format!("// makepad headless no-draw shader {:016x}", fnhash.0), + }, + ) + } else { + let gen_result = generate_headless_rust_shader_module(&mut output, vm, io_self); + ( + gen_result.varying_total_slots, + CxDrawShaderCode::Combined { + code: gen_result.source, + }, + ) }; { @@ -176,6 +188,21 @@ impl DrawVars { impl Cx { pub(crate) fn headless_compile_shaders(&mut self) { let compile_set = std::mem::take(&mut self.draw_shaders.compile_set); + if self.os.no_draw { + for shader_index in compile_set { + let cx_shader = &mut self.draw_shaders.shaders[shader_index]; + if cx_shader.os_shader_id.is_some() { + continue; + } + let os_shader_id = self.draw_shaders.os_shaders.len(); + self.draw_shaders.os_shaders.push(CxOsDrawShader { + load_error: Some("headless --no-draw: raster/JIT disabled".to_string()), + ..Default::default() + }); + cx_shader.os_shader_id = Some(os_shader_id); + } + return; + } for shader_index in compile_set { let cx_shader = &mut self.draw_shaders.shaders[shader_index]; if cx_shader.os_shader_id.is_some() { diff --git a/platform/src/os/linux/android/android.rs b/platform/src/os/linux/android/android.rs index ecc69934ba..e3b81c8c7e 100644 --- a/platform/src/os/linux/android/android.rs +++ b/platform/src/os/linux/android/android.rs @@ -140,6 +140,31 @@ fn install_android_panic_hook() { })); } +pub fn set_current_thread_priority(priority: crate::CxThreadPriority) { + use core::ffi::{c_int, c_uint}; + + const PRIO_PROCESS: c_int = 0; + + unsafe extern "C" { + fn gettid() -> c_int; + fn setpriority(which: c_int, who: c_uint, prio: c_int) -> c_int; + } + + let nice = match priority { + crate::CxThreadPriority::Normal => 0, + crate::CxThreadPriority::Utility => 5, + crate::CxThreadPriority::Background => 10, + crate::CxThreadPriority::Idle => 15, + }; + + unsafe { + let tid = gettid(); + if tid > 0 { + let _ = setpriority(PRIO_PROCESS, tid as c_uint, nice); + } + } +} + impl Cx { pub(crate) fn current_android_xr_options(&self) -> CxOpenXrOptions { CxOpenXrOptions { @@ -256,6 +281,11 @@ impl Cx { /// It handles all incoming messages, processes other events, and manages drawing operations. pub fn main_loop(&mut self, from_java_rx: mpsc::Receiver) { self.gpu_info.performance = GpuPerformance::Tier1; + // Populate display_context and script heap with safe area insets + // BEFORE Startup, so app script_mod! definitions can use them. + let insets = self.os.safe_area_insets; + self.display_context.safe_area_insets = insets; + self.update_safe_inset_script_values(insets); self.call_event_handler(&Event::Startup); self.redraw_all(); @@ -283,6 +313,13 @@ impl Cx { continue; } self.os.openxr.logged_waiting_for_session = false; + // If a script re-apply was requested (e.g., safe area insets + // changed on rotation), fire LiveEdit now. + if self.pending_script_reapply { + self.pending_script_reapply = false; + self.call_event_handler(&Event::LiveEdit); + self.redraw_all(); + } self.handle_drawing(); } Ok(message) => { @@ -504,6 +541,8 @@ impl Cx { position: dvec2(0.0, 0.0), inner_size: size, outer_size: size, + safe_area_insets: self.os.safe_area_insets, + ..Default::default() }; let new_geom = window.window_geom.clone(); self.call_event_handler(&Event::WindowGeomChange(WindowGeomChangeEvent { @@ -1057,6 +1096,26 @@ impl Cx { let e = Event::ImeAction(ImeActionEvent { action }); self.call_event_handler(&e); } + FromJavaMessage::SafeAreaInsets { top, right, bottom, left } => { + let new_insets = crate::event::SafeAreaInsets { top, right, bottom, left }; + if self.os.safe_area_insets != new_insets { + self.os.safe_area_insets = new_insets; + // Update the WindowGeom with the new safe area insets + let window_id = CxWindowPool::id_zero(); + let window = &mut self.windows[window_id]; + let old_geom = window.window_geom.clone(); + window.window_geom.safe_area_insets = new_insets; + let new_geom = window.window_geom.clone(); + if old_geom != new_geom { + self.call_event_handler(&Event::WindowGeomChange(WindowGeomChangeEvent { + window_id, + new_geom, + old_geom, + })); + self.redraw_all(); + } + } + } FromJavaMessage::Init(_) => {} } } @@ -1838,6 +1897,8 @@ impl Cx { position: dvec2(0.0, 0.0), inner_size: size, outer_size: size, + safe_area_insets: self.os.safe_area_insets, + ..Default::default() }; window.is_created = true; //let ret = unsafe{ndk_sys::ANativeWindow_setFrameRate(self.os.display.as_ref().unwrap().window, 120.0, 0)}; @@ -1870,6 +1931,7 @@ impl Cx { position, inner_size: size, outer_size: size, + ..Default::default() }; window.is_popup = true; window.popup_parent = Some(parent_window_id); @@ -2652,6 +2714,7 @@ impl Default for CxOs { frame_time: 0, display_size: dvec2(100., 100.), dpi_factor: 1.5, + safe_area_insets: Default::default(), keyboard_closed: 0.0, media: CxAndroidMedia::default(), display: None, @@ -2710,6 +2773,7 @@ pub struct CxOs { pub first_after_resize: bool, pub display_size: Vec2d, pub dpi_factor: f64, + pub safe_area_insets: crate::event::SafeAreaInsets, pub keyboard_closed: f64, pub frame_time: i64, pub quit: bool, diff --git a/platform/src/os/linux/android/android_jni.rs b/platform/src/os/linux/android/android_jni.rs index 5fd56d51c9..d612134111 100644 --- a/platform/src/os/linux/android/android_jni.rs +++ b/platform/src/os/linux/android/android_jni.rs @@ -68,6 +68,12 @@ pub enum FromJavaMessage { keyboard_height: u32, is_open: bool, }, + SafeAreaInsets { + top: f64, + right: f64, + bottom: f64, + left: f64, + }, HttpResponse { request_id: u64, metadata_id: u64, @@ -639,6 +645,23 @@ extern "C" fn Java_dev_makepad_android_MakepadNative_surfaceOnResizeTextIME( }); } +#[no_mangle] +extern "C" fn Java_dev_makepad_android_MakepadNative_surfaceOnSafeAreaInsets( + _: *mut jni_sys::JNIEnv, + _: jni_sys::jobject, + top: jni_sys::jfloat, + right: jni_sys::jfloat, + bottom: jni_sys::jfloat, + left: jni_sys::jfloat, +) { + send_from_java_message(FromJavaMessage::SafeAreaInsets { + top: top as f64, + right: right as f64, + bottom: bottom as f64, + left: left as f64, + }); +} + #[no_mangle] extern "C" fn Java_dev_makepad_android_MakepadNative_onRenderLoop( _: *mut jni_sys::JNIEnv, diff --git a/platform/src/os/linux/direct/linux_direct.rs b/platform/src/os/linux/direct/linux_direct.rs index eb53cedc83..74d81b6357 100644 --- a/platform/src/os/linux/direct/linux_direct.rs +++ b/platform/src/os/linux/direct/linux_direct.rs @@ -284,6 +284,7 @@ impl Cx { position: dvec2(0.0, 0.0), inner_size: size, outer_size: size, + ..Default::default() }; window.is_created = true; } @@ -304,6 +305,7 @@ impl Cx { position, inner_size: size, outer_size: size, + ..Default::default() }; window.is_popup = true; window.popup_parent = Some(parent_window_id); diff --git a/platform/src/os/linux/open_harmony/open_harmony.rs b/platform/src/os/linux/open_harmony/open_harmony.rs index 91eeecc73b..97edcdd327 100644 --- a/platform/src/os/linux/open_harmony/open_harmony.rs +++ b/platform/src/os/linux/open_harmony/open_harmony.rs @@ -185,6 +185,7 @@ impl Cx { position: dvec2(0.0, 0.0), inner_size: size, outer_size: size, + ..Default::default() }; let new_geom = window.window_geom.clone(); self.call_event_handler(&Event::WindowGeomChange(WindowGeomChangeEvent { @@ -527,6 +528,7 @@ impl Cx { position: dvec2(0.0, 0.0), inner_size: size, outer_size: size, + ..Default::default() }; window.is_created = true; } diff --git a/platform/src/os/linux/openxr.rs b/platform/src/os/linux/openxr.rs index eab51738e1..f87f36cef2 100644 --- a/platform/src/os/linux/openxr.rs +++ b/platform/src/os/linux/openxr.rs @@ -130,8 +130,7 @@ impl Cx { } } XrStructureType::EVENT_DATA_REFERENCE_SPACE_CHANGE_PENDING => { - let reset_generation = - crate::xr_depth_mesh::xr_depth_mesh_store().request_reset(); + let reset_generation = crate::xr_tsdf::xr_tsdf_store().request_reset(); redraw_requested = true; crate::log!( "OpenXR reference space changed, resetting depth mesh pipeline generation={}", diff --git a/platform/src/os/linux/openxr_depth.rs b/platform/src/os/linux/openxr_depth.rs index 9940db22af..f9fc4fa388 100644 --- a/platform/src/os/linux/openxr_depth.rs +++ b/platform/src/os/linux/openxr_depth.rs @@ -1,991 +1,126 @@ use crate::{ - makepad_math::{vec2f, vec3f, vec4f, Mat4f, Vec2f, Vec3f, Vec4f}, + makepad_math::{vec3f, vec4f}, os::linux::{ openxr::CxOpenXrFrame, vulkan::{CxVulkan, CxVulkanOpenXrSessionData}, }, thread::SignalToUI, - xr_depth_mesh::{ - empty_bounds, xr_depth_align_build_wall_feature_normal_histogram, - xr_depth_align_build_wall_normal_histogram, xr_depth_align_loopback_preview_solution, - xr_depth_align_solve_remote_to_local, xr_depth_align_test_markers, - xr_depth_align_transform_descriptor, xr_depth_mesh_store, ChunkKey, XrDepthAlignDebug, - XrDepthAlignDescriptor, XrDepthAlignPreview, XrDepthAlignSample, XrDepthAlignSampleKind, - XrDepthAlignSolution, XrDepthAlignWallFeature, XrDepthMesh, XrDepthMeshChunk, - XrDepthMeshQuery, XrDepthMeshQueryCollider, XrDepthMeshQueryColliderGeometry, - XrDepthMeshQueryColliderRole, XrDepthMeshQueryHit, XrDepthMeshQueryResolvedSurface, - XrDepthMeshQueryResult, XrDepthMeshQuerySupportPlane, XrDepthMeshQuerySurfaceHit, - XrDepthMeshStore, XrDepthPlaneKind, XrDepthPlanePatch, - XR_DEPTH_MESH_DEFAULT_VOXEL_SIZE_METERS, + xr_tsdf::{ + apply_preprocessed_depth_mesh, preprocess_depth_mesh, projected_height_refresh_budget, + refresh_projected_height_field, score_depth_job_novelty, + submit_should_readback_depth_frame, sync_projected_height_field_layout, + sync_projected_height_field_player_cutout, update_published_height_map, xr_tsdf_store, + DepthFrameNovelty, DepthMeshJob, DepthMeshVolume, DepthPreprocessWorkerState, XrTsdfStore, + DEPTH_ALIGN_PROJECTED_HEIGHT_MAX_SLICE_CREDITS, + DEPTH_PROJECTED_HEIGHT_REFRESH_INTERVAL_MILLIS, DEPTH_PUBLISHED_HEIGHT_MAP_INTERVAL_MILLIS, + DEPTH_TSD_TARGET_INTEGRATION_INTERVAL_MILLIS, DEPTH_VOXEL_EYE_INDEX, }, }; use std::{ - collections::{HashMap, HashSet, VecDeque}, - hash::{Hash, Hasher}, - sync::{ - atomic::{AtomicBool, Ordering}, - mpsc::{channel, Receiver, RecvTimeoutError, Sender}, - Arc, - }, + sync::{Arc, Condvar, Mutex}, time::{Duration, Instant}, }; -const DEPTH_VOXEL_EYE_INDEX: usize = 0; -const DEPTH_VOXEL_SAMPLE_STEP: u32 = 1; -const DEPTH_IMAGE_EDGE_MARGIN_PIXELS: usize = 32; -const DEPTH_VOXEL_SIZE_METERS: f32 = XR_DEPTH_MESH_DEFAULT_VOXEL_SIZE_METERS; -const DEPTH_VOXEL_MIN_DISTANCE_METERS: f32 = 0.08; -const DEPTH_VOXEL_MAX_DISTANCE_METERS: f32 = 6.0; -const DEPTH_TSD_MIN_UPDATE_DISTANCE_METERS: f32 = 0.5; -const DEPTH_TSD_UPDATE_IDLE_INTERVAL_MILLIS: u64 = 200; -const DEPTH_TSD_UPDATE_MOVING_INTERVAL_MILLIS: u64 = 33; -const DEPTH_TSD_UPDATE_TRANSLATION_TRIGGER_METERS: f32 = 0.04; -const DEPTH_TSD_UPDATE_ROTATION_TRIGGER_DOT: f32 = 0.999; -const DEPTH_VOXEL_MIN_DEPTH_VALUE: f32 = 1.0 / 65535.0; -const DEPTH_VOXEL_MAX_DEPTH_VALUE: f32 = 0.9995; -const DEPTH_TSD_MIN_NORMAL_DOT: f32 = 0.3; -const DEPTH_TSD_APPLY_DELTA_EPSILON: f32 = 0.01; -const DEPTH_TSD_MAX_CONFIDENCE: u8 = 32; -const DEPTH_TSD_MIN_MESH_CONFIDENCE: u8 = 3; -const DEPTH_TSD_RECENT_MESH_CONFIDENCE: u8 = 1; -const DEPTH_TSD_RECENT_MESH_GENERATIONS: u64 = 6; -const DEPTH_TSD_RECENT_MESH_MAX_ABS_DISTANCE: f32 = 0.6; -const DEPTH_TSD_DENSE_HOLE_FILL_MAX_PASSES: usize = 2; -const DEPTH_TSD_DENSE_HOLE_FILL_MIN_AXIS_PAIRS: usize = 2; -const DEPTH_TSD_STABLE_CONFIDENCE: u8 = 8; -const DEPTH_PLAYER_CLEAR_MAX_CONFIDENCE: u8 = 2; -const DEPTH_PLAYER_EXCLUDE_RADIUS_METERS: f32 = 0.32; -const DEPTH_PLAYER_EXCLUDE_TOP_METERS: f32 = 0.12; -const DEPTH_PLAYER_EXCLUDE_BOTTOM_METERS: f32 = 1.30; -const DEPTH_MESH_UPDATE_DISTANCE_METERS: f32 = 4.0; -const DEPTH_SURFACE_MESH_CHUNKS_PER_TICK: usize = 1; -const DEPTH_PLANE_SCAN_CHUNKS_PER_TICK: usize = 1; const DEPTH_SURFACE_MESH_IDLE_WAIT_MILLIS: u64 = 8; -const DEPTH_QUERY_BATCH_PER_TICK: usize = 24; -const DEPTH_QUERY_MAX_SURFACES_PER_QUERY: usize = 1; -const DEPTH_DEBUG_LOG_CHUNK_MESH_TIMING: bool = false; -const DEPTH_QUERY_MIN_OPPOSING_NORMAL_DOT: f32 = 0.2; -const DEPTH_QUERY_SUPPORT_NORMAL_Y_MIN: f32 = 0.25; -const DEPTH_QUERY_LATERAL_NORMAL_Y_MAX: f32 = 0.80; -const DEPTH_QUERY_SUPPORT_DISTINCT_RADIUS_SCALE: f32 = 0.18; -const DEPTH_QUERY_SUPPORT_DISTINCT_RADIUS_MIN: f32 = 0.01; -const DEPTH_QUERY_DISTINCT_RADIUS_SCALE: f32 = 0.35; -const DEPTH_QUERY_DISTINCT_RADIUS_MIN: f32 = 0.02; -const DEPTH_QUERY_SUPPORT_PLANE_RADIUS_SCALE: f32 = 3.2; -const DEPTH_QUERY_SUPPORT_PLANE_RADIUS_MIN: f32 = 0.08; -const DEPTH_QUERY_SUPPORT_PLANE_RADIUS_MAX: f32 = 0.26; -const DEPTH_QUERY_SUPPORT_PLANE_HEIGHT_TOLERANCE_SCALE: f32 = 0.45; -const DEPTH_QUERY_SUPPORT_PLANE_HEIGHT_TOLERANCE_MIN: f32 = 0.015; -const DEPTH_QUERY_SUPPORT_PLANE_HEIGHT_TOLERANCE_MAX: f32 = 0.05; -const DEPTH_QUERY_SUPPORT_PLANE_NORMAL_DOT_MIN: f32 = 0.90; -const DEPTH_QUERY_SUPPORT_PLANE_DEBUG_HALF_EXTENT_MIN: f32 = 0.05; -const DEPTH_QUERY_TSDF_SUPPORT_GRID_DIM: usize = 5; -const DEPTH_QUERY_TSDF_SUPPORT_MAX_SAMPLES: usize = - DEPTH_QUERY_TSDF_SUPPORT_GRID_DIM * DEPTH_QUERY_TSDF_SUPPORT_GRID_DIM; -const DEPTH_QUERY_TSDF_SUPPORT_MIN_SAMPLES: usize = 4; -const DEPTH_QUERY_TRAJECTORY_SAMPLE_COUNT: usize = 5; -const DEPTH_QUERY_TSDF_SUPPORT_NORMAL_Y_MIN: f32 = 0.60; -const DEPTH_QUERY_TSDF_SUPPORT_RADIUS_SCALE: f32 = 1.15; -const DEPTH_QUERY_TSDF_SUPPORT_RADIUS_MIN: f32 = 0.04; -const DEPTH_QUERY_TSDF_SUPPORT_RADIUS_MAX: f32 = 0.12; -const DEPTH_QUERY_TSDF_SUPPORT_EXTENT_PADDING_SCALE: f32 = 0.22; -const DEPTH_QUERY_TSDF_IMPACT_MIN_SPEED: f32 = 0.55; -const DEPTH_QUERY_TSDF_IMPACT_MIN_HORIZONTAL_SPEED: f32 = 0.40; -const DEPTH_QUERY_TSDF_IMPACT_MIN_UPWARD_SPEED: f32 = 0.55; -const DEPTH_QUERY_TSDF_IMPACT_NORMAL_Y_MAX: f32 = 0.72; -const DEPTH_QUERY_TSDF_IMPACT_CEILING_NORMAL_Y_MIN: f32 = 0.82; -const DEPTH_QUERY_TSDF_IMPACT_RAY_STEP_SCALE: f32 = 0.40; -const DEPTH_QUERY_TSDF_IMPACT_RAY_STEP_MIN: f32 = 0.02; -const DEPTH_QUERY_TSDF_IMPACT_EXTENT_SCALE: f32 = 1.20; -const DEPTH_QUERY_TSDF_IMPACT_EXTENT_MIN: f32 = 0.05; -const DEPTH_QUERY_TSDF_IMPACT_EXTENT_MAX: f32 = 0.16; -const DEPTH_QUERY_TSDF_IMPACT_RESTITUTION: f32 = 0.38; -const DEPTH_PLANE_HORIZONTAL_NORMAL_Y_MIN: f32 = 0.82; -const DEPTH_PLANE_VERTICAL_NORMAL_Y_MAX: f32 = 0.35; -const DEPTH_PLANE_SCAN_NEAR_SURFACE_BAND_SCALE: f32 = 1.25; -const DEPTH_PLANE_SCAN_NEAR_SURFACE_BAND_MIN: f32 = 0.04; -const DEPTH_PLANE_SCAN_MIN_CONFIDENCE: u8 = 6; -const DEPTH_PLANE_SCAN_VERTICAL_DISTANCE_BIN_METERS: f32 = 0.08; -const DEPTH_PLANE_SCAN_VERTICAL_NORMAL_QUANTUM: f32 = 0.14; -const DEPTH_PLANE_SCAN_MAX_RECTS_PER_GROUP: usize = 8; -const DEPTH_PLANE_SCAN_MERGE_NORMAL_DOT: f32 = 0.97; -const DEPTH_PLANE_SCAN_MERGE_DISTANCE_METERS: f32 = 0.10; -const DEPTH_PLANE_SCAN_MERGE_GAP_METERS: f32 = 0.16; -const DEPTH_PLANE_SCAN_VERTICAL_MIN_AREA_METERS2: f32 = 0.25; -const DEPTH_PLANE_SCAN_VERTICAL_MIN_DIM_METERS: f32 = 0.50; -const DEPTH_PLANE_STABLE_TRACK_MATCH_NORMAL_DOT: f32 = 0.97; -const DEPTH_PLANE_STABLE_TRACK_MATCH_DISTANCE_METERS: f32 = 0.14; -const DEPTH_PLANE_STABLE_TRACK_MATCH_ALONG_GAP_METERS: f32 = 0.40; -const DEPTH_PLANE_STABLE_TRACK_MATCH_HEIGHT_GAP_METERS: f32 = 0.35; -const DEPTH_PLANE_STABLE_TRACK_MISSING_UPDATES_MAX: u8 = 8; -const DEPTH_PLANE_STABLE_TRACK_MIN_SEEN_COUNT: u8 = 2; -const DEPTH_PLANE_STABLE_TRACK_SMOOTH_ALPHA: f32 = 0.16; -const DEPTH_ROOM_WALL_AXIS_ASSIGN_DOT_MIN: f32 = 0.90; -const DEPTH_ROOM_WALL_ORTHOGONAL_DOT_MAX: f32 = 0.45; -const DEPTH_ROOM_WALL_GROUP_DISTANCE_METERS: f32 = 0.22; -const DEPTH_ROOM_WALL_TRACK_MATCH_NORMAL_DOT: f32 = 0.96; -const DEPTH_ROOM_WALL_TRACK_MATCH_DISTANCE_METERS: f32 = 0.28; -const DEPTH_ROOM_WALL_TRACK_MISSING_UPDATES_MAX: u8 = u8::MAX; -const DEPTH_ROOM_WALL_TRACK_SMOOTH_ALPHA: f32 = 0.12; -const DEPTH_ROOM_WALL_TRACK_EXPAND_ALPHA: f32 = 0.30; -const DEPTH_ROOM_WALL_TRACK_SHRINK_ALPHA: f32 = 0.08; -const DEPTH_ROOM_WALL_HEIGHT_METERS: f32 = 2.0; -const DEPTH_ROOM_WALL_MAX_TRACKS: usize = 4; -const DEPTH_PLANE_SIMPLIFY_REGION_NORMAL_DOT: f32 = 0.95; -const DEPTH_PLANE_SIMPLIFY_REGION_DISTANCE_METERS: f32 = 0.10; -const DEPTH_PLANE_SIMPLIFY_MIN_AREA_METERS2: f32 = 0.12; -const DEPTH_PLANE_MIN_AREA_METERS2: f32 = 0.35; -const DEPTH_PLANE_MIN_DIM_METERS: f32 = 0.30; -const DEPTH_PLANE_MAX_PATCHES: usize = 24; -const DEPTH_PLANE_REGION_VERTICAL_NORMAL_DOT: f32 = 0.94; -const DEPTH_PLANE_SUPPORT_CELL_METERS: f32 = 0.12; -const DEPTH_PLANE_SUPPORT_GROW_WEIGHT: u8 = 4; -const DEPTH_PLANE_SUPPORT_MAX_WEIGHT: u8 = 10; -const DEPTH_PLANE_SUPPORT_OCCUPIED_WEIGHT: u8 = 2; -const DEPTH_MESH_PLANAR_SIMPLIFY_MIN_AREA_METERS2: f32 = 0.45; -const DEPTH_MESH_PLANAR_SIMPLIFY_MIN_RECT_AREA_METERS2: f32 = 0.12; -const DEPTH_MESH_PLANAR_SIMPLIFY_MAX_RECTS_PER_REGION: usize = 24; -const DEPTH_ENABLE_REDUCED_PLANAR_PATCHES: bool = true; -const DEPTH_ALIGN_WALL_SAMPLE_GRID_METERS: f32 = 0.24; -const DEPTH_ALIGN_WALL_HISTOGRAM_BINS: usize = 48; -const DEPTH_ALIGN_MAX_WALL_FEATURES: usize = 24; -const DEPTH_ALIGN_MAX_WALL_SAMPLES: usize = 96; -const DEPTH_ALIGN_MIN_WALL_SAMPLES: usize = 6; -const DEPTH_ALIGN_DESCRIPTOR_UPDATE_INTERVAL_MILLIS: u64 = 250; -const DEPTH_ALIGN_PREVIEW_SOLUTION_LERP_MIN: f32 = 0.16; -const DEPTH_ALIGN_PREVIEW_SOLUTION_LERP_MAX: f32 = 0.36; -const DEPTH_ALIGN_PREVIEW_MARKER_LERP: f32 = 0.24; -const DEPTH_ALIGN_PREVIEW_BIG_JUMP_TRANSLATION_METERS: f32 = 0.16; -const DEPTH_ALIGN_PREVIEW_BIG_JUMP_YAW_RADIANS: f32 = 0.14; -const DEPTH_ALIGN_PREVIEW_JUMP_MATCH_TRANSLATION_METERS: f32 = 0.08; -const DEPTH_ALIGN_PREVIEW_JUMP_MATCH_YAW_RADIANS: f32 = 0.08; -const DEPTH_ALIGN_PREVIEW_BIG_JUMP_CONFIRM_UPDATES: u8 = 2; -const DEPTH_ALIGN_PREVIEW_HOLD_MISSING_UPDATES: u8 = 2; - -const fn depth_tsd_distance_meters(voxel_size_meters: f32) -> f32 { - voxel_size_meters * 2.0 -} - -const fn depth_tsd_refresh_clearance_meters(voxel_size_meters: f32) -> f32 { - voxel_size_meters * 1.5 -} - -const fn depth_normal_neighbor_max_distance_delta_meters(voxel_size_meters: f32) -> f32 { - voxel_size_meters * 2.5 -} - -const fn depth_carve_neighbor_max_distance_delta_meters(voxel_size_meters: f32) -> f32 { - voxel_size_meters * 1.5 -} - -const fn depth_plane_vertex_link_meters(voxel_size_meters: f32) -> f32 { - voxel_size_meters * 0.75 -} - -#[derive(Copy, Clone, Debug, Default, PartialEq, Eq, Hash)] -pub struct VoxelCoord { - pub x: i32, - pub y: i32, - pub z: i32, -} - -impl VoxelCoord { - pub const fn new(x: i32, y: i32, z: i32) -> Self { - Self { x, y, z } - } -} - -impl core::ops::Add for VoxelCoord { - type Output = Self; - - fn add(self, rhs: Self) -> Self::Output { - Self::new(self.x + rhs.x, self.y + rhs.y, self.z + rhs.z) - } -} - -impl core::ops::Sub for VoxelCoord { - type Output = Self; - - fn sub(self, rhs: Self) -> Self::Output { - Self::new(self.x - rhs.x, self.y - rhs.y, self.z - rhs.z) - } -} - -#[derive(Clone, Debug, Default, PartialEq)] -pub struct SurfaceMesh32 { - pub positions: Vec<[f32; 3]>, - pub normals: Vec<[f32; 3]>, - pub indices: Vec, -} - -#[derive(Copy, Clone, Debug, Eq, PartialEq)] -pub struct SparseVoxelMeshingConfig { - pub mesh_chunk_edge_voxels: i32, - pub mesh_chunk_overlap_voxels: i32, - pub cell_stride_voxels: i32, -} - -impl SparseVoxelMeshingConfig { - pub fn mesh_chunk_edge_voxels(self) -> i32 { - self.mesh_chunk_edge_voxels.max(1) - } - - pub fn mesh_chunk_overlap_voxels(self) -> i32 { - self.mesh_chunk_overlap_voxels.max(0) - } +const DEPTH_COOPERATIVE_STEP_INTERVAL_MILLIS: u64 = 8; +const DEPTH_COOPERATIVE_IDLE_POLL_INTERVAL_MILLIS: u64 = 33; - pub fn cell_stride_voxels(self) -> i32 { - self.cell_stride_voxels.max(1) - } - - pub fn max_padding_voxels(self) -> i32 { - self.mesh_chunk_overlap_voxels() - } -} - -#[derive(Clone, Debug)] -struct SparseTsdChunk { - values: Vec, - valid: Vec, - confidence: Vec, - observed_generation: Vec, - live_count: usize, +#[derive(Default)] +struct LatestDepthJobMailbox { + latest: Option, } -impl SparseTsdChunk { - fn new(chunk_volume: usize) -> Self { - Self { - values: vec![0.0; chunk_volume], - valid: vec![0; chunk_volume], - confidence: vec![0; chunk_volume], - observed_generation: vec![0; chunk_volume], - live_count: 0, - } - } - - fn value(&self, id: usize) -> Option { - if self.valid[id] == 0 { - None - } else { - Some(self.values[id]) - } - } - - fn meshing_value(&self, id: usize, current_generation: u64) -> Option { - if self.valid[id] == 0 { - None - } else if self.confidence[id] >= DEPTH_TSD_MIN_MESH_CONFIDENCE { - Some(self.values[id]) - } else if self.confidence[id] >= DEPTH_TSD_RECENT_MESH_CONFIDENCE - && current_generation.saturating_sub(self.observed_generation[id]) - <= DEPTH_TSD_RECENT_MESH_GENERATIONS - && self.values[id].abs() <= DEPTH_TSD_RECENT_MESH_MAX_ABS_DISTANCE - { - Some(self.values[id]) - } else { - None - } - } - - fn confidence(&self, id: usize) -> u8 { - if self.valid[id] == 0 { - 0 - } else { - self.confidence[id] - } - } - - fn accumulate(&mut self, id: usize, value: f32, generation: u64) -> (bool, bool) { - let previous = self.value(id); - let next_value = if let Some(previous) = previous { - let delta = (previous - value).abs(); - let mut confidence = self.confidence[id].max(1); - if delta < 0.08 { - confidence = confidence.saturating_add(2).min(DEPTH_TSD_MAX_CONFIDENCE); - } else if delta > 0.35 { - confidence = confidence.saturating_sub(2).max(1); - } - let confidence = confidence as f32; - previous + (value - previous) / (confidence + 1.0) - } else { - value - }; - let changed = previous - .map(|previous| (previous - next_value).abs() > 1.0e-4) - .unwrap_or(true); - self.values[id] = next_value; - self.valid[id] = 1; - if let Some(previous) = previous { - let delta = (previous - value).abs(); - let confidence = &mut self.confidence[id]; - if delta < 0.08 { - *confidence = confidence.saturating_add(2).min(DEPTH_TSD_MAX_CONFIDENCE); - } else if delta < 0.18 { - *confidence = confidence.saturating_add(1).min(DEPTH_TSD_MAX_CONFIDENCE); - } else if delta > 0.35 { - *confidence = confidence.saturating_sub(2).max(1); - } else { - *confidence = confidence.saturating_sub(1).max(1); - } - } else { - self.confidence[id] = 1; - } - self.observed_generation[id] = generation; - if previous.is_none() { - self.live_count += 1; - } - (changed, previous.is_none()) - } - - fn overwrite(&mut self, id: usize, value: f32, generation: u64) -> (bool, bool) { - let previous = self.value(id); - let changed = previous - .map(|previous| (previous - value).abs() > 1.0e-4) - .unwrap_or(true); - self.values[id] = value; - self.valid[id] = 1; - self.confidence[id] = DEPTH_TSD_MAX_CONFIDENCE; - self.observed_generation[id] = generation; - if previous.is_none() { - self.live_count += 1; - } - (changed, previous.is_none()) - } -} +type SharedLatestDepthJobMailbox = Arc<(Mutex, Condvar)>; -#[derive(Clone, Debug)] -pub struct SparseTsdGrid { - voxel_size: f32, - chunk_edge: i32, - chunk_volume: usize, - chunks: HashMap, - active_value_count: usize, +struct PendingDepthCandidate { + job: DepthMeshJob, + novelty: DepthFrameNovelty, } -#[derive(Clone, Debug, Default)] -struct ReducedSurfaceMesh { - mesh: SurfaceMesh32, - planar_patches: Vec, +fn replace_latest_depth_job(mailbox: &SharedLatestDepthJobMailbox, job: DepthMeshJob) -> bool { + let (lock, condvar) = &**mailbox; + let mut state = lock.lock().unwrap_or_else(|err| err.into_inner()); + let replaced = state.latest.replace(job).is_some(); + condvar.notify_one(); + replaced } -impl SparseTsdGrid { - pub fn new(voxel_size: f32, chunk_edge: i32) -> Self { - Self { - voxel_size, - chunk_edge, - chunk_volume: (chunk_edge as usize).pow(3), - chunks: HashMap::new(), - active_value_count: 0, - } - } - - pub fn is_empty(&self) -> bool { - self.active_value_count == 0 - } - - pub fn normalized_distance(&self, coord: VoxelCoord) -> Option { - let (chunk_key, local_id) = self.chunk_key_and_id(coord); - let chunk = self.chunks.get(&chunk_key)?; - chunk.value(local_id) - } - - fn meshing_distance(&self, coord: VoxelCoord, generation: u64) -> Option { - let (chunk_key, local_id) = self.chunk_key_and_id(coord); - let chunk = self.chunks.get(&chunk_key)?; - chunk.meshing_value(local_id, generation) - } - - fn confidence(&self, coord: VoxelCoord) -> u8 { - let (chunk_key, local_id) = self.chunk_key_and_id(coord); - self.chunks - .get(&chunk_key) - .map(|chunk| chunk.confidence(local_id)) - .unwrap_or(0) - } - - pub fn accumulate_normalized_distance( - &mut self, - coord: VoxelCoord, - value: f32, - generation: u64, - ) -> bool { - let (chunk_key, local_id) = self.chunk_key_and_id(coord); - let chunk = self - .chunks - .entry(chunk_key) - .or_insert_with(|| SparseTsdChunk::new(self.chunk_volume)); - let (changed, became_live) = chunk.accumulate(local_id, value, generation); - if became_live { - self.active_value_count += 1; - } - changed - } - - pub fn overwrite_normalized_distance( - &mut self, - coord: VoxelCoord, - value: f32, - generation: u64, - ) -> bool { - let (chunk_key, local_id) = self.chunk_key_and_id(coord); - let chunk = self - .chunks - .entry(chunk_key) - .or_insert_with(|| SparseTsdChunk::new(self.chunk_volume)); - let (changed, became_live) = chunk.overwrite(local_id, value, generation); - if became_live { - self.active_value_count += 1; - } - changed - } - - pub fn world_to_voxel_coord(&self, point: Vec3f) -> VoxelCoord { - VoxelCoord::new( - (point.x / self.voxel_size).floor() as i32, - (point.y / self.voxel_size).floor() as i32, - (point.z / self.voxel_size).floor() as i32, - ) - } - - pub fn world_to_mesh_chunk_key(&self, point: Vec3f, mesh_chunk_edge_voxels: i32) -> VoxelCoord { - let edge_world = self.voxel_size * mesh_chunk_edge_voxels.max(1) as f32; - VoxelCoord::new( - (point.x / edge_world).floor() as i32, - (point.y / edge_world).floor() as i32, - (point.z / edge_world).floor() as i32, - ) - } - - pub fn voxel_center_world(&self, coord: VoxelCoord) -> Vec3f { - vec3f( - (coord.x as f32 + 0.5) * self.voxel_size, - (coord.y as f32 + 0.5) * self.voxel_size, - (coord.z as f32 + 0.5) * self.voxel_size, - ) - } - - pub fn world_bounds(&self, padding_voxels: i32) -> Option<(Vec3f, Vec3f)> { - if self.chunks.is_empty() { - return None; - } - let mut min = VoxelCoord::new(i32::MAX, i32::MAX, i32::MAX); - let mut max = VoxelCoord::new(i32::MIN, i32::MIN, i32::MIN); - for (key, chunk) in &self.chunks { - if chunk.live_count == 0 { - continue; - } - min.x = min.x.min(key.x * self.chunk_edge); - min.y = min.y.min(key.y * self.chunk_edge); - min.z = min.z.min(key.z * self.chunk_edge); - max.x = max.x.max((key.x + 1) * self.chunk_edge); - max.y = max.y.max((key.y + 1) * self.chunk_edge); - max.z = max.z.max((key.z + 1) * self.chunk_edge); - } - if min.x == i32::MAX { - return None; - } - Some(( - vec3f( - (min.x - padding_voxels) as f32 * self.voxel_size, - (min.y - padding_voxels) as f32 * self.voxel_size, - (min.z - padding_voxels) as f32 * self.voxel_size, - ), - vec3f( - (max.x + padding_voxels) as f32 * self.voxel_size, - (max.y + padding_voxels) as f32 * self.voxel_size, - (max.z + padding_voxels) as f32 * self.voxel_size, - ), - )) - } - - pub fn surface_net_chunk_mesh_with_scratch( - &self, - chunk_key: VoxelCoord, - config: SparseVoxelMeshingConfig, - current_generation: u64, - dense: &mut Vec, - fill_scratch: &mut Vec, - ) -> Option { - let edge = config.mesh_chunk_edge_voxels(); - let overlap = config.mesh_chunk_overlap_voxels(); - let stride = config.cell_stride_voxels(); - let start = VoxelCoord::new(chunk_key.x * edge, chunk_key.y * edge, chunk_key.z * edge); - let extent = VoxelCoord::new(edge + overlap, edge + overlap, edge + overlap); - if !self.region_has_surface(start, extent) { - return None; - } - let dense_size = VoxelCoord::new( - align_extent(extent.x, stride), - align_extent(extent.y, stride), - align_extent(extent.z, stride), - ); - self.extract_dense_region_into(start, dense_size, current_generation, dense); - repair_dense_meshing_holes(dense, fill_scratch, dense_size); - surface_net_mesh_from_dense(dense, dense_size, self.voxel_size, start, stride) - } - - fn region_has_surface(&self, start: VoxelCoord, extent: VoxelCoord) -> bool { - if self.is_empty() { - return false; - } - let max = VoxelCoord::new( - start.x + extent.x.saturating_sub(1), - start.y + extent.y.saturating_sub(1), - start.z + extent.z.saturating_sub(1), - ); - let min_chunk = VoxelCoord::new( - start.x.div_euclid(self.chunk_edge), - start.y.div_euclid(self.chunk_edge), - start.z.div_euclid(self.chunk_edge), - ); - let max_chunk = VoxelCoord::new( - max.x.div_euclid(self.chunk_edge), - max.y.div_euclid(self.chunk_edge), - max.z.div_euclid(self.chunk_edge), - ); - for z in min_chunk.z..=max_chunk.z { - for y in min_chunk.y..=max_chunk.y { - for x in min_chunk.x..=max_chunk.x { - if self - .chunks - .get(&VoxelCoord::new(x, y, z)) - .is_some_and(|chunk| chunk.live_count != 0) - { - return true; - } - } - } - } - false - } - - fn extract_dense_region_into( - &self, - start: VoxelCoord, - extent: VoxelCoord, - current_generation: u64, - dense: &mut Vec, - ) { - let sx = extent.x.max(0) as usize; - let sy = extent.y.max(0) as usize; - let sz = extent.z.max(0) as usize; - dense.clear(); - dense.resize(sx * sy * sz, f32::NEG_INFINITY); - for z in 0..extent.z.max(0) { - for y in 0..extent.y.max(0) { - for x in 0..extent.x.max(0) { - let coord = VoxelCoord::new(start.x + x, start.y + y, start.z + z); - let value = self - .meshing_distance(coord, current_generation) - .unwrap_or(f32::NEG_INFINITY); - dense[(x as usize) + (y as usize) * sx + (z as usize) * sx * sy] = value; - } - } - } - } - - fn chunk_key_and_id(&self, coord: VoxelCoord) -> (VoxelCoord, usize) { - let cx = coord.x.div_euclid(self.chunk_edge); - let cy = coord.y.div_euclid(self.chunk_edge); - let cz = coord.z.div_euclid(self.chunk_edge); - let lx = coord.x.rem_euclid(self.chunk_edge) as usize; - let ly = coord.y.rem_euclid(self.chunk_edge) as usize; - let lz = coord.z.rem_euclid(self.chunk_edge) as usize; - let edge = self.chunk_edge as usize; - let id = lx + ly * edge + lz * edge * edge; - (VoxelCoord::new(cx, cy, cz), id) +fn take_latest_depth_job( + mailbox: &SharedLatestDepthJobMailbox, + timeout: Duration, +) -> Option { + let (lock, condvar) = &**mailbox; + let mut state = lock.lock().unwrap_or_else(|err| err.into_inner()); + if state.latest.is_none() && !timeout.is_zero() { + let (guard, _) = condvar + .wait_timeout(state, timeout) + .unwrap_or_else(|err| err.into_inner()); + state = guard; } + state.latest.take() } -fn repair_dense_meshing_holes(dense: &mut Vec, scratch: &mut Vec, extent: VoxelCoord) { - let sx = extent.x.max(0) as usize; - let sy = extent.y.max(0) as usize; - let sz = extent.z.max(0) as usize; - if sx < 3 || sy < 3 || sz < 3 || dense.len() != sx * sy * sz { - return; +fn pending_depth_candidate_should_replace( + pending: &PendingDepthCandidate, + candidate: &PendingDepthCandidate, +) -> bool { + const EPSILON: f32 = 1.0e-4; + if candidate.novelty.score > pending.novelty.score + EPSILON { + return true; } - - scratch.clear(); - scratch.resize(dense.len(), f32::NEG_INFINITY); - - for _ in 0..DEPTH_TSD_DENSE_HOLE_FILL_MAX_PASSES { - scratch.as_mut_slice().copy_from_slice(dense.as_slice()); - let mut changed = false; - - for z in 1..(sz - 1) { - for y in 1..(sy - 1) { - for x in 1..(sx - 1) { - let coord = VoxelCoord::new(x as i32, y as i32, z as i32); - let index = flatten_coord(coord, extent); - if dense[index].is_finite() { - continue; - } - - let mut pair_sum = 0.0f32; - let mut pair_count = 0usize; - let mut sign_vote = 0i32; - - for axis in [ - VoxelCoord::new(1, 0, 0), - VoxelCoord::new(0, 1, 0), - VoxelCoord::new(0, 0, 1), - ] { - let a = dense[flatten_coord(coord - axis, extent)]; - let b = dense[flatten_coord(coord + axis, extent)]; - if !a.is_finite() || !b.is_finite() { - continue; - } - pair_sum += (a + b) * 0.5; - pair_count += 1; - let a_sign = a < 0.0; - let b_sign = b < 0.0; - if a_sign == b_sign { - sign_vote += if a_sign { -1 } else { 1 }; - } - } - - if pair_count < DEPTH_TSD_DENSE_HOLE_FILL_MIN_AXIS_PAIRS { - continue; - } - - let mut fill = pair_sum / pair_count as f32; - if fill.abs() <= 1.0e-4 && sign_vote != 0 { - fill = if sign_vote < 0 { -0.02 } else { 0.02 }; - } - if !fill.is_finite() { - continue; - } - - scratch[index] = fill; - changed = true; - } - } - } - - if !changed { - break; + if (candidate.novelty.score - pending.novelty.score).abs() <= EPSILON { + if candidate.novelty.valid_samples > pending.novelty.valid_samples { + return true; } - dense.as_mut_slice().copy_from_slice(scratch.as_slice()); - } -} - -#[derive(Debug)] -struct DepthMeshVolume { - generation: u64, - latest_topology_generation: u64, - eye_index: usize, - image_width: u32, - image_height: u32, - sample_step: u32, - voxel_size_meters: f32, - bounds_min: Vec3f, - bounds_max: Vec3f, - mesh_grid: SparseTsdGrid, - mesh_config: SparseVoxelMeshingConfig, - mesh_chunks: Vec, - mesh_generation: u64, - update_sequence: u64, - dirty_chunk_keys: Vec, - removed_chunk_keys: Vec, - mesh_vertex_count: usize, - mesh_triangle_count: usize, - plane_generation: u64, - plane_patches: Vec, - stable_wall_tracks: Vec, - room_wall_tracks: Vec, - plane_scan_chunks: HashMap, - alignment_descriptor: Option, - alignment_debug: XrDepthAlignDebug, - alignment_preview: XrDepthAlignPreview, - alignment_preview_temporal: AlignmentPreviewTemporalState, - pending_mesh_dirty_chunks: HashSet, - pending_mesh_chunk_queue: VecDeque, - pending_plane_scan_dirty_chunks: HashSet, - pending_plane_scan_chunk_queue: VecDeque, -} - -#[derive(Clone, Debug, Default)] -struct ScannedPlaneChunk { - patches: Vec, - near_surface_voxel_count: u32, - floor_candidate_count: u32, - wall_candidate_count: u32, -} - -#[derive(Clone, Debug)] -struct StableWallPatchTrack { - patch: XrDepthPlanePatch, - seen_count: u8, - missing_updates: u8, -} - -#[derive(Clone, Copy, Debug, Default)] -struct RoomWallCandidate { - normal: Vec3f, - plane_distance: f32, - observed_along_min: f32, - observed_along_max: f32, - support_area: f32, -} - -#[derive(Clone, Copy, Debug)] -struct RoomWallAccumulator { - normal: Vec3f, - weight_sum: f32, - plane_distance_sum: f32, - observed_along_min: f32, - observed_along_max: f32, - support_area: f32, -} - -impl Default for RoomWallAccumulator { - fn default() -> Self { - Self { - normal: Vec3f::default(), - weight_sum: 0.0, - plane_distance_sum: 0.0, - observed_along_min: f32::INFINITY, - observed_along_max: f32::NEG_INFINITY, - support_area: 0.0, + if candidate.novelty.valid_samples == pending.novelty.valid_samples { + return candidate.job.generation > pending.job.generation; } } + false } -#[derive(Clone, Debug)] -struct StableRoomWallTrack { - normal: Vec3f, - plane_distance: f32, - observed_along_min: f32, - observed_along_max: f32, - support_area: f32, - seen_count: u8, - missing_updates: u8, -} - -impl DepthMeshVolume { - fn new(sample_step: u32, voxel_size_meters: f32) -> Self { - let mesh_config = SparseVoxelMeshingConfig { - mesh_chunk_edge_voxels: 24, - mesh_chunk_overlap_voxels: 4, - cell_stride_voxels: 1, - }; - Self { - generation: 0, - latest_topology_generation: 0, - eye_index: 0, - image_width: 0, - image_height: 0, - sample_step, - voxel_size_meters, - bounds_min: vec3f(0.0, 0.0, 0.0), - bounds_max: vec3f(0.0, 0.0, 0.0), - mesh_grid: SparseTsdGrid::new(voxel_size_meters, 8), - mesh_config, - mesh_chunks: Vec::new(), - mesh_generation: 0, - update_sequence: 0, - dirty_chunk_keys: Vec::new(), - removed_chunk_keys: Vec::new(), - mesh_vertex_count: 0, - mesh_triangle_count: 0, - plane_generation: 0, - plane_patches: Vec::new(), - stable_wall_tracks: Vec::new(), - room_wall_tracks: Vec::new(), - plane_scan_chunks: HashMap::new(), - alignment_descriptor: None, - alignment_debug: XrDepthAlignDebug::default(), - alignment_preview: XrDepthAlignPreview::default(), - alignment_preview_temporal: AlignmentPreviewTemporalState::default(), - pending_mesh_dirty_chunks: HashSet::new(), - pending_mesh_chunk_queue: VecDeque::new(), - pending_plane_scan_dirty_chunks: HashSet::new(), - pending_plane_scan_chunk_queue: VecDeque::new(), - } - } - - fn reset_render_mesh_state(&mut self) { - self.mesh_chunks.clear(); - self.mesh_generation = self.mesh_generation.saturating_add(1); - self.update_sequence = self.update_sequence.saturating_add(1); - self.dirty_chunk_keys.clear(); - self.removed_chunk_keys.clear(); - self.mesh_vertex_count = 0; - self.mesh_triangle_count = 0; - self.pending_mesh_dirty_chunks.clear(); - self.pending_mesh_chunk_queue.clear(); - } - - fn reset_plane_scan_state(&mut self) -> bool { - let had_plane_scan = !self.plane_scan_chunks.is_empty() - || !self.plane_patches.is_empty() - || !self.stable_wall_tracks.is_empty() - || !self.room_wall_tracks.is_empty() - || !self.pending_plane_scan_dirty_chunks.is_empty() - || !self.pending_plane_scan_chunk_queue.is_empty(); - if !had_plane_scan { - return false; - } - self.plane_scan_chunks.clear(); - self.plane_patches.clear(); - self.stable_wall_tracks.clear(); - self.room_wall_tracks.clear(); - self.pending_plane_scan_dirty_chunks.clear(); - self.pending_plane_scan_chunk_queue.clear(); - self.plane_generation = self.plane_generation.saturating_add(1); - self.update_sequence = self.update_sequence.saturating_add(1); - true - } - - fn clear_alignment_descriptor(&mut self) -> bool { - if self.alignment_descriptor.is_none() - && self.alignment_debug == XrDepthAlignDebug::default() - { - return false; - } - self.alignment_descriptor = None; - self.alignment_debug = XrDepthAlignDebug::default(); - self.update_sequence = self.update_sequence.saturating_add(1); - true - } - - fn clear_alignment_preview(&mut self) -> bool { - if self.alignment_preview == XrDepthAlignPreview::default() - && self - .alignment_preview_temporal - .pending_jump_solution - .is_none() - && self.alignment_preview_temporal.pending_jump_updates == 0 - && self.alignment_preview_temporal.missing_solution_updates == 0 - { - return false; +fn enqueue_pending_depth_candidate( + pending_depth_candidate: &mut Option, + candidate: PendingDepthCandidate, + store: &XrTsdfStore, +) { + match pending_depth_candidate { + Some(pending) if pending_depth_candidate_should_replace(pending, &candidate) => { + store.record_drop(); + *pending = candidate; } - self.alignment_preview = XrDepthAlignPreview::default(); - self.alignment_preview_temporal = AlignmentPreviewTemporalState::default(); - self.update_sequence = self.update_sequence.saturating_add(1); - true - } - - fn update_bounds(&mut self) { - if let Some((min, max)) = self.mesh_grid.world_bounds(0) { - self.bounds_min = min; - self.bounds_max = max; - } else { - (self.bounds_min, self.bounds_max) = empty_bounds(); + Some(_) => { + store.record_drop(); } - } - - fn snapshot(&self) -> XrDepthMesh { - XrDepthMesh { - generation: self.generation, - latest_topology_generation: self.latest_topology_generation, - update_sequence: self.update_sequence, - eye_index: self.eye_index, - image_width: self.image_width, - image_height: self.image_height, - sample_step: self.sample_step, - voxel_size_meters: self.voxel_size_meters, - bounds_min: self.bounds_min, - bounds_max: self.bounds_max, - mesh_chunks: self.mesh_chunks.clone(), - plane_patches: self.plane_patches.clone(), - alignment_descriptor: self.alignment_descriptor.clone(), - alignment_debug: self.alignment_debug, - alignment_preview: self.alignment_preview.clone(), - dirty_chunk_keys: self.dirty_chunk_keys.clone(), - removed_chunk_keys: self.removed_chunk_keys.clone(), - mesh_generation: self.mesh_generation, - mesh_vertex_count: self.mesh_vertex_count, - mesh_triangle_count: self.mesh_triangle_count, + None => { + *pending_depth_candidate = Some(candidate); } } - - fn record_dirty_chunk(&mut self, chunk_key: ChunkKey) { - push_unique_chunk_key(&mut self.dirty_chunk_keys, chunk_key); - self.removed_chunk_keys.retain(|key| *key != chunk_key); - } - - fn record_removed_chunk(&mut self, chunk_key: ChunkKey) { - push_unique_chunk_key(&mut self.removed_chunk_keys, chunk_key); - self.dirty_chunk_keys.retain(|key| *key != chunk_key); - } -} - -pub(super) struct CxOpenXrDepthMeshJob { - reset_generation: u64, - generation: u64, - eye_index: usize, - width: u32, - height: u32, - sample_step: u32, - voxel_size_meters: f32, - camera_world: Vec3f, - depth_proj: Mat4f, - inv_depth_proj: Mat4f, - depth_view_from_world: Mat4f, - world_from_depth_view: Mat4f, - depth: Vec, -} - -struct CxOpenXrPreparedDepthMeshJob { - reset_generation: u64, - generation: u64, - eye_index: usize, - width: u32, - height: u32, - sample_step: u32, - voxel_size_meters: f32, - camera_world: Vec3f, - depth_proj: Mat4f, - inv_depth_proj: Mat4f, - depth_view_from_world: Mat4f, - frame_tsd_samples: HashMap, - visible_world_min: Vec3f, - visible_world_max: Vec3f, - depth: Vec, -} - -#[derive(Clone, Copy, Debug, Default)] -struct FrameTsdSampleAccum { - sum: f32, - count: u16, } pub(super) struct CxOpenXrDepthMeshPipeline { - sender: Sender, - busy: Arc, - store: XrDepthMeshStore, + mailbox: SharedLatestDepthJobMailbox, + store: XrTsdfStore, next_generation: u64, last_reset_generation: u64, - last_submit_at: Option, - last_camera_world: Option, - last_camera_forward: Option, + last_depth_readback_at: Option, } impl CxOpenXrDepthMeshPipeline { pub fn new() -> Self { - let store = xr_depth_mesh_store(); - let busy = Arc::new(AtomicBool::new(false)); - let (sender, receiver) = channel::(); - let (prepared_sender, prepared_receiver) = channel::(); - std::thread::spawn({ - let busy = busy.clone(); - let store = store.clone(); - move || depth_preprocess_worker(receiver, prepared_sender, busy, store) - }); + let store = xr_tsdf_store(); + let mailbox = Arc::new((Mutex::new(LatestDepthJobMailbox::default()), Condvar::new())); std::thread::spawn({ + let mailbox = mailbox.clone(); let store = store.clone(); - move || depth_mesher_worker(prepared_receiver, store) + move || depth_preprocess_tsdf_writer_worker(mailbox, store) }); Self { - sender, - busy, + mailbox, store, next_generation: 1, last_reset_generation: 0, - last_submit_at: None, - last_camera_world: None, - last_camera_forward: None, + last_depth_readback_at: None, } } @@ -997,13 +132,10 @@ impl CxOpenXrDepthMeshPipeline { depth_image_index: usize, ) -> Result<(), String> { self.store.record_seen(); - let now = Instant::now(); let reset_generation = self.store.reset_generation(); if self.last_reset_generation != reset_generation { self.last_reset_generation = reset_generation; - self.last_submit_at = None; - self.last_camera_world = None; - self.last_camera_forward = None; + self.last_depth_readback_at = None; } let pose_result = (|| { let width = render_targets.depth_width; @@ -1011,14 +143,13 @@ impl CxOpenXrDepthMeshPipeline { if width == 0 || height == 0 { return Err("OpenXR depth readback dimensions are zero".to_string()); } + let depth_proj = frame.eyes[DEPTH_VOXEL_EYE_INDEX].depth_proj_mat; + let inv_depth_proj = depth_proj.invert(); let world_from_depth_view = frame.eyes[DEPTH_VOXEL_EYE_INDEX].depth_view_mat.invert(); let camera_world = world_from_depth_view.transform_vec4(vec4f(0.0, 0.0, 0.0, 1.0)); if !camera_world.w.is_finite() || camera_world.w.abs() < 1.0e-6 { return Err("OpenXR depth camera transform is invalid".to_string()); } - let camera_forward = world_from_depth_view.transform_vec4(vec4f(0.0, 0.0, -1.0, 0.0)); - let camera_forward = - vec3f(camera_forward.x, camera_forward.y, camera_forward.z).normalize(); let camera_world = vec3f( camera_world.x / camera_world.w, camera_world.y / camera_world.w, @@ -1027,49 +158,31 @@ impl CxOpenXrDepthMeshPipeline { Ok(( width, height, + depth_proj, + inv_depth_proj, world_from_depth_view, camera_world, - camera_forward, )) })(); - let (width, height, world_from_depth_view, camera_world, camera_forward) = match pose_result - { - Ok(parts) => parts, - Err(err) => { - self.store.set_error(err.clone()); - return Err(err); - } - }; - - let moved = self - .last_camera_world - .map(|last| { - (camera_world - last).length() >= DEPTH_TSD_UPDATE_TRANSLATION_TRIGGER_METERS - }) - .unwrap_or(true); - let rotated = self - .last_camera_forward - .map(|last| camera_forward.dot(last) <= DEPTH_TSD_UPDATE_ROTATION_TRIGGER_DOT) - .unwrap_or(true); - let min_interval = if moved || rotated { - DEPTH_TSD_UPDATE_MOVING_INTERVAL_MILLIS - } else { - DEPTH_TSD_UPDATE_IDLE_INTERVAL_MILLIS - }; + let (width, height, depth_proj, inv_depth_proj, world_from_depth_view, camera_world) = + match pose_result { + Ok(parts) => parts, + Err(err) => { + self.store.set_error(err.clone()); + return Err(err); + } + }; - if self - .last_submit_at - .is_some_and(|last| now.duration_since(last) < Duration::from_millis(min_interval)) - { - self.store.record_drop(); - return Ok(()); - } - if self - .busy - .compare_exchange(false, true, Ordering::AcqRel, Ordering::Acquire) - .is_err() - { + let now = Instant::now(); + if !submit_should_readback_depth_frame( + self.store.latest_tsdf_snapshot().as_deref(), + camera_world, + inv_depth_proj, + world_from_depth_view, + now, + self.last_depth_readback_at, + ) { self.store.record_drop(); return Ok(()); } @@ -1078,23 +191,22 @@ impl CxOpenXrDepthMeshPipeline { self.next_generation += 1; let voxel_size_meters = self.store.voxel_size_meters(); - let job_result: Result = (|| { + let job_result: Result = (|| { let depth = vulkan.read_openxr_depth_image( render_targets, depth_image_index, DEPTH_VOXEL_EYE_INDEX, )?; - Ok(CxOpenXrDepthMeshJob { + Ok(DepthMeshJob { reset_generation, generation, eye_index: DEPTH_VOXEL_EYE_INDEX, width, height, - sample_step: DEPTH_VOXEL_SAMPLE_STEP, voxel_size_meters, camera_world, - depth_proj: frame.eyes[DEPTH_VOXEL_EYE_INDEX].depth_proj_mat, - inv_depth_proj: frame.eyes[DEPTH_VOXEL_EYE_INDEX].depth_proj_mat.invert(), + depth_proj, + inv_depth_proj, depth_view_from_world: frame.eyes[DEPTH_VOXEL_EYE_INDEX].depth_view_mat, world_from_depth_view, depth, @@ -1104,7400 +216,181 @@ impl CxOpenXrDepthMeshPipeline { let job = match job_result { Ok(job) => job, Err(err) => { - self.busy.store(false, Ordering::Release); self.store.set_error(err.clone()); return Err(err); } }; - if let Err(err) = self.sender.send(job) { - let err = format!("OpenXR depth worker is unavailable: {err}"); - self.busy.store(false, Ordering::Release); - self.store.set_error(err.clone()); - return Err(err); + if replace_latest_depth_job(&self.mailbox, job) { + self.store.record_drop(); } + self.last_depth_readback_at = Some(now); - self.last_submit_at = Some(now); - self.last_camera_world = Some(camera_world); - self.last_camera_forward = Some(camera_forward); Ok(()) } } -#[derive(Clone, Copy, Debug)] -struct DepthGridSample { - world: Vec3f, - normal: Vec3f, - ray_distance: f32, - valid: bool, - has_normal: bool, -} - -#[derive(Default)] -struct DepthPreprocessWorkerState { - sampled_depth: Vec, - depth_width: usize, - depth_height: usize, -} - -#[derive(Default)] -struct DepthMesherWorkerState { - mesh_scratch: Vec, - mesh_fill_scratch: Vec, -} - -fn depth_preprocess_worker( - receiver: Receiver, - sender: Sender, - busy: Arc, - store: XrDepthMeshStore, -) { - let mut worker_state = DepthPreprocessWorkerState::default(); - while let Ok(job) = receiver.recv() { - if job.reset_generation != store.reset_generation() { - busy.store(false, Ordering::Release); - continue; - } - if (job.voxel_size_meters - store.voxel_size_meters()).abs() > f32::EPSILON { - busy.store(false, Ordering::Release); - continue; - } - let result = preprocess_depth_mesh(job, &mut worker_state); - busy.store(false, Ordering::Release); - match result { - Ok(job) => { - if let Err(err) = sender.send(job) { - store.set_error(format!("OpenXR depth mesher is unavailable: {err}")); - break; - } - } - Err(err) => { - store.set_error(err); - } - } - } -} - -fn depth_mesher_worker(receiver: Receiver, store: XrDepthMeshStore) { - let mut worker_state = DepthMesherWorkerState::default(); - let mut volume = DepthMeshVolume::new(DEPTH_VOXEL_SAMPLE_STEP, store.voxel_size_meters()); - let mut next_alignment_descriptor_update_at = Instant::now(); +fn depth_preprocess_tsdf_writer_worker(mailbox: SharedLatestDepthJobMailbox, store: XrTsdfStore) { + let mut preprocess_state = DepthPreprocessWorkerState::default(); + let mut volume = DepthMeshVolume::new(store.voxel_size_meters()); + let mut next_height_map_slice_at = Instant::now(); + let mut next_height_map_publish_at = + next_height_map_slice_at + matched_height_map_publish_interval(&store); + let mut next_cooperative_step_at = next_height_map_slice_at; let mut applied_reset_generation = store.reset_generation(); + let mut pending_depth_candidate = None::; + let mut last_depth_integration_at = None::; loop { let configured_voxel_size = store.voxel_size_meters(); - if (volume.voxel_size_meters - configured_voxel_size).abs() > f32::EPSILON { - volume = DepthMeshVolume::new(DEPTH_VOXEL_SAMPLE_STEP, configured_voxel_size); - next_alignment_descriptor_update_at = Instant::now(); + if (volume.voxel_size_meters() - configured_voxel_size).abs() > f32::EPSILON { + volume = DepthMeshVolume::new(configured_voxel_size); + next_height_map_slice_at = Instant::now(); + next_height_map_publish_at = + next_height_map_slice_at + matched_height_map_publish_interval(&store); + next_cooperative_step_at = next_height_map_slice_at; + pending_depth_candidate = None; + last_depth_integration_at = None; } let requested_reset_generation = store.reset_generation(); if applied_reset_generation != requested_reset_generation { applied_reset_generation = requested_reset_generation; - volume = DepthMeshVolume::new(DEPTH_VOXEL_SAMPLE_STEP, configured_voxel_size); - next_alignment_descriptor_update_at = Instant::now(); + volume = DepthMeshVolume::new(configured_voxel_size); + next_height_map_slice_at = Instant::now(); + next_height_map_publish_at = + next_height_map_slice_at + matched_height_map_publish_interval(&store); + next_cooperative_step_at = next_height_map_slice_at; + pending_depth_candidate = None; + last_depth_integration_at = None; } let mut applied_update = false; - match receiver.recv_timeout(Duration::from_millis(DEPTH_SURFACE_MESH_IDLE_WAIT_MILLIS)) { - Ok(mut job) => { - while let Ok(newer) = receiver.try_recv() { - job = newer; - } + if let Some(mut job) = take_latest_depth_job( + &mailbox, + Duration::from_millis(DEPTH_SURFACE_MESH_IDLE_WAIT_MILLIS), + ) { + loop { if job.reset_generation != store.reset_generation() { - continue; + break; + } + if (job.voxel_size_meters - store.voxel_size_meters()).abs() > f32::EPSILON { + break; } - let configured_voxel_size = store.voxel_size_meters(); - if (job.voxel_size_meters - configured_voxel_size).abs() > f32::EPSILON { + let candidate = PendingDepthCandidate { + novelty: score_depth_job_novelty(&volume, &job), + job, + }; + enqueue_pending_depth_candidate(&mut pending_depth_candidate, candidate, &store); + if let Some(next_job) = take_latest_depth_job(&mailbox, Duration::ZERO) { + job = next_job; continue; } - apply_preprocessed_depth_mesh(job, &mut volume); - applied_update = true; + break; + } + } + + let can_integrate_depth = pending_depth_candidate.is_some() + && last_depth_integration_at.is_none_or(|last| { + last.elapsed() + >= Duration::from_millis(DEPTH_TSD_TARGET_INTEGRATION_INTERVAL_MILLIS) + }); + if can_integrate_depth { + let candidate = pending_depth_candidate + .take() + .expect("candidate should exist when integration is allowed"); + let result = preprocess_depth_mesh(candidate.job, &mut preprocess_state); + match result { + Ok(job) => { + if job.reset_generation() == store.reset_generation() + && (job.voxel_size_meters() - store.voxel_size_meters()).abs() + <= f32::EPSILON + { + apply_preprocessed_depth_mesh(job, &preprocess_state, &mut volume); + volume.discard_obsolete_surface_state(); + applied_update = true; + last_depth_integration_at = Some(Instant::now()); + } + } + Err(err) => { + store.set_error(err); + } } - Err(RecvTimeoutError::Timeout) => {} - Err(RecvTimeoutError::Disconnected) => break, } - let query_changed = process_geometry_queries(&volume, &store, DEPTH_QUERY_BATCH_PER_TICK); - let queries_pending = store.has_pending_queries(); - let mesh_enabled = store.mesh_enabled(); - let plane_scan_enabled = store.plane_scan_enabled(); let surface_analysis_enabled = store.surface_analysis_enabled(); - let alignment_preview_enabled = store.alignment_preview_enabled(); let now = Instant::now(); - let mesh_changed = if mesh_enabled { - process_incremental_surface_mesh( - &mut volume, - &mut worker_state, - DEPTH_SURFACE_MESH_CHUNKS_PER_TICK, - ) - } else if !volume.mesh_chunks.is_empty() - || !volume.pending_mesh_dirty_chunks.is_empty() - || !volume.pending_mesh_chunk_queue.is_empty() - { - volume.reset_render_mesh_state(); - true - } else { - false - }; - let plane_changed = if plane_scan_enabled { - process_incremental_tsdf_plane_scan(&mut volume, DEPTH_PLANE_SCAN_CHUNKS_PER_TICK) - } else if volume.reset_plane_scan_state() { - true - } else if mesh_enabled { - update_reduced_planar_patches(&mut volume, mesh_changed) - } else if !volume.plane_patches.is_empty() || !volume.room_wall_tracks.is_empty() { - volume.plane_patches.clear(); - volume.room_wall_tracks.clear(); - volume.plane_generation = volume.plane_generation.saturating_add(1); - volume.update_sequence = volume.update_sequence.saturating_add(1); - true + if surface_analysis_enabled { + sync_projected_height_field_layout(&mut volume); + sync_projected_height_field_player_cutout(&mut volume); } else { - false - }; - let alignment_changed = if surface_analysis_enabled { - if plane_changed - || (!applied_update - && !queries_pending - && now >= next_alignment_descriptor_update_at) + next_height_map_slice_at = now; + next_height_map_publish_at = now + matched_height_map_publish_interval(&store); + volume.set_projected_height_publish_pending(false); + } + let mut slice_credits = 0usize; + if surface_analysis_enabled { + let slice_interval = + Duration::from_millis(DEPTH_PROJECTED_HEIGHT_REFRESH_INTERVAL_MILLIS); + while now >= next_height_map_slice_at + && slice_credits < DEPTH_ALIGN_PROJECTED_HEIGHT_MAX_SLICE_CREDITS { - next_alignment_descriptor_update_at = - now + Duration::from_millis(DEPTH_ALIGN_DESCRIPTOR_UPDATE_INTERVAL_MILLIS); - update_tsdf_alignment_descriptor(&mut volume) - } else { - false + slice_credits += 1; + next_height_map_slice_at += slice_interval; + } + let refresh_budget = projected_height_refresh_budget( + volume.pending_projected_height_sample_count(), + now, + next_height_map_publish_at, + slice_credits, + ); + if refresh_budget != 0 && refresh_projected_height_field(&mut volume, refresh_budget) { + volume.set_projected_height_publish_pending(true); } - } else { - next_alignment_descriptor_update_at = now; - volume.clear_alignment_descriptor() - }; - let preview_changed = if alignment_preview_enabled && surface_analysis_enabled { - if alignment_changed - || (volume.alignment_descriptor.is_some() - && volume.alignment_preview == XrDepthAlignPreview::default()) + } + let height_map_changed = if surface_analysis_enabled { + if now >= next_height_map_publish_at + && (volume.projected_height_publish_pending() || !volume.has_published_height_map()) { - update_alignment_preview(&mut volume) + while now >= next_height_map_publish_at { + next_height_map_publish_at += matched_height_map_publish_interval(&store); + } + volume.set_projected_height_publish_pending(false); + update_published_height_map(&mut volume) } else { false } } else { - volume.clear_alignment_preview() - }; - if (mesh_enabled && applied_update) - || mesh_changed - || plane_changed - || alignment_changed - || preview_changed - || query_changed - { - store.publish(volume.snapshot()); + volume.clear_published_height_map() + }; + let snapshot_changed = applied_update || height_map_changed; + if snapshot_changed { + let previous_snapshot = store.latest_tsdf_snapshot(); + let snapshot = volume.published_tsdf_snapshot(previous_snapshot.as_deref()); + store.publish_tsdf_snapshot(snapshot); + volume.clear_published_tsdf_dirty_state(); SignalToUI::set_ui_signal(); } + + let now = Instant::now(); + if now >= next_cooperative_step_at { + let schedule_fast = store + .run_cooperative_step() + .is_some_and(|result| result.did_work || result.has_more_work); + next_cooperative_step_at = now + + Duration::from_millis(if schedule_fast { + DEPTH_COOPERATIVE_STEP_INTERVAL_MILLIS + } else { + DEPTH_COOPERATIVE_IDLE_POLL_INTERVAL_MILLIS + }); + } } } -fn preprocess_depth_mesh( - job: CxOpenXrDepthMeshJob, - worker_state: &mut DepthPreprocessWorkerState, -) -> Result { - rebuild_sampled_depth_grid(&job, worker_state); - - let voxel_size_meters = job.voxel_size_meters; - let tsd_distance_meters = depth_tsd_distance_meters(voxel_size_meters); - let mut frame_tsd_accum = HashMap::::new(); - let mut observed_world_min = vec3f(f32::INFINITY, f32::INFINITY, f32::INFINITY); - let mut observed_world_max = vec3f(f32::NEG_INFINITY, f32::NEG_INFINITY, f32::NEG_INFINITY); - let sample_step = job.sample_step.max(1) as usize; - let ray_step = (voxel_size_meters * 0.5).max(0.02); - - for y in (0..worker_state.depth_height).step_by(sample_step) { - for x in (0..worker_state.depth_width).step_by(sample_step) { - if !depth_pixel_inside_margin(worker_state.depth_width, worker_state.depth_height, x, y) - { - continue; - } - let sample = worker_state.sampled_depth[y * worker_state.depth_width + x]; - if !sample.valid || !sample.has_normal { - continue; - } - if point_inside_player_exclusion(job.camera_world, sample.world) { - continue; - } - - let surface_ray = sample.world - job.camera_world; - let surface_distance = sample.ray_distance; - if !surface_distance.is_finite() - || !(DEPTH_TSD_MIN_UPDATE_DISTANCE_METERS..=DEPTH_VOXEL_MAX_DISTANCE_METERS) - .contains(&surface_distance) - { - continue; - } - - let ray_dir = surface_ray.normalize(); - let norm_dot = if sample.has_normal { - (-sample.normal.dot(ray_dir)).clamp(0.0, 1.0) - } else { - 1.0 - }; - if sample.has_normal && norm_dot <= DEPTH_TSD_MIN_NORMAL_DOT { - continue; - } - - observed_world_min = Vec3f::min_componentwise(observed_world_min, sample.world); - observed_world_max = Vec3f::max_componentwise(observed_world_max, sample.world); - - let start_distance = - (surface_distance - tsd_distance_meters).max(DEPTH_TSD_MIN_UPDATE_DISTANCE_METERS); - let end_distance = - (surface_distance + tsd_distance_meters).min(DEPTH_VOXEL_MAX_DISTANCE_METERS); - let mut last_coord = None; - let mut distance = start_distance; - while distance <= end_distance { - let sample_world = job.camera_world + ray_dir.scale(distance); - let coord = VoxelCoord::new( - (sample_world.x / voxel_size_meters).floor() as i32, - (sample_world.y / voxel_size_meters).floor() as i32, - (sample_world.z / voxel_size_meters).floor() as i32, - ); - if last_coord == Some(coord) { - distance += ray_step; - continue; - } - last_coord = Some(coord); - - let voxel_world = vec3f( - (coord.x as f32 + 0.5) * voxel_size_meters, - (coord.y as f32 + 0.5) * voxel_size_meters, - (coord.z as f32 + 0.5) * voxel_size_meters, - ); - if point_inside_player_exclusion(job.camera_world, voxel_world) { - distance += ray_step; - continue; - } - - let voxel_distance = (voxel_world - job.camera_world).dot(ray_dir); - if !voxel_distance.is_finite() { - distance += ray_step; - continue; - } - let normalized = - ((surface_distance - voxel_distance) / tsd_distance_meters).clamp(-1.0, 1.0); - frame_tsd_accum - .entry(coord) - .and_modify(|current| { - current.sum += normalized; - current.count = current.count.saturating_add(1); - }) - .or_insert(FrameTsdSampleAccum { - sum: normalized, - count: 1, - }); - distance += ray_step; - } - } - } - - let frame_tsd_samples = frame_tsd_accum - .into_iter() - .map(|(coord, accum)| (coord, accum.sum / accum.count.max(1) as f32)) - .collect(); - - let (visible_world_min, visible_world_max) = if observed_world_min.x.is_finite() - && observed_world_min.y.is_finite() - && observed_world_min.z.is_finite() - && observed_world_max.x.is_finite() - && observed_world_max.y.is_finite() - && observed_world_max.z.is_finite() - { - let padding = vec3f( - tsd_distance_meters, - tsd_distance_meters, - tsd_distance_meters, - ); - (observed_world_min - padding, observed_world_max + padding) - } else { - depth_visible_world_bounds(&job).unwrap_or((vec3f(0.0, 0.0, 0.0), vec3f(0.0, 0.0, 0.0))) - }; - - Ok(CxOpenXrPreparedDepthMeshJob { - reset_generation: job.reset_generation, - generation: job.generation, - eye_index: job.eye_index, - width: job.width, - height: job.height, - sample_step: job.sample_step, - voxel_size_meters, - camera_world: job.camera_world, - depth_proj: job.depth_proj, - inv_depth_proj: job.inv_depth_proj, - depth_view_from_world: job.depth_view_from_world, - frame_tsd_samples, - visible_world_min, - visible_world_max, - depth: job.depth, - }) -} - -fn apply_preprocessed_depth_mesh(job: CxOpenXrPreparedDepthMeshJob, volume: &mut DepthMeshVolume) { - volume.generation = job.generation; - volume.eye_index = job.eye_index; - volume.image_width = job.width; - volume.image_height = job.height; - volume.sample_step = job.sample_step; - - let mut topology_changes = apply_tsd_samples(volume, &job.frame_tsd_samples); - topology_changes += refresh_visible_free_space(volume, &job); - topology_changes += clear_player_exclusion_volume(volume, job.camera_world); - if topology_changes != 0 { - volume.latest_topology_generation = job.generation; - } - volume.update_bounds(); - enqueue_visible_mesh_chunks(volume, job.visible_world_min, job.visible_world_max); -} - -#[derive(Clone, Copy, Debug, Default, PartialEq)] -struct AlignmentCandidateSample { - kind: XrDepthAlignSampleKind, - point: Vec3f, - normal: Vec3f, - weight: f32, -} - -#[derive(Clone, Copy, Debug, Default, Eq, Hash, PartialEq)] -struct AlignmentWallKey { - x: i32, - y: i32, - z: i32, -} - -#[derive(Clone, Copy, Debug, Default)] -struct AlignmentPreviewTemporalState { - pending_jump_solution: Option, - pending_jump_updates: u8, - missing_solution_updates: u8, -} - -fn update_tsdf_alignment_descriptor(volume: &mut DepthMeshVolume) -> bool { - let (next_descriptor, next_debug) = build_tsdf_alignment_descriptor(volume); - if volume.alignment_descriptor == next_descriptor && volume.alignment_debug == next_debug { - return false; - } - volume.alignment_descriptor = next_descriptor; - volume.alignment_debug = next_debug; - volume.update_sequence = volume.update_sequence.saturating_add(1); - true -} - -fn update_alignment_preview(volume: &mut DepthMeshVolume) -> bool { - let next_preview = volume - .alignment_descriptor - .as_ref() - .map(build_alignment_preview) - .unwrap_or_default(); - let next_preview = stabilize_alignment_preview( - &volume.alignment_preview, - next_preview, - &mut volume.alignment_preview_temporal, - ); - if volume.alignment_preview == next_preview { - return false; - } - volume.alignment_preview = next_preview; - volume.update_sequence = volume.update_sequence.saturating_add(1); - true -} - -fn build_alignment_preview(local: &XrDepthAlignDescriptor) -> XrDepthAlignPreview { - let (local_floor_sample_count, local_wall_sample_count) = descriptor_sample_counts(local); - let mut preview = XrDepthAlignPreview { - local_markers: xr_depth_align_test_markers(local), - local_sample_count: local.samples.len(), - local_floor_sample_count, - local_wall_sample_count, - ..XrDepthAlignPreview::default() - }; - - let remote = build_loopback_preview_remote_descriptor(local); - let (remote_floor_sample_count, remote_wall_sample_count) = descriptor_sample_counts(&remote); - preview.remote_sample_count = remote.samples.len(); - preview.remote_floor_sample_count = remote_floor_sample_count; - preview.remote_wall_sample_count = remote_wall_sample_count; - - let remote_markers = xr_depth_align_test_markers(&remote); - let solution = xr_depth_align_solve_remote_to_local(local, &remote); - if let (Some(local_markers), Some(remote_markers), Some(solution)) = - (preview.local_markers, remote_markers, solution) - { - let mapped_remote = [ - solution.map_point(remote_markers[0]), - solution.map_point(remote_markers[1]), - ]; - let (remote_markers_local, _) = best_marker_assignment(local_markers, mapped_remote); - preview.remote_markers_local = Some(remote_markers_local); - preview.solution = Some(solution); - } - - preview -} - -fn stabilize_alignment_preview( - previous: &XrDepthAlignPreview, - mut next: XrDepthAlignPreview, - temporal: &mut AlignmentPreviewTemporalState, -) -> XrDepthAlignPreview { - next.local_markers = smooth_marker_pair( - previous.local_markers, - next.local_markers, - DEPTH_ALIGN_PREVIEW_MARKER_LERP, - false, - ); - - let mut hold_previous_remote = false; - let mut remote_alpha = DEPTH_ALIGN_PREVIEW_MARKER_LERP; - - match (previous.solution, next.solution) { - (Some(previous_solution), Some(next_solution)) => { - temporal.missing_solution_updates = 0; - let position_jump = - (next_solution.translation - previous_solution.translation).length(); - let yaw_jump = - wrap_preview_angle(next_solution.yaw_radians - previous_solution.yaw_radians).abs(); - let big_jump = position_jump > DEPTH_ALIGN_PREVIEW_BIG_JUMP_TRANSLATION_METERS - || yaw_jump > DEPTH_ALIGN_PREVIEW_BIG_JUMP_YAW_RADIANS; - let should_gate_jump = big_jump - && previous_solution.confidence >= 0.20 - && previous_solution.matched_samples >= 6; - - if should_gate_jump { - if temporal - .pending_jump_solution - .is_some_and(|pending| preview_solution_near(&pending, &next_solution)) - { - temporal.pending_jump_updates = temporal - .pending_jump_updates - .saturating_add(1) - .min(DEPTH_ALIGN_PREVIEW_BIG_JUMP_CONFIRM_UPDATES); - } else { - temporal.pending_jump_solution = Some(next_solution); - temporal.pending_jump_updates = 1; - } - - if temporal.pending_jump_updates < DEPTH_ALIGN_PREVIEW_BIG_JUMP_CONFIRM_UPDATES { - next.solution = Some(previous_solution); - hold_previous_remote = true; - } else { - temporal.pending_jump_solution = None; - temporal.pending_jump_updates = 0; - let alpha = solution_lerp_alpha(&next_solution); - next.solution = Some(lerp_alignment_solution( - previous_solution, - next_solution, - alpha, - )); - remote_alpha = alpha.max(DEPTH_ALIGN_PREVIEW_MARKER_LERP); - } - } else { - temporal.pending_jump_solution = None; - temporal.pending_jump_updates = 0; - let alpha = solution_lerp_alpha(&next_solution); - next.solution = Some(lerp_alignment_solution( - previous_solution, - next_solution, - alpha, - )); - remote_alpha = alpha.max(DEPTH_ALIGN_PREVIEW_MARKER_LERP); - } - } - (Some(previous_solution), None) => { - temporal.pending_jump_solution = None; - temporal.pending_jump_updates = 0; - if temporal.missing_solution_updates < DEPTH_ALIGN_PREVIEW_HOLD_MISSING_UPDATES { - temporal.missing_solution_updates = - temporal.missing_solution_updates.saturating_add(1); - next.solution = Some(XrDepthAlignSolution { - confidence: previous_solution.confidence * 0.94, - ..previous_solution - }); - hold_previous_remote = true; - } else { - temporal.missing_solution_updates = 0; - } - } - (None, Some(_)) => { - temporal.pending_jump_solution = None; - temporal.pending_jump_updates = 0; - temporal.missing_solution_updates = 0; - } - (None, None) => { - temporal.pending_jump_solution = None; - temporal.pending_jump_updates = 0; - temporal.missing_solution_updates = 0; - } - } - - next.remote_markers_local = smooth_marker_pair( - previous.remote_markers_local, - if hold_previous_remote { - None - } else { - next.remote_markers_local - }, - remote_alpha, - hold_previous_remote, - ); - next -} - -fn solution_lerp_alpha(solution: &XrDepthAlignSolution) -> f32 { - let confidence = solution.confidence.clamp(0.0, 1.0); - let residual = if solution.residual_meters.is_finite() { - (1.0_f32 - solution.residual_meters / 0.28_f32).clamp(0.0, 1.0) - } else { - 0.0 - }; - let matched = (solution.matched_samples as f32 / 16.0).clamp(0.0, 1.0); - let quality = (confidence * 0.60 + residual * 0.20 + matched * 0.20).clamp(0.0, 1.0); - DEPTH_ALIGN_PREVIEW_SOLUTION_LERP_MIN - + (DEPTH_ALIGN_PREVIEW_SOLUTION_LERP_MAX - DEPTH_ALIGN_PREVIEW_SOLUTION_LERP_MIN) * quality -} - -fn lerp_alignment_solution( - previous: XrDepthAlignSolution, - next: XrDepthAlignSolution, - alpha: f32, -) -> XrDepthAlignSolution { - let alpha = alpha.clamp(0.0, 1.0); - let yaw_delta = wrap_preview_angle(next.yaw_radians - previous.yaw_radians); - XrDepthAlignSolution { - yaw_radians: wrap_preview_angle(previous.yaw_radians + yaw_delta * alpha), - translation: previous.translation + (next.translation - previous.translation) * alpha, - confidence: previous.confidence + (next.confidence - previous.confidence) * alpha, - residual_meters: if previous.residual_meters.is_finite() && next.residual_meters.is_finite() - { - previous.residual_meters + (next.residual_meters - previous.residual_meters) * alpha - } else { - next.residual_meters - }, - matched_samples: previous.matched_samples.max(next.matched_samples), - } -} - -fn preview_solution_near(previous: &XrDepthAlignSolution, next: &XrDepthAlignSolution) -> bool { - (next.translation - previous.translation).length() - <= DEPTH_ALIGN_PREVIEW_JUMP_MATCH_TRANSLATION_METERS - && wrap_preview_angle(next.yaw_radians - previous.yaw_radians).abs() - <= DEPTH_ALIGN_PREVIEW_JUMP_MATCH_YAW_RADIANS -} - -fn smooth_marker_pair( - previous: Option<[Vec3f; 2]>, - next: Option<[Vec3f; 2]>, - alpha: f32, - hold_previous: bool, -) -> Option<[Vec3f; 2]> { - match (previous, next) { - (Some(previous), Some(next)) => { - let (next, _) = best_marker_assignment(previous, next); - Some([ - previous[0] + (next[0] - previous[0]) * alpha.clamp(0.0, 1.0), - previous[1] + (next[1] - previous[1]) * alpha.clamp(0.0, 1.0), - ]) - } - (Some(previous), None) if hold_previous => Some(previous), - (_, next) => next, - } -} - -fn wrap_preview_angle(mut angle: f32) -> f32 { - while angle <= -std::f32::consts::PI { - angle += std::f32::consts::TAU; - } - while angle > std::f32::consts::PI { - angle -= std::f32::consts::TAU; - } - angle -} - -fn build_loopback_preview_remote_descriptor( - local: &XrDepthAlignDescriptor, -) -> XrDepthAlignDescriptor { - let local_to_remote = loopback_preview_remote_to_local_transform().invert(); - let mut remote = xr_depth_align_transform_descriptor(local, &local_to_remote); - let quant_step = local.voxel_size_meters.max(0.03) * 0.65; - let mut degraded = Vec::new(); - for (index, mut sample) in remote.samples.into_iter().enumerate() { - let keep = match sample.kind { - XrDepthAlignSampleKind::Floor => true, - XrDepthAlignSampleKind::Wall => index % 4 != 1, - XrDepthAlignSampleKind::Unknown => false, - }; - if !keep { - continue; - } - match sample.kind { - XrDepthAlignSampleKind::Floor => { - sample.point.x += ((index % 5) as f32 - 2.0) * quant_step * 0.18; - sample.point.z += (((index * 3) % 7) as f32 - 3.0) * quant_step * 0.12; - sample.point.x = quantize_metric(sample.point.x, quant_step); - sample.point.z = quantize_metric(sample.point.z, quant_step); - } - XrDepthAlignSampleKind::Wall => { - let tangent = wall_tangent(sample.normal); - let vertical = vec3f(0.0, 1.0, 0.0); - let tangent_jitter = ((index % 5) as f32 - 2.0) * quant_step * 0.15; - let vertical_jitter = (((index * 3) % 7) as f32 - 3.0) * quant_step * 0.10; - sample.point += tangent * tangent_jitter + vertical * vertical_jitter; - sample.point.x = quantize_metric(sample.point.x, quant_step); - sample.point.y = quantize_metric(sample.point.y, quant_step); - sample.point.z = quantize_metric(sample.point.z, quant_step); - } - XrDepthAlignSampleKind::Unknown => {} - } - sample.weight = (sample.weight * (0.84 + 0.04 * (index % 4) as f32)).clamp(0.1, 1.0); - degraded.push(sample); - } - remote.samples = degraded; - remote.wall_normal_histogram = if remote.wall_features.is_empty() { - Vec::new() - } else { - xr_depth_align_build_wall_feature_normal_histogram( - &remote.wall_features, - local.wall_normal_histogram.len(), - ) - }; - remote -} - -fn loopback_preview_remote_to_local_transform() -> Mat4f { - xr_depth_align_loopback_preview_solution().remote_to_local_transform() -} - -fn descriptor_sample_counts(descriptor: &XrDepthAlignDescriptor) -> (usize, usize) { - let mut floor_count = 0usize; - let mut wall_count = 0usize; - for sample in &descriptor.samples { - match sample.kind { - XrDepthAlignSampleKind::Floor => floor_count += 1, - XrDepthAlignSampleKind::Wall => wall_count += 1, - XrDepthAlignSampleKind::Unknown => {} - } - } - (floor_count, wall_count) -} - -fn best_marker_assignment(local: [Vec3f; 2], remote: [Vec3f; 2]) -> ([Vec3f; 2], f32) { - let direct = (remote[0] - local[0]).length() + (remote[1] - local[1]).length(); - let swapped = (remote[1] - local[0]).length() + (remote[0] - local[1]).length(); - if swapped < direct { - ([remote[1], remote[0]], swapped * 50.0) - } else { - (remote, direct * 50.0) - } -} - -fn wall_tangent(normal: Vec3f) -> Vec3f { - let tangent = vec3f(-normal.z, 0.0, normal.x); - if tangent.length() > 1.0e-5 { - tangent.normalize() - } else { - vec3f(1.0, 0.0, 0.0) - } -} - -fn build_tsdf_alignment_descriptor( - volume: &DepthMeshVolume, -) -> (Option, XrDepthAlignDebug) { - let mut debug = XrDepthAlignDebug::default(); - for chunk in volume.plane_scan_chunks.values() { - debug.near_surface_voxel_count = debug - .near_surface_voxel_count - .saturating_add(chunk.near_surface_voxel_count); - debug.floor_candidate_count = debug - .floor_candidate_count - .saturating_add(chunk.floor_candidate_count); - debug.wall_candidate_count = debug - .wall_candidate_count - .saturating_add(chunk.wall_candidate_count); - } - let wall_patches = visible_stable_wall_patches(&volume.stable_wall_tracks); - let wall_features = visible_room_wall_features(&volume.room_wall_tracks); - if wall_patches.is_empty() && wall_features.is_empty() { - return (None, debug); - } - - if wall_patches.is_empty() { - debug.wall_candidate_count = debug.wall_candidate_count.max(wall_features.len() as u32); - } - - let mut wall_samples = HashMap::::new(); - for patch in &wall_patches { - append_wall_patch_alignment_samples(&mut wall_samples, patch); - } - - let mut samples = wall_samples - .into_values() - .map(quantized_align_sample) - .collect::>(); - samples.sort_by(|a, b| b.weight.total_cmp(&a.weight)); - if samples.len() > DEPTH_ALIGN_MAX_WALL_SAMPLES { - samples.truncate(DEPTH_ALIGN_MAX_WALL_SAMPLES); - } - debug.wall_sample_count = samples.len() as u32; - if wall_features.is_empty() && debug.wall_sample_count < DEPTH_ALIGN_MIN_WALL_SAMPLES as u32 { - return (None, debug); - } - - let wall_histogram = if wall_features.is_empty() { - xr_depth_align_build_wall_normal_histogram(&samples, DEPTH_ALIGN_WALL_HISTOGRAM_BINS) - } else { - xr_depth_align_build_wall_feature_normal_histogram( - &wall_features, - DEPTH_ALIGN_WALL_HISTOGRAM_BINS, - ) - }; - - ( - Some(XrDepthAlignDescriptor { - voxel_size_meters: volume.voxel_size_meters, - floor_y: 0.0, - wall_normal_histogram: wall_histogram, - wall_features, - samples, - }), - debug, - ) -} - -fn append_wall_patch_alignment_samples( - wall_samples: &mut HashMap, - patch: &XrDepthPlanePatch, -) { - for_each_plane_patch_grid_sample( - patch, - DEPTH_ALIGN_WALL_SAMPLE_GRID_METERS, - |point, edge_bias| { - let key = AlignmentWallKey { - x: quantize_f32(point.x, DEPTH_ALIGN_WALL_SAMPLE_GRID_METERS), - y: quantize_f32(point.y, DEPTH_ALIGN_WALL_SAMPLE_GRID_METERS), - z: quantize_f32(point.z, DEPTH_ALIGN_WALL_SAMPLE_GRID_METERS), - }; - upsert_alignment_candidate( - wall_samples, - key, - AlignmentCandidateSample { - kind: XrDepthAlignSampleKind::Wall, - point, - normal: patch.normal, - weight: plane_patch_alignment_sample_weight( - patch, - DEPTH_ALIGN_WALL_SAMPLE_GRID_METERS, - edge_bias, - ), - }, - ); - }, - ); -} - -fn for_each_plane_patch_grid_sample(patch: &XrDepthPlanePatch, step: f32, mut visitor: F) -where - F: FnMut(Vec3f, f32), -{ - let plane_distance = patch.center.dot(patch.normal); - let min_u = patch.center.dot(patch.tangent) - patch.half_extent_tangent; - let max_u = patch.center.dot(patch.tangent) + patch.half_extent_tangent; - let min_v = patch.center.dot(patch.bitangent) - patch.half_extent_bitangent; - let max_v = patch.center.dot(patch.bitangent) + patch.half_extent_bitangent; - let min_u_cell = (min_u / step.max(f32::EPSILON)).ceil() as i32; - let max_u_cell = (max_u / step.max(f32::EPSILON)).floor() as i32; - let min_v_cell = (min_v / step.max(f32::EPSILON)).ceil() as i32; - let max_v_cell = (max_v / step.max(f32::EPSILON)).floor() as i32; - let mut emitted = false; - - for u_cell in min_u_cell..=max_u_cell { - for v_cell in min_v_cell..=max_v_cell { - let u = u_cell as f32 * step; - let v = v_cell as f32 * step; - let edge_distance = (u - min_u).min(max_u - u).min(v - min_v).min(max_v - v); - let edge_bias = 1.0 - (edge_distance / step.max(0.001)).clamp(0.0, 1.0); - visitor( - patch.normal.scale(plane_distance) - + patch.tangent.scale(u) - + patch.bitangent.scale(v), - edge_bias, - ); - emitted = true; - } - } - - if !emitted { - visitor(patch.center, 1.0); - } -} - -fn plane_patch_alignment_sample_weight( - patch: &XrDepthPlanePatch, - step: f32, - edge_bias: f32, -) -> f32 { - let base = (patch.area / step.max(0.01)).sqrt().clamp(0.25, 6.0); - base * (1.0 + edge_bias * 0.65) -} - -fn upsert_alignment_candidate( - map: &mut HashMap, - key: K, - candidate: AlignmentCandidateSample, -) { - match map.get_mut(&key) { - Some(existing) if existing.weight >= candidate.weight => {} - Some(existing) => *existing = candidate, - None => { - map.insert(key, candidate); - } - } -} - -fn quantized_align_sample(sample: AlignmentCandidateSample) -> XrDepthAlignSample { - let quantized_normal = vec3f( - quantize_metric(sample.normal.x, 0.001), - quantize_metric(sample.normal.y, 0.001), - quantize_metric(sample.normal.z, 0.001), - ); - XrDepthAlignSample { - kind: sample.kind, - point: vec3f( - quantize_metric(sample.point.x, 0.01), - quantize_metric(sample.point.y, 0.01), - quantize_metric(sample.point.z, 0.01), - ), - normal: if quantized_normal.length() > 1.0e-5 { - quantized_normal.normalize() - } else { - sample.normal - }, - weight: quantize_metric(sample.weight, 0.01).max(0.01), - } -} - -fn query_depth_grid_normal_at_coord(volume: &DepthMeshVolume, coord: VoxelCoord) -> Option { - let center = volume.mesh_grid.normalized_distance(coord)?; - let step = volume - .voxel_size_meters - .max(DEPTH_QUERY_TSDF_IMPACT_RAY_STEP_MIN); - let dx = finite_difference_axis( - center, - volume - .mesh_grid - .normalized_distance(coord + VoxelCoord::new(1, 0, 0)), - volume - .mesh_grid - .normalized_distance(coord - VoxelCoord::new(1, 0, 0)), - step, - )?; - let dy = finite_difference_axis( - center, - volume - .mesh_grid - .normalized_distance(coord + VoxelCoord::new(0, 1, 0)), - volume - .mesh_grid - .normalized_distance(coord - VoxelCoord::new(0, 1, 0)), - step, - )?; - let dz = finite_difference_axis( - center, - volume - .mesh_grid - .normalized_distance(coord + VoxelCoord::new(0, 0, 1)), - volume - .mesh_grid - .normalized_distance(coord - VoxelCoord::new(0, 0, 1)), - step, - )?; - let gradient = vec3f(dx, dy, dz); - (gradient.length() > 1.0e-5).then_some(gradient.normalize()) -} - -fn quantize_metric(value: f32, step: f32) -> f32 { - if step <= 0.0 { - return value; - } - (value / step).round() * step -} - -fn rebuild_sampled_depth_grid( - job: &CxOpenXrDepthMeshJob, - worker_state: &mut DepthPreprocessWorkerState, -) { - let normal_neighbor_max_distance_delta_meters = - depth_normal_neighbor_max_distance_delta_meters(job.voxel_size_meters); - let width = job.width as usize; - let height = job.height as usize; - worker_state.depth_width = width; - worker_state.depth_height = height; - worker_state.sampled_depth.clear(); - worker_state.sampled_depth.resize( - width * height, - DepthGridSample { - world: vec3f(0.0, 0.0, 0.0), - normal: vec3f(0.0, 1.0, 0.0), - ray_distance: 0.0, - valid: false, - has_normal: false, - }, - ); - - for y in 0..height { - for x in 0..width { - if !depth_pixel_inside_margin(width, height, x, y) { - continue; - } - let raw_depth = job.depth[y * width + x] as f32 / u16::MAX as f32; - if !(DEPTH_VOXEL_MIN_DEPTH_VALUE..DEPTH_VOXEL_MAX_DEPTH_VALUE).contains(&raw_depth) { - continue; - } - let Some(world) = depth_pixel_to_world(job, x as u32, y as u32) else { - continue; - }; - worker_state.sampled_depth[y * width + x] = DepthGridSample { - world, - normal: vec3f(0.0, 1.0, 0.0), - ray_distance: (world - job.camera_world).length(), - valid: true, - has_normal: false, - }; - } - } - - for y in 0..height { - for x in 0..width { - let index = y * width + x; - if !worker_state.sampled_depth[index].valid { - continue; - } - let world = worker_state.sampled_depth[index].world; - let sample_x = - sampled_depth_at_pixel(worker_state, (x + 2).min(width - 1) as u32, y as u32); - let sample_y = - sampled_depth_at_pixel(worker_state, x as u32, (y + 2).min(height - 1) as u32); - if !sample_x.valid || !sample_y.valid { - continue; - } - let ray_distance = worker_state.sampled_depth[index].ray_distance; - if (sample_x.ray_distance - ray_distance).abs() - > normal_neighbor_max_distance_delta_meters - || (sample_y.ray_distance - ray_distance).abs() - > normal_neighbor_max_distance_delta_meters - { - continue; - } - let h_deriv = sample_x.world - world; - let v_deriv = sample_y.world - world; - if h_deriv.length() <= 1.0e-4 || v_deriv.length() <= 1.0e-4 { - continue; - } - let mut normal = Vec3f::cross(h_deriv, v_deriv).normalize().scale(-1.0); - if normal.length() <= 1.0e-4 { - continue; - } - let view_dir = (job.camera_world - world).normalize(); - if normal.dot(view_dir) < 0.0 { - normal = normal.scale(-1.0); - } - worker_state.sampled_depth[index].normal = normal; - worker_state.sampled_depth[index].has_normal = true; - } - } -} - -fn sampled_depth_at_pixel( - worker_state: &DepthPreprocessWorkerState, - pixel_x: u32, - pixel_y: u32, -) -> DepthGridSample { - if worker_state.depth_width == 0 || worker_state.depth_height == 0 { - return DepthGridSample { - world: vec3f(0.0, 0.0, 0.0), - normal: vec3f(0.0, 1.0, 0.0), - ray_distance: 0.0, - valid: false, - has_normal: false, - }; - } - let x = pixel_x.min(worker_state.depth_width.saturating_sub(1) as u32) as usize; - let y = pixel_y.min(worker_state.depth_height.saturating_sub(1) as u32) as usize; - worker_state.sampled_depth[y * worker_state.depth_width + x] -} - -fn depth_pixel_inside_margin(width: usize, height: usize, x: usize, y: usize) -> bool { - let margin = DEPTH_IMAGE_EDGE_MARGIN_PIXELS; - if width <= margin * 2 || height <= margin * 2 { - return true; - } - x >= margin && y >= margin && x + margin < width && y + margin < height -} - -fn depth_ndc_to_view( - job: &CxOpenXrDepthMeshJob, - ndc_x: f32, - ndc_y: f32, - ndc_z: f32, -) -> Option { - let view = job - .inv_depth_proj - .transform_vec4(vec4f(ndc_x, ndc_y, ndc_z, 1.0)); - if !view.w.is_finite() || view.w.abs() < 1.0e-6 { - return None; - } - let inv_w = 1.0 / view.w; - let point = vec3f(view.x * inv_w, view.y * inv_w, view.z * inv_w); - (point.x.is_finite() && point.y.is_finite() && point.z.is_finite()).then_some(point) -} - -fn depth_view_to_world(job: &CxOpenXrDepthMeshJob, view: Vec3f) -> Option { - let world = job - .world_from_depth_view - .transform_vec4(vec4f(view.x, view.y, view.z, 1.0)); - if !world.w.is_finite() || world.w.abs() < 1.0e-6 { - return None; - } - let inv_w = 1.0 / world.w; - let point = vec3f(world.x * inv_w, world.y * inv_w, world.z * inv_w); - (point.x.is_finite() && point.y.is_finite() && point.z.is_finite()).then_some(point) -} - -fn depth_pixel_to_world(job: &CxOpenXrDepthMeshJob, x: u32, y: u32) -> Option { - let view = depth_pixel_to_view( - &job.depth, - job.width, - job.height, - job.inv_depth_proj, - x as usize, - y as usize, - )?; - let world = job.world_from_depth_view.transform_vec4(view); - if !world.w.is_finite() || world.w.abs() < 1.0e-6 { - return None; - } - let inv_w = 1.0 / world.w; - let point = vec3f(world.x * inv_w, world.y * inv_w, world.z * inv_w); - (point.x.is_finite() && point.y.is_finite() && point.z.is_finite()).then_some(point) -} - -fn depth_pixel_to_view( - depth: &[u16], - width: u32, - height: u32, - inv_depth_proj: Mat4f, - x: usize, - y: usize, -) -> Option { - let raw_depth = *depth.get(y * width as usize + x)? as f32 / u16::MAX as f32; - if !(DEPTH_VOXEL_MIN_DEPTH_VALUE..DEPTH_VOXEL_MAX_DEPTH_VALUE).contains(&raw_depth) { - return None; - } - let uv_x = (x as f32 + 0.5) / width as f32; - let uv_y = (y as f32 + 0.5) / height as f32; - let clip = vec4f( - uv_x * 2.0 - 1.0, - uv_y * 2.0 - 1.0, - raw_depth * 2.0 - 1.0, - 1.0, - ); - let view = inv_depth_proj.transform_vec4(clip); - if !view.w.is_finite() || view.w.abs() < 1.0e-6 { - return None; - } - let view = vec4f(view.x / view.w, view.y / view.w, view.z / view.w, 1.0); - let distance = view.to_vec3f().length(); - if !distance.is_finite() - || !(DEPTH_VOXEL_MIN_DISTANCE_METERS..=DEPTH_VOXEL_MAX_DISTANCE_METERS).contains(&distance) - { - return None; - } - Some(view) -} - -fn depth_world_to_pixel( - world: Vec3f, - width: u32, - height: u32, - depth_view_from_world: Mat4f, - depth_proj: Mat4f, -) -> Option<(usize, usize, f32)> { - let view = depth_view_from_world.transform_vec4(vec4f(world.x, world.y, world.z, 1.0)); - if !view.w.is_finite() || view.w.abs() < 1.0e-6 { - return None; - } - let view = vec4f(view.x / view.w, view.y / view.w, view.z / view.w, 1.0); - let view_pos = view.to_vec3f(); - let ray_distance = view_pos.length(); - if !ray_distance.is_finite() - || !(DEPTH_VOXEL_MIN_DISTANCE_METERS..=DEPTH_VOXEL_MAX_DISTANCE_METERS) - .contains(&ray_distance) - { - return None; - } - let clip = depth_proj.transform_vec4(view); - if !clip.w.is_finite() || clip.w.abs() < 1.0e-6 { - return None; - } - let ndc_x = clip.x / clip.w; - let ndc_y = clip.y / clip.w; - let ndc_z = clip.z / clip.w; - if !ndc_x.is_finite() || !ndc_y.is_finite() || !ndc_z.is_finite() { - return None; - } - if !(-1.0..=1.0).contains(&ndc_x) - || !(-1.0..=1.0).contains(&ndc_y) - || !(-1.0..=1.0).contains(&ndc_z) - { - return None; - } - let pixel_x = ((ndc_x * 0.5 + 0.5) * width as f32).floor() as isize; - let pixel_y = ((ndc_y * 0.5 + 0.5) * height as f32).floor() as isize; - if pixel_x < 0 || pixel_y < 0 || pixel_x >= width as isize || pixel_y >= height as isize { - return None; - } - Some((pixel_x as usize, pixel_y as usize, ray_distance)) -} - -fn depth_pixel_is_reliable_for_carve( - job: &CxOpenXrPreparedDepthMeshJob, - pixel_x: usize, - pixel_y: usize, - observed_distance: f32, -) -> bool { - let carve_neighbor_max_distance_delta_meters = - depth_carve_neighbor_max_distance_delta_meters(job.voxel_size_meters); - let width = job.width as usize; - let height = job.height as usize; - if !depth_pixel_inside_margin(width, height, pixel_x, pixel_y) { - return false; - } - - let mut agreeing_neighbors = 0u8; - for (nx, ny) in [ - (pixel_x.saturating_sub(1), pixel_y), - ((pixel_x + 1).min(width.saturating_sub(1)), pixel_y), - (pixel_x, pixel_y.saturating_sub(1)), - (pixel_x, (pixel_y + 1).min(height.saturating_sub(1))), - ] { - if nx == pixel_x && ny == pixel_y { - continue; - } - let Some(neighbor_view) = depth_pixel_to_view( - &job.depth, - job.width, - job.height, - job.inv_depth_proj, - nx, - ny, - ) else { - continue; - }; - let neighbor_distance = neighbor_view.to_vec3f().length(); - if (neighbor_distance - observed_distance).abs() <= carve_neighbor_max_distance_delta_meters - { - agreeing_neighbors = agreeing_neighbors.saturating_add(1); - } - } - agreeing_neighbors >= 2 -} - -fn apply_tsd_samples( - volume: &mut DepthMeshVolume, - frame_tsd_samples: &HashMap, -) -> usize { - let mut changed = 0; - for (&coord, &normalized) in frame_tsd_samples { - let previous = volume.mesh_grid.normalized_distance(coord).unwrap_or(2.0); - if volume - .mesh_grid - .accumulate_normalized_distance(coord, normalized, volume.generation) - { - let current = volume - .mesh_grid - .normalized_distance(coord) - .unwrap_or(previous); - if (previous - current).abs() >= DEPTH_TSD_APPLY_DELTA_EPSILON { - mark_mesh_chunk_dirty(volume, coord); - changed += 1; - } - } - } - changed -} - -fn refresh_visible_free_space( - volume: &mut DepthMeshVolume, - job: &CxOpenXrPreparedDepthMeshJob, -) -> usize { - let tsd_distance_meters = depth_tsd_distance_meters(job.voxel_size_meters); - let tsd_refresh_clearance_meters = depth_tsd_refresh_clearance_meters(job.voxel_size_meters); - let min_coord = volume.mesh_grid.world_to_voxel_coord(job.visible_world_min); - let max_coord = volume.mesh_grid.world_to_voxel_coord(job.visible_world_max); - let mut changed = 0; - - for z in min_coord.z..=max_coord.z { - for y in min_coord.y..=max_coord.y { - for x in min_coord.x..=max_coord.x { - let coord = VoxelCoord::new(x, y, z); - let Some(previous) = volume.mesh_grid.normalized_distance(coord) else { - continue; - }; - if previous >= 1.0 - 1.0e-4 { - continue; - } - let voxel_world = volume.mesh_grid.voxel_center_world(coord); - let Some((pixel_x, pixel_y, voxel_distance)) = depth_world_to_pixel( - voxel_world, - job.width, - job.height, - job.depth_view_from_world, - job.depth_proj, - ) else { - continue; - }; - if !depth_pixel_inside_margin( - job.width as usize, - job.height as usize, - pixel_x, - pixel_y, - ) { - continue; - } - let Some(observed_view) = depth_pixel_to_view( - &job.depth, - job.width, - job.height, - job.inv_depth_proj, - pixel_x, - pixel_y, - ) else { - continue; - }; - let observed_distance = observed_view.to_vec3f().length(); - if !depth_pixel_is_reliable_for_carve(job, pixel_x, pixel_y, observed_distance) { - continue; - } - let clearance = observed_distance - voxel_distance; - if !observed_distance.is_finite() || clearance < tsd_refresh_clearance_meters { - continue; - } - let confidence = volume.mesh_grid.confidence(coord); - if confidence >= DEPTH_TSD_STABLE_CONFIDENCE - && previous <= 0.25 - && clearance < tsd_distance_meters - { - continue; - } - if volume - .mesh_grid - .accumulate_normalized_distance(coord, 1.0, volume.generation) - { - let current = volume - .mesh_grid - .normalized_distance(coord) - .unwrap_or(previous); - if (previous - current).abs() >= DEPTH_TSD_APPLY_DELTA_EPSILON { - mark_mesh_chunk_dirty(volume, coord); - changed += 1; - } - } - } - } - } - - changed -} - -fn clear_player_exclusion_volume(volume: &mut DepthMeshVolume, camera_world: Vec3f) -> usize { - let min_world = vec3f( - camera_world.x - DEPTH_PLAYER_EXCLUDE_RADIUS_METERS, - camera_world.y - DEPTH_PLAYER_EXCLUDE_BOTTOM_METERS, - camera_world.z - DEPTH_PLAYER_EXCLUDE_RADIUS_METERS, - ); - let max_world = vec3f( - camera_world.x + DEPTH_PLAYER_EXCLUDE_RADIUS_METERS, - camera_world.y + DEPTH_PLAYER_EXCLUDE_TOP_METERS, - camera_world.z + DEPTH_PLAYER_EXCLUDE_RADIUS_METERS, - ); - let min_coord = volume.mesh_grid.world_to_voxel_coord(min_world); - let max_coord = volume.mesh_grid.world_to_voxel_coord(max_world); - let mut changed = 0; - for z in min_coord.z..=max_coord.z { - for y in min_coord.y..=max_coord.y { - for x in min_coord.x..=max_coord.x { - let coord = VoxelCoord::new(x, y, z); - let center = volume.mesh_grid.voxel_center_world(coord); - if !point_inside_player_exclusion(camera_world, center) { - continue; - } - let Some(previous) = volume.mesh_grid.normalized_distance(coord) else { - continue; - }; - if volume.mesh_grid.confidence(coord) > DEPTH_PLAYER_CLEAR_MAX_CONFIDENCE { - continue; - } - if previous >= 1.0 - 1.0e-4 { - continue; - } - if volume - .mesh_grid - .overwrite_normalized_distance(coord, 1.0, volume.generation) - { - mark_mesh_chunk_dirty(volume, coord); - changed += 1; - } - } - } - } - changed -} - -fn mark_mesh_chunk_dirty(volume: &mut DepthMeshVolume, voxel: VoxelCoord) { - let padding = volume.mesh_config.max_padding_voxels(); - let edge = volume.mesh_config.mesh_chunk_edge_voxels(); - let min_chunk = VoxelCoord::new( - (voxel.x - padding).div_euclid(edge), - (voxel.y - padding).div_euclid(edge), - (voxel.z - padding).div_euclid(edge), - ); - let max_chunk = VoxelCoord::new( - (voxel.x + padding).div_euclid(edge), - (voxel.y + padding).div_euclid(edge), - (voxel.z + padding).div_euclid(edge), - ); - for z in min_chunk.z..=max_chunk.z { - for y in min_chunk.y..=max_chunk.y { - for x in min_chunk.x..=max_chunk.x { - let key = ChunkKey::new(x, y, z); - if volume.pending_mesh_dirty_chunks.insert(key) { - volume.pending_mesh_chunk_queue.push_back(key); - } - if volume.pending_plane_scan_dirty_chunks.insert(key) { - volume.pending_plane_scan_chunk_queue.push_back(key); - } - } - } - } -} - -fn depth_visible_world_bounds(job: &CxOpenXrDepthMeshJob) -> Option<(Vec3f, Vec3f)> { - let Some(bottom_left) = depth_ndc_to_view(job, -1.0, -1.0, 0.0) else { - return None; - }; - let Some(bottom_right) = depth_ndc_to_view(job, 1.0, -1.0, 0.0) else { - return None; - }; - let Some(top_right) = depth_ndc_to_view(job, 1.0, 1.0, 0.0) else { - return None; - }; - let Some(top_left) = depth_ndc_to_view(job, -1.0, 1.0, 0.0) else { - return None; - }; - let mut corners = [bottom_left, bottom_right, top_right, top_left]; - for corner in &mut corners { - let scale = DEPTH_MESH_UPDATE_DISTANCE_METERS / (-corner.z).max(1.0e-6); - *corner = corner.scale(scale); - } - - let mut world_min = vec3f(f32::INFINITY, f32::INFINITY, f32::INFINITY); - let mut world_max = vec3f(f32::NEG_INFINITY, f32::NEG_INFINITY, f32::NEG_INFINITY); - for corner in corners { - let Some(world) = depth_view_to_world(job, corner) else { - continue; - }; - world_min = Vec3f::min_componentwise(world_min, world); - world_max = Vec3f::max_componentwise(world_max, world); - } - (world_min.x.is_finite() - && world_min.y.is_finite() - && world_min.z.is_finite() - && world_max.x.is_finite() - && world_max.y.is_finite() - && world_max.z.is_finite()) - .then_some((world_min, world_max)) -} - -fn enqueue_visible_mesh_chunks(volume: &mut DepthMeshVolume, world_min: Vec3f, world_max: Vec3f) { - let edge = volume.mesh_config.mesh_chunk_edge_voxels(); - let min_key = volume.mesh_grid.world_to_mesh_chunk_key(world_min, edge); - let max_key = volume.mesh_grid.world_to_mesh_chunk_key(world_max, edge); - let meshed_keys: HashSet = volume - .mesh_chunks - .iter() - .map(|chunk| chunk.chunk_key) - .collect(); - for z in (min_key.z - 1)..=(max_key.z + 1) { - for y in (min_key.y - 1)..=(max_key.y + 1) { - for x in (min_key.x - 1)..=(max_key.x + 1) { - let key = ChunkKey::new(x, y, z); - if !meshed_keys.contains(&key) && volume.pending_mesh_dirty_chunks.insert(key) { - volume.pending_mesh_chunk_queue.push_back(key); - } - } - } - } -} - -fn point_inside_player_exclusion(camera_world: Vec3f, world: Vec3f) -> bool { - let dx = world.x - camera_world.x; - let dz = world.z - camera_world.z; - let horizontal_sq = dx * dx + dz * dz; - horizontal_sq <= DEPTH_PLAYER_EXCLUDE_RADIUS_METERS * DEPTH_PLAYER_EXCLUDE_RADIUS_METERS - && world.y <= camera_world.y + DEPTH_PLAYER_EXCLUDE_TOP_METERS - && world.y >= camera_world.y - DEPTH_PLAYER_EXCLUDE_BOTTOM_METERS -} - -fn process_incremental_surface_mesh( - volume: &mut DepthMeshVolume, - worker_state: &mut DepthMesherWorkerState, - max_mesh_jobs: usize, -) -> bool { - if volume.mesh_grid.is_empty() { - if !volume.mesh_chunks.is_empty() { - volume.reset_render_mesh_state(); - return true; - } - return false; - } - - let mut mesh_changed = false; - for _ in 0..max_mesh_jobs { - let Some(chunk_key) = volume.pending_mesh_chunk_queue.pop_front() else { - break; - }; - volume.pending_mesh_dirty_chunks.remove(&chunk_key); - let started = Instant::now(); - let mesh = volume.mesh_grid.surface_net_chunk_mesh_with_scratch( - voxel_coord_from_chunk_key(chunk_key), - volume.mesh_config, - volume.generation, - &mut worker_state.mesh_scratch, - &mut worker_state.mesh_fill_scratch, - ); - if DEPTH_DEBUG_LOG_CHUNK_MESH_TIMING { - let elapsed = started.elapsed().as_secs_f32() * 1000.0; - let triangles = mesh - .as_ref() - .map(|mesh| mesh.indices.len() / 3) - .unwrap_or(0); - crate::log!( - "OpenXR depth meshed chunk ({}, {}, {}) in {:.2}ms tri={} pending={}", - chunk_key.x, - chunk_key.y, - chunk_key.z, - elapsed, - triangles, - volume.pending_mesh_chunk_queue.len() - ); - } - let update = update_incremental_mesh_chunk(volume, chunk_key, mesh); - if !mesh_changed && !matches!(update, MeshChunkUpdate::Unchanged) { - volume.dirty_chunk_keys.clear(); - volume.removed_chunk_keys.clear(); - } - match update { - MeshChunkUpdate::Unchanged => {} - MeshChunkUpdate::Upserted => { - volume.record_dirty_chunk(chunk_key); - mesh_changed = true; - } - MeshChunkUpdate::Removed => { - volume.record_removed_chunk(chunk_key); - mesh_changed = true; - } - } - } - - if mesh_changed { - volume.mesh_chunks.sort_by(|a, b| { - (a.chunk_key.x, a.chunk_key.y, a.chunk_key.z).cmp(&( - b.chunk_key.x, - b.chunk_key.y, - b.chunk_key.z, - )) - }); - volume.mesh_vertex_count = volume.mesh_chunks.iter().map(|c| c.vertices.len()).sum(); - volume.mesh_triangle_count = volume - .mesh_chunks - .iter() - .map(XrDepthMeshChunk::triangle_count) - .sum(); - volume.mesh_generation = volume.mesh_generation.saturating_add(1); - volume.update_sequence = volume.update_sequence.saturating_add(1); - } - mesh_changed -} - -enum MeshChunkUpdate { - Unchanged, - Upserted, - Removed, -} - -fn update_incremental_mesh_chunk( - volume: &mut DepthMeshVolume, - chunk_key: ChunkKey, - mesh: Option, -) -> MeshChunkUpdate { - let existing_index = volume - .mesh_chunks - .iter() - .position(|chunk| chunk.chunk_key == chunk_key); - let new_chunk = mesh - .and_then(|mesh| depth_mesh_chunk_from_surface_mesh(chunk_key, volume.generation, mesh)); - match (existing_index, new_chunk) { - (Some(index), Some(chunk)) => { - volume.mesh_chunks[index] = chunk; - MeshChunkUpdate::Upserted - } - (Some(index), None) => { - volume.mesh_chunks.swap_remove(index); - MeshChunkUpdate::Removed - } - (None, Some(chunk)) => { - volume.mesh_chunks.push(chunk); - MeshChunkUpdate::Upserted - } - (None, None) => MeshChunkUpdate::Unchanged, - } -} - -#[derive(Clone, Copy, Debug, PartialEq, Eq, Hash)] -enum ExtractedPlaneGroup { - HorizontalUp, - HorizontalDown, - Vertical, -} - -#[derive(Clone, Debug)] -struct ExtractedPlaneTriangle { - source_triangle_index: usize, - group: ExtractedPlaneGroup, - area: f32, - normal: Vec3f, - centroid: Vec3f, - vertices: [Vec3f; 3], -} - -#[derive(Clone, Debug)] -struct SimplifiedPlaneRegion { - group: ExtractedPlaneGroup, - source_triangle_indices: Vec, - triangles: Vec, -} - -#[derive(Clone, Copy, Debug, PartialEq, Eq, Hash)] -struct ExtractedPlaneVertexKey { - x: i32, - y: i32, - z: i32, -} - -#[derive(Clone, Copy, Debug)] -struct OrientedRect2 { - center: Vec2f, - axis_u: Vec2f, - axis_v: Vec2f, - half_u: f32, - half_v: f32, -} - -#[derive(Clone, Copy, Debug)] -struct PlanarRectCoverage { - min_u: f32, - max_u: f32, - min_v: f32, - max_v: f32, -} - -#[derive(Clone, Debug)] -struct EmittedPlanarRegion { - tangent: Vec3f, - bitangent: Vec3f, - rects: Vec, -} - -#[derive(Clone, Debug, Default)] -struct PlaneSupportMask { - cells: HashMap, -} - -#[derive(Clone, Copy, Debug, Default, PartialEq, Eq, Hash)] -struct PlaneSupportCellKey { - u: i32, - v: i32, -} - -#[derive(Clone, Debug, Default)] -struct PlaneSupportComponent { - cells: Vec, - min_u: i32, - max_u: i32, - min_v: i32, - max_v: i32, -} - -#[derive(Clone, Copy, Debug, PartialEq, Eq, Hash)] -enum TsdfPlaneScanGroup { - Vertical, -} - -#[derive(Clone, Copy, Debug, PartialEq, Eq, Hash)] -struct TsdfPlaneScanBinKey { - group: TsdfPlaneScanGroup, - plane_bin: i32, - normal_x_bin: i32, - normal_z_bin: i32, -} - -#[derive(Clone, Debug)] -struct TsdfPlaneScanBin { - normal_sum: Vec3f, - point_sum: Vec3f, - point_weight_sum: f32, - support_mask: PlaneSupportMask, - support_sample_count: usize, - support_samples: Vec, -} - -#[derive(Clone, Copy, Debug)] -struct TsdfPlaneSupportSample { - point: Vec3f, - cell: PlaneSupportCellKey, -} - -impl Default for TsdfPlaneScanBin { - fn default() -> Self { - Self { - normal_sum: Vec3f::default(), - point_sum: Vec3f::default(), - point_weight_sum: 0.0, - support_mask: PlaneSupportMask::default(), - support_sample_count: 0, - support_samples: Vec::new(), - } - } -} - -fn rebuild_reduced_planar_patches(volume: &mut DepthMeshVolume) -> bool { - let mut plane_patches = volume - .mesh_chunks - .iter() - .flat_map(|chunk| chunk.planar_patches.iter().cloned()) - .collect::>(); - classify_plane_patch_kinds(&mut plane_patches); - plane_patches.sort_by(|a, b| { - b.area - .partial_cmp(&a.area) - .unwrap_or(std::cmp::Ordering::Equal) - }); - plane_patches.truncate(DEPTH_PLANE_MAX_PATCHES); - - let changed = plane_patches.len() != volume.plane_patches.len() - || plane_patches - .iter() - .zip(volume.plane_patches.iter()) - .any(|(next, current)| { - next.kind != current.kind - || (next.center - current.center).length() > 1.0e-4 - || next.normal.dot(current.normal) < 0.999 - || (next.half_extent_tangent - current.half_extent_tangent).abs() > 1.0e-4 - || (next.half_extent_bitangent - current.half_extent_bitangent).abs() > 1.0e-4 - }); - if !changed { - return false; - } - - let next_generation = volume.plane_generation.saturating_add(1); - for patch in &mut plane_patches { - patch.generation = next_generation; - } - volume.plane_patches = plane_patches; - volume.plane_generation = next_generation; - volume.update_sequence = volume.update_sequence.saturating_add(1); - true -} - -fn plane_patch_matches(left: &XrDepthPlanePatch, right: &XrDepthPlanePatch) -> bool { - left.kind == right.kind - && (left.center - right.center).length() <= 1.0e-4 - && left.normal.dot(right.normal) >= 0.999 - && left.tangent.dot(right.tangent) >= 0.999 - && left.bitangent.dot(right.bitangent) >= 0.999 - && (left.half_extent_tangent - right.half_extent_tangent).abs() <= 1.0e-4 - && (left.half_extent_bitangent - right.half_extent_bitangent).abs() <= 1.0e-4 -} - -fn scanned_plane_chunk_matches(left: &ScannedPlaneChunk, right: &ScannedPlaneChunk) -> bool { - left.near_surface_voxel_count == right.near_surface_voxel_count - && left.floor_candidate_count == right.floor_candidate_count - && left.wall_candidate_count == right.wall_candidate_count - && left.patches.len() == right.patches.len() - && left - .patches - .iter() - .zip(right.patches.iter()) - .all(|(left, right)| plane_patch_matches(left, right)) -} - -fn lerp_metric(from: f32, to: f32, alpha: f32) -> f32 { - from + (to - from) * alpha.clamp(0.0, 1.0) -} - -fn stable_wall_patch_tracking_score( - track: &XrDepthPlanePatch, - candidate: &XrDepthPlanePatch, -) -> Option { - let aligned_normal = align_direction(track.normal, candidate.normal); - let normal_dot = track.normal.dot(aligned_normal); - if normal_dot < DEPTH_PLANE_STABLE_TRACK_MATCH_NORMAL_DOT { - return None; - } - - let track_plane_distance = track.center.dot(track.normal); - let candidate_plane_distance = candidate.center.dot(track.normal); - let plane_delta = (track_plane_distance - candidate_plane_distance).abs(); - if plane_delta > DEPTH_PLANE_STABLE_TRACK_MATCH_DISTANCE_METERS { - return None; - } - - let (track_min_t, track_max_t, track_min_b, track_max_b) = - plane_patch_projected_extents(track, track.tangent, track.bitangent); - let (cand_min_t, cand_max_t, cand_min_b, cand_max_b) = - project_patch_extents_to_basis(candidate, track.tangent, track.bitangent); - let height_gap = interval_gap(track_min_t, track_max_t, cand_min_t, cand_max_t); - let along_gap = interval_gap(track_min_b, track_max_b, cand_min_b, cand_max_b); - if height_gap > DEPTH_PLANE_STABLE_TRACK_MATCH_HEIGHT_GAP_METERS - || along_gap > DEPTH_PLANE_STABLE_TRACK_MATCH_ALONG_GAP_METERS - { - return None; - } - - let area_ratio = - (track.area.min(candidate.area) / track.area.max(candidate.area).max(0.05)).clamp(0.0, 1.0); - let gap_score = - (1.0 - along_gap / DEPTH_PLANE_STABLE_TRACK_MATCH_ALONG_GAP_METERS).clamp(0.0, 1.0); - let height_score = - (1.0 - height_gap / DEPTH_PLANE_STABLE_TRACK_MATCH_HEIGHT_GAP_METERS).clamp(0.0, 1.0); - Some( - normal_dot.powf(2.0) - * (-plane_delta / 0.05).exp() - * (0.35 + 0.65 * area_ratio) - * (0.30 + 0.70 * gap_score) - * (0.35 + 0.65 * height_score), - ) -} - -fn smooth_stable_wall_patch( - current: &XrDepthPlanePatch, - measurement: &XrDepthPlanePatch, -) -> XrDepthPlanePatch { - let alpha = DEPTH_PLANE_STABLE_TRACK_SMOOTH_ALPHA; - let aligned_normal = align_direction(current.normal, measurement.normal); - let blended_normal = current.normal.scale(1.0 - alpha) + aligned_normal.scale(alpha); - let normal = if blended_normal.length() > 1.0e-5 { - blended_normal.normalize() - } else { - current.normal - }; - let tangent = vec3f(0.0, 1.0, 0.0); - let bitangent = Vec3f::cross(normal, tangent).normalize(); - let bitangent = if bitangent.length() > 1.0e-5 { - bitangent - } else { - current.bitangent - }; - - let (cur_min_t, cur_max_t, cur_min_b, cur_max_b) = - project_patch_extents_to_basis(current, tangent, bitangent); - let (meas_min_t, meas_max_t, meas_min_b, meas_max_b) = - project_patch_extents_to_basis(measurement, tangent, bitangent); - let min_t = lerp_metric(cur_min_t, meas_min_t, alpha); - let max_t = lerp_metric(cur_max_t, meas_max_t, alpha); - let min_b = lerp_metric(cur_min_b, meas_min_b, alpha); - let max_b = lerp_metric(cur_max_b, meas_max_b, alpha); - let plane_distance = lerp_metric( - current.center.dot(normal), - measurement.center.dot(normal), - alpha, - ); - let center_t = (min_t + max_t) * 0.5; - let center_b = (min_b + max_b) * 0.5; - let half_extent_tangent = ((max_t - min_t) * 0.5).max(0.0); - let half_extent_bitangent = ((max_b - min_b) * 0.5).max(0.0); - XrDepthPlanePatch { - generation: current.generation, - kind: XrDepthPlaneKind::Wall, - center: normal.scale(plane_distance) + tangent.scale(center_t) + bitangent.scale(center_b), - normal, - tangent, - bitangent, - half_extent_tangent, - half_extent_bitangent, - area: (half_extent_tangent * 2.0) * (half_extent_bitangent * 2.0), - support_triangles: current.support_triangles.max(measurement.support_triangles), - } -} - -fn stabilize_wall_scan_patches( - tracks: &mut Vec, - patches: Vec, -) -> Vec { - let mut candidate_pairs = Vec::<(f32, usize, usize)>::new(); - for (track_index, track) in tracks.iter().enumerate() { - for (patch_index, patch) in patches.iter().enumerate() { - if let Some(score) = stable_wall_patch_tracking_score(&track.patch, patch) { - candidate_pairs.push((score, track_index, patch_index)); - } - } - } - candidate_pairs.sort_by(|a, b| b.0.total_cmp(&a.0)); - - let mut matched_tracks = vec![false; tracks.len()]; - let mut matched_patches = vec![false; patches.len()]; - let mut next_tracks = Vec::::with_capacity(tracks.len() + patches.len()); - - for (_, track_index, patch_index) in candidate_pairs { - if matched_tracks[track_index] || matched_patches[patch_index] { - continue; - } - matched_tracks[track_index] = true; - matched_patches[patch_index] = true; - let track = &tracks[track_index]; - next_tracks.push(StableWallPatchTrack { - patch: smooth_stable_wall_patch(&track.patch, &patches[patch_index]), - seen_count: track.seen_count.saturating_add(1), - missing_updates: 0, - }); - } - - for (track_index, track) in tracks.iter().enumerate() { - if matched_tracks[track_index] { - continue; - } - if track.missing_updates < DEPTH_PLANE_STABLE_TRACK_MISSING_UPDATES_MAX { - let mut track = track.clone(); - track.missing_updates = track.missing_updates.saturating_add(1); - next_tracks.push(track); - } - } - - for (patch_index, patch) in patches.into_iter().enumerate() { - if matched_patches[patch_index] { - continue; - } - next_tracks.push(StableWallPatchTrack { - patch, - seen_count: 1, - missing_updates: 0, - }); - } - - next_tracks.sort_by(|a, b| b.patch.area.total_cmp(&a.patch.area)); - if next_tracks.len() > DEPTH_PLANE_MAX_PATCHES * 2 { - next_tracks.truncate(DEPTH_PLANE_MAX_PATCHES * 2); - } - - let mut visible = next_tracks - .iter() - .filter(|track| { - track.seen_count >= DEPTH_PLANE_STABLE_TRACK_MIN_SEEN_COUNT - || track.missing_updates == 0 - }) - .map(|track| track.patch.clone()) - .collect::>(); - visible.sort_by(|a, b| b.area.total_cmp(&a.area)); - if visible.len() > DEPTH_PLANE_MAX_PATCHES { - visible.truncate(DEPTH_PLANE_MAX_PATCHES); - } - - *tracks = next_tracks; - visible -} - -fn visible_stable_wall_patches(tracks: &[StableWallPatchTrack]) -> Vec { - let mut visible = tracks - .iter() - .filter(|track| { - track.seen_count >= DEPTH_PLANE_STABLE_TRACK_MIN_SEEN_COUNT - || track.missing_updates == 0 - }) - .map(|track| track.patch.clone()) - .collect::>(); - visible.sort_by(|a, b| b.area.total_cmp(&a.area)); - if visible.len() > DEPTH_PLANE_MAX_PATCHES * 2 { - visible.truncate(DEPTH_PLANE_MAX_PATCHES * 2); - } - visible -} - -fn canonicalize_room_axis(axis: Vec3f) -> Option { - let axis = vec3f(axis.x, 0.0, axis.z); - let axis = if axis.length() > 1.0e-5 { - axis.normalize() - } else { - return None; - }; - if axis.x < -1.0e-5 || (axis.x.abs() <= 1.0e-5 && axis.z < 0.0) { - Some(axis.scale(-1.0)) - } else { - Some(axis) - } -} - -fn orthogonal_room_axis(axis: Vec3f) -> Vec3f { - vec3f(-axis.z, 0.0, axis.x).normalize() -} - -fn refine_room_wall_axis( - patches: &[XrDepthPlanePatch], - seed_axis: Vec3f, - threshold_dot: f32, -) -> Vec3f { - let mut axis_sum = Vec3f::default(); - let mut found = false; - for patch in patches { - let Some(axis) = canonicalize_room_axis(patch.normal) else { - continue; - }; - if axis.dot(seed_axis).abs() < threshold_dot { - continue; - } - let aligned_axis = if axis.dot(seed_axis) < 0.0 { - axis.scale(-1.0) - } else { - axis - }; - axis_sum += aligned_axis.scale(patch.area.max(0.05)); - found = true; - } - if found && axis_sum.length() > 1.0e-5 { - canonicalize_room_axis(axis_sum).unwrap_or(seed_axis) - } else { - seed_axis - } -} - -fn infer_room_wall_axes(patches: &[XrDepthPlanePatch]) -> Option<[Vec3f; 2]> { - let primary_seed = patches - .iter() - .filter_map(|patch| canonicalize_room_axis(patch.normal).map(|axis| (patch.area, axis))) - .max_by(|left, right| left.0.total_cmp(&right.0)) - .map(|(_, axis)| axis)?; - let primary = refine_room_wall_axis(patches, primary_seed, 0.82); - let secondary_seed = patches - .iter() - .filter_map(|patch| { - let axis = canonicalize_room_axis(patch.normal)?; - (axis.dot(primary).abs() <= DEPTH_ROOM_WALL_ORTHOGONAL_DOT_MAX) - .then_some((patch.area, axis)) - }) - .max_by(|left, right| left.0.total_cmp(&right.0)) - .map(|(_, axis)| axis) - .unwrap_or_else(|| orthogonal_room_axis(primary)); - let mut secondary = refine_room_wall_axis(patches, secondary_seed, 0.82); - if secondary.dot(primary).abs() > DEPTH_ROOM_WALL_ORTHOGONAL_DOT_MAX { - secondary = orthogonal_room_axis(primary); - } - Some([primary, secondary]) -} - -fn accumulate_room_wall_patch( - accumulator: &mut RoomWallAccumulator, - wall_normal: Vec3f, - patch: &XrDepthPlanePatch, -) { - let weight = patch.area.max(0.05); - if accumulator.weight_sum <= 0.0 { - accumulator.normal = wall_normal; - } - let along_axis = vec3f(-wall_normal.z, 0.0, wall_normal.x).normalize(); - let (_, _, along_min, along_max) = - project_patch_extents_to_basis(patch, vec3f(0.0, 1.0, 0.0), along_axis); - accumulator.weight_sum += weight; - accumulator.plane_distance_sum += patch.center.dot(wall_normal) * weight; - accumulator.support_area += patch.area.max(0.0); - accumulator.observed_along_min = accumulator.observed_along_min.min(along_min); - accumulator.observed_along_max = accumulator.observed_along_max.max(along_max); -} - -fn build_room_wall_candidates(patches: &[XrDepthPlanePatch]) -> Vec { - let Some(room_axes) = infer_room_wall_axes(patches) else { - return Vec::new(); - }; - let mut slot_groups = [ - Vec::::new(), - Vec::::new(), - Vec::::new(), - Vec::::new(), - ]; - for patch in patches { - let Some(axis) = canonicalize_room_axis(patch.normal) else { - continue; - }; - let primary_dot = axis.dot(room_axes[0]).abs(); - let secondary_dot = axis.dot(room_axes[1]).abs(); - let (axis_index, best_dot) = if primary_dot >= secondary_dot { - (0usize, primary_dot) - } else { - (1usize, secondary_dot) - }; - if best_dot < DEPTH_ROOM_WALL_AXIS_ASSIGN_DOT_MIN { - continue; - } - let axis = room_axes[axis_index]; - let axis_projection = patch.center.dot(axis); - let normal_projection = patch.normal.dot(axis); - let side_sign = if axis_projection.abs() >= 0.12 { - if axis_projection >= 0.0 { - 1.0 - } else { - -1.0 - } - } else if normal_projection >= 0.0 { - 1.0 - } else { - -1.0 - }; - let wall_normal = axis.scale(side_sign); - let slot_index = axis_index * 2 + if side_sign >= 0.0 { 1 } else { 0 }; - let plane_distance = patch.center.dot(wall_normal); - let mut best_group = None::<(usize, f32)>; - for (group_index, group) in slot_groups[slot_index].iter().enumerate() { - if group.weight_sum <= 0.0 { - continue; - } - let group_distance = group.plane_distance_sum / group.weight_sum.max(1.0e-5); - let delta = (group_distance - plane_distance).abs(); - if delta > DEPTH_ROOM_WALL_GROUP_DISTANCE_METERS { - continue; - } - if best_group.is_none_or(|(_, best_delta)| delta < best_delta) { - best_group = Some((group_index, delta)); - } - } - if let Some((group_index, _)) = best_group { - accumulate_room_wall_patch( - &mut slot_groups[slot_index][group_index], - wall_normal, - patch, - ); - } else { - let mut group = RoomWallAccumulator::default(); - accumulate_room_wall_patch(&mut group, wall_normal, patch); - slot_groups[slot_index].push(group); - } - } - - let mut candidates = Vec::::new(); - for groups in slot_groups { - let Some(accumulator) = groups - .into_iter() - .max_by(|left, right| left.support_area.total_cmp(&right.support_area)) - else { - continue; - }; - if accumulator.weight_sum <= 0.0 - || !accumulator.observed_along_min.is_finite() - || !accumulator.observed_along_max.is_finite() - { - continue; - } - candidates.push(RoomWallCandidate { - normal: accumulator.normal, - plane_distance: accumulator.plane_distance_sum / accumulator.weight_sum.max(1.0e-5), - observed_along_min: accumulator.observed_along_min, - observed_along_max: accumulator.observed_along_max, - support_area: accumulator.support_area.max(0.0), - }); - } - candidates.sort_by(|left, right| right.support_area.total_cmp(&left.support_area)); - candidates -} - -fn room_wall_track_score( - track: &StableRoomWallTrack, - candidate: &RoomWallCandidate, -) -> Option { - let normal_dot = track.normal.dot(candidate.normal); - if normal_dot < DEPTH_ROOM_WALL_TRACK_MATCH_NORMAL_DOT { - return None; - } - let plane_delta = (track.plane_distance - candidate.plane_distance).abs(); - if plane_delta > DEPTH_ROOM_WALL_TRACK_MATCH_DISTANCE_METERS { - return None; - } - let current_span = (track.observed_along_max - track.observed_along_min).max(0.05); - let candidate_span = (candidate.observed_along_max - candidate.observed_along_min).max(0.05); - let span_ratio = - (current_span.min(candidate_span) / current_span.max(candidate_span)).clamp(0.0, 1.0); - Some( - normal_dot.powf(2.0) - * (-plane_delta / 0.08).exp() - * (0.35 + 0.65 * span_ratio) - * (0.30 - + 0.70 - * (track.support_area.min(candidate.support_area) - / track.support_area.max(candidate.support_area).max(0.05)) - .clamp(0.0, 1.0)), - ) -} - -fn smooth_room_wall_extent_bound(current: f32, measurement: f32, expands_when_lower: bool) -> f32 { - let expands = if expands_when_lower { - measurement < current - } else { - measurement > current - }; - let alpha = if expands { - DEPTH_ROOM_WALL_TRACK_EXPAND_ALPHA - } else { - DEPTH_ROOM_WALL_TRACK_SHRINK_ALPHA - }; - lerp_metric(current, measurement, alpha) -} - -fn smooth_room_wall_track( - current: &StableRoomWallTrack, - measurement: &RoomWallCandidate, -) -> StableRoomWallTrack { - let alpha = DEPTH_ROOM_WALL_TRACK_SMOOTH_ALPHA; - let aligned_normal = align_direction(current.normal, measurement.normal); - let blended_normal = current.normal.scale(1.0 - alpha) + aligned_normal.scale(alpha); - let normal = if blended_normal.length() > 1.0e-5 { - blended_normal.normalize() - } else { - current.normal - }; - StableRoomWallTrack { - normal, - plane_distance: lerp_metric(current.plane_distance, measurement.plane_distance, alpha), - observed_along_min: smooth_room_wall_extent_bound( - current.observed_along_min, - measurement.observed_along_min, - true, - ), - observed_along_max: smooth_room_wall_extent_bound( - current.observed_along_max, - measurement.observed_along_max, - false, - ), - support_area: lerp_metric(current.support_area, measurement.support_area, alpha).max(0.05), - seen_count: current.seen_count.saturating_add(1), - missing_updates: 0, - } -} - -fn stabilize_room_wall_candidates( - tracks: &mut Vec, - candidates: Vec, -) { - let mut candidate_pairs = Vec::<(f32, usize, usize)>::new(); - for (track_index, track) in tracks.iter().enumerate() { - for (candidate_index, candidate) in candidates.iter().enumerate() { - if let Some(score) = room_wall_track_score(track, candidate) { - candidate_pairs.push((score, track_index, candidate_index)); - } - } - } - candidate_pairs.sort_by(|left, right| right.0.total_cmp(&left.0)); - - let mut matched_tracks = vec![false; tracks.len()]; - let mut matched_candidates = vec![false; candidates.len()]; - let mut next_tracks = - Vec::::with_capacity(tracks.len() + candidates.len()); - - for (_, track_index, candidate_index) in candidate_pairs { - if matched_tracks[track_index] || matched_candidates[candidate_index] { - continue; - } - matched_tracks[track_index] = true; - matched_candidates[candidate_index] = true; - next_tracks.push(smooth_room_wall_track( - &tracks[track_index], - &candidates[candidate_index], - )); - } - - for (track_index, track) in tracks.iter().enumerate() { - if matched_tracks[track_index] { - continue; - } - if track.missing_updates < DEPTH_ROOM_WALL_TRACK_MISSING_UPDATES_MAX { - let mut track = track.clone(); - track.missing_updates = track.missing_updates.saturating_add(1); - next_tracks.push(track); - } - } - - for (candidate_index, candidate) in candidates.into_iter().enumerate() { - if matched_candidates[candidate_index] { - continue; - } - if tracks.len() >= DEPTH_ROOM_WALL_MAX_TRACKS { - continue; - } - next_tracks.push(StableRoomWallTrack { - normal: candidate.normal, - plane_distance: candidate.plane_distance, - observed_along_min: candidate.observed_along_min, - observed_along_max: candidate.observed_along_max, - support_area: candidate.support_area.max(0.05), - seen_count: 1, - missing_updates: 0, - }); - } - - next_tracks.sort_by(|left, right| { - right - .seen_count - .cmp(&left.seen_count) - .then_with(|| left.missing_updates.cmp(&right.missing_updates)) - .then_with(|| right.support_area.total_cmp(&left.support_area)) - }); - if next_tracks.len() > DEPTH_ROOM_WALL_MAX_TRACKS { - next_tracks.truncate(DEPTH_ROOM_WALL_MAX_TRACKS); - } - - *tracks = next_tracks; -} - -fn visible_room_wall_candidates(tracks: &[StableRoomWallTrack]) -> Vec { - let mut visible = tracks - .iter() - .map(|track| RoomWallCandidate { - normal: track.normal, - plane_distance: track.plane_distance, - observed_along_min: track.observed_along_min, - observed_along_max: track.observed_along_max, - support_area: track.support_area, - }) - .collect::>(); - visible.sort_by(|left, right| right.support_area.total_cmp(&left.support_area)); - if visible.len() > DEPTH_ROOM_WALL_MAX_TRACKS { - visible.truncate(DEPTH_ROOM_WALL_MAX_TRACKS); - } - visible -} - -fn room_wall_feature_from_candidate( - candidate: &RoomWallCandidate, - all_walls: &[RoomWallCandidate], - wall_index: usize, -) -> Option { - let normal = if candidate.normal.length() > 1.0e-5 { - candidate.normal.normalize() - } else { - return None; - }; - let along_axis = vec3f(-normal.z, 0.0, normal.x); - let along_axis = if along_axis.length() > 1.0e-5 { - along_axis.normalize() - } else { - return None; - }; - let mut along_min = candidate.observed_along_min; - let mut along_max = candidate.observed_along_max; - let mut intersection_min = f32::INFINITY; - let mut intersection_max = f32::NEG_INFINITY; - let mut intersection_count = 0usize; - for (other_index, other) in all_walls.iter().enumerate() { - if other_index == wall_index { - continue; - } - if other.normal.dot(normal).abs() > DEPTH_ROOM_WALL_ORTHOGONAL_DOT_MAX { - continue; - } - let coordinate = other.normal.dot(along_axis) * other.plane_distance; - if !coordinate.is_finite() { - continue; - } - intersection_min = intersection_min.min(coordinate); - intersection_max = intersection_max.max(coordinate); - intersection_count += 1; - } - if intersection_count >= 2 { - along_min = intersection_min; - along_max = intersection_max; - } else if intersection_count == 1 { - along_min = along_min.min(intersection_min); - along_max = along_max.max(intersection_max); - } - if !along_min.is_finite() || !along_max.is_finite() { - return None; - } - let half_extent_along = ((along_max - along_min) * 0.5).max(0.25); - let along_center = (along_min + along_max) * 0.5; - Some(XrDepthAlignWallFeature { - center: normal.scale(candidate.plane_distance) - + along_axis.scale(along_center) - + vec3f(0.0, DEPTH_ROOM_WALL_HEIGHT_METERS * 0.5, 0.0), - normal, - along_axis, - plane_distance: candidate.plane_distance, - half_extent_along, - min_y: 0.0, - max_y: DEPTH_ROOM_WALL_HEIGHT_METERS, - area: candidate.support_area.max(0.25), - }) -} - -fn visible_room_wall_features(tracks: &[StableRoomWallTrack]) -> Vec { - let walls = visible_room_wall_candidates(tracks); - let mut features = Vec::::new(); - for (index, wall) in walls.iter().enumerate() { - if let Some(feature) = room_wall_feature_from_candidate(wall, &walls, index) { - features.push(feature); - } - } - features.sort_by(|left, right| right.area.total_cmp(&left.area)); - if features.len() > DEPTH_ALIGN_MAX_WALL_FEATURES { - features.truncate(DEPTH_ALIGN_MAX_WALL_FEATURES); - } - features -} - -fn process_incremental_tsdf_plane_scan(volume: &mut DepthMeshVolume, max_scan_jobs: usize) -> bool { - if volume.mesh_grid.is_empty() { - return volume.reset_plane_scan_state(); - } - if volume.plane_scan_chunks.is_empty() && volume.pending_plane_scan_chunk_queue.is_empty() { - enqueue_all_plane_scan_chunks(volume); - } - - let mut chunks_changed = false; - for _ in 0..max_scan_jobs { - let Some(chunk_key) = volume.pending_plane_scan_chunk_queue.pop_front() else { - break; - }; - volume.pending_plane_scan_dirty_chunks.remove(&chunk_key); - let next_chunk = scan_tsdf_plane_chunk(volume, chunk_key); - match (volume.plane_scan_chunks.get(&chunk_key), next_chunk) { - (Some(current), Some(next)) if scanned_plane_chunk_matches(current, &next) => {} - (_, Some(next)) => { - volume.plane_scan_chunks.insert(chunk_key, next); - chunks_changed = true; - } - (Some(_), None) => { - volume.plane_scan_chunks.remove(&chunk_key); - chunks_changed = true; - } - (None, None) => {} - } - } - - if chunks_changed { - rebuild_tsdf_plane_scan_patches(volume) - } else { - false - } -} - -fn enqueue_all_plane_scan_chunks(volume: &mut DepthMeshVolume) { - let mesh_edge = volume.mesh_config.mesh_chunk_edge_voxels().max(1); - let grid_edge = volume.mesh_grid.chunk_edge.max(1); - let mut keys = HashSet::::new(); - for key in volume.mesh_grid.chunks.keys() { - let min_voxel = VoxelCoord::new(key.x * grid_edge, key.y * grid_edge, key.z * grid_edge); - let max_voxel = VoxelCoord::new( - min_voxel.x + grid_edge - 1, - min_voxel.y + grid_edge - 1, - min_voxel.z + grid_edge - 1, - ); - let min_chunk = VoxelCoord::new( - min_voxel.x.div_euclid(mesh_edge), - min_voxel.y.div_euclid(mesh_edge), - min_voxel.z.div_euclid(mesh_edge), - ); - let max_chunk = VoxelCoord::new( - max_voxel.x.div_euclid(mesh_edge), - max_voxel.y.div_euclid(mesh_edge), - max_voxel.z.div_euclid(mesh_edge), - ); - for z in min_chunk.z..=max_chunk.z { - for y in min_chunk.y..=max_chunk.y { - for x in min_chunk.x..=max_chunk.x { - keys.insert(ChunkKey::new(x, y, z)); - } - } - } - } - let mut keys = keys.into_iter().collect::>(); - keys.sort_by_key(|key| (key.x, key.y, key.z)); - for key in keys { - if volume.pending_plane_scan_dirty_chunks.insert(key) { - volume.pending_plane_scan_chunk_queue.push_back(key); - } - } -} - -fn rebuild_tsdf_plane_scan_patches(volume: &mut DepthMeshVolume) -> bool { - let mut plane_patches = volume - .plane_scan_chunks - .values() - .flat_map(|chunk| chunk.patches.iter().cloned()) - .collect::>(); - plane_patches = merge_tsdf_plane_patches(plane_patches); - classify_plane_patch_kinds(&mut plane_patches); - plane_patches.retain(|patch| patch.kind == XrDepthPlaneKind::Wall); - plane_patches.sort_by(|a, b| { - b.area - .partial_cmp(&a.area) - .unwrap_or(std::cmp::Ordering::Equal) - }); - plane_patches.truncate(DEPTH_PLANE_MAX_PATCHES); - plane_patches = stabilize_wall_scan_patches(&mut volume.stable_wall_tracks, plane_patches); - stabilize_room_wall_candidates( - &mut volume.room_wall_tracks, - build_room_wall_candidates(&plane_patches), - ); - let changed = plane_patches.len() != volume.plane_patches.len() - || plane_patches - .iter() - .zip(volume.plane_patches.iter()) - .any(|(next, current)| !plane_patch_matches(next, current)); - if !changed { - return false; - } - - let next_generation = volume.plane_generation.saturating_add(1); - for patch in &mut plane_patches { - patch.generation = next_generation; - } - volume.plane_patches = plane_patches; - volume.plane_generation = next_generation; - volume.update_sequence = volume.update_sequence.saturating_add(1); - true -} - -fn merge_tsdf_plane_patches(mut patches: Vec) -> Vec { - patches.sort_by(|a, b| { - b.area - .partial_cmp(&a.area) - .unwrap_or(std::cmp::Ordering::Equal) - }); - - let mut merged = Vec::::new(); - 'outer: for patch in patches { - for existing in &mut merged { - if let Some(next) = try_merge_tsdf_plane_patches(existing, &patch) { - *existing = next; - continue 'outer; - } - } - merged.push(patch); - } - merged -} - -fn try_merge_tsdf_plane_patches( - left: &XrDepthPlanePatch, - right: &XrDepthPlanePatch, -) -> Option { - let aligned_normal = align_direction(left.normal, right.normal); - if left.normal.dot(aligned_normal) < DEPTH_PLANE_SCAN_MERGE_NORMAL_DOT { - return None; - } - - let left_distance = left.center.dot(left.normal); - let right_distance = right.center.dot(left.normal); - if (left_distance - right_distance).abs() > DEPTH_PLANE_SCAN_MERGE_DISTANCE_METERS { - return None; - } - - let (left_min_u, left_max_u, left_min_v, left_max_v) = - plane_patch_projected_extents(left, left.tangent, left.bitangent); - let (right_min_u, right_max_u, right_min_v, right_max_v) = - project_patch_extents_to_basis(right, left.tangent, left.bitangent); - let gap_u = interval_gap(left_min_u, left_max_u, right_min_u, right_max_u); - let gap_v = interval_gap(left_min_v, left_max_v, right_min_v, right_max_v); - if gap_u > DEPTH_PLANE_SCAN_MERGE_GAP_METERS || gap_v > DEPTH_PLANE_SCAN_MERGE_GAP_METERS { - return None; - } - if gap_u > 0.0 && gap_v > 0.0 { - return None; - } - - let min_u = left_min_u.min(right_min_u); - let max_u = left_max_u.max(right_max_u); - let min_v = left_min_v.min(right_min_v); - let max_v = left_max_v.max(right_max_v); - let width = max_u - min_u; - let height = max_v - min_v; - if width < DEPTH_PLANE_SCAN_VERTICAL_MIN_DIM_METERS - || height < DEPTH_PLANE_SCAN_VERTICAL_MIN_DIM_METERS - || width * height < DEPTH_PLANE_SCAN_VERTICAL_MIN_AREA_METERS2 - { - return None; - } - - let plane_distance = (left_distance * left.area.max(0.001) - + right_distance * right.area.max(0.001)) - / (left.area.max(0.001) + right.area.max(0.001)); - let center_u = (min_u + max_u) * 0.5; - let center_v = (min_v + max_v) * 0.5; - Some(XrDepthPlanePatch { - generation: 0, - kind: classify_planar_patch_kind_from_normal(left.normal), - center: left.normal.scale(plane_distance) - + left.tangent.scale(center_u) - + left.bitangent.scale(center_v), - normal: left.normal, - tangent: left.tangent, - bitangent: left.bitangent, - half_extent_tangent: width * 0.5, - half_extent_bitangent: height * 0.5, - area: width * height, - support_triangles: left.support_triangles + right.support_triangles, - }) -} - -fn plane_patch_projected_extents( - patch: &XrDepthPlanePatch, - tangent: Vec3f, - bitangent: Vec3f, -) -> (f32, f32, f32, f32) { - let corners = plane_patch_corners(patch); - let mut min_u = f32::INFINITY; - let mut max_u = f32::NEG_INFINITY; - let mut min_v = f32::INFINITY; - let mut max_v = f32::NEG_INFINITY; - for corner in corners { - let u = corner.dot(tangent); - let v = corner.dot(bitangent); - min_u = min_u.min(u); - max_u = max_u.max(u); - min_v = min_v.min(v); - max_v = max_v.max(v); - } - (min_u, max_u, min_v, max_v) -} - -fn project_patch_extents_to_basis( - patch: &XrDepthPlanePatch, - tangent: Vec3f, - bitangent: Vec3f, -) -> (f32, f32, f32, f32) { - plane_patch_projected_extents(patch, tangent, bitangent) -} - -fn interval_gap(left_min: f32, left_max: f32, right_min: f32, right_max: f32) -> f32 { - if left_max < right_min { - right_min - left_max - } else if right_max < left_min { - left_min - right_max - } else { - 0.0 - } -} - -fn scan_tsdf_plane_chunk( - volume: &DepthMeshVolume, - chunk_key: ChunkKey, -) -> Option { - let near_surface_band = (volume.voxel_size_meters * DEPTH_PLANE_SCAN_NEAR_SURFACE_BAND_SCALE) - .max(DEPTH_PLANE_SCAN_NEAR_SURFACE_BAND_MIN); - let tsd_distance_meters = depth_tsd_distance_meters(volume.voxel_size_meters); - let edge = volume.mesh_config.mesh_chunk_edge_voxels().max(1); - let origin = VoxelCoord::new(chunk_key.x * edge, chunk_key.y * edge, chunk_key.z * edge); - let mut bins = HashMap::::new(); - let mut near_surface_voxel_count = 0u32; - let floor_candidate_count = 0u32; - let mut wall_candidate_count = 0u32; - - for local_z in 0..edge { - for local_y in 0..edge { - for local_x in 0..edge { - let coord = origin + VoxelCoord::new(local_x, local_y, local_z); - if volume.mesh_grid.confidence(coord) < DEPTH_PLANE_SCAN_MIN_CONFIDENCE { - continue; - } - let Some(distance) = volume.mesh_grid.normalized_distance(coord) else { - continue; - }; - let signed_distance = distance * tsd_distance_meters; - if signed_distance.abs() > near_surface_band { - continue; - } - let Some(surface_normal) = query_depth_grid_normal_at_coord(volume, coord) else { - continue; - }; - near_surface_voxel_count = near_surface_voxel_count.saturating_add(1); - let point = volume.mesh_grid.voxel_center_world(coord) - - surface_normal.scale(signed_distance); - let Some((key, plane_normal, tangent, bitangent)) = - tsdf_plane_scan_key(point, surface_normal) - else { - continue; - }; - wall_candidate_count = wall_candidate_count.saturating_add(1); - let bin = bins.entry(key).or_default(); - let aligned_normal = align_direction(plane_normal, surface_normal); - let weight = volume.mesh_grid.confidence(coord) as f32; - bin.normal_sum += aligned_normal.scale(weight); - bin.point_sum += point.scale(weight); - bin.point_weight_sum += weight; - bin.support_sample_count += 1; - let support_u = point.dot(tangent); - let support_v = point.dot(bitangent); - let cell = stamp_plane_support_sample(&mut bin.support_mask, support_u, support_v); - bin.support_samples - .push(TsdfPlaneSupportSample { point, cell }); - } - } - } - - if near_surface_voxel_count == 0 { - return None; - } - - let mut patches = Vec::new(); - for (key, bin) in bins { - patches.extend(emit_tsdf_plane_scan_bin_patches(key, &bin)); - } - patches.sort_by(|a, b| { - b.area - .partial_cmp(&a.area) - .unwrap_or(std::cmp::Ordering::Equal) - }); - - Some(ScannedPlaneChunk { - patches, - near_surface_voxel_count, - floor_candidate_count, - wall_candidate_count, - }) -} - -fn tsdf_plane_scan_key( - point: Vec3f, - normal: Vec3f, -) -> Option<(TsdfPlaneScanBinKey, Vec3f, Vec3f, Vec3f)> { - let canonical_normal = canonicalize_vertical_plane_normal(normal)?; - if canonical_normal.y.abs() > DEPTH_PLANE_VERTICAL_NORMAL_Y_MAX { - return None; - } - let tangent = vec3f(0.0, 1.0, 0.0); - let bitangent = Vec3f::cross(canonical_normal, tangent).normalize(); - if bitangent.length() <= 1.0e-5 { - return None; - } - let key = TsdfPlaneScanBinKey { - group: TsdfPlaneScanGroup::Vertical, - plane_bin: quantize_f32( - point.dot(canonical_normal), - DEPTH_PLANE_SCAN_VERTICAL_DISTANCE_BIN_METERS, - ), - normal_x_bin: quantize_f32(canonical_normal.x, DEPTH_PLANE_SCAN_VERTICAL_NORMAL_QUANTUM), - normal_z_bin: quantize_f32(canonical_normal.z, DEPTH_PLANE_SCAN_VERTICAL_NORMAL_QUANTUM), - }; - Some((key, canonical_normal, tangent, bitangent)) -} - -fn canonicalize_vertical_plane_normal(normal: Vec3f) -> Option { - let mut canonical = vec3f(normal.x, 0.0, normal.z); - if canonical.length() <= 1.0e-5 { - return None; - } - canonical = canonical.normalize(); - if canonical.x < 0.0 || (canonical.x.abs() <= 1.0e-5 && canonical.z < 0.0) { - canonical = canonical.scale(-1.0); - } - Some(canonical) -} - -fn stamp_plane_support_sample(mask: &mut PlaneSupportMask, u: f32, v: f32) -> PlaneSupportCellKey { - let center = quantize_plane_support_cell(u, v); - for du in -1..=1 { - for dv in -1..=1 { - let add = if du == 0 && dv == 0 { - DEPTH_PLANE_SUPPORT_MAX_WEIGHT - } else if du == 0 || dv == 0 { - DEPTH_PLANE_SUPPORT_OCCUPIED_WEIGHT - } else { - 1 - }; - let key = PlaneSupportCellKey { - u: center.u + du, - v: center.v + dv, - }; - let weight = mask.cells.entry(key).or_insert(0); - *weight = weight - .saturating_add(add) - .min(DEPTH_PLANE_SUPPORT_MAX_WEIGHT); - } - } - center -} - -fn emit_tsdf_plane_scan_bin_patches( - key: TsdfPlaneScanBinKey, - bin: &TsdfPlaneScanBin, -) -> Vec { - let Some((seed_normal, _tangent, _bitangent, _plane_distance)) = - resolve_tsdf_plane_scan_bin_frame(key, bin) - else { - return Vec::new(); - }; - let mut patches = Vec::new(); - for mut component in decompose_support_mask_components(&bin.support_mask) { - let mut emitted_rects = 0usize; - loop { - if emitted_rects >= DEPTH_PLANE_SCAN_MAX_RECTS_PER_GROUP { - break; - } - let Some((min_u, max_u, min_v, max_v)) = largest_supported_rectangle(&component) else { - break; - }; - let width = (max_u - min_u + 1) as f32 * DEPTH_PLANE_SUPPORT_CELL_METERS; - let height = (max_v - min_v + 1) as f32 * DEPTH_PLANE_SUPPORT_CELL_METERS; - if width * height < DEPTH_PLANE_SCAN_VERTICAL_MIN_AREA_METERS2 - || width < DEPTH_PLANE_SCAN_VERTICAL_MIN_DIM_METERS - || height < DEPTH_PLANE_SCAN_VERTICAL_MIN_DIM_METERS - { - break; - } - if let Some(patch) = fit_vertical_wall_patch_from_support_samples( - &bin.support_samples, - min_u, - max_u, - min_v, - max_v, - seed_normal, - ) { - patches.push(patch); - } - emitted_rects += 1; - remove_rect_from_support_component(&mut component, min_u, max_u, min_v, max_v); - if component.cells.is_empty() { - break; - } - } - } - patches -} - -fn resolve_tsdf_plane_scan_bin_frame( - key: TsdfPlaneScanBinKey, - bin: &TsdfPlaneScanBin, -) -> Option<(Vec3f, Vec3f, Vec3f, f32)> { - let reference_normal = vec3f( - key.normal_x_bin as f32 * DEPTH_PLANE_SCAN_VERTICAL_NORMAL_QUANTUM, - 0.0, - key.normal_z_bin as f32 * DEPTH_PLANE_SCAN_VERTICAL_NORMAL_QUANTUM, - ); - let reference_normal = if reference_normal.length() > 1.0e-5 { - reference_normal.normalize() - } else { - vec3f(1.0, 0.0, 0.0) - }; - let normal = if bin.normal_sum.length() > 1.0e-5 { - align_direction(reference_normal, bin.normal_sum.normalize()) - } else { - reference_normal - }; - let tangent = vec3f(0.0, 1.0, 0.0); - let bitangent = Vec3f::cross(normal, tangent).normalize(); - if bitangent.length() <= 1.0e-5 { - return None; - } - let plane_distance = if bin.point_weight_sum > 0.0 { - bin.point_sum - .scale(bin.point_weight_sum.recip()) - .dot(normal) - } else { - key.plane_bin as f32 * DEPTH_PLANE_SCAN_VERTICAL_DISTANCE_BIN_METERS - }; - Some((normal, tangent, bitangent, plane_distance)) -} - -fn fit_vertical_wall_patch_from_support_samples( - support_samples: &[TsdfPlaneSupportSample], - min_u_cell: i32, - max_u_cell: i32, - min_v_cell: i32, - max_v_cell: i32, - seed_normal: Vec3f, -) -> Option { - let mut points = Vec::::new(); - for sample in support_samples { - if sample.cell.u < min_u_cell - || sample.cell.u > max_u_cell - || sample.cell.v < min_v_cell - || sample.cell.v > max_v_cell - { - continue; - } - points.push(sample.point); - } - if points.len() < 4 { - return None; - } - - let tangent = vec3f(0.0, 1.0, 0.0); - let mut centroid = Vec3f::default(); - for point in &points { - centroid += *point; - } - centroid = centroid.scale((points.len() as f32).recip()); - - let mut cov_xx = 0.0; - let mut cov_xz = 0.0; - let mut cov_zz = 0.0; - for point in &points { - let dx = point.x - centroid.x; - let dz = point.z - centroid.z; - cov_xx += dx * dx; - cov_xz += dx * dz; - cov_zz += dz * dz; - } - - let angle = 0.5 * (2.0 * cov_xz).atan2(cov_xx - cov_zz); - let mut bitangent = vec3f(angle.cos(), 0.0, angle.sin()); - if bitangent.length() <= 1.0e-5 { - bitangent = Vec3f::cross(seed_normal, tangent).normalize(); - } else { - bitangent = bitangent.normalize(); - } - if bitangent.length() <= 1.0e-5 { - return None; - } - - let mut normal = Vec3f::cross(tangent, bitangent).normalize(); - if normal.length() <= 1.0e-5 { - return None; - } - normal = align_direction(seed_normal, normal); - bitangent = Vec3f::cross(normal, tangent).normalize(); - if bitangent.length() <= 1.0e-5 { - return None; - } - - let plane_distance = - points.iter().map(|point| normal.dot(*point)).sum::() / points.len() as f32; - let mean_plane_residual = points - .iter() - .map(|point| (normal.dot(*point) - plane_distance).abs()) - .sum::() - / points.len() as f32; - if mean_plane_residual > DEPTH_PLANE_SCAN_VERTICAL_DISTANCE_BIN_METERS { - return None; - } - - let extent_padding = DEPTH_PLANE_SUPPORT_CELL_METERS * 0.5; - let mut min_tangent = f32::INFINITY; - let mut max_tangent = f32::NEG_INFINITY; - let mut min_bitangent = f32::INFINITY; - let mut max_bitangent = f32::NEG_INFINITY; - for point in &points { - let tangent_coord = point.dot(tangent); - let bitangent_coord = point.dot(bitangent); - min_tangent = min_tangent.min(tangent_coord); - max_tangent = max_tangent.max(tangent_coord); - min_bitangent = min_bitangent.min(bitangent_coord); - max_bitangent = max_bitangent.max(bitangent_coord); - } - if !min_tangent.is_finite() - || !max_tangent.is_finite() - || !min_bitangent.is_finite() - || !max_bitangent.is_finite() - { - return None; - } - - min_tangent -= extent_padding; - max_tangent += extent_padding; - min_bitangent -= extent_padding; - max_bitangent += extent_padding; - let height = max_tangent - min_tangent; - let width = max_bitangent - min_bitangent; - if width < DEPTH_PLANE_SCAN_VERTICAL_MIN_DIM_METERS - || height < DEPTH_PLANE_SCAN_VERTICAL_MIN_DIM_METERS - || width * height < DEPTH_PLANE_SCAN_VERTICAL_MIN_AREA_METERS2 - { - return None; - } - - let center_tangent = (min_tangent + max_tangent) * 0.5; - let center_bitangent = (min_bitangent + max_bitangent) * 0.5; - Some(XrDepthPlanePatch { - generation: 0, - kind: XrDepthPlaneKind::Wall, - center: normal.scale(plane_distance) - + tangent.scale(center_tangent) - + bitangent.scale(center_bitangent), - normal, - tangent, - bitangent, - half_extent_tangent: height * 0.5, - half_extent_bitangent: width * 0.5, - area: width * height, - support_triangles: points.len(), - }) -} - -fn simplify_plane_regions( - triangles: Vec, - voxel_size_meters: f32, -) -> Vec { - if triangles.is_empty() { - return Vec::new(); - } - - let mut vertex_links = HashMap::>::new(); - for (index, triangle) in triangles.iter().enumerate() { - for &vertex in &triangle.vertices { - vertex_links - .entry(quantize_plane_vertex(vertex, voxel_size_meters)) - .or_default() - .push(index); - } - } - - let mut visited = vec![false; triangles.len()]; - let mut regions = Vec::new(); - - for start_index in 0..triangles.len() { - if visited[start_index] { - continue; - } - let group = triangles[start_index].group; - let mut queue = VecDeque::from([start_index]); - let mut region_indices = Vec::new(); - let mut normal_sum = triangles[start_index] - .normal - .scale(triangles[start_index].area.max(0.001)); - let mut centroid_sum = triangles[start_index] - .centroid - .scale(triangles[start_index].area.max(0.001)); - let mut area_sum = triangles[start_index].area.max(0.001); - - while let Some(triangle_index) = queue.pop_front() { - if visited[triangle_index] { - continue; - } - let triangle = &triangles[triangle_index]; - if triangle.group != group { - continue; - } - if !planar_region_accepts_triangle(group, triangle, normal_sum, centroid_sum, area_sum) - { - continue; - } - - visited[triangle_index] = true; - region_indices.push(triangle_index); - - let reference_normal = if normal_sum.length() > 1.0e-5 { - normal_sum.normalize() - } else { - triangle.normal - }; - let aligned_normal = align_direction(reference_normal, triangle.normal); - normal_sum += aligned_normal.scale(triangle.area.max(0.001)); - centroid_sum += triangle.centroid.scale(triangle.area.max(0.001)); - area_sum += triangle.area.max(0.001); - - for &vertex in &triangle.vertices { - if let Some(neighbors) = - vertex_links.get(&quantize_plane_vertex(vertex, voxel_size_meters)) - { - for &neighbor_index in neighbors { - if !visited[neighbor_index] { - queue.push_back(neighbor_index); - } - } - } - } - } - - if region_indices.is_empty() { - continue; - } - - if let Some(region) = build_simplified_plane_region(group, &triangles, ®ion_indices) { - regions.push(region); - } - } - - regions -} - -fn collect_classified_plane_triangles_from_surface_mesh( - mesh: &SurfaceMesh32, -) -> Vec { - let mut triangles = Vec::new(); - for (source_triangle_index, triangle) in mesh.indices.chunks_exact(3).enumerate() { - let a = vec3f( - mesh.positions[triangle[0] as usize][0], - mesh.positions[triangle[0] as usize][1], - mesh.positions[triangle[0] as usize][2], - ); - let b = vec3f( - mesh.positions[triangle[1] as usize][0], - mesh.positions[triangle[1] as usize][1], - mesh.positions[triangle[1] as usize][2], - ); - let c = vec3f( - mesh.positions[triangle[2] as usize][0], - mesh.positions[triangle[2] as usize][1], - mesh.positions[triangle[2] as usize][2], - ); - let normal_area = Vec3f::cross(b - a, c - a); - let area_twice = normal_area.length(); - if area_twice <= 1.0e-5 { - continue; - } - let area = area_twice * 0.5; - if area <= 0.0025 { - continue; - } - let normal = normal_area.scale(1.0 / area_twice); - let centroid = (a + b + c).scale(1.0 / 3.0); - let group = if normal.y >= DEPTH_PLANE_HORIZONTAL_NORMAL_Y_MIN { - Some(ExtractedPlaneGroup::HorizontalUp) - } else if normal.y <= -DEPTH_PLANE_HORIZONTAL_NORMAL_Y_MIN { - Some(ExtractedPlaneGroup::HorizontalDown) - } else if normal.y.abs() <= DEPTH_PLANE_VERTICAL_NORMAL_Y_MAX { - Some(ExtractedPlaneGroup::Vertical) - } else { - None - }; - let Some(group) = group else { - continue; - }; - triangles.push(ExtractedPlaneTriangle { - source_triangle_index, - group, - area, - normal, - centroid, - vertices: [a, b, c], - }); - } - triangles -} - -fn planar_region_accepts_triangle( - group: ExtractedPlaneGroup, - triangle: &ExtractedPlaneTriangle, - normal_sum: Vec3f, - centroid_sum: Vec3f, - area_sum: f32, -) -> bool { - if area_sum <= 1.0e-5 || normal_sum.length() <= 1.0e-5 { - return true; - } - - let region_normal = simplified_region_normal(group, normal_sum); - let aligned_normal = align_direction(region_normal, triangle.normal); - let min_normal_dot = if group == ExtractedPlaneGroup::Vertical { - DEPTH_PLANE_REGION_VERTICAL_NORMAL_DOT - } else { - DEPTH_PLANE_SIMPLIFY_REGION_NORMAL_DOT - }; - if aligned_normal.dot(region_normal) < min_normal_dot { - return false; - } - - let region_center = centroid_sum.scale(1.0 / area_sum.max(f32::EPSILON)); - let plane_distance = region_center.dot(region_normal); - let centroid_distance = (triangle.centroid.dot(region_normal) - plane_distance).abs(); - centroid_distance <= DEPTH_PLANE_SIMPLIFY_REGION_DISTANCE_METERS -} - -fn simplified_region_normal(group: ExtractedPlaneGroup, normal_sum: Vec3f) -> Vec3f { - match group { - ExtractedPlaneGroup::HorizontalUp => vec3f(0.0, 1.0, 0.0), - ExtractedPlaneGroup::HorizontalDown => vec3f(0.0, -1.0, 0.0), - ExtractedPlaneGroup::Vertical => { - let normal = if normal_sum.length() > 1.0e-5 { - normal_sum.normalize() - } else { - vec3f(1.0, 0.0, 0.0) - }; - normal.normalize() - } - } -} - -fn build_simplified_plane_region( - group: ExtractedPlaneGroup, - triangles: &[ExtractedPlaneTriangle], - region_indices: &[usize], -) -> Option { - let mut normal_sum = Vec3f::default(); - let mut centroid_sum = Vec3f::default(); - let mut area_sum = 0.0f32; - - for &triangle_index in region_indices { - let triangle = &triangles[triangle_index]; - let reference = if normal_sum.length() > 1.0e-5 { - normal_sum.normalize() - } else { - triangle.normal - }; - let aligned_normal = align_direction(reference, triangle.normal); - normal_sum += aligned_normal.scale(triangle.area.max(0.001)); - centroid_sum += triangle.centroid.scale(triangle.area.max(0.001)); - area_sum += triangle.area.max(0.001); - } - - if area_sum < DEPTH_PLANE_SIMPLIFY_MIN_AREA_METERS2 { - return Some(SimplifiedPlaneRegion { - group, - source_triangle_indices: region_indices - .iter() - .map(|&triangle_index| triangles[triangle_index].source_triangle_index) - .collect(), - triangles: region_indices - .iter() - .map(|&triangle_index| triangles[triangle_index].clone()) - .collect(), - }); - } - - let normal = simplified_region_normal(group, normal_sum); - let centroid = centroid_sum.scale(1.0 / area_sum.max(f32::EPSILON)); - let plane_distance = centroid.dot(normal); - let mut simplified_triangles = Vec::with_capacity(region_indices.len()); - - for &triangle_index in region_indices { - let triangle = &triangles[triangle_index]; - let projected = [ - project_point_onto_plane(triangle.vertices[0], normal, plane_distance), - project_point_onto_plane(triangle.vertices[1], normal, plane_distance), - project_point_onto_plane(triangle.vertices[2], normal, plane_distance), - ]; - let projected_normal_area = - Vec3f::cross(projected[1] - projected[0], projected[2] - projected[0]); - let projected_area_twice = projected_normal_area.length(); - if projected_area_twice <= 1.0e-5 { - continue; - } - let projected_area = projected_area_twice * 0.5; - simplified_triangles.push(ExtractedPlaneTriangle { - source_triangle_index: triangle.source_triangle_index, - group, - area: projected_area, - normal, - centroid: (projected[0] + projected[1] + projected[2]).scale(1.0 / 3.0), - vertices: projected, - }); - } - - if simplified_triangles.is_empty() { - return None; - } - - Some(SimplifiedPlaneRegion { - group, - source_triangle_indices: region_indices - .iter() - .map(|&triangle_index| triangles[triangle_index].source_triangle_index) - .collect(), - triangles: simplified_triangles, - }) -} - -fn project_point_onto_plane(point: Vec3f, normal: Vec3f, plane_distance: f32) -> Vec3f { - point - normal.scale(point.dot(normal) - plane_distance) -} - -fn fit_plane_patch_from_region( - group: ExtractedPlaneGroup, - triangles: &[ExtractedPlaneTriangle], - region: &[usize], -) -> Option { - match group { - ExtractedPlaneGroup::HorizontalUp | ExtractedPlaneGroup::HorizontalDown => { - fit_horizontal_plane_patch(group, triangles, region) - } - ExtractedPlaneGroup::Vertical => fit_vertical_plane_patch(triangles, region), - } -} - -fn fit_horizontal_plane_patch( - group: ExtractedPlaneGroup, - triangles: &[ExtractedPlaneTriangle], - region: &[usize], -) -> Option { - let normal = match group { - ExtractedPlaneGroup::HorizontalUp => vec3f(0.0, 1.0, 0.0), - ExtractedPlaneGroup::HorizontalDown => vec3f(0.0, -1.0, 0.0), - ExtractedPlaneGroup::Vertical => return None, - }; - - let mut area_sum = 0.0f32; - let mut y_sum = 0.0f32; - let mut points = Vec::with_capacity(region.len() * 3); - for &triangle_index in region { - let triangle = &triangles[triangle_index]; - area_sum += triangle.area; - y_sum += triangle.centroid.y * triangle.area; - for &vertex in &triangle.vertices { - points.push(vec2f(vertex.x, vertex.z)); - } - } - if area_sum < DEPTH_PLANE_MIN_AREA_METERS2 || points.len() < 3 { - return None; - } - - let rect = fit_min_area_rect_2d(&points)?; - let width = rect.half_u * 2.0; - let height = rect.half_v * 2.0; - if width < DEPTH_PLANE_MIN_DIM_METERS || height < DEPTH_PLANE_MIN_DIM_METERS { - return None; - } - - let mut tangent = vec3f(rect.axis_u.x, 0.0, rect.axis_u.y).normalize(); - let mut bitangent = vec3f(rect.axis_v.x, 0.0, rect.axis_v.y).normalize(); - if tangent.length() <= 1.0e-5 || bitangent.length() <= 1.0e-5 { - tangent = vec3f(1.0, 0.0, 0.0); - bitangent = vec3f(0.0, 0.0, 1.0); - } - if Vec3f::cross(tangent, bitangent).dot(normal) < 0.0 { - bitangent = bitangent.scale(-1.0); - } - - Some(XrDepthPlanePatch { - generation: 0, - kind: XrDepthPlaneKind::Unknown, - center: vec3f( - rect.center.x, - y_sum / area_sum.max(f32::EPSILON), - rect.center.y, - ), - normal, - tangent, - bitangent, - half_extent_tangent: rect.half_u, - half_extent_bitangent: rect.half_v, - area: width * height, - support_triangles: region.len(), - }) -} - -fn fit_vertical_plane_patch( - triangles: &[ExtractedPlaneTriangle], - region: &[usize], -) -> Option { - let mut area_sum = 0.0f32; - let mut normal_sum = Vec3f::default(); - let mut seed_normal = None::; - for &triangle_index in region { - let triangle = &triangles[triangle_index]; - let aligned_normal = if let Some(seed_normal) = seed_normal { - align_direction(seed_normal, triangle.normal) - } else { - seed_normal = Some(triangle.normal); - triangle.normal - }; - if aligned_normal.length() > 1.0e-5 { - normal_sum += aligned_normal.normalize().scale(triangle.area); - area_sum += triangle.area; - } - } - if area_sum < DEPTH_PLANE_MIN_AREA_METERS2 || normal_sum.length() <= 1.0e-5 { - return None; - } - - let normal = normal_sum.normalize(); - let mut tangent = (vec3f(0.0, 1.0, 0.0) - normal.scale(normal.y)).normalize(); - if tangent.length() <= 1.0e-5 { - tangent = orthogonal_tangent(normal); - } - let bitangent = Vec3f::cross(normal, tangent).normalize(); - if bitangent.length() <= 1.0e-5 { - return None; - } - - let mut distance_sum = 0.0f32; - let mut min_u = f32::INFINITY; - let mut max_u = f32::NEG_INFINITY; - let mut min_v = f32::INFINITY; - let mut max_v = f32::NEG_INFINITY; - for &triangle_index in region { - let triangle = &triangles[triangle_index]; - distance_sum += triangle.centroid.dot(normal) * triangle.area; - for &vertex in &triangle.vertices { - let u = vertex.dot(bitangent); - let v = vertex.y; - min_u = min_u.min(u); - max_u = max_u.max(u); - min_v = min_v.min(v); - max_v = max_v.max(v); - } - } - - let width = max_u - min_u; - let height = max_v - min_v; - if width < DEPTH_PLANE_MIN_DIM_METERS || height < DEPTH_PLANE_MIN_DIM_METERS { - return None; - } - - let center_u = (min_u + max_u) * 0.5; - let center_v = (min_v + max_v) * 0.5; - let distance = distance_sum / area_sum.max(f32::EPSILON); - Some(XrDepthPlanePatch { - generation: 0, - kind: XrDepthPlaneKind::Wall, - center: normal.scale(distance) + bitangent.scale(center_u) + tangent.scale(center_v), - normal, - tangent, - bitangent, - half_extent_tangent: height * 0.5, - half_extent_bitangent: width * 0.5, - area: width * height, - support_triangles: region.len(), - }) -} - -fn quantize_plane_vertex(vertex: Vec3f, voxel_size_meters: f32) -> ExtractedPlaneVertexKey { - let inv = 1.0 / depth_plane_vertex_link_meters(voxel_size_meters).max(1.0e-5); - ExtractedPlaneVertexKey { - x: (vertex.x * inv).round() as i32, - y: (vertex.y * inv).round() as i32, - z: (vertex.z * inv).round() as i32, - } -} - -fn fit_min_area_rect_2d(points: &[Vec2f]) -> Option { - if points.is_empty() { - return None; - } - - let hull = convex_hull_2d(points); - let points = if hull.is_empty() { points } else { &hull }; - if points.len() == 1 { - return Some(OrientedRect2 { - center: points[0], - axis_u: vec2f(1.0, 0.0), - axis_v: vec2f(0.0, 1.0), - half_u: 0.0, - half_v: 0.0, - }); - } - - let mut best_rect = None::; - let mut best_area = f32::INFINITY; - for edge_index in 0..points.len() { - let a = points[edge_index]; - let b = points[(edge_index + 1) % points.len()]; - let edge = b - a; - let edge_length = edge.length(); - if edge_length <= 1.0e-5 { - continue; - } - let axis_u = edge / edge_length; - let axis_v = vec2f(-axis_u.y, axis_u.x); - let mut min_u = f32::INFINITY; - let mut max_u = f32::NEG_INFINITY; - let mut min_v = f32::INFINITY; - let mut max_v = f32::NEG_INFINITY; - for &point in points { - let u = dot2(point, axis_u); - let v = dot2(point, axis_v); - min_u = min_u.min(u); - max_u = max_u.max(u); - min_v = min_v.min(v); - max_v = max_v.max(v); - } - let width = max_u - min_u; - let height = max_v - min_v; - let area = width * height; - if area >= best_area { - continue; - } - best_area = area; - best_rect = Some(OrientedRect2 { - center: axis_u * ((min_u + max_u) * 0.5) + axis_v * ((min_v + max_v) * 0.5), - axis_u, - axis_v, - half_u: width * 0.5, - half_v: height * 0.5, - }); - } - best_rect -} - -fn convex_hull_2d(points: &[Vec2f]) -> Vec { - if points.len() <= 3 { - return points.to_vec(); - } - - let mut sorted = points.to_vec(); - sorted.sort_by(|a, b| { - a.x.partial_cmp(&b.x) - .unwrap_or(std::cmp::Ordering::Equal) - .then_with(|| a.y.partial_cmp(&b.y).unwrap_or(std::cmp::Ordering::Equal)) - }); - sorted.dedup_by(|a, b| (a.x - b.x).abs() <= 1.0e-4 && (a.y - b.y).abs() <= 1.0e-4); - if sorted.len() <= 3 { - return sorted; - } - - let mut lower = Vec::with_capacity(sorted.len()); - for &point in &sorted { - while lower.len() >= 2 - && orient2(lower[lower.len() - 2], lower[lower.len() - 1], point) <= 0.0 - { - lower.pop(); - } - lower.push(point); - } - - let mut upper = Vec::with_capacity(sorted.len()); - for &point in sorted.iter().rev() { - while upper.len() >= 2 - && orient2(upper[upper.len() - 2], upper[upper.len() - 1], point) <= 0.0 - { - upper.pop(); - } - upper.push(point); - } - - lower.pop(); - upper.pop(); - lower.extend(upper); - lower -} - -fn dot2(a: Vec2f, b: Vec2f) -> f32 { - a.x * b.x + a.y * b.y -} - -fn cross2(a: Vec2f, b: Vec2f) -> f32 { - a.x * b.y - a.y * b.x -} - -fn orient2(a: Vec2f, b: Vec2f, c: Vec2f) -> f32 { - cross2(b - a, c - a) -} - -fn classify_plane_patch_kinds(patches: &mut [XrDepthPlanePatch]) { - let mut floor_y = None; - let mut floor_area = 0.0f32; - let mut min_up_y = f32::INFINITY; - for patch in patches.iter() { - if patch.normal.y > DEPTH_PLANE_HORIZONTAL_NORMAL_Y_MIN { - min_up_y = min_up_y.min(patch.center.y); - } - } - for patch in patches.iter() { - if patch.normal.y > DEPTH_PLANE_HORIZONTAL_NORMAL_Y_MIN - && patch.center.y <= min_up_y + 0.25 - && patch.area > floor_area - { - floor_area = patch.area; - floor_y = Some(patch.center.y); - } - } - - for patch in patches.iter_mut() { - if patch.normal.y > DEPTH_PLANE_HORIZONTAL_NORMAL_Y_MIN { - patch.kind = if floor_y.map(|y| patch.center.y <= y + 0.18).unwrap_or(false) { - XrDepthPlaneKind::Floor - } else { - XrDepthPlaneKind::Table - }; - } else if patch.normal.y < -DEPTH_PLANE_HORIZONTAL_NORMAL_Y_MIN { - patch.kind = XrDepthPlaneKind::Ceiling; - } else if patch.normal.y.abs() <= DEPTH_PLANE_VERTICAL_NORMAL_Y_MAX { - patch.kind = XrDepthPlaneKind::Wall; - } else { - patch.kind = XrDepthPlaneKind::Unknown; - } - } -} - -fn rasterize_support_triangles_into_mask( - mask: &mut PlaneSupportMask, - triangles_world: &[[Vec3f; 3]], - tangent: Vec3f, - bitangent: Vec3f, -) { - for triangle in triangles_world { - let a = vec2f(triangle[0].dot(tangent), triangle[0].dot(bitangent)); - let b = vec2f(triangle[1].dot(tangent), triangle[1].dot(bitangent)); - let c = vec2f(triangle[2].dot(tangent), triangle[2].dot(bitangent)); - rasterize_triangle_2d_into_support_mask(mask, a, b, c); - } -} - -fn rasterize_triangle_2d_into_support_mask( - mask: &mut PlaneSupportMask, - a: Vec2f, - b: Vec2f, - c: Vec2f, -) { - let min_u = a.x.min(b.x).min(c.x); - let max_u = a.x.max(b.x).max(c.x); - let min_v = a.y.min(b.y).min(c.y); - let max_v = a.y.max(b.y).max(c.y); - let min_cell = quantize_plane_support_cell(min_u, min_v); - let max_cell = quantize_plane_support_cell(max_u - 1.0e-4, max_v - 1.0e-4); - let cell_radius = DEPTH_PLANE_SUPPORT_CELL_METERS * std::f32::consts::SQRT_2 * 0.5; - for u in min_cell.u..=max_cell.u { - for v in min_cell.v..=max_cell.v { - let center = vec2f( - (u as f32 + 0.5) * DEPTH_PLANE_SUPPORT_CELL_METERS, - (v as f32 + 0.5) * DEPTH_PLANE_SUPPORT_CELL_METERS, - ); - if !point_in_triangle_2d(center, a, b, c) - && point_segment_distance_2d(center, a, b) > cell_radius - && point_segment_distance_2d(center, b, c) > cell_radius - && point_segment_distance_2d(center, c, a) > cell_radius - { - continue; - } - let weight = mask.cells.entry(PlaneSupportCellKey { u, v }).or_insert(0); - *weight = weight - .saturating_add(DEPTH_PLANE_SUPPORT_GROW_WEIGHT) - .min(DEPTH_PLANE_SUPPORT_MAX_WEIGHT); - } - } -} - -fn quantize_plane_support_cell(u: f32, v: f32) -> PlaneSupportCellKey { - let inv = 1.0 / DEPTH_PLANE_SUPPORT_CELL_METERS.max(1.0e-5); - PlaneSupportCellKey { - u: (u * inv).floor() as i32, - v: (v * inv).floor() as i32, - } -} - -fn largest_supported_rectangle(component: &PlaneSupportComponent) -> Option<(i32, i32, i32, i32)> { - let width = (component.max_u - component.min_u + 1).max(0) as usize; - let height = (component.max_v - component.min_v + 1).max(0) as usize; - if width == 0 || height == 0 { - return None; - } - - let mut occupied = vec![false; width * height]; - for cell in &component.cells { - let x = (cell.u - component.min_u) as usize; - let y = (cell.v - component.min_v) as usize; - occupied[y * width + x] = true; - } - - let mut heights = vec![0usize; width]; - let mut best_area = 0usize; - let mut best = None; - for y in 0..height { - for x in 0..width { - if occupied[y * width + x] { - heights[x] += 1; - } else { - heights[x] = 0; - } - } - - let mut stack = Vec::::new(); - for x in 0..=width { - let current_height = if x < width { heights[x] } else { 0 }; - while let Some(&top) = stack.last() { - if heights[top] <= current_height { - break; - } - let h = heights[top]; - stack.pop(); - let left = stack.last().map(|&idx| idx + 1).unwrap_or(0); - let rect_width = x - left; - let area = h * rect_width; - if area > best_area && h > 0 && rect_width > 0 { - best_area = area; - best = Some(( - component.min_u + left as i32, - component.min_u + x as i32 - 1, - component.min_v + y as i32 - h as i32 + 1, - component.min_v + y as i32, - )); - } - } - stack.push(x); - } - } - best -} - -fn point_in_triangle_2d(p: Vec2f, a: Vec2f, b: Vec2f, c: Vec2f) -> bool { - let ab = orient2(a, b, p); - let bc = orient2(b, c, p); - let ca = orient2(c, a, p); - (ab >= 0.0 && bc >= 0.0 && ca >= 0.0) || (ab <= 0.0 && bc <= 0.0 && ca <= 0.0) -} - -fn point_segment_distance_2d(p: Vec2f, a: Vec2f, b: Vec2f) -> f32 { - let ab = b - a; - let length2 = dot2(ab, ab); - if length2 <= 1.0e-6 { - return (p - a).length(); - } - let t = (dot2(p - a, ab) / length2).clamp(0.0, 1.0); - let projected = a + ab * t; - (p - projected).length() -} - -fn plane_patch_corners(patch: &XrDepthPlanePatch) -> [Vec3f; 4] { - let du = patch.tangent.scale(patch.half_extent_tangent); - let dv = patch.bitangent.scale(patch.half_extent_bitangent); - [ - patch.center - du - dv, - patch.center - du + dv, - patch.center + du + dv, - patch.center + du - dv, - ] -} - -fn align_direction(reference: Vec3f, candidate: Vec3f) -> Vec3f { - if reference.dot(candidate) < 0.0 { - candidate.scale(-1.0) - } else { - candidate - } -} - -fn orthogonal_tangent(normal: Vec3f) -> Vec3f { - let fallback = if normal.y.abs() < 0.9 { - vec3f(0.0, 1.0, 0.0) - } else { - vec3f(1.0, 0.0, 0.0) - }; - Vec3f::cross(fallback, normal).normalize() -} - -fn process_geometry_queries( - volume: &DepthMeshVolume, - store: &XrDepthMeshStore, - max_queries: usize, -) -> bool { - let pending = store.drain_pending_queries(max_queries); - if pending.is_empty() { - return false; - } - - let mut results = Vec::with_capacity(pending.len()); - for pending_query in pending { - results.push(evaluate_geometry_query( - volume, - pending_query.query, - pending_query.version, - )); - } - store.publish_query_results(results); - true -} - -#[derive(Clone, Copy)] -struct ScoredQuerySurfaceHit { - score: f32, - surface: XrDepthMeshQuerySurfaceHit, -} - -fn query_support_plane_radius(query_radius: f32) -> f32 { - (query_radius * DEPTH_QUERY_SUPPORT_PLANE_RADIUS_SCALE).clamp( - DEPTH_QUERY_SUPPORT_PLANE_RADIUS_MIN, - DEPTH_QUERY_SUPPORT_PLANE_RADIUS_MAX, - ) -} - -fn query_support_plane_height_tolerance(query_radius: f32) -> f32 { - (query_radius * DEPTH_QUERY_SUPPORT_PLANE_HEIGHT_TOLERANCE_SCALE).clamp( - DEPTH_QUERY_SUPPORT_PLANE_HEIGHT_TOLERANCE_MIN, - DEPTH_QUERY_SUPPORT_PLANE_HEIGHT_TOLERANCE_MAX, - ) -} - -fn query_tsdf_support_radius(query_radius: f32) -> f32 { - (query_radius * DEPTH_QUERY_TSDF_SUPPORT_RADIUS_SCALE).clamp( - DEPTH_QUERY_TSDF_SUPPORT_RADIUS_MIN, - DEPTH_QUERY_TSDF_SUPPORT_RADIUS_MAX, - ) -} - -fn query_support_plane_fallback_tangent(normal: Vec3f) -> Vec3f { - let primary_axis = if normal.y.abs() < 0.9 { - vec3f(0.0, 1.0, 0.0) - } else { - vec3f(1.0, 0.0, 0.0) - }; - let tangent = Vec3f::cross(primary_axis, normal); - if tangent.length() > 1.0e-6 { - tangent.normalize() - } else { - Vec3f::cross(vec3f(0.0, 0.0, 1.0), normal).normalize() - } -} - -fn query_support_plane_seed_tangent(normal: Vec3f, surface: XrDepthMeshQuerySurfaceHit) -> Vec3f { - let mut best_tangent = None; - let mut best_length_sq = 0.0; - let edges = [ - surface.triangle[1] - surface.triangle[0], - surface.triangle[2] - surface.triangle[1], - surface.triangle[0] - surface.triangle[2], - ]; - for edge in edges { - let projected = edge - normal.scale(edge.dot(normal)); - let length_sq = projected.dot(projected); - if length_sq > best_length_sq && length_sq > 1.0e-8 { - best_length_sq = length_sq; - best_tangent = Some(projected.scale(length_sq.sqrt().recip())); - } - } - best_tangent.unwrap_or_else(|| query_support_plane_fallback_tangent(normal)) -} - -fn solve_linear3(mut a: [[f32; 3]; 3], mut b: [f32; 3]) -> Option<[f32; 3]> { - for pivot_index in 0..3 { - let mut pivot_row = pivot_index; - let mut pivot_abs = a[pivot_index][pivot_index].abs(); - for row in pivot_index + 1..3 { - let candidate_abs = a[row][pivot_index].abs(); - if candidate_abs > pivot_abs { - pivot_abs = candidate_abs; - pivot_row = row; - } - } - if pivot_abs <= 1.0e-8 { - return None; - } - if pivot_row != pivot_index { - a.swap(pivot_row, pivot_index); - b.swap(pivot_row, pivot_index); - } - - let pivot_inv = a[pivot_index][pivot_index].recip(); - for column in pivot_index..3 { - a[pivot_index][column] *= pivot_inv; - } - b[pivot_index] *= pivot_inv; - - for row in 0..3 { - if row == pivot_index { - continue; - } - let factor = a[row][pivot_index]; - if factor.abs() <= 1.0e-8 { - continue; - } - for column in pivot_index..3 { - a[row][column] -= factor * a[pivot_index][column]; - } - b[row] -= factor * b[pivot_index]; - } - } - Some(b) -} - -fn visit_support_plane_triangles( - volume: &DepthMeshVolume, - surface: XrDepthMeshQuerySurfaceHit, - query_radius: f32, - mut visitor: impl FnMut([Vec3f; 3], Vec3f, f32, Vec3f), -) { - let gather_radius = query_support_plane_radius(query_radius); - let gather_radius_sq = gather_radius * gather_radius; - let height_tolerance = query_support_plane_height_tolerance(query_radius); - let seed_normal = surface.normal.normalize(); - let point_min = surface.point; - let point_max = surface.point; - - for chunk in &volume.mesh_chunks { - if aabb_aabb_distance_sq(point_min, point_max, chunk.bounds_min, chunk.bounds_max) - > gather_radius_sq - { - continue; - } - for triangle in chunk.indices.chunks_exact(3) { - let triangle = [ - chunk.vertices[triangle[0] as usize], - chunk.vertices[triangle[1] as usize], - chunk.vertices[triangle[2] as usize], - ]; - let raw_normal = Vec3f::cross(triangle[1] - triangle[0], triangle[2] - triangle[0]); - let raw_length = raw_normal.length(); - if raw_length <= 1.0e-6 { - continue; - } - - let mut normal = raw_normal.scale(raw_length.recip()); - if normal.dot(seed_normal) < 0.0 { - normal = normal.scale(-1.0); - } - if normal.dot(seed_normal) < DEPTH_QUERY_SUPPORT_PLANE_NORMAL_DOT_MIN - || normal.y < DEPTH_QUERY_SUPPORT_NORMAL_Y_MIN - { - continue; - } - - let closest = - closest_point_on_triangle(surface.point, triangle[0], triangle[1], triangle[2]); - let delta = closest - surface.point; - if delta.dot(delta) > gather_radius_sq { - continue; - } - - let max_plane_offset = triangle - .iter() - .map(|vertex| (seed_normal.dot(*vertex - surface.point)).abs()) - .fold(0.0f32, f32::max); - if max_plane_offset > height_tolerance { - continue; - } - - let centroid = (triangle[0] + triangle[1] + triangle[2]).scale(1.0 / 3.0); - visitor(triangle, normal, raw_length * 0.5, centroid); - } - } -} - -fn query_support_plane_fingerprint( - plane: &XrDepthMeshQuerySupportPlane, - role: XrDepthMeshQueryColliderRole, -) -> u64 { - let mut hasher = std::collections::hash_map::DefaultHasher::new(); - role.hash(&mut hasher); - [ - quantize_f32(plane.normal.x, 0.02), - quantize_f32(plane.normal.y, 0.02), - quantize_f32(plane.normal.z, 0.02), - quantize_f32(plane.normal.dot(plane.point), 0.01), - ] - .hash(&mut hasher); - hasher.finish() -} - -fn make_query_halfspace_collider( - plane: XrDepthMeshQuerySupportPlane, - role: XrDepthMeshQueryColliderRole, - restitution: f32, -) -> XrDepthMeshQueryCollider { - XrDepthMeshQueryCollider { - fingerprint: query_support_plane_fingerprint(&plane, role), - geometry: XrDepthMeshQueryColliderGeometry::HalfSpace(plane), - role, - restitution: restitution.max(0.0), - } -} - -fn build_query_surface_halfspace_from_patch( - surface: XrDepthMeshQuerySurfaceHit, - query_radius: f32, -) -> XrDepthMeshQuerySupportPlane { - let tangent_raw = surface.patch[1] - surface.patch[0]; - let bitangent_raw = surface.patch[3] - surface.patch[0]; - let tangent = if tangent_raw.length() > 1.0e-6 { - tangent_raw.normalize() - } else { - query_support_plane_seed_tangent(surface.normal, surface) - }; - let bitangent = if bitangent_raw.length() > 1.0e-6 { - bitangent_raw.normalize() - } else { - Vec3f::cross(surface.normal, tangent).normalize() - }; - let debug_half_extent = query_support_plane_radius(query_radius); - XrDepthMeshQuerySupportPlane { - point: closest_point_on_plane_patch( - surface.point, - &XrDepthPlanePatch { - generation: 0, - kind: XrDepthPlaneKind::Unknown, - center: (surface.patch[0] + surface.patch[1] + surface.patch[2] + surface.patch[3]) - .scale(0.25), - normal: surface.normal, - tangent, - bitangent, - half_extent_tangent: (surface.patch[1] - surface.patch[0]).length() * 0.5, - half_extent_bitangent: (surface.patch[3] - surface.patch[0]).length() * 0.5, - area: 0.0, - support_triangles: 0, - }, - ), - normal: surface.normal, - tangent, - bitangent, - half_extent_tangent: debug_half_extent, - half_extent_bitangent: debug_half_extent, - } -} - -fn build_query_surface_halfspace_from_triangles( - volume: &DepthMeshVolume, - surface: XrDepthMeshQuerySurfaceHit, - query_radius: f32, -) -> XrDepthMeshQuerySupportPlane { - let mut sum_w = 0.0; - let mut weighted_normal = vec3f(0.0, 0.0, 0.0); - let mut sum_xx = 0.0; - let mut sum_xz = 0.0; - let mut sum_x = 0.0; - let mut sum_zz = 0.0; - let mut sum_z = 0.0; - let mut sum_xy = 0.0; - let mut sum_zy = 0.0; - let mut sum_y = 0.0; - - visit_support_plane_triangles( - volume, - surface, - query_radius, - |triangle, normal, area, _centroid| { - weighted_normal = weighted_normal + normal.scale(area); - let vertex_weight = area * (1.0 / 3.0); - for vertex in triangle { - let local = vertex - surface.point; - let x = local.x; - let z = local.z; - let y = local.y; - sum_w += vertex_weight; - sum_xx += vertex_weight * x * x; - sum_xz += vertex_weight * x * z; - sum_x += vertex_weight * x; - sum_zz += vertex_weight * z * z; - sum_z += vertex_weight * z; - sum_xy += vertex_weight * x * y; - sum_zy += vertex_weight * z * y; - sum_y += vertex_weight * y; - } - }, - ); - - let avg_normal = if weighted_normal.length() > 1.0e-6 { - weighted_normal.normalize() - } else { - surface.normal.normalize() - }; - let mut normal = solve_linear3( - [ - [sum_xx, sum_xz, sum_x], - [sum_xz, sum_zz, sum_z], - [sum_x, sum_z, sum_w], - ], - [sum_xy, sum_zy, sum_y], - ) - .map(|solution| vec3f(-solution[0], 1.0, -solution[1]).normalize()) - .unwrap_or(avg_normal); - if normal.dot(avg_normal) < 0.0 { - normal = normal.scale(-1.0); - } - normal = (normal + avg_normal.scale(0.75)).normalize(); - let up_blend = ((avg_normal.y - DEPTH_QUERY_SUPPORT_NORMAL_Y_MIN) - / (1.0 - DEPTH_QUERY_SUPPORT_NORMAL_Y_MIN)) - .clamp(0.0, 1.0); - normal = (normal + vec3f(0.0, 1.0, 0.0).scale(up_blend * 1.25)).normalize(); - - let tangent = query_support_plane_seed_tangent(normal, surface); - let bitangent = Vec3f::cross(normal, tangent).normalize(); - let debug_half_extent_max = query_support_plane_radius(query_radius); - let debug_half_extent_min = - (query_radius * 1.1).max(DEPTH_QUERY_SUPPORT_PLANE_DEBUG_HALF_EXTENT_MIN); - let mut max_plane_offset = normal.dot(surface.point); - let mut min_u = f32::INFINITY; - let mut max_u = -f32::INFINITY; - let mut min_v = f32::INFINITY; - let mut max_v = -f32::INFINITY; - - visit_support_plane_triangles( - volume, - surface, - query_radius, - |triangle, _normal, _area, _centroid| { - for vertex in triangle { - max_plane_offset = max_plane_offset.max(normal.dot(vertex)); - let offset = vertex - surface.point; - let u = offset.dot(tangent); - let v = offset.dot(bitangent); - min_u = min_u.min(u); - max_u = max_u.max(u); - min_v = min_v.min(v); - max_v = max_v.max(v); - } - }, - ); - let point = surface.point - normal.scale(normal.dot(surface.point) - max_plane_offset); - - let half_extent_tangent = if min_u.is_finite() && max_u.is_finite() { - ((max_u - min_u) * 0.5 + query_radius * 0.5) - .clamp(debug_half_extent_min, debug_half_extent_max) - } else { - debug_half_extent_min - }; - let half_extent_bitangent = if min_v.is_finite() && max_v.is_finite() { - ((max_v - min_v) * 0.5 + query_radius * 0.5) - .clamp(debug_half_extent_min, debug_half_extent_max) - } else { - debug_half_extent_min - }; - - XrDepthMeshQuerySupportPlane { - point, - normal, - tangent, - bitangent, - half_extent_tangent, - half_extent_bitangent, - } -} - -fn build_query_surface_collider( - volume: &DepthMeshVolume, - surface: XrDepthMeshQuerySurfaceHit, - query_radius: f32, -) -> XrDepthMeshQueryCollider { - let plane = if surface.from_planar_patch { - build_query_surface_halfspace_from_patch(surface, query_radius) - } else { - build_query_surface_halfspace_from_triangles(volume, surface, query_radius) - }; - let role = if query_surface_priority(surface) == 0 { - XrDepthMeshQueryColliderRole::Support - } else { - XrDepthMeshQueryColliderRole::Impact - }; - let restitution = if role == XrDepthMeshQueryColliderRole::Impact { - DEPTH_QUERY_TSDF_IMPACT_RESTITUTION - } else { - 0.0 - }; - make_query_halfspace_collider(plane, role, restitution) -} - -fn build_query_surface_result( - volume: &DepthMeshVolume, - surface: XrDepthMeshQuerySurfaceHit, - query_radius: f32, -) -> XrDepthMeshQueryResolvedSurface { - XrDepthMeshQueryResolvedSurface { - surface, - collider: build_query_surface_collider(volume, surface, query_radius), - } -} - -#[derive(Clone, Copy)] -struct DepthGridSupportSample { - point: Vec3f, - radial_weight: f32, -} - -fn voxel_center_axis(voxel_size: f32, coord: i32) -> f32 { - (coord as f32 + 0.5) * voxel_size -} - -fn make_query_halfspace_surface( - distance: f32, - plane: XrDepthMeshQuerySupportPlane, - role: XrDepthMeshQueryColliderRole, - restitution: f32, -) -> XrDepthMeshQueryResolvedSurface { - let patch = query_support_plane_quad(plane); - XrDepthMeshQueryResolvedSurface { - surface: XrDepthMeshQuerySurfaceHit { - distance, - point: plane.point, - normal: plane.normal, - from_planar_patch: true, - triangle: [patch[0], patch[1], patch[2]], - patch, - chunk_key: ChunkKey::new(0, 0, 0), - }, - collider: make_query_halfspace_collider(plane, role, restitution), - } -} - -fn query_grid_bilinear_distance_at_y( - volume: &DepthMeshVolume, - sample_x: f32, - sample_z: f32, - y_coord: i32, -) -> Option { - let voxel_size = volume.voxel_size_meters; - let grid_x = sample_x / voxel_size - 0.5; - let grid_z = sample_z / voxel_size - 0.5; - let x0 = grid_x.floor() as i32; - let z0 = grid_z.floor() as i32; - let tx = grid_x - x0 as f32; - let tz = grid_z - z0 as f32; - - let v00 = volume - .mesh_grid - .normalized_distance(VoxelCoord::new(x0, y_coord, z0))?; - let v10 = volume - .mesh_grid - .normalized_distance(VoxelCoord::new(x0 + 1, y_coord, z0))?; - let v01 = volume - .mesh_grid - .normalized_distance(VoxelCoord::new(x0, y_coord, z0 + 1))?; - let v11 = volume - .mesh_grid - .normalized_distance(VoxelCoord::new(x0 + 1, y_coord, z0 + 1))?; - - let vx0 = v00 + (v10 - v00) * tx; - let vx1 = v01 + (v11 - v01) * tx; - Some(vx0 + (vx1 - vx0) * tz) -} - -fn query_depth_grid_trilinear_distance(volume: &DepthMeshVolume, point: Vec3f) -> Option { - let voxel_size = volume.voxel_size_meters; - let tsd_distance_meters = depth_tsd_distance_meters(voxel_size); - let grid_x = point.x / voxel_size - 0.5; - let grid_y = point.y / voxel_size - 0.5; - let grid_z = point.z / voxel_size - 0.5; - let x0 = grid_x.floor() as i32; - let y0 = grid_y.floor() as i32; - let z0 = grid_z.floor() as i32; - let tx = grid_x - x0 as f32; - let ty = grid_y - y0 as f32; - let tz = grid_z - z0 as f32; - - let sample = |x: i32, y: i32, z: i32| -> Option { - volume - .mesh_grid - .normalized_distance(VoxelCoord::new(x, y, z)) - .map(|distance| distance * tsd_distance_meters) - }; - - let s000 = sample(x0, y0, z0)?; - let s100 = sample(x0 + 1, y0, z0)?; - let s010 = sample(x0, y0 + 1, z0)?; - let s110 = sample(x0 + 1, y0 + 1, z0)?; - let s001 = sample(x0, y0, z0 + 1)?; - let s101 = sample(x0 + 1, y0, z0 + 1)?; - let s011 = sample(x0, y0 + 1, z0 + 1)?; - let s111 = sample(x0 + 1, y0 + 1, z0 + 1)?; - - let x00 = s000 + (s100 - s000) * tx; - let x10 = s010 + (s110 - s010) * tx; - let x01 = s001 + (s101 - s001) * tx; - let x11 = s011 + (s111 - s011) * tx; - let y0_mix = x00 + (x10 - x00) * ty; - let y1_mix = x01 + (x11 - x01) * ty; - Some(y0_mix + (y1_mix - y0_mix) * tz) -} - -fn finite_difference_axis( - center: f32, - forward: Option, - backward: Option, - step: f32, -) -> Option { - match (forward, backward) { - (Some(forward), Some(backward)) => Some((forward - backward) / (step * 2.0)), - (Some(forward), None) => Some((forward - center) / step), - (None, Some(backward)) => Some((center - backward) / step), - (None, None) => None, - } -} - -fn query_depth_grid_distance_gradient(volume: &DepthMeshVolume, point: Vec3f) -> Option { - let center = query_depth_grid_trilinear_distance(volume, point)?; - let step = volume - .voxel_size_meters - .max(DEPTH_QUERY_TSDF_IMPACT_RAY_STEP_MIN); - let dx = finite_difference_axis( - center, - query_depth_grid_trilinear_distance(volume, point + vec3f(step, 0.0, 0.0)), - query_depth_grid_trilinear_distance(volume, point - vec3f(step, 0.0, 0.0)), - step, - )?; - let dy = finite_difference_axis( - center, - query_depth_grid_trilinear_distance(volume, point + vec3f(0.0, step, 0.0)), - query_depth_grid_trilinear_distance(volume, point - vec3f(0.0, step, 0.0)), - step, - )?; - let dz = finite_difference_axis( - center, - query_depth_grid_trilinear_distance(volume, point + vec3f(0.0, 0.0, step)), - query_depth_grid_trilinear_distance(volume, point - vec3f(0.0, 0.0, step)), - step, - )?; - let gradient = vec3f(dx, dy, dz); - (gradient.length() > 1.0e-5).then_some(gradient.normalize()) -} - -fn query_tsdf_impact_half_extent(query_radius: f32, voxel_size: f32) -> f32 { - (query_radius * DEPTH_QUERY_TSDF_IMPACT_EXTENT_SCALE + voxel_size * 0.5).clamp( - DEPTH_QUERY_TSDF_IMPACT_EXTENT_MIN, - DEPTH_QUERY_TSDF_IMPACT_EXTENT_MAX, - ) -} - -fn evaluate_depth_grid_impact_query( - volume: &DepthMeshVolume, - query: XrDepthMeshQuery, -) -> Option { - let travel = query.predicted_center - query.center; - let travel_distance = travel.length(); - let velocity_length = query.velocity.length(); - let horizontal_speed = vec2f(query.velocity.x, query.velocity.z).length(); - let upward_speed = query.velocity.y.max(0.0); - if velocity_length < DEPTH_QUERY_TSDF_IMPACT_MIN_SPEED && travel_distance < 0.03 { - return None; - } - if horizontal_speed < DEPTH_QUERY_TSDF_IMPACT_MIN_HORIZONTAL_SPEED - && upward_speed < DEPTH_QUERY_TSDF_IMPACT_MIN_UPWARD_SPEED - { - return None; - } - - let motion_dir = if velocity_length > 1.0e-4 { - query.velocity.scale(1.0 / velocity_length) - } else if travel_distance > 1.0e-4 { - travel.scale(1.0 / travel_distance) - } else { - return None; - }; - let max_search_distance = (travel_distance + query.radius + query.max_distance) - .max(query.radius + volume.voxel_size_meters * 0.75); - let step_distance = (volume.voxel_size_meters * DEPTH_QUERY_TSDF_IMPACT_RAY_STEP_SCALE) - .max(DEPTH_QUERY_TSDF_IMPACT_RAY_STEP_MIN) - .min(max_search_distance); - let hit_threshold = query.radius + volume.voxel_size_meters * 0.20; - let mut previous_t = 0.0f32; - let mut t = step_distance; - - while t <= max_search_distance + 1.0e-4 { - let sample_position = query.center + motion_dir.scale(t); - let Some(sample_distance) = query_depth_grid_trilinear_distance(volume, sample_position) - else { - previous_t = t; - t += step_distance; - continue; - }; - if sample_distance <= hit_threshold { - let mut lo = previous_t; - let mut hi = t; - for _ in 0..5 { - let mid = (lo + hi) * 0.5; - let mid_position = query.center + motion_dir.scale(mid); - if let Some(mid_distance) = - query_depth_grid_trilinear_distance(volume, mid_position) - { - if mid_distance <= hit_threshold { - hi = mid; - } else { - lo = mid; - } - } - } - - let hit_position = query.center + motion_dir.scale(hi); - let signed_distance = query_depth_grid_trilinear_distance(volume, hit_position)?; - let mut normal = query_depth_grid_distance_gradient(volume, hit_position)?; - let mut opposing_dot = normal.dot(motion_dir.scale(-1.0)); - if opposing_dot <= DEPTH_QUERY_MIN_OPPOSING_NORMAL_DOT { - let flipped = normal.scale(-1.0); - let flipped_opposing_dot = flipped.dot(motion_dir.scale(-1.0)); - if flipped_opposing_dot > DEPTH_QUERY_MIN_OPPOSING_NORMAL_DOT { - normal = flipped; - opposing_dot = flipped_opposing_dot; - } - } - let is_lateral_impact = normal.y.abs() <= DEPTH_QUERY_TSDF_IMPACT_NORMAL_Y_MAX; - let is_ceiling_impact = upward_speed >= DEPTH_QUERY_TSDF_IMPACT_MIN_UPWARD_SPEED - && normal.y <= -DEPTH_QUERY_TSDF_IMPACT_CEILING_NORMAL_Y_MIN; - if !(is_lateral_impact || is_ceiling_impact) - || opposing_dot <= DEPTH_QUERY_MIN_OPPOSING_NORMAL_DOT - { - previous_t = t; - t += step_distance; - continue; - } - - let tangent_raw = motion_dir - normal.scale(motion_dir.dot(normal)); - let tangent = if tangent_raw.length() > 1.0e-5 { - tangent_raw.normalize() - } else { - query_support_plane_fallback_tangent(normal) - }; - let bitangent = Vec3f::cross(normal, tangent).normalize(); - let half_extent = query_tsdf_impact_half_extent(query.radius, volume.voxel_size_meters); - let plane = XrDepthMeshQuerySupportPlane { - point: hit_position - normal.scale(signed_distance), - normal, - tangent, - bitangent, - half_extent_tangent: half_extent, - half_extent_bitangent: half_extent, - }; - return Some(make_query_halfspace_surface( - signed_distance.abs(), - plane, - XrDepthMeshQueryColliderRole::Impact, - DEPTH_QUERY_TSDF_IMPACT_RESTITUTION, - )); - } - previous_t = t; - t += step_distance; - } - - None -} - -fn query_depth_grid_first_support_height( - volume: &DepthMeshVolume, - sample_x: f32, - sample_z: f32, - top_y: f32, - bottom_y: f32, -) -> Option { - let top_coord = volume - .mesh_grid - .world_to_voxel_coord(vec3f(sample_x, top_y, sample_z)) - .y; - let bottom_coord = volume - .mesh_grid - .world_to_voxel_coord(vec3f(sample_x, bottom_y, sample_z)) - .y; - if top_coord <= bottom_coord { - return None; - } - - for y_coord in (bottom_coord + 1..=top_coord).rev() { - let Some(above) = query_grid_bilinear_distance_at_y(volume, sample_x, sample_z, y_coord) - else { - continue; - }; - let Some(below) = - query_grid_bilinear_distance_at_y(volume, sample_x, sample_z, y_coord - 1) - else { - continue; - }; - if above <= 0.0 || below > 0.0 { - continue; - } - - let denom = above - below; - let blend = if denom.abs() > 1.0e-6 { - (above / denom).clamp(0.0, 1.0) - } else { - 0.5 - }; - let y_above = voxel_center_axis(volume.voxel_size_meters, y_coord); - let y_below = voxel_center_axis(volume.voxel_size_meters, y_coord - 1); - return Some(y_above + (y_below - y_above) * blend); - } - - None -} - -fn query_support_plane_quad(plane: XrDepthMeshQuerySupportPlane) -> [Vec3f; 4] { - let tangent = plane.tangent.scale(plane.half_extent_tangent); - let bitangent = plane.bitangent.scale(plane.half_extent_bitangent); - [ - plane.point - tangent - bitangent, - plane.point + tangent - bitangent, - plane.point + tangent + bitangent, - plane.point - tangent + bitangent, - ] -} - -fn make_query_hit_from_resolved_surface( - query: XrDepthMeshQuery, - version: u64, - mesh_generation: u64, - surface: XrDepthMeshQueryResolvedSurface, -) -> XrDepthMeshQueryHit { - XrDepthMeshQueryHit { - key: query.key, - version, - mesh_generation, - distance: surface.surface.distance, - point: surface.surface.point, - normal: surface.surface.normal, - from_planar_patch: surface.surface.from_planar_patch, - triangle: surface.surface.triangle, - patch: surface.surface.patch, - chunk_key: surface.surface.chunk_key, - collider: surface.collider, - additional_hits: Vec::new(), - } -} - -fn evaluate_depth_grid_support_query( - volume: &DepthMeshVolume, - query: XrDepthMeshQuery, - version: u64, -) -> Option { - const GRID_LAST: f32 = (DEPTH_QUERY_TSDF_SUPPORT_GRID_DIM - 1) as f32; - - let trajectory_samples = query_trajectory_samples(query); - let (trajectory_bounds_min, trajectory_bounds_max, travel_distance) = - query_trajectory_bounds_and_length(&trajectory_samples); - let support_radius = query_tsdf_support_radius(query.radius); - let tsd_distance_meters = depth_tsd_distance_meters(volume.voxel_size_meters); - let top_y = trajectory_bounds_max.y + query.radius + volume.voxel_size_meters; - let bottom_y = trajectory_bounds_min.y - - (query.radius + query.max_distance + travel_distance + tsd_distance_meters); - let (_, search_sample, center_support_y) = trajectory_samples - .iter() - .filter_map(|sample| { - let support_y = query_depth_grid_first_support_height( - volume, - sample.point.x, - sample.point.z, - top_y, - bottom_y, - )?; - let support_point = vec3f(sample.point.x, support_y, sample.point.z); - let score = (sample.point - support_point).length() - - sample.progress * (query.radius + volume.voxel_size_meters) * 0.35; - Some((score, *sample, support_y)) - }) - .min_by(|a, b| a.0.total_cmp(&b.0))?; - let search_center = search_sample.point; - - let mut samples = [None; DEPTH_QUERY_TSDF_SUPPORT_MAX_SAMPLES]; - let mut sample_count = 0usize; - let mut max_height = f32::NEG_INFINITY; - - for row in 0..DEPTH_QUERY_TSDF_SUPPORT_GRID_DIM { - for column in 0..DEPTH_QUERY_TSDF_SUPPORT_GRID_DIM { - let u = if GRID_LAST > 0.0 { - column as f32 / GRID_LAST * 2.0 - 1.0 - } else { - 0.0 - }; - let v = if GRID_LAST > 0.0 { - row as f32 / GRID_LAST * 2.0 - 1.0 - } else { - 0.0 - }; - let sample_x = search_center.x + u * support_radius; - let sample_z = search_center.z + v * support_radius; - let Some(sample_y) = - query_depth_grid_first_support_height(volume, sample_x, sample_z, top_y, bottom_y) - else { - continue; - }; - let radial_weight = 1.0 / (1.0 + (u * u + v * v) * 1.5); - let point = vec3f(sample_x, sample_y, sample_z); - max_height = max_height.max(sample_y); - samples[sample_count] = Some(DepthGridSupportSample { - point, - radial_weight, - }); - sample_count += 1; - } - } - - if sample_count < DEPTH_QUERY_TSDF_SUPPORT_MIN_SAMPLES { - return None; - } - - let mut height_tolerance = - query_support_plane_height_tolerance(query.radius).max(volume.voxel_size_meters * 0.25); - let mut selected_count = 0usize; - for _ in 0..3 { - selected_count = samples[..sample_count] - .iter() - .filter_map(|sample| *sample) - .filter(|sample| max_height - sample.point.y <= height_tolerance) - .count(); - if selected_count >= DEPTH_QUERY_TSDF_SUPPORT_MIN_SAMPLES { - break; - } - height_tolerance += volume.voxel_size_meters * 0.35; - } - if selected_count < DEPTH_QUERY_TSDF_SUPPORT_MIN_SAMPLES { - height_tolerance = f32::INFINITY; - selected_count = sample_count; - } - - let mut sum_w = 0.0; - let mut sum_xx = 0.0; - let mut sum_xz = 0.0; - let mut sum_x = 0.0; - let mut sum_zz = 0.0; - let mut sum_z = 0.0; - let mut sum_xy = 0.0; - let mut sum_zy = 0.0; - let mut sum_y = 0.0; - - for sample in samples[..sample_count].iter().filter_map(|sample| *sample) { - if max_height - sample.point.y > height_tolerance { - continue; - } - let local = sample.point - search_center; - let weight = sample.radial_weight; - sum_w += weight; - sum_xx += weight * local.x * local.x; - sum_xz += weight * local.x * local.z; - sum_x += weight * local.x; - sum_zz += weight * local.z * local.z; - sum_z += weight * local.z; - sum_xy += weight * local.x * local.y; - sum_zy += weight * local.z * local.y; - sum_y += weight * local.y; - } - - if selected_count < 3 || sum_w <= 1.0e-5 { - return None; - } - - let mut normal = solve_linear3( - [ - [sum_xx, sum_xz, sum_x], - [sum_xz, sum_zz, sum_z], - [sum_x, sum_z, sum_w], - ], - [sum_xy, sum_zy, sum_y], - ) - .map(|solution| vec3f(-solution[0], 1.0, -solution[1]).normalize()) - .unwrap_or(vec3f(0.0, 1.0, 0.0)); - if normal.y < 0.0 { - normal = normal.scale(-1.0); - } - normal = (normal + vec3f(0.0, 1.0, 0.0).scale(0.9)).normalize(); - if normal.y < DEPTH_QUERY_TSDF_SUPPORT_NORMAL_Y_MIN { - return None; - } - - let tangent = query_support_plane_fallback_tangent(normal); - let bitangent = Vec3f::cross(normal, tangent).normalize(); - let mut plane_offset = f32::NEG_INFINITY; - let mut min_u = f32::INFINITY; - let mut max_u = f32::NEG_INFINITY; - let mut min_v = f32::INFINITY; - let mut max_v = f32::NEG_INFINITY; - - for sample in samples[..sample_count].iter().filter_map(|sample| *sample) { - if max_height - sample.point.y > height_tolerance { - continue; - } - plane_offset = plane_offset.max(normal.dot(sample.point)); - let offset = sample.point - search_center; - let u = offset.dot(tangent); - let v = offset.dot(bitangent); - min_u = min_u.min(u); - max_u = max_u.max(u); - min_v = min_v.min(v); - max_v = max_v.max(v); - } - - let point = search_center - normal.scale(normal.dot(search_center) - plane_offset); - let extent_padding = (query.radius * DEPTH_QUERY_TSDF_SUPPORT_EXTENT_PADDING_SCALE) - .max(volume.voxel_size_meters * 0.45); - let debug_half_extent_max = support_radius; - let debug_half_extent_min = (query.radius * 0.9) - .max(volume.voxel_size_meters * 0.35) - .min(debug_half_extent_max); - let half_extent_tangent = if min_u.is_finite() && max_u.is_finite() { - ((max_u - min_u) * 0.5 + extent_padding).clamp(debug_half_extent_min, debug_half_extent_max) - } else { - debug_half_extent_min - }; - let half_extent_bitangent = if min_v.is_finite() && max_v.is_finite() { - ((max_v - min_v) * 0.5 + extent_padding).clamp(debug_half_extent_min, debug_half_extent_max) - } else { - debug_half_extent_min - }; - - let plane = XrDepthMeshQuerySupportPlane { - point, - normal, - tangent, - bitangent, - half_extent_tangent, - half_extent_bitangent, - }; - let center_support_point = vec3f(search_center.x, center_support_y, search_center.z); - let support_point = - center_support_point - normal.scale(normal.dot(center_support_point) - plane_offset); - let support_surface = make_query_halfspace_surface( - (support_point - search_center).length(), - XrDepthMeshQuerySupportPlane { - point: support_point, - ..plane - }, - XrDepthMeshQueryColliderRole::Support, - 0.0, - ); - let impact_surface = evaluate_depth_grid_impact_query(volume, query); - let additional_hits = impact_surface.into_iter().collect::>(); - - Some(XrDepthMeshQueryHit { - key: query.key, - version, - mesh_generation: volume.mesh_generation, - distance: support_surface.surface.distance, - point: support_surface.surface.point, - normal: support_surface.surface.normal, - from_planar_patch: support_surface.surface.from_planar_patch, - triangle: support_surface.surface.triangle, - patch: support_surface.surface.patch, - chunk_key: support_surface.surface.chunk_key, - collider: support_surface.collider, - additional_hits, - }) -} - -fn evaluate_geometry_query( - volume: &DepthMeshVolume, - query: XrDepthMeshQuery, - version: u64, -) -> XrDepthMeshQueryResult { - if !volume.mesh_grid.is_empty() { - let impact_surface = evaluate_depth_grid_impact_query(volume, query); - let prefer_impact = impact_surface.as_ref().is_some_and(|impact_surface| { - let XrDepthMeshQueryColliderGeometry::HalfSpace(plane) = - &impact_surface.collider.geometry; - query.velocity.y >= DEPTH_QUERY_TSDF_IMPACT_MIN_UPWARD_SPEED - && plane.normal.y <= -DEPTH_QUERY_TSDF_IMPACT_CEILING_NORMAL_Y_MIN - }); - if prefer_impact { - return impact_surface - .map(|impact_surface| { - XrDepthMeshQueryResult::Hit(make_query_hit_from_resolved_surface( - query, - version, - volume.mesh_generation, - impact_surface, - )) - }) - .unwrap_or(XrDepthMeshQueryResult::Miss { - key: query.key, - version, - mesh_generation: volume.mesh_generation, - }); - } - return evaluate_depth_grid_support_query(volume, query, version) - .or_else(|| { - impact_surface.map(|impact_surface| { - make_query_hit_from_resolved_surface( - query, - version, - volume.mesh_generation, - impact_surface, - ) - }) - }) - .map(XrDepthMeshQueryResult::Hit) - .unwrap_or(XrDepthMeshQueryResult::Miss { - key: query.key, - version, - mesh_generation: volume.mesh_generation, - }); - } - - let trajectory_samples = query_trajectory_samples(query); - let (sweep_bounds_min, sweep_bounds_max, travel_distance) = - query_trajectory_bounds_and_length(&trajectory_samples); - let max_search_distance = (query.radius + query.max_distance + travel_distance).max(0.0); - let max_search_distance_sq = max_search_distance * max_search_distance; - let mut best_hits = [None; DEPTH_QUERY_MAX_SURFACES_PER_QUERY]; - let sweep_radius = query.radius + query.max_distance; - let sweep_radius_sq = sweep_radius * sweep_radius; - - if query.include_planar_patches { - for patch in &volume.plane_patches { - let patch_corners = plane_patch_corners(patch); - let mut patch_bounds_min = patch_corners[0]; - let mut patch_bounds_max = patch_corners[0]; - for &corner in &patch_corners[1..] { - patch_bounds_min = Vec3f::min_componentwise(patch_bounds_min, corner); - patch_bounds_max = Vec3f::max_componentwise(patch_bounds_max, corner); - } - if aabb_aabb_distance_sq( - sweep_bounds_min, - sweep_bounds_max, - patch_bounds_min, - patch_bounds_max, - ) > max_search_distance_sq - { - continue; - } - - let mut best_sample_score = f32::INFINITY; - let mut best_closest = vec3f(0.0, 0.0, 0.0); - let mut best_distance_sq = f32::INFINITY; - let mut best_facing_point = query.center; - - for sample in &trajectory_samples { - let sample_point = sample.point; - let closest = closest_point_on_plane_patch(sample_point, patch); - let delta = closest - sample_point; - let distance_sq = delta.dot(delta); - if distance_sq > max_search_distance_sq { - continue; - } - let lateral_sq = point_polyline_distance_sq(closest, &trajectory_samples); - if lateral_sq > sweep_radius_sq { - continue; - } - - let distance = distance_sq.sqrt(); - let mut score = distance; - if travel_distance > 1.0e-4 { - let motion = if sample.velocity.length() > 1.0e-4 { - sample.velocity.normalize() - } else { - let fallback = query.predicted_center - query.center; - if fallback.length() > 1.0e-4 { - fallback.normalize() - } else { - vec3f(0.0, 0.0, 0.0) - } - }; - let mut candidate_normal = patch.normal; - if candidate_normal.dot(sample_point - closest) < 0.0 { - candidate_normal = candidate_normal.scale(-1.0); - } - let opposing = candidate_normal.dot(-motion); - if opposing <= DEPTH_QUERY_MIN_OPPOSING_NORMAL_DOT { - continue; - } - score -= sample.progress * travel_distance * 0.35; - score -= opposing * 0.08; - score += lateral_sq.sqrt() * 0.2; - } - if score < best_sample_score { - best_sample_score = score; - best_closest = closest; - best_distance_sq = distance_sq; - best_facing_point = sample_point; - } - } - - if !best_distance_sq.is_finite() { - continue; - } - - let mut normal = patch.normal; - if normal.dot(best_facing_point - best_closest) < 0.0 { - normal = normal.scale(-1.0); - } - - let surface = XrDepthMeshQuerySurfaceHit { - distance: best_distance_sq.sqrt(), - point: best_closest, - normal, - from_planar_patch: true, - triangle: [patch_corners[0], patch_corners[1], patch_corners[2]], - patch: patch_corners, - chunk_key: ChunkKey::new(0, 0, 0), - }; - consider_query_surface_candidate( - &mut best_hits, - best_sample_score, - surface, - query.radius, - ); - } - } - - for chunk in &volume.mesh_chunks { - if aabb_aabb_distance_sq( - sweep_bounds_min, - sweep_bounds_max, - chunk.bounds_min, - chunk.bounds_max, - ) > max_search_distance_sq - { - continue; - } - - for triangle in chunk.indices.chunks_exact(3) { - let a = chunk.vertices[triangle[0] as usize]; - let b = chunk.vertices[triangle[1] as usize]; - let c = chunk.vertices[triangle[2] as usize]; - let raw_normal = Vec3f::cross(b - a, c - a); - if raw_normal.length() <= 1.0e-6 { - continue; - } - let mut best_sample_score = f32::INFINITY; - let mut best_closest = vec3f(0.0, 0.0, 0.0); - let mut best_distance_sq = f32::INFINITY; - let mut best_facing_point = query.center; - - for sample in &trajectory_samples { - let sample_point = sample.point; - let closest = closest_point_on_triangle(sample_point, a, b, c); - let delta = closest - sample_point; - let distance_sq = delta.dot(delta); - if distance_sq > max_search_distance_sq { - continue; - } - let lateral_sq = point_polyline_distance_sq(closest, &trajectory_samples); - if lateral_sq > sweep_radius_sq { - continue; - } - let distance = distance_sq.sqrt(); - let mut score = distance; - if travel_distance > 1.0e-4 { - let motion = if sample.velocity.length() > 1.0e-4 { - sample.velocity.normalize() - } else { - let fallback = query.predicted_center - query.center; - if fallback.length() > 1.0e-4 { - fallback.normalize() - } else { - vec3f(0.0, 0.0, 0.0) - } - }; - let mut candidate_normal = raw_normal.normalize(); - if candidate_normal.dot(sample_point - closest) < 0.0 { - candidate_normal = candidate_normal.scale(-1.0); - } - let opposing = candidate_normal.dot(-motion); - if opposing <= DEPTH_QUERY_MIN_OPPOSING_NORMAL_DOT { - continue; - } - score -= sample.progress * travel_distance * 0.35; - score -= opposing * 0.08; - score += lateral_sq.sqrt() * 0.2; - } - if score < best_sample_score { - best_sample_score = score; - best_closest = closest; - best_distance_sq = distance_sq; - best_facing_point = sample_point; - } - } - - if !best_distance_sq.is_finite() { - continue; - } - - let mut normal = raw_normal.normalize(); - if normal.length() <= 1.0e-6 { - continue; - } - let mut hit_triangle = [a, b, c]; - if normal.dot(best_facing_point - best_closest) < 0.0 { - normal = normal.scale(-1.0); - hit_triangle.swap(1, 2); - } - let matched_planar_patch = - matching_reduced_planar_patch(hit_triangle, normal, &chunk.planar_patches); - let surface = XrDepthMeshQuerySurfaceHit { - distance: best_distance_sq.sqrt(), - point: best_closest, - normal, - from_planar_patch: matched_planar_patch.is_some(), - triangle: hit_triangle, - patch: matched_planar_patch - .map(|patch| plane_patch_corners(patch)) - .unwrap_or([best_closest; 4]), - chunk_key: chunk.chunk_key, - }; - consider_query_surface_candidate( - &mut best_hits, - best_sample_score, - surface, - query.radius, - ); - } - } - - if let Some(primary_hit) = best_hits[0] { - if query_surface_priority(primary_hit.surface) != 0 { - return XrDepthMeshQueryResult::Miss { - key: query.key, - version, - mesh_generation: volume.mesh_generation, - }; - } - let primary_resolved = - build_query_surface_result(volume, primary_hit.surface, query.radius); - let primary_surface = primary_resolved.surface; - XrDepthMeshQueryResult::Hit(XrDepthMeshQueryHit { - key: query.key, - version, - mesh_generation: volume.mesh_generation, - distance: primary_surface.distance, - point: primary_surface.point, - normal: primary_surface.normal, - from_planar_patch: primary_surface.from_planar_patch, - triangle: primary_surface.triangle, - patch: primary_surface.patch, - chunk_key: primary_surface.chunk_key, - collider: primary_resolved.collider, - additional_hits: Vec::new(), - }) - } else { - XrDepthMeshQueryResult::Miss { - key: query.key, - version, - mesh_generation: volume.mesh_generation, - } - } -} - -fn consider_query_surface_candidate( - best_hits: &mut [Option; DEPTH_QUERY_MAX_SURFACES_PER_QUERY], - score: f32, - surface: XrDepthMeshQuerySurfaceHit, - query_radius: f32, -) { - let candidate = ScoredQuerySurfaceHit { score, surface }; - - for index in 0..best_hits.len() { - if let Some(current) = best_hits[index] { - let distinct_radius = - query_surface_distinct_radius(query_radius, ¤t.surface, &candidate.surface); - if !query_surface_hits_are_distinct( - ¤t.surface, - &candidate.surface, - distinct_radius, - ) { - if scored_query_surface_is_better(&candidate, ¤t) { - remove_best_query_surface(best_hits, index); - } else { - return; - } - break; - } - } - } - - let mut insert_at = best_hits.len(); - for index in 0..best_hits.len() { - match best_hits[index] { - Some(current) => { - if scored_query_surface_is_better(&candidate, ¤t) { - insert_at = index; - break; - } - } - None => { - insert_at = index; - break; - } - } - } - - if insert_at >= best_hits.len() { - return; - } - - for index in (insert_at + 1..best_hits.len()).rev() { - best_hits[index] = best_hits[index - 1]; - } - best_hits[insert_at] = Some(candidate); -} - -fn query_surface_distinct_radius( - query_radius: f32, - a: &XrDepthMeshQuerySurfaceHit, - b: &XrDepthMeshQuerySurfaceHit, -) -> f32 { - let both_support = query_surface_priority(*a) == 0 && query_surface_priority(*b) == 0; - if both_support { - (query_radius * DEPTH_QUERY_SUPPORT_DISTINCT_RADIUS_SCALE) - .max(DEPTH_QUERY_SUPPORT_DISTINCT_RADIUS_MIN) - } else { - (query_radius * DEPTH_QUERY_DISTINCT_RADIUS_SCALE).max(DEPTH_QUERY_DISTINCT_RADIUS_MIN) - } -} - -fn remove_best_query_surface( - best_hits: &mut [Option; DEPTH_QUERY_MAX_SURFACES_PER_QUERY], - index: usize, -) { - for slot in index..best_hits.len().saturating_sub(1) { - best_hits[slot] = best_hits[slot + 1]; - } - if let Some(last) = best_hits.last_mut() { - *last = None; - } -} - -fn scored_query_surface_is_better( - candidate: &ScoredQuerySurfaceHit, - current: &ScoredQuerySurfaceHit, -) -> bool { - query_surface_priority(candidate.surface) < query_surface_priority(current.surface) - || (query_surface_priority(candidate.surface) == query_surface_priority(current.surface) - && candidate.score < current.score) -} - -fn query_surface_priority(surface: XrDepthMeshQuerySurfaceHit) -> u8 { - if surface.normal.y >= DEPTH_QUERY_SUPPORT_NORMAL_Y_MIN { - 0 - } else if surface.normal.y.abs() <= DEPTH_QUERY_LATERAL_NORMAL_Y_MAX { - 1 - } else { - 2 - } -} - -fn query_surface_geometry_fingerprint(surface: &XrDepthMeshQuerySurfaceHit) -> u64 { - let mut hasher = std::collections::hash_map::DefaultHasher::new(); - surface.from_planar_patch.hash(&mut hasher); - if surface.from_planar_patch { - let mut vertices = surface.patch.map(|vertex| { - [ - quantize_f32(vertex.x, 0.01), - quantize_f32(vertex.y, 0.01), - quantize_f32(vertex.z, 0.01), - ] - }); - vertices.sort_unstable(); - vertices.hash(&mut hasher); - } else { - let mut vertices = surface.triangle.map(|vertex| { - [ - quantize_f32(vertex.x, 0.01), - quantize_f32(vertex.y, 0.01), - quantize_f32(vertex.z, 0.01), - ] - }); - vertices.sort_unstable(); - vertices.hash(&mut hasher); - } - hasher.finish() -} - -fn query_surface_same_geometry( - a: &XrDepthMeshQuerySurfaceHit, - b: &XrDepthMeshQuerySurfaceHit, -) -> bool { - query_surface_geometry_fingerprint(a) == query_surface_geometry_fingerprint(b) -} - -fn query_surface_hits_are_distinct( - a: &XrDepthMeshQuerySurfaceHit, - b: &XrDepthMeshQuerySurfaceHit, - radius: f32, -) -> bool { - if query_surface_same_geometry(a, b) { - return false; - } - if query_surface_priority(*a) == 0 && query_surface_priority(*b) == 0 { - return true; - } - if a.normal.dot(b.normal).abs() < 0.85 { - return true; - } - (a.point - b.point).length() > radius.max(0.02) -} - -fn update_reduced_planar_patches(volume: &mut DepthMeshVolume, mesh_changed: bool) -> bool { - if DEPTH_ENABLE_REDUCED_PLANAR_PATCHES { - if mesh_changed { - rebuild_reduced_planar_patches(volume) - } else { - false - } - } else if !volume.plane_patches.is_empty() { - volume.plane_patches.clear(); - volume.plane_generation = volume.plane_generation.saturating_add(1); - true - } else { - false - } -} - -fn closest_point_on_plane_patch(point: Vec3f, patch: &XrDepthPlanePatch) -> Vec3f { - let offset = point - patch.center; - let u = offset - .dot(patch.tangent) - .clamp(-patch.half_extent_tangent, patch.half_extent_tangent); - let v = offset - .dot(patch.bitangent) - .clamp(-patch.half_extent_bitangent, patch.half_extent_bitangent); - patch.center + patch.tangent.scale(u) + patch.bitangent.scale(v) -} - -fn matching_reduced_planar_patch( - triangle: [Vec3f; 3], - normal: Vec3f, - patches: &[XrDepthPlanePatch], -) -> Option<&XrDepthPlanePatch> { - let centroid = (triangle[0] + triangle[1] + triangle[2]).scale(1.0 / 3.0); - patches.iter().find(|patch| { - if normal.dot(patch.normal).abs() < 0.995 { - return false; - } - if (centroid - patch.center).dot(patch.normal).abs() > 0.01 { - return false; - } - triangle.iter().all(|vertex| { - let offset = *vertex - patch.center; - if offset.dot(patch.normal).abs() > 0.015 { - return false; - } - let u = offset.dot(patch.tangent); - let v = offset.dot(patch.bitangent); - u.abs() <= patch.half_extent_tangent + 0.02 - && v.abs() <= patch.half_extent_bitangent + 0.02 - }) - }) -} - -fn aabb_aabb_distance_sq(a_min: Vec3f, a_max: Vec3f, b_min: Vec3f, b_max: Vec3f) -> f32 { - let dx = if a_max.x < b_min.x { - b_min.x - a_max.x - } else if b_max.x < a_min.x { - a_min.x - b_max.x - } else { - 0.0 - }; - let dy = if a_max.y < b_min.y { - b_min.y - a_max.y - } else if b_max.y < a_min.y { - a_min.y - b_max.y - } else { - 0.0 - }; - let dz = if a_max.z < b_min.z { - b_min.z - a_max.z - } else if b_max.z < a_min.z { - a_min.z - b_max.z - } else { - 0.0 - }; - dx * dx + dy * dy + dz * dz -} - -fn point_segment_distance_sq(point: Vec3f, start: Vec3f, end: Vec3f) -> f32 { - let segment = end - start; - let segment_length_sq = segment.dot(segment); - if segment_length_sq <= 1.0e-8 { - let delta = point - start; - return delta.dot(delta); - } - let t = ((point - start).dot(segment) / segment_length_sq).clamp(0.0, 1.0); - let closest = start + segment.scale(t); - let delta = point - closest; - delta.dot(delta) -} - -#[derive(Clone, Copy, Debug)] -struct QueryTrajectorySample { - progress: f32, - point: Vec3f, - velocity: Vec3f, -} - -fn query_trajectory_time_seconds(query: XrDepthMeshQuery) -> Option { - let horizontal_displacement = vec2f( - query.predicted_center.x - query.center.x, - query.predicted_center.z - query.center.z, - ); - let horizontal_velocity = vec2f(query.velocity.x, query.velocity.z); - let horizontal_speed_sq = horizontal_velocity.x * horizontal_velocity.x - + horizontal_velocity.y * horizontal_velocity.y; - if horizontal_speed_sq <= 1.0e-6 { - return None; - } - let dt = (horizontal_displacement.x * horizontal_velocity.x - + horizontal_displacement.y * horizontal_velocity.y) - / horizontal_speed_sq; - (dt.is_finite() && dt > 1.0e-4).then_some(dt) -} - -fn query_trajectory_sample(query: XrDepthMeshQuery, progress: f32) -> QueryTrajectorySample { - let progress = progress.clamp(0.0, 1.0); - if let Some(total_time) = query_trajectory_time_seconds(query) { - let t = total_time * progress; - let dy = query.predicted_center.y - query.center.y; - let accel_y = 2.0 * (dy - query.velocity.y * total_time) / (total_time * total_time); - return QueryTrajectorySample { - progress, - point: vec3f( - query.center.x + query.velocity.x * t, - query.center.y + query.velocity.y * t + 0.5 * accel_y * t * t, - query.center.z + query.velocity.z * t, - ), - velocity: vec3f( - query.velocity.x, - query.velocity.y + accel_y * t, - query.velocity.z, - ), - }; - } - - let travel = query.predicted_center - query.center; - QueryTrajectorySample { - progress, - point: query.center + travel.scale(progress), - velocity: if query.velocity.length() > 1.0e-4 { - query.velocity - } else { - travel - }, - } -} - -fn query_trajectory_samples( - query: XrDepthMeshQuery, -) -> [QueryTrajectorySample; DEPTH_QUERY_TRAJECTORY_SAMPLE_COUNT] { - std::array::from_fn(|index| { - let progress = if DEPTH_QUERY_TRAJECTORY_SAMPLE_COUNT <= 1 { - 0.0 - } else { - index as f32 / (DEPTH_QUERY_TRAJECTORY_SAMPLE_COUNT - 1) as f32 - }; - query_trajectory_sample(query, progress) - }) -} - -fn query_trajectory_bounds_and_length( - samples: &[QueryTrajectorySample; DEPTH_QUERY_TRAJECTORY_SAMPLE_COUNT], -) -> (Vec3f, Vec3f, f32) { - let mut min = samples[0].point; - let mut max = samples[0].point; - let mut length = 0.0; - for window in samples.windows(2) { - let a = window[0].point; - let b = window[1].point; - min = Vec3f::min_componentwise(min, b); - max = Vec3f::max_componentwise(max, b); - length += (b - a).length(); - } - (min, max, length) -} - -fn point_polyline_distance_sq( - point: Vec3f, - samples: &[QueryTrajectorySample; DEPTH_QUERY_TRAJECTORY_SAMPLE_COUNT], -) -> f32 { - let mut best = f32::INFINITY; - for window in samples.windows(2) { - best = best.min(point_segment_distance_sq( - point, - window[0].point, - window[1].point, - )); - } - best -} - -fn closest_point_on_triangle(point: Vec3f, a: Vec3f, b: Vec3f, c: Vec3f) -> Vec3f { - let ab = b - a; - let ac = c - a; - let ap = point - a; - let d1 = ab.dot(ap); - let d2 = ac.dot(ap); - if d1 <= 0.0 && d2 <= 0.0 { - return a; - } - - let bp = point - b; - let d3 = ab.dot(bp); - let d4 = ac.dot(bp); - if d3 >= 0.0 && d4 <= d3 { - return b; - } - - let vc = d1 * d4 - d3 * d2; - if vc <= 0.0 && d1 >= 0.0 && d3 <= 0.0 { - let v = d1 / (d1 - d3).max(f32::EPSILON); - return a + ab.scale(v); - } - - let cp = point - c; - let d5 = ab.dot(cp); - let d6 = ac.dot(cp); - if d6 >= 0.0 && d5 <= d6 { - return c; - } - - let vb = d5 * d2 - d1 * d6; - if vb <= 0.0 && d2 >= 0.0 && d6 <= 0.0 { - let w = d2 / (d2 - d6).max(f32::EPSILON); - return a + ac.scale(w); - } - - let va = d3 * d6 - d5 * d4; - if va <= 0.0 && (d4 - d3) >= 0.0 && (d5 - d6) >= 0.0 { - let edge = c - b; - let w = (d4 - d3) / ((d4 - d3) + (d5 - d6)).max(f32::EPSILON); - return b + edge.scale(w); - } - - let denom = (va + vb + vc).max(f32::EPSILON); - let v = vb / denom; - let w = vc / denom; - a + ab.scale(v) + ac.scale(w) -} - -fn simplify_surface_mesh_planar_regions(mesh: SurfaceMesh32) -> ReducedSurfaceMesh { - if !DEPTH_ENABLE_REDUCED_PLANAR_PATCHES { - return ReducedSurfaceMesh { - mesh, - planar_patches: Vec::new(), - }; - } - let regions = simplify_plane_regions( - collect_classified_plane_triangles_from_surface_mesh(&mesh), - DEPTH_VOXEL_SIZE_METERS, - ); - if regions.is_empty() { - return ReducedSurfaceMesh { - mesh, - planar_patches: Vec::new(), - }; - } - - let mut consumed_triangles = HashSet::new(); - let mut positions = Vec::<[f32; 3]>::new(); - let mut normals = Vec::<[f32; 3]>::new(); - let mut indices = Vec::::new(); - let mut planar_patches = Vec::::new(); - - for region in ®ions { - let region_area: f32 = region.triangles.iter().map(|triangle| triangle.area).sum(); - if region_area < DEPTH_MESH_PLANAR_SIMPLIFY_MIN_AREA_METERS2 { - continue; - } - if let Some(emitted) = emit_planar_region_rect_mesh( - region, - &mut positions, - &mut normals, - &mut indices, - &mut planar_patches, - ) { - consumed_triangles - .extend(covered_planar_region_triangles(region, &emitted).into_iter()); - } - } - - if consumed_triangles.is_empty() { - return ReducedSurfaceMesh { - mesh, - planar_patches: Vec::new(), - }; - } - - for (triangle_index, triangle) in mesh.indices.chunks_exact(3).enumerate() { - if consumed_triangles.contains(&triangle_index) { - continue; - } - let vertices = [ - vec3f( - mesh.positions[triangle[0] as usize][0], - mesh.positions[triangle[0] as usize][1], - mesh.positions[triangle[0] as usize][2], - ), - vec3f( - mesh.positions[triangle[1] as usize][0], - mesh.positions[triangle[1] as usize][1], - mesh.positions[triangle[1] as usize][2], - ), - vec3f( - mesh.positions[triangle[2] as usize][0], - mesh.positions[triangle[2] as usize][1], - mesh.positions[triangle[2] as usize][2], - ), - ]; - append_surface_mesh_triangle(&mut positions, &mut normals, &mut indices, vertices); - } - - ReducedSurfaceMesh { - mesh: SurfaceMesh32 { - positions, - normals, - indices, - }, - planar_patches, - } -} - -fn emit_planar_region_rect_mesh( - region: &SimplifiedPlaneRegion, - positions: &mut Vec<[f32; 3]>, - normals: &mut Vec<[f32; 3]>, - indices: &mut Vec, - planar_patches: &mut Vec, -) -> Option { - let region_indices: Vec = (0..region.triangles.len()).collect(); - let Some(patch) = fit_plane_patch_from_region(region.group, ®ion.triangles, ®ion_indices) - else { - return None; - }; - - let mut support_mask = PlaneSupportMask::default(); - let support_triangles = region - .triangles - .iter() - .map(|triangle| triangle.vertices) - .collect::>(); - rasterize_support_triangles_into_mask( - &mut support_mask, - &support_triangles, - patch.tangent, - patch.bitangent, - ); - - let plane_distance = patch.center.dot(patch.normal); - let mut rects = Vec::new(); - let mut emitted_rects = 0usize; - for mut component in decompose_support_mask_components(&support_mask) { - loop { - if emitted_rects >= DEPTH_MESH_PLANAR_SIMPLIFY_MAX_RECTS_PER_REGION { - break; - } - let Some((min_u, max_u, min_v, max_v)) = largest_supported_rectangle(&component) else { - break; - }; - let width = (max_u - min_u + 1) as f32 * DEPTH_PLANE_SUPPORT_CELL_METERS; - let height = (max_v - min_v + 1) as f32 * DEPTH_PLANE_SUPPORT_CELL_METERS; - if width * height < DEPTH_MESH_PLANAR_SIMPLIFY_MIN_RECT_AREA_METERS2 { - break; - } - append_surface_mesh_quad_rect( - positions, - normals, - indices, - planar_patches, - patch.normal, - patch.tangent, - patch.bitangent, - plane_distance, - min_u, - max_u, - min_v, - max_v, - ); - rects.push(PlanarRectCoverage { - min_u: min_u as f32 * DEPTH_PLANE_SUPPORT_CELL_METERS, - max_u: (max_u + 1) as f32 * DEPTH_PLANE_SUPPORT_CELL_METERS, - min_v: min_v as f32 * DEPTH_PLANE_SUPPORT_CELL_METERS, - max_v: (max_v + 1) as f32 * DEPTH_PLANE_SUPPORT_CELL_METERS, - }); - emitted_rects += 1; - remove_rect_from_support_component(&mut component, min_u, max_u, min_v, max_v); - if component.cells.is_empty() { - break; - } - } - } - - (!rects.is_empty()).then_some(EmittedPlanarRegion { - tangent: patch.tangent, - bitangent: patch.bitangent, - rects, - }) -} - -fn covered_planar_region_triangles( - region: &SimplifiedPlaneRegion, - emitted: &EmittedPlanarRegion, -) -> HashSet { - let mut covered = HashSet::new(); - for (index, triangle) in region.triangles.iter().enumerate() { - let centroid_u = triangle.centroid.dot(emitted.tangent); - let centroid_v = triangle.centroid.dot(emitted.bitangent); - if emitted - .rects - .iter() - .any(|rect| planar_rect_contains_uv(*rect, centroid_u, centroid_v)) - { - covered.insert(region.source_triangle_indices[index]); - } - } - covered -} - -fn planar_rect_contains_uv(rect: PlanarRectCoverage, u: f32, v: f32) -> bool { - let epsilon = DEPTH_PLANE_SUPPORT_CELL_METERS * 0.25; - u >= rect.min_u - epsilon - && u <= rect.max_u + epsilon - && v >= rect.min_v - epsilon - && v <= rect.max_v + epsilon -} - -fn decompose_support_mask_components(mask: &PlaneSupportMask) -> Vec { - let occupied = mask - .cells - .iter() - .filter_map(|(&key, &weight)| { - (weight >= DEPTH_PLANE_SUPPORT_OCCUPIED_WEIGHT).then_some(key) - }) - .collect::>(); - if occupied.is_empty() { - return Vec::new(); - } - - let mut visited = HashSet::new(); - let mut components = Vec::new(); - for &start in &occupied { - if !visited.insert(start) { - continue; - } - let mut queue = VecDeque::from([start]); - let mut cells = Vec::new(); - while let Some(cell) = queue.pop_front() { - cells.push(cell); - for (du, dv) in [(1, 0), (-1, 0), (0, 1), (0, -1)] { - let neighbor = PlaneSupportCellKey { - u: cell.u + du, - v: cell.v + dv, - }; - if occupied.contains(&neighbor) && visited.insert(neighbor) { - queue.push_back(neighbor); - } - } - } - if let Some(component) = rebuild_support_component_from_cells(cells) { - components.push(component); - } - } - components -} - -fn rebuild_support_component_from_cells( - cells: Vec, -) -> Option { - let first = *cells.first()?; - let mut component = PlaneSupportComponent { - cells, - min_u: first.u, - max_u: first.u, - min_v: first.v, - max_v: first.v, - }; - for cell in &component.cells { - component.min_u = component.min_u.min(cell.u); - component.max_u = component.max_u.max(cell.u); - component.min_v = component.min_v.min(cell.v); - component.max_v = component.max_v.max(cell.v); - } - Some(component) -} - -fn remove_rect_from_support_component( - component: &mut PlaneSupportComponent, - min_u: i32, - max_u: i32, - min_v: i32, - max_v: i32, -) { - let remaining = component - .cells - .iter() - .copied() - .filter(|cell| cell.u < min_u || cell.u > max_u || cell.v < min_v || cell.v > max_v) - .collect::>(); - if let Some(rebuilt) = rebuild_support_component_from_cells(remaining) { - *component = rebuilt; - } else { - component.cells.clear(); - } -} - -fn append_surface_mesh_quad_rect( - positions: &mut Vec<[f32; 3]>, - normals: &mut Vec<[f32; 3]>, - indices: &mut Vec, - planar_patches: &mut Vec, - normal: Vec3f, - tangent: Vec3f, - bitangent: Vec3f, - plane_distance: f32, - min_u_cell: i32, - max_u_cell: i32, - min_v_cell: i32, - max_v_cell: i32, -) { - let min_u = min_u_cell as f32 * DEPTH_PLANE_SUPPORT_CELL_METERS; - let max_u = (max_u_cell + 1) as f32 * DEPTH_PLANE_SUPPORT_CELL_METERS; - let min_v = min_v_cell as f32 * DEPTH_PLANE_SUPPORT_CELL_METERS; - let max_v = (max_v_cell + 1) as f32 * DEPTH_PLANE_SUPPORT_CELL_METERS; - let center_u = (min_u + max_u) * 0.5; - let center_v = (min_v + max_v) * 0.5; - let center = normal.scale(plane_distance) + tangent.scale(center_u) + bitangent.scale(center_v); - let quad = [ - center + tangent.scale(min_u - center_u) + bitangent.scale(min_v - center_v), - center + tangent.scale(max_u - center_u) + bitangent.scale(min_v - center_v), - center + tangent.scale(max_u - center_u) + bitangent.scale(max_v - center_v), - center + tangent.scale(min_u - center_u) + bitangent.scale(max_v - center_v), - ]; - append_surface_mesh_quad(positions, normals, indices, quad, normal); - planar_patches.push(XrDepthPlanePatch { - generation: 0, - kind: classify_planar_patch_kind_from_normal(normal), - center, - normal, - tangent, - bitangent, - half_extent_tangent: (max_u - min_u) * 0.5, - half_extent_bitangent: (max_v - min_v) * 0.5, - area: (max_u - min_u) * (max_v - min_v), - support_triangles: 2, - }); -} - -fn classify_planar_patch_kind_from_normal(normal: Vec3f) -> XrDepthPlaneKind { - if normal.y >= DEPTH_PLANE_HORIZONTAL_NORMAL_Y_MIN { - XrDepthPlaneKind::Unknown - } else if normal.y <= -DEPTH_PLANE_HORIZONTAL_NORMAL_Y_MIN { - XrDepthPlaneKind::Ceiling - } else if normal.y.abs() <= DEPTH_PLANE_VERTICAL_NORMAL_Y_MAX { - XrDepthPlaneKind::Wall - } else { - XrDepthPlaneKind::Unknown - } -} - -fn append_surface_mesh_quad( - positions: &mut Vec<[f32; 3]>, - normals: &mut Vec<[f32; 3]>, - indices: &mut Vec, - quad: [Vec3f; 4], - normal: Vec3f, -) { - let base = positions.len() as u32; - for vertex in quad { - positions.push([vertex.x, vertex.y, vertex.z]); - normals.push([normal.x, normal.y, normal.z]); - } - indices.extend_from_slice(&[base, base + 1, base + 2, base, base + 2, base + 3]); -} - -fn append_surface_mesh_triangle( - positions: &mut Vec<[f32; 3]>, - normals: &mut Vec<[f32; 3]>, - indices: &mut Vec, - triangle: [Vec3f; 3], -) { - let base = positions.len() as u32; - let normal = Vec3f::cross(triangle[1] - triangle[0], triangle[2] - triangle[0]).normalize(); - for vertex in triangle { - positions.push([vertex.x, vertex.y, vertex.z]); - normals.push([normal.x, normal.y, normal.z]); - } - indices.extend_from_slice(&[base, base + 1, base + 2]); -} - -fn depth_mesh_chunk_from_surface_mesh( - chunk_key: ChunkKey, - generation: u64, - mesh: SurfaceMesh32, -) -> Option { - let reduced = simplify_surface_mesh_planar_regions(mesh); - let mesh = reduced.mesh; - if mesh.positions.is_empty() || mesh.indices.is_empty() { - return None; - } - let mut bounds_min = vec3f( - mesh.positions[0][0], - mesh.positions[0][1], - mesh.positions[0][2], - ); - let mut bounds_max = bounds_min; - let mut vertices = Vec::with_capacity(mesh.positions.len()); - let mut normals = Vec::with_capacity(mesh.normals.len()); - let mut hasher = std::collections::hash_map::DefaultHasher::new(); - chunk_key.hash(&mut hasher); - - for (position, normal) in mesh.positions.iter().zip(mesh.normals.iter()) { - let position = vec3f(position[0], position[1], position[2]); - let normal = vec3f(normal[0], normal[1], normal[2]); - bounds_min = Vec3f::min_componentwise(bounds_min, position); - bounds_max = Vec3f::max_componentwise(bounds_max, position); - quantize_f32(position.x, 0.01).hash(&mut hasher); - quantize_f32(position.y, 0.01).hash(&mut hasher); - quantize_f32(position.z, 0.01).hash(&mut hasher); - vertices.push(position); - normals.push(normal); - } - mesh.indices.hash(&mut hasher); - - Some(XrDepthMeshChunk { - generation, - chunk_key, - fingerprint: hasher.finish(), - bounds_min, - bounds_max, - vertices, - normals, - indices: mesh.indices, - planar_patches: reduced.planar_patches, - }) -} - -fn voxel_coord_from_chunk_key(key: ChunkKey) -> VoxelCoord { - VoxelCoord::new(key.x, key.y, key.z) -} - -fn push_unique_chunk_key(keys: &mut Vec, key: ChunkKey) { - if !keys.contains(&key) { - keys.push(key); - } -} - -fn quantize_f32(value: f32, quantum: f32) -> i32 { - (value / quantum.max(f32::EPSILON)).round() as i32 -} - -fn align_extent(extent: i32, stride: i32) -> i32 { - let stride = stride.max(1); - if extent <= 0 { - 0 - } else { - ((extent + stride - 1) / stride) * stride - } -} - -const SURFACE_NET_CORNERS: [VoxelCoord; 8] = [ - VoxelCoord::new(0, 0, 0), - VoxelCoord::new(1, 0, 0), - VoxelCoord::new(1, 0, 1), - VoxelCoord::new(0, 0, 1), - VoxelCoord::new(0, 1, 0), - VoxelCoord::new(1, 1, 0), - VoxelCoord::new(1, 1, 1), - VoxelCoord::new(0, 1, 1), -]; - -const SURFACE_NET_EDGES: [(usize, usize); 12] = [ - (0, 1), - (1, 2), - (2, 3), - (3, 0), - (4, 5), - (5, 6), - (6, 7), - (7, 4), - (0, 4), - (1, 5), - (2, 6), - (3, 7), -]; - -fn surface_net_mesh_from_dense( - volume: &[f32], - voxel_count: VoxelCoord, - voxel_size: f32, - start_coord: VoxelCoord, - stride: i32, -) -> Option { - if voxel_count.x <= 1 || voxel_count.y <= 1 || voxel_count.z <= 1 { - return None; - } - let stride = stride.max(1); - let scaled_count = VoxelCoord::new( - voxel_count.x.div_euclid(stride), - voxel_count.y.div_euclid(stride), - voxel_count.z.div_euclid(stride), - ); - if scaled_count.x <= 1 || scaled_count.y <= 1 || scaled_count.z <= 1 { - return None; - } - - let sample_value = |coord: VoxelCoord| -> f32 { - let raw = volume[flatten_coord(coord, voxel_count)]; - if !raw.is_finite() { - 0.0 - } else { - raw - } - }; - let raw_value = |coord: VoxelCoord| -> f32 { volume[flatten_coord(coord, voxel_count)] }; - - let mut positions = Vec::<[f32; 3]>::new(); - let mut normals = Vec::<[f32; 3]>::new(); - let mut indices = Vec::::new(); - let mut coord_vert_map = - vec![i32::MIN; (scaled_count.x * scaled_count.y * scaled_count.z) as usize]; - let mut vert_coords = Vec::::new(); - - for z in 0..scaled_count.z { - for y in 0..scaled_count.y { - for x in 0..scaled_count.x { - if x == scaled_count.x - 1 || y == scaled_count.y - 1 || z == scaled_count.z - 1 { - continue; - } - let coord = VoxelCoord::new(x, y, z); - let mut pos_coord = vec3f(0.0, 0.0, 0.0); - let mut direction = vec3f(0.0, 0.0, 0.0); - let mut crossings = 0u8; - let mut bad_crossings = 0u8; - - for (a_idx, b_idx) in SURFACE_NET_EDGES { - let coord_a = dense_corner_coord(coord, SURFACE_NET_CORNERS[a_idx], stride); - let coord_b = dense_corner_coord(coord, SURFACE_NET_CORNERS[b_idx], stride); - let value_a = sample_value(coord_a); - let value_b = sample_value(coord_b); - let change = value_a - value_b; - direction += vec3f( - (coord_a.x - coord_b.x) as f32, - (coord_a.y - coord_b.y) as f32, - (coord_a.z - coord_b.z) as f32, - ) - .scale(change); - if (value_a < 0.0) == (value_b < 0.0) || change.abs() <= f32::EPSILON { - continue; - } - if !raw_value(coord_a).is_finite() || !raw_value(coord_b).is_finite() { - bad_crossings = bad_crossings.saturating_add(1); - } - let t = value_a / change; - pos_coord += vec3f( - coord_a.x as f32 + (coord_b.x - coord_a.x) as f32 * t, - coord_a.y as f32 + (coord_b.y - coord_a.y) as f32 * t, - coord_a.z as f32 + (coord_b.z - coord_a.z) as f32 * t, - ); - crossings = crossings.saturating_add(1); - } - - if crossings < 3 || crossings == bad_crossings { - continue; - } - - pos_coord = pos_coord.scale(1.0 / crossings as f32); - let world = vec3f( - (start_coord.x as f32 + pos_coord.x + 0.5) * voxel_size, - (start_coord.y as f32 + pos_coord.y + 0.5) * voxel_size, - (start_coord.z as f32 + pos_coord.z + 0.5) * voxel_size, - ); - let normal = if direction.length() > 1.0e-6 { - direction.normalize() - } else { - vec3f(0.0, 1.0, 0.0) - }; - let vertex_index = positions.len() as u32; - positions.push([world.x, world.y, world.z]); - normals.push([normal.x, normal.y, normal.z]); - coord_vert_map[flatten_coord(coord, scaled_count)] = vertex_index as i32; - vert_coords.push(coord); - } - } - } - - for coord in vert_coords { - surface_net_tris_for_axis( - &mut indices, - &coord_vert_map, - scaled_count, - &sample_value, - coord, - VoxelCoord::new(1, 0, 0), - VoxelCoord::new(0, 0, 1), - VoxelCoord::new(0, 1, 0), - stride, - ); - surface_net_tris_for_axis( - &mut indices, - &coord_vert_map, - scaled_count, - &sample_value, - coord, - VoxelCoord::new(0, 1, 0), - VoxelCoord::new(1, 0, 0), - VoxelCoord::new(0, 0, 1), - stride, - ); - surface_net_tris_for_axis( - &mut indices, - &coord_vert_map, - scaled_count, - &sample_value, - coord, - VoxelCoord::new(0, 0, 1), - VoxelCoord::new(0, 1, 0), - VoxelCoord::new(1, 0, 0), - stride, - ); - } - - if indices.is_empty() { - None - } else { - Some(SurfaceMesh32 { - positions, - normals, - indices, - }) - } -} - -fn dense_corner_coord(base: VoxelCoord, corner: VoxelCoord, stride: i32) -> VoxelCoord { - VoxelCoord::new( - base.x * stride + corner.x * stride, - base.y * stride + corner.y * stride, - base.z * stride + corner.z * stride, - ) -} - -fn flatten_coord(coord: VoxelCoord, size: VoxelCoord) -> usize { - coord.x as usize - + coord.y as usize * size.x as usize - + coord.z as usize * size.x as usize * size.y as usize -} - -fn surface_net_tris_for_axis( - indices: &mut Vec, - coord_vert_map: &[i32], - size: VoxelCoord, - sample_value: &impl Fn(VoxelCoord) -> f32, - coord: VoxelCoord, - axis: VoxelCoord, - d1: VoxelCoord, - d2: VoxelCoord, - stride: i32, -) { - if coord.x - d1.x < 0 - || coord.y - d1.y < 0 - || coord.z - d1.z < 0 - || coord.x - d2.x < 0 - || coord.y - d2.y < 0 - || coord.z - d2.z < 0 - { - return; - } - let scaled = VoxelCoord::new(coord.x * stride, coord.y * stride, coord.z * stride); - let value_a = sample_value(scaled); - let value_b = - sample_value(scaled + VoxelCoord::new(axis.x * stride, axis.y * stride, axis.z * stride)); - if (value_a < 0.0) == (value_b < 0.0) { - return; - } - let a = coord_vert_map[flatten_coord(coord, size)]; - let b = coord_vert_map[flatten_coord(coord - d1, size)]; - let c = coord_vert_map[flatten_coord(coord - d1 - d2, size)]; - let d = coord_vert_map[flatten_coord(coord - d2, size)]; - if a < 0 || b < 0 || c < 0 || d < 0 { - return; - } - let (a, b, c, d) = (a as u32, b as u32, c as u32, d as u32); - if value_a < 0.0 { - indices.extend_from_slice(&[c, b, a, d, c, a]); +fn matched_height_map_publish_interval(store: &XrTsdfStore) -> Duration { + let base = Duration::from_millis(DEPTH_PUBLISHED_HEIGHT_MAP_INTERVAL_MILLIS); + let stats = store.cooperative_step_stats(); + let cycle_micros = stats.average_cycle_micros.max(stats.last_cycle_micros); + if cycle_micros == 0 { + base } else { - indices.extend_from_slice(&[a, c, d, a, b, c]); - } -} - -#[cfg(test)] -mod tests { - use super::*; - - fn quad_vertices( - center: Vec3f, - axis_u: Vec3f, - axis_v: Vec3f, - half_u: f32, - half_v: f32, - ) -> [Vec3f; 4] { - let du = axis_u.normalize().scale(half_u); - let dv = axis_v.normalize().scale(half_v); - [ - center - du - dv, - center + du - dv, - center + du + dv, - center - du + dv, - ] - } - - fn push_quad( - vertices: &mut Vec, - normals: &mut Vec, - indices: &mut Vec, - quad: [Vec3f; 4], - ) { - let base = vertices.len() as u32; - let normal = Vec3f::cross(quad[1] - quad[0], quad[2] - quad[0]).normalize(); - vertices.extend_from_slice(&quad); - normals.extend_from_slice(&[normal; 4]); - indices.extend_from_slice(&[base, base + 1, base + 2, base, base + 2, base + 3]); - } - - fn make_triangle_chunk(triangle: [Vec3f; 3]) -> XrDepthMeshChunk { - make_triangle_chunk_with_key(ChunkKey::new(0, 0, 0), &[triangle], 1) - } - - fn make_triangle_chunk_with_key( - chunk_key: ChunkKey, - triangles: &[[Vec3f; 3]], - fingerprint: u64, - ) -> XrDepthMeshChunk { - let mut vertices = Vec::with_capacity(triangles.len() * 3); - let mut normals = Vec::with_capacity(triangles.len() * 3); - let mut indices = Vec::with_capacity(triangles.len() * 3); - let mut bounds_min = triangles[0][0]; - let mut bounds_max = triangles[0][0]; - for triangle in triangles { - let normal = - Vec3f::cross(triangle[1] - triangle[0], triangle[2] - triangle[0]).normalize(); - let base = vertices.len() as u32; - vertices.extend_from_slice(triangle); - normals.extend_from_slice(&[normal; 3]); - indices.extend_from_slice(&[base, base + 1, base + 2]); - for vertex in triangle { - bounds_min = Vec3f::min_componentwise(bounds_min, *vertex); - bounds_max = Vec3f::max_componentwise(bounds_max, *vertex); - } - } - - XrDepthMeshChunk { - generation: 1, - chunk_key, - fingerprint, - bounds_min, - bounds_max, - vertices, - normals, - indices, - planar_patches: Vec::new(), - } - } - - fn make_bulged_plane_chunk_with_key( - chunk_key: ChunkKey, - grid: usize, - size: f32, - bulge: f32, - fingerprint: u64, - ) -> XrDepthMeshChunk { - let grid = grid.max(2); - let step = size / (grid.saturating_sub(1) as f32); - let half = size * 0.5; - let mut triangles = Vec::with_capacity((grid - 1) * (grid - 1) * 2); - let mut points = vec![vec3f(0.0, 0.0, 0.0); grid * grid]; - - for z in 0..grid { - for x in 0..grid { - let px = -half + x as f32 * step; - let pz = -half + z as f32 * step; - let radial = ((px / half).powi(2) + (pz / half).powi(2)).clamp(0.0, 1.0); - let py = bulge * (1.0 - radial); - points[z * grid + x] = vec3f(px, py, pz); - } - } - - for z in 0..grid - 1 { - for x in 0..grid - 1 { - let p00 = points[z * grid + x]; - let p10 = points[z * grid + x + 1]; - let p01 = points[(z + 1) * grid + x]; - let p11 = points[(z + 1) * grid + x + 1]; - triangles.push([p00, p10, p01]); - triangles.push([p10, p11, p01]); - } - } - - make_triangle_chunk_with_key(chunk_key, &triangles, fingerprint) - } - - fn make_surface_mesh(quads: &[[Vec3f; 4]]) -> SurfaceMesh32 { - let mut positions = Vec::new(); - let mut normals = Vec::new(); - let mut indices = Vec::new(); - for quad in quads { - let base = positions.len() as u32; - let normal = Vec3f::cross(quad[1] - quad[0], quad[2] - quad[0]).normalize(); - for vertex in quad { - positions.push([vertex.x, vertex.y, vertex.z]); - normals.push([normal.x, normal.y, normal.z]); - } - indices.extend_from_slice(&[base, base + 1, base + 2, base, base + 2, base + 3]); - } - SurfaceMesh32 { - positions, - normals, - indices, - } - } - - fn plane_dense_field(size: VoxelCoord, y_plane: f32) -> Vec { - let mut dense = vec![0.0; (size.x * size.y * size.z) as usize]; - for z in 0..size.z { - for y in 0..size.y { - for x in 0..size.x { - let coord = VoxelCoord::new(x, y, z); - dense[flatten_coord(coord, size)] = y as f32 - y_plane; - } - } - } - dense - } - - fn fill_volume_signed_distance_field( - volume: &mut DepthMeshVolume, - min_coord: VoxelCoord, - max_coord: VoxelCoord, - signed_distance: impl Fn(Vec3f) -> f32, - ) { - let tsd_distance_meters = depth_tsd_distance_meters(volume.voxel_size_meters); - for z in min_coord.z..=max_coord.z { - for y in min_coord.y..=max_coord.y { - for x in min_coord.x..=max_coord.x { - let coord = VoxelCoord::new(x, y, z); - let world = volume.mesh_grid.voxel_center_world(coord); - let normalized = - (signed_distance(world) / tsd_distance_meters).clamp(-1.0, 1.0); - volume.mesh_grid.overwrite_normalized_distance( - coord, - normalized, - volume.generation.max(1), - ); - } - } - } - volume.update_bounds(); - } - - fn room_signed_distance( - point: Vec3f, - half_width_x: f32, - half_width_z: f32, - floor_y: f32, - ceiling_y: f32, - ) -> f32 { - (point.y - floor_y) - .min(ceiling_y - point.y) - .min(point.x + half_width_x) - .min(half_width_x - point.x) - .min(point.z + half_width_z) - .min(half_width_z - point.z) - } - - fn scan_full_tsdf_plane_set(volume: &mut DepthMeshVolume) { - enqueue_all_plane_scan_chunks(volume); - let mut guard = 0usize; - while !volume.pending_plane_scan_chunk_queue.is_empty() { - process_incremental_tsdf_plane_scan(volume, 32); - guard += 1; - assert!( - guard <= 1024, - "plane scan did not drain pending queue, pending={}", - volume.pending_plane_scan_chunk_queue.len() - ); - } - } - - fn describe_plane_patches(patches: &[XrDepthPlanePatch]) -> String { - patches - .iter() - .map(|patch| { - format!( - "{:?} center=({:.2},{:.2},{:.2}) normal=({:.2},{:.2},{:.2}) ext=({:.2},{:.2}) area={:.2}", - patch.kind, - patch.center.x, - patch.center.y, - patch.center.z, - patch.normal.x, - patch.normal.y, - patch.normal.z, - patch.half_extent_tangent * 2.0, - patch.half_extent_bitangent * 2.0, - patch.area, - ) - }) - .collect::>() - .join(" | ") - } - - fn make_wall_plane_patch( - center: Vec3f, - normal: Vec3f, - half_height: f32, - half_width: f32, - ) -> XrDepthPlanePatch { - let normal = vec3f(normal.x, 0.0, normal.z).normalize(); - let tangent = vec3f(0.0, 1.0, 0.0); - let bitangent = Vec3f::cross(normal, tangent).normalize(); - XrDepthPlanePatch { - generation: 1, - kind: XrDepthPlaneKind::Wall, - center, - normal, - tangent, - bitangent, - half_extent_tangent: half_height, - half_extent_bitangent: half_width, - area: (half_height * 2.0) * (half_width * 2.0), - support_triangles: 8, - } - } - - fn bulged_plane_height(world_x: f32, world_z: f32, radius: f32, bulge: f32) -> f32 { - let radial = ((world_x / radius).powi(2) + (world_z / radius).powi(2)).clamp(0.0, 1.0); - bulge * (1.0 - radial) - } - - fn box_signed_distance(point: Vec3f, center: Vec3f, half_extents: Vec3f) -> f32 { - let local = point - center; - let q = vec3f( - local.x.abs() - half_extents.x, - local.y.abs() - half_extents.y, - local.z.abs() - half_extents.z, - ); - let outside = vec3f(q.x.max(0.0), q.y.max(0.0), q.z.max(0.0)).length(); - let inside = q.x.max(q.y.max(q.z)).min(0.0); - outside + inside - } - - #[test] - fn planar_surface_mesh_reduction_collapses_bumpy_wall() { - if !DEPTH_ENABLE_REDUCED_PLANAR_PATCHES { - return; - } - let mut quads = Vec::new(); - for y in 0..3 { - for z in 0..3 { - let y0 = y as f32 * 0.6; - let y1 = y0 + 0.6; - let z0 = z as f32 * 0.6; - let z1 = z0 + 0.6; - let x00 = 2.0 + if (y + z) % 2 == 0 { 0.03 } else { -0.02 }; - let x10 = 2.0 + if (y + z + 1) % 2 == 0 { 0.03 } else { -0.02 }; - let x11 = 2.0 + if (y + z + 2) % 2 == 0 { 0.03 } else { -0.02 }; - let x01 = 2.0 + if (y + z + 3) % 2 == 0 { 0.03 } else { -0.02 }; - quads.push([ - vec3f(x00, y0, z0), - vec3f(x10, y0, z1), - vec3f(x11, y1, z1), - vec3f(x01, y1, z0), - ]); - } - } - let raw = make_surface_mesh(&quads); - let simplified = simplify_surface_mesh_planar_regions(raw.clone()); - assert!( - simplified.mesh.indices.len() < raw.indices.len(), - "expected fewer triangles after planar reduction: raw={} simplified={}", - raw.indices.len() / 3, - simplified.mesh.indices.len() / 3 - ); - assert!( - simplified.mesh.indices.len() / 3 <= 4, - "bumpy wall should collapse close to one quad, got {} triangles", - simplified.mesh.indices.len() / 3 - ); - } - - #[test] - fn planar_surface_mesh_reduction_keeps_doorway_gap() { - if !DEPTH_ENABLE_REDUCED_PLANAR_PATCHES { - return; - } - let mut quads = Vec::new(); - for y in 0..4 { - let y0 = y as f32 * 0.5; - let y1 = y0 + 0.5; - quads.push([ - vec3f(0.0, y0, 0.0), - vec3f(0.0, y0, 0.8), - vec3f(0.0, y1, 0.8), - vec3f(0.0, y1, 0.0), - ]); - quads.push([ - vec3f(0.0, y0, 1.6), - vec3f(0.0, y0, 2.4), - vec3f(0.0, y1, 2.4), - vec3f(0.0, y1, 1.6), - ]); - } - for z in 0..2 { - let z0 = 0.8 + z as f32 * 0.4; - let z1 = z0 + 0.4; - quads.push([ - vec3f(0.0, 2.0, z0), - vec3f(0.0, 2.0, z1), - vec3f(0.0, 2.4, z1), - vec3f(0.0, 2.4, z0), - ]); - } - let raw = make_surface_mesh(&quads); - let simplified = simplify_surface_mesh_planar_regions(raw.clone()); - let simplified_triangles = simplified.mesh.indices.len() / 3; - assert!( - simplified_triangles < raw.indices.len() / 3, - "expected doorway wall to simplify: raw={} simplified={}", - raw.indices.len() / 3, - simplified_triangles - ); - assert!( - simplified_triangles >= 6, - "doorway should stay split into multiple wall quads, got {} triangles", - simplified_triangles - ); - } - - #[test] - fn planar_surface_mesh_reduction_only_consumes_rect_covered_triangles() { - if !DEPTH_ENABLE_REDUCED_PLANAR_PATCHES { - return; - } - let region = SimplifiedPlaneRegion { - group: ExtractedPlaneGroup::Vertical, - source_triangle_indices: vec![11, 12], - triangles: vec![ - ExtractedPlaneTriangle { - source_triangle_index: 11, - group: ExtractedPlaneGroup::Vertical, - area: 0.25, - normal: vec3f(1.0, 0.0, 0.0), - centroid: vec3f(0.0, 0.25, 0.25), - vertices: [ - vec3f(0.0, 0.0, 0.0), - vec3f(0.0, 0.5, 0.0), - vec3f(0.0, 0.25, 0.5), - ], - }, - ExtractedPlaneTriangle { - source_triangle_index: 12, - group: ExtractedPlaneGroup::Vertical, - area: 0.25, - normal: vec3f(1.0, 0.0, 0.0), - centroid: vec3f(0.0, 0.25, 1.25), - vertices: [ - vec3f(0.0, 0.0, 1.0), - vec3f(0.0, 0.5, 1.0), - vec3f(0.0, 0.25, 1.5), - ], - }, - ], - }; - let emitted = EmittedPlanarRegion { - tangent: vec3f(0.0, 1.0, 0.0), - bitangent: vec3f(0.0, 0.0, 1.0), - rects: vec![PlanarRectCoverage { - min_u: 0.0, - max_u: 0.6, - min_v: 0.0, - max_v: 0.6, - }], - }; - - let covered = covered_planar_region_triangles(®ion, &emitted); - assert!(covered.contains(&11), "covered triangle should be consumed"); - assert!( - !covered.contains(&12), - "triangle outside emitted rect coverage must stay as raw mesh" - ); - } - - #[test] - fn dense_hole_repair_fills_isolated_missing_sample() { - let size = VoxelCoord::new(6, 6, 6); - let mut dense = plane_dense_field(size, 2.4); - let missing = VoxelCoord::new(3, 2, 3); - let missing_index = flatten_coord(missing, size); - dense[missing_index] = f32::NEG_INFINITY; - let mut scratch = Vec::new(); - repair_dense_meshing_holes(&mut dense, &mut scratch, size); - assert!( - dense[missing_index].is_finite(), - "isolated dense puncture should be repaired for meshing" - ); - assert!( - (dense[missing_index] + 0.4).abs() < 0.11, - "repaired sample drifted too far from local plane field: {}", - dense[missing_index] - ); - } - - #[test] - fn geometry_query_skips_planar_patches_when_requested() { - if !DEPTH_ENABLE_REDUCED_PLANAR_PATCHES { - return; - } - let mut volume = DepthMeshVolume::new(1, 0.1); - volume.mesh_generation = 7; - volume.plane_patches.push(XrDepthPlanePatch { - generation: 1, - kind: XrDepthPlaneKind::Table, - center: vec3f(0.0, 0.75, 0.0), - normal: vec3f(0.0, 1.0, 0.0), - tangent: vec3f(1.0, 0.0, 0.0), - bitangent: vec3f(0.0, 0.0, 1.0), - half_extent_tangent: 0.4, - half_extent_bitangent: 0.3, - area: 0.48, - support_triangles: 2, - }); - - let planar_query = XrDepthMeshQuery { - key: 1, - center: vec3f(0.0, 0.83, 0.0), - predicted_center: vec3f(0.0, 0.83, 0.0), - velocity: vec3f(0.0, 0.0, 0.0), - radius: 0.02, - max_distance: 0.15, - include_planar_patches: false, - }; - assert!( - matches!( - evaluate_geometry_query(&volume, planar_query, 1), - XrDepthMeshQueryResult::Miss { .. } - ), - "planar query should miss when planar patches are disabled" - ); - - let planar_hit = evaluate_geometry_query( - &volume, - XrDepthMeshQuery { - include_planar_patches: true, - ..planar_query - }, - 2, - ); - match planar_hit { - XrDepthMeshQueryResult::Hit(hit) => { - assert!(hit.from_planar_patch, "expected a planar patch hit"); - } - XrDepthMeshQueryResult::Miss { .. } => { - panic!("expected a planar hit when planar patches are enabled"); - } - } - - volume.mesh_chunks.push(make_triangle_chunk([ - vec3f(0.0, 0.4, 0.0), - vec3f(0.2, 0.4, 0.0), - vec3f(0.0, 0.4, 0.2), - ])); - let mesh_hit = evaluate_geometry_query( - &volume, - XrDepthMeshQuery { - key: 2, - center: vec3f(0.05, 0.5, 0.05), - predicted_center: vec3f(0.05, 0.5, 0.05), - velocity: vec3f(0.0, 0.0, 0.0), - radius: 0.02, - max_distance: 0.15, - include_planar_patches: false, - }, - 3, - ); - match mesh_hit { - XrDepthMeshQueryResult::Hit(hit) => { - assert!(!hit.from_planar_patch, "expected raw mesh fallback hit"); - } - XrDepthMeshQueryResult::Miss { .. } => { - panic!("expected a raw mesh hit with planar patches disabled"); - } - } - } - - #[test] - fn geometry_query_preserves_exact_reduced_planar_mesh_triangle() { - if !DEPTH_ENABLE_REDUCED_PLANAR_PATCHES { - return; - } - let triangle = [ - vec3f(-0.25, 0.75, -0.20), - vec3f(0.25, 0.75, -0.20), - vec3f(-0.25, 0.75, 0.20), - ]; - let mut chunk = make_triangle_chunk(triangle); - chunk.planar_patches.push(XrDepthPlanePatch { - generation: 1, - kind: XrDepthPlaneKind::Table, - center: vec3f(0.0, 0.75, 0.0), - normal: vec3f(0.0, 1.0, 0.0), - tangent: vec3f(1.0, 0.0, 0.0), - bitangent: vec3f(0.0, 0.0, 1.0), - half_extent_tangent: 0.25, - half_extent_bitangent: 0.20, - area: 0.20, - support_triangles: 2, - }); - - let mut volume = DepthMeshVolume::new(1, 0.1); - volume.mesh_generation = 9; - volume.mesh_chunks.push(chunk); - - let result = evaluate_geometry_query( - &volume, - XrDepthMeshQuery { - key: 7, - center: vec3f(-0.05, 0.82, -0.05), - predicted_center: vec3f(-0.05, 0.82, -0.05), - velocity: vec3f(0.0, 0.0, 0.0), - radius: 0.02, - max_distance: 0.15, - include_planar_patches: false, - }, - 1, - ); - - match result { - XrDepthMeshQueryResult::Hit(hit) => { - assert!( - hit.from_planar_patch, - "expected reduced planar mesh classification" - ); - assert!( - hit.patch - .windows(2) - .any(|edge| (edge[1] - edge[0]).length() > 1.0e-4), - "expected reduced planar mesh hit to carry its full support quad" - ); - for expected in &triangle { - assert!( - hit.triangle - .iter() - .any(|got| (*got - *expected).length() < 1.0e-4), - "expected exact triangle preservation: got {:?}, expected vertex {:?}", - hit.triangle, - expected - ); - } - } - XrDepthMeshQueryResult::Miss { .. } => { - panic!("expected reduced planar mesh hit"); - } - } - } - - #[test] - fn geometry_query_returns_support_halfspace_only() { - let mut volume = DepthMeshVolume::new(1, 0.1); - volume.mesh_generation = 10; - volume.mesh_chunks.push(make_triangle_chunk([ - vec3f(-0.30, 0.0, -0.30), - vec3f(0.30, 0.0, -0.30), - vec3f(-0.30, 0.0, 0.30), - ])); - volume.mesh_chunks.push(make_triangle_chunk([ - vec3f(0.18, -0.20, -0.20), - vec3f(0.18, 0.30, -0.20), - vec3f(0.18, -0.20, 0.20), - ])); - - let result = evaluate_geometry_query( - &volume, - XrDepthMeshQuery { - key: 11, - center: vec3f(0.05, 0.08, 0.0), - predicted_center: vec3f(0.05, 0.08, 0.0), - velocity: vec3f(0.0, 0.0, 0.0), - radius: 0.12, - max_distance: 0.20, - include_planar_patches: false, - }, - 1, - ); - - match result { - XrDepthMeshQueryResult::Hit(hit) => { - assert!( - hit.normal.y >= DEPTH_QUERY_SUPPORT_NORMAL_Y_MIN, - "expected primary hit to be the support surface, got normal {:?}", - hit.normal - ); - assert!( - hit.additional_hits.is_empty(), - "expected one support result only, got {:?}", - hit.additional_hits - ); - let XrDepthMeshQueryColliderGeometry::HalfSpace(plane) = hit.collider.geometry; - assert!( - plane.normal.y >= DEPTH_QUERY_SUPPORT_NORMAL_Y_MIN, - "expected support half-space, got normal {:?}", - plane.normal - ); - } - XrDepthMeshQueryResult::Miss { .. } => { - panic!("expected support and lateral geometry hits"); - } - } - } - - #[test] - fn geometry_query_returns_miss_for_lateral_only_geometry() { - let mut volume = DepthMeshVolume::new(1, 0.1); - volume.mesh_generation = 12; - volume.mesh_chunks.push(make_triangle_chunk([ - vec3f(0.18, -0.20, -0.20), - vec3f(0.18, 0.30, -0.20), - vec3f(0.18, -0.20, 0.20), - ])); - - let result = evaluate_geometry_query( - &volume, - XrDepthMeshQuery { - key: 13, - center: vec3f(0.06, 0.08, 0.0), - predicted_center: vec3f(0.06, 0.08, 0.0), - velocity: vec3f(0.0, 0.0, 0.0), - radius: 0.04, - max_distance: 0.12, - include_planar_patches: false, - }, - 1, - ); - - assert!( - matches!(result, XrDepthMeshQueryResult::Miss { .. }), - "expected lateral-only geometry to return no support plane" - ); - } - - #[test] - fn geometry_query_builds_connected_support_halfspace_within_chunk() { - let mut volume = DepthMeshVolume::new(1, 0.1); - volume.mesh_generation = 14; - volume.mesh_chunks.push(make_triangle_chunk_with_key( - ChunkKey::new(0, 0, 0), - &[ - [ - vec3f(-0.20, 0.0, -0.20), - vec3f(0.00, 0.0, -0.20), - vec3f(-0.20, 0.0, 0.20), - ], - [ - vec3f(0.00, 0.0, -0.20), - vec3f(0.20, 0.0, -0.20), - vec3f(0.00, 0.0, 0.20), - ], - ], - 2, - )); - - let result = evaluate_geometry_query( - &volume, - XrDepthMeshQuery { - key: 15, - center: vec3f(0.01, 0.07, -0.18), - predicted_center: vec3f(0.01, 0.07, -0.18), - velocity: vec3f(0.0, 0.0, 0.0), - radius: 0.04, - max_distance: 0.12, - include_planar_patches: false, - }, - 1, - ); - - match result { - XrDepthMeshQueryResult::Hit(hit) => { - let XrDepthMeshQueryColliderGeometry::HalfSpace(plane) = hit.collider.geometry; - assert!( - plane.half_extent_tangent >= 0.10 || plane.half_extent_bitangent >= 0.10, - "expected connected support plane extents, got {:?}", - plane - ); - } - XrDepthMeshQueryResult::Miss { .. } => { - panic!("expected support patch hit"); - } - } - } - - #[test] - fn geometry_query_builds_connected_support_halfspace_across_shared_edge() { - let mut volume = DepthMeshVolume::new(1, 0.1); - volume.mesh_generation = 13; - volume.mesh_chunks.push(make_triangle_chunk([ - vec3f(-0.20, 0.0, -0.20), - vec3f(0.20, 0.0, -0.20), - vec3f(-0.20, 0.0, 0.20), - ])); - volume.mesh_chunks.push(make_triangle_chunk([ - vec3f(0.20, 0.0, -0.20), - vec3f(0.20, 0.0, 0.20), - vec3f(-0.20, 0.0, 0.20), - ])); - - let result = evaluate_geometry_query( - &volume, - XrDepthMeshQuery { - key: 14, - center: vec3f(0.0, 0.07, 0.0), - predicted_center: vec3f(0.0, 0.07, 0.0), - velocity: vec3f(0.0, 0.0, 0.0), - radius: 0.04, - max_distance: 0.12, - include_planar_patches: false, - }, - 1, - ); - - match result { - XrDepthMeshQueryResult::Hit(hit) => { - let XrDepthMeshQueryColliderGeometry::HalfSpace(plane) = hit.collider.geometry; - assert!( - plane.half_extent_tangent >= 0.12 || plane.half_extent_bitangent >= 0.12, - "expected support plane to remain connected across the shared edge, got {:?}", - plane - ); - } - XrDepthMeshQueryResult::Miss { .. } => { - panic!("expected support hits across the shared edge"); - } - } - } - - #[test] - fn geometry_query_stabilizes_bulged_support_plane() { - let mut volume = DepthMeshVolume::new(1, 0.1); - volume.mesh_generation = 15; - volume.mesh_chunks.push(make_bulged_plane_chunk_with_key( - ChunkKey::new(0, 0, 0), - 5, - 0.40, - 0.012, - 3, - )); - - let query = XrDepthMeshQuery { - key: 16, - center: vec3f(0.0, 0.07, 0.0), - predicted_center: vec3f(0.0, 0.07, 0.0), - velocity: vec3f(0.0, 0.0, 0.0), - radius: 0.05, - max_distance: 0.12, - include_planar_patches: false, - }; - - let result = evaluate_geometry_query(&volume, query, 1); - match result { - XrDepthMeshQueryResult::Hit(hit) => { - let XrDepthMeshQueryColliderGeometry::HalfSpace(plane) = hit.collider.geometry; - assert!( - plane.normal.y >= 0.985, - "expected nearly horizontal support plane on shallow bulge, got {:?}", - plane.normal - ); - - let support_surface = XrDepthMeshQuerySurfaceHit { - distance: hit.distance, - point: hit.point, - normal: hit.normal, - from_planar_patch: hit.from_planar_patch, - triangle: hit.triangle, - patch: hit.patch, - chunk_key: hit.chunk_key, - }; - let mut max_outside = f32::NEG_INFINITY; - visit_support_plane_triangles( - &volume, - support_surface, - query.radius, - |triangle, _normal, _area, _centroid| { - for vertex in triangle { - max_outside = max_outside.max(plane.normal.dot(vertex - plane.point)); - } - }, - ); - assert!( - max_outside <= 0.003, - "support plane should stay on the local support envelope, got outside distance {}", - max_outside - ); - } - XrDepthMeshQueryResult::Miss { .. } => { - panic!("expected support hit on bulged synthetic plane"); - } - } - } - - #[test] - fn geometry_query_from_depth_grid_returns_support_halfspace() { - let mut volume = DepthMeshVolume::new(1, 0.1); - volume.generation = 21; - fill_volume_signed_distance_field( - &mut volume, - VoxelCoord::new(-4, -4, -4), - VoxelCoord::new(4, 4, 4), - |world| world.y, - ); - - let result = evaluate_geometry_query( - &volume, - XrDepthMeshQuery { - key: 101, - center: vec3f(0.0, 0.09, 0.0), - predicted_center: vec3f(0.0, 0.07, 0.0), - velocity: vec3f(0.0, -0.2, 0.0), - radius: 0.05, - max_distance: 0.12, - include_planar_patches: false, - }, - 1, - ); - - match result { - XrDepthMeshQueryResult::Hit(hit) => { - let XrDepthMeshQueryColliderGeometry::HalfSpace(plane) = hit.collider.geometry; - assert!( - plane.normal.y >= 0.98, - "expected near-horizontal support plane from TSDF, got {:?}", - plane.normal - ); - assert!( - plane.point.y.abs() <= 0.025, - "expected plane close to y=0, got {:?}", - plane.point - ); - } - XrDepthMeshQueryResult::Miss { .. } => { - panic!("expected TSDF support hit"); - } - } - } - - #[test] - fn tsdf_alignment_descriptor_extracts_wall_samples_without_meshing() { - let mut volume = DepthMeshVolume::new(1, 0.05); - volume.generation = 27; - fill_volume_signed_distance_field( - &mut volume, - VoxelCoord::new(-18, -6, -18), - VoxelCoord::new(18, 28, 18), - |world| world.y.min(0.55 - world.x), - ); - scan_full_tsdf_plane_set(&mut volume); - - let (descriptor, debug) = build_tsdf_alignment_descriptor(&volume); - let descriptor = descriptor.unwrap_or_else(|| { - panic!( - "expected TSDF-native alignment descriptor, debug={debug:?}, chunks={}", - volume.mesh_grid.chunks.len() - ) - }); - - assert!( - volume.mesh_chunks.is_empty(), - "TSDF descriptor test should not depend on render-mesh chunks" - ); - assert!( - !volume.plane_patches.is_empty(), - "expected plane scan to populate plane patches" - ); - assert!( - descriptor - .samples - .iter() - .any(|sample| sample.kind == XrDepthAlignSampleKind::Wall), - "expected wall samples in TSDF descriptor" - ); - assert!( - debug.wall_sample_count >= DEPTH_ALIGN_MIN_WALL_SAMPLES as u32, - "expected enough wall samples, got {}", - debug.wall_sample_count - ); - assert!( - descriptor.floor_y.abs() <= 0.001, - "expected wall-only descriptor floor_y to stay at 0, got {}", - descriptor.floor_y - ); - assert!( - descriptor - .wall_normal_histogram - .iter() - .copied() - .sum::() - > 0.0, - "expected non-empty wall histogram from TSDF descriptor" - ); - } - - #[test] - fn tsdf_plane_scan_detects_box_room_walls() { - let mut volume = DepthMeshVolume::new(1, 0.05); - volume.generation = 31; - let room_half_x = 1.40; - let room_half_z = 1.10; - let floor_y = 0.0; - let ceiling_y = 2.40; - fill_volume_signed_distance_field( - &mut volume, - VoxelCoord::new(-34, -8, -30), - VoxelCoord::new(34, 56, 30), - |world| room_signed_distance(world, room_half_x, room_half_z, floor_y, ceiling_y), - ); - - scan_full_tsdf_plane_set(&mut volume); - let patches = &volume.plane_patches; - assert!( - !patches.is_empty(), - "expected room plane scan to emit patches" - ); - - let wall_patches = patches - .iter() - .filter(|patch| patch.kind == XrDepthPlaneKind::Wall) - .collect::>(); - assert!( - wall_patches.len() >= 3, - "expected several wall patches, got {} from {}", - wall_patches.len(), - describe_plane_patches(patches) - ); - assert!( - wall_patches.len() == patches.len(), - "expected wall-only plane scan output, got {}", - describe_plane_patches(patches) - ); - assert!( - wall_patches - .iter() - .any(|patch| (patch.center.x.abs() - room_half_x).abs() <= 0.14), - "expected wall near x bounds, got {}", - describe_plane_patches(patches) - ); - assert!( - wall_patches - .iter() - .any(|patch| (patch.center.z.abs() - room_half_z).abs() <= 0.14), - "expected wall near z bounds, got {}", - describe_plane_patches(patches) - ); - assert!( - patches.iter().all(|patch| { - patch.center.x.abs() <= room_half_x + 0.25 - && patch.center.z.abs() <= room_half_z + 0.25 - && patch.center.y >= floor_y - 0.25 - && patch.center.y <= ceiling_y + 0.25 - }), - "expected all room patches inside expanded room bounds, got {}", - describe_plane_patches(patches) - ); - } - - #[test] - fn wall_track_stability_retains_wall_through_brief_miss() { - let mut tracks = Vec::new(); - let wall = - make_wall_plane_patch(vec3f(1.20, 1.10, -0.35), vec3f(1.0, 0.0, 0.0), 0.55, 0.45); - - let first = stabilize_wall_scan_patches(&mut tracks, vec![wall.clone()]); - assert_eq!(first.len(), 1, "new wall should appear immediately"); - - let second = stabilize_wall_scan_patches(&mut tracks, vec![wall.clone()]); - assert_eq!(second.len(), 1, "wall should remain after confirmation"); - - let missing = stabilize_wall_scan_patches(&mut tracks, Vec::new()); - assert_eq!( - missing.len(), - 1, - "stable wall should persist across a brief missing update" - ); - assert!( - (missing[0].center - wall.center).length() < 0.02, - "missing update should not move the stable wall too far: {:?}", - missing[0] - ); - } - - #[test] - fn wall_track_stability_damps_plane_distance_jitter() { - let mut tracks = Vec::new(); - let base = - make_wall_plane_patch(vec3f(1.00, 1.00, -0.20), vec3f(1.0, 0.0, 0.0), 0.60, 0.40); - let jittered = - make_wall_plane_patch(vec3f(1.10, 1.02, -0.12), vec3f(1.0, 0.0, 0.0), 0.58, 0.44); - - let _ = stabilize_wall_scan_patches(&mut tracks, vec![base.clone()]); - let stabilized = stabilize_wall_scan_patches(&mut tracks, vec![jittered.clone()]); - assert_eq!(stabilized.len(), 1); - - let stable_distance = stabilized[0].center.dot(stabilized[0].normal); - let base_distance = base.center.dot(base.normal); - let jittered_distance = jittered.center.dot(jittered.normal); - assert!( - stable_distance > base_distance && stable_distance < jittered_distance, - "stable wall should move toward jittered measurement without fully snapping: stable={stable_distance} base={base_distance} jittered={jittered_distance}" - ); - assert!( - (stable_distance - base_distance) < (jittered_distance - base_distance), - "stable wall should damp depth jitter" - ); - } - - #[test] - fn room_wall_candidates_pick_dominant_plane_group_per_side() { - let east_major_a = - make_wall_plane_patch(vec3f(1.20, 1.00, -0.85), vec3f(1.0, 0.0, 0.0), 0.60, 0.28); - let east_major_b = - make_wall_plane_patch(vec3f(1.22, 1.04, 0.15), vec3f(1.0, 0.0, 0.0), 0.58, 0.34); - let east_alcove = - make_wall_plane_patch(vec3f(1.48, 0.98, 0.95), vec3f(1.0, 0.0, 0.0), 0.54, 0.18); - let west = - make_wall_plane_patch(vec3f(-1.05, 1.02, -0.10), vec3f(-1.0, 0.0, 0.0), 0.62, 0.44); - let north = - make_wall_plane_patch(vec3f(0.10, 1.01, 1.32), vec3f(0.0, 0.0, 1.0), 0.60, 0.42); - let south = - make_wall_plane_patch(vec3f(-0.18, 0.99, -0.92), vec3f(0.0, 0.0, -1.0), 0.58, 0.40); - - let candidates = build_room_wall_candidates(&[ - east_major_a, - east_major_b, - east_alcove, - west, - north, - south, - ]); - let east = candidates - .iter() - .find(|candidate| candidate.normal.x > 0.9) - .copied() - .expect("expected positive-x room wall candidate"); - assert!( - (east.plane_distance - 1.21).abs() <= 0.08, - "dominant east wall should stay on the major wall plane, got {east:?}" - ); - assert!( - (east.plane_distance - 1.48).abs() > 0.14, - "alcove plane should not become the exported room wall, got {east:?}" - ); - } - - #[test] - fn room_wall_features_form_box_from_stable_tracks() { - let mut room_tracks = Vec::new(); - let patches = vec![ - make_wall_plane_patch(vec3f(1.18, 1.00, -0.82), vec3f(1.0, 0.0, 0.0), 0.62, 0.30), - make_wall_plane_patch(vec3f(1.19, 1.02, 0.28), vec3f(1.0, 0.0, 0.0), 0.58, 0.38), - make_wall_plane_patch(vec3f(-1.08, 1.01, -0.06), vec3f(-1.0, 0.0, 0.0), 0.60, 0.42), - make_wall_plane_patch(vec3f(0.06, 0.99, 1.36), vec3f(0.0, 0.0, 1.0), 0.58, 0.46), - make_wall_plane_patch(vec3f(-0.12, 1.03, -0.94), vec3f(0.0, 0.0, -1.0), 0.60, 0.44), - ]; - - for _ in 0..2 { - stabilize_room_wall_candidates(&mut room_tracks, build_room_wall_candidates(&patches)); - } - let features = visible_room_wall_features(&room_tracks); - assert_eq!( - features.len(), - 4, - "expected four inferred room walls from the patch box, got {features:?}" - ); - let east = features - .iter() - .find(|feature| feature.normal.x > 0.9) - .expect("expected positive-x inferred wall"); - assert!( - east.half_extent_along >= 1.00, - "orthogonal wall intersections should expand east wall span, got {east:?}" - ); - assert!( - east.min_y.abs() <= 1.0e-4 - && (east.max_y - DEPTH_ROOM_WALL_HEIGHT_METERS).abs() <= 1.0e-4, - "room walls should use fixed height for debug and matching, got {east:?}" - ); - assert!( - (east.center.y - DEPTH_ROOM_WALL_HEIGHT_METERS * 0.5).abs() <= 1.0e-4, - "room wall center should sit halfway up the fixed wall height, got {east:?}" - ); - } - - #[test] - fn room_wall_tracks_remain_visible_when_patch_updates_pause() { - let mut room_tracks = Vec::new(); - let patches = vec![ - make_wall_plane_patch(vec3f(1.18, 1.00, -0.82), vec3f(1.0, 0.0, 0.0), 0.62, 0.30), - make_wall_plane_patch(vec3f(1.19, 1.02, 0.28), vec3f(1.0, 0.0, 0.0), 0.58, 0.38), - make_wall_plane_patch(vec3f(-1.08, 1.01, -0.06), vec3f(-1.0, 0.0, 0.0), 0.60, 0.42), - make_wall_plane_patch(vec3f(0.06, 0.99, 1.36), vec3f(0.0, 0.0, 1.0), 0.58, 0.46), - make_wall_plane_patch(vec3f(-0.12, 1.03, -0.94), vec3f(0.0, 0.0, -1.0), 0.60, 0.44), - ]; - - for _ in 0..2 { - stabilize_room_wall_candidates(&mut room_tracks, build_room_wall_candidates(&patches)); - } - for _ in 0..24 { - stabilize_room_wall_candidates(&mut room_tracks, Vec::new()); - } - - let features = visible_room_wall_features(&room_tracks); - assert_eq!( - features.len(), - 4, - "room walls should persist even when fresh patch evidence is briefly missing, got {features:?}" - ); - } - - #[test] - fn geometry_query_from_depth_grid_returns_lateral_impact_plane() { - let mut volume = DepthMeshVolume::new(1, 0.1); - volume.generation = 22; - fill_volume_signed_distance_field( - &mut volume, - VoxelCoord::new(-4, -4, -4), - VoxelCoord::new(4, 4, 4), - |world| 0.18 - world.x, - ); - - let result = evaluate_geometry_query( - &volume, - XrDepthMeshQuery { - key: 102, - center: vec3f(0.06, 0.10, 0.0), - predicted_center: vec3f(0.16, 0.10, 0.0), - velocity: vec3f(0.8, 0.0, 0.0), - radius: 0.05, - max_distance: 0.12, - include_planar_patches: false, - }, - 1, - ); - - match result { - XrDepthMeshQueryResult::Hit(hit) => { - let XrDepthMeshQueryColliderGeometry::HalfSpace(plane) = hit.collider.geometry; - assert_eq!( - hit.collider.role, - XrDepthMeshQueryColliderRole::Impact, - "expected TSDF wall hit to resolve as an impact plane" - ); - assert!( - plane.normal.x <= -0.85 && plane.normal.y.abs() <= 0.25, - "expected mostly vertical wall normal, got {:?}", - plane.normal - ); - assert!( - hit.collider.restitution >= 0.3, - "expected impact plane restitution, got {}", - hit.collider.restitution - ); - } - XrDepthMeshQueryResult::Miss { .. } => { - panic!("expected TSDF wall hit to produce an impact plane"); - } - } - } - - #[test] - fn geometry_query_from_depth_grid_returns_ceiling_impact_plane() { - let mut volume = DepthMeshVolume::new(1, 0.1); - volume.generation = 23; - fill_volume_signed_distance_field( - &mut volume, - VoxelCoord::new(-4, -4, -4), - VoxelCoord::new(4, 4, 4), - |world| world.y - 0.18, - ); - - let result = evaluate_geometry_query( - &volume, - XrDepthMeshQuery { - key: 103, - center: vec3f(0.0, 0.06, 0.0), - predicted_center: vec3f(0.0, 0.16, 0.0), - velocity: vec3f(0.0, 0.9, 0.0), - radius: 0.05, - max_distance: 0.12, - include_planar_patches: false, - }, - 1, - ); - - match result { - XrDepthMeshQueryResult::Hit(hit) => { - let XrDepthMeshQueryColliderGeometry::HalfSpace(plane) = hit.collider.geometry; - assert_eq!( - hit.collider.role, - XrDepthMeshQueryColliderRole::Impact, - "expected TSDF ceiling hit to resolve as an impact plane" - ); - assert!( - plane.normal.y <= -0.85, - "expected mostly downward ceiling normal, got {:?}", - plane.normal - ); - assert!( - hit.collider.restitution >= 0.3, - "expected impact plane restitution, got {}", - hit.collider.restitution - ); - } - XrDepthMeshQueryResult::Miss { .. } => { - panic!("expected TSDF ceiling hit to produce an impact plane"); - } - } - } - - #[test] - fn geometry_query_from_depth_grid_stabilizes_bulged_support_plane() { - let mut volume = DepthMeshVolume::new(1, 0.1); - volume.generation = 23; - fill_volume_signed_distance_field( - &mut volume, - VoxelCoord::new(-5, -4, -5), - VoxelCoord::new(5, 4, 5), - |world| world.y - bulged_plane_height(world.x, world.z, 0.45, 0.02), - ); - - let result = evaluate_geometry_query( - &volume, - XrDepthMeshQuery { - key: 103, - center: vec3f(0.0, 0.08, 0.0), - predicted_center: vec3f(0.0, 0.06, 0.0), - velocity: vec3f(0.0, -0.2, 0.0), - radius: 0.05, - max_distance: 0.12, - include_planar_patches: false, - }, - 1, - ); - - match result { - XrDepthMeshQueryResult::Hit(hit) => { - let XrDepthMeshQueryColliderGeometry::HalfSpace(plane) = hit.collider.geometry; - let center_height = bulged_plane_height(0.0, 0.0, 0.45, 0.02); - assert!( - plane.normal.y >= 0.985, - "expected nearly horizontal TSDF support plane on shallow bulge, got {:?}", - plane.normal - ); - assert!( - plane.point.y >= center_height - 0.01 && plane.point.y <= center_height + 0.03, - "expected plane near bulged top envelope, got {:?} expected around {}", - plane.point, - center_height - ); - } - XrDepthMeshQueryResult::Miss { .. } => { - panic!("expected TSDF support hit on bulged surface"); - } - } - } - - #[test] - fn geometry_query_from_depth_grid_clears_near_table_overhang() { - let mut volume = DepthMeshVolume::new(1, 0.1); - volume.generation = 24; - let table_center = vec3f(0.0, -0.05, 0.0); - let table_half_extents = vec3f(0.20, 0.05, 0.20); - fill_volume_signed_distance_field( - &mut volume, - VoxelCoord::new(-5, -4, -5), - VoxelCoord::new(5, 4, 5), - |world| box_signed_distance(world, table_center, table_half_extents), - ); - - let inside_result = evaluate_geometry_query( - &volume, - XrDepthMeshQuery { - key: 104, - center: vec3f(0.17, 0.08, 0.0), - predicted_center: vec3f(0.17, 0.06, 0.0), - velocity: vec3f(0.0, -0.2, 0.0), - radius: 0.05, - max_distance: 0.12, - include_planar_patches: false, - }, - 1, - ); - assert!( - matches!(inside_result, XrDepthMeshQueryResult::Hit(_)), - "expected support while still inside the tabletop footprint" - ); - - let outside_result = evaluate_geometry_query( - &volume, - XrDepthMeshQuery { - key: 105, - center: vec3f(0.24, 0.08, 0.0), - predicted_center: vec3f(0.24, 0.06, 0.0), - velocity: vec3f(0.0, -0.2, 0.0), - radius: 0.05, - max_distance: 0.12, - include_planar_patches: false, - }, - 1, - ); - assert!( - matches!(outside_result, XrDepthMeshQueryResult::Miss { .. }), - "expected support to clear once the center leaves the tabletop footprint" - ); - } - - #[test] - fn geometry_query_from_depth_grid_tracks_grazing_ballistic_support_into_table() { - let mut volume = DepthMeshVolume::new(1, 0.05); - volume.generation = 26; - let table_center = vec3f(0.10, -0.05, 0.0); - let table_half_extents = vec3f(0.10, 0.05, 0.20); - fill_volume_signed_distance_field( - &mut volume, - VoxelCoord::new(-6, -5, -6), - VoxelCoord::new(6, 5, 6), - |world| box_signed_distance(world, table_center, table_half_extents), - ); - - let result = evaluate_geometry_query( - &volume, - XrDepthMeshQuery { - key: 106, - center: vec3f(-0.07, 0.08, 0.0), - predicted_center: vec3f(0.10, 0.02, 0.0), - velocity: vec3f(1.20, 0.0, 0.0), - radius: 0.05, - max_distance: 0.12, - include_planar_patches: false, - }, - 1, - ); - - match result { - XrDepthMeshQueryResult::Hit(hit) => { - let XrDepthMeshQueryColliderGeometry::HalfSpace(plane) = hit.collider.geometry; - assert!( - plane.normal.y >= 0.90, - "expected grazing ballistic support to resolve to an upright tabletop plane, got {:?}", - plane.normal - ); - assert!( - plane.point.x >= -0.02 && plane.point.x <= 0.22, - "expected support point to land on the future tabletop footprint, got {:?}", - plane.point - ); - } - XrDepthMeshQueryResult::Miss { .. } => { - panic!("expected grazing ballistic path to resolve future tabletop support"); - } - } - } - - #[test] - fn geometry_query_from_depth_grid_handles_small_radius_without_panicking() { - let mut volume = DepthMeshVolume::new(1, 0.1); - volume.generation = 25; - fill_volume_signed_distance_field( - &mut volume, - VoxelCoord::new(-4, -4, -4), - VoxelCoord::new(4, 4, 4), - |world| world.y, - ); - - let result = evaluate_geometry_query( - &volume, - XrDepthMeshQuery { - key: 106, - center: vec3f(0.0, 0.03, 0.0), - predicted_center: vec3f(0.0, 0.02, 0.0), - velocity: vec3f(0.0, -0.1, 0.0), - radius: 0.01, - max_distance: 0.12, - include_planar_patches: false, - }, - 1, - ); - - assert!( - matches!( - result, - XrDepthMeshQueryResult::Hit(_) | XrDepthMeshQueryResult::Miss { .. } - ), - "small-radius TSDF query should complete without panicking" - ); - } - - #[test] - fn plane_patch_corner_winding_matches_patch_normal() { - let patch = XrDepthPlanePatch { - generation: 1, - kind: XrDepthPlaneKind::Table, - center: vec3f(0.0, 0.75, 0.0), - normal: vec3f(0.0, 1.0, 0.0), - tangent: vec3f(1.0, 0.0, 0.0), - bitangent: vec3f(0.0, 0.0, 1.0), - half_extent_tangent: 0.3, - half_extent_bitangent: 0.2, - area: 0.24, - support_triangles: 2, - }; - let corners = plane_patch_corners(&patch); - let tri_normal = Vec3f::cross(corners[1] - corners[0], corners[2] - corners[0]).normalize(); - assert!( - tri_normal.dot(patch.normal) > 0.99, - "patch quad winding should align with patch normal: tri_normal={tri_normal:?} patch_normal={:?}", - patch.normal - ); + base.max(Duration::from_micros(cycle_micros)) } } diff --git a/platform/src/os/linux/openxr_input.rs b/platform/src/os/linux/openxr_input.rs index c6e916628b..61b0055a14 100644 --- a/platform/src/os/linux/openxr_input.rs +++ b/platform/src/os/linux/openxr_input.rs @@ -48,14 +48,20 @@ impl CxOpenXrSession { &self.inputs.actions, ); - let left_hand = - self.inputs - .left_hand - .poll(xr, self.handle, local_space, predicted_display_time); - let right_hand = - self.inputs - .right_hand - .poll(xr, self.handle, local_space, predicted_display_time); + let left_hand = self.inputs.left_hand.poll( + xr, + self.handle, + local_space, + predicted_display_time, + &self.inputs.actions, + ); + let right_hand = self.inputs.right_hand.poll( + xr, + self.handle, + local_space, + predicted_display_time, + &self.inputs.actions, + ); let anchor = self .anchor @@ -67,6 +73,7 @@ impl CxOpenXrSession { time: (frame.frame_state.predicted_display_time.as_nanos() as f64) / 1e9f64, head_pose: frame.local_from_head.pose, anchor, + sync_anchor: None, left_controller, right_controller, left_hand, @@ -85,6 +92,8 @@ impl CxOpenXrSession { struct CxOpenXrInputActions { trigger_action: XrAction, grip_action: XrAction, + hand_grab_action: XrAction, + hand_grab_ready_action: XrAction, thumbstick_action: XrAction, click_a_action: XrAction, click_b_action: XrAction, @@ -114,8 +123,11 @@ pub struct CxOpenXrInputs { } pub struct CxOpenXrHand { + path: XrPath, tracker: XrHandTrackerEXT, joint_locations: [XrHandJointLocationEXT; HAND_JOINT_COUNT_EXT], + grab_active: bool, + last_hand: XrHand, } impl CxOpenXrHand { @@ -126,9 +138,10 @@ impl CxOpenXrHand { fn poll( &mut self, xr: &LibOpenXr, - _session: XrSession, + session: XrSession, local_space: XrSpace, time: XrTime, + actions: &CxOpenXrInputActions, ) -> XrHand { let mut scale = XrHandTrackingScaleFB { sensor_output: 1.0, @@ -155,8 +168,15 @@ impl CxOpenXrHand { unsafe { (xr.xrLocateHandJointsEXT)(self.tracker, &locate_info, &mut locations) } .log_error("xrLocateHandJointsEXT"); // alrighty lets convert the joints to our XrHand - let mut hand = XrHand::default(); - let mut hand_in_view = false; + let mut hand = self.last_hand.clone(); + hand.flags = 0; + hand.tips_active = 0; + hand.pinch = [0; 4]; + let grasp = XrActionStateFloat::get(xr, session, actions.hand_grab_action, self.path); + let grasp_ready = + XrActionStateBoolean::get(xr, session, actions.hand_grab_ready_action, self.path); + let mut center_tracked = false; + let mut wrist_tracked = false; let mut s = 0; for i in 0..self.joint_locations.len() { let tracked = self.joint_locations[i] @@ -165,6 +185,8 @@ impl CxOpenXrHand { && self.joint_locations[i] .location_flags .contains(XrSpaceLocationFlags::POSITION_TRACKED); + let pose = self.joint_locations[i].pose; + let pose_finite = xr_pose_is_finite(pose); // we're going to skip the tips and only store the distance if i == 5 || i == 10 || i == 15 || i == 20 || i == 25 { // indices @@ -198,22 +220,42 @@ impl CxOpenXrHand { pub const PINKY_TIP: usize = 25;*/ // only store the distance to the tip so we can fit the entire // tracked quest state in a UDP packet :) - let d = self.joint_locations[i].pose.position - - self.joint_locations[i - 1].pose.position; let slot = i / 5 - 1; - hand.tips[slot] = d.length(); - hand.tips_active |= if tracked { 1 << slot } else { 0 }; + let prev_pose = self.joint_locations[i - 1].pose; + let prev_tracked = self.joint_locations[i - 1] + .location_flags + .contains(XrSpaceLocationFlags::ORIENTATION_TRACKED) + && self.joint_locations[i - 1] + .location_flags + .contains(XrSpaceLocationFlags::POSITION_TRACKED) + && xr_pose_is_finite(prev_pose); + if tracked && pose_finite && prev_tracked { + let d = pose.position - prev_pose.position; + if d.x.is_finite() && d.y.is_finite() && d.z.is_finite() { + hand.tips[slot] = d.length(); + hand.tips_active |= 1 << slot; + } + } continue; } - hand.joints[s] = self.joint_locations[i].pose; - s += 1; - if tracked { - hand_in_view = true + if tracked && pose_finite { + hand.joints[s] = pose; + if s == XrHand::CENTER { + center_tracked = true; + } + if s == XrHand::WRIST { + wrist_tracked = true; + } } + s += 1; + } + let aim_pose_valid = xr_pose_is_finite(aim_state.aim_pose); + if aim_pose_valid { + hand.aim_pose = aim_state.aim_pose; } - hand.flags |= if hand_in_view{XrHand::IN_VIEW}else{0}| - if aim_state.status.contains(XrHandTrackingAimFlagsFB::VALID){XrHand::AIM_VALID}else{0}| + hand.flags |= if center_tracked && wrist_tracked{XrHand::IN_VIEW}else{0}| + if aim_pose_valid && aim_state.status.contains(XrHandTrackingAimFlagsFB::VALID){XrHand::AIM_VALID}else{0}| if aim_state.status.contains(XrHandTrackingAimFlagsFB::INDEX_PINCHING){XrHand::PINCH_INDEX}else{0}| if aim_state.status.contains(XrHandTrackingAimFlagsFB::MIDDLE_PINCHING){XrHand::PINCH_MIDDLE}else{0}| if aim_state.status.contains(XrHandTrackingAimFlagsFB::RING_PINCHING){XrHand::PINCH_RING}else{0}| @@ -222,7 +264,16 @@ impl CxOpenXrHand { if aim_state.status.contains(XrHandTrackingAimFlagsFB::MENU_PRESSED){XrHand::MENU_PRESSED}else{0}| //if aim_state.status.contains(XrHandTrackingAimFlagsFB::SYSTEM_GESTURE){XrHand::SYSTEM_GESTURE}else{0}| 0; - hand.aim_pose = aim_state.aim_pose; + let grab_ready = grasp_ready.is_active.as_bool() && grasp_ready.current_state.as_bool(); + let grab_strength = if grasp.is_active.as_bool() { + grasp.current_state + } else { + 0.0 + }; + self.grab_active = xr_hand_grab_active(self.grab_active, grab_ready, grab_strength); + if self.grab_active { + hand.tips_active |= XrHand::GRAB_ACTIVE; + } hand.pinch[XrHand::PINCH_STRENGTH_INDEX] = (aim_state.pinch_strength_index * u8::MAX as f32) as u8; hand.pinch[XrHand::PINCH_STRENGTH_MIDDLE] = @@ -231,10 +282,35 @@ impl CxOpenXrHand { (aim_state.pinch_strength_ring * u8::MAX as f32) as u8; hand.pinch[XrHand::PINCH_STRENGTH_LITTLE] = (aim_state.pinch_strength_little * u8::MAX as f32) as u8; + self.last_hand = hand.clone(); hand } } +fn xr_pose_is_finite(pose: XrPosef) -> bool { + pose.position.x.is_finite() + && pose.position.y.is_finite() + && pose.position.z.is_finite() + && pose.orientation.x.is_finite() + && pose.orientation.y.is_finite() + && pose.orientation.z.is_finite() + && pose.orientation.w.is_finite() +} + +const XR_HAND_GRAB_ACTIVATE_THRESHOLD: f32 = 0.72; +const XR_HAND_GRAB_RELEASE_THRESHOLD: f32 = 0.42; + +fn xr_hand_grab_active(previous: bool, grab_ready: bool, grab_strength: f32) -> bool { + if !grab_ready || !grab_strength.is_finite() { + return false; + } + if previous { + grab_strength >= XR_HAND_GRAB_RELEASE_THRESHOLD + } else { + grab_strength >= XR_HAND_GRAB_ACTIVATE_THRESHOLD + } +} + pub struct CxOpenXrController { path: XrPath, aim_space: XrSpace, @@ -447,6 +523,22 @@ impl CxOpenXrInputs { "", &hand_paths, )?; + let hand_grab_action = XrAction::new( + xr, + action_set, + XrActionType::FLOAT_INPUT, + "hand_grab", + "", + &hand_paths, + )?; + let hand_grab_ready_action = XrAction::new( + xr, + action_set, + XrActionType::BOOLEAN_INPUT, + "hand_grab_ready", + "", + &hand_paths, + )?; let grip_action = XrAction::new( xr, action_set, @@ -607,6 +699,11 @@ impl CxOpenXrInputs { instance, "/interaction_profiles/meta/touch_controller_plus", )?; + let hand_interaction_profile = XrPath::new( + xr, + instance, + "/interaction_profiles/ext/hand_interaction_ext", + )?; let bindings = [ XrActionSuggestedBinding::new( @@ -797,6 +894,33 @@ impl CxOpenXrInputs { )?, ]; + let hand_bindings = [ + XrActionSuggestedBinding::new( + xr, + instance, + hand_grab_action, + "/user/hand/left/input/grasp_ext/value", + )?, + XrActionSuggestedBinding::new( + xr, + instance, + hand_grab_action, + "/user/hand/right/input/grasp_ext/value", + )?, + XrActionSuggestedBinding::new( + xr, + instance, + hand_grab_ready_action, + "/user/hand/left/input/grasp_ext/ready_ext", + )?, + XrActionSuggestedBinding::new( + xr, + instance, + hand_grab_ready_action, + "/user/hand/right/input/grasp_ext/ready_ext", + )?, + ]; + let suggested_bindings = XrInteractionProfileSuggestedBinding { interaction_profile, count_suggested_bindings: bindings.len() as _, @@ -807,6 +931,16 @@ impl CxOpenXrInputs { unsafe { (xr.xrSuggestInteractionProfileBindings)(instance, &suggested_bindings) } .to_result("xrSuggestInteractionProfileBindings")?; + let hand_suggested_bindings = XrInteractionProfileSuggestedBinding { + interaction_profile: hand_interaction_profile, + count_suggested_bindings: hand_bindings.len() as _, + suggested_bindings: hand_bindings.as_ptr(), + ..Default::default() + }; + + unsafe { (xr.xrSuggestInteractionProfileBindings)(instance, &hand_suggested_bindings) } + .to_result("xrSuggestInteractionProfileBindings hand_interaction_ext")?; + let attach_info = XrSessionActionSetsAttachInfo { count_action_sets: 1, action_sets: &action_set as *const _, @@ -825,6 +959,8 @@ impl CxOpenXrInputs { let actions = CxOpenXrInputActions { trigger_action, grip_action, + hand_grab_action, + hand_grab_ready_action, thumbstick_action, click_a_action, click_b_action, @@ -847,12 +983,18 @@ impl CxOpenXrInputs { action_set, actions, left_hand: CxOpenXrHand { + path: left_hand_path, tracker: left_hand_track, joint_locations: Default::default(), + grab_active: false, + last_hand: XrHand::default(), }, right_hand: CxOpenXrHand { + path: right_hand_path, tracker: right_hand_track, joint_locations: Default::default(), + grab_active: false, + last_hand: XrHand::default(), }, left_controller: CxOpenXrController { path: left_hand_path, @@ -928,3 +1070,22 @@ impl CxOpenXrInputs { unsafe { (xr.xrDestroyActionSet)(self.action_set) }.log_error("xrDestroyActionSet"); } } + +#[cfg(test)] +mod tests { + use super::*; + + #[test] + fn xr_hand_grab_hysteresis_requires_stronger_activation_than_release() { + assert!(!xr_hand_grab_active(false, true, 0.60)); + assert!(xr_hand_grab_active(false, true, 0.80)); + assert!(xr_hand_grab_active(true, true, 0.50)); + assert!(!xr_hand_grab_active(true, true, 0.30)); + } + + #[test] + fn xr_hand_grab_hysteresis_clears_when_grab_ready_drops() { + assert!(!xr_hand_grab_active(true, false, 1.0)); + assert!(!xr_hand_grab_active(true, true, f32::NAN)); + } +} diff --git a/platform/src/os/linux/openxr_vulkan.rs b/platform/src/os/linux/openxr_vulkan.rs index db74a75429..d29b172e73 100644 --- a/platform/src/os/linux/openxr_vulkan.rs +++ b/platform/src/os/linux/openxr_vulkan.rs @@ -2,7 +2,7 @@ use super::*; use crate::{ os::linux::openxr_depth::CxOpenXrDepthMeshPipeline, os::linux::vulkan::{CxVulkan, CxVulkanOpenXrFoveationImageInfo, CxVulkanOpenXrSessionData}, - xr_depth_mesh::xr_depth_mesh_store, + xr_tsdf::xr_tsdf_store, }; pub(super) struct CxOpenXrVulkanSession { _color_images: Vec, @@ -581,6 +581,6 @@ impl CxOpenXrSession { ); } } - xr_depth_mesh_store().clear(); + xr_tsdf_store().clear(); } } diff --git a/platform/src/os/linux/wayland/linux_wayland.rs b/platform/src/os/linux/wayland/linux_wayland.rs index ee5d8e34b5..8f4c17b0ea 100644 --- a/platform/src/os/linux/wayland/linux_wayland.rs +++ b/platform/src/os/linux/wayland/linux_wayland.rs @@ -9,7 +9,7 @@ use crate::cx_native::EventFlow; use crate::egl_sys::NativeDisplayType; use crate::gl_sys::TEXTURE0; use crate::makepad_live_id::*; -use crate::makepad_math::dvec2; +use crate::makepad_math::{dvec2, Rect, Vec2d}; use crate::opengl_cx::OpenglCx; use crate::os::linux::gstreamer_sys::LibGStreamer; use crate::os::linux::linux_video_playback::GStreamerVideoPlayer; @@ -162,7 +162,6 @@ impl WaylandCx { cx.repaint_pass(main_pass_id); } } - //paint_dirty = true; cx.call_event_handler(&Event::WindowGotFocus(window_id)); } XlibEvent::WindowLostFocus(window_id) => { @@ -172,6 +171,20 @@ impl WaylandCx { XlibEvent::WindowGeomChange(mut re) => { // do this here because mac let mut cx = self.cx.borrow_mut(); + + // When drawing our own window chrome (no server-side decorations), + // populate the chrome buttons bounding box: three buttons right-aligned + // at the top of the caption bar, matching the Makepad widget layout. + if matches!(cx.os_type(), OsType::LinuxWindow(LinuxWindowParams { custom_window_chrome: true, .. })) { + const BUTTONS_W: f64 = 46.0 * 3.0; + const BUTTONS_H: f64 = 29.0; + let w = re.new_geom.inner_size.x; + re.new_geom.window_chrome_buttons = Rect { + pos: Vec2d { x: w - BUTTONS_W, y: 0.0 }, + size: Vec2d { x: BUTTONS_W, y: BUTTONS_H }, + }; + } + if let Some(window) = state .windows .iter_mut() @@ -440,7 +453,17 @@ impl WaylandCx { return EventFlow::Wait; } } - return EventFlow::Poll; + let cx = self.cx.borrow(); + if cx.any_passes_dirty() + || cx.need_redrawing() + || cx.new_next_frames.len() != 0 + || cx.screenshot_requests.len() > 0 + || cx.demo_time_repaint + { + return EventFlow::Poll; + } else { + return EventFlow::Wait; + } } fn app_event_callback(&mut self, wayland_app: &mut WaylandApp, event: XlibEvent) -> EventFlow { diff --git a/platform/src/os/linux/wayland/opengl_wayland.rs b/platform/src/os/linux/wayland/opengl_wayland.rs index 235cc8ad2e..bc25aa593a 100644 --- a/platform/src/os/linux/wayland/opengl_wayland.rs +++ b/platform/src/os/linux/wayland/opengl_wayland.rs @@ -111,6 +111,7 @@ impl WaylandWindow { outer_size: inner_size, dpi_factor: 1.0, position: position, + ..Default::default() }; Self { base_surface, @@ -338,6 +339,7 @@ impl WaylandPopupWindow { outer_size: size, dpi_factor: 1.0, position, + ..Default::default() }; Self { diff --git a/platform/src/os/linux/wayland/wayland_state.rs b/platform/src/os/linux/wayland/wayland_state.rs index 73c1b229a2..7b538a4db8 100644 --- a/platform/src/os/linux/wayland/wayland_state.rs +++ b/platform/src/os/linux/wayland/wayland_state.rs @@ -418,6 +418,7 @@ impl Dispatch for WaylandState { position: dvec2(0., 0.), inner_size, outer_size: inner_size, + ..Default::default() }, })); } diff --git a/platform/src/os/linux/x11/linux_x11.rs b/platform/src/os/linux/x11/linux_x11.rs index 6016aeac22..d2d15280af 100644 --- a/platform/src/os/linux/x11/linux_x11.rs +++ b/platform/src/os/linux/x11/linux_x11.rs @@ -110,9 +110,6 @@ impl X11Cx { return EventFlow::Exit; } - //let mut paint_dirty = false; - - //self.process_desktop_pre_event(&mut event); match event { XlibEvent::WindowGotFocus(window_id) => { // repaint all window passes. Metal sometimes doesnt flip buffers when hidden/no focus @@ -122,7 +119,6 @@ impl X11Cx { cx.repaint_pass(main_pass_id); } } - //paint_dirty = true; cx.call_event_handler(&Event::WindowGotFocus(window_id)); } XlibEvent::WindowLostFocus(window_id) => { @@ -409,11 +405,17 @@ impl X11Cx { } } - //if self.any_passes_dirty() || self.need_redrawing() || paint_dirty { - EventFlow::Poll - //} else { - // EventFlow::Wait - // } + let cx = self.cx.borrow(); + if cx.any_passes_dirty() + || cx.need_redrawing() + || cx.new_next_frames.len() != 0 + || cx.screenshot_requests.len() > 0 + || cx.demo_time_repaint + { + EventFlow::Poll + } else { + EventFlow::Wait + } } pub(crate) fn handle_repaint(&mut self, opengl_windows: &mut Vec) { diff --git a/platform/src/os/linux/x11/xlib_window.rs b/platform/src/os/linux/x11/xlib_window.rs index 25cf15b89e..f2f9233e9a 100644 --- a/platform/src/os/linux/x11/xlib_window.rs +++ b/platform/src/os/linux/x11/xlib_window.rs @@ -472,6 +472,7 @@ impl XlibWindow { outer_size: self.get_outer_size(), dpi_factor: self.get_dpi_factor(), position: self.get_position(), + ..Default::default() } } diff --git a/platform/src/os/web/to_wasm.rs b/platform/src/os/web/to_wasm.rs index 8d8a2100c6..5d09e71cc3 100644 --- a/platform/src/os/web/to_wasm.rs +++ b/platform/src/os/web/to_wasm.rs @@ -75,6 +75,7 @@ impl Into for WWindowInfo { position: Vec2d { x: 0., y: 0. }, xr_is_presenting: self.xr_is_presenting, can_fullscreen: self.can_fullscreen, + ..Default::default() } } } diff --git a/platform/src/os/windows/win32_window.rs b/platform/src/os/windows/win32_window.rs index 83c969f373..7110991eda 100644 --- a/platform/src/os/windows/win32_window.rs +++ b/platform/src/os/windows/win32_window.rs @@ -841,19 +841,31 @@ impl Win32Window { } pub fn get_window_geom(&self) -> WindowGeom { + // Three caption buttons (minimize / maximize / close), each 46 × 29 logical px, + // right-aligned at the top of the caption bar. + const BUTTON_W: f64 = 46.0; + const BUTTON_H: f64 = 29.0; + const BUTTON_COUNT: f64 = 3.0; + const BUTTONS_W: f64 = BUTTON_W * BUTTON_COUNT; + let inner_size = if self.get_is_maximized() { + self.get_outer_size() + } else { + self.get_inner_size() + }; WindowGeom { xr_is_presenting: false, can_fullscreen: false, is_topmost: self.get_is_topmost(), is_fullscreen: self.get_is_maximized(), - inner_size: if self.get_is_maximized() { - self.get_outer_size() - } else { - self.get_inner_size() - }, + inner_size, outer_size: self.get_outer_size(), dpi_factor: self.get_dpi_factor(), position: self.get_position(), + window_chrome_buttons: Rect { + pos: Vec2d { x: inner_size.x - BUTTONS_W, y: 0.0 }, + size: Vec2d { x: BUTTONS_W, y: BUTTON_H }, + }, + ..Default::default() } } diff --git a/platform/src/window.rs b/platform/src/window.rs index 76179533d5..a5f4da8c02 100644 --- a/platform/src/window.rs +++ b/platform/src/window.rs @@ -393,6 +393,13 @@ pub struct ScriptWindowHandle { pub backdrop_intensity: f32, #[live(MacosWindowConfig::default())] pub macos: MacosWindowConfig, + /// Optionally override the caption bar height. + /// * If `None` (the default), the caption bar's height is based on a system-calculated height + /// derived from window chrome button geometry, which will make the window chrome buttons + /// nicely vertically centered within the caption bar on all platforms. + /// * If `Some(value)`, the caption bar height is overridden with the specified fixed value. + #[live] + pub caption_bar_height_override: Option, } impl std::ops::Deref for ScriptWindowHandle { @@ -863,6 +870,7 @@ mod tests { backdrop: WindowBackdrop::Blur, backdrop_intensity: 0.5, macos: MacosWindowConfig::floating_panel(), + caption_bar_height_override: None, }; script_window.on_after_apply(&mut vm, &Apply::New, &mut Scope::empty(), NIL); diff --git a/platform/src/xr_depth_mesh.rs b/platform/src/xr_depth_mesh.rs deleted file mode 100644 index 9eaa8e2b48..0000000000 --- a/platform/src/xr_depth_mesh.rs +++ /dev/null @@ -1,2336 +0,0 @@ -use crate::{ - makepad_math::{vec3, vec4f, Mat4f, Pose, Quat, Vec3f}, - makepad_micro_serde::*, -}; -use std::{ - collections::{HashMap, VecDeque}, - sync::{ - atomic::{AtomicBool, AtomicU32, AtomicU64, Ordering}, - Arc, Mutex, OnceLock, RwLock, - }, -}; - -const XR_DEPTH_QUERY_MAX_PENDING: usize = 256; -pub const XR_DEPTH_MESH_DEFAULT_VOXEL_SIZE_METERS: f32 = 0.05; -const XR_DEPTH_ALIGN_MIN_WALL_SAMPLES: usize = 4; -const XR_DEPTH_ALIGN_MIN_WALL_FEATURES: usize = 2; -const XR_DEPTH_ALIGN_ACCEPT_MIN_MATCHED_SAMPLES: usize = 6; -const XR_DEPTH_ALIGN_ACCEPT_MIN_CONFIDENCE: f32 = 0.12; -const XR_DEPTH_ALIGN_ACCEPT_MIN_MATCHED_WALL_FEATURES: usize = 3; -const XR_DEPTH_ALIGN_ACCEPT_MIN_WALL_FEATURE_CONFIDENCE: f32 = 0.18; -const XR_DEPTH_ALIGN_TRANSLATION_VOTE_STEP_METERS: f32 = 0.08; -const XR_DEPTH_ALIGN_WALL_FEATURE_NORMAL_DOT_MIN: f32 = 0.94; -const XR_DEPTH_ALIGN_WALL_FEATURE_PLANE_RESIDUAL_MAX_METERS: f32 = 0.18; -const XR_DEPTH_ALIGN_WALL_FEATURE_HEIGHT_OVERLAP_MIN: f32 = 0.30; -const XR_DEPTH_ALIGN_WALL_FEATURE_PAIR_ANGLE_MIN_RADIANS: f32 = 0.28; - -#[derive(Clone, Copy, Debug, Default, PartialEq, SerBin, DeBin)] -pub struct XrDepthAlignWallFeature { - pub center: Vec3f, - pub normal: Vec3f, - pub along_axis: Vec3f, - pub plane_distance: f32, - pub half_extent_along: f32, - pub min_y: f32, - pub max_y: f32, - pub area: f32, -} - -#[derive(Clone, Copy, Debug, Default, PartialEq, Eq, Hash)] -pub struct ChunkKey { - pub x: i32, - pub y: i32, - pub z: i32, -} - -impl ChunkKey { - pub const fn new(x: i32, y: i32, z: i32) -> Self { - Self { x, y, z } - } -} - -#[derive(Clone, Debug, Default)] -pub struct XrDepthMeshStats { - pub frames_seen: u64, - pub frames_meshed: u64, - pub frames_dropped: u64, -} - -#[derive(Clone, Debug, Default)] -pub struct XrDepthMeshChunk { - pub generation: u64, - pub chunk_key: ChunkKey, - pub fingerprint: u64, - pub bounds_min: Vec3f, - pub bounds_max: Vec3f, - pub vertices: Vec, - pub normals: Vec, - pub indices: Vec, - pub planar_patches: Vec, -} - -impl XrDepthMeshChunk { - pub fn triangle_count(&self) -> usize { - self.indices.len() / 3 - } -} - -#[derive(Clone, Debug, Default)] -pub struct XrDepthMesh { - pub generation: u64, - pub latest_topology_generation: u64, - pub update_sequence: u64, - pub eye_index: usize, - pub image_width: u32, - pub image_height: u32, - pub sample_step: u32, - pub voxel_size_meters: f32, - pub bounds_min: Vec3f, - pub bounds_max: Vec3f, - pub mesh_chunks: Vec, - pub plane_patches: Vec, - pub alignment_descriptor: Option, - pub alignment_debug: XrDepthAlignDebug, - pub alignment_preview: XrDepthAlignPreview, - pub dirty_chunk_keys: Vec, - pub removed_chunk_keys: Vec, - pub mesh_generation: u64, - pub mesh_vertex_count: usize, - pub mesh_triangle_count: usize, -} - -#[derive(Clone, Debug, Default)] -pub struct XrDepthMeshState { - pub latest_mesh: Option>, - pub stats: XrDepthMeshStats, - pub last_error: Option, -} - -#[derive(Clone, Debug, Default, PartialEq)] -pub struct XrDepthAlignState { - pub update_sequence: u64, - pub descriptor: Option, - pub debug: XrDepthAlignDebug, - pub preview: XrDepthAlignPreview, -} - -#[derive(Clone, Copy, Debug, Default, PartialEq, Eq, Hash, SerBin, DeBin)] -pub enum XrDepthAlignSampleKind { - Floor, - Wall, - #[default] - Unknown, -} - -#[derive(Clone, Copy, Debug, Default, PartialEq, SerBin, DeBin)] -pub struct XrDepthAlignSample { - pub kind: XrDepthAlignSampleKind, - pub point: Vec3f, - pub normal: Vec3f, - pub weight: f32, -} - -#[derive(Clone, Debug, Default, PartialEq, SerBin, DeBin)] -pub struct XrDepthAlignDescriptor { - pub voxel_size_meters: f32, - pub floor_y: f32, - pub wall_normal_histogram: Vec, - pub wall_features: Vec, - pub samples: Vec, -} - -#[derive(Clone, Copy, Debug, Default, PartialEq, Eq, SerBin, DeBin)] -pub struct XrDepthAlignDebug { - pub near_surface_voxel_count: u32, - pub floor_candidate_count: u32, - pub wall_candidate_count: u32, - pub floor_sample_count: u32, - pub wall_sample_count: u32, -} - -#[derive(Clone, Copy, Debug, Default, PartialEq)] -pub struct XrDepthAlignSolution { - pub yaw_radians: f32, - pub translation: Vec3f, - pub confidence: f32, - pub residual_meters: f32, - pub matched_samples: usize, -} - -impl XrDepthAlignSolution { - pub fn remote_to_local_transform(&self) -> Mat4f { - Pose::new( - Quat::from_axis_angle(vec3(0.0, 1.0, 0.0), self.yaw_radians), - self.translation, - ) - .to_mat4() - } - - pub fn map_point(&self, point: Vec3f) -> Vec3f { - self.remote_to_local_transform() - .transform_vec4(vec4f(point.x, point.y, point.z, 1.0)) - .to_vec3f() - } -} - -#[derive(Clone, Copy, Debug, Default, PartialEq, Eq)] -pub enum XrDepthAlignSolveOutcome { - #[default] - MissingSamples, - NoCandidate, - Rejected, - Accepted, -} - -#[derive(Clone, Copy, Debug, Default, PartialEq)] -pub struct XrDepthAlignSolveDiagnostic { - pub used_wall_features: bool, - pub local_wall_features: usize, - pub remote_wall_features: usize, - pub local_floor_samples: usize, - pub local_wall_samples: usize, - pub remote_floor_samples: usize, - pub remote_wall_samples: usize, - pub yaw_candidate_count: usize, - pub pose_candidate_count: usize, - pub best_solution: Option, -} - -impl XrDepthAlignSolveDiagnostic { - pub fn accepted_solution(&self) -> Option { - self.best_solution.filter(|solution| { - if self.used_wall_features { - solution.matched_samples >= XR_DEPTH_ALIGN_ACCEPT_MIN_MATCHED_WALL_FEATURES - && solution.confidence > XR_DEPTH_ALIGN_ACCEPT_MIN_WALL_FEATURE_CONFIDENCE - } else { - solution.matched_samples >= XR_DEPTH_ALIGN_ACCEPT_MIN_MATCHED_SAMPLES - && solution.confidence > XR_DEPTH_ALIGN_ACCEPT_MIN_CONFIDENCE - } - }) - } - - pub fn outcome(&self) -> XrDepthAlignSolveOutcome { - if self.used_wall_features { - if self.local_wall_features < XR_DEPTH_ALIGN_MIN_WALL_FEATURES - || self.remote_wall_features < XR_DEPTH_ALIGN_MIN_WALL_FEATURES - { - return XrDepthAlignSolveOutcome::MissingSamples; - } - } else if self.local_wall_samples < XR_DEPTH_ALIGN_MIN_WALL_SAMPLES - || self.remote_wall_samples < XR_DEPTH_ALIGN_MIN_WALL_SAMPLES - { - return XrDepthAlignSolveOutcome::MissingSamples; - } - if self.best_solution.is_none() { - XrDepthAlignSolveOutcome::NoCandidate - } else if self.accepted_solution().is_some() { - XrDepthAlignSolveOutcome::Accepted - } else { - XrDepthAlignSolveOutcome::Rejected - } - } -} - -pub fn xr_depth_align_loopback_preview_solution() -> XrDepthAlignSolution { - XrDepthAlignSolution { - yaw_radians: 0.58, - translation: vec3(-0.82, 0.0, 0.67), - confidence: 1.0, - residual_meters: 0.0, - matched_samples: 0, - } -} - -#[derive(Clone, Debug, Default, PartialEq)] -pub struct XrDepthAlignPreview { - pub local_markers: Option<[Vec3f; 2]>, - pub remote_markers_local: Option<[Vec3f; 2]>, - pub solution: Option, - pub local_sample_count: usize, - pub local_floor_sample_count: usize, - pub local_wall_sample_count: usize, - pub remote_sample_count: usize, - pub remote_floor_sample_count: usize, - pub remote_wall_sample_count: usize, -} - -pub fn xr_depth_align_transform_descriptor( - descriptor: &XrDepthAlignDescriptor, - transform: &Mat4f, -) -> XrDepthAlignDescriptor { - let transform_dir = |dir: Vec3f| { - transform - .transform_vec4(vec4f(dir.x, dir.y, dir.z, 0.0)) - .to_vec3f() - }; - let mut descriptor = descriptor.clone(); - for sample in &mut descriptor.samples { - sample.point = transform - .transform_vec4(vec4f(sample.point.x, sample.point.y, sample.point.z, 1.0)) - .to_vec3f(); - sample.normal = align_safe_normalize(transform_dir(sample.normal)).unwrap_or(sample.normal); - } - for wall in &mut descriptor.wall_features { - let center = transform - .transform_vec4(vec4f(wall.center.x, wall.center.y, wall.center.z, 1.0)) - .to_vec3f(); - let along_start = transform - .transform_vec4(vec4f( - wall.center.x - wall.along_axis.x * wall.half_extent_along, - wall.center.y - 0.0, - wall.center.z - wall.along_axis.z * wall.half_extent_along, - 1.0, - )) - .to_vec3f(); - let along_end = transform - .transform_vec4(vec4f( - wall.center.x + wall.along_axis.x * wall.half_extent_along, - wall.center.y + 0.0, - wall.center.z + wall.along_axis.z * wall.half_extent_along, - 1.0, - )) - .to_vec3f(); - let bottom = transform - .transform_vec4(vec4f(wall.center.x, wall.min_y, wall.center.z, 1.0)) - .to_vec3f(); - let top = transform - .transform_vec4(vec4f(wall.center.x, wall.max_y, wall.center.z, 1.0)) - .to_vec3f(); - wall.center = center; - wall.normal = align_safe_normalize(transform_dir(wall.normal)).unwrap_or(wall.normal); - wall.along_axis = align_safe_normalize(along_end - along_start).unwrap_or_else(|| { - align_safe_normalize(transform_dir(wall.along_axis)).unwrap_or(wall.along_axis) - }); - wall.half_extent_along = 0.5 * (along_end - along_start).length(); - wall.min_y = bottom.y.min(top.y); - wall.max_y = bottom.y.max(top.y); - wall.plane_distance = wall.center.dot(wall.normal); - } - descriptor.floor_y = transform - .transform_vec4(vec4f(0.0, descriptor.floor_y, 0.0, 1.0)) - .to_vec3f() - .y; - descriptor.wall_normal_histogram = if descriptor.wall_features.is_empty() { - xr_depth_align_build_wall_normal_histogram( - &descriptor.samples, - descriptor.wall_normal_histogram.len(), - ) - } else { - xr_depth_align_build_wall_feature_normal_histogram( - &descriptor.wall_features, - descriptor.wall_normal_histogram.len(), - ) - }; - descriptor -} - -pub fn xr_depth_align_test_markers(descriptor: &XrDepthAlignDescriptor) -> Option<[Vec3f; 2]> { - let wall_samples = descriptor_samples_of_kind(descriptor, XrDepthAlignSampleKind::Wall); - let samples = if wall_samples.len() >= 2 { - wall_samples - } else { - descriptor - .samples - .iter() - .filter(|sample| sample.kind != XrDepthAlignSampleKind::Unknown) - .collect::>() - }; - let mut best = None::<(f32, f32, Vec3f, Vec3f)>; - for (index, first) in samples.iter().enumerate() { - for second in samples.iter().skip(index + 1) { - let distance = (second.point - first.point).length(); - if distance < 0.18 { - continue; - } - let weight = first.weight + second.weight; - if best - .as_ref() - .is_none_or(|(best_distance, best_weight, _, _)| { - distance > *best_distance + 1.0e-4 - || ((distance - *best_distance).abs() <= 1.0e-4 && weight > *best_weight) - }) - { - best = Some((distance, weight, first.point, second.point)); - } - } - } - best.map(|(_, _, first, second)| [first, second]) -} - -pub fn xr_depth_align_solve_remote_to_local( - local: &XrDepthAlignDescriptor, - remote: &XrDepthAlignDescriptor, -) -> Option { - xr_depth_align_analyze_remote_to_local(local, remote).accepted_solution() -} - -pub fn xr_depth_align_analyze_remote_to_local( - local: &XrDepthAlignDescriptor, - remote: &XrDepthAlignDescriptor, -) -> XrDepthAlignSolveDiagnostic { - let local_wall_features = descriptor_wall_features(local); - let remote_wall_features = descriptor_wall_features(remote); - let local_floor_samples = descriptor_samples_of_kind(local, XrDepthAlignSampleKind::Floor); - let local_wall_samples = descriptor_samples_of_kind(local, XrDepthAlignSampleKind::Wall); - let remote_floor_samples = descriptor_samples_of_kind(remote, XrDepthAlignSampleKind::Floor); - let remote_wall_samples = descriptor_samples_of_kind(remote, XrDepthAlignSampleKind::Wall); - let mut diagnostic = XrDepthAlignSolveDiagnostic { - local_wall_features: local_wall_features.len(), - remote_wall_features: remote_wall_features.len(), - local_floor_samples: local_floor_samples.len(), - local_wall_samples: local_wall_samples.len(), - remote_floor_samples: remote_floor_samples.len(), - remote_wall_samples: remote_wall_samples.len(), - ..XrDepthAlignSolveDiagnostic::default() - }; - if local_wall_features.len() >= XR_DEPTH_ALIGN_MIN_WALL_FEATURES - && remote_wall_features.len() >= XR_DEPTH_ALIGN_MIN_WALL_FEATURES - { - diagnostic.used_wall_features = true; - let mut best = None::; - let floor_y = local.floor_y - remote.floor_y; - let yaw_candidates = candidate_wall_feature_yaws( - &local.wall_normal_histogram, - &remote.wall_normal_histogram, - &local_wall_features, - &remote_wall_features, - ); - diagnostic.yaw_candidate_count = yaw_candidates.len(); - for yaw in yaw_candidates { - let translations = candidate_wall_feature_translations( - &local_wall_features, - &remote_wall_features, - floor_y, - yaw, - ); - diagnostic.pose_candidate_count += translations.len(); - for translation in translations { - let (refined_yaw, refined_translation) = refine_wall_feature_alignment( - &local_wall_features, - &remote_wall_features, - yaw, - translation, - ); - let candidate = score_wall_feature_alignment( - &local_wall_features, - &remote_wall_features, - &local_wall_samples, - &remote_wall_samples, - refined_yaw, - refined_translation, - ); - if best - .as_ref() - .is_none_or(|current| alignment_solution_better(&candidate, current)) - { - best = Some(candidate); - } - } - } - diagnostic.best_solution = best; - return diagnostic; - } - - if local_wall_samples.len() < XR_DEPTH_ALIGN_MIN_WALL_SAMPLES - || remote_wall_samples.len() < XR_DEPTH_ALIGN_MIN_WALL_SAMPLES - { - return diagnostic; - } - - let floor_y = local.floor_y - remote.floor_y; - let mut best = None::; - let yaw_candidates = candidate_yaws( - &local.wall_normal_histogram, - &remote.wall_normal_histogram, - &local_wall_samples, - &remote_wall_samples, - ); - diagnostic.yaw_candidate_count = yaw_candidates.len(); - for yaw in yaw_candidates { - let translations = - candidate_translations(&local_wall_samples, &remote_wall_samples, floor_y, yaw); - diagnostic.pose_candidate_count += translations.len(); - for translation in translations { - let (refined_yaw, refined_translation) = refine_alignment( - &local_wall_samples, - &remote_wall_samples, - floor_y, - yaw, - translation, - ); - let candidate = score_alignment_solution( - &local_wall_samples, - &remote_wall_samples, - refined_yaw, - refined_translation, - ); - if best - .as_ref() - .is_none_or(|current| alignment_solution_better(&candidate, current)) - { - best = Some(candidate); - } - } - } - diagnostic.best_solution = best; - diagnostic -} - -pub fn xr_depth_align_build_wall_normal_histogram( - samples: &[XrDepthAlignSample], - bin_count: usize, -) -> Vec { - let bin_count = bin_count.max(1); - let mut histogram = vec![0.0; bin_count]; - for sample in samples { - if sample.kind != XrDepthAlignSampleKind::Wall { - continue; - } - let Some(axis) = xz_axis(sample.normal) else { - continue; - }; - let angle = axis.x.atan2(-axis.z); - let normalized = (angle + std::f32::consts::PI) / std::f32::consts::TAU; - let bin = (normalized * bin_count as f32).floor() as isize; - histogram[bin.rem_euclid(bin_count as isize) as usize] += sample.weight.max(0.01); - } - let total = histogram.iter().copied().sum::(); - if total > 0.0 { - for value in &mut histogram { - *value = (*value / total * 100.0).round() / 100.0; - } - } - histogram -} - -pub fn xr_depth_align_build_wall_feature_normal_histogram( - features: &[XrDepthAlignWallFeature], - bin_count: usize, -) -> Vec { - let bin_count = bin_count.max(1); - let mut histogram = vec![0.0; bin_count]; - for feature in features { - let Some(axis) = xz_axis(feature.normal) else { - continue; - }; - let angle = axis.x.atan2(-axis.z); - let normalized = (angle + std::f32::consts::PI) / std::f32::consts::TAU; - let bin = (normalized * bin_count as f32).floor() as isize; - histogram[bin.rem_euclid(bin_count as isize) as usize] += wall_feature_weight(feature); - } - let total = histogram.iter().copied().sum::(); - if total > 0.0 { - for value in &mut histogram { - *value = (*value / total * 100.0).round() / 100.0; - } - } - histogram -} - -fn align_safe_normalize(v: Vec3f) -> Option { - let len = v.length(); - (len > 0.0001).then_some(v * (1.0 / len)) -} - -fn yaw_rotation(yaw: f32) -> Quat { - Quat::from_axis_angle(vec3(0.0, 1.0, 0.0), yaw) -} - -fn rotate_y(yaw: f32, vector: Vec3f) -> Vec3f { - yaw_rotation(yaw).rotate_vec3(&vector) -} - -fn xz_axis(vector: Vec3f) -> Option { - align_safe_normalize(vec3(vector.x, 0.0, vector.z)) -} - -fn signed_xz_angle(from: Vec3f, to: Vec3f) -> f32 { - let cross = from.z * to.x - from.x * to.z; - let dot = from.x * to.x + from.z * to.z; - cross.atan2(dot) -} - -fn wrap_angle(mut angle: f32) -> f32 { - while angle <= -std::f32::consts::PI { - angle += std::f32::consts::TAU; - } - while angle > std::f32::consts::PI { - angle -= std::f32::consts::TAU; - } - angle -} - -fn descriptor_wall_features<'a>( - descriptor: &'a XrDepthAlignDescriptor, -) -> Vec<&'a XrDepthAlignWallFeature> { - let mut walls = descriptor.wall_features.iter().collect::>(); - walls.sort_by(|a, b| b.area.total_cmp(&a.area)); - walls -} - -fn wall_feature_height(feature: &XrDepthAlignWallFeature) -> f32 { - (feature.max_y - feature.min_y).max(0.0) -} - -fn wall_feature_width(feature: &XrDepthAlignWallFeature) -> f32 { - (feature.half_extent_along * 2.0).max(0.0) -} - -fn wall_feature_weight(feature: &XrDepthAlignWallFeature) -> f32 { - feature.area.max(0.05) -} - -fn interval_overlap_ratio(first_min: f32, first_max: f32, second_min: f32, second_max: f32) -> f32 { - let overlap = (first_max.min(second_max) - first_min.max(second_min)).max(0.0); - let union = (first_max.max(second_max) - first_min.min(second_min)).max(1.0e-3); - (overlap / union).clamp(0.0, 1.0) -} - -fn wall_feature_interval_on_axis( - center: Vec3f, - along_axis: Vec3f, - half_extent_along: f32, - axis: Vec3f, -) -> (f32, f32) { - let left = (center - along_axis.scale(half_extent_along)).dot(axis); - let right = (center + along_axis.scale(half_extent_along)).dot(axis); - if left <= right { - (left, right) - } else { - (right, left) - } -} - -#[derive(Clone, Copy)] -struct WallFeaturePairCandidate<'a> { - local_index: usize, - remote_index: usize, - local: &'a XrDepthAlignWallFeature, - remote: &'a XrDepthAlignWallFeature, - score_hint: f32, -} - -#[derive(Clone, Copy)] -struct WallFeatureMatch<'a> { - local_index: usize, - remote_index: usize, - local: &'a XrDepthAlignWallFeature, - remote: &'a XrDepthAlignWallFeature, - transformed_remote_center: Vec3f, - rotated_remote_normal: Vec3f, - rotated_remote_axis: Vec3f, - transformed_remote_plane_distance: f32, - plane_residual: f32, - alignment: f32, - height_overlap: f32, - horizontal_overlap: f32, - width_ratio: f32, - score: f32, -} - -fn candidate_wall_feature_yaws( - local_histogram: &[f32], - remote_histogram: &[f32], - local_walls: &[&XrDepthAlignWallFeature], - remote_walls: &[&XrDepthAlignWallFeature], -) -> Vec { - let mut candidates = vec![0.0, std::f32::consts::PI]; - if local_histogram.len() == remote_histogram.len() && !local_histogram.is_empty() { - let bins = local_histogram.len(); - let mut shifts = Vec::<(f32, usize)>::new(); - for shift in 0..bins { - let score = (0..bins) - .map(|index| { - local_histogram[index] * remote_histogram[(index + bins - shift) % bins] - }) - .sum::(); - shifts.push((score, shift)); - } - shifts.sort_by(|a, b| b.0.total_cmp(&a.0)); - for (_, shift) in shifts.into_iter().take(6) { - let angle = wrap_angle(shift as f32 * std::f32::consts::TAU / bins as f32); - candidates.push(angle); - candidates.push(wrap_angle(angle + std::f32::consts::PI)); - } - } - - for local_wall in local_walls.iter().take(12) { - let Some(local_axis) = xz_axis(local_wall.normal) else { - continue; - }; - for remote_wall in remote_walls.iter().take(12) { - let Some(remote_axis) = xz_axis(remote_wall.normal) else { - continue; - }; - let angle = wrap_angle(signed_xz_angle(remote_axis, local_axis)); - candidates.push(angle); - candidates.push(wrap_angle(angle + std::f32::consts::PI)); - } - } - dedupe_angles(candidates, 0.05) -} - -fn collect_wall_feature_pair_candidates<'a>( - local_walls: &[&'a XrDepthAlignWallFeature], - remote_walls: &[&'a XrDepthAlignWallFeature], - yaw: f32, -) -> Vec> { - let mut candidates = Vec::new(); - for (local_index, local) in local_walls.iter().enumerate() { - for (remote_index, remote) in remote_walls.iter().enumerate() { - let rotated_remote_normal = - align_safe_normalize(rotate_y(yaw, remote.normal)).unwrap_or(remote.normal); - let alignment = local.normal.dot(rotated_remote_normal); - if alignment < XR_DEPTH_ALIGN_WALL_FEATURE_NORMAL_DOT_MIN { - continue; - } - let height_overlap = - interval_overlap_ratio(local.min_y, local.max_y, remote.min_y, remote.max_y); - if height_overlap < XR_DEPTH_ALIGN_WALL_FEATURE_HEIGHT_OVERLAP_MIN { - continue; - } - let height_ratio = (wall_feature_height(local).min(wall_feature_height(remote)) - / wall_feature_height(local) - .max(wall_feature_height(remote)) - .max(0.05)) - .clamp(0.0, 1.0); - let width_ratio = (wall_feature_width(local).min(wall_feature_width(remote)) - / wall_feature_width(local) - .max(wall_feature_width(remote)) - .max(0.05)) - .clamp(0.0, 1.0); - let score_hint = (wall_feature_weight(local) * wall_feature_weight(remote)).sqrt() - * alignment.powf(2.0) - * (0.35 + 0.65 * height_overlap) - * (0.40 + 0.60 * height_ratio) - * (0.30 + 0.70 * width_ratio); - candidates.push(WallFeaturePairCandidate { - local_index, - remote_index, - local, - remote, - score_hint, - }); - } - } - candidates.sort_by(|a, b| b.score_hint.total_cmp(&a.score_hint)); - candidates -} - -fn solve_translation_from_wall_constraints<'a, I>(constraints: I, floor_y: f32) -> Option -where - I: IntoIterator, -{ - let mut a00 = 0.0; - let mut a01 = 0.0; - let mut a11 = 0.0; - let mut b0 = 0.0; - let mut b1 = 0.0; - for (normal, rhs, weight) in constraints { - let Some(axis) = xz_axis(normal) else { - continue; - }; - let weight = weight.max(0.001); - a00 += weight * axis.x * axis.x; - a01 += weight * axis.x * axis.z; - a11 += weight * axis.z * axis.z; - b0 += weight * axis.x * rhs; - b1 += weight * axis.z * rhs; - } - let det = a00 * a11 - a01 * a01; - if det.abs() <= 1.0e-4 { - return None; - } - let inv_det = det.recip(); - let tx = (b0 * a11 - b1 * a01) * inv_det; - let tz = (a00 * b1 - a01 * b0) * inv_det; - Some(vec3(tx, floor_y, tz)) -} - -fn candidate_wall_feature_translations( - local_walls: &[&XrDepthAlignWallFeature], - remote_walls: &[&XrDepthAlignWallFeature], - floor_y: f32, - yaw: f32, -) -> Vec { - let pair_candidates = collect_wall_feature_pair_candidates(local_walls, remote_walls, yaw); - if pair_candidates.is_empty() { - return Vec::new(); - } - - let mut translations = Vec::::new(); - let max_candidates = pair_candidates.len().min(18); - for first_index in 0..max_candidates { - let first = pair_candidates[first_index]; - for second in pair_candidates - .iter() - .take(max_candidates) - .skip(first_index + 1) - { - if first.local_index == second.local_index || first.remote_index == second.remote_index - { - continue; - } - let angle_cos = first.local.normal.dot(second.local.normal).abs(); - if angle_cos > XR_DEPTH_ALIGN_WALL_FEATURE_PAIR_ANGLE_MIN_RADIANS.cos() { - continue; - } - let Some(translation) = solve_translation_from_wall_constraints( - [ - ( - first.local.normal, - first.local.plane_distance - first.remote.plane_distance, - first.score_hint, - ), - ( - second.local.normal, - second.local.plane_distance - second.remote.plane_distance, - second.score_hint, - ), - ], - floor_y, - ) else { - continue; - }; - translations.push(translation); - } - } - - if translations.is_empty() { - let lsq = solve_translation_from_wall_constraints( - pair_candidates - .iter() - .take(max_candidates) - .map(|candidate| { - ( - candidate.local.normal, - candidate.local.plane_distance - candidate.remote.plane_distance, - candidate.score_hint, - ) - }), - floor_y, - ); - if let Some(translation) = lsq { - translations.push(translation); - } - } - - dedupe_translations(translations, 0.05) -} - -fn collect_unique_wall_feature_matches<'a>( - local_walls: &[&'a XrDepthAlignWallFeature], - remote_walls: &[&'a XrDepthAlignWallFeature], - yaw: f32, - translation: Vec3f, -) -> Vec> { - let mut candidates = Vec::>::new(); - for (remote_index, remote) in remote_walls.iter().enumerate() { - let transformed_remote_center = rotate_y(yaw, remote.center) + translation; - let rotated_remote_normal = - align_safe_normalize(rotate_y(yaw, remote.normal)).unwrap_or(remote.normal); - let rotated_remote_axis = - align_safe_normalize(rotate_y(yaw, remote.along_axis)).unwrap_or(remote.along_axis); - let transformed_remote_plane_distance = - rotated_remote_normal.dot(transformed_remote_center); - let transformed_remote_min_y = remote.min_y + translation.y; - let transformed_remote_max_y = remote.max_y + translation.y; - for (local_index, local) in local_walls.iter().enumerate() { - let alignment = local.normal.dot(rotated_remote_normal); - if alignment < XR_DEPTH_ALIGN_WALL_FEATURE_NORMAL_DOT_MIN { - continue; - } - let plane_residual = - (local.normal.dot(transformed_remote_center) - local.plane_distance).abs(); - if plane_residual > XR_DEPTH_ALIGN_WALL_FEATURE_PLANE_RESIDUAL_MAX_METERS { - continue; - } - let height_overlap = interval_overlap_ratio( - local.min_y, - local.max_y, - transformed_remote_min_y, - transformed_remote_max_y, - ); - if height_overlap < XR_DEPTH_ALIGN_WALL_FEATURE_HEIGHT_OVERLAP_MIN { - continue; - } - let (local_min_u, local_max_u) = wall_feature_interval_on_axis( - local.center, - local.along_axis, - local.half_extent_along, - local.along_axis, - ); - let (remote_min_u, remote_max_u) = wall_feature_interval_on_axis( - transformed_remote_center, - rotated_remote_axis, - remote.half_extent_along, - local.along_axis, - ); - let horizontal_overlap = - interval_overlap_ratio(local_min_u, local_max_u, remote_min_u, remote_max_u); - let width_ratio = (wall_feature_width(local).min(wall_feature_width(remote)) - / wall_feature_width(local) - .max(wall_feature_width(remote)) - .max(0.05)) - .clamp(0.0, 1.0); - let score = (wall_feature_weight(local) * wall_feature_weight(remote)).sqrt() - * alignment.powf(2.0) - * (-plane_residual / 0.08).exp() - * (0.35 + 0.65 * height_overlap) - * (0.55 + 0.45 * horizontal_overlap) - * (0.35 + 0.65 * width_ratio); - candidates.push(WallFeatureMatch { - local_index, - remote_index, - local, - remote, - transformed_remote_center, - rotated_remote_normal, - rotated_remote_axis, - transformed_remote_plane_distance, - plane_residual, - alignment, - height_overlap, - horizontal_overlap, - width_ratio, - score, - }); - } - } - - candidates.sort_by(|a, b| { - b.score - .total_cmp(&a.score) - .then_with(|| a.plane_residual.total_cmp(&b.plane_residual)) - }); - - let mut used_local = vec![false; local_walls.len()]; - let mut used_remote = vec![false; remote_walls.len()]; - let mut matches = Vec::>::new(); - for candidate in candidates { - if used_local[candidate.local_index] || used_remote[candidate.remote_index] { - continue; - } - used_local[candidate.local_index] = true; - used_remote[candidate.remote_index] = true; - matches.push(candidate); - } - matches -} - -fn refine_wall_feature_alignment( - local_walls: &[&XrDepthAlignWallFeature], - remote_walls: &[&XrDepthAlignWallFeature], - yaw: f32, - translation: Vec3f, -) -> (f32, Vec3f) { - let mut refined_yaw = yaw; - let mut refined_translation = translation; - for _ in 0..3 { - let matches = collect_unique_wall_feature_matches( - local_walls, - remote_walls, - refined_yaw, - refined_translation, - ); - if matches.is_empty() { - break; - } - - if let Some(next_translation) = solve_translation_from_wall_constraints( - matches.iter().map(|matched| { - ( - matched.local.normal, - matched.local.plane_distance - matched.remote.plane_distance, - matched.score, - ) - }), - refined_translation.y, - ) { - refined_translation = next_translation; - } - - let mut yaw_sin = 0.0; - let mut yaw_cos = 0.0; - let mut yaw_weight_sum = 0.0; - for matched in &matches { - let Some(local_axis) = xz_axis(matched.local.normal) else { - continue; - }; - let Some(remote_axis) = xz_axis(matched.rotated_remote_normal) else { - continue; - }; - let delta_yaw = wrap_angle(signed_xz_angle(remote_axis, local_axis)); - let weight = matched.score.max(0.001); - yaw_sin += delta_yaw.sin() * weight; - yaw_cos += delta_yaw.cos() * weight; - yaw_weight_sum += weight; - } - if yaw_weight_sum > 0.0 { - refined_yaw = wrap_angle(refined_yaw + yaw_sin.atan2(yaw_cos)); - } - } - (refined_yaw, refined_translation) -} - -fn score_wall_feature_alignment( - local_walls: &[&XrDepthAlignWallFeature], - remote_walls: &[&XrDepthAlignWallFeature], - local_wall_samples: &[&XrDepthAlignSample], - remote_wall_samples: &[&XrDepthAlignSample], - yaw: f32, - translation: Vec3f, -) -> XrDepthAlignSolution { - let matches = collect_unique_wall_feature_matches(local_walls, remote_walls, yaw, translation); - let total_weight = remote_walls - .iter() - .map(|feature| wall_feature_weight(feature)) - .sum::() - .max(0.01); - let mut matched_weight = 0.0; - let mut residual_sum = 0.0; - let mut alignment_sum = 0.0; - let mut overlap_sum = 0.0; - for matched in &matches { - let weight = wall_feature_weight(matched.remote); - matched_weight += weight; - residual_sum += matched.plane_residual * weight; - alignment_sum += matched.alignment * weight; - overlap_sum += (matched.height_overlap * matched.horizontal_overlap * matched.width_ratio) - .sqrt() - * weight; - } - let residual_meters = if matched_weight > 0.0 { - residual_sum / matched_weight - } else { - f32::INFINITY - }; - let coverage = (matched_weight / total_weight).clamp(0.0, 1.0); - let mean_alignment = if matched_weight > 0.0 { - alignment_sum / matched_weight - } else { - 0.0 - }; - let overlap_quality = if matched_weight > 0.0 { - overlap_sum / matched_weight - } else { - 0.0 - }; - let corner_consistency = wall_feature_corner_consistency(&matches); - let residual_confidence = if residual_meters.is_finite() { - (1.0 - (residual_meters / XR_DEPTH_ALIGN_WALL_FEATURE_PLANE_RESIDUAL_MAX_METERS)) - .clamp(0.0, 1.0) - } else { - 0.0 - }; - let mut confidence = (coverage.sqrt() - * mean_alignment.powf(1.4).clamp(0.0, 1.0) - * (0.25 + 0.75 * overlap_quality) - * (0.70 + 0.30 * corner_consistency) - * residual_confidence.max(0.05)) - .clamp(0.0, 1.0); - let mut blended_residual = residual_meters; - if !local_wall_samples.is_empty() && !remote_wall_samples.is_empty() { - let sample_solution = - score_alignment_solution(local_wall_samples, remote_wall_samples, yaw, translation); - let sample_support = match sample_solution.matched_samples { - matches if matches >= 3 => 0.35 + 0.65 * sample_solution.confidence, - 2 => 0.28 + 0.72 * sample_solution.confidence, - 1 => 0.55, - _ => 0.22, - }; - confidence = (confidence * sample_support.clamp(0.0, 1.0)).clamp(0.0, 1.0); - if sample_solution.residual_meters.is_finite() { - blended_residual = if blended_residual.is_finite() { - blended_residual * 0.78 + sample_solution.residual_meters * 0.22 - } else { - sample_solution.residual_meters - }; - } - } - - XrDepthAlignSolution { - yaw_radians: wrap_angle(yaw), - translation, - confidence, - residual_meters: blended_residual, - matched_samples: matches.len(), - } -} - -fn wall_feature_signed_offset( - center: Vec3f, - along_axis: Vec3f, - half_extent_along: f32, - point: Vec3f, -) -> f32 { - (point - center).dot(along_axis) / half_extent_along.max(0.05) -} - -fn wall_feature_corner_consistency(matches: &[WallFeatureMatch<'_>]) -> f32 { - if matches.len() < 2 { - return 0.5; - } - let mut score_sum = 0.0; - let mut weight_sum = 0.0; - for (index, first) in matches.iter().enumerate() { - for second in matches.iter().skip(index + 1) { - let angle_cos = first.local.normal.dot(second.local.normal).abs(); - if angle_cos > XR_DEPTH_ALIGN_WALL_FEATURE_PAIR_ANGLE_MIN_RADIANS.cos() { - continue; - } - let Some(local_corner) = solve_translation_from_wall_constraints( - [ - (first.local.normal, first.local.plane_distance, 1.0), - (second.local.normal, second.local.plane_distance, 1.0), - ], - 0.0, - ) else { - continue; - }; - let Some(remote_corner) = solve_translation_from_wall_constraints( - [ - ( - first.rotated_remote_normal, - first.transformed_remote_plane_distance, - 1.0, - ), - ( - second.rotated_remote_normal, - second.transformed_remote_plane_distance, - 1.0, - ), - ], - 0.0, - ) else { - continue; - }; - let local_first_offset = wall_feature_signed_offset( - first.local.center, - first.local.along_axis, - first.local.half_extent_along, - local_corner, - ); - let local_second_offset = wall_feature_signed_offset( - second.local.center, - second.local.along_axis, - second.local.half_extent_along, - local_corner, - ); - let remote_first_offset = wall_feature_signed_offset( - first.transformed_remote_center, - first.rotated_remote_axis, - first.remote.half_extent_along, - remote_corner, - ); - let remote_second_offset = wall_feature_signed_offset( - second.transformed_remote_center, - second.rotated_remote_axis, - second.remote.half_extent_along, - remote_corner, - ); - let sign_score_first = - wall_feature_signed_offset_consistency(local_first_offset, remote_first_offset); - let sign_score_second = - wall_feature_signed_offset_consistency(local_second_offset, remote_second_offset); - let magnitude_score = (1.0 - - ((local_first_offset.abs() - remote_first_offset.abs()).abs() - + (local_second_offset.abs() - remote_second_offset.abs()).abs()) - / 4.0) - .clamp(0.0, 1.0); - let local_coverage = (1.0 - - ((local_first_offset.abs() - 1.0).max(0.0) - + (local_second_offset.abs() - 1.0).max(0.0)) - * 0.5) - .clamp(0.0, 1.0); - let remote_coverage = (1.0 - - ((remote_first_offset.abs() - 1.0).max(0.0) - + (remote_second_offset.abs() - 1.0).max(0.0)) - * 0.5) - .clamp(0.0, 1.0); - if local_coverage < 0.25 || remote_coverage < 0.25 { - continue; - } - let pair_weight = - (wall_feature_weight(first.remote) * wall_feature_weight(second.remote)).sqrt(); - let pair_score = sign_score_first - * sign_score_second - * (0.75 + 0.25 * magnitude_score) - * (local_coverage * remote_coverage).sqrt(); - score_sum += pair_score * pair_weight; - weight_sum += pair_weight; - } - } - if weight_sum > 0.0 { - (score_sum / weight_sum).clamp(0.0, 1.0) - } else { - 0.5 - } -} - -fn wall_feature_signed_offset_consistency(local: f32, remote: f32) -> f32 { - if local.abs() <= 0.20 || remote.abs() <= 0.20 { - 1.0 - } else if local.signum() == remote.signum() { - 1.0 - } else { - 0.05 - } -} - -fn candidate_yaws( - local_histogram: &[f32], - remote_histogram: &[f32], - local_walls: &[&XrDepthAlignSample], - remote_walls: &[&XrDepthAlignSample], -) -> Vec { - let mut candidates = vec![0.0]; - if local_histogram.len() == remote_histogram.len() && !local_histogram.is_empty() { - let bins = local_histogram.len(); - let mut shifts = Vec::<(f32, usize)>::new(); - for shift in 0..bins { - let score = (0..bins) - .map(|index| { - local_histogram[index] * remote_histogram[(index + bins - shift) % bins] - }) - .sum::(); - shifts.push((score, shift)); - } - shifts.sort_by(|a, b| b.0.total_cmp(&a.0)); - for (_, shift) in shifts.into_iter().take(6) { - candidates.push(wrap_angle( - shift as f32 * std::f32::consts::TAU / bins as f32, - )); - } - } - - for local_sample in local_walls.iter().take(12) { - let Some(local_axis) = xz_axis(local_sample.normal) else { - continue; - }; - for remote_sample in remote_walls.iter().take(12) { - let Some(remote_axis) = xz_axis(remote_sample.normal) else { - continue; - }; - candidates.push(wrap_angle(signed_xz_angle(remote_axis, local_axis))); - } - } - - dedupe_angles(candidates, 0.06) -} - -fn dedupe_angles(angles: Vec, epsilon: f32) -> Vec { - let mut deduped = Vec::::new(); - for angle in angles { - if deduped - .iter() - .any(|existing| wrap_angle(*existing - angle).abs() <= epsilon) - { - continue; - } - deduped.push(wrap_angle(angle)); - } - deduped -} - -fn candidate_translations( - local_walls: &[&XrDepthAlignSample], - remote_walls: &[&XrDepthAlignSample], - floor_y: f32, - yaw: f32, -) -> Vec { - let mut votes = HashMap::<(i32, i32), TranslationVote>::new(); - accumulate_translation_votes(&mut votes, local_walls, remote_walls, yaw, 0.88, 1.8); - if votes.is_empty() { - return vec![vec3(0.0, floor_y, 0.0)]; - } - - let mut ranked = votes.into_iter().collect::>(); - ranked.sort_by(|a, b| { - b.1.score - .total_cmp(&a.1.score) - .then_with(|| b.1.count.cmp(&a.1.count)) - }); - let translations = ranked - .into_iter() - .take(10) - .filter_map(|(_, vote)| { - (vote.weight_sum > 0.0).then_some(vec3( - vote.sum_x / vote.weight_sum, - floor_y, - vote.sum_z / vote.weight_sum, - )) - }) - .collect::>(); - dedupe_translations(translations, 0.08) -} - -fn dedupe_translations(translations: Vec, step: f32) -> Vec { - let mut deduped = Vec::::new(); - for translation in translations { - if deduped.iter().any(|existing| { - let delta = *existing - translation; - delta.x.abs() <= step && delta.y.abs() <= step && delta.z.abs() <= step - }) { - continue; - } - deduped.push(translation); - } - deduped -} - -#[derive(Clone, Copy)] -struct WallSampleMatch<'a> { - local: &'a XrDepthAlignSample, - remote: &'a XrDepthAlignSample, - distance: f32, - alignment: f32, - score: f32, -} - -fn collect_unique_wall_matches<'a>( - local_walls: &[&'a XrDepthAlignSample], - remote_walls: &[&'a XrDepthAlignSample], - yaw: f32, - translation: Vec3f, -) -> Vec> { - #[derive(Clone, Copy)] - struct Candidate<'a> { - local_index: usize, - remote_index: usize, - local: &'a XrDepthAlignSample, - remote: &'a XrDepthAlignSample, - distance: f32, - alignment: f32, - score: f32, - } - - let mut candidates = Vec::>::new(); - for (remote_index, remote_sample) in remote_walls.iter().enumerate() { - let transformed_point = rotate_y(yaw, remote_sample.point) + translation; - let transformed_normal = align_safe_normalize(rotate_y(yaw, remote_sample.normal)) - .unwrap_or(remote_sample.normal); - for (local_index, local_sample) in local_walls.iter().enumerate() { - let alignment = local_sample.normal.dot(transformed_normal); - if alignment < match_normal_dot(XrDepthAlignSampleKind::Wall) { - continue; - } - let distance = (local_sample.point - transformed_point).length(); - if distance > match_radius(XrDepthAlignSampleKind::Wall) { - continue; - } - let score = (sample_alignment_weight(local_sample) - * sample_alignment_weight(remote_sample)) - .sqrt() - * alignment - * (-distance / match_radius(XrDepthAlignSampleKind::Wall).max(0.05)).exp(); - candidates.push(Candidate { - local_index, - remote_index, - local: local_sample, - remote: remote_sample, - distance, - alignment, - score, - }); - } - } - - candidates.sort_by(|a, b| { - b.score - .total_cmp(&a.score) - .then_with(|| a.distance.total_cmp(&b.distance)) - }); - - let mut used_local = vec![false; local_walls.len()]; - let mut used_remote = vec![false; remote_walls.len()]; - let mut matches = Vec::>::new(); - for candidate in candidates { - if used_local[candidate.local_index] || used_remote[candidate.remote_index] { - continue; - } - used_local[candidate.local_index] = true; - used_remote[candidate.remote_index] = true; - matches.push(WallSampleMatch { - local: candidate.local, - remote: candidate.remote, - distance: candidate.distance, - alignment: candidate.alignment, - score: candidate.score, - }); - } - matches -} - -fn refine_alignment( - local_walls: &[&XrDepthAlignSample], - remote_walls: &[&XrDepthAlignSample], - floor_y: f32, - yaw: f32, - translation: Vec3f, -) -> (f32, Vec3f) { - let mut refined_yaw = yaw; - let mut refined_translation = translation; - refined_translation.y = floor_y; - - for _ in 0..2 { - let mut translation_sum = vec3(0.0, 0.0, 0.0); - let mut translation_weight_sum = 0.0; - let mut yaw_sin = 0.0; - let mut yaw_cos = 0.0; - let mut yaw_weight_sum = 0.0; - - let matches = collect_unique_wall_matches( - local_walls, - remote_walls, - refined_yaw, - refined_translation, - ); - for matched in matches { - let local_sample = matched.local; - let remote_sample = matched.remote; - let weight = (local_sample.weight * remote_sample.weight).sqrt() * matched.alignment; - let candidate_translation = - local_sample.point - rotate_y(refined_yaw, remote_sample.point); - translation_sum += candidate_translation * weight; - translation_weight_sum += weight; - let Some(local_axis) = xz_axis(local_sample.normal) else { - continue; - }; - let Some(remote_axis) = xz_axis(remote_sample.normal) else { - continue; - }; - let candidate_yaw = wrap_angle(signed_xz_angle(remote_axis, local_axis)); - yaw_sin += candidate_yaw.sin() * weight; - yaw_cos += candidate_yaw.cos() * weight; - yaw_weight_sum += weight; - } - - if yaw_weight_sum > 0.0 { - refined_yaw = wrap_angle(yaw_sin.atan2(yaw_cos)); - } - if translation_weight_sum > 0.0 { - refined_translation = translation_sum * (1.0 / translation_weight_sum); - refined_translation.y = floor_y; - } - } - - (refined_yaw, refined_translation) -} - -fn score_alignment_solution( - local_walls: &[&XrDepthAlignSample], - remote_walls: &[&XrDepthAlignSample], - yaw: f32, - translation: Vec3f, -) -> XrDepthAlignSolution { - let mut total_score = 0.0; - let mut residual_sum = 0.0; - let max_score = remote_walls - .iter() - .map(|sample| sample_alignment_weight(sample)) - .sum::() - .max(0.01); - let matches = collect_unique_wall_matches(local_walls, remote_walls, yaw, translation); - for matched in &matches { - total_score += matched.score; - residual_sum += matched.distance; - } - let matched_samples = matches.len(); - let residual_meters = if matched_samples > 0 { - residual_sum / matched_samples as f32 - } else { - f32::INFINITY - }; - let coverage = (matched_samples as f32 / remote_walls.len().max(1) as f32).clamp(0.0, 1.0); - let residual_confidence = if residual_meters.is_finite() { - (1.0 - (residual_meters / 0.42)).clamp(0.0, 1.0) - } else { - 0.0 - }; - let confidence = ((total_score / max_score).clamp(0.0, 1.0) - * coverage.sqrt() - * residual_confidence.max(0.2)) - .clamp(0.0, 1.0); - XrDepthAlignSolution { - yaw_radians: wrap_angle(yaw), - translation, - confidence, - residual_meters, - matched_samples, - } -} - -#[derive(Default)] -struct TranslationVote { - score: f32, - weight_sum: f32, - sum_x: f32, - sum_z: f32, - count: usize, -} - -fn descriptor_samples_of_kind<'a>( - descriptor: &'a XrDepthAlignDescriptor, - kind: XrDepthAlignSampleKind, -) -> Vec<&'a XrDepthAlignSample> { - let mut samples = descriptor - .samples - .iter() - .filter(|sample| sample.kind == kind) - .collect::>(); - samples.sort_by(|a, b| b.weight.total_cmp(&a.weight)); - samples -} - -fn accumulate_translation_votes( - votes: &mut HashMap<(i32, i32), TranslationVote>, - local_samples: &[&XrDepthAlignSample], - remote_samples: &[&XrDepthAlignSample], - yaw: f32, - normal_dot_min: f32, - class_bias: f32, -) { - for local_sample in local_samples.iter().take(64) { - for remote_sample in remote_samples.iter().take(64) { - let rotated_normal = align_safe_normalize(rotate_y(yaw, remote_sample.normal)) - .unwrap_or(remote_sample.normal); - let alignment = local_sample.normal.dot(rotated_normal); - if alignment < normal_dot_min { - continue; - } - let delta = local_sample.point - rotate_y(yaw, remote_sample.point); - if delta.x.abs() > 8.0 || delta.z.abs() > 8.0 { - continue; - } - let weight = - class_bias * (local_sample.weight * remote_sample.weight).sqrt() * alignment; - let key = ( - quantize_translation_axis(delta.x, XR_DEPTH_ALIGN_TRANSLATION_VOTE_STEP_METERS), - quantize_translation_axis(delta.z, XR_DEPTH_ALIGN_TRANSLATION_VOTE_STEP_METERS), - ); - let vote = votes.entry(key).or_default(); - vote.score += weight; - vote.weight_sum += weight; - vote.sum_x += delta.x * weight; - vote.sum_z += delta.z * weight; - vote.count += 1; - } - } -} - -fn sample_alignment_weight(sample: &XrDepthAlignSample) -> f32 { - let kind_bias = match sample.kind { - XrDepthAlignSampleKind::Floor => 1.0, - XrDepthAlignSampleKind::Wall => 1.7, - XrDepthAlignSampleKind::Unknown => 0.0, - }; - sample.weight.max(0.01) * kind_bias -} - -fn match_radius(kind: XrDepthAlignSampleKind) -> f32 { - match kind { - XrDepthAlignSampleKind::Floor => 0.42, - XrDepthAlignSampleKind::Wall => 0.32, - XrDepthAlignSampleKind::Unknown => 0.25, - } -} - -fn match_normal_dot(kind: XrDepthAlignSampleKind) -> f32 { - match kind { - XrDepthAlignSampleKind::Floor => 0.92, - XrDepthAlignSampleKind::Wall => 0.86, - XrDepthAlignSampleKind::Unknown => 0.80, - } -} - -fn quantize_translation_axis(value: f32, step: f32) -> i32 { - (value / step.max(f32::EPSILON)).round() as i32 -} - -fn alignment_solution_better( - candidate: &XrDepthAlignSolution, - current: &XrDepthAlignSolution, -) -> bool { - candidate - .confidence - .partial_cmp(¤t.confidence) - .unwrap_or(std::cmp::Ordering::Equal) - .then_with(|| candidate.matched_samples.cmp(¤t.matched_samples)) - .then_with(|| { - current - .residual_meters - .partial_cmp(&candidate.residual_meters) - .unwrap_or(std::cmp::Ordering::Equal) - }) - .is_gt() -} - -#[derive(Clone, Copy, Debug, Default, PartialEq, Eq)] -pub enum XrDepthPlaneKind { - Floor, - Table, - Ceiling, - Wall, - #[default] - Unknown, -} - -#[derive(Clone, Debug, Default)] -pub struct XrDepthPlanePatch { - pub generation: u64, - pub kind: XrDepthPlaneKind, - pub center: Vec3f, - pub normal: Vec3f, - pub tangent: Vec3f, - pub bitangent: Vec3f, - pub half_extent_tangent: f32, - pub half_extent_bitangent: f32, - pub area: f32, - pub support_triangles: usize, -} - -#[derive(Clone, Copy, Debug)] -pub struct XrDepthMeshQuery { - pub key: u64, - pub center: Vec3f, - pub predicted_center: Vec3f, - pub velocity: Vec3f, - pub radius: f32, - pub max_distance: f32, - pub include_planar_patches: bool, -} - -#[derive(Clone, Copy, Debug)] -pub struct XrDepthMeshQuerySurfaceHit { - pub distance: f32, - pub point: Vec3f, - pub normal: Vec3f, - pub from_planar_patch: bool, - pub triangle: [Vec3f; 3], - pub patch: [Vec3f; 4], - pub chunk_key: ChunkKey, -} - -#[derive(Clone, Copy, Debug)] -pub struct XrDepthMeshQuerySupportPlane { - pub point: Vec3f, - pub normal: Vec3f, - pub tangent: Vec3f, - pub bitangent: Vec3f, - pub half_extent_tangent: f32, - pub half_extent_bitangent: f32, -} - -#[derive(Clone, Debug)] -pub enum XrDepthMeshQueryColliderGeometry { - HalfSpace(XrDepthMeshQuerySupportPlane), -} - -#[derive(Clone, Copy, Debug, PartialEq, Eq, Hash)] -pub enum XrDepthMeshQueryColliderRole { - Support, - Impact, -} - -#[derive(Clone, Debug)] -pub struct XrDepthMeshQueryCollider { - pub fingerprint: u64, - pub geometry: XrDepthMeshQueryColliderGeometry, - pub role: XrDepthMeshQueryColliderRole, - pub restitution: f32, -} - -impl XrDepthMeshQueryCollider { - pub fn vertex_count(&self) -> usize { - 0 - } - - pub fn triangle_count(&self) -> usize { - 0 - } -} - -#[derive(Clone, Debug)] -pub struct XrDepthMeshQueryResolvedSurface { - pub surface: XrDepthMeshQuerySurfaceHit, - pub collider: XrDepthMeshQueryCollider, -} - -#[derive(Clone, Debug)] -pub struct XrDepthMeshQueryHit { - pub key: u64, - pub version: u64, - pub mesh_generation: u64, - pub distance: f32, - pub point: Vec3f, - pub normal: Vec3f, - pub from_planar_patch: bool, - pub triangle: [Vec3f; 3], - pub patch: [Vec3f; 4], - pub chunk_key: ChunkKey, - pub collider: XrDepthMeshQueryCollider, - pub additional_hits: Vec, -} - -#[derive(Clone, Debug)] -pub enum XrDepthMeshQueryResult { - Hit(XrDepthMeshQueryHit), - Miss { - key: u64, - version: u64, - mesh_generation: u64, - }, -} - -impl XrDepthMeshQueryResult { - pub fn key(&self) -> u64 { - match self { - Self::Hit(hit) => hit.key, - Self::Miss { key, .. } => *key, - } - } - - pub fn version(&self) -> u64 { - match self { - Self::Hit(hit) => hit.version, - Self::Miss { version, .. } => *version, - } - } -} - -#[derive(Clone, Debug)] -pub(crate) struct XrDepthMeshPendingQuery { - pub query: XrDepthMeshQuery, - pub version: u64, -} - -#[derive(Default)] -struct XrDepthMeshQueryState { - next_versions: HashMap, - pending: HashMap, - pending_order: VecDeque, - results: HashMap, -} - -#[derive(Clone)] -pub struct XrDepthMeshStore { - state: Arc>, - alignment_state: Arc>>, - queries: Arc>, - reset_generation: Arc, - mesh_enabled: Arc, - plane_scan_enabled: Arc, - surface_analysis_enabled: Arc, - alignment_preview_enabled: Arc, - voxel_size_meters_bits: Arc, -} - -impl Default for XrDepthMeshStore { - fn default() -> Self { - Self { - state: Arc::new(RwLock::new(XrDepthMeshState::default())), - alignment_state: Arc::new(RwLock::new(Arc::new(XrDepthAlignState::default()))), - queries: Arc::new(Mutex::new(XrDepthMeshQueryState::default())), - reset_generation: Arc::new(AtomicU64::new(0)), - mesh_enabled: Arc::new(AtomicBool::new(false)), - plane_scan_enabled: Arc::new(AtomicBool::new(false)), - surface_analysis_enabled: Arc::new(AtomicBool::new(false)), - alignment_preview_enabled: Arc::new(AtomicBool::new(false)), - voxel_size_meters_bits: Arc::new(AtomicU32::new( - XR_DEPTH_MESH_DEFAULT_VOXEL_SIZE_METERS.to_bits(), - )), - } - } -} - -impl XrDepthMeshStore { - fn keeps_latest_mesh_alive(&self) -> bool { - self.mesh_enabled() || self.surface_analysis_enabled() || self.plane_scan_enabled() - } - - pub fn set_voxel_size_meters(&self, voxel_size_meters: f32) -> f32 { - let voxel_size_meters = voxel_size_meters.clamp(0.03, 0.10); - let previous = self - .voxel_size_meters_bits - .swap(voxel_size_meters.to_bits(), Ordering::AcqRel); - if previous != voxel_size_meters.to_bits() { - self.clear(); - } - voxel_size_meters - } - - pub fn voxel_size_meters(&self) -> f32 { - f32::from_bits(self.voxel_size_meters_bits.load(Ordering::Acquire)) - } - - pub fn reset_generation(&self) -> u64 { - self.reset_generation.load(Ordering::Acquire) - } - - pub fn request_reset(&self) -> u64 { - let generation = self.reset_generation.fetch_add(1, Ordering::AcqRel) + 1; - self.clear(); - generation - } - - pub fn set_mesh_enabled(&self, enabled: bool) { - let was_enabled = self.mesh_enabled.swap(enabled, Ordering::AcqRel); - if was_enabled && !enabled && !self.keeps_latest_mesh_alive() { - if let Ok(mut state) = self.state.write() { - state.latest_mesh = None; - } - } - } - - pub fn mesh_enabled(&self) -> bool { - self.mesh_enabled.load(Ordering::Acquire) - } - - pub fn set_plane_scan_enabled(&self, enabled: bool) { - let was_enabled = self.plane_scan_enabled.swap(enabled, Ordering::AcqRel); - if was_enabled && !enabled && !self.keeps_latest_mesh_alive() { - if let Ok(mut state) = self.state.write() { - state.latest_mesh = None; - } - } - } - - pub fn plane_scan_enabled(&self) -> bool { - self.plane_scan_enabled.load(Ordering::Acquire) - } - - pub fn set_surface_analysis_enabled(&self, enabled: bool) { - let was_enabled = self - .surface_analysis_enabled - .swap(enabled, Ordering::AcqRel); - if was_enabled && !enabled && !self.keeps_latest_mesh_alive() { - if let Ok(mut state) = self.state.write() { - state.latest_mesh = None; - } - } - } - - pub fn surface_analysis_enabled(&self) -> bool { - self.surface_analysis_enabled.load(Ordering::Acquire) - } - - pub fn set_alignment_preview_enabled(&self, enabled: bool) { - self.alignment_preview_enabled - .store(enabled, Ordering::Release); - } - - pub fn alignment_preview_enabled(&self) -> bool { - self.alignment_preview_enabled.load(Ordering::Acquire) - } - - pub fn state(&self) -> Arc> { - self.state.clone() - } - - pub fn latest_mesh(&self) -> Option> { - self.state - .read() - .ok() - .and_then(|state| state.latest_mesh.clone()) - } - - pub fn latest_alignment_state(&self) -> Arc { - self.alignment_state - .read() - .map(|state| state.clone()) - .unwrap_or_else(|_| Arc::new(XrDepthAlignState::default())) - } - - pub fn submit_query(&self, query: XrDepthMeshQuery) -> Option { - let Ok(mut state) = self.queries.lock() else { - return None; - }; - let version = state - .next_versions - .entry(query.key) - .and_modify(|version| *version = version.saturating_add(1)) - .or_insert(1); - let version = *version; - - if let Some(pending) = state.pending.get_mut(&query.key) { - pending.query = query; - pending.version = version; - return Some(version); - } - - if state.pending.len() >= XR_DEPTH_QUERY_MAX_PENDING { - return None; - } - - state - .pending - .insert(query.key, XrDepthMeshPendingQuery { query, version }); - state.pending_order.push_back(query.key); - Some(version) - } - - pub fn latest_query_result(&self, key: u64) -> Option { - self.queries - .lock() - .ok() - .and_then(|state| state.results.get(&key).cloned()) - } - - pub fn clear_query(&self, key: u64) { - if let Ok(mut state) = self.queries.lock() { - state.pending.remove(&key); - state - .pending_order - .retain(|pending_key| *pending_key != key); - state.results.remove(&key); - state.next_versions.remove(&key); - } - } - - #[allow(dead_code)] - pub(crate) fn record_seen(&self) { - if let Ok(mut state) = self.state.write() { - state.stats.frames_seen += 1; - } - } - - #[allow(dead_code)] - pub(crate) fn record_drop(&self) { - if let Ok(mut state) = self.state.write() { - state.stats.frames_dropped += 1; - } - } - - #[allow(dead_code)] - pub(crate) fn set_error(&self, error: String) { - if let Ok(mut state) = self.state.write() { - state.last_error = Some(error); - } - } - - #[allow(dead_code)] - pub(crate) fn publish(&self, mesh: XrDepthMesh) { - if let Ok(mut state) = self.state.write() { - state.latest_mesh = Some(Arc::new(mesh)); - state.stats.frames_meshed += 1; - state.last_error = None; - } - } - - #[allow(dead_code)] - pub(crate) fn publish_alignment_state(&self, alignment_state: XrDepthAlignState) { - if let Ok(mut state) = self.alignment_state.write() { - *state = Arc::new(alignment_state); - } - } - - #[allow(dead_code)] - pub(crate) fn drain_pending_queries(&self, max_queries: usize) -> Vec { - let Ok(mut state) = self.queries.lock() else { - return Vec::new(); - }; - let mut drained = Vec::with_capacity(max_queries.min(state.pending.len())); - for _ in 0..max_queries { - let Some(key) = state.pending_order.pop_front() else { - break; - }; - let Some(query) = state.pending.remove(&key) else { - continue; - }; - drained.push(query); - } - drained - } - - #[allow(dead_code)] - pub(crate) fn has_pending_queries(&self) -> bool { - self.queries - .lock() - .map(|state| !state.pending.is_empty()) - .unwrap_or(false) - } - - #[allow(dead_code)] - pub(crate) fn publish_query_results(&self, results: Vec) { - if results.is_empty() { - return; - } - if let Ok(mut state) = self.queries.lock() { - for result in results { - state.results.insert(result.key(), result); - } - } - } - - #[allow(dead_code)] - pub(crate) fn clear(&self) { - if let Ok(mut state) = self.state.write() { - state.latest_mesh = None; - state.last_error = None; - state.stats = XrDepthMeshStats::default(); - } - if let Ok(mut alignment_state) = self.alignment_state.write() { - *alignment_state = Arc::new(XrDepthAlignState::default()); - } - if let Ok(mut queries) = self.queries.lock() { - *queries = XrDepthMeshQueryState::default(); - } - } -} - -pub fn xr_depth_mesh_store() -> XrDepthMeshStore { - static STORE: OnceLock = OnceLock::new(); - STORE.get_or_init(XrDepthMeshStore::default).clone() -} - -#[allow(dead_code)] -pub(crate) fn empty_bounds() -> (Vec3f, Vec3f) { - (vec3(0.0, 0.0, 0.0), vec3(0.0, 0.0, 0.0)) -} - -#[cfg(test)] -mod tests { - use super::*; - - fn make_wall_feature( - center: Vec3f, - normal: Vec3f, - half_extent_along: f32, - min_y: f32, - max_y: f32, - ) -> XrDepthAlignWallFeature { - let normal = normal.normalize(); - let along_axis = vec3(-normal.z, 0.0, normal.x).normalize(); - XrDepthAlignWallFeature { - center, - normal, - along_axis, - plane_distance: center.dot(normal), - half_extent_along, - min_y, - max_y, - area: (half_extent_along * 2.0) * (max_y - min_y).max(0.0), - } - } - - fn make_wall_sample(point: Vec3f, normal: Vec3f, weight: f32) -> XrDepthAlignSample { - XrDepthAlignSample { - kind: XrDepthAlignSampleKind::Wall, - point, - normal: normal.normalize(), - weight, - } - } - - fn make_wall_samples_from_feature( - feature: &XrDepthAlignWallFeature, - weight: f32, - ) -> [XrDepthAlignSample; 2] { - [ - make_wall_sample( - vec3( - feature.center.x - feature.along_axis.x * feature.half_extent_along * 0.55, - feature.min_y + 0.18, - feature.center.z - feature.along_axis.z * feature.half_extent_along * 0.55, - ), - feature.normal, - weight, - ), - make_wall_sample( - vec3( - feature.center.x + feature.along_axis.x * feature.half_extent_along * 0.55, - feature.max_y - 0.18, - feature.center.z + feature.along_axis.z * feature.half_extent_along * 0.55, - ), - feature.normal, - weight * 0.96, - ), - ] - } - - fn make_asymmetric_wall_descriptor() -> XrDepthAlignDescriptor { - let wall_features = vec![ - make_wall_feature( - vec3(-1.10, 0.95, -0.90), - vec3(1.0, 0.0, 0.0), - 0.46, - 0.52, - 1.38, - ), - make_wall_feature( - vec3(0.98, 0.88, -1.26), - vec3(1.0, 0.0, 0.0), - 0.34, - 0.54, - 1.22, - ), - make_wall_feature( - vec3(0.08, 0.98, -2.12), - vec3(0.0, 0.0, 1.0), - 0.38, - 0.58, - 1.34, - ), - make_wall_feature( - vec3(0.36, 0.92, -0.42), - vec3(0.0, 0.0, 1.0), - 0.30, - 0.60, - 1.24, - ), - ]; - let mut samples = Vec::new(); - for (index, feature) in wall_features.iter().enumerate() { - samples.extend(make_wall_samples_from_feature( - feature, - 0.92 - index as f32 * 0.04, - )); - } - XrDepthAlignDescriptor { - voxel_size_meters: 0.05, - floor_y: 0.0, - wall_normal_histogram: xr_depth_align_build_wall_feature_normal_histogram( - &wall_features, - 48, - ), - wall_features, - samples, - } - } - - fn make_box_room_descriptor_with_patch_asymmetry() -> XrDepthAlignDescriptor { - let wall_features = vec![ - make_wall_feature(vec3(1.42, 1.0, 0.18), vec3(1.0, 0.0, 0.0), 1.18, 0.0, 2.0), - make_wall_feature(vec3(-1.08, 1.0, 0.22), vec3(-1.0, 0.0, 0.0), 1.18, 0.0, 2.0), - make_wall_feature(vec3(0.16, 1.0, 1.54), vec3(0.0, 0.0, 1.0), 1.25, 0.0, 2.0), - make_wall_feature(vec3(0.14, 1.0, -0.96), vec3(0.0, 0.0, -1.0), 1.25, 0.0, 2.0), - ]; - let samples = vec![ - make_wall_sample(vec3(1.42, 1.28, -0.52), vec3(1.0, 0.0, 0.0), 0.96), - make_wall_sample(vec3(1.42, 0.42, -0.18), vec3(1.0, 0.0, 0.0), 0.90), - make_wall_sample(vec3(0.78, 1.14, 1.54), vec3(0.0, 0.0, 1.0), 0.92), - make_wall_sample(vec3(0.36, 0.52, 1.54), vec3(0.0, 0.0, 1.0), 0.88), - ]; - XrDepthAlignDescriptor { - voxel_size_meters: 0.05, - floor_y: 0.0, - wall_normal_histogram: xr_depth_align_build_wall_feature_normal_histogram( - &wall_features, - 48, - ), - wall_features, - samples, - } - } - - fn reflection_x() -> Mat4f { - Mat4f { - v: [ - -1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, - ], - } - } - - fn angle_error(a: f32, b: f32) -> f32 { - wrap_angle(a - b).abs() - } - - #[test] - fn disabling_mesh_keeps_latest_mesh_while_surface_analysis_is_enabled() { - let store = XrDepthMeshStore::default(); - store.publish(XrDepthMesh::default()); - store.set_surface_analysis_enabled(true); - store.set_mesh_enabled(true); - store.set_mesh_enabled(false); - assert!(store.latest_mesh().is_some()); - } - - #[test] - fn disabling_last_surface_consumer_clears_latest_mesh() { - let store = XrDepthMeshStore::default(); - store.publish(XrDepthMesh::default()); - store.set_surface_analysis_enabled(true); - store.set_surface_analysis_enabled(false); - assert!(store.latest_mesh().is_none()); - } - - #[test] - fn wall_solver_recovers_asymmetric_pose() { - let local = make_asymmetric_wall_descriptor(); - let remote_to_local = Pose::new( - Quat::from_axis_angle(vec3(0.0, 1.0, 0.0), 0.58), - vec3(-0.82, 0.0, 0.67), - ) - .to_mat4(); - let local_to_remote = remote_to_local.invert(); - let mut remote = xr_depth_align_transform_descriptor(&local, &local_to_remote); - remote.samples = remote - .samples - .into_iter() - .enumerate() - .filter_map(|(index, mut sample)| { - if index % 5 == 2 { - return None; - } - sample.point += vec3( - ((index % 4) as f32 - 1.5) * 0.012, - (((index * 3) % 5) as f32 - 2.0) * 0.020, - (((index * 5) % 7) as f32 - 3.0) * 0.012, - ); - sample.weight = - (sample.weight * (0.84 + 0.03 * (index % 4) as f32)).clamp(0.1, 1.0); - Some(sample) - }) - .collect(); - remote.wall_normal_histogram = - xr_depth_align_build_wall_feature_normal_histogram(&remote.wall_features, 48); - - let diagnostic = xr_depth_align_analyze_remote_to_local(&local, &remote); - let solution = diagnostic.accepted_solution().unwrap_or_else(|| { - panic!("solver should recover asymmetric wall pose: {diagnostic:?}") - }); - - assert!( - angle_error(solution.yaw_radians, 0.58) < 0.08, - "{solution:?}" - ); - assert!( - (solution.translation - vec3(-0.82, 0.0, 0.67)).length() < 0.12, - "{solution:?}" - ); - assert!(solution.confidence > 0.20, "{solution:?}"); - assert!(solution.matched_samples >= 2, "{solution:?}"); - } - - #[test] - fn wall_solver_rejects_mirrored_asymmetric_descriptor() { - let local = make_asymmetric_wall_descriptor(); - let mirrored = xr_depth_align_transform_descriptor(&local, &reflection_x()); - let diagnostic = xr_depth_align_analyze_remote_to_local(&local, &mirrored); - - assert!( - diagnostic.accepted_solution().is_none(), - "mirrored descriptor should not be accepted: {diagnostic:?}" - ); - } - - #[test] - fn wall_solver_ignores_along_wall_patch_sliding() { - let local = make_asymmetric_wall_descriptor(); - let remote_to_local = Pose::new( - Quat::from_axis_angle(vec3(0.0, 1.0, 0.0), 0.44), - vec3(-0.46, 0.0, 0.58), - ) - .to_mat4(); - let local_to_remote = remote_to_local.invert(); - let mut remote = xr_depth_align_transform_descriptor(&local, &local_to_remote); - - for (index, feature) in remote.wall_features.iter_mut().enumerate() { - let slide = match index { - 0 => 0.32, - 1 => -0.24, - 2 => 0.28, - _ => -0.18, - }; - feature.center += feature.along_axis.scale(slide); - feature.plane_distance = feature.center.dot(feature.normal); - } - for (index, sample) in remote.samples.iter_mut().enumerate() { - let slide = match index / 2 { - 0 => 0.32, - 1 => -0.24, - 2 => 0.28, - _ => -0.18, - }; - let axis = vec3(-sample.normal.z, 0.0, sample.normal.x).normalize(); - sample.point += axis.scale(slide); - } - remote.wall_normal_histogram = - xr_depth_align_build_wall_feature_normal_histogram(&remote.wall_features, 48); - - let diagnostic = xr_depth_align_analyze_remote_to_local(&local, &remote); - let solution = diagnostic.accepted_solution().unwrap_or_else(|| { - panic!( - "solver should still recover transform from orthogonal wall offsets: {diagnostic:?}" - ) - }); - - assert!( - angle_error(solution.yaw_radians, 0.44) < 0.08, - "{solution:?}" - ); - assert!( - (solution.translation - vec3(-0.46, 0.0, 0.58)).length() < 0.08, - "{solution:?}" - ); - assert!(solution.confidence > 0.18, "{solution:?}"); - assert!(solution.matched_samples >= 2, "{solution:?}"); - } - - #[test] - fn wall_solver_uses_patch_asymmetry_to_break_box_room_flip() { - let local = make_box_room_descriptor_with_patch_asymmetry(); - let expected_yaw = 0.37; - let expected_translation = vec3(-0.62, 0.0, 0.48); - let remote_to_local = Pose::new( - Quat::from_axis_angle(vec3(0.0, 1.0, 0.0), expected_yaw), - expected_translation, - ) - .to_mat4(); - let local_to_remote = remote_to_local.invert(); - let mut remote = xr_depth_align_transform_descriptor(&local, &local_to_remote); - remote.wall_normal_histogram = - xr_depth_align_build_wall_feature_normal_histogram(&remote.wall_features, 48); - - let diagnostic = xr_depth_align_analyze_remote_to_local(&local, &remote); - let solution = diagnostic.accepted_solution().unwrap_or_else(|| { - panic!("room-box solver should accept the patch-supported pose: {diagnostic:?}") - }); - assert!( - angle_error(solution.yaw_radians, expected_yaw) < 0.08, - "{solution:?}" - ); - assert!( - (solution.translation - expected_translation).length() < 0.10, - "{solution:?}" - ); - - let local_walls = descriptor_wall_features(&local); - let remote_walls = descriptor_wall_features(&remote); - let local_samples = descriptor_samples_of_kind(&local, XrDepthAlignSampleKind::Wall); - let remote_samples = descriptor_samples_of_kind(&remote, XrDepthAlignSampleKind::Wall); - let correct = score_wall_feature_alignment( - &local_walls, - &remote_walls, - &local_samples, - &remote_samples, - expected_yaw, - expected_translation, - ); - let flipped_yaw = wrap_angle(expected_yaw + std::f32::consts::PI); - let flipped = - candidate_wall_feature_translations(&local_walls, &remote_walls, 0.0, flipped_yaw) - .into_iter() - .map(|translation| { - score_wall_feature_alignment( - &local_walls, - &remote_walls, - &local_samples, - &remote_samples, - flipped_yaw, - translation, - ) - }) - .max_by(|left, right| { - left.confidence - .total_cmp(&right.confidence) - .then_with(|| left.matched_samples.cmp(&right.matched_samples)) - }) - .expect("expected flipped box-room hypothesis"); - assert!( - correct.confidence > flipped.confidence + 0.06, - "patch asymmetry should make the correct box-room pose rank above the flipped one: correct={correct:?} flipped={flipped:?}" - ); - } -} diff --git a/platform/src/xr_tsdf.rs b/platform/src/xr_tsdf.rs new file mode 100644 index 0000000000..b64f72a041 --- /dev/null +++ b/platform/src/xr_tsdf.rs @@ -0,0 +1 @@ +pub use makepad_tsdf::*; diff --git a/studio/desktop/src/app_backend.rs b/studio/desktop/src/app_backend.rs index 80fa036ecf..c834baf48c 100644 --- a/studio/desktop/src/app_backend.rs +++ b/studio/desktop/src/app_backend.rs @@ -574,10 +574,51 @@ impl App { } } - pub(super) fn terminal_tab_title(path: &str) -> String { + pub(super) fn default_terminal_tab_title(path: &str) -> String { path.rsplit('/').next().unwrap_or("terminal").to_string() } + pub(super) fn terminal_tab_title(&self, path: &str) -> String { + self.data + .terminal_title_by_path + .get(path) + .cloned() + .unwrap_or_else(|| Self::default_terminal_tab_title(path)) + } + + pub(super) fn apply_terminal_tab_title(&mut self, cx: &mut Cx, path: &str, title: String) { + self.data + .terminal_title_by_path + .insert(path.to_string(), title.clone()); + let Some((mount, tab_id)) = Self::mount_from_virtual_path(path).and_then(|mount| { + self.mount_state(mount) + .and_then(|state| state.terminal_path_to_tab.get(path).copied()) + .map(|tab_id| (mount.to_string(), tab_id)) + }) else { + return; + }; + if let Some(dock) = self.mount_workspace_dock(cx, &mount) { + dock.set_tab_title(cx, tab_id, title); + dock.redraw_tab(cx, tab_id); + } + } + + pub(super) fn reset_terminal_tab_title(&mut self, cx: &mut Cx, path: &str) { + self.data.terminal_title_by_path.remove(path); + let title = Self::default_terminal_tab_title(path); + let Some((mount, tab_id)) = Self::mount_from_virtual_path(path).and_then(|mount| { + self.mount_state(mount) + .and_then(|state| state.terminal_path_to_tab.get(path).copied()) + .map(|tab_id| (mount.to_string(), tab_id)) + }) else { + return; + }; + if let Some(dock) = self.mount_workspace_dock(cx, &mount) { + dock.set_tab_title(cx, tab_id, title); + dock.redraw_tab(cx, tab_id); + } + } + pub(super) fn terminal_tab_mount_path(&self, tab_id: LiveId) -> Option<(String, String)> { for (mount, state) in &self.data.mounts { if let Some(path) = state.terminal_tab_to_path.get(&tab_id) { @@ -795,6 +836,10 @@ impl App { .mount_state(mount) .map(|mount| mount.terminal_files.clone()) .unwrap_or_default(); + let titles: HashMap = files + .iter() + .map(|path| (path.clone(), self.terminal_tab_title(path))) + .collect(); let Some(dock) = self.mount_workspace_dock(cx, mount) else { return; @@ -812,10 +857,14 @@ impl App { dock.set_tab_title(cx, id!(terminal_first), String::new()); for path in files.iter() { + let title = titles + .get(path) + .cloned() + .unwrap_or_else(|| Self::default_terminal_tab_title(path)); // If a valid tab already exists for this path, just update its title. if let Some(existing) = path_to_tab.get(path).copied() { if dock.find_tab_bar_of_tab(existing).is_some() { - dock.set_tab_title(cx, existing, Self::terminal_tab_title(path)); + dock.set_tab_title(cx, existing, title); continue; } path_to_tab.remove(path); @@ -833,7 +882,7 @@ impl App { tab_bar, tab_id, id!(TerminalPane), - Self::terminal_tab_title(path), + title, id!(TerminalCloseableTab), Some(pos.saturating_sub(1)), ) diff --git a/studio/desktop/src/app_data.rs b/studio/desktop/src/app_data.rs index 446c54705e..311d9fe551 100644 --- a/studio/desktop/src/app_data.rs +++ b/studio/desktop/src/app_data.rs @@ -141,6 +141,7 @@ pub struct AppData { pub terminal_frame_id_by_path: HashMap, pub terminal_viewport_request_by_path: HashMap, pub terminal_open_paths: HashSet, + pub terminal_title_by_path: HashMap, pub file_filter_mount_by_query: HashMap, pub pending_stop_all_mount: Option, pub pending_log_jumps: HashMap, diff --git a/studio/desktop/src/app_messages.rs b/studio/desktop/src/app_messages.rs index e3fd0a9923..f9e158ca29 100644 --- a/studio/desktop/src/app_messages.rs +++ b/studio/desktop/src/app_messages.rs @@ -641,10 +641,14 @@ impl App { self.data.terminal_framebuffer_by_path.insert(path, frame); self.refresh_active_mount_log_panels(cx); } + HubToClient::TerminalTitle { path, title } => { + self.apply_terminal_tab_title(cx, &path, title); + } HubToClient::TerminalExited { path, code } => { self.data.terminal_open_paths.remove(&path); self.data.terminal_frame_id_by_path.remove(&path); self.data.terminal_framebuffer_by_path.remove(&path); + self.reset_terminal_tab_title(cx, &path); self.set_status(cx, &format!("terminal exited ({})", code)); } HubToClient::Error { message } => { diff --git a/studio/hub/src/dispatch.rs b/studio/hub/src/dispatch.rs index 2a6f9828b5..98c01e4d16 100644 --- a/studio/hub/src/dispatch.rs +++ b/studio/hub/src/dispatch.rs @@ -222,6 +222,7 @@ struct TerminalSession { applied_rows: u16, // Monotonic frame sequence to let clients drop stale frames. frame_seq: u64, + bell_pending: bool, subscribers: HashMap, } @@ -1392,6 +1393,7 @@ impl HubCore { client_id, HubToClient::TerminalOpened { path: path.clone() }, ); + self.send_terminal_title_to_client(client_id, &path); self.send_terminal_viewport_for_client( client_id, &path, @@ -1448,6 +1450,7 @@ impl HubCore { applied_cols: cols, applied_rows: rows, frame_seq: 0, + bell_pending: false, subscribers: HashMap::new(), }, ); @@ -1468,8 +1471,9 @@ impl HubCore { } } ClientToHub::TerminalInput { path, data } => { - if let Err(err) = self.terminal_manager.send_input(&path, data) { - self.send_ui_error(client_id, err); + match self.terminal_manager.send_input(&path, data) { + Ok(()) => self.set_terminal_bell_state(&path, false), + Err(err) => self.send_ui_error(client_id, err), } } ClientToHub::TerminalViewportRequest { @@ -3016,12 +3020,14 @@ impl HubCore { self.mount_suppress_fs_until .insert(mount, Instant::now() + Duration::from_millis(750)); let mut force_bottom_for_sticky = true; + let mut bell_rang = false; if let Some(session) = self.terminal_sessions.get_mut(&path) { let old_total_rows = { let screen = session.terminal.screen(); screen.scrollback_len() + screen.used_rows() }; session.terminal.process_bytes(&data); + bell_rang = session.terminal.take_bell(); let outbound = session.terminal.take_outbound(); if !outbound.is_empty() { let _ = self.terminal_manager.send_input(&path, outbound); @@ -3035,6 +3041,9 @@ impl HubCore { // viewport jump during rapid resize sequences. force_bottom_for_sticky = new_total_rows > old_total_rows; } + if bell_rang { + self.set_terminal_bell_state(&path, true); + } self.push_terminal_frame_updates(&path, force_bottom_for_sticky); // Persist terminal history off the dispatch thread so fs I/O cannot // block terminal framebuffer delivery. @@ -3142,6 +3151,36 @@ impl HubCore { self.push_terminal_frame_updates(path, false); } + fn send_terminal_title_to_client(&mut self, client_id: ClientId, path: &str) { + let Some(session) = self.terminal_sessions.get(path) else { + return; + }; + if !session.bell_pending { + return; + } + self.send_ui_reply( + client_id, + HubToClient::TerminalTitle { + path: path.to_string(), + title: Self::terminal_tab_title(path, true), + }, + ); + } + + fn set_terminal_bell_state(&mut self, path: &str, bell_pending: bool) { + let Some(session) = self.terminal_sessions.get_mut(path) else { + return; + }; + if session.bell_pending == bell_pending { + return; + } + session.bell_pending = bell_pending; + self.broadcast_ui_message(HubToClient::TerminalTitle { + path: path.to_string(), + title: Self::terminal_tab_title(path, bell_pending), + }); + } + fn push_terminal_frame_updates(&mut self, path: &str, force_bottom_for_sticky: bool) { let updates = { let Some(session) = self.terminal_sessions.get_mut(path) else { @@ -3308,6 +3347,15 @@ impl HubCore { } } + fn terminal_tab_title(path: &str, bell_pending: bool) -> String { + let title = path.rsplit('/').next().unwrap_or("terminal"); + if bell_pending { + format!("@ {}", title) + } else { + title.to_string() + } + } + fn send_ui_reply(&self, client_id: ClientId, msg: HubToClient) { self.send_ui_message(client_id, msg, self.ui_format(client_id)); } diff --git a/studio/hub/tests/in_process_test.rs b/studio/hub/tests/in_process_test.rs index 26cd26b73f..227cf9fcce 100644 --- a/studio/hub/tests/in_process_test.rs +++ b/studio/hub/tests/in_process_test.rs @@ -324,6 +324,65 @@ fn terminal_large_paste_keeps_session_alive() { ); } +#[cfg(any(target_os = "macos", target_os = "linux"))] +#[test] +fn terminal_bell_sets_title_badge_until_next_input() { + let dir = makepad_studio_hub::test_support::tempdir().unwrap(); + fs::create_dir_all(dir.path().join("src")).unwrap(); + fs::write(dir.path().join("src/lib.rs"), "pub fn hi() {}\n").unwrap(); + + let config = HubConfig { + mounts: vec![MountConfig { + name: "repo".to_string(), + path: dir.path().to_path_buf(), + }], + ..Default::default() + }; + let mut connection = StudioHub::start_in_process(config).expect("start in-process backend"); + + let path = "repo/.makepad/bell.term".to_string(); + let _ = connection.send(ClientToHub::TerminalOpen { + path: path.clone(), + cols: 120, + rows: 30, + env: std::collections::HashMap::new(), + }); + let opened = wait_for_message( + &connection, + Duration::from_secs(4), + |msg| matches!(msg, HubToClient::TerminalOpened { path: p, .. } if p == &path), + ); + assert!(opened.is_some(), "did not receive TerminalOpened"); + request_terminal_viewport(&mut connection, &path, 120, 30, usize::MAX); + + let _ = connection.send(ClientToHub::TerminalInput { + path: path.clone(), + data: b"printf '\\a'\n".to_vec(), + }); + let bell = wait_for_message( + &connection, + Duration::from_secs(6), + |msg| matches!(msg, HubToClient::TerminalTitle { path: p, title } if p == &path && title == "@ bell.term"), + ); + assert!(bell.is_some(), "did not observe terminal bell title"); + + let _ = connection.send(ClientToHub::TerminalInput { + path: path.clone(), + data: b":\n".to_vec(), + }); + let cleared = wait_for_message( + &connection, + Duration::from_secs(6), + |msg| matches!(msg, HubToClient::TerminalTitle { path: p, title } if p == &path && title == "bell.term"), + ); + assert!( + cleared.is_some(), + "did not observe terminal title clearing after input" + ); + + let _ = connection.send(ClientToHub::TerminalClose { path }); +} + #[cfg(any(target_os = "macos", target_os = "linux"))] #[test] fn terminal_resize_delivers_sigwinch_with_updated_stty_size() { diff --git a/tools/cargo_makepad/src/android/compile.rs b/tools/cargo_makepad/src/android/compile.rs index 286af6abc5..6f7bcff122 100644 --- a/tools/cargo_makepad/src/android/compile.rs +++ b/tools/cargo_makepad/src/android/compile.rs @@ -1420,7 +1420,12 @@ pub fn run( &[], &cwd, sdk_dir.join("platform-tools/adb").to_str().unwrap(), - &["install", "-r", (result.dst_apk.to_str().unwrap())], + &[ + "install", + "--no-incremental", + "-r", + (result.dst_apk.to_str().unwrap()), + ], )?; println!("Starting android application"); let start_args = android_start_args(&result.java_url); @@ -1465,6 +1470,7 @@ pub fn run( "-s", &device, "install", + "--no-incremental", "-r", (result.dst_apk.to_str().unwrap()), ], diff --git a/tools/cargo_makepad/src/android/java/dev/makepad/android/MakepadActivity.java b/tools/cargo_makepad/src/android/java/dev/makepad/android/MakepadActivity.java index eed663723f..63425d8581 100644 --- a/tools/cargo_makepad/src/android/java/dev/makepad/android/MakepadActivity.java +++ b/tools/cargo_makepad/src/android/java/dev/makepad/android/MakepadActivity.java @@ -620,6 +620,20 @@ public ResizingLayout(Context context){ public WindowInsets onApplyWindowInsets(View v, WindowInsets insets) { Insets imeInsets = insets.getInsets(WindowInsets.Type.ime()); v.setPadding(0, 0, 0, imeInsets.bottom); + + // Compute safe area insets from system bars and display cutout. + // These are in physical pixels; convert to logical points by dividing by density. + float density = getResources().getDisplayMetrics().density; + Insets systemBarInsets = insets.getInsets( + WindowInsets.Type.systemBars() | WindowInsets.Type.displayCutout() + ); + MakepadNative.surfaceOnSafeAreaInsets( + systemBarInsets.top / density, + systemBarInsets.right / density, + systemBarInsets.bottom / density, + systemBarInsets.left / density + ); + return insets; } } diff --git a/tools/cargo_makepad/src/android/java/dev/makepad/android/MakepadNative.java b/tools/cargo_makepad/src/android/java/dev/makepad/android/MakepadNative.java index 04946d01c4..ecdd4581de 100644 --- a/tools/cargo_makepad/src/android/java/dev/makepad/android/MakepadNative.java +++ b/tools/cargo_makepad/src/android/java/dev/makepad/android/MakepadNative.java @@ -29,6 +29,7 @@ public static native void onAndroidParams(String cache_path, String data_path, f public native static void surfaceOnKeyUp(int keycode, int meta_state); public native static void surfaceOnCharacter(int character); public native static void surfaceOnResizeTextIME(int keyboard_height, boolean is_open); + public native static void surfaceOnSafeAreaInsets(float top, float right, float bottom, float left); // networking public native static void onHttpResponse(long id, long metadata_id, int status_code, String headers, byte[] body); diff --git a/widgets/resources/jetbrains_mono_variable.ttf b/widgets/resources/jetbrains_mono_variable.ttf new file mode 100644 index 0000000000..b60e77f5db Binary files /dev/null and b/widgets/resources/jetbrains_mono_variable.ttf differ diff --git a/widgets/src/desktop_button.rs b/widgets/src/desktop_button.rs index c38f668ed7..fa0c11f8fc 100644 --- a/widgets/src/desktop_button.rs +++ b/widgets/src/desktop_button.rs @@ -26,12 +26,23 @@ script_mod! { color_hover: uniform(theme.color_label_inner_hover) color_down: uniform(theme.color_label_inner_down) + bg_color: uniform(#00000000) + bg_color_hover: uniform(#00000000) + bg_color_down: uniform(#00000000) + pixel: fn() { let sdf = Sdf2d.viewport(self.pos * self.rect_size) sdf.aa = sdf.aa * 3.0 let sz = 4.5 let c = self.rect_size * vec2(0.5, 0.5) + // Draw background + let bg = self.bg_color + .mix(self.bg_color_hover, self.hover) + .mix(self.bg_color_down, self.down); + sdf.rect(0., 0., self.rect_size.x, self.rect_size.y); + sdf.fill(bg); + let color = self.color .mix(self.color_hover, self.hover) .mix(self.color_down, self.down) diff --git a/widgets/src/lib.rs b/widgets/src/lib.rs index 5d3c718a39..b6571d3f39 100644 --- a/widgets/src/lib.rs +++ b/widgets/src/lib.rs @@ -590,6 +590,19 @@ pub fn widgets_mod(vm: &mut ScriptVm) { crate::map::view::script_mod(vm); crate::math_view::script_mod(vm); + // Safe area inset values (in logical points). Populated from the platform's + // display_context which is set before Startup on iOS/Android. On desktop + // platforms these remain 0.0. Updated at runtime on WindowGeomChange events. + { + use makepad_script::trap::NoTrap; + let insets = vm.cx().display_context.safe_area_insets; + let widgets = vm.module(id!(widgets)); + vm.bx.heap.set_value(widgets, id!(SAFE_INSET_PAD_TOP).into(), insets.top.into(), NoTrap); + vm.bx.heap.set_value(widgets, id!(SAFE_INSET_PAD_BOTTOM).into(), insets.bottom.into(), NoTrap); + vm.bx.heap.set_value(widgets, id!(SAFE_INSET_PAD_LEFT).into(), insets.left.into(), NoTrap); + vm.bx.heap.set_value(widgets, id!(SAFE_INSET_PAD_RIGHT).into(), insets.right.into(), NoTrap); + } + script_eval!(vm, { mod.prelude.widgets = { ..mod.prelude.widgets_header, diff --git a/widgets/src/math_view.rs b/widgets/src/math_view.rs index 06408efb75..8b382dc09f 100644 --- a/widgets/src/math_view.rs +++ b/widgets/src/math_view.rs @@ -19,12 +19,7 @@ script_mod! { font_size: 11.0 baseline_offset: -2.0 draw_glyph +: { - aa_2x2: 1.0 - aa_4x4: 1.0 aa_pad_px: 1.0 - axis_relief: 0.65 - stem_darken: 0.25 - stem_darken_max: 0.025 } draw_text +: { text_style: TextStyle{ @@ -277,7 +272,7 @@ fn build_glyph_shape( } } dg.fill_layer(); - dg.commit_shape(Some(0)) + dg.commit_shape(None) } fn build_rule_shape( @@ -293,7 +288,7 @@ fn build_rect_shape(dg: &mut DrawGlyph, x: f32, y: f32, w: f32, h: f32) -> Optio dg.set_color(1.0, 1.0, 1.0, 1.0); dg.rect(x, y, w, h); dg.fill_layer(); - dg.commit_shape(Some(0)) + dg.commit_shape(None) } fn push_component(dg: &DrawGlyph, shape_id: GlyphShapeId, components: &mut Vec) { diff --git a/widgets/src/portal_list.rs b/widgets/src/portal_list.rs index 3181f7f5eb..d6f3794b73 100644 --- a/widgets/src/portal_list.rs +++ b/widgets/src/portal_list.rs @@ -1,7 +1,7 @@ use { crate::{ animator::AnimatorImpl, - event::TouchState, + event::{TouchState, TAP_COUNT_DISTANCE}, makepad_derive_widget::*, makepad_draw::*, scroll_bar::{ScrollAxis, ScrollBar, ScrollBarAction}, @@ -39,6 +39,11 @@ enum ScrollState { Stopped, Drag { samples: Vec, + /// The initial touch position along the scroll axis at FingerDown. + initial_abs: f64, + /// Whether this drag has exceeded the minimum threshold to be treated + /// as scroll rather than a tap. Until committed, no scroll deltas are applied. + committed: bool, }, Flick { delta: f64, @@ -51,6 +56,9 @@ enum ScrollState { target_id: usize, delta: f64, next_frame: NextFrame, + /// Pixel offset from the top of the viewport where the target item's + /// top edge should end up once scrolling completes. + top_offset: f64, }, Tailing { next_frame: NextFrame, @@ -375,6 +383,22 @@ pub struct PortalList { grab_key_focus: bool, #[live(true)] drag_scrolling: bool, + /// The minimum distance (in pixels) a finger/mouse must move before a drag is + /// "committed" and treated as a scroll gesture rather than a tap/click. + /// This prevents accidental micro-scrolling when tapping interactive items. + /// Defaults to `TAP_COUNT_DISTANCE`, keeping it consistent with the + /// platform's tap-vs-drag distinction. + #[rust(TAP_COUNT_DISTANCE)] + drag_scroll_threshold: f64, + + /// Whether the current gesture should be suppressed from child widgets. + /// Set when either: + /// - A drag scroll commits (finger moves past threshold), or + /// - A finger-down/click occurs while a scroll animation was in progress + /// (the tap-to-stop-scroll gesture). + /// Cleared on the next finger-down that arrives when not scrolling. + #[rust] + suppress_child_events: bool, #[rust] first_id: usize, @@ -528,7 +552,11 @@ impl PortalList { } fn end(&mut self, cx: &mut Cx2d) { - self.at_end = false; + // Note: we intentionally do NOT reset `at_end` here. It is explicitly + // set to true or false in every code path below. If the draw cycle + // doesn't reach the calculation (e.g., draw_state isn't End, or list + // is empty), we preserve the previous value rather than introducing + // a spurious `false` that would cause downstream consumers to flicker. self.not_filling_viewport = false; let vi = self.vec_index; @@ -548,15 +576,25 @@ impl PortalList { let mut last_pos = self.first_scroll; let mut last_item_pos = None; + let mut last_drawn_index = None; for i in first_index..list.len() { let item = &list[i]; last_pos += item.size.index(vi); if item.index < self.range_end { last_item_pos = Some(last_pos); + last_drawn_index = Some(item.index); } else { break; } } + // Whether the very last item in the range was actually drawn. + // The draw loop can stop early when it encounters a zero-size + // item (e.g., an empty placeholder widget), which would leave + // `last_item_pos` far short of the viewport bottom. Without + // this guard, `at_end` could become a false positive whenever + // a zero-size item appears in the middle of the visible range. + let drew_last_item = last_drawn_index + == Some(self.range_end.saturating_sub(1)); if list[0].index == self.range_start { let mut total = 0.0; @@ -570,6 +608,10 @@ impl PortalList { } if list.first().unwrap().index == self.range_start && first_pos > 0.0 { + // We're at the top of the list with a gap above the first item. + // We're also at the end if all content fits in the viewport. + self.at_end = self.not_filling_viewport && drew_last_item; + let min = if let ScrollState::Stopped = self.scroll_state { 0.0 } else { @@ -591,15 +633,19 @@ impl PortalList { } else { let shift = if let Some(last_item_pos) = last_item_pos { if self.align_top_when_empty && self.not_filling_viewport { + // All items fit in the viewport without filling it. + self.at_end = drew_last_item; -first_pos } else { let ret = viewport.size.index(vi) - last_item_pos; - if ret >= 0.0 { - self.at_end = true; - } + // Use a 1px tolerance for floating-point accumulation + // errors across item sizes, and require that the last + // item in the range was actually drawn. + self.at_end = ret >= -1.0 && drew_last_item; ret.max(0.0) } } else { + self.at_end = false; 0.0 }; @@ -1228,13 +1274,42 @@ impl PortalList { self.visible_items } - /// Initiates a smooth scrolling animation to the specified target item in the list. + /// Computes the top position of `target_id` relative to the viewport top + /// using `first_id`, `first_scroll`, and the height tree. + /// + /// Returns `None` if the height tree is not yet available (before the first draw). + /// A return value of `0.0` means the item's top is exactly at the viewport top; + /// negative means it is above the viewport, positive means below. + fn item_top_from_height_tree(&self, target_id: usize) -> Option { + let tree = self.height_tree.as_ref()?; + let first_idx = self.first_id.saturating_sub(self.range_start); + let target_idx = target_id.saturating_sub(self.range_start); + + let prefix = |i: usize| if i > 0 { tree.prefix_sum(i - 1) } else { 0.0 }; + + if target_id >= self.first_id { + Some(self.first_scroll + prefix(target_idx) - prefix(first_idx)) + } else { + Some(self.first_scroll - (prefix(first_idx) - prefix(target_idx))) + } + } + + /// Initiates a smooth scrolling animation to the specified target item. + /// + /// If the target item's top is already visible within the viewport, no scrolling + /// occurs and [`PortalListAction::SmoothScrollReached`] is emitted immediately. + /// + /// Otherwise, the list animates until the target item's top edge is positioned at + /// `top_offset` pixels below the viewport's top edge. A value of `0.0` places the + /// item flush with the viewport top; `20.0` leaves a 20 px margin. Negative values + /// are clamped to `0.0`. pub fn smooth_scroll_to( &mut self, cx: &mut Cx, target_id: usize, speed: f64, max_items_to_show: Option, + top_offset: f64, ) { if self.items.is_empty() { return; @@ -1243,15 +1318,40 @@ impl PortalList { return; } + // Check if the target item's top is already visible in the viewport. + // Compute the target's top position relative to viewport using first_id, + // first_scroll, and the height_tree — this avoids relying on widget rects + // which may be clipped for partially-visible items. + let vi = self.vec_index; + let viewport_size = self.area.rect(cx).size.index(vi); + let item_top = self.item_top_from_height_tree(target_id); + if viewport_size > 0.0 { + if let Some(item_top) = item_top { + if item_top >= 0.0 && item_top < viewport_size { + cx.widget_action(self.widget_uid(), PortalListAction::SmoothScrollReached); + return; + } + } + } + let max_items_to_show = max_items_to_show.unwrap_or(SMOOTH_SCROLL_MAXIMUM_WINDOW); - let scroll_direction: f64; + + // Determine scroll direction from the item's actual pixel position + // relative to the viewport, not from index comparison alone. + // When first_scroll is very negative, items with target_id > first_id + // can still be above the viewport. + let scroll_direction: f64 = if let Some(item_top) = item_top { + if item_top < 0.0 { 1.0 } else { -1.0 } + } else { + // Height tree unavailable; fall back to index comparison. + if target_id > self.first_id { -1.0 } else { 1.0 } + }; + let starting_id: Option; if target_id > self.first_id { - scroll_direction = -1.0; starting_id = ((target_id.saturating_sub(self.first_id)) > max_items_to_show) .then_some(target_id.saturating_sub(max_items_to_show)); } else { - scroll_direction = 1.0; starting_id = ((self.first_id.saturating_sub(target_id)) > max_items_to_show) .then_some(target_id + max_items_to_show); } @@ -1263,6 +1363,7 @@ impl PortalList { target_id, delta: speed.abs() * scroll_direction, next_frame: cx.new_next_frame(), + top_offset, }; } @@ -1277,7 +1378,7 @@ impl PortalList { return; } let speed = speed * self.range_end as f64; - self.smooth_scroll_to(cx, self.range_end, speed, max_items_to_show); + self.smooth_scroll_to(cx, self.range_end, speed, max_items_to_show, 0.0); } /// Returns whether this PortalList is currently filling the viewport. @@ -1679,7 +1780,36 @@ impl Widget for PortalList { // Hover events (FingerHoverIn/Out/Over) are ALWAYS passed through so interactive items // can properly show/hide their hover states. let mut pass_through_to_children = true; - if self.selectable { + + // Suppress all interaction events to children when: + // - A committed drag scroll is active (finger/mouse moved past threshold), or + // - The current gesture started as a tap-to-stop-scroll (user tapped while + // a flick animation was in progress). + // Hover events are not suppressed so widgets can still update their hover state. + // + // For down events (MouseDown/TouchStart), we also check whether the scroll + // is currently animating. This is needed because `suppress_child_events` is + // set in hit processing which runs *after* child event forwarding. + let is_scroll_animating = matches!( + self.scroll_state, + ScrollState::Flick { .. } + | ScrollState::Pulldown { .. } + | ScrollState::ScrollingTo { .. } + | ScrollState::Tailing { .. } + ); + if self.suppress_child_events || is_scroll_animating { + match event { + Event::TouchUpdate(_) | Event::MouseDown(_) + | Event::MouseMove(_) | Event::MouseUp(_) => { + pass_through_to_children = false; + } + _ => {} + } + } + + // The selectable logic can only further restrict pass-through, never override + // the drag-scroll suppression above. + if self.selectable && pass_through_to_children { match event { // Always pass hover events through for proper hover state management Event::MouseMove(_) => { @@ -1763,25 +1893,62 @@ impl Widget for PortalList { target_id, delta, next_frame, + top_offset, } => { if next_frame.is_event(event).is_some() { + // Copy values out of the borrow so we can call &self methods. let target_id = *target_id; + let delta_val = *delta; + let top_offset = *top_offset; + let scrolling_down = delta_val < 0.0; + + // Check if the target item has reached (or passed) the + // desired position using the height tree for accuracy. + let vi = self.vec_index; + let viewport_size = self.area.rect(cx).size.index(vi); + let item_top = self.item_top_from_height_tree(target_id); + let mut target_reached = false; + + if let Some(item_top) = item_top { + // Clamp: the item must at least reach the viewport (top >= 0). + let effective_target = top_offset.max(0.0); + if scrolling_down { + target_reached = item_top <= effective_target; + } else { + target_reached = item_top >= effective_target; + } - let distance_to_target = target_id as isize - self.first_id as isize; - let target_passed = distance_to_target.signum() == delta.signum() as isize; - if target_passed { - self.first_id = target_id; - self.area.redraw(cx); + // For boundary conditions (e.g., near list start/end), the + // effective_target may not be reachable. Consider it reached + // if the item's top is visible and we're at a list boundary. + if !target_reached + && item_top >= 0.0 + && item_top < viewport_size + && (self.first_id == self.range_start || self.at_end) + { + target_reached = true; + } } - let distance_to_target = target_id as isize - self.first_id as isize; - let target_visible_at_end = self.at_end && target_id > self.first_id; - let target_reached = distance_to_target == 0 || target_visible_at_end; + // Fallback: if we're scrolling down and the list has hit + // the end, the target is as visible as it can get. + if scrolling_down && self.at_end && target_id > self.first_id { + target_reached = true; + } if !target_reached { - *next_frame = cx.new_next_frame(); - let delta = *delta; - self.delta_top_scroll(cx, delta, true, false); + // Overshoot protection: if first_id has scrolled past + // target_id, snap it back so the item gets drawn. + let distance_to_target = target_id as isize - self.first_id as isize; + let overshot = distance_to_target.signum() == delta_val.signum() as isize; + if overshot { + self.first_id = target_id; + } + + if let ScrollState::ScrollingTo { next_frame, .. } = &mut self.scroll_state { + *next_frame = cx.new_next_frame(); + } + self.delta_top_scroll(cx, delta_val, true, false); cx.widget_action(uid, PortalListAction::Scroll); self.area.redraw(cx); } else { @@ -1893,16 +2060,15 @@ impl Widget for PortalList { self.detect_tail_in_draw = true; self.was_scrolling = false; self.scroll_state = ScrollState::Stopped; - let prev_first_id = self.first_id; - let prev_first_scroll = self.first_scroll; // For mouse wheel: clip to top and don't transition to pulldown // (pulldown/overscroll is only for touch drag/flick) self.delta_top_scroll(cx, -e.scroll.index(vi), true, false); - if self.first_id != prev_first_id - || (self.first_scroll - prev_first_scroll).abs() > f64::EPSILON - { - self.at_end = false; - } + // Note: we intentionally do NOT reset `at_end` here. + // `at_end` is authoritatively recalculated each draw cycle + // in `end()`, and the redraw is already triggered below. + // Eagerly resetting it here would create a stale `false` value + // visible to any code that checks `is_at_end()` in response + // to the Scroll action before the next draw completes. cx.widget_action(uid, PortalListAction::Scroll); self.area.redraw(cx); } @@ -1977,11 +2143,17 @@ impl Widget for PortalList { } self.tail_range = false; self.was_scrolling = match &self.scroll_state { - ScrollState::Drag { samples } => samples.len() > 1, + ScrollState::Drag { samples, .. } => samples.len() > 1, ScrollState::Stopped => false, _ => true, }; + // If the list was animating (flick, pulldown, etc.) when the user + // tapped/clicked, suppress forwarding this entire gesture to children. + // The tap should only stop the scroll, not activate a child widget. + // If the list was NOT scrolling, clear any previous suppression. + self.suppress_child_events = self.was_scrolling; + // Handle selection when selectable, but not if clicking on interactive items let on_interactive = self.point_hits_interactive_item(cx, fe.abs); if self.selectable && fe.is_primary_hit() && !on_interactive { @@ -2000,12 +2172,23 @@ impl Widget for PortalList { }); self.update_item_selections(cx); } - } else if self.drag_scrolling && fe.is_primary_hit() && !on_interactive { + } else if self.drag_scrolling && fe.is_primary_hit() { + // Always enter drag state to enable drag-to-scroll even over + // interactive widgets (buttons, links, etc.). The drag threshold + // prevents micro-scrolling during taps/clicks, and child widgets + // use `was_tap()` to distinguish taps from drags on FingerUp. + let initial = fe.abs.index(vi); self.scroll_state = ScrollState::Drag { samples: vec![ScrollSample { - abs: fe.abs.index(vi), + abs: initial, time: fe.time, }], + initial_abs: initial, + // When on interactive items, require a drag threshold before + // committing to scroll (for both touch and mouse). + // For non-interactive areas, commit immediately since + // there's no ambiguity. + committed: !on_interactive, }; } } @@ -2030,8 +2213,35 @@ impl Widget for PortalList { if !self.point_hits_interactive_item(cx, e.abs) { cx.set_cursor(MouseCursor::Default); } - if let ScrollState::Drag { samples } = &mut self.scroll_state { + if let ScrollState::Drag { + samples, + initial_abs, + committed, + } = &mut self.scroll_state + { let new_abs = e.abs.index(vi); + + // Check if the drag threshold has been exceeded. + if !*committed { + if (new_abs - *initial_abs).abs() + >= self.drag_scroll_threshold + { + *committed = true; + self.suppress_child_events = true; + } else { + // Still under threshold — track samples but don't scroll. + samples.push(ScrollSample { + abs: new_abs, + time: e.time, + }); + if samples.len() > 4 { + samples.remove(0); + } + // Don't apply scroll delta yet. + return; + } + } + let old_sample = *samples.last().unwrap(); samples.push(ScrollSample { abs: new_abs, @@ -2062,36 +2272,50 @@ impl Widget for PortalList { } } - if let ScrollState::Drag { samples } = &mut self.scroll_state { - let mut last = None; - let mut scaled_delta = 0.0; - let mut total_delta = 0.0; - for sample in samples.iter().rev() { - if last.is_none() { - last = Some(sample); - } else { - total_delta += last.unwrap().abs - sample.abs; - scaled_delta += (last.unwrap().abs - sample.abs) - / (last.unwrap().time - sample.time); - } - } - scaled_delta *= self.flick_scroll_scaling; - if self.first_id == self.range_start && self.first_scroll > 0.0 { - self.scroll_state = ScrollState::Pulldown { - next_frame: cx.new_next_frame(), - }; - } else if total_delta.abs() > 10.0 - && scaled_delta.abs() > self.flick_scroll_minimum - { - self.scroll_state = ScrollState::Flick { - delta: scaled_delta - .min(self.flick_scroll_maximum) - .max(-self.flick_scroll_maximum), - next_frame: cx.new_next_frame(), - }; - } else { + // Always clear touch drag scrolling active flag on finger up. + self.suppress_child_events = false; + + if let ScrollState::Drag { + samples, committed, .. + } = &mut self.scroll_state + { + // If the drag was never committed (finger didn't move past + // threshold), just stop — this was a tap, not a scroll. + if !*committed { self.was_scrolling = false; self.scroll_state = ScrollState::Stopped; + } else { + let mut last = None; + let mut scaled_delta = 0.0; + let mut total_delta = 0.0; + for sample in samples.iter().rev() { + if last.is_none() { + last = Some(sample); + } else { + total_delta += last.unwrap().abs - sample.abs; + scaled_delta += (last.unwrap().abs - sample.abs) + / (last.unwrap().time - sample.time); + } + } + scaled_delta *= self.flick_scroll_scaling; + if self.first_id == self.range_start && self.first_scroll > 0.0 + { + self.scroll_state = ScrollState::Pulldown { + next_frame: cx.new_next_frame(), + }; + } else if total_delta.abs() > 10.0 + && scaled_delta.abs() > self.flick_scroll_minimum + { + self.scroll_state = ScrollState::Flick { + delta: scaled_delta + .min(self.flick_scroll_maximum) + .max(-self.flick_scroll_maximum), + next_frame: cx.new_next_frame(), + }; + } else { + self.was_scrolling = false; + self.scroll_state = ScrollState::Stopped; + } } } } @@ -2228,7 +2452,7 @@ impl PortalListRef { match &inner.scroll_state { ScrollState::Stopped => {} - ScrollState::Drag { samples } => { + ScrollState::Drag { samples, .. } => { state = "Drag"; drag_samples = samples.len(); } @@ -2359,18 +2583,21 @@ impl PortalListRef { false } - /// Initiates a smooth scrolling animation to the specified target item in the list. + /// Initiates a smooth scrolling animation to the specified target item. + /// + /// See [`PortalList::smooth_scroll_to()`] for full documentation. pub fn smooth_scroll_to( &self, cx: &mut Cx, target_id: usize, speed: f64, max_items_to_show: Option, + top_offset: f64, ) { let Some(mut inner) = self.borrow_mut() else { return; }; - inner.smooth_scroll_to(cx, target_id, speed, max_items_to_show); + inner.smooth_scroll_to(cx, target_id, speed, max_items_to_show, top_offset); } /// Returns the ID of the item that is currently being smoothly scrolled to, if any. diff --git a/widgets/src/stack_navigation.rs b/widgets/src/stack_navigation.rs index ea065a845d..d8f950aab7 100644 --- a/widgets/src/stack_navigation.rs +++ b/widgets/src/stack_navigation.rs @@ -189,14 +189,17 @@ impl Widget for StackNavigationView { } fn draw_walk(&mut self, cx: &mut Cx2d, scope: &mut Scope, walk: Walk) -> DrawStep { + let parent_rect = cx.peek_walk_turtle(walk); let abs_pos = if self.full_screen { // In full screen mode, position at the offset. + // Use the larger of the safe area inset or the parent's y position + // so the view respects both mobile safe areas and desktop title bars. + let safe_top = cx.display_context.safe_area_insets.top; Vec2d { x: self.offset, - y: 0., + y: safe_top.max(parent_rect.pos.y), } } else { - let parent_rect = cx.peek_walk_turtle(walk); // Non-fullscreen: ignore offset, position at parent. Vec2d { x: parent_rect.pos.x, diff --git a/widgets/src/text_input.rs b/widgets/src/text_input.rs index b8020d330e..175e8d9174 100644 --- a/widgets/src/text_input.rs +++ b/widgets/src/text_input.rs @@ -17,6 +17,7 @@ use { *, }, makepad_script::{ScriptFnRef, ScriptRefOptionExt}, + scroll_bar::{ScrollBar, ScrollAxis}, widget::*, widget_async::ScriptAsyncResult, }, @@ -40,6 +41,12 @@ script_mod! { is_read_only: false is_numeric_only: false empty_text: "Your text here" + scroll_bar: mod.widgets.ScrollBar { + bar_size: 8.0 + bar_side_margin: 2.0 + min_handle_size: 20.0 + drag_scrolling: true + } draw_bg +: { hover: instance(0.0) @@ -524,10 +531,12 @@ pub struct TextInput { #[live(false)] is_multiline: bool, #[live] + scroll_bar: ScrollBar, + #[live] scroll_y: f64, #[live] empty_text: String, - #[rust] + #[live] text: String, #[live(0.5)] blink_speed: f64, @@ -537,6 +546,8 @@ pub struct TextInput { #[rust] laidout_text: Option>, #[rust] + laidout_width: Option, + #[rust] text_area: Area, #[rust] selection: Selection, @@ -546,6 +557,15 @@ pub struct TextInput { blink_timer: Timer, #[rust] preserved_selection_cursor: Option, + /// When true, the next draw will scroll to keep the cursor visible. + /// Set when the cursor/selection changes; cleared after scroll_to_cursor runs. + #[rust(true)] + needs_scroll_to_cursor: bool, + /// Cached maximum scroll offset from the last draw pass. Used during event + /// handling to ensure boundary checks match exactly (avoiding floating-point + /// mismatch between draw-time and event-time computations). + #[rust] + cached_max_scroll_y: f64, /// Skip finger move after long press to prevent selection changes #[rust] ignore_next_move: bool, @@ -605,6 +625,20 @@ impl TextInput { } } + pub fn is_multiline(&self) -> bool { + self.is_multiline + } + + pub fn set_is_multiline(&mut self, cx: &mut Cx, is_multiline: bool) { + self.is_multiline = is_multiline; + if !is_multiline { + self.scroll_y = 0.0; + self.cached_max_scroll_y = 0.0; + } + self.laidout_text = None; + self.draw_bg.redraw(cx); + } + pub fn is_password(&self) -> bool { self.is_password } @@ -664,6 +698,7 @@ impl TextInput { pub fn set_selection(&mut self, cx: &mut Cx, selection: Selection) { self.selection = selection; + self.needs_scroll_to_cursor = true; self.history.force_new_edit_group(); self.draw_bg.redraw(cx); } @@ -781,7 +816,13 @@ impl TextInput { } fn layout_text(&mut self, cx: &mut Cx2d) { - if self.laidout_text.is_some() { + let turtle_rect = cx.turtle().inner_rect(); + let max_width_in_lpxs = if !turtle_rect.size.x.is_nan() { + Some(turtle_rect.size.x as f32) + } else { + None + }; + if self.laidout_text.is_some() && self.laidout_width == max_width_in_lpxs { return; } let text = if self.is_password { @@ -794,13 +835,9 @@ impl TextInput { } else { &self.text }; - let turtle_rect = cx.turtle().inner_rect(); - let max_width_in_lpxs = if !turtle_rect.size.x.is_nan() { - Some(turtle_rect.size.x as f32) - } else { - None - }; - let wrap = cx.turtle().layout().flow == Flow::right_wrap(); + + let wrap = self.is_multiline && cx.turtle().layout().flow == Flow::right_wrap(); + self.laidout_width = max_width_in_lpxs; self.laidout_text = Some(self.draw_text.layout( cx, 0.0, @@ -929,33 +966,55 @@ impl TextInput { rect(sel_x, sel_y, sel_width.max(10.0), sel_height.max(20.0)) } - fn scroll_to_cursor(&mut self, cx: &mut Cx2d) { + fn scroll_to_cursor(&mut self, cx: &mut Cx2d, content_clip_index: Option) { // Compute the final size of the turtle, and obtain its inner height. + // For multiline inputs, also clamp to the tightest ancestor max height, + // so that scrolling kicks in even when the TextInput's own walk height + // is unbounded Fit (relying on ancestors for the constraint). cx.compute_final_size(); - let height = cx.turtle().inner_rect().size.y; + if self.is_multiline { + let ancestor_max = cx.compute_max_height_from_ancestors(); + if ancestor_max < f64::MAX { + let turtle = cx.turtle_mut(); + if turtle.height() > ancestor_max { + turtle.set_height(ancestor_max); + } + } + } + let inner_rect = cx.turtle().inner_rect(); + let height = inner_rect.size.y; - // Compute the min and max y of the row that the cursor is on. - let laidout_text = self.laidout_text.as_ref().unwrap(); - let laidout_text_height = laidout_text.size_in_lpxs.height as f64; - let position = self.cursor_to_position(self.cursor()).unwrap(); - let laidout_row = &laidout_text.rows[position.row_index]; - let y_min = (laidout_row.origin_in_lpxs.y - laidout_row.ascender_in_lpxs) as f64; - let y_max = (laidout_row.origin_in_lpxs.y - laidout_row.descender_in_lpxs) as f64; + // Only auto-scroll to keep the cursor visible when the cursor has actually + // moved (typing, arrow keys, clicking). Don't do this on every redraw, as + // that would fight with user-initiated mouse wheel scrolling. + if self.needs_scroll_to_cursor { + self.needs_scroll_to_cursor = false; - // If the min y of the row is less than the scroll position, scroll up so that the top of - // the row appears at the top. - if y_min < self.scroll_y { - self.scroll_y = y_min; - } + let laidout_text = self.laidout_text.as_ref().unwrap(); + let position = self.cursor_to_position(self.cursor()).unwrap(); + let laidout_row = &laidout_text.rows[position.row_index]; + let y_min = (laidout_row.origin_in_lpxs.y - laidout_row.ascender_in_lpxs) as f64; + let y_max = (laidout_row.origin_in_lpxs.y - laidout_row.descender_in_lpxs) as f64; + + // If the min y of the row is less than the scroll position, scroll up so + // that the top of the row appears at the top. + if y_min < self.scroll_y { + self.scroll_y = y_min; + } - // If the max y of the row is greater than the scroll position, scroll down so that the - // bottom of the row appears at the bottom. - if y_max > self.scroll_y + height { - self.scroll_y = y_max - height; + // If the max y of the row is greater than the scroll position, scroll down + // so that the bottom of the row appears at the bottom. + if y_max > self.scroll_y + height { + self.scroll_y = y_max - height; + } } - // Clamp the scroll position so that we cannot scroll past the start or end of the text. - let max_scroll_y = laidout_text_height.max(height) - height; + // Always clamp the scroll position to valid bounds, and cache + // max_scroll_y so the event handler uses the exact same value + // (avoiding floating-point mismatch with relative Fit bounds). + let laidout_text_height = self.laidout_text.as_ref().unwrap().size_in_lpxs.height as f64; + let max_scroll_y = (laidout_text_height - height).max(0.0); + self.cached_max_scroll_y = max_scroll_y; self.scroll_y = self.scroll_y.max(0.0).min(max_scroll_y); // Shift the align range of the turtle with the scroll position, but do not include the @@ -968,6 +1027,33 @@ impl TextInput { }, dvec2(0.0, -self.scroll_y), ); + + // Update the content clip rect AFTER shift_align_range, because the shift + // also moves BeginClip entries. By setting the clip to inner_rect here, + // we override whatever shift was applied, keeping the clip at the correct + // absolute position (the inner area excluding padding). + if let Some(clip_index) = content_clip_index { + cx.update_clip_rect_at(clip_index, inner_rect); + } + } + + /// Draws the vertical scrollbar when the text content overflows the visible area. + fn draw_scroll_bar(&mut self, cx: &mut Cx2d) { + if !self.is_multiline { + return; + } + let Some(laidout_text) = self.laidout_text.as_ref() else { + return; + }; + let view_rect = cx.turtle().inner_rect(); + let view_total = dvec2( + view_rect.size.x, + laidout_text.size_in_lpxs.height as f64, + ); + // Sync scroll_y (which scroll_to_cursor may have updated) into the scrollbar. + self.scroll_bar.set_scroll_pos_no_action(cx, self.scroll_y); + self.scroll_bar + .draw_scroll_bar(cx, ScrollAxis::Vertical, view_rect, view_total); } /// Moves the cursor one column to the left. @@ -1287,6 +1373,7 @@ impl TextInput { fn apply_edit(&mut self, cx: &mut Cx, edit: Edit) { self.selection.cursor.index = edit.start + edit.replace_with.len(); self.selection.anchor.index = self.selection.cursor.index; + self.needs_scroll_to_cursor = true; self.history.apply_edit(edit, &mut self.text); self.laidout_text = None; self.check_text_is_empty(cx); @@ -1296,6 +1383,7 @@ impl TextInput { if let Some(new_selection) = self.history.undo(self.selection, &mut self.text) { self.laidout_text = None; self.selection = new_selection; + self.needs_scroll_to_cursor = true; self.check_text_is_empty(cx); true } else { @@ -1307,6 +1395,7 @@ impl TextInput { if let Some(new_selection) = self.history.redo(self.selection, &mut self.text) { self.laidout_text = None; self.selection = new_selection; + self.needs_scroll_to_cursor = true; self.check_text_is_empty(cx); true } else { @@ -1382,12 +1471,31 @@ impl Widget for TextInput { self.draw_bg.begin(cx, walk, self.layout); self.draw_selection.append_to_draw_call(cx); self.draw_composition_underline.append_to_draw_call(cx); + // For multiline inputs, push an inner clip rect to prevent scrolled text + // from bleeding into the padding area. We use a placeholder rect here + // (inner_origin with large size); scroll_to_cursor will tighten the + // bottom bound after compute_final_size determines the actual height. + let content_clip_index = if self.is_multiline { + let inner_origin = cx.turtle().inner_origin(); + Some(cx.push_clip_rect_tracked(rect( + inner_origin.x, + inner_origin.y, + f64::MAX, + f64::MAX, + ))) + } else { + None + }; self.layout_text(cx); let text_rect = self.draw_text(cx); let cursor_rect = self.draw_cursor(cx, text_rect); self.draw_selection(cx, text_rect); self.draw_composition_underline(cx, text_rect); - self.scroll_to_cursor(cx); + self.scroll_to_cursor(cx, content_clip_index); + if content_clip_index.is_some() { + cx.pop_clip_rect(); + } + self.draw_scroll_bar(cx); self.draw_bg.end(cx); if cx.has_key_focus(self.draw_bg.area()) { if self.ime_update_frame != cx.redraw_id() { @@ -1463,6 +1571,55 @@ impl Widget for TextInput { } } + // Handle scrollbar events for multiline text inputs. + if self.is_multiline { + // Handle mouse wheel / trackpad scroll directly. + // Makepad convention: positive scroll.y = viewport moves down = scroll_pos increases. + // self.scroll_y is the single source of truth; the ScrollBar is synced from it + // during draw_scroll_bar, so we don't update the ScrollBar here (which would + // trigger next_frame callbacks and cause feedback loops). + if let Event::Scroll(e) = event { + let bg_rect = self.draw_bg.area().rect(cx); + if !e.handled_y.get() && bg_rect.contains(e.abs) { + // Use the cached max_scroll_y from the last draw pass to ensure + // boundary checks match exactly, avoiding floating-point mismatch + // with relative Fit bounds. + let max_scroll_y = self.cached_max_scroll_y; + if max_scroll_y > 0.0 { + let new_scroll_y = (self.scroll_y + e.scroll.y) + .max(0.0) + .min(max_scroll_y); + if new_scroll_y != self.scroll_y { + self.scroll_y = new_scroll_y; + self.draw_bg.redraw(cx); + e.handled_y.set(true); + } + } + } + } + + // Handle clicking/dragging on the scrollbar handle itself. + // We pass an empty callback because we sync scroll_y from the ScrollBar + // below, only when the scrollbar has actually captured the finger. + self.scroll_bar.handle_event_with( + cx, + event, + &mut |_, _| {}, + ); + + // If the scrollbar has captured the finger (user is dragging the handle), + // sync scroll_y from the ScrollBar (which is the source of truth during + // drag). + if self.scroll_bar.is_area_captured(cx) { + self.scroll_y = self.scroll_bar.get_scroll_pos(); + self.draw_bg.redraw(cx); + } + } + + // Skip finger event processing if the scrollbar owns the finger, + // so dragging the scrollbar doesn't also move the text cursor. + let scrollbar_captured = self.is_multiline && self.scroll_bar.is_area_captured(cx); + match event.hits(cx, self.draw_bg.area()) { Hit::FingerHoverIn(_) => { cx.set_cursor(MouseCursor::Text); @@ -1593,7 +1750,7 @@ impl Widget for TextInput { tap_count, device, .. - }) if device.is_primary_hit() => { + }) if device.is_primary_hit() && !scrollbar_captured => { self.reset_blink_timer(cx); self.set_key_focus(cx); let rel = abs - self.text_area.rect(cx).pos; @@ -1705,7 +1862,7 @@ impl Widget for TextInput { tap_count, device, .. - }) if device.is_primary_hit() => { + }) if device.is_primary_hit() && !scrollbar_captured => { // Skip first move after long press to prevent selection changes if self.ignore_next_move { self.ignore_next_move = false; @@ -1863,6 +2020,7 @@ impl Widget for TextInput { let sel_start_byte = full_state.selection.start.to_byte_index(&self.text); let sel_end_byte = full_state.selection.end.to_byte_index(&self.text); + self.needs_scroll_to_cursor = true; self.selection = Selection { anchor: Cursor { index: sel_start_byte, @@ -2098,6 +2256,20 @@ impl Widget for TextInput { } impl TextInputRef { + pub fn is_multiline(&self) -> bool { + if let Some(inner) = self.borrow() { + inner.is_multiline() + } else { + false + } + } + + pub fn set_is_multiline(&self, cx: &mut Cx, is_multiline: bool) { + if let Some(mut inner) = self.borrow_mut() { + inner.set_is_multiline(cx, is_multiline); + } + } + pub fn is_password(&self) -> bool { if let Some(inner) = self.borrow() { inner.is_password() diff --git a/widgets/src/window.rs b/widgets/src/window.rs index 3a8fed6cfd..748441f5a5 100644 --- a/widgets/src/window.rs +++ b/widgets/src/window.rs @@ -32,7 +32,10 @@ script_mod! { flow: Right draw_bg.color: theme.color_app_caption_bar - height: 27 + // Note: by default, the caption bar height is calculated at runtime + // based on window chrome button geometry to ensure the buttons are vertically centered. + // If you want to override this height with a fixed value, set the `caption_bar_height_override` on the Window itself. + height: Fit caption_label := View { width: Fill height: Fill align: Center @@ -42,9 +45,30 @@ script_mod! { windows_buttons := View { visible: false width: Fit height: Fit - min := DesktopButton {draw_bg.button_type: DesktopButtonType.WindowsMin width: 46 height: 29} - max := DesktopButton {draw_bg.button_type: DesktopButtonType.WindowsMax width: 46 height: 29} - close := DesktopButton {draw_bg.button_type: DesktopButtonType.WindowsClose width: 46 height: 29} + min := DesktopButton { + draw_bg.button_type: DesktopButtonType.WindowsMin + width: 46 height: 29 + draw_bg +: { + color: #000, color_hover: #000, color_down: #000 + bg_color_hover: #E9E9E9, bg_color_down: #CCCCCC + } + } + max := DesktopButton { + draw_bg.button_type: DesktopButtonType.WindowsMax + width: 46 height: 29 + draw_bg +: { + color: #000, color_hover: #000, color_down: #000 + bg_color_hover: #E9E9E9, bg_color_down: #CCCCCC + } + } + close := DesktopButton { + draw_bg.button_type: DesktopButtonType.WindowsClose + width: 46 height: 29 + draw_bg +: { + color: #000, color_hover: #FFF, color_down: #FFF + bg_color_hover: #E81123, bg_color_down: #F1707A + } + } } web_fullscreen := View { visible: false @@ -196,6 +220,11 @@ pub struct Window { show_performance_view: bool, #[rust] has_focus: bool, + /// The calculated value of the caption bar height, a value that will result in + /// the window chrome buttons being nicely vertically centered within the caption bar. + /// `None` means no geometry has been reported by the platform yet. + #[rust] + system_caption_bar_height: Option, #[deref] view: View, @@ -222,9 +251,6 @@ pub enum WindowAction { impl Window { fn sync_caption_bar_state(&mut self, cx: &mut Cx) { - let linux_custom_window_chrome = - matches!(cx.os_type(), OsType::LinuxWindow(params) if params.custom_window_chrome); - match cx.os_type() { OsType::Windows => { self.view(cx, ids!(caption_bar)) @@ -232,13 +258,20 @@ impl Window { self.view(cx, ids!(windows_buttons)).set_visible(cx, true); } OsType::Macos => { + // In macOS fullscreen, the OS provides its own auto-hiding + // toolbar with traffic-light buttons, so hide our caption bar. + let is_fullscreen = self.window.handle.is_fullscreen(cx); self.view(cx, ids!(caption_bar)) - .set_visible(cx, self.show_caption_bar); + .set_visible(cx, self.show_caption_bar && !is_fullscreen); } - OsType::LinuxWindow(_) => { + OsType::LinuxWindow(params) => { + // Only show the caption bar if we're drawing our own window chrome + // (e.g. Wayland without server-side decorations). On X11 the WM + // provides native decorations, so we hide the in-app caption bar. + let custom_chrome = params.custom_window_chrome; self.view(cx, ids!(caption_bar)) - .set_visible(cx, self.show_caption_bar); - if linux_custom_window_chrome { + .set_visible(cx, self.show_caption_bar && custom_chrome); + if custom_chrome { self.view(cx, ids!(windows_buttons)).set_visible(cx, true); } } @@ -252,6 +285,45 @@ impl Window { } } + fn sync_caption_bar_height(&mut self, cx: &mut Cx) { + // Explicit DSL override takes priority, then system-calculated. + let height = self.window.caption_bar_height_override + .or(self.system_caption_bar_height); + if let Some(h) = height { + let caption_bar = self.view(cx, ids!(caption_bar)); + if let Some(mut bar) = caption_bar.borrow_mut() { + bar.walk.height = Size::Fixed(h); + } + drop(caption_bar); + } + } + + /// Adjusts the caption label's left padding so that the title text appears + /// centered in the full caption bar width when there's enough room. + /// When the window is too narrow, the padding gracefully reduces to 0, + /// transitioning to a left-aligned title. + fn sync_caption_centering(&mut self, cx: &mut Cx) { + let bar_width = self.view(cx, ids!(caption_bar)).area().rect(cx).size.x; + let buttons_width = self.view(cx, ids!(windows_buttons)).area().rect(cx).size.x; + + if bar_width <= 0.0 { + return; // No area info yet (first frame) + } + + let fill_width = bar_width - buttons_width; + // At wide widths: padding = buttons_width, so the label's center + // aligns with the bar's center (truly centered). + // At narrow widths: padding shrinks toward 0, so the title + // shifts left to maximize the available text space. + let padding_left = buttons_width.min((fill_width - buttons_width).max(0.0)); + + let caption_label = self.view(cx, ids!(caption_label)); + if let Some(mut inner) = caption_label.borrow_mut() { + inner.layout.padding.left = padding_left; + } + drop(caption_label); + } + fn sync_caption_title(&mut self, cx: &mut Cx) { let title = if self.window.title.is_empty() { cx.windows[self.window.handle.window_id()] @@ -268,7 +340,9 @@ impl Window { fn ensure_initialized(&mut self, cx: &mut Cx) { self.sync_caption_bar_state(cx); + self.sync_caption_bar_height(cx); self.sync_caption_title(cx); + self.sync_caption_centering(cx); if self.initialized { return; @@ -537,9 +611,33 @@ impl Widget for Window { } // Update the display context if the screen size has changed + let old_insets = cx.display_context.safe_area_insets; cx.display_context.screen_size = ev.new_geom.inner_size; + cx.display_context.safe_area_insets = ev.new_geom.safe_area_insets; cx.display_context.updated_on_event_id = cx.event_id(); + // Update safe area inset values on the script heap so + // Splash code can reference mod.widgets.SAFE_INSET_PAD_*. + cx.update_safe_inset_script_values(ev.new_geom.safe_area_insets); + + // If the platform reports native chrome button geometry, derive + // the caption bar height so the buttons are vertically centered: + // height = top_margin * 2 + button_height = pos.y * 2 + size.y. + let new_buttons = ev.new_geom.window_chrome_buttons; + if new_buttons != Rect::default() { + let h = (new_buttons.pos.y * 2.0 + new_buttons.size.y).ceil(); + if self.system_caption_bar_height != Some(h) { + self.system_caption_bar_height = Some(h); + self.view(cx, ids!(caption_bar)).redraw(cx); + } + } + + // If safe area insets changed, trigger a script re-apply + // so widgets pick up new values. + if old_insets != ev.new_geom.safe_area_insets { + cx.request_script_reapply(); + } + cx.widget_action(uid, WindowAction::WindowGeomChange(ev.clone())); return; } @@ -548,19 +646,17 @@ impl Widget for Window { Event::WindowDragQuery(dq) => { if dq.window_id == self.window.window_id() { if self.view(cx, ids!(caption_bar)).visible() { - let size = self.window.get_inner_size(cx); + let caption_rect = self.view(cx, ids!(caption_bar)).area().rect(cx); + let buttons_rect = self.view(cx, ids!(windows_buttons)).area().rect(cx); - if dq.abs.y < 25. { - if dq.abs.x < size.x - 250.0 { - dq.response.set(WindowDragQueryResponse::Caption); - } else { + if caption_rect.contains(dq.abs) { + if buttons_rect.size != Vec2d::default() && buttons_rect.contains(dq.abs) { dq.response.set(WindowDragQueryResponse::Client); + } else { + dq.response.set(WindowDragQueryResponse::Caption); } cx.set_cursor(MouseCursor::Default); } - /* - if dq.abs.x < self.caption_size.x && dq.abs.y < self.caption_size.y { - }*/ } } true @@ -662,4 +758,4 @@ impl Widget for Window { DrawStep::done() } -} +} \ No newline at end of file diff --git a/xr/Cargo.toml b/xr/Cargo.toml index f7df26b907..41d39107a5 100644 --- a/xr/Cargo.toml +++ b/xr/Cargo.toml @@ -6,8 +6,12 @@ edition = "2021" description = "Reusable XR scene components for Makepad" license = "MIT OR Apache-2.0" +[features] +lz4_force_scalar = ["makepad-lz4/force-scalar"] + [dependencies] +makepad-lz4 = { path = "../libs/lz4", version = "0.1.0" } makepad-widgets = { path = "../widgets", version = "2.0.0" } makepad-gltf = { path = "../libs/gltf", version = "1.0.0" } makepad-splat = { path = "../libs/splat", version = "1.0.0" } -rapier3d = { path = "../libs/rapier/crates/rapier3d", default-features = false, features = ["dim3", "f32", "enhanced-determinism"] } +rapier3d = { path = "../libs/rapier/crates/rapier3d", default-features = false, features = ["dim3", "f32"] } diff --git a/xr/src/3d.md b/xr/src/3d.md new file mode 100644 index 0000000000..4dd02b28ab --- /dev/null +++ b/xr/src/3d.md @@ -0,0 +1,230 @@ +# `xr/src` Refactor Plan + +## Goal + +Remove `XrScene` and rebuild `XrRoot` as the single scene host. Add `XrView` for hosting 2D UI inside 3D space. + +## Current State (before refactor) + +``` +App + └─ XrRoot (shell: desktop preview, XR mode, panel placement, orbit camera) + ├─ XrEnv (environment draws: depth mesh, passthrough, PBR, env cube) + ├─ XrScene (scene host: physics, XrNode tree walk, runtime bodies) + │ └─ XrNode children (Cube, Gltf, Tree, RefractiveCube, …) + └─ controls widget (2D UI panel rendered into 3D via panel_matrix) +``` + +`XrScene` sits between `XrRoot` and the content. It owns: +- Rapier physics scene (`RapierScene`) +- runtime body state (`runtime_bodies`) +- cube collection from the widget tree +- physics step loop (both desktop preview and XR) +- `draw_3d` delegation to child nodes + +`XrRoot` owns: +- desktop orbit camera + presentation state +- XR panel matrix computation + XR interaction routing +- scene switching (template_widgets, scene_order, active_scene) +- pass/window setup +- `XrEnv` (which does env draws but also calls into `XrScene` for physics state) + +## Target State (after refactor) + +``` +App + └─ XrRoot (scene host + shell) + ├─ XrEnv (owned, not a child widget — provides physics + env draws) + ├─ named children drawn in order (Cube, Gltf, Tree, XrView, …) + └─ z-sorted primary drawlist +``` + +### XrScene: removed + +All of `XrScene`'s responsibilities move into `XrRoot`: +- Physics ownership (`RapierScene`, runtime bodies) → `XrRoot` via `XrEnv` +- Cube collection + `rebuild_runtime_scene` → `XrRoot` or `XrEnv` +- `draw_3d` delegation → `XrRoot` draws children directly +- `on_render` / `script_call(render)` → `XrRoot` calls render on itself + +### XrRoot: rebuilt + +`XrRoot` becomes the single scene host. It: + +1. **Owns an `XrEnv`** (not as a child widget, but as an owned struct field). `XrEnv` provides: + - Physics scene (Rapier): create, step, sync bodies + - Environment draws: depth mesh, passthrough env atlas, PBR setup, env cube + - Hand rendering (optional debug geometry) + +2. **Has named children** via `:=` syntax in script_mod. These are the scene content widgets (Cube, Gltf, Tree, XrView, etc.). They are drawn in declaration order. + +3. **Z-sorts its primary drawlist**. After all children have drawn into the 3D drawlist, `XrRoot` sorts draw calls by distance from camera before submitting. This gives correct transparency ordering without per-widget sorting. + +4. **Keeps existing shell duties**: + - Desktop orbit camera + preview pass + - XR mode pass setup + - XR panel matrix for UI overlay + - Scene switching (if we keep multi-scene support, or simplify to single scene) + +### XrView: new widget + +`XrView` hosts a 2D makepad UI inside 3D space. It: + +- Has an `XrNode` (position, rotation, scale in the scene) +- Creates a `Cx2d` from the `Cx3d` and uses a `DrawList2d` with the right view transform matrix (computed from the node's world pose + pixel scale). No offscreen texture — the 2D draw calls go directly into the 3D pass with a transformed draw list. +- Routes XR finger-tip events into 2D hit-testing on the plane +- Replaces the current `controls` panel mechanism in `XrRoot` (which was hardcoded panel_matrix logic) + +In the example app (`examples/xr/src/main.rs`), `XrView` is used like: +``` +block_ctrl := XrView{ + pos: vec3(-0.92, 0.42, -0.62) + look_at: vec3(-0.10, 0.32, -0.62) + logical_size: vec2(320, 400) + pixel_scale: 0.0004 + RoundedView{ + // 2D UI content — drawn directly into the 3D pass + // via a DrawList2d with the right view transform + } +} +``` + +## Refactor Steps + +### Step 1: Move physics into XrEnv + +`XrEnv` already owns the draw objects (draw_cube, draw_pbr, draw_depth_mesh, draw_passthrough_env_atlas). Add to it: +- `RapierScene` (currently on `XrScene`) +- `runtime_bodies` (currently on `XrScene`) +- `collect_cubes_from_widget` + `rebuild_runtime_scene` (currently on `XrScene`) +- Physics step + sync (currently split between `XrScene::handle_event` and `XrEnv::prepare_draw_scope`) + +`XrEnv` becomes the complete scene-environment owner: physics + rendering environment. + +### Step 2: Remove XrScene + +- Delete `scene/xr_scene.rs` +- Remove from `lib.rs` module list and `script_mod` registration +- Remove `pub use xr_scene::XrScene` + +### Step 3: Rebuild XrRoot from scratch + +The current `XrRoot` is ~1300 lines of tangled concerns. Nuke it and write a clean one. + +**What XrRoot IS**: a Window that sets up a 3D pass with a camera, draws its named children into that pass, and z-sorts the result. That's it. + +**What XrRoot is NOT**: a scene switcher, a panel hit-tester, a controls host, an orbit camera manager, or an XR interaction router. Those things either move to `XrView` or become app-level concerns. + +Clean `XrRoot` struct fields: +```rust +pub struct XrRoot { + // Widget basics + #[walk] walk: Walk, + #[layout] layout: Layout, + #[rust] area: Area, + + // Scene environment (physics + env draws) + #[live] env: XrEnv, + + // Pass / window (XrRoot IS a window) + #[live] window: ScriptWindowHandle, + #[live] pass: ScriptDrawPass, + #[new] depth_texture: Texture, + #[new] main_draw_list: DrawList2d, + #[new] xr_draw_list: DrawList, + + // Desktop camera (simple orbit) + #[live(28.0)] camera_fov_y: f32, + #[live(3.4)] camera_distance: f32, + #[live(0.05)] camera_near: f32, + #[live(200.0)] camera_far: f32, + #[rust(0.0)] orbit_yaw: f32, + #[rust(0.45)] orbit_pitch: f32, + + // Children (named widgets from := declarations) + #[rust] children: Vec<(LiveId, WidgetRef)>, +} +``` + +Clean `XrRoot` behavior: +1. `on_after_apply`: collect `:=` children into `self.children` vec (preserving order) +2. `handle_event`: + - Forward events to `env` and all children + - Handle desktop orbit (mouse drag on background = orbit, scroll = zoom) + - On `Event::XrUpdate`: forward to env for physics step +3. `draw_walk` (desktop path): + - Set up pass with orbit camera projection + - Call `env.prepare_draw(cx)` for env rendering + - Iterate children, call `draw_3d_all` on each + - Z-sort the drawlist +4. `draw_3d` (XR path): + - Same but camera comes from XR headset pose + +**Things that are GONE from XrRoot**: +- `controls` / `controls_widget()` — use XrView instead +- `scene` / `active_scene` / `scene_order` / `switch_scene` / `select_scene` — app toggles `visible` on children +- `template_widget()` / `active_scene_widget()` — just iterate `self.children` +- `xr_panel_pixels` / `xr_dpi_factor` / `xr_pixel_scale` / `xr_depth_scale` — moves to XrView +- `xr_position_offset` / `xr_forward_offset` / `desktop_panel_position` / `desktop_panel_look_at` — moves to XrView +- `compute_panel_matrix_from_pose` / `panel_pose_from_head_pose` — moves to XrView +- `desktop_panel_hit` / `desktop_panel_contains` / `desktop_panel_active` — moves to XrView +- `make_desktop_xr_local_event` / `dispatch_desktop_xr_local_event` — moves to XrView +- `xr_toggle_with_menu` / `xr_visible` — per-widget concern (XrView or app-level) +- `scene_anchor_to_head` / `scene_anchor_*` — moves to XrEnv or app-level +- `xr_view_matrix` / `xr_hit_matrix` / `xr_view_matrix_initialized` — moves to XrView +- All the `xr_panel_log_count` / `xr_draw_logged` / `scene_anchor_log_count` debug noise +- `DesktopPresentationState` struct — simplified inline +- `DrawState` enum — unnecessary for a simple single-pass draw + +The desktop orbit camera is ~40 lines. The pass setup is ~30 lines. The child iteration is ~15 lines. The whole file should be ~200-300 lines, not 1300. + +### Step 4: Create XrView + +New file `obj/xr_view.rs`: +- Struct with `XrNode` (for 3D placement), `DrawList2d`, pixel scale, logical size +- `draw_3d`: creates `Cx2d` from `Cx3d`, sets the draw list's view transform matrix (node world pose × pixel-to-world scale), draws 2D children into it with a root turtle at the logical size. No offscreen texture — draw calls go straight into the 3D pass. +- `handle_event`: converts XrLocal finger-tip events into 2D coordinates on the plane, forwards to 2D children. Also handles desktop mouse ray-casting for the preview. + +### Step 5: Update examples/xr/src/main.rs + +- Remove `XrScene` wrapping +- Place content directly as `XrRoot` children +- Replace the old `controls` panel with `XrView` +- Scene switching becomes toggling `visible` on child nodes + +## Open Questions + +- **Z-sort granularity**: Sort at the draw-call level within the DrawList? Or just sort the order children are drawn? Draw-call level is more correct for intersecting geometry but may need DrawList API support. Child-level is simpler and probably good enough. + +- **XrView desktop interaction**: XrView needs to handle both XR finger-tips AND desktop mouse ray-casting (for the desktop preview). The ray-cast math is straightforward (project mouse through orbit camera, intersect with the quad plane). Should XrView own this, or should XrRoot provide a helper that converts mouse events into ray events that XrView consumes? + +## Directory Layout (after refactor) + +``` +xr/src/ +├── obj/ +│ ├── cube.rs +│ ├── gltf.rs +│ ├── physics_view.rs +│ ├── refractive_cube.rs +│ ├── tree.rs +│ ├── view_splat.rs +│ └── xr_view.rs ← NEW +├── scene/ +│ ├── xr_depth.rs +│ ├── xr_env.rs ← expanded (+ physics) +│ ├── xr_hands.rs +│ ├── xr_node.rs +│ ├── xr_passthrough.rs +│ ├── xr_permissions_flow.rs +│ ├── xr_physics.rs +│ └── xr_root.rs ← rebuilt +├── util/ +│ ├── gltf_bridge.rs +│ ├── mesh_generators.rs +│ ├── passthrough_env.rs +│ └── scene_draw.rs +└── lib.rs +``` + +Removed: `scene/xr_scene.rs` diff --git a/xr/src/algorithms/depth_align.rs b/xr/src/algorithms/depth_align.rs new file mode 100644 index 0000000000..ae98d096ce --- /dev/null +++ b/xr/src/algorithms/depth_align.rs @@ -0,0 +1,4228 @@ +use crate::prelude::*; +use makepad_widgets::makepad_platform::makepad_micro_serde::*; +use std::collections::HashMap; + +#[derive(Clone, Copy, Debug, Default, PartialEq, Eq, Hash, SerBin, DeBin)] +pub enum XrDepthAlignSampleKind { + Floor, + Wall, + #[default] + Unknown, +} + +#[derive(Clone, Copy, Debug, Default, PartialEq, SerBin, DeBin)] +pub struct XrDepthAlignSample { + pub kind: XrDepthAlignSampleKind, + pub point: Vec3f, + pub normal: Vec3f, + pub weight: f32, +} + +#[derive(Clone, Debug, Default, PartialEq)] +pub struct XrDepthAlignSlicePreview { + pub height_map: XrDepthAlignHeightMap, + pub cutout_center: Option, + pub cutout_forward: Option, + pub cutout_radius_meters: f32, +} + +impl XrDepthAlignSlicePreview { + pub fn from_tsdf_snapshot(snapshot: &TsdfPublishedSnapshot) -> Option { + let height_map = snapshot.height_map.clone()?; + Some(Self { + cutout_center: height_map.player_cutout_center, + cutout_forward: None, + cutout_radius_meters: height_map.player_cutout_radius_meters, + height_map, + }) + } +} + +#[derive(Clone, Debug, Default, PartialEq, SerBin, DeBin)] +pub struct XrDepthAlignVerticalDescriptor { + pub origin_x: f32, + pub origin_z: f32, + pub cell_size_meters: f32, + pub size: u16, + pub vertical_surface_masks: Vec, + pub clutter_surface_masks: Vec, + pub free_space_masks: Vec, + pub height_u8: Vec, +} + +impl XrDepthAlignVerticalDescriptor { + pub fn cell_count(&self) -> usize { + self.size as usize * self.size as usize + } + + pub fn is_empty(&self) -> bool { + self.size == 0 + || self.vertical_surface_masks.len() != self.cell_count() + || self.clutter_surface_masks.len() != self.cell_count() + || self.free_space_masks.len() != self.cell_count() + || self.height_u8.len() != self.cell_count() + || self + .vertical_surface_masks + .iter() + .zip(self.clutter_surface_masks.iter()) + .zip(self.free_space_masks.iter()) + .zip(self.height_u8.iter()) + .all(|(((vertical, clutter), free), height)| { + *vertical == 0 && *clutter == 0 && *free == 0 && *height == 0 + }) + } +} + +#[derive(Clone, Debug, Default, PartialEq, SerBin, DeBin)] +pub struct XrDepthAlignDescriptor { + pub voxel_size_meters: f32, + pub floor_y: f32, + pub wall_normal_histogram: Vec, + pub samples: Vec, + pub vertical_descriptor: Option, + pub height_map: Option, +} + +impl XrDepthAlignDescriptor { + pub fn build_wall_normal_histogram( + samples: &[XrDepthAlignSample], + bin_count: usize, + ) -> Vec { + let bin_count = bin_count.max(1); + let mut histogram = vec![0.0; bin_count]; + for sample in samples { + if sample.kind != XrDepthAlignSampleKind::Wall { + continue; + } + let Some(axis) = xz_axis(sample.normal) else { + continue; + }; + let angle = axis.x.atan2(-axis.z); + let normalized = (angle + std::f32::consts::PI) / std::f32::consts::TAU; + let bin = (normalized * bin_count as f32).floor() as isize; + histogram[bin.rem_euclid(bin_count as isize) as usize] += sample.weight.max(0.01); + } + normalize_histogram(&mut histogram); + histogram + } + + pub fn transformed(&self, transform: &Mat4f) -> Self { + let transform_dir = |dir: Vec3f| { + transform + .transform_vec4(vec4f(dir.x, dir.y, dir.z, 0.0)) + .to_vec3f() + }; + let mut descriptor = self.clone(); + for sample in &mut descriptor.samples { + sample.point = transform + .transform_vec4(vec4f(sample.point.x, sample.point.y, sample.point.z, 1.0)) + .to_vec3f(); + sample.normal = + align_safe_normalize(transform_dir(sample.normal)).unwrap_or(sample.normal); + } + descriptor.floor_y = transform + .transform_vec4(vec4f(0.0, descriptor.floor_y, 0.0, 1.0)) + .to_vec3f() + .y; + descriptor.vertical_descriptor = descriptor + .vertical_descriptor + .as_ref() + .and_then(|vertical| transform_vertical_descriptor(vertical, transform)); + descriptor.height_map = descriptor + .height_map + .as_ref() + .and_then(|height_map| transform_height_map(height_map, transform)); + descriptor.wall_normal_histogram = + if !descriptor.samples.is_empty() && !descriptor.wall_normal_histogram.is_empty() { + Self::build_wall_normal_histogram( + &descriptor.samples, + descriptor.wall_normal_histogram.len(), + ) + } else { + Vec::new() + }; + descriptor + } + + pub fn test_markers(&self) -> Option<[Vec3f; 2]> { + let signal = selected_descriptor_signal_cells(self); + let mut best = None::<(f32, f32, Vec3f, Vec3f)>; + for (index, first) in signal.iter().enumerate() { + for second in signal.iter().skip(index + 1) { + let distance = (second.point - first.point).length(); + if distance < 0.18 { + continue; + } + let weight = first.weight + second.weight; + if best + .as_ref() + .is_none_or(|(best_distance, best_weight, _, _)| { + distance > *best_distance + 1.0e-4 + || ((distance - *best_distance).abs() <= 1.0e-4 + && weight > *best_weight) + }) + { + best = Some((distance, weight, first.point, second.point)); + } + } + } + best.map(|(_, _, first, second)| [first, second]) + } + + pub fn solver<'a>(&'a self, remote: &'a Self) -> XrDepthAlignSolver<'a> { + XrDepthAlignSolver::new(self, remote) + } + + pub fn solve_remote_to_local(&self, remote: &Self) -> Option { + self.solver(remote).solve() + } + + pub fn analyze_remote_to_local(&self, remote: &Self) -> XrDepthAlignSolveDiagnostic { + self.solver(remote).analyze() + } + + pub fn analyze_remote_to_local_seeded( + &self, + remote: &Self, + previous_solution: Option, + ) -> XrDepthAlignSolveDiagnostic { + self.solver(remote).analyze_seeded(previous_solution) + } + + pub fn rescore_remote_to_local( + &self, + remote: &Self, + solution: XrDepthAlignSolution, + ) -> XrDepthAlignSolution { + self.solver(remote).rescore(solution) + } +} + +#[derive(Clone, Copy, Debug, Default, PartialEq, Eq, SerBin, DeBin)] +pub struct XrDepthAlignDebug { + pub near_surface_voxel_count: u32, + pub floor_candidate_count: u32, + pub wall_candidate_count: u32, + pub floor_sample_count: u32, + pub wall_sample_count: u32, +} + +#[derive(Clone, Copy, Debug, Default, PartialEq)] +pub struct XrDepthAlignSolution { + pub yaw_radians: f32, + pub translation: Vec3f, + pub confidence: f32, + pub symmetry_confidence: f32, + pub residual_meters: f32, + pub matched_samples: usize, +} + +impl XrDepthAlignSolution { + pub fn remote_to_local_transform(&self) -> Mat4f { + Pose::new( + Quat::from_axis_angle(vec3(0.0, 1.0, 0.0), self.yaw_radians), + self.translation, + ) + .to_mat4() + } + + pub fn map_point(&self, point: Vec3f) -> Vec3f { + self.remote_to_local_transform() + .transform_vec4(vec4f(point.x, point.y, point.z, 1.0)) + .to_vec3f() + } + + pub fn ranking_confidence(&self) -> f32 { + (self.confidence * (0.30 + 0.70 * self.symmetry_confidence.clamp(0.0, 1.0))).clamp(0.0, 1.0) + } + + pub fn is_accepted(self, diagnostic: &XrDepthAlignSolveDiagnostic) -> bool { + self.matched_samples >= XR_DEPTH_ALIGN_ACCEPT_MIN_MATCHED_SAMPLES + && self.confidence > XR_DEPTH_ALIGN_ACCEPT_MIN_CONFIDENCE + && (!diagnostic.local_vertical_descriptor + || !diagnostic.remote_vertical_descriptor + || self.symmetry_confidence > XR_DEPTH_ALIGN_ACCEPT_MIN_SYMMETRY_CONFIDENCE) + } +} + +#[derive(Clone, Copy, Debug, Default, PartialEq, Eq)] +pub enum XrDepthAlignSolveOutcome { + #[default] + MissingSamples, + NoCandidate, + Rejected, + Accepted, +} + +#[derive(Clone, Copy, Debug, Default, PartialEq)] +pub struct XrDepthAlignSolveDiagnostic { + pub local_vertical_descriptor: bool, + pub remote_vertical_descriptor: bool, + pub local_floor_samples: usize, + pub local_wall_samples: usize, + pub remote_floor_samples: usize, + pub remote_wall_samples: usize, + pub remote_dense_wall_samples: usize, + pub remote_refine_wall_samples: usize, + pub yaw_candidate_count: usize, + pub pose_candidate_count: usize, + pub shortlisted_pose_count: usize, + pub signal_build_ms: u32, + pub yaw_candidate_ms: u32, + pub translation_vote_ms: u32, + pub signal_refine_ms: u32, + pub signal_score_ms: u32, + pub height_refine_ms: u32, + pub final_score_ms: u32, + pub wall_profile_ms: u32, + pub total_compute_ms: u32, + pub step_count: u32, + pub max_step_ms: u32, + pub best_solution: Option, +} + +impl XrDepthAlignSolveDiagnostic { + pub fn accepted_solution(&self) -> Option { + self.best_solution + .filter(|solution| solution.is_accepted(self)) + } + + pub fn outcome(&self) -> XrDepthAlignSolveOutcome { + if self.local_wall_samples < XR_DEPTH_ALIGN_MIN_SIGNAL_SAMPLES + || self.remote_wall_samples < XR_DEPTH_ALIGN_MIN_SIGNAL_SAMPLES + { + return XrDepthAlignSolveOutcome::MissingSamples; + } + if self.best_solution.is_none() { + XrDepthAlignSolveOutcome::NoCandidate + } else if self.accepted_solution().is_some() { + XrDepthAlignSolveOutcome::Accepted + } else { + XrDepthAlignSolveOutcome::Rejected + } + } +} + +#[derive(Clone, Debug, Default, PartialEq)] +pub struct XrDepthAlignPreview { + pub local_markers: Option<[Vec3f; 2]>, + pub remote_markers_local: Option<[Vec3f; 2]>, + pub solution: Option, + pub local_sample_count: usize, + pub local_floor_sample_count: usize, + pub local_wall_sample_count: usize, + pub remote_sample_count: usize, + pub remote_floor_sample_count: usize, + pub remote_wall_sample_count: usize, +} + +#[derive(Clone, Copy, Debug)] +pub struct XrDepthAlignSolver<'a> { + local: &'a XrDepthAlignDescriptor, + remote: &'a XrDepthAlignDescriptor, +} + +impl<'a> XrDepthAlignSolver<'a> { + pub fn new(local: &'a XrDepthAlignDescriptor, remote: &'a XrDepthAlignDescriptor) -> Self { + Self { local, remote } + } + + pub fn matcher(self, previous_solution: Option) -> XrDepthAlignMatcher { + XrDepthAlignMatcher::new(self.local, self.remote, previous_solution) + } + + pub fn solve(self) -> Option { + self.analyze().accepted_solution() + } + + pub fn analyze(self) -> XrDepthAlignSolveDiagnostic { + self.analyze_seeded(None) + } + + pub fn analyze_seeded( + self, + previous_solution: Option, + ) -> XrDepthAlignSolveDiagnostic { + self.matcher(previous_solution).finish() + } + + pub fn rescore(self, solution: XrDepthAlignSolution) -> XrDepthAlignSolution { + let local_signal = selected_descriptor_signal_cells(self.local); + let remote_signal = selected_descriptor_signal_cells(self.remote); + let remote_dense_signal = descriptor_dense_signal_cells(self.remote); + let remote_flat_signal = decimate_signal_cells( + &descriptor_flat_surface_cells(self.remote), + XR_DEPTH_ALIGN_VERTICAL_OFFSET_MAX_FLAT_SAMPLES, + ); + let local_sample_cache = self + .local + .height_map + .as_ref() + .and_then(build_height_map_sample_cache); + if local_signal.is_empty() || remote_signal.is_empty() { + return XrDepthAlignSolution { + yaw_radians: solution.yaw_radians, + translation: solution.translation, + confidence: 0.0, + symmetry_confidence: 0.0, + residual_meters: f32::INFINITY, + matched_samples: 0, + }; + } + let rescored = score_signal_alignment_solution( + &local_signal, + &remote_signal, + solution.yaw_radians, + solution.translation, + ); + let rescored = match ( + self.local.height_map.as_ref(), + self.remote.height_map.as_ref(), + ) { + (Some(local_map), Some(remote_map)) => refine_solution_vertical_offset_from_overlap( + local_map, + remote_map, + &remote_flat_signal, + rescored, + ), + _ => rescored, + }; + apply_height_map_alignment_support( + rescored, + self.local.height_map.as_ref(), + local_sample_cache.as_ref(), + self.remote.height_map.as_ref(), + &remote_dense_signal, + ) + } +} + +const XR_DEPTH_ALIGN_MIN_SIGNAL_SAMPLES: usize = 4; +const XR_DEPTH_ALIGN_ACCEPT_MIN_MATCHED_SAMPLES: usize = 6; +const XR_DEPTH_ALIGN_ACCEPT_MIN_CONFIDENCE: f32 = 0.12; +const XR_DEPTH_ALIGN_ACCEPT_MIN_SYMMETRY_CONFIDENCE: f32 = 0.10; +const XR_DEPTH_ALIGN_TRANSLATION_VOTE_STEP_METERS: f32 = 0.08; +const XR_DEPTH_ALIGN_HEIGHT_MAP_HISTOGRAM_BINS: usize = 48; +const XR_DEPTH_ALIGN_HEIGHT_MAP_MAX_SAMPLES: usize = 96; +const XR_DEPTH_ALIGN_SIGNAL_SHORTLIST_MAX_CANDIDATES: usize = 80; +const XR_DEPTH_ALIGN_SIGNAL_SHORTLIST_PER_YAW: usize = 2; +const XR_DEPTH_ALIGN_SIGNAL_SHORTLIST_YAW_EPSILON_RADIANS: f32 = 1.0_f32.to_radians(); +const XR_DEPTH_ALIGN_SIGNAL_SHORTLIST_TRANSLATION_EPSILON_METERS: f32 = 0.10; +const XR_DEPTH_ALIGN_HEIGHT_REFINE_MAX_DENSE_SAMPLES: usize = 1536; +const XR_DEPTH_ALIGN_HEIGHT_MAP_GRADIENT_MIN_METERS: f32 = 0.05; +const XR_DEPTH_ALIGN_HEIGHT_MAP_MIN_SPACING_METERS: f32 = 0.14; +const XR_DEPTH_ALIGN_VERTICAL_OFFSET_MAX_DELTA_METERS: f32 = 0.80; +const XR_DEPTH_ALIGN_VERTICAL_OFFSET_MIN_MATCHES: usize = 24; +const XR_DEPTH_ALIGN_VERTICAL_OFFSET_MIN_SUPPORT_RATIO: f32 = 0.08; +const XR_DEPTH_ALIGN_VERTICAL_OFFSET_CONSENSUS_RADIUS_METERS: f32 = 0.06; +const XR_DEPTH_ALIGN_VERTICAL_OFFSET_MAX_FLAT_SAMPLES: usize = 4096; +const XR_DEPTH_ALIGN_VERTICAL_SURFACE_MAX_GRADIENT_METERS: f32 = 0.11; +const XR_DEPTH_ALIGN_VERTICAL_SURFACE_MAX_NEIGHBOR_DELTA_METERS: f32 = 0.07; +const XR_DEPTH_ALIGN_VERTICAL_SURFACE_MIN_NEIGHBORS: usize = 7; +const XR_DEPTH_ALIGN_VERTICAL_SURFACE_MAX_RELATIVE_HEIGHT_METERS: f32 = 1.80; +const XR_DEPTH_ALIGN_SIGNAL_MATCH_RADIUS_METERS: f32 = 0.42; +const XR_DEPTH_ALIGN_SIGNAL_MATCH_MIN_DIRECTION_DOT: f32 = 0.45; +const XR_DEPTH_ALIGN_SIGNAL_MATCH_MAX_HEIGHT_DELTA_METERS: f32 = 0.75; +const XR_DEPTH_ALIGN_SEEDED_LOCK_MIN_CONFIDENCE: f32 = 0.20; +const XR_DEPTH_ALIGN_SEEDED_LOCK_MIN_SYMMETRY_CONFIDENCE: f32 = 0.18; +const XR_DEPTH_ALIGN_SEEDED_LOCK_MIN_OVERLAP: f32 = 0.35; +const XR_DEPTH_ALIGN_SEEDED_LOCK_MAX_TRANSLATION_JUMP_METERS: f32 = 0.75; +const XR_DEPTH_ALIGN_SEEDED_LOCK_MAX_YAW_JUMP_RADIANS: f32 = 0.45; +const XR_DEPTH_ALIGN_WALL_PROFILE_MIN_HEIGHT_METERS: f32 = 0.60; +const XR_DEPTH_ALIGN_WALL_PROFILE_MAX_HEIGHT_METERS: f32 = 1.80; +const XR_DEPTH_ALIGN_WALL_PROFILE_CELL_METERS: f32 = 0.09; +const XR_DEPTH_ALIGN_WALL_PROFILE_MIN_POINTS: usize = 16; +const XR_DEPTH_ALIGN_WALL_PROFILE_YAW_WINDOW_RADIANS: f32 = 10.0_f32.to_radians(); +const XR_DEPTH_ALIGN_WALL_PROFILE_YAW_STEP_RADIANS: f32 = 0.5_f32.to_radians(); +const XR_DEPTH_ALIGN_WALL_PROFILE_TRANSLATION_WINDOW_METERS: f32 = 0.18; +const XR_DEPTH_ALIGN_WALL_PROFILE_TRANSLATION_STEP_METERS: f32 = 0.06; +const XR_DEPTH_ALIGN_WALL_PROFILE_TRANSLATION_SIGMA_METERS: f32 = 0.12; +const XR_DEPTH_ALIGN_WALL_PROFILE_MIN_YAW_DELTA_RADIANS: f32 = 1.2_f32.to_radians(); +const XR_DEPTH_ALIGN_WALL_PROFILE_MIN_SCORE_GAIN: f32 = 0.010; +const XR_DEPTH_ALIGN_WALL_PROFILE_MIN_SCORE_RATIO: f32 = 1.02; +const XR_DEPTH_ALIGN_WALL_PROFILE_MIN_CONFIDENCE_RATIO: f32 = 0.82; +const XR_DEPTH_ALIGN_WALL_PROFILE_MAX_RESIDUAL_INCREASE_METERS: f32 = 0.08; +const XR_DEPTH_ALIGN_WALL_PROFILE_MAX_MATCH_LOSS: usize = 6; + +#[derive(Clone, Copy, Debug)] +struct HeightMapSignalCell { + point: Vec3f, + height: f32, + gradient: Vec3f, + axis_xz: Vec2f, + weight: f32, +} + +#[derive(Clone, Copy, Debug)] +struct DenseHeightMapSignalCell { + point_x: f32, + point_z: f32, + height: f32, + axis_x: f32, + axis_z: f32, + weight: f32, +} + +#[derive(Clone, Copy, Debug)] +struct FlatHeightMapSurfaceCell { + point_x: f32, + point_z: f32, + height: f32, + weight: f32, +} + +#[derive(Clone, Copy, Debug)] +struct RotatedHeightMapSignalCell { + point_x: f32, + point_z: f32, + axis_x: f32, + axis_z: f32, + height: f32, + weight: f32, +} + +#[derive(Clone, Copy, Debug)] +struct ProjectionBounds { + min: f32, + max: f32, +} + +#[derive(Clone, Debug)] +struct HeightMapSampleCache { + origin_x: f32, + origin_z: f32, + inv_cell_size_meters: f32, + size_x: usize, + size_z: usize, + signal_axes: Vec, +} + +fn duration_ms_u32(duration: std::time::Duration) -> u32 { + duration.as_millis().min(u32::MAX as u128) as u32 +} + +const XR_DEPTH_ALIGN_HEIGHT_REFINE_PHASES: [(f32, f32); 5] = [ + (0.18, 0.36), + (0.08, 0.14), + (0.03, 0.06), + (0.012, 0.025), + (0.005, 0.01), +]; + +const XR_DEPTH_ALIGN_SEEDED_LOCAL_SEARCH_PHASES: [(f32, f32); 4] = + [(0.10, 0.18), (0.04, 0.07), (0.015, 0.03), (0.006, 0.012)]; + +const XR_DEPTH_ALIGN_TRANSLATION_ONLY_SEARCH_PHASES: [(f32, f32); 3] = + [(0.0, 0.06), (0.0, 0.025), (0.0, 0.01)]; + +#[derive(Debug)] +struct FullAlignmentSearchTask { + best: XrDepthAlignSolution, + floor_y: f32, + phases: &'static [(f32, f32)], + translation_only: bool, + phase_index: usize, + yaw_index: usize, + tx_index: usize, + tz_index: usize, + improved_in_pass: bool, +} + +impl FullAlignmentSearchTask { + fn new( + best: XrDepthAlignSolution, + floor_y: f32, + phases: &'static [(f32, f32)], + translation_only: bool, + ) -> Self { + Self { + best, + floor_y, + phases, + translation_only, + phase_index: 0, + yaw_index: 0, + tx_index: 0, + tz_index: 0, + improved_in_pass: false, + } + } + + fn step( + &mut self, + local_signal: &[HeightMapSignalCell], + remote_signal: &[HeightMapSignalCell], + local_map: Option<&XrDepthAlignHeightMap>, + local_sample_cache: Option<&HeightMapSampleCache>, + remote_map: Option<&XrDepthAlignHeightMap>, + remote_dense_signal: &[DenseHeightMapSignalCell], + ) -> Option { + let translation_deltas = [-1.0f32, 0.0, 1.0]; + loop { + let Some(&(yaw_step, translation_step)) = self.phases.get(self.phase_index) else { + return Some(self.best); + }; + let yaw_count = if self.translation_only { 1 } else { 3 }; + while self.yaw_index < yaw_count { + let yaw_delta = if self.translation_only { + 0.0 + } else { + [-yaw_step, 0.0, yaw_step][self.yaw_index] + }; + let tx_delta = translation_deltas[self.tx_index] * translation_step; + let tz_delta = translation_deltas[self.tz_index] * translation_step; + self.advance_indices(yaw_count); + if yaw_delta == 0.0 && tx_delta == 0.0 && tz_delta == 0.0 { + continue; + } + let candidate = score_full_alignment_solution( + local_signal, + remote_signal, + local_map, + local_sample_cache, + remote_map, + remote_dense_signal, + wrap_angle(self.best.yaw_radians + yaw_delta), + vec3( + self.best.translation.x + tx_delta, + self.floor_y, + self.best.translation.z + tz_delta, + ), + ); + if alignment_solution_better(&candidate, &self.best) { + self.best = candidate; + self.improved_in_pass = true; + } + return None; + } + if self.improved_in_pass { + self.improved_in_pass = false; + self.reset_pass(); + continue; + } + self.phase_index += 1; + self.reset_pass(); + } + } + + fn advance_indices(&mut self, yaw_count: usize) { + self.tz_index += 1; + if self.tz_index < 3 { + return; + } + self.tz_index = 0; + self.tx_index += 1; + if self.tx_index < 3 { + return; + } + self.tx_index = 0; + self.yaw_index += 1; + if self.yaw_index > yaw_count { + self.yaw_index = yaw_count; + } + } + + fn reset_pass(&mut self) { + self.yaw_index = 0; + self.tx_index = 0; + self.tz_index = 0; + } +} + +#[derive(Debug)] +struct HeightMapRefineTask { + best_yaw: f32, + best_translation: Vec3f, + best_score: (f32, f32, usize), + initialized: bool, + phase_index: usize, + yaw_index: usize, + tx_index: usize, + tz_index: usize, + improved_in_pass: bool, + rotated_remote_signal: Vec, + rotated_signal_yaw: Option, +} + +impl HeightMapRefineTask { + fn new(yaw: f32, translation: Vec3f) -> Self { + Self { + best_yaw: wrap_angle(yaw), + best_translation: translation, + best_score: (0.0, f32::INFINITY, 0), + initialized: false, + phase_index: 0, + yaw_index: 0, + tx_index: 0, + tz_index: 0, + improved_in_pass: false, + rotated_remote_signal: Vec::new(), + rotated_signal_yaw: None, + } + } + + fn step( + &mut self, + local_map: &XrDepthAlignHeightMap, + local_sample_cache: Option<&HeightMapSampleCache>, + remote_map: &XrDepthAlignHeightMap, + remote_signal: &[DenseHeightMapSignalCell], + ) -> Option<(f32, Vec3f)> { + if !self.initialized { + self.best_score = score_height_map_alignment( + local_map, + local_sample_cache, + remote_map, + remote_signal, + self.best_yaw, + self.best_translation, + ); + self.initialized = true; + return None; + } + let translation_deltas = [-1.0f32, 0.0, 1.0]; + loop { + let Some(&(yaw_step, translation_step)) = + XR_DEPTH_ALIGN_HEIGHT_REFINE_PHASES.get(self.phase_index) + else { + return Some((self.best_yaw, self.best_translation)); + }; + while self.yaw_index < 3 { + let yaw_delta = [-yaw_step, 0.0, yaw_step][self.yaw_index]; + let tx_delta = translation_deltas[self.tx_index] * translation_step; + let tz_delta = translation_deltas[self.tz_index] * translation_step; + self.advance_indices(); + if yaw_delta == 0.0 && tx_delta == 0.0 && tz_delta == 0.0 { + continue; + } + let candidate_yaw = wrap_angle(self.best_yaw + yaw_delta); + if self.rotated_signal_yaw != Some(candidate_yaw) { + fill_rotated_height_map_signal( + &mut self.rotated_remote_signal, + remote_signal, + candidate_yaw, + ); + self.rotated_signal_yaw = Some(candidate_yaw); + } + let candidate_translation = vec3( + self.best_translation.x + tx_delta, + self.best_translation.y, + self.best_translation.z + tz_delta, + ); + let candidate_score = score_rotated_height_map_alignment( + local_map, + local_sample_cache, + remote_map, + &self.rotated_remote_signal, + candidate_yaw, + candidate_translation, + ); + if height_map_score_better(candidate_score, self.best_score) { + self.best_yaw = candidate_yaw; + self.best_translation = candidate_translation; + self.best_score = candidate_score; + self.improved_in_pass = true; + } + return None; + } + if self.improved_in_pass { + self.improved_in_pass = false; + self.reset_pass(); + continue; + } + self.phase_index += 1; + self.reset_pass(); + } + } + + fn advance_indices(&mut self) { + self.tz_index += 1; + if self.tz_index < 3 { + return; + } + self.tz_index = 0; + self.tx_index += 1; + if self.tx_index < 3 { + return; + } + self.tx_index = 0; + self.yaw_index += 1; + if self.yaw_index > 3 { + self.yaw_index = 3; + } + } + + fn reset_pass(&mut self) { + self.yaw_index = 0; + self.tx_index = 0; + self.tz_index = 0; + } +} + +#[derive(Debug)] +enum WallProfileTaskStage { + BuildProfiles, + SearchYaw { + current_profile_score: f32, + best_profile_yaw: f32, + best_profile_score: f32, + next_yaw_index: isize, + yaw_steps: isize, + }, + ScoreCorrectedInitial { + current_profile_score: f32, + best_profile_yaw: f32, + best_profile_score: f32, + }, + RefineTranslation { + current_profile_score: f32, + best_profile_score: f32, + search: FullAlignmentSearchTask, + }, + Done(XrDepthAlignSolution), +} + +#[derive(Debug)] +struct WallProfileTask { + current: XrDepthAlignSolution, + floor_y: f32, + local_profile: Vec<(f32, f32)>, + remote_profile: Vec<(f32, f32)>, + stage: WallProfileTaskStage, +} + +impl WallProfileTask { + fn new(current: XrDepthAlignSolution, floor_y: f32) -> Self { + Self { + current, + floor_y, + local_profile: Vec::new(), + remote_profile: Vec::new(), + stage: WallProfileTaskStage::BuildProfiles, + } + } + + fn step( + &mut self, + local_signal: &[HeightMapSignalCell], + remote_signal: &[HeightMapSignalCell], + local_map: &XrDepthAlignHeightMap, + local_sample_cache: Option<&HeightMapSampleCache>, + remote_map: &XrDepthAlignHeightMap, + remote_dense_signal: &[DenseHeightMapSignalCell], + ) -> Option { + loop { + match &mut self.stage { + WallProfileTaskStage::BuildProfiles => { + self.local_profile = build_wall_profile_contour_points(local_map); + self.remote_profile = build_wall_profile_contour_points(remote_map); + if self.local_profile.len() < XR_DEPTH_ALIGN_WALL_PROFILE_MIN_POINTS + || self.remote_profile.len() < XR_DEPTH_ALIGN_WALL_PROFILE_MIN_POINTS + { + self.stage = WallProfileTaskStage::Done(self.current); + continue; + } + let current_profile_score = score_wall_profile_alignment_with_translation_prior( + &self.local_profile, + &self.remote_profile, + self.current.yaw_radians, + self.current.translation, + ); + let yaw_step = XR_DEPTH_ALIGN_WALL_PROFILE_YAW_STEP_RADIANS; + let yaw_steps = (XR_DEPTH_ALIGN_WALL_PROFILE_YAW_WINDOW_RADIANS / yaw_step) + .ceil() + .max(1.0) as isize; + self.stage = WallProfileTaskStage::SearchYaw { + current_profile_score, + best_profile_yaw: self.current.yaw_radians, + best_profile_score: current_profile_score, + next_yaw_index: -yaw_steps, + yaw_steps, + }; + return None; + } + WallProfileTaskStage::SearchYaw { + current_profile_score, + best_profile_yaw, + best_profile_score, + next_yaw_index, + yaw_steps, + } => { + if *next_yaw_index > *yaw_steps { + if wrap_angle(*best_profile_yaw - self.current.yaw_radians).abs() + < XR_DEPTH_ALIGN_WALL_PROFILE_MIN_YAW_DELTA_RADIANS + { + self.stage = WallProfileTaskStage::Done(self.current); + continue; + } + self.stage = WallProfileTaskStage::ScoreCorrectedInitial { + current_profile_score: *current_profile_score, + best_profile_yaw: *best_profile_yaw, + best_profile_score: *best_profile_score, + }; + continue; + } + let yaw = wrap_angle( + self.current.yaw_radians + + *next_yaw_index as f32 * XR_DEPTH_ALIGN_WALL_PROFILE_YAW_STEP_RADIANS, + ); + let score = score_wall_profile_alignment_with_translation_prior( + &self.local_profile, + &self.remote_profile, + yaw, + self.current.translation, + ); + if score > *best_profile_score { + *best_profile_score = score; + *best_profile_yaw = yaw; + } + *next_yaw_index += 1; + return None; + } + WallProfileTaskStage::ScoreCorrectedInitial { + current_profile_score, + best_profile_yaw, + best_profile_score, + } => { + let corrected_initial = score_full_alignment_solution( + local_signal, + remote_signal, + Some(local_map), + local_sample_cache, + Some(remote_map), + remote_dense_signal, + *best_profile_yaw, + vec3( + self.current.translation.x, + self.floor_y, + self.current.translation.z, + ), + ); + let search = FullAlignmentSearchTask::new( + corrected_initial, + self.floor_y, + &XR_DEPTH_ALIGN_TRANSLATION_ONLY_SEARCH_PHASES, + true, + ); + self.stage = WallProfileTaskStage::RefineTranslation { + current_profile_score: *current_profile_score, + best_profile_score: *best_profile_score, + search, + }; + return None; + } + WallProfileTaskStage::RefineTranslation { + current_profile_score, + best_profile_score, + search, + } => { + let Some(corrected) = search.step( + local_signal, + remote_signal, + Some(local_map), + local_sample_cache, + Some(remote_map), + remote_dense_signal, + ) else { + return None; + }; + let result = if alignment_solution_better(&corrected, &self.current) + || wall_profile_yaw_sidecar_is_safe( + self.current, + corrected, + *current_profile_score, + *best_profile_score, + ) { + corrected + } else { + self.current + }; + self.stage = WallProfileTaskStage::Done(result); + continue; + } + WallProfileTaskStage::Done(result) => return Some(*result), + } + } + } +} + +#[derive(Debug)] +enum XrDepthAlignMatcherState { + BuildSignals, + SeedStart, + SeedHeightRefine(HeightMapRefineTask), + SeedSignalRefine { + best_yaw: f32, + best_translation: Vec3f, + }, + SeedScore { + best_yaw: f32, + best_translation: Vec3f, + }, + SeedLocalSearch(FullAlignmentSearchTask), + SeedWallProfile(WallProfileTask), + SeedVerticalOffset { + candidate: XrDepthAlignSolution, + }, + SeedComplete { + candidate: XrDepthAlignSolution, + }, + BuildYawCandidates, + BuildTranslations { + yaw_index: usize, + }, + ScoreTranslations { + yaw_index: usize, + yaw: f32, + translations: Vec, + translation_index: usize, + yaw_shortlist: Vec, + }, + HeightRefineCandidate { + shortlist_index: usize, + signal_candidate: XrDepthAlignSolution, + task: HeightMapRefineTask, + }, + FinalScoreCandidate { + shortlist_index: usize, + refined_yaw: f32, + refined_translation: Vec3f, + }, + FinalWallProfile(WallProfileTask), + FinalVerticalOffset { + candidate: XrDepthAlignSolution, + }, + Finish, + Done, +} + +#[derive(Clone, Copy, Debug, Default, PartialEq, Eq)] +pub struct XrDepthAlignMatcherProgress { + pub stage_label: &'static str, + pub step_count: u32, + pub yaw_candidate_count: u32, + pub current_yaw_index: u32, + pub current_translation_index: u32, + pub current_translation_count: u32, + pub shortlist_count: u32, + pub current_shortlist_index: u32, +} + +#[derive(Debug)] +pub struct XrDepthAlignMatcher { + local: XrDepthAlignDescriptor, + remote: XrDepthAlignDescriptor, + previous_solution: Option, + state: XrDepthAlignMatcherState, + diagnostic: XrDepthAlignSolveDiagnostic, + floor_y: f32, + local_signal: Vec, + remote_signal: Vec, + remote_dense_signal: Vec, + remote_refine_signal: Vec, + remote_flat_signal: Vec, + local_histogram: Vec, + remote_histogram: Vec, + local_sample_cache: Option, + use_height_refine: bool, + yaw_candidates: Vec, + shortlist: Vec, + best: Option, + translation_vote_time: std::time::Duration, + signal_refine_time: std::time::Duration, + signal_score_time: std::time::Duration, + height_refine_time: std::time::Duration, + final_score_time: std::time::Duration, + wall_profile_time: std::time::Duration, + total_compute_time: std::time::Duration, + max_step_time: std::time::Duration, +} + +impl XrDepthAlignMatcher { + pub fn new( + local: &XrDepthAlignDescriptor, + remote: &XrDepthAlignDescriptor, + previous_solution: Option, + ) -> Self { + Self { + local: local.clone(), + remote: remote.clone(), + previous_solution, + state: XrDepthAlignMatcherState::BuildSignals, + diagnostic: XrDepthAlignSolveDiagnostic::default(), + floor_y: 0.0, + local_signal: Vec::new(), + remote_signal: Vec::new(), + remote_dense_signal: Vec::new(), + remote_refine_signal: Vec::new(), + remote_flat_signal: Vec::new(), + local_histogram: Vec::new(), + remote_histogram: Vec::new(), + local_sample_cache: None, + use_height_refine: false, + yaw_candidates: Vec::new(), + shortlist: Vec::new(), + best: None, + translation_vote_time: std::time::Duration::ZERO, + signal_refine_time: std::time::Duration::ZERO, + signal_score_time: std::time::Duration::ZERO, + height_refine_time: std::time::Duration::ZERO, + final_score_time: std::time::Duration::ZERO, + wall_profile_time: std::time::Duration::ZERO, + total_compute_time: std::time::Duration::ZERO, + max_step_time: std::time::Duration::ZERO, + } + } + + pub fn is_finished(&self) -> bool { + matches!(self.state, XrDepthAlignMatcherState::Done) + } + + pub fn diagnostic(&self) -> Option { + self.is_finished().then_some(self.diagnostic) + } + + pub fn finish(mut self) -> XrDepthAlignSolveDiagnostic { + while self.step() {} + self.diagnostic + } + + pub fn progress(&self) -> XrDepthAlignMatcherProgress { + let mut progress = XrDepthAlignMatcherProgress { + step_count: self.diagnostic.step_count, + yaw_candidate_count: self.yaw_candidates.len() as u32, + shortlist_count: self.shortlist.len() as u32, + ..XrDepthAlignMatcherProgress::default() + }; + match &self.state { + XrDepthAlignMatcherState::BuildSignals => { + progress.stage_label = "signals"; + } + XrDepthAlignMatcherState::SeedStart => { + progress.stage_label = "seed-start"; + } + XrDepthAlignMatcherState::SeedHeightRefine(_) => { + progress.stage_label = "seed-height"; + } + XrDepthAlignMatcherState::SeedSignalRefine { .. } => { + progress.stage_label = "seed-signal"; + } + XrDepthAlignMatcherState::SeedScore { .. } => { + progress.stage_label = "seed-score"; + } + XrDepthAlignMatcherState::SeedLocalSearch(_) => { + progress.stage_label = "seed-search"; + } + XrDepthAlignMatcherState::SeedWallProfile(_) => { + progress.stage_label = "seed-wall"; + } + XrDepthAlignMatcherState::SeedVerticalOffset { .. } => { + progress.stage_label = "seed-vertical"; + } + XrDepthAlignMatcherState::SeedComplete { .. } => { + progress.stage_label = "seed-complete"; + } + XrDepthAlignMatcherState::BuildYawCandidates => { + progress.stage_label = "yaw"; + } + XrDepthAlignMatcherState::BuildTranslations { yaw_index } => { + progress.stage_label = "vote"; + progress.current_yaw_index = (*yaw_index + 1) as u32; + } + XrDepthAlignMatcherState::ScoreTranslations { + yaw_index, + translations, + translation_index, + .. + } => { + progress.stage_label = "score"; + progress.current_yaw_index = (*yaw_index + 1) as u32; + progress.current_translation_count = translations.len() as u32; + progress.current_translation_index = + ((*translation_index).min(translations.len().saturating_sub(1)) + 1) as u32; + } + XrDepthAlignMatcherState::HeightRefineCandidate { + shortlist_index, .. + } => { + progress.stage_label = "height"; + progress.current_shortlist_index = (*shortlist_index + 1) as u32; + } + XrDepthAlignMatcherState::FinalScoreCandidate { + shortlist_index, .. + } => { + progress.stage_label = "final-score"; + progress.current_shortlist_index = (*shortlist_index + 1) as u32; + } + XrDepthAlignMatcherState::FinalWallProfile(_) => { + progress.stage_label = "wall"; + } + XrDepthAlignMatcherState::FinalVerticalOffset { .. } => { + progress.stage_label = "vertical"; + } + XrDepthAlignMatcherState::Finish => { + progress.stage_label = "finish"; + } + XrDepthAlignMatcherState::Done => { + progress.stage_label = "done"; + } + } + progress + } + + pub fn step_for_budget(&mut self, budget: std::time::Duration, max_steps: usize) -> u32 { + if self.is_finished() || max_steps == 0 { + return 0; + } + let started = std::time::Instant::now(); + let mut steps = 0u32; + while steps < max_steps as u32 { + if steps != 0 && !budget.is_zero() && started.elapsed() >= budget { + break; + } + if !self.step() { + break; + } + steps = steps.saturating_add(1); + } + steps + } + + pub fn step(&mut self) -> bool { + if self.is_finished() { + return false; + } + let step_started = std::time::Instant::now(); + let did_work = loop { + let state = std::mem::replace(&mut self.state, XrDepthAlignMatcherState::Done); + match state { + XrDepthAlignMatcherState::BuildSignals => { + let started = std::time::Instant::now(); + self.local_signal = selected_descriptor_signal_cells(&self.local); + self.remote_signal = selected_descriptor_signal_cells(&self.remote); + self.remote_dense_signal = descriptor_dense_signal_cells(&self.remote); + self.remote_refine_signal = decimate_signal_cells( + &self.remote_dense_signal, + XR_DEPTH_ALIGN_HEIGHT_REFINE_MAX_DENSE_SAMPLES, + ); + self.remote_flat_signal = decimate_signal_cells( + &descriptor_flat_surface_cells(&self.remote), + XR_DEPTH_ALIGN_VERTICAL_OFFSET_MAX_FLAT_SAMPLES, + ); + self.local_histogram = build_height_map_signal_histogram( + &self.local_signal, + XR_DEPTH_ALIGN_HEIGHT_MAP_HISTOGRAM_BINS, + ); + self.remote_histogram = build_height_map_signal_histogram( + &self.remote_signal, + XR_DEPTH_ALIGN_HEIGHT_MAP_HISTOGRAM_BINS, + ); + self.local_sample_cache = self + .local + .height_map + .as_ref() + .and_then(build_height_map_sample_cache); + self.floor_y = self.local.floor_y - self.remote.floor_y; + self.use_height_refine = self.local.height_map.is_some() + && self.remote.height_map.is_some() + && !self.remote_refine_signal.is_empty(); + self.diagnostic = XrDepthAlignSolveDiagnostic { + local_vertical_descriptor: self.local.vertical_descriptor.is_some(), + remote_vertical_descriptor: self.remote.vertical_descriptor.is_some(), + local_wall_samples: self.local_signal.len(), + remote_wall_samples: self.remote_signal.len(), + remote_dense_wall_samples: self.remote_dense_signal.len(), + remote_refine_wall_samples: self.remote_refine_signal.len(), + signal_build_ms: duration_ms_u32(started.elapsed()), + ..XrDepthAlignSolveDiagnostic::default() + }; + self.state = if self.local_signal.len() < XR_DEPTH_ALIGN_MIN_SIGNAL_SAMPLES + || self.remote_signal.len() < XR_DEPTH_ALIGN_MIN_SIGNAL_SAMPLES + { + XrDepthAlignMatcherState::Finish + } else if self.previous_solution.is_some() { + XrDepthAlignMatcherState::SeedStart + } else { + XrDepthAlignMatcherState::BuildYawCandidates + }; + break true; + } + XrDepthAlignMatcherState::SeedStart => { + self.diagnostic.yaw_candidate_count += 1; + self.diagnostic.pose_candidate_count += 1; + let seed = self.previous_solution.expect("seeded state requires seed"); + let translation = vec3(seed.translation.x, self.floor_y, seed.translation.z); + self.state = if self.use_height_refine { + XrDepthAlignMatcherState::SeedHeightRefine(HeightMapRefineTask::new( + seed.yaw_radians, + translation, + )) + } else { + XrDepthAlignMatcherState::SeedSignalRefine { + best_yaw: wrap_angle(seed.yaw_radians), + best_translation: translation, + } + }; + continue; + } + XrDepthAlignMatcherState::SeedHeightRefine(mut task) => { + let started = std::time::Instant::now(); + let Some(local_map) = self.local_map() else { + self.state = XrDepthAlignMatcherState::SeedSignalRefine { + best_yaw: task.best_yaw, + best_translation: task.best_translation, + }; + continue; + }; + let Some(remote_map) = self.remote_map() else { + self.state = XrDepthAlignMatcherState::SeedSignalRefine { + best_yaw: task.best_yaw, + best_translation: task.best_translation, + }; + continue; + }; + let completed = task.step( + local_map, + self.local_sample_cache.as_ref(), + remote_map, + &self.remote_refine_signal, + ); + self.height_refine_time += started.elapsed(); + if let Some((best_yaw, best_translation)) = completed { + self.state = XrDepthAlignMatcherState::SeedSignalRefine { + best_yaw, + best_translation, + }; + } else { + self.state = XrDepthAlignMatcherState::SeedHeightRefine(task); + } + break true; + } + XrDepthAlignMatcherState::SeedSignalRefine { + best_yaw, + best_translation, + } => { + let started = std::time::Instant::now(); + let (signal_refined_yaw, signal_refined_translation) = refine_signal_alignment( + &self.local_signal, + &self.remote_signal, + self.floor_y, + best_yaw, + best_translation, + ); + self.signal_refine_time += started.elapsed(); + let signal_translation_jump = vec3( + signal_refined_translation.x - best_translation.x, + 0.0, + signal_refined_translation.z - best_translation.z, + ) + .length(); + let (next_yaw, next_translation) = + if wrap_angle(signal_refined_yaw - best_yaw).abs() <= 0.18 + && signal_translation_jump <= 0.28 + { + (signal_refined_yaw, signal_refined_translation) + } else { + (best_yaw, best_translation) + }; + self.state = XrDepthAlignMatcherState::SeedScore { + best_yaw: next_yaw, + best_translation: next_translation, + }; + break true; + } + XrDepthAlignMatcherState::SeedScore { + best_yaw, + best_translation, + } => { + let started = std::time::Instant::now(); + let best = score_full_alignment_solution( + &self.local_signal, + &self.remote_signal, + self.local_map(), + self.local_sample_cache.as_ref(), + self.remote_map(), + &self.remote_dense_signal, + best_yaw, + best_translation, + ); + self.final_score_time += started.elapsed(); + self.state = + XrDepthAlignMatcherState::SeedLocalSearch(FullAlignmentSearchTask::new( + best, + self.floor_y, + &XR_DEPTH_ALIGN_SEEDED_LOCAL_SEARCH_PHASES, + false, + )); + break true; + } + XrDepthAlignMatcherState::SeedLocalSearch(mut task) => { + let started = std::time::Instant::now(); + let completed = task.step( + &self.local_signal, + &self.remote_signal, + self.local_map(), + self.local_sample_cache.as_ref(), + self.remote_map(), + &self.remote_dense_signal, + ); + self.final_score_time += started.elapsed(); + if let Some(candidate) = completed { + self.state = if self.local_map().is_some() && self.remote_map().is_some() { + XrDepthAlignMatcherState::SeedWallProfile(WallProfileTask::new( + candidate, + self.floor_y, + )) + } else { + XrDepthAlignMatcherState::SeedComplete { candidate } + }; + } else { + self.state = XrDepthAlignMatcherState::SeedLocalSearch(task); + } + break true; + } + XrDepthAlignMatcherState::SeedWallProfile(mut task) => { + let Some(local_map) = self.local_map() else { + self.state = XrDepthAlignMatcherState::SeedComplete { + candidate: task.current, + }; + continue; + }; + let Some(remote_map) = self.remote_map() else { + self.state = XrDepthAlignMatcherState::SeedComplete { + candidate: task.current, + }; + continue; + }; + let started = std::time::Instant::now(); + let completed = task.step( + &self.local_signal, + &self.remote_signal, + local_map, + self.local_sample_cache.as_ref(), + remote_map, + &self.remote_dense_signal, + ); + self.wall_profile_time += started.elapsed(); + if let Some(candidate) = completed { + self.state = XrDepthAlignMatcherState::SeedVerticalOffset { candidate }; + } else { + self.state = XrDepthAlignMatcherState::SeedWallProfile(task); + } + break true; + } + XrDepthAlignMatcherState::SeedVerticalOffset { candidate } => { + let started = std::time::Instant::now(); + let candidate = if let (Some(local_map), Some(remote_map)) = + (self.local_map(), self.remote_map()) + { + refine_solution_vertical_offset_from_overlap( + local_map, + remote_map, + &self.remote_flat_signal, + candidate, + ) + } else { + candidate + }; + self.final_score_time += started.elapsed(); + self.state = XrDepthAlignMatcherState::SeedComplete { candidate }; + break true; + } + XrDepthAlignMatcherState::SeedComplete { candidate } => { + self.best = Some(candidate); + if let Some(seed) = self.previous_solution { + if seeded_alignment_lock_is_strong( + &self.diagnostic, + candidate, + seed, + self.remote_signal.len(), + self.local_map(), + self.remote_map(), + ) { + self.state = XrDepthAlignMatcherState::Finish; + continue; + } + } + self.state = XrDepthAlignMatcherState::BuildYawCandidates; + continue; + } + XrDepthAlignMatcherState::BuildYawCandidates => { + let started = std::time::Instant::now(); + self.yaw_candidates = candidate_signal_yaws( + &self.local_histogram, + &self.remote_histogram, + &self.local_signal, + &self.remote_signal, + ); + self.diagnostic.yaw_candidate_ms = duration_ms_u32(started.elapsed()); + self.diagnostic.yaw_candidate_count = self.yaw_candidates.len(); + self.state = if self.yaw_candidates.is_empty() { + XrDepthAlignMatcherState::Finish + } else { + XrDepthAlignMatcherState::BuildTranslations { yaw_index: 0 } + }; + break true; + } + XrDepthAlignMatcherState::BuildTranslations { yaw_index } => { + let yaw = self.yaw_candidates[yaw_index]; + let started = std::time::Instant::now(); + let translations = candidate_signal_translations( + &self.local_signal, + &self.remote_signal, + self.floor_y, + yaw, + ); + self.translation_vote_time += started.elapsed(); + self.diagnostic.pose_candidate_count += translations.len(); + self.state = XrDepthAlignMatcherState::ScoreTranslations { + yaw_index, + yaw, + translations, + translation_index: 0, + yaw_shortlist: Vec::new(), + }; + break true; + } + XrDepthAlignMatcherState::ScoreTranslations { + yaw_index, + yaw, + translations, + translation_index, + mut yaw_shortlist, + } => { + if translation_index >= translations.len() { + for signal_candidate in yaw_shortlist { + push_shortlisted_alignment_solution( + &mut self.shortlist, + signal_candidate, + XR_DEPTH_ALIGN_SIGNAL_SHORTLIST_MAX_CANDIDATES, + XR_DEPTH_ALIGN_SIGNAL_SHORTLIST_TRANSLATION_EPSILON_METERS, + XR_DEPTH_ALIGN_SIGNAL_SHORTLIST_YAW_EPSILON_RADIANS, + ); + } + self.state = if yaw_index + 1 < self.yaw_candidates.len() { + XrDepthAlignMatcherState::BuildTranslations { + yaw_index: yaw_index + 1, + } + } else if self.use_height_refine && !self.shortlist.is_empty() { + self.diagnostic.shortlisted_pose_count = self.shortlist.len(); + let signal_candidate = self.shortlist[0]; + XrDepthAlignMatcherState::HeightRefineCandidate { + shortlist_index: 0, + signal_candidate, + task: HeightMapRefineTask::new( + signal_candidate.yaw_radians, + signal_candidate.translation, + ), + } + } else if let Some(best) = self.best { + if self.local_map().is_some() && self.remote_map().is_some() { + XrDepthAlignMatcherState::FinalWallProfile(WallProfileTask::new( + best, + self.floor_y, + )) + } else { + XrDepthAlignMatcherState::Finish + } + } else { + XrDepthAlignMatcherState::Finish + }; + continue; + } + + let translation = translations[translation_index]; + let signal_refine_started = std::time::Instant::now(); + let (refined_yaw, refined_translation) = refine_signal_alignment( + &self.local_signal, + &self.remote_signal, + self.floor_y, + yaw, + translation, + ); + self.signal_refine_time += signal_refine_started.elapsed(); + + let signal_score_started = std::time::Instant::now(); + let signal_candidate = score_signal_alignment_solution( + &self.local_signal, + &self.remote_signal, + refined_yaw, + refined_translation, + ); + self.signal_score_time += signal_score_started.elapsed(); + + if self.use_height_refine { + push_shortlisted_alignment_solution( + &mut yaw_shortlist, + signal_candidate, + XR_DEPTH_ALIGN_SIGNAL_SHORTLIST_PER_YAW, + XR_DEPTH_ALIGN_SIGNAL_SHORTLIST_TRANSLATION_EPSILON_METERS, + XR_DEPTH_ALIGN_SIGNAL_SHORTLIST_YAW_EPSILON_RADIANS, + ); + } else if self + .best + .as_ref() + .is_none_or(|current| alignment_solution_better(&signal_candidate, current)) + { + self.best = Some(signal_candidate); + } + self.state = XrDepthAlignMatcherState::ScoreTranslations { + yaw_index, + yaw, + translations, + translation_index: translation_index + 1, + yaw_shortlist, + }; + break true; + } + XrDepthAlignMatcherState::HeightRefineCandidate { + shortlist_index, + signal_candidate, + mut task, + } => { + let Some(local_map) = self.local_map() else { + self.state = XrDepthAlignMatcherState::FinalScoreCandidate { + shortlist_index, + refined_yaw: signal_candidate.yaw_radians, + refined_translation: signal_candidate.translation, + }; + continue; + }; + let Some(remote_map) = self.remote_map() else { + self.state = XrDepthAlignMatcherState::FinalScoreCandidate { + shortlist_index, + refined_yaw: signal_candidate.yaw_radians, + refined_translation: signal_candidate.translation, + }; + continue; + }; + let started = std::time::Instant::now(); + let completed = task.step( + local_map, + self.local_sample_cache.as_ref(), + remote_map, + &self.remote_refine_signal, + ); + self.height_refine_time += started.elapsed(); + if let Some((refined_yaw, refined_translation)) = completed { + self.state = XrDepthAlignMatcherState::FinalScoreCandidate { + shortlist_index, + refined_yaw, + refined_translation, + }; + } else { + self.state = XrDepthAlignMatcherState::HeightRefineCandidate { + shortlist_index, + signal_candidate, + task, + }; + } + break true; + } + XrDepthAlignMatcherState::FinalScoreCandidate { + shortlist_index, + refined_yaw, + refined_translation, + } => { + let started = std::time::Instant::now(); + let candidate = score_full_alignment_solution( + &self.local_signal, + &self.remote_signal, + self.local_map(), + self.local_sample_cache.as_ref(), + self.remote_map(), + &self.remote_dense_signal, + refined_yaw, + vec3(refined_translation.x, self.floor_y, refined_translation.z), + ); + self.final_score_time += started.elapsed(); + if self + .best + .as_ref() + .is_none_or(|current| alignment_solution_better(&candidate, current)) + { + self.best = Some(candidate); + } + self.state = if shortlist_index + 1 < self.shortlist.len() { + let signal_candidate = self.shortlist[shortlist_index + 1]; + XrDepthAlignMatcherState::HeightRefineCandidate { + shortlist_index: shortlist_index + 1, + signal_candidate, + task: HeightMapRefineTask::new( + signal_candidate.yaw_radians, + signal_candidate.translation, + ), + } + } else if let Some(best) = self.best { + if self.local_map().is_some() && self.remote_map().is_some() { + XrDepthAlignMatcherState::FinalWallProfile(WallProfileTask::new( + best, + self.floor_y, + )) + } else { + XrDepthAlignMatcherState::Finish + } + } else { + XrDepthAlignMatcherState::Finish + }; + break true; + } + XrDepthAlignMatcherState::FinalWallProfile(mut task) => { + let Some(local_map) = self.local_map() else { + self.state = XrDepthAlignMatcherState::Finish; + continue; + }; + let Some(remote_map) = self.remote_map() else { + self.state = XrDepthAlignMatcherState::Finish; + continue; + }; + let started = std::time::Instant::now(); + let completed = task.step( + &self.local_signal, + &self.remote_signal, + local_map, + self.local_sample_cache.as_ref(), + remote_map, + &self.remote_dense_signal, + ); + self.wall_profile_time += started.elapsed(); + if let Some(candidate) = completed { + self.state = XrDepthAlignMatcherState::FinalVerticalOffset { candidate }; + } else { + self.state = XrDepthAlignMatcherState::FinalWallProfile(task); + } + break true; + } + XrDepthAlignMatcherState::FinalVerticalOffset { candidate } => { + let started = std::time::Instant::now(); + self.best = Some( + if let (Some(local_map), Some(remote_map)) = + (self.local_map(), self.remote_map()) + { + refine_solution_vertical_offset_from_overlap( + local_map, + remote_map, + &self.remote_flat_signal, + candidate, + ) + } else { + candidate + }, + ); + self.final_score_time += started.elapsed(); + self.state = XrDepthAlignMatcherState::Finish; + break true; + } + XrDepthAlignMatcherState::Finish => { + self.finalize_diagnostic(); + self.state = XrDepthAlignMatcherState::Done; + continue; + } + XrDepthAlignMatcherState::Done => { + self.state = XrDepthAlignMatcherState::Done; + return false; + } + } + }; + if did_work { + let elapsed = step_started.elapsed(); + self.total_compute_time += elapsed; + self.max_step_time = self.max_step_time.max(elapsed); + self.diagnostic.step_count = self.diagnostic.step_count.saturating_add(1); + } + did_work + } + + fn local_map(&self) -> Option<&XrDepthAlignHeightMap> { + self.local.height_map.as_ref() + } + + fn remote_map(&self) -> Option<&XrDepthAlignHeightMap> { + self.remote.height_map.as_ref() + } + + fn finalize_diagnostic(&mut self) { + self.diagnostic.translation_vote_ms = duration_ms_u32(self.translation_vote_time); + self.diagnostic.signal_refine_ms = duration_ms_u32(self.signal_refine_time); + self.diagnostic.signal_score_ms = duration_ms_u32(self.signal_score_time); + self.diagnostic.height_refine_ms = duration_ms_u32(self.height_refine_time); + self.diagnostic.final_score_ms = duration_ms_u32(self.final_score_time); + self.diagnostic.wall_profile_ms = duration_ms_u32(self.wall_profile_time); + self.diagnostic.total_compute_ms = duration_ms_u32(self.total_compute_time); + self.diagnostic.max_step_ms = duration_ms_u32(self.max_step_time); + self.diagnostic.best_solution = self.best; + } +} + +fn transform_height_map( + height_map: &XrDepthAlignHeightMap, + transform: &Mat4f, +) -> Option { + let size_x = height_map.size_x_usize(); + let size_z = height_map.size_z_usize(); + if size_x == 0 || size_z == 0 || height_map.heights_meters.len() != size_x * size_z { + return None; + } + let cell_size = height_map.cell_size_meters.max(1.0e-5); + let extent_x = cell_size * size_x as f32; + let extent_z = cell_size * size_z as f32; + let corners = [ + vec3(height_map.origin_x, 0.0, height_map.origin_z), + vec3(height_map.origin_x + extent_x, 0.0, height_map.origin_z), + vec3( + height_map.origin_x + extent_x, + 0.0, + height_map.origin_z + extent_z, + ), + vec3(height_map.origin_x, 0.0, height_map.origin_z + extent_z), + ]; + let mut min_x = f32::INFINITY; + let mut min_z = f32::INFINITY; + let mut max_x = f32::NEG_INFINITY; + let mut max_z = f32::NEG_INFINITY; + for corner in corners { + let transformed = transform + .transform_vec4(vec4f(corner.x, corner.y, corner.z, 1.0)) + .to_vec3f(); + min_x = min_x.min(transformed.x); + max_x = max_x.max(transformed.x); + min_z = min_z.min(transformed.z); + max_z = max_z.max(transformed.z); + } + if !min_x.is_finite() || !min_z.is_finite() || !max_x.is_finite() || !max_z.is_finite() { + return None; + } + let min_cell_x = (min_x / cell_size).floor() as i32; + let max_cell_x = (max_x / cell_size).ceil() as i32; + let min_cell_z = (min_z / cell_size).floor() as i32; + let max_cell_z = (max_z / cell_size).ceil() as i32; + let target_size_x = (max_cell_x - min_cell_x).max(1) as usize; + let target_size_z = (max_cell_z - min_cell_z).max(1) as usize; + if target_size_x > u16::MAX as usize || target_size_z > u16::MAX as usize { + return None; + } + let origin_x = min_cell_x as f32 * cell_size; + let origin_z = min_cell_z as f32 * cell_size; + let y_offset = transform + .transform_vec4(vec4f(0.0, 0.0, 0.0, 1.0)) + .to_vec3f() + .y; + let inverse = transform.invert(); + let mut transformed = XrDepthAlignHeightMap { + origin_x, + origin_z, + cell_size_meters: cell_size, + size_x: target_size_x as u16, + size_z: target_size_z as u16, + bottom_y_meters: height_map.bottom_y_meters + y_offset, + top_y_meters: height_map.top_y_meters + y_offset, + floor_y_meters: height_map.floor_y_meters + y_offset, + player_cutout_center: height_map.player_cutout_center.map(|center| { + let mapped = transform + .transform_vec4(vec4f(center.x, 0.0, center.y, 1.0)) + .to_vec3f(); + vec2f(mapped.x, mapped.z) + }), + player_cutout_radius_meters: height_map.player_cutout_radius_meters, + heights_meters: vec![f32::NAN; target_size_x * target_size_z], + }; + for z in 0..target_size_z { + for x in 0..target_size_x { + let world_x = origin_x + (x as f32 + 0.5) * transformed.cell_size_meters; + let world_z = origin_z + (z as f32 + 0.5) * transformed.cell_size_meters; + let source = inverse + .transform_vec4(vec4f(world_x, 0.0, world_z, 1.0)) + .to_vec3f(); + let Some(height) = sample_height_map_nearest(height_map, source.x, source.z) else { + continue; + }; + let target_index = transformed.cell_index(x, z); + transformed.heights_meters[target_index] = height + y_offset; + } + } + Some(transformed) +} + +fn build_wall_profile_contour_points(height_map: &XrDepthAlignHeightMap) -> Vec<(f32, f32)> { + let coarse_cell = XR_DEPTH_ALIGN_WALL_PROFILE_CELL_METERS + .max(height_map.cell_size_meters) + .max(1.0e-3); + let size_x = ((height_map.extent_x_meters() / coarse_cell).ceil() as usize).max(1); + let size_z = ((height_map.extent_z_meters() / coarse_cell).ceil() as usize).max(1); + let mut occupied = vec![false; size_x * size_z]; + let cutout_center = height_map.player_cutout_center; + let cutout_radius = (height_map.player_cutout_radius_meters + height_map.cell_size_meters) + .max(height_map.cell_size_meters * 2.0); + for src_z in 0..height_map.size_z_usize() { + for src_x in 0..height_map.size_x_usize() { + let index = height_map.cell_index(src_x, src_z); + let Some(height) = height_map_cell_height(height_map, index) else { + continue; + }; + let world_x = height_map.origin_x + (src_x as f32 + 0.5) * height_map.cell_size_meters; + let world_z = height_map.origin_z + (src_z as f32 + 0.5) * height_map.cell_size_meters; + if cutout_center.is_some_and(|center| { + let dx = world_x - center.x; + let dz = world_z - center.y; + (dx * dx + dz * dz).sqrt() <= cutout_radius + }) { + continue; + } + let relative_height = height - height_map.floor_y_meters; + if relative_height < XR_DEPTH_ALIGN_WALL_PROFILE_MIN_HEIGHT_METERS + || relative_height > XR_DEPTH_ALIGN_WALL_PROFILE_MAX_HEIGHT_METERS + { + continue; + } + let coarse_x = ((world_x - height_map.origin_x) / coarse_cell) + .floor() + .clamp(0.0, size_x.saturating_sub(1) as f32) as usize; + let coarse_z = ((world_z - height_map.origin_z) / coarse_cell) + .floor() + .clamp(0.0, size_z.saturating_sub(1) as f32) as usize; + occupied[coarse_x + coarse_z * size_x] = true; + } + } + + let mut contour_points = Vec::new(); + for z in 0..size_z { + for x in 0..size_x { + let index = x + z * size_x; + if !occupied[index] { + continue; + } + let mut is_edge = false; + for (dx, dz) in [(-1isize, 0isize), (1, 0), (0, -1), (0, 1)] { + let nx = x as isize + dx; + let nz = z as isize + dz; + if nx < 0 + || nz < 0 + || nx >= size_x as isize + || nz >= size_z as isize + || !occupied[nx as usize + nz as usize * size_x] + { + is_edge = true; + break; + } + } + if is_edge { + contour_points.push(( + height_map.origin_x + (x as f32 + 0.5) * coarse_cell, + height_map.origin_z + (z as f32 + 0.5) * coarse_cell, + )); + } + } + } + if contour_points.len() >= XR_DEPTH_ALIGN_WALL_PROFILE_MIN_POINTS { + contour_points + } else { + Vec::new() + } +} + +fn projection_bounds( + local_points: &[(f32, f32)], + remote_points: &[(f32, f32)], + axis_x: f32, + axis_z: f32, +) -> Option { + let mut min_projection = f32::INFINITY; + let mut max_projection = f32::NEG_INFINITY; + for &(x, z) in local_points.iter().chain(remote_points.iter()) { + let projection = x * axis_x + z * axis_z; + min_projection = min_projection.min(projection); + max_projection = max_projection.max(projection); + } + (min_projection.is_finite() && max_projection.is_finite()).then_some(ProjectionBounds { + min: min_projection, + max: max_projection, + }) +} + +fn projection_histogram_overlap( + local_points: &[(f32, f32)], + remote_points: &[(f32, f32)], + axis_x: f32, + axis_z: f32, + bin_size_meters: f32, +) -> f32 { + let Some(bounds) = projection_bounds(local_points, remote_points, axis_x, axis_z) else { + return 0.0; + }; + let bin_size = bin_size_meters.max(0.04); + let bin_count = (((bounds.max - bounds.min) / bin_size).ceil() as usize).max(1) + 1; + let mut local_hist = vec![0.0f32; bin_count]; + let mut remote_hist = vec![0.0f32; bin_count]; + for &(x, z) in local_points { + let projection = x * axis_x + z * axis_z; + let bin = ((projection - bounds.min) / bin_size) + .floor() + .clamp(0.0, bin_count.saturating_sub(1) as f32) as usize; + local_hist[bin] += 1.0; + } + for &(x, z) in remote_points { + let projection = x * axis_x + z * axis_z; + let bin = ((projection - bounds.min) / bin_size) + .floor() + .clamp(0.0, bin_count.saturating_sub(1) as f32) as usize; + remote_hist[bin] += 1.0; + } + let mut intersection = 0.0f32; + let mut union = 0.0f32; + for index in 0..bin_count { + intersection += local_hist[index].min(remote_hist[index]); + union += local_hist[index].max(remote_hist[index]); + } + intersection / union.max(1.0e-6) +} + +fn score_wall_profile_alignment_at_translation( + local_points: &[(f32, f32)], + remote_points: &[(f32, f32)], + yaw: f32, + translation_x: f32, + translation_z: f32, + bin_size_meters: f32, +) -> f32 { + if local_points.len() < XR_DEPTH_ALIGN_WALL_PROFILE_MIN_POINTS + || remote_points.len() < XR_DEPTH_ALIGN_WALL_PROFILE_MIN_POINTS + { + return 0.0; + } + let transformed_remote = remote_points + .iter() + .map(|&(x, z)| { + let (rx, rz) = rotate_xz(yaw, x, z); + (rx + translation_x, rz + translation_z) + }) + .collect::>(); + let (axis_z, axis_x) = yaw.sin_cos(); + let parallel = projection_histogram_overlap( + local_points, + &transformed_remote, + axis_x, + axis_z, + bin_size_meters, + ); + let perpendicular = projection_histogram_overlap( + local_points, + &transformed_remote, + -axis_z, + axis_x, + bin_size_meters, + ); + (parallel * perpendicular).sqrt() +} + +fn score_wall_profile_alignment_with_translation_prior( + local_points: &[(f32, f32)], + remote_points: &[(f32, f32)], + yaw: f32, + base_translation: Vec3f, +) -> f32 { + let step = XR_DEPTH_ALIGN_WALL_PROFILE_TRANSLATION_STEP_METERS.max(1.0e-3); + let window = XR_DEPTH_ALIGN_WALL_PROFILE_TRANSLATION_WINDOW_METERS.max(step); + let steps = (window / step).ceil().max(1.0) as isize; + let mut best = 0.0f32; + for x_step in -steps..=steps { + let translation_x = base_translation.x + x_step as f32 * step; + for z_step in -steps..=steps { + let translation_z = base_translation.z + z_step as f32 * step; + let translation_delta = ((translation_x - base_translation.x).powi(2) + + (translation_z - base_translation.z).powi(2)) + .sqrt(); + let prior = (-0.5 + * (translation_delta / XR_DEPTH_ALIGN_WALL_PROFILE_TRANSLATION_SIGMA_METERS) + .powi(2)) + .exp() + .clamp(0.0, 1.0); + let score = score_wall_profile_alignment_at_translation( + local_points, + remote_points, + yaw, + translation_x, + translation_z, + XR_DEPTH_ALIGN_WALL_PROFILE_CELL_METERS, + ) * prior; + best = best.max(score); + } + } + best +} + +fn transform_vertical_descriptor( + descriptor: &XrDepthAlignVerticalDescriptor, + transform: &Mat4f, +) -> Option { + let size = descriptor.size as usize; + if size == 0 + || descriptor.vertical_surface_masks.len() != size * size + || descriptor.clutter_surface_masks.len() != size * size + || descriptor.free_space_masks.len() != size * size + || descriptor.height_u8.len() != size * size + { + return None; + } + let max = descriptor.origin_x + descriptor.cell_size_meters * size as f32; + let max_z = descriptor.origin_z + descriptor.cell_size_meters * size as f32; + let corners = [ + vec3(descriptor.origin_x, 0.0, descriptor.origin_z), + vec3(max, 0.0, descriptor.origin_z), + vec3(max, 0.0, max_z), + vec3(descriptor.origin_x, 0.0, max_z), + ]; + let mut min_x = f32::INFINITY; + let mut min_z = f32::INFINITY; + let mut max_x = f32::NEG_INFINITY; + let mut max_z = f32::NEG_INFINITY; + for corner in corners { + let transformed = transform + .transform_vec4(vec4f(corner.x, corner.y, corner.z, 1.0)) + .to_vec3f(); + min_x = min_x.min(transformed.x); + max_x = max_x.max(transformed.x); + min_z = min_z.min(transformed.z); + max_z = max_z.max(transformed.z); + } + if !min_x.is_finite() || !min_z.is_finite() || !max_x.is_finite() || !max_z.is_finite() { + return None; + } + let extent = (max_x - min_x) + .max(max_z - min_z) + .max(descriptor.cell_size_meters); + let origin_x = (min_x + max_x) * 0.5 - extent * 0.5; + let origin_z = (min_z + max_z) * 0.5 - extent * 0.5; + let cell_size = extent / size as f32; + let mut transformed = XrDepthAlignVerticalDescriptor { + origin_x, + origin_z, + cell_size_meters: cell_size, + size: descriptor.size, + vertical_surface_masks: vec![0; size * size], + clutter_surface_masks: vec![0; size * size], + free_space_masks: vec![0; size * size], + height_u8: vec![0; size * size], + }; + for z in 0..size { + for x in 0..size { + let index = x + z * size; + let active = descriptor.vertical_surface_masks[index] != 0 + || descriptor.clutter_surface_masks[index] != 0 + || descriptor.free_space_masks[index] != 0 + || descriptor.height_u8[index] != 0; + if !active { + continue; + } + let point = vec3( + descriptor.origin_x + (x as f32 + 0.5) * descriptor.cell_size_meters, + 0.0, + descriptor.origin_z + (z as f32 + 0.5) * descriptor.cell_size_meters, + ); + let mapped = transform + .transform_vec4(vec4f(point.x, point.y, point.z, 1.0)) + .to_vec3f(); + let tx = ((mapped.x - origin_x) / cell_size).floor() as isize; + let tz = ((mapped.z - origin_z) / cell_size).floor() as isize; + if tx < 0 || tz < 0 || tx >= size as isize || tz >= size as isize { + continue; + } + let target_index = tx as usize + tz as usize * size; + transformed.vertical_surface_masks[target_index] |= + descriptor.vertical_surface_masks[index]; + transformed.clutter_surface_masks[target_index] |= + descriptor.clutter_surface_masks[index]; + transformed.free_space_masks[target_index] |= descriptor.free_space_masks[index]; + transformed.height_u8[target_index] = + transformed.height_u8[target_index].max(descriptor.height_u8[index]); + } + } + Some(transformed) +} + +fn height_map_cell_height(height_map: &XrDepthAlignHeightMap, index: usize) -> Option { + height_map + .heights_meters + .get(index) + .copied() + .filter(|height| height.is_finite()) +} + +fn height_map_signal_weight( + height_map: &XrDepthAlignHeightMap, + height: f32, + gradient_magnitude: f32, + straightness: f32, +) -> f32 { + let height_span = (height_map.top_y_meters - height_map.bottom_y_meters).max(1.0e-3); + let height_bias = ((height - height_map.bottom_y_meters) / height_span).clamp(0.0, 1.0); + (gradient_magnitude * height_map.cell_size_meters.max(1.0e-3) * 2.0).clamp(0.08, 2.8) + * (0.85 + 0.15 * height_bias) + * (0.75 + 0.60 * straightness.clamp(0.0, 1.0)) +} + +fn height_map_cell_signal( + height_map: &XrDepthAlignHeightMap, + x: usize, + z: usize, +) -> Option<(f32, Vec3f)> { + let size_x = height_map.size_x_usize(); + let size_z = height_map.size_z_usize(); + if size_x < 3 || size_z < 3 || x == 0 || z == 0 || x + 1 >= size_x || z + 1 >= size_z { + return None; + } + let center = height_map_cell_height(height_map, height_map.cell_index(x, z))?; + let left = height_map_cell_height(height_map, height_map.cell_index(x - 1, z))?; + let right = height_map_cell_height(height_map, height_map.cell_index(x + 1, z))?; + let up = height_map_cell_height(height_map, height_map.cell_index(x, z - 1))?; + let down = height_map_cell_height(height_map, height_map.cell_index(x, z + 1))?; + let cell_size = height_map.cell_size_meters.max(1.0e-3); + let gradient = vec3( + (right - left) / (2.0 * cell_size), + 0.0, + (down - up) / (2.0 * cell_size), + ); + Some((center, gradient)) +} + +fn height_map_straightness( + height_map: &XrDepthAlignHeightMap, + x: usize, + z: usize, + normal: Vec3f, +) -> f32 { + let mut alignment_sum = 0.0; + let mut count = 0usize; + for (nx, nz) in [(x - 1, z), (x + 1, z), (x, z - 1), (x, z + 1)] { + let Some((_height, gradient)) = height_map_cell_signal(height_map, nx, nz) else { + continue; + }; + let magnitude = gradient.length(); + if magnitude < XR_DEPTH_ALIGN_HEIGHT_MAP_GRADIENT_MIN_METERS * 0.7 { + continue; + } + let Some(neighbor_normal) = align_safe_normalize(gradient) else { + continue; + }; + alignment_sum += normal.dot(neighbor_normal).abs(); + count += 1; + } + if count == 0 { + 0.5 + } else { + (alignment_sum / count as f32).clamp(0.0, 1.0) + } +} + +fn build_height_map_signal_cells(height_map: &XrDepthAlignHeightMap) -> Vec { + let size_x = height_map.size_x_usize(); + let size_z = height_map.size_z_usize(); + if size_x < 3 || size_z < 3 || height_map.heights_meters.len() != size_x * size_z { + return Vec::new(); + } + let mut signal = Vec::::new(); + for z in (1..size_z - 1).step_by(2) { + for x in (1..size_x - 1).step_by(2) { + let Some((height, gradient)) = height_map_cell_signal(height_map, x, z) else { + continue; + }; + let magnitude = gradient.length(); + if magnitude < XR_DEPTH_ALIGN_HEIGHT_MAP_GRADIENT_MIN_METERS { + continue; + } + let Some(normal) = align_safe_normalize(gradient) else { + continue; + }; + let straightness = height_map_straightness(height_map, x, z, normal); + let weight = height_map_signal_weight(height_map, height, magnitude, straightness); + if weight < 0.10 { + continue; + } + signal.push(HeightMapSignalCell { + point: vec3( + height_map.origin_x + (x as f32 + 0.5) * height_map.cell_size_meters, + 0.0, + height_map.origin_z + (z as f32 + 0.5) * height_map.cell_size_meters, + ), + height, + gradient, + axis_xz: vec2f(normal.x, normal.z), + weight, + }); + } + } + signal +} + +fn height_map_flat_surface_cell( + height_map: &XrDepthAlignHeightMap, + x: usize, + z: usize, +) -> Option<(f32, f32)> { + let size_x = height_map.size_x_usize(); + let size_z = height_map.size_z_usize(); + if size_x < 3 || size_z < 3 || x == 0 || z == 0 || x + 1 >= size_x || z + 1 >= size_z { + return None; + } + let center = height_map_cell_height(height_map, height_map.cell_index(x, z))?; + let relative_height = center - height_map.floor_y_meters; + if relative_height > XR_DEPTH_ALIGN_VERTICAL_SURFACE_MAX_RELATIVE_HEIGHT_METERS { + return None; + } + let Some((_height, gradient)) = height_map_cell_signal(height_map, x, z) else { + return None; + }; + let gradient_magnitude = gradient.length(); + if gradient_magnitude > XR_DEPTH_ALIGN_VERTICAL_SURFACE_MAX_GRADIENT_METERS { + return None; + } + let mut support = 0usize; + let mut mean_abs_delta = 0.0f32; + let mut max_abs_delta = 0.0f32; + for nz in z - 1..=z + 1 { + for nx in x - 1..=x + 1 { + let Some(height) = height_map_cell_height(height_map, height_map.cell_index(nx, nz)) + else { + continue; + }; + let abs_delta = (height - center).abs(); + support += 1; + mean_abs_delta += abs_delta; + max_abs_delta = max_abs_delta.max(abs_delta); + } + } + if support < XR_DEPTH_ALIGN_VERTICAL_SURFACE_MIN_NEIGHBORS + || max_abs_delta > XR_DEPTH_ALIGN_VERTICAL_SURFACE_MAX_NEIGHBOR_DELTA_METERS + { + return None; + } + mean_abs_delta /= support as f32; + let support_ratio = support as f32 / 9.0; + let gradient_score = 1.0 + - (gradient_magnitude / XR_DEPTH_ALIGN_VERTICAL_SURFACE_MAX_GRADIENT_METERS) + .clamp(0.0, 1.0); + let plateau_score = 1.0 + - (max_abs_delta / XR_DEPTH_ALIGN_VERTICAL_SURFACE_MAX_NEIGHBOR_DELTA_METERS) + .clamp(0.0, 1.0); + let consistency_score = 1.0 + - (mean_abs_delta / XR_DEPTH_ALIGN_VERTICAL_SURFACE_MAX_NEIGHBOR_DELTA_METERS) + .clamp(0.0, 1.0); + let weight = (0.25 + 0.75 * support_ratio) + * (0.30 + 0.70 * gradient_score) + * (0.30 + 0.70 * plateau_score) + * (0.30 + 0.70 * consistency_score); + (weight > 0.05).then_some((center, weight.clamp(0.05, 1.0))) +} + +fn build_flat_height_map_surface_cells( + height_map: &XrDepthAlignHeightMap, +) -> Vec { + let size_x = height_map.size_x_usize(); + let size_z = height_map.size_z_usize(); + if size_x < 3 || size_z < 3 || height_map.heights_meters.len() != size_x * size_z { + return Vec::new(); + } + let mut surfaces = Vec::::new(); + for z in 1..size_z - 1 { + for x in 1..size_x - 1 { + let Some((height, weight)) = height_map_flat_surface_cell(height_map, x, z) else { + continue; + }; + surfaces.push(FlatHeightMapSurfaceCell { + point_x: height_map.origin_x + (x as f32 + 0.5) * height_map.cell_size_meters, + point_z: height_map.origin_z + (z as f32 + 0.5) * height_map.cell_size_meters, + height, + weight, + }); + } + } + surfaces +} + +fn select_height_map_alignment_signal_cells( + signal_cells: &[HeightMapSignalCell], +) -> Vec { + let mut candidates = signal_cells.to_vec(); + candidates.sort_by(|a, b| b.weight.total_cmp(&a.weight)); + let mut selected = Vec::::new(); + for candidate in candidates { + if selected.iter().any(|existing| { + let delta = existing.point - candidate.point; + (delta.x * delta.x + delta.z * delta.z).sqrt() + < XR_DEPTH_ALIGN_HEIGHT_MAP_MIN_SPACING_METERS + }) { + continue; + } + selected.push(candidate); + if selected.len() >= XR_DEPTH_ALIGN_HEIGHT_MAP_MAX_SAMPLES { + break; + } + } + selected +} + +fn build_height_map_signal_histogram( + signal_cells: &[HeightMapSignalCell], + bin_count: usize, +) -> Vec { + let bin_count = bin_count.max(1); + let mut histogram = vec![0.0; bin_count]; + for cell in signal_cells { + let angle = cell.axis_xz.x.atan2(-cell.axis_xz.y); + let normalized = (angle + std::f32::consts::PI) / std::f32::consts::TAU; + let bin = (normalized * bin_count as f32).floor() as isize; + histogram[bin.rem_euclid(bin_count as isize) as usize] += cell.weight.max(0.01); + } + normalize_histogram(&mut histogram); + histogram +} + +fn normalize_histogram(histogram: &mut [f32]) { + let total = histogram.iter().copied().sum::(); + if total > 0.0 { + for value in histogram { + *value = (*value / total * 100.0).round() / 100.0; + } + } +} + +fn decimate_signal_cells(signal_cells: &[T], max_samples: usize) -> Vec { + if signal_cells.len() <= max_samples { + return signal_cells.to_vec(); + } + let stride = signal_cells.len().div_ceil(max_samples); + signal_cells + .iter() + .copied() + .step_by(stride.max(1)) + .collect() +} + +fn build_height_map_sample_cache( + height_map: &XrDepthAlignHeightMap, +) -> Option { + let size_x = height_map.size_x_usize(); + let size_z = height_map.size_z_usize(); + if size_x < 3 || size_z < 3 || height_map.heights_meters.len() != size_x * size_z { + return None; + } + let mut signal_axes = vec![vec2f(f32::NAN, f32::NAN); size_x * size_z]; + for z in 1..size_z - 1 { + for x in 1..size_x - 1 { + let Some((_height, gradient)) = height_map_cell_signal(height_map, x, z) else { + continue; + }; + let Some(axis) = xz_axis(gradient) else { + continue; + }; + let index = height_map.cell_index(x, z); + signal_axes[index] = vec2f(axis.x, axis.z); + } + } + let cell_size_meters = height_map.cell_size_meters.max(1.0e-3); + Some(HeightMapSampleCache { + origin_x: height_map.origin_x, + origin_z: height_map.origin_z, + inv_cell_size_meters: cell_size_meters.recip(), + size_x, + size_z, + signal_axes, + }) +} + +fn descriptor_signal_cells(descriptor: &XrDepthAlignDescriptor) -> Vec { + descriptor + .height_map + .as_ref() + .map(build_height_map_signal_cells) + .unwrap_or_default() +} + +fn descriptor_dense_signal_cells( + descriptor: &XrDepthAlignDescriptor, +) -> Vec { + descriptor_signal_cells(descriptor) + .into_iter() + .map(|cell| DenseHeightMapSignalCell { + point_x: cell.point.x, + point_z: cell.point.z, + height: cell.height, + axis_x: cell.axis_xz.x, + axis_z: cell.axis_xz.y, + weight: cell.weight, + }) + .collect() +} + +fn descriptor_flat_surface_cells( + descriptor: &XrDepthAlignDescriptor, +) -> Vec { + descriptor + .height_map + .as_ref() + .map(build_flat_height_map_surface_cells) + .unwrap_or_default() +} + +fn selected_descriptor_signal_cells( + descriptor: &XrDepthAlignDescriptor, +) -> Vec { + select_height_map_alignment_signal_cells(&descriptor_signal_cells(descriptor)) +} + +fn sample_height_map_nearest( + height_map: &XrDepthAlignHeightMap, + world_x: f32, + world_z: f32, +) -> Option { + let size_x = height_map.size_x_usize(); + let size_z = height_map.size_z_usize(); + if size_x == 0 || size_z == 0 || height_map.heights_meters.len() != size_x * size_z { + return None; + } + let cell_size = height_map.cell_size_meters.max(1.0e-3); + let grid_x = ((world_x - height_map.origin_x) / cell_size).floor() as isize; + let grid_z = ((world_z - height_map.origin_z) / cell_size).floor() as isize; + if grid_x < 0 || grid_z < 0 || grid_x >= size_x as isize || grid_z >= size_z as isize { + return None; + } + height_map_cell_height( + height_map, + height_map.cell_index(grid_x as usize, grid_z as usize), + ) +} + +fn sample_height_map_bilinear( + height_map: &XrDepthAlignHeightMap, + world_x: f32, + world_z: f32, +) -> Option { + let size_x = height_map.size_x_usize(); + let size_z = height_map.size_z_usize(); + if size_x == 0 || size_z == 0 || height_map.heights_meters.len() != size_x * size_z { + return None; + } + if size_x == 1 && size_z == 1 { + return height_map_cell_height(height_map, 0); + } + + let cell_size = height_map.cell_size_meters.max(1.0e-3); + let sample_x = + ((world_x - height_map.origin_x) / cell_size - 0.5).clamp(0.0, size_x as f32 - 1.0); + let sample_z = + ((world_z - height_map.origin_z) / cell_size - 0.5).clamp(0.0, size_z as f32 - 1.0); + let x0 = sample_x.floor() as usize; + let z0 = sample_z.floor() as usize; + let x1 = (x0 + 1).min(size_x - 1); + let z1 = (z0 + 1).min(size_z - 1); + let fx = (sample_x - x0 as f32).clamp(0.0, 1.0); + let fz = (sample_z - z0 as f32).clamp(0.0, 1.0); + + let h00 = height_map_cell_height(height_map, height_map.cell_index(x0, z0)); + let h10 = height_map_cell_height(height_map, height_map.cell_index(x1, z0)); + let h01 = height_map_cell_height(height_map, height_map.cell_index(x0, z1)); + let h11 = height_map_cell_height(height_map, height_map.cell_index(x1, z1)); + match (h00, h10, h01, h11) { + (Some(h00), Some(h10), Some(h01), Some(h11)) => { + let hx0 = h00 + (h10 - h00) * fx; + let hx1 = h01 + (h11 - h01) * fx; + Some(hx0 + (hx1 - hx0) * fz) + } + _ => sample_height_map_nearest(height_map, world_x, world_z), + } +} + +#[inline(always)] +fn sample_height_map_bilinear_and_axis_fast( + height_map: &XrDepthAlignHeightMap, + sample_cache: &HeightMapSampleCache, + world_x: f32, + world_z: f32, +) -> Option<(f32, Option)> { + let size_x = sample_cache.size_x; + let size_z = sample_cache.size_z; + let heights = &height_map.heights_meters; + if size_x == 0 || size_z == 0 || heights.len() != size_x * size_z { + return None; + } + if size_x == 1 && size_z == 1 { + let height = *heights.first()?; + return height.is_finite().then_some((height, None)); + } + + let raw_x = (world_x - sample_cache.origin_x) * sample_cache.inv_cell_size_meters; + let raw_z = (world_z - sample_cache.origin_z) * sample_cache.inv_cell_size_meters; + let sample_x = (raw_x - 0.5).clamp(0.0, size_x as f32 - 1.0); + let sample_z = (raw_z - 0.5).clamp(0.0, size_z as f32 - 1.0); + let x0 = sample_x.floor() as usize; + let z0 = sample_z.floor() as usize; + let x1 = (x0 + 1).min(size_x - 1); + let z1 = (z0 + 1).min(size_z - 1); + let fx = (sample_x - x0 as f32).clamp(0.0, 1.0); + let fz = (sample_z - z0 as f32).clamp(0.0, 1.0); + + let index00 = x0 + z0 * size_x; + let index10 = x1 + z0 * size_x; + let index01 = x0 + z1 * size_x; + let index11 = x1 + z1 * size_x; + let h00 = heights[index00]; + let h10 = heights[index10]; + let h01 = heights[index01]; + let h11 = heights[index11]; + let height = if h00.is_finite() && h10.is_finite() && h01.is_finite() && h11.is_finite() { + let hx0 = h00 + (h10 - h00) * fx; + let hx1 = h01 + (h11 - h01) * fx; + hx0 + (hx1 - hx0) * fz + } else { + let grid_x = raw_x.floor() as isize; + let grid_z = raw_z.floor() as isize; + if grid_x < 0 || grid_z < 0 || grid_x >= size_x as isize || grid_z >= size_z as isize { + return None; + } + let height = heights[grid_x as usize + grid_z as usize * size_x]; + if !height.is_finite() { + return None; + } + height + }; + + let axis = { + let grid_x = raw_x.round() as isize; + let grid_z = raw_z.round() as isize; + if grid_x <= 0 + || grid_z <= 0 + || grid_x + 1 >= size_x as isize + || grid_z + 1 >= size_z as isize + { + None + } else { + let index = grid_x as usize + grid_z as usize * size_x; + let axis = sample_cache.signal_axes[index]; + (axis.x.is_finite() && axis.y.is_finite()).then_some(axis) + } + }; + + Some((height, axis)) +} + +fn sample_height_map_flat_surface_weight( + height_map: &XrDepthAlignHeightMap, + world_x: f32, + world_z: f32, +) -> Option { + let size_x = height_map.size_x_usize(); + let size_z = height_map.size_z_usize(); + if size_x < 3 || size_z < 3 || height_map.heights_meters.len() != size_x * size_z { + return None; + } + let cell_size = height_map.cell_size_meters.max(1.0e-3); + let grid_x = ((world_x - height_map.origin_x) / cell_size - 0.5).round() as isize; + let grid_z = ((world_z - height_map.origin_z) / cell_size - 0.5).round() as isize; + if grid_x <= 0 || grid_z <= 0 || grid_x + 1 >= size_x as isize || grid_z + 1 >= size_z as isize + { + return None; + } + height_map_flat_surface_cell(height_map, grid_x as usize, grid_z as usize) + .map(|(_, weight)| weight) +} + +fn weighted_delta_median(samples: &[(f32, f32)]) -> Option { + let mut sorted = samples + .iter() + .copied() + .filter(|(delta, weight)| delta.is_finite() && *weight > 0.0) + .collect::>(); + if sorted.is_empty() { + return None; + } + sorted.sort_by(|left, right| left.0.total_cmp(&right.0)); + let total_weight = sorted.iter().map(|(_, weight)| *weight).sum::(); + if total_weight <= 1.0e-4 { + return None; + } + let target = total_weight * 0.5; + let mut running_weight = 0.0f32; + for (delta, weight) in sorted { + running_weight += weight; + if running_weight >= target { + return Some(delta); + } + } + None +} + +fn score_height_map_alignment( + local_map: &XrDepthAlignHeightMap, + local_sample_cache: Option<&HeightMapSampleCache>, + remote_map: &XrDepthAlignHeightMap, + remote_signal: &[DenseHeightMapSignalCell], + yaw: f32, + translation: Vec3f, +) -> (f32, f32, usize) { + score_height_map_alignment_with_stride( + local_map, + local_sample_cache, + remote_map, + remote_signal, + yaw, + translation, + 1, + ) +} + +fn fill_rotated_height_map_signal( + rotated: &mut Vec, + remote_signal: &[DenseHeightMapSignalCell], + yaw: f32, +) { + let (sin_yaw, cos_yaw) = yaw.sin_cos(); + rotated.clear(); + rotated.reserve(remote_signal.len()); + fill_rotated_height_map_signal_scalar(rotated, remote_signal, sin_yaw, cos_yaw) +} + +fn fill_rotated_height_map_signal_scalar( + rotated: &mut Vec, + remote_signal: &[DenseHeightMapSignalCell], + sin_yaw: f32, + cos_yaw: f32, +) { + for cell in remote_signal { + let (point_x, point_z) = + rotate_xz_quat_with_sin_cos(sin_yaw, cos_yaw, cell.point_x, cell.point_z); + let (axis_x, axis_z) = + rotate_xz_quat_with_sin_cos(sin_yaw, cos_yaw, cell.axis_x, cell.axis_z); + rotated.push(RotatedHeightMapSignalCell { + point_x, + point_z, + axis_x, + axis_z, + height: cell.height, + weight: cell.weight, + }); + } +} + +fn score_height_map_alignment_with_stride( + local_map: &XrDepthAlignHeightMap, + local_sample_cache: Option<&HeightMapSampleCache>, + remote_map: &XrDepthAlignHeightMap, + remote_signal: &[DenseHeightMapSignalCell], + yaw: f32, + translation: Vec3f, + sample_stride: usize, +) -> (f32, f32, usize) { + if remote_signal.is_empty() { + return (0.0, f32::INFINITY, 0); + } + let (sin_yaw, cos_yaw) = yaw.sin_cos(); + let mapped_remote_cutout_center = remote_map + .player_cutout_center + .map(|center| rotate_xz_quat_with_sin_cos(sin_yaw, cos_yaw, center.x, center.y)); + let mapped_remote_cutout_center = mapped_remote_cutout_center + .map(|(center_x, center_z)| (center_x + translation.x, center_z + translation.z)); + let mapped_remote_cutout_radius = + (remote_map.player_cutout_radius_meters + 0.14).max(remote_map.cell_size_meters * 2.0); + let mapped_remote_cutout_radius_sq = mapped_remote_cutout_radius * mapped_remote_cutout_radius; + let mut support_sum = 0.0; + let mut weight_sum = 0.0; + let mut total_weight = 0.0; + let mut residual_sum = 0.0; + let mut matched = 0usize; + let sample_stride = sample_stride.max(1); + if let Some(sample_cache) = local_sample_cache { + for cell in remote_signal.iter().step_by(sample_stride) { + total_weight += cell.weight; + let (rotated_x, rotated_z) = + rotate_xz_quat_with_sin_cos(sin_yaw, cos_yaw, cell.point_x, cell.point_z); + let mapped_x = rotated_x + translation.x; + let mapped_z = rotated_z + translation.z; + if mapped_remote_cutout_center.is_some_and(|(center_x, center_z)| { + let delta_x = mapped_x - center_x; + let delta_z = mapped_z - center_z; + delta_x * delta_x + delta_z * delta_z <= mapped_remote_cutout_radius_sq + }) { + continue; + } + let Some((local_height, local_axis)) = sample_height_map_bilinear_and_axis_fast( + local_map, + sample_cache, + mapped_x, + mapped_z, + ) else { + continue; + }; + let diff = (local_height - cell.height).abs(); + let height_similarity = (1.0 - diff / 0.45).clamp(0.0, 1.0); + let (remote_axis_x, remote_axis_z) = + rotate_xz_quat_with_sin_cos(sin_yaw, cos_yaw, cell.axis_x, cell.axis_z); + let direction_similarity = local_axis + .map(|local_axis| { + (local_axis.x * remote_axis_x + local_axis.y * remote_axis_z).clamp(0.0, 1.0) + }) + .unwrap_or(0.5); + let similarity = + (height_similarity * 0.65 + direction_similarity * 0.35).clamp(0.0, 1.0); + support_sum += cell.weight * similarity; + weight_sum += cell.weight; + residual_sum += diff; + matched += 1; + } + } else { + for cell in remote_signal.iter().step_by(sample_stride) { + total_weight += cell.weight; + let (rotated_x, rotated_z) = + rotate_xz_quat_with_sin_cos(sin_yaw, cos_yaw, cell.point_x, cell.point_z); + let mapped_x = rotated_x + translation.x; + let mapped_z = rotated_z + translation.z; + if mapped_remote_cutout_center.is_some_and(|(center_x, center_z)| { + let delta_x = mapped_x - center_x; + let delta_z = mapped_z - center_z; + delta_x * delta_x + delta_z * delta_z <= mapped_remote_cutout_radius_sq + }) { + continue; + } + let Some(local_height) = sample_height_map_bilinear(local_map, mapped_x, mapped_z) + else { + continue; + }; + let diff = (local_height - cell.height).abs(); + let height_similarity = (1.0 - diff / 0.45).clamp(0.0, 1.0); + let similarity = (height_similarity * 0.65 + 0.5 * 0.35).clamp(0.0, 1.0); + support_sum += cell.weight * similarity; + weight_sum += cell.weight; + residual_sum += diff; + matched += 1; + } + } + + if matched < 8 || weight_sum <= 1.0e-4 || total_weight <= 1.0e-4 { + return (0.0, f32::INFINITY, matched); + } + let coverage = (weight_sum / total_weight).clamp(0.0, 1.0); + if coverage < 0.20 { + return (0.0, f32::INFINITY, matched); + } + ( + (support_sum / weight_sum) * coverage.sqrt(), + residual_sum / matched as f32, + matched, + ) +} + +fn score_rotated_height_map_alignment( + local_map: &XrDepthAlignHeightMap, + local_sample_cache: Option<&HeightMapSampleCache>, + remote_map: &XrDepthAlignHeightMap, + rotated_remote_signal: &[RotatedHeightMapSignalCell], + yaw: f32, + translation: Vec3f, +) -> (f32, f32, usize) { + if rotated_remote_signal.is_empty() { + return (0.0, f32::INFINITY, 0); + } + let (sin_yaw, cos_yaw) = yaw.sin_cos(); + let mapped_remote_cutout_center = remote_map + .player_cutout_center + .map(|center| rotate_xz_quat_with_sin_cos(sin_yaw, cos_yaw, center.x, center.y)); + let mapped_remote_cutout_center = mapped_remote_cutout_center + .map(|(center_x, center_z)| (center_x + translation.x, center_z + translation.z)); + let mapped_remote_cutout_radius = + (remote_map.player_cutout_radius_meters + 0.14).max(remote_map.cell_size_meters * 2.0); + let mapped_remote_cutout_radius_sq = mapped_remote_cutout_radius * mapped_remote_cutout_radius; + let mut support_sum = 0.0; + let mut weight_sum = 0.0; + let mut total_weight = 0.0; + let mut residual_sum = 0.0; + let mut matched = 0usize; + if let Some(sample_cache) = local_sample_cache { + for cell in rotated_remote_signal { + total_weight += cell.weight; + let mapped_x = cell.point_x + translation.x; + let mapped_z = cell.point_z + translation.z; + if mapped_remote_cutout_center.is_some_and(|(center_x, center_z)| { + let delta_x = mapped_x - center_x; + let delta_z = mapped_z - center_z; + delta_x * delta_x + delta_z * delta_z <= mapped_remote_cutout_radius_sq + }) { + continue; + } + let Some((local_height, local_axis)) = sample_height_map_bilinear_and_axis_fast( + local_map, + sample_cache, + mapped_x, + mapped_z, + ) else { + continue; + }; + let diff = (local_height - cell.height).abs(); + let height_similarity = (1.0 - diff / 0.45).clamp(0.0, 1.0); + let direction_similarity = local_axis + .map(|local_axis| { + (local_axis.x * cell.axis_x + local_axis.y * cell.axis_z).clamp(0.0, 1.0) + }) + .unwrap_or(0.5); + let similarity = + (height_similarity * 0.65 + direction_similarity * 0.35).clamp(0.0, 1.0); + support_sum += cell.weight * similarity; + weight_sum += cell.weight; + residual_sum += diff; + matched += 1; + } + } else { + for cell in rotated_remote_signal { + total_weight += cell.weight; + let mapped_x = cell.point_x + translation.x; + let mapped_z = cell.point_z + translation.z; + if mapped_remote_cutout_center.is_some_and(|(center_x, center_z)| { + let delta_x = mapped_x - center_x; + let delta_z = mapped_z - center_z; + delta_x * delta_x + delta_z * delta_z <= mapped_remote_cutout_radius_sq + }) { + continue; + } + let Some(local_height) = sample_height_map_bilinear(local_map, mapped_x, mapped_z) + else { + continue; + }; + let diff = (local_height - cell.height).abs(); + let height_similarity = (1.0 - diff / 0.45).clamp(0.0, 1.0); + let similarity = (height_similarity * 0.65 + 0.5 * 0.35).clamp(0.0, 1.0); + support_sum += cell.weight * similarity; + weight_sum += cell.weight; + residual_sum += diff; + matched += 1; + } + } + + if matched < 8 || weight_sum <= 1.0e-4 || total_weight <= 1.0e-4 { + return (0.0, f32::INFINITY, matched); + } + let coverage = (weight_sum / total_weight).clamp(0.0, 1.0); + if coverage < 0.20 { + return (0.0, f32::INFINITY, matched); + } + ( + (support_sum / weight_sum) * coverage.sqrt(), + residual_sum / matched as f32, + matched, + ) +} + +fn estimate_height_map_vertical_offset( + local_map: &XrDepthAlignHeightMap, + remote_map: &XrDepthAlignHeightMap, + remote_signal: &[FlatHeightMapSurfaceCell], + yaw: f32, + translation: Vec3f, +) -> Option { + if remote_signal.len() < XR_DEPTH_ALIGN_VERTICAL_OFFSET_MIN_MATCHES { + return None; + } + let (sin_yaw, cos_yaw) = yaw.sin_cos(); + let mapped_remote_cutout_center = remote_map + .player_cutout_center + .map(|center| rotate_xz_quat_with_sin_cos(sin_yaw, cos_yaw, center.x, center.y)); + let mapped_remote_cutout_center = mapped_remote_cutout_center + .map(|(center_x, center_z)| (center_x + translation.x, center_z + translation.z)); + let mapped_remote_cutout_radius = + (remote_map.player_cutout_radius_meters + 0.14).max(remote_map.cell_size_meters * 2.0); + let mapped_remote_cutout_radius_sq = mapped_remote_cutout_radius * mapped_remote_cutout_radius; + let min_delta = translation.y - XR_DEPTH_ALIGN_VERTICAL_OFFSET_MAX_DELTA_METERS; + let max_delta = translation.y + XR_DEPTH_ALIGN_VERTICAL_OFFSET_MAX_DELTA_METERS; + let mut deltas = Vec::<(f32, f32)>::new(); + let mut total_weight = 0.0f32; + + for cell in remote_signal { + let (rotated_x, rotated_z) = + rotate_xz_quat_with_sin_cos(sin_yaw, cos_yaw, cell.point_x, cell.point_z); + let mapped_x = rotated_x + translation.x; + let mapped_z = rotated_z + translation.z; + if mapped_remote_cutout_center.is_some_and(|(center_x, center_z)| { + let delta_x = mapped_x - center_x; + let delta_z = mapped_z - center_z; + delta_x * delta_x + delta_z * delta_z <= mapped_remote_cutout_radius_sq + }) { + continue; + } + let Some(local_height) = sample_height_map_bilinear(local_map, mapped_x, mapped_z) else { + continue; + }; + let Some(local_weight) = + sample_height_map_flat_surface_weight(local_map, mapped_x, mapped_z) + else { + continue; + }; + let delta = local_height - cell.height; + if !delta.is_finite() || delta < min_delta || delta > max_delta { + continue; + } + let weight = cell.weight.max(0.01) * local_weight.max(0.01); + total_weight += weight; + deltas.push((delta, weight)); + } + + if deltas.len() < XR_DEPTH_ALIGN_VERTICAL_OFFSET_MIN_MATCHES || total_weight <= 1.0e-4 { + return None; + } + let median = weighted_delta_median(&deltas)?; + let consensus = deltas + .iter() + .copied() + .filter(|(delta, _)| { + (*delta - median).abs() <= XR_DEPTH_ALIGN_VERTICAL_OFFSET_CONSENSUS_RADIUS_METERS + }) + .collect::>(); + let consensus_weight = consensus.iter().map(|(_, weight)| *weight).sum::(); + if consensus.len() < XR_DEPTH_ALIGN_VERTICAL_OFFSET_MIN_MATCHES + || consensus_weight < total_weight * XR_DEPTH_ALIGN_VERTICAL_OFFSET_MIN_SUPPORT_RATIO + { + return None; + } + weighted_delta_median(&consensus).or(Some(median)) +} + +fn refine_solution_vertical_offset_from_overlap( + local_map: &XrDepthAlignHeightMap, + remote_map: &XrDepthAlignHeightMap, + remote_signal: &[FlatHeightMapSurfaceCell], + current: XrDepthAlignSolution, +) -> XrDepthAlignSolution { + let Some(refined_y) = estimate_height_map_vertical_offset( + local_map, + remote_map, + remote_signal, + current.yaw_radians, + current.translation, + ) else { + return current; + }; + let mut corrected = current; + corrected.translation.y = refined_y; + corrected +} + +fn apply_height_map_alignment_support( + candidate: XrDepthAlignSolution, + local_map: Option<&XrDepthAlignHeightMap>, + local_sample_cache: Option<&HeightMapSampleCache>, + remote_map: Option<&XrDepthAlignHeightMap>, + remote_signal: &[DenseHeightMapSignalCell], +) -> XrDepthAlignSolution { + let (Some(local_map), Some(remote_map)) = (local_map, remote_map) else { + return candidate; + }; + let (support, residual, matched) = score_height_map_alignment( + local_map, + local_sample_cache, + remote_map, + remote_signal, + candidate.yaw_radians, + candidate.translation, + ); + if matched == 0 { + return candidate; + } + let mut candidate = candidate; + candidate.confidence = (candidate.confidence * 0.45 + support * 0.55).clamp(0.0, 1.0); + candidate.symmetry_confidence = support.clamp(0.0, 1.0); + if residual.is_finite() { + candidate.residual_meters = if candidate.residual_meters.is_finite() { + candidate.residual_meters * 0.55 + residual * 0.45 + } else { + residual + }; + } + candidate +} + +fn score_full_alignment_solution( + local_signal: &[HeightMapSignalCell], + remote_signal: &[HeightMapSignalCell], + local_map: Option<&XrDepthAlignHeightMap>, + local_sample_cache: Option<&HeightMapSampleCache>, + remote_map: Option<&XrDepthAlignHeightMap>, + remote_dense_signal: &[DenseHeightMapSignalCell], + yaw: f32, + translation: Vec3f, +) -> XrDepthAlignSolution { + apply_height_map_alignment_support( + score_signal_alignment_solution(local_signal, remote_signal, yaw, translation), + local_map, + local_sample_cache, + remote_map, + remote_dense_signal, + ) +} + +fn wall_profile_yaw_sidecar_is_safe( + current: XrDepthAlignSolution, + corrected: XrDepthAlignSolution, + current_profile_score: f32, + best_profile_score: f32, +) -> bool { + let yaw_delta = wrap_angle(corrected.yaw_radians - current.yaw_radians).abs(); + if yaw_delta < XR_DEPTH_ALIGN_WALL_PROFILE_MIN_YAW_DELTA_RADIANS { + return false; + } + if best_profile_score < current_profile_score + XR_DEPTH_ALIGN_WALL_PROFILE_MIN_SCORE_GAIN + && best_profile_score < current_profile_score * XR_DEPTH_ALIGN_WALL_PROFILE_MIN_SCORE_RATIO + { + return false; + } + corrected.confidence >= current.confidence * XR_DEPTH_ALIGN_WALL_PROFILE_MIN_CONFIDENCE_RATIO + && corrected.matched_samples + XR_DEPTH_ALIGN_WALL_PROFILE_MAX_MATCH_LOSS + >= current.matched_samples + && corrected.residual_meters.is_finite() + && current.residual_meters.is_finite() + && corrected.residual_meters + <= current.residual_meters + XR_DEPTH_ALIGN_WALL_PROFILE_MAX_RESIDUAL_INCREASE_METERS +} + +fn height_map_score_better(candidate: (f32, f32, usize), current: (f32, f32, usize)) -> bool { + candidate + .0 + .partial_cmp(¤t.0) + .unwrap_or(std::cmp::Ordering::Equal) + .then_with(|| { + current + .1 + .partial_cmp(&candidate.1) + .unwrap_or(std::cmp::Ordering::Equal) + }) + .then_with(|| candidate.2.cmp(¤t.2)) + .is_gt() +} + +fn candidate_signal_yaws( + local_histogram: &[f32], + remote_histogram: &[f32], + local_signal: &[HeightMapSignalCell], + remote_signal: &[HeightMapSignalCell], +) -> Vec { + let mut candidates = vec![0.0]; + if local_histogram.len() == remote_histogram.len() && !local_histogram.is_empty() { + let bins = local_histogram.len(); + let mut shifts = Vec::<(f32, usize)>::new(); + for shift in 0..bins { + let score = (0..bins) + .map(|index| { + local_histogram[index] * remote_histogram[(index + bins - shift) % bins] + }) + .sum::(); + shifts.push((score, shift)); + } + shifts.sort_by(|a, b| b.0.total_cmp(&a.0)); + for (_, shift) in shifts.into_iter().take(8) { + candidates.push(wrap_angle( + shift as f32 * std::f32::consts::TAU / bins as f32, + )); + } + } + + for local_cell in local_signal.iter().take(16) { + for remote_cell in remote_signal.iter().take(16) { + candidates.push(wrap_angle(signed_xz_angle_2d( + remote_cell.axis_xz.x, + remote_cell.axis_xz.y, + local_cell.axis_xz.x, + local_cell.axis_xz.y, + ))); + } + } + dedupe_angles(candidates, 0.05) +} + +fn candidate_signal_translations( + local_signal: &[HeightMapSignalCell], + remote_signal: &[HeightMapSignalCell], + floor_y: f32, + yaw: f32, +) -> Vec { + let mut votes = HashMap::<(i32, i32), TranslationVote>::new(); + for local_cell in local_signal.iter().take(64) { + let Some(local_axis) = xz_axis(local_cell.gradient) else { + continue; + }; + for remote_cell in remote_signal.iter().take(64) { + let Some(remote_axis) = xz_axis(remote_cell.gradient) else { + continue; + }; + let rotated_remote_axis = rotate_y(yaw, remote_axis); + let alignment = local_axis.dot(rotated_remote_axis); + if alignment < XR_DEPTH_ALIGN_SIGNAL_MATCH_MIN_DIRECTION_DOT { + continue; + } + let height_delta = (local_cell.height - remote_cell.height).abs(); + if height_delta > XR_DEPTH_ALIGN_SIGNAL_MATCH_MAX_HEIGHT_DELTA_METERS { + continue; + } + let delta = local_cell.point - rotate_y(yaw, remote_cell.point); + if delta.x.abs() > 8.0 || delta.z.abs() > 8.0 { + continue; + } + let height_factor = (1.0 + - height_delta / XR_DEPTH_ALIGN_SIGNAL_MATCH_MAX_HEIGHT_DELTA_METERS) + .clamp(0.0, 1.0); + let weight = (local_cell.weight * remote_cell.weight).sqrt() + * (0.30 + 0.70 * alignment) + * (0.35 + 0.65 * height_factor); + let key = ( + quantize_translation_axis(delta.x, XR_DEPTH_ALIGN_TRANSLATION_VOTE_STEP_METERS), + quantize_translation_axis(delta.z, XR_DEPTH_ALIGN_TRANSLATION_VOTE_STEP_METERS), + ); + let vote = votes.entry(key).or_default(); + vote.score += weight; + vote.weight_sum += weight; + vote.sum_x += delta.x * weight; + vote.sum_z += delta.z * weight; + vote.count += 1; + } + } + if votes.is_empty() { + return vec![vec3(0.0, floor_y, 0.0)]; + } + let mut ranked = votes.into_iter().collect::>(); + ranked.sort_by(|a, b| { + b.1.score + .total_cmp(&a.1.score) + .then_with(|| b.1.count.cmp(&a.1.count)) + }); + let translations = ranked + .into_iter() + .take(12) + .filter_map(|(_, vote)| { + (vote.weight_sum > 0.0).then_some(vec3( + vote.sum_x / vote.weight_sum, + floor_y, + vote.sum_z / vote.weight_sum, + )) + }) + .collect::>(); + dedupe_translations(translations, 0.08) +} + +#[derive(Default)] +struct TranslationVote { + score: f32, + weight_sum: f32, + sum_x: f32, + sum_z: f32, + count: usize, +} + +#[derive(Clone, Copy)] +struct HeightMapSignalMatch<'a> { + local: &'a HeightMapSignalCell, + remote: &'a HeightMapSignalCell, + planar_distance: f32, + height_distance: f32, + alignment: f32, + score: f32, +} + +fn collect_unique_signal_matches<'a>( + local_signal: &'a [HeightMapSignalCell], + remote_signal: &'a [HeightMapSignalCell], + yaw: f32, + translation: Vec3f, +) -> Vec> { + #[derive(Clone, Copy)] + struct Candidate<'a> { + local_index: usize, + remote_index: usize, + local: &'a HeightMapSignalCell, + remote: &'a HeightMapSignalCell, + planar_distance: f32, + height_distance: f32, + alignment: f32, + score: f32, + } + + let mut candidates = Vec::>::new(); + for (remote_index, remote_cell) in remote_signal.iter().enumerate() { + let transformed_point = rotate_y(yaw, remote_cell.point) + translation; + let Some(transformed_axis) = xz_axis(rotate_y(yaw, remote_cell.gradient)) else { + continue; + }; + for (local_index, local_cell) in local_signal.iter().enumerate() { + let Some(local_axis) = xz_axis(local_cell.gradient) else { + continue; + }; + let alignment = local_axis.dot(transformed_axis); + if alignment < XR_DEPTH_ALIGN_SIGNAL_MATCH_MIN_DIRECTION_DOT { + continue; + } + let planar_delta = local_cell.point - transformed_point; + let planar_distance = + (planar_delta.x * planar_delta.x + planar_delta.z * planar_delta.z).sqrt(); + if planar_distance > XR_DEPTH_ALIGN_SIGNAL_MATCH_RADIUS_METERS { + continue; + } + let height_distance = (local_cell.height - remote_cell.height).abs(); + if height_distance > XR_DEPTH_ALIGN_SIGNAL_MATCH_MAX_HEIGHT_DELTA_METERS { + continue; + } + let height_factor = (1.0 + - height_distance / XR_DEPTH_ALIGN_SIGNAL_MATCH_MAX_HEIGHT_DELTA_METERS) + .clamp(0.0, 1.0); + let score = (local_cell.weight * remote_cell.weight).sqrt() + * (0.30 + 0.70 * alignment) + * (-planar_distance / XR_DEPTH_ALIGN_SIGNAL_MATCH_RADIUS_METERS.max(0.05)).exp() + * (0.35 + 0.65 * height_factor); + candidates.push(Candidate { + local_index, + remote_index, + local: local_cell, + remote: remote_cell, + planar_distance, + height_distance, + alignment, + score, + }); + } + } + candidates.sort_by(|a, b| { + b.score + .total_cmp(&a.score) + .then_with(|| a.planar_distance.total_cmp(&b.planar_distance)) + }); + + let mut used_local = vec![false; local_signal.len()]; + let mut used_remote = vec![false; remote_signal.len()]; + let mut matches = Vec::>::new(); + for candidate in candidates { + if used_local[candidate.local_index] || used_remote[candidate.remote_index] { + continue; + } + used_local[candidate.local_index] = true; + used_remote[candidate.remote_index] = true; + matches.push(HeightMapSignalMatch { + local: candidate.local, + remote: candidate.remote, + planar_distance: candidate.planar_distance, + height_distance: candidate.height_distance, + alignment: candidate.alignment, + score: candidate.score, + }); + } + matches +} + +fn refine_signal_alignment( + local_signal: &[HeightMapSignalCell], + remote_signal: &[HeightMapSignalCell], + floor_y: f32, + yaw: f32, + translation: Vec3f, +) -> (f32, Vec3f) { + let mut refined_yaw = yaw; + let mut refined_translation = translation; + refined_translation.y = floor_y; + for _ in 0..2 { + let matches = collect_unique_signal_matches( + local_signal, + remote_signal, + refined_yaw, + refined_translation, + ); + let mut translation_sum = vec3(0.0, 0.0, 0.0); + let mut translation_weight_sum = 0.0; + let mut yaw_sin = 0.0; + let mut yaw_cos = 0.0; + let mut yaw_weight_sum = 0.0; + for matched in matches { + let Some(local_axis) = xz_axis(matched.local.gradient) else { + continue; + }; + let Some(remote_axis) = xz_axis(matched.remote.gradient) else { + continue; + }; + let height_factor = (1.0 + - matched.height_distance / XR_DEPTH_ALIGN_SIGNAL_MATCH_MAX_HEIGHT_DELTA_METERS) + .clamp(0.0, 1.0); + let weight = (matched.local.weight * matched.remote.weight).sqrt() + * (0.30 + 0.70 * matched.alignment) + * (0.35 + 0.65 * height_factor); + let candidate_translation = + matched.local.point - rotate_y(refined_yaw, matched.remote.point); + translation_sum += candidate_translation * weight; + translation_weight_sum += weight; + let candidate_yaw = wrap_angle(signed_xz_angle(remote_axis, local_axis)); + yaw_sin += candidate_yaw.sin() * weight; + yaw_cos += candidate_yaw.cos() * weight; + yaw_weight_sum += weight; + } + if yaw_weight_sum > 0.0 { + refined_yaw = wrap_angle(yaw_sin.atan2(yaw_cos)); + } + if translation_weight_sum > 0.0 { + refined_translation = translation_sum * (1.0 / translation_weight_sum); + refined_translation.y = floor_y; + } + } + (refined_yaw, refined_translation) +} + +fn score_signal_alignment_solution( + local_signal: &[HeightMapSignalCell], + remote_signal: &[HeightMapSignalCell], + yaw: f32, + translation: Vec3f, +) -> XrDepthAlignSolution { + let matches = collect_unique_signal_matches(local_signal, remote_signal, yaw, translation); + let mut total_score = 0.0; + let mut residual_sum = 0.0; + let max_score = remote_signal + .iter() + .map(|cell| cell.weight.max(0.01)) + .sum::() + .max(0.01); + for matched in &matches { + total_score += matched.score; + residual_sum += matched.planar_distance * 0.65 + matched.height_distance * 0.35; + } + let matched_samples = matches.len(); + let residual_meters = if matched_samples > 0 { + residual_sum / matched_samples as f32 + } else { + f32::INFINITY + }; + let coverage = (matched_samples as f32 / remote_signal.len().max(1) as f32).clamp(0.0, 1.0); + let residual_confidence = if residual_meters.is_finite() { + (1.0 - (residual_meters / 0.50)).clamp(0.0, 1.0) + } else { + 0.0 + }; + let confidence = ((total_score / max_score).clamp(0.0, 1.0) + * coverage.sqrt() + * residual_confidence.max(0.2)) + .clamp(0.0, 1.0); + XrDepthAlignSolution { + yaw_radians: wrap_angle(yaw), + translation, + confidence, + symmetry_confidence: 1.0, + residual_meters, + matched_samples, + } +} + +fn seeded_alignment_lock_is_strong( + diagnostic: &XrDepthAlignSolveDiagnostic, + candidate: XrDepthAlignSolution, + previous: XrDepthAlignSolution, + remote_signal_count: usize, + local_map: Option<&XrDepthAlignHeightMap>, + remote_map: Option<&XrDepthAlignHeightMap>, +) -> bool { + if !candidate.is_accepted(diagnostic) { + return false; + } + if candidate.confidence < XR_DEPTH_ALIGN_SEEDED_LOCK_MIN_CONFIDENCE + || candidate.symmetry_confidence < XR_DEPTH_ALIGN_SEEDED_LOCK_MIN_SYMMETRY_CONFIDENCE + { + return false; + } + let overlap = candidate.matched_samples as f32 / remote_signal_count.max(1) as f32; + if overlap < XR_DEPTH_ALIGN_SEEDED_LOCK_MIN_OVERLAP { + return false; + } + let max_cell_size = local_map + .map(|map| map.cell_size_meters) + .unwrap_or(0.03) + .max(remote_map.map(|map| map.cell_size_meters).unwrap_or(0.03)); + // Dense gradient matching keeps some soft residual even on a stable lock, so + // the seeded fast-path needs a slightly looser gate than the old wall-only solver. + let max_residual_meters = (max_cell_size * 6.0).clamp(0.12, 0.26); + if !candidate.residual_meters.is_finite() || candidate.residual_meters > max_residual_meters { + return false; + } + let translation_jump = vec3( + candidate.translation.x - previous.translation.x, + 0.0, + candidate.translation.z - previous.translation.z, + ) + .length(); + translation_jump <= XR_DEPTH_ALIGN_SEEDED_LOCK_MAX_TRANSLATION_JUMP_METERS + && wrap_angle(candidate.yaw_radians - previous.yaw_radians).abs() + <= XR_DEPTH_ALIGN_SEEDED_LOCK_MAX_YAW_JUMP_RADIANS +} + +fn dedupe_angles(angles: Vec, epsilon: f32) -> Vec { + let mut deduped = Vec::::new(); + for angle in angles { + if deduped + .iter() + .any(|existing| wrap_angle(*existing - angle).abs() <= epsilon) + { + continue; + } + deduped.push(wrap_angle(angle)); + } + deduped +} + +fn dedupe_translations(translations: Vec, step: f32) -> Vec { + let mut deduped = Vec::::new(); + for translation in translations { + if deduped.iter().any(|existing| { + let delta = *existing - translation; + delta.x.abs() <= step && delta.y.abs() <= step && delta.z.abs() <= step + }) { + continue; + } + deduped.push(translation); + } + deduped +} + +fn alignment_solution_is_distinct( + candidate: XrDepthAlignSolution, + current: XrDepthAlignSolution, + translation_epsilon: f32, + yaw_epsilon: f32, +) -> bool { + let translation_delta = vec3( + candidate.translation.x - current.translation.x, + 0.0, + candidate.translation.z - current.translation.z, + ) + .length(); + let yaw_delta = wrap_angle(candidate.yaw_radians - current.yaw_radians).abs(); + translation_delta > translation_epsilon || yaw_delta > yaw_epsilon +} + +fn push_shortlisted_alignment_solution( + shortlist: &mut Vec, + candidate: XrDepthAlignSolution, + max_candidates: usize, + translation_epsilon: f32, + yaw_epsilon: f32, +) { + if shortlist.iter().any(|existing| { + !alignment_solution_is_distinct(candidate, *existing, translation_epsilon, yaw_epsilon) + }) { + if let Some(existing) = shortlist.iter_mut().find(|existing| { + !alignment_solution_is_distinct(candidate, **existing, translation_epsilon, yaw_epsilon) + }) { + if alignment_solution_better(&candidate, existing) { + *existing = candidate; + } + } + } else { + shortlist.push(candidate); + } + shortlist.sort_by(|left, right| { + right + .ranking_confidence() + .partial_cmp(&left.ranking_confidence()) + .unwrap_or(std::cmp::Ordering::Equal) + .then_with(|| right.matched_samples.cmp(&left.matched_samples)) + .then_with(|| { + left.residual_meters + .partial_cmp(&right.residual_meters) + .unwrap_or(std::cmp::Ordering::Equal) + }) + }); + if shortlist.len() > max_candidates { + shortlist.truncate(max_candidates); + } +} + +fn quantize_translation_axis(value: f32, step: f32) -> i32 { + (value / step.max(f32::EPSILON)).round() as i32 +} + +fn alignment_solution_better( + candidate: &XrDepthAlignSolution, + current: &XrDepthAlignSolution, +) -> bool { + candidate + .ranking_confidence() + .partial_cmp(¤t.ranking_confidence()) + .unwrap_or(std::cmp::Ordering::Equal) + .then_with(|| { + candidate + .symmetry_confidence + .partial_cmp(¤t.symmetry_confidence) + .unwrap_or(std::cmp::Ordering::Equal) + }) + .then_with(|| { + candidate + .confidence + .partial_cmp(¤t.confidence) + .unwrap_or(std::cmp::Ordering::Equal) + }) + .then_with(|| candidate.matched_samples.cmp(¤t.matched_samples)) + .then_with(|| { + current + .residual_meters + .partial_cmp(&candidate.residual_meters) + .unwrap_or(std::cmp::Ordering::Equal) + }) + .is_gt() +} + +fn align_safe_normalize(v: Vec3f) -> Option { + let len = v.length(); + (len > 0.0001).then_some(v * (1.0 / len)) +} + +fn yaw_rotation(yaw: f32) -> Quat { + Quat::from_axis_angle(vec3(0.0, 1.0, 0.0), yaw) +} + +fn rotate_y(yaw: f32, vector: Vec3f) -> Vec3f { + yaw_rotation(yaw).rotate_vec3(&vector) +} + +fn rotate_xz(yaw: f32, x: f32, z: f32) -> (f32, f32) { + let (sin_yaw, cos_yaw) = yaw.sin_cos(); + (x * cos_yaw - z * sin_yaw, x * sin_yaw + z * cos_yaw) +} + +#[inline(always)] +fn rotate_xz_quat_with_sin_cos(sin_yaw: f32, cos_yaw: f32, x: f32, z: f32) -> (f32, f32) { + (x * cos_yaw + z * sin_yaw, z * cos_yaw - x * sin_yaw) +} + +fn signed_xz_angle_2d(from_x: f32, from_z: f32, to_x: f32, to_z: f32) -> f32 { + let cross = from_z * to_x - from_x * to_z; + let dot = from_x * to_x + from_z * to_z; + cross.atan2(dot) +} + +fn xz_axis(vector: Vec3f) -> Option { + align_safe_normalize(vec3(vector.x, 0.0, vector.z)) +} + +fn signed_xz_angle(from: Vec3f, to: Vec3f) -> f32 { + let cross = from.z * to.x - from.x * to.z; + let dot = from.x * to.x + from.z * to.z; + cross.atan2(dot) +} + +fn wrap_angle(mut angle: f32) -> f32 { + while angle <= -std::f32::consts::PI { + angle += std::f32::consts::TAU; + } + while angle > std::f32::consts::PI { + angle -= std::f32::consts::TAU; + } + angle +} + +#[cfg(test)] +mod tests { + use super::*; + use std::{ + fs, + path::{Path, PathBuf}, + time::Instant, + }; + + #[derive(Clone, Copy, Debug, Default)] + struct HeightMapTestArtifacts { + cutout_center: Option, + occlusion_center: Option, + extra_blob_center: Option, + noise_seed: f32, + noise_scale: f32, + height_bias: f32, + } + + #[derive(Clone, Copy, Debug)] + struct TestRng(u64); + + #[derive(Debug, Default)] + struct RandomCaseSummary { + case_count: usize, + accepted_cases: usize, + seeded_reuse_cases: usize, + max_yaw_error: f32, + max_translation_error: f32, + failures: Vec, + } + + impl RandomCaseSummary { + fn merge(&mut self, other: Self) { + self.case_count += other.case_count; + self.accepted_cases += other.accepted_cases; + self.seeded_reuse_cases += other.seeded_reuse_cases; + self.max_yaw_error = self.max_yaw_error.max(other.max_yaw_error); + self.max_translation_error = + self.max_translation_error.max(other.max_translation_error); + self.failures.extend(other.failures); + } + } + + impl TestRng { + fn new(seed: u64) -> Self { + Self(seed) + } + + fn next_u32(&mut self) -> u32 { + self.0 = self + .0 + .wrapping_mul(6364136223846793005) + .wrapping_add(1442695040888963407); + (self.0 >> 32) as u32 + } + + fn next_f32(&mut self) -> f32 { + self.next_u32() as f32 / u32::MAX as f32 + } + + fn range_f32(&mut self, min: f32, max: f32) -> f32 { + min + (max - min) * self.next_f32() + } + + fn chance(&mut self, probability: f32) -> bool { + self.next_f32() <= probability + } + } + + fn angle_error(a: f32, b: f32) -> f32 { + wrap_angle(a - b).abs() + } + + fn synthetic_scene_height(point: Vec2f) -> f32 { + let mut height: f32 = 0.02; + if point.x.abs() >= 2.05 && point.x.abs() <= 2.25 && point.y >= -2.30 && point.y <= 2.10 { + height = height.max(2.15); + } + if point.y >= 1.75 && point.y <= 1.95 && point.x >= -2.25 && point.x <= 2.25 { + height = height.max(2.15); + } + if point.y <= -1.95 && point.y >= -2.20 && point.x >= -2.25 && point.x <= 2.05 { + height = height.max(2.15); + } + if point.x >= -0.95 && point.x <= -0.10 && point.y >= -0.42 && point.y <= 0.36 { + height = height.max(0.84); + } + if point.x >= 0.52 && point.x <= 1.12 && point.y >= 0.52 && point.y <= 1.18 { + height = height.max(1.38); + } + if point.x >= -1.58 && point.x <= -1.20 && point.y >= 0.92 && point.y <= 1.34 { + height = height.max(1.62); + } + if point.x >= 0.96 && point.x <= 1.34 && point.y >= -1.42 && point.y <= -0.66 { + height = height.max(0.68); + } + let wobble = ((point.x * 1.13 + point.y * 0.73).sin() * 0.018) + + ((point.x * 0.41 - point.y * 1.51).cos() * 0.014); + (height + wobble).clamp(0.0, 2.25) + } + + fn deterministic_height_noise(point: Vec2f, seed: f32) -> f32 { + ((point.x * 2.73 + point.y * 1.91 + seed * 0.37).sin() * 0.021) + + ((point.x * 0.84 - point.y * 3.14 + seed * 0.61).cos() * 0.017) + } + + fn random_artifacts(rng: &mut TestRng) -> HeightMapTestArtifacts { + HeightMapTestArtifacts { + cutout_center: rng + .chance(0.8) + .then(|| vec2f(rng.range_f32(-0.55, 0.55), rng.range_f32(-0.55, 0.55))), + occlusion_center: rng + .chance(0.7) + .then(|| vec2f(rng.range_f32(-1.15, 1.15), rng.range_f32(-0.95, 0.95))), + extra_blob_center: None, + noise_seed: rng.range_f32(-8.0, 8.0), + noise_scale: rng.range_f32(0.25, 0.85), + height_bias: rng.range_f32(-0.05, 0.05), + } + } + + fn make_height_map_descriptor_with_artifacts( + map_to_scene: Mat4f, + artifacts: HeightMapTestArtifacts, + size_x: usize, + size_z: usize, + ) -> XrDepthAlignDescriptor { + let cell_size_meters = 0.05; + let extent_x = size_x as f32 * cell_size_meters; + let extent_z = size_z as f32 * cell_size_meters; + let origin_x = -extent_x * 0.5; + let origin_z = -extent_z * 0.5; + let bottom_y_meters = 0.0; + let top_y_meters = 2.3; + let floor_y_meters = 0.0; + let mut heights_meters = vec![f32::NAN; size_x * size_z]; + for z in 0..size_z { + for x in 0..size_x { + let map_point = vec2f( + origin_x + (x as f32 + 0.5) * cell_size_meters, + origin_z + (z as f32 + 0.5) * cell_size_meters, + ); + if artifacts + .cutout_center + .is_some_and(|center| (map_point - center).length() <= 0.36) + { + continue; + } + if artifacts.occlusion_center.is_some_and(|center| { + (map_point.x - center.x).abs() <= 0.52 && (map_point.y - center.y).abs() <= 0.44 + }) { + continue; + } + let scene_point = map_to_scene + .transform_vec4(vec4f(map_point.x, 0.0, map_point.y, 1.0)) + .to_vec3f(); + let mut height = synthetic_scene_height(vec2f(scene_point.x, scene_point.z)); + if artifacts + .extra_blob_center + .is_some_and(|center| (map_point - center).length() <= 0.30) + { + height = height.max(1.68); + } + height += deterministic_height_noise(map_point, artifacts.noise_seed) + * artifacts.noise_scale; + height += artifacts.height_bias; + height = height.clamp(0.0, 2.25); + heights_meters[x + z * size_x] = height; + } + } + XrDepthAlignDescriptor { + voxel_size_meters: 0.05, + floor_y: floor_y_meters, + wall_normal_histogram: Vec::new(), + samples: Vec::new(), + vertical_descriptor: None, + height_map: Some(XrDepthAlignHeightMap { + origin_x, + origin_z, + cell_size_meters, + size_x: size_x as u16, + size_z: size_z as u16, + bottom_y_meters, + top_y_meters, + floor_y_meters, + player_cutout_center: artifacts.cutout_center, + player_cutout_radius_meters: 0.36, + heights_meters, + }), + } + } + + fn run_random_noisy_cases( + seed: u64, + case_count: usize, + size_x: usize, + size_z: usize, + ) -> RandomCaseSummary { + let mut rng = TestRng::new(seed); + let mut summary = RandomCaseSummary { + case_count, + ..RandomCaseSummary::default() + }; + + for case_index in 0..case_count { + let expected_yaw = rng.range_f32(-0.85, 0.85); + let expected_translation = + vec3(rng.range_f32(-0.75, 0.75), 0.0, rng.range_f32(-0.75, 0.75)); + let remote_to_local = Pose::new( + Quat::from_axis_angle(vec3(0.0, 1.0, 0.0), expected_yaw), + expected_translation, + ) + .to_mat4(); + + let mut local_artifacts = random_artifacts(&mut rng); + let remote_artifacts = random_artifacts(&mut rng); + if let Some(remote_cutout_center) = remote_artifacts.cutout_center { + let mapped_remote_center = rotate_y( + expected_yaw, + vec3(remote_cutout_center.x, 0.0, remote_cutout_center.y), + ) + expected_translation; + local_artifacts.extra_blob_center = + Some(vec2f(mapped_remote_center.x, mapped_remote_center.z)); + } + + let local = make_height_map_descriptor_with_artifacts( + Mat4f::identity(), + local_artifacts, + size_x, + size_z, + ); + let remote = make_height_map_descriptor_with_artifacts( + remote_to_local, + remote_artifacts, + size_x, + size_z, + ); + + let diagnostic = local.analyze_remote_to_local(&remote); + let Some(solution) = diagnostic.accepted_solution() else { + summary.failures.push(format!( + "case {case_index}: no accepted solution for yaw={expected_yaw:.3} translation=({:.3},{:.3}) diag={diagnostic:?}", + expected_translation.x, expected_translation.z + )); + continue; + }; + + let yaw_error = angle_error(solution.yaw_radians, expected_yaw); + let translation_error = (solution.translation - expected_translation).length(); + summary.max_yaw_error = summary.max_yaw_error.max(yaw_error); + summary.max_translation_error = summary.max_translation_error.max(translation_error); + if yaw_error > 0.18 || translation_error > 0.26 { + summary.failures.push(format!( + "case {case_index}: large error yaw={yaw_error:.3} translation={translation_error:.3} solution={solution:?}" + )); + continue; + } + + summary.accepted_cases += 1; + + let seeded = local.analyze_remote_to_local_seeded(&remote, Some(solution)); + if seeded.yaw_candidate_count == 1 && seeded.pose_candidate_count == 1 { + summary.seeded_reuse_cases += 1; + } + } + + summary + } + + fn make_height_map_descriptor(map_to_scene: Mat4f) -> XrDepthAlignDescriptor { + let size = 120usize; + let cell_size_meters = 0.05; + let extent = size as f32 * cell_size_meters; + let origin = -extent * 0.5; + let bottom_y_meters = 0.0; + let top_y_meters = 2.3; + let floor_y_meters = 0.0; + let mut heights_meters = vec![f32::NAN; size * size]; + for z in 0..size { + for x in 0..size { + let point = vec2f( + origin + (x as f32 + 0.5) * cell_size_meters, + origin + (z as f32 + 0.5) * cell_size_meters, + ); + let scene_point = map_to_scene + .transform_vec4(vec4f(point.x, 0.0, point.y, 1.0)) + .to_vec3f(); + heights_meters[x + z * size] = + synthetic_scene_height(vec2f(scene_point.x, scene_point.z)); + } + } + XrDepthAlignDescriptor { + voxel_size_meters: 0.05, + floor_y: floor_y_meters, + wall_normal_histogram: Vec::new(), + samples: Vec::new(), + vertical_descriptor: None, + height_map: Some(XrDepthAlignHeightMap { + origin_x: origin, + origin_z: origin, + cell_size_meters, + size_x: size as u16, + size_z: size as u16, + bottom_y_meters, + top_y_meters, + floor_y_meters, + player_cutout_center: None, + player_cutout_radius_meters: 0.0, + heights_meters, + }), + } + } + + #[derive(Clone, Copy, Debug, Default)] + struct ReferenceManualPose { + shift_x_meters: f32, + shift_z_meters: f32, + rotation_radians: f32, + } + + fn reference_dump_path() -> PathBuf { + PathBuf::from(env!("CARGO_MANIFEST_DIR")) + .join("dump/dumps/align-pair-226a39e4b300-r0097-1774792873191.bin") + } + + fn reference_manual_pose_path(dump_path: &Path) -> PathBuf { + let stem = dump_path + .file_stem() + .and_then(|stem| stem.to_str()) + .unwrap_or("align-pair"); + dump_path.with_file_name(format!("{stem}.manual_pose.ron")) + } + + fn load_reference_manual_pose(dump_path: &Path) -> Option { + let text = fs::read_to_string(reference_manual_pose_path(dump_path)).ok()?; + let mut pose = ReferenceManualPose::default(); + for line in text.lines() { + let (key, value) = line.split_once(':')?; + let value = value.trim(); + match key.trim() { + "shift_x_meters" => pose.shift_x_meters = value.parse().ok()?, + "shift_y_meters" => pose.shift_z_meters = value.parse().ok()?, + "rotation_radians" => pose.rotation_radians = value.parse().ok()?, + _ => {} + } + } + Some(pose) + } + + #[test] + fn dense_height_map_solver_recovers_rotated_translated_pose() { + let local = make_height_map_descriptor(Mat4f::identity()); + let expected_yaw = 0.58; + let expected_translation = vec3f(-0.82, 0.0, 0.67); + let remote_to_local = Pose::new( + Quat::from_axis_angle(vec3f(0.0, 1.0, 0.0), expected_yaw), + expected_translation, + ) + .to_mat4(); + let remote = local.transformed(&remote_to_local.invert()); + + let solution = local + .analyze_remote_to_local(&remote) + .accepted_solution() + .expect("dense solver should recover the pose"); + + assert!( + angle_error(solution.yaw_radians, expected_yaw) < 0.12, + "{solution:?}" + ); + assert!( + (solution.translation - expected_translation).length() < 0.18, + "{solution:?}" + ); + assert!(solution.confidence > 0.14, "{solution:?}"); + assert!(solution.matched_samples >= 6, "{solution:?}"); + } + + #[test] + fn dense_height_map_solver_refines_vertical_offset_from_overlap() { + let local = make_height_map_descriptor(Mat4f::identity()); + let expected_yaw = 0.41; + let expected_translation = vec3f(-0.46, 0.24, 0.58); + let remote_to_local = Pose::new( + Quat::from_axis_angle(vec3f(0.0, 1.0, 0.0), expected_yaw), + expected_translation, + ) + .to_mat4(); + let mut remote = local.transformed(&remote_to_local.invert()); + let poisoned_floor_bias = 0.33; + remote.floor_y += poisoned_floor_bias; + if let Some(height_map) = &mut remote.height_map { + height_map.floor_y_meters += poisoned_floor_bias; + } + + let solution = local + .analyze_remote_to_local(&remote) + .accepted_solution() + .expect("dense solver should recover vertical offset from overlap"); + + assert!( + angle_error(solution.yaw_radians, expected_yaw) < 0.12, + "{solution:?}" + ); + assert!( + vec3( + solution.translation.x - expected_translation.x, + 0.0, + solution.translation.z - expected_translation.z, + ) + .length() + < 0.18, + "{solution:?}" + ); + assert!( + (solution.translation.y - expected_translation.y).abs() < 0.06, + "{solution:?}" + ); + assert!( + (solution.translation.y - (local.floor_y - remote.floor_y)).abs() > 0.12, + "{solution:?}" + ); + } + + #[test] + fn flat_vertical_offset_samples_focus_on_floor_and_tables() { + let descriptor = make_height_map_descriptor(Mat4f::identity()); + let height_map = descriptor + .height_map + .as_ref() + .expect("synthetic descriptor should carry a height map"); + let samples = descriptor_flat_surface_cells(&descriptor); + assert!(!samples.is_empty()); + assert!(samples.iter().all(|cell| { + cell.height - height_map.floor_y_meters + <= XR_DEPTH_ALIGN_VERTICAL_SURFACE_MAX_RELATIVE_HEIGHT_METERS + 1.0e-3 + })); + assert!(samples.iter().any(|cell| { + let relative_height = cell.height - height_map.floor_y_meters; + relative_height <= 0.08 + })); + assert!(samples.iter().any(|cell| { + let relative_height = cell.height - height_map.floor_y_meters; + relative_height >= 0.45 + && relative_height <= XR_DEPTH_ALIGN_VERTICAL_SURFACE_MAX_RELATIVE_HEIGHT_METERS + })); + } + + #[test] + fn dense_seeded_solver_reuses_stable_lock() { + let local = make_height_map_descriptor(Mat4f::identity()); + let first_remote_to_local = Pose::new( + Quat::from_axis_angle(vec3(0.0, 1.0, 0.0), 0.34), + vec3(-0.52, 0.0, 0.46), + ) + .to_mat4(); + let first_remote = local.transformed(&first_remote_to_local.invert()); + let first_solution = local + .analyze_remote_to_local(&first_remote) + .accepted_solution() + .expect("expected initial alignment"); + + let diagnostic = local.analyze_remote_to_local_seeded(&first_remote, Some(first_solution)); + let solution = diagnostic + .accepted_solution() + .expect("expected seeded solve to reuse the stable lock"); + + assert_eq!(diagnostic.yaw_candidate_count, 1, "{diagnostic:?}"); + assert_eq!(diagnostic.pose_candidate_count, 1, "{diagnostic:?}"); + assert!( + angle_error(solution.yaw_radians, 0.34) < 0.03, + "{solution:?}" + ); + assert!( + (solution.translation - vec3(-0.52, 0.0, 0.46)).length() < 0.05, + "{solution:?}" + ); + } + + #[test] + fn dense_seeded_solver_recovers_after_seed_mismatch() { + let local = make_height_map_descriptor(Mat4f::identity()); + let first_remote_to_local = Pose::new( + Quat::from_axis_angle(vec3(0.0, 1.0, 0.0), -0.28), + vec3(0.42, 0.0, -0.34), + ) + .to_mat4(); + let first_remote = local.transformed(&first_remote_to_local.invert()); + let stale_seed = local + .analyze_remote_to_local(&first_remote) + .accepted_solution() + .expect("expected initial alignment"); + + let resumed_yaw = 1.02; + let resumed_translation = vec3(-0.64, 0.0, 0.78); + let resumed_remote_to_local = Pose::new( + Quat::from_axis_angle(vec3(0.0, 1.0, 0.0), resumed_yaw), + resumed_translation, + ) + .to_mat4(); + let resumed_remote = local.transformed(&resumed_remote_to_local.invert()); + let seeded = local.analyze_remote_to_local_seeded(&resumed_remote, Some(stale_seed)); + let seeded_solution = seeded + .accepted_solution() + .expect("expected seeded solve to fall back to global search"); + let global = local.analyze_remote_to_local(&resumed_remote); + let global_solution = global + .accepted_solution() + .expect("expected global solve to recover resumed pose"); + + assert!( + angle_error(seeded_solution.yaw_radians, global_solution.yaw_radians) < 0.05, + "{seeded_solution:?} {global_solution:?}" + ); + assert!( + (seeded_solution.translation - global_solution.translation).length() < 0.08, + "{seeded_solution:?} {global_solution:?}" + ); + } + + #[test] + fn dense_height_map_solver_handles_random_noisy_inputs() { + let summary = run_random_noisy_cases(0x5eed_cafe_d15c_a11e, 4, 120, 114); + assert!(summary.accepted_cases >= 4, "summary={summary:?}"); + assert!(summary.seeded_reuse_cases >= 2, "summary={summary:?}"); + } + + #[test] + fn timesliced_solver_matches_reference_dump_and_keeps_steps_bounded() { + let dump_path = reference_dump_path(); + let bytes = fs::read(&dump_path).expect("reference dump should exist"); + let pair = XrNetAlignmentDescriptorDumpPair::from_file_bytes(&bytes) + .expect("reference dump should decode"); + let manual_pose = + load_reference_manual_pose(&dump_path).expect("reference manual pose should exist"); + let local = &pair.local_descriptor.descriptor; + let remote = &pair.remote_descriptor.descriptor; + let mut matcher = XrDepthAlignMatcher::new(local, remote, None); + let mut measured_steps = 0u32; + let mut max_step = std::time::Duration::ZERO; + + while !matcher.is_finished() { + let started = Instant::now(); + let progressed = matcher.step(); + let elapsed = started.elapsed(); + if progressed { + measured_steps = measured_steps.saturating_add(1); + max_step = max_step.max(elapsed); + } + } + + let diagnostic = matcher + .diagnostic() + .expect("timesliced matcher should finish with a diagnostic"); + let solution = diagnostic + .accepted_solution() + .expect("reference dump should produce an accepted solution"); + let manual_translation = vec3( + manual_pose.shift_x_meters, + solution.translation.y, + manual_pose.shift_z_meters, + ); + let planar_error = vec3( + solution.translation.x - manual_translation.x, + 0.0, + solution.translation.z - manual_translation.z, + ) + .length(); + let yaw_error = wrap_angle(solution.yaw_radians - manual_pose.rotation_radians).abs(); + + assert_eq!(diagnostic.step_count, measured_steps, "{diagnostic:?}"); + assert!( + diagnostic.max_step_ms <= 50, + "diagnostic={diagnostic:?} measured_max_step={:?}", + max_step + ); + assert!(yaw_error <= 0.10, "{solution:?} manual={manual_pose:?}"); + assert!(planar_error <= 0.08, "{solution:?} manual={manual_pose:?}"); + } + + #[test] + #[ignore = "expensive randomized stress run"] + fn dense_height_map_solver_handles_random_noisy_inputs_parallel_stress() { + let mut handles = Vec::new(); + for thread_index in 0..16u64 { + handles.push(std::thread::spawn(move || { + run_random_noisy_cases( + 0x9e37_79b9_7f4a_7c15 ^ (thread_index.wrapping_mul(0x94d0_49bb_1331_11eb)), + 1, + 200, + 150, + ) + })); + } + + let mut summary = RandomCaseSummary::default(); + for handle in handles { + summary.merge( + handle + .join() + .expect("random stress worker should not panic"), + ); + } + + assert!( + summary.accepted_cases >= 14, + "parallel stress summary={summary:?}" + ); + assert!( + summary.seeded_reuse_cases >= 8, + "parallel stress summary={summary:?}" + ); + } +} diff --git a/xr/src/algorithms/mod.rs b/xr/src/algorithms/mod.rs new file mode 100644 index 0000000000..af39de8124 --- /dev/null +++ b/xr/src/algorithms/mod.rs @@ -0,0 +1,2 @@ +pub mod depth_align; +pub mod tsdf_query; diff --git a/xr/src/algorithms/tsdf_query.rs b/xr/src/algorithms/tsdf_query.rs new file mode 100644 index 0000000000..516660fba3 --- /dev/null +++ b/xr/src/algorithms/tsdf_query.rs @@ -0,0 +1,999 @@ +use crate::prelude::*; +use std::{ + collections::hash_map::DefaultHasher, + hash::{Hash, Hasher}, +}; + +const TSDF_QUERY_MIN_OPPOSING_NORMAL_DOT: f32 = 0.2; +const TSDF_QUERY_TSDF_SUPPORT_GRID_DIM: usize = 5; +const TSDF_QUERY_TSDF_SUPPORT_MAX_SAMPLES: usize = + TSDF_QUERY_TSDF_SUPPORT_GRID_DIM * TSDF_QUERY_TSDF_SUPPORT_GRID_DIM; +const TSDF_QUERY_TSDF_SUPPORT_MIN_SAMPLES: usize = 4; +const TSDF_QUERY_TRAJECTORY_SAMPLE_COUNT: usize = 5; +const TSDF_QUERY_TSDF_SUPPORT_NORMAL_Y_MIN: f32 = 0.60; +const TSDF_QUERY_TSDF_SUPPORT_RADIUS_SCALE: f32 = 1.15; +const TSDF_QUERY_TSDF_SUPPORT_RADIUS_MIN: f32 = 0.04; +const TSDF_QUERY_TSDF_SUPPORT_RADIUS_MAX: f32 = 0.12; +const TSDF_QUERY_TSDF_SUPPORT_EXTENT_PADDING_SCALE: f32 = 0.22; +const TSDF_QUERY_TSDF_IMPACT_MIN_SPEED: f32 = 0.55; +const TSDF_QUERY_TSDF_IMPACT_MIN_HORIZONTAL_SPEED: f32 = 0.40; +const TSDF_QUERY_TSDF_IMPACT_MIN_UPWARD_SPEED: f32 = 0.55; +const TSDF_QUERY_TSDF_IMPACT_NORMAL_Y_MAX: f32 = 0.72; +const TSDF_QUERY_TSDF_IMPACT_CEILING_NORMAL_Y_MIN: f32 = 0.82; +const TSDF_QUERY_TSDF_IMPACT_RAY_STEP_SCALE: f32 = 0.40; +const TSDF_QUERY_TSDF_IMPACT_RAY_STEP_MIN: f32 = 0.02; +const TSDF_QUERY_TSDF_IMPACT_EXTENT_SCALE: f32 = 1.20; +const TSDF_QUERY_TSDF_IMPACT_EXTENT_MIN: f32 = 0.05; +const TSDF_QUERY_TSDF_IMPACT_EXTENT_MAX: f32 = 0.16; +const TSDF_QUERY_CHUNK_CACHE_SLOTS: usize = 32; +pub(crate) const TSDF_QUERY_IMPACT_RESTITUTION: f32 = 0.38; + +#[derive(Clone, Copy, Debug)] +pub(crate) struct DepthQuery { + pub(crate) key: u64, + pub(crate) center: Vec3f, + pub(crate) predicted_center: Vec3f, + pub(crate) velocity: Vec3f, + pub(crate) radius: f32, + pub(crate) max_distance: f32, +} + +#[derive(Clone, Copy, Debug)] +pub(crate) struct DepthQuerySurfaceHit { + pub(crate) distance: f32, + pub(crate) point: Vec3f, + pub(crate) normal: Vec3f, + pub(crate) triangle: [Vec3f; 3], + pub(crate) patch: [Vec3f; 4], +} + +#[derive(Clone, Copy, Debug)] +pub(crate) struct DepthQuerySupportPlane { + pub(crate) point: Vec3f, + pub(crate) normal: Vec3f, + pub(crate) tangent: Vec3f, + pub(crate) bitangent: Vec3f, + pub(crate) half_extent_tangent: f32, + pub(crate) half_extent_bitangent: f32, +} + +#[derive(Clone, Copy, Debug)] +pub(crate) enum DepthQueryColliderGeometry { + HalfSpace(DepthQuerySupportPlane), +} + +#[derive(Clone, Copy, Debug, PartialEq, Eq, Hash)] +pub(crate) enum DepthQueryColliderRole { + Support, + Impact, +} + +#[derive(Clone, Copy, Debug)] +pub(crate) struct DepthQueryCollider { + pub(crate) fingerprint: u64, + pub(crate) geometry: DepthQueryColliderGeometry, + pub(crate) role: DepthQueryColliderRole, + pub(crate) restitution: f32, +} + +#[derive(Clone, Copy, Debug)] +pub(crate) struct DepthQueryResolvedSurface { + pub(crate) surface: DepthQuerySurfaceHit, + pub(crate) collider: DepthQueryCollider, +} + +#[allow(dead_code)] +#[derive(Clone, Debug)] +pub(crate) struct DepthQueryHit { + pub(crate) key: u64, + pub(crate) distance: f32, + pub(crate) point: Vec3f, + pub(crate) normal: Vec3f, + pub(crate) triangle: [Vec3f; 3], + pub(crate) patch: [Vec3f; 4], + pub(crate) collider: DepthQueryCollider, + pub(crate) additional_hits: Vec, +} + +#[allow(dead_code)] +#[derive(Clone, Debug)] +pub(crate) enum DepthQueryResult { + Hit(DepthQueryHit), + Miss { key: u64 }, +} + +#[derive(Clone, Copy, Debug, Default, Eq, Hash, PartialEq)] +struct TsdfVoxelCoord { + x: i32, + y: i32, + z: i32, +} + +impl TsdfVoxelCoord { + const fn new(x: i32, y: i32, z: i32) -> Self { + Self { x, y, z } + } +} + +impl core::ops::Add for TsdfVoxelCoord { + type Output = Self; + + fn add(self, rhs: Self) -> Self::Output { + Self::new(self.x + rhs.x, self.y + rhs.y, self.z + rhs.z) + } +} + +impl core::ops::Sub for TsdfVoxelCoord { + type Output = Self; + + fn sub(self, rhs: Self) -> Self::Output { + Self::new(self.x - rhs.x, self.y - rhs.y, self.z - rhs.z) + } +} + +#[allow(dead_code)] +#[derive(Clone, Copy, Debug)] +struct QueryTrajectorySample { + progress: f32, + point: Vec3f, + velocity: Vec3f, +} + +#[derive(Clone, Copy)] +struct DepthGridSupportSample { + point: Vec3f, + radial_weight: f32, +} + +#[derive(Clone, Copy)] +struct TsdfQueryChunkCacheEntry<'a> { + key: ChunkKey, + chunk: &'a SparseTsdReadChunk, +} + +struct TsdfQuerySampler<'a> { + grid: &'a SparseTsdGridReadSnapshot, + tsd_distance_meters: f32, + local_y_stride: usize, + local_z_stride: usize, + chunk_cache: [Option>; TSDF_QUERY_CHUNK_CACHE_SLOTS], +} + +#[derive(Clone, Copy)] +struct TsdfQueryVoxelColumn { + chunk_x: i32, + chunk_z: i32, + local_base: usize, +} + +#[derive(Clone, Copy)] +struct TsdfQueryBilinearColumn { + v00: TsdfQueryVoxelColumn, + v10: TsdfQueryVoxelColumn, + v01: TsdfQueryVoxelColumn, + v11: TsdfQueryVoxelColumn, + tx: f32, + tz: f32, +} + +fn depth_tsd_distance_meters(voxel_size_meters: f32) -> f32 { + voxel_size_meters * 2.0 +} + +fn quantize_f32(value: f32, quantum: f32) -> i32 { + (value / quantum.max(f32::EPSILON)).round() as i32 +} + +fn tsdf_query_chunk_cache_slot(key: ChunkKey) -> usize { + let hash = (key.x as u32).wrapping_mul(0x9E37_79B9) + ^ (key.y as u32).wrapping_mul(0x85EB_CA6B) + ^ (key.z as u32).wrapping_mul(0xC2B2_AE35); + hash as usize & (TSDF_QUERY_CHUNK_CACHE_SLOTS - 1) +} + +impl<'a> TsdfQuerySampler<'a> { + fn new(grid: &'a SparseTsdGridReadSnapshot) -> Self { + let chunk_edge = grid.chunk_edge.max(1) as usize; + Self { + grid, + tsd_distance_meters: depth_tsd_distance_meters(grid.voxel_size), + local_y_stride: chunk_edge, + local_z_stride: chunk_edge * chunk_edge, + chunk_cache: [None; TSDF_QUERY_CHUNK_CACHE_SLOTS], + } + } + + fn world_to_voxel_coord(&self, point: Vec3f) -> TsdfVoxelCoord { + let (x, y, z) = self.grid.world_to_voxel_xyz(point); + TsdfVoxelCoord::new(x, y, z) + } + + fn chunk(&mut self, key: ChunkKey) -> Option<&'a SparseTsdReadChunk> { + let slot = tsdf_query_chunk_cache_slot(key); + if let Some(entry) = self.chunk_cache[slot] { + if entry.key == key { + return Some(entry.chunk); + } + } + let chunk = self.grid.chunks.get(&key)?.as_ref(); + self.chunk_cache[slot] = Some(TsdfQueryChunkCacheEntry { key, chunk }); + Some(chunk) + } + + fn voxel_column(&self, x: i32, z: i32) -> TsdfQueryVoxelColumn { + let (chunk_x, local_x) = self.grid.chunk_axis_and_local(x); + let (chunk_z, local_z) = self.grid.chunk_axis_and_local(z); + TsdfQueryVoxelColumn { + chunk_x, + chunk_z, + local_base: local_x + local_z * self.local_z_stride, + } + } + + fn normalized_distance_at_y( + &mut self, + column: TsdfQueryVoxelColumn, + y_coord: i32, + ) -> Option { + let (chunk_y, local_y) = self.grid.chunk_axis_and_local(y_coord); + let local_id = column.local_base + local_y * self.local_y_stride; + self.chunk(ChunkKey::new(column.chunk_x, chunk_y, column.chunk_z))? + .value(local_id) + } + + fn bilinear_column(&self, sample_x: f32, sample_z: f32) -> TsdfQueryBilinearColumn { + let voxel_size = self.grid.voxel_size; + let grid_x = sample_x / voxel_size - 0.5; + let grid_z = sample_z / voxel_size - 0.5; + let x0 = grid_x.floor() as i32; + let z0 = grid_z.floor() as i32; + TsdfQueryBilinearColumn { + v00: self.voxel_column(x0, z0), + v10: self.voxel_column(x0 + 1, z0), + v01: self.voxel_column(x0, z0 + 1), + v11: self.voxel_column(x0 + 1, z0 + 1), + tx: grid_x - x0 as f32, + tz: grid_z - z0 as f32, + } + } + + fn bilinear_distance_at_y( + &mut self, + column: TsdfQueryBilinearColumn, + y_coord: i32, + ) -> Option { + let v00 = self.normalized_distance_at_y(column.v00, y_coord)?; + let v10 = self.normalized_distance_at_y(column.v10, y_coord)?; + let v01 = self.normalized_distance_at_y(column.v01, y_coord)?; + let v11 = self.normalized_distance_at_y(column.v11, y_coord)?; + + let vx0 = v00 + (v10 - v00) * column.tx; + let vx1 = v01 + (v11 - v01) * column.tx; + Some(vx0 + (vx1 - vx0) * column.tz) + } + + fn trilinear_distance(&mut self, point: Vec3f) -> Option { + let voxel_size = self.grid.voxel_size; + let grid_y = point.y / voxel_size - 0.5; + let y0 = grid_y.floor() as i32; + let ty = grid_y - y0 as f32; + let column = self.bilinear_column(point.x, point.z); + let s0 = self.bilinear_distance_at_y(column, y0)? * self.tsd_distance_meters; + let s1 = self.bilinear_distance_at_y(column, y0 + 1)? * self.tsd_distance_meters; + Some(s0 + (s1 - s0) * ty) + } + + fn distance_gradient(&mut self, point: Vec3f) -> Option { + let center = self.trilinear_distance(point)?; + let step = self + .grid + .voxel_size + .max(TSDF_QUERY_TSDF_IMPACT_RAY_STEP_MIN); + let dx = finite_difference_axis( + center, + self.trilinear_distance(point + vec3f(step, 0.0, 0.0)), + self.trilinear_distance(point - vec3f(step, 0.0, 0.0)), + step, + )?; + let dy = finite_difference_axis( + center, + self.trilinear_distance(point + vec3f(0.0, step, 0.0)), + self.trilinear_distance(point - vec3f(0.0, step, 0.0)), + step, + )?; + let dz = finite_difference_axis( + center, + self.trilinear_distance(point + vec3f(0.0, 0.0, step)), + self.trilinear_distance(point - vec3f(0.0, 0.0, step)), + step, + )?; + let gradient = vec3f(dx, dy, dz); + (gradient.length() > 1.0e-5).then_some(gradient.normalize()) + } +} + +fn voxel_center_axis(voxel_size: f32, coord: i32) -> f32 { + (coord as f32 + 0.5) * voxel_size +} + +fn finite_difference_axis( + center: f32, + forward: Option, + backward: Option, + step: f32, +) -> Option { + match (forward, backward) { + (Some(forward), Some(backward)) => Some((forward - backward) / (step * 2.0)), + (Some(forward), None) => Some((forward - center) / step), + (None, Some(backward)) => Some((center - backward) / step), + (None, None) => None, + } +} + +fn solve_linear3(mut a: [[f32; 3]; 3], mut b: [f32; 3]) -> Option<[f32; 3]> { + for pivot in 0..3 { + let mut best = pivot; + let mut best_value = a[pivot][pivot].abs(); + for row in (pivot + 1)..3 { + let candidate = a[row][pivot].abs(); + if candidate > best_value { + best = row; + best_value = candidate; + } + } + if best_value <= 1.0e-6 { + return None; + } + if best != pivot { + a.swap(best, pivot); + b.swap(best, pivot); + } + let inv = a[pivot][pivot].recip(); + for col in pivot..3 { + a[pivot][col] *= inv; + } + b[pivot] *= inv; + for row in 0..3 { + if row == pivot { + continue; + } + let factor = a[row][pivot]; + if factor.abs() <= 1.0e-6 { + continue; + } + for col in pivot..3 { + a[row][col] -= factor * a[pivot][col]; + } + b[row] -= factor * b[pivot]; + } + } + Some(b) +} + +fn query_support_plane_fallback_tangent(normal: Vec3f) -> Vec3f { + let axis = if normal.y.abs() < 0.95 { + vec3f(0.0, 1.0, 0.0) + } else { + vec3f(1.0, 0.0, 0.0) + }; + Vec3f::cross(axis, normal).normalize() +} + +fn query_tsdf_support_radius(query_radius: f32) -> f32 { + (query_radius * TSDF_QUERY_TSDF_SUPPORT_RADIUS_SCALE).clamp( + TSDF_QUERY_TSDF_SUPPORT_RADIUS_MIN, + TSDF_QUERY_TSDF_SUPPORT_RADIUS_MAX, + ) +} + +fn query_support_plane_height_tolerance(query_radius: f32, voxel_size: f32) -> f32 { + (query_radius * 0.45) + .clamp(0.015, 0.05) + .max(voxel_size * 0.25) +} + +fn query_tsdf_impact_half_extent(query_radius: f32, voxel_size: f32) -> f32 { + (query_radius * TSDF_QUERY_TSDF_IMPACT_EXTENT_SCALE + voxel_size * 0.5).clamp( + TSDF_QUERY_TSDF_IMPACT_EXTENT_MIN, + TSDF_QUERY_TSDF_IMPACT_EXTENT_MAX, + ) +} + +pub(crate) fn depth_query_plane_supports_body( + plane: DepthQuerySupportPlane, + body_position: Vec3f, + query_radius: f32, + lateral_margin: f32, +) -> bool { + let offset = body_position - plane.point; + let signed_height = offset.dot(plane.normal); + if signed_height < -query_radius.max(0.0005) { + return false; + } + if signed_height > query_radius + 0.12 + lateral_margin { + return false; + } + let tangent_limit = plane.half_extent_tangent + lateral_margin; + let bitangent_limit = plane.half_extent_bitangent + lateral_margin; + offset.dot(plane.tangent).abs() <= tangent_limit + && offset.dot(plane.bitangent).abs() <= bitangent_limit +} + +pub(crate) fn depth_query_plane_quad(plane: DepthQuerySupportPlane) -> [Vec3f; 4] { + let tangent = plane.tangent.scale(plane.half_extent_tangent); + let bitangent = plane.bitangent.scale(plane.half_extent_bitangent); + [ + plane.point - tangent - bitangent, + plane.point + tangent - bitangent, + plane.point + tangent + bitangent, + plane.point - tangent + bitangent, + ] +} + +pub(crate) fn depth_query_might_need_impact_refresh(query: DepthQuery) -> bool { + let travel = query.predicted_center - query.center; + let travel_distance = travel.length(); + let velocity_length = query.velocity.length(); + if velocity_length < TSDF_QUERY_TSDF_IMPACT_MIN_SPEED && travel_distance < 0.03 { + return false; + } + + let horizontal_speed = vec2f(query.velocity.x, query.velocity.z).length(); + let upward_speed = query.velocity.y.max(0.0); + horizontal_speed >= TSDF_QUERY_TSDF_IMPACT_MIN_HORIZONTAL_SPEED + || upward_speed >= TSDF_QUERY_TSDF_IMPACT_MIN_UPWARD_SPEED +} + +fn query_support_plane_fingerprint( + plane: &DepthQuerySupportPlane, + role: DepthQueryColliderRole, +) -> u64 { + let mut hasher = DefaultHasher::new(); + role.hash(&mut hasher); + [ + quantize_f32(plane.normal.x, 0.02), + quantize_f32(plane.normal.y, 0.02), + quantize_f32(plane.normal.z, 0.02), + quantize_f32(plane.normal.dot(plane.point), 0.01), + ] + .hash(&mut hasher); + hasher.finish() +} + +fn make_query_halfspace_collider( + plane: DepthQuerySupportPlane, + role: DepthQueryColliderRole, + restitution: f32, +) -> DepthQueryCollider { + DepthQueryCollider { + fingerprint: query_support_plane_fingerprint(&plane, role), + geometry: DepthQueryColliderGeometry::HalfSpace(plane), + role, + restitution, + } +} + +fn make_query_halfspace_surface( + distance: f32, + plane: DepthQuerySupportPlane, + role: DepthQueryColliderRole, + restitution: f32, +) -> DepthQueryResolvedSurface { + let patch = depth_query_plane_quad(plane); + DepthQueryResolvedSurface { + surface: DepthQuerySurfaceHit { + distance, + point: plane.point, + normal: plane.normal, + triangle: [patch[0], patch[1], patch[2]], + patch, + }, + collider: make_query_halfspace_collider(plane, role, restitution), + } +} + +fn query_first_support_height( + sampler: &mut TsdfQuerySampler<'_>, + sample_x: f32, + sample_z: f32, + top_y: f32, + bottom_y: f32, +) -> Option { + let top_coord = sampler + .world_to_voxel_coord(vec3f(sample_x, top_y, sample_z)) + .y; + let bottom_coord = sampler + .world_to_voxel_coord(vec3f(sample_x, bottom_y, sample_z)) + .y; + if top_coord <= bottom_coord { + return None; + } + let column = sampler.bilinear_column(sample_x, sample_z); + let mut above = sampler.bilinear_distance_at_y(column, top_coord); + + for y_coord in (bottom_coord + 1..=top_coord).rev() { + let below = sampler.bilinear_distance_at_y(column, y_coord - 1); + let (Some(above_distance), Some(below_distance)) = (above, below) else { + above = below; + continue; + }; + if above_distance <= 0.0 || below_distance > 0.0 { + above = below; + continue; + } + let denom = above_distance - below_distance; + let blend = if denom.abs() > 1.0e-6 { + (above_distance / denom).clamp(0.0, 1.0) + } else { + 0.5 + }; + let y_above = voxel_center_axis(sampler.grid.voxel_size, y_coord); + let y_below = voxel_center_axis(sampler.grid.voxel_size, y_coord - 1); + return Some(y_above + (y_below - y_above) * blend); + } + + None +} + +fn query_trajectory_time_seconds(query: DepthQuery) -> Option { + let horizontal_displacement = vec2f( + query.predicted_center.x - query.center.x, + query.predicted_center.z - query.center.z, + ); + let horizontal_velocity = vec2f(query.velocity.x, query.velocity.z); + let horizontal_speed_sq = horizontal_velocity.x * horizontal_velocity.x + + horizontal_velocity.y * horizontal_velocity.y; + if horizontal_speed_sq <= 1.0e-6 { + return None; + } + let dt = (horizontal_displacement.x * horizontal_velocity.x + + horizontal_displacement.y * horizontal_velocity.y) + / horizontal_speed_sq; + (dt.is_finite() && dt > 1.0e-4).then_some(dt) +} + +fn query_trajectory_sample(query: DepthQuery, progress: f32) -> QueryTrajectorySample { + let progress = progress.clamp(0.0, 1.0); + if let Some(total_time) = query_trajectory_time_seconds(query) { + let t = total_time * progress; + let dy = query.predicted_center.y - query.center.y; + let accel_y = 2.0 * (dy - query.velocity.y * total_time) / (total_time * total_time); + return QueryTrajectorySample { + progress, + point: vec3f( + query.center.x + query.velocity.x * t, + query.center.y + query.velocity.y * t + 0.5 * accel_y * t * t, + query.center.z + query.velocity.z * t, + ), + velocity: vec3f( + query.velocity.x, + query.velocity.y + accel_y * t, + query.velocity.z, + ), + }; + } + + let travel = query.predicted_center - query.center; + QueryTrajectorySample { + progress, + point: query.center + travel.scale(progress), + velocity: if query.velocity.length() > 1.0e-4 { + query.velocity + } else { + travel + }, + } +} + +fn query_trajectory_samples( + query: DepthQuery, +) -> [QueryTrajectorySample; TSDF_QUERY_TRAJECTORY_SAMPLE_COUNT] { + std::array::from_fn(|index| { + let progress = if TSDF_QUERY_TRAJECTORY_SAMPLE_COUNT <= 1 { + 0.0 + } else { + index as f32 / (TSDF_QUERY_TRAJECTORY_SAMPLE_COUNT - 1) as f32 + }; + query_trajectory_sample(query, progress) + }) +} + +fn query_trajectory_bounds_and_length( + samples: &[QueryTrajectorySample; TSDF_QUERY_TRAJECTORY_SAMPLE_COUNT], +) -> (Vec3f, Vec3f, f32) { + let mut min = samples[0].point; + let mut max = samples[0].point; + let mut length = 0.0; + for window in samples.windows(2) { + min = Vec3f::min_componentwise(min, window[1].point); + max = Vec3f::max_componentwise(max, window[1].point); + length += (window[1].point - window[0].point).length(); + } + (min, max, length) +} + +fn make_query_hit_from_resolved_surface( + query: DepthQuery, + surface: DepthQueryResolvedSurface, +) -> DepthQueryHit { + DepthQueryHit { + key: query.key, + distance: surface.surface.distance, + point: surface.surface.point, + normal: surface.surface.normal, + triangle: surface.surface.triangle, + patch: surface.surface.patch, + collider: surface.collider, + additional_hits: Vec::new(), + } +} + +fn query_might_overlap_active_bounds(snapshot: &TsdfPublishedSnapshot, query: DepthQuery) -> bool { + let Some((bounds_min, bounds_max)) = snapshot.grid.active_bounds else { + return false; + }; + let padding = + query.radius + query.max_distance + depth_tsd_distance_meters(snapshot.grid.voxel_size); + let padding_vec = vec3f(padding, padding, padding); + let query_min = Vec3f::min_componentwise(query.center, query.predicted_center) - padding_vec; + let query_max = Vec3f::max_componentwise(query.center, query.predicted_center) + padding_vec; + query_max.x >= bounds_min.x + && query_min.x <= bounds_max.x + && query_max.y >= bounds_min.y + && query_min.y <= bounds_max.y + && query_max.z >= bounds_min.z + && query_min.z <= bounds_max.z +} + +fn evaluate_tsdf_impact_query( + sampler: &mut TsdfQuerySampler<'_>, + query: DepthQuery, +) -> Option { + let grid = sampler.grid; + if !depth_query_might_need_impact_refresh(query) { + return None; + } + let travel = query.predicted_center - query.center; + let travel_distance = travel.length(); + let velocity_length = query.velocity.length(); + let upward_speed = query.velocity.y.max(0.0); + + let motion_dir = if velocity_length > 1.0e-4 { + query.velocity.scale(1.0 / velocity_length) + } else if travel_distance > 1.0e-4 { + travel.scale(1.0 / travel_distance) + } else { + return None; + }; + let max_search_distance = (travel_distance + query.radius + query.max_distance) + .max(query.radius + grid.voxel_size * 0.75); + let step_distance = (grid.voxel_size * TSDF_QUERY_TSDF_IMPACT_RAY_STEP_SCALE) + .max(TSDF_QUERY_TSDF_IMPACT_RAY_STEP_MIN) + .min(max_search_distance); + let hit_threshold = query.radius + grid.voxel_size * 0.20; + let mut previous_t = 0.0f32; + let mut t = step_distance; + + while t <= max_search_distance + 1.0e-4 { + let sample_position = query.center + motion_dir.scale(t); + let Some(sample_distance) = sampler.trilinear_distance(sample_position) else { + previous_t = t; + t += step_distance; + continue; + }; + if sample_distance <= hit_threshold { + let mut lo = previous_t; + let mut hi = t; + for _ in 0..5 { + let mid = (lo + hi) * 0.5; + let mid_position = query.center + motion_dir.scale(mid); + if let Some(mid_distance) = sampler.trilinear_distance(mid_position) { + if mid_distance <= hit_threshold { + hi = mid; + } else { + lo = mid; + } + } + } + + let hit_position = query.center + motion_dir.scale(hi); + let signed_distance = sampler.trilinear_distance(hit_position)?; + let mut normal = sampler.distance_gradient(hit_position)?; + let mut opposing_dot = normal.dot(motion_dir.scale(-1.0)); + if opposing_dot <= TSDF_QUERY_MIN_OPPOSING_NORMAL_DOT { + let flipped = normal.scale(-1.0); + let flipped_opposing_dot = flipped.dot(motion_dir.scale(-1.0)); + if flipped_opposing_dot > TSDF_QUERY_MIN_OPPOSING_NORMAL_DOT { + normal = flipped; + opposing_dot = flipped_opposing_dot; + } + } + let is_lateral_impact = normal.y.abs() <= TSDF_QUERY_TSDF_IMPACT_NORMAL_Y_MAX; + let is_ceiling_impact = upward_speed >= TSDF_QUERY_TSDF_IMPACT_MIN_UPWARD_SPEED + && normal.y <= -TSDF_QUERY_TSDF_IMPACT_CEILING_NORMAL_Y_MIN; + if !(is_lateral_impact || is_ceiling_impact) + || opposing_dot <= TSDF_QUERY_MIN_OPPOSING_NORMAL_DOT + { + previous_t = t; + t += step_distance; + continue; + } + + let tangent_raw = motion_dir - normal.scale(motion_dir.dot(normal)); + let tangent = if tangent_raw.length() > 1.0e-5 { + tangent_raw.normalize() + } else { + query_support_plane_fallback_tangent(normal) + }; + let bitangent = Vec3f::cross(normal, tangent).normalize(); + let half_extent = query_tsdf_impact_half_extent(query.radius, grid.voxel_size); + let plane = DepthQuerySupportPlane { + point: hit_position - normal.scale(signed_distance), + normal, + tangent, + bitangent, + half_extent_tangent: half_extent, + half_extent_bitangent: half_extent, + }; + return Some(make_query_halfspace_surface( + signed_distance.abs(), + plane, + DepthQueryColliderRole::Impact, + TSDF_QUERY_IMPACT_RESTITUTION, + )); + } + previous_t = t; + t += step_distance; + } + + None +} + +fn evaluate_tsdf_support_query( + sampler: &mut TsdfQuerySampler<'_>, + query: DepthQuery, + impact_surface: Option, +) -> Option { + const GRID_LAST: f32 = (TSDF_QUERY_TSDF_SUPPORT_GRID_DIM - 1) as f32; + + let grid = sampler.grid; + let trajectory_samples = query_trajectory_samples(query); + let (trajectory_bounds_min, trajectory_bounds_max, travel_distance) = + query_trajectory_bounds_and_length(&trajectory_samples); + let support_radius = query_tsdf_support_radius(query.radius); + let tsd_distance_meters = depth_tsd_distance_meters(grid.voxel_size); + let top_y = trajectory_bounds_max.y + query.radius + grid.voxel_size; + let bottom_y = trajectory_bounds_min.y + - (query.radius + query.max_distance + travel_distance + tsd_distance_meters); + let (_, search_sample, center_support_y) = trajectory_samples + .iter() + .filter_map(|sample| { + let support_y = query_first_support_height( + sampler, + sample.point.x, + sample.point.z, + top_y, + bottom_y, + )?; + let support_point = vec3f(sample.point.x, support_y, sample.point.z); + let score = (sample.point - support_point).length() + - sample.progress * (query.radius + grid.voxel_size) * 0.35; + Some((score, *sample, support_y)) + }) + .min_by(|a, b| a.0.total_cmp(&b.0))?; + let search_center = search_sample.point; + + let mut samples = [None; TSDF_QUERY_TSDF_SUPPORT_MAX_SAMPLES]; + let mut sample_count = 0usize; + let mut max_height = f32::NEG_INFINITY; + + for row in 0..TSDF_QUERY_TSDF_SUPPORT_GRID_DIM { + for column in 0..TSDF_QUERY_TSDF_SUPPORT_GRID_DIM { + let u = if GRID_LAST > 0.0 { + column as f32 / GRID_LAST * 2.0 - 1.0 + } else { + 0.0 + }; + let v = if GRID_LAST > 0.0 { + row as f32 / GRID_LAST * 2.0 - 1.0 + } else { + 0.0 + }; + let sample_x = search_center.x + u * support_radius; + let sample_z = search_center.z + v * support_radius; + let Some(sample_y) = + query_first_support_height(sampler, sample_x, sample_z, top_y, bottom_y) + else { + continue; + }; + let radial_weight = 1.0 / (1.0 + (u * u + v * v) * 1.5); + let point = vec3f(sample_x, sample_y, sample_z); + max_height = max_height.max(sample_y); + samples[sample_count] = Some(DepthGridSupportSample { + point, + radial_weight, + }); + sample_count += 1; + } + } + + if sample_count < TSDF_QUERY_TSDF_SUPPORT_MIN_SAMPLES { + return None; + } + + let mut height_tolerance = query_support_plane_height_tolerance(query.radius, grid.voxel_size); + let mut selected_count = 0usize; + for _ in 0..3 { + selected_count = samples[..sample_count] + .iter() + .filter_map(|sample| *sample) + .filter(|sample| max_height - sample.point.y <= height_tolerance) + .count(); + if selected_count >= TSDF_QUERY_TSDF_SUPPORT_MIN_SAMPLES { + break; + } + height_tolerance += grid.voxel_size * 0.35; + } + if selected_count < TSDF_QUERY_TSDF_SUPPORT_MIN_SAMPLES { + height_tolerance = f32::INFINITY; + selected_count = sample_count; + } + + let mut sum_w = 0.0; + let mut sum_xx = 0.0; + let mut sum_xz = 0.0; + let mut sum_x = 0.0; + let mut sum_zz = 0.0; + let mut sum_z = 0.0; + let mut sum_xy = 0.0; + let mut sum_zy = 0.0; + let mut sum_y = 0.0; + + for sample in samples[..sample_count].iter().filter_map(|sample| *sample) { + if max_height - sample.point.y > height_tolerance { + continue; + } + let local = sample.point - search_center; + let weight = sample.radial_weight; + sum_w += weight; + sum_xx += weight * local.x * local.x; + sum_xz += weight * local.x * local.z; + sum_x += weight * local.x; + sum_zz += weight * local.z * local.z; + sum_z += weight * local.z; + sum_xy += weight * local.x * local.y; + sum_zy += weight * local.z * local.y; + sum_y += weight * local.y; + } + + if selected_count < 3 || sum_w <= 1.0e-5 { + return None; + } + + let mut normal = solve_linear3( + [ + [sum_xx, sum_xz, sum_x], + [sum_xz, sum_zz, sum_z], + [sum_x, sum_z, sum_w], + ], + [sum_xy, sum_zy, sum_y], + ) + .map(|solution| vec3f(-solution[0], 1.0, -solution[1]).normalize()) + .unwrap_or(vec3f(0.0, 1.0, 0.0)); + if normal.y < 0.0 { + normal = normal.scale(-1.0); + } + normal = (normal + vec3f(0.0, 1.0, 0.0).scale(0.9)).normalize(); + if normal.y < TSDF_QUERY_TSDF_SUPPORT_NORMAL_Y_MIN { + return None; + } + + let tangent = query_support_plane_fallback_tangent(normal); + let bitangent = Vec3f::cross(normal, tangent).normalize(); + let mut plane_offset = f32::NEG_INFINITY; + let mut min_u = f32::INFINITY; + let mut max_u = f32::NEG_INFINITY; + let mut min_v = f32::INFINITY; + let mut max_v = f32::NEG_INFINITY; + + for sample in samples[..sample_count].iter().filter_map(|sample| *sample) { + if max_height - sample.point.y > height_tolerance { + continue; + } + plane_offset = plane_offset.max(normal.dot(sample.point)); + let offset = sample.point - search_center; + let u = offset.dot(tangent); + let v = offset.dot(bitangent); + min_u = min_u.min(u); + max_u = max_u.max(u); + min_v = min_v.min(v); + max_v = max_v.max(v); + } + + let point = search_center - normal.scale(normal.dot(search_center) - plane_offset); + let extent_padding = + (query.radius * TSDF_QUERY_TSDF_SUPPORT_EXTENT_PADDING_SCALE).max(grid.voxel_size * 0.45); + let debug_half_extent_max = support_radius; + let debug_half_extent_min = (query.radius * 0.9) + .max(grid.voxel_size * 0.35) + .min(debug_half_extent_max); + let half_extent_tangent = if min_u.is_finite() && max_u.is_finite() { + ((max_u - min_u) * 0.5 + extent_padding).clamp(debug_half_extent_min, debug_half_extent_max) + } else { + debug_half_extent_min + }; + let half_extent_bitangent = if min_v.is_finite() && max_v.is_finite() { + ((max_v - min_v) * 0.5 + extent_padding).clamp(debug_half_extent_min, debug_half_extent_max) + } else { + debug_half_extent_min + }; + + let plane = DepthQuerySupportPlane { + point, + normal, + tangent, + bitangent, + half_extent_tangent, + half_extent_bitangent, + }; + let center_support_point = vec3f(search_center.x, center_support_y, search_center.z); + let support_point = + center_support_point - normal.scale(normal.dot(center_support_point) - plane_offset); + let support_surface = make_query_halfspace_surface( + (support_point - search_center).length(), + DepthQuerySupportPlane { + point: support_point, + ..plane + }, + DepthQueryColliderRole::Support, + 0.0, + ); + let additional_hits = impact_surface.into_iter().collect::>(); + + Some(DepthQueryHit { + key: query.key, + distance: support_surface.surface.distance, + point: support_surface.surface.point, + normal: support_surface.surface.normal, + triangle: support_surface.surface.triangle, + patch: support_surface.surface.patch, + collider: support_surface.collider, + additional_hits, + }) +} + +pub(crate) fn evaluate_tsdf_query( + snapshot: &TsdfPublishedSnapshot, + query: DepthQuery, +) -> DepthQueryResult { + if snapshot.grid.active_value_count == 0 || !query_might_overlap_active_bounds(snapshot, query) + { + return DepthQueryResult::Miss { key: query.key }; + } + + let mut sampler = TsdfQuerySampler::new(snapshot.grid.as_ref()); + let impact_surface = evaluate_tsdf_impact_query(&mut sampler, query); + let prefer_impact = impact_surface.as_ref().is_some_and(|impact_surface| { + let DepthQueryColliderGeometry::HalfSpace(plane) = &impact_surface.collider.geometry; + query.velocity.y >= TSDF_QUERY_TSDF_IMPACT_MIN_UPWARD_SPEED + && plane.normal.y <= -TSDF_QUERY_TSDF_IMPACT_CEILING_NORMAL_Y_MIN + }); + if prefer_impact { + return impact_surface + .map(|impact_surface| { + DepthQueryResult::Hit(make_query_hit_from_resolved_surface(query, impact_surface)) + }) + .unwrap_or(DepthQueryResult::Miss { key: query.key }); + } + + evaluate_tsdf_support_query(&mut sampler, query, impact_surface) + .or_else(|| { + impact_surface + .map(|impact_surface| make_query_hit_from_resolved_surface(query, impact_surface)) + }) + .map(DepthQueryResult::Hit) + .unwrap_or(DepthQueryResult::Miss { key: query.key }) +} + +#[cfg(test)] +include!("../tests/algorithms/tsdf_query.rs"); diff --git a/xr/src/bin/analyze_dump.rs b/xr/src/bin/analyze_dump.rs new file mode 100644 index 0000000000..8fd2758b80 --- /dev/null +++ b/xr/src/bin/analyze_dump.rs @@ -0,0 +1,6843 @@ +#![allow(dead_code, unused_mut, unused_variables)] + +use makepad_widgets::*; +use makepad_xr::{algorithms::depth_align::*, net::*}; +use std::{ + env, + f32::consts::{PI, TAU}, + fs, + path::{Path, PathBuf}, + time::{Instant, SystemTime}, +}; + +#[derive(Clone, Copy, Debug, Default)] +struct AnalyzeOptions { + solve: bool, + latest_count: usize, +} + +const FLOOR_BIN_METERS: f32 = 0.04; +const FLOOR_MIN_SUPPORT_RATIO: f32 = 0.005; +const FLOOR_MIN_SUPPORT_CELLS: usize = 24; +const FLOOR_SUPPORT_WINDOW_BINS: usize = 2; +const SHAPE_OCCUPIED_MIN_HEIGHT_METERS: f32 = 0.18; +const SHAPE_MATCH_DISTANCE_TRUNCATE_METERS: f32 = 0.60; +const SHAPE_GLOBAL_CELL_METERS: f32 = 0.24; +const SHAPE_REFINE_CELL_METERS: f32 = 0.12; +const SHAPE_FINAL_CELL_METERS: f32 = 0.06; +const SHAPE_GLOBAL_YAW_STEP_RADIANS: f32 = 10.0_f32.to_radians(); +const SHAPE_REFINE_YAW_WINDOW_RADIANS: f32 = 12.0_f32.to_radians(); +const SHAPE_REFINE_YAW_STEP_RADIANS: f32 = 2.0_f32.to_radians(); +const SHAPE_FINAL_YAW_WINDOW_RADIANS: f32 = 3.0_f32.to_radians(); +const SHAPE_FINAL_YAW_STEP_RADIANS: f32 = 0.5_f32.to_radians(); +const SHAPE_GLOBAL_TOP_K: usize = 12; +const SHAPE_REFINE_TOP_K: usize = 8; +const SHAPE_FINAL_TOP_K: usize = 6; +const SHAPE_MIN_CONTOUR_POINTS: usize = 12; +const SHAPE_FURNITURE_MIN_HEIGHT_METERS: f32 = 0.22; +const SHAPE_FURNITURE_MAX_HEIGHT_METERS: f32 = 1.10; +const SHAPE_FURNITURE_MIN_COMPONENT_AREA_METERS2: f32 = 0.05; +const SHAPE_SCAN_FLOOR_OFFSETS_METERS: [f32; 5] = [-0.10, -0.05, 0.0, 0.05, 0.10]; +const SHAPE_SEEDED_PRIOR_TRANSLATION_SIGMA_METERS: f32 = 0.32; +const SHAPE_SEEDED_PRIOR_YAW_SIGMA_RADIANS: f32 = 9.0_f32.to_radians(); +const SHAPE_SEEDED_PRIOR_FLOOR: f32 = 0.55; +const SHAPE_SEEDED_SIGNED_EMPTY_PENALTY: f32 = 1.10; +const SHAPE_SEEDED_UNSUPPORTED_PENALTY: f32 = 2.50; +const SHAPE_SEEDED_INTERIOR_BONUS_CAP_METERS: f32 = 0.18; +const SHAPE_SEEDED_LOCAL_TRANSLATION_WINDOW_METERS: f32 = 0.42; +const SHAPE_SEEDED_LOCAL_YAW_WINDOW_RADIANS: f32 = 16.0_f32.to_radians(); +const SHAPE_SEEDED_LOCAL_YAW_STEP_RADIANS: f32 = 1.0_f32.to_radians(); +const SHAPE_HOUGH_THETA_BINS: usize = 180; +const SHAPE_HOUGH_TOP_PEAKS: usize = 4; +const SHAPE_HOUGH_THETA_SUPPRESS_RADIANS: f32 = 8.0_f32.to_radians(); +const SHAPE_COMPONENT_CONTEXT_RADIAL_BINS: usize = 3; +const SHAPE_COMPONENT_CONTEXT_ANGULAR_BINS: usize = 12; +const SHAPE_COMPONENT_CONTEXT_CHANNELS: usize = 3; +const SHAPE_COMPONENT_CONTEXT_RADIUS_METERS: f32 = 2.40; +const SHAPE_COMPONENT_CONTEXT_SIGNATURE_LEN: usize = SHAPE_COMPONENT_CONTEXT_CHANNELS + * SHAPE_COMPONENT_CONTEXT_RADIAL_BINS + * SHAPE_COMPONENT_CONTEXT_ANGULAR_BINS; +const SHAPE_COMPONENT_SIGNATURE_BINS: usize = SHAPE_COMPONENT_CONTEXT_ANGULAR_BINS; +const SHAPE_CORR_COARSE_CELL_METERS: f32 = 0.48; +const SHAPE_CORR_MEDIUM_CELL_METERS: f32 = 0.24; +const SHAPE_CORR_FINE_CELL_METERS: f32 = 0.12; +const SHAPE_CORR_FINAL_CELL_METERS: f32 = 0.06; +const SHAPE_CORR_COARSE_YAW_STEP_RADIANS: f32 = 8.0_f32.to_radians(); +const SHAPE_CORR_MEDIUM_YAW_STEP_RADIANS: f32 = 3.0_f32.to_radians(); +const SHAPE_CORR_FINE_YAW_STEP_RADIANS: f32 = 1.0_f32.to_radians(); +const SHAPE_CORR_FINAL_YAW_STEP_RADIANS: f32 = 0.5_f32.to_radians(); +const SHAPE_CORR_COARSE_TOP_K: usize = 96; +const SHAPE_CORR_MEDIUM_TOP_K: usize = 64; +const SHAPE_CORR_FINE_TOP_K: usize = 32; +const SHAPE_CORR_FINAL_TOP_K: usize = 12; +const ANALYSIS_MAX_THREADS: usize = 16; +const FOCUSED_HOUGH_TOP_LINES: usize = 10; +const FOCUSED_HOUGH_RHO_SUPPRESS_METERS: f32 = 0.16; +const FOCUSED_HOUGH_ANGLE_SIGMA_RADIANS: f32 = 5.0_f32.to_radians(); +const FOCUSED_HOUGH_RHO_SIGMA_METERS: f32 = 0.18; +const FOCUSED_COMPONENT_MAX_DISTANCE_METERS: f32 = 0.70; +const FOCUSED_COMPONENT_DISTANCE_SIGMA_METERS: f32 = 0.24; +const FOCUSED_LOCAL_TRANSLATION_WINDOW_METERS: f32 = 0.30; +const FOCUSED_LOCAL_YAW_WINDOW_RADIANS: f32 = 10.0_f32.to_radians(); +const FOCUSED_LOCAL_YAW_STEP_RADIANS: f32 = 1.0_f32.to_radians(); +const FOCUSED_REFINE_TRANSLATION_WINDOW_METERS: f32 = 0.12; +const FOCUSED_REFINE_YAW_WINDOW_RADIANS: f32 = 2.0_f32.to_radians(); +const FOCUSED_REFINE_YAW_STEP_RADIANS: f32 = 0.25_f32.to_radians(); +const FOCUSED_SEARCH_TOP_K: usize = 24; +const FOCUSED_FINAL_TOP_K: usize = 6; +const ANALYZE_TSDF_CALLBACK_INTERVAL_MILLIS: u64 = 8; +const ANALYZE_TSDF_CALLBACK_BUDGET_MILLIS: u64 = 40; +const ANALYZE_TSDF_CALLBACK_MAX_STEPS: usize = 4096; + +#[derive(Clone, Copy, Debug, Default)] +struct ManualPose { + shift_x_meters: f32, + shift_y_meters: f32, + rotation_radians: f32, +} + +#[derive(Clone, Debug)] +struct ShapeMatchGrid { + origin_x: f32, + origin_z: f32, + cell_size_meters: f32, + size_x: usize, + size_z: usize, + occupied_mask: Vec, + occupied_weight: Vec, + occupied_points: Vec<(f32, f32)>, + distance_meters: Vec, + signed_distance_meters: Vec, + contour_points: Vec<(f32, f32)>, + support_mask: Vec, + support_points: Vec<(f32, f32)>, + free_mask: Vec, +} + +#[derive(Clone, Debug)] +struct ShapeBandSpec { + label: String, + min_height_meters: f32, + max_height_meters: Option, + floor_offset_meters: f32, + drop_border_components: bool, +} + +#[derive(Clone, Copy, Debug, Default)] +struct ShapeMatchCandidate { + score: f32, + feature_score: f32, + support_score: f32, + coverage: f32, + close_ratio: f32, + mean_distance_meters: f32, + yaw_radians: f32, + translation_x: f32, + translation_z: f32, + in_bounds_points: usize, +} + +#[derive(Clone, Debug)] +struct ShapeMatchReport { + label: String, + finalists: Vec, + manual_candidate: Option, +} + +#[derive(Clone, Copy, Debug)] +struct HoughLine { + theta_radians: f32, + rho_meters: f32, + strength: f32, +} + +#[derive(Clone, Copy, Debug)] +struct FocusedCueWeights { + label: &'static str, + runtime_weight: f32, + signed_weight: f32, + line_weight: f32, + blob_weight: f32, +} + +#[derive(Clone, Copy, Debug, Default)] +struct FocusedPoseEvidence { + pose: ShapeMatchCandidate, + signed_score: f32, + line_score: f32, + blob_score: f32, + runtime_score: f32, + runtime_residual_meters: f32, + seed_prior: f32, +} + +#[derive(Clone, Copy, Debug, Default)] +struct FocusedMatchCandidate { + pose: ShapeMatchCandidate, + score: f32, + runtime_score: f32, + runtime_residual_meters: f32, + signed_score: f32, + line_score: f32, + blob_score: f32, + seed_prior: f32, +} + +#[derive(Clone, Debug)] +struct FocusedMatchReport { + label: String, + finalists: Vec, + manual_candidate: Option, +} + +#[derive(Clone, Copy, Debug)] +struct NearestManualFinalist { + candidate: ShapeMatchCandidate, + planar_delta_meters: f32, + yaw_delta_radians: f32, + combined_distance: f32, +} + +#[derive(Clone, Debug)] +struct ShapeBandScanResult { + report: ShapeMatchReport, + nearest_manual_finalist: Option, +} + +#[derive(Clone)] +struct BandGridPair { + local: ShapeMatchGrid, + remote: ShapeMatchGrid, +} + +#[derive(Clone, Copy, Debug)] +struct ShapeComponent { + centroid_x: f32, + centroid_z: f32, + area_m2: f32, + score: f32, + compactness: f32, + radial_signature: [f32; SHAPE_COMPONENT_SIGNATURE_BINS], + context_signature: [f32; SHAPE_COMPONENT_CONTEXT_SIGNATURE_LEN], +} + +fn analysis_thread_count(work_items: usize) -> usize { + let available = std::thread::available_parallelism() + .map(|parallelism| parallelism.get()) + .unwrap_or(1); + work_items.max(1).min(available).min(ANALYSIS_MAX_THREADS) +} + +fn latest_dump_paths(count: usize) -> Vec { + let dump_dir = PathBuf::from("xr/dump/dumps"); + let mut entries = fs::read_dir(dump_dir) + .ok() + .into_iter() + .flatten() + .filter_map(|entry| entry.ok()) + .filter_map(|entry| { + let metadata = entry.metadata().ok()?; + metadata + .is_file() + .then_some((entry.path(), metadata.modified().ok()?)) + }) + .collect::>(); + entries.sort_by(|left, right| right.1.cmp(&left.1)); + entries + .into_iter() + .filter_map(|(path, _)| { + path.file_name() + .and_then(|name| name.to_str()) + .is_some_and(|name| name.ends_with(".bin") && name != "manual-smoke.bin") + .then_some(path) + }) + .take(count.max(1)) + .collect() +} + +fn parse_args() -> Result<(AnalyzeOptions, Vec), String> { + let mut options = AnalyzeOptions { + latest_count: 1, + ..AnalyzeOptions::default() + }; + let mut paths = Vec::::new(); + let mut args = env::args_os().skip(1); + while let Some(arg) = args.next() { + if arg == "--solve" { + options.solve = true; + continue; + } + if arg == "--latest" { + let Some(count) = args.next() else { + return Err("expected a count after --latest".to_string()); + }; + let count = count + .to_string_lossy() + .parse::() + .map_err(|_| "failed to parse --latest count".to_string())?; + options.latest_count = count.max(1); + continue; + } + paths.push(PathBuf::from(arg)); + } + if paths.is_empty() { + paths = latest_dump_paths(options.latest_count); + } + if paths.is_empty() { + return Err("no dump files found in xr/dump/dumps".to_string()); + } + Ok((options, paths)) +} + +fn count_valid_heights(height_map: &XrDepthAlignHeightMap) -> usize { + height_map + .heights_meters + .iter() + .filter(|height| height.is_finite()) + .count() +} + +fn min_max_height(height_map: &XrDepthAlignHeightMap) -> Option<(f32, f32)> { + let mut min_height = f32::INFINITY; + let mut max_height = f32::NEG_INFINITY; + for height in height_map + .heights_meters + .iter() + .copied() + .filter(|height| height.is_finite()) + { + min_height = min_height.min(height); + max_height = max_height.max(height); + } + (min_height.is_finite() && max_height.is_finite()).then_some((min_height, max_height)) +} + +fn sorted_heights(height_map: &XrDepthAlignHeightMap) -> Vec { + let mut heights = height_map + .heights_meters + .iter() + .copied() + .filter(|height| height.is_finite()) + .collect::>(); + heights.sort_by(|left, right| left.total_cmp(right)); + heights +} + +fn percentile(sorted_heights: &[f32], t: f32) -> Option { + let last = sorted_heights.len().checked_sub(1)?; + let index = (last as f32 * t.clamp(0.0, 1.0)).round() as usize; + sorted_heights.get(index).copied() +} + +fn recompute_floor_y(height_map: &XrDepthAlignHeightMap) -> Option { + let heights = sorted_heights(height_map); + if heights.is_empty() { + return None; + } + let bottom_y = height_map.bottom_y_meters; + let top_y = height_map.top_y_meters; + let span = (top_y - bottom_y).max(1.0e-3); + let bin_size = FLOOR_BIN_METERS.max(span / 256.0).min(span); + let bin_count = ((span / bin_size).ceil() as usize).max(1) + 1; + let mut bins = vec![0usize; bin_count]; + for height in &heights { + let bin = (((*height - bottom_y) / bin_size).floor() as isize) + .clamp(0, bin_count.saturating_sub(1) as isize) as usize; + bins[bin] += 1; + } + let min_support = FLOOR_MIN_SUPPORT_CELLS + .max((heights.len() as f32 * FLOOR_MIN_SUPPORT_RATIO).ceil() as usize); + let support_window_bins = FLOOR_SUPPORT_WINDOW_BINS.max(1); + for start_bin in 0..bin_count { + let end_bin = (start_bin + support_window_bins).min(bin_count); + let support = bins[start_bin..end_bin].iter().copied().sum::(); + if support < min_support { + continue; + } + let low = bottom_y + start_bin as f32 * bin_size; + let high = bottom_y + end_bin as f32 * bin_size; + let window = heights + .iter() + .copied() + .filter(|height| *height >= low && *height <= high) + .collect::>(); + if !window.is_empty() { + return Some(window.iter().copied().sum::() / window.len() as f32); + } + } + let fallback_count = ((heights.len() as f32 * 0.01).ceil() as usize).clamp(1, 32); + let fallback = &heights[..fallback_count]; + Some(fallback.iter().copied().sum::() / fallback.len() as f32) +} + +fn print_height_map_stats(label: &str, height_map: Option<&XrDepthAlignHeightMap>) { + let Some(height_map) = height_map else { + println!("{label}: no height map"); + return; + }; + let valid_cells = count_valid_heights(height_map); + let cell_count = height_map.cell_count(); + let min_max = min_max_height(height_map); + println!( + "{label}: {}x{} cells, valid {} / {}, cell {:.3} m, bounds_y [{:.3}, {:.3}], floor_y {:.3}", + height_map.size_x, + height_map.size_z, + valid_cells, + cell_count, + height_map.cell_size_meters, + height_map.bottom_y_meters, + height_map.top_y_meters, + height_map.floor_y_meters, + ); + if let Some((min_height, max_height)) = min_max { + println!( + "{label}: observed height range [{:.3}, {:.3}]", + min_height, max_height + ); + } + let sorted = sorted_heights(height_map); + if !sorted.is_empty() { + println!( + "{label}: percentiles p01 {:.3} | p05 {:.3} | p08 {:.3} | p10 {:.3} | p20 {:.3}", + percentile(&sorted, 0.01).unwrap_or(f32::NAN), + percentile(&sorted, 0.05).unwrap_or(f32::NAN), + percentile(&sorted, 0.08).unwrap_or(f32::NAN), + percentile(&sorted, 0.10).unwrap_or(f32::NAN), + percentile(&sorted, 0.20).unwrap_or(f32::NAN), + ); + } + if let Some(recomputed_floor_y) = recompute_floor_y(height_map) { + println!( + "{label}: recomputed floor_y {:.3} (delta {:.3})", + recomputed_floor_y, + recomputed_floor_y - height_map.floor_y_meters + ); + } +} + +fn print_descriptor_stats(label: &str, descriptor: &XrDepthAlignDescriptor) { + println!( + "{label}: floor_y {:.3}, samples {} (walls {} floors {}), vertical {}", + descriptor.floor_y, + descriptor.samples.len(), + descriptor + .samples + .iter() + .filter(|sample| sample.kind == XrDepthAlignSampleKind::Wall) + .count(), + descriptor + .samples + .iter() + .filter(|sample| sample.kind == XrDepthAlignSampleKind::Floor) + .count(), + descriptor.vertical_descriptor.is_some() + ); + print_height_map_stats(label, descriptor.height_map.as_ref()); + if let Some(markers) = descriptor.test_markers() { + println!( + "{label}: markers ({:.2}, {:.2}, {:.2}) and ({:.2}, {:.2}, {:.2})", + markers[0].x, markers[0].y, markers[0].z, markers[1].x, markers[1].y, markers[1].z + ); + } else { + println!("{label}: no test markers"); + } +} + +fn explain_outcome(diagnostic: &XrDepthAlignSolveDiagnostic) -> &'static str { + match diagnostic.outcome() { + XrDepthAlignSolveOutcome::MissingSamples => "missing samples", + XrDepthAlignSolveOutcome::NoCandidate => "no candidate survived matching", + XrDepthAlignSolveOutcome::Rejected => "solver found a candidate but rejected it", + XrDepthAlignSolveOutcome::Accepted => "accepted", + } +} + +fn system_time_hint(path: &Path) -> String { + fs::metadata(path) + .ok() + .and_then(|metadata| metadata.modified().ok()) + .and_then(|modified| modified.duration_since(SystemTime::UNIX_EPOCH).ok()) + .map(|duration| duration.as_secs().to_string()) + .unwrap_or_else(|| "unknown".to_string()) +} + +fn manual_sidecar_path(dump_path: &Path) -> PathBuf { + let stem = dump_path + .file_stem() + .and_then(|stem| stem.to_str()) + .unwrap_or("align-pair"); + dump_path.with_file_name(format!("{stem}.manual_pose.ron")) +} + +fn load_manual_pose(dump_path: &Path) -> Option { + let text = fs::read_to_string(manual_sidecar_path(dump_path)).ok()?; + let mut pose = ManualPose::default(); + for line in text.lines() { + let (key, value) = line.split_once(':')?; + let value = value.trim(); + match key.trim() { + "shift_x_meters" => pose.shift_x_meters = value.parse().ok()?, + "shift_y_meters" => pose.shift_y_meters = value.parse().ok()?, + "rotation_radians" => pose.rotation_radians = value.parse().ok()?, + _ => {} + } + } + Some(pose) +} + +fn manual_pose_solution( + local: &XrDepthAlignDescriptor, + remote: &XrDepthAlignDescriptor, + pose: ManualPose, +) -> XrDepthAlignSolution { + XrDepthAlignSolution { + yaw_radians: pose.rotation_radians, + translation: vec3( + pose.shift_x_meters, + local.floor_y - remote.floor_y, + pose.shift_y_meters, + ), + confidence: 0.0, + symmetry_confidence: 0.0, + residual_meters: f32::INFINITY, + matched_samples: 0, + } +} + +fn manual_pose_delta_text(solution: XrDepthAlignSolution, manual_pose: ManualPose) -> String { + let manual_yaw = manual_pose.rotation_radians; + let manual_translation = vec3( + manual_pose.shift_x_meters, + solution.translation.y, + manual_pose.shift_y_meters, + ); + format!( + "manual_delta: yaw {:.3} rad ({:.1} deg) | planar {:.3} m | dy {:.3} m", + wrap_angle(solution.yaw_radians - manual_yaw), + wrap_angle(solution.yaw_radians - manual_yaw).to_degrees(), + vec3( + solution.translation.x - manual_translation.x, + 0.0, + solution.translation.z - manual_translation.z, + ) + .length(), + solution.translation.y - manual_translation.y, + ) +} + +impl ShapeMatchGrid { + fn cell_index(&self, x: usize, z: usize) -> usize { + x + z * self.size_x + } + + fn max_x(&self) -> f32 { + self.origin_x + self.size_x as f32 * self.cell_size_meters + } + + fn max_z(&self) -> f32 { + self.origin_z + self.size_z as f32 * self.cell_size_meters + } + + fn sample_distance_nearest(&self, world_x: f32, world_z: f32) -> Option { + let local_x = (world_x - self.origin_x) / self.cell_size_meters; + let local_z = (world_z - self.origin_z) / self.cell_size_meters; + if !local_x.is_finite() + || !local_z.is_finite() + || local_x < 0.0 + || local_z < 0.0 + || local_x >= self.size_x as f32 + || local_z >= self.size_z as f32 + { + return None; + } + let x = local_x.floor() as usize; + let z = local_z.floor() as usize; + self.distance_meters.get(self.cell_index(x, z)).copied() + } + + fn sample_free_nearest(&self, world_x: f32, world_z: f32) -> Option { + let local_x = (world_x - self.origin_x) / self.cell_size_meters; + let local_z = (world_z - self.origin_z) / self.cell_size_meters; + if !local_x.is_finite() + || !local_z.is_finite() + || local_x < 0.0 + || local_z < 0.0 + || local_x >= self.size_x as f32 + || local_z >= self.size_z as f32 + { + return None; + } + let x = local_x.floor() as usize; + let z = local_z.floor() as usize; + self.free_mask.get(self.cell_index(x, z)).copied() + } + + fn sample_support_nearest(&self, world_x: f32, world_z: f32) -> Option { + let local_x = (world_x - self.origin_x) / self.cell_size_meters; + let local_z = (world_z - self.origin_z) / self.cell_size_meters; + if !local_x.is_finite() + || !local_z.is_finite() + || local_x < 0.0 + || local_z < 0.0 + || local_x >= self.size_x as f32 + || local_z >= self.size_z as f32 + { + return None; + } + let x = local_x.floor() as usize; + let z = local_z.floor() as usize; + self.support_mask.get(self.cell_index(x, z)).copied() + } + + fn sample_occupied_nearest(&self, world_x: f32, world_z: f32) -> Option { + let local_x = (world_x - self.origin_x) / self.cell_size_meters; + let local_z = (world_z - self.origin_z) / self.cell_size_meters; + if !local_x.is_finite() + || !local_z.is_finite() + || local_x < 0.0 + || local_z < 0.0 + || local_x >= self.size_x as f32 + || local_z >= self.size_z as f32 + { + return None; + } + let x = local_x.floor() as usize; + let z = local_z.floor() as usize; + self.occupied_mask.get(self.cell_index(x, z)).copied() + } + + fn sample_occupied_weight_nearest(&self, world_x: f32, world_z: f32) -> Option { + let local_x = (world_x - self.origin_x) / self.cell_size_meters; + let local_z = (world_z - self.origin_z) / self.cell_size_meters; + if !local_x.is_finite() + || !local_z.is_finite() + || local_x < 0.0 + || local_z < 0.0 + || local_x >= self.size_x as f32 + || local_z >= self.size_z as f32 + { + return None; + } + let x = local_x.floor() as usize; + let z = local_z.floor() as usize; + self.occupied_weight.get(self.cell_index(x, z)).copied() + } + + fn sample_signed_distance_nearest(&self, world_x: f32, world_z: f32) -> Option { + let local_x = (world_x - self.origin_x) / self.cell_size_meters; + let local_z = (world_z - self.origin_z) / self.cell_size_meters; + if !local_x.is_finite() + || !local_z.is_finite() + || local_x < 0.0 + || local_z < 0.0 + || local_x >= self.size_x as f32 + || local_z >= self.size_z as f32 + { + return None; + } + let x = local_x.floor() as usize; + let z = local_z.floor() as usize; + self.signed_distance_meters + .get(self.cell_index(x, z)) + .copied() + } +} + +fn wrap_angle(mut angle: f32) -> f32 { + while angle > PI { + angle -= TAU; + } + while angle <= -PI { + angle += TAU; + } + angle +} + +fn rotate_xz(yaw: f32, x: f32, z: f32) -> (f32, f32) { + let (sin_yaw, cos_yaw) = yaw.sin_cos(); + (x * cos_yaw - z * sin_yaw, x * sin_yaw + z * cos_yaw) +} + +fn line_theta(theta_radians: f32) -> f32 { + theta_radians.rem_euclid(PI) +} + +fn line_angle_delta(left: f32, right: f32) -> f32 { + let delta = (line_theta(left) - line_theta(right)).abs().rem_euclid(PI); + delta.min(PI - delta) +} + +fn signature_shift_for_angle(angle_radians: f32, bin_count: usize) -> usize { + ((wrap_angle(angle_radians).rem_euclid(TAU) / TAU) * bin_count as f32) + .round() + .rem_euclid(bin_count.max(1) as f32) as usize +} + +fn seed_prior_factor(candidate: ShapeMatchCandidate, seed: ShapeMatchCandidate) -> f32 { + let translation_delta = ((candidate.translation_x - seed.translation_x).powi(2) + + (candidate.translation_z - seed.translation_z).powi(2)) + .sqrt(); + let yaw_delta = wrap_angle(candidate.yaw_radians - seed.yaw_radians).abs(); + let translation_term = + -0.5 * (translation_delta / SHAPE_SEEDED_PRIOR_TRANSLATION_SIGMA_METERS).powi(2); + let yaw_term = -0.5 * (yaw_delta / SHAPE_SEEDED_PRIOR_YAW_SIGMA_RADIANS).powi(2); + let prior = (translation_term + yaw_term).exp().clamp(0.0, 1.0); + SHAPE_SEEDED_PRIOR_FLOOR + (1.0 - SHAPE_SEEDED_PRIOR_FLOOR) * prior +} + +fn shape_band_obstacle() -> ShapeBandSpec { + ShapeBandSpec { + label: "shape".to_string(), + min_height_meters: SHAPE_OCCUPIED_MIN_HEIGHT_METERS, + max_height_meters: None, + floor_offset_meters: 0.0, + drop_border_components: false, + } +} + +fn shape_band_furniture() -> ShapeBandSpec { + ShapeBandSpec { + label: "shape_furniture".to_string(), + min_height_meters: SHAPE_FURNITURE_MIN_HEIGHT_METERS, + max_height_meters: Some(SHAPE_FURNITURE_MAX_HEIGHT_METERS), + floor_offset_meters: 0.0, + drop_border_components: true, + } +} + +fn shape_band_scans() -> Vec { + let mut bands = Vec::new(); + for &floor_offset in &SHAPE_SCAN_FLOOR_OFFSETS_METERS { + for min_height in [0.12f32, 0.20, 0.28, 0.36] { + for max_height in [0.70f32, 0.90, 1.10, 1.35] { + if max_height - min_height < 0.26 { + continue; + } + bands.push(ShapeBandSpec { + label: format!( + "shape_scan_{min_height:.2}_{max_height:.2}_f{floor_offset:+.2}" + ), + min_height_meters: min_height, + max_height_meters: Some(max_height), + floor_offset_meters: floor_offset, + drop_border_components: true, + }); + } + } + } + bands +} + +fn seeded_consensus_bands() -> Vec { + vec![ + ShapeBandSpec { + label: "cons_low".to_string(), + min_height_meters: 0.12, + max_height_meters: Some(0.70), + floor_offset_meters: 0.0, + drop_border_components: true, + }, + ShapeBandSpec { + label: "cons_mid".to_string(), + min_height_meters: 0.20, + max_height_meters: Some(1.10), + floor_offset_meters: 0.0, + drop_border_components: true, + }, + ShapeBandSpec { + label: "cons_upper".to_string(), + min_height_meters: 0.60, + max_height_meters: Some(1.80), + floor_offset_meters: 0.0, + drop_border_components: false, + }, + ] +} + +fn filter_shape_components( + occupied: &[bool], + size_x: usize, + size_z: usize, + cell_size_meters: f32, + drop_border_components: bool, +) -> Vec { + let mut filtered = vec![false; occupied.len()]; + let mut visited = vec![false; occupied.len()]; + let min_component_cells = (SHAPE_FURNITURE_MIN_COMPONENT_AREA_METERS2 + / (cell_size_meters * cell_size_meters).max(1.0e-4)) + .ceil() + .max(1.0) as usize; + for start in 0..occupied.len() { + if visited[start] || !occupied[start] { + continue; + } + let mut stack = vec![start]; + let mut component = Vec::new(); + let mut touches_border = false; + visited[start] = true; + while let Some(index) = stack.pop() { + component.push(index); + let x = index % size_x; + let z = index / size_x; + if x == 0 || z == 0 || x + 1 == size_x || z + 1 == size_z { + touches_border = true; + } + for (dx, dz) in [(-1isize, 0isize), (1, 0), (0, -1), (0, 1)] { + let nx = x as isize + dx; + let nz = z as isize + dz; + if nx < 0 || nz < 0 || nx >= size_x as isize || nz >= size_z as isize { + continue; + } + let neighbor = nx as usize + nz as usize * size_x; + if visited[neighbor] || !occupied[neighbor] { + continue; + } + visited[neighbor] = true; + stack.push(neighbor); + } + } + if component.len() < min_component_cells { + continue; + } + if drop_border_components && touches_border { + continue; + } + for index in component { + filtered[index] = true; + } + } + filtered +} + +fn erode_mask(mask: &[bool], size_x: usize, size_z: usize) -> Vec { + let mut eroded = vec![false; mask.len()]; + for z in 0..size_z { + for x in 0..size_x { + let index = x + z * size_x; + if !mask[index] { + continue; + } + let mut keep = true; + for dz in -1isize..=1 { + for dx in -1isize..=1 { + let nx = x as isize + dx; + let nz = z as isize + dz; + if nx < 0 + || nz < 0 + || nx >= size_x as isize + || nz >= size_z as isize + || !mask[nx as usize + nz as usize * size_x] + { + keep = false; + break; + } + } + if !keep { + break; + } + } + eroded[index] = keep; + } + } + eroded +} + +fn dilate_mask(mask: &[bool], size_x: usize, size_z: usize) -> Vec { + let mut dilated = vec![false; mask.len()]; + for z in 0..size_z { + for x in 0..size_x { + let mut on = false; + for dz in -1isize..=1 { + for dx in -1isize..=1 { + let nx = x as isize + dx; + let nz = z as isize + dz; + if nx < 0 || nz < 0 || nx >= size_x as isize || nz >= size_z as isize { + continue; + } + if mask[nx as usize + nz as usize * size_x] { + on = true; + break; + } + } + if on { + break; + } + } + dilated[x + z * size_x] = on; + } + } + dilated +} + +fn clean_binary_mask(mask: &[bool], size_x: usize, size_z: usize) -> Vec { + let opened = dilate_mask(&erode_mask(mask, size_x, size_z), size_x, size_z); + erode_mask(&dilate_mask(&opened, size_x, size_z), size_x, size_z) +} + +fn build_support_frontier_mask(grid: &ShapeMatchGrid) -> Vec { + let mut frontier = vec![false; grid.support_mask.len()]; + for z in 0..grid.size_z { + for x in 0..grid.size_x { + let index = x + z * grid.size_x; + if !grid.support_mask[index] { + continue; + } + let mut edge = false; + for (dx, dz) in [(-1isize, 0isize), (1, 0), (0, -1), (0, 1)] { + let nx = x as isize + dx; + let nz = z as isize + dz; + if nx < 0 + || nz < 0 + || nx >= grid.size_x as isize + || nz >= grid.size_z as isize + || !grid.support_mask[nx as usize + nz as usize * grid.size_x] + { + edge = true; + break; + } + } + frontier[index] = edge; + } + } + frontier +} + +fn component_context_index(channel: usize, radial: usize, angular: usize) -> usize { + channel * SHAPE_COMPONENT_CONTEXT_RADIAL_BINS * SHAPE_COMPONENT_CONTEXT_ANGULAR_BINS + + radial * SHAPE_COMPONENT_CONTEXT_ANGULAR_BINS + + angular +} + +fn build_component_context_signature( + grid: &ShapeMatchGrid, + support_frontier_mask: &[bool], + centroid_x: f32, + centroid_z: f32, +) -> [f32; SHAPE_COMPONENT_CONTEXT_SIGNATURE_LEN] { + let mut signature = [0.0f32; SHAPE_COMPONENT_CONTEXT_SIGNATURE_LEN]; + let mut channel_totals = [0.0f32; SHAPE_COMPONENT_CONTEXT_CHANNELS]; + for z in 0..grid.size_z { + for x in 0..grid.size_x { + let index = x + z * grid.size_x; + if !grid.support_mask[index] { + continue; + } + let world_x = grid.origin_x + (x as f32 + 0.5) * grid.cell_size_meters; + let world_z = grid.origin_z + (z as f32 + 0.5) * grid.cell_size_meters; + let dx = world_x - centroid_x; + let dz = world_z - centroid_z; + let distance = (dx * dx + dz * dz).sqrt(); + if distance <= grid.cell_size_meters * 0.5 + || distance > SHAPE_COMPONENT_CONTEXT_RADIUS_METERS + { + continue; + } + let radial = (((distance / SHAPE_COMPONENT_CONTEXT_RADIUS_METERS) + * SHAPE_COMPONENT_CONTEXT_RADIAL_BINS as f32) + .floor() as isize) + .clamp( + 0, + SHAPE_COMPONENT_CONTEXT_RADIAL_BINS.saturating_sub(1) as isize, + ) as usize; + let angle = wrap_angle(dz.atan2(dx)); + let angular = (((angle + PI) / (TAU / SHAPE_COMPONENT_CONTEXT_ANGULAR_BINS as f32)) + .floor() as isize) + .rem_euclid(SHAPE_COMPONENT_CONTEXT_ANGULAR_BINS as isize) + as usize; + if grid.occupied_mask[index] { + let weight = grid.occupied_weight[index].max(0.35); + signature[component_context_index(0, radial, angular)] += weight; + channel_totals[0] += weight; + } else { + signature[component_context_index(1, radial, angular)] += 1.0; + channel_totals[1] += 1.0; + } + if support_frontier_mask[index] { + signature[component_context_index(2, radial, angular)] += 1.0; + channel_totals[2] += 1.0; + } + } + } + for channel in 0..SHAPE_COMPONENT_CONTEXT_CHANNELS { + let total = channel_totals[channel]; + if total <= 1.0e-6 { + continue; + } + let start = component_context_index(channel, 0, 0); + let end = + start + SHAPE_COMPONENT_CONTEXT_RADIAL_BINS * SHAPE_COMPONENT_CONTEXT_ANGULAR_BINS; + for value in &mut signature[start..end] { + *value /= total; + } + } + signature +} + +fn extract_shape_components(grid: &ShapeMatchGrid, max_components: usize) -> Vec { + let mut visited = vec![false; grid.occupied_mask.len()]; + let support_frontier_mask = build_support_frontier_mask(grid); + let mut components = Vec::new(); + + // threshold the occupied mask to only deep thick blobs to sever simple noisy connections + let mut deep_occupied = vec![false; grid.occupied_mask.len()]; + for i in 0..grid.occupied_mask.len() { + deep_occupied[i] = grid.occupied_mask[i] && grid.distance_meters[i] > 0.50; + // at least 50cm inside + } + + for start in 0..deep_occupied.len() { + if visited[start] || !deep_occupied[start] { + continue; + } + let mut stack = vec![start]; + let mut component = Vec::new(); + let mut area_cells = 0usize; + let mut perimeter_edges = 0usize; + let mut sum_x = 0.0f32; + let mut sum_z = 0.0f32; + visited[start] = true; + while let Some(index) = stack.pop() { + component.push(index); + area_cells += 1; + let x = index % grid.size_x; + let z = index / grid.size_x; + let world_x = grid.origin_x + (x as f32 + 0.5) * grid.cell_size_meters; + let world_z = grid.origin_z + (z as f32 + 0.5) * grid.cell_size_meters; + sum_x += world_x; + sum_z += world_z; + for (dx, dz) in [(-1isize, 0isize), (1, 0), (0, -1), (0, 1)] { + let nx = x as isize + dx; + let nz = z as isize + dz; + if nx < 0 || nz < 0 || nx >= grid.size_x as isize || nz >= grid.size_z as isize { + perimeter_edges += 1; + continue; + } + let neighbor = nx as usize + nz as usize * grid.size_x; + if !deep_occupied[neighbor] { + perimeter_edges += 1; + continue; + } + if visited[neighbor] { + continue; + } + visited[neighbor] = true; + stack.push(neighbor); + } + } + if area_cells == 0 { + continue; + } + let area_m2 = area_cells as f32 * grid.cell_size_meters * grid.cell_size_meters; + let perimeter_m = perimeter_edges as f32 * grid.cell_size_meters; + let compactness = if perimeter_m > 1.0e-6 { + (4.0 * PI * area_m2 / (perimeter_m * perimeter_m)).clamp(0.0, 1.25) + } else { + 0.0 + }; + let centroid_x = sum_x / area_cells as f32; + let centroid_z = sum_z / area_cells as f32; + let mut radial_sum = [0.0f32; SHAPE_COMPONENT_SIGNATURE_BINS]; + let mut radial_count = [0usize; SHAPE_COMPONENT_SIGNATURE_BINS]; + for &index in &component { + let x = index % grid.size_x; + let z = index / grid.size_x; + let world_x = grid.origin_x + (x as f32 + 0.5) * grid.cell_size_meters; + let world_z = grid.origin_z + (z as f32 + 0.5) * grid.cell_size_meters; + let dx = world_x - centroid_x; + let dz = world_z - centroid_z; + let angle = wrap_angle(dz.atan2(dx)); + let sector = + (((angle + PI) / (TAU / SHAPE_COMPONENT_SIGNATURE_BINS as f32)).floor() as isize) + .rem_euclid(SHAPE_COMPONENT_SIGNATURE_BINS as isize) as usize; + radial_sum[sector] += (dx * dx + dz * dz).sqrt(); + radial_count[sector] += 1; + } + let mut radial_signature = [0.0f32; SHAPE_COMPONENT_SIGNATURE_BINS]; + for sector in 0..SHAPE_COMPONENT_SIGNATURE_BINS { + radial_signature[sector] = if radial_count[sector] > 0 { + radial_sum[sector] / radial_count[sector] as f32 + } else { + 0.0 + }; + } + let radial_norm = radial_signature + .iter() + .copied() + .fold(0.0f32, f32::max) + .max(1.0e-4); + for value in &mut radial_signature { + *value /= radial_norm; + } + let area_score = area_m2.sqrt().min(1.5); + let score = area_score * (0.25 + compactness * 0.75); + let context_signature = + build_component_context_signature(grid, &support_frontier_mask, centroid_x, centroid_z); + components.push(ShapeComponent { + centroid_x, + centroid_z, + area_m2, + score, + compactness, + radial_signature, + context_signature, + }); + } + components.sort_by(|left, right| { + right + .score + .total_cmp(&left.score) + .then_with(|| right.area_m2.total_cmp(&left.area_m2)) + }); + components.truncate(max_components.max(1)); + components +} + +fn build_shape_match_grid( + height_map: &XrDepthAlignHeightMap, + coarse_cell_size_meters: f32, + band: &ShapeBandSpec, +) -> Option { + let size_x = ((height_map.extent_x_meters() / coarse_cell_size_meters).ceil() as usize).max(1); + let size_z = ((height_map.extent_z_meters() / coarse_cell_size_meters).ceil() as usize).max(1); + let mut occupied = vec![false; size_x * size_z]; + let mut support_mask = vec![false; size_x * size_z]; + let mut free_mask = vec![true; size_x * size_z]; + let cutout_center = height_map.player_cutout_center; + let cutout_radius = (height_map.player_cutout_radius_meters + height_map.cell_size_meters) + .max(height_map.cell_size_meters * 2.0); + let coarse_cell_size = coarse_cell_size_meters.max(height_map.cell_size_meters); + let band_floor_y = height_map.floor_y_meters + band.floor_offset_meters; + for src_z in 0..height_map.size_z_usize() { + for src_x in 0..height_map.size_x_usize() { + let index = height_map.cell_index(src_x, src_z); + let height = *height_map.heights_meters.get(index)?; + if !height.is_finite() { + continue; + } + let world_x = height_map.origin_x + (src_x as f32 + 0.5) * height_map.cell_size_meters; + let world_z = height_map.origin_z + (src_z as f32 + 0.5) * height_map.cell_size_meters; + if cutout_center.is_some_and(|center| { + let dx = world_x - center.x; + let dz = world_z - center.y; + (dx * dx + dz * dz).sqrt() <= cutout_radius + }) { + continue; + } + let coarse_x = ((world_x - height_map.origin_x) / coarse_cell_size) + .floor() + .clamp(0.0, size_x.saturating_sub(1) as f32) as usize; + let coarse_z = ((world_z - height_map.origin_z) / coarse_cell_size) + .floor() + .clamp(0.0, size_z.saturating_sub(1) as f32) as usize; + let coarse_index = coarse_x + coarse_z * size_x; + support_mask[coarse_index] = true; + let relative_height = height - band_floor_y; + if relative_height >= SHAPE_OCCUPIED_MIN_HEIGHT_METERS { + free_mask[coarse_index] = false; + } + if relative_height < band.min_height_meters { + continue; + } + if band + .max_height_meters + .is_some_and(|max_height| relative_height > max_height) + { + continue; + } + occupied[coarse_index] = true; + } + } + support_mask = filter_shape_components(&support_mask, size_x, size_z, coarse_cell_size, false); + support_mask = clean_binary_mask(&support_mask, size_x, size_z); + occupied = filter_shape_components( + &occupied, + size_x, + size_z, + coarse_cell_size, + band.drop_border_components, + ); + occupied = clean_binary_mask(&occupied, size_x, size_z); + if occupied.iter().all(|value| !*value) { + return None; + } + + for i in 0..free_mask.len() { + free_mask[i] = free_mask[i] && support_mask[i]; + } + // DO NOT ERODE free_mask! Narrow door streaks vanished because of erosion! + + let mut contour_mask = vec![false; occupied.len()]; + for z in 0..size_z { + for x in 0..size_x { + let index = x + z * size_x; + if !occupied[index] { + continue; + } + let mut edge = false; + for (dx, dz) in [(-1isize, 0isize), (1, 0), (0, -1), (0, 1)] { + let nx = x as isize + dx; + let nz = z as isize + dz; + if nx < 0 + || nz < 0 + || nx >= size_x as isize + || nz >= size_z as isize + || !occupied[nx as usize + nz as usize * size_x] + { + edge = true; + break; + } + } + contour_mask[index] = edge; + } + } + if contour_mask.iter().filter(|value| **value).count() < SHAPE_MIN_CONTOUR_POINTS { + contour_mask.copy_from_slice(&occupied); + } + + let distance_meters = + chamfer_distance_transform(&contour_mask, size_x, size_z, coarse_cell_size); + let signed_distance_meters = distance_meters + .iter() + .enumerate() + .map(|(index, distance)| { + if occupied[index] { + *distance + } else { + -*distance + } + }) + .collect::>(); + let occupied_weight = occupied + .iter() + .enumerate() + .map(|(index, is_occupied)| { + if !*is_occupied { + return 0.0; + } + let interior_bonus = + (distance_meters[index] / SHAPE_SEEDED_INTERIOR_BONUS_CAP_METERS).clamp(0.0, 1.0); + 1.0 + interior_bonus * 0.65 + }) + .collect::>(); + let mut contour_points = Vec::new(); + let mut occupied_points = Vec::new(); + for z in 0..size_z { + for x in 0..size_x { + if !occupied[x + z * size_x] { + continue; + } + occupied_points.push(( + height_map.origin_x + (x as f32 + 0.5) * coarse_cell_size, + height_map.origin_z + (z as f32 + 0.5) * coarse_cell_size, + )); + } + } + for z in 0..size_z { + for x in 0..size_x { + if !contour_mask[x + z * size_x] { + continue; + } + contour_points.push(( + height_map.origin_x + (x as f32 + 0.5) * coarse_cell_size, + height_map.origin_z + (z as f32 + 0.5) * coarse_cell_size, + )); + } + } + let mut support_points = Vec::new(); + for z in 0..size_z { + for x in 0..size_x { + if !support_mask[x + z * size_x] { + continue; + } + support_points.push(( + height_map.origin_x + (x as f32 + 0.5) * coarse_cell_size, + height_map.origin_z + (z as f32 + 0.5) * coarse_cell_size, + )); + } + } + if contour_points.len() < SHAPE_MIN_CONTOUR_POINTS { + return None; + } + Some(ShapeMatchGrid { + origin_x: height_map.origin_x, + origin_z: height_map.origin_z, + cell_size_meters: coarse_cell_size, + size_x, + size_z, + occupied_mask: occupied, + occupied_weight, + occupied_points, + distance_meters, + signed_distance_meters, + contour_points, + support_mask, + support_points, + free_mask, + }) +} + +fn chamfer_distance_transform( + contour_mask: &[bool], + size_x: usize, + size_z: usize, + cell_size_meters: f32, +) -> Vec { + let len = size_x * size_z; + let inf = 1.0e6f32; + let diag = 2.0f32.sqrt(); + let mut distance = vec![inf; len]; + for (index, occupied) in contour_mask.iter().enumerate() { + if *occupied { + distance[index] = 0.0; + } + } + for z in 0..size_z { + for x in 0..size_x { + let index = x + z * size_x; + let mut best = distance[index]; + if x > 0 { + best = best.min(distance[index - 1] + 1.0); + } + if z > 0 { + best = best.min(distance[index - size_x] + 1.0); + if x > 0 { + best = best.min(distance[index - size_x - 1] + diag); + } + if x + 1 < size_x { + best = best.min(distance[index - size_x + 1] + diag); + } + } + distance[index] = best; + } + } + for z in (0..size_z).rev() { + for x in (0..size_x).rev() { + let index = x + z * size_x; + let mut best = distance[index]; + if x + 1 < size_x { + best = best.min(distance[index + 1] + 1.0); + } + if z + 1 < size_z { + best = best.min(distance[index + size_x] + 1.0); + if x > 0 { + best = best.min(distance[index + size_x - 1] + diag); + } + if x + 1 < size_x { + best = best.min(distance[index + size_x + 1] + diag); + } + } + distance[index] = best; + } + } + for value in &mut distance { + *value *= cell_size_meters; + } + distance +} + +fn candidate_is_distinct( + left: ShapeMatchCandidate, + right: ShapeMatchCandidate, + translation_epsilon: f32, + yaw_epsilon: f32, +) -> bool { + (left.translation_x - right.translation_x).abs() > translation_epsilon + || (left.translation_z - right.translation_z).abs() > translation_epsilon + || wrap_angle(left.yaw_radians - right.yaw_radians).abs() > yaw_epsilon +} + +fn push_shape_candidate( + finalists: &mut Vec, + candidate: ShapeMatchCandidate, + limit: usize, + translation_epsilon: f32, + yaw_epsilon: f32, +) { + if !candidate.score.is_finite() || candidate.score <= 0.0 { + return; + } + if let Some(existing) = finalists.iter_mut().find(|existing| { + !candidate_is_distinct(**existing, candidate, translation_epsilon, yaw_epsilon) + }) { + if candidate.score > existing.score { + *existing = candidate; + } + } else { + finalists.push(candidate); + } + finalists.sort_by(|left, right| { + right.score.total_cmp(&left.score).then_with(|| { + left.mean_distance_meters + .total_cmp(&right.mean_distance_meters) + }) + }); + finalists.truncate(limit.max(1)); +} + +fn score_shape_alignment_one_way( + local: &ShapeMatchGrid, + remote_points: &[(f32, f32)], + yaw_radians: f32, + translation_x: f32, + translation_z: f32, +) -> ShapeMatchCandidate { + if remote_points.is_empty() { + return ShapeMatchCandidate::default(); + } + let mut in_bounds_points = 0usize; + let mut close_points = 0usize; + let mut similarity_sum = 0.0; + let mut distance_sum = 0.0; + let mut free_spill = 0usize; + for &(remote_x, remote_z) in remote_points { + let (rotated_x, rotated_z) = rotate_xz(yaw_radians, remote_x, remote_z); + let world_x = rotated_x + translation_x; + let world_z = rotated_z + translation_z; + let Some(distance) = local.sample_distance_nearest(world_x, world_z) else { + continue; + }; + in_bounds_points += 1; + if local.sample_free_nearest(world_x, world_z).unwrap_or(false) { + free_spill += 1; + similarity_sum -= 5.0; + distance_sum += SHAPE_MATCH_DISTANCE_TRUNCATE_METERS; + continue; + } + let clamped_distance = distance.min(SHAPE_MATCH_DISTANCE_TRUNCATE_METERS); + similarity_sum += 1.0 - clamped_distance / SHAPE_MATCH_DISTANCE_TRUNCATE_METERS; + distance_sum += distance; + + if distance <= (local.cell_size_meters * 1.5 + 0.06) { + close_points += 1; + } + } + + if free_spill as f32 / in_bounds_points.max(1) as f32 > 0.05 { + return ShapeMatchCandidate::default(); + } + + if in_bounds_points < SHAPE_MIN_CONTOUR_POINTS.min(remote_points.len()) { + return ShapeMatchCandidate::default(); + } + let coverage = in_bounds_points as f32 / remote_points.len().max(1) as f32; + let close_ratio = close_points as f32 / remote_points.len().max(1) as f32; + let mean_similarity = similarity_sum / in_bounds_points as f32; + ShapeMatchCandidate { + score: mean_similarity * coverage.sqrt() * (0.20 + 0.80 * close_ratio.sqrt()), + feature_score: mean_similarity * coverage.sqrt() * (0.20 + 0.80 * close_ratio.sqrt()), + support_score: 0.0, + coverage, + close_ratio, + mean_distance_meters: distance_sum / in_bounds_points as f32, + yaw_radians, + translation_x, + translation_z, + in_bounds_points, + } +} + +fn score_support_iou_symmetric( + local: &ShapeMatchGrid, + remote: &ShapeMatchGrid, + yaw_radians: f32, + translation_x: f32, + translation_z: f32, +) -> f32 { + let mut intersection = 0usize; + let mut remote_area = 0usize; + + // We iterate over remote support points + for &(remote_x, remote_z) in &remote.support_points { + remote_area += 1; + let (rotated_x, rotated_z) = rotate_xz(yaw_radians, remote_x, remote_z); + let world_x = rotated_x + translation_x; + let world_z = rotated_z + translation_z; + if local + .sample_support_nearest(world_x, world_z) + .unwrap_or(false) + { + intersection += 1; + } + } + + let local_area = local.support_points.len(); + if local_area == 0 || remote_area == 0 || intersection == 0 { + return 0.0; + } + + let union = local_area + remote_area - intersection; + intersection as f32 / union.max(1) as f32 +} + +fn score_support_overlap_one_way( + local: &ShapeMatchGrid, + remote_points: &[(f32, f32)], + yaw_radians: f32, + translation_x: f32, + translation_z: f32, +) -> (f32, f32, f32, usize) { + if remote_points.is_empty() { + return (0.0, 0.0, 0.0, 0); + } + let mut in_bounds = 0usize; + let mut hits = 0usize; + for &(remote_x, remote_z) in remote_points { + let (rotated_x, rotated_z) = rotate_xz(yaw_radians, remote_x, remote_z); + let world_x = rotated_x + translation_x; + let world_z = rotated_z + translation_z; + let Some(is_supported) = local.sample_support_nearest(world_x, world_z) else { + continue; + }; + in_bounds += 1; + if is_supported { + hits += 1; + } + } + if in_bounds < SHAPE_MIN_CONTOUR_POINTS.min(remote_points.len()) { + return (0.0, 0.0, 0.0, in_bounds); + } + let coverage = in_bounds as f32 / remote_points.len().max(1) as f32; + let hit_ratio = hits as f32 / in_bounds.max(1) as f32; + (coverage.sqrt() * hit_ratio, coverage, hit_ratio, in_bounds) +} + +fn score_combined_alignment_one_way( + local: &ShapeMatchGrid, + remote: &ShapeMatchGrid, + yaw_radians: f32, + translation_x: f32, + translation_z: f32, +) -> ShapeMatchCandidate { + let feature = score_shape_alignment_one_way( + local, + &remote.contour_points, + yaw_radians, + translation_x, + translation_z, + ); + let (support_score, support_coverage, support_hit_ratio, support_points) = + score_support_overlap_one_way( + local, + &remote.support_points, + yaw_radians, + translation_x, + translation_z, + ); + if feature.score <= 0.0 && support_score <= 0.0 { + return ShapeMatchCandidate::default(); + } + ShapeMatchCandidate { + score: feature.score * 0.72 + support_score * 0.28, + feature_score: feature.score, + support_score, + coverage: feature.coverage * 0.7 + support_coverage * 0.3, + close_ratio: feature.close_ratio * 0.65 + support_hit_ratio * 0.35, + mean_distance_meters: feature.mean_distance_meters, + yaw_radians, + translation_x, + translation_z, + in_bounds_points: feature + .in_bounds_points + .min(support_points.max(feature.in_bounds_points)), + } +} + +fn score_shape_alignment_symmetric( + local: &ShapeMatchGrid, + remote: &ShapeMatchGrid, + yaw_radians: f32, + translation_x: f32, + translation_z: f32, +) -> ShapeMatchCandidate { + let forward = + score_combined_alignment_one_way(local, remote, yaw_radians, translation_x, translation_z); + if forward.score <= 0.0 { + return forward; + } + let inverse = score_combined_alignment_one_way( + remote, + local, + -yaw_radians, + -translation_x, + -translation_z, + ); + if inverse.score <= 0.0 { + return forward; + } + let feature_score = (forward.feature_score * inverse.feature_score).sqrt(); + let support_score = (forward.support_score * inverse.support_score).sqrt(); + ShapeMatchCandidate { + score: feature_score * 0.72 + support_score * 0.28, + feature_score, + support_score, + coverage: 0.5 * (forward.coverage + inverse.coverage), + close_ratio: 0.5 * (forward.close_ratio + inverse.close_ratio), + mean_distance_meters: 0.5 * (forward.mean_distance_meters + inverse.mean_distance_meters), + yaw_radians, + translation_x, + translation_z, + in_bounds_points: forward.in_bounds_points.min(inverse.in_bounds_points), + } +} + +fn dominant_hough_orientations(grid: &ShapeMatchGrid) -> Vec { + if grid.contour_points.is_empty() { + return Vec::new(); + } + let center_x = grid.origin_x + grid.size_x as f32 * grid.cell_size_meters * 0.5; + let center_z = grid.origin_z + grid.size_z as f32 * grid.cell_size_meters * 0.5; + let half_extent_x = grid.size_x as f32 * grid.cell_size_meters * 0.5; + let half_extent_z = grid.size_z as f32 * grid.cell_size_meters * 0.5; + let max_rho = (half_extent_x * half_extent_x + half_extent_z * half_extent_z) + .sqrt() + .max(grid.cell_size_meters); + let rho_step = (grid.cell_size_meters * 1.5).max(0.06); + let rho_bins = ((2.0 * max_rho) / rho_step).ceil().max(1.0) as usize + 1; + let mut accum = vec![0.0f32; SHAPE_HOUGH_THETA_BINS * rho_bins]; + let theta_step = PI / SHAPE_HOUGH_THETA_BINS as f32; + + for &(world_x, world_z) in &grid.contour_points { + let x = world_x - center_x; + let z = world_z - center_z; + for theta_index in 0..SHAPE_HOUGH_THETA_BINS { + let theta = theta_index as f32 * theta_step; + let (sin_theta, cos_theta) = theta.sin_cos(); + let rho = x * cos_theta + z * sin_theta; + let rho_index = ((rho + max_rho) / rho_step) + .round() + .clamp(0.0, rho_bins.saturating_sub(1) as f32) as usize; + accum[theta_index * rho_bins + rho_index] += 1.0; + } + } + + let mut theta_scores = vec![0.0f32; SHAPE_HOUGH_THETA_BINS]; + for theta_index in 0..SHAPE_HOUGH_THETA_BINS { + let row = &accum[theta_index * rho_bins..(theta_index + 1) * rho_bins]; + let mut best = 0.0f32; + let mut second = 0.0f32; + for &value in row { + if value > best { + second = best; + best = value; + } else if value > second { + second = value; + } + } + theta_scores[theta_index] = best * 0.8 + second * 0.2; + } + + let mut peaks = Vec::<(f32, f32)>::new(); + for theta_index in 0..SHAPE_HOUGH_THETA_BINS { + let theta = theta_index as f32 * theta_step; + let score = theta_scores[theta_index]; + if score <= 0.0 { + continue; + } + let mut suppressed = false; + for &(existing_theta, existing_score) in &peaks { + let delta = wrap_angle(theta - existing_theta) + .abs() + .min(wrap_angle(theta - existing_theta + PI).abs()); + if delta < SHAPE_HOUGH_THETA_SUPPRESS_RADIANS && existing_score >= score { + suppressed = true; + break; + } + } + if suppressed { + continue; + } + peaks.retain(|(existing_theta, existing_score)| { + let delta = wrap_angle(theta - *existing_theta) + .abs() + .min(wrap_angle(theta - *existing_theta + PI).abs()); + !(delta < SHAPE_HOUGH_THETA_SUPPRESS_RADIANS && score > *existing_score) + }); + peaks.push((theta, score)); + peaks.sort_by(|left, right| right.1.total_cmp(&left.1)); + peaks.truncate(SHAPE_HOUGH_TOP_PEAKS.max(1)); + } + peaks + .into_iter() + .map(|(theta, _)| wrap_angle(theta + PI * 0.5)) + .collect() +} + +fn extract_hough_lines(grid: &ShapeMatchGrid, max_lines: usize) -> Vec { + if grid.contour_points.is_empty() || max_lines == 0 { + return Vec::new(); + } + let max_rho = grid + .contour_points + .iter() + .map(|(x, z)| (x * x + z * z).sqrt()) + .fold(0.0f32, f32::max) + .max(grid.cell_size_meters); + let rho_step = (grid.cell_size_meters * 1.25).max(0.05); + let rho_bins = ((2.0 * max_rho) / rho_step).ceil().max(1.0) as usize + 1; + let theta_step = PI / SHAPE_HOUGH_THETA_BINS as f32; + let mut accum = vec![0.0f32; SHAPE_HOUGH_THETA_BINS * rho_bins]; + + for &(world_x, world_z) in &grid.contour_points { + for theta_index in 0..SHAPE_HOUGH_THETA_BINS { + let theta = theta_index as f32 * theta_step; + let (sin_theta, cos_theta) = theta.sin_cos(); + let rho = world_x * cos_theta + world_z * sin_theta; + let rho_index = ((rho + max_rho) / rho_step) + .round() + .clamp(0.0, rho_bins.saturating_sub(1) as f32) as usize; + accum[theta_index * rho_bins + rho_index] += 1.0; + } + } + + let best_score = accum.iter().copied().fold(0.0f32, f32::max); + if best_score <= 0.0 { + return Vec::new(); + } + let min_score = (best_score * 0.18).max(6.0); + let mut peaks = Vec::::new(); + for theta_index in 0..SHAPE_HOUGH_THETA_BINS { + let theta = theta_index as f32 * theta_step; + for rho_index in 0..rho_bins { + let score = accum[theta_index * rho_bins + rho_index]; + if score < min_score { + continue; + } + let rho = rho_index as f32 * rho_step - max_rho; + let mut suppressed = false; + for existing in &peaks { + if line_angle_delta(existing.theta_radians, theta) + < SHAPE_HOUGH_THETA_SUPPRESS_RADIANS + && (existing.rho_meters - rho).abs() < FOCUSED_HOUGH_RHO_SUPPRESS_METERS + && existing.strength >= score + { + suppressed = true; + break; + } + } + if suppressed { + continue; + } + peaks.retain(|existing| { + !(line_angle_delta(existing.theta_radians, theta) + < SHAPE_HOUGH_THETA_SUPPRESS_RADIANS + && (existing.rho_meters - rho).abs() < FOCUSED_HOUGH_RHO_SUPPRESS_METERS + && score > existing.strength) + }); + peaks.push(HoughLine { + theta_radians: theta, + rho_meters: rho, + strength: score, + }); + peaks.sort_by(|left, right| right.strength.total_cmp(&left.strength)); + peaks.truncate(max_lines.max(1)); + } + } + let norm = peaks + .first() + .map(|line| line.strength.max(1.0e-4)) + .unwrap_or(1.0); + for line in &mut peaks { + line.theta_radians = line_theta(line.theta_radians); + line.strength = (line.strength / norm).clamp(0.0, 1.0); + } + peaks +} + +fn transform_hough_line( + line: HoughLine, + yaw_radians: f32, + translation_x: f32, + translation_z: f32, +) -> HoughLine { + let raw_theta = line.theta_radians + yaw_radians; + let (sin_theta, cos_theta) = raw_theta.sin_cos(); + let mut theta_radians = raw_theta.rem_euclid(TAU); + let mut rho_meters = line.rho_meters + translation_x * cos_theta + translation_z * sin_theta; + if theta_radians >= PI { + theta_radians -= PI; + rho_meters = -rho_meters; + } + HoughLine { + theta_radians, + rho_meters, + strength: line.strength, + } +} + +fn score_hough_line_alignment_one_way( + local_lines: &[HoughLine], + remote_lines: &[HoughLine], + yaw_radians: f32, + translation_x: f32, + translation_z: f32, +) -> f32 { + if local_lines.is_empty() || remote_lines.is_empty() { + return 0.0; + } + let total_weight = remote_lines + .iter() + .map(|line| line.strength.max(0.05)) + .sum::() + .max(1.0e-6); + let mut score_sum = 0.0f32; + let mut matched_weight = 0.0f32; + for &remote_line in remote_lines { + let transformed = + transform_hough_line(remote_line, yaw_radians, translation_x, translation_z); + let mut best = 0.0f32; + for &local_line in local_lines { + let angle_delta = line_angle_delta(local_line.theta_radians, transformed.theta_radians); + if angle_delta > 16.0_f32.to_radians() { + continue; + } + let rho_delta = (local_line.rho_meters - transformed.rho_meters).abs(); + if rho_delta > 0.80 { + continue; + } + let angle_score = + (-0.5 * (angle_delta / FOCUSED_HOUGH_ANGLE_SIGMA_RADIANS).powi(2)).exp(); + let rho_score = (-0.5 * (rho_delta / FOCUSED_HOUGH_RHO_SIGMA_METERS).powi(2)).exp(); + best = best.max(angle_score * rho_score * local_line.strength.sqrt()); + } + let weight = remote_line.strength.max(0.05); + score_sum += best * weight; + matched_weight += best.sqrt() * weight; + } + let coverage = (matched_weight / total_weight).clamp(0.0, 1.0); + (score_sum / total_weight) * coverage.sqrt() +} + +fn score_hough_line_alignment_symmetric( + local_lines: &[HoughLine], + remote_lines: &[HoughLine], + yaw_radians: f32, + translation_x: f32, + translation_z: f32, +) -> f32 { + let forward = score_hough_line_alignment_one_way( + local_lines, + remote_lines, + yaw_radians, + translation_x, + translation_z, + ); + if forward <= 0.0 { + return 0.0; + } + let inverse = score_hough_line_alignment_one_way( + remote_lines, + local_lines, + -yaw_radians, + -translation_x, + -translation_z, + ); + if inverse <= 0.0 { + return 0.0; + } + (forward * inverse).sqrt() +} + +fn projection_histogram_overlap( + local_points: &[(f32, f32)], + remote_points: &[(f32, f32)], + axis_x: f32, + axis_z: f32, + bin_size_meters: f32, +) -> f32 { + if local_points.is_empty() || remote_points.is_empty() { + return 0.0; + } + let mut min_projection = f32::INFINITY; + let mut max_projection = f32::NEG_INFINITY; + for &(x, z) in local_points.iter().chain(remote_points.iter()) { + let projection = x * axis_x + z * axis_z; + min_projection = min_projection.min(projection); + max_projection = max_projection.max(projection); + } + if !min_projection.is_finite() || !max_projection.is_finite() { + return 0.0; + } + let bin_size_meters = bin_size_meters.max(0.04); + let bin_count = + (((max_projection - min_projection) / bin_size_meters).ceil() as usize).max(1) + 1; + let mut local_hist = vec![0.0f32; bin_count]; + let mut remote_hist = vec![0.0f32; bin_count]; + for &(x, z) in local_points { + let projection = x * axis_x + z * axis_z; + let index = ((projection - min_projection) / bin_size_meters) + .floor() + .clamp(0.0, bin_count.saturating_sub(1) as f32) as usize; + local_hist[index] += 1.0; + } + for &(x, z) in remote_points { + let projection = x * axis_x + z * axis_z; + let index = ((projection - min_projection) / bin_size_meters) + .floor() + .clamp(0.0, bin_count.saturating_sub(1) as f32) as usize; + remote_hist[index] += 1.0; + } + let mut intersection = 0.0f32; + let mut union = 0.0f32; + for index in 0..bin_count { + intersection += local_hist[index].min(remote_hist[index]); + union += local_hist[index].max(remote_hist[index]); + } + intersection / union.max(1.0e-6) +} + +fn score_wall_profile_alignment( + local_points: &[(f32, f32)], + remote_points: &[(f32, f32)], + yaw_radians: f32, + translation_x: f32, + translation_z: f32, + bin_size_meters: f32, +) -> f32 { + if local_points.len() < SHAPE_MIN_CONTOUR_POINTS + || remote_points.len() < SHAPE_MIN_CONTOUR_POINTS + { + return 0.0; + } + let transformed_remote = remote_points + .iter() + .map(|&(x, z)| { + let (rotated_x, rotated_z) = rotate_xz(yaw_radians, x, z); + (rotated_x + translation_x, rotated_z + translation_z) + }) + .collect::>(); + let (axis_z, axis_x) = yaw_radians.sin_cos(); + let score_parallel = projection_histogram_overlap( + local_points, + &transformed_remote, + axis_x, + axis_z, + bin_size_meters, + ); + let score_perp = projection_histogram_overlap( + local_points, + &transformed_remote, + -axis_z, + axis_x, + bin_size_meters, + ); + (score_parallel * score_perp).sqrt() +} + +fn hough_yaw_candidates(local: &ShapeMatchGrid, remote: &ShapeMatchGrid) -> Vec { + let local_orientations = dominant_hough_orientations(local); + let remote_orientations = dominant_hough_orientations(remote); + let mut yaws = Vec::::new(); + for &local_theta in &local_orientations { + for &remote_theta in &remote_orientations { + push_shape_candidate( + &mut yaws, + ShapeMatchCandidate { + yaw_radians: wrap_angle(local_theta - remote_theta), + score: 1.0, + ..ShapeMatchCandidate::default() + }, + 12, + 0.01, + 4.0_f32.to_radians(), + ); + } + } + yaws.into_iter() + .map(|candidate| candidate.yaw_radians) + .collect() +} + +fn rotated_bounds(points: &[(f32, f32)], yaw_radians: f32) -> Option<(f32, f32, f32, f32)> { + let mut min_x = f32::INFINITY; + let mut max_x = f32::NEG_INFINITY; + let mut min_z = f32::INFINITY; + let mut max_z = f32::NEG_INFINITY; + for &(x, z) in points { + let (rotated_x, rotated_z) = rotate_xz(yaw_radians, x, z); + min_x = min_x.min(rotated_x); + max_x = max_x.max(rotated_x); + min_z = min_z.min(rotated_z); + max_z = max_z.max(rotated_z); + } + (min_x.is_finite() && max_x.is_finite() && min_z.is_finite() && max_z.is_finite()) + .then_some((min_x, max_x, min_z, max_z)) +} + +fn search_shape_global( + local: &ShapeMatchGrid, + remote: &ShapeMatchGrid, + top_k: usize, +) -> Vec { + let local_min_x = local.origin_x; + let local_max_x = local.max_x(); + let local_min_z = local.origin_z; + let local_max_z = local.max_z(); + let step = local.cell_size_meters.max(1.0e-3); + let translation_tolerance = step * 1.5; + let yaw_tolerance = SHAPE_GLOBAL_YAW_STEP_RADIANS * 1.25; + let local_top_k = top_k.saturating_mul(4).max(top_k); + let mut yaws = Vec::new(); + let mut yaw = -PI; + while yaw < PI { + yaws.push(yaw); + yaw += SHAPE_GLOBAL_YAW_STEP_RADIANS; + } + let worker_count = analysis_thread_count(yaws.len()); + let chunk_size = yaws.len().div_ceil(worker_count); + let partials = std::thread::scope(|scope| { + let mut handles = Vec::new(); + for yaw_chunk in yaws.chunks(chunk_size.max(1)) { + let yaw_chunk = yaw_chunk.to_vec(); + handles.push(scope.spawn(move || { + let mut finalists = Vec::new(); + for yaw in yaw_chunk { + let Some((remote_min_x, remote_max_x, remote_min_z, remote_max_z)) = + rotated_bounds(&remote.contour_points, yaw) + else { + continue; + }; + let translation_min_x = local_min_x - remote_max_x; + let translation_max_x = local_max_x - remote_min_x; + let translation_min_z = local_min_z - remote_max_z; + let translation_max_z = local_max_z - remote_min_z; + let steps_x = ((translation_max_x - translation_min_x) / step) + .ceil() + .max(0.0) as usize; + let steps_z = ((translation_max_z - translation_min_z) / step) + .ceil() + .max(0.0) as usize; + for x_step in 0..=steps_x { + let translation_x = translation_min_x + x_step as f32 * step; + for z_step in 0..=steps_z { + let translation_z = translation_min_z + z_step as f32 * step; + let candidate = score_combined_alignment_one_way( + local, + remote, + yaw, + translation_x, + translation_z, + ); + push_shape_candidate( + &mut finalists, + candidate, + local_top_k, + translation_tolerance, + yaw_tolerance, + ); + } + } + } + finalists + })); + } + let mut partials = Vec::new(); + for handle in handles { + partials.push(handle.join().expect("shape-global worker should not panic")); + } + partials + }); + let mut finalists = Vec::new(); + for partial in partials { + for candidate in partial { + push_shape_candidate( + &mut finalists, + candidate, + top_k, + translation_tolerance, + yaw_tolerance, + ); + } + } + finalists +} + +fn search_shape_global_yaws( + local: &ShapeMatchGrid, + remote: &ShapeMatchGrid, + yaw_candidates: &[f32], + top_k: usize, +) -> Vec { + let local_min_x = local.origin_x; + let local_max_x = local.max_x(); + let local_min_z = local.origin_z; + let local_max_z = local.max_z(); + let step = local.cell_size_meters.max(1.0e-3); + let translation_tolerance = step * 1.5; + let yaw_tolerance = SHAPE_GLOBAL_YAW_STEP_RADIANS * 1.25; + let local_top_k = top_k.saturating_mul(4).max(top_k); + let worker_count = analysis_thread_count(yaw_candidates.len()); + let chunk_size = yaw_candidates.len().max(1).div_ceil(worker_count); + let partials = std::thread::scope(|scope| { + let mut handles = Vec::new(); + for yaw_chunk in yaw_candidates.chunks(chunk_size.max(1)) { + let yaw_chunk = yaw_chunk.to_vec(); + handles.push(scope.spawn(move || { + let mut finalists = Vec::new(); + for yaw in yaw_chunk { + let Some((remote_min_x, remote_max_x, remote_min_z, remote_max_z)) = + rotated_bounds(&remote.contour_points, yaw) + else { + continue; + }; + let translation_min_x = local_min_x - remote_max_x; + let translation_max_x = local_max_x - remote_min_x; + let translation_min_z = local_min_z - remote_max_z; + let translation_max_z = local_max_z - remote_min_z; + let steps_x = ((translation_max_x - translation_min_x) / step) + .ceil() + .max(0.0) as usize; + let steps_z = ((translation_max_z - translation_min_z) / step) + .ceil() + .max(0.0) as usize; + for x_step in 0..=steps_x { + let translation_x = translation_min_x + x_step as f32 * step; + for z_step in 0..=steps_z { + let translation_z = translation_min_z + z_step as f32 * step; + let candidate = score_combined_alignment_one_way( + local, + remote, + yaw, + translation_x, + translation_z, + ); + push_shape_candidate( + &mut finalists, + candidate, + local_top_k, + translation_tolerance, + yaw_tolerance, + ); + } + } + } + finalists + })); + } + let mut partials = Vec::new(); + for handle in handles { + partials.push( + handle + .join() + .expect("shape-global-yaw worker should not panic"), + ); + } + partials + }); + let mut finalists = Vec::new(); + for partial in partials { + for candidate in partial { + push_shape_candidate( + &mut finalists, + candidate, + top_k, + translation_tolerance, + yaw_tolerance, + ); + } + } + finalists +} + +fn search_shape_refine( + local: &ShapeMatchGrid, + remote: &ShapeMatchGrid, + seeds: &[ShapeMatchCandidate], + translation_window_meters: f32, + yaw_window_radians: f32, + yaw_step_radians: f32, + top_k: usize, + symmetric: bool, +) -> Vec { + let step = local.cell_size_meters.max(1.0e-3); + let translation_steps = (translation_window_meters / step).ceil().max(1.0) as isize; + let yaw_steps = (yaw_window_radians / yaw_step_radians).ceil().max(1.0) as isize; + let translation_tolerance = step * 1.25; + let yaw_tolerance = yaw_step_radians * 1.25; + let local_top_k = top_k.saturating_mul(4).max(top_k); + let worker_count = analysis_thread_count(seeds.len()); + let chunk_size = seeds.len().max(1).div_ceil(worker_count); + let partials = std::thread::scope(|scope| { + let mut handles = Vec::new(); + for seed_chunk in seeds.chunks(chunk_size.max(1)) { + let seed_chunk = seed_chunk.to_vec(); + handles.push(scope.spawn(move || { + let mut finalists = Vec::new(); + for seed in seed_chunk { + for yaw_step in -yaw_steps..=yaw_steps { + let yaw_radians = + wrap_angle(seed.yaw_radians + yaw_step as f32 * yaw_step_radians); + for x_step in -translation_steps..=translation_steps { + let translation_x = seed.translation_x + x_step as f32 * step; + for z_step in -translation_steps..=translation_steps { + let translation_z = seed.translation_z + z_step as f32 * step; + let candidate = if symmetric { + score_shape_alignment_symmetric( + local, + remote, + yaw_radians, + translation_x, + translation_z, + ) + } else { + score_combined_alignment_one_way( + local, + remote, + yaw_radians, + translation_x, + translation_z, + ) + }; + push_shape_candidate( + &mut finalists, + candidate, + local_top_k, + translation_tolerance, + yaw_tolerance, + ); + } + } + } + } + finalists + })); + } + let mut partials = Vec::new(); + for handle in handles { + partials.push(handle.join().expect("shape-refine worker should not panic")); + } + partials + }); + let mut finalists = Vec::new(); + for partial in partials { + for candidate in partial { + push_shape_candidate( + &mut finalists, + candidate, + top_k, + translation_tolerance, + yaw_tolerance, + ); + } + } + finalists +} + +fn manual_shape_candidate( + local: &ShapeMatchGrid, + remote: &ShapeMatchGrid, + manual_pose: ManualPose, +) -> ShapeMatchCandidate { + score_shape_alignment_symmetric( + local, + remote, + manual_pose.rotation_radians, + manual_pose.shift_x_meters, + manual_pose.shift_y_meters, + ) +} + +fn score_mask_overlap_one_way( + local: &ShapeMatchGrid, + remote_points: &[(f32, f32)], + yaw_radians: f32, + translation_x: f32, + translation_z: f32, +) -> ShapeMatchCandidate { + if remote_points.is_empty() { + return ShapeMatchCandidate::default(); + } + let mut in_bounds = 0usize; + let mut hits = 0f32; + let mut spill = 0f32; + let mut unsupported = 0usize; + let mut free_spill = 0usize; + for &(remote_x, remote_z) in remote_points { + let (rotated_x, rotated_z) = rotate_xz(yaw_radians, remote_x, remote_z); + let world_x = rotated_x + translation_x; + let world_z = rotated_z + translation_z; + let Some(in_support) = local.sample_support_nearest(world_x, world_z) else { + continue; + }; + in_bounds += 1; + if !in_support { + unsupported += 1; + continue; + } + if local.sample_free_nearest(world_x, world_z).unwrap_or(false) { + free_spill += 1; + spill += 8.0; + continue; + } + if let Some(dist) = local.sample_distance_nearest(world_x, world_z) { + if dist < 0.15 { + hits += 1.0; + } else { + spill += 1.0; + } + } else { + spill += 1.0; + } + } + + if free_spill as f32 / in_bounds.max(1) as f32 > 0.05 { + return ShapeMatchCandidate::default(); + } + + if in_bounds < SHAPE_MIN_CONTOUR_POINTS.min(remote_points.len()) { + return ShapeMatchCandidate::default(); + } + + let coverage = in_bounds as f32 / remote_points.len().max(1) as f32; + let false_positives = spill + (unsupported as f32 * SHAPE_SEEDED_UNSUPPORTED_PENALTY); + let precision = hits / (hits + false_positives).max(1.0); + let score = coverage.sqrt() * precision; + + ShapeMatchCandidate { + score, + feature_score: score, + support_score: 0.0, + coverage, + close_ratio: precision, + mean_distance_meters: 1.0 - precision, + yaw_radians, + translation_x, + translation_z, + in_bounds_points: in_bounds, + } +} + +fn score_mask_overlap_symmetric( + local: &ShapeMatchGrid, + remote: &ShapeMatchGrid, + yaw_radians: f32, + translation_x: f32, + translation_z: f32, +) -> ShapeMatchCandidate { + let forward = score_mask_overlap_one_way( + local, + &remote.occupied_points, + yaw_radians, + translation_x, + translation_z, + ); + if forward.score <= 0.0 { + return forward; + } + let inverse = score_mask_overlap_one_way( + remote, + &local.occupied_points, + -yaw_radians, + -translation_x, + -translation_z, + ); + if inverse.score <= 0.0 { + return forward; + } + let score = (forward.score * inverse.score).sqrt(); + ShapeMatchCandidate { + score, + feature_score: score, + support_score: 0.0, + coverage: 0.5 * (forward.coverage + inverse.coverage), + close_ratio: 0.5 * (forward.close_ratio + inverse.close_ratio), + mean_distance_meters: 1.0 - 0.5 * (forward.close_ratio + inverse.close_ratio), + yaw_radians, + translation_x, + translation_z, + in_bounds_points: forward.in_bounds_points.min(inverse.in_bounds_points), + } +} + +fn score_signed_overlap_one_way( + local: &ShapeMatchGrid, + remote: &ShapeMatchGrid, + remote_points: &[(f32, f32)], + yaw_radians: f32, + translation_x: f32, + translation_z: f32, +) -> ShapeMatchCandidate { + if remote_points.is_empty() { + return ShapeMatchCandidate::default(); + } + let mut in_bounds_points = 0usize; + let mut in_bounds_weight = 0.0f32; + let mut supported_weight = 0.0f32; + let mut free_weight = 0.0f32; + let mut reward_sum = 0.0f32; + let mut penalty_sum = 0.0f32; + let mut close_weight = 0.0f32; + let mut total_weight = 0.0f32; + let mut distance_sum = 0.0f32; + for &(remote_x, remote_z) in remote_points { + let remote_weight = remote + .sample_occupied_weight_nearest(remote_x, remote_z) + .unwrap_or(1.0) + .max(0.25); + total_weight += remote_weight; + let (rotated_x, rotated_z) = rotate_xz(yaw_radians, remote_x, remote_z); + let world_x = rotated_x + translation_x; + let world_z = rotated_z + translation_z; + let Some(in_support) = local.sample_support_nearest(world_x, world_z) else { + continue; + }; + in_bounds_points += 1; + in_bounds_weight += remote_weight; + if !in_support { + penalty_sum += remote_weight * SHAPE_SEEDED_UNSUPPORTED_PENALTY; + distance_sum += SHAPE_MATCH_DISTANCE_TRUNCATE_METERS * remote_weight * 0.85; + continue; + } + if local.sample_free_nearest(world_x, world_z).unwrap_or(false) { + free_weight += remote_weight; + // Dramatically scale the penalty so that a 1-2% violation (solid wall blocking a doorway) + // drops the score far enough to break the mirror alias tie. + penalty_sum += remote_weight * 35.0; + distance_sum += SHAPE_MATCH_DISTANCE_TRUNCATE_METERS * remote_weight * 15.0; + continue; + } + supported_weight += remote_weight; + let signed_distance = local + .sample_signed_distance_nearest(world_x, world_z) + .unwrap_or(-SHAPE_MATCH_DISTANCE_TRUNCATE_METERS); + if signed_distance >= 0.0 { + let local_weight = local + .sample_occupied_weight_nearest(world_x, world_z) + .unwrap_or(1.0) + .max(0.25); + let interior_bonus = + (signed_distance / SHAPE_SEEDED_INTERIOR_BONUS_CAP_METERS).clamp(0.0, 1.0); + reward_sum += (remote_weight * local_weight).sqrt() * (1.0 + interior_bonus * 0.35); + distance_sum += (SHAPE_SEEDED_INTERIOR_BONUS_CAP_METERS - signed_distance) + .max(0.0) + .min(SHAPE_MATCH_DISTANCE_TRUNCATE_METERS) + * remote_weight; + if signed_distance >= local.cell_size_meters * 0.75 { + close_weight += remote_weight; + } + } else { + let empty_distance = (-signed_distance).min(SHAPE_MATCH_DISTANCE_TRUNCATE_METERS); + let severity = empty_distance / SHAPE_MATCH_DISTANCE_TRUNCATE_METERS; + penalty_sum += remote_weight * (0.22 + severity * SHAPE_SEEDED_SIGNED_EMPTY_PENALTY); + distance_sum += empty_distance * remote_weight; + } + } + + // HARD VETO: If more than 5% of our points landed squarely in the remote's known empty space, + // this pose is physically impossible (a table inside an empty hallway). Zero it entirely. + let free_ratio = free_weight / in_bounds_weight.max(1.0e-6); + if free_ratio > 0.07 { + if free_ratio > 0.15 { + // println!("Hard Veto applied! free_ratio={:.3}", free_ratio); + } + return ShapeMatchCandidate::default(); + } + + if in_bounds_points < SHAPE_MIN_CONTOUR_POINTS.min(remote_points.len()) + || total_weight <= 1.0e-6 + { + return ShapeMatchCandidate::default(); + } + let coverage = (in_bounds_weight / total_weight).clamp(0.0, 1.0); + let support_ratio = (supported_weight / in_bounds_weight.max(1.0e-6)).clamp(0.0, 1.0); + let close_ratio = (close_weight / total_weight).clamp(0.0, 1.0); + let reward_norm = reward_sum / total_weight.max(1.0e-6); + let penalty_norm = penalty_sum / total_weight.max(1.0e-6); + let raw = (reward_norm - penalty_norm * 0.72).max(0.0); + let score = raw * coverage.sqrt() * (0.30 + 0.70 * support_ratio.sqrt()); + ShapeMatchCandidate { + score, + feature_score: raw, + support_score: support_ratio, + coverage, + close_ratio, + mean_distance_meters: distance_sum / total_weight.max(1.0e-6), + yaw_radians, + translation_x, + translation_z, + in_bounds_points, + } +} + +fn score_signed_overlap_symmetric( + local: &ShapeMatchGrid, + remote: &ShapeMatchGrid, + yaw_radians: f32, + translation_x: f32, + translation_z: f32, +) -> ShapeMatchCandidate { + let forward = score_signed_overlap_one_way( + local, + remote, + &remote.occupied_points, + yaw_radians, + translation_x, + translation_z, + ); + if forward.score <= 0.0 { + return forward; + } + let inverse = score_signed_overlap_one_way( + remote, + local, + &local.occupied_points, + -yaw_radians, + -translation_x, + -translation_z, + ); + if inverse.score <= 0.0 { + return forward; + } + ShapeMatchCandidate { + score: (forward.score * inverse.score).sqrt(), + feature_score: (forward.feature_score * inverse.feature_score).sqrt(), + support_score: (forward.support_score * inverse.support_score).sqrt(), + coverage: 0.5 * (forward.coverage + inverse.coverage), + close_ratio: 0.5 * (forward.close_ratio + inverse.close_ratio), + mean_distance_meters: 0.5 * (forward.mean_distance_meters + inverse.mean_distance_meters), + yaw_radians, + translation_x, + translation_z, + in_bounds_points: forward.in_bounds_points.min(inverse.in_bounds_points), + } +} + +fn score_signed_support_symmetric( + local: &ShapeMatchGrid, + remote: &ShapeMatchGrid, + yaw_radians: f32, + translation_x: f32, + translation_z: f32, +) -> ShapeMatchCandidate { + let signed = + score_signed_overlap_symmetric(local, remote, yaw_radians, translation_x, translation_z); + if signed.score <= 0.0 { + return signed; + } + let (support_forward, support_cov_forward, support_hit_forward, support_points_forward) = + score_support_overlap_one_way( + local, + &remote.support_points, + yaw_radians, + translation_x, + translation_z, + ); + let (support_inverse, support_cov_inverse, support_hit_inverse, support_points_inverse) = + score_support_overlap_one_way( + remote, + &local.support_points, + -yaw_radians, + -translation_x, + -translation_z, + ); + let support_score = if support_forward > 0.0 && support_inverse > 0.0 { + (support_forward * support_inverse).sqrt() + } else { + 0.0 + }; + ShapeMatchCandidate { + score: signed.score * 0.62 + support_score * 0.38, + feature_score: signed.score, + support_score, + coverage: signed.coverage * 0.65 + 0.35 * (support_cov_forward + support_cov_inverse) * 0.5, + close_ratio: signed.close_ratio * 0.65 + + 0.35 * (support_hit_forward + support_hit_inverse) * 0.5, + mean_distance_meters: signed.mean_distance_meters, + yaw_radians, + translation_x, + translation_z, + in_bounds_points: signed + .in_bounds_points + .min(support_points_forward.max(signed.in_bounds_points)) + .min(support_points_inverse.max(signed.in_bounds_points)), + } +} + +fn apply_seed_prior( + mut candidate: ShapeMatchCandidate, + seed: ShapeMatchCandidate, +) -> ShapeMatchCandidate { + let translation_delta = ((candidate.translation_x - seed.translation_x).powi(2) + + (candidate.translation_z - seed.translation_z).powi(2)) + .sqrt(); + let yaw_delta = wrap_angle(candidate.yaw_radians - seed.yaw_radians).abs(); + let translation_term = + -0.5 * (translation_delta / SHAPE_SEEDED_PRIOR_TRANSLATION_SIGMA_METERS).powi(2); + let yaw_term = -0.5 * (yaw_delta / SHAPE_SEEDED_PRIOR_YAW_SIGMA_RADIANS).powi(2); + let prior = (translation_term + yaw_term).exp().clamp(0.0, 1.0); + let factor = SHAPE_SEEDED_PRIOR_FLOOR + (1.0 - SHAPE_SEEDED_PRIOR_FLOOR) * prior; + candidate.score *= factor; + candidate.support_score = candidate.support_score * 0.75 + prior * 0.25; + candidate +} + +fn search_mask_overlap_global( + local: &ShapeMatchGrid, + remote: &ShapeMatchGrid, + top_k: usize, +) -> Vec { + let mut finalists = Vec::new(); + let local_min_x = local.origin_x; + let local_max_x = local.max_x(); + let local_min_z = local.origin_z; + let local_max_z = local.max_z(); + let mut yaw = -PI; + while yaw < PI { + let Some((remote_min_x, remote_max_x, remote_min_z, remote_max_z)) = + rotated_bounds(&remote.occupied_points, yaw) + else { + yaw += SHAPE_GLOBAL_YAW_STEP_RADIANS; + continue; + }; + let translation_min_x = local_min_x - remote_max_x; + let translation_max_x = local_max_x - remote_min_x; + let translation_min_z = local_min_z - remote_max_z; + let translation_max_z = local_max_z - remote_min_z; + let step = local.cell_size_meters.max(1.0e-3); + let steps_x = ((translation_max_x - translation_min_x) / step) + .ceil() + .max(0.0) as usize; + let steps_z = ((translation_max_z - translation_min_z) / step) + .ceil() + .max(0.0) as usize; + for x_step in 0..=steps_x { + let translation_x = translation_min_x + x_step as f32 * step; + for z_step in 0..=steps_z { + let translation_z = translation_min_z + z_step as f32 * step; + let candidate = score_mask_overlap_one_way( + local, + &remote.occupied_points, + yaw, + translation_x, + translation_z, + ); + push_shape_candidate( + &mut finalists, + candidate, + top_k, + step * 1.5, + SHAPE_GLOBAL_YAW_STEP_RADIANS * 1.25, + ); + } + } + yaw += SHAPE_GLOBAL_YAW_STEP_RADIANS; + } + finalists +} + +fn search_mask_overlap_refine( + local: &ShapeMatchGrid, + remote: &ShapeMatchGrid, + seeds: &[ShapeMatchCandidate], + translation_window_meters: f32, + yaw_window_radians: f32, + yaw_step_radians: f32, + top_k: usize, +) -> Vec { + let mut finalists = Vec::new(); + let step = local.cell_size_meters.max(1.0e-3); + let translation_steps = (translation_window_meters / step).ceil().max(1.0) as isize; + let yaw_steps = (yaw_window_radians / yaw_step_radians).ceil().max(1.0) as isize; + for seed in seeds { + for yaw_step in -yaw_steps..=yaw_steps { + let yaw_radians = wrap_angle(seed.yaw_radians + yaw_step as f32 * yaw_step_radians); + for x_step in -translation_steps..=translation_steps { + let translation_x = seed.translation_x + x_step as f32 * step; + for z_step in -translation_steps..=translation_steps { + let translation_z = seed.translation_z + z_step as f32 * step; + let candidate = score_mask_overlap_symmetric( + local, + remote, + yaw_radians, + translation_x, + translation_z, + ); + push_shape_candidate( + &mut finalists, + candidate, + top_k, + step * 1.25, + yaw_step_radians * 1.25, + ); + } + } + } + } + finalists +} + +fn search_mask_overlap_translation_only( + local: &ShapeMatchGrid, + remote: &ShapeMatchGrid, + seed: ShapeMatchCandidate, + translation_window_meters: f32, + top_k: usize, +) -> Vec { + let mut finalists = Vec::new(); + let step = local.cell_size_meters.max(1.0e-3); + let translation_steps = (translation_window_meters / step).ceil().max(1.0) as isize; + for x_step in -translation_steps..=translation_steps { + let translation_x = seed.translation_x + x_step as f32 * step; + for z_step in -translation_steps..=translation_steps { + let translation_z = seed.translation_z + z_step as f32 * step; + let candidate = score_mask_overlap_symmetric( + local, + remote, + seed.yaw_radians, + translation_x, + translation_z, + ); + push_shape_candidate(&mut finalists, candidate, top_k, step * 1.25, 0.02); + } + } + finalists +} + +fn search_signed_overlap_seeded( + local: &ShapeMatchGrid, + remote: &ShapeMatchGrid, + seeds: &[ShapeMatchCandidate], + translation_window_meters: f32, + yaw_window_radians: f32, + yaw_step_radians: f32, + top_k: usize, +) -> Vec { + let mut finalists = Vec::new(); + let step = local.cell_size_meters.max(1.0e-3); + let translation_steps = (translation_window_meters / step).ceil().max(1.0) as isize; + let yaw_steps = (yaw_window_radians / yaw_step_radians).ceil().max(1.0) as isize; + for seed in seeds { + for yaw_step in -yaw_steps..=yaw_steps { + let yaw_radians = wrap_angle(seed.yaw_radians + yaw_step as f32 * yaw_step_radians); + for x_step in -translation_steps..=translation_steps { + let translation_x = seed.translation_x + x_step as f32 * step; + for z_step in -translation_steps..=translation_steps { + let translation_z = seed.translation_z + z_step as f32 * step; + let candidate = apply_seed_prior( + score_signed_overlap_symmetric( + local, + remote, + yaw_radians, + translation_x, + translation_z, + ), + *seed, + ); + push_shape_candidate( + &mut finalists, + candidate, + top_k, + step * 1.25, + yaw_step_radians * 1.25, + ); + } + } + } + } + finalists +} + +fn search_signed_support_seeded( + local: &ShapeMatchGrid, + remote: &ShapeMatchGrid, + seeds: &[ShapeMatchCandidate], + translation_window_meters: f32, + yaw_window_radians: f32, + yaw_step_radians: f32, + top_k: usize, +) -> Vec { + let mut finalists = Vec::new(); + let step = local.cell_size_meters.max(1.0e-3); + let translation_steps = (translation_window_meters / step).ceil().max(1.0) as isize; + let yaw_steps = (yaw_window_radians / yaw_step_radians).ceil().max(1.0) as isize; + for seed in seeds { + for yaw_step in -yaw_steps..=yaw_steps { + let yaw_radians = wrap_angle(seed.yaw_radians + yaw_step as f32 * yaw_step_radians); + for x_step in -translation_steps..=translation_steps { + let translation_x = seed.translation_x + x_step as f32 * step; + for z_step in -translation_steps..=translation_steps { + let translation_z = seed.translation_z + z_step as f32 * step; + let candidate = apply_seed_prior( + score_signed_support_symmetric( + local, + remote, + yaw_radians, + translation_x, + translation_z, + ), + *seed, + ); + push_shape_candidate( + &mut finalists, + candidate, + top_k, + step * 1.25, + yaw_step_radians * 1.25, + ); + } + } + } + } + finalists +} + +fn nearest_manual_finalist( + finalists: &[ShapeMatchCandidate], + manual_pose: ManualPose, +) -> Option { + finalists + .iter() + .copied() + .map(|candidate| { + let planar_delta_meters = ((candidate.translation_x - manual_pose.shift_x_meters) + .powi(2) + + (candidate.translation_z - manual_pose.shift_y_meters).powi(2)) + .sqrt(); + let yaw_delta_radians = + wrap_angle(candidate.yaw_radians - manual_pose.rotation_radians).abs(); + let combined_distance = planar_delta_meters + yaw_delta_radians * 0.35; + NearestManualFinalist { + candidate, + planar_delta_meters, + yaw_delta_radians, + combined_distance, + } + }) + .min_by(|left, right| { + left.combined_distance + .total_cmp(&right.combined_distance) + .then_with(|| right.candidate.score.total_cmp(&left.candidate.score)) + }) +} + +fn run_shape_match_analysis( + local: &XrDepthAlignDescriptor, + remote: &XrDepthAlignDescriptor, + manual_pose: Option, + band: &ShapeBandSpec, +) -> Option { + let (Some(local_map), Some(remote_map)) = + (local.height_map.as_ref(), remote.height_map.as_ref()) + else { + return None; + }; + + let local_global = build_shape_match_grid(local_map, SHAPE_GLOBAL_CELL_METERS, band)?; + let remote_global = build_shape_match_grid(remote_map, SHAPE_GLOBAL_CELL_METERS, band)?; + let global = search_shape_global(&local_global, &remote_global, SHAPE_GLOBAL_TOP_K); + if global.is_empty() { + return None; + } + + let local_refine = build_shape_match_grid(local_map, SHAPE_REFINE_CELL_METERS, band)?; + let remote_refine = build_shape_match_grid(remote_map, SHAPE_REFINE_CELL_METERS, band)?; + let refined = search_shape_refine( + &local_refine, + &remote_refine, + &global, + 0.60, + SHAPE_REFINE_YAW_WINDOW_RADIANS, + SHAPE_REFINE_YAW_STEP_RADIANS, + SHAPE_REFINE_TOP_K, + false, + ); + if refined.is_empty() { + return None; + } + + let local_final = build_shape_match_grid(local_map, SHAPE_FINAL_CELL_METERS, band)?; + let remote_final = build_shape_match_grid(remote_map, SHAPE_FINAL_CELL_METERS, band)?; + let finalists = search_shape_refine( + &local_final, + &remote_final, + &refined, + 0.24, + SHAPE_FINAL_YAW_WINDOW_RADIANS, + SHAPE_FINAL_YAW_STEP_RADIANS, + SHAPE_FINAL_TOP_K, + true, + ); + if finalists.is_empty() { + return None; + } + + let manual_candidate = manual_pose + .map(|manual_pose| manual_shape_candidate(&local_final, &remote_final, manual_pose)); + Some(ShapeMatchReport { + label: band.label.clone(), + finalists, + manual_candidate, + }) +} + +fn run_hough_shape_analysis( + local: &XrDepthAlignDescriptor, + remote: &XrDepthAlignDescriptor, + manual_pose: Option, +) -> Option { + let (Some(local_map), Some(remote_map)) = + (local.height_map.as_ref(), remote.height_map.as_ref()) + else { + return None; + }; + let band = shape_band_obstacle(); + let local_global = build_shape_match_grid(local_map, SHAPE_GLOBAL_CELL_METERS, &band)?; + let remote_global = build_shape_match_grid(remote_map, SHAPE_GLOBAL_CELL_METERS, &band)?; + let yaw_candidates = hough_yaw_candidates(&local_global, &remote_global); + if yaw_candidates.is_empty() { + return None; + } + let global = search_shape_global_yaws( + &local_global, + &remote_global, + &yaw_candidates, + SHAPE_GLOBAL_TOP_K, + ); + if global.is_empty() { + return None; + } + let local_refine = build_shape_match_grid(local_map, SHAPE_REFINE_CELL_METERS, &band)?; + let remote_refine = build_shape_match_grid(remote_map, SHAPE_REFINE_CELL_METERS, &band)?; + let refined = search_shape_refine( + &local_refine, + &remote_refine, + &global, + 0.60, + SHAPE_REFINE_YAW_WINDOW_RADIANS, + SHAPE_REFINE_YAW_STEP_RADIANS, + SHAPE_REFINE_TOP_K, + false, + ); + if refined.is_empty() { + return None; + } + let local_final = build_shape_match_grid(local_map, SHAPE_FINAL_CELL_METERS, &band)?; + let remote_final = build_shape_match_grid(remote_map, SHAPE_FINAL_CELL_METERS, &band)?; + let finalists = search_shape_refine( + &local_final, + &remote_final, + &refined, + 0.24, + SHAPE_FINAL_YAW_WINDOW_RADIANS, + SHAPE_FINAL_YAW_STEP_RADIANS, + SHAPE_FINAL_TOP_K, + true, + ); + if finalists.is_empty() { + return None; + } + let manual_candidate = manual_pose + .map(|manual_pose| manual_shape_candidate(&local_final, &remote_final, manual_pose)); + Some(ShapeMatchReport { + label: "shape_hough".to_string(), + finalists, + manual_candidate, + }) +} + +fn print_shape_match_report( + local: &XrDepthAlignDescriptor, + remote: &XrDepthAlignDescriptor, + report: &ShapeMatchReport, + elapsed_millis: u128, +) { + println!("{}_match_ms: {}", report.label, elapsed_millis); + for (index, candidate) in report.finalists.iter().take(3).enumerate() { + println!( + "{}_best_{}: yaw {:.3} rad ({:.1} deg) | translation ({:.3}, {:.3}, {:.3}) | score {:.3} | iou {:.3} | coverage {:.3} | close {:.3} | dist {:.3} m | points {}", + report.label, + index + 1, + candidate.yaw_radians, + candidate.yaw_radians.to_degrees(), + candidate.translation_x, + local.floor_y - remote.floor_y, + candidate.translation_z, + candidate.score, + candidate.support_score, + candidate.coverage, + candidate.close_ratio, + candidate.mean_distance_meters, + candidate.in_bounds_points, + ); + } + if let Some(manual_candidate) = report.manual_candidate { + let finalist_rank = report + .finalists + .iter() + .position(|candidate| candidate.score < manual_candidate.score) + .map(|rank| rank + 1) + .unwrap_or(report.finalists.len() + 1); + println!( + "{}_manual: yaw {:.3} rad ({:.1} deg) | translation ({:.3}, {:.3}, {:.3}) | score {:.3} | iou {:.3} | coverage {:.3} | close {:.3} | dist {:.3} m | finalist_rank {}", + report.label, + manual_candidate.yaw_radians, + manual_candidate.yaw_radians.to_degrees(), + manual_candidate.translation_x, + local.floor_y - remote.floor_y, + manual_candidate.translation_z, + manual_candidate.score, + manual_candidate.support_score, + manual_candidate.coverage, + manual_candidate.close_ratio, + manual_candidate.mean_distance_meters, + finalist_rank, + ); + } else { + println!("{}_manual: none", report.label); + } +} + +fn print_focused_match_report( + local: &XrDepthAlignDescriptor, + remote: &XrDepthAlignDescriptor, + report: &FocusedMatchReport, + elapsed_millis: u128, +) { + println!("{}_match_ms: {}", report.label, elapsed_millis); + for (index, candidate) in report.finalists.iter().take(3).enumerate() { + println!( + "{}_best_{}: yaw {:.3} rad ({:.1} deg) | translation ({:.3}, {:.3}, {:.3}) | score {:.3} | runtime {:.3} | base {:.3} | line {:.3} | blob {:.3} | prior {:.3} | residual {:.3} m | points {}", + report.label, + index + 1, + candidate.pose.yaw_radians, + candidate.pose.yaw_radians.to_degrees(), + candidate.pose.translation_x, + local.floor_y - remote.floor_y, + candidate.pose.translation_z, + candidate.score, + candidate.runtime_score, + candidate.signed_score, + candidate.line_score, + candidate.blob_score, + candidate.seed_prior, + candidate.runtime_residual_meters, + candidate.pose.in_bounds_points, + ); + } + if let Some(manual_candidate) = report.manual_candidate { + let finalist_rank = report + .finalists + .iter() + .position(|candidate| candidate.score < manual_candidate.score) + .map(|rank| rank + 1) + .unwrap_or(report.finalists.len() + 1); + println!( + "{}_manual: yaw {:.3} rad ({:.1} deg) | translation ({:.3}, {:.3}, {:.3}) | score {:.3} | runtime {:.3} | base {:.3} | line {:.3} | blob {:.3} | prior {:.3} | residual {:.3} m | finalist_rank {}", + report.label, + manual_candidate.pose.yaw_radians, + manual_candidate.pose.yaw_radians.to_degrees(), + manual_candidate.pose.translation_x, + local.floor_y - remote.floor_y, + manual_candidate.pose.translation_z, + manual_candidate.score, + manual_candidate.runtime_score, + manual_candidate.signed_score, + manual_candidate.line_score, + manual_candidate.blob_score, + manual_candidate.seed_prior, + manual_candidate.runtime_residual_meters, + finalist_rank, + ); + } else { + println!("{}_manual: none", report.label); + } +} + +fn focused_variant_score( + evidence: FocusedPoseEvidence, + weights: FocusedCueWeights, + has_blob_cue: bool, +) -> FocusedMatchCandidate { + let mut score_sum = weights.runtime_weight * evidence.runtime_score + + weights.signed_weight * evidence.signed_score + + weights.line_weight * evidence.line_score; + let mut total_weight = weights.runtime_weight + weights.signed_weight + weights.line_weight; + if has_blob_cue && weights.blob_weight > 0.0 { + score_sum += weights.blob_weight * evidence.blob_score; + total_weight += weights.blob_weight; + } + let score = (score_sum / total_weight.max(1.0e-6)) * evidence.seed_prior; + FocusedMatchCandidate { + pose: evidence.pose, + score, + runtime_score: evidence.runtime_score, + runtime_residual_meters: evidence.runtime_residual_meters, + signed_score: evidence.signed_score, + line_score: evidence.line_score, + blob_score: evidence.blob_score, + seed_prior: evidence.seed_prior, + } +} + +fn push_focused_evidence( + finalists: &mut Vec, + candidate: FocusedPoseEvidence, + limit: usize, + translation_epsilon: f32, + yaw_epsilon: f32, + has_blob_cue: bool, +) { + let candidate_score = focused_variant_score( + candidate, + FocusedCueWeights { + label: "coarse", + runtime_weight: 0.0, + signed_weight: 0.58, + line_weight: 0.28, + blob_weight: 0.14, + }, + has_blob_cue, + ) + .score; + if !candidate_score.is_finite() || candidate_score <= 0.0 { + return; + } + if let Some(existing) = finalists.iter_mut().find(|existing| { + !candidate_is_distinct( + existing.pose, + candidate.pose, + translation_epsilon, + yaw_epsilon, + ) + }) { + let existing_score = focused_variant_score( + *existing, + FocusedCueWeights { + label: "coarse", + runtime_weight: 0.0, + signed_weight: 0.58, + line_weight: 0.28, + blob_weight: 0.14, + }, + has_blob_cue, + ) + .score; + if candidate_score > existing_score { + *existing = candidate; + } + } else { + finalists.push(candidate); + } + finalists.sort_by(|left, right| { + focused_variant_score( + *right, + FocusedCueWeights { + label: "coarse", + runtime_weight: 0.0, + signed_weight: 0.58, + line_weight: 0.28, + blob_weight: 0.14, + }, + has_blob_cue, + ) + .score + .total_cmp( + &focused_variant_score( + *left, + FocusedCueWeights { + label: "coarse", + runtime_weight: 0.0, + signed_weight: 0.58, + line_weight: 0.28, + blob_weight: 0.14, + }, + has_blob_cue, + ) + .score, + ) + .then_with(|| { + left.pose + .mean_distance_meters + .total_cmp(&right.pose.mean_distance_meters) + }) + }); + finalists.truncate(limit.max(1)); +} + +fn score_focused_pose_evidence( + local: &XrDepthAlignDescriptor, + remote: &XrDepthAlignDescriptor, + wall_local: &ShapeMatchGrid, + wall_remote: &ShapeMatchGrid, + wall_local_points: &[(f32, f32)], + wall_remote_points: &[(f32, f32)], + blob_local_points: &[(f32, f32)], + blob_remote_points: &[(f32, f32)], + local_components: &[ShapeComponent], + remote_components: &[ShapeComponent], + seed_pose: ShapeMatchCandidate, + yaw_radians: f32, + translation_x: f32, + translation_z: f32, + with_runtime: bool, +) -> Option { + let base = score_shape_alignment_symmetric( + wall_local, + wall_remote, + yaw_radians, + translation_x, + translation_z, + ); + if base.score <= 0.0 { + return None; + } + let line_score = score_wall_profile_alignment( + wall_local_points, + wall_remote_points, + yaw_radians, + translation_x, + translation_z, + wall_local.cell_size_meters, + ); + let blob_score = if blob_local_points.len() >= SHAPE_MIN_CONTOUR_POINTS + && blob_remote_points.len() >= SHAPE_MIN_CONTOUR_POINTS + { + score_wall_profile_alignment( + blob_local_points, + blob_remote_points, + yaw_radians, + translation_x, + translation_z, + SHAPE_REFINE_CELL_METERS, + ) + } else { + 0.0 + }; + let pose = ShapeMatchCandidate { + yaw_radians, + translation_x, + translation_z, + coverage: base.coverage, + close_ratio: base.close_ratio, + mean_distance_meters: base.mean_distance_meters, + in_bounds_points: base.in_bounds_points, + ..ShapeMatchCandidate::default() + }; + let mut evidence = FocusedPoseEvidence { + pose, + signed_score: base.score, + line_score, + blob_score, + runtime_score: 0.0, + runtime_residual_meters: base.mean_distance_meters, + seed_prior: seed_prior_factor(pose, seed_pose), + }; + if with_runtime { + let rescored = local.rescore_remote_to_local( + remote, + XrDepthAlignSolution { + yaw_radians, + translation: vec3(translation_x, local.floor_y - remote.floor_y, translation_z), + ..XrDepthAlignSolution::default() + }, + ); + evidence.runtime_score = rescored.ranking_confidence(); + evidence.runtime_residual_meters = rescored.residual_meters; + evidence.pose.mean_distance_meters = if rescored.residual_meters.is_finite() { + rescored.residual_meters + } else { + base.mean_distance_meters + }; + } + Some(evidence) +} + +fn search_focused_pose_evidences( + local: &XrDepthAlignDescriptor, + remote: &XrDepthAlignDescriptor, + wall_local: &ShapeMatchGrid, + wall_remote: &ShapeMatchGrid, + wall_local_points: &[(f32, f32)], + wall_remote_points: &[(f32, f32)], + blob_local_points: &[(f32, f32)], + blob_remote_points: &[(f32, f32)], + local_components: &[ShapeComponent], + remote_components: &[ShapeComponent], + seeds: &[ShapeMatchCandidate], + translation_window_meters: f32, + translation_step_meters: f32, + yaw_window_radians: f32, + yaw_step_radians: f32, + top_k: usize, +) -> Vec { + if seeds.is_empty() { + return Vec::new(); + } + let has_blob_cue = blob_local_points.len() >= SHAPE_MIN_CONTOUR_POINTS + && blob_remote_points.len() >= SHAPE_MIN_CONTOUR_POINTS; + let worker_count = analysis_thread_count(seeds.len()); + let chunk_size = seeds.len().div_ceil(worker_count.max(1)); + let partials = std::thread::scope(|scope| { + let mut handles = Vec::new(); + for seed_chunk in seeds.chunks(chunk_size.max(1)) { + let seed_chunk = seed_chunk.to_vec(); + handles.push(scope.spawn(move || { + let mut finalists = Vec::new(); + let translation_steps = (translation_window_meters / translation_step_meters) + .ceil() + .max(1.0) as isize; + let yaw_steps = (yaw_window_radians / yaw_step_radians).ceil().max(1.0) as isize; + for seed in &seed_chunk { + for yaw_step in -yaw_steps..=yaw_steps { + let yaw_radians = + wrap_angle(seed.yaw_radians + yaw_step as f32 * yaw_step_radians); + for x_step in -translation_steps..=translation_steps { + let translation_x = + seed.translation_x + x_step as f32 * translation_step_meters; + for z_step in -translation_steps..=translation_steps { + let translation_z = + seed.translation_z + z_step as f32 * translation_step_meters; + let Some(evidence) = score_focused_pose_evidence( + local, + remote, + wall_local, + wall_remote, + wall_local_points, + wall_remote_points, + blob_local_points, + blob_remote_points, + local_components, + remote_components, + *seed, + yaw_radians, + translation_x, + translation_z, + false, + ) else { + continue; + }; + push_focused_evidence( + &mut finalists, + evidence, + top_k, + translation_step_meters * 0.9, + yaw_step_radians * 1.25, + has_blob_cue, + ); + } + } + } + } + finalists + })); + } + let mut partials = Vec::new(); + for handle in handles { + partials.push( + handle + .join() + .expect("focused local-search worker should not panic"), + ); + } + partials + }); + + let mut finalists = Vec::new(); + for partial in partials { + for candidate in partial { + push_focused_evidence( + &mut finalists, + candidate, + top_k, + translation_step_meters * 0.9, + yaw_step_radians * 1.25, + has_blob_cue, + ); + } + } + finalists +} + +fn build_focused_match_report( + local: &XrDepthAlignDescriptor, + remote: &XrDepthAlignDescriptor, + wall_local: &ShapeMatchGrid, + wall_remote: &ShapeMatchGrid, + wall_local_points: &[(f32, f32)], + wall_remote_points: &[(f32, f32)], + blob_local_points: &[(f32, f32)], + blob_remote_points: &[(f32, f32)], + local_components: &[ShapeComponent], + remote_components: &[ShapeComponent], + label: &str, + weights: FocusedCueWeights, + seed_pose: ShapeMatchCandidate, + evidences: &[FocusedPoseEvidence], + manual_pose: Option, +) -> FocusedMatchReport { + let has_blob_cue = blob_local_points.len() >= SHAPE_MIN_CONTOUR_POINTS + && blob_remote_points.len() >= SHAPE_MIN_CONTOUR_POINTS; + let mut finalists = evidences + .iter() + .copied() + .map(|evidence| focused_variant_score(evidence, weights, has_blob_cue)) + .collect::>(); + finalists.sort_by(|left, right| { + right.score.total_cmp(&left.score).then_with(|| { + left.runtime_residual_meters + .total_cmp(&right.runtime_residual_meters) + }) + }); + finalists.truncate(FOCUSED_FINAL_TOP_K); + let manual_candidate = manual_pose.and_then(|manual_pose| { + score_focused_pose_evidence( + local, + remote, + wall_local, + wall_remote, + wall_local_points, + wall_remote_points, + blob_local_points, + blob_remote_points, + local_components, + remote_components, + seed_pose, + manual_pose.rotation_radians, + manual_pose.shift_x_meters, + manual_pose.shift_y_meters, + true, + ) + .map(|evidence| focused_variant_score(evidence, weights, has_blob_cue)) + }); + FocusedMatchReport { + label: label.to_string(), + finalists, + manual_candidate, + } +} + +fn run_runtime_seeded_focus_suite( + local: &XrDepthAlignDescriptor, + remote: &XrDepthAlignDescriptor, + manual_pose: Option, + seed: XrDepthAlignSolution, +) -> Vec { + let structural_band = shape_band_obstacle(); + let wall_band = ShapeBandSpec { + label: "focused_wall".to_string(), + min_height_meters: 0.60, + max_height_meters: Some(1.80), + floor_offset_meters: 0.0, + drop_border_components: false, + }; + let (Some(local_map), Some(remote_map)) = + (local.height_map.as_ref(), remote.height_map.as_ref()) + else { + return Vec::new(); + }; + let Some(structural_local) = + build_shape_match_grid(local_map, SHAPE_FINAL_CELL_METERS, &structural_band) + else { + return Vec::new(); + }; + let Some(structural_remote) = + build_shape_match_grid(remote_map, SHAPE_FINAL_CELL_METERS, &structural_band) + else { + return Vec::new(); + }; + let Some(wall_local) = build_shape_match_grid(local_map, SHAPE_FINAL_CELL_METERS, &wall_band) + else { + return Vec::new(); + }; + let Some(wall_remote) = build_shape_match_grid(remote_map, SHAPE_FINAL_CELL_METERS, &wall_band) + else { + return Vec::new(); + }; + let local_lines = extract_hough_lines(&wall_local, FOCUSED_HOUGH_TOP_LINES); + let remote_lines = extract_hough_lines(&wall_remote, FOCUSED_HOUGH_TOP_LINES); + + let furniture_band = shape_band_furniture(); + let local_blob_grid = + build_shape_match_grid(local_map, SHAPE_REFINE_CELL_METERS, &furniture_band); + let remote_blob_grid = + build_shape_match_grid(remote_map, SHAPE_REFINE_CELL_METERS, &furniture_band); + let local_components = local_blob_grid + .as_ref() + .map(|grid| extract_shape_components(grid, 8)) + .unwrap_or_default(); + let remote_components = remote_blob_grid + .as_ref() + .map(|grid| extract_shape_components(grid, 8)) + .unwrap_or_default(); + let local_blob_points = local_blob_grid + .as_ref() + .map(|grid| grid.occupied_points.as_slice()) + .unwrap_or(&[]); + let remote_blob_points = remote_blob_grid + .as_ref() + .map(|grid| grid.occupied_points.as_slice()) + .unwrap_or(&[]); + let seed_pose = ShapeMatchCandidate { + yaw_radians: seed.yaw_radians, + translation_x: seed.translation.x, + translation_z: seed.translation.z, + ..ShapeMatchCandidate::default() + }; + let mut seeds = vec![seed_pose]; + for candidate in search_mask_overlap_translation_only( + &structural_local, + &structural_remote, + seed_pose, + FOCUSED_LOCAL_TRANSLATION_WINDOW_METERS, + 6, + ) { + push_shape_candidate( + &mut seeds, + candidate, + 6, + SHAPE_FINAL_CELL_METERS * 1.1, + 0.02, + ); + } + let coarse = search_focused_pose_evidences( + local, + remote, + &structural_local, + &structural_remote, + &wall_local.contour_points, + &wall_remote.contour_points, + local_blob_points, + remote_blob_points, + &local_components, + &remote_components, + &seeds, + FOCUSED_LOCAL_TRANSLATION_WINDOW_METERS, + SHAPE_FINAL_CELL_METERS * 2.0, + FOCUSED_LOCAL_YAW_WINDOW_RADIANS, + FOCUSED_LOCAL_YAW_STEP_RADIANS, + FOCUSED_SEARCH_TOP_K, + ); + if coarse.is_empty() { + return Vec::new(); + } + let refine_seeds = coarse + .iter() + .map(|evidence| evidence.pose) + .collect::>(); + let mut refined = search_focused_pose_evidences( + local, + remote, + &structural_local, + &structural_remote, + &wall_local.contour_points, + &wall_remote.contour_points, + local_blob_points, + remote_blob_points, + &local_components, + &remote_components, + &refine_seeds, + FOCUSED_REFINE_TRANSLATION_WINDOW_METERS, + SHAPE_FINAL_CELL_METERS * 0.5, + FOCUSED_REFINE_YAW_WINDOW_RADIANS, + FOCUSED_REFINE_YAW_STEP_RADIANS, + FOCUSED_SEARCH_TOP_K, + ); + if refined.is_empty() { + refined = coarse; + } + for evidence in &mut refined { + if let Some(runtime_evidence) = score_focused_pose_evidence( + local, + remote, + &structural_local, + &structural_remote, + &wall_local.contour_points, + &wall_remote.contour_points, + local_blob_points, + remote_blob_points, + &local_components, + &remote_components, + seed_pose, + evidence.pose.yaw_radians, + evidence.pose.translation_x, + evidence.pose.translation_z, + true, + ) { + evidence.runtime_score = runtime_evidence.runtime_score; + evidence.runtime_residual_meters = runtime_evidence.runtime_residual_meters; + evidence.pose.mean_distance_meters = runtime_evidence.pose.mean_distance_meters; + } + } + let mut locked_translation = search_focused_pose_evidences( + local, + remote, + &structural_local, + &structural_remote, + &wall_local.contour_points, + &wall_remote.contour_points, + local_blob_points, + remote_blob_points, + &local_components, + &remote_components, + &[seed_pose], + SHAPE_FINAL_CELL_METERS, + SHAPE_FINAL_CELL_METERS, + FOCUSED_LOCAL_YAW_WINDOW_RADIANS, + FOCUSED_REFINE_YAW_STEP_RADIANS, + FOCUSED_SEARCH_TOP_K, + ); + for evidence in &mut locked_translation { + if let Some(runtime_evidence) = score_focused_pose_evidence( + local, + remote, + &structural_local, + &structural_remote, + &wall_local.contour_points, + &wall_remote.contour_points, + local_blob_points, + remote_blob_points, + &local_components, + &remote_components, + seed_pose, + evidence.pose.yaw_radians, + evidence.pose.translation_x, + evidence.pose.translation_z, + true, + ) { + evidence.runtime_score = runtime_evidence.runtime_score; + evidence.runtime_residual_meters = runtime_evidence.runtime_residual_meters; + evidence.pose.mean_distance_meters = runtime_evidence.pose.mean_distance_meters; + } + } + let variants = [ + FocusedCueWeights { + label: "runtime_seeded_signed_local", + runtime_weight: 0.45, + signed_weight: 0.55, + line_weight: 0.0, + blob_weight: 0.0, + }, + FocusedCueWeights { + label: "runtime_seeded_line_local", + runtime_weight: 0.38, + signed_weight: 0.37, + line_weight: 0.25, + blob_weight: 0.0, + }, + FocusedCueWeights { + label: "runtime_seeded_blob_local", + runtime_weight: 0.38, + signed_weight: 0.37, + line_weight: 0.0, + blob_weight: 0.25, + }, + FocusedCueWeights { + label: "runtime_seeded_line_blob_local", + runtime_weight: 0.36, + signed_weight: 0.34, + line_weight: 0.22, + blob_weight: 0.08, + }, + ]; + let mut reports = vec![build_focused_match_report( + local, + remote, + &structural_local, + &structural_remote, + &wall_local.contour_points, + &wall_remote.contour_points, + local_blob_points, + remote_blob_points, + &local_components, + &remote_components, + "runtime_seeded_line_blob_locktx", + FocusedCueWeights { + label: "runtime_seeded_line_blob_locktx", + runtime_weight: 0.40, + signed_weight: 0.20, + line_weight: 0.28, + blob_weight: 0.12, + }, + seed_pose, + &locked_translation, + manual_pose, + )]; + reports.extend(variants.into_iter().map(|weights| { + build_focused_match_report( + local, + remote, + &structural_local, + &structural_remote, + &wall_local.contour_points, + &wall_remote.contour_points, + local_blob_points, + remote_blob_points, + &local_components, + &remote_components, + weights.label, + weights, + seed_pose, + &refined, + manual_pose, + ) + })); + reports +} + +fn run_shape_band_scan( + local: &XrDepthAlignDescriptor, + remote: &XrDepthAlignDescriptor, + manual_pose: Option, +) -> Vec { + let bands = shape_band_scans(); + if bands.is_empty() { + return Vec::new(); + } + let worker_count = analysis_thread_count(bands.len()); + let chunk_size = bands.len().div_ceil(worker_count); + std::thread::scope(|scope| { + let mut handles = Vec::new(); + for band_chunk in bands.chunks(chunk_size.max(1)) { + let band_chunk = band_chunk.to_vec(); + handles.push(scope.spawn(move || { + let mut results = Vec::new(); + for band in band_chunk { + let Some(report) = run_shape_match_analysis(local, remote, manual_pose, &band) + else { + continue; + }; + let nearest_manual_finalist = manual_pose.and_then(|manual_pose| { + nearest_manual_finalist(&report.finalists, manual_pose) + }); + results.push(ShapeBandScanResult { + report, + nearest_manual_finalist, + }); + } + results + })); + } + let mut results = Vec::new(); + for handle in handles { + results.extend(handle.join().expect("shape-scan worker should not panic")); + } + results + }) +} + +fn run_fixed_overlap_band_analysis( + local: &XrDepthAlignDescriptor, + remote: &XrDepthAlignDescriptor, + manual_pose: Option, + band: &ShapeBandSpec, +) -> Option { + let (Some(local_map), Some(remote_map)) = + (local.height_map.as_ref(), remote.height_map.as_ref()) + else { + return None; + }; + let local_global = build_shape_match_grid(local_map, SHAPE_GLOBAL_CELL_METERS, band)?; + let remote_global = build_shape_match_grid(remote_map, SHAPE_GLOBAL_CELL_METERS, band)?; + let global = search_mask_overlap_global(&local_global, &remote_global, SHAPE_GLOBAL_TOP_K); + if global.is_empty() { + return None; + } + let local_final = build_shape_match_grid(local_map, SHAPE_FINAL_CELL_METERS, band)?; + let remote_final = build_shape_match_grid(remote_map, SHAPE_FINAL_CELL_METERS, band)?; + let finalists = search_mask_overlap_refine( + &local_final, + &remote_final, + &global, + 0.36, + SHAPE_REFINE_YAW_WINDOW_RADIANS, + SHAPE_REFINE_YAW_STEP_RADIANS, + SHAPE_FINAL_TOP_K, + ); + if finalists.is_empty() { + return None; + } + let manual_candidate = manual_pose.map(|manual_pose| { + score_mask_overlap_symmetric( + &local_final, + &remote_final, + manual_pose.rotation_radians, + manual_pose.shift_x_meters, + manual_pose.shift_y_meters, + ) + }); + Some(ShapeMatchReport { + label: band.label.clone(), + finalists, + manual_candidate, + }) +} + +fn run_seeded_overlap_band_analysis( + local: &XrDepthAlignDescriptor, + remote: &XrDepthAlignDescriptor, + band: &ShapeBandSpec, + seed: XrDepthAlignSolution, +) -> Option { + let (Some(local_map), Some(remote_map)) = + (local.height_map.as_ref(), remote.height_map.as_ref()) + else { + return None; + }; + let local_final = build_shape_match_grid(local_map, SHAPE_FINAL_CELL_METERS, band)?; + let remote_final = build_shape_match_grid(remote_map, SHAPE_FINAL_CELL_METERS, band)?; + let seed_candidate = ShapeMatchCandidate { + yaw_radians: seed.yaw_radians, + translation_x: seed.translation.x, + translation_z: seed.translation.z, + ..ShapeMatchCandidate::default() + }; + let finalists = search_mask_overlap_refine( + &local_final, + &remote_final, + &[seed_candidate], + 0.36, + 10.0_f32.to_radians(), + 1.0_f32.to_radians(), + SHAPE_FINAL_TOP_K, + ); + if finalists.is_empty() { + return None; + } + Some(ShapeMatchReport { + label: format!("{}_seeded", band.label), + finalists, + manual_candidate: None, + }) +} + +fn gather_seed_candidates( + diagnostic: &XrDepthAlignSolveDiagnostic, + reports: &[&ShapeMatchReport], +) -> Vec { + let mut seeds = Vec::new(); + if let Some(best) = diagnostic.best_solution { + push_shape_candidate( + &mut seeds, + ShapeMatchCandidate { + yaw_radians: best.yaw_radians, + translation_x: best.translation.x, + translation_z: best.translation.z, + score: best.confidence.max(0.001), + ..ShapeMatchCandidate::default() + }, + 8, + 0.06, + 1.0_f32.to_radians(), + ); + } + for report in reports { + for candidate in report.finalists.iter().take(2) { + push_shape_candidate(&mut seeds, *candidate, 8, 0.06, 1.0_f32.to_radians()); + } + } + seeds +} + +fn run_seeded_signed_overlap_band_analysis( + local: &XrDepthAlignDescriptor, + remote: &XrDepthAlignDescriptor, + manual_pose: Option, + band: &ShapeBandSpec, + seeds: &[ShapeMatchCandidate], +) -> Option { + if seeds.is_empty() { + return None; + } + let (Some(local_map), Some(remote_map)) = + (local.height_map.as_ref(), remote.height_map.as_ref()) + else { + return None; + }; + let local_final = build_shape_match_grid(local_map, SHAPE_FINAL_CELL_METERS, band)?; + let remote_final = build_shape_match_grid(remote_map, SHAPE_FINAL_CELL_METERS, band)?; + let mut finalists = search_signed_overlap_seeded( + &local_final, + &remote_final, + seeds, + SHAPE_SEEDED_LOCAL_TRANSLATION_WINDOW_METERS, + SHAPE_SEEDED_LOCAL_YAW_WINDOW_RADIANS, + SHAPE_SEEDED_LOCAL_YAW_STEP_RADIANS, + SHAPE_FINAL_TOP_K, + ); + if finalists.is_empty() { + return None; + } + let manual_candidate = manual_pose.map(|manual_pose| { + let manual_candidate = ShapeMatchCandidate { + yaw_radians: manual_pose.rotation_radians, + translation_x: manual_pose.shift_x_meters, + translation_z: manual_pose.shift_y_meters, + ..ShapeMatchCandidate::default() + }; + let nearest_seed = seeds + .iter() + .copied() + .min_by(|left, right| { + let left_delta = ((left.translation_x - manual_candidate.translation_x).powi(2) + + (left.translation_z - manual_candidate.translation_z).powi(2)) + .sqrt() + + wrap_angle(left.yaw_radians - manual_candidate.yaw_radians).abs() * 0.35; + let right_delta = ((right.translation_x - manual_candidate.translation_x).powi(2) + + (right.translation_z - manual_candidate.translation_z).powi(2)) + .sqrt() + + wrap_angle(right.yaw_radians - manual_candidate.yaw_radians).abs() * 0.35; + left_delta.total_cmp(&right_delta) + }) + .unwrap_or(manual_candidate); + apply_seed_prior( + score_signed_overlap_symmetric( + &local_final, + &remote_final, + manual_pose.rotation_radians, + manual_pose.shift_x_meters, + manual_pose.shift_y_meters, + ), + nearest_seed, + ) + }); + + Some(ShapeMatchReport { + label: format!("{}_signed_seeded", band.label), + finalists, + manual_candidate, + }) +} + +fn run_seeded_signed_support_band_analysis( + local: &XrDepthAlignDescriptor, + remote: &XrDepthAlignDescriptor, + manual_pose: Option, + band: &ShapeBandSpec, + seeds: &[ShapeMatchCandidate], +) -> Option { + if seeds.is_empty() { + return None; + } + let (Some(local_map), Some(remote_map)) = + (local.height_map.as_ref(), remote.height_map.as_ref()) + else { + return None; + }; + let local_final = build_shape_match_grid(local_map, SHAPE_FINAL_CELL_METERS, band)?; + let remote_final = build_shape_match_grid(remote_map, SHAPE_FINAL_CELL_METERS, band)?; + let finalists = search_signed_support_seeded( + &local_final, + &remote_final, + seeds, + SHAPE_SEEDED_LOCAL_TRANSLATION_WINDOW_METERS, + SHAPE_SEEDED_LOCAL_YAW_WINDOW_RADIANS, + SHAPE_SEEDED_LOCAL_YAW_STEP_RADIANS, + SHAPE_FINAL_TOP_K, + ); + if finalists.is_empty() { + return None; + } + let manual_candidate = manual_pose.map(|manual_pose| { + let manual_candidate = ShapeMatchCandidate { + yaw_radians: manual_pose.rotation_radians, + translation_x: manual_pose.shift_x_meters, + translation_z: manual_pose.shift_y_meters, + ..ShapeMatchCandidate::default() + }; + let nearest_seed = seeds + .iter() + .copied() + .min_by(|left, right| { + let left_delta = ((left.translation_x - manual_candidate.translation_x).powi(2) + + (left.translation_z - manual_candidate.translation_z).powi(2)) + .sqrt() + + wrap_angle(left.yaw_radians - manual_candidate.yaw_radians).abs() * 0.35; + let right_delta = ((right.translation_x - manual_candidate.translation_x).powi(2) + + (right.translation_z - manual_candidate.translation_z).powi(2)) + .sqrt() + + wrap_angle(right.yaw_radians - manual_candidate.yaw_radians).abs() * 0.35; + left_delta.total_cmp(&right_delta) + }) + .unwrap_or(manual_candidate); + apply_seed_prior( + score_signed_support_symmetric( + &local_final, + &remote_final, + manual_pose.rotation_radians, + manual_pose.shift_x_meters, + manual_pose.shift_y_meters, + ), + nearest_seed, + ) + }); + Some(ShapeMatchReport { + label: format!("{}_signed_support_seeded", band.label), + finalists, + manual_candidate, + }) +} + +fn build_seeded_consensus_band_grids( + local: &XrDepthAlignDescriptor, + remote: &XrDepthAlignDescriptor, +) -> Vec { + build_consensus_band_grids_at_cell(local, remote, SHAPE_FINAL_CELL_METERS) +} + +fn build_consensus_band_grids_at_cell( + local: &XrDepthAlignDescriptor, + remote: &XrDepthAlignDescriptor, + cell_size_meters: f32, +) -> Vec { + let (Some(local_map), Some(remote_map)) = + (local.height_map.as_ref(), remote.height_map.as_ref()) + else { + return Vec::new(); + }; + seeded_consensus_bands() + .into_iter() + .filter_map(|band| { + let local_grid = build_shape_match_grid(local_map, cell_size_meters, &band)?; + let remote_grid = build_shape_match_grid(remote_map, cell_size_meters, &band)?; + Some(BandGridPair { + local: local_grid, + remote: remote_grid, + }) + }) + .collect() +} + +fn score_consensus_candidate( + candidate: ShapeMatchCandidate, + bands: &[BandGridPair], +) -> ShapeMatchCandidate { + let mut scores = Vec::new(); + let mut coverage_sum = 0.0; + let mut close_sum = 0.0; + let mut dist_sum = 0.0; + let mut points_sum = 0usize; + for band in bands { + let score = score_mask_overlap_symmetric( + &band.local, + &band.remote, + candidate.yaw_radians, + candidate.translation_x, + candidate.translation_z, + ); + scores.push(score.score); + coverage_sum += score.coverage; + close_sum += score.close_ratio; + dist_sum += score.mean_distance_meters; + points_sum += score.in_bounds_points; + } + if scores.is_empty() { + return ShapeMatchCandidate::default(); + } + + // Evaluate pure floor-plan Support IoU to mathematically break symmetric wall mirror aliases. + // The Flipped Room fills the unobserved "gap" of the true room, maximizing overlap but ALSO massively inflating the Union. + let mut iou_score = 1.0f32; + if let Some(longest_band) = bands.iter().max_by_key(|b| b.local.support_points.len()) { + iou_score = score_support_iou_symmetric( + &longest_band.local, + &longest_band.remote, + candidate.yaw_radians, + candidate.translation_x, + candidate.translation_z, + ); + } + + let avg_score = scores.iter().copied().sum::() / scores.len() as f32; + let min_score = scores.iter().copied().fold(f32::INFINITY, f32::min); + + // IOU is a huge separator for identical overlapping walls that are structurally flipped. + let raw_score = (avg_score * 0.72 + min_score * 0.28) * iou_score.sqrt(); + + ShapeMatchCandidate { + score: raw_score, + feature_score: avg_score, + support_score: iou_score, + coverage: coverage_sum / scores.len() as f32, + close_ratio: close_sum / scores.len() as f32, + mean_distance_meters: dist_sum / scores.len() as f32, + yaw_radians: candidate.yaw_radians, + translation_x: candidate.translation_x, + translation_z: candidate.translation_z, + in_bounds_points: points_sum / scores.len(), + } +} + +fn representative_band<'a>(bands: &'a [BandGridPair]) -> Option<&'a BandGridPair> { + bands + .iter() + .max_by_key(|band| band.local.occupied_points.len()) +} + +fn search_consensus_global( + bands: &[BandGridPair], + yaw_step_radians: f32, + top_k: usize, +) -> Vec { + let Some(representative) = representative_band(bands) else { + return Vec::new(); + }; + let local = &representative.local; + let remote = &representative.remote; + let local_min_x = local.origin_x; + let local_max_x = local.max_x(); + let local_min_z = local.origin_z; + let local_max_z = local.max_z(); + let mut finalists = Vec::new(); + let mut yaw = -PI; + while yaw < PI { + let Some((remote_min_x, remote_max_x, remote_min_z, remote_max_z)) = + rotated_bounds(&remote.occupied_points, yaw) + else { + yaw += yaw_step_radians; + continue; + }; + let translation_min_x = local_min_x - remote_max_x; + let translation_max_x = local_max_x - remote_min_x; + let translation_min_z = local_min_z - remote_max_z; + let translation_max_z = local_max_z - remote_min_z; + let step = local.cell_size_meters.max(1.0e-3); + let steps_x = ((translation_max_x - translation_min_x) / step) + .ceil() + .max(0.0) as usize; + let steps_z = ((translation_max_z - translation_min_z) / step) + .ceil() + .max(0.0) as usize; + for x_step in 0..=steps_x { + let translation_x = translation_min_x + x_step as f32 * step; + for z_step in 0..=steps_z { + let translation_z = translation_min_z + z_step as f32 * step; + let candidate = score_consensus_candidate( + ShapeMatchCandidate { + yaw_radians: yaw, + translation_x, + translation_z, + ..ShapeMatchCandidate::default() + }, + bands, + ); + push_shape_candidate( + &mut finalists, + candidate, + top_k, + step * 1.5, + yaw_step_radians * 1.25, + ); + } + } + yaw += yaw_step_radians; + } + finalists +} + +fn search_consensus_refine( + bands: &[BandGridPair], + seeds: &[ShapeMatchCandidate], + translation_window_meters: f32, + yaw_window_radians: f32, + yaw_step_radians: f32, + top_k: usize, +) -> Vec { + let Some(representative) = representative_band(bands) else { + return Vec::new(); + }; + let mut finalists = Vec::new(); + let step = representative.local.cell_size_meters.max(1.0e-3); + let translation_steps = (translation_window_meters / step).ceil().max(1.0) as isize; + let yaw_steps = (yaw_window_radians / yaw_step_radians).ceil().max(1.0) as isize; + for seed in seeds { + for yaw_step in -yaw_steps..=yaw_steps { + let yaw_radians = wrap_angle(seed.yaw_radians + yaw_step as f32 * yaw_step_radians); + for x_step in -translation_steps..=translation_steps { + let translation_x = seed.translation_x + x_step as f32 * step; + for z_step in -translation_steps..=translation_steps { + let translation_z = seed.translation_z + z_step as f32 * step; + let candidate = score_consensus_candidate( + ShapeMatchCandidate { + yaw_radians, + translation_x, + translation_z, + ..ShapeMatchCandidate::default() + }, + bands, + ); + push_shape_candidate( + &mut finalists, + candidate, + top_k, + step * 1.25, + yaw_step_radians * 1.25, + ); + } + } + } + } + finalists +} + +fn run_seeded_multiband_consensus_analysis( + local: &XrDepthAlignDescriptor, + remote: &XrDepthAlignDescriptor, + manual_pose: Option, + seed: XrDepthAlignSolution, +) -> Option { + let bands = build_seeded_consensus_band_grids(local, remote); + if bands.is_empty() { + return None; + } + + let seed_candidate = ShapeMatchCandidate { + yaw_radians: seed.yaw_radians, + translation_x: seed.translation.x, + translation_z: seed.translation.z, + ..ShapeMatchCandidate::default() + }; + + let mut pooled = Vec::::new(); + for band in &bands { + let finalists = search_mask_overlap_translation_only( + &band.local, + &band.remote, + seed_candidate, + 0.36, + 4, + ); + for candidate in finalists { + push_shape_candidate( + &mut pooled, + candidate, + 12, + band.local.cell_size_meters * 1.25, + 0.02, + ); + } + } + if pooled.is_empty() { + return None; + } + + let mut finalists = pooled + .into_iter() + .map(|candidate| score_consensus_candidate(candidate, &bands)) + .filter(|candidate| candidate.score.is_finite() && candidate.score > 0.0) + .collect::>(); + finalists.sort_by(|left, right| { + right.score.total_cmp(&left.score).then_with(|| { + left.mean_distance_meters + .total_cmp(&right.mean_distance_meters) + }) + }); + finalists.truncate(SHAPE_FINAL_TOP_K); + if finalists.is_empty() { + return None; + } + + let manual_candidate = manual_pose.map(|manual_pose| { + score_consensus_candidate( + ShapeMatchCandidate { + yaw_radians: manual_pose.rotation_radians, + translation_x: manual_pose.shift_x_meters, + translation_z: manual_pose.shift_y_meters, + ..ShapeMatchCandidate::default() + }, + &bands, + ) + }); + + Some(ShapeMatchReport { + label: "shape_multiband_seeded".to_string(), + finalists, + manual_candidate, + }) +} + +fn run_runtime_seeded_consensus_refine_analysis( + local: &XrDepthAlignDescriptor, + remote: &XrDepthAlignDescriptor, + manual_pose: Option, + seed: XrDepthAlignSolution, +) -> Option { + let bands = build_seeded_consensus_band_grids(local, remote); + if bands.is_empty() { + return None; + } + + let seed_candidate = ShapeMatchCandidate { + yaw_radians: seed.yaw_radians, + translation_x: seed.translation.x, + translation_z: seed.translation.z, + ..ShapeMatchCandidate::default() + }; + let rescore_local_candidate = |candidate: ShapeMatchCandidate| { + let mut rescored = score_consensus_candidate(candidate, &bands); + // The runtime-seeded local experiment should not get zeroed out solely by the + // harsh global Support-IoU term. Keep some feature score alive locally. + rescored.score = rescored.score.max(rescored.feature_score * 0.35); + apply_seed_prior(rescored, seed_candidate) + }; + + let mut pooled = Vec::::new(); + for band in &bands { + let finalists = search_mask_overlap_translation_only( + &band.local, + &band.remote, + seed_candidate, + 0.28, + 6, + ); + for candidate in finalists { + push_shape_candidate( + &mut pooled, + candidate, + 16, + band.local.cell_size_meters * 1.25, + 0.02, + ); + } + } + if pooled.is_empty() { + return None; + } + + let mut finalists = pooled + .into_iter() + .map(&rescore_local_candidate) + .filter(|candidate| candidate.score.is_finite() && candidate.score > 0.0) + .collect::>(); + finalists.sort_by(|left, right| { + right.score.total_cmp(&left.score).then_with(|| { + left.mean_distance_meters + .total_cmp(&right.mean_distance_meters) + }) + }); + finalists.truncate(6); + if finalists.is_empty() { + return None; + } + + let mut refined = search_consensus_refine( + &bands, + &finalists, + 0.18, + 10.0_f32.to_radians(), + 1.0_f32.to_radians(), + SHAPE_FINAL_TOP_K, + ) + .into_iter() + .map(&rescore_local_candidate) + .collect::>(); + if refined.is_empty() { + refined = finalists; + } else { + refined.sort_by(|left, right| { + right.score.total_cmp(&left.score).then_with(|| { + left.mean_distance_meters + .total_cmp(&right.mean_distance_meters) + }) + }); + refined.truncate(SHAPE_FINAL_TOP_K); + } + + let manual_candidate = manual_pose.map(|manual_pose| { + rescore_local_candidate(ShapeMatchCandidate { + yaw_radians: manual_pose.rotation_radians, + translation_x: manual_pose.shift_x_meters, + translation_z: manual_pose.shift_y_meters, + ..ShapeMatchCandidate::default() + }) + }); + + Some(ShapeMatchReport { + label: "runtime_seeded_consensus".to_string(), + finalists: refined, + manual_candidate, + }) +} + +fn run_seeded_multiband_vote_analysis( + local: &XrDepthAlignDescriptor, + remote: &XrDepthAlignDescriptor, + manual_pose: Option, + seed: XrDepthAlignSolution, +) -> Option { + let bands = build_seeded_consensus_band_grids(local, remote); + if bands.is_empty() { + return None; + } + + let seed_candidate = ShapeMatchCandidate { + yaw_radians: seed.yaw_radians, + translation_x: seed.translation.x, + translation_z: seed.translation.z, + ..ShapeMatchCandidate::default() + }; + + let mut weighted_sum_x = 0.0; + let mut weighted_sum_z = 0.0; + let mut weight_sum = 0.0; + let mut candidates = Vec::new(); + for band in &bands { + let finalists = search_mask_overlap_translation_only( + &band.local, + &band.remote, + seed_candidate, + 0.36, + 3, + ); + if let Some(best) = finalists.first().copied() { + let weight = best.score.max(0.001); + weighted_sum_x += best.translation_x * weight; + weighted_sum_z += best.translation_z * weight; + weight_sum += weight; + candidates.push(best); + } + } + if candidates.is_empty() || weight_sum <= 1.0e-6 { + return None; + } + + candidates.push(ShapeMatchCandidate { + yaw_radians: seed.yaw_radians, + translation_x: weighted_sum_x / weight_sum, + translation_z: weighted_sum_z / weight_sum, + ..ShapeMatchCandidate::default() + }); + candidates.push(seed_candidate); + + let mut finalists = candidates + .into_iter() + .map(|candidate| score_consensus_candidate(candidate, &bands)) + .collect::>(); + finalists.sort_by(|left, right| { + right.score.total_cmp(&left.score).then_with(|| { + left.mean_distance_meters + .total_cmp(&right.mean_distance_meters) + }) + }); + finalists.truncate(SHAPE_FINAL_TOP_K); + let manual_candidate = manual_pose.map(|manual_pose| { + score_consensus_candidate( + ShapeMatchCandidate { + yaw_radians: manual_pose.rotation_radians, + translation_x: manual_pose.shift_x_meters, + translation_z: manual_pose.shift_y_meters, + ..ShapeMatchCandidate::default() + }, + &bands, + ) + }); + Some(ShapeMatchReport { + label: "shape_multiband_vote_seeded".to_string(), + finalists, + manual_candidate, + }) +} + +fn run_correlative_consensus_analysis( + local: &XrDepthAlignDescriptor, + remote: &XrDepthAlignDescriptor, + manual_pose: Option, +) -> Option { + let coarse_bands = + build_consensus_band_grids_at_cell(local, remote, SHAPE_CORR_COARSE_CELL_METERS); + if coarse_bands.is_empty() { + return None; + } + let coarse = search_consensus_global( + &coarse_bands, + SHAPE_CORR_COARSE_YAW_STEP_RADIANS, + SHAPE_CORR_COARSE_TOP_K, + ); + if coarse.is_empty() { + return None; + } + + let medium_bands = + build_consensus_band_grids_at_cell(local, remote, SHAPE_CORR_MEDIUM_CELL_METERS); + if medium_bands.is_empty() { + return None; + } + let medium = search_consensus_refine( + &medium_bands, + &coarse, + SHAPE_CORR_COARSE_CELL_METERS * 1.5, + SHAPE_CORR_COARSE_YAW_STEP_RADIANS * 1.5, + SHAPE_CORR_MEDIUM_YAW_STEP_RADIANS, + SHAPE_CORR_MEDIUM_TOP_K, + ); + if medium.is_empty() { + return None; + } + + let fine_bands = build_consensus_band_grids_at_cell(local, remote, SHAPE_CORR_FINE_CELL_METERS); + if fine_bands.is_empty() { + return None; + } + let fine = search_consensus_refine( + &fine_bands, + &medium, + SHAPE_CORR_MEDIUM_CELL_METERS * 1.25, + SHAPE_CORR_MEDIUM_YAW_STEP_RADIANS * 1.5, + SHAPE_CORR_FINE_YAW_STEP_RADIANS, + SHAPE_CORR_FINE_TOP_K, + ); + if fine.is_empty() { + return None; + } + + let final_bands = + build_consensus_band_grids_at_cell(local, remote, SHAPE_CORR_FINAL_CELL_METERS); + if final_bands.is_empty() { + return None; + } + let mut finalists = search_consensus_refine( + &final_bands, + &fine, + SHAPE_CORR_FINE_CELL_METERS * 1.0, + SHAPE_CORR_FINE_YAW_STEP_RADIANS * 1.5, + SHAPE_CORR_FINAL_YAW_STEP_RADIANS, + SHAPE_CORR_FINAL_TOP_K, + ); + if finalists.is_empty() { + return None; + } + + let manual_candidate = manual_pose.map(|manual_pose| { + score_consensus_candidate( + ShapeMatchCandidate { + yaw_radians: manual_pose.rotation_radians, + translation_x: manual_pose.shift_x_meters, + translation_z: manual_pose.shift_y_meters, + ..ShapeMatchCandidate::default() + }, + &final_bands, + ) + }); + Some(ShapeMatchReport { + label: "shape_correlative_consensus".to_string(), + finalists, + manual_candidate, + }) +} + +fn generate_component_pair_seeds( + local: &ShapeMatchGrid, + remote: &ShapeMatchGrid, +) -> Vec { + let local_components = extract_shape_components(local, 6); + let remote_components = extract_shape_components(remote, 6); + let mut seeds = Vec::new(); + + for local_component in &local_components { + for remote_component in &remote_components { + let area_ratio = (local_component.area_m2 / remote_component.area_m2.max(1.0e-4)) + .max(remote_component.area_m2 / local_component.area_m2.max(1.0e-4)); + if area_ratio > 2.8 { + continue; + } + push_shape_candidate( + &mut seeds, + ShapeMatchCandidate { + yaw_radians: 0.0, + translation_x: local_component.centroid_x - remote_component.centroid_x, + translation_z: local_component.centroid_z - remote_component.centroid_z, + score: local_component.score.min(remote_component.score), + ..ShapeMatchCandidate::default() + }, + 24, + 0.12, + 8.0_f32.to_radians(), + ); + } + } + + for local_i in 0..local_components.len() { + for local_j in local_i + 1..local_components.len() { + let local_dx = + local_components[local_j].centroid_x - local_components[local_i].centroid_x; + let local_dz = + local_components[local_j].centroid_z - local_components[local_i].centroid_z; + let local_dist = (local_dx * local_dx + local_dz * local_dz).sqrt(); + if local_dist < 0.18 { + continue; + } + let local_angle = local_dz.atan2(local_dx); + for remote_i in 0..remote_components.len() { + for remote_j in remote_i + 1..remote_components.len() { + let remote_dx = remote_components[remote_j].centroid_x + - remote_components[remote_i].centroid_x; + let remote_dz = remote_components[remote_j].centroid_z + - remote_components[remote_i].centroid_z; + let remote_dist = (remote_dx * remote_dx + remote_dz * remote_dz).sqrt(); + if remote_dist < 0.18 { + continue; + } + let distance_ratio = (local_dist / remote_dist.max(1.0e-4)) + .max(remote_dist / local_dist.max(1.0e-4)); + if distance_ratio > 1.8 { + continue; + } + let area_ratio_a = (local_components[local_i].area_m2 + / remote_components[remote_i].area_m2.max(1.0e-4)) + .max( + remote_components[remote_i].area_m2 + / local_components[local_i].area_m2.max(1.0e-4), + ); + let area_ratio_b = (local_components[local_j].area_m2 + / remote_components[remote_j].area_m2.max(1.0e-4)) + .max( + remote_components[remote_j].area_m2 + / local_components[local_j].area_m2.max(1.0e-4), + ); + if area_ratio_a > 2.8 || area_ratio_b > 2.8 { + continue; + } + let remote_angle = remote_dz.atan2(remote_dx); + let yaw_radians = wrap_angle(local_angle - remote_angle); + let (rotated_x, rotated_z) = rotate_xz( + yaw_radians, + remote_components[remote_i].centroid_x, + remote_components[remote_i].centroid_z, + ); + let translation_x = local_components[local_i].centroid_x - rotated_x; + let translation_z = local_components[local_i].centroid_z - rotated_z; + let pair_score = local_components[local_i] + .score + .min(remote_components[remote_i].score) + + local_components[local_j] + .score + .min(remote_components[remote_j].score); + push_shape_candidate( + &mut seeds, + ShapeMatchCandidate { + yaw_radians, + translation_x, + translation_z, + score: pair_score, + ..ShapeMatchCandidate::default() + }, + 36, + 0.12, + 6.0_f32.to_radians(), + ); + } + } + } + } + + seeds +} + +fn component_signature_match( + local: ShapeComponent, + remote: ShapeComponent, +) -> Option<(usize, f32)> { + let area_ratio = (local.area_m2 / remote.area_m2.max(1.0e-4)) + .max(remote.area_m2 / local.area_m2.max(1.0e-4)); + if area_ratio > 3.2 { + return None; + } + let compactness_delta = (local.compactness - remote.compactness).abs(); + if compactness_delta > 0.65 { + return None; + } + let mut best_shift = 0usize; + let mut best_error = f32::INFINITY; + for shift in 0..SHAPE_COMPONENT_SIGNATURE_BINS { + let mut error = 0.0f32; + for i in 0..SHAPE_COMPONENT_SIGNATURE_BINS { + let j = (i + shift) % SHAPE_COMPONENT_SIGNATURE_BINS; + error += (local.radial_signature[i] - remote.radial_signature[j]).abs(); + } + if error < best_error { + best_error = error; + best_shift = shift; + } + } + let signature_error = best_error / SHAPE_COMPONENT_SIGNATURE_BINS as f32; + if signature_error > 0.42 { + return None; + } + let quality = 1.0 + - ((area_ratio - 1.0) * 0.35 + compactness_delta * 0.45 + signature_error * 1.35) + .clamp(0.0, 0.98); + Some((best_shift, quality.max(0.001))) +} + +fn component_context_signature_error( + local: &ShapeComponent, + remote: &ShapeComponent, + shift: usize, +) -> f32 { + let channel_weights = [1.0f32, 0.8f32, 0.6f32]; + let mut weighted_error = 0.0f32; + let mut total_weight = 0.0f32; + for (channel, channel_weight) in channel_weights.into_iter().enumerate() { + for radial in 0..SHAPE_COMPONENT_CONTEXT_RADIAL_BINS { + for angular in 0..SHAPE_COMPONENT_CONTEXT_ANGULAR_BINS { + let local_index = component_context_index(channel, radial, angular); + let remote_index = component_context_index( + channel, + radial, + (angular + shift) % SHAPE_COMPONENT_CONTEXT_ANGULAR_BINS, + ); + weighted_error += channel_weight + * (local.context_signature[local_index] + - remote.context_signature[remote_index]) + .abs(); + total_weight += channel_weight; + } + } + } + weighted_error / total_weight.max(1.0e-6) +} + +fn component_context_match(local: ShapeComponent, remote: ShapeComponent) -> Option<(usize, f32)> { + let area_ratio = (local.area_m2 / remote.area_m2.max(1.0e-4)) + .max(remote.area_m2 / local.area_m2.max(1.0e-4)); + if area_ratio > 3.4 { + return None; + } + let compactness_delta = (local.compactness - remote.compactness).abs(); + if compactness_delta > 0.75 { + return None; + } + let mut best_shift = 0usize; + let mut best_blob_error = f32::INFINITY; + let mut best_context_error = f32::INFINITY; + let mut best_total_error = f32::INFINITY; + for shift in 0..SHAPE_COMPONENT_CONTEXT_ANGULAR_BINS { + let mut blob_error = 0.0f32; + for i in 0..SHAPE_COMPONENT_SIGNATURE_BINS { + let j = (i + shift) % SHAPE_COMPONENT_SIGNATURE_BINS; + blob_error += (local.radial_signature[i] - remote.radial_signature[j]).abs(); + } + blob_error /= SHAPE_COMPONENT_SIGNATURE_BINS as f32; + let context_error = component_context_signature_error(&local, &remote, shift); + let total_error = blob_error * 0.45 + context_error * 0.85; + if total_error < best_total_error { + best_total_error = total_error; + best_blob_error = blob_error; + best_context_error = context_error; + best_shift = shift; + } + } + if best_blob_error > 0.55 || best_context_error > 0.16 { + return None; + } + let quality = 1.0 + - ((area_ratio - 1.0) * 0.22 + + compactness_delta * 0.25 + + best_blob_error * 0.60 + + best_context_error * 3.25) + .clamp(0.0, 0.985); + Some((best_shift, quality.max(0.001))) +} + +fn component_radial_signature_error( + local: &ShapeComponent, + remote: &ShapeComponent, + shift: usize, +) -> f32 { + let mut error = 0.0f32; + for index in 0..SHAPE_COMPONENT_SIGNATURE_BINS { + let remote_index = (index + shift) % SHAPE_COMPONENT_SIGNATURE_BINS; + error += (local.radial_signature[index] - remote.radial_signature[remote_index]).abs(); + } + error / SHAPE_COMPONENT_SIGNATURE_BINS as f32 +} + +fn component_pair_alignment_score( + local: &ShapeComponent, + remote: &ShapeComponent, + yaw_radians: f32, + translation_x: f32, + translation_z: f32, +) -> Option { + let (rotated_x, rotated_z) = rotate_xz(yaw_radians, remote.centroid_x, remote.centroid_z); + let planar_distance = ((local.centroid_x - (rotated_x + translation_x)).powi(2) + + (local.centroid_z - (rotated_z + translation_z)).powi(2)) + .sqrt(); + if planar_distance > FOCUSED_COMPONENT_MAX_DISTANCE_METERS { + return None; + } + let area_ratio = (local.area_m2 / remote.area_m2.max(1.0e-4)) + .max(remote.area_m2 / local.area_m2.max(1.0e-4)); + if area_ratio > 3.6 { + return None; + } + let compactness_delta = (local.compactness - remote.compactness).abs(); + if compactness_delta > 0.80 { + return None; + } + let base_sig_shift = signature_shift_for_angle(yaw_radians, SHAPE_COMPONENT_SIGNATURE_BINS); + let base_ctx_shift = + signature_shift_for_angle(yaw_radians, SHAPE_COMPONENT_CONTEXT_ANGULAR_BINS); + let mut best_signature_error = f32::INFINITY; + let mut best_context_error = f32::INFINITY; + for sig_delta in [-1isize, 0, 1] { + let signature_shift = (base_sig_shift as isize + sig_delta) + .rem_euclid(SHAPE_COMPONENT_SIGNATURE_BINS as isize) + as usize; + let signature_error = component_radial_signature_error(local, remote, signature_shift); + let ctx_delta = if sig_delta < 0 { + -1 + } else if sig_delta > 0 { + 1 + } else { + 0 + }; + let context_shift = (base_ctx_shift as isize + ctx_delta) + .rem_euclid(SHAPE_COMPONENT_CONTEXT_ANGULAR_BINS as isize) + as usize; + let context_error = component_context_signature_error(local, remote, context_shift); + if signature_error + context_error * 0.8 < best_signature_error + best_context_error * 0.8 { + best_signature_error = signature_error; + best_context_error = context_error; + } + } + if best_signature_error > 0.70 || best_context_error > 0.22 { + return None; + } + let spatial_score = + (-0.5 * (planar_distance / FOCUSED_COMPONENT_DISTANCE_SIGMA_METERS).powi(2)).exp(); + let shape_score = 1.0 + - ((area_ratio - 1.0) * 0.18 + + compactness_delta * 0.18 + + best_signature_error * 0.55 + + best_context_error * 2.70) + .clamp(0.0, 0.985); + let component_weight = local.score.min(remote.score).sqrt().max(0.05); + Some((spatial_score * shape_score.max(0.0) * component_weight).clamp(0.0, 1.0)) +} + +fn score_component_alignment_one_way( + local_components: &[ShapeComponent], + remote_components: &[ShapeComponent], + yaw_radians: f32, + translation_x: f32, + translation_z: f32, +) -> Option { + if local_components.is_empty() || remote_components.is_empty() { + return None; + } + #[derive(Clone, Copy)] + struct PairCandidate { + local_index: usize, + remote_index: usize, + score: f32, + weight: f32, + } + let mut pair_candidates = Vec::::new(); + let total_remote_weight = remote_components + .iter() + .map(|component| component.score.max(0.05)) + .sum::() + .max(1.0e-6); + for (remote_index, remote_component) in remote_components.iter().enumerate() { + for (local_index, local_component) in local_components.iter().enumerate() { + let Some(score) = component_pair_alignment_score( + local_component, + remote_component, + yaw_radians, + translation_x, + translation_z, + ) else { + continue; + }; + pair_candidates.push(PairCandidate { + local_index, + remote_index, + score, + weight: remote_component.score.max(0.05), + }); + } + } + if pair_candidates.is_empty() { + return Some(0.0); + } + pair_candidates.sort_by(|left, right| right.score.total_cmp(&left.score)); + let mut used_local = vec![false; local_components.len()]; + let mut used_remote = vec![false; remote_components.len()]; + let mut score_sum = 0.0f32; + let mut matched_weight = 0.0f32; + for candidate in pair_candidates { + if used_local[candidate.local_index] || used_remote[candidate.remote_index] { + continue; + } + used_local[candidate.local_index] = true; + used_remote[candidate.remote_index] = true; + score_sum += candidate.score * candidate.weight; + if candidate.score > 0.30 { + matched_weight += candidate.weight; + } + } + let coverage = (matched_weight / total_remote_weight).clamp(0.0, 1.0); + Some((score_sum / total_remote_weight) * coverage.sqrt()) +} + +fn score_component_alignment_symmetric( + local_components: &[ShapeComponent], + remote_components: &[ShapeComponent], + yaw_radians: f32, + translation_x: f32, + translation_z: f32, +) -> f32 { + let Some(forward) = score_component_alignment_one_way( + local_components, + remote_components, + yaw_radians, + translation_x, + translation_z, + ) else { + return 0.0; + }; + if forward <= 0.0 { + return 0.0; + } + let Some(inverse) = score_component_alignment_one_way( + remote_components, + local_components, + -yaw_radians, + -translation_x, + -translation_z, + ) else { + return forward; + }; + if inverse <= 0.0 { + return forward; + } + (forward * inverse).sqrt() +} + +fn generate_component_descriptor_seeds( + local: &ShapeMatchGrid, + remote: &ShapeMatchGrid, +) -> Vec { + let local_components = extract_shape_components(local, 6); + let remote_components = extract_shape_components(remote, 6); + let mut matches = Vec::<(usize, usize, usize, f32)>::new(); + for (local_index, &local_component) in local_components.iter().enumerate() { + for (remote_index, &remote_component) in remote_components.iter().enumerate() { + if let Some((shift, quality)) = + component_signature_match(local_component, remote_component) + { + matches.push((local_index, remote_index, shift, quality)); + } + } + } + matches.sort_by(|left, right| right.3.total_cmp(&left.3)); + matches.truncate(16); + + let mut seeds = Vec::new(); + for &(local_index, remote_index, shift, quality) in &matches { + let local_component = local_components[local_index]; + let remote_component = remote_components[remote_index]; + let yaw_radians = shift as f32 * (TAU / SHAPE_COMPONENT_SIGNATURE_BINS as f32); + let (rotated_x, rotated_z) = rotate_xz( + yaw_radians, + remote_component.centroid_x, + remote_component.centroid_z, + ); + push_shape_candidate( + &mut seeds, + ShapeMatchCandidate { + yaw_radians, + translation_x: local_component.centroid_x - rotated_x, + translation_z: local_component.centroid_z - rotated_z, + score: quality * local_component.score.min(remote_component.score), + ..ShapeMatchCandidate::default() + }, + 32, + 0.12, + 12.0_f32.to_radians(), + ); + } + + for left in 0..matches.len() { + for right in left + 1..matches.len() { + let (local_a_idx, remote_a_idx, _shift_a, qa) = matches[left]; + let (local_b_idx, remote_b_idx, _shift_b, qb) = matches[right]; + if local_a_idx == local_b_idx || remote_a_idx == remote_b_idx { + continue; + } + let local_a = local_components[local_a_idx]; + let local_b = local_components[local_b_idx]; + let remote_a = remote_components[remote_a_idx]; + let remote_b = remote_components[remote_b_idx]; + let local_dx = local_b.centroid_x - local_a.centroid_x; + let local_dz = local_b.centroid_z - local_a.centroid_z; + let remote_dx = remote_b.centroid_x - remote_a.centroid_x; + let remote_dz = remote_b.centroid_z - remote_a.centroid_z; + let local_len = (local_dx * local_dx + local_dz * local_dz).sqrt(); + let remote_len = (remote_dx * remote_dx + remote_dz * remote_dz).sqrt(); + if local_len < 0.18 || remote_len < 0.18 { + continue; + } + let len_ratio = + (local_len / remote_len.max(1.0e-4)).max(remote_len / local_len.max(1.0e-4)); + if len_ratio > 1.9 { + continue; + } + let yaw_radians = wrap_angle(local_dz.atan2(local_dx) - remote_dz.atan2(remote_dx)); + let (rotated_ax, rotated_az) = + rotate_xz(yaw_radians, remote_a.centroid_x, remote_a.centroid_z); + let translation_x = local_a.centroid_x - rotated_ax; + let translation_z = local_a.centroid_z - rotated_az; + let (rotated_bx, rotated_bz) = + rotate_xz(yaw_radians, remote_b.centroid_x, remote_b.centroid_z); + let closure_error = ((local_b.centroid_x - (rotated_bx + translation_x)).powi(2) + + (local_b.centroid_z - (rotated_bz + translation_z)).powi(2)) + .sqrt(); + if closure_error > 0.42 { + continue; + } + push_shape_candidate( + &mut seeds, + ShapeMatchCandidate { + yaw_radians, + translation_x, + translation_z, + score: (qa + qb) * 0.5 - closure_error * 0.8, + ..ShapeMatchCandidate::default() + }, + 40, + 0.12, + 8.0_f32.to_radians(), + ); + } + } + + seeds +} + +fn generate_component_context_seeds( + local: &ShapeMatchGrid, + remote: &ShapeMatchGrid, +) -> Vec { + let local_components = extract_shape_components(local, 6); + let remote_components = extract_shape_components(remote, 6); + let mut matches = Vec::<(usize, usize, usize, f32)>::new(); + for (local_index, &local_component) in local_components.iter().enumerate() { + for (remote_index, &remote_component) in remote_components.iter().enumerate() { + if let Some((shift, quality)) = + component_context_match(local_component, remote_component) + { + matches.push((local_index, remote_index, shift, quality)); + } + } + } + matches.sort_by(|left, right| right.3.total_cmp(&left.3)); + matches.truncate(20); + + let mut seeds = Vec::new(); + for &(local_index, remote_index, shift, quality) in &matches { + let local_component = local_components[local_index]; + let remote_component = remote_components[remote_index]; + let yaw_radians = shift as f32 * (TAU / SHAPE_COMPONENT_CONTEXT_ANGULAR_BINS as f32); + let (rotated_x, rotated_z) = rotate_xz( + yaw_radians, + remote_component.centroid_x, + remote_component.centroid_z, + ); + push_shape_candidate( + &mut seeds, + ShapeMatchCandidate { + yaw_radians, + translation_x: local_component.centroid_x - rotated_x, + translation_z: local_component.centroid_z - rotated_z, + score: quality * local_component.score.min(remote_component.score), + ..ShapeMatchCandidate::default() + }, + 36, + 0.12, + 10.0_f32.to_radians(), + ); + } + + for left in 0..matches.len() { + for right in left + 1..matches.len() { + let (local_a_idx, remote_a_idx, _shift_a, qa) = matches[left]; + let (local_b_idx, remote_b_idx, _shift_b, qb) = matches[right]; + if local_a_idx == local_b_idx || remote_a_idx == remote_b_idx { + continue; + } + let local_a = local_components[local_a_idx]; + let local_b = local_components[local_b_idx]; + let remote_a = remote_components[remote_a_idx]; + let remote_b = remote_components[remote_b_idx]; + let local_dx = local_b.centroid_x - local_a.centroid_x; + let local_dz = local_b.centroid_z - local_a.centroid_z; + let remote_dx = remote_b.centroid_x - remote_a.centroid_x; + let remote_dz = remote_b.centroid_z - remote_a.centroid_z; + let local_len = (local_dx * local_dx + local_dz * local_dz).sqrt(); + let remote_len = (remote_dx * remote_dx + remote_dz * remote_dz).sqrt(); + if local_len < 0.18 || remote_len < 0.18 { + continue; + } + let len_ratio = + (local_len / remote_len.max(1.0e-4)).max(remote_len / local_len.max(1.0e-4)); + if len_ratio > 1.9 { + continue; + } + let yaw_radians = wrap_angle(local_dz.atan2(local_dx) - remote_dz.atan2(remote_dx)); + let (rotated_ax, rotated_az) = + rotate_xz(yaw_radians, remote_a.centroid_x, remote_a.centroid_z); + let translation_x = local_a.centroid_x - rotated_ax; + let translation_z = local_a.centroid_z - rotated_az; + let (rotated_bx, rotated_bz) = + rotate_xz(yaw_radians, remote_b.centroid_x, remote_b.centroid_z); + let closure_error = ((local_b.centroid_x - (rotated_bx + translation_x)).powi(2) + + (local_b.centroid_z - (rotated_bz + translation_z)).powi(2)) + .sqrt(); + if closure_error > 0.48 { + continue; + } + push_shape_candidate( + &mut seeds, + ShapeMatchCandidate { + yaw_radians, + translation_x, + translation_z, + score: (qa + qb) * 0.5 - closure_error * 0.7, + ..ShapeMatchCandidate::default() + }, + 42, + 0.12, + 8.0_f32.to_radians(), + ); + } + } + + seeds +} + +fn generate_component_graph_seeds( + local: &ShapeMatchGrid, + remote: &ShapeMatchGrid, +) -> Vec { + // Increase to 12 components to get more potential pairs + let local_components = extract_shape_components(local, 12); + let remote_components = extract_shape_components(remote, 12); + let mut seeds = Vec::new(); + println!( + "generate_component_graph_seeds: local_components {}, remote_components {}", + local_components.len(), + remote_components.len() + ); + + for local_a in 0..local_components.len() { + for local_b in 0..local_components.len() { + if local_b == local_a { + continue; + } + let local_ab_x = + local_components[local_b].centroid_x - local_components[local_a].centroid_x; + let local_ab_z = + local_components[local_b].centroid_z - local_components[local_a].centroid_z; + let local_ab = (local_ab_x * local_ab_x + local_ab_z * local_ab_z).sqrt(); + if local_ab < 0.20 { + continue; // Points too close to form a stable rotation vector + } + let local_angle = local_ab_z.atan2(local_ab_x); + + for remote_a in 0..remote_components.len() { + for remote_b in 0..remote_components.len() { + if remote_b == remote_a { + continue; + } + let area_ratio_a = (local_components[local_a].area_m2 + / remote_components[remote_a].area_m2.max(1.0e-4)) + .max( + remote_components[remote_a].area_m2 + / local_components[local_a].area_m2.max(1.0e-4), + ); + let area_ratio_b = (local_components[local_b].area_m2 + / remote_components[remote_b].area_m2.max(1.0e-4)) + .max( + remote_components[remote_b].area_m2 + / local_components[local_b].area_m2.max(1.0e-4), + ); + + // Slightly looser area tolerance since partial occlusion can shrink blobs + if area_ratio_a > 3.5 || area_ratio_b > 3.5 { + continue; + } + + let remote_ab_x = remote_components[remote_b].centroid_x + - remote_components[remote_a].centroid_x; + let remote_ab_z = remote_components[remote_b].centroid_z + - remote_components[remote_a].centroid_z; + let remote_ab = (remote_ab_x * remote_ab_x + remote_ab_z * remote_ab_z).sqrt(); + if remote_ab < 0.20 { + continue; + } + + let length_ratio = (local_ab / remote_ab).max(remote_ab / local_ab); + // Segment lengths between the two centroids must match reasonably well + // Since it's just the distance between two objects, it should be quite stable + if length_ratio > 1.35 { + continue; + } + + let remote_angle = remote_ab_z.atan2(remote_ab_x); + let yaw_radians = wrap_angle(local_angle - remote_angle); + + let (rotated_ax, rotated_az) = rotate_xz( + yaw_radians, + remote_components[remote_a].centroid_x, + remote_components[remote_a].centroid_z, + ); + let translation_x = local_components[local_a].centroid_x - rotated_ax; + let translation_z = local_components[local_a].centroid_z - rotated_az; + + let seed_score = local_components[local_a] + .score + .min(remote_components[remote_a].score) + + local_components[local_b] + .score + .min(remote_components[remote_b].score) + + 1.0 / length_ratio; // Small bonus for perfectly matching distances + + push_shape_candidate( + &mut seeds, + ShapeMatchCandidate { + yaw_radians, + translation_x, + translation_z, + score: seed_score.max(0.001), + ..ShapeMatchCandidate::default() + }, + 128, + 0.15, + 6.0_f32.to_radians(), + ); + } + } + } + } + seeds +} + +fn search_consensus_seeded( + bands: &[BandGridPair], + seeds: &[ShapeMatchCandidate], + translation_window_meters: f32, + yaw_window_radians: f32, + yaw_step_radians: f32, + top_k: usize, +) -> Vec { + let Some(first_band) = bands.first() else { + return Vec::new(); + }; + let mut finalists = Vec::new(); + let step = first_band.local.cell_size_meters.max(1.0e-3); + let translation_steps = (translation_window_meters / step).ceil().max(1.0) as isize; + let yaw_steps = (yaw_window_radians / yaw_step_radians).ceil().max(1.0) as isize; + for seed in seeds { + for yaw_step in -yaw_steps..=yaw_steps { + let yaw_radians = wrap_angle(seed.yaw_radians + yaw_step as f32 * yaw_step_radians); + for x_step in -translation_steps..=translation_steps { + let translation_x = seed.translation_x + x_step as f32 * step; + for z_step in -translation_steps..=translation_steps { + let translation_z = seed.translation_z + z_step as f32 * step; + let candidate = score_consensus_candidate( + ShapeMatchCandidate { + yaw_radians, + translation_x, + translation_z, + ..ShapeMatchCandidate::default() + }, + bands, + ); + push_shape_candidate( + &mut finalists, + candidate, + top_k, + step * 1.25, + yaw_step_radians * 1.25, + ); + } + } + } + } + finalists +} + +fn run_component_pair_seeded_analysis( + local: &XrDepthAlignDescriptor, + remote: &XrDepthAlignDescriptor, + manual_pose: Option, +) -> Option { + let bands = build_seeded_consensus_band_grids(local, remote); + if bands.is_empty() { + return None; + } + let furniture_band = ShapeBandSpec { + label: "component_seed_band".to_string(), + min_height_meters: 0.20, + max_height_meters: Some(1.20), + floor_offset_meters: 0.0, + drop_border_components: true, + }; + let (Some(local_map), Some(remote_map)) = + (local.height_map.as_ref(), remote.height_map.as_ref()) + else { + return None; + }; + let local_grid = build_shape_match_grid(local_map, SHAPE_GLOBAL_CELL_METERS, &furniture_band)?; + let remote_grid = + build_shape_match_grid(remote_map, SHAPE_GLOBAL_CELL_METERS, &furniture_band)?; + let seeds = generate_component_pair_seeds(&local_grid, &remote_grid); + if seeds.is_empty() { + return None; + } + let finalists = search_consensus_seeded( + &bands, + &seeds, + 0.36, + 18.0_f32.to_radians(), + 1.5_f32.to_radians(), + SHAPE_FINAL_TOP_K, + ); + if finalists.is_empty() { + return None; + } + let manual_candidate = manual_pose.map(|manual_pose| { + score_consensus_candidate( + ShapeMatchCandidate { + yaw_radians: manual_pose.rotation_radians, + translation_x: manual_pose.shift_x_meters, + translation_z: manual_pose.shift_y_meters, + ..ShapeMatchCandidate::default() + }, + &bands, + ) + }); + Some(ShapeMatchReport { + label: "shape_component_pair_seeded".to_string(), + finalists, + manual_candidate, + }) +} + +fn run_component_descriptor_seeded_analysis( + local: &XrDepthAlignDescriptor, + remote: &XrDepthAlignDescriptor, + manual_pose: Option, +) -> Option { + let bands = build_seeded_consensus_band_grids(local, remote); + if bands.is_empty() { + return None; + } + let furniture_band = ShapeBandSpec { + label: "component_descriptor_band".to_string(), + min_height_meters: 0.20, + max_height_meters: Some(1.20), + floor_offset_meters: 0.0, + drop_border_components: true, + }; + let (Some(local_map), Some(remote_map)) = + (local.height_map.as_ref(), remote.height_map.as_ref()) + else { + return None; + }; + let local_grid = build_shape_match_grid(local_map, SHAPE_GLOBAL_CELL_METERS, &furniture_band)?; + let remote_grid = + build_shape_match_grid(remote_map, SHAPE_GLOBAL_CELL_METERS, &furniture_band)?; + let seeds = generate_component_descriptor_seeds(&local_grid, &remote_grid); + if seeds.is_empty() { + return None; + } + let finalists = search_consensus_seeded( + &bands, + &seeds, + 0.30, + 16.0_f32.to_radians(), + 1.0_f32.to_radians(), + SHAPE_FINAL_TOP_K, + ); + if finalists.is_empty() { + return None; + } + let manual_candidate = manual_pose.map(|manual_pose| { + score_consensus_candidate( + ShapeMatchCandidate { + yaw_radians: manual_pose.rotation_radians, + translation_x: manual_pose.shift_x_meters, + translation_z: manual_pose.shift_y_meters, + ..ShapeMatchCandidate::default() + }, + &bands, + ) + }); + Some(ShapeMatchReport { + label: "shape_component_descriptor_seeded".to_string(), + finalists, + manual_candidate, + }) +} + +fn run_component_context_seeded_analysis( + local: &XrDepthAlignDescriptor, + remote: &XrDepthAlignDescriptor, + manual_pose: Option, +) -> Option { + let bands = build_seeded_consensus_band_grids(local, remote); + if bands.is_empty() { + return None; + } + let furniture_band = ShapeBandSpec { + label: "component_context_band".to_string(), + min_height_meters: 0.20, + max_height_meters: Some(1.20), + floor_offset_meters: 0.0, + drop_border_components: true, + }; + let (Some(local_map), Some(remote_map)) = + (local.height_map.as_ref(), remote.height_map.as_ref()) + else { + return None; + }; + let local_grid = build_shape_match_grid(local_map, SHAPE_GLOBAL_CELL_METERS, &furniture_band)?; + let remote_grid = + build_shape_match_grid(remote_map, SHAPE_GLOBAL_CELL_METERS, &furniture_band)?; + let seeds = generate_component_context_seeds(&local_grid, &remote_grid); + if seeds.is_empty() { + return None; + } + let finalists = search_consensus_seeded( + &bands, + &seeds, + 0.30, + 16.0_f32.to_radians(), + 1.0_f32.to_radians(), + SHAPE_FINAL_TOP_K, + ); + if finalists.is_empty() { + return None; + } + let manual_candidate = manual_pose.map(|manual_pose| { + score_consensus_candidate( + ShapeMatchCandidate { + yaw_radians: manual_pose.rotation_radians, + translation_x: manual_pose.shift_x_meters, + translation_z: manual_pose.shift_y_meters, + ..ShapeMatchCandidate::default() + }, + &bands, + ) + }); + Some(ShapeMatchReport { + label: "shape_component_context_seeded".to_string(), + finalists, + manual_candidate, + }) +} + +fn run_component_graph_seeded_analysis( + local: &XrDepthAlignDescriptor, + remote: &XrDepthAlignDescriptor, + manual_pose: Option, +) -> Option { + let bands = build_seeded_consensus_band_grids(local, remote); + if bands.is_empty() { + return None; + } + let furniture_band = ShapeBandSpec { + label: "component_graph_band".to_string(), + min_height_meters: 0.20, + max_height_meters: Some(1.20), + floor_offset_meters: 0.0, + drop_border_components: false, + }; + let (Some(local_map), Some(remote_map)) = + (local.height_map.as_ref(), remote.height_map.as_ref()) + else { + return None; + }; + let local_grid = build_shape_match_grid(local_map, SHAPE_GLOBAL_CELL_METERS, &furniture_band)?; + let remote_grid = + build_shape_match_grid(remote_map, SHAPE_GLOBAL_CELL_METERS, &furniture_band)?; + let seeds = generate_component_graph_seeds(&local_grid, &remote_grid); + if seeds.is_empty() { + return None; + } + let finalists = search_consensus_seeded( + &bands, + &seeds, + 0.28, + 12.0_f32.to_radians(), + 1.0_f32.to_radians(), + SHAPE_FINAL_TOP_K, + ); + if finalists.is_empty() { + return None; + } + let manual_candidate = manual_pose.map(|manual_pose| { + score_consensus_candidate( + ShapeMatchCandidate { + yaw_radians: manual_pose.rotation_radians, + translation_x: manual_pose.shift_x_meters, + translation_z: manual_pose.shift_y_meters, + ..ShapeMatchCandidate::default() + }, + &bands, + ) + }); + Some(ShapeMatchReport { + label: "shape_component_graph_seeded".to_string(), + finalists, + manual_candidate, + }) +} + +fn print_shape_band_scan_results( + local: &XrDepthAlignDescriptor, + remote: &XrDepthAlignDescriptor, + results: &[ShapeBandScanResult], +) { + if results.is_empty() { + println!("shape_scan: unavailable"); + return; + } + + let mut global = results.iter().collect::>(); + global.sort_by(|left, right| { + right.report.finalists[0] + .score + .total_cmp(&left.report.finalists[0].score) + .then_with(|| { + left.report.finalists[0] + .mean_distance_meters + .total_cmp(&right.report.finalists[0].mean_distance_meters) + }) + }); + for (index, result) in global.iter().take(3).enumerate() { + let best = result.report.finalists[0]; + let (manual_planar, manual_yaw_deg) = result + .nearest_manual_finalist + .map(|nearest| { + ( + nearest.planar_delta_meters, + nearest.yaw_delta_radians.to_degrees(), + ) + }) + .unwrap_or((f32::NAN, f32::NAN)); + println!( + "shape_scan_best_{}: band {} | yaw {:.3} rad ({:.1} deg) | translation ({:.3}, {:.3}, {:.3}) | score {:.3} | close {:.3} | dist {:.3} m | manual_dxz {:.3} | manual_dyaw {:.1} deg", + index + 1, + result.report.label, + best.yaw_radians, + best.yaw_radians.to_degrees(), + best.translation_x, + local.floor_y - remote.floor_y, + best.translation_z, + best.score, + best.close_ratio, + best.mean_distance_meters, + manual_planar, + manual_yaw_deg, + ); + } + + let mut nearest = results + .iter() + .filter_map(|result| { + result + .nearest_manual_finalist + .map(|nearest| (result, nearest)) + }) + .collect::>(); + nearest.sort_by( + |(left_result, left_nearest), (right_result, right_nearest)| { + left_nearest + .combined_distance + .total_cmp(&right_nearest.combined_distance) + .then_with(|| { + right_nearest + .candidate + .score + .total_cmp(&left_nearest.candidate.score) + }) + .then_with(|| { + right_result.report.finalists[0] + .score + .total_cmp(&left_result.report.finalists[0].score) + }) + }, + ); + for (index, (result, nearest)) in nearest.iter().take(3).enumerate() { + println!( + "shape_scan_near_{}: band {} | yaw {:.3} rad ({:.1} deg) | translation ({:.3}, {:.3}, {:.3}) | score {:.3} | manual_dxz {:.3} | manual_dyaw {:.1} deg | global_score {:.3}", + index + 1, + result.report.label, + nearest.candidate.yaw_radians, + nearest.candidate.yaw_radians.to_degrees(), + nearest.candidate.translation_x, + local.floor_y - remote.floor_y, + nearest.candidate.translation_z, + nearest.candidate.score, + nearest.planar_delta_meters, + nearest.yaw_delta_radians.to_degrees(), + result.report.finalists[0].score, + ); + } +} + +// --- Heightmap correlation matching --- +// Uses raw float heights (not binary masks) for SE(2) alignment. +// The key insight: binary occupancy is mirror-symmetric for rectangular rooms, +// but actual height values (table=0.75m vs bookshelf=1.8m) break that symmetry. + +#[derive(Clone, Debug)] +struct HeightCorrelationGrid { + origin_x: f32, + origin_z: f32, + cell_size_meters: f32, + size_x: usize, + size_z: usize, + /// Floor-relative height per cell, NaN if unobserved + heights: Vec, + /// Whether this cell was observed at all + observed: Vec, +} + +fn build_height_correlation_grid( + height_map: &XrDepthAlignHeightMap, + cell_size_meters: f32, +) -> Option { + if height_map.is_empty() { + return None; + } + let size_x = ((height_map.extent_x_meters() / cell_size_meters).ceil() as usize).max(1); + let size_z = ((height_map.extent_z_meters() / cell_size_meters).ceil() as usize).max(1); + let len = size_x * size_z; + let mut heights = vec![f32::NAN; len]; + let mut weight_sum = vec![0.0f32; len]; + let mut height_sum = vec![0.0f32; len]; + + let cutout_center = height_map.player_cutout_center; + let cutout_radius = (height_map.player_cutout_radius_meters + height_map.cell_size_meters) + .max(height_map.cell_size_meters * 2.0); + let floor_y = height_map.floor_y_meters; + + for src_z in 0..height_map.size_z_usize() { + for src_x in 0..height_map.size_x_usize() { + let index = height_map.cell_index(src_x, src_z); + let height = *height_map.heights_meters.get(index)?; + if !height.is_finite() { + continue; + } + let world_x = height_map.origin_x + (src_x as f32 + 0.5) * height_map.cell_size_meters; + let world_z = height_map.origin_z + (src_z as f32 + 0.5) * height_map.cell_size_meters; + if cutout_center.is_some_and(|center| { + let dx = world_x - center.x; + let dz = world_z - center.y; + (dx * dx + dz * dz).sqrt() <= cutout_radius + }) { + continue; + } + let relative_height = height - floor_y; + // Only include cells above the floor with some signal + if relative_height < 0.05 { + continue; + } + let cx = ((world_x - height_map.origin_x) / cell_size_meters) + .floor() + .clamp(0.0, size_x.saturating_sub(1) as f32) as usize; + let cz = ((world_z - height_map.origin_z) / cell_size_meters) + .floor() + .clamp(0.0, size_z.saturating_sub(1) as f32) as usize; + let ci = cx + cz * size_x; + // Use max height in cell (captures furniture tops) + let w = 1.0f32; + weight_sum[ci] += w; + height_sum[ci] += relative_height * w; + } + } + + let mut observed = vec![false; len]; + for i in 0..len { + if weight_sum[i] > 0.0 { + heights[i] = height_sum[i] / weight_sum[i]; + observed[i] = true; + } + } + + Some(HeightCorrelationGrid { + origin_x: height_map.origin_x, + origin_z: height_map.origin_z, + cell_size_meters, + size_x, + size_z, + heights, + observed, + }) +} + +/// Sample the height grid at a world position using nearest-neighbor lookup. +/// Returns None if out of bounds or unobserved. +fn sample_height_grid(grid: &HeightCorrelationGrid, world_x: f32, world_z: f32) -> Option { + let local_x = (world_x - grid.origin_x) / grid.cell_size_meters; + let local_z = (world_z - grid.origin_z) / grid.cell_size_meters; + if local_x < 0.0 + || local_z < 0.0 + || local_x >= grid.size_x as f32 + || local_z >= grid.size_z as f32 + { + return None; + } + let x = local_x.floor() as usize; + let z = local_z.floor() as usize; + let i = x + z * grid.size_x; + if grid.observed[i] { + Some(grid.heights[i]) + } else { + None + } +} + +/// Score a pose hypothesis by normalized cross-correlation of overlapping height values. +/// Returns (ncc, overlap_count) where ncc in [-1, 1] and higher is better. +fn score_height_correlation( + local_grid: &HeightCorrelationGrid, + remote_grid: &HeightCorrelationGrid, + yaw_radians: f32, + translation_x: f32, + translation_z: f32, +) -> (f32, usize) { + let (sin_yaw, cos_yaw) = yaw_radians.sin_cos(); + let mut sum_l = 0.0f64; + let mut sum_r = 0.0f64; + let mut sum_ll = 0.0f64; + let mut sum_rr = 0.0f64; + let mut sum_lr = 0.0f64; + let mut count = 0usize; + + for rz in 0..remote_grid.size_z { + for rx in 0..remote_grid.size_x { + let ri = rx + rz * remote_grid.size_x; + if !remote_grid.observed[ri] { + continue; + } + let remote_h = remote_grid.heights[ri]; + let world_rx = remote_grid.origin_x + (rx as f32 + 0.5) * remote_grid.cell_size_meters; + let world_rz = remote_grid.origin_z + (rz as f32 + 0.5) * remote_grid.cell_size_meters; + let rotated_x = world_rx * cos_yaw - world_rz * sin_yaw; + let rotated_z = world_rx * sin_yaw + world_rz * cos_yaw; + let local_x = rotated_x + translation_x; + let local_z = rotated_z + translation_z; + let Some(local_h) = sample_height_grid(local_grid, local_x, local_z) else { + continue; + }; + let lh = local_h as f64; + let rh = remote_h as f64; + sum_l += lh; + sum_r += rh; + sum_ll += lh * lh; + sum_rr += rh * rh; + sum_lr += lh * rh; + count += 1; + } + } + + if count < 8 { + return (0.0, count); + } + + let n = count as f64; + let mean_l = sum_l / n; + let mean_r = sum_r / n; + let var_l = (sum_ll / n - mean_l * mean_l).max(0.0); + let var_r = (sum_rr / n - mean_r * mean_r).max(0.0); + let cov_lr = sum_lr / n - mean_l * mean_r; + let denom = (var_l * var_r).sqrt(); + if denom < 1.0e-8 { + return (0.0, count); + } + let ncc = (cov_lr / denom) as f32; + (ncc, count) +} + +/// Convert height correlation score to a ShapeMatchCandidate for reporting. +fn height_correlation_candidate( + local_grid: &HeightCorrelationGrid, + remote_grid: &HeightCorrelationGrid, + yaw_radians: f32, + translation_x: f32, + translation_z: f32, +) -> ShapeMatchCandidate { + // Score forward + let (ncc_fwd, count_fwd) = score_height_correlation( + local_grid, + remote_grid, + yaw_radians, + translation_x, + translation_z, + ); + // Score inverse + let (ncc_inv, count_inv) = score_height_correlation( + remote_grid, + local_grid, + -yaw_radians, + // Inverse transform: rotate(-yaw) then translate(-tx, -tz) + // But we need to negate translation in the rotated frame + { + let (s, c) = (-yaw_radians).sin_cos(); + -translation_x * c + translation_z * s + }, + { + let (s, c) = (-yaw_radians).sin_cos(); + -translation_x * s - translation_z * c + }, + ); + + let min_count = count_fwd.min(count_inv); + let total_remote = remote_grid.observed.iter().filter(|&&o| o).count().max(1); + let total_local = local_grid.observed.iter().filter(|&&o| o).count().max(1); + let coverage_fwd = count_fwd as f32 / total_remote as f32; + let coverage_inv = count_inv as f32 / total_local as f32; + let coverage = (coverage_fwd * coverage_inv).sqrt(); + + // Symmetric NCC + let ncc = if ncc_fwd > 0.0 && ncc_inv > 0.0 { + (ncc_fwd * ncc_inv).sqrt() + } else { + (ncc_fwd + ncc_inv) * 0.5 + }; + + // Score combines correlation strength with coverage + let score = ncc.max(0.0) * coverage.sqrt(); + + ShapeMatchCandidate { + score, + feature_score: ncc, + support_score: coverage, + coverage, + close_ratio: ncc.max(0.0), + mean_distance_meters: (1.0 - ncc.max(0.0)).max(0.0), + yaw_radians, + translation_x, + translation_z, + in_bounds_points: min_count, + } +} + +const HEIGHT_CORR_COARSE_CELL_METERS: f32 = 0.20; +const HEIGHT_CORR_FINE_CELL_METERS: f32 = 0.10; +const HEIGHT_CORR_COARSE_YAW_STEP: f32 = 4.0_f32 * PI / 180.0; +const HEIGHT_CORR_FINE_YAW_STEP: f32 = 1.0_f32 * PI / 180.0; +const HEIGHT_CORR_FINAL_YAW_STEP: f32 = 0.25_f32 * PI / 180.0; +const HEIGHT_CORR_COARSE_TOP_K: usize = 24; +const HEIGHT_CORR_FINE_TOP_K: usize = 12; +const HEIGHT_CORR_FINAL_TOP_K: usize = 8; + +fn run_height_correlation_analysis( + local: &XrDepthAlignDescriptor, + remote: &XrDepthAlignDescriptor, + manual_pose: Option, +) -> Option { + let (Some(local_map), Some(remote_map)) = + (local.height_map.as_ref(), remote.height_map.as_ref()) + else { + return None; + }; + + // Build coarse height grids + let local_coarse = build_height_correlation_grid(local_map, HEIGHT_CORR_COARSE_CELL_METERS)?; + let remote_coarse = build_height_correlation_grid(remote_map, HEIGHT_CORR_COARSE_CELL_METERS)?; + + // Stage 1: coarse global search over (yaw, tx, tz) + let local_min_x = local_coarse.origin_x; + let local_max_x = + local_coarse.origin_x + local_coarse.size_x as f32 * local_coarse.cell_size_meters; + let local_min_z = local_coarse.origin_z; + let local_max_z = + local_coarse.origin_z + local_coarse.size_z as f32 * local_coarse.cell_size_meters; + + let mut coarse_finalists = Vec::new(); + let t_step = HEIGHT_CORR_COARSE_CELL_METERS * 2.0; + + let mut yaw = -PI; + while yaw < PI { + let (sin_yaw, cos_yaw) = yaw.sin_cos(); + // Compute rotated remote bounds + let mut rmin_x = f32::INFINITY; + let mut rmax_x = f32::NEG_INFINITY; + let mut rmin_z = f32::INFINITY; + let mut rmax_z = f32::NEG_INFINITY; + let corners = [ + (remote_coarse.origin_x, remote_coarse.origin_z), + ( + remote_coarse.origin_x + + remote_coarse.size_x as f32 * remote_coarse.cell_size_meters, + remote_coarse.origin_z, + ), + ( + remote_coarse.origin_x, + remote_coarse.origin_z + + remote_coarse.size_z as f32 * remote_coarse.cell_size_meters, + ), + ( + remote_coarse.origin_x + + remote_coarse.size_x as f32 * remote_coarse.cell_size_meters, + remote_coarse.origin_z + + remote_coarse.size_z as f32 * remote_coarse.cell_size_meters, + ), + ]; + for &(cx, cz) in &corners { + let rx = cx * cos_yaw - cz * sin_yaw; + let rz = cx * sin_yaw + cz * cos_yaw; + rmin_x = rmin_x.min(rx); + rmax_x = rmax_x.max(rx); + rmin_z = rmin_z.min(rz); + rmax_z = rmax_z.max(rz); + } + + let t_min_x = local_min_x - rmax_x; + let t_max_x = local_max_x - rmin_x; + let t_min_z = local_min_z - rmax_z; + let t_max_z = local_max_z - rmin_z; + + let steps_x = ((t_max_x - t_min_x) / t_step).ceil().max(0.0) as usize; + let steps_z = ((t_max_z - t_min_z) / t_step).ceil().max(0.0) as usize; + + for xi in 0..=steps_x { + let tx = t_min_x + xi as f32 * t_step; + for zi in 0..=steps_z { + let tz = t_min_z + zi as f32 * t_step; + let candidate = + height_correlation_candidate(&local_coarse, &remote_coarse, yaw, tx, tz); + push_shape_candidate( + &mut coarse_finalists, + candidate, + HEIGHT_CORR_COARSE_TOP_K, + t_step * 1.5, + HEIGHT_CORR_COARSE_YAW_STEP * 1.25, + ); + } + } + yaw += HEIGHT_CORR_COARSE_YAW_STEP; + } + + if coarse_finalists.is_empty() { + return None; + } + + // Stage 2: refine on finer grid + let local_fine = build_height_correlation_grid(local_map, HEIGHT_CORR_FINE_CELL_METERS)?; + let remote_fine = build_height_correlation_grid(remote_map, HEIGHT_CORR_FINE_CELL_METERS)?; + + let refine_t_step = HEIGHT_CORR_FINE_CELL_METERS; + let refine_t_window = HEIGHT_CORR_COARSE_CELL_METERS * 2.5; + let refine_yaw_window = HEIGHT_CORR_COARSE_YAW_STEP * 1.5; + + let mut fine_finalists = Vec::new(); + for seed in &coarse_finalists { + let yaw_min = seed.yaw_radians - refine_yaw_window; + let yaw_max = seed.yaw_radians + refine_yaw_window; + let mut yaw = yaw_min; + while yaw <= yaw_max { + let t_steps = (refine_t_window / refine_t_step).ceil().max(1.0) as isize; + for dxi in -t_steps..=t_steps { + let tx = seed.translation_x + dxi as f32 * refine_t_step; + for dzi in -t_steps..=t_steps { + let tz = seed.translation_z + dzi as f32 * refine_t_step; + let candidate = + height_correlation_candidate(&local_fine, &remote_fine, yaw, tx, tz); + push_shape_candidate( + &mut fine_finalists, + candidate, + HEIGHT_CORR_FINE_TOP_K, + refine_t_step * 1.5, + HEIGHT_CORR_FINE_YAW_STEP * 1.25, + ); + } + } + yaw += HEIGHT_CORR_FINE_YAW_STEP; + } + } + + if fine_finalists.is_empty() { + return None; + } + + // Stage 3: final sub-cell refinement + let final_t_step = HEIGHT_CORR_FINE_CELL_METERS * 0.5; + let final_t_window = HEIGHT_CORR_FINE_CELL_METERS * 2.0; + let final_yaw_window = HEIGHT_CORR_FINE_YAW_STEP * 1.5; + + let mut final_finalists = Vec::new(); + for seed in &fine_finalists { + let yaw_min = seed.yaw_radians - final_yaw_window; + let yaw_max = seed.yaw_radians + final_yaw_window; + let mut yaw = yaw_min; + while yaw <= yaw_max { + let t_steps = (final_t_window / final_t_step).ceil().max(1.0) as isize; + for dxi in -t_steps..=t_steps { + let tx = seed.translation_x + dxi as f32 * final_t_step; + for dzi in -t_steps..=t_steps { + let tz = seed.translation_z + dzi as f32 * final_t_step; + let candidate = + height_correlation_candidate(&local_fine, &remote_fine, yaw, tx, tz); + push_shape_candidate( + &mut final_finalists, + candidate, + HEIGHT_CORR_FINAL_TOP_K, + final_t_step * 1.5, + HEIGHT_CORR_FINAL_YAW_STEP * 1.25, + ); + } + } + yaw += HEIGHT_CORR_FINAL_YAW_STEP; + } + } + + if final_finalists.is_empty() { + return None; + } + + let manual_candidate = manual_pose.map(|mp| { + height_correlation_candidate( + &local_fine, + &remote_fine, + mp.rotation_radians, + mp.shift_x_meters, + mp.shift_y_meters, + ) + }); + + Some(ShapeMatchReport { + label: "height_correlation".to_string(), + finalists: final_finalists, + manual_candidate, + }) +} + +// --- Blob RANSAC matching --- +// Extracts furniture blobs with mean heights from both scans, then exhaustively +// tries all 2-blob correspondence pairs to generate SE(2) hypotheses. +// Scores each hypothesis by heightmap correlation. + +#[derive(Clone, Copy, Debug)] +struct HeightBlob { + centroid_x: f32, + centroid_z: f32, + mean_height: f32, + area_m2: f32, +} + +fn extract_height_blobs( + height_map: &XrDepthAlignHeightMap, + cell_size_meters: f32, +) -> Vec { + let size_x = ((height_map.extent_x_meters() / cell_size_meters).ceil() as usize).max(1); + let size_z = ((height_map.extent_z_meters() / cell_size_meters).ceil() as usize).max(1); + let len = size_x * size_z; + let mut occupied = vec![false; len]; + let mut cell_height = vec![0.0f32; len]; + let mut cell_count = vec![0usize; len]; + + let cutout_center = height_map.player_cutout_center; + let cutout_radius = (height_map.player_cutout_radius_meters + height_map.cell_size_meters) + .max(height_map.cell_size_meters * 2.0); + let floor_y = height_map.floor_y_meters; + let min_h = SHAPE_FURNITURE_MIN_HEIGHT_METERS; + let max_h = SHAPE_FURNITURE_MAX_HEIGHT_METERS; + + for src_z in 0..height_map.size_z_usize() { + for src_x in 0..height_map.size_x_usize() { + let index = height_map.cell_index(src_x, src_z); + let Some(&height) = height_map.heights_meters.get(index) else { + continue; + }; + if !height.is_finite() { + continue; + } + let world_x = height_map.origin_x + (src_x as f32 + 0.5) * height_map.cell_size_meters; + let world_z = height_map.origin_z + (src_z as f32 + 0.5) * height_map.cell_size_meters; + if cutout_center.is_some_and(|center| { + let dx = world_x - center.x; + let dz = world_z - center.y; + (dx * dx + dz * dz).sqrt() <= cutout_radius + }) { + continue; + } + let rel_h = height - floor_y; + if rel_h < min_h || rel_h > max_h { + continue; + } + let cx = ((world_x - height_map.origin_x) / cell_size_meters) + .floor() + .clamp(0.0, size_x.saturating_sub(1) as f32) as usize; + let cz = ((world_z - height_map.origin_z) / cell_size_meters) + .floor() + .clamp(0.0, size_z.saturating_sub(1) as f32) as usize; + let ci = cx + cz * size_x; + occupied[ci] = true; + cell_height[ci] += rel_h; + cell_count[ci] += 1; + } + } + + // Connected component extraction with mean height + let min_cells = (SHAPE_FURNITURE_MIN_COMPONENT_AREA_METERS2 + / (cell_size_meters * cell_size_meters).max(1.0e-4)) + .ceil() + .max(1.0) as usize; + + let mut visited = vec![false; len]; + let mut blobs = Vec::new(); + + for start in 0..len { + if visited[start] || !occupied[start] { + continue; + } + let mut stack = vec![start]; + let mut component = Vec::new(); + let mut touches_border = false; + visited[start] = true; + while let Some(idx) = stack.pop() { + component.push(idx); + let x = idx % size_x; + let z = idx / size_x; + if x == 0 || z == 0 || x + 1 == size_x || z + 1 == size_z { + touches_border = true; + } + for (dx, dz) in [(-1isize, 0isize), (1, 0), (0, -1), (0, 1)] { + let nx = x as isize + dx; + let nz = z as isize + dz; + if nx < 0 || nz < 0 || nx >= size_x as isize || nz >= size_z as isize { + continue; + } + let ni = nx as usize + nz as usize * size_x; + if visited[ni] || !occupied[ni] { + continue; + } + visited[ni] = true; + stack.push(ni); + } + } + + if component.len() < min_cells || touches_border { + continue; + } + + let mut sum_x = 0.0f32; + let mut sum_z = 0.0f32; + let mut sum_h = 0.0f32; + let mut total_samples = 0usize; + for &ci in &component { + let x = ci % size_x; + let z = ci / size_x; + let wx = height_map.origin_x + (x as f32 + 0.5) * cell_size_meters; + let wz = height_map.origin_z + (z as f32 + 0.5) * cell_size_meters; + sum_x += wx * cell_count[ci] as f32; + sum_z += wz * cell_count[ci] as f32; + sum_h += cell_height[ci]; + total_samples += cell_count[ci]; + } + if total_samples == 0 { + continue; + } + let n = total_samples as f32; + blobs.push(HeightBlob { + centroid_x: sum_x / n, + centroid_z: sum_z / n, + mean_height: sum_h / n, + area_m2: component.len() as f32 * cell_size_meters * cell_size_meters, + }); + } + + blobs.sort_by(|a, b| b.area_m2.total_cmp(&a.area_m2)); + blobs.truncate(20); + blobs +} + +/// From two corresponding blob pairs (l0↔r0, l1↔r1), compute the SE(2) transform. +/// Returns (yaw, tx, tz) or None if degenerate. +fn se2_from_two_point_pairs( + l0x: f32, + l0z: f32, + l1x: f32, + l1z: f32, + r0x: f32, + r0z: f32, + r1x: f32, + r1z: f32, +) -> Option<(f32, f32, f32)> { + let dl_x = l1x - l0x; + let dl_z = l1z - l0z; + let dr_x = r1x - r0x; + let dr_z = r1z - r0z; + let dl_len = (dl_x * dl_x + dl_z * dl_z).sqrt(); + let dr_len = (dr_x * dr_x + dr_z * dr_z).sqrt(); + if dl_len < 0.15 || dr_len < 0.15 { + return None; // Too close together + } + // Check distance ratio - should be ~1.0 for rigid transform + let ratio = dl_len / dr_len; + if ratio < 0.6 || ratio > 1.67 { + return None; + } + let angle_l = dl_z.atan2(dl_x); + let angle_r = dr_z.atan2(dr_x); + let yaw = wrap_angle(angle_l - angle_r); + + let (sin_yaw, cos_yaw) = yaw.sin_cos(); + let rotated_r0x = r0x * cos_yaw - r0z * sin_yaw; + let rotated_r0z = r0x * sin_yaw + r0z * cos_yaw; + let tx = l0x - rotated_r0x; + let tz = l0z - rotated_r0z; + Some((yaw, tx, tz)) +} + +fn run_blob_ransac_analysis( + local: &XrDepthAlignDescriptor, + remote: &XrDepthAlignDescriptor, + manual_pose: Option, +) -> Option { + let (Some(local_map), Some(remote_map)) = + (local.height_map.as_ref(), remote.height_map.as_ref()) + else { + return None; + }; + + let local_blobs = extract_height_blobs(local_map, 0.10); + let remote_blobs = extract_height_blobs(remote_map, 0.10); + + if local_blobs.len() < 2 || remote_blobs.len() < 2 { + return None; + } + + // Build height correlation grids for scoring + let local_grid = build_height_correlation_grid(local_map, HEIGHT_CORR_FINE_CELL_METERS)?; + let remote_grid = build_height_correlation_grid(remote_map, HEIGHT_CORR_FINE_CELL_METERS)?; + + let mut finalists = Vec::new(); + + // Exhaustive 2-pair RANSAC + let nl = local_blobs.len().min(12); + let nr = remote_blobs.len().min(12); + + for li0 in 0..nl { + for li1 in (li0 + 1)..nl { + for ri0 in 0..nr { + for ri1 in 0..nr { + if ri0 == ri1 { + continue; + } + // Check height compatibility + let h_diff_0 = + (local_blobs[li0].mean_height - remote_blobs[ri0].mean_height).abs(); + let h_diff_1 = + (local_blobs[li1].mean_height - remote_blobs[ri1].mean_height).abs(); + if h_diff_0 > 0.35 || h_diff_1 > 0.35 { + continue; + } + + let Some((yaw, tx, tz)) = se2_from_two_point_pairs( + local_blobs[li0].centroid_x, + local_blobs[li0].centroid_z, + local_blobs[li1].centroid_x, + local_blobs[li1].centroid_z, + remote_blobs[ri0].centroid_x, + remote_blobs[ri0].centroid_z, + remote_blobs[ri1].centroid_x, + remote_blobs[ri1].centroid_z, + ) else { + continue; + }; + + let candidate = + height_correlation_candidate(&local_grid, &remote_grid, yaw, tx, tz); + push_shape_candidate( + &mut finalists, + candidate, + HEIGHT_CORR_FINE_TOP_K, + 0.30, + 5.0_f32.to_radians(), + ); + } + } + } + } + + if finalists.is_empty() { + return None; + } + + // Refine top candidates with local search + let mut refined = Vec::new(); + let refine_step = HEIGHT_CORR_FINE_CELL_METERS * 0.5; + let refine_window = HEIGHT_CORR_FINE_CELL_METERS * 3.0; + let yaw_step = 0.5_f32.to_radians(); + let yaw_window = 3.0_f32.to_radians(); + + for seed in finalists.iter().take(8) { + let mut yaw = seed.yaw_radians - yaw_window; + while yaw <= seed.yaw_radians + yaw_window { + let t_steps = (refine_window / refine_step).ceil() as isize; + for dxi in -t_steps..=t_steps { + let tx = seed.translation_x + dxi as f32 * refine_step; + for dzi in -t_steps..=t_steps { + let tz = seed.translation_z + dzi as f32 * refine_step; + let candidate = + height_correlation_candidate(&local_grid, &remote_grid, yaw, tx, tz); + push_shape_candidate( + &mut refined, + candidate, + HEIGHT_CORR_FINAL_TOP_K, + refine_step * 1.5, + yaw_step * 1.25, + ); + } + } + yaw += yaw_step; + } + } + + if refined.is_empty() { + refined = finalists; + } + + let manual_candidate = manual_pose.map(|mp| { + height_correlation_candidate( + &local_grid, + &remote_grid, + mp.rotation_radians, + mp.shift_x_meters, + mp.shift_y_meters, + ) + }); + + Some(ShapeMatchReport { + label: "blob_ransac_height".to_string(), + finalists: refined, + manual_candidate, + }) +} + +fn analyze_path(options: AnalyzeOptions, path: &Path) -> Result<(), String> { + let bytes = + fs::read(&path).map_err(|err| format!("failed to read {}: {err}", path.display()))?; + let pair = XrNetAlignmentDescriptorDumpPair::from_file_bytes(&bytes) + .ok_or_else(|| format!("failed to decode {}", path.display()))?; + + println!("dump: {}", path.display()); + println!( + "file_bytes: {} | file_mtime_unix: {} | format_version: {} | captured_at_unix_ms: {} | remote_peer_id: {}", + bytes.len(), + system_time_hint(&path), + pair.format_version, + pair.captured_at_unix_ms, + pair.remote_peer_id.0 + ); + + let mut local = pair.local_descriptor.descriptor.clone(); + let mut remote = pair.remote_descriptor.descriptor.clone(); + let local = &local; + let remote = &remote; + print_descriptor_stats("local", local); + print_descriptor_stats("remote", remote); + let manual_pose = load_manual_pose(path); + if let Some(manual_pose) = manual_pose { + println!( + "manual_pose: x {:.3} | y {:.3} | rot {:.3} rad ({:.1} deg)", + manual_pose.shift_x_meters, + manual_pose.shift_y_meters, + manual_pose.rotation_radians, + manual_pose.rotation_radians.to_degrees() + ); + } else { + println!("manual_pose: none"); + } + + if !options.solve { + println!("solve: skipped (pass --solve to run the full matcher)"); + return Ok(()); + } + + let solve_started = Instant::now(); + let callback_budget = std::time::Duration::from_millis(ANALYZE_TSDF_CALLBACK_BUDGET_MILLIS); + let mut matcher = XrDepthAlignMatcher::new(local, remote, None); + let mut callback_count = 0u32; + let mut total_callback_steps = 0u32; + let mut max_callback_steps = 0u32; + let mut max_callback_elapsed = std::time::Duration::ZERO; + while !matcher.is_finished() { + let callback_started = Instant::now(); + let callback_steps = + matcher.step_for_budget(callback_budget, ANALYZE_TSDF_CALLBACK_MAX_STEPS); + if callback_steps == 0 { + break; + } + callback_count = callback_count.saturating_add(1); + total_callback_steps = total_callback_steps.saturating_add(callback_steps); + max_callback_steps = max_callback_steps.max(callback_steps); + max_callback_elapsed = max_callback_elapsed.max(callback_started.elapsed()); + } + let diagnostic = matcher + .diagnostic() + .expect("timesliced analyzer matcher should produce a diagnostic"); + let solve_elapsed = solve_started.elapsed(); + let estimated_callback_wall_ms = diagnostic.total_compute_ms as u64 + + ANALYZE_TSDF_CALLBACK_INTERVAL_MILLIS * callback_count.saturating_sub(1) as u64; + let estimated_callback_busy_ratio = if estimated_callback_wall_ms == 0 { + 0.0 + } else { + diagnostic.total_compute_ms as f64 / estimated_callback_wall_ms as f64 + }; + println!( + "diagnostic: outcome {} | solve_ms {} | total_match_ms {} | steps {} | max_step_ms {} | yaw candidates {} | pose candidates {} | shortlisted {} | local walls {} | remote walls {} | remote dense {}", + explain_outcome(&diagnostic), + solve_elapsed.as_millis(), + diagnostic.total_compute_ms, + diagnostic.step_count, + diagnostic.max_step_ms, + diagnostic.yaw_candidate_count, + diagnostic.pose_candidate_count, + diagnostic.shortlisted_pose_count, + diagnostic.local_wall_samples, + diagnostic.remote_wall_samples, + diagnostic.remote_dense_wall_samples, + ); + println!( + "timing_ms: build {} | yaw {} | vote {} | signal_refine {} | signal_score {} | height_refine {} | final_score {} | wall_profile {}", + diagnostic.signal_build_ms, + diagnostic.yaw_candidate_ms, + diagnostic.translation_vote_ms, + diagnostic.signal_refine_ms, + diagnostic.signal_score_ms, + diagnostic.height_refine_ms, + diagnostic.final_score_ms, + diagnostic.wall_profile_ms, + ); + println!( + "callback_sim: budget_ms {} | interval_ms {} | callbacks {} | avg_steps_per_callback {:.1} | max_steps_per_callback {} | max_callback_ms {} | est_wall_ms {} | est_busy {:.0}%", + ANALYZE_TSDF_CALLBACK_BUDGET_MILLIS, + ANALYZE_TSDF_CALLBACK_INTERVAL_MILLIS, + callback_count, + if callback_count == 0 { + 0.0 + } else { + total_callback_steps as f64 / callback_count as f64 + }, + max_callback_steps, + max_callback_elapsed.as_millis(), + estimated_callback_wall_ms, + estimated_callback_busy_ratio * 100.0, + ); + + if let Some(best) = diagnostic.best_solution { + println!( + "best: yaw {:.3} rad ({:.1} deg) | translation ({:.3}, {:.3}, {:.3}) | conf {:.3} | sym {:.3} | residual {:.3} m | matched {}", + best.yaw_radians, + best.yaw_radians.to_degrees(), + best.translation.x, + best.translation.y, + best.translation.z, + best.confidence, + best.symmetry_confidence, + best.residual_meters, + best.matched_samples + ); + } else { + println!("best: none"); + } + + if let Some(accepted) = diagnostic.accepted_solution() { + println!( + "accepted: yaw {:.3} rad ({:.1} deg) | translation ({:.3}, {:.3}, {:.3})", + accepted.yaw_radians, + accepted.yaw_radians.to_degrees(), + accepted.translation.x, + accepted.translation.y, + accepted.translation.z, + ); + if let Some(manual_pose) = manual_pose { + println!("{}", manual_pose_delta_text(accepted, manual_pose)); + } + } else { + println!("accepted: none"); + } + + if let Some(manual_pose) = manual_pose { + let manual_seed = manual_pose_solution(local, remote, manual_pose); + let manual_scored = local.rescore_remote_to_local(remote, manual_seed); + println!( + "manual_scored: yaw {:.3} rad ({:.1} deg) | translation ({:.3}, {:.3}, {:.3}) | conf {:.3} | sym {:.3} | residual {:.3} m | matched {} | accepted {}", + manual_scored.yaw_radians, + manual_scored.yaw_radians.to_degrees(), + manual_scored.translation.x, + manual_scored.translation.y, + manual_scored.translation.z, + manual_scored.confidence, + manual_scored.symmetry_confidence, + manual_scored.residual_meters, + manual_scored.matched_samples, + manual_scored.is_accepted(&diagnostic) + ); + } + let run_legacy_search_paths = false; + if !run_legacy_search_paths { + println!("legacy_search_paths: disabled while focusing on runtime matcher perf"); + return Ok(()); + } + + let overlap_band = ShapeBandSpec { + label: "shape_overlap_0.60_1.80".to_string(), + min_height_meters: 0.60, + max_height_meters: Some(1.80), + floor_offset_meters: 0.0, + drop_border_components: false, + }; + let obstacle_band = shape_band_obstacle(); + let furniture_band = shape_band_furniture(); + let ( + obstacle_result, + furniture_result, + hough_result, + overlap_result, + component_pair_result, + component_descriptor_result, + component_context_result, + component_graph_result, + correlative_result, + height_corr_result, + blob_ransac_result, + shape_scan_result, + ) = std::thread::scope(|scope| { + let obstacle_band = obstacle_band.clone(); + let obstacle_handle = scope.spawn(move || { + let started = Instant::now(); + let report = run_shape_match_analysis(local, remote, manual_pose, &obstacle_band); + (started.elapsed().as_millis(), report) + }); + let furniture_band = furniture_band.clone(); + let furniture_handle = scope.spawn(move || { + let started = Instant::now(); + let report = run_shape_match_analysis(local, remote, manual_pose, &furniture_band); + (started.elapsed().as_millis(), report) + }); + let hough_handle = scope.spawn(|| { + let started = Instant::now(); + let report = run_hough_shape_analysis(local, remote, manual_pose); + (started.elapsed().as_millis(), report) + }); + let overlap_band = overlap_band.clone(); + let overlap_handle = scope.spawn(move || { + let started = Instant::now(); + let report = run_fixed_overlap_band_analysis(local, remote, manual_pose, &overlap_band); + (started.elapsed().as_millis(), report) + }); + let component_pair_handle = scope.spawn(|| { + let started = Instant::now(); + let report = run_component_pair_seeded_analysis(local, remote, manual_pose); + (started.elapsed().as_millis(), report) + }); + let component_descriptor_handle = scope.spawn(|| { + let started = Instant::now(); + let report = run_component_descriptor_seeded_analysis(local, remote, manual_pose); + (started.elapsed().as_millis(), report) + }); + let component_context_handle = scope.spawn(|| { + let started = Instant::now(); + let report = run_component_context_seeded_analysis(local, remote, manual_pose); + (started.elapsed().as_millis(), report) + }); + let component_graph_handle = scope.spawn(|| { + let started = Instant::now(); + let report = run_component_graph_seeded_analysis(local, remote, manual_pose); + (started.elapsed().as_millis(), report) + }); + let correlative_handle = scope.spawn(|| { + let started = Instant::now(); + let report = run_correlative_consensus_analysis(local, remote, manual_pose); + (started.elapsed().as_millis(), report) + }); + let height_corr_handle = scope.spawn(|| { + let started = Instant::now(); + let report = run_height_correlation_analysis(local, remote, manual_pose); + (started.elapsed().as_millis(), report) + }); + let blob_ransac_handle = scope.spawn(|| { + let started = Instant::now(); + let report = run_blob_ransac_analysis(local, remote, manual_pose); + (started.elapsed().as_millis(), report) + }); + let shape_scan_handle = scope.spawn(|| { + let started = Instant::now(); + let results = run_shape_band_scan(local, remote, manual_pose); + (started.elapsed().as_millis(), results) + }); + ( + obstacle_handle + .join() + .expect("obstacle analysis should not panic"), + furniture_handle + .join() + .expect("furniture analysis should not panic"), + hough_handle + .join() + .expect("hough analysis should not panic"), + overlap_handle + .join() + .expect("overlap analysis should not panic"), + component_pair_handle + .join() + .expect("component-pair analysis should not panic"), + component_descriptor_handle + .join() + .expect("component-descriptor analysis should not panic"), + component_context_handle + .join() + .expect("component-context analysis should not panic"), + component_graph_handle + .join() + .expect("component-graph analysis should not panic"), + correlative_handle + .join() + .expect("correlative analysis should not panic"), + height_corr_handle + .join() + .expect("height-correlation analysis should not panic"), + blob_ransac_handle + .join() + .expect("blob-ransac analysis should not panic"), + shape_scan_handle + .join() + .expect("shape scan should not panic"), + ) + }); + + let mut primary_shape_reports = Vec::::new(); + match obstacle_result { + (elapsed_millis, Some(shape_report)) => { + print_shape_match_report(local, remote, &shape_report, elapsed_millis); + primary_shape_reports.push(shape_report); + } + _ => { + println!("{}_match: unavailable", obstacle_band.label); + } + } + match furniture_result { + (elapsed_millis, Some(shape_report)) => { + print_shape_match_report(local, remote, &shape_report, elapsed_millis); + primary_shape_reports.push(shape_report); + } + _ => { + println!("{}_match: unavailable", furniture_band.label); + } + } + match hough_result { + (elapsed_millis, Some(hough_report)) => { + print_shape_match_report(local, remote, &hough_report, elapsed_millis); + primary_shape_reports.push(hough_report); + } + _ => { + println!("shape_hough_match: unavailable"); + } + } + match overlap_result { + (elapsed_millis, Some(overlap_report)) => { + print_shape_match_report(local, remote, &overlap_report, elapsed_millis); + primary_shape_reports.push(overlap_report); + } + _ => { + println!("{}_match: unavailable", overlap_band.label); + } + } + if let Some(seed) = diagnostic.best_solution { + let seed_candidates = gather_seed_candidates( + &diagnostic, + &primary_shape_reports.iter().collect::>(), + ); + let ( + overlap_seeded_result, + consensus_result, + vote_result, + signed_seeded_result, + signed_support_result, + ) = std::thread::scope(|scope| { + let overlap_seeded_band = overlap_band.clone(); + let overlap_seeded_handle = scope.spawn(move || { + let started = Instant::now(); + let report = + run_seeded_overlap_band_analysis(local, remote, &overlap_seeded_band, seed); + (started.elapsed().as_millis(), report) + }); + let consensus_handle = scope.spawn(|| { + let started = Instant::now(); + let report = + run_seeded_multiband_consensus_analysis(local, remote, manual_pose, seed); + (started.elapsed().as_millis(), report) + }); + let vote_handle = scope.spawn(|| { + let started = Instant::now(); + let report = run_seeded_multiband_vote_analysis(local, remote, manual_pose, seed); + (started.elapsed().as_millis(), report) + }); + let signed_seeded_band = overlap_band.clone(); + let signed_seed_candidates = seed_candidates.clone(); + let signed_seeded_handle = scope.spawn(move || { + let started = Instant::now(); + let report = run_seeded_signed_overlap_band_analysis( + local, + remote, + manual_pose, + &signed_seeded_band, + &signed_seed_candidates, + ); + (started.elapsed().as_millis(), report) + }); + let signed_support_band = overlap_band.clone(); + let signed_support_candidates = seed_candidates.clone(); + let signed_support_handle = scope.spawn(move || { + let started = Instant::now(); + let report = run_seeded_signed_support_band_analysis( + local, + remote, + manual_pose, + &signed_support_band, + &signed_support_candidates, + ); + (started.elapsed().as_millis(), report) + }); + ( + overlap_seeded_handle + .join() + .expect("seeded overlap analysis should not panic"), + consensus_handle + .join() + .expect("seeded consensus analysis should not panic"), + vote_handle + .join() + .expect("seeded vote analysis should not panic"), + signed_seeded_handle + .join() + .expect("seeded signed-overlap analysis should not panic"), + signed_support_handle + .join() + .expect("seeded signed-support analysis should not panic"), + ) + }); + + match overlap_seeded_result { + (elapsed_millis, Some(overlap_seeded_report)) => { + print_shape_match_report(local, remote, &overlap_seeded_report, elapsed_millis); + } + _ => { + println!("{}_seeded_match: unavailable", overlap_band.label); + } + } + match consensus_result { + (elapsed_millis, Some(consensus_report)) => { + print_shape_match_report(local, remote, &consensus_report, elapsed_millis); + } + _ => { + println!("shape_multiband_seeded_match: unavailable"); + } + } + match vote_result { + (elapsed_millis, Some(vote_report)) => { + print_shape_match_report(local, remote, &vote_report, elapsed_millis); + } + _ => { + println!("shape_multiband_vote_seeded_match: unavailable"); + } + } + match signed_seeded_result { + (elapsed_millis, Some(signed_seeded_report)) => { + print_shape_match_report(local, remote, &signed_seeded_report, elapsed_millis); + } + _ => { + println!("{}_signed_seeded_match: unavailable", overlap_band.label); + } + } + match signed_support_result { + (elapsed_millis, Some(signed_support_seeded_report)) => { + print_shape_match_report( + local, + remote, + &signed_support_seeded_report, + elapsed_millis, + ); + } + _ => { + println!( + "{}_signed_support_seeded_match: unavailable", + overlap_band.label + ); + } + } + } + + match component_pair_result { + (elapsed_millis, Some(component_pair_report)) => { + print_shape_match_report(local, remote, &component_pair_report, elapsed_millis); + } + _ => { + println!("shape_component_pair_seeded_match: unavailable"); + } + } + match component_descriptor_result { + (elapsed_millis, Some(component_descriptor_report)) => { + print_shape_match_report(local, remote, &component_descriptor_report, elapsed_millis); + } + _ => { + println!("shape_component_descriptor_seeded_match: unavailable"); + } + } + match component_context_result { + (elapsed_millis, Some(component_context_report)) => { + print_shape_match_report(local, remote, &component_context_report, elapsed_millis); + } + _ => { + println!("shape_component_context_seeded_match: unavailable"); + } + } + match component_graph_result { + (elapsed_millis, Some(component_graph_report)) => { + print_shape_match_report(local, remote, &component_graph_report, elapsed_millis); + } + _ => { + println!("shape_component_graph_seeded_match: unavailable"); + } + } + match correlative_result { + (elapsed_millis, Some(correlative_report)) => { + print_shape_match_report(local, remote, &correlative_report, elapsed_millis); + } + _ => { + println!("shape_correlative_consensus_match: unavailable"); + } + } + match height_corr_result { + (elapsed_millis, Some(height_corr_report)) => { + print_shape_match_report(local, remote, &height_corr_report, elapsed_millis); + } + _ => { + println!("height_correlation_match: unavailable"); + } + } + match blob_ransac_result { + (elapsed_millis, Some(blob_ransac_report)) => { + print_shape_match_report(local, remote, &blob_ransac_report, elapsed_millis); + } + _ => { + println!("blob_ransac_height_match: unavailable"); + } + } + + let (shape_scan_elapsed_millis, shape_scan_results) = shape_scan_result; + println!("shape_scan_ms: {}", shape_scan_elapsed_millis); + print_shape_band_scan_results(local, remote, &shape_scan_results); + + Ok(()) +} + +fn main() -> Result<(), String> { + let (options, paths) = parse_args()?; + for (index, path) in paths.iter().enumerate() { + if index != 0 { + println!(); + } + analyze_path(options, path)?; + } + Ok(()) +} diff --git a/xr/src/bin/bench_lz4_dump.rs b/xr/src/bin/bench_lz4_dump.rs new file mode 100644 index 0000000000..0ed617acba --- /dev/null +++ b/xr/src/bin/bench_lz4_dump.rs @@ -0,0 +1,279 @@ +use makepad_lz4::{compress_bound, compress_fast_into, decompress_safe, implementation_name}; +use makepad_widgets::*; +use makepad_xr::net::*; +use std::{ + env, fs, + path::{Path, PathBuf}, + time::Instant, +}; + +#[derive(Clone, Copy, Debug)] +struct BenchOptions { + latest_count: usize, + acceleration: usize, + timed_mib: usize, + samples: usize, +} + +#[derive(Clone, Copy, Debug)] +struct BenchResult { + input_bytes: usize, + compressed_bytes: usize, + compression_mbps: f64, + decompression_mbps: f64, +} + +fn latest_dump_paths(count: usize) -> Vec { + let dump_dir = PathBuf::from("xr/dump/dumps"); + let mut entries = fs::read_dir(dump_dir) + .ok() + .into_iter() + .flatten() + .filter_map(|entry| entry.ok()) + .filter_map(|entry| { + let metadata = entry.metadata().ok()?; + metadata + .is_file() + .then_some((entry.path(), metadata.modified().ok()?)) + }) + .collect::>(); + entries.sort_by(|left, right| right.1.cmp(&left.1)); + entries + .into_iter() + .filter_map(|(path, _)| { + path.file_name() + .and_then(|name| name.to_str()) + .is_some_and(|name| name.ends_with(".bin") && name != "manual-smoke.bin") + .then_some(path) + }) + .take(count.max(1)) + .collect() +} + +fn parse_args() -> Result<(BenchOptions, Vec), String> { + let mut options = BenchOptions { + latest_count: 2, + acceleration: 1, + timed_mib: 512, + samples: 5, + }; + let mut paths = Vec::::new(); + let mut args = env::args_os().skip(1); + while let Some(arg) = args.next() { + if arg == "--latest" { + let Some(count) = args.next() else { + return Err("expected a count after --latest".to_string()); + }; + let count = count + .to_string_lossy() + .parse::() + .map_err(|_| "failed to parse --latest count".to_string())?; + options.latest_count = count.max(1); + continue; + } + if arg == "--accel" { + let Some(value) = args.next() else { + return Err("expected a value after --accel".to_string()); + }; + let acceleration = value + .to_string_lossy() + .parse::() + .map_err(|_| "failed to parse --accel".to_string())?; + options.acceleration = acceleration.max(1); + continue; + } + if arg == "--timed-mib" { + let Some(value) = args.next() else { + return Err("expected a value after --timed-mib".to_string()); + }; + let timed_mib = value + .to_string_lossy() + .parse::() + .map_err(|_| "failed to parse --timed-mib".to_string())?; + options.timed_mib = timed_mib.max(1); + continue; + } + if arg == "--samples" { + let Some(value) = args.next() else { + return Err("expected a value after --samples".to_string()); + }; + let samples = value + .to_string_lossy() + .parse::() + .map_err(|_| "failed to parse --samples".to_string())?; + options.samples = samples.max(1); + continue; + } + paths.push(PathBuf::from(arg)); + } + if paths.is_empty() { + paths = latest_dump_paths(options.latest_count); + } + if paths.is_empty() { + return Err("no dump files found in xr/dump/dumps".to_string()); + } + Ok((options, paths)) +} + +fn height_bytes(height_map: &XrDepthAlignHeightMap) -> Vec { + let mut bytes = Vec::::with_capacity(height_map.heights_meters.len() * 4); + for height in &height_map.heights_meters { + bytes.extend_from_slice(&height.to_le_bytes()); + } + bytes +} + +fn target_raw_bytes(options: BenchOptions) -> usize { + options.timed_mib * 1024 * 1024 +} + +fn timing_iterations(input_bytes: usize, options: BenchOptions) -> usize { + target_raw_bytes(options) + .div_ceil(input_bytes.max(1)) + .max(1) +} + +fn median(samples: &mut [f64]) -> f64 { + samples.sort_by(f64::total_cmp); + let mid = samples.len() / 2; + if samples.len() % 2 == 0 { + (samples[mid - 1] + samples[mid]) * 0.5 + } else { + samples[mid] + } +} + +fn bench_buffer( + bytes: &[u8], + acceleration: usize, + options: BenchOptions, +) -> Result { + let bound = compress_bound(bytes.len()); + if bound == 0 { + return Err(format!("buffer too large for lz4: {} bytes", bytes.len())); + } + let mut compressed_buf = vec![0u8; bound]; + let compressed_len = compress_fast_into(bytes, &mut compressed_buf, acceleration) + .map_err(|err| format!("compression failed: {err}"))?; + let compressed_bytes = compressed_buf[..compressed_len].to_vec(); + let mut roundtrip = vec![0u8; bytes.len()]; + let decoded = decompress_safe(&compressed_bytes, &mut roundtrip) + .map_err(|err| format!("decompression failed: {err}"))?; + if decoded != bytes.len() || roundtrip != bytes { + return Err("lz4 roundtrip mismatch".to_string()); + } + + let iterations = timing_iterations(bytes.len(), options); + let raw_mebibytes = (bytes.len() * iterations) as f64 / (1024.0 * 1024.0); + let mut compression_samples = Vec::::with_capacity(options.samples); + let mut decompression_samples = Vec::::with_capacity(options.samples); + + for _ in 0..options.samples { + let compress_started = Instant::now(); + for _ in 0..iterations { + let written = compress_fast_into(bytes, &mut compressed_buf, acceleration) + .map_err(|err| format!("compression failed during timing: {err}"))?; + if written != compressed_len { + return Err("compressed length changed during timing".to_string()); + } + } + compression_samples + .push(raw_mebibytes / compress_started.elapsed().as_secs_f64().max(1.0e-9)); + } + + let mut decoded_buf = vec![0u8; bytes.len()]; + for _ in 0..options.samples { + let decompress_started = Instant::now(); + for _ in 0..iterations { + let decoded = decompress_safe(&compressed_bytes, &mut decoded_buf) + .map_err(|err| format!("decompression failed during timing: {err}"))?; + if decoded != bytes.len() { + return Err("decoded size changed during timing".to_string()); + } + } + decompression_samples + .push(raw_mebibytes / decompress_started.elapsed().as_secs_f64().max(1.0e-9)); + } + + Ok(BenchResult { + input_bytes: bytes.len(), + compressed_bytes: compressed_len, + compression_mbps: median(&mut compression_samples), + decompression_mbps: median(&mut decompression_samples), + }) +} + +fn print_result(label: &str, result: BenchResult) { + let ratio = result.compressed_bytes as f64 / result.input_bytes.max(1) as f64; + let saved = 100.0 * (1.0 - ratio); + println!( + "{label}: {} -> {} bytes | ratio {:.3}x | saved {:.1}% | compress {:.0} MiB/s | decompress {:.0} MiB/s", + result.input_bytes, + result.compressed_bytes, + ratio, + saved, + result.compression_mbps, + result.decompression_mbps, + ); +} + +fn bench_dump(path: &Path, options: BenchOptions) -> Result<(), String> { + let file_bytes = + fs::read(path).map_err(|err| format!("failed to read {}: {err}", path.display()))?; + println!("dump: {}", path.display()); + println!("lz4_impl: {}", implementation_name()); + println!( + "lz4_acceleration: {} | timed_mib: {} | samples: {}", + options.acceleration, options.timed_mib, options.samples + ); + print_result( + "file", + bench_buffer(&file_bytes, options.acceleration, options)?, + ); + + let Some(pair) = XrNetAlignmentDescriptorDumpPair::from_file_bytes(&file_bytes) else { + println!(); + return Ok(()); + }; + + let local_height_map = pair + .local_descriptor + .descriptor + .height_map + .as_ref() + .ok_or_else(|| "local descriptor has no height map".to_string())?; + let remote_height_map = pair + .remote_descriptor + .descriptor + .height_map + .as_ref() + .ok_or_else(|| "remote descriptor has no height map".to_string())?; + let local_height_bytes = height_bytes(local_height_map); + let remote_height_bytes = height_bytes(remote_height_map); + let mut combined_height_bytes = + Vec::::with_capacity(local_height_bytes.len() + remote_height_bytes.len()); + combined_height_bytes.extend_from_slice(&local_height_bytes); + combined_height_bytes.extend_from_slice(&remote_height_bytes); + print_result( + "local heights", + bench_buffer(&local_height_bytes, options.acceleration, options)?, + ); + print_result( + "remote heights", + bench_buffer(&remote_height_bytes, options.acceleration, options)?, + ); + print_result( + "combined heights", + bench_buffer(&combined_height_bytes, options.acceleration, options)?, + ); + println!(); + Ok(()) +} + +fn main() -> Result<(), String> { + let (options, paths) = parse_args()?; + for path in paths { + bench_dump(&path, options)?; + } + Ok(()) +} diff --git a/xr/src/bin/manual_align.rs b/xr/src/bin/manual_align.rs new file mode 100644 index 0000000000..9765a485f6 --- /dev/null +++ b/xr/src/bin/manual_align.rs @@ -0,0 +1,1096 @@ +pub use makepad_widgets; + +use makepad_widgets::makepad_draw::DrawVector; +use makepad_widgets::makepad_platform::{TextureFormat, TextureUpdated, XrDepthAlignHeightMap}; +use makepad_widgets::*; +use makepad_xr::net::*; +use std::{ + env, fs, + path::{Path, PathBuf}, +}; + +app_main!(App); + +script_mod! { + use mod.prelude.widgets.* + + let DrawHeightMap = set_type_default() do #(DrawHeightMap::script_shader(vm)){ + ..mod.draw.DrawQuad + height_texture: texture_2d(float) + alpha: 0.9 + uv_min: vec2(0.0, 0.0) + uv_max: vec2(1.0, 1.0) + tint: vec4(0.82, 0.94, 1.0, 1.0) + wall_band_start: 0.72 + + pixel: fn() { + let uv = self.uv_min + (self.uv_max - self.uv_min) * self.pos + let sample = self.height_texture.sample(uv).x + if sample <= 0.00001 { + return #0000 + } + let wall_mix = clamp( + (sample - self.wall_band_start) / max(1.0 - self.wall_band_start, 0.0001), + 0.0, + 1.0 + ) + let lifted = pow(sample, mix(1.24, 0.76, wall_mix)) + let base = self.tint.xyz * (0.18 + lifted * 0.82) + let wall = mix(base, self.tint.xyz, wall_mix * 0.75) + return Pal.premul(vec4(wall, self.alpha * self.tint.w)) + } + } + + let ManualAlignPreviewBase = #(ManualAlignPreview::register_widget(vm)) + let ManualAlignPreview = set_type_default() do ManualAlignPreviewBase{ + width: Fill + height: Fill + draw_bg +: { + color: #x09141d + } + } + + load_all_resources() do #(App::script_component(vm)) { + ui: Root{ + main_window := Window{ + window.inner_size: vec2(1440, 920) + pass.clear_color: #x0b1118 + body +: { + width: Fill + height: Fill + flow: Down + spacing: 12 + padding: Inset{top: 18 bottom: 18 left: 18 right: 18} + + RoundedView{ + width: Fill + height: Fit + flow: Down + spacing: 6 + padding: Inset{top: 16 bottom: 16 left: 16 right: 16} + draw_bg.color: #x16223c + draw_bg.border_radius: 12.0 + + Label{ + text: "XR Manual Align" + draw_text.color: #xfff + draw_text.text_style: theme.font_bold{font_size: 20} + } + + dump_path_label := Label{ + text: "Dump: loading" + draw_text.color: #xb9c5eb + draw_text.text_style: theme.font_regular{font_size: 10} + } + + save_status_label := Label{ + text: "Save: waiting" + draw_text.color: #x8fd7b7 + draw_text.text_style: theme.font_regular{font_size: 10} + } + } + + View{ + width: Fill + height: Fit + flow: Right + spacing: 10 + + shift_x_slider := Slider{ + width: Fill + text: "X Shift (m)" + min: -8.0 + max: 8.0 + step: 0.01 + precision: 1000 + default: 0.0 + } + + shift_y_slider := Slider{ + width: Fill + text: "Y Shift (m)" + min: -8.0 + max: 8.0 + step: 0.01 + precision: 1000 + default: 0.0 + } + + rotate_slider := Slider{ + width: Fill + text: "Rotate (deg)" + min: -180.0 + max: 180.0 + step: 0.1 + precision: 100 + default: 0.0 + } + } + + View{ + width: Fill + height: Fit + flow: Right + spacing: 10 + + band_min_slider := Slider{ + width: Fill + text: "Band Min (m)" + min: -0.20 + max: 2.20 + step: 0.01 + precision: 1000 + default: 0.00 + } + + band_max_slider := Slider{ + width: Fill + text: "Band Max (m)" + min: -0.20 + max: 2.20 + step: 0.01 + precision: 1000 + default: 2.00 + } + } + + View{ + width: Fill + height: Fit + flow: Right + spacing: 10 + align: Align{y: 0.5} + + save_button := Button{ + width: 110 + text: "Save" + draw_bg +: { + color: uniform(#x355fd1) + color_hover: uniform(#x4875ef) + color_down: uniform(#x284eb5) + border_radius: 8.0 + } + draw_text +: { + color: #xfff + text_style +: {font_size: 11} + } + } + + pose_label := Label{ + width: Fill + text: "Pose: x 0.00 | y 0.00 | rot 0.0 deg" + draw_text.color: #xffd29a + draw_text.text_style: theme.font_regular{font_size: 11} + } + } + + RoundedView{ + width: Fill + height: Fill + flow: Down + spacing: 8 + padding: Inset{top: 10 bottom: 10 left: 10 right: 10} + draw_bg.color: #x0d1424 + draw_bg.border_radius: 12.0 + + View{ + width: Fill + height: Fit + flow: Right + spacing: 10 + + Label{ + width: Fill + text: "Local" + draw_text.color: #x9ed5ff + draw_text.text_style: theme.font_bold{font_size: 11} + } + + Label{ + width: Fill + text: "Remote" + draw_text.color: #xffbb7e + draw_text.text_style: theme.font_bold{font_size: 11} + } + + Label{ + width: Fill + text: "Overlay" + draw_text.color: #xffe7a1 + draw_text.text_style: theme.font_bold{font_size: 11} + } + } + + preview := ManualAlignPreview{} + } + } + } + } + } +} + +#[derive(Script, ScriptHook)] +#[repr(C)] +pub struct DrawHeightMap { + #[deref] + draw_super: DrawQuad, + #[live] + alpha: f32, + #[live] + uv_min: Vec2f, + #[live] + uv_max: Vec2f, + #[live] + tint: Vec4f, + #[live] + wall_band_start: f32, +} + +#[derive(Clone, Copy, Debug, Default, PartialEq)] +struct ManualAlignPose { + shift_x_meters: f32, + shift_y_meters: f32, + rotation_radians: f32, +} + +impl ManualAlignPose { + fn rotation_degrees(self) -> f32 { + self.rotation_radians.to_degrees() + } + + fn to_mat4(self) -> Mat4f { + Pose::new( + Quat::from_axis_angle(vec3f(0.0, 1.0, 0.0), self.rotation_radians), + vec3(self.shift_x_meters, 0.0, self.shift_y_meters), + ) + .to_mat4() + } +} + +#[derive(Clone, Debug, Default)] +struct ManualAlignSidecar { + dump_file: String, + shift_x_meters: f32, + shift_y_meters: f32, + rotation_radians: f32, +} + +impl ManualAlignSidecar { + fn from_pose(dump_path: &Path, pose: ManualAlignPose) -> Self { + Self { + dump_file: dump_path.display().to_string(), + shift_x_meters: pose.shift_x_meters, + shift_y_meters: pose.shift_y_meters, + rotation_radians: pose.rotation_radians, + } + } + + fn pose(&self) -> ManualAlignPose { + ManualAlignPose { + shift_x_meters: self.shift_x_meters, + shift_y_meters: self.shift_y_meters, + rotation_radians: self.rotation_radians, + } + } + + fn to_text(&self) -> String { + format!( + "dump_file: {}\nshift_x_meters: {:.6}\nshift_y_meters: {:.6}\nrotation_radians: {:.6}\n", + self.dump_file, + self.shift_x_meters, + self.shift_y_meters, + self.rotation_radians + ) + } + + fn from_text(text: &str) -> Option { + let mut sidecar = Self::default(); + for line in text.lines() { + let (key, value) = line.split_once(':')?; + let value = value.trim(); + match key.trim() { + "dump_file" => sidecar.dump_file = value.to_string(), + "shift_x_meters" => sidecar.shift_x_meters = value.parse().ok()?, + "shift_y_meters" => sidecar.shift_y_meters = value.parse().ok()?, + "rotation_radians" => sidecar.rotation_radians = value.parse().ok()?, + _ => {} + } + } + Some(sidecar) + } +} + +#[derive(Clone)] +struct LoadedDump { + dump_path: PathBuf, + sidecar_path: PathBuf, + pair: XrNetAlignmentDescriptorDumpPair, +} + +#[derive(Clone, Copy, Debug, PartialEq)] +struct PreviewBounds { + min_x: f32, + max_x: f32, + min_z: f32, + max_z: f32, +} + +impl PreviewBounds { + fn span_x(self) -> f32 { + (self.max_x - self.min_x).max(1.0e-5) + } + + fn span_z(self) -> f32 { + (self.max_z - self.min_z).max(1.0e-5) + } + + fn expand(mut self, amount: f32) -> Self { + self.min_x -= amount; + self.max_x += amount; + self.min_z -= amount; + self.max_z += amount; + self + } +} + +#[derive(Clone, Copy, Debug, PartialEq)] +struct HeightBandPass { + min_meters: f32, + max_meters: f32, +} + +impl Default for HeightBandPass { + fn default() -> Self { + Self { + min_meters: 0.0, + max_meters: 2.0, + } + } +} + +#[derive(Script, ScriptHook, Widget)] +pub struct ManualAlignPreview { + #[uid] + uid: WidgetUid, + #[walk] + walk: Walk, + #[redraw] + #[live] + draw_bg: DrawQuad, + #[redraw] + #[live] + draw_map: DrawHeightMap, + #[redraw] + #[live] + draw_vector: DrawVector, + #[rust] + area: Area, + #[rust] + local_texture: Option, + #[rust] + remote_texture: Option, + #[rust] + local_height_map: Option, + #[rust] + transformed_remote_height_map: Option, + #[rust] + preview_bounds: Option, + #[rust] + band_pass: HeightBandPass, + #[rust(1.18)] + zoom: f32, +} + +impl ManualAlignPreview { + const PAD: f32 = 12.0; + const PANEL_GAP: f32 = 10.0; + const GRID_DIVISIONS: usize = 4; + const CUTOUT_STEPS: usize = 40; + + fn set_height_maps( + &mut self, + cx: &mut Cx, + local_height_map: Option, + transformed_remote_height_map: Option, + preview_bounds: Option, + band_pass: HeightBandPass, + ) { + if self.local_height_map == local_height_map + && self.transformed_remote_height_map == transformed_remote_height_map + && self.preview_bounds == preview_bounds + && self.band_pass == band_pass + { + return; + } + self.local_height_map = local_height_map; + self.transformed_remote_height_map = transformed_remote_height_map; + self.preview_bounds = preview_bounds; + self.band_pass = band_pass; + if self.local_height_map.is_none() { + self.local_texture = None; + } + if self.transformed_remote_height_map.is_none() { + self.remote_texture = None; + } + self.area.redraw(cx); + } + + fn preview_height_to_u8( + height_map: &XrDepthAlignHeightMap, + band_pass: HeightBandPass, + value: f32, + ) -> u8 { + if !value.is_finite() { + 0 + } else { + let relative_height = value - height_map.floor_y_meters; + if relative_height < band_pass.min_meters || relative_height > band_pass.max_meters { + return 0; + } + let span = (band_pass.max_meters - band_pass.min_meters).max(1.0e-5); + let normalized = ((relative_height - band_pass.min_meters) / span).clamp(0.0, 1.0); + 1 + (normalized * 254.0).round() as u8 + } + } + + fn ensure_height_texture( + texture_slot: &mut Option, + cx: &mut Cx, + height_map: &XrDepthAlignHeightMap, + band_pass: HeightBandPass, + ) { + let map_width = height_map.size_x as usize; + let map_height = height_map.size_z as usize; + if map_width == 0 + || map_height == 0 + || height_map.heights_meters.len() != map_width * map_height + { + *texture_slot = None; + return; + } + + let needs_recreate = texture_slot.as_ref().is_none_or(|texture| { + !matches!( + texture.get_format(cx), + TextureFormat::VecRu8 { width, height, .. } + if *width == map_width && *height == map_height + ) + }); + + let pixels = height_map + .heights_meters + .iter() + .map(|value| Self::preview_height_to_u8(height_map, band_pass, *value)) + .collect::>(); + + if needs_recreate { + *texture_slot = Some(Texture::new_with_format( + cx, + TextureFormat::VecRu8 { + width: map_width, + height: map_height, + data: Some(pixels), + unpack_row_length: None, + updated: TextureUpdated::Full, + }, + )); + return; + } + + if let Some(texture) = texture_slot.as_ref() { + let mut data = texture.take_vec_u8(cx); + if data.len() != pixels.len() { + data.resize(pixels.len(), 0); + } + data.copy_from_slice(&pixels); + texture.put_back_vec_u8(cx, data, None); + } + } + + fn map_bounds(height_map: &XrDepthAlignHeightMap) -> Option { + let span_x = height_map.extent_x_meters(); + let span_z = height_map.extent_z_meters(); + if span_x <= 1.0e-5 || span_z <= 1.0e-5 { + return None; + } + Some(PreviewBounds { + min_x: height_map.origin_x, + max_x: height_map.origin_x + span_x, + min_z: height_map.origin_z, + max_z: height_map.origin_z + span_z, + }) + } + + fn preview_rect(rect: Rect, bounds: PreviewBounds, zoom: f32) -> Rect { + let available_w = (rect.size.x as f32 - Self::PAD * 2.0).max(1.0); + let available_h = (rect.size.y as f32 - Self::PAD * 2.0).max(1.0); + let scale = (available_w / bounds.span_x()).min(available_h / bounds.span_z()) + * zoom.max(0.2).max(1.0e-5); + let draw_w = bounds.span_x() * scale; + let draw_h = bounds.span_z() * scale; + Rect { + pos: dvec2( + (rect.pos.x as f32 + (rect.size.x as f32 - draw_w) * 0.5) as f64, + (rect.pos.y as f32 + (rect.size.y as f32 - draw_h) * 0.5) as f64, + ), + size: dvec2(draw_w as f64, draw_h as f64), + } + } + + fn split_panel_rects(rect: Rect) -> [Rect; 3] { + let panel_gap = Self::PANEL_GAP; + let inner_w = (rect.size.x as f32 - panel_gap * 2.0).max(3.0); + let panel_w = inner_w / 3.0; + [ + Rect { + pos: rect.pos, + size: dvec2(panel_w as f64, rect.size.y), + }, + Rect { + pos: dvec2(rect.pos.x + (panel_w + panel_gap) as f64, rect.pos.y), + size: dvec2(panel_w as f64, rect.size.y), + }, + Rect { + pos: dvec2( + rect.pos.x + ((panel_w + panel_gap) * 2.0) as f64, + rect.pos.y, + ), + size: dvec2(panel_w as f64, rect.size.y), + }, + ] + } + + fn world_to_preview(bounds: PreviewBounds, preview_rect: Rect, point: Vec2f) -> (f32, f32) { + let nx = (point.x - bounds.min_x) / bounds.span_x(); + let nz = (point.y - bounds.min_z) / bounds.span_z(); + ( + preview_rect.pos.x as f32 + nx * preview_rect.size.x as f32, + preview_rect.pos.y as f32 + nz * preview_rect.size.y as f32, + ) + } + + fn height_map_draw_rect( + bounds: PreviewBounds, + preview_rect: Rect, + height_map: &XrDepthAlignHeightMap, + ) -> Rect { + let (left, top) = Self::world_to_preview( + bounds, + preview_rect, + vec2f(height_map.origin_x, height_map.origin_z), + ); + let (right, bottom) = Self::world_to_preview( + bounds, + preview_rect, + vec2f( + height_map.origin_x + height_map.extent_x_meters(), + height_map.origin_z + height_map.extent_z_meters(), + ), + ); + Rect { + pos: dvec2(left as f64, top as f64), + size: dvec2( + (right - left).max(1.0) as f64, + (bottom - top).max(1.0) as f64, + ), + } + } + + fn draw_grid(&mut self, preview_rect: Rect) { + let ox = preview_rect.pos.x as f32; + let oy = preview_rect.pos.y as f32; + let w = preview_rect.size.x as f32; + let h = preview_rect.size.y as f32; + self.draw_vector.set_color_hex(0x163042, 1.0); + for step in 0..=Self::GRID_DIVISIONS { + let t = step as f32 / Self::GRID_DIVISIONS as f32; + let px = ox + w * t; + let py = oy + h * t; + self.draw_vector.move_to(px, oy); + self.draw_vector.line_to(px, oy + h); + self.draw_vector.move_to(ox, py); + self.draw_vector.line_to(ox + w, py); + } + self.draw_vector.stroke(1.0); + } + + fn draw_origin_cross(&mut self, bounds: PreviewBounds, preview_rect: Rect) { + if bounds.min_x > 0.0 || bounds.max_x < 0.0 || bounds.min_z > 0.0 || bounds.max_z < 0.0 { + return; + } + let (cx, cy) = Self::world_to_preview(bounds, preview_rect, vec2f(0.0, 0.0)); + self.draw_vector.set_color_hex(0xffcf6a, 1.0); + self.draw_vector.move_to(cx - 6.0, cy); + self.draw_vector.line_to(cx + 6.0, cy); + self.draw_vector.move_to(cx, cy - 6.0); + self.draw_vector.line_to(cx, cy + 6.0); + self.draw_vector.stroke(1.2); + } + + fn draw_cutout_ring( + &mut self, + bounds: PreviewBounds, + preview_rect: Rect, + height_map: &XrDepthAlignHeightMap, + color_hex: u32, + ) { + let Some(center) = height_map.player_cutout_center else { + return; + }; + let (cx, cy) = Self::world_to_preview(bounds, preview_rect, center); + let scale = preview_rect.size.x as f32 / bounds.span_x(); + let radius = height_map.player_cutout_radius_meters * scale; + self.draw_vector.set_color_hex(color_hex, 1.0); + for step in 0..=Self::CUTOUT_STEPS { + let angle = step as f32 / Self::CUTOUT_STEPS as f32 * std::f32::consts::TAU; + let px = cx + angle.cos() * radius; + let py = cy + angle.sin() * radius; + if step == 0 { + self.draw_vector.move_to(px, py); + } else { + self.draw_vector.line_to(px, py); + } + } + self.draw_vector.stroke(1.2); + } + + fn draw_height_map_rect( + &mut self, + cx: &mut Cx2d, + texture: &Texture, + rect: Rect, + alpha: f32, + tint: Vec4f, + ) { + self.draw_map.alpha = alpha; + self.draw_map.tint = tint; + self.draw_map.uv_min = vec2f(0.0, 0.0); + self.draw_map.uv_max = vec2f(1.0, 1.0); + self.draw_map.draw_vars.set_texture(0, texture); + self.draw_map.draw_abs(cx, rect); + } + + fn draw_panel_contents( + &mut self, + cx: &mut Cx2d, + panel_rect: Rect, + bounds: PreviewBounds, + draw_local: bool, + draw_remote: bool, + local_height_map: Option<&XrDepthAlignHeightMap>, + remote_height_map: Option<&XrDepthAlignHeightMap>, + ) { + let preview_rect = Self::preview_rect(panel_rect, bounds, self.zoom); + if draw_local { + if let Some(local_height_map) = local_height_map { + Self::ensure_height_texture( + &mut self.local_texture, + cx.cx, + local_height_map, + self.band_pass, + ); + if let Some(texture) = self.local_texture.as_ref().cloned() { + self.draw_height_map_rect( + cx, + &texture, + Self::height_map_draw_rect(bounds, preview_rect, local_height_map), + 0.92, + vec4f(0.74, 0.96, 1.0, 0.92), + ); + } + } + } + if draw_remote { + if let Some(remote_height_map) = remote_height_map { + Self::ensure_height_texture( + &mut self.remote_texture, + cx.cx, + remote_height_map, + self.band_pass, + ); + if let Some(texture) = self.remote_texture.as_ref().cloned() { + self.draw_height_map_rect( + cx, + &texture, + Self::height_map_draw_rect(bounds, preview_rect, remote_height_map), + if draw_local { 0.74 } else { 0.90 }, + vec4f(1.0, 0.62, 0.24, if draw_local { 0.78 } else { 0.92 }), + ); + } + } + } + + self.draw_vector.begin(); + self.draw_grid(preview_rect); + self.draw_origin_cross(bounds, preview_rect); + if draw_local { + if let Some(local_height_map) = local_height_map { + self.draw_cutout_ring(bounds, preview_rect, local_height_map, 0x9ed5ff); + } + } + if draw_remote { + if let Some(remote_height_map) = remote_height_map { + self.draw_cutout_ring(bounds, preview_rect, remote_height_map, 0xffbb7e); + } + } + self.draw_vector.end(cx); + } +} + +impl Widget for ManualAlignPreview { + fn draw_walk(&mut self, cx: &mut Cx2d, _scope: &mut Scope, walk: Walk) -> DrawStep { + let rect = cx.walk_turtle(walk); + self.draw_bg.draw_abs(cx, rect); + self.area = self.draw_bg.area(); + + let local_height_map = self.local_height_map.clone(); + let transformed_remote_height_map = self.transformed_remote_height_map.clone(); + let Some(bounds) = self.preview_bounds else { + return DrawStep::done(); + }; + let [local_rect, remote_rect, overlay_rect] = Self::split_panel_rects(rect); + self.draw_panel_contents( + cx, + local_rect, + bounds, + true, + false, + local_height_map.as_ref(), + transformed_remote_height_map.as_ref(), + ); + self.draw_panel_contents( + cx, + remote_rect, + bounds, + false, + true, + local_height_map.as_ref(), + transformed_remote_height_map.as_ref(), + ); + self.draw_panel_contents( + cx, + overlay_rect, + bounds, + true, + true, + local_height_map.as_ref(), + transformed_remote_height_map.as_ref(), + ); + DrawStep::done() + } + + fn handle_event(&mut self, cx: &mut Cx, event: &Event, _scope: &mut Scope) { + match event.hits(cx, self.area) { + Hit::FingerScroll(fs) => { + let scroll = if fs.scroll.y.abs() > f64::EPSILON { + fs.scroll.y + } else { + fs.scroll.x + }; + let factor = if scroll > 0.0 { 1.10 } else { 1.0 / 1.10 }; + self.zoom = (self.zoom * factor).clamp(0.45, 8.0); + self.area.redraw(cx); + } + _ => { + if matches!(event, Event::Signal) { + self.area.redraw(cx); + } + } + } + } +} + +#[derive(Script, ScriptHook)] +pub struct App { + #[live] + ui: WidgetRef, + #[rust] + loaded_dump: Option, + #[rust] + pose: ManualAlignPose, + #[rust] + band_pass: HeightBandPass, +} + +impl App { + fn initial_dump_path() -> Option { + env::args_os() + .nth(1) + .map(PathBuf::from) + .or_else(Self::latest_dump_path) + } + + fn latest_dump_path() -> Option { + let mut entries = fs::read_dir("xr/dump/dumps") + .ok()? + .filter_map(|entry| entry.ok()) + .filter_map(|entry| { + let metadata = entry.metadata().ok()?; + metadata + .is_file() + .then_some((entry.path(), metadata.modified().ok()?)) + }) + .collect::>(); + entries.sort_by(|left, right| right.1.cmp(&left.1)); + entries.into_iter().find_map(|(path, _)| { + let name = path.file_name()?.to_str()?; + (name.ends_with(".bin") && name != "manual-smoke.bin").then_some(path) + }) + } + + fn sidecar_path_for_dump(dump_path: &Path) -> PathBuf { + let stem = dump_path + .file_stem() + .and_then(|stem| stem.to_str()) + .unwrap_or("align-pair"); + dump_path.with_file_name(format!("{stem}.manual_pose.ron")) + } + + fn load_sidecar_pose(sidecar_path: &Path) -> Option { + let text = fs::read_to_string(sidecar_path).ok()?; + ManualAlignSidecar::from_text(&text).map(|sidecar| sidecar.pose()) + } + + fn load_dump_pair(path: &Path) -> Result { + let bytes = + fs::read(path).map_err(|err| format!("failed to read {}: {err}", path.display()))?; + let pair = XrNetAlignmentDescriptorDumpPair::from_file_bytes(&bytes) + .ok_or_else(|| format!("failed to decode {}", path.display()))?; + Ok(LoadedDump { + dump_path: path.to_path_buf(), + sidecar_path: Self::sidecar_path_for_dump(path), + pair, + }) + } + + fn current_preview_bounds(&self) -> Option { + let loaded_dump = self.loaded_dump.as_ref()?; + let local_bounds = loaded_dump + .pair + .local_descriptor + .descriptor + .height_map + .as_ref() + .and_then(ManualAlignPreview::map_bounds); + let remote_height_map = loaded_dump + .pair + .remote_descriptor + .descriptor + .height_map + .as_ref()?; + let remote_extent_x = remote_height_map.extent_x_meters(); + let remote_extent_z = remote_height_map.extent_z_meters(); + if remote_extent_x <= 1.0e-5 || remote_extent_z <= 1.0e-5 { + return local_bounds.map(|bounds| bounds.expand(0.12)); + } + let remote_center = vec3( + remote_height_map.origin_x + remote_extent_x * 0.5, + 0.0, + remote_height_map.origin_z + remote_extent_z * 0.5, + ); + let transformed_center = self + .pose + .to_mat4() + .transform_vec4(vec4f(remote_center.x, 0.0, remote_center.z, 1.0)) + .to_vec3f(); + let remote_radius = + (remote_extent_x * remote_extent_x + remote_extent_z * remote_extent_z).sqrt() * 0.5; + let remote_bounds = PreviewBounds { + min_x: transformed_center.x - remote_radius, + max_x: transformed_center.x + remote_radius, + min_z: transformed_center.z - remote_radius, + max_z: transformed_center.z + remote_radius, + }; + let mut bounds = local_bounds.unwrap_or(remote_bounds); + bounds.min_x = bounds.min_x.min(remote_bounds.min_x); + bounds.max_x = bounds.max_x.max(remote_bounds.max_x); + bounds.min_z = bounds.min_z.min(remote_bounds.min_z); + bounds.max_z = bounds.max_z.max(remote_bounds.max_z); + Some(bounds.expand(0.12)) + } + + fn update_dump_label(&self, cx: &mut Cx, text: String) { + self.ui.label(cx, ids!(dump_path_label)).set_text(cx, &text); + } + + fn update_save_label(&self, cx: &mut Cx, text: String) { + self.ui + .label(cx, ids!(save_status_label)) + .set_text(cx, &text); + } + + fn update_pose_label(&self, cx: &mut Cx) { + self.ui.label(cx, ids!(pose_label)).set_text( + cx, + &format!( + "Pose: x {:.3} | y {:.3} | rot {:.1} deg | band {:.2}..{:.2} m", + self.pose.shift_x_meters, + self.pose.shift_y_meters, + self.pose.rotation_degrees(), + self.band_pass.min_meters, + self.band_pass.max_meters, + ), + ); + } + + fn set_slider_value(&self, cx: &mut Cx, id: LiveId, value: f64) { + if let Some(mut slider) = self.ui.widget(cx, &[id]).borrow_mut::() { + slider.set_value(cx, value); + } + } + + fn slider_action_value(&self, cx: &mut Cx, actions: &Actions, id: LiveId) -> Option { + let widget = self.ui.widget(cx, &[id]); + let action = actions.find_widget_action(widget.widget_uid())?; + match action.cast() { + SliderAction::TextSlide(v) | SliderAction::Slide(v) | SliderAction::EndSlide(v) => { + Some(v) + } + _ => None, + } + } + + fn sync_pose_to_ui(&self, cx: &mut Cx) { + self.set_slider_value(cx, id!(shift_x_slider), self.pose.shift_x_meters as f64); + self.set_slider_value(cx, id!(shift_y_slider), self.pose.shift_y_meters as f64); + self.set_slider_value(cx, id!(rotate_slider), self.pose.rotation_degrees() as f64); + self.set_slider_value(cx, id!(band_min_slider), self.band_pass.min_meters as f64); + self.set_slider_value(cx, id!(band_max_slider), self.band_pass.max_meters as f64); + self.update_pose_label(cx); + } + + fn current_transformed_remote_height_map(&self) -> Option { + let loaded_dump = self.loaded_dump.as_ref()?; + let transformed = loaded_dump + .pair + .remote_descriptor + .descriptor + .transformed(&self.pose.to_mat4()); + transformed.height_map + } + + fn refresh_preview(&mut self, cx: &mut Cx) { + let local_height_map = self + .loaded_dump + .as_ref() + .and_then(|loaded| loaded.pair.local_descriptor.descriptor.height_map.clone()); + let remote_height_map = self.current_transformed_remote_height_map(); + let preview_bounds = self.current_preview_bounds(); + if let Some(mut preview) = self + .ui + .widget(cx, ids!(preview)) + .borrow_mut::() + { + preview.set_height_maps( + cx, + local_height_map, + remote_height_map, + preview_bounds, + self.band_pass, + ); + } + self.update_pose_label(cx); + } + + fn load_initial_dump(&mut self, cx: &mut Cx) { + let Some(path) = Self::initial_dump_path() else { + self.update_dump_label(cx, "Dump: no dump files found".to_string()); + self.update_save_label(cx, "Save: waiting for dump".to_string()); + return; + }; + match Self::load_dump_pair(&path) { + Ok(loaded_dump) => { + self.pose = Self::load_sidecar_pose(&loaded_dump.sidecar_path).unwrap_or_default(); + self.loaded_dump = Some(loaded_dump.clone()); + self.update_dump_label(cx, format!("Dump: {}", loaded_dump.dump_path.display())); + self.update_save_label(cx, format!("Save: {}", loaded_dump.sidecar_path.display())); + self.sync_pose_to_ui(cx); + self.refresh_preview(cx); + } + Err(err) => { + self.update_dump_label(cx, format!("Dump: {err}")); + self.update_save_label(cx, "Save: waiting for dump".to_string()); + } + } + } + + fn save_pose(&mut self, cx: &mut Cx) { + let Some(loaded_dump) = self.loaded_dump.as_ref() else { + self.update_save_label(cx, "Save: no dump loaded".to_string()); + return; + }; + let sidecar = ManualAlignSidecar::from_pose(&loaded_dump.dump_path, self.pose); + match fs::write(&loaded_dump.sidecar_path, sidecar.to_text().as_bytes()) { + Ok(()) => { + self.update_save_label( + cx, + format!("Save: wrote {}", loaded_dump.sidecar_path.display()), + ); + } + Err(err) => { + self.update_save_label( + cx, + format!( + "Save: failed to write {}: {err}", + loaded_dump.sidecar_path.display() + ), + ); + } + } + } +} + +impl MatchEvent for App { + fn handle_startup(&mut self, cx: &mut Cx) { + self.load_initial_dump(cx); + } + + fn handle_actions(&mut self, cx: &mut Cx, actions: &Actions) { + let mut changed = false; + if let Some(value) = self.slider_action_value(cx, actions, id!(shift_x_slider)) { + self.pose.shift_x_meters = value as f32; + changed = true; + } + if let Some(value) = self.slider_action_value(cx, actions, id!(shift_y_slider)) { + self.pose.shift_y_meters = value as f32; + changed = true; + } + if let Some(value) = self.slider_action_value(cx, actions, id!(rotate_slider)) { + self.pose.rotation_radians = (value as f32).to_radians(); + changed = true; + } + if let Some(value) = self.slider_action_value(cx, actions, id!(band_min_slider)) { + self.band_pass.min_meters = value as f32; + if self.band_pass.max_meters < self.band_pass.min_meters + 0.05 { + self.band_pass.max_meters = self.band_pass.min_meters + 0.05; + } + changed = true; + } + if let Some(value) = self.slider_action_value(cx, actions, id!(band_max_slider)) { + self.band_pass.max_meters = value as f32; + if self.band_pass.min_meters > self.band_pass.max_meters - 0.05 { + self.band_pass.min_meters = self.band_pass.max_meters - 0.05; + } + changed = true; + } + if changed { + self.sync_pose_to_ui(cx); + self.refresh_preview(cx); + } + if self.ui.button(cx, ids!(save_button)).clicked(actions) { + self.save_pose(cx); + } + } +} + +impl AppMain for App { + fn script_mod(vm: &mut ScriptVm) -> ScriptValue { + crate::makepad_widgets::script_mod(vm); + self::script_mod(vm) + } + + fn handle_event(&mut self, cx: &mut Cx, event: &Event) { + self.match_event(cx, event); + self.ui.handle_event(cx, event, &mut Scope::empty()); + } +} diff --git a/xr/src/lib.rs b/xr/src/lib.rs index f2b282d3cf..929379e8d0 100644 --- a/xr/src/lib.rs +++ b/xr/src/lib.rs @@ -1,87 +1,44 @@ pub use makepad_widgets; pub use makepad_widgets::*; -#[path = "obj/cube.rs"] -pub mod cube; -#[path = "obj/gltf.rs"] -pub mod gltf; -#[path = "util/gltf_bridge.rs"] -pub mod gltf_bridge; -#[path = "obj/icosphere.rs"] -pub mod icosphere; -#[path = "util/mesh_generators.rs"] -pub mod mesh_generators; -#[path = "util/passthrough_env.rs"] -pub mod passthrough_env; -#[path = "obj/physics_view.rs"] -pub mod physics_view; -#[path = "obj/refractive_cube.rs"] -pub mod refractive_cube; -#[path = "util/scene_draw.rs"] -mod scene_draw; -#[path = "obj/shooter.rs"] -pub mod shooter; -#[path = "obj/tree.rs"] -pub mod tree; -#[path = "obj/view_splat.rs"] -pub mod view_splat; -#[path = "scene/xr_body_spawn.rs"] -pub mod xr_body_spawn; -#[path = "scene/xr_env.rs"] -pub mod xr_env; -#[path = "xr_net.rs"] -pub mod xr_net; -#[path = "scene/xr_node.rs"] -pub mod xr_node; -#[path = "scene/xr_people_debug.rs"] -pub mod xr_people_debug; -#[path = "scene/xr_permissions_flow.rs"] -pub mod xr_permissions_flow; -#[path = "scene/xr_root.rs"] -pub mod xr_root; -#[path = "scene/xr_select.rs"] -pub mod xr_select; -#[path = "scene/xr_view.rs"] -pub mod xr_view; +pub mod algorithms; +pub mod net; +pub mod obj; +pub mod scene; +pub mod sync; +mod util; -pub use cube::*; -pub use gltf::*; -pub use gltf_bridge::*; -pub use icosphere::*; -pub use passthrough_env::DrawPassthroughEnvFace; -pub use physics_view::*; -pub use refractive_cube::*; -pub use shooter::*; -pub use tree::{ - CpuPythagoreanTree, DrawTreeBranches, DrawTreeLeaves, Tree, PYTHAGOREAN_TREE_ROOT_DROP, -}; -pub use view_splat::*; -pub use xr_body_spawn::*; -pub use xr_env::XrEnv; -pub use xr_net::*; -pub use xr_node::*; -pub use xr_people_debug::XrPeopleDebug; -pub use xr_permissions_flow::*; -pub use xr_root::XrRoot; -pub use xr_select::XrSelect; -pub use xr_view::XrView; +pub mod render { + pub use crate::util::gltf_bridge::{ + GltfDecodedMeshes, GltfDecodedPrimitiveObject, GltfDefaultView, GltfDrawObject, + GltfMaterialState, GltfMeshObjects, GltfPrimitiveObject, GltfRenderer, + }; + pub use crate::util::passthrough_env::DrawPassthroughEnvFace; +} + +pub(crate) mod prelude { + pub use crate::algorithms::depth_align::*; + pub use crate::{net::*, render::*, scene::*}; + pub use makepad_widgets::*; +} pub fn script_mod(vm: &mut ScriptVm) -> ScriptValue { - xr_node::script_mod(vm); - gltf::script_mod(vm); - icosphere::script_mod(vm); - cube::script_mod(vm); - xr_permissions_flow::script_mod(vm); - physics_view::script_mod(vm); - refractive_cube::script_mod(vm); - shooter::script_mod(vm); + scene::xr_node::script_mod(vm); + obj::gltf::script_mod(vm); + obj::icosphere::script_mod(vm); + obj::cube::script_mod(vm); + scene::xr_permissions_flow::script_mod(vm); + obj::physics_view::script_mod(vm); + obj::refractive_cube::script_mod(vm); + obj::shooter::script_mod(vm); + obj::tank::script_mod(vm); - passthrough_env::script_mod(vm); - tree::script_mod(vm); - view_splat::script_mod(vm); - xr_env::script_mod(vm); - xr_people_debug::script_mod(vm); - xr_select::script_mod(vm); - xr_view::script_mod(vm); - xr_root::script_mod(vm) + util::passthrough_env::script_mod(vm); + obj::tree::script_mod(vm); + obj::view_splat::script_mod(vm); + scene::xr_env::script_mod(vm); + sync::xr_peer_sync::script_mod(vm); + scene::xr_select::script_mod(vm); + scene::xr_view::script_mod(vm); + scene::xr_root::script_mod(vm) } diff --git a/xr/src/net/mod.rs b/xr/src/net/mod.rs new file mode 100644 index 0000000000..9f0c035862 --- /dev/null +++ b/xr/src/net/mod.rs @@ -0,0 +1,1511 @@ +use crate::prelude::*; +use makepad_widgets::makepad_platform::makepad_micro_serde::*; +use std::{ + io, + net::{IpAddr, Ipv4Addr, SocketAddr, TcpListener, UdpSocket}, + sync::{ + atomic::{AtomicBool, Ordering}, + mpsc, Arc, + }, + thread::JoinHandle, + time::{Duration, SystemTime, UNIX_EPOCH}, +}; + +mod xr_net_protocol; +mod xr_net_transport; + +use self::{ + xr_net_protocol::*, + xr_net_transport::{ + spawn_sync_worker_thread, spawn_udp_worker_thread, XrNetSyncOutgoing, XrNetUdpOutgoing, + }, +}; + +pub const XR_NET_PROTOCOL_VERSION: u16 = 11; +pub const XR_NET_DEFAULT_DISCOVERY_PORT: u16 = 41546; +pub const XR_NET_DEFAULT_DATA_PORT: u16 = 41547; +pub const XR_NET_DEFAULT_SYNC_PORT: u16 = 41548; + +const XR_NET_DEFAULT_DISCOVERY_INTERVAL: Duration = Duration::from_millis(100); +const XR_NET_DEFAULT_POLL_INTERVAL: Duration = Duration::from_millis(10); +const XR_NET_DEFAULT_PEER_TIMEOUT: Duration = Duration::from_secs(2); +const XR_NET_DEFAULT_SYNC_CONNECT_RETRY: Duration = Duration::from_millis(250); +const XR_NET_SYNC_CONNECT_TIMEOUT: Duration = Duration::from_millis(120); +const XR_NET_SYNC_READ_BUDGET_BYTES_PER_POLL: usize = 256 * 1024; +const XR_NET_SYNC_WRITE_BUDGET_BYTES_PER_POLL: usize = 256 * 1024; +const XR_NET_SYNC_LZ4_ACCELERATION: usize = 4; +const XR_NET_SYNC_FRAME_RAW_TAG: u8 = 0; +const XR_NET_SYNC_FRAME_LZ4_TAG: u8 = 1; +const XR_NET_UDP_SHARED_OBJECT_STATE_BATCH_MAX_BYTES: usize = 1200; +// Alignment descriptors now carry full f32 heightmaps, so the old 256 KiB cap +// is too small once the published map grows beyond modest extents. +const XR_NET_SYNC_MAX_FRAME_BYTES: usize = 4 * 1024 * 1024; + +#[derive(Clone, Copy, Debug, Default, PartialEq, Eq, Hash, SerBin, DeBin)] +pub struct XrNetPeerId(pub u64); + +impl XrNetPeerId { + pub fn to_live_id(self) -> LiveId { + LiveId(self.0) + } +} + +#[derive(Clone, Copy, Debug, Default, PartialEq, Eq, Hash, SerBin, DeBin)] +pub struct XrActivityId(pub LiveId); + +impl XrActivityId { + pub fn to_live_id(self) -> LiveId { + self.0 + } +} + +#[derive(Clone, Copy, Debug, Default, PartialEq, Eq, Hash, SerBin, DeBin)] +pub struct XrSpawnableObjectId(pub u64); + +#[derive(Clone, Copy, Debug, Default, PartialEq, Eq, Hash, SerBin, DeBin)] +pub struct XrSharedObjectCounter(pub u64); + +#[derive(Clone, Copy, Debug, Default, PartialEq, Eq, Hash, SerBin, DeBin)] +pub struct XrSharedObjectId(pub u64); + +impl XrSharedObjectId { + pub fn to_live_id(self) -> LiveId { + LiveId(self.0) + } +} + +pub fn xr_make_shared_object_id( + peer_id: XrNetPeerId, + counter: XrSharedObjectCounter, +) -> Option { + let live_id = peer_id.to_live_id().bytes_append(&counter.0.to_be_bytes()); + Some(XrSharedObjectId(if live_id.0 == 0 { 1 } else { live_id.0 })) +} + +#[derive(Clone, Copy, Debug, PartialEq, Eq, Hash)] +pub struct XrNetPeer { + pub id: XrNetPeerId, + pub addr: SocketAddr, +} + +impl XrNetPeer { + pub fn to_live_id(self) -> LiveId { + self.id.to_live_id() + } +} + +#[derive(Clone, Copy, Debug, PartialEq, Eq)] +pub enum XrNetLeaveReason { + Explicit, + Timeout, +} + +#[derive(Clone, Debug, SerBin, DeBin)] +pub struct XrNetStateFrame { + pub seq: u32, + pub sent_at: f64, + pub state: XrState, +} + +#[derive(Clone, Copy, Debug, Default, PartialEq, SerBin, DeBin)] +pub struct XrNetBodySpawn { + pub activity_id: XrActivityId, + pub object_id: XrSpawnableObjectId, + pub pose: Pose, + pub linvel: Vec3f, + pub angvel: Vec3f, +} + +#[derive(Clone, Copy, Debug, Default, PartialEq, Eq, SerBin, DeBin)] +pub enum XrSharedHand { + LeftHand, + RightHand, + LeftController, + RightController, + #[default] + Unknown, +} + +#[derive(Clone, Copy, Debug, Default, PartialEq, Eq, SerBin, DeBin)] +pub enum XrSharedObjectFidelity { + #[default] + Interpolated, + ContactPredictive, + ImpactCritical, +} + +#[derive(Clone, Copy, Debug, Default, PartialEq, Eq, SerBin, DeBin)] +pub enum XrSharedObjectMode { + #[default] + Dynamic, + ContactDominated { + authority: XrNetPeerId, + hand: XrSharedHand, + }, + Sleeping, +} + +#[derive(Clone, Copy, Debug, PartialEq, Eq, SerBin, DeBin)] +pub enum XrSharedObjectShape { + ActivitySpawnable { + activity_id: XrActivityId, + spawnable_id: XrSpawnableObjectId, + }, +} + +impl Default for XrSharedObjectShape { + fn default() -> Self { + Self::ActivitySpawnable { + activity_id: XrActivityId(LiveId(0)), + spawnable_id: XrSpawnableObjectId(0), + } + } +} + +#[derive(Clone, Copy, Debug, Default, PartialEq, SerBin, DeBin)] +pub struct XrNetSharedObjectState { + pub seq: u32, + pub sent_at: f64, + pub physics_tick: u32, + pub object_id: XrSharedObjectId, + pub epoch: u32, + pub authority: XrNetPeerId, + pub fidelity: XrSharedObjectFidelity, + pub mode: XrSharedObjectMode, + pub pose: Pose, + pub linvel: Vec3f, + pub angvel: Vec3f, +} + +#[derive(Clone, Debug, PartialEq, SerBin, DeBin)] +pub enum XrNetSharedObjectControl { + XrSpawnObject { + object_id: XrSharedObjectId, + epoch: u32, + authority: XrNetPeerId, + fidelity: XrSharedObjectFidelity, + shape: XrSharedObjectShape, + pose: Pose, + linvel: Vec3f, + angvel: Vec3f, + }, + XrDespawnObject { + object_id: XrSharedObjectId, + epoch: u32, + }, + XrTakeoverRequest { + object_id: XrSharedObjectId, + epoch: u32, + request_id: u32, + based_on_seq: u32, + based_on_tick: u32, + candidate_owner: XrNetPeerId, + hand: XrSharedHand, + hand_pose: Pose, + hand_linvel: Vec3f, + }, + XrTakeoverAccept { + object_id: XrSharedObjectId, + epoch: u32, + request_id: u32, + new_authority: XrNetPeerId, + effective_at: f64, + effective_tick: u32, + pose: Pose, + linvel: Vec3f, + angvel: Vec3f, + }, + XrTakeoverReject { + object_id: XrSharedObjectId, + epoch: u32, + request_id: u32, + authoritative_seq: u32, + authoritative_tick: u32, + }, + XrContactImpulse { + object_id: XrSharedObjectId, + epoch: u32, + based_on_seq: u32, + based_on_tick: u32, + hand: XrSharedHand, + hand_pose: Pose, + point: Vec3f, + impulse: Vec3f, + }, + XrResetObject { + object_id: XrSharedObjectId, + epoch: u32, + pose: Pose, + linvel: Vec3f, + angvel: Vec3f, + }, + XrResetActivityPose { + activity_id: XrActivityId, + pose: Pose, + }, + XrClockPing { + seq: u32, + sent_at: f64, + }, + XrClockPong { + seq: u32, + echoed_at: f64, + replied_at: f64, + }, +} + +#[derive(Clone, Copy, Debug, Default, PartialEq, SerBin, DeBin)] +pub struct XrNetActivityState { + pub activity_id: XrActivityId, + pub changed_at: f64, + pub changed_tick: u32, + pub changed_by: XrNetPeerId, +} + +impl XrNetActivityState { + pub fn is_newer_than(&self, other: &Self) -> bool { + use std::cmp::Ordering; + + match self.changed_tick.cmp(&other.changed_tick) { + Ordering::Greater => true, + Ordering::Less => false, + Ordering::Equal => match self.changed_at.total_cmp(&other.changed_at) { + Ordering::Greater => true, + Ordering::Less => false, + Ordering::Equal => self.changed_by.0 > other.changed_by.0, + }, + } + } +} + +#[derive(Clone, Debug, PartialEq, SerBin, DeBin)] +pub enum XrNetActivityControl { + XrSetActivity(XrNetActivityState), +} + +impl XrNetActivityControl { + pub fn state(&self) -> XrNetActivityState { + match self { + Self::XrSetActivity(state) => *state, + } + } +} + +#[derive(Clone, Copy, Debug, Default, PartialEq, SerBin, DeBin)] +pub struct XrNetAlignmentFrame { + pub seq: u32, + pub sent_at: f64, + pub anchor: XrAnchor, + pub confidence: f32, +} + +impl XrNetAlignmentFrame { + pub fn remote_to_local_transform( + local: &XrNetAlignmentFrame, + remote: &XrNetAlignmentFrame, + ) -> Option { + if local.confidence <= 0.0 || remote.confidence <= 0.0 { + return None; + } + Some(Mat4f::mul( + &local.anchor.to_pose().to_mat4(), + &remote.anchor.to_pose().to_mat4().invert(), + )) + } +} + +#[derive(Clone, Debug, Default, PartialEq, SerBin, DeBin)] +pub struct XrNetAlignmentDescriptorFrame { + pub seq: u32, + pub sent_at: f64, + pub descriptor: XrDepthAlignDescriptor, +} + +impl XrNetAlignmentDescriptorFrame { + pub fn from_tsdf_snapshot(snapshot: &TsdfPublishedSnapshot, sent_at: f64) -> Option { + let height_map = snapshot.height_map.clone()?; + let floor_y = height_map.floor_y_meters; + Some(Self { + seq: 0, + sent_at, + descriptor: XrDepthAlignDescriptor { + voxel_size_meters: snapshot.grid.voxel_size, + floor_y, + wall_normal_histogram: Vec::new(), + samples: Vec::new(), + vertical_descriptor: None, + height_map: Some(height_map), + }, + }) + } + + pub fn transformed(&self, transform: &Mat4f) -> Self { + Self { + descriptor: self.descriptor.transformed(transform), + ..self.clone() + } + } + + pub fn test_markers(&self) -> Option<[Vec3f; 2]> { + self.descriptor.test_markers() + } + + pub fn solve_remote_to_local( + local: &XrNetAlignmentDescriptorFrame, + remote: &XrNetAlignmentDescriptorFrame, + ) -> Option { + local.descriptor.solve_remote_to_local(&remote.descriptor) + } + + pub fn to_packet_bytes(&self, node_id: XrNetPeerId) -> Vec { + XrNetDataPacket::alignment_descriptor(node_id, self.clone()).to_bytes() + } + + pub fn from_packet_bytes(bytes: &[u8]) -> Option<(XrNetPeerId, Self)> { + match XrNetDataPacket::from_bytes(bytes)? { + XrNetDataPacket::AlignmentDescriptor { + version, + node_id, + frame, + } if version == XR_NET_PROTOCOL_VERSION => Some((node_id, frame)), + _ => None, + } + } +} + +pub type XrNetAlignmentSolution = XrDepthAlignSolution; + +#[derive(Clone, Debug, Default, PartialEq, SerBin, DeBin)] +pub struct XrNetAlignmentDescriptorDumpPair { + pub format_version: u32, + pub captured_at_unix_ms: u64, + pub remote_peer_id: XrNetPeerId, + pub local_descriptor: XrNetAlignmentDescriptorFrame, + pub remote_descriptor: XrNetAlignmentDescriptorFrame, +} + +impl XrNetAlignmentDescriptorDumpPair { + pub const FORMAT_VERSION: u32 = 2; + pub const FILE_MAGIC: &[u8; 8] = b"MXRPAIR1"; + + pub fn new( + remote_peer_id: XrNetPeerId, + local_descriptor: XrNetAlignmentDescriptorFrame, + remote_descriptor: XrNetAlignmentDescriptorFrame, + ) -> Self { + let captured_at_unix_ms = SystemTime::now() + .duration_since(UNIX_EPOCH) + .map(|duration| duration.as_millis().min(u64::MAX as u128) as u64) + .unwrap_or(0); + Self { + format_version: Self::FORMAT_VERSION, + captured_at_unix_ms, + remote_peer_id, + local_descriptor, + remote_descriptor, + } + } + + pub fn to_file_bytes(&self) -> Vec { + let mut bytes = Vec::with_capacity(Self::FILE_MAGIC.len() + 512); + bytes.extend_from_slice(Self::FILE_MAGIC); + bytes.extend(self.serialize_bin()); + bytes + } + + pub fn from_file_bytes(bytes: &[u8]) -> Option { + if !bytes.starts_with(Self::FILE_MAGIC) { + return None; + } + Self::deserialize_bin(&bytes[Self::FILE_MAGIC.len()..]).ok() + } +} + +#[derive(Clone, Debug)] +pub enum XrNetIncoming { + Join { + peer: XrNetPeer, + }, + Leave { + peer: XrNetPeer, + reason: XrNetLeaveReason, + }, + State { + peer: XrNetPeer, + frame: XrNetStateFrame, + }, + Alignment { + peer: XrNetPeer, + frame: XrNetAlignmentFrame, + }, + AlignmentDescriptor { + peer: XrNetPeer, + frame: XrNetAlignmentDescriptorFrame, + }, + Activity { + peer: XrNetPeer, + control: XrNetActivityControl, + }, + BodySpawn { + peer: XrNetPeer, + spawn: XrNetBodySpawn, + }, + SharedObjectControl { + peer: XrNetPeer, + control: XrNetSharedObjectControl, + }, + SharedObjectState { + peer: XrNetPeer, + state: XrNetSharedObjectState, + }, +} + +#[derive(Clone, Debug)] +pub struct XrNetConfig { + pub node_id: XrNetPeerId, + pub discovery_bind: SocketAddr, + pub data_bind: SocketAddr, + pub sync_bind: SocketAddr, + pub discovery_targets: Vec, + pub timing: XrNetTimingConfig, +} + +#[derive(Clone, Copy, Debug, PartialEq, Eq)] +pub struct XrNetTimingConfig { + pub discovery_interval: Duration, + pub peer_timeout: Duration, + pub poll_interval: Duration, +} + +impl Default for XrNetTimingConfig { + fn default() -> Self { + Self { + discovery_interval: XR_NET_DEFAULT_DISCOVERY_INTERVAL, + peer_timeout: XR_NET_DEFAULT_PEER_TIMEOUT, + poll_interval: XR_NET_DEFAULT_POLL_INTERVAL, + } + } +} + +impl Default for XrNetConfig { + fn default() -> Self { + Self { + node_id: XrNetPeerId(default_node_id()), + discovery_bind: SocketAddr::new( + IpAddr::V4(Ipv4Addr::UNSPECIFIED), + XR_NET_DEFAULT_DISCOVERY_PORT, + ), + data_bind: SocketAddr::new(IpAddr::V4(Ipv4Addr::UNSPECIFIED), XR_NET_DEFAULT_DATA_PORT), + sync_bind: SocketAddr::new(IpAddr::V4(Ipv4Addr::UNSPECIFIED), XR_NET_DEFAULT_SYNC_PORT), + discovery_targets: vec![SocketAddr::new( + IpAddr::V4(Ipv4Addr::new(255, 255, 255, 255)), + XR_NET_DEFAULT_DISCOVERY_PORT, + )], + timing: XrNetTimingConfig::default(), + } + } +} + +#[derive(Debug)] +pub struct XrNetNode { + node_id: XrNetPeerId, + pub incoming_receiver: mpsc::Receiver, + udp_outgoing_sender: mpsc::Sender, + sync_outgoing_sender: mpsc::Sender, + thread_loop: Arc, + udp_worker_thread: Option>, + sync_worker_thread: Option>, + next_state_seq: u32, + next_alignment_seq: u32, + next_alignment_descriptor_seq: u32, + next_activity_tick: u32, + next_shared_object_state_seq: u32, +} + +impl XrNetNode { + pub fn new() -> io::Result { + Self::with_config(XrNetConfig::default()) + } + + pub fn with_config(config: XrNetConfig) -> io::Result { + let discovery_socket = UdpSocket::bind(config.discovery_bind)?; + discovery_socket.set_nonblocking(true)?; + discovery_socket.set_broadcast(true)?; + + let data_socket = UdpSocket::bind(config.data_bind)?; + data_socket.set_nonblocking(true)?; + + let sync_listener = TcpListener::bind(config.sync_bind)?; + sync_listener.set_nonblocking(true)?; + + let bound_data_addr = data_socket.local_addr()?; + let bound_sync_addr = sync_listener.local_addr()?; + let (incoming_sender, incoming_receiver) = mpsc::channel(); + let (udp_outgoing_sender, udp_outgoing_receiver) = mpsc::channel(); + let (sync_outgoing_sender, sync_outgoing_receiver) = mpsc::channel(); + let thread_loop = Arc::new(AtomicBool::new(true)); + let udp_worker_thread = Some(spawn_udp_worker_thread( + config.node_id, + config.discovery_targets, + config.timing.discovery_interval, + config.timing.peer_timeout, + config.timing.poll_interval, + bound_data_addr.port(), + bound_sync_addr.port(), + discovery_socket, + data_socket, + incoming_sender.clone(), + udp_outgoing_receiver, + sync_outgoing_sender.clone(), + thread_loop.clone(), + )); + let sync_worker_thread = Some(spawn_sync_worker_thread( + config.node_id, + config.timing.poll_interval, + sync_listener, + incoming_sender, + sync_outgoing_receiver, + thread_loop.clone(), + )); + + Ok(Self { + node_id: config.node_id, + incoming_receiver, + udp_outgoing_sender, + sync_outgoing_sender, + thread_loop, + udp_worker_thread, + sync_worker_thread, + next_state_seq: 0, + next_alignment_seq: 0, + next_alignment_descriptor_seq: 0, + next_activity_tick: 0, + next_shared_object_state_seq: 0, + }) + } + + pub fn node_id(&self) -> XrNetPeerId { + self.node_id + } + pub fn send_state(&mut self, state: XrState) { + let frame = XrNetStateFrame { + seq: self.next_state_seq, + sent_at: state.time, + state, + }; + self.next_state_seq = self.next_state_seq.wrapping_add(1); + let _ = self + .udp_outgoing_sender + .send(XrNetUdpOutgoing::State(frame)); + } + + pub fn send_alignment(&mut self, anchor: XrAnchor, confidence: f32, sent_at: f64) { + let frame = XrNetAlignmentFrame { + seq: self.next_alignment_seq, + sent_at, + anchor, + confidence, + }; + self.next_alignment_seq = self.next_alignment_seq.wrapping_add(1); + let _ = self + .sync_outgoing_sender + .send(XrNetSyncOutgoing::Alignment(frame)); + } + + pub fn send_alignment_descriptor(&mut self, mut frame: XrNetAlignmentDescriptorFrame) { + frame.seq = self.next_alignment_descriptor_seq; + self.next_alignment_descriptor_seq = self.next_alignment_descriptor_seq.wrapping_add(1); + let _ = self + .sync_outgoing_sender + .send(XrNetSyncOutgoing::AlignmentDescriptor(frame)); + } + + pub fn send_activity( + &mut self, + activity_id: XrActivityId, + changed_at: f64, + ) -> XrNetActivityState { + let state = XrNetActivityState { + activity_id, + changed_at, + changed_tick: self.next_activity_tick, + changed_by: self.node_id, + }; + self.next_activity_tick = self.next_activity_tick.wrapping_add(1); + let _ = self.sync_outgoing_sender.send(XrNetSyncOutgoing::Activity( + XrNetActivityControl::XrSetActivity(state), + )); + state + } + + pub fn send_body_spawn(&mut self, spawn: XrNetBodySpawn) { + let _ = self + .sync_outgoing_sender + .send(XrNetSyncOutgoing::BodySpawn(spawn)); + } + + pub fn send_shared_object_control(&mut self, control: XrNetSharedObjectControl) { + let _ = self + .sync_outgoing_sender + .send(XrNetSyncOutgoing::SharedObjectControl(control)); + } + + pub fn assign_shared_object_state_seqs(&mut self, states: &mut [XrNetSharedObjectState]) { + for state in states { + state.seq = self.next_shared_object_state_seq; + self.next_shared_object_state_seq = self.next_shared_object_state_seq.wrapping_add(1); + } + } + + pub fn send_shared_object_states(&mut self, states: I) + where + I: IntoIterator, + { + let mut batch = states.into_iter().collect::>(); + if batch.is_empty() { + return; + } + self.assign_shared_object_state_seqs(&mut batch); + self.send_prepared_shared_object_states(batch); + } + + pub fn send_prepared_shared_object_states(&mut self, states: I) + where + I: IntoIterator, + { + let batch = states.into_iter().collect::>(); + if batch.is_empty() { + return; + } + let _ = self + .udp_outgoing_sender + .send(XrNetUdpOutgoing::SharedObjectStates(batch)); + } + + pub fn send_shared_object_state(&mut self, state: XrNetSharedObjectState) { + self.send_shared_object_states([state]); + } +} + +impl Drop for XrNetNode { + fn drop(&mut self) { + self.thread_loop.store(false, Ordering::Relaxed); + let _ = self.udp_outgoing_sender.send(XrNetUdpOutgoing::Break); + let _ = self.sync_outgoing_sender.send(XrNetSyncOutgoing::Break); + if let Some(worker_thread) = self.udp_worker_thread.take() { + let _ = worker_thread.join(); + } + if let Some(worker_thread) = self.sync_worker_thread.take() { + let _ = worker_thread.join(); + } + } +} + +#[cfg(test)] +fn wrap_angle(mut angle: f32) -> f32 { + while angle <= -std::f32::consts::PI { + angle += std::f32::consts::TAU; + } + while angle > std::f32::consts::PI { + angle -= std::f32::consts::TAU; + } + angle +} + +fn accept_seq(last: Option, next: u32) -> bool { + let Some(last) = last else { + return true; + }; + next != last && next.wrapping_sub(last) < (u32::MAX / 2) +} + +fn default_node_id() -> u64 { + let now = SystemTime::now() + .duration_since(UNIX_EPOCH) + .unwrap_or_default() + .as_nanos(); + let pid = std::process::id() as u128; + LiveId::from_str(&format!("xr-net-{}-{}", now, pid)).0 +} + +#[cfg(test)] +mod tests { + use super::*; + use makepad_lz4::{compress_bound, compress_fast_into}; + use std::{sync::Mutex, time::Instant}; + + const TEST_IO_TIMEOUT: Duration = Duration::from_secs(3); + static NET_LOOPBACK_TEST_LOCK: Mutex<()> = Mutex::new(()); + + fn wait_for_event(node: &XrNetNode, mut predicate: F) -> Option + where + F: FnMut(&XrNetIncoming) -> bool, + { + let start = Instant::now(); + while start.elapsed() < TEST_IO_TIMEOUT { + match node + .incoming_receiver + .recv_timeout(Duration::from_millis(50)) + { + Ok(event) if predicate(&event) => return Some(event), + Ok(_) => continue, + Err(mpsc::RecvTimeoutError::Timeout) => continue, + Err(mpsc::RecvTimeoutError::Disconnected) => return None, + } + } + None + } + + fn wait_for_matching_events( + node: &XrNetNode, + expected: usize, + mut predicate: F, + ) -> Vec + where + F: FnMut(&XrNetIncoming) -> bool, + { + let start = Instant::now(); + let mut matches = Vec::new(); + while start.elapsed() < TEST_IO_TIMEOUT && matches.len() < expected { + match node + .incoming_receiver + .recv_timeout(Duration::from_millis(50)) + { + Ok(event) if predicate(&event) => matches.push(event), + Ok(_) => continue, + Err(mpsc::RecvTimeoutError::Timeout) => continue, + Err(mpsc::RecvTimeoutError::Disconnected) => break, + } + } + matches + } + + fn localhost_config( + node_id: u64, + discovery_port: u16, + data_port: u16, + sync_port: u16, + discovery_targets: Vec, + ) -> XrNetConfig { + XrNetConfig { + node_id: XrNetPeerId(node_id), + discovery_bind: SocketAddr::new(IpAddr::V4(Ipv4Addr::LOCALHOST), discovery_port), + data_bind: SocketAddr::new(IpAddr::V4(Ipv4Addr::LOCALHOST), data_port), + sync_bind: SocketAddr::new(IpAddr::V4(Ipv4Addr::LOCALHOST), sync_port), + discovery_targets: discovery_targets + .into_iter() + .map(|port| SocketAddr::new(IpAddr::V4(Ipv4Addr::LOCALHOST), port)) + .collect(), + timing: XrNetTimingConfig { + discovery_interval: Duration::from_millis(20), + peer_timeout: Duration::from_millis(150), + poll_interval: Duration::from_millis(5), + }, + } + } + + fn make_test_state(head_position: Vec3f) -> XrState { + fn make_test_hand(origin: Vec3f, aim_offset: Vec3f) -> XrHand { + let mut hand = XrHand::default(); + hand.flags = XrHand::IN_VIEW | XrHand::AIM_VALID; + hand.tips_active = 0x1f; + for (index, joint) in hand.joints.iter_mut().enumerate() { + *joint = Pose::new( + Quat::default(), + origin + vec3f(index as f32 * 0.004, (index % 5) as f32 * 0.003, -0.02), + ); + } + hand.tips = [0.026, 0.031, 0.033, 0.029, 0.027]; + hand.aim_pose = Pose::new(Quat::default(), origin + aim_offset); + hand.pinch = [32, 96, 48, 12]; + hand + } + + XrState { + time: 1.0, + head_pose: Pose::new(Quat::default(), head_position), + left_hand: make_test_hand(vec3f(-0.18, 1.34, -0.22), vec3f(0.03, 0.02, -0.08)), + right_hand: make_test_hand(vec3f(0.18, 1.36, -0.20), vec3f(-0.03, 0.02, -0.08)), + ..XrState::default() + } + } + + #[test] + fn xr_state_packet_stays_within_udp_budget() { + let packet = XrNetDataPacket::state( + XrNetPeerId(99), + XrNetStateFrame { + seq: 7, + sent_at: 3.5, + state: make_test_state(vec3f(0.0, 1.6, 0.0)), + }, + ) + .to_bytes(); + let bound = compress_bound(packet.len()); + let mut compressed = vec![0u8; bound]; + let compressed_len = + compress_fast_into(&packet, &mut compressed, XR_NET_SYNC_LZ4_ACCELERATION) + .expect("lz4 compression should succeed for state packet"); + + println!( + "xr_state_packet_bytes={} xr_state_packet_lz4_bytes={}", + packet.len(), + compressed_len + ); + + assert!( + packet.len() <= 1536, + "state packet grew past the current UDP budget: {} bytes", + packet.len() + ); + assert!( + compressed_len <= packet.len(), + "lz4 should not expand the representative state packet: raw={} compressed={compressed_len}", + packet.len() + ); + } + + fn transform_point(mat: &Mat4f, point: Vec3f) -> Vec3f { + mat.transform_vec4(vec4f(point.x, point.y, point.z, 1.0)) + .to_vec3f() + } + + fn angle_error(a: f32, b: f32) -> f32 { + wrap_angle(a - b).abs() + } + + fn synthetic_scene_height(point: Vec2f) -> f32 { + let mut height: f32 = 0.02; + if point.x.abs() >= 2.05 && point.x.abs() <= 2.25 && point.y >= -2.30 && point.y <= 2.10 { + height = height.max(2.15); + } + if point.y >= 1.75 && point.y <= 1.95 && point.x >= -2.25 && point.x <= 2.25 { + height = height.max(2.15); + } + if point.y <= -1.95 && point.y >= -2.20 && point.x >= -2.25 && point.x <= 2.05 { + height = height.max(2.15); + } + if point.x >= -0.95 && point.x <= -0.10 && point.y >= -0.42 && point.y <= 0.36 { + height = height.max(0.84); + } + if point.x >= 0.52 && point.x <= 1.12 && point.y >= 0.52 && point.y <= 1.18 { + height = height.max(1.38); + } + if point.x >= -1.58 && point.x <= -1.20 && point.y >= 0.92 && point.y <= 1.34 { + height = height.max(1.62); + } + if point.x >= 0.96 && point.x <= 1.34 && point.y >= -1.42 && point.y <= -0.66 { + height = height.max(0.68); + } + let wobble = ((point.x * 1.13 + point.y * 0.73).sin() * 0.018) + + ((point.x * 0.41 - point.y * 1.51).cos() * 0.014); + (height + wobble).clamp(0.0, 2.25) + } + + fn make_descriptor_frame() -> XrNetAlignmentDescriptorFrame { + let size = 120usize; + let cell_size_meters = 0.05; + let extent = size as f32 * cell_size_meters; + let origin = -extent * 0.5; + let bottom_y_meters = 0.0; + let top_y_meters = 2.3; + let floor_y_meters = 0.0; + let mut heights_meters = vec![f32::NAN; size * size]; + for z in 0..size { + for x in 0..size { + let point = vec2f( + origin + (x as f32 + 0.5) * cell_size_meters, + origin + (z as f32 + 0.5) * cell_size_meters, + ); + heights_meters[x + z * size] = synthetic_scene_height(point); + } + } + XrNetAlignmentDescriptorFrame { + seq: 0, + sent_at: 1.0, + descriptor: XrDepthAlignDescriptor { + voxel_size_meters: 0.05, + floor_y: floor_y_meters, + wall_normal_histogram: Vec::new(), + samples: Vec::new(), + vertical_descriptor: None, + height_map: Some(XrDepthAlignHeightMap { + origin_x: origin, + origin_z: origin, + cell_size_meters, + size_x: size as u16, + size_z: size as u16, + bottom_y_meters, + top_y_meters, + floor_y_meters, + player_cutout_center: None, + player_cutout_radius_meters: 0.0, + heights_meters, + }), + }, + } + } + + fn make_tsdf_snapshot_for_descriptor( + descriptor: XrDepthAlignDescriptor, + ) -> TsdfPublishedSnapshot { + TsdfPublishedSnapshot { + generation: 1, + latest_topology_generation: 1, + update_sequence: 1, + grid: Arc::new(SparseTsdGridReadSnapshot { + voxel_size: descriptor.voxel_size_meters, + ..Default::default() + }), + height_map: descriptor.height_map.clone(), + } + } + + #[test] + fn peer_timeout_emits_leave_without_needing_more_packets() { + let _guard = NET_LOOPBACK_TEST_LOCK.lock().unwrap(); + let config = localhost_config(1, 42646, 42647, 42648, Vec::new()); + let node = XrNetNode::with_config(config).expect("node should bind localhost test ports"); + + let discovery_socket = + UdpSocket::bind(SocketAddr::new(IpAddr::V4(Ipv4Addr::LOCALHOST), 0)).unwrap(); + let data_socket = + UdpSocket::bind(SocketAddr::new(IpAddr::V4(Ipv4Addr::LOCALHOST), 0)).unwrap(); + + let discovery_packet = XrNetDiscoveryPacket::Hello(XrNetDiscoveryHello { + version: XR_NET_PROTOCOL_VERSION, + node_id: XrNetPeerId(2), + data_port: data_socket.local_addr().unwrap().port(), + sync_port: 42658, + }); + let discovery_target = SocketAddr::new(IpAddr::V4(Ipv4Addr::LOCALHOST), 42646); + let _ = discovery_socket.send_to(&discovery_packet.serialize_bin(), discovery_target); + + let join = wait_for_event(&node, |event| matches!(event, XrNetIncoming::Join { .. })) + .expect("join should arrive from a single hello"); + let peer = match join { + XrNetIncoming::Join { peer } => peer, + _ => unreachable!(), + }; + + let state_packet = XrNetDataPacket::State { + version: XR_NET_PROTOCOL_VERSION, + node_id: XrNetPeerId(2), + frame: XrNetStateFrame { + seq: 0, + sent_at: 1.0, + state: make_test_state(vec3f(0.0, 1.6, 0.0)), + }, + }; + let state_target = SocketAddr::new(IpAddr::V4(Ipv4Addr::LOCALHOST), 42647); + let _ = data_socket.send_to(&state_packet.serialize_bin(), state_target); + + let _ = wait_for_event(&node, |event| { + matches!(event, XrNetIncoming::State { peer: event_peer, .. } if *event_peer == peer) + }) + .expect("state should arrive"); + + let leave = wait_for_event(&node, |event| { + matches!( + event, + XrNetIncoming::Leave { + peer: event_peer, + reason: XrNetLeaveReason::Timeout, + } if *event_peer == peer + ) + }) + .expect("peer should time out without any extra packets"); + match leave { + XrNetIncoming::Leave { reason, .. } => { + assert_eq!(reason, XrNetLeaveReason::Timeout); + } + _ => unreachable!(), + } + } + + #[test] + fn two_test_clients_align_two_cubes_via_protocol() { + let _guard = NET_LOOPBACK_TEST_LOCK.lock().unwrap(); + let mut left = + XrNetNode::with_config(localhost_config(11, 42746, 42747, 42748, vec![42756])) + .expect("left test client should bind"); + let mut right = + XrNetNode::with_config(localhost_config(22, 42756, 42757, 42758, vec![42746])) + .expect("right test client should bind"); + + let left_cube_world = vec3f(-0.25, 0.82, -0.62); + let right_cube_world = vec3f(0.21, 0.82, -0.38); + let local_anchor = XrAnchor { + left: left_cube_world, + right: right_cube_world, + }; + + let remote_scene_to_local = Pose::new( + Quat::from_axis_angle(vec3f(0.0, 1.0, 0.0), 0.42), + vec3f(-0.34, 0.05, 0.27), + ) + .to_mat4(); + let local_to_remote_scene = remote_scene_to_local.invert(); + let remote_anchor = XrAnchor { + left: transform_point(&local_to_remote_scene, left_cube_world), + right: transform_point(&local_to_remote_scene, right_cube_world), + }; + + left.send_alignment(local_anchor, 0.95, 1.0); + right.send_alignment(remote_anchor, 0.95, 1.0); + + let left_remote = wait_for_event(&left, |event| { + matches!(event, XrNetIncoming::Alignment { .. }) + }) + .expect("left test client should receive remote alignment"); + let right_remote = wait_for_event(&right, |event| { + matches!(event, XrNetIncoming::Alignment { .. }) + }) + .expect("right test client should receive remote alignment"); + + let left_remote_frame = match left_remote { + XrNetIncoming::Alignment { frame, .. } => frame, + _ => unreachable!(), + }; + let right_remote_frame = match right_remote { + XrNetIncoming::Alignment { frame, .. } => frame, + _ => unreachable!(), + }; + + let solved_remote_to_local = XrNetAlignmentFrame::remote_to_local_transform( + &local_anchor_frame(local_anchor), + &left_remote_frame, + ) + .expect("left side should solve a remote->local transform"); + let solved_local_to_remote = XrNetAlignmentFrame::remote_to_local_transform( + &remote_anchor_frame(remote_anchor), + &right_remote_frame, + ) + .expect("right side should solve a local->remote transform"); + + let mapped_left = transform_point(&solved_remote_to_local, remote_anchor.left); + let mapped_right = transform_point(&solved_remote_to_local, remote_anchor.right); + assert!((mapped_left - left_cube_world).length() < 0.02); + assert!((mapped_right - right_cube_world).length() < 0.02); + + let mapped_back_left = transform_point(&solved_local_to_remote, left_cube_world); + let mapped_back_right = transform_point(&solved_local_to_remote, right_cube_world); + assert!((mapped_back_left - remote_anchor.left).length() < 0.02); + assert!((mapped_back_right - remote_anchor.right).length() < 0.02); + } + + #[test] + fn cached_alignment_descriptor_is_sent_to_late_joiner() { + let _guard = NET_LOOPBACK_TEST_LOCK.lock().unwrap(); + let mut left = + XrNetNode::with_config(localhost_config(101, 42846, 42847, 42848, vec![42856])) + .expect("left test client should bind"); + let right = XrNetNode::with_config(localhost_config(202, 42856, 42857, 42858, vec![42846])) + .expect("right test client should bind"); + + let descriptor = make_descriptor_frame(); + left.send_alignment_descriptor(descriptor.clone()); + + let event = wait_for_event(&right, |event| { + matches!(event, XrNetIncoming::AlignmentDescriptor { .. }) + }) + .expect("late joiner should receive cached descriptor"); + + let received = match event { + XrNetIncoming::AlignmentDescriptor { frame, .. } => frame, + _ => unreachable!(), + }; + assert_eq!(received.descriptor, descriptor.descriptor); + } + + #[test] + fn cached_activity_is_sent_to_late_joiner() { + let _guard = NET_LOOPBACK_TEST_LOCK.lock().unwrap(); + let mut left = + XrNetNode::with_config(localhost_config(301, 42946, 42947, 42948, vec![42956])) + .expect("left test client should bind"); + let right = XrNetNode::with_config(localhost_config(302, 42956, 42957, 42958, vec![42946])) + .expect("right test client should bind"); + + let sent = left.send_activity(XrActivityId(live_id!(tree_scene)), 3.5); + + let event = wait_for_event(&right, |event| { + matches!(event, XrNetIncoming::Activity { .. }) + }) + .expect("late joiner should receive cached activity"); + + let received = match event { + XrNetIncoming::Activity { control, .. } => control.state(), + _ => unreachable!(), + }; + assert_eq!(received, sent); + } + + #[test] + fn body_spawn_is_sent_over_sync_channel() { + let _guard = NET_LOOPBACK_TEST_LOCK.lock().unwrap(); + let mut left = + XrNetNode::with_config(localhost_config(311, 43046, 43047, 43048, vec![43056])) + .expect("left test client should bind"); + let right = XrNetNode::with_config(localhost_config(312, 43056, 43057, 43058, vec![43046])) + .expect("right test client should bind"); + + let _ = left.send_activity(XrActivityId(live_id!(ico_shoot_scene)), 4.0); + let _ = wait_for_event(&right, |event| { + matches!(event, XrNetIncoming::Activity { .. }) + }) + .expect("sync channel should deliver an activity before body spawns"); + + let sent = XrNetBodySpawn { + activity_id: XrActivityId(live_id!(ico_shoot_scene)), + object_id: XrSpawnableObjectId(0xfeed_cafe_dead_beef), + pose: Pose::new(Quat::default(), vec3f(0.2, 1.1, -0.7)), + linvel: vec3f(1.0, 2.0, 3.0), + angvel: vec3f(-0.5, 0.0, 0.5), + }; + left.send_body_spawn(sent); + + let event = wait_for_event(&right, |event| { + matches!(event, XrNetIncoming::BodySpawn { .. }) + }) + .expect("body spawn should arrive over sync channel"); + + let received = match event { + XrNetIncoming::BodySpawn { spawn, .. } => spawn, + _ => unreachable!(), + }; + assert_eq!(received, sent); + } + + #[test] + fn shared_object_id_hashes_peer_and_counter_deterministically() { + let object_id = xr_make_shared_object_id(XrNetPeerId(42), XrSharedObjectCounter(123456)) + .expect("hashed shared object id should allocate"); + assert_ne!(object_id, XrSharedObjectId(0)); + assert_eq!( + object_id, + xr_make_shared_object_id(XrNetPeerId(42), XrSharedObjectCounter(123456)) + .expect("same seed should hash the same way") + ); + assert_ne!( + object_id, + xr_make_shared_object_id(XrNetPeerId(42), XrSharedObjectCounter(123457)) + .expect("different counters should hash differently") + ); + } + + #[test] + fn shared_object_id_depends_on_net_peer_id_not_socket_addr() { + let node_id = XrNetPeerId(0x1234); + let local_bind_id = xr_make_shared_object_id(node_id, XrSharedObjectCounter(9)) + .expect("local bind should hash"); + let remote_source_id = xr_make_shared_object_id(node_id, XrSharedObjectCounter(9)) + .expect("remote source should hash"); + + assert_eq!(local_bind_id, remote_source_id); + } + + #[test] + fn shared_object_control_is_sent_over_sync_channel() { + let _guard = NET_LOOPBACK_TEST_LOCK.lock().unwrap(); + let mut left = + XrNetNode::with_config(localhost_config(321, 43146, 43147, 43148, vec![43156])) + .expect("left test client should bind"); + let right = XrNetNode::with_config(localhost_config(322, 43156, 43157, 43158, vec![43146])) + .expect("right test client should bind"); + + let _ = left.send_activity(XrActivityId(live_id!(ico_shoot_scene)), 5.0); + let _ = wait_for_event(&right, |event| { + matches!(event, XrNetIncoming::Activity { .. }) + }) + .expect("sync channel should be ready before shared object control"); + + let sent = XrNetSharedObjectControl::XrSpawnObject { + object_id: xr_make_shared_object_id(XrNetPeerId(77), XrSharedObjectCounter(9)) + .expect("counter should fit"), + epoch: 0, + authority: XrNetPeerId(77), + fidelity: XrSharedObjectFidelity::ImpactCritical, + shape: XrSharedObjectShape::ActivitySpawnable { + activity_id: XrActivityId(live_id!(ico_shoot_scene)), + spawnable_id: XrSpawnableObjectId(0x44), + }, + pose: Pose::new(Quat::default(), vec3f(0.0, 1.2, -0.4)), + linvel: vec3f(3.0, 0.0, -9.0), + angvel: vec3f(0.0, 0.0, 0.0), + }; + left.send_shared_object_control(sent.clone()); + + let event = wait_for_event(&right, |event| { + matches!(event, XrNetIncoming::SharedObjectControl { .. }) + }) + .expect("shared object control should arrive over sync channel"); + + let received = match event { + XrNetIncoming::SharedObjectControl { control, .. } => control, + _ => unreachable!(), + }; + assert_eq!(received, sent); + } + + #[test] + fn shared_object_state_is_sent_over_udp() { + let _guard = NET_LOOPBACK_TEST_LOCK.lock().unwrap(); + let mut left = + XrNetNode::with_config(localhost_config(323, 43246, 43247, 43248, vec![43256])) + .expect("left test client should bind"); + let right = XrNetNode::with_config(localhost_config(324, 43256, 43257, 43258, vec![43246])) + .expect("right test client should bind"); + + let _ = wait_for_event(&left, |event| matches!(event, XrNetIncoming::Join { .. })) + .expect("left test client should discover right before sending shared state"); + let _ = wait_for_event(&right, |event| matches!(event, XrNetIncoming::Join { .. })) + .expect("right test client should discover left before receiving shared state"); + + let sent = XrNetSharedObjectState { + seq: 0, + sent_at: 7.5, + physics_tick: 42, + object_id: xr_make_shared_object_id(XrNetPeerId(77), XrSharedObjectCounter(9)) + .expect("counter should fit"), + epoch: 3, + authority: XrNetPeerId(77), + fidelity: XrSharedObjectFidelity::ImpactCritical, + mode: XrSharedObjectMode::Dynamic, + pose: Pose::new(Quat::default(), vec3f(0.0, 1.2, -0.4)), + linvel: vec3f(3.0, 0.0, -9.0), + angvel: vec3f(0.0, 0.0, 0.0), + }; + left.send_shared_object_state(sent); + + let event = wait_for_event(&right, |event| { + matches!(event, XrNetIncoming::SharedObjectState { .. }) + }) + .expect("shared object state should arrive over udp"); + + let received = match event { + XrNetIncoming::SharedObjectState { state, .. } => state, + _ => unreachable!(), + }; + assert_eq!(received.sent_at, sent.sent_at); + assert_eq!(received.physics_tick, sent.physics_tick); + assert_eq!(received.object_id, sent.object_id); + assert_eq!(received.epoch, sent.epoch); + assert_eq!(received.authority, sent.authority); + assert_eq!(received.fidelity, sent.fidelity); + assert_eq!(received.mode, sent.mode); + assert_eq!(received.pose, sent.pose); + assert_eq!(received.linvel, sent.linvel); + assert_eq!(received.angvel, sent.angvel); + } + + #[test] + fn shared_object_state_batches_roundtrip_over_udp() { + let _guard = NET_LOOPBACK_TEST_LOCK.lock().unwrap(); + let mut left = + XrNetNode::with_config(localhost_config(325, 43266, 43267, 43268, vec![43276])) + .expect("left test client should bind"); + let right = XrNetNode::with_config(localhost_config(326, 43276, 43277, 43278, vec![43266])) + .expect("right test client should bind"); + + let _ = wait_for_event(&left, |event| matches!(event, XrNetIncoming::Join { .. })) + .expect("left test client should discover right before sending batched shared state"); + let _ = wait_for_event(&right, |event| matches!(event, XrNetIncoming::Join { .. })) + .expect("right test client should discover left before receiving batched shared state"); + + let states = (0..24) + .map(|index| XrNetSharedObjectState { + seq: 0, + sent_at: 10.0 + index as f64 * 0.01, + physics_tick: 100 + index as u32, + object_id: xr_make_shared_object_id( + XrNetPeerId(77), + XrSharedObjectCounter(1000 + index as u64), + ) + .expect("counter should fit"), + epoch: index as u32, + authority: XrNetPeerId(77), + fidelity: XrSharedObjectFidelity::ImpactCritical, + mode: XrSharedObjectMode::Dynamic, + pose: Pose::new(Quat::default(), vec3f(index as f32 * 0.01, 1.0, -0.4)), + linvel: vec3f(3.0 + index as f32, 0.0, -9.0), + angvel: vec3f(0.0, 0.1 * index as f32, 0.0), + }) + .collect::>(); + assert!( + XrNetDataPacket::shared_object_states(XrNetPeerId(11), states.clone()) + .to_bytes() + .len() + > XR_NET_UDP_SHARED_OBJECT_STATE_BATCH_MAX_BYTES, + "test fixture should exceed the per-datagram shared-object budget to exercise batching", + ); + + left.send_shared_object_states(states.clone()); + let received = wait_for_matching_events(&right, states.len(), |event| { + matches!(event, XrNetIncoming::SharedObjectState { .. }) + }); + assert_eq!(received.len(), states.len(), "{received:?}"); + + let received_states = received + .into_iter() + .map(|event| match event { + XrNetIncoming::SharedObjectState { state, .. } => state, + _ => unreachable!(), + }) + .collect::>(); + for (received, expected) in received_states.iter().zip(states.iter()) { + assert_eq!(received.sent_at, expected.sent_at); + assert_eq!(received.physics_tick, expected.physics_tick); + assert_eq!(received.object_id, expected.object_id); + assert_eq!(received.epoch, expected.epoch); + assert_eq!(received.authority, expected.authority); + assert_eq!(received.fidelity, expected.fidelity); + assert_eq!(received.mode, expected.mode); + assert_eq!(received.pose, expected.pose); + assert_eq!(received.linvel, expected.linvel); + assert_eq!(received.angvel, expected.angvel); + } + } + + #[test] + fn cached_shared_object_control_is_sent_to_late_joiner() { + let _guard = NET_LOOPBACK_TEST_LOCK.lock().unwrap(); + let mut left = + XrNetNode::with_config(localhost_config(325, 43346, 43347, 43348, vec![43356])) + .expect("left test client should bind"); + + let _ = left.send_activity(XrActivityId(live_id!(ico_shoot_scene)), 8.0); + let sent = XrNetSharedObjectControl::XrSpawnObject { + object_id: xr_make_shared_object_id(XrNetPeerId(77), XrSharedObjectCounter(13)) + .expect("counter should fit"), + epoch: 2, + authority: XrNetPeerId(77), + fidelity: XrSharedObjectFidelity::ImpactCritical, + shape: XrSharedObjectShape::ActivitySpawnable { + activity_id: XrActivityId(live_id!(ico_shoot_scene)), + spawnable_id: XrSpawnableObjectId(0x44), + }, + pose: Pose::new(Quat::default(), vec3f(0.0, 1.2, -0.4)), + linvel: vec3f(3.0, 0.0, -9.0), + angvel: vec3f(0.0, 0.0, 0.0), + }; + left.send_shared_object_control(sent.clone()); + + let right = XrNetNode::with_config(localhost_config(326, 43356, 43357, 43358, vec![43346])) + .expect("right test client should bind"); + + let start = Instant::now(); + let mut received_activity = None; + let mut received_control = None; + while start.elapsed() < TEST_IO_TIMEOUT + && (received_activity.is_none() || received_control.is_none()) + { + match right + .incoming_receiver + .recv_timeout(Duration::from_millis(50)) + { + Ok(XrNetIncoming::Activity { control, .. }) => { + received_activity = Some(control.state()); + } + Ok(XrNetIncoming::SharedObjectControl { control, .. }) => { + received_control = Some(control); + } + Ok(_) => {} + Err(mpsc::RecvTimeoutError::Timeout) => {} + Err(mpsc::RecvTimeoutError::Disconnected) => break, + } + } + + assert_eq!( + received_activity, + Some(XrNetActivityState { + activity_id: XrActivityId(live_id!(ico_shoot_scene)), + changed_at: 8.0, + changed_tick: 0, + changed_by: XrNetPeerId(325), + }), + "late joiner should receive the cached activity" + ); + assert_eq!( + received_control, + Some(sent), + "late joiner should receive the cached shared object control" + ); + } + + #[test] + fn activity_order_prefers_tick_then_time_then_peer() { + let base = XrNetActivityState { + activity_id: XrActivityId(live_id!(ico_box_scene)), + changed_at: 10.0, + changed_tick: 4, + changed_by: XrNetPeerId(7), + }; + + assert!(XrNetActivityState { + changed_tick: 5, + ..base + } + .is_newer_than(&base)); + assert!(XrNetActivityState { + changed_at: 11.0, + ..base + } + .is_newer_than(&base)); + assert!(XrNetActivityState { + changed_by: XrNetPeerId(8), + ..base + } + .is_newer_than(&base)); + assert!(!base.is_newer_than(&base)); + } + + #[test] + fn descriptor_solver_recovers_yaw_and_position_from_walls_and_vertical_descriptor() { + let local = make_descriptor_frame(); + let ground_truth_remote_to_local = Pose::new( + Quat::from_axis_angle(vec3f(0.0, 1.0, 0.0), 0.58), + vec3f(-0.82, 0.0, 0.67), + ) + .to_mat4(); + let local_to_remote = ground_truth_remote_to_local.invert(); + let remote = local.transformed(&local_to_remote); + + let solution = XrNetAlignmentDescriptorFrame::solve_remote_to_local(&local, &remote) + .expect("descriptor solver should find a transform"); + + assert!(angle_error(solution.yaw_radians, 0.58) < 0.08); + assert!((solution.translation - vec3f(-0.82, 0.0, 0.67)).length() < 0.12); + assert!(solution.confidence > 0.15); + assert!(solution.matched_samples >= 2); + } + + #[test] + fn descriptor_from_tsdf_snapshot_uses_tsdf_alignment_descriptor_snapshot() { + let descriptor = make_descriptor_frame().descriptor; + let snapshot = make_tsdf_snapshot_for_descriptor(descriptor.clone()); + let descriptor = XrNetAlignmentDescriptorFrame::from_tsdf_snapshot(&snapshot, 1.0) + .expect("descriptor should use the TSDF alignment descriptor from the depth snapshot"); + + assert_eq!(descriptor.descriptor.height_map, snapshot.height_map); + assert!(descriptor.test_markers().is_some()); + } + + fn local_anchor_frame(anchor: XrAnchor) -> XrNetAlignmentFrame { + XrNetAlignmentFrame { + seq: 0, + sent_at: 1.0, + anchor, + confidence: 1.0, + } + } + + fn remote_anchor_frame(anchor: XrAnchor) -> XrNetAlignmentFrame { + XrNetAlignmentFrame { + seq: 0, + sent_at: 1.0, + anchor, + confidence: 1.0, + } + } +} diff --git a/xr/src/net/xr_net_protocol.rs b/xr/src/net/xr_net_protocol.rs new file mode 100644 index 0000000000..0ede55a14b --- /dev/null +++ b/xr/src/net/xr_net_protocol.rs @@ -0,0 +1,399 @@ +use super::{ + XrNetActivityControl, XrNetAlignmentDescriptorFrame, XrNetAlignmentFrame, XrNetBodySpawn, + XrNetPeerId, XrNetSharedObjectControl, XrNetSharedObjectState, XrNetStateFrame, + XR_NET_PROTOCOL_VERSION, XR_NET_SYNC_FRAME_LZ4_TAG, XR_NET_SYNC_FRAME_RAW_TAG, + XR_NET_SYNC_LZ4_ACCELERATION, XR_NET_SYNC_MAX_FRAME_BYTES, +}; +use makepad_lz4::{compress_bound, compress_fast_into, decompress_safe}; +use makepad_widgets::makepad_platform::makepad_micro_serde::*; +use std::io; + +#[derive(Clone, Debug, SerBin, DeBin)] +pub struct XrNetDiscoveryHello { + pub version: u16, + pub node_id: XrNetPeerId, + pub data_port: u16, + pub sync_port: u16, +} + +impl XrNetDiscoveryHello { + pub fn is_compatible_for(&self, local_node_id: XrNetPeerId) -> bool { + self.version == XR_NET_PROTOCOL_VERSION && self.node_id != local_node_id + } +} + +#[derive(Clone, Debug, SerBin, DeBin)] +pub struct XrNetLeavePacket { + pub version: u16, + pub node_id: XrNetPeerId, +} + +impl XrNetLeavePacket { + pub fn is_compatible_for(&self, local_node_id: XrNetPeerId) -> bool { + self.version == XR_NET_PROTOCOL_VERSION && self.node_id != local_node_id + } +} + +#[derive(Clone, Debug, SerBin, DeBin)] +pub enum XrNetDiscoveryPacket { + Hello(XrNetDiscoveryHello), + Leave(XrNetLeavePacket), +} + +impl XrNetDiscoveryPacket { + pub fn hello(node_id: XrNetPeerId, data_port: u16, sync_port: u16) -> Self { + Self::Hello(XrNetDiscoveryHello { + version: XR_NET_PROTOCOL_VERSION, + node_id, + data_port, + sync_port, + }) + } + + pub fn leave(node_id: XrNetPeerId) -> Self { + Self::Leave(XrNetLeavePacket { + version: XR_NET_PROTOCOL_VERSION, + node_id, + }) + } + + pub fn from_bytes(bytes: &[u8]) -> Option { + Self::deserialize_bin(bytes).ok() + } + + pub fn to_bytes(&self) -> Vec { + self.serialize_bin() + } +} + +#[derive(Clone, Debug, SerBin, DeBin)] +pub struct XrNetSyncHello { + pub version: u16, + pub node_id: XrNetPeerId, +} + +impl XrNetSyncHello { + pub fn is_compatible_for(&self, local_node_id: XrNetPeerId) -> bool { + self.version == XR_NET_PROTOCOL_VERSION && self.node_id != local_node_id + } + + pub fn matches_peer(&self, peer_id: XrNetPeerId) -> bool { + self.version == XR_NET_PROTOCOL_VERSION && self.node_id == peer_id + } +} + +#[derive(Clone, Debug, SerBin, DeBin)] +pub enum XrNetSyncPacket { + Hello(XrNetSyncHello), + Data(XrNetDataPacket), +} + +impl XrNetSyncPacket { + pub fn hello(node_id: XrNetPeerId) -> Self { + Self::Hello(XrNetSyncHello { + version: XR_NET_PROTOCOL_VERSION, + node_id, + }) + } + + pub fn data(packet: XrNetDataPacket) -> Self { + Self::Data(packet) + } +} + +#[derive(Clone, Debug, SerBin, DeBin)] +pub enum XrNetDataPacket { + State { + version: u16, + node_id: XrNetPeerId, + frame: XrNetStateFrame, + }, + Alignment { + version: u16, + node_id: XrNetPeerId, + frame: XrNetAlignmentFrame, + }, + AlignmentDescriptor { + version: u16, + node_id: XrNetPeerId, + frame: XrNetAlignmentDescriptorFrame, + }, + ActivityControl { + version: u16, + node_id: XrNetPeerId, + control: XrNetActivityControl, + }, + BodySpawn { + version: u16, + node_id: XrNetPeerId, + spawn: XrNetBodySpawn, + }, + SharedObjectStates { + version: u16, + node_id: XrNetPeerId, + states: Vec, + }, + SharedObjectState { + version: u16, + node_id: XrNetPeerId, + state: XrNetSharedObjectState, + }, + SharedObjectControl { + version: u16, + node_id: XrNetPeerId, + control: XrNetSharedObjectControl, + }, + Leave(XrNetLeavePacket), +} + +impl XrNetDataPacket { + pub fn state(node_id: XrNetPeerId, frame: XrNetStateFrame) -> Self { + Self::State { + version: XR_NET_PROTOCOL_VERSION, + node_id, + frame, + } + } + + pub fn alignment(node_id: XrNetPeerId, frame: XrNetAlignmentFrame) -> Self { + Self::Alignment { + version: XR_NET_PROTOCOL_VERSION, + node_id, + frame, + } + } + + pub fn alignment_descriptor( + node_id: XrNetPeerId, + frame: XrNetAlignmentDescriptorFrame, + ) -> Self { + Self::AlignmentDescriptor { + version: XR_NET_PROTOCOL_VERSION, + node_id, + frame, + } + } + + pub fn activity_control(node_id: XrNetPeerId, control: XrNetActivityControl) -> Self { + Self::ActivityControl { + version: XR_NET_PROTOCOL_VERSION, + node_id, + control, + } + } + + pub fn body_spawn(node_id: XrNetPeerId, spawn: XrNetBodySpawn) -> Self { + Self::BodySpawn { + version: XR_NET_PROTOCOL_VERSION, + node_id, + spawn, + } + } + + pub fn shared_object_state(node_id: XrNetPeerId, state: XrNetSharedObjectState) -> Self { + Self::SharedObjectState { + version: XR_NET_PROTOCOL_VERSION, + node_id, + state, + } + } + + pub fn shared_object_states(node_id: XrNetPeerId, states: Vec) -> Self { + Self::SharedObjectStates { + version: XR_NET_PROTOCOL_VERSION, + node_id, + states, + } + } + + pub fn shared_object_control(node_id: XrNetPeerId, control: XrNetSharedObjectControl) -> Self { + Self::SharedObjectControl { + version: XR_NET_PROTOCOL_VERSION, + node_id, + control, + } + } + + pub fn leave(node_id: XrNetPeerId) -> Self { + Self::Leave(XrNetLeavePacket { + version: XR_NET_PROTOCOL_VERSION, + node_id, + }) + } + + pub fn from_bytes(bytes: &[u8]) -> Option { + Self::deserialize_bin(bytes).ok() + } + + pub fn to_bytes(&self) -> Vec { + self.serialize_bin() + } + + pub fn version(&self) -> u16 { + match self { + Self::State { version, .. } + | Self::Alignment { version, .. } + | Self::AlignmentDescriptor { version, .. } + | Self::ActivityControl { version, .. } + | Self::BodySpawn { version, .. } + | Self::SharedObjectStates { version, .. } + | Self::SharedObjectState { version, .. } + | Self::SharedObjectControl { version, .. } => *version, + Self::Leave(packet) => packet.version, + } + } + + pub fn sender(&self) -> XrNetPeerId { + match self { + Self::State { node_id, .. } + | Self::Alignment { node_id, .. } + | Self::AlignmentDescriptor { node_id, .. } + | Self::ActivityControl { node_id, .. } + | Self::BodySpawn { node_id, .. } + | Self::SharedObjectStates { node_id, .. } + | Self::SharedObjectState { node_id, .. } + | Self::SharedObjectControl { node_id, .. } => *node_id, + Self::Leave(packet) => packet.node_id, + } + } + + pub fn is_compatible_for(&self, local_node_id: XrNetPeerId) -> bool { + self.version() == XR_NET_PROTOCOL_VERSION && self.sender() != local_node_id + } +} + +pub struct XrNetSyncFrameCodec; + +impl XrNetSyncFrameCodec { + pub fn encode(packet: &XrNetSyncPacket) -> Option> { + let payload = packet.serialize_bin(); + let raw_frame_len = 1 + payload.len(); + if raw_frame_len > XR_NET_SYNC_MAX_FRAME_BYTES { + return None; + } + + let bound = compress_bound(payload.len()); + if bound != 0 { + let mut compressed = vec![0u8; bound]; + if let Ok(compressed_len) = + compress_fast_into(&payload, &mut compressed, XR_NET_SYNC_LZ4_ACCELERATION) + { + let compressed_frame_len = 1 + 4 + compressed_len; + if compressed_frame_len < raw_frame_len + && compressed_frame_len <= XR_NET_SYNC_MAX_FRAME_BYTES + { + let mut frame = Vec::with_capacity(compressed_frame_len); + frame.push(XR_NET_SYNC_FRAME_LZ4_TAG); + frame.extend_from_slice(&(payload.len() as u32).to_le_bytes()); + frame.extend_from_slice(&compressed[..compressed_len]); + return Some(frame); + } + } + } + + let mut frame = Vec::with_capacity(raw_frame_len); + frame.push(XR_NET_SYNC_FRAME_RAW_TAG); + frame.extend_from_slice(&payload); + Some(frame) + } + + pub fn decode(frame: &[u8]) -> io::Result { + let Some((&tag, payload)) = frame.split_first() else { + return Err(io::Error::new( + io::ErrorKind::InvalidData, + "empty sync frame", + )); + }; + + let packet_bytes = match tag { + XR_NET_SYNC_FRAME_RAW_TAG => payload.to_vec(), + XR_NET_SYNC_FRAME_LZ4_TAG => { + if payload.len() < 4 { + return Err(io::Error::new( + io::ErrorKind::InvalidData, + "compressed sync frame missing decoded length", + )); + } + let decoded_len = + u32::from_le_bytes([payload[0], payload[1], payload[2], payload[3]]) as usize; + if decoded_len > XR_NET_SYNC_MAX_FRAME_BYTES { + return Err(io::Error::new( + io::ErrorKind::InvalidData, + "compressed sync frame too large", + )); + } + let mut decoded = vec![0u8; decoded_len]; + let written = decompress_safe(&payload[4..], &mut decoded).map_err(|_| { + io::Error::new( + io::ErrorKind::InvalidData, + "failed to decompress sync frame", + ) + })?; + if written != decoded_len { + return Err(io::Error::new( + io::ErrorKind::InvalidData, + "compressed sync frame decoded length mismatch", + )); + } + decoded + } + _ => { + return Err(io::Error::new( + io::ErrorKind::InvalidData, + "unknown sync frame tag", + )) + } + }; + + XrNetSyncPacket::deserialize_bin(&packet_bytes).map_err(|_| { + io::Error::new( + io::ErrorKind::InvalidData, + "failed to decode sync packet payload", + ) + }) + } + + pub fn drain_packets(read_buf: &mut Vec) -> io::Result> { + let mut packets = Vec::::new(); + let mut offset = 0usize; + while read_buf.len().saturating_sub(offset) >= 4 { + let frame_len = u32::from_le_bytes([ + read_buf[offset], + read_buf[offset + 1], + read_buf[offset + 2], + read_buf[offset + 3], + ]) as usize; + if frame_len > XR_NET_SYNC_MAX_FRAME_BYTES { + return Err(io::Error::new( + io::ErrorKind::InvalidData, + "sync frame exceeded max size", + )); + } + if read_buf.len().saturating_sub(offset + 4) < frame_len { + break; + } + let start = offset + 4; + let end = start + frame_len; + let packet = Self::decode(&read_buf[start..end])?; + packets.push(packet); + offset = end; + } + if offset > 0 { + Self::consume_vec_prefix(read_buf, offset); + } + Ok(packets) + } + + fn consume_vec_prefix(buf: &mut Vec, len: usize) { + if len == 0 { + return; + } + if len >= buf.len() { + buf.clear(); + return; + } + let remaining = buf.len() - len; + buf.copy_within(len.., 0); + buf.truncate(remaining); + } +} diff --git a/xr/src/net/xr_net_transport.rs b/xr/src/net/xr_net_transport.rs new file mode 100644 index 0000000000..5f50fbeefc --- /dev/null +++ b/xr/src/net/xr_net_transport.rs @@ -0,0 +1,1156 @@ +use super::*; +use std::{ + collections::HashMap, + io::{self, Read, Write}, + net::{Shutdown, SocketAddr, TcpListener, TcpStream, UdpSocket}, + sync::{ + atomic::{AtomicBool, Ordering}, + mpsc, Arc, + }, + thread::{self, JoinHandle}, + time::{Duration, Instant}, +}; + +#[derive(Debug)] +pub(super) enum XrNetUdpOutgoing { + State(XrNetStateFrame), + SharedObjectStates(Vec), + Break, +} + +#[derive(Debug)] +pub(super) enum XrNetSyncOutgoing { + PeerUp { + peer: XrNetPeer, + sync_addr: SocketAddr, + }, + PeerRemoved { + peer_id: XrNetPeerId, + }, + Alignment(XrNetAlignmentFrame), + AlignmentDescriptor(XrNetAlignmentDescriptorFrame), + Activity(XrNetActivityControl), + BodySpawn(XrNetBodySpawn), + SharedObjectControl(XrNetSharedObjectControl), + Break, +} + +struct UdpWorkerPeerState { + peer: XrNetPeer, + sync_addr: SocketAddr, + last_seen: Instant, + last_state_seq: Option, + last_shared_object_state_seq: Option, + last_alignment_seq: Option, + last_alignment_descriptor_seq: Option, +} + +impl UdpWorkerPeerState { + fn new(peer: XrNetPeer, sync_addr: SocketAddr, now: Instant) -> Self { + Self { + peer, + sync_addr, + last_seen: now, + last_state_seq: None, + last_shared_object_state_seq: None, + last_alignment_seq: None, + last_alignment_descriptor_seq: None, + } + } +} + +struct SyncWorkerPeerState { + peer: XrNetPeer, + sync_addr: SocketAddr, + last_state_seq: Option, + last_alignment_seq: Option, + last_alignment_descriptor_seq: Option, + sync_connection: Option, + next_sync_connect_attempt_at: Instant, +} + +impl SyncWorkerPeerState { + fn new(peer: XrNetPeer, sync_addr: SocketAddr, now: Instant) -> Self { + Self { + peer, + sync_addr, + last_state_seq: None, + last_alignment_seq: None, + last_alignment_descriptor_seq: None, + sync_connection: None, + next_sync_connect_attempt_at: now, + } + } + + fn close_connection(&mut self) { + if let Some(connection) = self.sync_connection.as_mut() { + connection.shutdown(); + } + self.sync_connection = None; + } + + fn route_data_packet( + &mut self, + incoming_sender: &mpsc::Sender, + node_id: XrNetPeerId, + packet: XrNetDataPacket, + ) { + if !packet.is_compatible_for(node_id) { + return; + } + let peer = self.peer; + match packet { + XrNetDataPacket::State { frame, .. } => { + if !accept_seq(self.last_state_seq, frame.seq) { + return; + } + self.last_state_seq = Some(frame.seq); + let _ = incoming_sender.send(XrNetIncoming::State { peer, frame }); + } + XrNetDataPacket::Alignment { frame, .. } => { + if !accept_seq(self.last_alignment_seq, frame.seq) { + return; + } + self.last_alignment_seq = Some(frame.seq); + let _ = incoming_sender.send(XrNetIncoming::Alignment { peer, frame }); + } + XrNetDataPacket::AlignmentDescriptor { frame, .. } => { + if !accept_seq(self.last_alignment_descriptor_seq, frame.seq) { + return; + } + self.last_alignment_descriptor_seq = Some(frame.seq); + let _ = incoming_sender.send(XrNetIncoming::AlignmentDescriptor { peer, frame }); + } + XrNetDataPacket::ActivityControl { control, .. } => { + let _ = incoming_sender.send(XrNetIncoming::Activity { peer, control }); + } + XrNetDataPacket::BodySpawn { spawn, .. } => { + let _ = incoming_sender.send(XrNetIncoming::BodySpawn { peer, spawn }); + } + XrNetDataPacket::SharedObjectStates { states, .. } => { + for state in states { + let _ = incoming_sender.send(XrNetIncoming::SharedObjectState { peer, state }); + } + } + XrNetDataPacket::SharedObjectState { state, .. } => { + let _ = incoming_sender.send(XrNetIncoming::SharedObjectState { peer, state }); + } + XrNetDataPacket::SharedObjectControl { control, .. } => { + let _ = incoming_sender.send(XrNetIncoming::SharedObjectControl { peer, control }); + } + XrNetDataPacket::Leave(_) => {} + } + } +} + +struct XrNetSyncConnection { + stream: TcpStream, + peer_id: Option, + read_buf: Vec, + write_buf: Vec, + handshake_received: bool, +} + +impl XrNetSyncConnection { + fn new(stream: TcpStream, peer_id: Option) -> io::Result { + stream.set_nonblocking(true)?; + let _ = stream.set_nodelay(true); + Ok(Self { + stream, + peer_id, + read_buf: Vec::new(), + write_buf: Vec::new(), + handshake_received: false, + }) + } + + fn queue_packet(&mut self, packet: &XrNetSyncPacket) { + let Some(payload) = XrNetSyncFrameCodec::encode(packet) else { + return; + }; + let frame_len = payload.len().min(u32::MAX as usize) as u32; + self.write_buf.extend_from_slice(&frame_len.to_le_bytes()); + self.write_buf + .extend_from_slice(&payload[..frame_len as usize]); + } + + fn queue_hello(&mut self, node_id: XrNetPeerId) { + self.queue_packet(&XrNetSyncPacket::hello(node_id)); + } + + fn pump(&mut self) -> io::Result> { + self.flush()?; + + let mut read_chunk = [0u8; 16384]; + let mut total_read = 0usize; + loop { + match self.stream.read(&mut read_chunk) { + Ok(0) => { + return Err(io::Error::new( + io::ErrorKind::UnexpectedEof, + "sync connection closed", + )) + } + Ok(len) => { + self.read_buf.extend_from_slice(&read_chunk[..len]); + total_read += len; + if total_read >= XR_NET_SYNC_READ_BUDGET_BYTES_PER_POLL { + break; + } + } + Err(err) if err.kind() == io::ErrorKind::WouldBlock => break, + Err(err) => return Err(err), + } + } + XrNetSyncFrameCodec::drain_packets(&mut self.read_buf) + } + + fn flush(&mut self) -> io::Result<()> { + let mut total_written = 0usize; + while !self.write_buf.is_empty() { + match self.stream.write(&self.write_buf) { + Ok(0) => { + return Err(io::Error::new( + io::ErrorKind::WriteZero, + "sync connection write zero", + )) + } + Ok(written) => { + Self::consume_written_prefix(&mut self.write_buf, written); + total_written += written; + if total_written >= XR_NET_SYNC_WRITE_BUDGET_BYTES_PER_POLL { + break; + } + } + Err(err) if err.kind() == io::ErrorKind::WouldBlock => break, + Err(err) => return Err(err), + } + } + Ok(()) + } + + fn consume_written_prefix(buf: &mut Vec, len: usize) { + if len == 0 { + return; + } + if len >= buf.len() { + buf.clear(); + return; + } + let remaining = buf.len() - len; + buf.copy_within(len.., 0); + buf.truncate(remaining); + } + + fn shutdown(&mut self) { + let _ = self.stream.shutdown(Shutdown::Both); + } +} + +struct XrNetUdpWorker { + node_id: XrNetPeerId, + discovery_targets: Vec, + discovery_interval: Duration, + peer_timeout: Duration, + poll_interval: Duration, + bound_data_port: u16, + bound_sync_port: u16, + discovery_socket: UdpSocket, + data_socket: UdpSocket, + incoming_sender: mpsc::Sender, + outgoing_receiver: mpsc::Receiver, + sync_outgoing_sender: mpsc::Sender, + thread_loop: Arc, +} + +impl XrNetUdpWorker { + fn run(self) { + let mut peers = HashMap::::new(); + let mut cached_state = None; + let mut next_discovery_at = Instant::now(); + let mut read_buf = [0u8; 65536]; + + while self.thread_loop.load(Ordering::Relaxed) { + let now = Instant::now(); + if now >= next_discovery_at { + self.send_discovery_hello(); + next_discovery_at = now + self.discovery_interval; + } + + let should_break = self.drain_outgoing_messages(&mut peers, &mut cached_state); + self.process_data_packets(&mut read_buf, &mut peers); + self.process_discovery_packets(&mut read_buf, &mut peers, &cached_state); + self.expire_timed_out_peers(&mut peers); + + if should_break { + break; + } + thread::sleep(self.poll_interval); + } + + self.broadcast_explicit_leave(&peers); + self.broadcast_discovery_leave(); + } + + fn send_discovery_hello(&self) { + let buf = + XrNetDiscoveryPacket::hello(self.node_id, self.bound_data_port, self.bound_sync_port) + .to_bytes(); + for target in &self.discovery_targets { + let _ = self.discovery_socket.send_to(&buf, target); + } + } + + fn broadcast_discovery_leave(&self) { + let buf = XrNetDiscoveryPacket::leave(self.node_id).to_bytes(); + for target in &self.discovery_targets { + let _ = self.discovery_socket.send_to(&buf, target); + } + } + + fn broadcast_explicit_leave(&self, peers: &HashMap) { + let buf = XrNetDataPacket::leave(self.node_id).to_bytes(); + for peer in peers.values() { + let _ = self.data_socket.send_to(&buf, peer.peer.addr); + } + } + + fn drain_outgoing_messages( + &self, + peers: &mut HashMap, + cached_state: &mut Option, + ) -> bool { + loop { + match self.outgoing_receiver.try_recv() { + Ok(XrNetUdpOutgoing::State(frame)) => { + *cached_state = Some(frame.clone()); + let buf = XrNetDataPacket::state(self.node_id, frame).to_bytes(); + for peer in peers.values() { + let _ = self.data_socket.send_to(&buf, peer.peer.addr); + } + } + Ok(XrNetUdpOutgoing::SharedObjectStates(states)) => { + let packets = self.shared_object_state_packets(states); + for buf in &packets { + for peer in peers.values() { + let _ = self.data_socket.send_to(buf, peer.peer.addr); + } + } + } + Ok(XrNetUdpOutgoing::Break) => return true, + Err(mpsc::TryRecvError::Empty) => return false, + Err(mpsc::TryRecvError::Disconnected) => return true, + } + } + } + + fn process_discovery_packets( + &self, + read_buf: &mut [u8], + peers: &mut HashMap, + cached_state: &Option, + ) { + loop { + match self.discovery_socket.recv_from(read_buf) { + Ok((len, source_addr)) => { + let Some(packet) = XrNetDiscoveryPacket::from_bytes(&read_buf[..len]) else { + continue; + }; + match packet { + XrNetDiscoveryPacket::Hello(packet) => { + if !packet.is_compatible_for(self.node_id) { + continue; + } + let peer_addr = SocketAddr::new(source_addr.ip(), packet.data_port); + let peer_sync_addr = + SocketAddr::new(source_addr.ip(), packet.sync_port); + let (peer, is_new) = + self.touch_peer(peers, packet.node_id, peer_addr, peer_sync_addr); + if is_new { + self.send_cached_state_to_peer(peer, cached_state); + } + } + XrNetDiscoveryPacket::Leave(packet) => { + if !packet.is_compatible_for(self.node_id) { + continue; + } + self.remove_peer(peers, packet.node_id, XrNetLeaveReason::Explicit); + } + } + } + Err(err) if err.kind() == io::ErrorKind::WouldBlock => return, + Err(_) => return, + } + } + } + + fn process_data_packets( + &self, + read_buf: &mut [u8], + peers: &mut HashMap, + ) { + loop { + match self.data_socket.recv_from(read_buf) { + Ok((len, source_addr)) => { + let Some(packet) = XrNetDataPacket::from_bytes(&read_buf[..len]) else { + continue; + }; + if !packet.is_compatible_for(self.node_id) { + continue; + } + match packet { + XrNetDataPacket::State { + node_id: remote_id, + frame, + .. + } => { + let sync_addr = peers + .get(&remote_id) + .map(|peer_state| peer_state.sync_addr) + .unwrap_or_else(|| { + SocketAddr::new(source_addr.ip(), XR_NET_DEFAULT_SYNC_PORT) + }); + let (peer, _) = + self.touch_peer(peers, remote_id, source_addr, sync_addr); + let Some(peer_state) = peers.get_mut(&remote_id) else { + continue; + }; + if !accept_seq(peer_state.last_state_seq, frame.seq) { + continue; + } + peer_state.last_state_seq = Some(frame.seq); + let _ = self + .incoming_sender + .send(XrNetIncoming::State { peer, frame }); + } + XrNetDataPacket::Alignment { + node_id: remote_id, + frame, + .. + } => { + let sync_addr = peers + .get(&remote_id) + .map(|peer_state| peer_state.sync_addr) + .unwrap_or_else(|| { + SocketAddr::new(source_addr.ip(), XR_NET_DEFAULT_SYNC_PORT) + }); + let (peer, _) = + self.touch_peer(peers, remote_id, source_addr, sync_addr); + let Some(peer_state) = peers.get_mut(&remote_id) else { + continue; + }; + if !accept_seq(peer_state.last_alignment_seq, frame.seq) { + continue; + } + peer_state.last_alignment_seq = Some(frame.seq); + let _ = self + .incoming_sender + .send(XrNetIncoming::Alignment { peer, frame }); + } + XrNetDataPacket::AlignmentDescriptor { + node_id: remote_id, + frame, + .. + } => { + let sync_addr = peers + .get(&remote_id) + .map(|peer_state| peer_state.sync_addr) + .unwrap_or_else(|| { + SocketAddr::new(source_addr.ip(), XR_NET_DEFAULT_SYNC_PORT) + }); + let (peer, _) = + self.touch_peer(peers, remote_id, source_addr, sync_addr); + let Some(peer_state) = peers.get_mut(&remote_id) else { + continue; + }; + if !accept_seq(peer_state.last_alignment_descriptor_seq, frame.seq) { + continue; + } + peer_state.last_alignment_descriptor_seq = Some(frame.seq); + let _ = self + .incoming_sender + .send(XrNetIncoming::AlignmentDescriptor { peer, frame }); + } + XrNetDataPacket::SharedObjectState { + node_id: remote_id, + state, + .. + } => { + let sync_addr = peers + .get(&remote_id) + .map(|peer_state| peer_state.sync_addr) + .unwrap_or_else(|| { + SocketAddr::new(source_addr.ip(), XR_NET_DEFAULT_SYNC_PORT) + }); + let (peer, _) = + self.touch_peer(peers, remote_id, source_addr, sync_addr); + let Some(peer_state) = peers.get_mut(&remote_id) else { + continue; + }; + if !accept_seq(peer_state.last_shared_object_state_seq, state.seq) { + continue; + } + peer_state.last_shared_object_state_seq = Some(state.seq); + let _ = self + .incoming_sender + .send(XrNetIncoming::SharedObjectState { peer, state }); + } + XrNetDataPacket::SharedObjectStates { + node_id: remote_id, + states, + .. + } => { + let sync_addr = peers + .get(&remote_id) + .map(|peer_state| peer_state.sync_addr) + .unwrap_or_else(|| { + SocketAddr::new(source_addr.ip(), XR_NET_DEFAULT_SYNC_PORT) + }); + let (peer, _) = + self.touch_peer(peers, remote_id, source_addr, sync_addr); + let Some(peer_state) = peers.get_mut(&remote_id) else { + continue; + }; + for state in states { + if !accept_seq(peer_state.last_shared_object_state_seq, state.seq) { + continue; + } + peer_state.last_shared_object_state_seq = Some(state.seq); + let _ = self + .incoming_sender + .send(XrNetIncoming::SharedObjectState { peer, state }); + } + } + XrNetDataPacket::Leave(packet) => { + self.remove_peer(peers, packet.node_id, XrNetLeaveReason::Explicit); + } + XrNetDataPacket::ActivityControl { .. } => {} + XrNetDataPacket::BodySpawn { .. } => {} + XrNetDataPacket::SharedObjectControl { .. } => {} + } + } + Err(err) if err.kind() == io::ErrorKind::WouldBlock => return, + Err(_) => return, + } + } + } + + fn touch_peer( + &self, + peers: &mut HashMap, + peer_id: XrNetPeerId, + addr: SocketAddr, + sync_addr: SocketAddr, + ) -> (XrNetPeer, bool) { + let now = Instant::now(); + if let Some(peer_state) = peers.get_mut(&peer_id) { + let peer_changed = peer_state.peer.addr != addr || peer_state.sync_addr != sync_addr; + peer_state.peer.addr = addr; + peer_state.sync_addr = sync_addr; + peer_state.last_seen = now; + if peer_changed { + let _ = self.sync_outgoing_sender.send(XrNetSyncOutgoing::PeerUp { + peer: peer_state.peer, + sync_addr, + }); + } + (peer_state.peer, false) + } else { + let peer = XrNetPeer { id: peer_id, addr }; + peers.insert(peer_id, UdpWorkerPeerState::new(peer, sync_addr, now)); + let _ = self.incoming_sender.send(XrNetIncoming::Join { peer }); + let _ = self + .sync_outgoing_sender + .send(XrNetSyncOutgoing::PeerUp { peer, sync_addr }); + (peer, true) + } + } + + fn send_cached_state_to_peer(&self, peer: XrNetPeer, cached_state: &Option) { + if let Some(frame) = cached_state { + let _ = self.data_socket.send_to( + &XrNetDataPacket::state(self.node_id, frame.clone()).to_bytes(), + peer.addr, + ); + } + } + + fn shared_object_state_packets(&self, states: Vec) -> Vec> { + let mut packets = Vec::new(); + let mut batch = Vec::new(); + + for state in states { + batch.push(state); + let batch_bytes = + XrNetDataPacket::shared_object_states(self.node_id, batch.clone()).to_bytes(); + if batch_bytes.len() <= XR_NET_UDP_SHARED_OBJECT_STATE_BATCH_MAX_BYTES { + continue; + } + + let state = batch.pop().unwrap(); + if !batch.is_empty() { + packets.push( + XrNetDataPacket::shared_object_states(self.node_id, std::mem::take(&mut batch)) + .to_bytes(), + ); + } + + let single = XrNetDataPacket::shared_object_state(self.node_id, state).to_bytes(); + if single.len() > XR_NET_UDP_SHARED_OBJECT_STATE_BATCH_MAX_BYTES { + packets.push(single); + } else { + batch.push(state); + } + } + + if !batch.is_empty() { + packets.push(XrNetDataPacket::shared_object_states(self.node_id, batch).to_bytes()); + } + + packets + } + + fn remove_peer( + &self, + peers: &mut HashMap, + peer_id: XrNetPeerId, + reason: XrNetLeaveReason, + ) { + let Some(peer_state) = peers.remove(&peer_id) else { + return; + }; + let _ = self + .sync_outgoing_sender + .send(XrNetSyncOutgoing::PeerRemoved { peer_id }); + let _ = self.incoming_sender.send(XrNetIncoming::Leave { + peer: peer_state.peer, + reason, + }); + } + + fn expire_timed_out_peers(&self, peers: &mut HashMap) { + let now = Instant::now(); + let expired: Vec<_> = peers + .iter() + .filter_map(|(peer_id, peer_state)| { + (now.duration_since(peer_state.last_seen) > self.peer_timeout).then_some(*peer_id) + }) + .collect(); + for peer_id in expired { + self.remove_peer(peers, peer_id, XrNetLeaveReason::Timeout); + } + } +} + +struct XrNetSyncWorker { + node_id: XrNetPeerId, + poll_interval: Duration, + sync_listener: TcpListener, + incoming_sender: mpsc::Sender, + outgoing_receiver: mpsc::Receiver, + thread_loop: Arc, +} + +impl XrNetSyncWorker { + fn run(self) { + let mut peers = HashMap::::new(); + let mut pending_sync_connections = Vec::::new(); + let mut cached_alignment = None; + let mut cached_alignment_descriptor = None; + let mut cached_activity = None; + let mut cached_shared_object_controls = + HashMap::::new(); + + while self.thread_loop.load(Ordering::Relaxed) { + let should_break = self.drain_outgoing_messages( + &mut peers, + &mut cached_alignment, + &mut cached_alignment_descriptor, + &mut cached_activity, + &mut cached_shared_object_controls, + ); + self.accept_connections(&mut pending_sync_connections); + self.ensure_outbound_connections(&mut peers); + self.process_pending_connections( + &mut pending_sync_connections, + &mut peers, + &cached_alignment, + &cached_alignment_descriptor, + &cached_activity, + &cached_shared_object_controls, + ); + self.process_connections( + &mut peers, + &cached_alignment, + &cached_alignment_descriptor, + &cached_activity, + &cached_shared_object_controls, + ); + + if should_break { + break; + } + thread::sleep(self.poll_interval); + } + + for peer_state in peers.values_mut() { + peer_state.close_connection(); + } + for pending in &mut pending_sync_connections { + pending.shutdown(); + } + } + + fn drain_outgoing_messages( + &self, + peers: &mut HashMap, + cached_alignment: &mut Option, + cached_alignment_descriptor: &mut Option, + cached_activity: &mut Option, + cached_shared_object_controls: &mut HashMap, + ) -> bool { + loop { + match self.outgoing_receiver.try_recv() { + Ok(XrNetSyncOutgoing::PeerUp { peer, sync_addr }) => { + self.register_peer(peers, peer, sync_addr); + } + Ok(XrNetSyncOutgoing::PeerRemoved { peer_id }) => { + self.remove_peer(peers, peer_id); + } + Ok(XrNetSyncOutgoing::Alignment(frame)) => { + *cached_alignment = Some(frame); + let packet = XrNetDataPacket::alignment(self.node_id, frame); + for peer_state in peers.values_mut() { + if let Some(connection) = peer_state.sync_connection.as_mut() { + connection.queue_packet(&XrNetSyncPacket::data(packet.clone())); + } + } + } + Ok(XrNetSyncOutgoing::AlignmentDescriptor(frame)) => { + *cached_alignment_descriptor = Some(frame.clone()); + let packet = XrNetDataPacket::alignment_descriptor(self.node_id, frame); + for peer_state in peers.values_mut() { + if let Some(connection) = peer_state.sync_connection.as_mut() { + connection.queue_packet(&XrNetSyncPacket::data(packet.clone())); + } + } + } + Ok(XrNetSyncOutgoing::Activity(control)) => { + *cached_activity = Some(control.clone()); + cached_shared_object_controls.clear(); + let packet = XrNetDataPacket::activity_control(self.node_id, control); + for peer_state in peers.values_mut() { + if let Some(connection) = peer_state.sync_connection.as_mut() { + connection.queue_packet(&XrNetSyncPacket::data(packet.clone())); + } + } + } + Ok(XrNetSyncOutgoing::BodySpawn(spawn)) => { + let packet = XrNetDataPacket::body_spawn(self.node_id, spawn); + for peer_state in peers.values_mut() { + if let Some(connection) = peer_state.sync_connection.as_mut() { + connection.queue_packet(&XrNetSyncPacket::data(packet.clone())); + } + } + } + Ok(XrNetSyncOutgoing::SharedObjectControl(control)) => { + Self::cache_shared_object_control(cached_shared_object_controls, &control); + let packet = XrNetDataPacket::shared_object_control(self.node_id, control); + for peer_state in peers.values_mut() { + if let Some(connection) = peer_state.sync_connection.as_mut() { + connection.queue_packet(&XrNetSyncPacket::data(packet.clone())); + } + } + } + Ok(XrNetSyncOutgoing::Break) => return true, + Err(mpsc::TryRecvError::Empty) => return false, + Err(mpsc::TryRecvError::Disconnected) => return true, + } + } + } + + fn register_peer( + &self, + peers: &mut HashMap, + peer: XrNetPeer, + sync_addr: SocketAddr, + ) { + let now = Instant::now(); + if let Some(peer_state) = peers.get_mut(&peer.id) { + let sync_addr_changed = peer_state.sync_addr != sync_addr; + peer_state.peer = peer; + peer_state.sync_addr = sync_addr; + if sync_addr_changed { + peer_state.close_connection(); + peer_state.next_sync_connect_attempt_at = now; + } + } else { + peers.insert(peer.id, SyncWorkerPeerState::new(peer, sync_addr, now)); + } + } + + fn remove_peer( + &self, + peers: &mut HashMap, + peer_id: XrNetPeerId, + ) { + let Some(mut peer_state) = peers.remove(&peer_id) else { + return; + }; + peer_state.close_connection(); + } + + fn accept_connections(&self, pending_sync_connections: &mut Vec) { + loop { + match self.sync_listener.accept() { + Ok((stream, _)) => { + let Ok(mut connection) = XrNetSyncConnection::new(stream, None) else { + continue; + }; + connection.queue_hello(self.node_id); + pending_sync_connections.push(connection); + } + Err(err) if err.kind() == io::ErrorKind::WouldBlock => return, + Err(_) => return, + } + } + } + + fn ensure_outbound_connections(&self, peers: &mut HashMap) { + let now = Instant::now(); + let peer_ids = peers.keys().copied().collect::>(); + for peer_id in peer_ids { + let Some(peer_state) = peers.get_mut(&peer_id) else { + continue; + }; + if peer_state.sync_connection.is_some() + || !Self::should_initiate_connection(self.node_id, peer_id) + || now < peer_state.next_sync_connect_attempt_at + { + continue; + } + peer_state.next_sync_connect_attempt_at = now + XR_NET_DEFAULT_SYNC_CONNECT_RETRY; + let Ok(stream) = + TcpStream::connect_timeout(&peer_state.sync_addr, XR_NET_SYNC_CONNECT_TIMEOUT) + else { + continue; + }; + let Ok(mut connection) = XrNetSyncConnection::new(stream, Some(peer_id)) else { + continue; + }; + connection.queue_hello(self.node_id); + peer_state.sync_connection = Some(connection); + } + } + + fn process_pending_connections( + &self, + pending_sync_connections: &mut Vec, + peers: &mut HashMap, + cached_alignment: &Option, + cached_alignment_descriptor: &Option, + cached_activity: &Option, + cached_shared_object_controls: &HashMap, + ) { + let pending_count = pending_sync_connections.len(); + for _ in 0..pending_count { + let mut connection = pending_sync_connections.swap_remove(0); + let Ok(packets) = connection.pump() else { + connection.shutdown(); + continue; + }; + let mut queued_data = Vec::::new(); + let mut invalid = false; + + for packet in packets { + match packet { + XrNetSyncPacket::Hello(hello) => { + if !hello.is_compatible_for(self.node_id) { + invalid = true; + break; + } + if connection + .peer_id + .is_some_and(|existing| existing != hello.node_id) + { + invalid = true; + break; + } + connection.peer_id = Some(hello.node_id); + connection.handshake_received = true; + } + XrNetSyncPacket::Data(data) => { + if !connection.handshake_received { + invalid = true; + break; + } + queued_data.push(data); + } + } + } + + if invalid { + connection.shutdown(); + continue; + } + + let Some(peer_id) = connection.peer_id else { + pending_sync_connections.push(connection); + continue; + }; + let Some(peer_state) = peers.get_mut(&peer_id) else { + pending_sync_connections.push(connection); + continue; + }; + + peer_state.close_connection(); + connection.handshake_received = true; + self.send_cached_frames_to_connection( + &mut connection, + cached_alignment, + cached_alignment_descriptor, + cached_activity, + cached_shared_object_controls, + ); + peer_state.sync_connection = Some(connection); + + for data in queued_data { + peer_state.route_data_packet(&self.incoming_sender, self.node_id, data); + } + } + } + + fn process_connections( + &self, + peers: &mut HashMap, + cached_alignment: &Option, + cached_alignment_descriptor: &Option, + cached_activity: &Option, + cached_shared_object_controls: &HashMap, + ) { + let peer_ids = peers.keys().copied().collect::>(); + for peer_id in peer_ids { + let Some(mut connection) = peers + .get_mut(&peer_id) + .and_then(|peer_state| peer_state.sync_connection.take()) + else { + continue; + }; + + let Ok(packets) = connection.pump() else { + connection.shutdown(); + continue; + }; + + let mut invalid = false; + let mut became_ready = false; + let mut queued_data = Vec::::new(); + for packet in packets { + match packet { + XrNetSyncPacket::Hello(hello) => { + if !hello.matches_peer(peer_id) { + invalid = true; + break; + } + if !connection.handshake_received { + connection.handshake_received = true; + became_ready = true; + } + } + XrNetSyncPacket::Data(data) => { + if !connection.handshake_received { + invalid = true; + break; + } + queued_data.push(data); + } + } + } + + let Some(peer_state) = peers.get_mut(&peer_id) else { + connection.shutdown(); + continue; + }; + if invalid { + connection.shutdown(); + peer_state.next_sync_connect_attempt_at = + Instant::now() + XR_NET_DEFAULT_SYNC_CONNECT_RETRY; + continue; + } + if became_ready { + self.send_cached_frames_to_connection( + &mut connection, + cached_alignment, + cached_alignment_descriptor, + cached_activity, + cached_shared_object_controls, + ); + } + for data in queued_data { + peer_state.route_data_packet(&self.incoming_sender, self.node_id, data); + } + peer_state.sync_connection = Some(connection); + } + } + + fn send_cached_frames_to_connection( + &self, + connection: &mut XrNetSyncConnection, + cached_alignment: &Option, + cached_alignment_descriptor: &Option, + cached_activity: &Option, + cached_shared_object_controls: &HashMap, + ) { + if let Some(frame) = cached_alignment { + connection.queue_packet(&XrNetSyncPacket::data(XrNetDataPacket::alignment( + self.node_id, + *frame, + ))); + } + if let Some(frame) = cached_alignment_descriptor { + connection.queue_packet(&XrNetSyncPacket::data( + XrNetDataPacket::alignment_descriptor(self.node_id, frame.clone()), + )); + } + if let Some(control) = cached_activity { + connection.queue_packet(&XrNetSyncPacket::data(XrNetDataPacket::activity_control( + self.node_id, + control.clone(), + ))); + } + let mut object_ids = cached_shared_object_controls + .keys() + .copied() + .collect::>(); + object_ids.sort_by_key(|object_id| object_id.0); + for object_id in object_ids { + let Some(control) = cached_shared_object_controls.get(&object_id) else { + continue; + }; + connection.queue_packet(&XrNetSyncPacket::data( + XrNetDataPacket::shared_object_control(self.node_id, control.clone()), + )); + } + } + + fn cache_shared_object_control( + cached_shared_object_controls: &mut HashMap, + control: &XrNetSharedObjectControl, + ) { + match control { + XrNetSharedObjectControl::XrSpawnObject { object_id, .. } => { + cached_shared_object_controls.insert(*object_id, control.clone()); + } + XrNetSharedObjectControl::XrTakeoverAccept { + object_id, + epoch, + new_authority, + pose, + linvel, + angvel, + .. + } => { + if let Some(XrNetSharedObjectControl::XrSpawnObject { + epoch: cached_epoch, + authority, + pose: cached_pose, + linvel: cached_linvel, + angvel: cached_angvel, + .. + }) = cached_shared_object_controls.get_mut(object_id) + { + *cached_epoch = *epoch; + *authority = *new_authority; + *cached_pose = *pose; + *cached_linvel = *linvel; + *cached_angvel = *angvel; + } + } + XrNetSharedObjectControl::XrResetObject { + object_id, + epoch, + pose, + linvel, + angvel, + } => { + if let Some(XrNetSharedObjectControl::XrSpawnObject { + epoch: cached_epoch, + pose: cached_pose, + linvel: cached_linvel, + angvel: cached_angvel, + .. + }) = cached_shared_object_controls.get_mut(object_id) + { + *cached_epoch = *epoch; + *cached_pose = *pose; + *cached_linvel = *linvel; + *cached_angvel = *angvel; + } + } + XrNetSharedObjectControl::XrDespawnObject { object_id, .. } => { + cached_shared_object_controls.remove(object_id); + } + _ => {} + } + } + + fn should_initiate_connection(local_node_id: XrNetPeerId, peer_id: XrNetPeerId) -> bool { + local_node_id.0 < peer_id.0 + } +} + +pub(super) fn spawn_udp_worker_thread( + node_id: XrNetPeerId, + discovery_targets: Vec, + discovery_interval: Duration, + peer_timeout: Duration, + poll_interval: Duration, + bound_data_port: u16, + bound_sync_port: u16, + discovery_socket: UdpSocket, + data_socket: UdpSocket, + incoming_sender: mpsc::Sender, + outgoing_receiver: mpsc::Receiver, + sync_outgoing_sender: mpsc::Sender, + thread_loop: Arc, +) -> JoinHandle<()> { + thread::spawn(move || { + XrNetUdpWorker { + node_id, + discovery_targets, + discovery_interval, + peer_timeout, + poll_interval, + bound_data_port, + bound_sync_port, + discovery_socket, + data_socket, + incoming_sender, + outgoing_receiver, + sync_outgoing_sender, + thread_loop, + } + .run(); + }) +} + +pub(super) fn spawn_sync_worker_thread( + node_id: XrNetPeerId, + poll_interval: Duration, + sync_listener: TcpListener, + incoming_sender: mpsc::Sender, + outgoing_receiver: mpsc::Receiver, + thread_loop: Arc, +) -> JoinHandle<()> { + thread::spawn(move || { + XrNetSyncWorker { + node_id, + poll_interval, + sync_listener, + incoming_sender, + outgoing_receiver, + thread_loop, + } + .run(); + }) +} diff --git a/xr/src/obj/cube.rs b/xr/src/obj/cube.rs index 1956c7adf0..e6bc78b1f7 100644 --- a/xr/src/obj/cube.rs +++ b/xr/src/obj/cube.rs @@ -1,6 +1,7 @@ -use crate::{makepad_derive_widget::*, makepad_draw::*, widget::*}; +use crate::scene::{xr_widget_world_transform, XrNode}; +use makepad_widgets::{makepad_derive_widget::*, makepad_draw::*, widget::*}; -use super::{scene_draw::apply_scene_to_draw_cube, xr_node::xr_widget_world_transform, XrNode}; +use crate::util::scene_draw::apply_scene_to_draw_cube; script_mod! { use mod.prelude.widgets_internal.* @@ -8,6 +9,7 @@ script_mod! { mod.widgets.CubeBase = #(Cube::register_widget(vm)) mod.widgets.Cube = set_type_default() do mod.widgets.CubeBase{ body: mod.widgets.XrBodyKind.Dynamic + shared_object_policy: mod.widgets.XrSharedObjectPolicy.BootstrapShared size: vec3(0.1, 0.1, 0.1) color: vec4(0.82, 0.48, 0.28, 1.0) draw_cube +: { diff --git a/xr/src/obj/gltf.rs b/xr/src/obj/gltf.rs index 4b847dcfbd..520441bd81 100644 --- a/xr/src/obj/gltf.rs +++ b/xr/src/obj/gltf.rs @@ -1,11 +1,10 @@ -use crate::{makepad_derive_widget::*, makepad_draw::*, widget::*}; +use crate::render::GltfRenderer; +use crate::scene::{xr_widget_world_transform, XrNode}; +use makepad_widgets::{makepad_derive_widget::*, makepad_draw::*, widget::*}; use std::{path::PathBuf, rc::Rc}; -use super::{ - gltf_bridge::GltfRenderer, - scene_draw::{apply_scene_to_draw_pbr, compose_scene_node_transform, scene_state_from_cx}, - xr_node::xr_widget_world_transform, - XrNode, +use crate::util::scene_draw::{ + apply_scene_to_draw_pbr, compose_scene_node_transform, scene_state_from_cx, }; script_mod! { diff --git a/xr/src/obj/icosphere.rs b/xr/src/obj/icosphere.rs index 970ae533f7..0b83375976 100644 --- a/xr/src/obj/icosphere.rs +++ b/xr/src/obj/icosphere.rs @@ -1,11 +1,7 @@ -use crate::{makepad_derive_widget::*, makepad_draw::*, widget::*}; +use crate::scene::{xr_widget_world_transform, XrDrawContext, XrNode}; +use makepad_widgets::{makepad_derive_widget::*, makepad_draw::*, widget::*}; use std::cell::RefCell; -use super::{ - xr_node::{xr_env_texture_from_scope, xr_widget_world_transform}, - XrNode, -}; - const ICO_SPHERE_PHYSICS_DIAMETER_SCALE: f32 = 0.88; thread_local! { @@ -108,6 +104,8 @@ script_mod! { mod.widgets.IcoSphereBase = #(IcoSphere::register_widget(vm)) mod.widgets.IcoSphere = set_type_default() do mod.widgets.IcoSphereBase{ body: mod.widgets.XrBodyKind.Dynamic + physics_shape: mod.widgets.XrPhysicsShape.Sphere + shared_object_policy: mod.widgets.XrSharedObjectPolicy.BootstrapShared radius: 0.037 diffuse: vec4(0.63, 0.65, 0.69, 1.0) color: vec4(0.95, 0.62, 0.28, 1.0) @@ -258,8 +256,8 @@ impl Widget for IcoSphere { self.draw_ico.diffuse = self.diffuse; self.draw_ico.color = self.color; self.draw_ico.depth_clip = 1.0; - self.draw_ico - .set_env_texture(xr_env_texture_from_scope(scope)); + let draw_context = XrDrawContext::from_scope(scope); + self.draw_ico.set_env_texture(draw_context.env_texture()); self.draw_ico.draw(cx, geometry_id); self.node.draw_3d(cx, scope) diff --git a/xr/src/obj/mod.rs b/xr/src/obj/mod.rs new file mode 100644 index 0000000000..d89a465d91 --- /dev/null +++ b/xr/src/obj/mod.rs @@ -0,0 +1,21 @@ +pub mod cube; +pub mod gltf; +pub mod icosphere; +pub mod physics_view; +pub mod refractive_cube; +pub mod shooter; +pub mod tank; +pub mod tree; +pub mod view_splat; + +pub use cube::Cube; +pub use gltf::Gltf; +pub use icosphere::{DrawIcoSolid, IcoSphere}; +pub use physics_view::PhysicsWorld3D; +pub use refractive_cube::RefractiveCube; +pub use shooter::{Shooter, XrProjectileEmitterConfig}; +pub use tank::Tank; +pub use tree::{ + CpuPythagoreanTree, DrawTreeBranches, DrawTreeLeaves, Tree, PYTHAGOREAN_TREE_ROOT_DROP, +}; +pub use view_splat::{DrawSplatPbr, ViewSplat}; diff --git a/xr/src/obj/physics_view.rs b/xr/src/obj/physics_view.rs index 61edebe223..1bc7dc55ae 100644 --- a/xr/src/obj/physics_view.rs +++ b/xr/src/obj/physics_view.rs @@ -1,13 +1,13 @@ -use ::rapier3d::prelude::{ +use makepad_widgets::event::TouchState; +use makepad_widgets::*; +use rapier3d::prelude::{ BroadPhaseBvh, CCDSolver, ColliderBuilder, ColliderSet, ImpulseJointSet, IntegrationParameters, IslandManager, MultibodyJointSet, NarrowPhase, PhysicsPipeline, QueryFilter, Ray as RapierRay, Real as RapierReal, RigidBodyBuilder, RigidBodyHandle, RigidBodySet, Rotation as RapierRotation, SharedShape, Vector as RapierVector, }; -use makepad_widgets::event::TouchState; -use makepad_widgets::*; -use crate::scene_draw::{ +use crate::util::scene_draw::{ apply_scene_to_draw_pbr, ray_from_scene_viewport, scene_state_from_cx, SceneState3D, }; diff --git a/xr/src/obj/refractive_cube.rs b/xr/src/obj/refractive_cube.rs index d48ca42bd9..2fffcecf27 100644 --- a/xr/src/obj/refractive_cube.rs +++ b/xr/src/obj/refractive_cube.rs @@ -1,11 +1,7 @@ -use crate::{makepad_derive_widget::*, makepad_draw::*, widget::*}; +use crate::scene::{xr_widget_world_transform, XrDrawContext, XrNode}; +use makepad_widgets::{makepad_derive_widget::*, makepad_draw::*, widget::*}; -use super::{ - scene_draw::{apply_scene_to_draw_pbr, scene_state_from_cx}, - xr_node::{ - xr_env_texture_from_scope, xr_passthrough_from_scope, xr_widget_world_transform, XrNode, - }, -}; +use crate::util::scene_draw::{apply_scene_to_draw_pbr, scene_state_from_cx}; const XR_REFRACTIVE_CAMERA_FOV_Y_DEGREES: f32 = 92.0; const XR_REFRACTIVE_CAMERA_PROJECTION_SCALE: f32 = 0.6825; @@ -19,6 +15,8 @@ script_mod! { mod.widgets.RefractiveCubeBase = #(RefractiveCube::register_widget(vm)) mod.widgets.RefractiveCube = set_type_default() do mod.widgets.RefractiveCubeBase{ body: mod.widgets.XrBodyKind.Dynamic + render_class: mod.widgets.XrRenderClass.Transparent + shared_object_policy: mod.widgets.XrSharedObjectPolicy.BootstrapShared size: vec3(0.12, 0.12, 0.12) color: vec4(0.80, 0.92, 1.0, 0.18) corner_radius: 0.024 @@ -96,7 +94,8 @@ impl Widget for RefractiveCube { let half_extents = self.half_extents(); let _ = apply_scene_to_draw_pbr(&mut self.draw_pbr.draw_super, cx); - let passthrough = xr_passthrough_from_scope(scope); + let draw_context = XrDrawContext::from_scope(scope); + let passthrough = draw_context.passthrough(); self.draw_pbr.source_size = passthrough.source_size; self.draw_pbr.camera_enabled = if passthrough.enabled { 1.0 } else { 0.0 }; self.draw_pbr.rotation_steps = passthrough.rotation_steps; @@ -122,7 +121,7 @@ impl Widget for RefractiveCube { self.draw_pbr.transmission_focus_distance = self.focus_distance; self.draw_pbr.set_depth_write(true); self.draw_pbr.set_camera_texture(passthrough.camera_texture); - if let Some(env_texture) = xr_env_texture_from_scope(scope) { + if let Some(env_texture) = draw_context.env_texture() { self.draw_pbr.set_env_face_textures(None); self.draw_pbr.set_env_texture(Some(env_texture)); self.draw_pbr.set_env_atlas_texture(None); diff --git a/xr/src/obj/shooter.rs b/xr/src/obj/shooter.rs index 139233ec37..cb82e06dfa 100644 --- a/xr/src/obj/shooter.rs +++ b/xr/src/obj/shooter.rs @@ -1,13 +1,12 @@ -use crate::{ +use crate::prelude::XrSharedObjectMode; +use crate::scene::{xr_widget_children, xr_widget_with_scene_node, XrBodySpawn, XrNode}; +use makepad_widgets::{ makepad_derive_widget::*, makepad_draw::*, widget::*, widget_async::{ScriptAsyncId, ScriptAsyncResult}, - XrBodySpawn, }; -use super::XrNode; - #[derive(Clone, Copy, Debug)] pub struct XrProjectileEmitterConfig { pub rate_hz: f32, @@ -44,6 +43,16 @@ pub struct Shooter { const SHOOTER_MAX_EMITS_PER_UPDATE: usize = 2; const SHOOTER_PROJECTILE_SPAWN_OFFSET: f32 = 0.064; +const SHOOTER_INDEX_BEND_MAX_DEGREES: f32 = 34.0; +const SHOOTER_INDEX_STRAIGHTNESS_MIN: f32 = 0.78; +const SHOOTER_INDEX_EXTENSION_RATIO_MIN: f32 = 0.90; + +#[derive(Clone, Copy)] +struct ShooterHandEmitMetrics { + max_bend_angle_degrees: f32, + straightness: f32, + extension_ratio: f32, +} impl Shooter { pub fn emitter_config(&self) -> XrProjectileEmitterConfig { @@ -81,13 +90,16 @@ impl Shooter { if !widget.visible() { return; } - if let Some(node) = widget.cast_inner::() { - if node.projectile_pool() { + if let Some(node) = xr_widget_with_scene_node(widget, |node| { + if node.spawn_pool() { projectile_pool_uids.push(widget.widget_uid()); } + }) { + let _ = node; } - widget - .children(&mut |_, child| Self::collect_projectile_pool(&child, projectile_pool_uids)); + xr_widget_children(widget, &mut |_, child| { + Self::collect_projectile_pool(&child, projectile_pool_uids) + }); } fn next_projectile_widget_uid(&mut self) -> Option { @@ -108,59 +120,83 @@ impl Shooter { (delta.length() > 0.0001).then_some(delta.normalize()) } - fn hand_index_finger_straight(hand: &XrHand) -> bool { - if !hand.in_view() || !hand.tip_active(XrHand::INDEX_TIP) { - return false; - } - - let base = hand.joints[XrHand::INDEX_BASE].position; - let knuckle1 = hand.joints[XrHand::INDEX_KNUCKLE1].position; - let knuckle2 = hand.joints[XrHand::INDEX_KNUCKLE2].position; - let knuckle3 = hand.joints[XrHand::INDEX_KNUCKLE3].position; - let tip = hand.tip_pos_index(); + fn hand_index_finger_stretch_metrics(hand: &XrHand) -> Option { + let points = hand.finger_chain_positions(XrHand::INDEX_TIP)?; + let [base, knuckle1, knuckle2, knuckle3, tip] = + [points[0], points[1], points[2], points[3], points[4]]; let Some(seg0) = Self::normalized_segment_direction(base, knuckle1) else { - return false; + return None; }; let Some(seg1) = Self::normalized_segment_direction(knuckle1, knuckle2) else { - return false; + return None; }; let Some(seg2) = Self::normalized_segment_direction(knuckle2, knuckle3) else { - return false; + return None; }; let Some(seg3) = Self::normalized_segment_direction(knuckle3, tip) else { - return false; + return None; }; let chain_length = (knuckle1 - base).length() + (knuckle2 - knuckle1).length() + (knuckle3 - knuckle2).length() + (tip - knuckle3).length(); - let direct_length = (tip - base).length(); if chain_length <= 0.0001 { - return false; + return None; + } + let direct_length = (tip - base).length(); + let segment_dots = [seg0.dot(seg1), seg1.dot(seg2), seg2.dot(seg3)]; + let max_bend_angle_degrees = segment_dots + .into_iter() + .map(|dot| dot.clamp(-1.0, 1.0).acos().to_degrees()) + .fold(0.0, f32::max); + let straightness = segment_dots.into_iter().fold(1.0, f32::min); + Some(ShooterHandEmitMetrics { + max_bend_angle_degrees, + straightness, + extension_ratio: direct_length / chain_length, + }) + } + + fn hand_index_forward_direction(hand: &XrHand) -> Option { + let points = hand.finger_chain_positions(XrHand::INDEX_TIP)?; + let [base, knuckle1, knuckle2, knuckle3, tip] = + [points[0], points[1], points[2], points[3], points[4]]; + + let seg0 = Self::normalized_segment_direction(base, knuckle1)?; + let seg1 = Self::normalized_segment_direction(knuckle1, knuckle2)?; + let seg2 = Self::normalized_segment_direction(knuckle2, knuckle3)?; + let seg3 = Self::normalized_segment_direction(knuckle3, tip)?; + + let blended = seg0 * 0.10 + seg1 * 0.20 + seg2 * 0.30 + seg3 * 0.40; + if blended.length() > 0.0001 { + return Some(blended.normalize()); } + Self::normalized_segment_direction(base, tip) + } - let straightness = seg0.dot(seg1).min(seg1.dot(seg2)).min(seg2.dot(seg3)); - let extension_ratio = direct_length / chain_length; - straightness >= 0.92 && extension_ratio >= 0.96 + fn hand_emit_gesture_active(hand: &XrHand) -> bool { + // Shooter owns its own firing gesture semantics. Do not couple emission to the + // generic OpenXR hand-grab bit, which represents a broader whole-hand intent and + // can stay active across close/open transitions on device. + if !hand.in_view() { + return false; + } + let Some(metrics) = Self::hand_index_finger_stretch_metrics(hand) else { + return false; + }; + metrics.max_bend_angle_degrees <= SHOOTER_INDEX_BEND_MAX_DEGREES + && metrics.straightness >= SHOOTER_INDEX_STRAIGHTNESS_MIN + && metrics.extension_ratio >= SHOOTER_INDEX_EXTENSION_RATIO_MIN } fn projectile_emitter_pose(hand: &XrHand) -> Option<(Vec3f, Vec3f)> { - if !Self::hand_index_finger_straight(hand) { + if !Self::hand_emit_gesture_active(hand) { return None; } - let tip_position = hand.tip_pos_index(); - let knuckle_position = hand.joints[XrHand::INDEX_KNUCKLE3].position; - let direction = tip_position - knuckle_position; - let direction = if direction.length() > 0.0001 { - direction.normalize() - } else { - hand.joints[XrHand::INDEX_KNUCKLE3] - .orientation - .rotate_vec3(&vec3f(0.0, 0.0, -1.0)) - .normalize() - }; + let tip_position = hand.tip_pos_checked(XrHand::INDEX_TIP)?; + let direction = Self::hand_index_forward_direction(hand)?; Some((tip_position, direction)) } @@ -207,6 +243,8 @@ impl Shooter { self.widget_uid(), XrBodySpawn { widget_uid, + shadow: false, + mode: XrSharedObjectMode::Dynamic, pose: Pose::new( Quat::default(), tip_position + direction * SHOOTER_PROJECTILE_SPAWN_OFFSET, @@ -266,3 +304,6 @@ impl Widget for Shooter { DrawStep::done() } } + +#[cfg(test)] +include!("../tests/obj/shooter.rs"); diff --git a/xr/src/obj/tank.rs b/xr/src/obj/tank.rs new file mode 100644 index 0000000000..f8e9b4144a --- /dev/null +++ b/xr/src/obj/tank.rs @@ -0,0 +1,376 @@ +use crate::prelude::*; +use crate::scene::{xr_widget_world_transform, XrDrawContext, XrNode}; +use crate::util::scene_draw::{apply_scene_to_draw_cube, compose_scene_node_transform}; +use makepad_widgets::{ + makepad_derive_widget::*, + makepad_draw::*, + widget::*, + widget_async::{ScriptAsyncId, ScriptAsyncResult}, +}; + +script_mod! { + use mod.prelude.widgets_internal.* + + mod.widgets.TankBase = #(Tank::register_widget(vm)) + mod.widgets.Tank = set_type_default() do mod.widgets.TankBase{ + body: mod.widgets.XrBodyKind.Dynamic + shared_object_policy: mod.widgets.XrSharedObjectPolicy.BootstrapShared + hull_size: vec3(0.24, 0.08, 0.34) + turret_size: vec3(0.13, 0.06, 0.13) + barrel_size: vec3(0.045, 0.028, 0.19) + hull_color: vec4(0.33, 0.42, 0.28, 1.0) + turret_color: vec4(0.40, 0.49, 0.34, 1.0) + barrel_color: vec4(0.18, 0.20, 0.16, 1.0) + density: 1.2 + friction: 1.3 + restitution: 0.02 + drive_impulse_per_second: 1.6 + turn_gain: 1.35 + max_speed_mps: 1.45 + stick_deadzone: 0.16 + draw_hull +: { + light_dir: vec3(0.35, 0.8, 0.45) + } + draw_turret +: { + light_dir: vec3(0.35, 0.8, 0.45) + } + draw_barrel +: { + light_dir: vec3(0.35, 0.8, 0.45) + } + } +} + +#[derive(Script, Widget)] +pub struct Tank { + #[redraw] + #[live] + draw_hull: DrawCube, + #[redraw] + #[live] + draw_turret: DrawCube, + #[redraw] + #[live] + draw_barrel: DrawCube, + #[live(vec3(0.24, 0.08, 0.34))] + hull_size: Vec3f, + #[live(vec3(0.13, 0.06, 0.13))] + turret_size: Vec3f, + #[live(vec3(0.045, 0.028, 0.19))] + barrel_size: Vec3f, + #[live(vec4(0.33, 0.42, 0.28, 1.0))] + hull_color: Vec4f, + #[live(vec4(0.40, 0.49, 0.34, 1.0))] + turret_color: Vec4f, + #[live(vec4(0.18, 0.20, 0.16, 1.0))] + barrel_color: Vec4f, + #[live(1.6)] + drive_impulse_per_second: f32, + #[live(1.35)] + turn_gain: f32, + #[live(1.45)] + max_speed_mps: f32, + #[live(0.16)] + stick_deadzone: f32, + #[rust] + cached_runtime_body: Option, + #[cast] + #[deref] + node: XrNode, +} + +#[derive(Clone, Copy, Debug)] +struct TankRuntimeBodyCache { + pose: Pose, + linvel: Vec3f, + held_by: Option, +} + +fn stick_deadzone_scaled_direction( + controller: &XrController, + head_orientation: Quat, + deadzone: f32, +) -> Option<(Vec3f, f32)> { + if !controller.active() { + return None; + } + let stick = controller.stick; + let magnitude = stick.length(); + let deadzone = deadzone.clamp(0.0, 0.95); + if !magnitude.is_finite() || magnitude <= deadzone { + return None; + } + let forward = Tank::flat_forward(head_orientation); + let right = Tank::flat_right(head_orientation); + let direction = right * stick.x + forward * stick.y; + if direction.length() <= 1.0e-6 { + return None; + } + let scaled = ((magnitude - deadzone) / (1.0 - deadzone)).clamp(0.0, 1.0); + Some((direction.normalize(), scaled)) +} + +fn differential_track_commands( + pose: Pose, + linvel: Vec3f, + desired_direction: Vec3f, + input_amount: f32, + turn_gain: f32, + max_speed_mps: f32, +) -> (f32, f32) { + let body_forward = Tank::flat_forward(pose.orientation); + let body_right = Tank::flat_right(pose.orientation); + let mut forward_cmd = body_forward.dot(desired_direction) * input_amount; + let turn_cmd = body_right.dot(desired_direction) * turn_gain.max(0.0) * input_amount; + let max_speed = max_speed_mps.max(0.05); + let mut flat_velocity = linvel; + flat_velocity.y = 0.0; + let forward_speed = flat_velocity.dot(body_forward); + + if forward_cmd.signum() == forward_speed.signum() { + let speed_ratio = (forward_speed.abs() / max_speed).clamp(0.0, 1.2); + forward_cmd *= (1.0 - speed_ratio).clamp(0.0, 1.0); + } + + let mut left = forward_cmd - turn_cmd; + let mut right = forward_cmd + turn_cmd; + let max_component = left.abs().max(right.abs()).max(1.0); + left /= max_component; + right /= max_component; + (left, right) +} + +impl Tank { + fn flat_forward(orientation: Quat) -> Vec3f { + let mut forward = orientation.rotate_vec3(&vec3f(0.0, 0.0, -1.0)); + forward.y = 0.0; + if forward.length() <= 1.0e-6 { + vec3f(0.0, 0.0, -1.0) + } else { + forward.normalize() + } + } + + fn flat_right(orientation: Quat) -> Vec3f { + let mut right = orientation.rotate_vec3(&vec3f(1.0, 0.0, 0.0)); + right.y = 0.0; + if right.length() <= 1.0e-6 { + vec3f(1.0, 0.0, 0.0) + } else { + right.normalize() + } + } + + fn physics_size(&self) -> Vec3f { + vec3f( + self.hull_size.x.max(0.0), + (self.hull_size.y + self.turret_size.y * 0.55).max(0.0), + self.hull_size.z.max(0.0), + ) + } + + fn head_relative_drive_direction( + &self, + controller: &XrController, + head_orientation: Quat, + ) -> Option<(Vec3f, f32)> { + stick_deadzone_scaled_direction(controller, head_orientation, self.stick_deadzone) + } + + fn track_commands( + &self, + pose: Pose, + linvel: Vec3f, + desired_direction: Vec3f, + input_amount: f32, + ) -> (f32, f32) { + differential_track_commands( + pose, + linvel, + desired_direction, + input_amount, + self.turn_gain, + self.max_speed_mps, + ) + } + + fn emit_drive_impulses(&mut self, cx: &mut Cx, dt: f32, left: f32, right: f32, pose: Pose) { + let impulse_scale = self.drive_impulse_per_second.max(0.0) * dt.clamp(1.0 / 240.0, 0.08); + if impulse_scale <= 0.0 { + return; + } + let body_forward = Self::flat_forward(pose.orientation); + let body_right = Self::flat_right(pose.orientation); + let track_half_width = (self.hull_size.x * 0.34).clamp(0.03, 0.20); + let left_point = pose.position - body_right * track_half_width; + let right_point = pose.position + body_right * track_half_width; + + if left.abs() > 0.0001 { + cx.widget_action( + self.widget_uid(), + XrBodyImpulse { + widget_uid: self.widget_uid(), + point: left_point, + impulse: body_forward * (left * impulse_scale), + }, + ); + } + if right.abs() > 0.0001 { + cx.widget_action( + self.widget_uid(), + XrBodyImpulse { + widget_uid: self.widget_uid(), + point: right_point, + impulse: body_forward * (right * impulse_scale), + }, + ); + } + } + + fn handle_xr_update(&mut self, cx: &mut Cx, update: &XrUpdateEvent) { + let Some(runtime_body) = self.cached_runtime_body else { + return; + }; + if runtime_body.held_by.is_some() { + return; + } + let Some((desired_direction, input_amount)) = self.head_relative_drive_direction( + &update.state.right_controller, + update.state.head_pose.orientation, + ) else { + return; + }; + let dt = (update.state.time - update.last.time) as f32; + if !dt.is_finite() || dt <= 0.0 { + return; + } + let (left, right) = self.track_commands( + runtime_body.pose, + runtime_body.linvel, + desired_direction, + input_amount, + ); + self.emit_drive_impulses(cx, dt, left, right, runtime_body.pose); + } + + fn cache_runtime_body_state(&mut self, scope: &mut Scope) { + self.cached_runtime_body = XrDrawContext::from_scope(scope) + .runtime_body(self.widget_uid()) + .map(|body| TankRuntimeBodyCache { + pose: body.pose, + linvel: body.linvel, + held_by: body.held_by, + }); + } + + fn draw_part( + draw_cube: &mut DrawCube, + cx: &mut Cx3d, + transform: Mat4f, + size: Vec3f, + color: Vec4f, + ) { + draw_cube.transform = transform; + draw_cube.cube_pos = vec3f(0.0, 0.0, 0.0); + draw_cube.cube_size = size; + draw_cube.color = color; + draw_cube.depth_clip = 1.0; + draw_cube.draw(cx); + } +} + +impl ScriptHook for Tank { + fn on_after_apply( + &mut self, + _vm: &mut ScriptVm, + _apply: &Apply, + _scope: &mut Scope, + _value: ScriptValue, + ) { + self.node.set_implicit_physics_size(self.physics_size()); + } +} + +impl Widget for Tank { + fn script_call( + &mut self, + vm: &mut ScriptVm, + method: LiveId, + args: ScriptValue, + ) -> ScriptAsyncResult { + self.node.script_call(vm, method, args) + } + + fn script_result(&mut self, vm: &mut ScriptVm, id: ScriptAsyncId, result: ScriptValue) { + self.node.script_result(vm, id, result); + } + + fn handle_event(&mut self, cx: &mut Cx, event: &Event, scope: &mut Scope) { + if let Event::XrUpdate(update) = event { + self.handle_xr_update(cx, update); + } + self.node.handle_event(cx, event, scope); + } + + fn draw_3d(&mut self, cx: &mut Cx3d, scope: &mut Scope) -> DrawStep { + let Some(_scene) = apply_scene_to_draw_cube(&mut self.draw_hull, cx) else { + return DrawStep::done(); + }; + let _ = apply_scene_to_draw_cube(&mut self.draw_turret, cx); + let _ = apply_scene_to_draw_cube(&mut self.draw_barrel, cx); + + self.cache_runtime_body_state(scope); + + let tank_world = xr_widget_world_transform(cx, scope, self.widget_uid(), &self.node); + let turret_transform = Mat4f::mul( + &tank_world, + &compose_scene_node_transform( + vec3f(0.0, self.hull_size.y * 0.42 + self.turret_size.y * 0.48, 0.0), + vec3f(0.0, 0.0, 0.0), + vec3f(1.0, 1.0, 1.0), + ), + ); + let barrel_transform = Mat4f::mul( + &tank_world, + &compose_scene_node_transform( + vec3f( + 0.0, + self.hull_size.y * 0.44 + self.turret_size.y * 0.26, + -(self.turret_size.z * 0.42 + self.barrel_size.z * 0.46), + ), + vec3f(0.0, 0.0, 0.0), + vec3f(1.0, 1.0, 1.0), + ), + ); + + Self::draw_part( + &mut self.draw_hull, + cx, + tank_world, + self.hull_size, + self.hull_color, + ); + Self::draw_part( + &mut self.draw_turret, + cx, + turret_transform, + self.turret_size, + self.turret_color, + ); + Self::draw_part( + &mut self.draw_barrel, + cx, + barrel_transform, + self.barrel_size, + self.barrel_color, + ); + + self.node.draw_3d(cx, scope) + } + + fn draw_walk(&mut self, _cx: &mut Cx2d, _scope: &mut Scope, _walk: Walk) -> DrawStep { + DrawStep::done() + } +} + +#[cfg(test)] +include!("../tests/obj/tank.rs"); diff --git a/xr/src/obj/tree.rs b/xr/src/obj/tree.rs index 0e3b13e1f0..0f6ceb5b0a 100644 --- a/xr/src/obj/tree.rs +++ b/xr/src/obj/tree.rs @@ -1,16 +1,12 @@ -use super::{ +use crate::scene::{ + xr_widget_world_transform, XrDrawContext, XrHandInfluencePoint, XrNode, + XR_HAND_INFLUENCE_POINT_COUNT, +}; +use crate::util::{ mesh_generators::{stylized_leaf_mesh, tree_branch_segment_mesh}, scene_draw::scene_state_from_cx, }; -use crate::{ - makepad_derive_widget::*, - makepad_draw::*, - widget::*, - xr_node::{ - xr_hand_influence_points_from_scope, xr_widget_world_transform, XrHandInfluencePoint, - XrNode, XR_HAND_INFLUENCE_POINT_COUNT, - }, -}; +use makepad_widgets::{makepad_derive_widget::*, makepad_draw::*, widget::*}; use std::f32::consts::PI; pub const PYTHAGOREAN_TREE_ROOT_DROP: f32 = 0.60; @@ -1050,7 +1046,8 @@ impl Widget for Tree { return DrawStep::done(); }; let world = xr_widget_world_transform(cx, scope, self.widget_uid(), &self.node); - let hand_influences = xr_hand_influence_points_from_scope(scope); + let draw_context = XrDrawContext::from_scope(scope); + let hand_influences = draw_context.hand_influence_points(); let time = scene.time as f32; let template_config = self.template_config(); diff --git a/xr/src/obj/view_splat.rs b/xr/src/obj/view_splat.rs index 748b326b5d..48598bbc7c 100644 --- a/xr/src/obj/view_splat.rs +++ b/xr/src/obj/view_splat.rs @@ -1,12 +1,15 @@ -use crate::{makepad_derive_widget::*, makepad_draw::*, widget::*}; use makepad_splat::{load_splat_from_bytes, SplatFileFormat, SplatScene}; +use makepad_widgets::{ + makepad_derive_widget::*, + makepad_draw::{shader::draw_pbr::PbrMeshHandle, *}, + widget::*, +}; use std::{mem, path::PathBuf, rc::Rc, sync::mpsc::TryRecvError}; -use super::scene_draw::{ +use crate::util::scene_draw::{ apply_scene_to_draw_pbr, compose_scene_node_transform, scene_node_world_transform_from_cx, scene_state_from_cx, SceneState3D, }; -use crate::makepad_draw::shader::draw_pbr::PbrMeshHandle; script_mod! { use mod.prelude.widgets_internal.* diff --git a/xr/src/scene/mod.rs b/xr/src/scene/mod.rs new file mode 100644 index 0000000000..e5c372208e --- /dev/null +++ b/xr/src/scene/mod.rs @@ -0,0 +1,38 @@ +pub mod xr_body_spawn; +pub mod xr_env; +mod xr_gesture; +pub mod xr_node; +pub mod xr_permissions_flow; +pub mod xr_root; +pub mod xr_select; +pub mod xr_shared_object_sync; +pub mod xr_view; + +pub mod xr_peer_sync { + pub use crate::sync::xr_peer_sync::*; +} + +pub(crate) use xr_gesture::{ + arm_pair_metrics, flat_head_forward, hand_closed_fist_contact_point, + hand_closed_fist_contact_point_geometry_only, +}; + +pub use crate::net::{XrActivityId, XrSpawnableObjectId}; +pub use xr_body_spawn::{XrBodyImpulse, XrBodySpawn}; +pub use xr_env::{DrawDepthMeshBasic, XrEnv}; +pub use xr_node::{ + xr_draw_list_depth, xr_sort_child_draw_order, xr_widget_children, xr_widget_is_transparent, + xr_widget_local_sort_center, xr_widget_with_scene_node, xr_widget_world_transform, XrBodyKind, + XrDrawContext, XrDrawScopeData, XrHandInfluencePoint, XrNode, XrNodeAction, + XrPassthroughScopeData, XrPhysicsShape, XrRenderClass, XrRuntimeBodyState, + XrSharedObjectPolicy, XR_HAND_INFLUENCE_POINTS_PER_HAND, XR_HAND_INFLUENCE_POINT_COUNT, +}; +pub use crate::sync::xr_peer_sync::{XrPeerSync, XrPeerSyncAction}; +pub use xr_permissions_flow::XrPermissionsFlow; +pub use xr_root::{XrCamera, XrRoot, XrRootAction}; +pub use xr_select::{XrSelect, XrSelectAction}; +pub use xr_shared_object_sync::{ + collect_scene_spawnable_objects, XrLocalSharedObjectSnapshot, XrRemoteSharedObjectSnapshot, + XrScheduledAuthorityTransfer, XrSharedObjectRegistry, XrSpawnableObjectBinding, +}; +pub use xr_view::{DrawXrFingerCursor, XrView, XrViewEventScopeData, XrViewMode}; diff --git a/xr/src/scene/xr_body_spawn.rs b/xr/src/scene/xr_body_spawn.rs index 7b342a1ca5..c30e2c3c31 100644 --- a/xr/src/scene/xr_body_spawn.rs +++ b/xr/src/scene/xr_body_spawn.rs @@ -1,9 +1,18 @@ -use crate::*; +use crate::prelude::*; -#[derive(Clone, Copy, Debug)] +#[derive(Clone, Copy, Debug, PartialEq)] pub struct XrBodySpawn { pub widget_uid: WidgetUid, + pub shadow: bool, + pub mode: XrSharedObjectMode, pub pose: Pose, pub linvel: Vec3f, pub angvel: Vec3f, } + +#[derive(Clone, Copy, Debug, PartialEq)] +pub struct XrBodyImpulse { + pub widget_uid: WidgetUid, + pub point: Vec3f, + pub impulse: Vec3f, +} diff --git a/xr/src/scene/xr_depth.rs b/xr/src/scene/xr_depth.rs index 9fc229276b..1b7eccd305 100644 --- a/xr/src/scene/xr_depth.rs +++ b/xr/src/scene/xr_depth.rs @@ -2,10 +2,16 @@ use super::xr_physics::{makepad_pose, RapierScene}; use super::*; -use makepad_widgets::makepad_platform::XrDepthMeshStore; -use std::collections::HashSet; - -const XR_DEBUG_DEPTH_TRIANGLE_SHRINK: f32 = 0.90; +use crate::algorithms::tsdf_query::{ + depth_query_might_need_impact_refresh, depth_query_plane_supports_body, evaluate_tsdf_query, + DepthQuery, DepthQueryCollider, DepthQueryColliderGeometry, DepthQueryColliderRole, + DepthQueryHit, DepthQueryResult, DepthQuerySupportPlane, +}; +use crate::util::depth_debug_mesh::{push_debug_depth_plane, DebugDepthMeshChunk}; +use crate::util::depth_debug_mesh_worker::{ + XrDepthDebugMeshVisibleSet, XrDepthDebugMeshWorker, XrDepthDebugMeshWorkerResult, +}; +use makepad_widgets::makepad_platform::XrTsdfStore; #[derive(Clone, Copy)] pub(super) struct DepthSurfaceMeshChunkHandle { @@ -19,9 +25,9 @@ pub(super) struct DepthQuerySurfaceCollider { pub(super) fingerprint: u64, } -#[derive(Clone)] +#[derive(Clone, Copy)] pub(super) struct DepthQuerySurfaceTarget { - pub(super) collider: XrDepthMeshQueryCollider, + pub(super) collider: DepthQueryCollider, } #[derive(Clone)] @@ -30,28 +36,7 @@ struct RetainedDepthQuerySurface { misses_left: u8, } -pub(super) fn depth_query_plane_supports_body( - plane: XrDepthMeshQuerySupportPlane, - body_position: Vec3f, - query_radius: f32, - lateral_margin: f32, -) -> bool { - let offset = body_position - plane.point; - let signed_height = offset.dot(plane.normal); - if signed_height < -query_radius.max(0.0005) { - return false; - } - if signed_height > query_radius + XR_DEPTH_QUERY_MAX_DISTANCE + lateral_margin { - return false; - } - - let tangent_limit = plane.half_extent_tangent + lateral_margin; - let bitangent_limit = plane.half_extent_bitangent + lateral_margin; - offset.dot(plane.tangent).abs() <= tangent_limit - && offset.dot(plane.bitangent).abs() <= bitangent_limit -} - -fn depth_query_plane_edge_slack(plane: XrDepthMeshQuerySupportPlane, body_position: Vec3f) -> f32 { +fn depth_query_plane_edge_slack(plane: DepthQuerySupportPlane, body_position: Vec3f) -> f32 { let offset = body_position - plane.point; let tangent_slack = plane.half_extent_tangent - offset.dot(plane.tangent).abs(); let bitangent_slack = plane.half_extent_bitangent - offset.dot(plane.bitangent).abs(); @@ -69,12 +54,24 @@ fn depth_query_support_refresh_margin_scale(body_speed: f32) -> f32 { (body_speed / XR_DEPTH_QUERY_SUPPORT_REFRESH_SPEED_MIN).clamp(0.25, 1.0) } +fn depth_query_should_refresh_from_tsdf( + body_sleeping: bool, + can_skip_support_refresh: bool, + body_speed: f32, + needs_impact_refresh: bool, +) -> bool { + !body_sleeping + && (!can_skip_support_refresh + || needs_impact_refresh + || body_speed >= XR_DEPTH_QUERY_SUPPORT_REFRESH_SPEED_MIN) +} + impl RetainedDepthQuerySurface { fn sticky_supports(&self, body_position: Vec3f, query_radius: f32) -> bool { - if self.target.collider.role != XrDepthMeshQueryColliderRole::Support { + if self.target.collider.role != DepthQueryColliderRole::Support { return false; } - let XrDepthMeshQueryColliderGeometry::HalfSpace(plane) = self.target.collider.geometry; + let DepthQueryColliderGeometry::HalfSpace(plane) = self.target.collider.geometry; depth_query_plane_supports_body( plane, body_position, @@ -84,10 +81,10 @@ impl RetainedDepthQuerySurface { } fn safely_supports(&self, body_position: Vec3f, query_radius: f32, edge_margin: f32) -> bool { - if self.target.collider.role != XrDepthMeshQueryColliderRole::Support { + if self.target.collider.role != DepthQueryColliderRole::Support { return false; } - let XrDepthMeshQueryColliderGeometry::HalfSpace(plane) = self.target.collider.geometry; + let DepthQueryColliderGeometry::HalfSpace(plane) = self.target.collider.geometry; depth_query_plane_supports_body(plane, body_position, query_radius, 0.0) && depth_query_plane_edge_slack(plane, body_position) > edge_margin } @@ -99,7 +96,7 @@ pub(super) struct RetainedDepthQueryHit { } impl RetainedDepthQueryHit { - fn new(hit: &XrDepthMeshQueryHit) -> Self { + fn new(hit: &DepthQueryHit) -> Self { let mut retained = Self { surfaces: std::array::from_fn(|_| None), }; @@ -107,7 +104,7 @@ impl RetainedDepthQueryHit { retained } - fn refresh_from_hit(&mut self, hit: &XrDepthMeshQueryHit) { + fn refresh_from_hit(&mut self, hit: &DepthQueryHit) { let mut matched = [false; XR_DEPTH_QUERY_SURFACES_PER_BODY]; self.absorb_surface( DepthQuerySurfaceTarget { @@ -146,8 +143,8 @@ impl RetainedDepthQueryHit { matched: &mut [bool; XR_DEPTH_QUERY_SURFACES_PER_BODY], ) { let misses_left = match target.collider.role { - XrDepthMeshQueryColliderRole::Support => XR_DEPTH_QUERY_HIT_MISS_GRACE_FRAMES, - XrDepthMeshQueryColliderRole::Impact => 0, + DepthQueryColliderRole::Support => XR_DEPTH_QUERY_HIT_MISS_GRACE_FRAMES, + DepthQueryColliderRole::Impact => 0, }; let retained_surface = RetainedDepthQuerySurface { target, @@ -239,305 +236,31 @@ impl RetainedDepthQueryHit { } } -fn pack_depth_mesh_debug_triangles(chunk: &XrDepthMeshChunk) -> (Vec, Vec) { - const FLOATS_PER_VERTEX: usize = 16; - let triangle_count = chunk.indices.len() / 3; - let mut indices = Vec::with_capacity(chunk.indices.len()); - let mut vertices = Vec::with_capacity(triangle_count * 3 * FLOATS_PER_VERTEX); - - for triangle in chunk.indices.chunks_exact(3) { - let a = chunk.vertices[triangle[0] as usize]; - let b = chunk.vertices[triangle[1] as usize]; - let c = chunk.vertices[triangle[2] as usize]; - let centroid = (a + b + c).scale(1.0 / 3.0); - let raw_normal = Vec3f::cross(b - a, c - a); - let normal = if raw_normal.length() > 1.0e-6 { - raw_normal.normalize() - } else { - vec3f(0.0, 1.0, 0.0) - }; - let base = (vertices.len() / FLOATS_PER_VERTEX) as u32; - for position in [a, b, c] { - let shrunken = centroid + (position - centroid).scale(XR_DEBUG_DEPTH_TRIANGLE_SHRINK); - vertices.extend_from_slice(&[ - shrunken.x, shrunken.y, shrunken.z, normal.x, normal.y, normal.z, 0.0, 0.0, 1.0, - 1.0, 1.0, 1.0, 1.0, 0.0, 0.0, 1.0, - ]); - } - indices.extend_from_slice(&[base, base + 1, base + 2]); - } - - (indices, vertices) -} - -fn push_debug_depth_vertex(vertices: &mut Vec, position: Vec3f, normal: Vec3f) { - vertices.extend_from_slice(&[ - position.x, position.y, position.z, normal.x, normal.y, normal.z, 0.0, 0.0, 1.0, 1.0, 1.0, - 1.0, 1.0, 0.0, 0.0, 1.0, - ]); -} - -fn push_debug_depth_triangle( - indices: &mut Vec, - vertices: &mut Vec, - triangle: [Vec3f; 3], -) { - let base = (vertices.len() / 16) as u32; - let raw_normal = Vec3f::cross(triangle[1] - triangle[0], triangle[2] - triangle[0]); - let normal = if raw_normal.length() > 1.0e-6 { - raw_normal.normalize() - } else { - vec3f(0.0, 1.0, 0.0) - }; - for position in triangle { - push_debug_depth_vertex(vertices, position, normal); - } - indices.extend_from_slice(&[base, base + 1, base + 2]); -} - -fn push_debug_depth_quad(indices: &mut Vec, vertices: &mut Vec, quad: [Vec3f; 4]) { - let base = (vertices.len() / 16) as u32; - let raw_normal = Vec3f::cross(quad[1] - quad[0], quad[2] - quad[0]); - let normal = if raw_normal.length() > 1.0e-6 { - raw_normal.normalize() - } else { - vec3f(0.0, 1.0, 0.0) - }; - for position in quad { - push_debug_depth_vertex(vertices, position, normal); - } - indices.extend_from_slice(&[base, base + 1, base + 2, base, base + 2, base + 3]); -} - -fn push_debug_depth_plane( - indices: &mut Vec, - vertices: &mut Vec, - plane: XrDepthMeshQuerySupportPlane, -) { - let center = plane.point; - let tangent = plane.tangent.scale(plane.half_extent_tangent); - let bitangent = plane.bitangent.scale(plane.half_extent_bitangent); - push_debug_depth_quad( - indices, - vertices, - [ - center - tangent - bitangent, - center + tangent - bitangent, - center + tangent + bitangent, - center - tangent + bitangent, - ], - ); -} - #[cfg(test)] -mod tests { - use super::*; - - #[test] - fn depth_query_plane_supports_body_respects_exact_quad_edge() { - let plane = XrDepthMeshQuerySupportPlane { - point: vec3f(0.0, 0.0, 0.0), - normal: vec3f(0.0, 1.0, 0.0), - tangent: vec3f(1.0, 0.0, 0.0), - bitangent: vec3f(0.0, 0.0, 1.0), - half_extent_tangent: 0.10, - half_extent_bitangent: 0.08, - }; - - assert!(depth_query_plane_supports_body( - plane, - vec3f(0.099, 0.03, 0.0), - 0.05, - 0.0, - )); - assert!(!depth_query_plane_supports_body( - plane, - vec3f(0.101, 0.03, 0.0), - 0.05, - 0.0, - )); - } - - #[test] - fn retained_support_refresh_requeries_before_body_reaches_quad_edge() { - let plane = XrDepthMeshQuerySupportPlane { - point: vec3f(0.0, 0.0, 0.0), - normal: vec3f(0.0, 1.0, 0.0), - tangent: vec3f(1.0, 0.0, 0.0), - bitangent: vec3f(0.0, 0.0, 1.0), - half_extent_tangent: 0.10, - half_extent_bitangent: 0.10, - }; - let retained = RetainedDepthQueryHit { - surfaces: std::array::from_fn(|index| { - (index == 0).then_some(RetainedDepthQuerySurface { - target: DepthQuerySurfaceTarget { - collider: XrDepthMeshQueryCollider { - fingerprint: 1, - geometry: XrDepthMeshQueryColliderGeometry::HalfSpace(plane), - role: XrDepthMeshQueryColliderRole::Support, - restitution: 0.0, - }, - }, - misses_left: XR_DEPTH_QUERY_HIT_MISS_GRACE_FRAMES, - }) - }), - }; - - assert!(retained.can_skip_refresh( - vec3f(0.00, 0.03, 0.0), - vec3f(0.02, 0.03, 0.0), - 0.05, - 0.30, - )); - assert!(!retained.can_skip_refresh( - vec3f(0.06, 0.03, 0.0), - vec3f(0.09, 0.03, 0.0), - 0.05, - 0.30, - )); - } - - #[test] - fn slow_support_refresh_margin_is_less_aggressive_than_fast_motion() { - let plane = XrDepthMeshQuerySupportPlane { - point: vec3f(0.0, 0.0, 0.0), - normal: vec3f(0.0, 1.0, 0.0), - tangent: vec3f(1.0, 0.0, 0.0), - bitangent: vec3f(0.0, 0.0, 1.0), - half_extent_tangent: 0.10, - half_extent_bitangent: 0.10, - }; - let retained = RetainedDepthQueryHit { - surfaces: std::array::from_fn(|index| { - (index == 0).then_some(RetainedDepthQuerySurface { - target: DepthQuerySurfaceTarget { - collider: XrDepthMeshQueryCollider { - fingerprint: 2, - geometry: XrDepthMeshQueryColliderGeometry::HalfSpace(plane), - role: XrDepthMeshQueryColliderRole::Support, - restitution: 0.0, - }, - }, - misses_left: XR_DEPTH_QUERY_HIT_MISS_GRACE_FRAMES, - }) - }), - }; - - assert!(retained.can_skip_refresh( - vec3f(0.082, 0.03, 0.0), - vec3f(0.088, 0.03, 0.0), - 0.05, - 0.05, - )); - assert!(!retained.can_skip_refresh( - vec3f(0.082, 0.03, 0.0), - vec3f(0.088, 0.03, 0.0), - 0.05, - 0.30, - )); - } -} +include!("../tests/scene/xr_depth.rs"); impl XrEnv { pub(super) fn depth_debug_enabled(&self) -> bool { self.depth_mesh_visible() || self.depth_query_hits_visible() } - pub(super) fn clear_depth_surface_mesh(&mut self) { - self.depth_surface_mesh_generation = 0; - self.depth_surface_mesh_update_sequence = 0; - self.depth_surface_mesh_chunks.clear(); - self.depth_surface_mesh_upload_count = 0; - } - - fn upsert_depth_query_hit_geometry( - slot: &mut Option, - cx: &mut Cx2d, - indices: Vec, - vertices: Vec, - ) -> Option { - if indices.is_empty() { - return None; - } - let geometry = slot.get_or_insert_with(|| Geometry::new(cx.cx.cx)); - geometry.update(cx.cx.cx, indices, vertices); - Some(geometry.geometry_id()) - } - - fn build_depth_query_hit_geometry(&mut self, cx: &mut Cx2d) -> Option { - let mut quad_indices = Vec::new(); - let mut quad_vertices = Vec::new(); - - let mut push_surface = |retained: &RetainedDepthQuerySurface| { - let XrDepthMeshQueryColliderGeometry::HalfSpace(plane) = - retained.target.collider.geometry; - push_debug_depth_plane(&mut quad_indices, &mut quad_vertices, plane); - }; - - for retained in self.depth_query_retained_hits.values() { - for surface in retained.retained_surfaces() { - push_surface(surface); - } - } - - Self::upsert_depth_query_hit_geometry( - &mut self.depth_query_hit_geometry, - cx, - quad_indices, - quad_vertices, - ) - } - - fn upsert_depth_surface_mesh_chunk(&mut self, cx: &mut Cx2d, chunk: &XrDepthMeshChunk) { - let key = (chunk.chunk_key.x, chunk.chunk_key.y, chunk.chunk_key.z); - if self - .depth_surface_mesh_chunks - .get(&key) - .map(|gpu_chunk| gpu_chunk.1.fingerprint == chunk.fingerprint) - .unwrap_or(false) - { - return; - } - - let (indices, vertices) = pack_depth_mesh_debug_triangles(chunk); - if let Some((geometry, handle)) = self.depth_surface_mesh_chunks.get_mut(&key) { - geometry.update(cx.cx.cx, indices, vertices); - *handle = DepthSurfaceMeshChunkHandle { - geometry_id: geometry.geometry_id(), - fingerprint: chunk.fingerprint, - }; - } else { - let geometry = Geometry::new(cx.cx.cx); - geometry.update(cx.cx.cx, indices, vertices); - let handle = DepthSurfaceMeshChunkHandle { - geometry_id: geometry.geometry_id(), - fingerprint: chunk.fingerprint, - }; - self.depth_surface_mesh_chunks - .insert(key, (geometry, handle)); - self.depth_surface_mesh_upload_count = - self.depth_surface_mesh_upload_count.saturating_add(1); - } - } - fn sync_retained_depth_query_result( retained_hits: &mut HashMap, key: u64, - latest_result: Option, + latest_result: Option, body_position: Vec3f, query_radius: f32, expired_retained_keys: &mut Vec, ) { match latest_result { - Some(XrDepthMeshQueryResult::Hit(hit)) => { + Some(DepthQueryResult::Hit(hit)) => { if let Some(retained) = retained_hits.get_mut(&key) { retained.refresh_from_hit(&hit); } else { retained_hits.insert(key, RetainedDepthQueryHit::new(&hit)); } } - Some(XrDepthMeshQueryResult::Miss { .. }) | None => { + Some(DepthQueryResult::Miss { .. }) | None => { if let Some(retained) = retained_hits.get_mut(&key) { if !retained.age_on_miss_with_sticky_support(body_position, query_radius) { expired_retained_keys.push(key); @@ -553,7 +276,7 @@ impl XrEnv { velocity: Vec3f, gravity: Vec3f, query_radius: f32, - ) -> XrDepthMeshQuery { + ) -> DepthQuery { let speed = velocity.length(); let lookahead_seconds = if speed > 1.0e-6 { XR_DEPTH_QUERY_LOOKAHEAD_SECONDS.min(XR_DEPTH_QUERY_MAX_LOOKAHEAD_DISTANCE / speed) @@ -562,14 +285,13 @@ impl XrEnv { }; let lookahead = velocity.scale(lookahead_seconds) + gravity.scale(0.5 * lookahead_seconds * lookahead_seconds); - XrDepthMeshQuery { + DepthQuery { key, center: pose.position, predicted_center: pose.position + lookahead, velocity, radius: query_radius.max(0.0005), max_distance: XR_DEPTH_QUERY_MAX_DISTANCE, - include_planar_patches: XR_DEPTH_QUERY_INCLUDE_PLANAR_PATCHES, } } @@ -584,124 +306,238 @@ impl XrEnv { let Some(scene) = scene else { return; }; - sync_depth_query_surfaces_with_store(retained_hits, Some(scene), &cx.xr_depth_mesh()); + sync_depth_query_surfaces_with_store(retained_hits, Some(scene), &cx.xr_tsdf()); } +} - pub(super) fn sync_depth_surface_mesh(&mut self, cx: &mut Cx2d) { - if !self.depth_mesh_visible() { - return; +impl XrDepthRuntime { + pub(super) fn clear_surface_mesh(&mut self) { + self.surface_mesh_generation = 0; + self.surface_mesh_update_sequence = 0; + self.requested_snapshot_grid = None; + self.requested_head_pose = None; + self.snapshot_grid = None; + self.visible_request_id = 0; + self.visible_chunks.clear(); + self.mesh_chunks.clear(); + self.pending_upserts.clear(); + self.surface_mesh_worker = None; + } + + pub(super) fn ensure_surface_mesh_worker(&mut self) -> &mut XrDepthDebugMeshWorker { + self.surface_mesh_worker + .get_or_insert_with(XrDepthDebugMeshWorker::new) + } + + fn upsert_query_hit_geometry( + slot: &mut Option, + cx: &mut Cx2d, + indices: Vec, + vertices: Vec, + ) -> Option { + if indices.is_empty() { + return None; } + let geometry = slot.get_or_insert_with(|| Geometry::new(cx.cx.cx)); + geometry.update(cx.cx.cx, indices, vertices); + Some(geometry.geometry_id()) + } - let Some(depth_mesh) = cx.cx.xr_depth_mesh().latest_mesh() else { - self.clear_depth_surface_mesh(); - return; + fn build_query_hit_geometry(&mut self, cx: &mut Cx2d) -> Option { + let mut quad_indices = Vec::new(); + let mut quad_vertices = Vec::new(); + + let mut push_surface = |retained: &RetainedDepthQuerySurface| { + let DepthQueryColliderGeometry::HalfSpace(plane) = retained.target.collider.geometry; + push_debug_depth_plane(&mut quad_indices, &mut quad_vertices, plane); }; - let previous_mesh_generation = self.depth_surface_mesh_generation; - let previous_update_sequence = self.depth_surface_mesh_update_sequence; - if self.depth_surface_mesh_generation == depth_mesh.mesh_generation - && self.depth_surface_mesh_update_sequence == depth_mesh.update_sequence + + for retained in self.query_retained_hits.values() { + for surface in retained.retained_surfaces() { + push_surface(surface); + } + } + + Self::upsert_query_hit_geometry( + &mut self.query_hit_geometry, + cx, + quad_indices, + quad_vertices, + ) + } + + fn upsert_surface_mesh_chunk(&mut self, cx: &mut Cx2d, chunk: DebugDepthMeshChunk) { + let key = chunk.chunk_key; + if self + .mesh_chunks + .get(&key) + .map(|gpu_chunk| gpu_chunk.1.fingerprint == chunk.fingerprint) + .unwrap_or(false) { return; } - self.depth_surface_mesh_generation = depth_mesh.mesh_generation; - self.depth_surface_mesh_update_sequence = depth_mesh.update_sequence; - if depth_mesh.mesh_chunks.is_empty() { - self.clear_depth_surface_mesh(); - return; + if let Some((geometry, handle)) = self.mesh_chunks.get_mut(&key) { + geometry.update(cx.cx.cx, chunk.indices, chunk.vertices); + *handle = DepthSurfaceMeshChunkHandle { + geometry_id: geometry.geometry_id(), + fingerprint: chunk.fingerprint, + }; + } else { + let geometry = Geometry::new(cx.cx.cx); + geometry.update(cx.cx.cx, chunk.indices, chunk.vertices); + let handle = DepthSurfaceMeshChunkHandle { + geometry_id: geometry.geometry_id(), + fingerprint: chunk.fingerprint, + }; + self.mesh_chunks.insert(key, (geometry, handle)); } + } - let active_chunk_count = depth_mesh.mesh_chunks.len(); - if self.depth_surface_mesh_upload_count > active_chunk_count.saturating_mul(3) + 64 { - self.clear_depth_surface_mesh(); + fn apply_surface_mesh_visible_set(&mut self, result: XrDepthDebugMeshVisibleSet) { + if result.request_id < self.visible_request_id { + return; } + self.visible_request_id = result.request_id; + self.surface_mesh_generation = result.generation; + self.surface_mesh_update_sequence = result.update_sequence; + self.snapshot_grid = Some(result.snapshot_grid); + self.visible_chunks = result.visible_chunk_keys.into_iter().collect(); + self.pending_upserts + .retain(|(request_id, _)| *request_id >= result.request_id); + } - let needs_full_resync = previous_mesh_generation == 0 - || self.depth_surface_mesh_chunks.is_empty() - || depth_mesh.update_sequence != previous_update_sequence.saturating_add(1); + fn remove_surface_mesh_chunk(&mut self, chunk_key: ChunkKey) { + self.mesh_chunks.remove(&chunk_key); + } + + fn enqueue_surface_mesh_upsert(&mut self, request_id: u64, chunk: DebugDepthMeshChunk) { + self.pending_upserts + .retain(|(queued_request_id, queued_chunk)| { + !(*queued_request_id == request_id && queued_chunk.chunk_key == chunk.chunk_key) + }); + self.pending_upserts.push_back((request_id, chunk)); + } - if needs_full_resync { - let mut desired_keys = HashSet::with_capacity(depth_mesh.mesh_chunks.len()); - for chunk in &depth_mesh.mesh_chunks { - desired_keys.insert((chunk.chunk_key.x, chunk.chunk_key.y, chunk.chunk_key.z)); - self.upsert_depth_surface_mesh_chunk(cx, chunk); + fn apply_pending_surface_mesh_upserts(&mut self, cx: &mut Cx2d) { + let pending_count = self.pending_upserts.len(); + for _ in 0..pending_count { + let Some((request_id, chunk)) = self.pending_upserts.pop_front() else { + break; + }; + if request_id != self.visible_request_id { + continue; + } + let is_new_chunk = !self.mesh_chunks.contains_key(&chunk.chunk_key); + self.upsert_surface_mesh_chunk(cx, chunk); + if is_new_chunk { + break; } - self.depth_surface_mesh_chunks - .retain(|key, _| desired_keys.contains(key)); - return; } + } - for key in &depth_mesh.removed_chunk_keys { - self.depth_surface_mesh_chunks - .remove(&(key.x, key.y, key.z)); - } - for key in &depth_mesh.dirty_chunk_keys { - if let Some(chunk) = depth_mesh - .mesh_chunks - .iter() - .find(|chunk| chunk.chunk_key == *key) - { - self.upsert_depth_surface_mesh_chunk(cx, chunk); + fn apply_surface_mesh_worker_result(&mut self, result: XrDepthDebugMeshWorkerResult) { + match result { + XrDepthDebugMeshWorkerResult::VisibleSet(result) => { + self.apply_surface_mesh_visible_set(result); + } + XrDepthDebugMeshWorkerResult::ChunkUpserts { request_id, chunks } => { + if request_id < self.visible_request_id { + return; + } + for chunk in chunks { + self.enqueue_surface_mesh_upsert(request_id, chunk); + } } + XrDepthDebugMeshWorkerResult::ChunkRemovals { + request_id, + chunk_keys, + } => { + if request_id != self.visible_request_id { + return; + } + for chunk_key in chunk_keys { + self.pending_upserts + .retain(|(queued_request_id, queued_chunk)| { + !(*queued_request_id == request_id + && queued_chunk.chunk_key == chunk_key) + }); + self.remove_surface_mesh_chunk(chunk_key); + } + } + } + } + + pub(super) fn poll_surface_mesh_worker(&mut self, cx: &mut Cx2d) { + while let Some(result) = self + .surface_mesh_worker + .as_mut() + .and_then(|worker| worker.take_next_result()) + { + self.apply_surface_mesh_worker_result(result); } + self.apply_pending_surface_mesh_upserts(cx); } - pub(super) fn draw_depth_surface_mesh(&mut self, cx: &mut Cx2d) { - let show_mesh = self.depth_mesh_visible(); - let show_query_hits = self.depth_query_hits_visible(); + pub(super) fn draw_surface_mesh( + &mut self, + draw_depth_mesh: &mut DrawDepthMeshBasic, + cx: &mut Cx2d, + show_mesh: bool, + show_query_hits: bool, + ) { if !show_mesh && !show_query_hits { return; } let query_hits = if show_query_hits { - self.build_depth_query_hit_geometry(cx) + self.build_query_hit_geometry(cx) } else { None }; - if (!show_mesh || self.depth_surface_mesh_chunks.is_empty()) && query_hits.is_none() { + if (!show_mesh || self.mesh_chunks.is_empty()) && query_hits.is_none() { return; } - self.draw_depth_mesh.base_color = vec4(0.76, 0.88, 0.98, 1.0); - if show_mesh && !self.depth_surface_mesh_chunks.is_empty() { + draw_depth_mesh.base_color = vec4(0.60, 0.62, 0.66, 0.95); + if show_mesh && !self.mesh_chunks.is_empty() && !self.visible_chunks.is_empty() { let mut chunk_handles: Vec<_> = self - .depth_surface_mesh_chunks + .visible_chunks .iter() - .map(|(key, chunk)| (*key, chunk.1.geometry_id)) + .filter_map(|key| { + self.mesh_chunks + .get(key) + .map(|chunk| (*key, chunk.1.geometry_id)) + }) .collect(); - chunk_handles.sort_by_key(|(key, _)| *key); + chunk_handles.sort_by_key(|(key, _)| (key.x, key.y, key.z)); for (_, geometry_id) in chunk_handles { - self.draw_depth_mesh.draw_geometry(cx, geometry_id); + draw_depth_mesh.draw_geometry(cx, geometry_id); } } - let mesh_normal_bias = self.draw_depth_mesh.normal_bias; - self.draw_depth_mesh.normal_bias = mesh_normal_bias + 0.004; + let mesh_depth_bias = draw_depth_mesh.depth_bias; + draw_depth_mesh.depth_bias = mesh_depth_bias + 0.004; if show_query_hits { if let Some(geometry_id) = query_hits { - self.draw_depth_mesh.base_color = vec4(1.0, 0.42, 0.08, 1.0); - self.draw_depth_mesh.draw_geometry(cx, geometry_id); + draw_depth_mesh.base_color = vec4(1.0, 0.42, 0.08, 0.95); + draw_depth_mesh.draw_geometry(cx, geometry_id); } } - self.draw_depth_mesh.normal_bias = mesh_normal_bias; + draw_depth_mesh.depth_bias = mesh_depth_bias; } } pub(super) fn clear_depth_query_state_for_scene( - depth_mesh: &XrDepthMeshStore, scene: Option<&RapierScene>, retained_hits: &mut HashMap, ) { - if let Some(scene) = scene { - for index in 0..scene.depth_query_surface_set_count() { - depth_mesh.clear_query(RapierScene::depth_query_key(index)); - } - } + let _ = scene; retained_hits.clear(); } pub(super) fn sync_depth_query_surfaces_with_store( retained_hits: &mut HashMap, scene: Option<&mut RapierScene>, - depth_mesh: &XrDepthMeshStore, + depth_mesh: &XrTsdfStore, ) { if !XR_ENABLE_DEPTH_QUERY_PHYSICS { return; @@ -709,9 +545,9 @@ pub(super) fn sync_depth_query_surfaces_with_store( let Some(scene) = scene else { return; }; + let snapshot = depth_mesh.latest_tsdf_snapshot(); let mut clear_keys = Vec::new(); - let mut query_requests = Vec::new(); let mut expired_retained_keys = Vec::new(); scene.begin_depth_query_stats_frame(); @@ -736,28 +572,6 @@ pub(super) fn sync_depth_query_surfaces_with_store( let body_velocity = vec3f(linvel.x, linvel.y, linvel.z); let gravity = scene.gravity_vector(); - let latest_result = if body_sleeping { - None - } else { - depth_mesh.latest_query_result(key) - }; - XrEnv::sync_retained_depth_query_result( - retained_hits, - key, - latest_result, - body_pose.position, - cube.query_radius, - &mut expired_retained_keys, - ); - let mut surface_targets = std::array::from_fn(|_| None); - if let Some(retained) = retained_hits.get(&key) { - retained.fill_targets(&mut surface_targets); - } - scene.sync_depth_query_surface_set(set_index, &surface_targets); - - if body_sleeping { - continue; - } let query_request = XrEnv::build_depth_query_request( key, body_pose, @@ -774,21 +588,37 @@ pub(super) fn sync_depth_query_surfaces_with_store( body_speed, ) }); - if can_skip_refresh && body_speed < XR_DEPTH_QUERY_SUPPORT_REFRESH_SPEED_MIN { - continue; + let needs_impact_refresh = depth_query_might_need_impact_refresh(query_request); + if depth_query_should_refresh_from_tsdf( + body_sleeping, + can_skip_refresh, + body_speed, + needs_impact_refresh, + ) { + let latest_result = snapshot + .as_ref() + .map(|snapshot| evaluate_tsdf_query(snapshot, query_request)); + XrEnv::sync_retained_depth_query_result( + retained_hits, + key, + latest_result, + body_pose.position, + cube.query_radius, + &mut expired_retained_keys, + ); } - query_requests.push(query_request); + + let mut surface_targets = std::array::from_fn(|_| None); + if let Some(retained) = retained_hits.get(&key) { + retained.fill_targets(&mut surface_targets); + } + scene.sync_depth_query_surface_set(set_index, &surface_targets); } for key in clear_keys { - depth_mesh.clear_query(key); retained_hits.remove(&key); } for key in expired_retained_keys { retained_hits.remove(&key); } - - for query in query_requests { - let _ = depth_mesh.submit_query(query); - } } diff --git a/xr/src/scene/xr_env.rs b/xr/src/scene/xr_env.rs index b0e3e2441d..4fb7e6d80c 100644 --- a/xr/src/scene/xr_env.rs +++ b/xr/src/scene/xr_env.rs @@ -1,8 +1,12 @@ -use crate::xr_node::{ - XrBodyKind, XrDrawScopeData, XrHandInfluencePoint, XrNode, XrRuntimeBodyState, - XR_HAND_INFLUENCE_POINTS_PER_HAND, XR_HAND_INFLUENCE_POINT_COUNT, +use super::xr_node::{ + xr_widget_children, xr_widget_with_scene_node, XrBodyKind, XrDrawScopeData, + XrHandInfluencePoint, XrNode, XrRuntimeBodyState, XR_HAND_INFLUENCE_POINTS_PER_HAND, + XR_HAND_INFLUENCE_POINT_COUNT, +}; +use crate::prelude::*; +use crate::util::{ + depth_debug_mesh::DebugDepthMeshChunk, depth_debug_mesh_worker::XrDepthDebugMeshWorker, }; -use crate::*; use makepad_widgets::makepad_platform::{ event::{CameraPreviewMode, VideoSource, VideoYuvMetadata}, permission::{Permission, PermissionStatus}, @@ -14,7 +18,11 @@ use rapier3d::prelude::{ Pose as RapierPose, Real as RapierReal, RigidBodyBuilder, RigidBodyHandle, RigidBodySet, Rotation as RapierRotation, SharedShape, Vector as RapierVector, }; -use std::{collections::HashMap, rc::Rc}; +use std::{ + collections::{HashMap, HashSet, VecDeque}, + rc::Rc, + sync::Arc, +}; #[path = "xr_depth.rs"] mod xr_depth; @@ -35,7 +43,6 @@ use self::{ }, xr_physics_worker::{XrPhysicsWorker, XrPhysicsWorkerResult}, }; - script_mod! { use mod.pod.* use mod.math.* @@ -51,34 +58,57 @@ script_mod! { draw_call: uniform_buffer(draw.DrawCallUniforms) draw_pass: uniform_buffer(draw.DrawPassUniforms) draw_list: uniform_buffer(draw.DrawListUniforms) - geom: vertex_buffer(geom.PbrVertex, geom.PbrGeom) + geom: vertex_buffer(geom.DepthMeshVertex, geom.DepthMeshGeom) + + v_barycentric: varying(vec3f) - v_world: varying(vec3f) - v_normal: varying(vec3f) + edge_distance_px: fn(bary: vec3f) -> f32 { + let bary_fw = vec3( + length(vec2(dFdx(bary.x), dFdy(bary.x))), + length(vec2(dFdx(bary.y), dFdy(bary.y))), + length(vec2(dFdx(bary.z), dFdy(bary.z))) + ); + return min( + bary.x / max(bary_fw.x, 0.00001), + min( + bary.y / max(bary_fw.y, 0.00001), + bary.z / max(bary_fw.z, 0.00001) + ) + ) + } + + wire_band_alpha: fn(bary: vec3f) -> f32 { + let edge_px = self.edge_distance_px(bary); + let inner = smoothstep(self.wire_inner_px - 0.75, self.wire_inner_px + 0.75, edge_px); + let outer = 1.0 - smoothstep(self.wire_outer_px - 0.75, self.wire_outer_px + 0.75, edge_px); + return clamp(inner * outer, 0.0, 1.0) + } vertex: fn() { let world = vec4( - self.geom.pos_nx.x, - self.geom.pos_nx.y, - self.geom.pos_nx.z, + self.geom.pos.x, + self.geom.pos.y, + self.geom.pos.z, 1.0 ); - self.v_normal = normalize(vec3( - self.geom.pos_nx.w, - self.geom.ny_nz_uv.x, - self.geom.ny_nz_uv.y - )); - let biased_world = vec4(world.xyz + self.v_normal * self.normal_bias, 1.0); - self.v_world = biased_world.xyz; - self.vertex_pos = self.draw_pass.camera_projection * (self.draw_pass.camera_view * biased_world); + let view = self.draw_pass.camera_view * world; + let biased_view = vec4(view.x, view.y, view.z + self.depth_bias, view.w); + self.v_barycentric = vec3( + self.geom.barycentric.x, + self.geom.barycentric.y, + self.geom.barycentric.z + ); + self.vertex_pos = self.draw_pass.camera_projection * biased_view; } pixel: fn() { - let n = normalize(self.v_normal); - let l = normalize(self.light_dir); - let diffuse = max(dot(n, l), 0.0); - let lit = self.ambient + diffuse * (1.0 - self.ambient); - return vec4(self.base_color.xyz * lit, self.base_color.w); + let wire_alpha = self.base_color.w * self.wire_band_alpha(self.v_barycentric); + return vec4( + self.base_color.x * wire_alpha, + self.base_color.y * wire_alpha, + self.base_color.z * wire_alpha, + wire_alpha + ); } fragment: fn() { @@ -89,11 +119,11 @@ script_mod! { mod.widgets.XrEnv = set_type_default() do #(XrEnv::script_component(vm)){ draw_cube: mod.draw.DrawCube{} draw_depth_mesh: mod.draw.DrawDepthMeshBasic{ - alpha_blend: false - light_dir: vec3(0.28, 0.86, 0.42) - ambient: 0.26 - normal_bias: 0.006 - base_color: vec4(0.76, 0.88, 0.98, 1.0) + alpha_blend: true + base_color: vec4(0.60, 0.62, 0.66, 0.95) + depth_bias: 0.006 + wire_outer_px: 2.4 + wire_inner_px: 0.7 } draw_pbr: mod.draw.DrawPbr{ light_dir: vec3(0.35, 0.8, 0.45) @@ -146,13 +176,14 @@ const XR_DEPTH_QUERY_SUPPORT_REFRESH_SPEED_MIN: f32 = 0.30; const XR_DEPTH_QUERY_SUPPORT_REFRESH_EDGE_MARGIN_SCALE: f32 = 0.45; const XR_DEPTH_QUERY_SUPPORT_REFRESH_EDGE_MARGIN_MIN: f32 = 0.012; const XR_DEPTH_QUERY_SUPPORT_REFRESH_EDGE_MARGIN_MAX: f32 = 0.04; -const XR_DEPTH_QUERY_INCLUDE_PLANAR_PATCHES: bool = false; #[allow(dead_code)] const XR_DEPTH_QUERY_STICKY_KEEP_MARGIN: f32 = 0.015; #[allow(dead_code)] const XR_DEPTH_QUERY_FINGERPRINT_QUANTIZATION_METERS: f32 = 0.01; #[allow(dead_code)] const XR_DEPTH_QUERY_HIT_MISS_GRACE_FRAMES: u8 = 3; +const XR_DEPTH_SURFACE_MESH_REQUEST_MOVE_METERS: f32 = 0.20; +const XR_DEPTH_SURFACE_MESH_REQUEST_ROTATE_DEGREES: f32 = 12.0; const XR_HAND_COLLIDER_SLOTS_PER_HAND: usize = 25; const XR_HAND_COLLIDER_FRICTION: f32 = 0.8; const XR_HAND_PLATE_HALF_WIDTH: f32 = 0.045; @@ -160,6 +191,9 @@ const XR_HAND_PLATE_HALF_HEIGHT: f32 = 0.005; const XR_HAND_PLATE_HALF_DEPTH: f32 = 0.028; const XR_HAND_PLATE_FORWARD_OFFSET: f32 = 0.004; const XR_HAND_TIP_RADIUS_SCALE: f32 = 0.72; +const XR_HAND_GRAB_MAX_DISTANCE: f32 = 0.24; +const XR_HAND_DUAL_GRAB_MIN_SPAN: f32 = 0.035; +const XR_HAND_GRAB_RELEASE_LINEAR_VELOCITY_SCALE: f32 = 1.0; const XR_BODY_LINEAR_DAMPING: f32 = 1.5; const XR_BODY_ANGULAR_DAMPING: f32 = 6.0; const XR_BODY_ADDITIONAL_SOLVER_ITERATIONS: usize = 4; @@ -167,6 +201,7 @@ const XR_BODY_SLEEP_ANGULAR_THRESHOLD: f32 = 2.0; const XR_BODY_SLEEP_TIME: f32 = 0.35; const XR_BODY_SNAP_SLEEP_LINEAR_SPEED: f32 = 0.03; const XR_BODY_SNAP_SLEEP_ANGULAR_SPEED: f32 = 1.0; +const XR_BODY_SNAP_SLEEP_SETTLE_FRAMES: u8 = 4; const XR_PBR_FACE_SUBDIVISIONS: usize = 1; const XR_PBR_CORNER_SEGMENTS: usize = 3; const XR_PBR_HAND_CAPSULE_SUBDIVISIONS: usize = 8; @@ -176,16 +211,133 @@ const XR_PBR_HAND_SPHERE_SUBDIVISIONS: usize = 8; struct CollectedXrCube { uid: WidgetUid, body_kind: XrBodyKind, - projectile_pool: bool, + spawn_pool: bool, pose: Pose, scale: Vec3f, half_extents: Vec3f, - is_sphere: bool, + physics_shape: XrPhysicsShape, density: f32, friction: f32, restitution: f32, } +#[derive(Clone, Copy, Debug, Default)] +struct XrPhysicsMetrics { + compute_ms: f64, + tsdf_query_ms: f64, + rapier_step_ms: f64, + depth_query_surface_count: usize, + scene_body_count: usize, + body_spawn_apply_count: usize, + body_spawn_miss_count: usize, +} + +struct XrPhysicsRuntime { + worker: Option, + runtime_bodies: Rc>, + root_pose: Option, + scene_dirty: bool, + revision: u64, + metrics: XrPhysicsMetrics, + pending_body_spawns: Vec, + pending_body_despawns: Vec, + pending_body_impulses: Vec, +} + +impl Default for XrPhysicsRuntime { + fn default() -> Self { + Self { + worker: None, + runtime_bodies: Rc::new(HashMap::new()), + root_pose: None, + scene_dirty: true, + revision: 0, + metrics: XrPhysicsMetrics::default(), + pending_body_spawns: Vec::new(), + pending_body_despawns: Vec::new(), + pending_body_impulses: Vec::new(), + } + } +} + +struct XrDepthRuntime { + surface_mesh_generation: u64, + surface_mesh_update_sequence: u64, + requested_snapshot_grid: Option>, + requested_head_pose: Option, + snapshot_grid: Option>, + visible_request_id: u64, + visible_chunks: HashSet, + mesh_chunks: HashMap, + pending_upserts: VecDeque<(u64, DebugDepthMeshChunk)>, + query_hit_geometry: Option, + surface_mesh_worker: Option, + query_retained_hits: HashMap, +} + +impl Default for XrDepthRuntime { + fn default() -> Self { + Self { + surface_mesh_generation: 0, + surface_mesh_update_sequence: 0, + requested_snapshot_grid: None, + requested_head_pose: None, + snapshot_grid: None, + visible_request_id: 0, + visible_chunks: HashSet::new(), + mesh_chunks: HashMap::new(), + pending_upserts: VecDeque::new(), + query_hit_geometry: None, + surface_mesh_worker: None, + query_retained_hits: HashMap::new(), + } + } +} + +struct XrPassthroughRuntime { + camera_choice: Option, + camera_textures: Option, + camera_video: VideoYuvMetadata, + camera_permission: Option, + camera_source_size: Vec2f, + camera_playback_requested: bool, + camera_failed: bool, + camera_has_frame: bool, + camera_choice_wait_logged: bool, + env_face_quad: Option, + env_cube: Option, +} + +impl Default for XrPassthroughRuntime { + fn default() -> Self { + Self { + camera_choice: None, + camera_textures: None, + camera_video: VideoYuvMetadata::disabled(), + camera_permission: None, + camera_source_size: Vec2f::default(), + camera_playback_requested: false, + camera_failed: false, + camera_has_frame: false, + camera_choice_wait_logged: false, + env_face_quad: None, + env_cube: None, + } + } +} + +#[derive(Default)] +struct XrHandSystem; + +#[derive(Default)] +struct XrWorld { + last_xr_state: Option>, + depth: XrDepthRuntime, + passthrough: XrPassthroughRuntime, + physics: XrPhysicsRuntime, + hands: XrHandSystem, +} + #[derive(Script, ScriptHook)] pub struct XrEnv { #[live] @@ -203,66 +355,13 @@ pub struct XrEnv { #[live(false)] env_cube: bool, #[rust] - last_xr_state: Option>, - #[rust] - depth_surface_mesh_generation: u64, - #[rust] - depth_surface_mesh_update_sequence: u64, - #[rust] - depth_surface_mesh_chunks: HashMap<(i32, i32, i32), (Geometry, DepthSurfaceMeshChunkHandle)>, - #[rust] - depth_query_hit_geometry: Option, - #[rust] - depth_surface_mesh_upload_count: usize, - #[allow(dead_code)] - #[rust] - depth_query_retained_hits: HashMap, - #[rust] - passthrough_camera_choice: Option, - #[rust] - passthrough_camera_textures: Option, - #[rust] - passthrough_camera_video: VideoYuvMetadata, - #[rust] - passthrough_camera_permission: Option, - #[rust] - passthrough_camera_source_size: Vec2f, - #[rust] - passthrough_camera_playback_requested: bool, - #[rust] - passthrough_camera_failed: bool, - #[rust] - passthrough_camera_has_frame: bool, - #[rust] - passthrough_env_face_quad: Option, - #[rust] - passthrough_env_cube: Option, + world: XrWorld, // Physics (moved from XrScene) #[live(9.81)] pub gravity: f32, #[rust(0.25)] physics_time_scale: f32, - #[rust] - physics_worker: Option, - #[rust] - runtime_bodies: Rc>, - #[rust] - root_pose: Option, - #[rust(true)] - scene_dirty: bool, - #[rust] - physics_revision: u64, - #[rust] - physics_compute_ms: f64, - #[rust] - physics_step_dt_ms: f64, - #[rust] - physics_depth_query_surface_count: usize, - #[rust] - physics_depth_query_vertex_count: usize, - #[rust] - physics_depth_query_triangle_count: usize, #[allow(dead_code)] #[rust] next_frame: NextFrame, @@ -298,15 +397,19 @@ impl XrEnv { } pub(crate) fn physics_compute_ms(&self) -> f64 { - self.physics_compute_ms + self.world.physics.metrics.compute_ms } - pub(crate) fn physics_time_scale(&self) -> f32 { - self.physics_time_scale + pub(crate) fn physics_tsdf_query_ms(&self) -> f64 { + self.world.physics.metrics.tsdf_query_ms } - pub(crate) fn physics_step_dt_ms(&self) -> f64 { - self.physics_step_dt_ms + pub(crate) fn physics_rapier_step_ms(&self) -> f64 { + self.world.physics.metrics.rapier_step_ms + } + + pub(crate) fn physics_time_scale(&self) -> f32 { + self.physics_time_scale } pub(crate) fn set_physics_time_scale(&mut self, cx: &mut Cx, scale: f32) -> f32 { @@ -320,15 +423,27 @@ impl XrEnv { } pub(crate) fn physics_depth_query_surface_count(&self) -> usize { - self.physics_depth_query_surface_count + self.world.physics.metrics.depth_query_surface_count + } + + pub(crate) fn physics_scene_body_count(&self) -> usize { + self.world.physics.metrics.scene_body_count + } + + pub(crate) fn physics_body_spawn_apply_count(&self) -> usize { + self.world.physics.metrics.body_spawn_apply_count } - pub(crate) fn physics_depth_query_vertex_count(&self) -> usize { - self.physics_depth_query_vertex_count + pub(crate) fn physics_body_spawn_miss_count(&self) -> usize { + self.world.physics.metrics.body_spawn_miss_count } - pub(crate) fn physics_depth_query_triangle_count(&self) -> usize { - self.physics_depth_query_triangle_count + pub(crate) fn physics_revision(&self) -> u64 { + self.world.physics.revision + } + + pub(crate) fn runtime_bodies(&self) -> Rc> { + self.world.physics.runtime_bodies.clone() } fn passthrough_video_id() -> LiveId { @@ -367,8 +482,58 @@ impl XrEnv { draw_pbr.set_env_texture(Some(env_tex)); } - fn prepare_depth_mesh(&mut self, _cx: &mut Cx2d) { - self.draw_depth_mesh.draw_vars.options.depth_write = true; + fn depth_surface_mesh_request_pose_changed(previous: Pose, next: Pose) -> bool { + if (next.position - previous.position).length() >= XR_DEPTH_SURFACE_MESH_REQUEST_MOVE_METERS + { + return true; + } + let mut previous_forward = previous.orientation.rotate_vec3(&vec3f(0.0, 0.0, -1.0)); + let mut next_forward = next.orientation.rotate_vec3(&vec3f(0.0, 0.0, -1.0)); + if previous_forward.length() <= 1.0e-4 || next_forward.length() <= 1.0e-4 { + return false; + } + previous_forward = previous_forward.normalize(); + next_forward = next_forward.normalize(); + let cos_threshold = XR_DEPTH_SURFACE_MESH_REQUEST_ROTATE_DEGREES + .to_radians() + .cos(); + previous_forward.dot(next_forward) <= cos_threshold + } + + fn prepare_depth_mesh(&mut self, cx: &mut Cx2d, state: &XrState) { + self.draw_depth_mesh.draw_vars.options.depth_write = false; + self.world.depth.poll_surface_mesh_worker(cx); + if !self.depth_mesh_visible() { + self.world.depth.clear_surface_mesh(); + return; + } + let Some(snapshot) = cx.cx.xr_tsdf().latest_tsdf_snapshot() else { + self.world.depth.clear_surface_mesh(); + return; + }; + let snapshot_unchanged = self + .world + .depth + .requested_snapshot_grid + .as_ref() + .is_some_and(|previous| Arc::ptr_eq(previous, &snapshot.grid)); + let pose_unchanged = self + .world + .depth + .requested_head_pose + .map(|previous| { + !Self::depth_surface_mesh_request_pose_changed(previous, state.head_pose) + }) + .unwrap_or(false); + if snapshot_unchanged && pose_unchanged { + return; + } + self.world + .depth + .ensure_surface_mesh_worker() + .request_snapshot(snapshot.clone(), state.head_pose); + self.world.depth.requested_snapshot_grid = Some(snapshot.grid.clone()); + self.world.depth.requested_head_pose = Some(state.head_pose); } fn draw_pbr_rounded_cube( @@ -426,104 +591,41 @@ impl XrEnv { .draw_sphere(cx, radius, XR_PBR_HAND_SPHERE_SUBDIVISIONS); } - fn hand_influence_tip_world(hand: &XrHand, tip: usize) -> Option { - if !hand.in_view() || !hand.tip_active(tip) { - return None; - } - Some(match tip { - XrHand::THUMB_TIP => hand.tip_pos_thumb(), - XrHand::INDEX_TIP => hand.tip_pos_index(), - XrHand::MIDDLE_TIP => hand.tip_pos_middle(), - XrHand::RING_TIP => hand.tip_pos_ring(), - XrHand::LITTLE_TIP => hand.tip_pos_little(), - _ => hand.tip_pos_index(), - }) - } - - fn hand_influence_point( - pos: Vec3f, - gain_scale: f32, - radius_scale: f32, - ) -> XrHandInfluencePoint { - XrHandInfluencePoint { - pos, - gain_scale, - radius_scale, - } - } - - fn palm_world(hand: &XrHand) -> Option { - if !hand.in_view() { - return None; - } - let center = hand.joints[XrHand::CENTER].position; - let wrist = hand.joints[XrHand::WRIST].position; - let thumb = hand.joints[XrHand::THUMB_BASE].position; - let index = hand.joints[XrHand::INDEX_BASE].position; - let middle = hand.joints[XrHand::MIDDLE_BASE].position; - let ring = hand.joints[XrHand::RING_BASE].position; - let little = hand.joints[XrHand::LITTLE_BASE].position; - Some( - center * 0.28 - + wrist * 0.10 - + thumb * 0.12 - + index * 0.13 - + middle * 0.18 - + ring * 0.11 - + little * 0.08, - ) - } - - fn write_hand_influence_points(hand: &XrHand, target: &mut [Option]) { - debug_assert_eq!(target.len(), XR_HAND_INFLUENCE_POINTS_PER_HAND); - target[0] = Self::hand_influence_tip_world(hand, XrHand::THUMB_TIP) - .map(|pos| Self::hand_influence_point(pos, 0.72, 0.92)); - target[1] = Self::hand_influence_tip_world(hand, XrHand::INDEX_TIP) - .map(|pos| Self::hand_influence_point(pos, 1.00, 1.00)); - target[2] = Self::hand_influence_tip_world(hand, XrHand::MIDDLE_TIP) - .map(|pos| Self::hand_influence_point(pos, 0.96, 1.00)); - target[3] = Self::hand_influence_tip_world(hand, XrHand::RING_TIP) - .map(|pos| Self::hand_influence_point(pos, 0.82, 0.94)); - target[4] = Self::hand_influence_tip_world(hand, XrHand::LITTLE_TIP) - .map(|pos| Self::hand_influence_point(pos, 0.68, 0.88)); - target[5] = Self::palm_world(hand).map(|pos| Self::hand_influence_point(pos, 1.30, 2.40)); - } - fn draw_scope_hand_influence_points( + &self, state: Option<&XrState>, ) -> [Option; XR_HAND_INFLUENCE_POINT_COUNT] { - let mut points = [None; XR_HAND_INFLUENCE_POINT_COUNT]; - let Some(state) = state else { - return points; - }; - let (left_points, right_points) = points.split_at_mut(XR_HAND_INFLUENCE_POINTS_PER_HAND); - Self::write_hand_influence_points(&state.left_hand, left_points); - Self::write_hand_influence_points(&state.right_hand, right_points); - points + self.world.hands.draw_scope_hand_influence_points(state) } pub(crate) fn handle_event(&mut self, cx: &mut Cx, event: &Event) { match event { Event::XrUpdate(update) => { - self.last_xr_state = Some(update.state.clone()); - self.sync_passthrough_camera(cx); + self.world.last_xr_state = Some(update.state.clone()); + self.world + .passthrough + .sync_camera(cx, Self::passthrough_video_id()); } Event::PermissionResult(result) if result.permission == Permission::HeadsetCamera => { - self.passthrough_camera_permission = Some(result.status); - self.sync_passthrough_camera(cx); + self.world.passthrough.camera_permission = Some(result.status); + self.world + .passthrough + .sync_camera(cx, Self::passthrough_video_id()); cx.redraw_all(); } Event::VideoInputs(ev) => { - self.passthrough_camera_failed = false; - self.passthrough_camera_choice = Self::pick_passthrough_camera_choice(ev); - if self.passthrough_camera_choice.is_none() { + self.world.passthrough.camera_failed = false; + self.world.passthrough.camera_choice = XrPassthroughRuntime::pick_camera_choice(ev); + if self.world.passthrough.camera_choice.is_none() { crate::warning!("XR passthrough camera: no suitable camera choice found"); } - self.sync_passthrough_camera(cx); + self.world + .passthrough + .sync_camera(cx, Self::passthrough_video_id()); cx.redraw_all(); } Event::VideoYuvTexturesReady(ev) if ev.video_id == Self::passthrough_video_id() => { - if let Some(textures) = self.passthrough_camera_textures.as_mut() { + if let Some(textures) = self.world.passthrough.camera_textures.as_mut() { textures.tex_y = Some(ev.tex_y.clone()); textures.tex_u = Some(ev.tex_u.clone()); textures.tex_v = Some(ev.tex_v.clone()); @@ -531,26 +633,26 @@ impl XrEnv { cx.redraw_all(); } Event::VideoTextureUpdated(ev) if ev.video_id == Self::passthrough_video_id() => { - self.passthrough_camera_video = ev.yuv; - self.passthrough_camera_has_frame = true; + self.world.passthrough.camera_video = ev.yuv; + self.world.passthrough.camera_has_frame = true; cx.redraw_all(); } Event::VideoPlaybackPrepared(ev) if ev.video_id == Self::passthrough_video_id() => { - self.passthrough_camera_source_size = + self.world.passthrough.camera_source_size = vec2f(ev.video_width as f32, ev.video_height as f32); cx.redraw_all(); } Event::VideoPlaybackResourcesReleased(ev) if ev.video_id == Self::passthrough_video_id() => { - self.reset_passthrough_camera_state(); + self.world.passthrough.reset_camera_state(); cx.redraw_all(); } Event::VideoDecodingError(ev) if ev.video_id == Self::passthrough_video_id() => { crate::warning!("XR passthrough camera error: {}", ev.error); - self.passthrough_camera_playback_requested = false; - self.passthrough_camera_failed = true; - self.passthrough_camera_has_frame = false; + self.world.passthrough.camera_playback_requested = false; + self.world.passthrough.camera_failed = true; + self.world.passthrough.camera_has_frame = false; cx.redraw_all(); } _ => {} @@ -597,39 +699,37 @@ impl XrEnv { if !widget.visible() { return; } - let Some(node) = widget.cast_inner::() else { - widget.children(&mut |_, child| { + let Some(()) = xr_widget_with_scene_node(widget, |node| { + let (pos, ori, scale) = + Self::transform_with_node(parent_pos, parent_ori, parent_scale, node); + let half = node.physics_half_extents(); + let should_push = node.body_kind() != XrBodyKind::Disabled + && (half.x > 0.0 || half.y > 0.0 || half.z > 0.0); + + if should_push { + cubes.push(CollectedXrCube { + uid: widget.widget_uid(), + body_kind: node.body_kind(), + spawn_pool: node.spawn_pool(), + pose: Pose::new(ori, pos), + scale, + half_extents: vec3f(half.x * scale.x, half.y * scale.y, half.z * scale.z), + physics_shape: node.physics_shape(), + density: node.density(), + friction: node.friction(), + restitution: node.restitution(), + }); + } + + xr_widget_children(widget, &mut |_, child| { + Self::collect_cubes_from_widget(&child, pos, ori, scale, cubes) + }); + }) else { + xr_widget_children(widget, &mut |_, child| { Self::collect_cubes_from_widget(&child, parent_pos, parent_ori, parent_scale, cubes) }); return; }; - - let is_sphere = widget.borrow::().is_some(); - let (pos, ori, scale) = - Self::transform_with_node(parent_pos, parent_ori, parent_scale, &node); - let half = node.physics_half_extents(); - let should_push = node.body_kind() != XrBodyKind::Disabled - && (half.x > 0.0 || half.y > 0.0 || half.z > 0.0); - - if should_push { - cubes.push(CollectedXrCube { - uid: widget.widget_uid(), - body_kind: node.body_kind(), - projectile_pool: node.projectile_pool(), - pose: Pose::new(ori, pos), - scale, - half_extents: vec3f(half.x * scale.x, half.y * scale.y, half.z * scale.z), - is_sphere, - density: node.density(), - friction: node.friction(), - restitution: node.restitution(), - }); - } - - drop(node); - widget.children(&mut |_, child| { - Self::collect_cubes_from_widget(&child, pos, ori, scale, cubes) - }); } fn collect_cubes_from_children( @@ -637,7 +737,7 @@ impl XrEnv { children: &[(LiveId, WidgetRef)], ) -> Vec { let mut cubes = Vec::new(); - let (root_pos, root_ori) = if let Some(root_pose) = self.root_pose { + let (root_pos, root_ori) = if let Some(root_pose) = self.world.physics.root_pose { (root_pose.position, root_pose.orientation) } else { (vec3f(0.0, 0.0, 0.0), Quat::default()) @@ -650,30 +750,38 @@ impl XrEnv { } fn ensure_physics_worker(&mut self, cx: &mut Cx) -> &mut XrPhysicsWorker { - self.physics_worker - .get_or_insert_with(|| XrPhysicsWorker::new(cx.xr_depth_mesh())) + self.world + .physics + .worker + .get_or_insert_with(|| XrPhysicsWorker::new(cx.xr_tsdf())) } fn apply_physics_worker_result(&mut self, result: XrPhysicsWorkerResult) -> bool { - if result.revision != self.physics_revision { + if result.revision != self.world.physics.revision { return false; } - self.runtime_bodies = Rc::new(result.runtime_bodies); + self.world.physics.runtime_bodies = Rc::new(result.runtime_bodies); if let Some(retained_hits) = result.depth_query_retained_hits { - self.depth_query_retained_hits = retained_hits; + self.world.depth.query_retained_hits = retained_hits; } - self.physics_compute_ms = result.physics_compute_ms; - self.physics_step_dt_ms = result.physics_step_dt_ms; - self.physics_depth_query_surface_count = result.physics_depth_query_surface_count; - self.physics_depth_query_vertex_count = result.physics_depth_query_vertex_count; - self.physics_depth_query_triangle_count = result.physics_depth_query_triangle_count; + self.world.physics.metrics = XrPhysicsMetrics { + compute_ms: result.physics_compute_ms, + tsdf_query_ms: result.physics_tsdf_query_ms, + rapier_step_ms: result.physics_rapier_step_ms, + depth_query_surface_count: result.physics_depth_query_surface_count, + scene_body_count: result.physics_scene_body_count, + body_spawn_apply_count: result.physics_body_spawn_apply_count, + body_spawn_miss_count: result.physics_body_spawn_miss_count, + }; true } fn poll_physics_worker(&mut self, cx: &mut Cx) { let mut applied = false; while let Some(result) = self - .physics_worker + .world + .physics + .worker .as_mut() .and_then(|worker| worker.take_latest_result()) { @@ -686,57 +794,62 @@ impl XrEnv { fn request_physics_rebuild(&mut self, cx: &mut Cx, children: &[(LiveId, WidgetRef)]) { let cubes = self.collect_cubes_from_children(children); - self.depth_query_retained_hits.clear(); - self.physics_revision = self.physics_revision.saturating_add(1); - let revision = self.physics_revision; + self.world.depth.query_retained_hits.clear(); + self.world.physics.revision = self.world.physics.revision.saturating_add(1); + let revision = self.world.physics.revision; let gravity = self.gravity; self.ensure_physics_worker(cx) .request_rebuild(revision, gravity, cubes); - self.scene_dirty = false; + self.world.physics.scene_dirty = false; } pub fn ensure_physics(&mut self, cx: &mut Cx, children: &[(LiveId, WidgetRef)]) { self.poll_physics_worker(cx); - if self.scene_dirty || self.physics_worker.is_none() { + if self.world.physics.scene_dirty || self.world.physics.worker.is_none() { self.request_physics_rebuild(cx, children); cx.redraw_all(); } + self.flush_pending_physics_commands(cx); } - pub fn spawn_body(&mut self, cx: &mut Cx, spawn: XrBodySpawn) { - self.poll_physics_worker(cx); - if self.scene_dirty || self.physics_worker.is_none() { - return; - } - let revision = self.physics_revision; - self.ensure_physics_worker(cx) - .request_body_spawn(revision, spawn); + pub fn spawn_body(&mut self, _cx: &mut Cx, spawn: XrBodySpawn) { + self.world.physics.pending_body_spawns.push(spawn); + } + + pub fn despawn_body(&mut self, _cx: &mut Cx, widget_uid: WidgetUid) { + self.world.physics.pending_body_despawns.push(widget_uid); + } + + pub fn apply_body_impulse(&mut self, _cx: &mut Cx, impulse: XrBodyImpulse) { + self.world.physics.pending_body_impulses.push(impulse); } pub fn mark_scene_dirty(&mut self) { - self.scene_dirty = true; + self.world.physics.scene_dirty = true; } pub fn set_root_pose(&mut self, cx: &mut Cx, pose: Option) { - if self.root_pose == pose { + if self.world.physics.root_pose == pose { return; } - self.root_pose = pose; - self.scene_dirty = true; + self.world.physics.root_pose = pose; + self.world.physics.scene_dirty = true; cx.redraw_all(); } #[allow(dead_code)] fn has_dynamic_bodies(&self) -> bool { - !self.runtime_bodies.is_empty() + !self.world.physics.runtime_bodies.is_empty() } pub fn step_physics(&mut self, cx: &mut Cx) { self.poll_physics_worker(cx); - let revision = self.physics_revision; + self.flush_pending_physics_commands(cx); + let revision = self.world.physics.revision; let physics_time_scale = self.physics_time_scale; let include_retained_hits = self.depth_query_hits_visible(); let (left_hand, right_hand) = self + .world .last_xr_state .as_deref() .map(|state| (state.left_hand.clone(), state.right_hand.clone())) @@ -751,21 +864,47 @@ impl XrEnv { } pub fn reset_physics(&mut self, cx: &mut Cx) { - self.physics_revision = self.physics_revision.saturating_add(1); - if let Some(worker) = self.physics_worker.as_mut() { - worker.request_reset(self.physics_revision); - } - self.physics_compute_ms = 0.0; - self.physics_step_dt_ms = 0.0; - self.physics_depth_query_surface_count = 0; - self.physics_depth_query_vertex_count = 0; - self.physics_depth_query_triangle_count = 0; - self.depth_query_retained_hits.clear(); - Rc::make_mut(&mut self.runtime_bodies).clear(); - self.scene_dirty = true; + self.world.physics.revision = self.world.physics.revision.saturating_add(1); + if let Some(worker) = self.world.physics.worker.as_mut() { + worker.request_reset(self.world.physics.revision); + } + self.world.physics.metrics = XrPhysicsMetrics::default(); + self.world.depth.query_retained_hits.clear(); + Rc::make_mut(&mut self.world.physics.runtime_bodies).clear(); + self.world.physics.pending_body_spawns.clear(); + self.world.physics.pending_body_despawns.clear(); + self.world.physics.pending_body_impulses.clear(); + self.world.physics.scene_dirty = true; cx.redraw_all(); } + pub fn flush_pending_physics_commands(&mut self, cx: &mut Cx) { + self.poll_physics_worker(cx); + if self.world.physics.scene_dirty || self.world.physics.worker.is_none() { + return; + } + if self.world.physics.pending_body_spawns.is_empty() + && self.world.physics.pending_body_despawns.is_empty() + && self.world.physics.pending_body_impulses.is_empty() + { + return; + } + let revision = self.world.physics.revision; + let pending_body_spawns = std::mem::take(&mut self.world.physics.pending_body_spawns); + let pending_body_despawns = std::mem::take(&mut self.world.physics.pending_body_despawns); + let pending_body_impulses = std::mem::take(&mut self.world.physics.pending_body_impulses); + let worker = self.ensure_physics_worker(cx); + for spawn in pending_body_spawns { + worker.request_body_spawn(revision, spawn); + } + for widget_uid in pending_body_despawns { + worker.request_body_despawn(revision, widget_uid); + } + for impulse in pending_body_impulses { + worker.request_body_impulse(revision, impulse); + } + } + #[allow(dead_code)] pub(crate) fn runtime_scene_ref(&self) -> Option<&RapierScene> { None @@ -779,17 +918,21 @@ impl XrEnv { // --- New API for XrRoot --- pub fn prepare_and_draw(&mut self, cx: &mut Cx2d) -> XrDrawScopeData { - cx.cx - .xr_depth_mesh() - .set_mesh_enabled(self.depth_mesh_visible()); - let state = self.last_xr_state.clone(); + let state = self.world.last_xr_state.clone(); if let Some(state) = state.as_deref() { if self.depth_debug_enabled() { - self.prepare_depth_mesh(cx); - if self.depth_mesh_visible() { - self.sync_depth_surface_mesh(cx); + self.prepare_depth_mesh(cx, state); + let show_depth_mesh = self.depth_mesh_visible(); + let show_depth_query_hits = self.depth_query_hits_visible(); + if show_depth_mesh { + self.world.depth.poll_surface_mesh_worker(cx); } - self.draw_depth_surface_mesh(cx); + self.world.depth.draw_surface_mesh( + &mut self.draw_depth_mesh, + cx, + show_depth_mesh, + show_depth_query_hits, + ); } if XR_RENDER_HAND_GEOMETRY { @@ -800,27 +943,33 @@ impl XrEnv { } let env_texture = if self.env_cube { - state - .as_deref() - .and_then(|state| self.render_passthrough_env_cube(cx, state)) + state.as_deref().and_then(|state| { + self.world.passthrough.render_env_cube( + &mut self.draw_passthrough_env_face, + cx, + state, + ) + }) } else { None }; XrDrawScopeData { - runtime_bodies: self.runtime_bodies.clone(), + runtime_bodies: self.world.physics.runtime_bodies.clone(), tracking_from_content: Mat4f::identity(), content_from_tracking: Mat4f::identity(), env_texture, camera_texture: self - .passthrough_camera_textures + .world + .passthrough + .camera_textures .as_ref() .map(|textures| textures.camera.clone()), - camera_source_size: self.passthrough_camera_source_size, - camera_rotation_steps: self.passthrough_camera_video.rotation_steps, - camera_center_offset_uv: self.passthrough_camera_center_offset_uv(), - camera_enabled: self.passthrough_camera_has_frame && state.is_some(), - hand_influence_points: Self::draw_scope_hand_influence_points(state.as_deref()), + camera_source_size: self.world.passthrough.camera_source_size, + camera_rotation_steps: self.world.passthrough.camera_video.rotation_steps, + camera_center_offset_uv: self.world.passthrough.camera_center_offset_uv(), + camera_enabled: self.world.passthrough.camera_has_frame && state.is_some(), + hand_influence_points: self.draw_scope_hand_influence_points(state.as_deref()), } } } @@ -832,12 +981,12 @@ pub struct DrawDepthMeshBasic { pub draw_vars: DrawVars, #[live] pub base_color: Vec4f, - #[live] - pub light_dir: Vec3f, #[live(0.006)] - pub normal_bias: f32, - #[live(0.26)] - pub ambient: f32, + pub depth_bias: f32, + #[live(2.4)] + pub wire_outer_px: f32, + #[live(0.7)] + pub wire_inner_px: f32, } impl DrawDepthMeshBasic { diff --git a/xr/src/scene/xr_gesture.rs b/xr/src/scene/xr_gesture.rs new file mode 100644 index 0000000000..cc8744184f --- /dev/null +++ b/xr/src/scene/xr_gesture.rs @@ -0,0 +1,93 @@ +use crate::prelude::*; + +const OPEN_HAND_SYNC_FORWARD_OFFSET_METERS: f32 = 0.040; + +#[derive(Clone, Copy, Debug)] +pub(crate) struct XrArmPairMetrics { + pub left_forward: f32, + pub right_forward: f32, + pub left_lateral: f32, + pub right_lateral: f32, + pub hand_gap: f32, + pub average_forward_distance: f32, + pub left_elevation_degrees: f32, + pub right_elevation_degrees: f32, +} + +pub(crate) fn flat_head_forward(orientation: Quat) -> Vec3f { + let mut forward = orientation.rotate_vec3(&vec3f(0.0, 0.0, -1.0)); + forward.y = 0.0; + if forward.length() <= 1.0e-6 { + vec3f(0.0, 0.0, -1.0) + } else { + forward.normalize() + } +} + +pub(crate) fn flat_head_right(orientation: Quat) -> Vec3f { + let mut right = orientation.rotate_vec3(&vec3f(1.0, 0.0, 0.0)); + right.y = 0.0; + if right.length() <= 1.0e-6 { + vec3f(1.0, 0.0, 0.0) + } else { + right.normalize() + } +} + +pub(crate) fn arm_pair_metrics( + head_pose: Pose, + left_point: Vec3f, + right_point: Vec3f, +) -> Option { + let forward = flat_head_forward(head_pose.orientation); + let right = flat_head_right(head_pose.orientation); + let left_local = left_point - head_pose.position; + let right_local = right_point - head_pose.position; + let left_forward = left_local.dot(forward); + let right_forward = right_local.dot(forward); + let left_lateral = left_local.dot(right); + let right_lateral = right_local.dot(right); + let hand_gap = (right_point - left_point).length(); + let left_horizontal = (left_forward * left_forward + left_lateral * left_lateral).sqrt(); + let right_horizontal = (right_forward * right_forward + right_lateral * right_lateral).sqrt(); + if !left_horizontal.is_finite() + || !right_horizontal.is_finite() + || left_horizontal <= 1.0e-4 + || right_horizontal <= 1.0e-4 + { + return None; + } + Some(XrArmPairMetrics { + left_forward, + right_forward, + left_lateral, + right_lateral, + hand_gap, + average_forward_distance: (left_forward + right_forward) * 0.5, + left_elevation_degrees: left_local.y.atan2(left_horizontal).abs().to_degrees(), + right_elevation_degrees: right_local.y.atan2(right_horizontal).abs().to_degrees(), + }) +} + +pub(crate) fn hand_closed_fist_contact_point( + hand: &XrHand, + forward: Vec3f, + _is_left: bool, +) -> Option { + if !(hand.is_open() && hand.is_upright_for_box_sync()) { + return None; + } + hand.tracking_pose() + .map(|pose| pose.position + forward.scale(OPEN_HAND_SYNC_FORWARD_OFFSET_METERS)) +} + +pub(crate) fn hand_closed_fist_contact_point_geometry_only( + hand: &XrHand, + forward: Vec3f, + is_left: bool, +) -> Option { + hand_closed_fist_contact_point(hand, forward, is_left) +} + +#[cfg(test)] +include!("../tests/scene/xr_gesture.rs"); diff --git a/xr/src/scene/xr_hands.rs b/xr/src/scene/xr_hands.rs index 7a754eb62c..79fc1c6309 100644 --- a/xr/src/scene/xr_hands.rs +++ b/xr/src/scene/xr_hands.rs @@ -1,7 +1,7 @@ use super::xr_physics::{capsule_pose, makepad_pose, HandCollider, HandColliderBody, RapierScene}; use super::*; -impl XrEnv { +impl XrHandSystem { fn pose_point_world(pose: Pose, local: Vec3f) -> Vec3f { pose.to_mat4().transform_vec4(local.to_vec4()).to_vec3f() } @@ -18,20 +18,16 @@ impl XrEnv { colliders.push(HandCollider::Box { pose, half_extents }); } - fn hand_plate_pose(hand: &XrHand) -> Pose { - let palm_pose = hand.joints[XrHand::CENTER]; - Pose::new( + fn hand_plate_pose(hand: &XrHand) -> Option { + let palm_pose = hand.tracking_pose()?; + Some(Pose::new( palm_pose.orientation, Self::pose_point_world(palm_pose, vec3f(0.0, 0.0, XR_HAND_PLATE_FORWARD_OFFSET)), - ) + )) } - fn hand_tip_world(hand: &XrHand, finger_index: usize) -> Vec3f { - let tip_len = hand.tips[finger_index].max(0.0); - hand.joints[XrHand::END_KNUCKLES[finger_index]] - .to_mat4() - .transform_vec4(vec4(0.0, 0.0, -tip_len, 1.0)) - .to_vec3f() + fn hand_tip_world(hand: &XrHand, finger_index: usize) -> Option { + hand.tip_pos_checked(finger_index) } fn append_finger_chain_colliders( @@ -41,22 +37,19 @@ impl XrEnv { tip_index: usize, radius: f32, ) { - for segment in chain.windows(2) { - Self::append_capsule_collider( - colliders, - hand.joints[segment[0]].position, - hand.joints[segment[1]].position, - radius, - ); + let Some(points) = hand.joint_chain_positions(chain) else { + return; + }; + for segment in points.windows(2) { + Self::append_capsule_collider(colliders, segment[0], segment[1], radius); } if hand.tip_active(tip_index) { - let end_joint = *chain.last().unwrap_or(&XrHand::CENTER); - Self::append_capsule_collider( - colliders, - hand.joints[end_joint].position, + if let (Some(end_joint), Some(tip_world)) = ( + points.last().copied(), Self::hand_tip_world(hand, tip_index), - radius * 0.85, - ); + ) { + Self::append_capsule_collider(colliders, end_joint, tip_world, radius * 0.85); + } } } @@ -67,25 +60,29 @@ impl XrEnv { radius: f32, ) { if hand.tip_active(tip_index) { - Self::append_ball_collider(colliders, Self::hand_tip_world(hand, tip_index), radius); + if let Some(tip_world) = Self::hand_tip_world(hand, tip_index) { + Self::append_ball_collider(colliders, tip_world, radius); + } } } - fn build_hand_colliders(hand: &XrHand) -> Vec { + pub(super) fn build_hand_colliders(&self, hand: &XrHand) -> Vec { let mut colliders = Vec::with_capacity(XR_HAND_COLLIDER_SLOTS_PER_HAND); if !hand.in_view() { return colliders; } - Self::append_box_collider( - &mut colliders, - Self::hand_plate_pose(hand), - vec3f( - XR_HAND_PLATE_HALF_WIDTH, - XR_HAND_PLATE_HALF_HEIGHT, - XR_HAND_PLATE_HALF_DEPTH, - ), - ); + if let Some(hand_plate_pose) = Self::hand_plate_pose(hand) { + Self::append_box_collider( + &mut colliders, + hand_plate_pose, + vec3f( + XR_HAND_PLATE_HALF_WIDTH, + XR_HAND_PLATE_HALF_HEIGHT, + XR_HAND_PLATE_HALF_DEPTH, + ), + ); + } Self::append_finger_chain_colliders( &mut colliders, @@ -182,6 +179,7 @@ impl XrEnv { #[allow(dead_code)] pub(super) fn collect_live_hand_colliders( + &self, scene: &RapierScene, slots: &[HandColliderBody], ) -> Vec { @@ -235,6 +233,83 @@ impl XrEnv { colliders } + fn hand_influence_tip_world(&self, hand: &XrHand, tip: usize) -> Option { + if !hand.in_view() || !hand.tip_active(tip) { + return None; + } + hand.tip_pos_checked(tip) + } + + fn hand_influence_point( + &self, + pos: Vec3f, + gain_scale: f32, + radius_scale: f32, + ) -> XrHandInfluencePoint { + XrHandInfluencePoint { + pos, + gain_scale, + radius_scale, + } + } + + fn palm_world(&self, hand: &XrHand) -> Option { + let center = hand.joint_pose_checked(XrHand::CENTER)?.position; + let wrist = hand.joint_pose_checked(XrHand::WRIST)?.position; + let thumb = hand.joint_pose_checked(XrHand::THUMB_BASE)?.position; + let index = hand.joint_pose_checked(XrHand::INDEX_BASE)?.position; + let middle = hand.joint_pose_checked(XrHand::MIDDLE_BASE)?.position; + let ring = hand.joint_pose_checked(XrHand::RING_BASE)?.position; + let little = hand.joint_pose_checked(XrHand::LITTLE_BASE)?.position; + Some( + center * 0.28 + + wrist * 0.10 + + thumb * 0.12 + + index * 0.13 + + middle * 0.18 + + ring * 0.11 + + little * 0.08, + ) + } + + fn write_influence_points(&self, hand: &XrHand, target: &mut [Option]) { + debug_assert_eq!(target.len(), XR_HAND_INFLUENCE_POINTS_PER_HAND); + target[0] = self + .hand_influence_tip_world(hand, XrHand::THUMB_TIP) + .map(|pos| self.hand_influence_point(pos, 0.72, 0.92)); + target[1] = self + .hand_influence_tip_world(hand, XrHand::INDEX_TIP) + .map(|pos| self.hand_influence_point(pos, 1.00, 1.00)); + target[2] = self + .hand_influence_tip_world(hand, XrHand::MIDDLE_TIP) + .map(|pos| self.hand_influence_point(pos, 0.96, 1.00)); + target[3] = self + .hand_influence_tip_world(hand, XrHand::RING_TIP) + .map(|pos| self.hand_influence_point(pos, 0.82, 0.94)); + target[4] = self + .hand_influence_tip_world(hand, XrHand::LITTLE_TIP) + .map(|pos| self.hand_influence_point(pos, 0.68, 0.88)); + target[5] = self + .palm_world(hand) + .map(|pos| self.hand_influence_point(pos, 1.30, 2.40)); + } + + pub(super) fn draw_scope_hand_influence_points( + &self, + state: Option<&XrState>, + ) -> [Option; XR_HAND_INFLUENCE_POINT_COUNT] { + let mut points = [None; XR_HAND_INFLUENCE_POINT_COUNT]; + let Some(state) = state else { + return points; + }; + let (left_points, right_points) = points.split_at_mut(XR_HAND_INFLUENCE_POINTS_PER_HAND); + self.write_influence_points(&state.left_hand, left_points); + self.write_influence_points(&state.right_hand, right_points); + points + } +} + +impl XrEnv { fn draw_hand_shapes(&mut self, cx: &mut Cx2d, colliders: &[HandCollider], is_left: bool) { let color = if is_left { vec4(0.18, 0.72, 1.0, 1.0) @@ -284,7 +359,7 @@ impl XrEnv { let colliders = if let Some(physics_colliders) = physics_colliders { physics_colliders } else { - raw_colliders = Self::build_hand_colliders(hand); + raw_colliders = self.world.hands.build_hand_colliders(hand); &raw_colliders }; self.draw_hand_shapes(cx, colliders, is_left); @@ -295,33 +370,10 @@ impl XrEnv { } self.draw_cube.end_many_instances(cx); } - - #[allow(dead_code)] - pub(super) fn sync_hands(scene: Option<&mut RapierScene>, state: &XrState) { - if !XR_ENABLE_HAND_PHYSICS { - return; - } - - let Some(scene) = scene else { - return; - }; - - let left = Self::build_hand_colliders(&state.left_hand); - let right = Self::build_hand_colliders(&state.right_hand); - let RapierScene { - bodies, - colliders, - left_hand, - right_hand, - .. - } = scene; - RapierScene::sync_hand_bodies(left_hand, &left, bodies, colliders); - RapierScene::sync_hand_bodies(right_hand, &right, bodies, colliders); - } } pub(super) fn build_hand_colliders_for_physics(hand: &XrHand) -> Vec { - XrEnv::build_hand_colliders(hand) + XrHandSystem.build_hand_colliders(hand) } pub(super) fn sync_hands_on_scene( @@ -348,4 +400,5 @@ pub(super) fn sync_hands_on_scene( } = scene; RapierScene::sync_hand_bodies(left_slots, &left, bodies, colliders); RapierScene::sync_hand_bodies(right_slots, &right, bodies, colliders); + scene.sync_tracked_hands(left_hand, right_hand); } diff --git a/xr/src/scene/xr_node.rs b/xr/src/scene/xr_node.rs index e99c669c34..633c5a6386 100644 --- a/xr/src/scene/xr_node.rs +++ b/xr/src/scene/xr_node.rs @@ -1,15 +1,15 @@ -use crate::{ +use crate::prelude::XrSharedHand; +use makepad_widgets::{ makepad_derive_widget::*, makepad_draw::*, makepad_script::ScriptFnRef, widget::*, widget_async::{CxWidgetToScriptCallExt, ScriptAsyncCalls, ScriptAsyncId, ScriptAsyncResult}, widget_tree::CxWidgetExt, - Cube, Gltf, IcoSphere, RefractiveCube, Tree, XrSelect, XrView, }; use std::{cmp::Ordering, collections::HashMap, rc::Rc}; -use super::scene_draw::compose_scene_node_transform; +use crate::util::scene_draw::compose_scene_node_transform; script_mod! { use mod.prelude.widgets_internal.* @@ -17,10 +17,22 @@ script_mod! { let XrBodyKind = set_type_default() do #(XrBodyKind::script_api(vm)) mod.widgets.XrBodyKind = XrBodyKind + let XrPhysicsShape = set_type_default() do #(XrPhysicsShape::script_api(vm)) + mod.widgets.XrPhysicsShape = XrPhysicsShape + + let XrRenderClass = set_type_default() do #(XrRenderClass::script_api(vm)) + mod.widgets.XrRenderClass = XrRenderClass + + let XrSharedObjectPolicy = set_type_default() do #(XrSharedObjectPolicy::script_api(vm)) + mod.widgets.XrSharedObjectPolicy = XrSharedObjectPolicy + mod.widgets.XrNodeBase = #(XrNode::register_widget(vm)) mod.widgets.XrNode = set_type_default() do mod.widgets.XrNodeBase{ body: XrBodyKind.Disabled - projectile_pool: false + physics_shape: XrPhysicsShape.Box + render_class: XrRenderClass.Opaque + shared_object_policy: XrSharedObjectPolicy.None + spawn_pool: false physics_size: vec3(0.0, 0.0, 0.0) density: 1.0 friction: 0.8 @@ -42,6 +54,47 @@ impl Default for XrBodyKind { } } +#[derive(Clone, Copy, Debug, PartialEq, Eq, Script, ScriptHook)] +pub enum XrPhysicsShape { + #[pick] + Box, + Sphere, +} + +impl Default for XrPhysicsShape { + fn default() -> Self { + Self::Box + } +} + +#[derive(Clone, Copy, Debug, PartialEq, Eq, Script, ScriptHook)] +pub enum XrRenderClass { + #[pick] + Opaque, + Transparent, +} + +impl Default for XrRenderClass { + fn default() -> Self { + Self::Opaque + } +} + +#[derive(Clone, Copy, Debug, PartialEq, Eq, Script, ScriptHook)] +pub enum XrSharedObjectPolicy { + None, + #[pick] + BootstrapShared, + OnDemandShared, + PooledOnDemand, +} + +impl Default for XrSharedObjectPolicy { + fn default() -> Self { + Self::None + } +} + pub const XR_HAND_INFLUENCE_POINTS_PER_HAND: usize = 6; pub const XR_HAND_INFLUENCE_POINT_COUNT: usize = XR_HAND_INFLUENCE_POINTS_PER_HAND * 2; @@ -56,6 +109,17 @@ pub struct XrHandInfluencePoint { pub struct XrRuntimeBodyState { pub pose: Pose, pub scale: Vec3f, + pub linvel: Vec3f, + pub angvel: Vec3f, + pub sleeping: bool, + pub held_by: Option, +} + +#[derive(Clone, Copy, Debug, Default)] +pub enum XrNodeAction { + SceneChanged, + #[default] + None, } #[derive(Clone, Default)] @@ -72,46 +136,6 @@ pub struct XrDrawScopeData { pub hand_influence_points: [Option; XR_HAND_INFLUENCE_POINT_COUNT], } -pub fn xr_runtime_body_from_scope(scope: &mut Scope, uid: WidgetUid) -> Option { - scope - .data - .get::() - .and_then(|scope_data| scope_data.runtime_bodies.get(&uid).cloned()) -} - -pub fn xr_tracking_from_content_from_scope(scope: &mut Scope) -> Mat4f { - scope - .data - .get::() - .map(|scope_data| scope_data.tracking_from_content) - .unwrap_or_else(Mat4f::identity) -} - -pub fn xr_content_from_tracking_from_scope(scope: &mut Scope) -> Mat4f { - scope - .data - .get::() - .map(|scope_data| scope_data.content_from_tracking) - .unwrap_or_else(Mat4f::identity) -} - -pub fn xr_hand_influence_points_from_scope( - scope: &mut Scope, -) -> [Option; XR_HAND_INFLUENCE_POINT_COUNT] { - scope - .data - .get::() - .map(|scope_data| scope_data.hand_influence_points) - .unwrap_or([None; XR_HAND_INFLUENCE_POINT_COUNT]) -} - -pub fn xr_env_texture_from_scope(scope: &mut Scope) -> Option { - scope - .data - .get::() - .and_then(|scope_data| scope_data.env_texture.clone()) -} - #[derive(Clone, Default)] pub struct XrPassthroughScopeData { pub camera_texture: Option, @@ -121,18 +145,53 @@ pub struct XrPassthroughScopeData { pub enabled: bool, } -pub fn xr_passthrough_from_scope(scope: &mut Scope) -> XrPassthroughScopeData { - scope - .data - .get::() - .map(|scope_data| XrPassthroughScopeData { - camera_texture: scope_data.camera_texture.clone(), - source_size: scope_data.camera_source_size, - rotation_steps: scope_data.camera_rotation_steps, - center_offset_uv: scope_data.camera_center_offset_uv, - enabled: scope_data.camera_enabled, - }) - .unwrap_or_default() +#[derive(Clone, Default)] +pub struct XrDrawContext { + scope_data: XrDrawScopeData, +} + +impl XrDrawContext { + pub fn from_scope(scope: &mut Scope) -> Self { + Self { + scope_data: scope + .data + .get::() + .cloned() + .unwrap_or_default(), + } + } + + pub fn runtime_body(&self, uid: WidgetUid) -> Option { + self.scope_data.runtime_bodies.get(&uid).cloned() + } + + pub fn tracking_from_content(&self) -> Mat4f { + self.scope_data.tracking_from_content + } + + pub fn content_from_tracking(&self) -> Mat4f { + self.scope_data.content_from_tracking + } + + pub fn hand_influence_points( + &self, + ) -> [Option; XR_HAND_INFLUENCE_POINT_COUNT] { + self.scope_data.hand_influence_points + } + + pub fn env_texture(&self) -> Option { + self.scope_data.env_texture.clone() + } + + pub fn passthrough(&self) -> XrPassthroughScopeData { + XrPassthroughScopeData { + camera_texture: self.scope_data.camera_texture.clone(), + source_size: self.scope_data.camera_source_size, + rotation_steps: self.scope_data.camera_rotation_steps, + center_offset_uv: self.scope_data.camera_center_offset_uv, + enabled: self.scope_data.camera_enabled, + } + } } #[derive(Script, WidgetRef, WidgetRegister)] @@ -157,8 +216,14 @@ pub struct XrNode { scale: Vec3f, #[live] body: XrBodyKind, + #[live] + physics_shape: XrPhysicsShape, + #[live] + render_class: XrRenderClass, + #[live] + shared_object_policy: XrSharedObjectPolicy, #[live(false)] - projectile_pool: bool, + spawn_pool: bool, #[live(vec3(0.0, 0.0, 0.0))] physics_size: Vec3f, #[rust] @@ -217,8 +282,31 @@ impl XrNode { self.body } - pub fn projectile_pool(&self) -> bool { - self.projectile_pool + pub fn physics_shape(&self) -> XrPhysicsShape { + self.physics_shape + } + + pub fn render_class(&self) -> XrRenderClass { + self.render_class + } + + pub fn shared_object_policy(&self) -> XrSharedObjectPolicy { + self.shared_object_policy + } + + pub fn bootstrap_shared(&self) -> bool { + matches!( + self.shared_object_policy, + XrSharedObjectPolicy::BootstrapShared + ) + } + + pub fn is_transparent(&self) -> bool { + matches!(self.render_class, XrRenderClass::Transparent) + } + + pub fn spawn_pool(&self) -> bool { + self.spawn_pool } pub fn set_implicit_physics_size(&mut self, size: Vec3f) { @@ -259,35 +347,11 @@ impl XrNode { } fn child_world_sort_center(child: &WidgetRef) -> Option { - if let Some(select) = child.borrow::() { - return Some(select.node().pos()); - } - if let Some(view) = child.borrow::() { - return Some(view.node().pos()); - } - if let Some(cube) = child.borrow::() { - return Some(cube.node().pos()); - } - if let Some(ico) = child.borrow::() { - return Some(ico.node().pos()); - } - if let Some(refractive_cube) = child.borrow::() { - return Some(refractive_cube.node().pos()); - } - if let Some(gltf) = child.borrow::() { - return Some(gltf.node().pos()); - } - if let Some(tree) = child.borrow::() { - return Some(tree.node().pos()); - } - if let Some(node) = child.borrow::() { - return Some(node.pos()); - } - None + xr_widget_local_sort_center(child) } fn child_is_transparent(child: &WidgetRef) -> bool { - child.borrow::().is_some() || child.borrow::().is_some() + xr_widget_is_transparent(child) } fn transform_point(transform: &Mat4f, point: Vec3f) -> Vec3f { @@ -295,48 +359,74 @@ impl XrNode { .transform_vec4(vec4f(point.x, point.y, point.z, 1.0)) .to_vec3f() } +} - fn draw_list_depth(scene_state: &SceneState3D, world_pos: Vec3f) -> f32 { - let view_pos = - scene_state - .view - .transform_vec4(vec4f(world_pos.x, world_pos.y, world_pos.z, 1.0)); - if view_pos.w.abs() > 1.0e-6 { - view_pos.z / view_pos.w - } else { - view_pos.z - } +pub fn xr_widget_with_scene_node( + widget: &WidgetRef, + visit: impl FnOnce(&XrNode) -> R, +) -> Option { + if let Some(node) = widget.borrow::() { + return Some(visit(&node)); + } + if let Some(node) = widget.cast_inner::() { + return Some(visit(&node)); } + None +} - fn sort_child_draw_order(draw_order_entries: &mut [(usize, f32, bool)]) { - if draw_order_entries.len() <= 1 { - return; - } +pub fn xr_widget_children(widget: &WidgetRef, visit: &mut dyn FnMut(LiveId, WidgetRef)) { + widget.children(visit); +} - draw_order_entries.sort_by(|a, b| match (a.2, b.2) { - (false, true) => Ordering::Less, - (true, false) => Ordering::Greater, - (false, false) => { - b.1.partial_cmp(&a.1) - .unwrap_or(Ordering::Equal) - .then_with(|| a.0.cmp(&b.0)) - } - (true, true) => { - a.1.partial_cmp(&b.1) - .unwrap_or(Ordering::Equal) - .then_with(|| a.0.cmp(&b.0)) - } - }); +pub fn xr_widget_local_sort_center(widget: &WidgetRef) -> Option { + xr_widget_with_scene_node(widget, |node| node.pos()) +} + +pub fn xr_widget_is_transparent(widget: &WidgetRef) -> bool { + xr_widget_with_scene_node(widget, |node| node.is_transparent()).unwrap_or(false) +} + +pub fn xr_draw_list_depth(scene_state: &SceneState3D, world_pos: Vec3f) -> f32 { + let view_pos = + scene_state + .view + .transform_vec4(vec4f(world_pos.x, world_pos.y, world_pos.z, 1.0)); + if view_pos.w.abs() > 1.0e-6 { + view_pos.z / view_pos.w + } else { + view_pos.z } } +pub fn xr_sort_child_draw_order(draw_order_entries: &mut [(usize, f32, bool)]) { + if draw_order_entries.len() <= 1 { + return; + } + + draw_order_entries.sort_by(|a, b| match (a.2, b.2) { + (false, true) => Ordering::Less, + (true, false) => Ordering::Greater, + (false, false) => { + b.1.partial_cmp(&a.1) + .unwrap_or(Ordering::Equal) + .then_with(|| a.0.cmp(&b.0)) + } + (true, true) => { + a.1.partial_cmp(&b.1) + .unwrap_or(Ordering::Equal) + .then_with(|| a.0.cmp(&b.0)) + } + }); +} + pub fn xr_widget_world_transform( cx: &mut Cx3d, scope: &mut Scope, uid: WidgetUid, node: &XrNode, ) -> Mat4f { - if let Some(runtime_body) = xr_runtime_body_from_scope(scope, uid) { + let draw_context = XrDrawContext::from_scope(scope); + if let Some(runtime_body) = draw_context.runtime_body(uid) { Mat4f::mul( &runtime_body.pose.to_mat4(), &Mat4f::nonuniform_scaled_translation( @@ -498,6 +588,8 @@ impl Widget for XrNode { if call.method() == id!(render) && !result.is_err() { if let Some(me_obj) = call.me().as_object() { self.script_apply(vm, &Apply::Reload, &mut Scope::empty(), me_obj.into()); + vm.cx_mut() + .widget_action(self.uid, XrNodeAction::SceneChanged); vm.cx_mut().redraw_all(); } } @@ -553,14 +645,14 @@ impl Widget for XrNode { let child_center = Self::transform_point(&world_transform, child_center); draw_order_entries.push(( index, - Self::draw_list_depth(&scene_state, child_center), + xr_draw_list_depth(&scene_state, child_center), Self::child_is_transparent(&child), )); } else { draw_order_entries.push((index, 0.0, false)); } } - Self::sort_child_draw_order(&mut draw_order_entries); + xr_sort_child_draw_order(&mut draw_order_entries); for (index, _, _) in draw_order_entries { let id = self.child_order[index]; diff --git a/xr/src/scene/xr_passthrough.rs b/xr/src/scene/xr_passthrough.rs index de9af72cd3..9bde915af3 100644 --- a/xr/src/scene/xr_passthrough.rs +++ b/xr/src/scene/xr_passthrough.rs @@ -63,9 +63,9 @@ impl XrPassthroughEnvCube { } } -impl XrEnv { - pub(crate) fn passthrough_camera_center_offset_uv(&self) -> Vec2f { - let source_size = self.passthrough_camera_source_size; +impl XrPassthroughRuntime { + pub(crate) fn camera_center_offset_uv(&self) -> Vec2f { + let source_size = self.camera_source_size; let aspect = if source_size.y > 1.0 { source_size.x / source_size.y } else { @@ -81,9 +81,7 @@ impl XrEnv { ) } - pub(super) fn pick_passthrough_camera_choice( - ev: &VideoInputsEvent, - ) -> Option { + pub(super) fn pick_camera_choice(ev: &VideoInputsEvent) -> Option { fn better( a: &makepad_widgets::makepad_platform::video::VideoFormat, b: &makepad_widgets::makepad_platform::video::VideoFormat, @@ -139,63 +137,68 @@ impl XrEnv { }) } - pub(super) fn reset_passthrough_camera_state(&mut self) { - self.passthrough_camera_playback_requested = false; - self.passthrough_camera_failed = false; - self.passthrough_camera_textures = None; - self.passthrough_camera_video = VideoYuvMetadata::disabled(); - self.passthrough_camera_has_frame = false; - if let Some(cube) = self.passthrough_env_cube.as_mut() { + pub(super) fn reset_camera_state(&mut self) { + self.camera_playback_requested = false; + self.camera_failed = false; + self.camera_textures = None; + self.camera_video = VideoYuvMetadata::disabled(); + self.camera_has_frame = false; + self.camera_choice_wait_logged = false; + if let Some(cube) = self.env_cube.as_mut() { cube.reset_state(); } } - pub(super) fn sync_passthrough_camera(&mut self, cx: &mut Cx) { + pub(super) fn sync_camera(&mut self, cx: &mut Cx, video_id: LiveId) { if matches!( - self.passthrough_camera_permission, + self.camera_permission, Some(PermissionStatus::DeniedCanRetry) | Some(PermissionStatus::DeniedPermanent) ) { crate::warning!( "XR passthrough camera: sync blocked by permission state {:?}", - self.passthrough_camera_permission + self.camera_permission ); return; } - let Some(choice) = self.passthrough_camera_choice.clone() else { - crate::warning!("XR passthrough camera: sync waiting for camera choice"); + let Some(choice) = self.camera_choice.clone() else { + if !self.camera_choice_wait_logged { + crate::warning!("XR passthrough camera: sync waiting for camera choice"); + self.camera_choice_wait_logged = true; + } return; }; + self.camera_choice_wait_logged = false; - self.passthrough_camera_source_size = vec2f(choice.width as f32, choice.height as f32); - if self.passthrough_camera_textures.is_none() { - self.passthrough_camera_textures = Some(XrPassthroughCameraTextures { + self.camera_source_size = vec2f(choice.width as f32, choice.height as f32); + if self.camera_textures.is_none() { + self.camera_textures = Some(XrPassthroughCameraTextures { camera: Texture::new_with_format(cx, TextureFormat::VideoExternal), tex_y: None, tex_u: None, tex_v: None, }); } - if self.passthrough_camera_failed || self.passthrough_camera_playback_requested { + if self.camera_failed || self.camera_playback_requested { return; } cx.prepare_headset_camera_playback( - Self::passthrough_video_id(), + video_id, VideoSource::Camera(choice.input_id, choice.format_id), CameraPreviewMode::Texture, 0, - self.passthrough_camera_textures + self.camera_textures .as_ref() .map(|textures| textures.camera.texture_id()) .unwrap_or_default(), false, false, ); - self.passthrough_camera_playback_requested = true; + self.camera_playback_requested = true; } - fn upsert_passthrough_env_face_geometry(&mut self, cx: &mut Cx2d, size: f64) -> GeometryId { + fn upsert_env_face_geometry(&mut self, cx: &mut Cx2d, size: f64) -> GeometryId { let corners = [ [0.0f32, 0.0f32, 0.0f32], [size as f32, 0.0f32, 0.0f32], @@ -216,7 +219,7 @@ impl XrEnv { } let indices = vec![0, 1, 2, 2, 3, 0]; let geometry = self - .passthrough_env_face_quad + .env_face_quad .get_or_insert_with(|| Geometry::new(cx.cx.cx)); geometry.update(cx.cx.cx, indices, vertices); geometry.geometry_id() @@ -300,35 +303,29 @@ impl XrEnv { ranked.into_iter().map(|(face, _)| face).collect() } - pub fn render_passthrough_env_cube( + pub fn render_env_cube( &mut self, + draw_passthrough_env_face: &mut DrawPassthroughEnvFace, cx: &mut Cx2d, state: &XrState, ) -> Option { - let source_size = self.passthrough_camera_source_size; - let rotation_steps = self.passthrough_camera_video.rotation_steps; - let camera_enabled = if self.passthrough_camera_has_frame { - 1.0 - } else { - 0.0 - }; + let source_size = self.camera_source_size; + let rotation_steps = self.camera_video.rotation_steps; + let camera_enabled = if self.camera_has_frame { 1.0 } else { 0.0 }; let camera_texture = self - .passthrough_camera_textures + .camera_textures .as_ref() .map(|textures| textures.camera.clone())?; let face_size = XR_PASSTHROUGH_ENV_FACE_SIZE as f64; let (_, camera_right, camera_up, camera_forward) = Self::current_head_basis(state); - let camera_center_offset_uv = self.passthrough_camera_center_offset_uv(); - let geometry_id = self.upsert_passthrough_env_face_geometry(cx, face_size); + let camera_center_offset_uv = self.camera_center_offset_uv(); + let geometry_id = self.upsert_env_face_geometry(cx, face_size); let visible_faces = Self::visible_env_faces(source_size, camera_right, camera_up, camera_forward); - let Self { - passthrough_env_cube, - draw_passthrough_env_face, - .. - } = self; - let cube = passthrough_env_cube.get_or_insert_with(|| XrPassthroughEnvCube::new(cx.cx.cx)); + let cube = self + .env_cube + .get_or_insert_with(|| XrPassthroughEnvCube::new(cx.cx.cx)); for face_index in visible_faces { let face = &mut cube.faces[face_index]; diff --git a/xr/src/scene/xr_people_debug.rs b/xr/src/scene/xr_people_debug.rs deleted file mode 100644 index 4c40f9a052..0000000000 --- a/xr/src/scene/xr_people_debug.rs +++ /dev/null @@ -1,1291 +0,0 @@ -use crate::*; -use std::{ - collections::HashMap, - sync::{mpsc::TryRecvError, Arc, Condvar, Mutex}, - thread::{self, JoinHandle}, - time::Instant, -}; - -script_mod! { - use mod.prelude.widgets_internal.* - - mod.widgets.XrPeopleDebugBase = #(XrPeopleDebug::register_widget(vm)) - mod.widgets.XrPeopleDebug = set_type_default() do mod.widgets.XrPeopleDebugBase{ - body: mod.widgets.XrBodyKind.Disabled - draw_cube +: { - light_dir: vec3(0.35, 0.8, 0.45) - } - } -} - -#[derive(Clone, Copy, Debug, Default, PartialEq, Eq)] -enum RemoteTransformSource { - #[default] - Raw, - Descriptor, -} - -#[derive(Clone, Debug)] -struct RemotePeerState { - peer: XrNetPeer, - latest_state: Option, - has_descriptor: bool, - remote_to_local: Option, - transform_source: RemoteTransformSource, - last_solve_diagnostic: Option, -} - -impl RemotePeerState { - fn new(peer: XrNetPeer) -> Self { - Self { - peer, - latest_state: None, - has_descriptor: false, - remote_to_local: None, - transform_source: RemoteTransformSource::Raw, - last_solve_diagnostic: None, - } - } -} - -#[derive(Clone, Debug)] -struct AlignmentWorkerPeerState { - peer: XrNetPeer, - latest_descriptor: Option, - remote_to_local: Option, - last_accepted_solution: Option, - last_solve_diagnostic: Option, - last_solve_ms: f64, - last_solved_local_descriptor_version: Option<(u64, u64)>, - last_solved_remote_descriptor_seq: Option, -} - -impl AlignmentWorkerPeerState { - fn new(peer: XrNetPeer) -> Self { - Self { - peer, - latest_descriptor: None, - remote_to_local: None, - last_accepted_solution: None, - last_solve_diagnostic: None, - last_solve_ms: 0.0, - last_solved_local_descriptor_version: None, - last_solved_remote_descriptor_seq: None, - } - } - - fn to_result(&self) -> AlignmentWorkerPeerResult { - AlignmentWorkerPeerResult { - remote_to_local: self.remote_to_local, - transform_source: if self.remote_to_local.is_some() { - RemoteTransformSource::Descriptor - } else { - RemoteTransformSource::Raw - }, - last_solve_diagnostic: self.last_solve_diagnostic, - } - } -} - -#[derive(Clone, Copy, Debug, Default)] -struct AlignmentWorkerPeerResult { - remote_to_local: Option, - transform_source: RemoteTransformSource, - last_solve_diagnostic: Option, -} - -#[derive(Clone, Debug)] -struct XrPeopleAlignmentWorkerResult { - peer_results: HashMap, - alignment_debug_text: String, -} - -#[derive(Clone)] -enum PendingLocalDescriptorUpdate { - Set { - frame: XrNetAlignmentDescriptorFrame, - version: (u64, u64), - }, - Clear, -} - -#[derive(Clone)] -enum PendingPeerDescriptorUpdate { - Set { - peer: XrNetPeer, - frame: XrNetAlignmentDescriptorFrame, - }, - Remove, -} - -#[derive(Default)] -struct XrPeopleAlignmentWorkerMailbox { - version: u64, - shutdown: bool, - pending_local_descriptor: Option, - pending_peer_updates: HashMap, -} - -struct XrPeopleAlignmentWorker { - mailbox: Arc<(Mutex, Condvar)>, - latest_result: Arc>>, - join_handle: Option>, -} - -impl XrPeopleAlignmentWorker { - fn new() -> Self { - let mailbox = Arc::new(( - Mutex::new(XrPeopleAlignmentWorkerMailbox::default()), - Condvar::new(), - )); - let latest_result = Arc::new(Mutex::new(None)); - let mailbox_thread = mailbox.clone(); - let result_thread = latest_result.clone(); - let join_handle = Some(thread::spawn(move || { - xr_people_alignment_worker_loop(mailbox_thread, result_thread) - })); - - Self { - mailbox, - latest_result, - join_handle, - } - } - - fn set_local_descriptor(&mut self, frame: XrNetAlignmentDescriptorFrame, version: (u64, u64)) { - self.send_mailbox_update(|mailbox| { - mailbox.pending_local_descriptor = - Some(PendingLocalDescriptorUpdate::Set { frame, version }); - }); - } - - fn clear_local_descriptor(&mut self) { - self.send_mailbox_update(|mailbox| { - mailbox.pending_local_descriptor = Some(PendingLocalDescriptorUpdate::Clear); - }); - } - - fn set_peer_descriptor(&mut self, peer: XrNetPeer, frame: XrNetAlignmentDescriptorFrame) { - self.send_mailbox_update(move |mailbox| { - mailbox - .pending_peer_updates - .insert(peer.id, PendingPeerDescriptorUpdate::Set { peer, frame }); - }); - } - - fn remove_peer(&mut self, peer_id: XrNetPeerId) { - self.send_mailbox_update(move |mailbox| { - mailbox - .pending_peer_updates - .insert(peer_id, PendingPeerDescriptorUpdate::Remove); - }); - } - - fn take_latest_result(&mut self) -> Option { - self.latest_result - .lock() - .ok() - .and_then(|mut result| result.take()) - } - - fn send_mailbox_update(&mut self, update: F) - where - F: FnOnce(&mut XrPeopleAlignmentWorkerMailbox), - { - let (lock, wake) = &*self.mailbox; - if let Ok(mut mailbox) = lock.lock() { - update(&mut mailbox); - mailbox.version = mailbox.version.saturating_add(1); - wake.notify_one(); - } - } -} - -impl Drop for XrPeopleAlignmentWorker { - fn drop(&mut self) { - let (lock, wake) = &*self.mailbox; - if let Ok(mut mailbox) = lock.lock() { - mailbox.shutdown = true; - mailbox.version = mailbox.version.saturating_add(1); - wake.notify_one(); - } - if let Some(join_handle) = self.join_handle.take() { - let _ = join_handle.join(); - } - } -} - -#[derive(Default)] -struct AlignmentWorkerState { - peers: HashMap, - local_descriptor: Option, - local_descriptor_version: Option<(u64, u64)>, -} - -impl AlignmentWorkerState { - fn apply_local_descriptor_update(&mut self, update: PendingLocalDescriptorUpdate) -> bool { - match update { - PendingLocalDescriptorUpdate::Set { frame, version } => { - if descriptor_frames_equal(self.local_descriptor.as_ref(), Some(&frame)) { - return false; - } - self.local_descriptor = Some(frame); - self.local_descriptor_version = Some(version); - self.refresh_all_peer_alignments() - } - PendingLocalDescriptorUpdate::Clear => { - if self.local_descriptor.is_none() { - return false; - } - self.local_descriptor = None; - self.local_descriptor_version = None; - self.refresh_all_peer_alignments() - } - } - } - - fn apply_peer_update( - &mut self, - peer_id: XrNetPeerId, - update: PendingPeerDescriptorUpdate, - ) -> bool { - match update { - PendingPeerDescriptorUpdate::Set { peer, frame } => { - let peer_state = self - .peers - .entry(peer_id) - .or_insert_with(|| AlignmentWorkerPeerState::new(peer)); - peer_state.peer = peer; - if descriptor_frames_equal(peer_state.latest_descriptor.as_ref(), Some(&frame)) { - return false; - } - peer_state.latest_descriptor = Some(frame); - self.refresh_peer_alignment(peer_id) - } - PendingPeerDescriptorUpdate::Remove => self.peers.remove(&peer_id).is_some(), - } - } - - fn refresh_all_peer_alignments(&mut self) -> bool { - let local_descriptor = self.local_descriptor.clone(); - let local_descriptor_version = self.local_descriptor_version; - let mut changed = false; - for peer_state in self.peers.values_mut() { - changed |= refresh_alignment_worker_peer( - peer_state, - local_descriptor.as_ref(), - local_descriptor_version, - ); - } - changed - } - - fn refresh_peer_alignment(&mut self, peer_id: XrNetPeerId) -> bool { - let local_descriptor = self.local_descriptor.clone(); - let local_descriptor_version = self.local_descriptor_version; - let Some(peer_state) = self.peers.get_mut(&peer_id) else { - return false; - }; - refresh_alignment_worker_peer( - peer_state, - local_descriptor.as_ref(), - local_descriptor_version, - ) - } - - fn make_result(&self) -> XrPeopleAlignmentWorkerResult { - XrPeopleAlignmentWorkerResult { - peer_results: self - .peers - .iter() - .map(|(peer_id, peer_state)| (*peer_id, peer_state.to_result())) - .collect(), - alignment_debug_text: make_alignment_debug_text( - self.local_descriptor.is_some(), - &self.peers, - ), - } - } -} - -fn xr_people_alignment_worker_loop( - mailbox: Arc<(Mutex, Condvar)>, - latest_result: Arc>>, -) { - let mut seen_version = 0u64; - let mut state = AlignmentWorkerState::default(); - - loop { - let (local_update, peer_updates) = { - let (lock, wake) = &*mailbox; - let mut guard = match lock.lock() { - Ok(guard) => guard, - Err(_) => return, - }; - while !guard.shutdown && guard.version == seen_version { - guard = match wake.wait(guard) { - Ok(guard) => guard, - Err(_) => return, - }; - } - if guard.shutdown { - return; - } - seen_version = guard.version; - ( - guard.pending_local_descriptor.take(), - std::mem::take(&mut guard.pending_peer_updates), - ) - }; - - let mut dirty = false; - if let Some(local_update) = local_update { - dirty |= state.apply_local_descriptor_update(local_update); - } - for (peer_id, update) in peer_updates { - dirty |= state.apply_peer_update(peer_id, update); - } - if dirty { - if let Ok(mut result_slot) = latest_result.lock() { - *result_slot = Some(state.make_result()); - } - } - } -} - -fn descriptor_frames_equal( - left: Option<&XrNetAlignmentDescriptorFrame>, - right: Option<&XrNetAlignmentDescriptorFrame>, -) -> bool { - match (left, right) { - (Some(left), Some(right)) => left.descriptor == right.descriptor, - (None, None) => true, - _ => false, - } -} - -fn refresh_alignment_worker_peer( - peer_state: &mut AlignmentWorkerPeerState, - local_descriptor: Option<&XrNetAlignmentDescriptorFrame>, - local_descriptor_version: Option<(u64, u64)>, -) -> bool { - let (Some(local_descriptor), Some(local_descriptor_version), Some(remote_descriptor)) = ( - local_descriptor, - local_descriptor_version, - peer_state.latest_descriptor.as_ref(), - ) else { - return clear_alignment_worker_peer(peer_state); - }; - - if peer_state.last_solved_local_descriptor_version == Some(local_descriptor_version) - && peer_state.last_solved_remote_descriptor_seq == Some(remote_descriptor.seq) - { - return false; - } - - peer_state.last_solved_local_descriptor_version = Some(local_descriptor_version); - peer_state.last_solved_remote_descriptor_seq = Some(remote_descriptor.seq); - - let solve_started = Instant::now(); - let diagnostic = xr_depth_align_analyze_remote_to_local( - &local_descriptor.descriptor, - &remote_descriptor.descriptor, - ); - peer_state.last_solve_ms = solve_started.elapsed().as_secs_f64() * 1000.0; - peer_state.last_solve_diagnostic = Some(diagnostic); - - let next_solution = - choose_stable_alignment_solution(peer_state.last_accepted_solution, &diagnostic); - peer_state.last_accepted_solution = next_solution; - let next_transform = next_solution.map(|solution| solution.remote_to_local_transform()); - let changed = peer_state.remote_to_local != next_transform; - peer_state.remote_to_local = next_transform; - changed -} - -fn clear_alignment_worker_peer(peer_state: &mut AlignmentWorkerPeerState) -> bool { - let changed = - peer_state.remote_to_local.is_some() || peer_state.last_solve_diagnostic.is_some(); - peer_state.remote_to_local = None; - peer_state.last_accepted_solution = None; - peer_state.last_solve_diagnostic = None; - peer_state.last_solve_ms = 0.0; - peer_state.last_solved_local_descriptor_version = None; - peer_state.last_solved_remote_descriptor_seq = None; - changed -} - -fn choose_stable_alignment_solution( - previous: Option, - diagnostic: &XrDepthAlignSolveDiagnostic, -) -> Option { - let candidate = diagnostic.accepted_solution(); - let Some(previous) = previous else { - return candidate; - }; - let Some(candidate) = candidate else { - return Some(previous); - }; - if is_large_alignment_jump(previous, candidate) - && candidate.confidence < previous.confidence + 0.18 - && candidate.matched_samples <= previous.matched_samples + 1 - && candidate.residual_meters >= previous.residual_meters - 0.03 - { - return Some(previous); - } - Some(candidate) -} - -fn wrap_alignment_angle(mut angle: f32) -> f32 { - while angle <= -std::f32::consts::PI { - angle += std::f32::consts::TAU; - } - while angle > std::f32::consts::PI { - angle -= std::f32::consts::TAU; - } - angle -} - -fn is_large_alignment_jump(left: XrDepthAlignSolution, right: XrDepthAlignSolution) -> bool { - let yaw_delta = wrap_alignment_angle(left.yaw_radians - right.yaw_radians).abs(); - let translation_delta = (left.translation - right.translation).length(); - yaw_delta > 0.22 || translation_delta > 0.18 -} - -fn make_alignment_debug_text( - has_local_descriptor: bool, - peers: &HashMap, -) -> String { - let Some((peer_id, peer_state)) = peers.iter().min_by_key(|(peer_id, _)| peer_id.0) else { - return if has_local_descriptor { - "AlignDbg: waiting for peer descriptor".to_string() - } else { - "AlignDbg: waiting for local scene descriptor".to_string() - }; - }; - let peer_label = format!("{:08x}", peer_id.0); - if !has_local_descriptor { - return format!("AlignDbg {peer_label}: waiting for local scene descriptor"); - } - let Some(diagnostic) = peer_state.last_solve_diagnostic else { - return format!("AlignDbg {peer_label}: waiting for peer descriptor"); - }; - - let samples = format!( - "lw{} rw{} y{} p{}", - diagnostic.local_wall_samples, - diagnostic.remote_wall_samples, - diagnostic.yaw_candidate_count, - diagnostic.pose_candidate_count, - ); - match diagnostic.outcome() { - XrDepthAlignSolveOutcome::MissingSamples => { - format!("AlignDbg {peer_label}: need samples {samples}") - } - XrDepthAlignSolveOutcome::NoCandidate => { - format!( - "AlignDbg {peer_label}: no candidate t{:.1}ms {samples}", - peer_state.last_solve_ms, - ) - } - XrDepthAlignSolveOutcome::Rejected => { - let Some(best) = diagnostic.best_solution else { - return format!("AlignDbg {peer_label}: reject {samples}"); - }; - format!( - "AlignDbg {peer_label}: reject c{:.2} m{} r{:.2} t{:.1}ms {samples}", - best.confidence, - best.matched_samples, - best.residual_meters, - peer_state.last_solve_ms, - ) - } - XrDepthAlignSolveOutcome::Accepted => { - let Some(best) = diagnostic.best_solution else { - return format!("AlignDbg {peer_label}: aligned {samples}"); - }; - format!( - "AlignDbg {peer_label}: ok c{:.2} m{} r{:.2} t{:.1}ms yaw{:.0} tx{:.2} ty{:.2} tz{:.2}", - best.confidence, - best.matched_samples, - best.residual_meters, - peer_state.last_solve_ms, - best.yaw_radians.to_degrees(), - best.translation.x, - best.translation.y, - best.translation.z, - ) - } - } -} - -#[derive(Script, ScriptHook, Widget)] -pub struct XrPeopleDebug { - #[redraw] - #[live] - draw_cube: DrawCube, - #[rust] - enabled: bool, - #[rust] - last_status: String, - #[rust] - last_network_status: String, - #[rust] - last_alignment_debug_status: String, - #[rust] - last_plane_scan_status: String, - #[rust] - net_node: Option, - #[rust] - alignment_worker: Option, - #[rust] - peers: HashMap, - #[rust] - local_descriptor: Option, - #[rust] - local_plane_patches: Vec, - #[rust] - local_alignment_wall_features: Vec, - #[rust] - local_alignment_debug: XrDepthAlignDebug, - #[rust] - last_sent_descriptor_signature: Option<(u64, u64)>, - #[rust] - tx_state_count: u64, - #[rust] - tx_descriptor_count: u64, - #[rust] - rx_join_count: u64, - #[rust] - rx_leave_count: u64, - #[rust] - rx_state_count: u64, - #[rust] - rx_descriptor_count: u64, - #[rust] - last_event_text: String, - #[cast] - #[deref] - node: XrNode, -} - -impl XrPeopleDebug { - const HEADSET_SIZE: Vec3f = Vec3f { - x: 0.18, - y: 0.11, - z: 0.14, - }; - const HAND_SIZE: Vec3f = Vec3f { - x: 0.08, - y: 0.05, - z: 0.10, - }; - const PLANE_THICKNESS: f32 = 0.032; - const ALIGN_WALL_BEAM_HEIGHT: f32 = 0.10; - const ALIGN_WALL_BEAM_THICKNESS: f32 = 0.045; - const ALIGN_WALL_BEAM_NORMAL_OFFSET: f32 = 0.05; - const PLANE_MARKER_SIZE: Vec3f = Vec3f { - x: 0.055, - y: 0.055, - z: 0.055, - }; - - pub fn status_text(&self) -> &str { - if self.last_status.is_empty() { - "Peers: off" - } else { - &self.last_status - } - } - - pub fn enabled(&self) -> bool { - self.enabled - } - - pub fn network_status_text(&self) -> &str { - if self.last_network_status.is_empty() { - "Network: off" - } else { - &self.last_network_status - } - } - - pub fn alignment_debug_text(&self) -> &str { - if self.last_alignment_debug_status.is_empty() { - "AlignDbg: off" - } else { - &self.last_alignment_debug_status - } - } - - pub fn plane_scan_text(&self) -> &str { - if self.last_plane_scan_status.is_empty() { - "PlaneScan: off" - } else { - &self.last_plane_scan_status - } - } - - pub fn set_enabled(&mut self, cx: &mut Cx, enabled: bool) -> bool { - if self.enabled == enabled { - return self.enabled; - } - self.enabled = enabled; - cx.xr_depth_mesh().set_plane_scan_enabled(enabled); - cx.xr_depth_mesh().set_surface_analysis_enabled(enabled); - self.net_node = None; - self.alignment_worker = None; - self.peers.clear(); - self.local_descriptor = None; - self.local_plane_patches.clear(); - self.local_alignment_wall_features.clear(); - self.local_alignment_debug = XrDepthAlignDebug::default(); - self.last_sent_descriptor_signature = None; - self.tx_state_count = 0; - self.tx_descriptor_count = 0; - self.rx_join_count = 0; - self.rx_leave_count = 0; - self.rx_state_count = 0; - self.rx_descriptor_count = 0; - self.last_event_text.clear(); - self.last_plane_scan_status.clear(); - - if enabled { - self.alignment_worker = Some(XrPeopleAlignmentWorker::new()); - self.ensure_net_node(); - if self.net_node.is_some() { - self.last_status = "Peers: scanning LAN for clients".to_string(); - self.last_network_status = - "Network: bridge ready | waiting for local XR frames".to_string(); - } else { - self.last_status = "Peers: network unavailable".to_string(); - self.last_network_status = "Network: bind failed".to_string(); - } - self.last_alignment_debug_status = - "AlignDbg: waiting for local scene descriptor".to_string(); - self.last_plane_scan_status = "PlaneScan: waiting for TSDF scan".to_string(); - } else { - self.last_status = "Peers: off".to_string(); - self.last_network_status = "Network: off".to_string(); - self.last_alignment_debug_status = "AlignDbg: off".to_string(); - self.last_plane_scan_status = "PlaneScan: off".to_string(); - } - self.redraw(cx); - self.enabled - } - - fn ensure_net_node(&mut self) { - if self.net_node.is_some() { - return; - } - match XrNetNode::new() { - Ok(node) => { - self.net_node = Some(node); - self.last_event_text = "node started".to_string(); - } - Err(err) => { - self.last_status = format!("Peers: network bind failed ({err})"); - self.last_network_status = format!("Network: bind failed ({err})"); - } - } - } - - fn refresh_from_local_state(&mut self, cx: &mut Cx, state: &XrState) { - if !self.enabled { - return; - } - self.ensure_net_node(); - let Some(net_node) = self.net_node.as_mut() else { - return; - }; - - net_node.send_state(state.clone()); - self.tx_state_count = self.tx_state_count.saturating_add(1); - - let next_mesh = cx.xr_depth_mesh().latest_mesh(); - self.local_alignment_debug = next_mesh - .as_ref() - .map(|mesh| mesh.alignment_debug) - .unwrap_or_default(); - self.local_plane_patches = next_mesh - .as_ref() - .map(|mesh| mesh.plane_patches.clone()) - .unwrap_or_default(); - self.local_alignment_wall_features = next_mesh - .as_ref() - .and_then(|mesh| { - mesh.alignment_descriptor - .as_ref() - .map(|descriptor| descriptor.wall_features.clone()) - }) - .unwrap_or_default(); - let next_signature = next_mesh - .as_ref() - .map(|mesh| (mesh.mesh_generation, mesh.update_sequence)); - let next_descriptor = next_mesh.as_ref().and_then(|mesh| { - XrNetAlignmentDescriptorFrame::from_depth_mesh(mesh.as_ref(), state.time) - }); - - if !descriptor_frames_equal(self.local_descriptor.as_ref(), next_descriptor.as_ref()) { - self.local_descriptor = next_descriptor.clone(); - if let Some(worker) = self.alignment_worker.as_mut() { - if let (Some(frame), Some(version)) = (next_descriptor.clone(), next_signature) { - worker.set_local_descriptor(frame, version); - } else { - worker.clear_local_descriptor(); - } - } - } - - if let (Some(signature), Some(frame)) = (next_signature, next_descriptor) { - if self.last_sent_descriptor_signature != Some(signature) { - net_node.send_alignment_descriptor(frame); - self.last_sent_descriptor_signature = Some(signature); - self.tx_descriptor_count = self.tx_descriptor_count.saturating_add(1); - } - } else { - self.last_sent_descriptor_signature = None; - } - } - - fn poll_network(&mut self) { - if !self.enabled { - return; - } - - let mut disconnected = false; - loop { - let result = match self.net_node.as_mut() { - Some(net_node) => net_node.incoming_receiver.try_recv(), - None => break, - }; - match result { - Ok(message) => self.handle_network_message(message), - Err(TryRecvError::Empty) => break, - Err(TryRecvError::Disconnected) => { - disconnected = true; - break; - } - } - } - - if disconnected { - self.net_node = None; - self.last_status = "Peers: network worker disconnected, retrying".to_string(); - self.last_network_status = "Network: worker disconnected".to_string(); - } - } - - fn handle_network_message(&mut self, message: XrNetIncoming) { - match message { - XrNetIncoming::Join { peer } => { - self.rx_join_count = self.rx_join_count.saturating_add(1); - self.last_event_text = format!("join {}", Self::peer_label(peer.id)); - self.peers - .entry(peer.id) - .or_insert_with(|| RemotePeerState::new(peer)); - } - XrNetIncoming::Leave { peer, .. } => { - self.rx_leave_count = self.rx_leave_count.saturating_add(1); - self.last_event_text = format!("leave {}", Self::peer_label(peer.id)); - self.peers.remove(&peer.id); - if let Some(worker) = self.alignment_worker.as_mut() { - worker.remove_peer(peer.id); - } - } - XrNetIncoming::State { peer, frame } => { - self.rx_state_count = self.rx_state_count.saturating_add(1); - self.last_event_text = - format!("state {} seq {}", Self::peer_label(peer.id), frame.seq); - let peer_state = self - .peers - .entry(peer.id) - .or_insert_with(|| RemotePeerState::new(peer)); - peer_state.peer = peer; - peer_state.latest_state = Some(frame); - } - XrNetIncoming::AlignmentDescriptor { peer, frame } => { - self.rx_descriptor_count = self.rx_descriptor_count.saturating_add(1); - self.last_event_text = - format!("desc {} seq {}", Self::peer_label(peer.id), frame.seq); - let peer_state = self - .peers - .entry(peer.id) - .or_insert_with(|| RemotePeerState::new(peer)); - peer_state.peer = peer; - peer_state.has_descriptor = true; - if let Some(worker) = self.alignment_worker.as_mut() { - worker.set_peer_descriptor(peer, frame); - } - } - XrNetIncoming::Alignment { .. } => {} - } - } - - fn apply_alignment_results(&mut self, cx: &mut Cx) { - let Some(worker) = self.alignment_worker.as_mut() else { - return; - }; - let Some(result) = worker.take_latest_result() else { - return; - }; - - for peer_state in self.peers.values_mut() { - peer_state.remote_to_local = None; - peer_state.transform_source = RemoteTransformSource::Raw; - } - for (peer_id, peer_result) in result.peer_results { - if let Some(peer_state) = self.peers.get_mut(&peer_id) { - peer_state.remote_to_local = peer_result.remote_to_local; - peer_state.transform_source = peer_result.transform_source; - peer_state.last_solve_diagnostic = peer_result.last_solve_diagnostic; - peer_state.has_descriptor = - peer_state.has_descriptor || peer_result.last_solve_diagnostic.is_some(); - } - } - self.last_alignment_debug_status = result.alignment_debug_text; - self.redraw(cx); - } - - fn refresh_status(&mut self) { - if !self.enabled { - self.last_status = "Peers: off".to_string(); - self.last_network_status = "Network: off".to_string(); - self.last_alignment_debug_status = "AlignDbg: off".to_string(); - self.last_plane_scan_status = "PlaneScan: off".to_string(); - return; - } - if self.net_node.is_none() { - if self.last_status.is_empty() { - self.last_status = "Peers: network unavailable".to_string(); - } - if self.last_network_status.is_empty() { - self.last_network_status = "Network: unavailable".to_string(); - } - return; - } - - let peer_count = self.peers.len(); - let visible_count = self - .peers - .values() - .filter(|peer| peer.latest_state.is_some()) - .count(); - let descriptor_count = self - .peers - .values() - .filter(|peer| peer.has_descriptor) - .count(); - let aligned_count = self - .peers - .values() - .filter(|peer| peer.latest_state.is_some() && peer.remote_to_local.is_some()) - .count(); - - self.last_status = if peer_count == 0 { - "Peers: scanning LAN for clients".to_string() - } else if self.local_descriptor.is_some() { - format!("Peers: {peer_count} seen | {visible_count} state | {aligned_count} descriptor-solved") - } else { - format!("Peers: {peer_count} seen | {visible_count} state | waiting for local scene descriptor") - }; - - let last_event = if self.last_event_text.is_empty() { - "none" - } else { - &self.last_event_text - }; - self.last_network_status = format!( - "Network: tx s{} d{} | rx j{} l{} s{} d{} | peers {} vis {} desc {} align {} | local desc {} | last {}", - self.tx_state_count, - self.tx_descriptor_count, - self.rx_join_count, - self.rx_leave_count, - self.rx_state_count, - self.rx_descriptor_count, - peer_count, - visible_count, - descriptor_count, - aligned_count, - if self.local_descriptor.is_some() { "yes" } else { "no" }, - last_event, - ); - self.last_plane_scan_status = self.local_plane_scan_text(); - if self.local_descriptor.is_none() { - self.last_alignment_debug_status = self.local_descriptor_debug_text(); - } - } - - fn peer_base_color(peer_id: XrNetPeerId) -> Vec4f { - match (peer_id.0 % 5) as usize { - 0 => vec4f(0.92, 0.38, 0.31, 1.0), - 1 => vec4f(0.24, 0.74, 0.58, 1.0), - 2 => vec4f(0.35, 0.58, 0.98, 1.0), - 3 => vec4f(0.93, 0.70, 0.28, 1.0), - _ => vec4f(0.80, 0.48, 0.94, 1.0), - } - } - - fn peer_label(peer_id: XrNetPeerId) -> String { - format!("{:08x}", peer_id.0) - } - - fn peer_transform(peer: &RemotePeerState) -> Mat4f { - peer.remote_to_local.unwrap_or_default() - } - - fn peer_alpha(peer: &RemotePeerState) -> f32 { - match peer.transform_source { - RemoteTransformSource::Descriptor => 1.0, - RemoteTransformSource::Raw => 0.42, - } - } - - fn draw_cube_at( - &mut self, - cx: &mut Cx3d, - world: &Mat4f, - pose_transform: &Mat4f, - size: Vec3f, - color: Vec4f, - ) { - self.draw_cube.transform = Mat4f::mul(world, pose_transform); - self.draw_cube.cube_pos = vec3(0.0, 0.0, 0.0); - self.draw_cube.cube_size = size; - self.draw_cube.color = color; - self.draw_cube.depth_clip = 1.0; - self.draw_cube.draw(cx); - } - - fn local_descriptor_debug_text(&self) -> String { - let debug = self.local_alignment_debug; - if debug.near_surface_voxel_count == 0 { - "AlignDbg: waiting for local scene descriptor".to_string() - } else { - format!( - "AlignDbg: local near {} wall {}->{}", - debug.near_surface_voxel_count, debug.wall_candidate_count, debug.wall_sample_count, - ) - } - } - - fn local_plane_scan_text(&self) -> String { - let mut wall_count = 0usize; - let mut other_count = 0usize; - for patch in &self.local_plane_patches { - match patch.kind { - XrDepthPlaneKind::Wall => wall_count += 1, - XrDepthPlaneKind::Floor - | XrDepthPlaneKind::Ceiling - | XrDepthPlaneKind::Table - | XrDepthPlaneKind::Unknown => other_count += 1, - } - } - let debug = self.local_alignment_debug; - if debug.near_surface_voxel_count == 0 && self.local_plane_patches.is_empty() { - "PlaneScan: waiting for TSDF scan".to_string() - } else { - format!( - "PlaneScan: patches {} wall {} room {} other {} | near {} wall vox {}", - self.local_plane_patches.len(), - wall_count, - self.local_alignment_wall_features.len(), - other_count, - debug.near_surface_voxel_count, - debug.wall_candidate_count, - ) - } - } - - fn draw_local_plane_patches(&mut self, cx: &mut Cx3d, world: &Mat4f) { - for patch in self.local_plane_patches.clone() { - let color = match patch.kind { - XrDepthPlaneKind::Floor => vec4f(0.18, 0.96, 0.34, 0.42), - XrDepthPlaneKind::Wall => vec4f(0.18, 0.66, 1.0, 0.36), - XrDepthPlaneKind::Ceiling => vec4f(1.0, 0.62, 0.28, 0.32), - XrDepthPlaneKind::Table => vec4f(0.92, 0.84, 0.18, 0.34), - XrDepthPlaneKind::Unknown => vec4f(0.72, 0.72, 0.72, 0.24), - }; - let center = patch.center + patch.normal.scale(0.024); - let transform = Mat4f { - v: [ - patch.tangent.x, - patch.tangent.y, - patch.tangent.z, - 0.0, - patch.normal.x, - patch.normal.y, - patch.normal.z, - 0.0, - patch.bitangent.x, - patch.bitangent.y, - patch.bitangent.z, - 0.0, - center.x, - center.y, - center.z, - 1.0, - ], - }; - self.draw_cube_at( - cx, - world, - &transform, - vec3( - patch.half_extent_tangent * 2.0, - Self::PLANE_THICKNESS, - patch.half_extent_bitangent * 2.0, - ), - color, - ); - let marker_transform = - Pose::new(Quat::default(), center + patch.normal.scale(0.03)).to_mat4(); - self.draw_cube_at( - cx, - world, - &marker_transform, - Self::PLANE_MARKER_SIZE, - vec4f(color.x, color.y, color.z, 0.92), - ); - } - } - - fn draw_local_alignment_walls(&mut self, cx: &mut Cx3d, world: &Mat4f) { - for wall in self.local_alignment_wall_features.clone() { - let center = vec3( - wall.center.x + wall.normal.x * Self::ALIGN_WALL_BEAM_NORMAL_OFFSET, - wall.max_y - Self::ALIGN_WALL_BEAM_HEIGHT * 0.5, - wall.center.z + wall.normal.z * Self::ALIGN_WALL_BEAM_NORMAL_OFFSET, - ); - let transform = Mat4f { - v: [ - wall.along_axis.x, - wall.along_axis.y, - wall.along_axis.z, - 0.0, - 0.0, - 1.0, - 0.0, - 0.0, - wall.normal.x, - wall.normal.y, - wall.normal.z, - 0.0, - center.x, - center.y, - center.z, - 1.0, - ], - }; - self.draw_cube_at( - cx, - world, - &transform, - vec3( - wall.half_extent_along * 2.0, - Self::ALIGN_WALL_BEAM_HEIGHT, - Self::ALIGN_WALL_BEAM_THICKNESS, - ), - vec4f(1.0, 0.18, 0.14, 0.92), - ); - } - } - - fn draw_peer_hand( - &mut self, - cx: &mut Cx3d, - world: &Mat4f, - root_transform: &Mat4f, - hand: &XrHand, - controller: &XrController, - color: Vec4f, - ) { - let pose = if hand.in_view() { - Some(hand.joints[XrHand::CENTER]) - } else if controller.active() { - Some(controller.grip_pose) - } else { - None - }; - let Some(pose) = pose else { - return; - }; - let transform = Mat4f::mul(root_transform, &pose.to_mat4()); - self.draw_cube_at(cx, world, &transform, Self::HAND_SIZE, color); - } - - fn draw_remote_peers(&mut self, cx: &mut Cx3d, world: &Mat4f) { - let peer_ids = self.peers.keys().copied().collect::>(); - for peer_id in peer_ids { - let Some(peer) = self.peers.get(&peer_id).cloned() else { - continue; - }; - let Some(state_frame) = peer.latest_state.as_ref() else { - continue; - }; - - let alpha = Self::peer_alpha(&peer); - let base = Self::peer_base_color(peer.peer.id); - let root_transform = Self::peer_transform(&peer); - let head_color = vec4f(base.x, base.y, base.z, alpha); - let left_color = vec4f( - (base.x * 0.72).min(1.0), - (base.y * 1.05).min(1.0), - 1.0, - alpha, - ); - let right_color = vec4f( - 1.0, - (base.y * 0.82).min(1.0), - (base.z * 0.72).min(1.0), - alpha, - ); - - let head_transform = - Mat4f::mul(&root_transform, &state_frame.state.head_pose.to_mat4()); - self.draw_cube_at(cx, world, &head_transform, Self::HEADSET_SIZE, head_color); - self.draw_peer_hand( - cx, - world, - &root_transform, - &state_frame.state.left_hand, - &state_frame.state.left_controller, - left_color, - ); - self.draw_peer_hand( - cx, - world, - &root_transform, - &state_frame.state.right_hand, - &state_frame.state.right_controller, - right_color, - ); - } - } -} - -impl Widget for XrPeopleDebug { - fn script_call( - &mut self, - vm: &mut ScriptVm, - method: LiveId, - args: ScriptValue, - ) -> ScriptAsyncResult { - if method == live_id!(set_enabled) { - let mut enabled = self.enabled; - if let Some(args_obj) = args.as_object() { - let trap = vm.bx.threads.cur().trap.pass(); - enabled = vm - .bx - .heap - .cast_to_bool(vm.bx.heap.vec_value(args_obj, 0, trap)); - } - let enabled = vm.with_cx_mut(|cx| self.set_enabled(cx, enabled)); - return ScriptAsyncResult::Return(ScriptValue::from_bool(enabled)); - } - if method == live_id!(toggle_enabled) || method == live_id!(toggle_test) { - let enabled = vm.with_cx_mut(|cx| self.set_enabled(cx, !self.enabled)); - return ScriptAsyncResult::Return(ScriptValue::from_bool(enabled)); - } - if method == live_id!(enabled) { - return ScriptAsyncResult::Return(ScriptValue::from_bool(self.enabled)); - } - self.node.script_call(vm, method, args) - } - - fn script_result(&mut self, vm: &mut ScriptVm, id: ScriptAsyncId, result: ScriptValue) { - self.node.script_result(vm, id, result); - } - - fn handle_event(&mut self, cx: &mut Cx, event: &Event, scope: &mut Scope) { - if self.enabled { - if let Event::XrUpdate(update) = event { - self.refresh_from_local_state(cx, update.state.as_ref()); - } - self.poll_network(); - self.apply_alignment_results(cx); - self.refresh_status(); - } - self.node.handle_event(cx, event, scope); - } - - fn draw_3d(&mut self, cx: &mut Cx3d, scope: &mut Scope) -> DrawStep { - if !self.enabled { - return self.node.draw_3d(cx, scope); - } - if cx.scene_state_3d().is_none() { - return DrawStep::done(); - } - let world = if cx.cx.in_xr_mode() { - // Depth mesh and plane scan patches live in raw OpenXR local-space. - // Keep debug planes and remote peers in that same space so - // root/content recentering does not skew or shift them. - Mat4f::identity() - } else { - self.node.local_transform() - }; - self.draw_cube.begin_many_instances(cx); - self.draw_local_plane_patches(cx, &world); - self.draw_local_alignment_walls(cx, &world); - self.draw_remote_peers(cx, &world); - self.draw_cube.end_many_instances(cx); - self.node.draw_3d(cx, scope) - } - - fn draw_walk(&mut self, _cx: &mut Cx2d, _scope: &mut Scope, _walk: Walk) -> DrawStep { - DrawStep::done() - } -} - -#[cfg(test)] -mod tests { - use super::*; - - fn make_solution( - yaw_radians: f32, - translation: Vec3f, - confidence: f32, - residual_meters: f32, - matched_samples: usize, - ) -> XrDepthAlignSolution { - XrDepthAlignSolution { - yaw_radians, - translation, - confidence, - residual_meters, - matched_samples, - } - } - - fn make_diagnostic(solution: XrDepthAlignSolution) -> XrDepthAlignSolveDiagnostic { - XrDepthAlignSolveDiagnostic { - used_wall_features: true, - local_wall_features: 4, - remote_wall_features: 4, - best_solution: Some(solution), - ..XrDepthAlignSolveDiagnostic::default() - } - } - - #[test] - fn stable_alignment_prefers_existing_solution_over_flip() { - let previous = make_solution(0.42, vec3(0.28, 0.0, -0.64), 0.41, 0.03, 4); - let flipped = make_solution(-2.71, vec3(-0.34, 0.0, 0.71), 0.44, 0.03, 4); - - let chosen = - choose_stable_alignment_solution(Some(previous), &make_diagnostic(flipped)).unwrap(); - - assert_eq!(chosen, previous); - } - - #[test] - fn stable_alignment_accepts_small_refinement() { - let previous = make_solution(0.42, vec3(0.28, 0.0, -0.64), 0.28, 0.06, 3); - let refined = make_solution(0.46, vec3(0.24, 0.0, -0.60), 0.35, 0.03, 4); - - let chosen = - choose_stable_alignment_solution(Some(previous), &make_diagnostic(refined)).unwrap(); - - assert_eq!(chosen, refined); - } -} diff --git a/xr/src/scene/xr_permissions_flow.rs b/xr/src/scene/xr_permissions_flow.rs index 70694df7e4..31c8284ef7 100644 --- a/xr/src/scene/xr_permissions_flow.rs +++ b/xr/src/scene/xr_permissions_flow.rs @@ -1,4 +1,4 @@ -use crate::*; +use crate::prelude::*; use makepad_platform::permission::{Permission, PermissionStatus}; script_mod! { diff --git a/xr/src/scene/xr_physics.rs b/xr/src/scene/xr_physics.rs index ddaedae06c..c28bae93c8 100644 --- a/xr/src/scene/xr_physics.rs +++ b/xr/src/scene/xr_physics.rs @@ -1,12 +1,14 @@ #![allow(dead_code)] -use super::xr_depth::{ - depth_query_plane_supports_body, DepthQuerySurfaceCollider, DepthQuerySurfaceTarget, -}; +use super::xr_depth::{DepthQuerySurfaceCollider, DepthQuerySurfaceTarget}; use super::*; +use crate::algorithms::tsdf_query::{ + depth_query_plane_supports_body, DepthQueryColliderGeometry, DepthQueryColliderRole, +}; use rapier3d::dynamics::CoefficientCombineRule; use rapier3d::pipeline::{ActiveHooks, PairFilterContext, PhysicsHooks}; -use rapier3d::prelude::SolverFlags; +use rapier3d::prelude::{RigidBodyType, SolverFlags}; +use std::collections::HashMap; #[derive(Clone, Copy, Debug)] pub(super) enum HandCollider { @@ -33,17 +35,58 @@ pub(super) struct HandColliderBody { pub(super) collider: ColliderHandle, } +#[derive(Clone, Copy, Debug)] +struct HandGrabState { + shared_hand: XrSharedHand, + pose: Pose, + previous_pose: Pose, + linvel: Vec3f, + tracked: bool, + gripping: bool, + held_body: Option, + grab_offset: Pose, +} + +impl HandGrabState { + fn new(shared_hand: XrSharedHand) -> Self { + Self { + shared_hand, + pose: Pose::default(), + previous_pose: Pose::default(), + linvel: vec3f(0.0, 0.0, 0.0), + tracked: false, + gripping: false, + held_body: None, + grab_offset: Pose::default(), + } + } +} + +#[derive(Clone, Copy, Debug)] +struct ShadowBodyMotion { + pose: Pose, + linvel: Vec3f, + angvel: Vec3f, + remaining_prediction_seconds: f32, +} + struct DepthQueryBodySurfaceSet { + owner_widget_uid: WidgetUid, body: RigidBodyHandle, query_radius: f32, surfaces: [DepthQuerySurfaceCollider; XR_DEPTH_QUERY_SURFACES_PER_BODY], } +#[derive(Clone, Copy, Default)] +struct SettleContactState { + has_active_contact: bool, + has_depth_query_contact: bool, + has_support_contact: bool, +} + #[derive(Clone, Copy, Default)] pub(crate) struct DepthQueryPhysicsStats { - pub(crate) active_surface_count: usize, - pub(crate) vertex_count: usize, - pub(crate) triangle_count: usize, + pub(crate) surface_count: usize, } struct RapierDepthQueryHooks; @@ -53,6 +96,10 @@ const DEPTH_QUERY_SURFACE_USER_DATA_TAG: u128 = 1u128 << 126; const DEPTH_QUERY_USER_DATA_OWNER_MASK: u128 = u64::MAX as u128; static RAPIER_DEPTH_QUERY_HOOKS: RapierDepthQueryHooks = RapierDepthQueryHooks; const XR_ENABLE_SYNTHETIC_GROUND_PLANE: bool = false; +const DEPTH_QUERY_SURFACE_IMPACT_ROLE_TAG: u128 = 1u128 << 125; +// Keep shadow extrapolation aligned with XrPeerSync so remote bodies do not drift forever +// when UDP state stalls. +const XR_SHADOW_BODY_MAX_EXTRAPOLATION_SECONDS: f32 = 0.10; pub(crate) struct RapierScene { gravity: RapierVector, @@ -67,12 +114,16 @@ pub(crate) struct RapierScene { multibody_joints: MultibodyJointSet, ccd_solver: CCDSolver, pub(crate) cubes: Vec, - projectile_cube_indices: Vec, - projectile_cube_cursor: usize, + spawn_pool_cube_indices: Vec, + spawn_pool_cube_cursor: usize, depth_query_surface_sets: Vec, depth_query_stats: DepthQueryPhysicsStats, pub(super) left_hand: Vec, pub(super) right_hand: Vec, + left_hand_grab: HandGrabState, + right_hand_grab: HandGrabState, + shadow_body_motion: HashMap, + settle_frame_counts: HashMap, } fn rapier_vec3(v: Vec3f) -> RapierVector { @@ -90,12 +141,60 @@ fn rapier_pose(pose: Pose) -> RapierPose { ) } +fn makepad_vec3(v: RapierVector) -> Vec3f { + vec3f(v.x, v.y, v.z) +} + +fn predict_pose(pose: Pose, linvel: Vec3f, angvel: Vec3f, dt: f32) -> Pose { + let position = pose.position + linvel * dt; + let angular_speed = angvel.length(); + let orientation = if angular_speed > 1.0e-4 { + let axis = angvel * (1.0 / angular_speed); + Quat::multiply( + &Quat::from_axis_angle(axis, angular_speed * dt), + &pose.orientation, + ) + } else { + pose.orientation + }; + Pose::new(orientation, position) +} + +fn grab_offset_from_body_anchor( + hand_pose: Pose, + body_pose: Pose, + body_anchor_local: Vec3f, +) -> Pose { + let mut grab_offset = Pose::multiply(&hand_pose.invert(), &body_pose); + grab_offset.position = grab_offset + .orientation + .rotate_vec3(&(body_anchor_local * -1.0)); + grab_offset +} + +fn hand_grab_pose(hand: &XrHand) -> Option { + let palm_pose = hand.tracking_pose()?; + let thumb_tip = hand.tip_pos_checked(XrHand::THUMB_TIP); + let index_tip = hand.tip_pos_checked(XrHand::INDEX_TIP); + let position = match (thumb_tip, index_tip) { + (Some(thumb_tip), Some(index_tip)) => (thumb_tip + index_tip) * 0.5, + (_, Some(index_tip)) => index_tip, + _ => return None, + }; + Some(Pose::new(palm_pose.orientation, position)) +} + fn depth_query_body_user_data(widget_uid: WidgetUid) -> u128 { DEPTH_QUERY_BODY_USER_DATA_TAG | widget_uid.0 as u128 } -fn depth_query_surface_user_data(widget_uid: WidgetUid) -> u128 { - DEPTH_QUERY_SURFACE_USER_DATA_TAG | widget_uid.0 as u128 +fn depth_query_surface_user_data(widget_uid: WidgetUid, role: DepthQueryColliderRole) -> u128 { + DEPTH_QUERY_SURFACE_USER_DATA_TAG + | widget_uid.0 as u128 + | match role { + DepthQueryColliderRole::Support => 0, + DepthQueryColliderRole::Impact => DEPTH_QUERY_SURFACE_IMPACT_ROLE_TAG, + } } fn decode_depth_query_body_owner(user_data: u128) -> Option { @@ -108,6 +207,65 @@ fn decode_depth_query_surface_owner(user_data: u128) -> Option { .then_some((user_data & DEPTH_QUERY_USER_DATA_OWNER_MASK) as u64) } +fn decode_depth_query_surface_role(user_data: u128) -> Option { + ((user_data & DEPTH_QUERY_SURFACE_USER_DATA_TAG) != 0).then_some( + if (user_data & DEPTH_QUERY_SURFACE_IMPACT_ROLE_TAG) != 0 { + DepthQueryColliderRole::Impact + } else { + DepthQueryColliderRole::Support + }, + ) +} + +fn quat_from_to(from: Vec3f, to: Vec3f) -> Quat { + let from_len = from.length(); + let to_len = to.length(); + if from_len <= 1.0e-6 || to_len <= 1.0e-6 { + return Quat::default(); + } + let from = from * (1.0 / from_len); + let to = to * (1.0 / to_len); + let dot = from.dot(to).clamp(-1.0, 1.0); + if dot >= 0.9999 { + return Quat::default(); + } + if dot <= -0.9999 { + let fallback_axis = if from.x.abs() < 0.8 { + vec3f(1.0, 0.0, 0.0) + } else { + vec3f(0.0, 1.0, 0.0) + }; + let axis = Vec3f::cross(from, fallback_axis).normalize(); + return Quat::from_axis_angle(axis, std::f32::consts::PI); + } + let axis = Vec3f::cross(from, to).normalize(); + Quat::from_axis_angle(axis, dot.acos()) +} + +fn depth_query_surface_target_should_enable( + target: DepthQuerySurfaceTarget, + body_position: Vec3f, + body_velocity: Vec3f, + query_radius: f32, + lateral_margin: f32, +) -> bool { + let DepthQueryColliderGeometry::HalfSpace(plane) = target.collider.geometry; + match target.collider.role { + DepthQueryColliderRole::Support => { + depth_query_plane_supports_body(plane, body_position, query_radius, lateral_margin) + } + DepthQueryColliderRole::Impact => { + let speed = body_velocity.length(); + let approach_speed = -body_velocity.dot(plane.normal); + // Impact planes are predictive: the TSDF query may place the quad ahead of the body + // along its current path, so requiring current-footprint overlap here makes late wall + // and ceiling hits tunnel through. + speed >= XR_DEPTH_QUERY_IMPACT_ENABLE_SPEED_MIN + && approach_speed >= XR_DEPTH_QUERY_IMPACT_ENABLE_APPROACH_SPEED_MIN + } + } +} + impl PhysicsHooks for RapierDepthQueryHooks { fn filter_contact_pair(&self, context: &PairFilterContext) -> Option { let collider1 = context.colliders.get(context.collider1)?; @@ -349,12 +507,16 @@ impl RapierScene { multibody_joints: MultibodyJointSet::new(), ccd_solver: CCDSolver::new(), cubes: Vec::new(), - projectile_cube_indices: Vec::new(), - projectile_cube_cursor: 0, + spawn_pool_cube_indices: Vec::new(), + spawn_pool_cube_cursor: 0, depth_query_surface_sets: Vec::new(), depth_query_stats: DepthQueryPhysicsStats::default(), left_hand: Vec::new(), right_hand: Vec::new(), + left_hand_grab: HandGrabState::new(XrSharedHand::LeftHand), + right_hand_grab: HandGrabState::new(XrSharedHand::RightHand), + shadow_body_motion: HashMap::new(), + settle_frame_counts: HashMap::new(), }; if XR_ENABLE_SYNTHETIC_GROUND_PLANE { @@ -375,6 +537,34 @@ impl RapierScene { scene } + pub(super) fn sync_tracked_hands(&mut self, left_hand: &XrHand, right_hand: &XrHand) { + self.left_hand_grab = self.updated_hand_grab_state(self.left_hand_grab, left_hand); + self.right_hand_grab = self.updated_hand_grab_state(self.right_hand_grab, right_hand); + } + + fn updated_hand_grab_state(&self, mut state: HandGrabState, hand: &XrHand) -> HandGrabState { + let Some(pose) = hand_grab_pose(hand) else { + state.previous_pose = state.pose; + state.linvel = vec3f(0.0, 0.0, 0.0); + state.tracked = false; + state.gripping = false; + return state; + }; + let was_tracked = state.tracked; + let previous_pose = if was_tracked { state.pose } else { pose }; + let dt = self.integration_parameters.dt.max(0.0001); + state.previous_pose = previous_pose; + state.pose = pose; + state.linvel = if was_tracked { + (pose.position - previous_pose.position) * (1.0 / dt) + } else { + vec3f(0.0, 0.0, 0.0) + }; + state.tracked = true; + state.gripping = hand.grab_intent(); + state + } + fn spawn_hand_colliders(&mut self, count: usize) -> Vec { let mut result = Vec::with_capacity(count); for _ in 0..count { @@ -403,7 +593,10 @@ impl RapierScene { let body = self.bodies.insert(RigidBodyBuilder::fixed().build()); let collider = self.colliders.insert_with_parent( ColliderBuilder::new(SharedShape::halfspace(RapierVector::new(0.0, 1.0, 0.0))) - .user_data(depth_query_surface_user_data(owner_widget_uid)) + .user_data(depth_query_surface_user_data( + owner_widget_uid, + DepthQueryColliderRole::Support, + )) .active_hooks(ActiveHooks::FILTER_CONTACT_PAIRS) .friction(XR_DEPTH_QUERY_FRICTION) .restitution(0.0) @@ -430,6 +623,7 @@ impl RapierScene { let index = self.depth_query_surface_sets.len(); self.depth_query_surface_sets .push(DepthQueryBodySurfaceSet { + owner_widget_uid, body, query_radius, surfaces, @@ -474,6 +668,7 @@ impl RapierScene { body.set_position(target_pose, false); } body.set_next_kinematic_position(target_pose); + body.wake_up(true); } } else if let Some(collider) = collider_set.get_mut(slot.collider) { collider.set_enabled(false); @@ -482,6 +677,8 @@ impl RapierScene { } pub(crate) fn step(&mut self) { + self.apply_held_body_targets(); + self.apply_shadow_body_targets(); self.pipeline.step( self.gravity, &self.integration_parameters, @@ -496,47 +693,500 @@ impl RapierScene { &RAPIER_DEPTH_QUERY_HOOKS, &(), ); + self.acquire_hand_grabs(); self.settle_resting_bodies(); } + fn apply_shadow_body_targets(&mut self) { + let dt = self + .integration_parameters + .dt + .clamp(0.0, XR_SHADOW_BODY_MAX_EXTRAPOLATION_SECONDS); + let shadow_bodies = self + .shadow_body_motion + .iter() + .map(|(&body_handle, &motion)| (body_handle, motion)) + .collect::>(); + let mut stale = Vec::new(); + for (body_handle, mut motion) in shadow_bodies { + if self.held_by_for_body(body_handle).is_some() { + continue; + } + let Some(body) = self.bodies.get_mut(body_handle) else { + stale.push(body_handle); + continue; + }; + if !body.is_enabled() { + stale.push(body_handle); + continue; + } + if body.body_type() != RigidBodyType::KinematicPositionBased { + stale.push(body_handle); + continue; + } + let advance_dt = dt.min(motion.remaining_prediction_seconds.max(0.0)); + if advance_dt > 0.0 { + motion.pose = predict_pose(motion.pose, motion.linvel, motion.angvel, advance_dt); + motion.remaining_prediction_seconds = + (motion.remaining_prediction_seconds - advance_dt).max(0.0); + } + body.set_next_kinematic_position(rapier_pose(motion.pose)); + body.wake_up(true); + self.shadow_body_motion.insert(body_handle, motion); + } + for body_handle in stale { + self.shadow_body_motion.remove(&body_handle); + } + } + + fn apply_held_body_targets(&mut self) { + if let (Some(left_body), Some(right_body)) = ( + self.left_hand_grab.held_body, + self.right_hand_grab.held_body, + ) { + if left_body == right_body { + match ( + self.left_hand_grab.tracked && self.left_hand_grab.gripping, + self.right_hand_grab.tracked && self.right_hand_grab.gripping, + ) { + (true, true) => { + self.apply_dual_held_body_target(self.left_hand_grab, self.right_hand_grab); + return; + } + (true, false) => { + self.right_hand_grab = Self::drop_hand_hold(self.right_hand_grab); + self.left_hand_grab = self.apply_held_body_target(self.left_hand_grab); + return; + } + (false, true) => { + self.left_hand_grab = Self::drop_hand_hold(self.left_hand_grab); + self.right_hand_grab = self.apply_held_body_target(self.right_hand_grab); + return; + } + (false, false) => { + let (left, right) = + self.release_dual_hand_hold(self.left_hand_grab, self.right_hand_grab); + self.left_hand_grab = left; + self.right_hand_grab = right; + return; + } + } + } + } + self.left_hand_grab = self.apply_held_body_target(self.left_hand_grab); + self.right_hand_grab = self.apply_held_body_target(self.right_hand_grab); + } + + fn drop_hand_hold(mut hand: HandGrabState) -> HandGrabState { + hand.held_body = None; + hand.grab_offset = Pose::default(); + hand + } + + fn apply_held_body_target(&mut self, mut hand: HandGrabState) -> HandGrabState { + let Some(body_handle) = hand.held_body else { + return hand; + }; + if !hand.tracked || !hand.gripping { + return self.release_hand_hold(hand); + } + let Some(body) = self.bodies.get_mut(body_handle) else { + hand.held_body = None; + return hand; + }; + if !body.is_enabled() { + hand.held_body = None; + return hand; + } + let target_pose = Pose::multiply(&hand.pose, &hand.grab_offset); + if !target_pose.is_finite() { + return self.release_hand_hold(hand); + } + if body.body_type() != RigidBodyType::KinematicPositionBased { + body.set_body_type(RigidBodyType::KinematicPositionBased, true); + body.set_position(rapier_pose(target_pose), false); + } + body.set_next_kinematic_position(rapier_pose(target_pose)); + body.wake_up(true); + hand + } + + fn apply_dual_held_body_target(&mut self, primary: HandGrabState, secondary: HandGrabState) { + let Some(body_handle) = primary.held_body else { + return; + }; + let Some(body) = self.bodies.get_mut(body_handle) else { + self.left_hand_grab = Self::drop_hand_hold(self.left_hand_grab); + self.right_hand_grab = Self::drop_hand_hold(self.right_hand_grab); + return; + }; + if !body.is_enabled() { + self.left_hand_grab = Self::drop_hand_hold(self.left_hand_grab); + self.right_hand_grab = Self::drop_hand_hold(self.right_hand_grab); + return; + } + let primary_single_target = Pose::multiply(&primary.pose, &primary.grab_offset); + let primary_anchor_local = primary.grab_offset.invert().position; + let secondary_anchor_local = secondary.grab_offset.invert().position; + let local_anchor_delta = secondary_anchor_local - primary_anchor_local; + let world_hand_delta = secondary.pose.position - primary.pose.position; + let target_pose = if local_anchor_delta.length() >= XR_HAND_DUAL_GRAB_MIN_SPAN + && world_hand_delta.length() >= XR_HAND_DUAL_GRAB_MIN_SPAN + { + let baseline_world_delta = primary_single_target + .orientation + .rotate_vec3(&local_anchor_delta); + let orientation = Quat::multiply( + &quat_from_to(baseline_world_delta, world_hand_delta), + &primary_single_target.orientation, + ); + let primary_world_anchor = orientation.rotate_vec3(&primary_anchor_local); + let secondary_world_anchor = orientation.rotate_vec3(&secondary_anchor_local); + let position = ((primary.pose.position - primary_world_anchor) + + (secondary.pose.position - secondary_world_anchor)) + * 0.5; + Pose::new(orientation, position) + } else { + primary_single_target + }; + if !target_pose.is_finite() { + let _ = body; + let (left, right) = + self.release_dual_hand_hold(self.left_hand_grab, self.right_hand_grab); + self.left_hand_grab = left; + self.right_hand_grab = right; + return; + } + if body.body_type() != RigidBodyType::KinematicPositionBased { + body.set_body_type(RigidBodyType::KinematicPositionBased, true); + body.set_position(rapier_pose(target_pose), false); + } + body.set_next_kinematic_position(rapier_pose(target_pose)); + body.wake_up(true); + } + + fn release_hand_hold(&mut self, mut hand: HandGrabState) -> HandGrabState { + let Some(body_handle) = hand.held_body.take() else { + return hand; + }; + self.release_body_into_dynamics( + body_handle, + hand.linvel * XR_HAND_GRAB_RELEASE_LINEAR_VELOCITY_SCALE, + vec3f(0.0, 0.0, 0.0), + ); + hand + } + + fn release_dual_hand_hold( + &mut self, + mut primary: HandGrabState, + mut secondary: HandGrabState, + ) -> (HandGrabState, HandGrabState) { + let Some(body_handle) = primary.held_body else { + return ( + Self::drop_hand_hold(primary), + Self::drop_hand_hold(secondary), + ); + }; + self.release_body_into_dynamics( + body_handle, + (primary.linvel + secondary.linvel) + * (0.5 * XR_HAND_GRAB_RELEASE_LINEAR_VELOCITY_SCALE), + vec3f(0.0, 0.0, 0.0), + ); + primary.held_body = None; + primary.grab_offset = Pose::default(); + secondary.held_body = None; + secondary.grab_offset = Pose::default(); + (primary, secondary) + } + + fn acquire_hand_grabs(&mut self) { + let left_slots = self.left_hand.clone(); + let right_slots = self.right_hand.clone(); + let right_held = self.right_hand_grab.held_body; + self.left_hand_grab = + self.try_acquire_hand_hold(self.left_hand_grab, &left_slots, right_held); + let left_held = self.left_hand_grab.held_body; + self.right_hand_grab = + self.try_acquire_hand_hold(self.right_hand_grab, &right_slots, left_held); + } + + fn try_acquire_hand_hold( + &mut self, + mut hand: HandGrabState, + slots: &[HandColliderBody], + other_held_body: Option, + ) -> HandGrabState { + if !hand.tracked || !hand.gripping || hand.held_body.is_some() { + return hand; + } + + let mut best_candidate = None; + for cube in &self.cubes { + if cube.body_kind != XrBodyKind::Dynamic { + continue; + } + let shared_candidate = other_held_body == Some(cube.body); + if self.shadow_body_motion.contains_key(&cube.body) { + continue; + } + let Some(body) = self.bodies.get(cube.body) else { + continue; + }; + if !body.is_enabled() { + continue; + } + let body_type = body.body_type(); + if !(body_type == RigidBodyType::Dynamic + || (shared_candidate && body_type == RigidBodyType::KinematicPositionBased)) + { + continue; + } + let Some((distance, body_pose, body_anchor_local)) = + self.project_hand_anchor_to_body(cube, hand.pose) + else { + continue; + }; + if distance > XR_HAND_GRAB_MAX_DISTANCE { + continue; + } + if !shared_candidate && !self.cube_has_hand_contact(cube.collider, slots) { + continue; + } + if best_candidate + .map(|(best_distance, _, _, _)| distance < best_distance) + .unwrap_or(true) + { + best_candidate = Some((distance, cube.body, body_pose, body_anchor_local)); + } + } + + let Some((_, body_handle, body_pose, body_anchor_local)) = best_candidate else { + return hand; + }; + hand.grab_offset = grab_offset_from_body_anchor(hand.pose, body_pose, body_anchor_local); + hand.held_body = Some(body_handle); + self.release_settle_state(body_handle); + let target_pose = Pose::multiply(&hand.pose, &hand.grab_offset); + if !target_pose.is_finite() { + hand.held_body = None; + hand.grab_offset = Pose::default(); + return hand; + } + if other_held_body != Some(body_handle) { + if let Some(body) = self.bodies.get_mut(body_handle) { + body.set_body_type(RigidBodyType::KinematicPositionBased, true); + body.set_position(rapier_pose(target_pose), false); + body.set_next_kinematic_position(rapier_pose(target_pose)); + body.wake_up(true); + } + } else if let Some(body) = self.bodies.get_mut(body_handle) { + body.wake_up(true); + } + hand + } + + fn project_hand_anchor_to_body( + &self, + cube: &PhysicsCube, + hand_pose: Pose, + ) -> Option<(f32, Pose, Vec3f)> { + let body = self.bodies.get(cube.body)?; + let collider = self.colliders.get(cube.collider)?; + let body_pose = makepad_pose(body.position()); + let surface_projection = collider.shape().project_point( + collider.position(), + rapier_vec3(hand_pose.position), + false, + ); + let world_anchor = makepad_vec3(surface_projection.point); + let body_anchor_local = body_pose.invert().transform_vec3(&world_anchor); + let distance = (world_anchor - hand_pose.position).length(); + (distance.is_finite() && body_anchor_local.is_finite()).then_some(( + distance, + body_pose, + body_anchor_local, + )) + } + + fn cube_has_hand_contact( + &self, + cube_collider: ColliderHandle, + slots: &[HandColliderBody], + ) -> bool { + self.narrow_phase + .contact_pairs_with(cube_collider) + .any(|pair| { + pair.has_any_active_contact() + && slots.iter().any(|slot| { + self.colliders + .get(slot.collider) + .map(|collider| collider.is_enabled()) + .unwrap_or(false) + && (pair.collider1 == slot.collider || pair.collider2 == slot.collider) + }) + }) + } + + fn cube_settle_contact_state(&self, cube_collider: ColliderHandle) -> SettleContactState { + let mut state = SettleContactState::default(); + for pair in self.narrow_phase.contact_pairs_with(cube_collider) { + if !pair.has_any_active_contact() { + continue; + } + state.has_active_contact = true; + let other = if pair.collider1 == cube_collider { + pair.collider2 + } else { + pair.collider1 + }; + let Some(other_collider) = self.colliders.get(other) else { + continue; + }; + if let Some(role) = decode_depth_query_surface_role(other_collider.user_data) { + state.has_depth_query_contact = true; + if role == DepthQueryColliderRole::Support { + state.has_support_contact = true; + } + } + } + state + } + + fn release_settle_state(&mut self, body_handle: RigidBodyHandle) { + self.settle_frame_counts.remove(&body_handle); + } + + fn release_body_into_dynamics( + &mut self, + body_handle: RigidBodyHandle, + linvel: Vec3f, + angvel: Vec3f, + ) { + self.shadow_body_motion.remove(&body_handle); + self.release_settle_state(body_handle); + let Some(body) = self.bodies.get_mut(body_handle) else { + return; + }; + if body.body_type() != RigidBodyType::Dynamic { + body.set_body_type(RigidBodyType::Dynamic, true); + } + body.set_linvel(rapier_vec3(linvel), true); + body.set_angvel(rapier_vec3(angvel), true); + body.wake_up(true); + } + + fn clear_held_body(&mut self, body_handle: RigidBodyHandle) { + self.release_settle_state(body_handle); + if self.left_hand_grab.held_body == Some(body_handle) { + self.left_hand_grab = self.release_hand_hold(self.left_hand_grab); + } + if self.right_hand_grab.held_body == Some(body_handle) { + self.right_hand_grab = self.release_hand_hold(self.right_hand_grab); + } + } + + pub(crate) fn held_by_for_body(&self, body_handle: RigidBodyHandle) -> Option { + if self.left_hand_grab.held_body == Some(body_handle) { + Some(self.left_hand_grab.shared_hand) + } else if self.right_hand_grab.held_body == Some(body_handle) { + Some(self.right_hand_grab.shared_hand) + } else { + None + } + } + + pub(crate) fn shadow_body_motion_for_body( + &self, + body_handle: RigidBodyHandle, + ) -> Option<(Vec3f, Vec3f)> { + let motion = self.shadow_body_motion.get(&body_handle)?; + if motion.remaining_prediction_seconds > 0.0 { + Some((motion.linvel, motion.angvel)) + } else { + Some((vec3f(0.0, 0.0, 0.0), vec3f(0.0, 0.0, 0.0))) + } + } + + pub(crate) fn is_shadow_body(&self, body_handle: RigidBodyHandle) -> bool { + self.shadow_body_motion.contains_key(&body_handle) + } + fn settle_resting_bodies(&mut self) { let linear_speed_sq = XR_BODY_SNAP_SLEEP_LINEAR_SPEED * XR_BODY_SNAP_SLEEP_LINEAR_SPEED; let angular_speed_sq = XR_BODY_SNAP_SLEEP_ANGULAR_SPEED * XR_BODY_SNAP_SLEEP_ANGULAR_SPEED; let mut to_sleep = Vec::new(); + let mut to_reset = Vec::new(); for cube in &self.cubes { if !matches!(cube.body_kind, XrBodyKind::Dynamic) { continue; } - let has_active_contact = self - .narrow_phase - .contact_pairs_with(cube.collider) - .any(|pair| pair.has_any_active_contact()); - if !has_active_contact { + if self.left_hand_grab.held_body == Some(cube.body) + || self.right_hand_grab.held_body == Some(cube.body) + { + to_reset.push(cube.body); + continue; + } + let contact_state = self.cube_settle_contact_state(cube.collider); + if !contact_state.has_active_contact { + to_reset.push(cube.body); continue; } + let has_hand_contact = self.cube_has_hand_contact(cube.collider, &self.left_hand) + || self.cube_has_hand_contact(cube.collider, &self.right_hand); let Some(body) = self.bodies.get(cube.body) else { + to_reset.push(cube.body); continue; }; - if body.is_sleeping() { + let body_type = body.body_type(); + let body_is_sleeping = body.is_sleeping(); + let linvel = body.linvel(); + let angvel = body.angvel(); + if body_type != RigidBodyType::Dynamic { + to_reset.push(cube.body); + continue; + } + if body_is_sleeping { + if has_hand_contact { + if let Some(body) = self.bodies.get_mut(cube.body) { + body.wake_up(true); + } + } + to_reset.push(cube.body); continue; } - let linvel = body.linvel(); - let angvel = body.angvel(); let linvel_sq = linvel.x * linvel.x + linvel.y * linvel.y + linvel.z * linvel.z; let angvel_sq = angvel.x * angvel.x + angvel.y * angvel.y + angvel.z * angvel.z; - if linvel_sq <= linear_speed_sq && angvel_sq <= angular_speed_sq { - to_sleep.push(cube.body); + let allow_settle = (!contact_state.has_depth_query_contact + || contact_state.has_support_contact) + && !has_hand_contact; + if allow_settle && linvel_sq <= linear_speed_sq && angvel_sq <= angular_speed_sq { + let frames = self + .settle_frame_counts + .entry(cube.body) + .and_modify(|frames| *frames = frames.saturating_add(1)) + .or_insert(1); + if *frames >= XR_BODY_SNAP_SLEEP_SETTLE_FRAMES { + to_sleep.push(cube.body); + } + } else { + to_reset.push(cube.body); } } + for handle in to_reset { + self.release_settle_state(handle); + } for handle in to_sleep { if let Some(body) = self.bodies.get_mut(handle) { body.set_linvel(RapierVector::ZERO, false); body.set_angvel(RapierVector::ZERO, false); } + self.release_settle_state(handle); } } @@ -556,11 +1206,12 @@ impl RapierScene { self.depth_query_stats } - pub(crate) fn register_projectile_cube(&mut self, cube_index: usize) { + pub(crate) fn register_spawn_pool_cube(&mut self, cube_index: usize) { let Some(cube) = self.cubes.get(cube_index).copied() else { return; }; - self.projectile_cube_indices.push(cube_index); + self.spawn_pool_cube_indices.push(cube_index); + self.shadow_body_motion.remove(&cube.body); if let Some(body) = self.bodies.get_mut(cube.body) { body.set_enabled(false); body.set_linvel(RapierVector::ZERO, false); @@ -573,6 +1224,8 @@ impl RapierScene { pub(crate) fn respawn_body( &mut self, widget_uid: WidgetUid, + shadow: bool, + mode: XrSharedObjectMode, pose: Pose, linvel: Vec3f, angvel: Vec3f, @@ -582,14 +1235,105 @@ impl RapierScene { .iter() .find(|cube| cube.widget_uid == widget_uid) .copied()?; + self.clear_held_body(cube.body); + self.shadow_body_motion.remove(&cube.body); + self.release_settle_state(cube.body); if let Some(body) = self.bodies.get_mut(cube.body) { body.set_enabled(true); body.set_position(rapier_pose(pose), false); - body.set_linvel(rapier_vec3(linvel), true); - body.set_angvel(rapier_vec3(angvel), true); + if shadow { + let remaining_prediction_seconds = if matches!(mode, XrSharedObjectMode::Sleeping) { + 0.0 + } else { + XR_SHADOW_BODY_MAX_EXTRAPOLATION_SECONDS + }; + body.set_body_type(RigidBodyType::KinematicPositionBased, true); + body.set_next_kinematic_position(rapier_pose(pose)); + body.set_linvel(RapierVector::ZERO, false); + body.set_angvel(RapierVector::ZERO, false); + body.wake_up(true); + self.shadow_body_motion.insert( + cube.body, + ShadowBodyMotion { + pose, + linvel, + angvel, + remaining_prediction_seconds, + }, + ); + } else { + match mode { + XrSharedObjectMode::ContactDominated { .. } => { + body.set_body_type(RigidBodyType::KinematicPositionBased, true); + body.set_next_kinematic_position(rapier_pose(pose)); + body.set_linvel(RapierVector::ZERO, false); + body.set_angvel(RapierVector::ZERO, false); + body.wake_up(true); + } + XrSharedObjectMode::Dynamic => { + body.set_body_type(RigidBodyType::Dynamic, true); + body.set_linvel(rapier_vec3(linvel), true); + body.set_angvel(rapier_vec3(angvel), true); + body.wake_up(true); + } + XrSharedObjectMode::Sleeping => { + body.set_body_type(RigidBodyType::Dynamic, true); + body.set_linvel(rapier_vec3(linvel), false); + body.set_angvel(rapier_vec3(angvel), false); + } + } + } + body.reset_forces(false); + body.reset_torques(false); + } + cube.depth_query_surface_set + .map(RapierScene::depth_query_key) + } + + pub(crate) fn apply_impulse( + &mut self, + widget_uid: WidgetUid, + point: Vec3f, + impulse: Vec3f, + ) -> bool { + let Some(cube) = self + .cubes + .iter() + .find(|cube| cube.widget_uid == widget_uid) + .copied() + else { + return false; + }; + self.release_settle_state(cube.body); + if self.shadow_body_motion.contains_key(&cube.body) { + return false; + } + let Some(body) = self.bodies.get_mut(cube.body) else { + return false; + }; + if !body.is_enabled() || body.body_type() != RigidBodyType::Dynamic { + return false; + } + body.apply_impulse_at_point(rapier_vec3(impulse), rapier_vec3(point), true); + body.wake_up(true); + true + } + + pub(crate) fn despawn_body(&mut self, widget_uid: WidgetUid) -> Option { + let cube = self + .cubes + .iter() + .find(|cube| cube.widget_uid == widget_uid) + .copied()?; + self.clear_held_body(cube.body); + self.shadow_body_motion.remove(&cube.body); + self.release_settle_state(cube.body); + if let Some(body) = self.bodies.get_mut(cube.body) { + body.set_enabled(false); + body.set_linvel(RapierVector::ZERO, false); + body.set_angvel(RapierVector::ZERO, false); body.reset_forces(false); body.reset_torques(false); - body.wake_up(true); } cube.depth_query_surface_set .map(RapierScene::depth_query_key) @@ -603,6 +1347,14 @@ impl RapierScene { let Some(surface_set) = self.depth_query_surface_sets.get_mut(set_index) else { return; }; + if self.shadow_body_motion.contains_key(&surface_set.body) { + for surface in &surface_set.surfaces { + if let Some(collider) = self.colliders.get_mut(surface.collider) { + collider.set_enabled(false); + } + } + return; + } let body_position = self .bodies .get(surface_set.body) @@ -626,23 +1378,14 @@ impl RapierScene { continue; }; if let Some(collider) = self.colliders.get_mut(surface.collider) { - let XrDepthMeshQueryColliderGeometry::HalfSpace(plane) = target.collider.geometry; - let footprint_supports_body = depth_query_plane_supports_body( - plane, + let DepthQueryColliderGeometry::HalfSpace(plane) = target.collider.geometry; + let supports_body = depth_query_surface_target_should_enable( + *target, body_position, + body_velocity, surface_set.query_radius, physics_edge_margin, ); - let supports_body = match target.collider.role { - XrDepthMeshQueryColliderRole::Support => footprint_supports_body, - XrDepthMeshQueryColliderRole::Impact => { - let speed = body_velocity.length(); - let approach_speed = -body_velocity.dot(plane.normal); - footprint_supports_body - && speed >= XR_DEPTH_QUERY_IMPACT_ENABLE_SPEED_MIN - && approach_speed >= XR_DEPTH_QUERY_IMPACT_ENABLE_APPROACH_SPEED_MIN - } - }; if surface.fingerprint != target.collider.fingerprint { collider.set_shape(SharedShape::halfspace(rapier_vec3(plane.normal))); collider.set_position_wrt_parent(RapierPose::from_parts( @@ -651,14 +1394,19 @@ impl RapierScene { )); surface.fingerprint = target.collider.fingerprint; } + collider.user_data = depth_query_surface_user_data( + surface_set.owner_widget_uid, + target.collider.role, + ); collider.set_restitution(target.collider.restitution.max(0.0)); collider.set_enabled(supports_body); if supports_body { - self.depth_query_stats.active_surface_count += 1; - self.depth_query_stats.vertex_count += target.collider.vertex_count(); - self.depth_query_stats.triangle_count += target.collider.triangle_count(); + self.depth_query_stats.surface_count += 1; } } } } } + +#[cfg(test)] +include!("../tests/scene/xr_physics.rs"); diff --git a/xr/src/scene/xr_physics_worker.rs b/xr/src/scene/xr_physics_worker.rs index 80dc33c6ab..d01c317643 100644 --- a/xr/src/scene/xr_physics_worker.rs +++ b/xr/src/scene/xr_physics_worker.rs @@ -8,10 +8,10 @@ use super::{ CollectedXrCube, }; use crate::{ - xr_node::{XrBodyKind, XrRuntimeBodyState}, - *, + prelude::*, + scene::{XrBodyKind, XrRuntimeBodyState}, }; -use makepad_widgets::makepad_platform::{event::XrHand, XrDepthMeshStore}; +use makepad_widgets::makepad_platform::{event::XrHand, XrTsdfStore}; use std::{ collections::HashMap, mem, @@ -26,7 +26,8 @@ const XR_WORKER_SIMULATION_DT_MAX: f32 = 1.0 / 45.0; const XR_WORKER_SIMULATION_DT_SMOOTHING: f32 = 0.35; const XR_WORKER_SIMULATION_DT_LADDER: [f32; 5] = [1.0 / 120.0, 1.0 / 90.0, 1.0 / 72.0, 1.0 / 60.0, 1.0 / 45.0]; -const XR_PHYSICS_WORKER_MAX_PENDING_BODY_SPAWNS: usize = 8; +const XR_PHYSICS_WORKER_MAX_PENDING_BODY_DESPAWNS: usize = 8; +const XR_PHYSICS_WORKER_MAX_PENDING_BODY_IMPULSES: usize = 8; #[derive(Clone)] struct PhysicsWorkerRebuild { @@ -50,6 +51,18 @@ struct PhysicsWorkerBodySpawn { spawn: XrBodySpawn, } +#[derive(Clone, Copy)] +struct PhysicsWorkerBodyDespawn { + revision: u64, + widget_uid: WidgetUid, +} + +#[derive(Clone, Copy)] +struct PhysicsWorkerBodyImpulse { + revision: u64, + impulse: XrBodyImpulse, +} + #[derive(Default)] struct PhysicsWorkerMailbox { version: u64, @@ -57,8 +70,11 @@ struct PhysicsWorkerMailbox { pending_reset_revision: Option, pending_rebuild: Option, pending_step: Option, - pending_body_spawns: - SmallVec<[PhysicsWorkerBodySpawn; XR_PHYSICS_WORKER_MAX_PENDING_BODY_SPAWNS]>, + pending_body_spawns: Vec, + pending_body_despawns: + SmallVec<[PhysicsWorkerBodyDespawn; XR_PHYSICS_WORKER_MAX_PENDING_BODY_DESPAWNS]>, + pending_body_impulses: + SmallVec<[PhysicsWorkerBodyImpulse; XR_PHYSICS_WORKER_MAX_PENDING_BODY_IMPULSES]>, } pub(super) struct XrPhysicsWorkerResult { @@ -66,10 +82,12 @@ pub(super) struct XrPhysicsWorkerResult { pub(super) runtime_bodies: HashMap, pub(super) depth_query_retained_hits: Option>, pub(super) physics_compute_ms: f64, - pub(super) physics_step_dt_ms: f64, + pub(super) physics_tsdf_query_ms: f64, + pub(super) physics_rapier_step_ms: f64, pub(super) physics_depth_query_surface_count: usize, - pub(super) physics_depth_query_vertex_count: usize, - pub(super) physics_depth_query_triangle_count: usize, + pub(super) physics_scene_body_count: usize, + pub(super) physics_body_spawn_apply_count: usize, + pub(super) physics_body_spawn_miss_count: usize, } pub(super) struct XrPhysicsWorker { @@ -79,7 +97,7 @@ pub(super) struct XrPhysicsWorker { } impl XrPhysicsWorker { - pub(super) fn new(depth_mesh: XrDepthMeshStore) -> Self { + pub(super) fn new(depth_mesh: XrTsdfStore) -> Self { let mailbox = Arc::new((Mutex::new(PhysicsWorkerMailbox::default()), Condvar::new())); let latest_result = Arc::new(Mutex::new(None)); let mailbox_thread = mailbox.clone(); @@ -139,10 +157,46 @@ impl XrPhysicsWorker { pub(super) fn request_body_spawn(&mut self, revision: u64, spawn: XrBodySpawn) { let (lock, wake) = &*self.mailbox; if let Ok(mut mailbox) = lock.lock() { - if mailbox.pending_body_spawns.len() < XR_PHYSICS_WORKER_MAX_PENDING_BODY_SPAWNS { + if let Some(pending) = mailbox + .pending_body_spawns + .iter_mut() + .find(|pending| pending.spawn.widget_uid == spawn.widget_uid) + { + pending.revision = revision; + pending.spawn = spawn; + } else { mailbox .pending_body_spawns .push(PhysicsWorkerBodySpawn { revision, spawn }); + } + mailbox.version = mailbox.version.saturating_add(1); + wake.notify_one(); + } + } + + pub(super) fn request_body_despawn(&mut self, revision: u64, widget_uid: WidgetUid) { + let (lock, wake) = &*self.mailbox; + if let Ok(mut mailbox) = lock.lock() { + if mailbox.pending_body_despawns.len() < XR_PHYSICS_WORKER_MAX_PENDING_BODY_DESPAWNS { + mailbox + .pending_body_despawns + .push(PhysicsWorkerBodyDespawn { + revision, + widget_uid, + }); + mailbox.version = mailbox.version.saturating_add(1); + wake.notify_one(); + } + } + } + + pub(super) fn request_body_impulse(&mut self, revision: u64, impulse: XrBodyImpulse) { + let (lock, wake) = &*self.mailbox; + if let Ok(mut mailbox) = lock.lock() { + if mailbox.pending_body_impulses.len() < XR_PHYSICS_WORKER_MAX_PENDING_BODY_IMPULSES { + mailbox + .pending_body_impulses + .push(PhysicsWorkerBodyImpulse { revision, impulse }); mailbox.version = mailbox.version.saturating_add(1); wake.notify_one(); } @@ -156,6 +210,8 @@ impl XrPhysicsWorker { mailbox.pending_rebuild = None; mailbox.pending_step = None; mailbox.pending_body_spawns.clear(); + mailbox.pending_body_despawns.clear(); + mailbox.pending_body_impulses.clear(); mailbox.version = mailbox.version.saturating_add(1); wake.notify_one(); } @@ -184,7 +240,7 @@ impl Drop for XrPhysicsWorker { } fn physics_worker_loop( - depth_mesh: XrDepthMeshStore, + depth_mesh: XrTsdfStore, mailbox: Arc<(Mutex, Condvar)>, latest_result: Arc>>, ) { @@ -196,9 +252,18 @@ fn physics_worker_loop( let mut retained_hits_snapshot_scratch = HashMap::new(); let mut last_step_started_at: Option = None; let mut adaptive_step_dt = XR_WORKER_SIMULATION_DT_DEFAULT; + let mut total_body_spawn_apply_count = 0usize; + let mut total_body_spawn_miss_count = 0usize; loop { - let (pending_reset_revision, pending_rebuild, pending_step, pending_body_spawns) = { + let ( + pending_reset_revision, + pending_rebuild, + pending_step, + pending_body_spawns, + pending_body_despawns, + pending_body_impulses, + ) = { let (lock, wake) = &*mailbox; let mut guard = match lock.lock() { Ok(guard) => guard, @@ -211,7 +276,7 @@ fn physics_worker_loop( }; } if guard.shutdown { - clear_depth_query_state_for_scene(&depth_mesh, scene.as_ref(), &mut retained_hits); + clear_depth_query_state_for_scene(scene.as_ref(), &mut retained_hits); return; } seen_version = guard.version; @@ -220,14 +285,18 @@ fn physics_worker_loop( guard.pending_rebuild.take(), guard.pending_step.take(), std::mem::take(&mut guard.pending_body_spawns), + std::mem::take(&mut guard.pending_body_despawns), + std::mem::take(&mut guard.pending_body_impulses), ) }; let mut should_publish = false; + let mut physics_body_spawn_apply_count = 0usize; + let mut physics_body_spawn_miss_count = 0usize; if let Some(reset_revision) = pending_reset_revision { revision = reset_revision; - clear_depth_query_state_for_scene(&depth_mesh, scene.as_ref(), &mut retained_hits); + clear_depth_query_state_for_scene(scene.as_ref(), &mut retained_hits); scene = None; last_step_started_at = None; adaptive_step_dt = XR_WORKER_SIMULATION_DT_DEFAULT; @@ -236,7 +305,7 @@ fn physics_worker_loop( if let Some(rebuild) = pending_rebuild { revision = rebuild.revision; - clear_depth_query_state_for_scene(&depth_mesh, scene.as_ref(), &mut retained_hits); + clear_depth_query_state_for_scene(scene.as_ref(), &mut retained_hits); scene = Some(build_scene(rebuild.gravity, rebuild.cubes)); last_step_started_at = None; adaptive_step_dt = scene @@ -255,12 +324,21 @@ fn physics_worker_loop( } if let Some(query_key) = scene.respawn_body( body_spawn.spawn.widget_uid, + body_spawn.spawn.shadow, + body_spawn.spawn.mode, body_spawn.spawn.pose, body_spawn.spawn.linvel, body_spawn.spawn.angvel, ) { - depth_mesh.clear_query(query_key); retained_hits.remove(&query_key); + physics_body_spawn_apply_count = + physics_body_spawn_apply_count.saturating_add(1); + total_body_spawn_apply_count = + total_body_spawn_apply_count.saturating_add(1); + } else { + physics_body_spawn_miss_count = + physics_body_spawn_miss_count.saturating_add(1); + total_body_spawn_miss_count = total_body_spawn_miss_count.saturating_add(1); } applied_spawn = true; } @@ -268,6 +346,39 @@ fn physics_worker_loop( } } + if !pending_body_despawns.is_empty() { + if let Some(scene) = scene.as_mut() { + let mut applied_despawn = false; + for body_despawn in pending_body_despawns { + if body_despawn.revision != revision { + continue; + } + if let Some(query_key) = scene.despawn_body(body_despawn.widget_uid) { + retained_hits.remove(&query_key); + } + applied_despawn = true; + } + should_publish |= applied_despawn; + } + } + + if !pending_body_impulses.is_empty() { + if let Some(scene) = scene.as_mut() { + let mut applied_impulse = false; + for body_impulse in pending_body_impulses { + if body_impulse.revision != revision { + continue; + } + applied_impulse |= scene.apply_impulse( + body_impulse.impulse.widget_uid, + body_impulse.impulse.point, + body_impulse.impulse.impulse, + ); + } + should_publish |= applied_impulse; + } + } + if let Some(step) = pending_step { if step.revision == revision { let started = Instant::now(); @@ -275,34 +386,32 @@ fn physics_worker_loop( choose_worker_simulation_dt(last_step_started_at, started, adaptive_step_dt); last_step_started_at = Some(started); sync_hands_on_scene(scene.as_mut(), &step.left_hand, &step.right_hand); + let tsdf_query_started = Instant::now(); sync_depth_query_surfaces_with_store( &mut retained_hits, scene.as_mut(), &depth_mesh, ); - let ( - runtime_bodies, - physics_step_dt_ms, - physics_depth_query_surface_count, - physics_depth_query_vertex_count, - physics_depth_query_triangle_count, - ) = if let Some(scene) = scene.as_mut() { - let simulation_dt = (adaptive_step_dt * step.time_scale.clamp(0.1, 1.0)) - .clamp(XR_WORKER_SIMULATION_DT_MIN, XR_WORKER_SIMULATION_DT_MAX); - scene.set_simulation_dt(simulation_dt); - scene.step(); - let stats = scene.depth_query_stats(); - snapshot_runtime_bodies(scene, &mut runtime_bodies_scratch); - ( - mem::take(&mut runtime_bodies_scratch), - simulation_dt as f64 * 1000.0, - stats.active_surface_count, - stats.vertex_count, - stats.triangle_count, - ) - } else { - (HashMap::new(), 0.0, 0, 0, 0) - }; + let physics_tsdf_query_ms = tsdf_query_started.elapsed().as_secs_f64() * 1000.0; + let (runtime_bodies, physics_rapier_step_ms, physics_depth_query_surface_count) = + if let Some(scene) = scene.as_mut() { + let simulation_dt = (adaptive_step_dt * step.time_scale.clamp(0.1, 1.0)) + .clamp(XR_WORKER_SIMULATION_DT_MIN, XR_WORKER_SIMULATION_DT_MAX); + scene.set_simulation_dt(simulation_dt); + let rapier_step_started = Instant::now(); + scene.step(); + let physics_rapier_step_ms = + rapier_step_started.elapsed().as_secs_f64() * 1000.0; + let stats = scene.depth_query_stats(); + snapshot_runtime_bodies(scene, &mut runtime_bodies_scratch); + ( + mem::take(&mut runtime_bodies_scratch), + physics_rapier_step_ms, + stats.surface_count, + ) + } else { + (HashMap::new(), 0.0, 0) + }; if step.include_retained_hits { snapshot_retained_hits(&retained_hits, &mut retained_hits_snapshot_scratch); } else { @@ -317,10 +426,15 @@ fn physics_worker_loop( .include_retained_hits .then(|| mem::take(&mut retained_hits_snapshot_scratch)), physics_compute_ms: started.elapsed().as_secs_f64() * 1000.0, - physics_step_dt_ms, + physics_tsdf_query_ms, + physics_rapier_step_ms, physics_depth_query_surface_count, - physics_depth_query_vertex_count, - physics_depth_query_triangle_count, + physics_scene_body_count: scene + .as_ref() + .map(|scene| scene.cubes.len()) + .unwrap_or(0), + physics_body_spawn_apply_count: total_body_spawn_apply_count, + physics_body_spawn_miss_count: total_body_spawn_miss_count, }, ); recycle_worker_buffers( @@ -333,19 +447,13 @@ fn physics_worker_loop( } if should_publish { - let (runtime_bodies, surface_count, vertex_count, triangle_count) = - if let Some(scene) = scene.as_ref() { - let stats = scene.depth_query_stats(); - snapshot_runtime_bodies(scene, &mut runtime_bodies_scratch); - ( - mem::take(&mut runtime_bodies_scratch), - stats.active_surface_count, - stats.vertex_count, - stats.triangle_count, - ) - } else { - (HashMap::new(), 0, 0, 0) - }; + let (runtime_bodies, surface_count) = if let Some(scene) = scene.as_ref() { + let stats = scene.depth_query_stats(); + snapshot_runtime_bodies(scene, &mut runtime_bodies_scratch); + (mem::take(&mut runtime_bodies_scratch), stats.surface_count) + } else { + (HashMap::new(), 0) + }; let recycled = publish_worker_result( &latest_result, XrPhysicsWorkerResult { @@ -353,13 +461,15 @@ fn physics_worker_loop( runtime_bodies, depth_query_retained_hits: None, physics_compute_ms: 0.0, - physics_step_dt_ms: scene - .as_ref() - .map(|scene| scene.simulation_dt() as f64 * 1000.0) - .unwrap_or(XR_WORKER_SIMULATION_DT_DEFAULT as f64 * 1000.0), + physics_tsdf_query_ms: 0.0, + physics_rapier_step_ms: 0.0, physics_depth_query_surface_count: surface_count, - physics_depth_query_vertex_count: vertex_count, - physics_depth_query_triangle_count: triangle_count, + physics_scene_body_count: scene + .as_ref() + .map(|scene| scene.cubes.len()) + .unwrap_or(0), + physics_body_spawn_apply_count: total_body_spawn_apply_count, + physics_body_spawn_miss_count: total_body_spawn_miss_count, }, ); recycle_worker_buffers( @@ -400,11 +510,11 @@ fn publish_worker_result( fn build_scene(gravity: f32, cubes: Vec) -> RapierScene { let mut scene = RapierScene::new(gravity); for cube in cubes { - let projectile_pool = cube.projectile_pool; + let spawn_pool = cube.spawn_pool; match cube.body_kind { XrBodyKind::Disabled => {} XrBodyKind::Dynamic => { - if cube.is_sphere { + if cube.physics_shape == XrPhysicsShape::Sphere { scene.spawn_dynamic_sphere( cube.uid, cube.pose, @@ -428,12 +538,12 @@ fn build_scene(gravity: f32, cubes: Vec) -> RapierScene { cube.restitution, ); } - if projectile_pool && !scene.cubes.is_empty() { - scene.register_projectile_cube(scene.cubes.len() - 1); + if spawn_pool && !scene.cubes.is_empty() { + scene.register_spawn_pool_cube(scene.cubes.len() - 1); } } XrBodyKind::Fixed => { - if cube.is_sphere { + if cube.physics_shape == XrPhysicsShape::Sphere { scene.spawn_fixed_sphere( cube.uid, cube.pose, @@ -477,6 +587,22 @@ fn snapshot_runtime_bodies( XrRuntimeBodyState { pose: makepad_pose(body.position()), scale: cube.scale, + linvel: scene + .shadow_body_motion_for_body(cube.body) + .map(|(linvel, _)| linvel) + .unwrap_or_else(|| { + let linvel = body.linvel(); + vec3f(linvel.x, linvel.y, linvel.z) + }), + angvel: scene + .shadow_body_motion_for_body(cube.body) + .map(|(_, angvel)| angvel) + .unwrap_or_else(|| { + let angvel = body.angvel(); + vec3f(angvel.x, angvel.y, angvel.z) + }), + sleeping: body.is_sleeping(), + held_by: scene.held_by_for_body(cube.body), }, ); } diff --git a/xr/src/scene/xr_root.rs b/xr/src/scene/xr_root.rs index 641da3fbeb..e84b4859e8 100644 --- a/xr/src/scene/xr_root.rs +++ b/xr/src/scene/xr_root.rs @@ -1,15 +1,27 @@ -use crate::scene_draw::{ray_from_scene_viewport, SceneState3D}; -use crate::xr_env::XrEnv; -use crate::xr_select::{XrSelect, XrSelectAction}; -use crate::*; -use makepad_widgets::event::XrFingerTip; +use super::xr_env::XrEnv; +use super::xr_select::XrSelectAction; +use super::{arm_pair_metrics, flat_head_forward, hand_closed_fist_contact_point}; +use crate::prelude::*; +use crate::util::scene_draw::{ray_from_scene_viewport, SceneState3D}; +use makepad_widgets::event::{XrFingerTip, XrSyncAnchor, XrSyncAnchorExtrema}; use makepad_widgets::makepad_script::ScriptFnRef; -use std::{cell::Cell, cmp::Ordering, rc::Rc, time::Instant}; +use std::{cell::Cell, collections::HashMap, rc::Rc, time::Instant}; const DESKTOP_TOUCH_DOWN_Z: f32 = 0.0; const DESKTOP_TOUCH_UP_Z: f32 = 64.0; const XR_CONTENT_FORWARD_OFFSET: f32 = 0.28; +const XR_ACTIVITY_RESET_FORWARD_OFFSET: f32 = 1.0; const XR_CONTENT_VERTICAL_OFFSET: f32 = -0.58; +const SYNC_BOX_BROADCAST_SECONDS: f64 = 1.1; +const SYNC_BOX_DIRECTION_DEADZONE_METERS: f32 = 0.006; +const SYNC_BOX_REVERSAL_MIN_TRAVEL_METERS: f32 = 0.05; +const SYNC_BOX_MAX_VERTICAL_DELTA_METERS: f32 = 0.22; +const SYNC_BOX_MAX_DEPTH_DELTA_METERS: f32 = 0.22; +const SYNC_BOX_MIN_HAND_GAP_METERS: f32 = 0.06; +const SYNC_BOX_MAX_HAND_GAP_METERS: f32 = 0.78; +const SYNC_BOX_MIN_CHEST_DISTANCE_METERS: f32 = 0.10; +const SYNC_BOX_MAX_CHEST_DISTANCE_METERS: f32 = 1.05; +const SYNC_BOX_MAX_ARM_ELEVATION_DEGREES: f32 = 60.0; script_mod! { use mod.prelude.widgets.* @@ -62,6 +74,14 @@ pub struct XrCamera { pub viewport_rect: Option, } +#[derive(Clone, Copy, Debug, Default, PartialEq)] +pub enum XrRootAction { + PhysicsReset, + ContentPoseReset(Pose), + #[default] + None, +} + impl Default for XrCamera { fn default() -> Self { Self { @@ -194,6 +214,302 @@ impl XrCamera { } } +#[derive(Clone, Copy, Debug)] +struct BoxSyncPoseSample { + anchor: XrAnchor, + captured_at: f64, + vertical_split: f32, +} + +#[derive(Clone, Copy, Debug, PartialEq, Eq)] +enum BoxSyncMotionDirection { + Rising, + Falling, +} + +#[derive(Clone, Debug, Default)] +struct BoxSyncGestureDetector { + previous_sample: Option, + direction: Option, + extreme_sample: Option, +} + +#[derive(Default)] +struct XrRootRuntime { + initialized: bool, + started: bool, + last_xr_state: Option>, + last_dispatched_xr_state: Option>, + next_frame: NextFrame, + desktop_ui_pointer_active: bool, +} + +#[derive(Default)] +struct XrRootFrameMetrics { + last_frame_update_cpu_ms: f64, + last_frame_draw_cpu_ms: f64, + last_frame_cpu_ms: f64, +} + +impl XrRootFrameMetrics { + fn update_total(&mut self) { + self.last_frame_cpu_ms = self.last_frame_update_cpu_ms + self.last_frame_draw_cpu_ms; + } + + fn finish_draw(&mut self, started: Instant) { + self.last_frame_draw_cpu_ms = started.elapsed().as_secs_f64() * 1000.0; + self.update_total(); + } + + fn finish_update(&mut self, started: Instant) { + self.last_frame_update_cpu_ms = started.elapsed().as_secs_f64() * 1000.0; + self.update_total(); + } +} + +#[derive(Clone, Copy, Default)] +struct XrContentRig { + pose: Option, +} + +impl XrContentRig { + fn resolved_pose(&self, state: Option<&XrState>) -> Option { + self.pose.or_else(|| state.map(Self::pose_from_state)) + } + + fn flat_forward(orientation: Quat) -> Vec3f { + let mut forward = orientation.rotate_vec3(&vec3f(0.0, 0.0, -1.0)); + forward.y = 0.0; + if forward.length() <= 1.0e-6 { + vec3f(0.0, 0.0, -1.0) + } else { + forward.normalize() + } + } + + fn pose_from_state_with_forward_offset(state: &XrState, forward_offset: f32) -> Pose { + let forward = Self::flat_forward(state.head_pose.orientation); + Pose { + position: state.head_pose.position + + forward.scale(forward_offset) + + vec3f(0.0, XR_CONTENT_VERTICAL_OFFSET, 0.0), + orientation: Quat::look_rotation(forward.scale(-1.0), vec3f(0.0, 1.0, 0.0)), + } + } + + fn pose_from_state(state: &XrState) -> Pose { + Self::pose_from_state_with_forward_offset(state, XR_CONTENT_FORWARD_OFFSET) + } + + fn reset_pose_from_state(state: &XrState) -> Pose { + Self::pose_from_state_with_forward_offset(state, XR_ACTIVITY_RESET_FORWARD_OFFSET) + } + + fn ensure_pose(&mut self, cx: &mut Cx, env: &mut XrEnv, state: &XrState) { + if self.pose.is_some() { + return; + } + let pose = Self::pose_from_state(state); + self.pose = Some(pose); + env.set_root_pose(cx, Some(pose)); + } + + fn clear_pose(&mut self, cx: &mut Cx, env: &mut XrEnv) { + if self.pose.is_none() { + return; + } + self.pose = None; + env.set_root_pose(cx, None); + } + + fn transform(&self, state: Option<&XrState>) -> Mat4f { + self.resolved_pose(state) + .map(|pose| pose.to_mat4()) + .unwrap_or_else(Mat4f::identity) + } +} + +#[derive(Clone, Debug, Default)] +struct XrSyncAnchorRuntime { + detector: BoxSyncGestureDetector, + pending_sync_anchor: Option, + pending_sync_anchor_emitted_at: Option, + next_sync_anchor_id: u32, +} + +impl XrSyncAnchorRuntime { + fn box_sync_pose_sample(state: &XrState) -> Option { + let forward = flat_head_forward(state.head_pose.orientation); + let left_point = hand_closed_fist_contact_point(&state.left_hand, forward, true)?; + let right_point = hand_closed_fist_contact_point(&state.right_hand, forward, false)?; + let metrics = arm_pair_metrics(state.head_pose, left_point, right_point)?; + if metrics.left_lateral >= metrics.right_lateral + || metrics.hand_gap < SYNC_BOX_MIN_HAND_GAP_METERS + || metrics.hand_gap > SYNC_BOX_MAX_HAND_GAP_METERS + || metrics.left_forward < SYNC_BOX_MIN_CHEST_DISTANCE_METERS + || metrics.left_forward > SYNC_BOX_MAX_CHEST_DISTANCE_METERS + || metrics.right_forward < SYNC_BOX_MIN_CHEST_DISTANCE_METERS + || metrics.right_forward > SYNC_BOX_MAX_CHEST_DISTANCE_METERS + || metrics.left_elevation_degrees > SYNC_BOX_MAX_ARM_ELEVATION_DEGREES + || metrics.right_elevation_degrees > SYNC_BOX_MAX_ARM_ELEVATION_DEGREES + || (left_point.y - right_point.y).abs() > SYNC_BOX_MAX_VERTICAL_DELTA_METERS * 2.0 + || (metrics.left_forward - metrics.right_forward).abs() + > SYNC_BOX_MAX_DEPTH_DELTA_METERS * 2.0 + { + return None; + } + if !(SYNC_BOX_MIN_CHEST_DISTANCE_METERS..=SYNC_BOX_MAX_CHEST_DISTANCE_METERS) + .contains(&metrics.average_forward_distance) + { + return None; + } + Some(BoxSyncPoseSample { + anchor: XrAnchor { + left: left_point, + right: right_point, + }, + captured_at: state.time, + // Opposing-hand wiggles keep the midpoint almost fixed, so track the + // vertical split between hands instead of the average hand height. + vertical_split: left_point.y - right_point.y, + }) + } + + fn update_detector_sample( + &mut self, + sample: Option, + ) -> Option { + let Some(sample) = sample else { + self.detector.previous_sample = None; + self.detector.direction = None; + self.detector.extreme_sample = None; + return None; + }; + let Some(previous_sample) = self.detector.previous_sample else { + self.detector.previous_sample = Some(sample); + self.detector.extreme_sample = Some(sample); + return None; + }; + + match self.detector.direction { + None => { + let phase_delta = sample.vertical_split - previous_sample.vertical_split; + if phase_delta.abs() < SYNC_BOX_DIRECTION_DEADZONE_METERS { + return None; + } + let next_direction = if phase_delta > 0.0 { + BoxSyncMotionDirection::Rising + } else { + BoxSyncMotionDirection::Falling + }; + self.detector.direction = Some(next_direction); + self.detector.extreme_sample = + Some(Self::more_extreme_sample(previous_sample, sample, next_direction)); + self.detector.previous_sample = Some(sample); + None + } + Some(current_direction) => { + let extreme_sample = self.detector.extreme_sample.unwrap_or(previous_sample); + let continues_current_direction = match current_direction { + BoxSyncMotionDirection::Rising => { + sample.vertical_split >= extreme_sample.vertical_split + } + BoxSyncMotionDirection::Falling => { + sample.vertical_split <= extreme_sample.vertical_split + } + }; + + if continues_current_direction { + self.detector.extreme_sample = Some(Self::more_extreme_sample( + extreme_sample, + sample, + current_direction, + )); + self.detector.previous_sample = Some(sample); + return None; + } + + let retreat = (sample.vertical_split - extreme_sample.vertical_split).abs(); + if retreat < SYNC_BOX_REVERSAL_MIN_TRAVEL_METERS { + self.detector.previous_sample = Some(sample); + return None; + } + + let emitted = Some(XrSyncAnchor { + id: self.next_sync_anchor_id, + captured_at: extreme_sample.captured_at, + extrema: match current_direction { + BoxSyncMotionDirection::Rising => XrSyncAnchorExtrema::High, + BoxSyncMotionDirection::Falling => XrSyncAnchorExtrema::Low, + }, + anchor: extreme_sample.anchor, + }); + self.next_sync_anchor_id = self.next_sync_anchor_id.wrapping_add(1); + self.detector.direction = Some(match current_direction { + BoxSyncMotionDirection::Rising => BoxSyncMotionDirection::Falling, + BoxSyncMotionDirection::Falling => BoxSyncMotionDirection::Rising, + }); + self.detector.extreme_sample = Some(sample); + self.detector.previous_sample = Some(sample); + emitted + } + } + } + + fn more_extreme_sample( + a: BoxSyncPoseSample, + b: BoxSyncPoseSample, + direction: BoxSyncMotionDirection, + ) -> BoxSyncPoseSample { + match direction { + BoxSyncMotionDirection::Rising => { + if b.vertical_split >= a.vertical_split { + b + } else { + a + } + } + BoxSyncMotionDirection::Falling => { + if b.vertical_split <= a.vertical_split { + b + } else { + a + } + } + } + } + + fn update_detector(&mut self, state: &XrState) -> Option { + self.update_detector_sample(Self::box_sync_pose_sample(state)) + } + + fn current_sync_anchor(&mut self, state_time: f64) -> Option { + let (Some(sync_anchor), Some(emitted_at)) = ( + self.pending_sync_anchor, + self.pending_sync_anchor_emitted_at, + ) else { + return None; + }; + if state_time - emitted_at <= SYNC_BOX_BROADCAST_SECONDS { + Some(sync_anchor) + } else { + self.pending_sync_anchor = None; + self.pending_sync_anchor_emitted_at = None; + None + } + } + + fn augment_state(&mut self, state: &XrState) -> Rc { + if let Some(sync_anchor) = self.update_detector(state) { + self.pending_sync_anchor = Some(sync_anchor); + self.pending_sync_anchor_emitted_at = Some(state.time); + } + let mut augmented = state.clone(); + augmented.sync_anchor = self.current_sync_anchor(state.time); + Rc::new(augmented) + } +} + #[derive(Script, WidgetRef, WidgetSet, WidgetRegister)] pub struct XrRoot { #[uid] @@ -237,28 +553,62 @@ pub struct XrRoot { // State #[rust] - initialized: bool, - #[rust] - started: bool, - #[rust] - last_xr_state: Option>, - #[rust] - xr_content_pose: Option, - #[rust] - next_frame: NextFrame, + runtime: XrRootRuntime, #[rust] - desktop_ui_pointer_active: bool, - #[rust] - last_frame_update_cpu_ms: f64, + frame_metrics: XrRootFrameMetrics, #[rust] - last_frame_draw_cpu_ms: f64, + content_rig: XrContentRig, #[rust] - last_frame_cpu_ms: f64, + sync_runtime: XrSyncAnchorRuntime, } impl XrRoot { - fn update_frame_cpu_total(&mut self) { - self.last_frame_cpu_ms = self.last_frame_update_cpu_ms + self.last_frame_draw_cpu_ms; + fn reset_scene_physics_and_emit_action(&mut self, cx: &mut Cx) { + self.env.reset_physics(cx); + cx.widget_action(self.widget_uid(), XrRootAction::PhysicsReset); + } + + fn apply_content_pose(&mut self, cx: &mut Cx, pose: Pose) { + self.content_rig.pose = Some(pose); + self.env.set_root_pose(cx, Some(pose)); + self.env.reset_physics(cx); + cx.redraw_all(); + } + + fn reset_scene_pose_to_headset_and_emit_action(&mut self, cx: &mut Cx) { + let Some(state) = self.runtime.last_xr_state.as_deref() else { + return; + }; + let pose = XrContentRig::reset_pose_from_state(state); + self.apply_content_pose(cx, pose); + cx.widget_action(self.widget_uid(), XrRootAction::ContentPoseReset(pose)); + } + + pub fn spawn_body(&mut self, cx: &mut Cx, spawn: XrBodySpawn) { + self.env.spawn_body(cx, spawn); + } + + pub fn despawn_body(&mut self, cx: &mut Cx, widget_uid: WidgetUid) { + self.env.despawn_body(cx, widget_uid); + } + + pub fn apply_body_impulse(&mut self, cx: &mut Cx, impulse: XrBodyImpulse) { + self.env.apply_body_impulse(cx, impulse); + } + + pub fn set_content_pose(&mut self, cx: &mut Cx, pose: Pose) { + self.apply_content_pose(cx, pose); + } + + pub fn content_pose(&self) -> Option { + self.content_rig + .resolved_pose(self.runtime.last_xr_state.as_deref()) + } + + pub fn force_scene_rebuild(&mut self, cx: &mut Cx) { + self.env.mark_scene_dirty(); + self.env.ensure_physics(cx, &self.children); + cx.redraw_all(); } fn set_depth_mesh_visible(&mut self, cx: &mut Cx, visible: bool) -> bool { @@ -276,7 +626,7 @@ impl XrRoot { } fn set_depth_voxel_size(&mut self, cx: &mut Cx, voxel_size_meters: f32) -> f32 { - let voxel_size_meters = cx.xr_depth_mesh().set_voxel_size_meters(voxel_size_meters); + let voxel_size_meters = cx.xr_tsdf().set_voxel_size_meters(voxel_size_meters); self.env.reset_physics(cx); voxel_size_meters } @@ -288,10 +638,10 @@ impl XrRoot { } fn ensure_initialized(&mut self, cx: &mut Cx) { - if self.initialized { + if self.runtime.initialized { return; } - self.initialized = true; + self.runtime.initialized = true; self.window.handle.set_pass(cx, &self.pass.handle); self.pass.handle.set_pass_name(cx, "xr_root_window"); self.depth_texture = Texture::new_with_format( @@ -400,48 +750,16 @@ impl XrRoot { false } - fn xr_flat_forward(orientation: Quat) -> Vec3f { - let mut forward = orientation.rotate_vec3(&vec3f(0.0, 0.0, -1.0)); - forward.y = 0.0; - if forward.length() <= 1.0e-6 { - vec3f(0.0, 0.0, -1.0) - } else { - forward.normalize() - } - } - - fn xr_content_pose_from_state(state: &XrState) -> Pose { - let forward = Self::xr_flat_forward(state.head_pose.orientation); - Pose { - position: state.head_pose.position - + forward.scale(XR_CONTENT_FORWARD_OFFSET) - + vec3f(0.0, XR_CONTENT_VERTICAL_OFFSET, 0.0), - orientation: Quat::look_rotation(forward.scale(-1.0), vec3f(0.0, 1.0, 0.0)), - } - } - fn ensure_xr_content_pose(&mut self, cx: &mut Cx, state: &XrState) { - if self.xr_content_pose.is_some() { - return; - } - let pose = Self::xr_content_pose_from_state(state); - self.xr_content_pose = Some(pose); - self.env.set_root_pose(cx, Some(pose)); + self.content_rig.ensure_pose(cx, &mut self.env, state); } fn clear_xr_content_pose(&mut self, cx: &mut Cx) { - if self.xr_content_pose.is_none() { - return; - } - self.xr_content_pose = None; - self.env.set_root_pose(cx, None); + self.content_rig.clear_pose(cx, &mut self.env); } fn xr_content_transform(&self, state: Option<&XrState>) -> Mat4f { - self.xr_content_pose - .or_else(|| state.map(Self::xr_content_pose_from_state)) - .map(|pose| pose.to_mat4()) - .unwrap_or_else(Mat4f::identity) + self.content_rig.transform(state) } fn transform_point(transform: &Mat4f, point: Vec3f) -> Vec3f { @@ -453,6 +771,10 @@ impl XrRoot { } } + fn augment_xr_state(&mut self, state: &XrState) -> Rc { + self.sync_runtime.augment_state(state) + } + fn handle_desktop_xr_pointer(&mut self, cx: &mut Cx, event: &Event, scope: &mut Scope) -> bool { match event { Event::MouseDown(fe) if fe.button.is_primary() => { @@ -462,7 +784,7 @@ impl XrRoot { if !self.desktop_ui_hit(ray_origin, ray_dir) { return false; } - self.desktop_ui_pointer_active = true; + self.runtime.desktop_ui_pointer_active = true; self.dispatch_desktop_xr_local( cx, scope, @@ -476,7 +798,7 @@ impl XrRoot { ); true } - Event::MouseMove(fe) if self.desktop_ui_pointer_active => { + Event::MouseMove(fe) if self.runtime.desktop_ui_pointer_active => { let tip = self .desktop_pick_ray(fe.abs, fe.time) .map(|(ray_origin, ray_dir)| { @@ -485,18 +807,20 @@ impl XrRoot { self.dispatch_desktop_xr_local(cx, scope, fe.time, fe.modifiers, tip); true } - Event::MouseUp(fe) if fe.button.is_primary() && self.desktop_ui_pointer_active => { + Event::MouseUp(fe) + if fe.button.is_primary() && self.runtime.desktop_ui_pointer_active => + { let tip = self .desktop_pick_ray(fe.abs, fe.time) .map(|(ray_origin, ray_dir)| { Self::desktop_mouse_tip(ray_origin, ray_dir, DESKTOP_TOUCH_UP_Z) }); - self.desktop_ui_pointer_active = false; + self.runtime.desktop_ui_pointer_active = false; self.dispatch_desktop_xr_local(cx, scope, fe.time, fe.modifiers, tip); true } - Event::MouseLeave(fe) if self.desktop_ui_pointer_active => { - self.desktop_ui_pointer_active = false; + Event::MouseLeave(fe) if self.runtime.desktop_ui_pointer_active => { + self.runtime.desktop_ui_pointer_active = false; self.dispatch_desktop_xr_local(cx, scope, fe.time, fe.modifiers, None); true } @@ -504,75 +828,10 @@ impl XrRoot { } } - fn child_world_sort_center(child: &WidgetRef) -> Option { - if let Some(select) = child.borrow::() { - return Some(select.node().pos()); - } - if let Some(view) = child.borrow::() { - return Some(view.node().pos()); - } - if let Some(cube) = child.borrow::() { - return Some(cube.node().pos()); - } - if let Some(ico) = child.borrow::() { - return Some(ico.node().pos()); - } - if let Some(refractive_cube) = child.borrow::() { - return Some(refractive_cube.node().pos()); - } - if let Some(gltf) = child.borrow::() { - return Some(gltf.node().pos()); - } - if let Some(tree) = child.borrow::() { - return Some(tree.node().pos()); - } - if let Some(node) = child.borrow::() { - return Some(node.pos()); - } - None - } - - fn child_is_transparent(child: &WidgetRef) -> bool { - child.borrow::().is_some() || child.borrow::().is_some() - } - - fn draw_list_depth(scene_state: &SceneState3D, world_pos: Vec3f) -> f32 { - let view_pos = - scene_state - .view - .transform_vec4(vec4f(world_pos.x, world_pos.y, world_pos.z, 1.0)); - if view_pos.w.abs() > 1.0e-6 { - view_pos.z / view_pos.w - } else { - view_pos.z - } - } - - fn sort_child_draw_order(draw_order_entries: &mut [(usize, f32, bool)]) { - if draw_order_entries.len() <= 1 { - return; - } - - draw_order_entries.sort_by(|a, b| match (a.2, b.2) { - (false, true) => Ordering::Less, - (true, false) => Ordering::Greater, - (false, false) => { - b.1.partial_cmp(&a.1) - .unwrap_or(Ordering::Equal) - .then_with(|| a.0.cmp(&b.0)) - } - (true, true) => { - a.1.partial_cmp(&b.1) - .unwrap_or(Ordering::Equal) - .then_with(|| a.0.cmp(&b.0)) - } - }); - } - fn draw_3d_content(&mut self, cx: &mut Cx3d, _scope: &mut Scope, scene_state: SceneState3D) { self.draw_list.begin_always(cx); let root_transform = if cx.cx.in_xr_mode() { - self.xr_content_transform(self.last_xr_state.as_deref()) + self.xr_content_transform(self.runtime.last_xr_state.as_deref()) } else { Mat4f::identity() }; @@ -591,20 +850,20 @@ impl XrRoot { let mut draw_order_entries = Vec::new(); for i in 0..self.children.len() { let child = self.children[i].1.clone(); - let child_center = Self::child_world_sort_center(&child) + let child_center = xr_widget_local_sort_center(&child) .map(|center| Self::transform_point(&root_transform, center)); if let Some(child_center) = child_center { draw_order_entries.push(( i, - Self::draw_list_depth(&scene_state, child_center), - Self::child_is_transparent(&child), + xr_draw_list_depth(&scene_state, child_center), + xr_widget_is_transparent(&child), )); } else { draw_order_entries.push((i, 0.0, false)); } } - Self::sort_child_draw_order(&mut draw_order_entries); + xr_sort_child_draw_order(&mut draw_order_entries); for (index, _, _) in draw_order_entries { let child = self.children[index].1.clone(); child.draw_3d_all(cx, &mut scene_scope); @@ -635,10 +894,17 @@ impl XrRoot { }, ); if cx.in_xr_mode() { - if let Some(xr_state) = e.xr_state.as_ref() { - self.last_xr_state = Some(xr_state.clone()); + if self.runtime.last_xr_state.is_none() { + if let Some(xr_state) = e.xr_state.as_ref() { + self.runtime.last_xr_state = Some(xr_state.clone()); + } } - let Some(xr_state) = self.last_xr_state.as_ref() else { + let Some(xr_state) = self + .runtime + .last_xr_state + .as_ref() + .or_else(|| e.xr_state.as_ref()) + else { return; }; let mut cx_draw = CxDraw::new(cx, e); @@ -652,8 +918,7 @@ impl XrRoot { let cx2d = &mut Cx2d::new(&mut cx_draw); self.draw_all(cx2d, scope); } - self.last_frame_draw_cpu_ms = started.elapsed().as_secs_f64() * 1000.0; - self.update_frame_cpu_total(); + self.frame_metrics.finish_draw(started); } pub fn depth_mesh_visible(&self) -> bool { @@ -680,36 +945,52 @@ impl XrRoot { self.env.physics_compute_ms() } - pub fn physics_time_scale(&self) -> f32 { - self.env.physics_time_scale() + pub fn physics_tsdf_query_ms(&self) -> f64 { + self.env.physics_tsdf_query_ms() + } + + pub fn physics_rapier_step_ms(&self) -> f64 { + self.env.physics_rapier_step_ms() } - pub fn physics_step_dt_ms(&self) -> f64 { - self.env.physics_step_dt_ms() + pub fn physics_time_scale(&self) -> f32 { + self.env.physics_time_scale() } pub fn physics_depth_query_surface_count(&self) -> usize { self.env.physics_depth_query_surface_count() } - pub fn physics_depth_query_vertex_count(&self) -> usize { - self.env.physics_depth_query_vertex_count() + pub fn physics_scene_body_count(&self) -> usize { + self.env.physics_scene_body_count() + } + + pub fn physics_body_spawn_apply_count(&self) -> usize { + self.env.physics_body_spawn_apply_count() } - pub fn physics_depth_query_triangle_count(&self) -> usize { - self.env.physics_depth_query_triangle_count() + pub fn physics_body_spawn_miss_count(&self) -> usize { + self.env.physics_body_spawn_miss_count() + } + + pub fn physics_revision(&self) -> u64 { + self.env.physics_revision() + } + + pub fn runtime_bodies(&self) -> Rc> { + self.env.runtime_bodies() } pub fn frame_cpu_ms(&self) -> f64 { - self.last_frame_cpu_ms + self.frame_metrics.last_frame_cpu_ms } pub fn frame_update_cpu_ms(&self) -> f64 { - self.last_frame_update_cpu_ms + self.frame_metrics.last_frame_update_cpu_ms } pub fn frame_draw_cpu_ms(&self) -> f64 { - self.last_frame_draw_cpu_ms + self.frame_metrics.last_frame_draw_cpu_ms } } @@ -841,7 +1122,7 @@ impl Widget for XrRoot { )); } if method == live_id!(set_depth_voxel_size) || method == live_id!(set_depth_resolution) { - let mut voxel_size_meters = vm.cx().xr_depth_mesh().voxel_size_meters(); + let mut voxel_size_meters = vm.cx().xr_tsdf().voxel_size_meters(); if let Some(args_obj) = args.as_object() { let trap = vm.bx.threads.cur().trap.pass(); if let Some(value) = vm.bx.heap.vec_value(args_obj, 0, trap).as_f64() { @@ -855,7 +1136,7 @@ impl Widget for XrRoot { } if method == live_id!(depth_voxel_size) || method == live_id!(depth_resolution) { return ScriptAsyncResult::Return(ScriptValue::from_f64( - vm.cx().xr_depth_mesh().voxel_size_meters() as f64, + vm.cx().xr_tsdf().voxel_size_meters() as f64, )); } if method == live_id!(set_render_scale) || method == live_id!(set_xr_render_scale) { @@ -881,7 +1162,13 @@ impl Widget for XrRoot { } if method == live_id!(reset_physics) || method == live_id!(reset_scene_physics) { vm.with_cx_mut(|cx| { - self.env.reset_physics(cx); + self.reset_scene_physics_and_emit_action(cx); + }); + return ScriptAsyncResult::Return(NIL); + } + if method == live_id!(reset_activity_pose) || method == live_id!(pose_activity) { + vm.with_cx_mut(|cx| { + self.reset_scene_pose_to_headset_and_emit_action(cx); }); return ScriptAsyncResult::Return(NIL); } @@ -920,12 +1207,20 @@ impl Widget for XrRoot { fn handle_event(&mut self, cx: &mut Cx, event: &Event, scope: &mut Scope) { if let Event::Draw(e) = event { + if !cx.in_xr_mode() { + for i in 0..self.children.len() { + let child = self.children[i].1.clone(); + if child.borrow::().is_some() { + child.handle_event(cx, event, scope); + } + } + } self.handle_draw_event(cx, e, scope); return; } - let measure_frame_cpu = - matches!(event, Event::XrUpdate(_)) || self.next_frame.is_event(event).is_some(); + let measure_frame_cpu = matches!(event, Event::XrUpdate(_)) + || self.runtime.next_frame.is_event(event).is_some(); let started = measure_frame_cpu.then(Instant::now); if !cx.in_xr_mode() { @@ -936,8 +1231,8 @@ impl Widget for XrRoot { match event { Event::Startup => { - if !self.started { - self.started = true; + if !self.runtime.started { + self.runtime.started = true; cx.widget_to_script_call( self.uid, NIL, @@ -952,25 +1247,47 @@ impl Widget for XrRoot { } }); self.env.ensure_physics(cx, &self.children); - self.next_frame = cx.new_next_frame(); + self.runtime.next_frame = cx.new_next_frame(); cx.redraw_all(); } } Event::XrUpdate(update) => { - self.ensure_xr_content_pose(cx, &update.state); - self.last_xr_state = Some(update.state.clone()); - if update.clicked_menu() { - self.env.reset_physics(cx); + let augmented_state = self.augment_xr_state(update.state.as_ref()); + let last = self + .runtime + .last_dispatched_xr_state + .clone() + .unwrap_or_else(|| augmented_state.clone()); + let augmented_update = XrUpdateEvent { + state: augmented_state.clone(), + last, + }; + self.runtime.last_dispatched_xr_state = Some(augmented_state.clone()); + if cx.in_xr_mode() { + self.ensure_xr_content_pose(cx, &augmented_state); + } + self.runtime.last_xr_state = Some(augmented_state.clone()); + if augmented_update.clicked_menu() { + self.reset_scene_physics_and_emit_action(cx); } self.env.ensure_physics(cx, &self.children); self.env.step_physics(cx); + let mut event_scope_data = super::xr_view::XrViewEventScopeData { + content_transform: self.xr_content_transform(Some(&augmented_update.state)), + }; + let mut event_scope = Scope::with_data(&mut event_scope_data); + let augmented_event = Event::XrUpdate(augmented_update); + for i in 0..self.children.len() { + let child = self.children[i].1.clone(); + child.handle_event(cx, &augmented_event, &mut event_scope); + } } - Event::NextFrame(_) if self.next_frame.is_event(event).is_some() => { + Event::NextFrame(_) if self.runtime.next_frame.is_event(event).is_some() => { if !cx.in_xr_mode() { self.env.ensure_physics(cx, &self.children); self.env.step_physics(cx); } - self.next_frame = cx.new_next_frame(); + self.runtime.next_frame = cx.new_next_frame(); } _ => {} } @@ -980,29 +1297,30 @@ impl Widget for XrRoot { let Some(widget_action) = action.as_widget_action() else { continue; }; - let Some(body_spawn) = widget_action.action.downcast_ref::() else { - continue; - }; - self.env.spawn_body(cx, *body_spawn); + if let Some(impulse) = widget_action.action.downcast_ref::() { + self.env.apply_body_impulse(cx, *impulse); + } } if actions.iter().any(|action| { action.as_widget_action().is_some_and(|action| { - matches!( - action.cast::(), - XrSelectAction::ActiveChildChanged(_) - ) + matches!(action.cast::(), XrNodeAction::SceneChanged) + || matches!( + action.cast::(), + XrSelectAction::ActiveChildChanged(_) + ) }) }) { self.env.mark_scene_dirty(); self.env.ensure_physics(cx, &self.children); } + self.env.flush_pending_physics_commands(cx); } let desktop_scene_interaction = !cx.in_xr_mode() && !self.permissions_ui_visible(); let handled_desktop_xr_pointer = if desktop_scene_interaction { self.handle_desktop_xr_pointer(cx, event, scope) } else { - self.desktop_ui_pointer_active = false; + self.runtime.desktop_ui_pointer_active = false; false }; let swallow_desktop_pointer_event = desktop_scene_interaction @@ -1016,15 +1334,7 @@ impl Widget for XrRoot { ); if swallow_desktop_pointer_event { - } else if let Event::XrUpdate(update) = event { - let mut event_scope_data = crate::xr_view::XrViewEventScopeData { - content_transform: self.xr_content_transform(Some(&update.state)), - }; - let mut event_scope = Scope::with_data(&mut event_scope_data); - for i in 0..self.children.len() { - let child = self.children[i].1.clone(); - child.handle_event(cx, event, &mut event_scope); - } + } else if matches!(event, Event::XrUpdate(_)) { } else { for i in 0..self.children.len() { let child = self.children[i].1.clone(); @@ -1037,8 +1347,7 @@ impl Widget for XrRoot { } if let Some(started) = started { - self.last_frame_update_cpu_ms = started.elapsed().as_secs_f64() * 1000.0; - self.update_frame_cpu_total(); + self.frame_metrics.finish_update(started); } } @@ -1078,3 +1387,6 @@ impl Widget for XrRoot { DrawStep::done() } } + +#[cfg(test)] +include!("../tests/scene/xr_root.rs"); diff --git a/xr/src/scene/xr_scene.rs b/xr/src/scene/xr_scene.rs deleted file mode 100644 index 005c03870b..0000000000 --- a/xr/src/scene/xr_scene.rs +++ /dev/null @@ -1,374 +0,0 @@ -use crate::{ - IcoSphere, - xr_env::{makepad_pose, RapierScene}, - xr_node::{XrBodyKind, XrNode, XrRuntimeBodyState}, -}; -use makepad_widgets::*; -use std::{collections::HashMap, rc::Rc}; - -script_mod! { - use mod.prelude.widgets.* - use mod.widgets.* - - mod.widgets.XrPhysics = set_type_default() do #(XrPhysics::script_component(vm)) - mod.widgets.XrSceneBase = #(XrScene::register_widget(vm)) - - mod.widgets.XrScene = set_type_default() do mod.widgets.XrSceneBase{ - width: Fill - height: Fill - physics: mod.widgets.XrPhysics{} - } -} - -#[derive(Script, ScriptHook, Clone, Copy)] -pub struct XrPhysics { - #[live(9.81)] - pub gravity: f32, -} - -impl Default for XrPhysics { - fn default() -> Self { - Self { gravity: 9.81 } - } -} - -#[derive(Clone, Copy)] -struct XrTransformState { - position: Vec3f, - orientation: Quat, - scale: Vec3f, -} - -impl Default for XrTransformState { - fn default() -> Self { - Self { - position: vec3f(0.0, 0.0, 0.0), - orientation: Quat::default(), - scale: vec3f(1.0, 1.0, 1.0), - } - } -} - -#[derive(Clone, Copy)] -struct CollectedXrCube { - uid: WidgetUid, - body_kind: XrBodyKind, - pose: Pose, - scale: Vec3f, - half_extents: Vec3f, - is_sphere: bool, - density: f32, - friction: f32, - restitution: f32, -} - -#[derive(Script, ScriptHook, Widget)] -pub struct XrScene { - #[live] - physics: XrPhysics, - #[rust] - scene: Option, - #[rust] - runtime_bodies: Rc>, - #[rust(true)] - scene_dirty: bool, - #[rust] - next_frame: NextFrame, - #[rust] - scene_root_pose: Option, - #[cast] - #[deref] - node: XrNode, -} - -impl XrScene { - pub fn reset_requested(update: &XrUpdateEvent) -> bool { - update.clicked_menu() - } - - fn reset_scene(&mut self, cx: &mut Cx) { - self.scene = None; - Rc::make_mut(&mut self.runtime_bodies).clear(); - self.scene_dirty = true; - self.redraw(cx); - } - - fn should_preview_step(&self) -> bool { - self.scene - .as_ref() - .map(|scene| { - scene - .cubes - .iter() - .any(|cube| matches!(cube.body_kind, XrBodyKind::Dynamic)) - }) - .unwrap_or(false) - } - - fn scene_root_transform_state(&self) -> XrTransformState { - if let Some(pose) = self.scene_root_pose { - XrTransformState { - position: pose.position, - orientation: pose.orientation, - scale: vec3f(1.0, 1.0, 1.0), - } - } else { - XrTransformState::default() - } - } - - fn rotation_quat(rot: Vec3f) -> Quat { - let x = Quat::from_axis_angle(vec3f(1.0, 0.0, 0.0), rot.x); - let y = Quat::from_axis_angle(vec3f(0.0, 1.0, 0.0), rot.y); - let z = Quat::from_axis_angle(vec3f(0.0, 0.0, 1.0), rot.z); - Quat::multiply(&z, &Quat::multiply(&y, &x)) - } - - fn transform_with_node(parent: XrTransformState, node: &XrNode) -> XrTransformState { - let local_pos = vec3f( - node.pos().x * parent.scale.x, - node.pos().y * parent.scale.y, - node.pos().z * parent.scale.z, - ); - let rotated_pos = parent.orientation.rotate_vec3(&local_pos); - let orientation = Quat::multiply(&Self::rotation_quat(node.rot()), &parent.orientation); - XrTransformState { - position: parent.position + rotated_pos, - orientation, - scale: vec3f( - parent.scale.x * node.scale().x, - parent.scale.y * node.scale().y, - parent.scale.z * node.scale().z, - ), - } - } - - fn collect_cubes_from_widget( - widget: &WidgetRef, - parent: XrTransformState, - cubes: &mut Vec, - ) { - let Some(node) = widget.cast_inner::() else { - widget.children(&mut |_, child| Self::collect_cubes_from_widget(&child, parent, cubes)); - return; - }; - - let is_sphere = widget.borrow::().is_some(); - let world = Self::transform_with_node(parent, &node); - let half_extents = node.physics_half_extents(); - let should_push = node.body_kind() != XrBodyKind::Disabled - && (half_extents.x > 0.0 || half_extents.y > 0.0 || half_extents.z > 0.0); - - if should_push { - cubes.push(CollectedXrCube { - uid: widget.widget_uid(), - body_kind: node.body_kind(), - pose: Pose::new(world.orientation, world.position), - scale: world.scale, - half_extents: vec3f( - half_extents.x * world.scale.x, - half_extents.y * world.scale.y, - half_extents.z * world.scale.z, - ), - is_sphere, - density: node.density(), - friction: node.friction(), - restitution: node.restitution(), - }); - } - - drop(node); - widget.children(&mut |_, child| Self::collect_cubes_from_widget(&child, world, cubes)); - } - - fn collect_rendered_cubes(&self) -> Vec { - let mut cubes = Vec::new(); - let root = self.scene_root_transform_state(); - self.node - .children(&mut |_, child| Self::collect_cubes_from_widget(&child, root, &mut cubes)); - cubes - } - - fn sync_runtime_bodies(&mut self) { - let runtime_bodies = Rc::make_mut(&mut self.runtime_bodies); - runtime_bodies.clear(); - let Some(scene) = self.scene.as_ref() else { - return; - }; - for cube in &scene.cubes { - if let Some(body) = scene.bodies.get(cube.body) { - runtime_bodies.insert( - cube.widget_uid, - XrRuntimeBodyState { - pose: makepad_pose(body.position()), - scale: cube.scale, - }, - ); - } - } - } - - fn rebuild_runtime_scene(&mut self, cx: &mut Cx) { - let cubes = self.collect_rendered_cubes(); - let dynamic_count = cubes - .iter() - .filter(|cube| matches!(cube.body_kind, XrBodyKind::Dynamic)) - .count(); - let fixed_count = cubes - .iter() - .filter(|cube| matches!(cube.body_kind, XrBodyKind::Fixed)) - .count(); - crate::log!( - "XrScene rebuild_runtime_scene cubes={} dynamic={} fixed={} child_count={}", - cubes.len(), - dynamic_count, - fixed_count, - self.node.child_count() - ); - let mut scene = RapierScene::new(self.physics.gravity); - for cube in cubes { - match cube.body_kind { - XrBodyKind::Disabled => {} - XrBodyKind::Dynamic => { - if cube.is_sphere { - scene.spawn_dynamic_sphere( - cube.uid, - cube.pose, - cube.half_extents.x.min(cube.half_extents.y).min(cube.half_extents.z), - cube.scale, - cube.density, - cube.friction, - cube.restitution, - ); - } else { - scene.spawn_dynamic_box( - cube.uid, - cube.pose, - cube.half_extents, - cube.scale, - cube.density, - cube.friction, - cube.restitution, - ); - } - } - XrBodyKind::Fixed => { - if cube.is_sphere { - scene.spawn_fixed_sphere( - cube.uid, - cube.pose, - cube.half_extents.x.min(cube.half_extents.y).min(cube.half_extents.z), - cube.scale, - cube.friction, - cube.restitution, - ); - } else { - scene.spawn_fixed_box( - cube.uid, - cube.pose, - cube.half_extents, - cube.scale, - cube.friction, - cube.restitution, - ); - } - } - } - } - self.scene = Some(scene); - self.scene_dirty = false; - self.sync_runtime_bodies(); - self.redraw(cx); - } - - pub(crate) fn ensure_runtime_scene(&mut self, cx: &mut Cx) { - if self.scene_dirty || self.scene.is_none() { - self.rebuild_runtime_scene(cx); - } - } - - pub(crate) fn runtime_scene_mut(&mut self) -> Option<&mut RapierScene> { - self.scene.as_mut() - } - - pub(crate) fn runtime_scene_ref(&self) -> Option<&RapierScene> { - self.scene.as_ref() - } - - pub(crate) fn runtime_bodies_clone(&self) -> Rc> { - self.runtime_bodies.clone() - } - - pub(crate) fn set_root_pose(&mut self, cx: &mut Cx, pose: Option) { - if self.scene_root_pose == pose { - return; - } - self.scene_root_pose = pose; - self.scene_dirty = true; - self.redraw(cx); - } -} - -impl Widget for XrScene { - fn script_call( - &mut self, - vm: &mut ScriptVm, - method: LiveId, - args: ScriptValue, - ) -> ScriptAsyncResult { - if method == live_id!(render) || method == live_id!(render_scene) { - self.scene_dirty = true; - return self.node.script_call(vm, live_id!(render), args); - } - ScriptAsyncResult::MethodNotFound - } - - fn script_result(&mut self, vm: &mut ScriptVm, id: ScriptAsyncId, result: ScriptValue) { - self.node.script_result(vm, id, result); - self.scene_dirty = true; - vm.with_cx_mut(|cx| self.ensure_runtime_scene(cx)); - } - - fn handle_event(&mut self, cx: &mut Cx, event: &Event, scope: &mut Scope) { - self.node.handle_event(cx, event, scope); - - match event { - Event::Startup => { - cx.with_vm(|vm| { - let _ = self.node.script_call(vm, live_id!(render), NIL); - }); - self.next_frame = cx.new_next_frame(); - self.ensure_runtime_scene(cx); - } - Event::NextFrame(ne) if self.next_frame.is_event(event).is_some() => { - if !cx.in_xr_mode() && self.should_preview_step() { - if let Some(scene) = self.scene.as_mut() { - scene.step(); - } - self.sync_runtime_bodies(); - self.redraw(cx); - } - self.next_frame = cx.new_next_frame(); - let _ = ne; - } - Event::XrUpdate(update) => { - if Self::reset_requested(update) { - self.reset_scene(cx); - } - self.ensure_runtime_scene(cx); - if let Some(scene) = self.scene.as_mut() { - scene.step(); - } - self.sync_runtime_bodies(); - self.redraw(cx); - } - _ => {} - } - } - - fn draw_3d(&mut self, cx: &mut Cx3d, scope: &mut Scope) -> DrawStep { - self.ensure_runtime_scene(cx.cx); - self.node.draw_3d_all(cx, scope); - DrawStep::done() - } -} diff --git a/xr/src/scene/xr_select.rs b/xr/src/scene/xr_select.rs index f6df747e99..c04816275e 100644 --- a/xr/src/scene/xr_select.rs +++ b/xr/src/scene/xr_select.rs @@ -1,5 +1,5 @@ -use crate::xr_node::{xr_widget_world_transform, XrNode}; -use crate::*; +use super::xr_node::{xr_widget_world_transform, XrNode}; +use crate::prelude::*; script_mod! { use mod.prelude.widgets_internal.* @@ -11,7 +11,7 @@ script_mod! { #[derive(Clone, Debug, Default)] pub enum XrSelectAction { - ActiveChildChanged(LiveId), + ActiveChildChanged(XrActivityId), #[default] None, } @@ -41,6 +41,14 @@ impl XrSelect { &self.node } + pub fn active_child_id(&self) -> LiveId { + self.active_child + } + + pub fn activity_id(&self) -> XrActivityId { + XrActivityId(self.active_child) + } + fn first_child_id(&self) -> Option { self.child_order .iter() @@ -79,7 +87,10 @@ impl XrSelect { self.active_child = child_id; self.sync_child_visibility(cx); cx.with_vm(|vm| self.render_child(vm, child_id)); - cx.widget_action(self.uid, XrSelectAction::ActiveChildChanged(child_id)); + cx.widget_action( + self.uid, + XrSelectAction::ActiveChildChanged(XrActivityId(child_id)), + ); self.redraw(cx); } else { child.set_visible(cx, true); @@ -87,6 +98,10 @@ impl XrSelect { Some(child) } + pub fn set_activity(&mut self, cx: &mut Cx, activity_id: XrActivityId) -> Option { + self.set_active_child(cx, activity_id.to_live_id()) + } + fn active_child_widget(&mut self, cx: &mut Cx) -> Option<(LiveId, WidgetRef)> { let active_child = self.ensure_active_child(cx)?; self.children @@ -295,4 +310,14 @@ impl XrSelectRef { let mut inner = self.borrow_mut()?; inner.set_active_child(cx, child_id) } + + pub fn activity_id(&self) -> Option { + let inner = self.borrow()?; + Some(inner.activity_id()) + } + + pub fn set_activity(&self, cx: &mut Cx, activity_id: XrActivityId) -> Option { + let mut inner = self.borrow_mut()?; + inner.set_activity(cx, activity_id) + } } diff --git a/xr/src/scene/xr_shared_object_sync.rs b/xr/src/scene/xr_shared_object_sync.rs new file mode 100644 index 0000000000..0d21a2fbd8 --- /dev/null +++ b/xr/src/scene/xr_shared_object_sync.rs @@ -0,0 +1,1549 @@ +use super::{xr_widget_children, xr_widget_with_scene_node, XrBodyKind}; +use crate::prelude::*; +use std::collections::{HashMap, VecDeque}; + +const XR_SHARED_OBJECT_HASH_OFFSET: u64 = 0xcbf29ce484222325; +const XR_SHARED_OBJECT_HASH_PRIME: u64 = 0x00000100000001b3; +const XR_SHARED_OBJECT_ROOT_TAG: u64 = 0x78725f7368617265; +const XR_SHARED_OBJECT_CHILD_TAG: u64 = 0x6368696c645f7061; +const XR_SHARED_OBJECT_INDEX_TAG: u64 = 0x696e6465785f7061; +const XR_SHARED_OBJECT_BODY_TAG: u64 = 0x626f64795f737061; +const XR_SHARED_OBJECT_POOL_GROUP_TAG: u64 = 0x706f6f6c5f677270; +const XR_SHARED_OBJECT_HISTORY_MAX_SAMPLES: usize = 48; +const XR_SHARED_OBJECT_HISTORY_MAX_SECONDS: f64 = 0.5; +const XR_SHARED_OBJECT_TAKEOVER_REQUEST_COOLDOWN_SECONDS: f64 = 0.18; +const XR_SHARED_OBJECT_CONTACT_IMPULSE_COOLDOWN_SECONDS: f64 = 0.08; +const XR_SHARED_OBJECT_SLEEPING_PUBLISH_INTERVAL_SECONDS: f64 = 0.25; +const XR_SHARED_OBJECT_PUBLISH_POSITION_EPSILON_METERS: f32 = 0.001; +const XR_SHARED_OBJECT_PUBLISH_ORIENTATION_EPSILON_DEGREES: f32 = 0.25; +const XR_SHARED_OBJECT_PUBLISH_LINVEL_EPSILON_MPS: f32 = 0.02; +const XR_SHARED_OBJECT_PUBLISH_ANGVEL_EPSILON_RADPS: f32 = 0.02; + +#[derive(Clone, Copy, Debug, PartialEq, Eq)] +pub struct XrSpawnableObjectBinding { + pub object_id: XrSpawnableObjectId, + pub allocation_group_id: XrSpawnableObjectId, + pub widget_uid: WidgetUid, + pub bootstrap_shared: bool, +} + +#[derive(Clone, Copy, Debug, PartialEq, Eq)] +pub struct XrLocalSharedObjectAllocation { + pub shared_object_id: XrSharedObjectId, + pub spawnable_object_id: XrSpawnableObjectId, + pub widget_uid: WidgetUid, + pub epoch: u32, + pub fidelity: XrSharedObjectFidelity, +} + +#[derive(Clone, Copy, Debug, PartialEq)] +pub struct XrLocalSharedObjectSnapshot { + pub object_id: XrSharedObjectId, + pub spawnable_object_id: XrSpawnableObjectId, + pub widget_uid: WidgetUid, + pub epoch: u32, + pub authority: XrNetPeerId, + pub fidelity: XrSharedObjectFidelity, + pub latest_state: Option, +} + +#[derive(Clone, Copy, Debug, PartialEq)] +pub struct XrRemoteSharedObjectSnapshot { + pub object_id: XrSharedObjectId, + pub spawnable_object_id: XrSpawnableObjectId, + pub widget_uid: WidgetUid, + pub epoch: u32, + pub authority: XrNetPeerId, + pub state_source_authority: XrNetPeerId, + pub fidelity: XrSharedObjectFidelity, + pub latest_state: Option, + pub pending_takeover_request_id: Option, +} + +#[derive(Clone, Copy, Debug, PartialEq)] +pub struct XrScheduledAuthorityTransfer { + pub object_id: XrSharedObjectId, + pub widget_uid: WidgetUid, + pub shadow: bool, + pub source_authority: XrNetPeerId, + pub new_authority: XrNetPeerId, + pub epoch: u32, + pub request_id: u32, + pub hand: Option, + pub pose: Pose, + pub linvel: Vec3f, + pub angvel: Vec3f, +} + +#[derive(Clone, Copy, Debug, PartialEq)] +struct XrPendingAuthorityTransfer { + epoch: u32, + source_authority: XrNetPeerId, + new_authority: XrNetPeerId, + effective_at: f64, + effective_tick: u32, + request_id: u32, + hand: Option, + pose: Pose, + linvel: Vec3f, + angvel: Vec3f, +} + +#[derive(Clone, Debug, PartialEq)] +struct XrLocalSharedObjectRecord { + spawnable_object_id: XrSpawnableObjectId, + widget_uid: WidgetUid, + epoch: u32, + authority: XrNetPeerId, + fidelity: XrSharedObjectFidelity, + activated_at_local_time: f64, + latest_state: Option, + last_published_state: Option, + last_published_at: Option, + history: VecDeque, + pending_transfer: Option, +} + +#[derive(Clone, Debug, PartialEq)] +struct XrRemoteSharedObjectRecord { + spawnable_object_id: XrSpawnableObjectId, + widget_uid: WidgetUid, + epoch: u32, + authority: XrNetPeerId, + state_source_authority: XrNetPeerId, + fidelity: XrSharedObjectFidelity, + activated_at_local_time: f64, + latest_state: Option, + history: VecDeque, + pending_takeover_request_id: Option, + last_takeover_request_at: Option, + last_contact_impulse_at: Option, + pending_transfer: Option, +} + +#[derive(Clone, Debug, Default)] +pub struct XrSharedObjectRegistry { + activity_id: Option, + local_peer_id: Option, + next_shared_object_counter: XrSharedObjectCounter, + object_to_widget: HashMap, + object_to_group: HashMap, + object_bootstrap_shared: HashMap, + widget_to_object: HashMap, + widget_bootstrap_shared: HashMap, + group_to_widgets: HashMap>, + local_objects: HashMap, + local_shared_object_to_widget: HashMap, + widget_to_local_shared_object: HashMap, + remote_objects: HashMap, + remote_object_to_widget: HashMap, + remote_widget_to_object: HashMap, +} + +impl XrSharedObjectRegistry { + fn release_remote_widget_claim(&mut self, widget_uid: WidgetUid) { + if let Some(object_id) = self.remote_widget_to_object.remove(&widget_uid) { + self.remote_object_to_widget.remove(&object_id); + self.remote_objects.remove(&object_id); + } + } + + fn clear_active_shared_objects(&mut self) { + self.local_objects.clear(); + self.local_shared_object_to_widget.clear(); + self.widget_to_local_shared_object.clear(); + self.remote_objects.clear(); + self.remote_object_to_widget.clear(); + self.remote_widget_to_object.clear(); + } + + fn rebuild_spawnable_bindings(&mut self, bindings: I) + where + I: IntoIterator, + { + self.object_to_widget.clear(); + self.object_to_group.clear(); + self.object_bootstrap_shared.clear(); + self.widget_to_object.clear(); + self.widget_bootstrap_shared.clear(); + self.group_to_widgets.clear(); + for binding in bindings { + self.object_to_widget + .insert(binding.object_id, binding.widget_uid); + self.object_to_group + .insert(binding.object_id, binding.allocation_group_id); + self.object_bootstrap_shared + .insert(binding.object_id, binding.bootstrap_shared); + self.widget_to_object + .insert(binding.widget_uid, binding.object_id); + self.widget_bootstrap_shared + .insert(binding.widget_uid, binding.bootstrap_shared); + self.group_to_widgets + .entry(binding.allocation_group_id) + .or_default() + .push(binding.widget_uid); + } + } + + fn remap_local_shared_objects(&mut self) { + let local_objects = std::mem::take(&mut self.local_objects); + self.local_shared_object_to_widget.clear(); + self.widget_to_local_shared_object.clear(); + for (object_id, mut record) in local_objects { + let Some(&widget_uid) = self.object_to_widget.get(&record.spawnable_object_id) else { + continue; + }; + record.widget_uid = widget_uid; + self.widget_to_local_shared_object + .insert(widget_uid, object_id); + self.local_shared_object_to_widget + .insert(object_id, widget_uid); + self.local_objects.insert(object_id, record); + } + } + + fn remap_remote_shared_objects(&mut self, activity_id: XrActivityId) { + let mut remote_objects = std::mem::take(&mut self.remote_objects); + let mut object_ids = remote_objects.keys().copied().collect::>(); + object_ids.sort_by_key(|object_id| object_id.0); + self.remote_object_to_widget.clear(); + self.remote_widget_to_object.clear(); + for object_id in object_ids { + let Some(mut record) = remote_objects.remove(&object_id) else { + continue; + }; + let Some(widget_uid) = + self.resolve_remote_widget_uid(activity_id, object_id, record.spawnable_object_id) + else { + continue; + }; + record.widget_uid = widget_uid; + self.remote_objects.insert(object_id, record); + } + } + + fn remote_state_sender_matches( + record: &XrRemoteSharedObjectRecord, + sender_authority: XrNetPeerId, + state: XrNetSharedObjectState, + ) -> bool { + sender_authority == record.authority + && state.authority == record.authority + && state.epoch == record.epoch + } + + pub fn activity_id(&self) -> Option { + self.activity_id + } + + pub fn len(&self) -> usize { + self.object_to_widget.len() + } + + pub fn active_count(&self) -> usize { + self.local_objects.len() + self.remote_object_to_widget.len() + } + + pub fn local_peer_id(&self) -> Option { + self.local_peer_id + } + + pub fn set_local_peer_id(&mut self, peer_id: XrNetPeerId) { + self.local_peer_id = Some(peer_id); + } + + pub fn clear(&mut self) { + self.activity_id = None; + self.object_to_widget.clear(); + self.object_to_group.clear(); + self.object_bootstrap_shared.clear(); + self.widget_to_object.clear(); + self.widget_bootstrap_shared.clear(); + self.group_to_widgets.clear(); + self.clear_active_shared_objects(); + } + + pub fn replace_spawnables(&mut self, activity_id: XrActivityId, bindings: I) + where + I: IntoIterator, + { + let same_activity = self.activity_id == Some(activity_id); + self.activity_id = Some(activity_id); + self.rebuild_spawnable_bindings(bindings); + if same_activity { + self.remap_local_shared_objects(); + self.remap_remote_shared_objects(activity_id); + } else { + self.clear_active_shared_objects(); + } + } + + pub fn resolve_object_id( + &self, + activity_id: XrActivityId, + widget_uid: WidgetUid, + ) -> Option { + (self.activity_id == Some(activity_id)) + .then(|| self.widget_to_object.get(&widget_uid).copied()) + .flatten() + } + + pub fn resolve_widget_uid( + &self, + activity_id: XrActivityId, + object_id: XrSpawnableObjectId, + ) -> Option { + (self.activity_id == Some(activity_id)) + .then(|| self.object_to_widget.get(&object_id).copied()) + .flatten() + } + + pub fn resolve_remote_shared_object_widget( + &self, + shared_object_id: XrSharedObjectId, + ) -> Option { + self.remote_object_to_widget.get(&shared_object_id).copied() + } + + pub fn resolve_remote_shared_object_for_widget( + &self, + widget_uid: WidgetUid, + ) -> Option { + self.remote_widget_to_object.get(&widget_uid).copied() + } + + pub fn resolve_local_shared_object_for_widget( + &self, + widget_uid: WidgetUid, + ) -> Option { + self.widget_to_local_shared_object.get(&widget_uid).copied() + } + + pub fn resolve_local_shared_object_widget( + &self, + shared_object_id: XrSharedObjectId, + ) -> Option { + self.local_shared_object_to_widget + .get(&shared_object_id) + .copied() + } + + pub fn bootstrap_shared_candidates(&self) -> Vec<(XrSpawnableObjectId, WidgetUid)> { + let mut candidates = self + .object_to_widget + .iter() + .filter_map(|(&object_id, &widget_uid)| { + self.object_bootstrap_shared + .get(&object_id) + .copied() + .unwrap_or(false) + .then_some((object_id, widget_uid)) + }) + .collect::>(); + candidates.sort_by_key(|(object_id, _)| object_id.0); + candidates + } + + fn spawnable_group_widgets(&self, widget_uid: WidgetUid) -> Option<&Vec> { + let object_id = self.widget_to_object.get(&widget_uid).copied()?; + let group_id = self.object_to_group.get(&object_id).copied()?; + self.group_to_widgets.get(&group_id) + } + + fn widget_is_local(&self, widget_uid: WidgetUid) -> bool { + self.widget_to_local_shared_object.contains_key(&widget_uid) + } + + fn widget_is_remote(&self, widget_uid: WidgetUid) -> bool { + self.remote_widget_to_object.contains_key(&widget_uid) + } + + fn widget_activation_time(&self, widget_uid: WidgetUid) -> Option { + if let Some(object_id) = self.widget_to_local_shared_object.get(&widget_uid).copied() { + return self + .local_objects + .get(&object_id) + .map(|record| record.activated_at_local_time); + } + if let Some(object_id) = self.remote_widget_to_object.get(&widget_uid).copied() { + return self + .remote_objects + .get(&object_id) + .map(|record| record.activated_at_local_time); + } + None + } + + fn first_free_group_widget(&self, group_widgets: &[WidgetUid]) -> Option { + group_widgets + .iter() + .copied() + .find(|widget_uid| !self.widget_is_local(*widget_uid) && !self.widget_is_remote(*widget_uid)) + } + + fn oldest_group_widget( + &self, + group_widgets: &[WidgetUid], + allow_local: bool, + allow_remote: bool, + ) -> Option { + let mut best = None::<(f64, usize, WidgetUid)>; + for (index, &widget_uid) in group_widgets.iter().enumerate() { + let eligible = (allow_local && self.widget_is_local(widget_uid)) + || (allow_remote && self.widget_is_remote(widget_uid)); + if !eligible { + continue; + } + let Some(activated_at) = self.widget_activation_time(widget_uid) else { + continue; + }; + if best.is_none_or(|current| (activated_at, index) < (current.0, current.1)) { + best = Some((activated_at, index, widget_uid)); + } + } + best.map(|(_, _, widget_uid)| widget_uid) + } + + fn select_local_spawn_widget_uid( + &self, + activity_id: XrActivityId, + preferred_widget_uid: WidgetUid, + ) -> Option { + if self.activity_id != Some(activity_id) { + return None; + } + let group_widgets = self.spawnable_group_widgets(preferred_widget_uid)?; + if !self.widget_is_local(preferred_widget_uid) && !self.widget_is_remote(preferred_widget_uid) { + return Some(preferred_widget_uid); + } + self.first_free_group_widget(group_widgets) + .or_else(|| self.oldest_group_widget(group_widgets, true, true)) + } + + fn select_remote_spawn_widget_uid( + &self, + activity_id: XrActivityId, + preferred_widget_uid: WidgetUid, + ) -> Option { + if self.activity_id != Some(activity_id) { + return None; + } + let group_widgets = self.spawnable_group_widgets(preferred_widget_uid)?; + if !self.widget_is_local(preferred_widget_uid) && !self.widget_is_remote(preferred_widget_uid) { + return Some(preferred_widget_uid); + } + self.first_free_group_widget(group_widgets) + .or_else(|| self.oldest_group_widget(group_widgets, false, true)) + } + + pub fn release_remote_shared_object( + &mut self, + shared_object_id: XrSharedObjectId, + ) -> Option { + let widget_uid = self.remote_object_to_widget.remove(&shared_object_id)?; + self.remote_widget_to_object.remove(&widget_uid); + self.remote_objects.remove(&shared_object_id); + Some(widget_uid) + } + + pub fn release_remote_shared_objects_by_peer_id( + &mut self, + peer_id: XrNetPeerId, + ) -> Vec { + let object_ids = self + .remote_objects + .iter() + .filter_map(|(&object_id, record)| (record.authority == peer_id).then_some(object_id)) + .collect::>(); + let mut widgets = Vec::with_capacity(object_ids.len()); + for object_id in object_ids { + if let Some(widget_uid) = self.release_remote_shared_object(object_id) { + widgets.push(widget_uid); + } + } + widgets + } + + pub fn resolve_remote_widget_uid( + &mut self, + activity_id: XrActivityId, + shared_object_id: XrSharedObjectId, + spawnable_object_id: XrSpawnableObjectId, + ) -> Option { + if self.activity_id != Some(activity_id) { + return None; + } + if let Some(widget_uid) = self.remote_object_to_widget.get(&shared_object_id).copied() { + return Some(widget_uid); + } + let preferred_widget = self.object_to_widget.get(&spawnable_object_id).copied()?; + let widget_uid = self.select_remote_spawn_widget_uid(activity_id, preferred_widget)?; + if let Some(previous_key) = self + .remote_widget_to_object + .insert(widget_uid, shared_object_id) + { + self.remote_object_to_widget.remove(&previous_key); + self.remote_objects.remove(&previous_key); + } + self.remote_object_to_widget + .insert(shared_object_id, widget_uid); + Some(widget_uid) + } + + pub fn register_remote_shared_object( + &mut self, + activity_id: XrActivityId, + activated_at_local_time: f64, + shared_object_id: XrSharedObjectId, + epoch: u32, + authority: XrNetPeerId, + fidelity: XrSharedObjectFidelity, + spawnable_object_id: XrSpawnableObjectId, + pose: Pose, + linvel: Vec3f, + angvel: Vec3f, + ) -> Option { + if self.local_objects.contains_key(&shared_object_id) { + return None; + } + let widget_uid = + self.resolve_remote_widget_uid(activity_id, shared_object_id, spawnable_object_id)?; + let state = XrNetSharedObjectState { + seq: 0, + sent_at: 0.0, + physics_tick: 0, + object_id: shared_object_id, + epoch, + authority, + fidelity, + mode: XrSharedObjectMode::Dynamic, + pose, + linvel, + angvel, + }; + let record = self + .remote_objects + .entry(shared_object_id) + .or_insert_with(|| XrRemoteSharedObjectRecord { + spawnable_object_id, + widget_uid, + epoch, + authority, + state_source_authority: authority, + fidelity, + activated_at_local_time, + latest_state: None, + history: VecDeque::new(), + pending_takeover_request_id: None, + last_takeover_request_at: None, + last_contact_impulse_at: None, + pending_transfer: None, + }); + record.spawnable_object_id = spawnable_object_id; + record.widget_uid = widget_uid; + record.epoch = epoch; + record.authority = authority; + record.state_source_authority = authority; + record.fidelity = fidelity; + record.activated_at_local_time = activated_at_local_time; + record.latest_state = Some(state); + push_shared_object_history(&mut record.history, state); + Some(widget_uid) + } + + pub fn prepare_local_spawn_allocation( + &mut self, + activity_id: XrActivityId, + preferred_widget_uid: WidgetUid, + activated_at_local_time: f64, + pose: Pose, + linvel: Vec3f, + angvel: Vec3f, + ) -> Option<(XrLocalSharedObjectAllocation, bool)> { + if self.activity_id != Some(activity_id) { + return None; + } + let widget_uid = self.select_local_spawn_widget_uid(activity_id, preferred_widget_uid)?; + let reused_remote = self.remote_widget_to_object.contains_key(&widget_uid); + let allocation = self.force_local_shared_object_reset( + activity_id, + widget_uid, + activated_at_local_time, + pose, + linvel, + angvel, + )?; + Some((allocation, reused_remote)) + } + + pub fn record_remote_shared_object_state( + &mut self, + sender_authority: XrNetPeerId, + state: XrNetSharedObjectState, + ) -> Option { + let record = self.remote_objects.get_mut(&state.object_id)?; + if !Self::remote_state_sender_matches(record, sender_authority, state) { + return None; + } + record.epoch = state.epoch; + record.authority = sender_authority; + record.state_source_authority = sender_authority; + record.fidelity = state.fidelity; + record.latest_state = Some(state); + push_shared_object_history(&mut record.history, state); + Some(record.widget_uid) + } + + pub fn local_shared_object_snapshot( + &self, + object_id: XrSharedObjectId, + ) -> Option { + let record = self.local_objects.get(&object_id)?; + Some(XrLocalSharedObjectSnapshot { + object_id, + spawnable_object_id: record.spawnable_object_id, + widget_uid: record.widget_uid, + epoch: record.epoch, + authority: record.authority, + fidelity: record.fidelity, + latest_state: record.latest_state, + }) + } + + pub fn local_shared_object_snapshots(&self) -> Vec { + let mut object_ids = self.local_objects.keys().copied().collect::>(); + object_ids.sort_by_key(|object_id| object_id.0); + object_ids + .into_iter() + .filter_map(|object_id| self.local_shared_object_snapshot(object_id)) + .collect() + } + + pub fn remote_shared_object_snapshot( + &self, + object_id: XrSharedObjectId, + ) -> Option { + let record = self.remote_objects.get(&object_id)?; + Some(XrRemoteSharedObjectSnapshot { + object_id, + spawnable_object_id: record.spawnable_object_id, + widget_uid: record.widget_uid, + epoch: record.epoch, + authority: record.authority, + state_source_authority: record.state_source_authority, + fidelity: record.fidelity, + latest_state: record.latest_state, + pending_takeover_request_id: record.pending_takeover_request_id, + }) + } + + pub fn remote_shared_object_snapshot_for_widget( + &self, + widget_uid: WidgetUid, + ) -> Option { + let object_id = self.remote_widget_to_object.get(&widget_uid).copied()?; + self.remote_shared_object_snapshot(object_id) + } + + pub fn remote_shared_object_snapshots(&self) -> Vec { + self.remote_objects + .iter() + .map(|(&object_id, record)| XrRemoteSharedObjectSnapshot { + object_id, + spawnable_object_id: record.spawnable_object_id, + widget_uid: record.widget_uid, + epoch: record.epoch, + authority: record.authority, + state_source_authority: record.state_source_authority, + fidelity: record.fidelity, + latest_state: record.latest_state, + pending_takeover_request_id: record.pending_takeover_request_id, + }) + .collect() + } + + pub fn can_send_takeover_request(&self, object_id: XrSharedObjectId, now: f64) -> bool { + let Some(record) = self.remote_objects.get(&object_id) else { + return false; + }; + record.pending_transfer.is_none() + && record.pending_takeover_request_id.is_none() + && record + .last_takeover_request_at + .is_none_or(|last| now - last >= XR_SHARED_OBJECT_TAKEOVER_REQUEST_COOLDOWN_SECONDS) + } + + pub fn note_takeover_request( + &mut self, + object_id: XrSharedObjectId, + request_id: u32, + now: f64, + ) -> bool { + let Some(record) = self.remote_objects.get_mut(&object_id) else { + return false; + }; + record.pending_takeover_request_id = Some(request_id); + record.last_takeover_request_at = Some(now); + true + } + + pub fn clear_takeover_request( + &mut self, + object_id: XrSharedObjectId, + request_id: Option, + ) -> bool { + let Some(record) = self.remote_objects.get_mut(&object_id) else { + return false; + }; + if request_id.is_none() || record.pending_takeover_request_id == request_id { + record.pending_takeover_request_id = None; + return true; + } + false + } + + pub fn can_send_contact_impulse_request(&self, object_id: XrSharedObjectId, now: f64) -> bool { + let Some(record) = self.remote_objects.get(&object_id) else { + return false; + }; + record.pending_transfer.is_none() + && record + .last_contact_impulse_at + .is_none_or(|last| now - last >= XR_SHARED_OBJECT_CONTACT_IMPULSE_COOLDOWN_SECONDS) + } + + pub fn note_contact_impulse_request(&mut self, object_id: XrSharedObjectId, now: f64) -> bool { + let Some(record) = self.remote_objects.get_mut(&object_id) else { + return false; + }; + record.last_contact_impulse_at = Some(now); + true + } + + pub fn find_local_state_for_request( + &self, + object_id: XrSharedObjectId, + epoch: u32, + based_on_seq: u32, + based_on_tick: u32, + ) -> Option { + let record = self.local_objects.get(&object_id)?; + if record.epoch != epoch { + return None; + } + record + .history + .iter() + .rev() + .copied() + .find(|state| { + state.epoch == epoch + && state_seq_not_after(state.seq, based_on_seq) + && state_tick_not_after(state.physics_tick, based_on_tick) + }) + .or(record.latest_state) + } + + pub fn schedule_authority_transfer( + &mut self, + object_id: XrSharedObjectId, + epoch: u32, + source_authority: XrNetPeerId, + new_authority: XrNetPeerId, + effective_at: f64, + effective_tick: u32, + request_id: u32, + hand: Option, + pose: Pose, + linvel: Vec3f, + angvel: Vec3f, + ) -> bool { + let pending = XrPendingAuthorityTransfer { + epoch, + source_authority, + new_authority, + effective_at, + effective_tick, + request_id, + hand, + pose, + linvel, + angvel, + }; + if let Some(record) = self.local_objects.get_mut(&object_id) { + record.pending_transfer = Some(pending); + return true; + } + if let Some(record) = self.remote_objects.get_mut(&object_id) { + record.pending_transfer = Some(pending); + record.pending_takeover_request_id = None; + return true; + } + false + } + + pub fn apply_scheduled_authority_transfers( + &mut self, + now: f64, + current_tick: u32, + ) -> Vec { + let local_peer_id = self.local_peer_id.unwrap_or_default(); + let local_due = self + .local_objects + .iter() + .filter_map(|(&object_id, record)| { + transfer_is_due(record.pending_transfer, now, current_tick).then_some(object_id) + }) + .collect::>(); + let remote_due = self + .remote_objects + .iter() + .filter_map(|(&object_id, record)| { + transfer_is_due(record.pending_transfer, now, current_tick).then_some(object_id) + }) + .collect::>(); + let mut transfers = Vec::with_capacity(local_due.len() + remote_due.len()); + + for object_id in local_due { + let Some(mut record) = self.local_objects.remove(&object_id) else { + continue; + }; + let Some(pending) = record.pending_transfer.take() else { + self.local_objects.insert(object_id, record); + continue; + }; + self.local_shared_object_to_widget.remove(&object_id); + self.widget_to_local_shared_object + .remove(&record.widget_uid); + if pending.new_authority == local_peer_id { + record.epoch = pending.epoch; + record.authority = local_peer_id; + record.latest_state = Some(transfer_state( + object_id, + pending.epoch, + local_peer_id, + record.fidelity, + pending.pose, + pending.linvel, + pending.angvel, + )); + record.history.clear(); + if let Some(state) = record.latest_state { + push_shared_object_history(&mut record.history, state); + } + self.widget_to_local_shared_object + .insert(record.widget_uid, object_id); + self.local_shared_object_to_widget + .insert(object_id, record.widget_uid); + self.local_objects.insert(object_id, record.clone()); + transfers.push(XrScheduledAuthorityTransfer { + object_id, + widget_uid: record.widget_uid, + shadow: false, + source_authority: pending.source_authority, + new_authority: pending.new_authority, + epoch: pending.epoch, + request_id: pending.request_id, + hand: pending.hand, + pose: pending.pose, + linvel: pending.linvel, + angvel: pending.angvel, + }); + continue; + } + + self.remote_widget_to_object + .insert(record.widget_uid, object_id); + self.remote_object_to_widget + .insert(object_id, record.widget_uid); + let state = transfer_state( + object_id, + pending.epoch, + pending.new_authority, + record.fidelity, + pending.pose, + pending.linvel, + pending.angvel, + ); + let mut remote_record = XrRemoteSharedObjectRecord { + spawnable_object_id: record.spawnable_object_id, + widget_uid: record.widget_uid, + epoch: pending.epoch, + authority: pending.new_authority, + state_source_authority: pending.source_authority, + fidelity: record.fidelity, + activated_at_local_time: record.activated_at_local_time, + latest_state: Some(state), + history: VecDeque::new(), + pending_takeover_request_id: None, + last_takeover_request_at: None, + last_contact_impulse_at: None, + pending_transfer: None, + }; + push_shared_object_history(&mut remote_record.history, state); + self.remote_objects.insert(object_id, remote_record); + transfers.push(XrScheduledAuthorityTransfer { + object_id, + widget_uid: record.widget_uid, + shadow: true, + source_authority: pending.source_authority, + new_authority: pending.new_authority, + epoch: pending.epoch, + request_id: pending.request_id, + hand: pending.hand, + pose: pending.pose, + linvel: pending.linvel, + angvel: pending.angvel, + }); + } + + for object_id in remote_due { + let Some(mut record) = self.remote_objects.remove(&object_id) else { + continue; + }; + let Some(pending) = record.pending_transfer.take() else { + self.remote_objects.insert(object_id, record); + continue; + }; + let widget_uid = record.widget_uid; + let spawnable_object_id = record.spawnable_object_id; + let fidelity = record.fidelity; + let state = transfer_state( + object_id, + pending.epoch, + pending.new_authority, + fidelity, + pending.pose, + pending.linvel, + pending.angvel, + ); + if pending.new_authority == local_peer_id { + self.remote_object_to_widget.remove(&object_id); + self.remote_widget_to_object.remove(&widget_uid); + self.widget_to_local_shared_object + .insert(widget_uid, object_id); + self.local_shared_object_to_widget + .insert(object_id, widget_uid); + let mut local_record = XrLocalSharedObjectRecord { + spawnable_object_id, + widget_uid, + epoch: pending.epoch, + authority: local_peer_id, + fidelity, + activated_at_local_time: record.activated_at_local_time, + latest_state: Some(state), + last_published_state: Some(state), + last_published_at: Some(state.sent_at), + history: VecDeque::new(), + pending_transfer: None, + }; + push_shared_object_history(&mut local_record.history, state); + self.local_objects.insert(object_id, local_record); + } else { + record.epoch = pending.epoch; + record.authority = pending.new_authority; + record.state_source_authority = pending.source_authority; + record.latest_state = Some(state); + record.pending_takeover_request_id = None; + record.history.clear(); + push_shared_object_history(&mut record.history, state); + self.remote_objects.insert(object_id, record); + } + transfers.push(XrScheduledAuthorityTransfer { + object_id, + widget_uid, + shadow: pending.new_authority != local_peer_id, + source_authority: pending.source_authority, + new_authority: pending.new_authority, + epoch: pending.epoch, + request_id: pending.request_id, + hand: pending.hand, + pose: pending.pose, + linvel: pending.linvel, + angvel: pending.angvel, + }); + } + + transfers + } + + pub fn force_local_shared_object_reset( + &mut self, + activity_id: XrActivityId, + widget_uid: WidgetUid, + activated_at_local_time: f64, + pose: Pose, + linvel: Vec3f, + angvel: Vec3f, + ) -> Option { + if self.activity_id != Some(activity_id) { + return None; + } + let local_peer_id = self.local_peer_id?; + if let Some(object_id) = self.widget_to_local_shared_object.get(&widget_uid).copied() { + let record = self.local_objects.get_mut(&object_id)?; + record.epoch = record.epoch.wrapping_add(1); + record.authority = local_peer_id; + record.latest_state = Some(transfer_state( + object_id, + record.epoch, + local_peer_id, + record.fidelity, + pose, + linvel, + angvel, + )); + record.activated_at_local_time = activated_at_local_time; + record.last_published_state = None; + record.last_published_at = None; + record.history.clear(); + record.pending_transfer = None; + return Some(XrLocalSharedObjectAllocation { + shared_object_id: object_id, + spawnable_object_id: record.spawnable_object_id, + widget_uid: record.widget_uid, + epoch: record.epoch, + fidelity: record.fidelity, + }); + } + if let Some(object_id) = self.remote_widget_to_object.remove(&widget_uid) { + self.remote_object_to_widget.remove(&object_id); + let remote = self.remote_objects.remove(&object_id)?; + self.widget_to_local_shared_object + .insert(widget_uid, object_id); + self.local_shared_object_to_widget + .insert(object_id, widget_uid); + let epoch = remote.epoch.wrapping_add(1); + self.local_objects.insert( + object_id, + XrLocalSharedObjectRecord { + spawnable_object_id: remote.spawnable_object_id, + widget_uid, + epoch, + authority: local_peer_id, + fidelity: remote.fidelity, + activated_at_local_time, + latest_state: Some(transfer_state( + object_id, + epoch, + local_peer_id, + remote.fidelity, + pose, + linvel, + angvel, + )), + last_published_state: None, + last_published_at: None, + history: VecDeque::new(), + pending_transfer: None, + }, + ); + return self + .local_shared_object_snapshot(object_id) + .map(|snapshot| XrLocalSharedObjectAllocation { + shared_object_id: snapshot.object_id, + spawnable_object_id: snapshot.spawnable_object_id, + widget_uid: snapshot.widget_uid, + epoch: snapshot.epoch, + fidelity: snapshot.fidelity, + }); + } + let (allocation, _) = self.ensure_local_shared_object(activity_id, widget_uid)?; + let record = self.local_objects.get_mut(&allocation.shared_object_id)?; + record.authority = local_peer_id; + record.activated_at_local_time = activated_at_local_time; + record.latest_state = Some(transfer_state( + allocation.shared_object_id, + allocation.epoch, + local_peer_id, + record.fidelity, + pose, + linvel, + angvel, + )); + record.last_published_state = None; + record.last_published_at = None; + record.history.clear(); + record.pending_transfer = None; + Some(XrLocalSharedObjectAllocation { + shared_object_id: allocation.shared_object_id, + spawnable_object_id: allocation.spawnable_object_id, + widget_uid: allocation.widget_uid, + epoch: allocation.epoch, + fidelity: allocation.fidelity, + }) + } + + pub fn apply_remote_shared_object_reset( + &mut self, + activity_id: XrActivityId, + activated_at_local_time: f64, + source_authority: XrNetPeerId, + object_id: XrSharedObjectId, + epoch: u32, + pose: Pose, + linvel: Vec3f, + angvel: Vec3f, + ) -> Option { + if self.activity_id != Some(activity_id) { + return None; + } + if let Some(record) = self.local_objects.remove(&object_id) { + self.local_shared_object_to_widget.remove(&object_id); + self.widget_to_local_shared_object + .remove(&record.widget_uid); + let widget_uid = record.widget_uid; + self.remote_widget_to_object.insert(widget_uid, object_id); + self.remote_object_to_widget.insert(object_id, widget_uid); + let state = transfer_state( + object_id, + epoch, + source_authority, + record.fidelity, + pose, + linvel, + angvel, + ); + let mut remote_record = XrRemoteSharedObjectRecord { + spawnable_object_id: record.spawnable_object_id, + widget_uid, + epoch, + authority: source_authority, + state_source_authority: source_authority, + fidelity: record.fidelity, + activated_at_local_time, + latest_state: Some(state), + history: VecDeque::new(), + pending_takeover_request_id: None, + last_takeover_request_at: None, + last_contact_impulse_at: None, + pending_transfer: None, + }; + push_shared_object_history(&mut remote_record.history, state); + self.remote_objects.insert(object_id, remote_record); + return Some(widget_uid); + } + let record = self.remote_objects.get_mut(&object_id)?; + let state = transfer_state( + object_id, + epoch, + source_authority, + record.fidelity, + pose, + linvel, + angvel, + ); + record.epoch = epoch; + record.authority = source_authority; + record.state_source_authority = source_authority; + record.latest_state = Some(state); + record.activated_at_local_time = activated_at_local_time; + record.pending_takeover_request_id = None; + record.pending_transfer = None; + record.history.clear(); + push_shared_object_history(&mut record.history, state); + Some(record.widget_uid) + } + + pub fn allocate_local_shared_object( + &mut self, + activity_id: XrActivityId, + widget_uid: WidgetUid, + ) -> Option { + if self.activity_id != Some(activity_id) { + return None; + } + let local_peer_id = self.local_peer_id?; + let spawnable_object_id = self.widget_to_object.get(&widget_uid).copied()?; + if let Some(object_id) = self.widget_to_local_shared_object.get(&widget_uid).copied() { + let record = self.local_objects.get_mut(&object_id)?; + record.spawnable_object_id = spawnable_object_id; + record.widget_uid = widget_uid; + record.epoch = record.epoch.wrapping_add(1); + record.authority = local_peer_id; + record.latest_state = None; + record.last_published_state = None; + record.last_published_at = None; + record.history.clear(); + record.pending_transfer = None; + return Some(XrLocalSharedObjectAllocation { + shared_object_id: object_id, + spawnable_object_id, + widget_uid, + epoch: record.epoch, + fidelity: record.fidelity, + }); + } + self.release_remote_widget_claim(widget_uid); + let object_id = xr_make_shared_object_id(local_peer_id, self.next_shared_object_counter)?; + self.next_shared_object_counter.0 = self.next_shared_object_counter.0.saturating_add(1); + self.widget_to_local_shared_object + .insert(widget_uid, object_id); + self.local_shared_object_to_widget + .insert(object_id, widget_uid); + let record = XrLocalSharedObjectRecord { + spawnable_object_id, + widget_uid, + epoch: 0, + authority: local_peer_id, + fidelity: XrSharedObjectFidelity::ImpactCritical, + activated_at_local_time: 0.0, + latest_state: None, + last_published_state: None, + last_published_at: None, + history: VecDeque::new(), + pending_transfer: None, + }; + let epoch = record.epoch; + let fidelity = record.fidelity; + self.local_objects.insert(object_id, record); + Some(XrLocalSharedObjectAllocation { + shared_object_id: object_id, + spawnable_object_id, + widget_uid, + epoch, + fidelity, + }) + } + + pub fn ensure_local_shared_object( + &mut self, + activity_id: XrActivityId, + widget_uid: WidgetUid, + ) -> Option<(XrLocalSharedObjectAllocation, bool)> { + if self.activity_id != Some(activity_id) { + return None; + } + let local_peer_id = self.local_peer_id?; + let spawnable_object_id = self.widget_to_object.get(&widget_uid).copied()?; + if let Some(object_id) = self.widget_to_local_shared_object.get(&widget_uid).copied() { + let record = self.local_objects.get(&object_id)?; + return Some(( + XrLocalSharedObjectAllocation { + shared_object_id: object_id, + spawnable_object_id: record.spawnable_object_id, + widget_uid: record.widget_uid, + epoch: record.epoch, + fidelity: record.fidelity, + }, + false, + )); + } + self.release_remote_widget_claim(widget_uid); + let object_id = xr_make_shared_object_id(local_peer_id, self.next_shared_object_counter)?; + self.next_shared_object_counter.0 = self.next_shared_object_counter.0.saturating_add(1); + self.widget_to_local_shared_object + .insert(widget_uid, object_id); + self.local_shared_object_to_widget + .insert(object_id, widget_uid); + let record = XrLocalSharedObjectRecord { + spawnable_object_id, + widget_uid, + epoch: 0, + authority: local_peer_id, + fidelity: XrSharedObjectFidelity::ImpactCritical, + activated_at_local_time: 0.0, + latest_state: None, + last_published_state: None, + last_published_at: None, + history: VecDeque::new(), + pending_transfer: None, + }; + let epoch = record.epoch; + let fidelity = record.fidelity; + self.local_objects.insert(object_id, record); + Some(( + XrLocalSharedObjectAllocation { + shared_object_id: object_id, + spawnable_object_id, + widget_uid, + epoch, + fidelity, + }, + true, + )) + } + + pub fn local_shared_object_states( + &mut self, + runtime_bodies: &HashMap, + sent_at: f64, + physics_tick: u32, + authority: XrNetPeerId, + ) -> Vec { + let mut states = Vec::with_capacity(self.local_objects.len()); + for (&object_id, record) in &mut self.local_objects { + let Some(body) = runtime_bodies.get(&record.widget_uid) else { + continue; + }; + let state = XrNetSharedObjectState { + seq: 0, + sent_at, + physics_tick, + object_id, + epoch: record.epoch, + authority, + fidelity: record.fidelity, + mode: if let Some(hand) = body.held_by { + XrSharedObjectMode::ContactDominated { authority, hand } + } else if body.sleeping { + XrSharedObjectMode::Sleeping + } else { + XrSharedObjectMode::Dynamic + }, + pose: body.pose, + linvel: body.linvel, + angvel: body.angvel, + }; + record.authority = authority; + record.latest_state = Some(state); + if should_publish_local_shared_object_state(record, state, sent_at) { + states.push(state); + } + } + states + } + + pub fn note_published_local_shared_object_states(&mut self, states: &[XrNetSharedObjectState]) { + for state in states { + let Some(record) = self.local_objects.get_mut(&state.object_id) else { + continue; + }; + record.epoch = state.epoch; + record.authority = state.authority; + record.fidelity = state.fidelity; + record.latest_state = Some(*state); + record.last_published_state = Some(*state); + record.last_published_at = Some(state.sent_at); + push_shared_object_history(&mut record.history, *state); + } + } + + pub fn remote_shared_object_history( + &self, + object_id: XrSharedObjectId, + ) -> Vec { + self.remote_objects + .get(&object_id) + .map(|record| record.history.iter().copied().collect()) + .unwrap_or_default() + } + + pub fn prune_missing_local_shared_objects( + &mut self, + runtime_bodies: &HashMap, + ) -> Vec<(XrSharedObjectId, u32, WidgetUid)> { + let mut missing = Vec::new(); + for (&object_id, record) in &self.local_objects { + if !runtime_bodies.contains_key(&record.widget_uid) { + missing.push((object_id, record.epoch, record.widget_uid)); + } + } + for (object_id, _, widget_uid) in &missing { + self.local_objects.remove(object_id); + self.local_shared_object_to_widget.remove(object_id); + self.widget_to_local_shared_object.remove(widget_uid); + } + missing + } +} + +fn push_shared_object_history( + history: &mut VecDeque, + state: XrNetSharedObjectState, +) { + if history + .back() + .is_some_and(|previous| previous.epoch != state.epoch) + { + history.clear(); + } + history.push_back(state); + while history.len() > XR_SHARED_OBJECT_HISTORY_MAX_SAMPLES { + history.pop_front(); + } + while history.front().is_some_and(|front| { + state.sent_at != 0.0 + && front.sent_at != 0.0 + && state.sent_at - front.sent_at > XR_SHARED_OBJECT_HISTORY_MAX_SECONDS + }) { + history.pop_front(); + } +} + +fn pose_publish_delta_exceeded(previous: Pose, next: Pose) -> bool { + (next.position - previous.position).length() > XR_SHARED_OBJECT_PUBLISH_POSITION_EPSILON_METERS + || previous.orientation.get_angle_with(next.orientation) + > XR_SHARED_OBJECT_PUBLISH_ORIENTATION_EPSILON_DEGREES +} + +fn velocity_publish_delta_exceeded(previous: Vec3f, next: Vec3f, epsilon: f32) -> bool { + (next - previous).length() > epsilon +} + +fn should_publish_local_shared_object_state( + record: &XrLocalSharedObjectRecord, + state: XrNetSharedObjectState, + sent_at: f64, +) -> bool { + let Some(previous) = record.last_published_state else { + return true; + }; + if previous.epoch != state.epoch + || previous.authority != state.authority + || previous.fidelity != state.fidelity + || previous.mode != state.mode + { + return true; + } + if pose_publish_delta_exceeded(previous.pose, state.pose) + || velocity_publish_delta_exceeded( + previous.linvel, + state.linvel, + XR_SHARED_OBJECT_PUBLISH_LINVEL_EPSILON_MPS, + ) + || velocity_publish_delta_exceeded( + previous.angvel, + state.angvel, + XR_SHARED_OBJECT_PUBLISH_ANGVEL_EPSILON_RADPS, + ) + { + return true; + } + matches!(state.mode, XrSharedObjectMode::Sleeping) + && record.last_published_at.is_none_or(|last_published_at| { + sent_at - last_published_at >= XR_SHARED_OBJECT_SLEEPING_PUBLISH_INTERVAL_SECONDS + }) +} + +fn state_seq_not_after(history_seq: u32, requested_seq: u32) -> bool { + history_seq == requested_seq || requested_seq.wrapping_sub(history_seq) < (u32::MAX / 2) +} + +fn state_tick_not_after(history_tick: u32, requested_tick: u32) -> bool { + history_tick == requested_tick || requested_tick.wrapping_sub(history_tick) < (u32::MAX / 2) +} + +fn transfer_is_due( + transfer: Option, + now: f64, + current_tick: u32, +) -> bool { + transfer.is_some_and(|pending| { + now >= pending.effective_at || state_tick_not_after(pending.effective_tick, current_tick) + }) +} + +fn transfer_state( + object_id: XrSharedObjectId, + epoch: u32, + authority: XrNetPeerId, + fidelity: XrSharedObjectFidelity, + pose: Pose, + linvel: Vec3f, + angvel: Vec3f, +) -> XrNetSharedObjectState { + XrNetSharedObjectState { + seq: 0, + sent_at: 0.0, + physics_tick: 0, + object_id, + epoch, + authority, + fidelity, + mode: XrSharedObjectMode::Dynamic, + pose, + linvel, + angvel, + } +} + +pub fn collect_scene_spawnable_objects( + activity_id: XrActivityId, + root: &WidgetRef, +) -> Vec { + let mut bindings = Vec::new(); + let root_hash = root_path_hash(activity_id); + collect_widget_spawnables(root, root_hash, root_hash, &mut bindings); + bindings +} + +fn collect_widget_spawnables( + widget: &WidgetRef, + path_hash: u64, + parent_hash: u64, + bindings: &mut Vec, +) { + if !widget.visible() { + return; + } + + if xr_widget_with_scene_node(widget, |node| { + if node.body_kind() == XrBodyKind::Dynamic { + let object_id = XrSpawnableObjectId(non_zero_hash(hash_u64( + path_hash, + XR_SHARED_OBJECT_BODY_TAG, + ))); + let allocation_group_id = if node.spawn_pool() { + XrSpawnableObjectId(non_zero_hash(hash_u64( + parent_hash, + XR_SHARED_OBJECT_POOL_GROUP_TAG, + ))) + } else { + object_id + }; + bindings.push(XrSpawnableObjectBinding { + object_id, + allocation_group_id, + widget_uid: widget.widget_uid(), + bootstrap_shared: node.bootstrap_shared(), + }); + } + let mut child_index = 0usize; + xr_widget_children(widget, &mut |child_id, child| { + let child_hash = child_path_hash(path_hash, child_id, child_index); + child_index = child_index.saturating_add(1); + collect_widget_spawnables(&child, child_hash, path_hash, bindings); + }); + }) + .is_some() + { + return; + } + + let mut child_index = 0usize; + xr_widget_children(widget, &mut |child_id, child| { + let child_hash = child_path_hash(path_hash, child_id, child_index); + child_index = child_index.saturating_add(1); + collect_widget_spawnables(&child, child_hash, path_hash, bindings); + }); +} + +fn root_path_hash(activity_id: XrActivityId) -> u64 { + hash_u64( + hash_u64(XR_SHARED_OBJECT_HASH_OFFSET, XR_SHARED_OBJECT_ROOT_TAG), + activity_id.to_live_id().0, + ) +} + +fn child_path_hash(parent_hash: u64, child_id: LiveId, child_index: usize) -> u64 { + let hash = hash_u64(parent_hash, XR_SHARED_OBJECT_CHILD_TAG); + let hash = hash_u64(hash, child_id.0); + let hash = hash_u64(hash, XR_SHARED_OBJECT_INDEX_TAG); + hash_u64(hash, child_index as u64) +} + +fn hash_u64(hash: u64, value: u64) -> u64 { + (hash ^ value).wrapping_mul(XR_SHARED_OBJECT_HASH_PRIME) +} + +fn non_zero_hash(hash: u64) -> u64 { + if hash == 0 { + 1 + } else { + hash + } +} + +#[cfg(test)] +include!("../tests/scene/xr_shared_object_sync.rs"); diff --git a/xr/src/scene/xr_view.rs b/xr/src/scene/xr_view.rs index b20dafd28c..d5e6658d43 100644 --- a/xr/src/scene/xr_view.rs +++ b/xr/src/scene/xr_view.rs @@ -1,8 +1,11 @@ -use crate::xr_node::{xr_widget_world_transform, XrNode}; -use crate::*; +use super::xr_node::{xr_widget_world_transform, XrDrawContext, XrNode}; +use crate::prelude::*; use makepad_widgets::{ animator::{Animator, AnimatorImpl}, - event::XrFingerTip, + event::{ + XrFingerTip, XR_TOUCH_CAPTURE_BACK, XR_TOUCH_CAPTURE_FRONT, XR_TOUCH_DOWN_BACK, + XR_TOUCH_DOWN_FRONT, + }, }; use std::{cell::Cell, rc::Rc}; @@ -37,6 +40,7 @@ script_mod! { mod.widgets.XrViewBase = #(XrView::register_widget(vm)) mod.widgets.XrView = set_type_default() do mod.widgets.XrViewBase{ mode: XrViewMode.World + render_class: XrRenderClass.Transparent wrist_left: true show_in_non_xr: false fit_size: false @@ -184,8 +188,6 @@ impl XrView { const XR_CURSOR_HOVER_FRONT: f32 = 84.0; const XR_CURSOR_SIZE_NEAR: f64 = 30.0; const XR_CURSOR_SIZE_FAR: f64 = 16.0; - const XR_TOUCH_DOWN_FRONT: f32 = 6.0; - const XR_TOUCH_DOWN_BACK: f32 = -12.0; const FACE_PANEL_DISTANCE: f32 = 0.46; const FACE_PANEL_VERTICAL_OFFSET: f32 = -0.10; const WRIST_PANEL_SURFACE_OFFSET: f32 = 0.048; @@ -194,10 +196,6 @@ impl XrView { const ARM_PANEL_MENU_SIDE_OFFSET: f32 = 0.15; const ARM_PANEL_MENU_BACK_OFFSET: f32 = 0.10; - pub(crate) fn node(&self) -> &XrNode { - &self.node - } - fn scaled_pose_matrix(&self, pose: Pose) -> Mat4f { Mat4f::mul( &pose.to_mat4(), @@ -219,7 +217,8 @@ impl XrView { } fn event_world_transform(&self, scope: &mut Scope) -> Mat4f { - if let Some(runtime_body) = xr_runtime_body_from_scope(scope, self.uid) { + let draw_context = XrDrawContext::from_scope(scope); + if let Some(runtime_body) = draw_context.runtime_body(self.uid) { Mat4f::mul( &runtime_body.pose.to_mat4(), &Mat4f::nonuniform_scaled_translation( @@ -249,7 +248,8 @@ impl XrView { } fn resolved_world_transform(&self, cx: &mut Cx3d, scope: &mut Scope) -> Mat4f { - if let Some(runtime_body) = xr_runtime_body_from_scope(scope, self.uid) { + let draw_context = XrDrawContext::from_scope(scope); + if let Some(runtime_body) = draw_context.runtime_body(self.uid) { Mat4f::mul( &runtime_body.pose.to_mat4(), &Mat4f::nonuniform_scaled_translation( @@ -562,7 +562,11 @@ impl XrView { } fn tip_is_touching_for_down(touch_z: f32) -> bool { - touch_z <= Self::XR_TOUCH_DOWN_FRONT && touch_z >= Self::XR_TOUCH_DOWN_BACK + touch_z <= XR_TOUCH_DOWN_FRONT && touch_z >= XR_TOUCH_DOWN_BACK + } + + fn tip_is_touching_for_capture(touch_z: f32) -> bool { + touch_z <= XR_TOUCH_CAPTURE_FRONT && touch_z >= XR_TOUCH_CAPTURE_BACK } fn fingertip_slot(is_left: bool, index: usize) -> usize { @@ -711,12 +715,15 @@ impl XrView { if let Some(hit) = hit { let tip_is_interactive = self.multitouch || tip.index == XrHand::INDEX_TIP; - let tip_is_down = tip.active - && tip_is_interactive - && self.contains_local(hit.projected) - && Self::tip_is_touching_for_down(hit.touch_z); let tip_slot = Self::fingertip_slot(tip.is_left, tip.index); let was_tip_down = self.tip_down_state[tip_slot]; + let tip_is_down = tip_is_interactive + && self.contains_local(hit.projected) + && if was_tip_down { + Self::tip_is_touching_for_capture(hit.touch_z) + } else { + tip.active && Self::tip_is_touching_for_down(hit.touch_z) + }; if let Some(mut cursor) = self.cursor_from_hit(hit, tip.is_left) { cursor.index = tip.index; @@ -835,6 +842,23 @@ impl WidgetNode for XrView { fn widget_uid(&self) -> WidgetUid { self.uid } + + fn cast_inner_any(&self, type_id: std::any::TypeId) -> Option<&dyn std::any::Any> { + if type_id == std::any::TypeId::of::() { + Some(&self.node) + } else { + None + } + } + + fn cast_inner_any_mut(&mut self, type_id: std::any::TypeId) -> Option<&mut dyn std::any::Any> { + if type_id == std::any::TypeId::of::() { + Some(&mut self.node) + } else { + None + } + } + fn walk(&mut self, _cx: &mut Cx) -> Walk { self.walk } diff --git a/xr/src/sync/alignment.rs b/xr/src/sync/alignment.rs new file mode 100644 index 0000000000..02663c7aa3 --- /dev/null +++ b/xr/src/sync/alignment.rs @@ -0,0 +1,690 @@ +use super::*; + +pub(super) const XR_ALIGNMENT_CALLBACK_BUDGET_MILLIS: u64 = 25; +pub(super) const XR_ALIGNMENT_CALLBACK_MAX_STEPS: usize = 4096; +pub(super) const XR_ALIGNMENT_PROGRESS_SIGNAL_INTERVAL_MILLIS: u64 = 100; + +#[derive(Debug)] +pub(super) struct AlignmentWorkerPeerState { + pub(super) peer: XrNetPeer, + pub(super) latest_descriptor: Option, + pub(super) remote_to_local: Option, + pub(super) last_accepted_solution: Option, + pub(super) last_solve_diagnostic: Option, + pub(super) last_solve_ms: f64, + pub(super) last_solved_local_descriptor_version: Option<(u64, u64)>, + pub(super) last_solved_remote_descriptor_seq: Option, + pub(super) active_local_descriptor_version: Option<(u64, u64)>, + pub(super) active_remote_descriptor_seq: Option, + pub(super) queued_rerun: bool, + pub(super) matcher: Option, +} + +impl AlignmentWorkerPeerState { + fn new(peer: XrNetPeer) -> Self { + Self { + peer, + latest_descriptor: None, + remote_to_local: None, + last_accepted_solution: None, + last_solve_diagnostic: None, + last_solve_ms: 0.0, + last_solved_local_descriptor_version: None, + last_solved_remote_descriptor_seq: None, + active_local_descriptor_version: None, + active_remote_descriptor_seq: None, + queued_rerun: false, + matcher: None, + } + } + + fn to_result(&self) -> AlignmentWorkerPeerResult { + AlignmentWorkerPeerResult { + remote_to_local: self.remote_to_local, + last_solve_diagnostic: self.last_solve_diagnostic, + last_solve_ms: self.last_solve_ms, + last_solved_local_descriptor_version: self.last_solved_local_descriptor_version, + last_solved_remote_descriptor_seq: self.last_solved_remote_descriptor_seq, + worker_progress: self.matcher.as_ref().map(XrDepthAlignMatcher::progress), + } + } +} + +#[derive(Clone, Copy, Debug, Default)] +pub(super) struct AlignmentWorkerPeerResult { + pub(super) remote_to_local: Option, + pub(super) last_solve_diagnostic: Option, + pub(super) last_solve_ms: f64, + pub(super) last_solved_local_descriptor_version: Option<(u64, u64)>, + pub(super) last_solved_remote_descriptor_seq: Option, + pub(super) worker_progress: Option, +} + +#[derive(Clone, Debug)] +pub(super) struct XrPeopleAlignmentWorkerResult { + pub(super) peer_results: HashMap, + pub(super) alignment_debug_text: String, +} + +#[derive(Clone)] +pub(super) enum PendingLocalDescriptorUpdate { + Set { + frame: XrNetAlignmentDescriptorFrame, + version: (u64, u64), + }, + Clear, +} + +#[derive(Clone)] +pub(super) enum PendingPeerDescriptorUpdate { + Set { + peer: XrNetPeer, + frame: XrNetAlignmentDescriptorFrame, + }, + Remove, +} + +#[derive(Default)] +struct XrPeopleAlignmentWorkerMailbox { + pending_local_descriptor: Option, + pending_peer_updates: HashMap, +} + +pub(super) struct XrPeopleAlignmentWorker { + store: XrTsdfStore, + mailbox: Arc>, + latest_result: Arc>>, +} + +impl XrPeopleAlignmentWorker { + pub(super) fn new(store: XrTsdfStore) -> Self { + let mailbox = Arc::new(Mutex::new(XrPeopleAlignmentWorkerMailbox::default())); + let latest_result = Arc::new(Mutex::new(None)); + let runtime = Arc::new(Mutex::new(AlignmentWorkerState::default())); + let mailbox_callback = mailbox.clone(); + let latest_result_callback = latest_result.clone(); + let runtime_callback = runtime.clone(); + store.set_cooperative_step_callback(Some(Box::new(move || { + xr_people_alignment_worker_step( + &mailbox_callback, + &runtime_callback, + &latest_result_callback, + ) + }))); + + Self { + store, + mailbox, + latest_result, + } + } + + pub(super) fn set_local_descriptor( + &mut self, + frame: XrNetAlignmentDescriptorFrame, + version: (u64, u64), + ) { + self.send_mailbox_update(|mailbox| { + mailbox.pending_local_descriptor = + Some(PendingLocalDescriptorUpdate::Set { frame, version }); + }); + } + + pub(super) fn clear_local_descriptor(&mut self) { + self.send_mailbox_update(|mailbox| { + mailbox.pending_local_descriptor = Some(PendingLocalDescriptorUpdate::Clear); + }); + } + + pub(super) fn set_peer_descriptor( + &mut self, + peer: XrNetPeer, + frame: XrNetAlignmentDescriptorFrame, + ) { + self.send_mailbox_update(move |mailbox| { + mailbox + .pending_peer_updates + .insert(peer.id, PendingPeerDescriptorUpdate::Set { peer, frame }); + }); + } + + pub(super) fn remove_peer(&mut self, peer_id: XrNetPeerId) { + self.send_mailbox_update(move |mailbox| { + mailbox + .pending_peer_updates + .insert(peer_id, PendingPeerDescriptorUpdate::Remove); + }); + } + + pub(super) fn take_latest_result(&mut self) -> Option { + self.latest_result + .lock() + .ok() + .and_then(|mut result| result.take()) + } + + fn send_mailbox_update(&mut self, update: F) + where + F: FnOnce(&mut XrPeopleAlignmentWorkerMailbox), + { + if let Ok(mut mailbox) = self.mailbox.lock() { + update(&mut mailbox); + } + } +} + +impl Drop for XrPeopleAlignmentWorker { + fn drop(&mut self) { + self.store.set_cooperative_step_callback(None); + } +} + +#[derive(Default)] +pub(super) struct AlignmentWorkerState { + pub(super) peers: HashMap, + pub(super) local_descriptor: Option, + pub(super) local_descriptor_version: Option<(u64, u64)>, + pub(super) last_progress_publish_at: Option, +} + +impl AlignmentWorkerState { + pub(super) fn apply_local_descriptor_update( + &mut self, + update: PendingLocalDescriptorUpdate, + ) -> bool { + match update { + PendingLocalDescriptorUpdate::Set { frame, version } => { + if descriptor_frames_equal(self.local_descriptor.as_ref(), Some(&frame)) { + return false; + } + self.local_descriptor = Some(frame); + self.local_descriptor_version = Some(version); + self.refresh_all_peer_alignments() + } + PendingLocalDescriptorUpdate::Clear => { + if self.local_descriptor.is_none() { + return false; + } + self.local_descriptor = None; + self.local_descriptor_version = None; + self.refresh_all_peer_alignments() + } + } + } + + pub(super) fn apply_peer_update( + &mut self, + peer_id: XrNetPeerId, + update: PendingPeerDescriptorUpdate, + ) -> bool { + match update { + PendingPeerDescriptorUpdate::Set { peer, frame } => { + let peer_state = self + .peers + .entry(peer_id) + .or_insert_with(|| AlignmentWorkerPeerState::new(peer)); + peer_state.peer = peer; + if descriptor_frames_equal(peer_state.latest_descriptor.as_ref(), Some(&frame)) { + return false; + } + peer_state.latest_descriptor = Some(frame); + self.refresh_peer_alignment(peer_id) + } + PendingPeerDescriptorUpdate::Remove => self.peers.remove(&peer_id).is_some(), + } + } + + fn refresh_all_peer_alignments(&mut self) -> bool { + let local_descriptor = self.local_descriptor.clone(); + let local_descriptor_version = self.local_descriptor_version; + let mut changed = false; + for peer_state in self.peers.values_mut() { + changed |= schedule_alignment_worker_peer( + peer_state, + local_descriptor.as_ref(), + local_descriptor_version, + ); + } + changed + } + + fn refresh_peer_alignment(&mut self, peer_id: XrNetPeerId) -> bool { + let local_descriptor = self.local_descriptor.clone(); + let local_descriptor_version = self.local_descriptor_version; + let Some(peer_state) = self.peers.get_mut(&peer_id) else { + return false; + }; + schedule_alignment_worker_peer( + peer_state, + local_descriptor.as_ref(), + local_descriptor_version, + ) + } + + fn next_pending_peer_id(&self) -> Option { + self.peers + .iter() + .find_map(|(peer_id, peer_state)| peer_state.matcher.is_some().then_some(*peer_id)) + } + + pub(super) fn has_pending_work(&self) -> bool { + self.peers + .values() + .any(|peer_state| peer_state.matcher.is_some()) + } + + pub(super) fn advance_pending_alignments( + &mut self, + budget: Duration, + max_steps: usize, + ) -> AlignmentWorkerStepOutcome { + let started = std::time::Instant::now(); + let mut aggregate = AlignmentWorkerStepOutcome::default(); + let mut steps = 0usize; + while steps < max_steps { + let Some(peer_id) = self.next_pending_peer_id() else { + break; + }; + let outcome = self.step_peer_alignment(peer_id); + aggregate.did_work |= outcome.did_work; + aggregate.completed_cycle |= outcome.completed_cycle; + aggregate.result_changed |= outcome.result_changed; + if !outcome.did_work { + break; + } + steps += 1; + if !outcome.has_more_work { + break; + } + if steps < max_steps && !budget.is_zero() && started.elapsed() >= budget { + break; + } + } + aggregate.has_more_work = self.has_pending_work(); + aggregate + } + + fn step_peer_alignment(&mut self, peer_id: XrNetPeerId) -> AlignmentWorkerStepOutcome { + let local_descriptor = self.local_descriptor.clone(); + let local_descriptor_version = self.local_descriptor_version; + let Some(peer_state) = self.peers.get_mut(&peer_id) else { + return AlignmentWorkerStepOutcome::default(); + }; + let previous_transform = peer_state.remote_to_local; + let previous_solution = peer_state.last_accepted_solution; + let previous_diagnostic = peer_state.last_solve_diagnostic; + let Some(matcher) = peer_state.matcher.as_mut() else { + return AlignmentWorkerStepOutcome::default(); + }; + let did_work = matcher.step(); + if !did_work && !matcher.is_finished() { + return AlignmentWorkerStepOutcome::default(); + } + if !matcher.is_finished() { + return AlignmentWorkerStepOutcome { + did_work: true, + has_more_work: true, + ..AlignmentWorkerStepOutcome::default() + }; + } + + let diagnostic = peer_state + .matcher + .take() + .and_then(|matcher| matcher.diagnostic()) + .expect("finished matcher should produce a diagnostic"); + peer_state.last_solved_local_descriptor_version = + peer_state.active_local_descriptor_version; + peer_state.last_solved_remote_descriptor_seq = peer_state.active_remote_descriptor_seq; + peer_state.active_local_descriptor_version = None; + peer_state.active_remote_descriptor_seq = None; + peer_state.last_solve_ms = diagnostic.total_compute_ms as f64; + peer_state.last_solve_diagnostic = Some(diagnostic); + + let previous_scored_on_current = previous_solution.and_then(|solution| { + let local_descriptor = local_descriptor.as_ref()?; + let remote_descriptor = peer_state.latest_descriptor.as_ref()?; + Some( + local_descriptor + .descriptor + .rescore_remote_to_local(&remote_descriptor.descriptor, solution), + ) + }); + let next_solution = choose_stable_alignment_solution( + peer_state.last_accepted_solution, + previous_scored_on_current, + &diagnostic, + ); + peer_state.last_accepted_solution = next_solution; + peer_state.remote_to_local = + next_solution.map(|solution| solution.remote_to_local_transform()); + let queued_rerun = peer_state.queued_rerun; + peer_state.queued_rerun = false; + let rerun_changed = if queued_rerun { + schedule_alignment_worker_peer( + peer_state, + local_descriptor.as_ref(), + local_descriptor_version, + ) + } else { + false + }; + AlignmentWorkerStepOutcome { + did_work: true, + completed_cycle: true, + result_changed: previous_transform != peer_state.remote_to_local + || previous_solution != peer_state.last_accepted_solution + || previous_diagnostic != peer_state.last_solve_diagnostic + || rerun_changed, + has_more_work: self.has_pending_work(), + } + } + + fn make_result(&self) -> XrPeopleAlignmentWorkerResult { + XrPeopleAlignmentWorkerResult { + peer_results: self + .peers + .iter() + .map(|(peer_id, peer_state)| (*peer_id, peer_state.to_result())) + .collect(), + alignment_debug_text: make_alignment_debug_text(LocalSceneState::Ready, &self.peers), + } + } +} + +#[derive(Clone, Copy, Debug, Default)] +pub(super) struct AlignmentWorkerStepOutcome { + pub(super) did_work: bool, + pub(super) completed_cycle: bool, + pub(super) result_changed: bool, + pub(super) has_more_work: bool, +} + +fn xr_people_alignment_worker_step( + mailbox: &Arc>, + runtime: &Arc>, + latest_result: &Arc>>, +) -> XrTsdfCooperativeStepResult { + let (local_update, peer_updates) = match mailbox.lock() { + Ok(mut mailbox) => ( + mailbox.pending_local_descriptor.take(), + std::mem::take(&mut mailbox.pending_peer_updates), + ), + Err(_) => return XrTsdfCooperativeStepResult::default(), + }; + + let (step_outcome, publish_result) = match runtime.lock() { + Ok(mut state) => { + let mut result_changed = false; + let mut did_work = false; + if let Some(local_update) = local_update { + let changed = state.apply_local_descriptor_update(local_update); + result_changed |= changed; + did_work |= changed; + } + for (peer_id, update) in peer_updates { + let changed = state.apply_peer_update(peer_id, update); + result_changed |= changed; + did_work |= changed; + } + let mut step_outcome = state.advance_pending_alignments( + Duration::from_millis(XR_ALIGNMENT_CALLBACK_BUDGET_MILLIS), + XR_ALIGNMENT_CALLBACK_MAX_STEPS, + ); + step_outcome.did_work |= did_work; + step_outcome.result_changed |= result_changed; + step_outcome.has_more_work |= state.has_pending_work(); + let publish_result = (result_changed + || step_outcome.result_changed + || step_outcome.completed_cycle + || (step_outcome.did_work + && state.last_progress_publish_at.is_none_or(|last_publish| { + last_publish.elapsed() + >= Duration::from_millis(XR_ALIGNMENT_PROGRESS_SIGNAL_INTERVAL_MILLIS) + }))) + .then(|| { + state.last_progress_publish_at = Some(Instant::now()); + state.make_result() + }); + if !step_outcome.has_more_work { + state.last_progress_publish_at = None; + } + (step_outcome, publish_result) + } + Err(_) => return XrTsdfCooperativeStepResult::default(), + }; + + if let Some(result) = publish_result { + if let Ok(mut slot) = latest_result.lock() { + *slot = Some(result); + } + SignalToUI::set_ui_signal(); + } + + XrTsdfCooperativeStepResult { + did_work: step_outcome.did_work, + has_more_work: step_outcome.has_more_work, + completed_cycle: step_outcome.completed_cycle, + } +} + +fn descriptor_frames_equal( + left: Option<&XrNetAlignmentDescriptorFrame>, + right: Option<&XrNetAlignmentDescriptorFrame>, +) -> bool { + match (left, right) { + (Some(left), Some(right)) => left.descriptor == right.descriptor, + (None, None) => true, + _ => false, + } +} + +fn height_map_change_score(previous: &XrDepthAlignHeightMap, next: &XrDepthAlignHeightMap) -> f32 { + if previous.size_x != next.size_x + || previous.size_z != next.size_z + || (previous.origin_x - next.origin_x).abs() > 1.0e-4 + || (previous.origin_z - next.origin_z).abs() > 1.0e-4 + || (previous.cell_size_meters - next.cell_size_meters).abs() > 1.0e-5 + || (previous.bottom_y_meters - next.bottom_y_meters).abs() > 1.0e-4 + || (previous.top_y_meters - next.top_y_meters).abs() > 1.0e-4 + || (previous.floor_y_meters - next.floor_y_meters).abs() > 1.0e-4 + { + return 1.0; + } + let total = previous + .heights_meters + .len() + .min(next.heights_meters.len()) + .max(1); + let mut changed = 0usize; + for (left, right) in previous + .heights_meters + .iter() + .zip(next.heights_meters.iter()) + { + if left.is_finite() != right.is_finite() + || (left.is_finite() && right.is_finite() && (*left - *right).abs() >= 0.05) + { + changed += 1; + } + } + changed as f32 / total as f32 +} + +pub(super) fn descriptor_change_score( + previous: &XrDepthAlignDescriptor, + next: &XrDepthAlignDescriptor, +) -> f32 { + match (previous.height_map.as_ref(), next.height_map.as_ref()) { + (Some(previous), Some(next)) => height_map_change_score(previous, next), + (None, None) => 0.0, + (Some(_), None) | (None, Some(_)) => 1.0, + } +} + +fn schedule_alignment_worker_peer( + peer_state: &mut AlignmentWorkerPeerState, + local_descriptor: Option<&XrNetAlignmentDescriptorFrame>, + local_descriptor_version: Option<(u64, u64)>, +) -> bool { + let (Some(local_descriptor), Some(local_descriptor_version), Some(remote_descriptor)) = ( + local_descriptor, + local_descriptor_version, + peer_state.latest_descriptor.as_ref(), + ) else { + return clear_alignment_worker_peer(peer_state); + }; + + if peer_state.matcher.is_some() { + let solving_current_pair = peer_state.active_local_descriptor_version + == Some(local_descriptor_version) + && peer_state.active_remote_descriptor_seq == Some(remote_descriptor.seq); + if solving_current_pair { + return false; + } + let changed = !peer_state.queued_rerun; + peer_state.queued_rerun = true; + return changed; + } + + if peer_state.last_solved_local_descriptor_version == Some(local_descriptor_version) + && peer_state.last_solved_remote_descriptor_seq == Some(remote_descriptor.seq) + { + return false; + } + + if !descriptor_pair_ready_for_solve( + peer_state.last_solved_local_descriptor_version, + peer_state.last_solved_remote_descriptor_seq, + local_descriptor_version, + remote_descriptor.seq, + ) { + return false; + } + + peer_state.last_solve_diagnostic = None; + peer_state.last_solve_ms = 0.0; + peer_state.active_local_descriptor_version = Some(local_descriptor_version); + peer_state.active_remote_descriptor_seq = Some(remote_descriptor.seq); + peer_state.queued_rerun = false; + peer_state.matcher = Some(XrDepthAlignMatcher::new( + &local_descriptor.descriptor, + &remote_descriptor.descriptor, + peer_state.last_accepted_solution, + )); + true +} + +pub(super) fn descriptor_pair_ready_for_solve( + last_solved_local_descriptor_version: Option<(u64, u64)>, + last_solved_remote_descriptor_seq: Option, + local_descriptor_version: (u64, u64), + remote_descriptor_seq: u32, +) -> bool { + match ( + last_solved_local_descriptor_version, + last_solved_remote_descriptor_seq, + ) { + (Some(last_local), Some(last_remote)) => { + local_descriptor_version != last_local || remote_descriptor_seq != last_remote + } + _ => true, + } +} + +fn clear_alignment_worker_peer(peer_state: &mut AlignmentWorkerPeerState) -> bool { + let changed = peer_state.remote_to_local.is_some() + || peer_state.last_solve_diagnostic.is_some() + || peer_state.matcher.is_some(); + peer_state.remote_to_local = None; + peer_state.last_accepted_solution = None; + peer_state.last_solve_diagnostic = None; + peer_state.last_solve_ms = 0.0; + peer_state.last_solved_local_descriptor_version = None; + peer_state.last_solved_remote_descriptor_seq = None; + peer_state.active_local_descriptor_version = None; + peer_state.active_remote_descriptor_seq = None; + peer_state.queued_rerun = false; + peer_state.matcher = None; + changed +} + +pub(super) fn choose_stable_alignment_solution( + previous: Option, + previous_scored_on_current: Option, + diagnostic: &XrDepthAlignSolveDiagnostic, +) -> Option { + let candidate = diagnostic.accepted_solution(); + let Some(previous) = previous else { + return candidate; + }; + let previous_on_current = previous_scored_on_current.unwrap_or(previous); + let previous_still_supported = previous_on_current.is_accepted(diagnostic); + let Some(candidate) = candidate else { + return previous_still_supported.then_some(previous); + }; + let previous_score = alignment_lock_score(previous_on_current, Some(*diagnostic)); + let candidate_score = alignment_lock_score(candidate, Some(*diagnostic)); + let score_delta = candidate_score - previous_score; + if previous_still_supported && score_delta < -0.02 { + return Some(previous); + } + if previous_still_supported + && is_large_alignment_jump(previous, candidate) + && score_delta < 0.08 + { + return Some(previous); + } + if previous_still_supported + && score_delta < 0.03 + && candidate.residual_meters >= previous_on_current.residual_meters - 0.02 + && candidate.symmetry_confidence <= previous_on_current.symmetry_confidence + 0.03 + && candidate.matched_samples <= previous_on_current.matched_samples + { + return Some(previous); + } + Some(candidate) +} + +fn alignment_lock_score( + solution: XrDepthAlignSolution, + diagnostic: Option, +) -> f32 { + let (signal_factor, overlap_factor) = diagnostic + .map(|diagnostic| { + let signal_factor = (diagnostic + .local_wall_samples + .min(diagnostic.remote_wall_samples) as f32 + / 12.0) + .clamp(0.0, 1.0); + let overlap_ratio = (solution.matched_samples as f32 + / diagnostic.remote_wall_samples.max(1) as f32) + .clamp(0.0, 1.0); + (signal_factor, overlap_ratio * signal_factor.sqrt()) + }) + .unwrap_or_else(|| { + let matched = (solution.matched_samples as f32 / 4.0).clamp(0.0, 1.0); + (0.5, matched) + }); + (solution.ranking_confidence() * 0.50 + + solution.symmetry_confidence * 0.20 + + signal_factor * 0.15 + + overlap_factor * 0.15) + .clamp(0.0, 1.0) +} + +fn wrap_alignment_angle(mut angle: f32) -> f32 { + while angle <= -std::f32::consts::PI { + angle += std::f32::consts::TAU; + } + while angle > std::f32::consts::PI { + angle -= std::f32::consts::TAU; + } + angle +} + +fn is_large_alignment_jump(left: XrDepthAlignSolution, right: XrDepthAlignSolution) -> bool { + let yaw_delta = wrap_alignment_angle(left.yaw_radians - right.yaw_radians).abs(); + let translation_delta = (left.translation - right.translation).length(); + yaw_delta > 0.22 || translation_delta > 0.18 +} diff --git a/xr/src/sync/diagnostics.rs b/xr/src/sync/diagnostics.rs new file mode 100644 index 0000000000..7318998823 --- /dev/null +++ b/xr/src/sync/diagnostics.rs @@ -0,0 +1,452 @@ +use super::*; + +#[derive(Clone, Copy, Debug, PartialEq, Eq)] +pub(super) enum LocalSceneState { + Missing, + PublishPending, + Ready, +} + +pub(super) fn transform_source_label(source: RemoteTransformSource) -> &'static str { + match source { + RemoteTransformSource::Raw => "raw", + RemoteTransformSource::Anchor => "anchor", + RemoteTransformSource::Descriptor => "solved", + } +} + +pub(super) fn bool_digit(value: bool) -> char { + if value { + '1' + } else { + '0' + } +} + +pub(super) fn sync_extrema_label(extrema: XrSyncAnchorExtrema) -> &'static str { + match extrema { + XrSyncAnchorExtrema::Low => "low", + XrSyncAnchorExtrema::High => "high", + } +} + +fn option_f32_text(value: Option, decimals: usize) -> String { + value + .map(|value| format!("{value:.decimals$}")) + .unwrap_or_else(|| "--".to_string()) +} + +fn finger_name(index: usize) -> &'static str { + match index { + 0 => "idx", + 1 => "mid", + 2 => "rng", + 3 => "lit", + _ => "?", + } +} + +pub(super) fn finger_pass_bits_text(fingers: &[Option; 4]) -> String { + fingers + .iter() + .map(|finger| { + bool_digit(finger.is_some_and(|bend| bend <= XrHand::OPEN_MAX_FINGER_BEND_DEGREES)) + }) + .collect() +} + +pub(super) fn finger_bends_text(fingers: &[Option; 4]) -> String { + fingers + .iter() + .map(|finger| option_f32_text(*finger, 0)) + .collect::>() + .join("/") +} + +pub(super) fn average_bend_text(fingers: &[Option; 4]) -> String { + let bends = fingers + .iter() + .copied() + .collect::>>() + .map(|bends| bends.into_iter().sum::() / fingers.len() as f32); + option_f32_text(bends, 0) +} + +pub(super) fn open_fail_reason(fingers: &[Option; 4]) -> String { + let mut reasons = Vec::new(); + for (index, finger) in fingers.iter().enumerate() { + if finger.is_none() { + reasons.push(format!("{}:bend?", finger_name(index))); + } else if finger.is_some_and(|bend| bend > XrHand::OPEN_MAX_FINGER_BEND_DEGREES) { + reasons.push(format!("{}:bend", finger_name(index))); + } + } + if let Some(average_bend) = fingers + .iter() + .copied() + .collect::>>() + .map(|bends| bends.into_iter().sum::() / fingers.len() as f32) + { + if average_bend > XrHand::OPEN_MAX_AVERAGE_FINGER_BEND_DEGREES { + reasons.push("avg".to_string()); + } + } else { + reasons.push("avg?".to_string()); + } + if reasons.is_empty() { + "ok".to_string() + } else { + reasons.join(",") + } +} + +pub(super) fn descriptor_version_label(version: Option<(u64, u64)>) -> String { + version + .map(|(mesh_generation, update_sequence)| format!("{mesh_generation}/{update_sequence}")) + .unwrap_or_else(|| "-".to_string()) +} + +pub(super) fn descriptor_seq_label(seq: Option) -> String { + seq.map(|seq| seq.to_string()) + .unwrap_or_else(|| "-".to_string()) +} + +pub(super) fn descriptor_contour_sample_count(descriptor: &XrDepthAlignDescriptor) -> usize { + let sample_count = descriptor + .samples + .iter() + .filter(|sample| sample.kind == XrDepthAlignSampleKind::Wall) + .count(); + if sample_count != 0 { + sample_count + } else if let Some(height_map) = descriptor.height_map.as_ref() { + let valid_cells = height_map + .heights_meters + .iter() + .filter(|value| value.is_finite()) + .count(); + ((valid_cells + 127) / 128).max(1) + } else { + 0 + } +} + +fn descriptor_height_map_filled_cells(descriptor: &XrDepthAlignDescriptor) -> usize { + descriptor + .height_map + .as_ref() + .map(|height_map| { + height_map + .heights_meters + .iter() + .filter(|value| value.is_finite()) + .count() + }) + .unwrap_or(0) +} + +pub(super) fn descriptor_height_map_status(descriptor: &XrDepthAlignDescriptor) -> String { + descriptor + .height_map + .as_ref() + .map(|height_map| { + format!( + "{}x{} fill {}", + height_map.size_x, + height_map.size_z, + descriptor_height_map_filled_cells(descriptor), + ) + }) + .unwrap_or_else(|| "missing".to_string()) +} + +fn solve_outcome_label(diagnostic: Option) -> &'static str { + let Some(diagnostic) = diagnostic else { + return "pending"; + }; + match diagnostic.outcome() { + XrDepthAlignSolveOutcome::MissingSamples => "need-signal", + XrDepthAlignSolveOutcome::NoCandidate => "no-candidate", + XrDepthAlignSolveOutcome::Rejected => "rejected", + XrDepthAlignSolveOutcome::Accepted => "accepted", + } +} + +pub(super) fn make_alignment_state_text( + local_scene_state: LocalSceneState, + local_descriptor_version: Option<(u64, u64)>, + peers: &HashMap, +) -> String { + let local_descriptor_ready = matches!(local_scene_state, LocalSceneState::Ready); + let local_version = descriptor_version_label(local_descriptor_version); + let Some((peer_id, peer_state)) = peers.iter().max_by_key(|(peer_id, peer_state)| { + ( + peer_state.worker_progress.is_some(), + peer_state.last_solve_diagnostic.is_some(), + peer_state.latest_descriptor.is_some(), + peer_state.latest_state.is_some(), + std::cmp::Reverse(peer_id.0), + ) + }) else { + return format!( + "AlignState: local map {} v{} | waiting for peer map", + if local_descriptor_ready { "yes" } else { "no" }, + local_version, + ); + }; + let peer_label = format!("{:08x}", peer_id.0); + let remote_descriptor = peer_state.latest_descriptor.as_ref(); + if peer_state.worker_progress.is_some() { + return format!("AlignState {peer_label}: solving"); + } + format!( + "AlignState {peer_label}: local map {} v{} | remote map {} seq {} | worker lv{} rv{} {} match {:.1}ms | pose {}", + if local_descriptor_ready { "yes" } else { "no" }, + local_version, + if remote_descriptor.is_some() { "yes" } else { "no" }, + descriptor_seq_label(remote_descriptor.map(|frame| frame.seq)), + descriptor_version_label(peer_state.last_solved_local_descriptor_version), + descriptor_seq_label(peer_state.last_solved_remote_descriptor_seq), + solve_outcome_label(peer_state.last_solve_diagnostic), + peer_state.last_solve_ms, + transform_source_label(peer_state.transform_source), + ) +} + +pub(super) fn make_peer_scene_debug_text( + has_local_descriptor: bool, + peers: &HashMap, +) -> String { + let Some((peer_id, peer_state)) = peers.iter().max_by_key(|(peer_id, peer_state)| { + ( + peer_state.last_solve_diagnostic.is_some(), + peer_state.latest_descriptor.is_some(), + peer_state.latest_state.is_some(), + std::cmp::Reverse(peer_id.0), + ) + }) else { + return "PeerMap: waiting for peer".to_string(); + }; + let peer_label = format!("{:08x}", peer_id.0); + let state_text = if peer_state.latest_state.is_some() { + "yes" + } else { + "no" + }; + let remote_seq = + descriptor_seq_label(peer_state.latest_descriptor.as_ref().map(|frame| frame.seq)); + if let Some(diagnostic) = peer_state.last_solve_diagnostic { + return format!( + "PeerMap {peer_label}: state {state_text} | map {} seq {} | signal {} | pose {}", + if peer_state.has_descriptor { + "yes" + } else { + "no" + }, + remote_seq, + diagnostic.remote_wall_samples, + transform_source_label(peer_state.transform_source), + ); + } + if let Some(descriptor) = peer_state.latest_descriptor.as_ref() { + return format!( + "PeerMap {peer_label}: state {state_text} | map yes seq {} {} | pose {}{}", + descriptor_seq_label(Some(descriptor.seq)), + descriptor_height_map_status(&descriptor.descriptor), + transform_source_label(peer_state.transform_source), + if has_local_descriptor { + " | solve pending" + } else { + "" + }, + ); + } + format!( + "PeerMap {peer_label}: state {state_text} | map {} seq {} | pose {}{}", + if peer_state.has_descriptor { + "yes" + } else { + "no" + }, + remote_seq, + transform_source_label(peer_state.transform_source), + if has_local_descriptor && peer_state.has_descriptor { + " | solve pending" + } else { + "" + }, + ) +} + +pub(super) fn make_pending_alignment_debug_text( + local_descriptor_text: &str, + peers: &HashMap, +) -> String { + let Some((peer_id, peer_state)) = peers.iter().max_by_key(|(peer_id, peer_state)| { + ( + peer_state.last_solve_diagnostic.is_some(), + peer_state.latest_descriptor.is_some(), + peer_state.latest_state.is_some(), + std::cmp::Reverse(peer_id.0), + ) + }) else { + return format!("{local_descriptor_text} | waiting for peer heightmap"); + }; + if peer_state.last_solve_diagnostic.is_some() { + return local_descriptor_text.to_string(); + } + let peer_label = format!("{:08x}", peer_id.0); + if let Some(descriptor) = peer_state.latest_descriptor.as_ref() { + format!( + "{local_descriptor_text} | {peer_label}: remote map seq {} {} | solve pending", + descriptor.seq, + descriptor_height_map_status(&descriptor.descriptor), + ) + } else if peer_state.has_descriptor { + format!("{local_descriptor_text} | {peer_label}: solve pending") + } else { + format!("{local_descriptor_text} | {peer_label}: waiting for peer heightmap") + } +} + +pub(super) fn make_alignment_debug_text( + local_scene_state: LocalSceneState, + peers: &HashMap, +) -> String { + let Some((peer_id, peer_state)) = peers.iter().max_by_key(|(peer_id, peer_state)| { + ( + peer_state.matcher.is_some(), + peer_state.last_solve_diagnostic.is_some(), + peer_state.latest_descriptor.is_some(), + std::cmp::Reverse(peer_id.0), + ) + }) else { + return match local_scene_state { + LocalSceneState::Ready => "AlignDbg: waiting for peer heightmap".to_string(), + LocalSceneState::PublishPending => { + "AlignDbg: local heightmap ready | waiting to publish".to_string() + } + LocalSceneState::Missing => "AlignDbg: waiting for local heightmap".to_string(), + }; + }; + let peer_label = format!("{:08x}", peer_id.0); + if local_scene_state == LocalSceneState::Missing { + return format!("AlignDbg {peer_label}: waiting for local heightmap"); + } + if local_scene_state == LocalSceneState::PublishPending { + return format!("AlignDbg {peer_label}: local heightmap ready | waiting to publish"); + } + let Some(diagnostic) = peer_state.last_solve_diagnostic else { + if peer_state.latest_descriptor.is_some() { + return format!( + "AlignDbg {peer_label}: {}", + if peer_state.matcher.is_some() { + "solving" + } else { + "waiting for solve" + }, + ); + } + return format!("AlignDbg {peer_label}: waiting for peer heightmap"); + }; + match diagnostic.outcome() { + XrDepthAlignSolveOutcome::MissingSamples => format!("AlignDbg {peer_label}: need-signal"), + XrDepthAlignSolveOutcome::NoCandidate => format!("AlignDbg {peer_label}: no-candidate"), + XrDepthAlignSolveOutcome::Rejected => format!("AlignDbg {peer_label}: rejected"), + XrDepthAlignSolveOutcome::Accepted => format!("AlignDbg {peer_label}: aligned"), + } +} + +#[derive(Default)] +pub(super) struct XrPeerSyncDiagnostics { + pub(super) status: String, + pub(super) network_status: String, + pub(super) peer_scene_status: String, + pub(super) alignment_debug_status: String, + pub(super) alignment_state_status: String, +} + +impl XrPeerSyncDiagnostics { + pub(super) fn status_text(&self) -> &str { + if self.status.is_empty() { + "AlignSync: off" + } else { + &self.status + } + } + + pub(super) fn network_status_text(&self) -> &str { + if self.network_status.is_empty() { + "Network: off" + } else { + &self.network_status + } + } + + pub(super) fn alignment_debug_text(&self) -> &str { + if self.alignment_debug_status.is_empty() { + "AlignDbg: off" + } else { + &self.alignment_debug_status + } + } + + pub(super) fn alignment_state_text(&self) -> &str { + if self.alignment_state_status.is_empty() { + "AlignState: off" + } else { + &self.alignment_state_status + } + } + + pub(super) fn peer_scene_text(&self) -> &str { + if self.peer_scene_status.is_empty() { + "PeerMap: off" + } else { + &self.peer_scene_status + } + } + + pub(super) fn set_enabled_defaults( + &mut self, + auto_alignment_enabled: bool, + network_ready: bool, + ) { + if network_ready { + self.status = "AlignSync: waiting for peer heightmap".to_string(); + self.network_status = "Network: bridge ready | waiting for local XR frames".to_string(); + } else { + self.status = "AlignSync: network unavailable".to_string(); + self.network_status = "Network: bind failed".to_string(); + } + self.peer_scene_status = "PeerMap: waiting for peer".to_string(); + self.alignment_debug_status = if auto_alignment_enabled { + "AlignDbg: waiting for local heightmap".to_string() + } else { + "AlignDbg: manual sync idle".to_string() + }; + self.alignment_state_status = if auto_alignment_enabled { + "AlignState: local map no v- | waiting for peer map".to_string() + } else { + "AlignState: local anchor no | sync idle | waiting for peer".to_string() + }; + } + + pub(super) fn set_disabled(&mut self) { + self.status = "AlignSync: off".to_string(); + self.network_status = "Network: off".to_string(); + self.peer_scene_status = "PeerMap: off".to_string(); + self.alignment_debug_status = "AlignDbg: off".to_string(); + self.alignment_state_status = "AlignState: off".to_string(); + } + + pub(super) fn set_network_bind_failed(&mut self, err: &str) { + self.status = format!("AlignSync: network bind failed ({err})"); + self.network_status = format!("Network: bind failed ({err})"); + } + + pub(super) fn set_network_disconnected(&mut self) { + self.status = "AlignSync: network worker disconnected, retrying".to_string(); + self.network_status = "Network: worker disconnected".to_string(); + } +} diff --git a/xr/src/sync/lifecycle.rs b/xr/src/sync/lifecycle.rs new file mode 100644 index 0000000000..cfa0408bf2 --- /dev/null +++ b/xr/src/sync/lifecycle.rs @@ -0,0 +1,992 @@ +use super::*; + +impl XrPeerSync { + pub(super) fn touch_signal_text_for_state( + header_label: &str, + left_label: &str, + right_label: &str, + state: &XrState, + ) -> String { + let left_fingers = [ + state.left_hand.finger_bend_degrees(XrHand::INDEX_TIP), + state.left_hand.finger_bend_degrees(XrHand::MIDDLE_TIP), + state.left_hand.finger_bend_degrees(XrHand::RING_TIP), + state.left_hand.finger_bend_degrees(XrHand::LITTLE_TIP), + ]; + let right_fingers = [ + state.right_hand.finger_bend_degrees(XrHand::INDEX_TIP), + state.right_hand.finger_bend_degrees(XrHand::MIDDLE_TIP), + state.right_hand.finger_bend_degrees(XrHand::RING_TIP), + state.right_hand.finger_bend_degrees(XrHand::LITTLE_TIP), + ]; + let left_open = state.left_hand.is_open(); + let right_open = state.right_hand.is_open(); + let left_up = state.left_hand.is_upright_for_box_sync(); + let right_up = state.right_hand.is_upright_for_box_sync(); + let arms_out = Self::state_arm_corridor_ready(state); + let left_pose_ready = left_open && left_up; + let right_pose_ready = right_open && right_up; + let ready = left_pose_ready && right_pose_ready && arms_out; + format!( + "{}: open L{} R{} | up L{} R{} | arms {} | ready {}\n{}: pass {} | bend {} | avg {} | why {}\n{}: pass {} | bend {} | avg {} | why {}", + header_label, + bool_digit(left_open), + bool_digit(right_open), + bool_digit(left_up), + bool_digit(right_up), + bool_digit(arms_out), + bool_digit(ready), + left_label, + finger_pass_bits_text(&left_fingers), + finger_bends_text(&left_fingers), + average_bend_text(&left_fingers), + open_fail_reason(&left_fingers), + right_label, + finger_pass_bits_text(&right_fingers), + finger_bends_text(&right_fingers), + average_bend_text(&right_fingers), + open_fail_reason(&right_fingers), + ) + } + + pub(super) fn ensure_net_node(&mut self) { + if self.runtime.net_node.is_some() { + return; + } + let node_result = if let Some(config) = self.net_config_override.clone() { + XrNetNode::with_config(config) + } else { + XrNetNode::new() + }; + match node_result { + Ok(node) => { + self.runtime.shared_objects.set_local_peer_id(node.node_id()); + self.runtime.net_node = Some(node); + self.runtime.local.last_sent_descriptor_signature = None; + self.runtime.local.last_sent_descriptor = None; + self.runtime.local.last_sent_descriptor_at = None; + self.runtime.metrics.record_node_started(); + } + Err(err) => { + self.diagnostics.set_network_bind_failed(&err.to_string()); + } + } + } + + pub(super) fn refresh_from_local_state(&mut self, cx: &mut Cx, state: &XrState) { + if !self.enabled { + return; + } + self.ensure_net_node(); + self.runtime.local.state_time = state.time; + if self + .runtime + .recent_anchor_confirmation + .is_some_and(|confirmation| state.time > confirmation.visible_until) + { + self.runtime.recent_anchor_confirmation = None; + } + self.runtime.local.anchor = state.anchor; + self.runtime.local.sync_anchor = state.sync_anchor; + if let Some(sync_anchor) = state.sync_anchor { + self.runtime.local.record_sync_anchor(sync_anchor); + } + self.runtime.local.prune_recent_sync_anchors(); + let fresh_fist_hold_anchor = Self::state_fist_ack_anchor(state); + if let Some(anchor) = fresh_fist_hold_anchor { + self.runtime.local.fist_hold_anchor = Some(anchor); + self.runtime.local.last_fist_hold_seen_at = Some(state.time); + } else if self + .runtime + .local + .last_fist_hold_seen_at + .is_some_and(|seen_at| state.time - seen_at > Self::FIST_ACK_STICKY_WINDOW_SECONDS) + { + self.runtime.local.fist_hold_anchor = None; + self.runtime.local.last_fist_hold_seen_at = None; + self.runtime.local.sync_anchor_accumulator = None; + self.runtime.local.last_sync_match_at = None; + } + self.runtime.local.previous_xr_state = self.runtime.local.latest_xr_state.take(); + self.runtime.local.latest_xr_state = Some(state.clone()); + if let (Some(local_anchor), Some(override_anchor)) = ( + self.runtime.local.anchor, + self.runtime.local.anchor_override, + ) { + if Self::anchors_match(local_anchor, override_anchor) { + self.runtime.local.anchor_override = None; + } + } + let effective_local_anchor = self.runtime.local.effective_anchor(); + let mut broadcast_state = state.clone(); + broadcast_state.anchor = effective_local_anchor; + if let Some(net_node) = self.runtime.net_node.as_mut() { + net_node.send_state(broadcast_state); + } else { + return; + } + self.service_clock_ping(state.time); + self.runtime.metrics.tx_state_count = self.runtime.metrics.tx_state_count.saturating_add(1); + + if !self.auto_alignment_enabled { + self.runtime.local.descriptor = None; + self.runtime.local.descriptor_version = None; + self.runtime.local.slice_preview = None; + self.runtime.local.last_sent_descriptor_signature = None; + self.runtime.local.last_sent_descriptor = None; + self.runtime.local.last_sent_descriptor_at = None; + if let Some(worker) = self.runtime.alignment_worker.as_mut() { + worker.clear_local_descriptor(); + } + self.refresh_peer_transforms(cx); + return; + } + + let next_snapshot = cx.xr_tsdf().latest_tsdf_snapshot(); + let next_signature = next_snapshot + .as_ref() + .map(|snapshot| (snapshot.generation, snapshot.update_sequence)); + let next_slice_preview = next_snapshot + .as_ref() + .and_then(|snapshot| XrDepthAlignSlicePreview::from_tsdf_snapshot(snapshot.as_ref())); + let next_descriptor = next_snapshot.as_ref().and_then(|snapshot| { + XrNetAlignmentDescriptorFrame::from_tsdf_snapshot(snapshot.as_ref(), state.time) + }); + + if let (Some(signature), Some(frame)) = (next_signature, next_descriptor) { + let change_score = self + .runtime + .local + .last_sent_descriptor + .as_ref() + .map(|previous| descriptor_change_score(previous, &frame.descriptor)) + .unwrap_or(1.0); + let should_publish = self.runtime.local.last_sent_descriptor_signature + != Some(signature) + && (self.runtime.local.last_sent_descriptor.is_none() + || change_score * 100.0 >= Self::DESCRIPTOR_SEND_MIN_CHANGE_PERCENT); + if should_publish { + self.runtime.local.descriptor = Some(frame.clone()); + self.runtime.local.descriptor_version = Some(signature); + self.runtime.local.slice_preview = next_slice_preview; + if let Some(worker) = self.runtime.alignment_worker.as_mut() { + worker.set_local_descriptor(frame.clone(), signature); + } + if let Some(net_node) = self.runtime.net_node.as_mut() { + net_node.send_alignment_descriptor(frame); + } + self.runtime.local.last_sent_descriptor_signature = Some(signature); + self.runtime.local.last_sent_descriptor = self + .runtime + .local + .descriptor + .as_ref() + .map(|frame| frame.descriptor.clone()); + self.runtime.local.last_sent_descriptor_at = Some(state.time); + self.runtime.metrics.tx_descriptor_count = + self.runtime.metrics.tx_descriptor_count.saturating_add(1); + } + } else { + self.runtime.local.descriptor = None; + self.runtime.local.descriptor_version = None; + self.runtime.local.slice_preview = None; + self.runtime.local.last_sent_descriptor_signature = None; + self.runtime.local.last_sent_descriptor = None; + self.runtime.local.last_sent_descriptor_at = None; + if let Some(worker) = self.runtime.alignment_worker.as_mut() { + worker.clear_local_descriptor(); + } + } + self.refresh_peer_transforms(cx); + } + + pub(super) fn timed_event_local_time(event: &Event) -> Option { + match event { + Event::Draw(draw) => Some(draw.time), + Event::NextFrame(next_frame) => Some(next_frame.time), + _ => None, + } + } + + pub(super) fn service_non_xr_local_clock(&mut self, local_time: f64) { + if self.runtime.local.latest_xr_state.is_some() { + return; + } + self.runtime.local.state_time = local_time; + self.runtime.metrics.record_non_xr_draw_clock(); + self.ensure_net_node(); + self.service_clock_ping(local_time); + } + + fn service_clock_ping(&mut self, local_time: f64) { + if local_time < self.runtime.next_clock_ping_at { + return; + } + let Some(net_node) = self.runtime.net_node.as_mut() else { + return; + }; + let seq = self.runtime.next_clock_ping_seq; + self.runtime.next_clock_ping_seq = self.runtime.next_clock_ping_seq.wrapping_add(1); + self.runtime.next_clock_ping_at = local_time + Self::CLOCK_PING_INTERVAL_SECONDS; + self.runtime.pending_clock_pings.push_back((seq, local_time)); + while self.runtime.pending_clock_pings.len() > 32 { + self.runtime.pending_clock_pings.pop_front(); + } + net_node.send_shared_object_control(XrNetSharedObjectControl::XrClockPing { + seq, + sent_at: local_time, + }); + self.runtime.metrics.record_clock_ping_tx(seq); + } + + pub(super) fn poll_network(&mut self, cx: &mut Cx) { + if !self.enabled { + return; + } + + let mut disconnected = false; + let mut received_message = false; + loop { + let result = match self.runtime.net_node.as_mut() { + Some(net_node) => net_node.incoming_receiver.try_recv(), + None => break, + }; + match result { + Ok(message) => { + received_message = true; + self.handle_network_message(cx, message); + } + Err(TryRecvError::Empty) => break, + Err(TryRecvError::Disconnected) => { + disconnected = true; + break; + } + } + } + + if disconnected { + self.runtime.net_node = None; + self.runtime.accepted_activity = None; + self.runtime.pending_shared_object_controls.clear(); + self.runtime.applied_remote_shadow_states.clear(); + self.runtime.local.last_sent_descriptor_signature = None; + self.runtime.local.last_sent_descriptor = None; + self.runtime.local.last_sent_descriptor_at = None; + self.diagnostics.set_network_disconnected(); + } else if received_message { + self.refresh_peer_transforms(cx); + } + } + + fn handle_network_message(&mut self, cx: &mut Cx, message: XrNetIncoming) { + match message { + XrNetIncoming::Join { peer } => { + self.runtime.metrics.record_join(peer.id); + self.runtime.registry.track_join(peer); + self.runtime.local_shared_object_reannounce_needed = true; + } + XrNetIncoming::Leave { peer, .. } => { + self.runtime.metrics.record_leave(peer.id); + for widget_uid in self + .runtime + .shared_objects + .release_remote_shared_objects_by_peer_id(peer.id) + { + self.emit_remote_body_despawn(cx, widget_uid); + } + self.runtime + .applied_remote_shadow_states + .retain(|_, applied| applied.peer_id != peer.id); + self.runtime.registry.track_leave(peer.id); + self.runtime + .pending_shared_object_controls + .retain(|(pending_peer, _)| pending_peer.id != peer.id); + if let Some(worker) = self.runtime.alignment_worker.as_mut() { + worker.remove_peer(peer.id); + } + } + XrNetIncoming::State { peer, frame } => { + self.runtime.metrics.record_state(peer.id, frame.seq); + self.runtime + .registry + .track_state(peer, frame, self.runtime.local.state_time); + } + XrNetIncoming::AlignmentDescriptor { peer, frame } => { + self.runtime.metrics.record_descriptor(peer.id, frame.seq); + self.runtime.registry.track_descriptor(peer, frame.clone()); + if let Some(worker) = self.runtime.alignment_worker.as_mut() { + worker.set_peer_descriptor(peer, frame); + } + } + XrNetIncoming::Activity { peer, control } => { + let activity = control.state(); + self.runtime.metrics.record_activity_rx(peer.id, activity); + if self + .runtime + .accepted_activity + .is_none_or(|current| activity.is_newer_than(¤t)) + { + let activity_changed = self + .runtime + .accepted_activity + .is_none_or(|current| current != activity); + self.runtime.accepted_activity = Some(activity); + if activity_changed { + self.runtime.registry.clear_remote_activity_poses(); + self.runtime.pending_shared_object_controls.clear(); + cx.widget_action( + self.widget_uid(), + XrPeerSyncAction::ActivityChanged(activity.activity_id), + ); + } + } + } + XrNetIncoming::BodySpawn { peer, spawn } => { + self.runtime + .metrics + .record_body_spawn_rx(peer.id, spawn.object_id.0); + let Some(widget_uid) = self + .runtime + .shared_objects + .resolve_widget_uid(spawn.activity_id, spawn.object_id) + else { + return; + }; + self.emit_remote_body_spawn( + cx, + peer.id, + widget_uid, + true, + XrSharedObjectMode::Dynamic, + spawn.pose, + spawn.linvel, + spawn.angvel, + ); + } + XrNetIncoming::SharedObjectControl { peer, control } => { + if !self.apply_shared_object_control(cx, peer, &control) { + self.runtime + .pending_shared_object_controls + .push((peer, control)); + } + } + XrNetIncoming::SharedObjectState { peer, state } => { + let received_at_local_time = self.current_local_time(); + let state = self.normalize_incoming_shared_object_state( + peer.id, + state, + received_at_local_time, + ); + self.runtime.metrics.record_shared_object_state_rx( + peer.id, + state.object_id, + state.seq, + ); + if self + .runtime + .shared_objects + .record_remote_shared_object_state(peer.id, state) + .is_none() + { + return; + } + } + XrNetIncoming::Alignment { .. } => {} + } + } + + pub(super) fn apply_alignment_results(&mut self, cx: &mut Cx) { + let Some(worker) = self.runtime.alignment_worker.as_mut() else { + return; + }; + let Some(result) = worker.take_latest_result() else { + return; + }; + + self.runtime + .registry + .apply_alignment_results(result.peer_results); + self.diagnostics.alignment_debug_status = result.alignment_debug_text; + self.refresh_peer_transforms(cx); + } + + fn local_sync_status_text(&self) -> String { + self.runtime + .local + .active_sync_anchor() + .map(|sync| format!("armed {} {}", sync_extrema_label(sync.extrema), sync.id)) + .unwrap_or_else(|| "idle".to_string()) + } + + fn poses_match(left: Pose, right: Pose) -> bool { + let translation_delta = (left.position - right.position).length(); + let rotation_dot = left + .orientation + .dot(right.orientation) + .abs() + .clamp(0.0, 1.0); + let rotation_delta_degrees = (2.0 * rotation_dot.acos()).to_degrees(); + translation_delta <= 0.01 && rotation_delta_degrees <= 1.5 + } + + pub(super) fn emit_authoritative_remote_activity_pose_if_changed(&mut self, cx: &mut Cx) { + let Some(activity) = self.runtime.accepted_activity else { + return; + }; + if self + .runtime + .net_node + .as_ref() + .is_some_and(|node| node.node_id() == activity.changed_by) + { + return; + } + let Some(peer_state) = self.runtime.registry.peers.get_mut(&activity.changed_by) else { + return; + }; + let (Some(remote_pose), Some(remote_to_local)) = + (peer_state.remote_activity_pose, peer_state.remote_to_local) + else { + return; + }; + let local_pose = Self::transform_pose(&remote_to_local, remote_pose); + if peer_state + .last_emitted_local_activity_pose + .is_some_and(|previous| Self::poses_match(previous, local_pose)) + { + return; + } + peer_state.last_emitted_local_activity_pose = Some(local_pose); + cx.widget_action(self.widget_uid(), XrPeerSyncAction::ActivityPoseReset(local_pose)); + } + + pub(super) fn remote_state_is_recent(peer_state: &RemotePeerState, now: f64) -> bool { + now - peer_state.last_state_received_at <= Self::SYNC_MATCH_RECEIVE_WINDOW_SECONDS + } + + pub(super) fn recent_remote_sync_anchor( + peer_state: &RemotePeerState, + now: f64, + ) -> Option { + if !Self::remote_state_is_recent(peer_state, now) { + return None; + } + peer_state + .recent_sync_anchors + .back() + .filter(|recent_sync| { + now - recent_sync.last_seen_at_local_time <= Self::SYNC_MATCH_RECEIVE_WINDOW_SECONDS + }) + .map(|recent_sync| recent_sync.sync) + } + + pub(super) fn recent_remote_fist_hold_anchor( + peer_state: &RemotePeerState, + now: f64, + ) -> Option { + if !Self::remote_state_is_recent(peer_state, now) { + return None; + } + peer_state.last_fist_hold_anchor.filter(|_| { + peer_state + .last_fist_hold_seen_at + .is_some_and(|seen_at| now - seen_at <= Self::FIST_ACK_STICKY_WINDOW_SECONDS) + }) + } + + pub(super) fn manual_touch_sync_status_text(&self) -> String { + let local_sync_pending = self.runtime.local.active_sync_anchor().is_some(); + let local_fists_ready = self.runtime.local.fist_hold_anchor.is_some(); + let matched_sample_count = self.runtime.local.matched_sync_sample_count(); + let Some((_, peer_state)) = self.runtime.registry.preferred_peer() else { + return if local_sync_pending { + "Touch sync: box sample armed".to_string() + } else if local_fists_ready { + "Touch sync: hold both hands open".to_string() + } else if matched_sample_count != 0 { + format!("Touch sync: stored {matched_sample_count} samples") + } else { + "Touch sync: idle".to_string() + }; + }; + let now = self.runtime.local.state_time; + let remote_sync_pending = Self::recent_remote_sync_anchor(&peer_state, now).is_some(); + let remote_fists_ready = Self::recent_remote_fist_hold_anchor(&peer_state, now).is_some(); + + if peer_state.transform_source == RemoteTransformSource::Anchor { + return if self.runtime.recent_anchor_confirmation.is_some() { + "Touch sync: anchor set".to_string() + } else if peer_state + .latest_state + .as_ref() + .is_some_and(|state| state.state.anchor.is_some()) + { + "Touch sync: using persistent anchors".to_string() + } else { + "Touch sync: anchor aligned".to_string() + }; + } + if local_sync_pending || remote_sync_pending { + return format!("Touch sync: box sampling {matched_sample_count}"); + } + if local_fists_ready && remote_fists_ready { + return "Touch sync: both players ready".to_string(); + } + if remote_fists_ready { + return "Touch sync: remote open hands ready".to_string(); + } + if local_fists_ready { + return "Touch sync: hold both hands open".to_string(); + } + "Touch sync: idle".to_string() + } + + fn manual_peer_scene_text(&self) -> String { + let Some((peer_id, peer_state)) = self.runtime.registry.preferred_peer() else { + return "PeerScene: waiting for peer".to_string(); + }; + let state_text = if peer_state.latest_state.is_some() { + "yes" + } else { + "no" + }; + let anchor_text = peer_state + .latest_state + .as_ref() + .and_then(|state| state.state.anchor) + .map(|_| "yes") + .unwrap_or("no"); + let sync_text = Self::recent_remote_sync_anchor(&peer_state, self.runtime.local.state_time) + .map(|sync| format!("yes {} {}", sync_extrema_label(sync.extrema), sync.id)) + .unwrap_or_else(|| "no".to_string()); + format!( + "PeerScene {:08x}: state {} | anchor {} | sync {} | pose {}", + peer_id.0, + state_text, + anchor_text, + sync_text, + transform_source_label(peer_state.transform_source), + ) + } + + fn manual_alignment_state_text(&self) -> String { + let local_anchor_text = if self.runtime.local.effective_anchor().is_some() { + "yes" + } else { + "no" + }; + let local_sync_text = self.local_sync_status_text(); + let Some((peer_id, peer_state)) = self.runtime.registry.preferred_peer() else { + return format!( + "AlignState: local anchor {} | sync {} | waiting for peer", + local_anchor_text, local_sync_text + ); + }; + let remote_anchor_text = peer_state + .latest_state + .as_ref() + .and_then(|state| state.state.anchor) + .map(|_| "yes") + .unwrap_or("no"); + let remote_sync_text = + Self::recent_remote_sync_anchor(&peer_state, self.runtime.local.state_time) + .map(|sync| format!("armed {} {}", sync_extrema_label(sync.extrema), sync.id)) + .unwrap_or_else(|| "idle".to_string()); + format!( + "AlignState {:08x}: local anchor {} | local sync {} | remote anchor {} | remote sync {} | samples {} | pose {}", + peer_id.0, + local_anchor_text, + local_sync_text, + remote_anchor_text, + remote_sync_text, + self.runtime.local.matched_sync_sample_count(), + transform_source_label(peer_state.transform_source), + ) + } + + fn manual_alignment_debug_text(&self) -> String { + let local_sync = self.runtime.local.active_sync_anchor(); + let local_fist_hold = self.runtime.local.fist_hold_anchor; + let Some((peer_id, peer_state)) = self.runtime.registry.preferred_peer() else { + return match (local_sync, local_fist_hold) { + (Some(sync), _) => format!( + "AlignDbg: box sample {} {} armed | waiting for peer sample", + sync_extrema_label(sync.extrema), + sync.id + ), + (None, Some(_)) => "AlignDbg: local double-open-hands ready".to_string(), + _ => "AlignDbg: manual sync idle".to_string(), + }; + }; + let now = self.runtime.local.state_time; + let remote_sync = Self::recent_remote_sync_anchor(&peer_state, now); + let remote_fist_hold = Self::recent_remote_fist_hold_anchor(&peer_state, now); + if peer_state.transform_source == RemoteTransformSource::Anchor { + if self.runtime.recent_anchor_confirmation.is_some() + || self.runtime.local.anchor_override.is_some() + { + if let Some(sync) = local_sync { + return format!( + "AlignDbg {:08x}: matched {} {} -> averaged anchor applied ({} samples)", + peer_id.0, + sync_extrema_label(sync.extrema), + sync.id, + self.runtime.local.matched_sync_sample_count() + ); + } + return format!( + "AlignDbg {:08x}: averaged anchor active ({} samples)", + peer_id.0, + self.runtime.local.matched_sync_sample_count() + ); + } + if peer_state + .latest_state + .as_ref() + .is_some_and(|state| state.state.anchor.is_some()) + { + return format!("AlignDbg {:08x}: using saved anchors", peer_id.0); + } + if let Some(sync) = local_sync { + return format!( + "AlignDbg {:08x}: matched {} {} -> persistent anchor requested", + peer_id.0, + sync_extrema_label(sync.extrema), + sync.id + ); + } + return format!("AlignDbg {:08x}: anchor transform active", peer_id.0); + } + if let Some(sync) = remote_sync { + return format!( + "AlignDbg {:08x}: remote {} sample {} seen | match your box extrema in 100ms", + peer_id.0, + sync_extrema_label(sync.extrema), + sync.id + ); + } + if remote_fist_hold.is_some() { + return format!( + "AlignDbg {:08x}: remote double-open-hands ready | start box motion", + peer_id.0 + ); + } + if let Some(sync) = local_sync { + return format!( + "AlignDbg {:08x}: local {} sample {} armed | waiting for matching remote extrema", + peer_id.0, + sync_extrema_label(sync.extrema), + sync.id + ); + } + if local_fist_hold.is_some() { + return format!("AlignDbg {:08x}: local double-open-hands ready", peer_id.0); + } + format!("AlignDbg {:08x}: manual sync idle", peer_id.0) + } + + fn local_descriptor_debug_text(&self) -> String { + let Some(descriptor) = self + .runtime + .local + .descriptor + .as_ref() + .map(|frame| &frame.descriptor) + else { + return match self.runtime.local.scene_state() { + LocalSceneState::Missing => "AlignDbg: waiting for local heightmap".to_string(), + LocalSceneState::PublishPending => { + "AlignDbg: local heightmap ready | publish pending".to_string() + } + LocalSceneState::Ready => "AlignDbg: local map missing".to_string(), + }; + }; + let map_status = descriptor_height_map_status(descriptor); + match self.runtime.local.scene_state() { + LocalSceneState::Missing => "AlignDbg: waiting for local heightmap".to_string(), + LocalSceneState::PublishPending => format!( + "AlignDbg: local map {} | signal {} | publish pending", + map_status, + self.runtime.local.contour_sample_count(), + ), + LocalSceneState::Ready => format!( + "AlignDbg: local map {} | signal {}", + map_status, + self.runtime.local.contour_sample_count(), + ), + } + } + + pub(super) fn refresh_status(&mut self) { + if !self.enabled { + self.diagnostics.set_disabled(); + return; + } + if self.runtime.net_node.is_none() { + if self.diagnostics.status.is_empty() { + self.diagnostics.status = "AlignSync: network unavailable".to_string(); + } + if self.diagnostics.network_status.is_empty() { + self.diagnostics.network_status = "Network: unavailable".to_string(); + } + if self.diagnostics.peer_scene_status.is_empty() { + self.diagnostics.peer_scene_status = "PeerMap: network unavailable".to_string(); + } + return; + } + + let peer_count = self.runtime.registry.len(); + let visible_count = self + .runtime + .registry + .peers + .values() + .filter(|peer| peer.latest_state.is_some()) + .count(); + let descriptor_count = self + .runtime + .registry + .peers + .values() + .filter(|peer| peer.has_descriptor) + .count(); + let aligned_count = self + .runtime + .registry + .peers + .values() + .filter(|peer| peer.latest_state.is_some() && peer.remote_to_local.is_some()) + .count(); + let anchor_aligned_count = self + .runtime + .registry + .peers + .values() + .filter(|peer| { + peer.latest_state.is_some() + && peer.transform_source == RemoteTransformSource::Anchor + }) + .count(); + let local_scene_state = self.runtime.local.scene_state(); + + if !self.auto_alignment_enabled { + self.diagnostics.status = if peer_count == 0 { + "Peers: scanning LAN for clients".to_string() + } else { + format!( + "Peers: {peer_count} seen | {visible_count} state | {anchor_aligned_count} anchor-aligned" + ) + }; + let last_event = self.runtime.metrics.last_event_label(); + self.diagnostics.network_status = format!( + "Network: tx s{} d{} a{} b{} | rx j{} l{} s{} d{} a{} b{} | peers {} vis {} anchor {} | local anchor {} sync {} | objects {} | last {}", + self.runtime.metrics.tx_state_count, + self.runtime.metrics.tx_descriptor_count, + self.runtime.metrics.tx_activity_count, + self.runtime.metrics.tx_body_spawn_count, + self.runtime.metrics.rx_join_count, + self.runtime.metrics.rx_leave_count, + self.runtime.metrics.rx_state_count, + self.runtime.metrics.rx_descriptor_count, + self.runtime.metrics.rx_activity_count, + self.runtime.metrics.rx_body_spawn_count, + peer_count, + visible_count, + anchor_aligned_count, + if self.runtime.local.effective_anchor().is_some() { + "yes" + } else { + "no" + }, + self.local_sync_status_text(), + self.runtime.shared_objects.len(), + last_event, + ); + self.diagnostics.peer_scene_status = self.manual_peer_scene_text(); + self.diagnostics.alignment_state_status = self.manual_alignment_state_text(); + self.diagnostics.alignment_debug_status = self.manual_alignment_debug_text(); + return; + } + + self.diagnostics.status = if peer_count == 0 { + "AlignSync: waiting for peer heightmap".to_string() + } else if local_scene_state == LocalSceneState::Ready { + format!( + "AlignSync: peers {peer_count} | visible {visible_count} | remote maps {descriptor_count} | solved {aligned_count}" + ) + } else if local_scene_state == LocalSceneState::PublishPending { + format!( + "AlignSync: peers {peer_count} | local map signal {} ready | publish pending", + self.runtime.local.contour_sample_count() + ) + } else { + format!("AlignSync: peers {peer_count} | waiting for local heightmap") + }; + + let last_event = self.runtime.metrics.last_event_label(); + self.diagnostics.network_status = format!( + "Network: tx state {} map {} activity {} spawn {} | rx join {} leave {} state {} map {} activity {} spawn {} | peers {} vis {} maps {} solved {} | local map {} signal {} | objects {} | last {}", + self.runtime.metrics.tx_state_count, + self.runtime.metrics.tx_descriptor_count, + self.runtime.metrics.tx_activity_count, + self.runtime.metrics.tx_body_spawn_count, + self.runtime.metrics.rx_join_count, + self.runtime.metrics.rx_leave_count, + self.runtime.metrics.rx_state_count, + self.runtime.metrics.rx_descriptor_count, + self.runtime.metrics.rx_activity_count, + self.runtime.metrics.rx_body_spawn_count, + peer_count, + visible_count, + descriptor_count, + aligned_count, + match local_scene_state { + LocalSceneState::Ready => "yes", + LocalSceneState::PublishPending => "pending", + LocalSceneState::Missing => "no", + }, + self.runtime.local.contour_sample_count(), + self.runtime.shared_objects.len(), + last_event, + ); + self.diagnostics.peer_scene_status = make_peer_scene_debug_text( + local_scene_state == LocalSceneState::Ready, + &self.runtime.registry.peers, + ); + self.diagnostics.alignment_state_status = make_alignment_state_text( + local_scene_state, + self.runtime.local.descriptor_version, + &self.runtime.registry.peers, + ); + let has_alignment_diagnostic = self + .runtime + .registry + .peers + .values() + .any(|peer| peer.last_solve_diagnostic.is_some()); + let has_alignment_worker_progress = self + .runtime + .registry + .peers + .values() + .any(|peer| peer.worker_progress.is_some()); + if local_scene_state != LocalSceneState::Ready + || (!has_alignment_diagnostic && !has_alignment_worker_progress) + { + let local_descriptor_text = self.local_descriptor_debug_text(); + self.diagnostics.alignment_debug_status = if local_scene_state == LocalSceneState::Ready + { + make_pending_alignment_debug_text( + &local_descriptor_text, + &self.runtime.registry.peers, + ) + } else { + local_descriptor_text + }; + } + } + + pub(super) fn state_arm_corridor_ready(state: &XrState) -> bool { + Self::arm_corridor_ready(state.head_pose, &state.left_hand, &state.right_hand) + } + + pub(super) fn arm_corridor_ready( + head_pose: Pose, + left_hand: &XrHand, + right_hand: &XrHand, + ) -> bool { + let left_pose = left_hand.tracking_pose(); + let right_pose = right_hand.tracking_pose(); + let (Some(left_pose), Some(right_pose)) = (left_pose, right_pose) else { + return false; + }; + let Some(metrics) = arm_pair_metrics(head_pose, left_pose.position, right_pose.position) + else { + return false; + }; + if metrics.left_lateral >= metrics.right_lateral + || metrics.hand_gap < Self::FIST_ACK_MIN_HAND_GAP_METERS + || metrics.hand_gap > Self::FIST_ACK_MAX_HAND_GAP_METERS + || metrics.left_forward < Self::FIST_ACK_MIN_CHEST_DISTANCE_METERS + || metrics.left_forward > Self::FIST_ACK_MAX_CHEST_DISTANCE_METERS + || metrics.right_forward < Self::FIST_ACK_MIN_CHEST_DISTANCE_METERS + || metrics.right_forward > Self::FIST_ACK_MAX_CHEST_DISTANCE_METERS + || metrics.left_elevation_degrees > Self::FIST_ACK_MAX_ARM_ELEVATION_DEGREES + || metrics.right_elevation_degrees > Self::FIST_ACK_MAX_ARM_ELEVATION_DEGREES + || (left_pose.position.y - right_pose.position.y).abs() + > Self::FIST_ACK_MAX_VERTICAL_DELTA_METERS * 2.0 + || (metrics.left_forward - metrics.right_forward).abs() + > Self::FIST_ACK_MAX_DEPTH_DELTA_METERS * 2.0 + { + return false; + } + (Self::FIST_ACK_MIN_CHEST_DISTANCE_METERS..=Self::FIST_ACK_MAX_CHEST_DISTANCE_METERS) + .contains(&metrics.average_forward_distance) + } + + pub(super) fn state_fist_ack_anchor(state: &XrState) -> Option { + Self::fist_ack_anchor(state.head_pose, &state.left_hand, &state.right_hand) + } + + fn fist_ack_anchor(head_pose: Pose, left_hand: &XrHand, right_hand: &XrHand) -> Option { + let forward = flat_head_forward(head_pose.orientation); + let left_point = hand_closed_fist_contact_point_geometry_only(left_hand, forward, true)?; + let right_point = hand_closed_fist_contact_point_geometry_only(right_hand, forward, false)?; + let metrics = arm_pair_metrics(head_pose, left_point, right_point)?; + if metrics.left_lateral >= metrics.right_lateral + || metrics.hand_gap < Self::FIST_ACK_MIN_HAND_GAP_METERS + || metrics.hand_gap > Self::FIST_ACK_MAX_HAND_GAP_METERS + || metrics.left_forward < Self::FIST_ACK_MIN_CHEST_DISTANCE_METERS + || metrics.left_forward > Self::FIST_ACK_MAX_CHEST_DISTANCE_METERS + || metrics.right_forward < Self::FIST_ACK_MIN_CHEST_DISTANCE_METERS + || metrics.right_forward > Self::FIST_ACK_MAX_CHEST_DISTANCE_METERS + || metrics.left_elevation_degrees > Self::FIST_ACK_MAX_ARM_ELEVATION_DEGREES + || metrics.right_elevation_degrees > Self::FIST_ACK_MAX_ARM_ELEVATION_DEGREES + || (left_point.y - right_point.y).abs() > Self::FIST_ACK_MAX_VERTICAL_DELTA_METERS * 2.0 + || (metrics.left_forward - metrics.right_forward).abs() + > Self::FIST_ACK_MAX_DEPTH_DELTA_METERS * 2.0 + { + return None; + } + if !(Self::FIST_ACK_MIN_CHEST_DISTANCE_METERS..=Self::FIST_ACK_MAX_CHEST_DISTANCE_METERS) + .contains(&metrics.average_forward_distance) + { + return None; + } + Some(XrAnchor { + left: left_point, + right: right_point, + }) + } + + fn anchors_match(left: XrAnchor, right: XrAnchor) -> bool { + (left.left - right.left).length() <= 0.025 && (left.right - right.right).length() <= 0.025 + } + + fn refresh_peer_transforms(&mut self, cx: &mut Cx) { + if let (Some(local_anchor), Some(override_anchor)) = ( + self.runtime.local.anchor, + self.runtime.local.anchor_override, + ) { + if Self::anchors_match(local_anchor, override_anchor) { + self.runtime.local.anchor_override = None; + } + } + + let now = self.runtime.local.state_time; + let changed = { + let (registry, local) = (&mut self.runtime.registry, &mut self.runtime.local); + registry.refresh_transforms( + cx, + local, + &mut self.runtime.recent_anchor_confirmation, + now, + ) + }; + + if changed { + self.redraw(cx); + } + self.emit_authoritative_remote_activity_pose_if_changed(cx); + } +} diff --git a/xr/src/sync/local_state.rs b/xr/src/sync/local_state.rs new file mode 100644 index 0000000000..6510138048 --- /dev/null +++ b/xr/src/sync/local_state.rs @@ -0,0 +1,276 @@ +use super::*; + +#[derive(Clone, Copy, Debug, Default, PartialEq, Eq)] +pub(super) enum RemoteTransformSource { + #[default] + Raw, + Anchor, + Descriptor, +} + +#[derive(Clone, Copy, Debug)] +pub(super) struct LocalSharedHandState { + pub(super) shared_hand: XrSharedHand, + pub(super) pose: Pose, + pub(super) linvel: Vec3f, + pub(super) gripping: bool, +} + +#[derive(Clone, Copy, Debug)] +pub(super) struct TimedRemoteSyncAnchor { + pub(super) sync: XrSyncAnchor, + pub(super) first_seen_at_local_time: f64, + pub(super) last_seen_at_local_time: f64, +} + +#[derive(Clone, Copy, Debug)] +pub(super) struct TimedLocalSyncAnchor { + pub(super) sync: XrSyncAnchor, + pub(super) last_seen_at_local_time: f64, +} + +#[derive(Clone, Copy, Debug, Default)] +pub(super) struct XrSyncAnchorAccumulator { + sum_left: Vec3f, + sum_right: Vec3f, + pub(super) sample_count: u32, +} + +impl XrSyncAnchorAccumulator { + fn push(&mut self, anchor: XrAnchor) -> XrAnchor { + self.sum_left += anchor.left; + self.sum_right += anchor.right; + self.sample_count = self.sample_count.saturating_add(1); + XrAnchor { + left: self.sum_left / self.sample_count as f32, + right: self.sum_right / self.sample_count as f32, + } + } +} + +#[derive(Clone, Debug)] +pub(super) struct RemotePeerState { + pub(super) peer: XrNetPeer, + pub(super) latest_state: Option, + pub(super) last_state_received_at: f64, + pub(super) last_sync_anchor_seen_at: Option, + pub(super) recent_sync_anchors: VecDeque, + pub(super) last_fist_hold_anchor: Option, + pub(super) last_fist_hold_seen_at: Option, + pub(super) remote_activity_pose: Option, + pub(super) last_emitted_local_activity_pose: Option, + pub(super) latest_descriptor: Option, + pub(super) has_descriptor: bool, + pub(super) anchor_remote_to_local: Option, + pub(super) descriptor_remote_to_local: Option, + pub(super) remote_to_local: Option, + pub(super) transform_source: RemoteTransformSource, + pub(super) last_solve_diagnostic: Option, + pub(super) last_solve_ms: f64, + pub(super) last_solved_local_descriptor_version: Option<(u64, u64)>, + pub(super) last_solved_remote_descriptor_seq: Option, + pub(super) worker_progress: Option, + pub(super) clock_offset_seconds: Option, + pub(super) clock_round_trip_seconds: Option, + pub(super) last_clock_sync_at: Option, +} + +impl RemotePeerState { + pub(super) fn new(peer: XrNetPeer) -> Self { + Self { + peer, + latest_state: None, + last_state_received_at: 0.0, + last_sync_anchor_seen_at: None, + recent_sync_anchors: VecDeque::new(), + last_fist_hold_anchor: None, + last_fist_hold_seen_at: None, + remote_activity_pose: None, + last_emitted_local_activity_pose: None, + latest_descriptor: None, + has_descriptor: false, + anchor_remote_to_local: None, + descriptor_remote_to_local: None, + remote_to_local: None, + transform_source: RemoteTransformSource::Raw, + last_solve_diagnostic: None, + last_solve_ms: 0.0, + last_solved_local_descriptor_version: None, + last_solved_remote_descriptor_seq: None, + worker_progress: None, + clock_offset_seconds: None, + clock_round_trip_seconds: None, + last_clock_sync_at: None, + } + } +} + +#[derive(Default)] +pub(super) struct XrPeerSyncLocalState { + pub(super) state_time: f64, + pub(super) anchor: Option, + pub(super) anchor_override: Option, + pub(super) sync_anchor: Option, + pub(super) last_sync_anchor_seen_at: Option, + pub(super) recent_sync_anchors: VecDeque, + pub(super) sync_anchor_accumulator: Option, + pub(super) last_sync_match_at: Option, + pub(super) fist_hold_anchor: Option, + pub(super) last_fist_hold_seen_at: Option, + pub(super) previous_xr_state: Option, + pub(super) latest_xr_state: Option, + pub(super) descriptor: Option, + pub(super) descriptor_version: Option<(u64, u64)>, + pub(super) slice_preview: Option, + pub(super) last_sent_descriptor_signature: Option<(u64, u64)>, + pub(super) last_sent_descriptor: Option, + pub(super) last_sent_descriptor_at: Option, +} + +impl XrPeerSyncLocalState { + fn far_relocated_anchor_override( + saved_anchor: XrAnchor, + motion_center: Vec3f, + ) -> Option { + let left_distance = (motion_center - saved_anchor.left).length(); + let right_distance = (motion_center - saved_anchor.right).length(); + let replace_left = left_distance <= right_distance; + let preserved_distance = if replace_left { + right_distance + } else { + left_distance + }; + if preserved_distance <= XrPeerSync::SYNC_EXISTING_MARKER_REUSE_DISTANCE_METERS { + return None; + } + Some(if replace_left { + XrAnchor { + left: motion_center, + right: saved_anchor.right, + } + } else { + XrAnchor { + left: saved_anchor.left, + right: motion_center, + } + }) + } + + pub(super) fn record_sync_anchor(&mut self, sync_anchor: XrSyncAnchor) { + self.last_sync_anchor_seen_at = Some(self.state_time); + if let Some(recent) = self + .recent_sync_anchors + .back_mut() + .filter(|recent| recent.sync.id == sync_anchor.id) + { + recent.last_seen_at_local_time = self.state_time; + return; + } + self.recent_sync_anchors.push_back(TimedLocalSyncAnchor { + sync: sync_anchor, + last_seen_at_local_time: self.state_time, + }); + } + + pub(super) fn prune_recent_sync_anchors(&mut self) { + while self.recent_sync_anchors.front().is_some_and(|sync| { + self.state_time - sync.last_seen_at_local_time > XrPeerSync::SYNC_SAMPLE_HISTORY_SECONDS + }) { + self.recent_sync_anchors.pop_front(); + } + } + + pub(super) fn record_matched_sync_anchor(&mut self, anchor: XrAnchor, now: f64) -> XrAnchor { + if self + .last_sync_match_at + .is_some_and(|last| now - last > XrPeerSync::SYNC_SAMPLE_SESSION_RESET_SECONDS) + { + self.sync_anchor_accumulator = None; + } + let mut accumulator = self.sync_anchor_accumulator.unwrap_or_default(); + let averaged_anchor = accumulator.push(anchor); + self.sync_anchor_accumulator = Some(accumulator); + self.last_sync_match_at = Some(now); + if let Some(saved_anchor) = self.anchor { + let averaged_motion_center = (averaged_anchor.left + averaged_anchor.right) * 0.5; + if let Some(override_anchor) = + Self::far_relocated_anchor_override(saved_anchor, averaged_motion_center) + { + return override_anchor; + } + } + averaged_anchor + } + + pub(super) fn matched_sync_sample_count(&self) -> u32 { + self.sync_anchor_accumulator + .map(|accumulator| accumulator.sample_count) + .unwrap_or(0) + } + + pub(super) fn effective_anchor(&self) -> Option { + self.anchor_override.or(self.anchor) + } + + pub(super) fn active_sync_anchor(&self) -> Option { + self.sync_anchor.filter(|_| { + self.last_sync_anchor_seen_at.is_some_and(|seen_at| { + self.state_time - seen_at <= XrPeerSync::SYNC_MATCH_ACTIVE_WINDOW_SECONDS + }) + }) + } + + pub(super) fn scene_state(&self) -> LocalSceneState { + if self.descriptor.is_some() { + LocalSceneState::Ready + } else if self.contour_sample_count() != 0 { + LocalSceneState::PublishPending + } else { + LocalSceneState::Missing + } + } + + pub(super) fn contour_sample_count(&self) -> usize { + self.descriptor + .as_ref() + .map(|frame| descriptor_contour_sample_count(&frame.descriptor)) + .unwrap_or(0) + } +} + +#[derive(Default)] +pub(super) struct XrPeerSyncRuntime { + pub(super) net_node: Option, + pub(super) alignment_worker: Option, + pub(super) local: XrPeerSyncLocalState, + pub(super) registry: XrPeerRegistry, + pub(super) recent_anchor_confirmation: Option, + pub(super) shared_objects: XrSharedObjectRegistry, + pub(super) next_shared_object_physics_tick: u32, + pub(super) next_shared_object_request_id: u32, + pub(super) applied_remote_shadow_states: HashMap, + pub(super) pending_shared_object_controls: Vec<(XrNetPeer, XrNetSharedObjectControl)>, + pub(super) pending_clock_pings: VecDeque<(u32, f64)>, + pub(super) next_clock_ping_seq: u32, + pub(super) next_clock_ping_at: f64, + pub(super) accepted_activity: Option, + pub(super) local_shared_object_reannounce_needed: bool, + pub(super) metrics: XrPeerSyncMetrics, +} + +#[derive(Clone, Copy, Debug, PartialEq)] +pub(super) struct XrAppliedRemoteShadowState { + pub(super) peer_id: XrNetPeerId, + pub(super) applied_at_local_time: f64, + pub(super) state_seq: Option, + pub(super) mode: XrSharedObjectMode, + pub(super) pose: Pose, + pub(super) linvel: Vec3f, + pub(super) angvel: Vec3f, +} + +#[derive(Clone, Copy, Debug, PartialEq)] +pub(super) struct XrRecentAnchorConfirmation { + pub(super) anchor: XrAnchor, + pub(super) visible_until: f64, +} diff --git a/xr/src/sync/metrics.rs b/xr/src/sync/metrics.rs new file mode 100644 index 0000000000..cb35a9bb46 --- /dev/null +++ b/xr/src/sync/metrics.rs @@ -0,0 +1,147 @@ +use super::*; + +#[derive(Default)] +pub(super) struct XrPeerSyncMetrics { + pub(super) tx_state_count: u64, + pub(super) tx_descriptor_count: u64, + pub(super) tx_activity_count: u64, + pub(super) tx_body_spawn_count: u64, + pub(super) tx_shared_object_state_count: u64, + pub(super) tx_clock_ping_count: u64, + pub(super) tx_clock_pong_count: u64, + pub(super) rx_join_count: u64, + pub(super) rx_leave_count: u64, + pub(super) rx_state_count: u64, + pub(super) rx_descriptor_count: u64, + pub(super) rx_activity_count: u64, + pub(super) rx_body_spawn_count: u64, + pub(super) rx_shared_object_state_count: u64, + pub(super) rx_clock_ping_count: u64, + pub(super) rx_clock_pong_count: u64, + pub(super) non_xr_draw_clock_count: u64, + pub(super) remote_shadow_apply_count: u64, + last_event_text: String, +} + +impl XrPeerSyncMetrics { + pub(super) fn record_node_started(&mut self) { + self.last_event_text = "node started".to_string(); + } + + pub(super) fn record_join(&mut self, peer_id: XrNetPeerId) { + self.rx_join_count = self.rx_join_count.saturating_add(1); + self.last_event_text = format!("join {}", XrPeerSync::peer_label(peer_id)); + } + + pub(super) fn record_leave(&mut self, peer_id: XrNetPeerId) { + self.rx_leave_count = self.rx_leave_count.saturating_add(1); + self.last_event_text = format!("leave {}", XrPeerSync::peer_label(peer_id)); + } + + pub(super) fn record_state(&mut self, peer_id: XrNetPeerId, seq: u32) { + self.rx_state_count = self.rx_state_count.saturating_add(1); + self.last_event_text = format!("state {} seq {}", XrPeerSync::peer_label(peer_id), seq); + } + + pub(super) fn record_descriptor(&mut self, peer_id: XrNetPeerId, seq: u32) { + self.rx_descriptor_count = self.rx_descriptor_count.saturating_add(1); + self.last_event_text = format!("desc {} seq {}", XrPeerSync::peer_label(peer_id), seq); + } + + pub(super) fn record_activity_tx(&mut self, activity: XrNetActivityState) { + self.tx_activity_count = self.tx_activity_count.saturating_add(1); + self.last_event_text = format!( + "tx activity {} tick {}", + activity.activity_id.to_live_id().0, + activity.changed_tick + ); + } + + pub(super) fn record_activity_rx(&mut self, peer_id: XrNetPeerId, activity: XrNetActivityState) { + self.rx_activity_count = self.rx_activity_count.saturating_add(1); + self.last_event_text = format!( + "activity {} {} tick {}", + XrPeerSync::peer_label(peer_id), + activity.activity_id.to_live_id().0, + activity.changed_tick + ); + } + + pub(super) fn record_body_spawn_tx(&mut self, spawn_label: u64) { + self.tx_body_spawn_count = self.tx_body_spawn_count.saturating_add(1); + self.last_event_text = format!("tx spawn {:016x}", spawn_label); + } + + pub(super) fn record_shared_object_state_tx(&mut self, object_id: XrSharedObjectId, seq: u32) { + self.tx_shared_object_state_count = self.tx_shared_object_state_count.saturating_add(1); + self.last_event_text = format!("tx shared {:016x} seq {seq}", object_id.0); + } + + pub(super) fn record_body_spawn_rx(&mut self, peer_id: XrNetPeerId, spawn_label: u64) { + self.rx_body_spawn_count = self.rx_body_spawn_count.saturating_add(1); + self.last_event_text = format!( + "spawn {} {:016x}", + XrPeerSync::peer_label(peer_id), + spawn_label + ); + } + + pub(super) fn record_shared_object_state_rx( + &mut self, + peer_id: XrNetPeerId, + object_id: XrSharedObjectId, + seq: u32, + ) { + self.rx_shared_object_state_count = self.rx_shared_object_state_count.saturating_add(1); + self.last_event_text = format!( + "shared {} {:016x} seq {seq}", + XrPeerSync::peer_label(peer_id), + object_id.0 + ); + } + + pub(super) fn record_clock_ping_tx(&mut self, seq: u32) { + self.tx_clock_ping_count = self.tx_clock_ping_count.saturating_add(1); + self.last_event_text = format!("tx clock ping {seq}"); + } + + pub(super) fn record_clock_ping_rx(&mut self, peer_id: XrNetPeerId, seq: u32) { + self.rx_clock_ping_count = self.rx_clock_ping_count.saturating_add(1); + self.last_event_text = format!("clock ping {} {seq}", XrPeerSync::peer_label(peer_id)); + } + + pub(super) fn record_clock_pong_tx(&mut self, seq: u32) { + self.tx_clock_pong_count = self.tx_clock_pong_count.saturating_add(1); + self.last_event_text = format!("tx clock pong {seq}"); + } + + pub(super) fn record_clock_pong_rx(&mut self, peer_id: XrNetPeerId, seq: u32) { + self.rx_clock_pong_count = self.rx_clock_pong_count.saturating_add(1); + self.last_event_text = format!("clock pong {} {seq}", XrPeerSync::peer_label(peer_id)); + } + + pub(super) fn record_non_xr_draw_clock(&mut self) { + self.non_xr_draw_clock_count = self.non_xr_draw_clock_count.saturating_add(1); + } + + pub(super) fn record_remote_shadow_apply( + &mut self, + object_id: XrSharedObjectId, + seq: Option, + ) { + self.remote_shadow_apply_count = self.remote_shadow_apply_count.saturating_add(1); + self.last_event_text = if let Some(seq) = seq { + format!("shadow {:016x} seq {seq}", object_id.0) + } else { + format!("shadow {:016x}", object_id.0) + }; + } + + pub(super) fn last_event_label(&self) -> &str { + if self.last_event_text.is_empty() { + "none" + } else { + &self.last_event_text + } + } +} diff --git a/xr/src/sync/mod.rs b/xr/src/sync/mod.rs new file mode 100644 index 0000000000..6e646bac5f --- /dev/null +++ b/xr/src/sync/mod.rs @@ -0,0 +1 @@ +pub mod xr_peer_sync; diff --git a/xr/src/sync/registry.rs b/xr/src/sync/registry.rs new file mode 100644 index 0000000000..265cd9ccf8 --- /dev/null +++ b/xr/src/sync/registry.rs @@ -0,0 +1,281 @@ +use super::*; + +#[derive(Default)] +pub(super) struct XrPeerRegistry { + pub(super) peers: HashMap, + accepted_local_sync_ids: HashMap, + accepted_remote_sync_ids: HashMap<(XrNetPeerId, u32), f64>, +} + +impl XrPeerRegistry { + fn descriptor_with_anchor_height_override( + descriptor_remote_to_local: Mat4f, + anchor_remote_to_local: Option, + ) -> Mat4f { + let Some(anchor_remote_to_local) = anchor_remote_to_local else { + return descriptor_remote_to_local; + }; + let mut corrected = descriptor_remote_to_local; + corrected.v[13] = anchor_remote_to_local.v[13]; + corrected + } + + pub(super) fn len(&self) -> usize { + self.peers.len() + } + + pub(super) fn peer_ids(&self) -> Vec { + self.peers.keys().copied().collect() + } + + pub(super) fn preferred_peer(&self) -> Option<(XrNetPeerId, RemotePeerState)> { + self.peers + .iter() + .max_by_key(|(peer_id, peer_state)| { + ( + peer_state.remote_to_local.is_some(), + peer_state.latest_state.is_some(), + std::cmp::Reverse(peer_id.0), + ) + }) + .map(|(peer_id, peer_state)| (*peer_id, peer_state.clone())) + } + + pub(super) fn track_join(&mut self, peer: XrNetPeer) { + self.peers + .entry(peer.id) + .or_insert_with(|| RemotePeerState::new(peer)); + } + + pub(super) fn track_leave(&mut self, peer_id: XrNetPeerId) { + self.peers.remove(&peer_id); + self.accepted_remote_sync_ids + .retain(|(accepted_peer_id, _), _| *accepted_peer_id != peer_id); + } + + pub(super) fn clear_remote_activity_poses(&mut self) { + for peer_state in self.peers.values_mut() { + peer_state.remote_activity_pose = None; + peer_state.last_emitted_local_activity_pose = None; + } + } + + pub(super) fn track_state( + &mut self, + peer: XrNetPeer, + frame: XrNetStateFrame, + local_state_time: f64, + ) { + let peer_state = self + .peers + .entry(peer.id) + .or_insert_with(|| RemotePeerState::new(peer)); + let fresh_fist_hold_anchor = XrPeerSync::state_fist_ack_anchor(&frame.state); + peer_state.peer = peer; + peer_state.last_state_received_at = local_state_time; + if let Some(sync_anchor) = frame.state.sync_anchor { + peer_state.last_sync_anchor_seen_at = Some(local_state_time); + if let Some(recent_sync) = peer_state + .recent_sync_anchors + .back_mut() + .filter(|recent_sync| recent_sync.sync.id == sync_anchor.id) + { + recent_sync.last_seen_at_local_time = local_state_time; + } else { + peer_state.recent_sync_anchors.push_back(TimedRemoteSyncAnchor { + sync: sync_anchor, + first_seen_at_local_time: local_state_time, + last_seen_at_local_time: local_state_time, + }); + } + } + if let Some(anchor) = fresh_fist_hold_anchor { + peer_state.last_fist_hold_anchor = Some(anchor); + peer_state.last_fist_hold_seen_at = Some(local_state_time); + } else if peer_state.last_fist_hold_seen_at.is_some_and(|seen_at| { + local_state_time - seen_at > XrPeerSync::FIST_ACK_STICKY_WINDOW_SECONDS + }) { + peer_state.last_fist_hold_anchor = None; + peer_state.last_fist_hold_seen_at = None; + peer_state.recent_sync_anchors.clear(); + } + while peer_state.recent_sync_anchors.front().is_some_and(|recent_sync| { + local_state_time - recent_sync.last_seen_at_local_time + > XrPeerSync::SYNC_SAMPLE_HISTORY_SECONDS + }) { + peer_state.recent_sync_anchors.pop_front(); + } + peer_state.latest_state = Some(frame); + } + + pub(super) fn track_descriptor(&mut self, peer: XrNetPeer, frame: XrNetAlignmentDescriptorFrame) { + let peer_state = self + .peers + .entry(peer.id) + .or_insert_with(|| RemotePeerState::new(peer)); + peer_state.peer = peer; + peer_state.latest_descriptor = Some(frame); + peer_state.has_descriptor = true; + } + + pub(super) fn apply_alignment_results( + &mut self, + peer_results: HashMap, + ) { + for peer_state in self.peers.values_mut() { + peer_state.descriptor_remote_to_local = None; + } + for (peer_id, peer_result) in peer_results { + if let Some(peer_state) = self.peers.get_mut(&peer_id) { + peer_state.descriptor_remote_to_local = peer_result.remote_to_local; + peer_state.last_solve_diagnostic = peer_result.last_solve_diagnostic; + peer_state.last_solve_ms = peer_result.last_solve_ms; + peer_state.last_solved_local_descriptor_version = + peer_result.last_solved_local_descriptor_version; + peer_state.last_solved_remote_descriptor_seq = + peer_result.last_solved_remote_descriptor_seq; + peer_state.worker_progress = peer_result.worker_progress; + peer_state.has_descriptor = + peer_state.has_descriptor || peer_result.last_solve_diagnostic.is_some(); + } + } + } + + fn prune_accepted_sync_ids(&mut self, now: f64) { + self.accepted_local_sync_ids.retain(|_, accepted_at| { + now - *accepted_at <= XrPeerSync::SYNC_SAMPLE_HISTORY_SECONDS + }); + self.accepted_remote_sync_ids.retain(|_, accepted_at| { + now - *accepted_at <= XrPeerSync::SYNC_SAMPLE_HISTORY_SECONDS + }); + } + + fn translated_remote_sync_time( + peer_state: &RemotePeerState, + remote_sync: &TimedRemoteSyncAnchor, + ) -> f64 { + peer_state + .clock_offset_seconds + .map(|clock_offset| remote_sync.sync.captured_at - clock_offset) + .unwrap_or(remote_sync.first_seen_at_local_time) + } + + fn best_sync_match( + peer_id: XrNetPeerId, + peer_state: &RemotePeerState, + accepted_local_sync_ids: &HashMap, + accepted_remote_sync_ids: &HashMap<(XrNetPeerId, u32), f64>, + local: &XrPeerSyncLocalState, + now: f64, + ) -> Option<(XrSyncAnchor, TimedRemoteSyncAnchor)> { + if !XrPeerSync::remote_state_is_recent(peer_state, now) + || local.fist_hold_anchor.is_none() + || XrPeerSync::recent_remote_fist_hold_anchor(peer_state, now).is_none() + { + return None; + } + + let mut best_match = None; + let mut best_dt = f64::MAX; + for local_sync in local.recent_sync_anchors.iter().rev() { + if accepted_local_sync_ids.contains_key(&local_sync.sync.id) { + continue; + } + for remote_sync in peer_state.recent_sync_anchors.iter().rev() { + if accepted_remote_sync_ids.contains_key(&(peer_id, remote_sync.sync.id)) + || local_sync.sync.extrema == remote_sync.sync.extrema + { + continue; + } + let remote_sync_local_time = + Self::translated_remote_sync_time(peer_state, remote_sync); + let dt = (local_sync.sync.captured_at - remote_sync_local_time).abs(); + if dt > XrPeerSync::SYNC_SAMPLE_PAIR_WINDOW_SECONDS || dt >= best_dt { + continue; + } + best_match = Some((local_sync.sync, *remote_sync)); + best_dt = dt; + } + } + best_match + } + + pub(super) fn refresh_transforms( + &mut self, + cx: &mut Cx, + local: &mut XrPeerSyncLocalState, + recent_anchor_confirmation: &mut Option, + now: f64, + ) -> bool { + let mut changed = false; + self.prune_accepted_sync_ids(now); + + for (peer_id, peer_state) in self.peers.iter_mut() { + peer_state.anchor_remote_to_local = None; + if let Some((local_sync_anchor, remote_sync_anchor)) = { + let accepted_local_sync_ids = &self.accepted_local_sync_ids; + let accepted_remote_sync_ids = &self.accepted_remote_sync_ids; + Self::best_sync_match( + *peer_id, + peer_state, + accepted_local_sync_ids, + accepted_remote_sync_ids, + local, + now, + ) + } { + self.accepted_local_sync_ids + .insert(local_sync_anchor.id, now); + self.accepted_remote_sync_ids + .insert((*peer_id, remote_sync_anchor.sync.id), now); + let averaged_anchor = local.record_matched_sync_anchor(local_sync_anchor.anchor, now); + local.anchor_override = Some(averaged_anchor); + cx.xr_set_local_anchor(averaged_anchor); + *recent_anchor_confirmation = Some(XrRecentAnchorConfirmation { + anchor: averaged_anchor, + visible_until: now + XrPeerSync::ANCHOR_CONFIRMATION_SECONDS, + }); + changed = true; + } + + if peer_state.anchor_remote_to_local.is_none() { + let effective_local_anchor = local.effective_anchor(); + if let (Some(local_anchor), Some(state_frame)) = + (effective_local_anchor, peer_state.latest_state.as_ref()) + { + if let Some(remote_anchor) = state_frame.state.anchor { + peer_state.anchor_remote_to_local = + Some(remote_anchor.mirrored().mapping_to(&local_anchor)); + } + } + } + + let next_transform = if let Some(descriptor_remote_to_local) = + peer_state.descriptor_remote_to_local + { + Some(Self::descriptor_with_anchor_height_override( + descriptor_remote_to_local, + peer_state.anchor_remote_to_local, + )) + } else { + peer_state.anchor_remote_to_local + }; + let next_source = if peer_state.descriptor_remote_to_local.is_some() { + RemoteTransformSource::Descriptor + } else if peer_state.anchor_remote_to_local.is_some() { + RemoteTransformSource::Anchor + } else { + RemoteTransformSource::Raw + }; + if peer_state.remote_to_local != next_transform + || peer_state.transform_source != next_source + { + peer_state.remote_to_local = next_transform; + peer_state.transform_source = next_source; + changed = true; + } + } + + changed + } +} diff --git a/xr/src/sync/render.rs b/xr/src/sync/render.rs new file mode 100644 index 0000000000..e820e3ccdf --- /dev/null +++ b/xr/src/sync/render.rs @@ -0,0 +1,489 @@ +use super::*; + +impl XrPeerSync { + fn peer_base_color(peer_id: XrNetPeerId) -> Vec4f { + match (peer_id.0 % 5) as usize { + 0 => vec4f(0.92, 0.38, 0.31, 1.0), + 1 => vec4f(0.24, 0.74, 0.58, 1.0), + 2 => vec4f(0.35, 0.58, 0.98, 1.0), + 3 => vec4f(0.93, 0.70, 0.28, 1.0), + _ => vec4f(0.80, 0.48, 0.94, 1.0), + } + } + + fn peer_alpha(peer: &RemotePeerState) -> f32 { + match peer.transform_source { + RemoteTransformSource::Anchor | RemoteTransformSource::Descriptor => 1.0, + RemoteTransformSource::Raw => 0.42, + } + } + + fn draw_cube_at( + &mut self, + cx: &mut Cx3d, + world: &Mat4f, + pose_transform: &Mat4f, + size: Vec3f, + color: Vec4f, + ) { + self.draw_cube.transform = Mat4f::mul(world, pose_transform); + self.draw_cube.cube_pos = vec3(0.0, 0.0, 0.0); + self.draw_cube.cube_size = size; + self.draw_cube.color = color; + self.draw_cube.depth_clip = 1.0; + self.draw_cube.draw(cx); + } + + fn descriptor_height_meters(height_u8: u8) -> f32 { + if height_u8 == 0 { + Self::DESCRIPTOR_MIN_HEIGHT_METERS + } else { + ((height_u8 as f32 / 255.0) * Self::DESCRIPTOR_MAX_HEIGHT_METERS).clamp( + Self::DESCRIPTOR_MIN_HEIGHT_METERS, + Self::DESCRIPTOR_MAX_HEIGHT_METERS, + ) + } + } + + pub(super) fn draw_local_descriptor(&mut self, cx: &mut Cx3d, world: &Mat4f) { + let Some(vertical) = self + .runtime + .local + .descriptor + .as_ref() + .and_then(|frame| frame.descriptor.vertical_descriptor.as_ref()) + .cloned() + else { + return; + }; + let size = vertical.size as usize; + if size == 0 + || vertical.vertical_surface_masks.len() != size * size + || vertical.clutter_surface_masks.len() != size * size + || vertical.height_u8.len() != size * size + { + return; + } + + let cell_size = vertical.cell_size_meters; + let footprint = cell_size * Self::DESCRIPTOR_CELL_FOOTPRINT; + for z in 0..size { + for x in 0..size { + let index = x + z * size; + let vertical_count = vertical.vertical_surface_masks[index].count_ones() as f32; + let clutter_count = vertical.clutter_surface_masks[index].count_ones() as f32; + if vertical_count <= 0.0 && clutter_count <= 0.0 { + continue; + } + let center_x = vertical.origin_x + (x as f32 + 0.5) * cell_size; + let center_z = vertical.origin_z + (z as f32 + 0.5) * cell_size; + let height = Self::descriptor_height_meters(vertical.height_u8[index]); + let weight = (vertical_count + clutter_count).max(1.0); + let vertical_mix = vertical_count / weight; + let clutter_mix = clutter_count / weight; + let alpha = (0.16 + 0.06 * weight.min(4.0)).clamp(0.16, 0.40); + let color = vec4f( + 0.14 + 0.18 * clutter_mix, + 0.42 + 0.36 * clutter_mix, + 0.96 - 0.48 * clutter_mix, + alpha, + ); + let transform = + Pose::new(Quat::default(), vec3(center_x, height * 0.5, center_z)).to_mat4(); + self.draw_cube_at( + cx, + world, + &transform, + vec3(footprint, height, footprint), + vec4f( + color.x + 0.06 * vertical_mix, + color.y, + color.z + 0.04 * vertical_mix, + color.w, + ), + ); + } + } + } + + fn draw_anchor_markers( + &mut self, + cx: &mut Cx3d, + world: &Mat4f, + anchor: XrAnchor, + size: f32, + left_color: Vec4f, + right_color: Vec4f, + ) { + let left_transform = Pose::new(anchor.to_quat(), anchor.left).to_mat4(); + self.draw_cube_at( + cx, + world, + &left_transform, + vec3(size, size, size), + left_color, + ); + let right_transform = Pose::new(anchor.to_quat_rev(), anchor.right).to_mat4(); + self.draw_cube_at( + cx, + world, + &right_transform, + vec3(size, size, size), + right_color, + ); + } + + pub(super) fn draw_recent_anchor_confirmation(&mut self, cx: &mut Cx3d, world: &Mat4f) { + let Some(confirmation) = self.runtime.recent_anchor_confirmation else { + return; + }; + if self.runtime.local.state_time > confirmation.visible_until { + return; + } + self.draw_anchor_markers( + cx, + world, + confirmation.anchor, + Self::ANCHOR_MARKER_SIZE * 0.5, + vec4f(1.0, 0.08, 0.08, 1.0), + vec4f(1.0, 0.08, 0.08, 1.0), + ); + } + + pub(super) fn draw_pending_sync_anchor_preview(&mut self, cx: &mut Cx3d, world: &Mat4f) { + let now = self.runtime.local.state_time; + let recent_local_sync_anchors: Vec = + self.runtime.local.recent_sync_anchors.iter().copied().collect(); + for recent_sync in recent_local_sync_anchors.iter().rev() { + let age = now - recent_sync.last_seen_at_local_time; + if !(0.0..=Self::LOCAL_SYNC_SAMPLE_PREVIEW_SECONDS).contains(&age) { + continue; + } + let fade = (1.0 - age as f32 / Self::LOCAL_SYNC_SAMPLE_PREVIEW_SECONDS as f32) + .clamp(0.18, 1.0); + let (left_color, right_color) = match recent_sync.sync.extrema { + XrSyncAnchorExtrema::High => ( + vec4f(1.0, 0.86, 0.44, 0.94 * fade), + vec4f(0.44, 1.0, 0.90, 0.94 * fade), + ), + XrSyncAnchorExtrema::Low => ( + vec4f(1.0, 0.46, 0.40, 0.94 * fade), + vec4f(0.40, 0.70, 1.0, 0.94 * fade), + ), + }; + self.draw_anchor_markers( + cx, + world, + recent_sync.sync.anchor, + Self::ANCHOR_MARKER_SIZE, + left_color, + right_color, + ); + } + + let Some((peer_id, peer_state)) = self.runtime.registry.preferred_peer() else { + return; + }; + let peer_transform = self.peer_remote_to_local_transform(peer_id); + let recent_remote_sync_anchors: Vec = + peer_state.recent_sync_anchors.iter().copied().collect(); + for recent_sync in recent_remote_sync_anchors.iter().rev() { + let age = now - recent_sync.last_seen_at_local_time; + if !(0.0..=Self::LOCAL_SYNC_SAMPLE_PREVIEW_SECONDS).contains(&age) { + continue; + } + let fade = (1.0 - age as f32 / Self::LOCAL_SYNC_SAMPLE_PREVIEW_SECONDS as f32) + .clamp(0.18, 1.0); + let (left_color, right_color) = match recent_sync.sync.extrema { + XrSyncAnchorExtrema::High => ( + vec4f(1.0, 0.72, 0.18, 0.80 * fade), + vec4f(0.18, 0.96, 0.82, 0.80 * fade), + ), + XrSyncAnchorExtrema::Low => ( + vec4f(1.0, 0.32, 0.26, 0.80 * fade), + vec4f(0.26, 0.58, 1.0, 0.80 * fade), + ), + }; + self.draw_anchor_markers( + cx, + world, + Self::transform_anchor(&peer_transform, recent_sync.sync.anchor.mirrored()), + Self::ANCHOR_MARKER_SIZE, + left_color, + right_color, + ); + } + } + + fn draw_peer_joint_cube( + &mut self, + cx: &mut Cx3d, + world: &Mat4f, + root_transform: &Mat4f, + position: Vec3f, + size: f32, + color: Vec4f, + ) { + let transform = Mat4f::mul(root_transform, &Pose::new(Quat::default(), position).to_mat4()); + self.draw_cube_at(cx, world, &transform, vec3(size, size, size), color); + } + + fn peer_bone_pose(a: Vec3f, b: Vec3f) -> Option<(Pose, f32)> { + let delta = b - a; + let length = delta.length(); + if !length.is_finite() || length <= 0.0001 { + return None; + } + let forward = delta.scale(1.0 / length); + let up = if forward.y.abs() >= 0.92 { + vec3f(1.0, 0.0, 0.0) + } else { + vec3f(0.0, 1.0, 0.0) + }; + Some(( + Pose::new(Quat::look_rotation(forward, up), (a + b) * 0.5), + length, + )) + } + + fn draw_peer_bone_box( + &mut self, + cx: &mut Cx3d, + world: &Mat4f, + root_transform: &Mat4f, + a: Vec3f, + b: Vec3f, + thickness: f32, + color: Vec4f, + ) { + let Some((pose, length)) = Self::peer_bone_pose(a, b) else { + return; + }; + let transform = Mat4f::mul(root_transform, &pose.to_mat4()); + self.draw_cube_at( + cx, + world, + &transform, + vec3(thickness, thickness, length), + color, + ); + } + + fn draw_peer_finger_chain( + &mut self, + cx: &mut Cx3d, + world: &Mat4f, + root_transform: &Mat4f, + hand: &XrHand, + chain: &[usize], + tip: usize, + thickness: f32, + color: Vec4f, + ) { + let Some(mut points) = hand.joint_chain_positions(chain) else { + return; + }; + if let Some(tip_position) = hand.tip_pos_checked(tip) { + points.push(tip_position); + } + for &point in points.iter() { + self.draw_peer_joint_cube( + cx, + world, + root_transform, + point, + Self::REMOTE_HAND_JOINT_SIZE.max(thickness * 1.25), + color, + ); + } + for segment in points.windows(2) { + self.draw_peer_bone_box( + cx, + world, + root_transform, + segment[0], + segment[1], + thickness, + color, + ); + } + } + + fn draw_peer_hand_skeleton( + &mut self, + cx: &mut Cx3d, + world: &Mat4f, + root_transform: &Mat4f, + hand: &XrHand, + color: Vec4f, + ) -> bool { + if !hand.in_view() { + return false; + } + if let Some(palm_pose) = hand.tracking_pose() { + let transform = Mat4f::mul(root_transform, &palm_pose.to_mat4()); + self.draw_cube_at(cx, world, &transform, Self::REMOTE_HAND_PALM_SIZE, color); + } + for joint in [XrHand::WRIST, XrHand::CENTER] { + if let Some(position) = hand.joint_pose_checked(joint).map(|pose| pose.position) { + self.draw_peer_joint_cube( + cx, + world, + root_transform, + position, + Self::REMOTE_HAND_JOINT_SIZE * 1.15, + color, + ); + } + } + self.draw_peer_finger_chain( + cx, + world, + root_transform, + hand, + &[XrHand::THUMB_BASE, XrHand::THUMB_KNUCKLE1, XrHand::THUMB_KNUCKLE2], + XrHand::THUMB_TIP, + 0.016, + color, + ); + self.draw_peer_finger_chain( + cx, + world, + root_transform, + hand, + &[ + XrHand::INDEX_BASE, + XrHand::INDEX_KNUCKLE1, + XrHand::INDEX_KNUCKLE2, + XrHand::INDEX_KNUCKLE3, + ], + XrHand::INDEX_TIP, + 0.014, + color, + ); + self.draw_peer_finger_chain( + cx, + world, + root_transform, + hand, + &[ + XrHand::MIDDLE_BASE, + XrHand::MIDDLE_KNUCKLE1, + XrHand::MIDDLE_KNUCKLE2, + XrHand::MIDDLE_KNUCKLE3, + ], + XrHand::MIDDLE_TIP, + 0.015, + color, + ); + self.draw_peer_finger_chain( + cx, + world, + root_transform, + hand, + &[ + XrHand::RING_BASE, + XrHand::RING_KNUCKLE1, + XrHand::RING_KNUCKLE2, + XrHand::RING_KNUCKLE3, + ], + XrHand::RING_TIP, + 0.014, + color, + ); + self.draw_peer_finger_chain( + cx, + world, + root_transform, + hand, + &[ + XrHand::LITTLE_BASE, + XrHand::LITTLE_KNUCKLE1, + XrHand::LITTLE_KNUCKLE2, + XrHand::LITTLE_KNUCKLE3, + ], + XrHand::LITTLE_TIP, + 0.013, + color, + ); + true + } + + fn draw_peer_hand( + &mut self, + cx: &mut Cx3d, + world: &Mat4f, + root_transform: &Mat4f, + hand: &XrHand, + controller: &XrController, + color: Vec4f, + ) { + if self.draw_peer_hand_skeleton(cx, world, root_transform, hand, color) { + return; + } + let pose = if controller.active() { + Some(controller.grip_pose) + } else { + None + }; + let Some(pose) = pose else { + return; + }; + let transform = Mat4f::mul(root_transform, &pose.to_mat4()); + self.draw_cube_at(cx, world, &transform, Self::HAND_SIZE, color); + } + + pub(super) fn draw_remote_peers(&mut self, cx: &mut Cx3d, world: &Mat4f) { + let peer_ids = self.runtime.registry.peer_ids(); + for peer_id in peer_ids { + let Some(peer) = self.runtime.registry.peers.get(&peer_id).cloned() else { + continue; + }; + let Some(state_frame) = peer.latest_state.as_ref() else { + continue; + }; + + let alpha = Self::peer_alpha(&peer); + let base = Self::peer_base_color(peer.peer.id); + let root_transform = Self::peer_transform(&peer); + let head_color = vec4f(base.x, base.y, base.z, alpha); + let left_color = vec4f( + (base.x * 0.72).min(1.0), + (base.y * 1.05).min(1.0), + 1.0, + alpha, + ); + let right_color = vec4f( + 1.0, + (base.y * 0.82).min(1.0), + (base.z * 0.72).min(1.0), + alpha, + ); + + let head_pose = state_frame.state.head_pose; + self.draw_cube_at( + cx, + world, + &Mat4f::mul(&root_transform, &head_pose.to_mat4()), + Self::HEADSET_SIZE, + head_color, + ); + self.draw_peer_hand( + cx, + world, + &root_transform, + &state_frame.state.left_hand, + &state_frame.state.left_controller, + left_color, + ); + self.draw_peer_hand( + cx, + world, + &root_transform, + &state_frame.state.right_hand, + &state_frame.state.right_controller, + right_color, + ); + } + } +} diff --git a/xr/src/sync/shared_objects.rs b/xr/src/sync/shared_objects.rs new file mode 100644 index 0000000000..0e557231a5 --- /dev/null +++ b/xr/src/sync/shared_objects.rs @@ -0,0 +1,1472 @@ +use super::*; + +impl XrPeerSync { + pub fn send_local_body_spawn(&mut self, spawn: XrBodySpawn) -> Option { + if !self.enabled { + return None; + } + self.ensure_net_node(); + let activity_id = self.runtime.shared_objects.activity_id()?; + let sent_at = if self.runtime.local.state_time != 0.0 { + self.runtime.local.state_time + } else { + Cx::time_now() + }; + let (allocation, reused_remote) = self + .runtime + .shared_objects + .prepare_local_spawn_allocation( + activity_id, + spawn.widget_uid, + sent_at, + spawn.pose, + spawn.linvel, + spawn.angvel, + )?; + let authority = self.runtime.shared_objects.local_peer_id()?; + let control = XrNetSharedObjectControl::XrSpawnObject { + object_id: allocation.shared_object_id, + epoch: allocation.epoch, + authority, + fidelity: allocation.fidelity, + shape: XrSharedObjectShape::ActivitySpawnable { + activity_id, + spawnable_id: allocation.spawnable_object_id, + }, + pose: spawn.pose, + linvel: spawn.linvel, + angvel: spawn.angvel, + }; + let net_node = self.runtime.net_node.as_mut()?; + net_node.send_shared_object_control(control); + if reused_remote { + net_node.send_shared_object_control(XrNetSharedObjectControl::XrResetObject { + object_id: allocation.shared_object_id, + epoch: allocation.epoch, + pose: spawn.pose, + linvel: spawn.linvel, + angvel: spawn.angvel, + }); + } + self.runtime + .metrics + .record_body_spawn_tx(allocation.shared_object_id.0); + Some(XrBodySpawn { + widget_uid: allocation.widget_uid, + shadow: spawn.shadow, + mode: spawn.mode, + pose: spawn.pose, + linvel: spawn.linvel, + angvel: spawn.angvel, + }) + } + + pub fn send_activity_pose_reset(&mut self, pose: Pose) -> bool { + if !self.enabled { + return false; + } + self.ensure_net_node(); + let Some(activity_id) = self.runtime.shared_objects.activity_id() else { + return false; + }; + let Some(net_node) = self.runtime.net_node.as_mut() else { + return false; + }; + net_node.send_shared_object_control(XrNetSharedObjectControl::XrResetActivityPose { + activity_id, + pose, + }); + true + } + + fn local_shared_object_spawn_control( + activity_id: XrActivityId, + snapshot: XrLocalSharedObjectSnapshot, + body: &XrRuntimeBodyState, + ) -> XrNetSharedObjectControl { + XrNetSharedObjectControl::XrSpawnObject { + object_id: snapshot.object_id, + epoch: snapshot.epoch, + authority: snapshot.authority, + fidelity: snapshot.fidelity, + shape: XrSharedObjectShape::ActivitySpawnable { + activity_id, + spawnable_id: snapshot.spawnable_object_id, + }, + pose: body.pose, + linvel: body.linvel, + angvel: body.angvel, + } + } + + fn bootstrap_owner_peer_id( + &self, + spawnable_object_id: XrSpawnableObjectId, + ) -> Option { + let local_node_id = self.runtime.net_node.as_ref()?.node_id(); + let mut peer_ids = self.runtime.registry.peer_ids(); + if peer_ids.is_empty() { + return None; + } + peer_ids.push(local_node_id); + peer_ids.sort_by_key(|peer_id| peer_id.0); + peer_ids.dedup_by_key(|peer_id| peer_id.0); + let owner_index = (Self::bootstrap_hash_u64( + spawnable_object_id.0, + Self::SHARED_OBJECT_BOOTSTRAP_OWNER_TAG, + ) as usize) + % peer_ids.len(); + peer_ids.get(owner_index).copied() + } + + fn bootstrap_hash_u64(hash: u64, value: u64) -> u64 { + (hash ^ value).wrapping_mul(0x00000100000001b3) + } + + fn bootstrap_local_shared_scene_objects( + &mut self, + runtime_bodies: &HashMap, + ) -> Vec { + if self.runtime.registry.len() == 0 { + return Vec::new(); + } + let Some(activity_id) = self.runtime.shared_objects.activity_id() else { + return Vec::new(); + }; + let Some(local_node_id) = self.runtime.net_node.as_ref().map(|node| node.node_id()) else { + return Vec::new(); + }; + let mut controls = Vec::new(); + for (spawnable_object_id, widget_uid) in + self.runtime.shared_objects.bootstrap_shared_candidates() + { + if self + .runtime + .shared_objects + .resolve_local_shared_object_for_widget(widget_uid) + .is_some() + || self + .runtime + .shared_objects + .resolve_remote_shared_object_for_widget(widget_uid) + .is_some() + { + continue; + } + if self.bootstrap_owner_peer_id(spawnable_object_id) != Some(local_node_id) { + continue; + } + let Some(body) = runtime_bodies.get(&widget_uid) else { + continue; + }; + let Some((allocation, is_new)) = self + .runtime + .shared_objects + .ensure_local_shared_object(activity_id, widget_uid) + else { + continue; + }; + if !is_new { + continue; + } + controls.push(XrNetSharedObjectControl::XrSpawnObject { + object_id: allocation.shared_object_id, + epoch: allocation.epoch, + authority: self + .runtime + .shared_objects + .local_peer_id() + .unwrap_or_default(), + fidelity: allocation.fidelity, + shape: XrSharedObjectShape::ActivitySpawnable { + activity_id, + spawnable_id: allocation.spawnable_object_id, + }, + pose: body.pose, + linvel: body.linvel, + angvel: body.angvel, + }); + } + controls + } + + fn reannounce_local_shared_objects( + &mut self, + runtime_bodies: &HashMap, + ) -> Vec { + if !self.runtime.local_shared_object_reannounce_needed { + return Vec::new(); + } + let Some(activity_id) = self.runtime.shared_objects.activity_id() else { + return Vec::new(); + }; + let controls = self + .runtime + .shared_objects + .local_shared_object_snapshots() + .into_iter() + .filter_map(|snapshot| { + let body = runtime_bodies.get(&snapshot.widget_uid)?; + Some(Self::local_shared_object_spawn_control( + activity_id, + snapshot, + body, + )) + }) + .collect::>(); + self.runtime.local_shared_object_reannounce_needed = false; + controls + } + + pub fn reset_local_shared_bootstrap_objects( + &mut self, + runtime_bodies: &HashMap, + ) -> usize { + if !self.enabled { + return 0; + } + self.ensure_net_node(); + let Some(activity_id) = self.runtime.shared_objects.activity_id() else { + return 0; + }; + let Some(local_node_id) = self.runtime.net_node.as_ref().map(|node| node.node_id()) else { + return 0; + }; + let mut controls = Vec::new(); + for (spawnable_object_id, widget_uid) in + self.runtime.shared_objects.bootstrap_shared_candidates() + { + let Some(body) = runtime_bodies.get(&widget_uid) else { + continue; + }; + let has_local_object = self + .runtime + .shared_objects + .resolve_local_shared_object_for_widget(widget_uid) + .is_some(); + if self + .runtime + .shared_objects + .resolve_remote_shared_object_for_widget(widget_uid) + .is_some() + { + continue; + } + if !has_local_object + && self.bootstrap_owner_peer_id(spawnable_object_id) != Some(local_node_id) + { + continue; + } + let Some(allocation) = self.runtime.shared_objects.force_local_shared_object_reset( + activity_id, + widget_uid, + self.current_local_time(), + body.pose, + body.linvel, + body.angvel, + ) else { + continue; + }; + let Some(snapshot) = self + .runtime + .shared_objects + .local_shared_object_snapshot(allocation.shared_object_id) + else { + continue; + }; + controls.push(Self::local_shared_object_spawn_control( + activity_id, + snapshot, + body, + )); + controls.push(XrNetSharedObjectControl::XrResetObject { + object_id: allocation.shared_object_id, + epoch: allocation.epoch, + pose: body.pose, + linvel: body.linvel, + angvel: body.angvel, + }); + } + let count = controls.len(); + if let Some(net_node) = self.runtime.net_node.as_mut() { + for control in controls { + if let Some(object_id) = Self::shared_object_control_object_id(&control) { + if matches!( + control, + XrNetSharedObjectControl::XrSpawnObject { .. } + | XrNetSharedObjectControl::XrResetObject { .. } + ) { + self.runtime.metrics.record_body_spawn_tx(object_id.0); + } + } + net_node.send_shared_object_control(control); + } + } + count + } + + pub fn publish_local_shared_object_states( + &mut self, + cx: &mut Cx, + runtime_bodies: &HashMap, + ) -> usize { + if !self.enabled { + return 0; + } + self.ensure_net_node(); + let authority = if let Some(authority) = self.runtime.shared_objects.local_peer_id() { + authority + } else { + return 0; + }; + let sent_at = if self.runtime.local.state_time != 0.0 { + self.runtime.local.state_time + } else { + Cx::time_now() + }; + let physics_tick = self.runtime.next_shared_object_physics_tick; + self.runtime.next_shared_object_physics_tick = + self.runtime.next_shared_object_physics_tick.wrapping_add(1); + let mut action_count = self.service_scheduled_authority_transfers(cx, physics_tick); + action_count += self.sync_remote_shadow_bodies(cx); + let mut outgoing_controls = self.reannounce_local_shared_objects(runtime_bodies); + outgoing_controls.extend(self.bootstrap_local_shared_scene_objects(runtime_bodies)); + outgoing_controls.extend(self.queue_remote_interaction_controls(runtime_bodies)); + if let Some(activity_id) = self.runtime.shared_objects.activity_id() { + for (&widget_uid, body) in runtime_bodies { + if body.held_by.is_none() { + continue; + } + if self + .runtime + .shared_objects + .resolve_remote_shared_object_for_widget(widget_uid) + .is_some() + { + continue; + } + let Some((allocation, is_new)) = self + .runtime + .shared_objects + .ensure_local_shared_object(activity_id, widget_uid) + else { + continue; + }; + if is_new { + outgoing_controls.push(XrNetSharedObjectControl::XrSpawnObject { + object_id: allocation.shared_object_id, + epoch: allocation.epoch, + authority, + fidelity: allocation.fidelity, + shape: XrSharedObjectShape::ActivitySpawnable { + activity_id, + spawnable_id: allocation.spawnable_object_id, + }, + pose: body.pose, + linvel: body.linvel, + angvel: body.angvel, + }); + } + } + } + let despawns = self + .runtime + .shared_objects + .prune_missing_local_shared_objects(runtime_bodies); + let mut states = self.runtime.shared_objects.local_shared_object_states( + runtime_bodies, + sent_at, + physics_tick, + authority, + ); + let count = action_count + outgoing_controls.len() + despawns.len() + states.len(); + for control in &outgoing_controls { + if let Some(object_id) = Self::shared_object_control_object_id(control) { + if matches!(control, XrNetSharedObjectControl::XrSpawnObject { .. }) { + self.runtime.metrics.record_body_spawn_tx(object_id.0); + } + } + } + if let Some(net_node) = self.runtime.net_node.as_mut() { + for control in outgoing_controls { + net_node.send_shared_object_control(control); + } + for (object_id, epoch, _) in despawns { + net_node.send_shared_object_control(XrNetSharedObjectControl::XrDespawnObject { + object_id, + epoch, + }); + } + if !states.is_empty() { + net_node.assign_shared_object_state_seqs(&mut states); + self.runtime + .shared_objects + .note_published_local_shared_object_states(&states); + for state in &states { + self.runtime + .metrics + .record_shared_object_state_tx(state.object_id, state.seq); + } + net_node.send_prepared_shared_object_states(states); + } + } + count + } + + pub fn flush_pending_shared_object_controls(&mut self, cx: &mut Cx) -> usize { + if self.runtime.pending_shared_object_controls.is_empty() { + return 0; + } + let pending = std::mem::take(&mut self.runtime.pending_shared_object_controls); + let mut remaining = Vec::new(); + let mut applied = 0usize; + for (peer, control) in pending { + if self.apply_shared_object_control(cx, peer, &control) { + applied += 1; + } else { + remaining.push((peer, control)); + } + } + self.runtime.pending_shared_object_controls = remaining; + applied + } + + fn shared_object_request_id(&mut self) -> u32 { + let request_id = self.runtime.next_shared_object_request_id; + self.runtime.next_shared_object_request_id = + self.runtime.next_shared_object_request_id.wrapping_add(1); + request_id + } + + pub(super) fn peer_id_for_authority(&self, authority: XrNetPeerId) -> Option { + self.runtime + .registry + .peers + .contains_key(&authority) + .then_some(authority) + } + + pub(super) fn peer_time_to_local_time( + &self, + peer_id: XrNetPeerId, + remote_time: f64, + ) -> Option { + let peer_state = self.runtime.registry.peers.get(&peer_id)?; + let clock_offset = peer_state.clock_offset_seconds?; + Some(remote_time - clock_offset) + } + + pub(super) fn normalize_incoming_shared_object_state( + &self, + peer_id: XrNetPeerId, + mut state: XrNetSharedObjectState, + received_at_local_time: f64, + ) -> XrNetSharedObjectState { + if state.sent_at <= 0.0 { + return state; + } + let translated_sent_at = self + .peer_time_to_local_time(peer_id, state.sent_at) + .unwrap_or(received_at_local_time); + state.sent_at = + Self::clamp_remote_shared_object_local_time(translated_sent_at, received_at_local_time); + state + } + + pub(super) fn clamp_remote_shared_object_local_time( + translated_sent_at: f64, + received_at_local_time: f64, + ) -> f64 { + translated_sent_at.min(received_at_local_time) + } + + fn predict_pose(pose: Pose, linvel: Vec3f, angvel: Vec3f, dt: f32) -> Pose { + let position = pose.position + linvel * dt; + let angular_speed = angvel.length(); + let orientation = if angular_speed > 1.0e-4 { + let axis = angvel * (1.0 / angular_speed); + Quat::multiply( + &Quat::from_axis_angle(axis, angular_speed * dt), + &pose.orientation, + ) + } else { + pose.orientation + }; + Pose::new(orientation, position) + } + + fn peer_grab_intent(&self, peer_id: XrNetPeerId, hand: XrSharedHand) -> bool { + let Some(peer_state) = self.runtime.registry.peers.get(&peer_id) else { + return false; + }; + let Some(state) = peer_state.latest_state.as_ref().map(|frame| &frame.state) else { + return false; + }; + match hand { + XrSharedHand::LeftHand => state.left_hand.grab_intent(), + XrSharedHand::RightHand => state.right_hand.grab_intent(), + XrSharedHand::LeftController => state.left_controller.grip >= 0.55, + XrSharedHand::RightController => state.right_controller.grip >= 0.55, + XrSharedHand::Unknown => false, + } + } + + pub(super) fn current_local_time(&self) -> f64 { + if self.runtime.local.state_time != 0.0 { + self.runtime.local.state_time + } else { + Cx::time_now() + } + } + + fn shared_object_state_local_time( + state: XrNetSharedObjectState, + fallback_local_time: f64, + ) -> f64 { + if state.sent_at > 0.0 { + state.sent_at + } else { + fallback_local_time + } + } + + fn shared_object_source_peer_id( + &self, + snapshot: &XrRemoteSharedObjectSnapshot, + ) -> Option { + self.peer_id_for_authority(snapshot.state_source_authority) + } + + pub(super) fn predict_remote_shadow_state_from_history( + playback_time: f64, + latest_state: XrNetSharedObjectState, + history: &[XrNetSharedObjectState], + fallback_local_time: f64, + ) -> (XrSharedObjectMode, Pose, Vec3f, Vec3f) { + for samples in history.windows(2) { + let previous = samples[0]; + let next = samples[1]; + if previous.epoch != latest_state.epoch || next.epoch != latest_state.epoch { + continue; + } + let previous_local_sent_at = + Self::shared_object_state_local_time(previous, fallback_local_time); + let next_local_sent_at = + Self::shared_object_state_local_time(next, fallback_local_time); + if playback_time < previous_local_sent_at || playback_time > next_local_sent_at { + continue; + } + let interpolation = ((playback_time - previous_local_sent_at) + / (next_local_sent_at - previous_local_sent_at).max(f64::EPSILON)) + .clamp(0.0, 1.0) as f32; + return ( + next.mode, + Pose::from_lerp(previous.pose, next.pose, interpolation), + Vec3f::from_lerp(previous.linvel, next.linvel, interpolation), + Vec3f::from_lerp(previous.angvel, next.angvel, interpolation), + ); + } + + let local_sent_at = Self::shared_object_state_local_time(latest_state, fallback_local_time); + let dt = (playback_time - local_sent_at).clamp( + 0.0, + Self::SHARED_OBJECT_SHADOW_MAX_EXTRAPOLATION_SECONDS as f64, + ) as f32; + ( + latest_state.mode, + Self::predict_pose( + latest_state.pose, + latest_state.linvel, + latest_state.angvel, + dt, + ), + latest_state.linvel, + latest_state.angvel, + ) + } + + fn predicted_remote_shadow_state( + &self, + snapshot: &XrRemoteSharedObjectSnapshot, + ) -> Option<(XrNetPeerId, XrSharedObjectMode, Pose, Vec3f, Vec3f)> { + let peer_id = self.shared_object_source_peer_id(snapshot)?; + let latest_state = snapshot.latest_state?; + let playback_time = + self.current_local_time() - Self::SHARED_OBJECT_SHADOW_INTERPOLATION_DELAY_SECONDS; + let history = self + .runtime + .shared_objects + .remote_shared_object_history(snapshot.object_id); + let (mode, pose, linvel, angvel) = Self::predict_remote_shadow_state_from_history( + playback_time, + latest_state, + history.as_slice(), + self.current_local_time(), + ); + Some((peer_id, mode, pose, linvel, angvel)) + } + + fn shadow_pose_correction_needed(previous: Pose, next: Pose) -> bool { + (next.position - previous.position).length() + > Self::SHARED_OBJECT_SHADOW_REAPPLY_POSITION_EPSILON_METERS + || previous.orientation.get_angle_with(next.orientation) + > Self::SHARED_OBJECT_SHADOW_REAPPLY_ORIENTATION_EPSILON_DEGREES + } + + fn shadow_velocity_correction_needed(previous: Vec3f, next: Vec3f, epsilon: f32) -> bool { + (next - previous).length() > epsilon + } + + fn note_applied_remote_shadow_state( + &mut self, + object_id: XrSharedObjectId, + peer_id: XrNetPeerId, + state_seq: Option, + mode: XrSharedObjectMode, + pose: Pose, + linvel: Vec3f, + angvel: Vec3f, + ) { + self.runtime.applied_remote_shadow_states.insert( + object_id, + XrAppliedRemoteShadowState { + peer_id, + applied_at_local_time: self.current_local_time(), + state_seq, + mode, + pose, + linvel, + angvel, + }, + ); + } + + fn clear_applied_remote_shadow_state(&mut self, object_id: XrSharedObjectId) { + self.runtime.applied_remote_shadow_states.remove(&object_id); + } + + pub(super) fn should_reapply_remote_shadow_state( + previous: &XrAppliedRemoteShadowState, + now: f64, + peer_id: XrNetPeerId, + _state_seq: Option, + mode: XrSharedObjectMode, + pose: Pose, + linvel: Vec3f, + angvel: Vec3f, + ) -> bool { + if previous.peer_id != peer_id || previous.mode != mode { + return true; + } + let expected_pose = Self::predict_pose( + previous.pose, + previous.linvel, + previous.angvel, + (now - previous.applied_at_local_time).clamp( + 0.0, + Self::SHARED_OBJECT_SHADOW_MAX_EXTRAPOLATION_SECONDS as f64, + ) as f32, + ); + Self::shadow_pose_correction_needed(expected_pose, pose) + || Self::shadow_velocity_correction_needed( + previous.linvel, + linvel, + Self::SHARED_OBJECT_SHADOW_REAPPLY_LINVEL_EPSILON_MPS, + ) + || Self::shadow_velocity_correction_needed( + previous.angvel, + angvel, + Self::SHARED_OBJECT_SHADOW_REAPPLY_ANGVEL_EPSILON_RADPS, + ) + } + + fn emit_body_spawn_local_space( + &mut self, + cx: &mut Cx, + widget_uid: WidgetUid, + shadow: bool, + mode: XrSharedObjectMode, + pose: Pose, + linvel: Vec3f, + angvel: Vec3f, + ) { + cx.widget_action( + self.widget_uid(), + XrPeerSyncAction::BodySpawn(XrBodySpawn { + widget_uid, + shadow, + mode, + pose, + linvel, + angvel, + }), + ); + } + + fn emit_body_impulse( + &mut self, + cx: &mut Cx, + widget_uid: WidgetUid, + point: Vec3f, + impulse: Vec3f, + ) { + cx.widget_action( + self.widget_uid(), + XrPeerSyncAction::BodyImpulse(XrBodyImpulse { + widget_uid, + point, + impulse, + }), + ); + } + + fn emit_authority_space_body_spawn( + &mut self, + cx: &mut Cx, + source_authority: XrNetPeerId, + widget_uid: WidgetUid, + shadow: bool, + mode: XrSharedObjectMode, + pose: Pose, + linvel: Vec3f, + angvel: Vec3f, + ) { + if self.runtime.shared_objects.local_peer_id() == Some(source_authority) { + self.emit_body_spawn_local_space(cx, widget_uid, shadow, mode, pose, linvel, angvel); + } else if let Some(peer_id) = self.peer_id_for_authority(source_authority) { + self.emit_remote_body_spawn( + cx, peer_id, widget_uid, shadow, mode, pose, linvel, angvel, + ); + } else { + self.emit_body_spawn_local_space(cx, widget_uid, shadow, mode, pose, linvel, angvel); + } + } + + fn service_scheduled_authority_transfers(&mut self, cx: &mut Cx, physics_tick: u32) -> usize { + let transfers = self + .runtime + .shared_objects + .apply_scheduled_authority_transfers(self.runtime.local.state_time, physics_tick); + for transfer in &transfers { + self.emit_authority_space_body_spawn( + cx, + transfer.source_authority, + transfer.widget_uid, + transfer.shadow, + XrSharedObjectMode::Dynamic, + transfer.pose, + transfer.linvel, + transfer.angvel, + ); + if transfer.shadow { + if let Some(peer_id) = self.peer_id_for_authority(transfer.source_authority) { + self.note_applied_remote_shadow_state( + transfer.object_id, + peer_id, + None, + XrSharedObjectMode::Dynamic, + transfer.pose, + transfer.linvel, + transfer.angvel, + ); + } else { + self.clear_applied_remote_shadow_state(transfer.object_id); + } + } else { + self.clear_applied_remote_shadow_state(transfer.object_id); + } + } + transfers.len() + } + + fn sync_remote_shadow_bodies(&mut self, cx: &mut Cx) -> usize { + let snapshots = self.runtime.shared_objects.remote_shared_object_snapshots(); + let now = self.current_local_time(); + let mut applied = 0usize; + let mut live_object_ids = Vec::with_capacity(snapshots.len()); + for snapshot in snapshots { + live_object_ids.push(snapshot.object_id); + let latest_state = snapshot.latest_state; + let Some((peer_id, mode, pose, linvel, angvel)) = + self.predicted_remote_shadow_state(&snapshot) + else { + self.clear_applied_remote_shadow_state(snapshot.object_id); + continue; + }; + if self + .runtime + .applied_remote_shadow_states + .get(&snapshot.object_id) + .is_some_and(|previous| { + !Self::should_reapply_remote_shadow_state( + previous, + now, + peer_id, + latest_state.map(|state| state.seq), + mode, + pose, + linvel, + angvel, + ) + }) + { + continue; + } + self.emit_remote_body_spawn( + cx, + peer_id, + snapshot.widget_uid, + true, + mode, + pose, + linvel, + angvel, + ); + self.note_applied_remote_shadow_state( + snapshot.object_id, + peer_id, + latest_state.map(|state| state.seq), + mode, + pose, + linvel, + angvel, + ); + self.runtime.metrics.record_remote_shadow_apply( + snapshot.object_id, + latest_state.map(|state| state.seq), + ); + applied += 1; + } + self.runtime + .applied_remote_shadow_states + .retain(|object_id, _| live_object_ids.contains(object_id)); + applied + } + + fn queue_remote_interaction_controls( + &mut self, + runtime_bodies: &HashMap, + ) -> Vec { + let local_peer_id = if let Some(peer_id) = self.runtime.shared_objects.local_peer_id() { + peer_id + } else { + return Vec::new(); + }; + let hands = self.local_shared_hands(); + if hands.is_empty() { + return Vec::new(); + } + let snapshots = self.runtime.shared_objects.remote_shared_object_snapshots(); + let mut controls = Vec::new(); + for snapshot in snapshots { + let Some(latest_state) = snapshot.latest_state else { + continue; + }; + let Some((_, _, predicted_pose, predicted_linvel, _)) = + self.predicted_remote_shadow_state(&snapshot) + else { + continue; + }; + if runtime_bodies + .get(&snapshot.widget_uid) + .is_some_and(|body| body.held_by.is_some()) + { + continue; + } + for hand in &hands { + let distance = (predicted_pose.position - hand.pose.position).length(); + let relative_speed = (predicted_linvel - hand.linvel).length(); + if hand.gripping + && matches!( + latest_state.mode, + XrSharedObjectMode::ContactDominated { .. } + ) + && distance <= Self::SHARED_OBJECT_TAKEOVER_DISTANCE_METERS + && relative_speed <= Self::SHARED_OBJECT_TAKEOVER_RELATIVE_SPEED_MAX + && self.runtime.shared_objects.can_send_takeover_request( + snapshot.object_id, + self.runtime.local.state_time, + ) + { + let request_id = self.shared_object_request_id(); + self.runtime.shared_objects.note_takeover_request( + snapshot.object_id, + request_id, + self.runtime.local.state_time, + ); + controls.push(XrNetSharedObjectControl::XrTakeoverRequest { + object_id: snapshot.object_id, + epoch: snapshot.epoch, + request_id, + based_on_seq: latest_state.seq, + based_on_tick: latest_state.physics_tick, + candidate_owner: local_peer_id, + hand: hand.shared_hand, + hand_pose: hand.pose, + hand_linvel: hand.linvel, + }); + break; + } + if !hand.gripping + && matches!( + latest_state.mode, + XrSharedObjectMode::Dynamic | XrSharedObjectMode::Sleeping + ) + && hand.linvel.length() >= Self::SHARED_OBJECT_IMPULSE_MIN_HAND_SPEED + && distance <= Self::SHARED_OBJECT_IMPULSE_DISTANCE_METERS + && self + .runtime + .shared_objects + .can_send_contact_impulse_request( + snapshot.object_id, + self.runtime.local.state_time, + ) + { + self.runtime.shared_objects.note_contact_impulse_request( + snapshot.object_id, + self.runtime.local.state_time, + ); + controls.push(XrNetSharedObjectControl::XrContactImpulse { + object_id: snapshot.object_id, + epoch: snapshot.epoch, + based_on_seq: latest_state.seq, + based_on_tick: latest_state.physics_tick, + hand: hand.shared_hand, + hand_pose: hand.pose, + point: hand.pose.position, + impulse: hand.linvel * Self::SHARED_OBJECT_IMPULSE_SCALE, + }); + break; + } + } + } + controls + } + + pub(super) fn emit_remote_body_spawn( + &mut self, + cx: &mut Cx, + peer_id: XrNetPeerId, + widget_uid: WidgetUid, + shadow: bool, + mode: XrSharedObjectMode, + pose: Pose, + linvel: Vec3f, + angvel: Vec3f, + ) { + let transform = self.peer_remote_to_local_transform(peer_id); + cx.widget_action( + self.widget_uid(), + XrPeerSyncAction::BodySpawn(XrBodySpawn { + widget_uid, + shadow, + mode, + pose: Self::transform_pose(&transform, pose), + linvel: Self::transform_direction(&transform, linvel), + angvel: Self::transform_direction(&transform, angvel), + }), + ); + } + + pub(super) fn emit_remote_body_despawn(&mut self, cx: &mut Cx, widget_uid: WidgetUid) { + cx.widget_action(self.widget_uid(), XrPeerSyncAction::BodyDespawn(widget_uid)); + } + + fn shared_object_control_object_id( + control: &XrNetSharedObjectControl, + ) -> Option { + match control { + XrNetSharedObjectControl::XrSpawnObject { object_id, .. } + | XrNetSharedObjectControl::XrDespawnObject { object_id, .. } + | XrNetSharedObjectControl::XrTakeoverRequest { object_id, .. } + | XrNetSharedObjectControl::XrTakeoverAccept { object_id, .. } + | XrNetSharedObjectControl::XrTakeoverReject { object_id, .. } + | XrNetSharedObjectControl::XrContactImpulse { object_id, .. } + | XrNetSharedObjectControl::XrResetObject { object_id, .. } => Some(*object_id), + XrNetSharedObjectControl::XrResetActivityPose { .. } + | XrNetSharedObjectControl::XrClockPing { .. } + | XrNetSharedObjectControl::XrClockPong { .. } => None, + } + } + + fn send_takeover_reject( + &mut self, + object_id: XrSharedObjectId, + epoch: u32, + request_id: u32, + authoritative_state: Option, + ) { + let authoritative_seq = authoritative_state.map(|state| state.seq).unwrap_or(0); + let authoritative_tick = authoritative_state + .map(|state| state.physics_tick) + .unwrap_or(self.runtime.next_shared_object_physics_tick); + if let Some(net_node) = self.runtime.net_node.as_mut() { + net_node.send_shared_object_control(XrNetSharedObjectControl::XrTakeoverReject { + object_id, + epoch, + request_id, + authoritative_seq, + authoritative_tick, + }); + } + } + + fn handle_takeover_request( + &mut self, + peer: XrNetPeer, + object_id: XrSharedObjectId, + epoch: u32, + request_id: u32, + based_on_seq: u32, + based_on_tick: u32, + candidate_owner: XrNetPeerId, + hand: XrSharedHand, + hand_pose: Pose, + hand_linvel: Vec3f, + ) -> bool { + let Some(local_snapshot) = self + .runtime + .shared_objects + .local_shared_object_snapshot(object_id) + else { + return true; + }; + let authoritative_state = self.runtime.shared_objects.find_local_state_for_request( + object_id, + epoch, + based_on_seq, + based_on_tick, + ); + if epoch != local_snapshot.epoch + || candidate_owner != peer.id + || !self.peer_grab_intent(peer.id, hand) + { + self.send_takeover_reject(object_id, epoch, request_id, authoritative_state); + return true; + } + let Some(authoritative_state) = authoritative_state else { + self.send_takeover_reject(object_id, epoch, request_id, None); + return true; + }; + let peer_transform = self.peer_remote_to_local_transform(peer.id); + let local_hand_pose = Self::transform_pose(&peer_transform, hand_pose); + let local_hand_linvel = Self::transform_direction(&peer_transform, hand_linvel); + let distance = (authoritative_state.pose.position - local_hand_pose.position).length(); + let relative_speed = (authoritative_state.linvel - local_hand_linvel).length(); + if distance > Self::SHARED_OBJECT_TAKEOVER_DISTANCE_METERS + || relative_speed > Self::SHARED_OBJECT_TAKEOVER_RELATIVE_SPEED_MAX + { + self.send_takeover_reject(object_id, epoch, request_id, Some(authoritative_state)); + return true; + } + let effective_at = (if self.runtime.local.state_time != 0.0 { + self.runtime.local.state_time + } else { + Cx::time_now() + }) + Self::SHARED_OBJECT_TAKEOVER_EFFECTIVE_DELAY_SECONDS; + let effective_tick = self + .runtime + .next_shared_object_physics_tick + .wrapping_add(Self::SHARED_OBJECT_TAKEOVER_EFFECTIVE_TICK_OFFSET); + let new_epoch = local_snapshot.epoch.wrapping_add(1); + self.runtime.shared_objects.schedule_authority_transfer( + object_id, + new_epoch, + local_snapshot.authority, + candidate_owner, + effective_at, + effective_tick, + request_id, + Some(hand), + authoritative_state.pose, + authoritative_state.linvel, + authoritative_state.angvel, + ); + if let Some(net_node) = self.runtime.net_node.as_mut() { + net_node.send_shared_object_control(XrNetSharedObjectControl::XrTakeoverAccept { + object_id, + epoch: new_epoch, + request_id, + new_authority: candidate_owner, + effective_at, + effective_tick, + pose: authoritative_state.pose, + linvel: authoritative_state.linvel, + angvel: authoritative_state.angvel, + }); + } + true + } + + fn handle_takeover_accept( + &mut self, + object_id: XrSharedObjectId, + epoch: u32, + request_id: u32, + source_authority: XrNetPeerId, + new_authority: XrNetPeerId, + effective_at: f64, + effective_tick: u32, + pose: Pose, + linvel: Vec3f, + angvel: Vec3f, + ) -> bool { + let local_effective_at = self + .peer_id_for_authority(source_authority) + .and_then(|peer_id| self.peer_time_to_local_time(peer_id, effective_at)) + .unwrap_or(effective_at); + self.runtime + .shared_objects + .clear_takeover_request(object_id, Some(request_id)); + self.runtime.shared_objects.schedule_authority_transfer( + object_id, + epoch, + source_authority, + new_authority, + local_effective_at, + effective_tick, + request_id, + None, + pose, + linvel, + angvel, + ) + } + + fn handle_takeover_reject(&mut self, object_id: XrSharedObjectId, request_id: u32) -> bool { + let _ = self + .runtime + .shared_objects + .clear_takeover_request(object_id, Some(request_id)); + true + } + + fn handle_contact_impulse( + &mut self, + cx: &mut Cx, + peer: XrNetPeer, + object_id: XrSharedObjectId, + epoch: u32, + based_on_seq: u32, + based_on_tick: u32, + hand: XrSharedHand, + hand_pose: Pose, + point: Vec3f, + impulse: Vec3f, + ) -> bool { + let Some(local_snapshot) = self + .runtime + .shared_objects + .local_shared_object_snapshot(object_id) + else { + return true; + }; + if epoch != local_snapshot.epoch || self.peer_grab_intent(peer.id, hand) { + return true; + } + let Some(authoritative_state) = self.runtime.shared_objects.find_local_state_for_request( + object_id, + epoch, + based_on_seq, + based_on_tick, + ) else { + return true; + }; + if !matches!( + authoritative_state.mode, + XrSharedObjectMode::Dynamic | XrSharedObjectMode::Sleeping + ) { + return true; + } + let peer_transform = self.peer_remote_to_local_transform(peer.id); + let local_hand_pose = Self::transform_pose(&peer_transform, hand_pose); + let local_point = Self::transform_point(&peer_transform, point); + let local_impulse = Self::transform_direction(&peer_transform, impulse); + let distance = (authoritative_state.pose.position - local_hand_pose.position).length(); + if distance > Self::SHARED_OBJECT_IMPULSE_DISTANCE_METERS * 1.25 + || (authoritative_state.pose.position - local_point).length() + > Self::SHARED_OBJECT_IMPULSE_DISTANCE_METERS * 1.5 + { + return true; + } + self.emit_body_impulse(cx, local_snapshot.widget_uid, local_point, local_impulse); + true + } + + fn handle_reset_object( + &mut self, + cx: &mut Cx, + source_authority: XrNetPeerId, + object_id: XrSharedObjectId, + epoch: u32, + pose: Pose, + linvel: Vec3f, + angvel: Vec3f, + ) -> bool { + let Some(activity_id) = self.runtime.shared_objects.activity_id() else { + return false; + }; + let Some(widget_uid) = self + .runtime + .shared_objects + .apply_remote_shared_object_reset( + activity_id, + self.current_local_time(), + source_authority, + object_id, + epoch, + pose, + linvel, + angvel, + ) + else { + return false; + }; + self.emit_authority_space_body_spawn( + cx, + source_authority, + widget_uid, + self.runtime.shared_objects.local_peer_id() != Some(source_authority), + XrSharedObjectMode::Dynamic, + pose, + linvel, + angvel, + ); + if let Some(peer_id) = self.peer_id_for_authority(source_authority) { + self.note_applied_remote_shadow_state( + object_id, + peer_id, + None, + XrSharedObjectMode::Dynamic, + pose, + linvel, + angvel, + ); + } else { + self.clear_applied_remote_shadow_state(object_id); + } + true + } + + pub(super) fn apply_shared_object_control( + &mut self, + cx: &mut Cx, + peer: XrNetPeer, + control: &XrNetSharedObjectControl, + ) -> bool { + match control { + XrNetSharedObjectControl::XrSpawnObject { + object_id, + epoch, + authority, + fidelity, + shape: + XrSharedObjectShape::ActivitySpawnable { + activity_id, + spawnable_id, + }, + pose, + linvel, + angvel, + .. + } => { + self.runtime + .metrics + .record_body_spawn_rx(peer.id, object_id.0); + if self + .runtime + .shared_objects + .resolve_local_shared_object_widget(*object_id) + .is_some() + { + return true; + } + let Some(widget_uid) = self.runtime.shared_objects.register_remote_shared_object( + *activity_id, + self.current_local_time(), + *object_id, + *epoch, + *authority, + *fidelity, + *spawnable_id, + *pose, + *linvel, + *angvel, + ) else { + return false; + }; + self.emit_remote_body_spawn( + cx, + peer.id, + widget_uid, + true, + XrSharedObjectMode::Dynamic, + *pose, + *linvel, + *angvel, + ); + self.note_applied_remote_shadow_state( + *object_id, + peer.id, + None, + XrSharedObjectMode::Dynamic, + *pose, + *linvel, + *angvel, + ); + true + } + XrNetSharedObjectControl::XrDespawnObject { object_id, .. } => { + self.runtime + .pending_shared_object_controls + .retain(|(_, pending_control)| { + Self::shared_object_control_object_id(pending_control) != Some(*object_id) + }); + if let Some(widget_uid) = self + .runtime + .shared_objects + .release_remote_shared_object(*object_id) + { + self.emit_remote_body_despawn(cx, widget_uid); + } + self.clear_applied_remote_shadow_state(*object_id); + true + } + XrNetSharedObjectControl::XrTakeoverAccept { + object_id, + epoch, + request_id, + new_authority, + effective_at, + effective_tick, + pose, + linvel, + angvel, + } => self.handle_takeover_accept( + *object_id, + *epoch, + *request_id, + peer.id, + *new_authority, + *effective_at, + *effective_tick, + *pose, + *linvel, + *angvel, + ), + XrNetSharedObjectControl::XrResetObject { + object_id, + epoch, + pose, + linvel, + angvel, + } => self.handle_reset_object(cx, peer.id, *object_id, *epoch, *pose, *linvel, *angvel), + XrNetSharedObjectControl::XrResetActivityPose { activity_id, pose } => { + if self.runtime.shared_objects.activity_id() != Some(*activity_id) { + return false; + } + let Some(peer_state) = self.runtime.registry.peers.get_mut(&peer.id) else { + return false; + }; + peer_state.remote_activity_pose = Some(*pose); + peer_state.last_emitted_local_activity_pose = None; + self.emit_authoritative_remote_activity_pose_if_changed(cx); + true + } + XrNetSharedObjectControl::XrClockPing { seq, sent_at } => { + self.runtime.metrics.record_clock_ping_rx(peer.id, *seq); + let replied_at = if self.runtime.local.state_time != 0.0 { + self.runtime.local.state_time + } else { + Cx::time_now() + }; + if let Some(net_node) = self.runtime.net_node.as_mut() { + net_node.send_shared_object_control(XrNetSharedObjectControl::XrClockPong { + seq: *seq, + echoed_at: *sent_at, + replied_at, + }); + self.runtime.metrics.record_clock_pong_tx(*seq); + } + true + } + XrNetSharedObjectControl::XrClockPong { + seq, + echoed_at: _, + replied_at, + } => { + self.runtime.metrics.record_clock_pong_rx(peer.id, *seq); + let Some(local_sent_at) = self + .runtime + .pending_clock_pings + .iter() + .find_map(|(pending_seq, sent_at)| (*pending_seq == *seq).then_some(*sent_at)) + else { + return true; + }; + let now = if self.runtime.local.state_time != 0.0 { + self.runtime.local.state_time + } else { + Cx::time_now() + }; + let round_trip_seconds = (now - local_sent_at).max(0.0); + let midpoint = local_sent_at + round_trip_seconds * 0.5; + let clock_offset_seconds = *replied_at - midpoint; + if let Some(peer_state) = self.runtime.registry.peers.get_mut(&peer.id) { + peer_state.clock_offset_seconds = Some(clock_offset_seconds); + peer_state.clock_round_trip_seconds = Some(round_trip_seconds); + peer_state.last_clock_sync_at = Some(now); + } + true + } + XrNetSharedObjectControl::XrTakeoverRequest { + object_id, + epoch, + request_id, + based_on_seq, + based_on_tick, + candidate_owner, + hand, + hand_pose, + hand_linvel, + } => self.handle_takeover_request( + peer, + *object_id, + *epoch, + *request_id, + *based_on_seq, + *based_on_tick, + *candidate_owner, + *hand, + *hand_pose, + *hand_linvel, + ), + XrNetSharedObjectControl::XrTakeoverReject { + object_id, + request_id, + .. + } => self.handle_takeover_reject(*object_id, *request_id), + XrNetSharedObjectControl::XrContactImpulse { + object_id, + epoch, + based_on_seq, + based_on_tick, + hand, + hand_pose, + point, + impulse, + } => self.handle_contact_impulse( + cx, + peer, + *object_id, + *epoch, + *based_on_seq, + *based_on_tick, + *hand, + *hand_pose, + *point, + *impulse, + ), + } + } +} diff --git a/xr/src/sync/space.rs b/xr/src/sync/space.rs new file mode 100644 index 0000000000..800f25ed96 --- /dev/null +++ b/xr/src/sync/space.rs @@ -0,0 +1,109 @@ +use super::*; + +impl XrPeerSync { + pub(super) fn peer_label(peer_id: XrNetPeerId) -> String { + format!("{:08x}", peer_id.0) + } + + pub(super) fn peer_transform(peer: &RemotePeerState) -> Mat4f { + peer.remote_to_local.unwrap_or_default() + } + + pub(super) fn peer_remote_to_local_transform(&self, peer_id: XrNetPeerId) -> Mat4f { + self.runtime + .registry + .peers + .get(&peer_id) + .map(Self::peer_transform) + .unwrap_or_default() + } + + pub(super) fn transform_point(transform: &Mat4f, point: Vec3f) -> Vec3f { + let point = transform.transform_vec4(vec4f(point.x, point.y, point.z, 1.0)); + if point.w.abs() > 1.0e-6 { + vec3f(point.x / point.w, point.y / point.w, point.z / point.w) + } else { + point.to_vec3f() + } + } + + pub(super) fn transform_anchor(transform: &Mat4f, anchor: XrAnchor) -> XrAnchor { + XrAnchor { + left: Self::transform_point(transform, anchor.left), + right: Self::transform_point(transform, anchor.right), + } + } + + pub(super) fn transform_direction(transform: &Mat4f, direction: Vec3f) -> Vec3f { + transform + .transform_vec4(vec4f(direction.x, direction.y, direction.z, 0.0)) + .to_vec3f() + } + + pub(super) fn transform_pose(transform: &Mat4f, pose: Pose) -> Pose { + let position = Self::transform_point(transform, pose.position); + let mut forward = Self::transform_direction( + transform, + pose.orientation.rotate_vec3(&vec3f(0.0, 0.0, -1.0)), + ); + let mut up = Self::transform_direction( + transform, + pose.orientation.rotate_vec3(&vec3f(0.0, 1.0, 0.0)), + ); + if forward.length() <= 1.0e-6 { + return Pose::new(pose.orientation, position); + } + forward = forward.normalize(); + if up.length() <= 1.0e-6 || Vec3f::cross(forward, up).length() <= 1.0e-6 { + up = vec3f(0.0, 1.0, 0.0); + } else { + up = up.normalize(); + } + Pose::new(Quat::look_rotation(forward, up), position) + } + + fn hand_tracking_pose(hand: &XrHand) -> Option { + hand.tracking_pose() + } + + fn local_hand_state_from_frames( + current: &XrState, + previous: Option<&XrState>, + shared_hand: XrSharedHand, + ) -> Option { + let (hand, previous_hand) = match shared_hand { + XrSharedHand::LeftHand => (¤t.left_hand, previous.map(|state| &state.left_hand)), + XrSharedHand::RightHand => { + (¤t.right_hand, previous.map(|state| &state.right_hand)) + } + _ => return None, + }; + let pose = Self::hand_tracking_pose(hand)?; + let previous_pose = previous_hand + .and_then(Self::hand_tracking_pose) + .unwrap_or(pose); + let dt = previous + .map(|previous| (current.time - previous.time).abs()) + .unwrap_or(0.0) + .max(0.0001) as f32; + Some(LocalSharedHandState { + shared_hand, + pose, + linvel: (pose.position - previous_pose.position) * (1.0 / dt), + gripping: hand.grab_intent(), + }) + } + + pub(super) fn local_shared_hands(&self) -> Vec { + let Some(current) = self.runtime.local.latest_xr_state.as_ref() else { + return Vec::new(); + }; + let previous = self.runtime.local.previous_xr_state.as_ref(); + [XrSharedHand::LeftHand, XrSharedHand::RightHand] + .into_iter() + .filter_map(|shared_hand| { + Self::local_hand_state_from_frames(current, previous, shared_hand) + }) + .collect() + } +} diff --git a/xr/src/sync/xr_peer_sync.rs b/xr/src/sync/xr_peer_sync.rs new file mode 100644 index 0000000000..95d8e7a607 --- /dev/null +++ b/xr/src/sync/xr_peer_sync.rs @@ -0,0 +1,513 @@ +use crate::scene::{ + arm_pair_metrics, flat_head_forward, hand_closed_fist_contact_point_geometry_only, +}; +use crate::prelude::*; +use makepad_widgets::event::{XrSyncAnchor, XrSyncAnchorExtrema}; +use std::{ + collections::{HashMap, VecDeque}, + sync::{mpsc::TryRecvError, Arc, Mutex}, + time::{Duration, Instant}, +}; + +#[path = "alignment.rs"] +mod alignment; +#[path = "diagnostics.rs"] +mod diagnostics; +#[path = "lifecycle.rs"] +mod lifecycle; +#[path = "local_state.rs"] +mod local_state; +#[path = "metrics.rs"] +mod metrics; +#[path = "registry.rs"] +mod registry; +#[path = "render.rs"] +mod render; +#[path = "shared_objects.rs"] +mod shared_objects; +#[path = "space.rs"] +mod space; + +use self::{alignment::*, diagnostics::*, local_state::*, metrics::*, registry::*}; + +script_mod! { + use mod.prelude.widgets_internal.* + + mod.widgets.XrPeerSyncBase = #(XrPeerSync::register_widget(vm)) + mod.widgets.XrPeerSync = set_type_default() do mod.widgets.XrPeerSyncBase{ + body: mod.widgets.XrBodyKind.Disabled + draw_cube +: { + light_dir: vec3(0.35, 0.8, 0.45) + } + } +} + +#[derive(Clone, Copy, Debug, Default, PartialEq)] +pub enum XrPeerSyncAction { + ActivityChanged(XrActivityId), + ActivityPoseReset(Pose), + BodySpawn(XrBodySpawn), + BodyImpulse(XrBodyImpulse), + BodyDespawn(WidgetUid), + #[default] + None, +} + +#[derive(Script, ScriptHook, Widget)] +pub struct XrPeerSync { + #[redraw] + #[live] + draw_cube: DrawCube, + #[live(false)] + auto_alignment_enabled: bool, + #[rust] + enabled: bool, + #[rust] + net_config_override: Option, + #[rust] + runtime: XrPeerSyncRuntime, + #[rust] + diagnostics: XrPeerSyncDiagnostics, + #[cast] + #[deref] + node: XrNode, +} + +impl XrPeerSync { + const HEADSET_SIZE: Vec3f = Vec3f { + x: 0.12, + y: 0.05, + z: 0.08, + }; + const HAND_SIZE: Vec3f = Vec3f { + x: 0.08, + y: 0.05, + z: 0.10, + }; + const REMOTE_HAND_PALM_SIZE: Vec3f = Vec3f { + x: 0.055, + y: 0.018, + z: 0.075, + }; + const REMOTE_HAND_JOINT_SIZE: f32 = 0.016; + const ANCHOR_MARKER_SIZE: f32 = 0.060; + const ANCHOR_CONFIRMATION_SECONDS: f64 = 2.0; + const LOCAL_SYNC_SAMPLE_PREVIEW_SECONDS: f64 = 1.0; + const SYNC_MATCH_RECEIVE_WINDOW_SECONDS: f64 = 0.45; + const SYNC_SAMPLE_PAIR_WINDOW_SECONDS: f64 = 0.10; + const SYNC_SAMPLE_HISTORY_SECONDS: f64 = 1.5; + const SYNC_SAMPLE_SESSION_RESET_SECONDS: f64 = 1.0; + const SYNC_EXISTING_MARKER_REUSE_DISTANCE_METERS: f32 = 1.0; + const DESCRIPTOR_SEND_MIN_CHANGE_PERCENT: f32 = 4.0; + const SYNC_MATCH_ACTIVE_WINDOW_SECONDS: f64 = 1.35; + const FIST_ACK_STICKY_WINDOW_SECONDS: f64 = 0.35; + const FIST_ACK_MAX_VERTICAL_DELTA_METERS: f32 = 0.22; + const FIST_ACK_MAX_DEPTH_DELTA_METERS: f32 = 0.22; + const FIST_ACK_MIN_HAND_GAP_METERS: f32 = 0.06; + const FIST_ACK_MAX_HAND_GAP_METERS: f32 = 0.78; + const FIST_ACK_MIN_CHEST_DISTANCE_METERS: f32 = 0.10; + const FIST_ACK_MAX_CHEST_DISTANCE_METERS: f32 = 1.05; + const FIST_ACK_MAX_ARM_ELEVATION_DEGREES: f32 = 60.0; + const DESCRIPTOR_MAX_HEIGHT_METERS: f32 = 2.00; + const DESCRIPTOR_MIN_HEIGHT_METERS: f32 = 0.08; + const DESCRIPTOR_CELL_FOOTPRINT: f32 = 0.62; + const SHOW_LOCAL_DESCRIPTOR_DEBUG: bool = false; + const CLOCK_PING_INTERVAL_SECONDS: f64 = 1.0; + const SHARED_OBJECT_SHADOW_MAX_EXTRAPOLATION_SECONDS: f32 = 0.10; + const SHARED_OBJECT_SHADOW_INTERPOLATION_DELAY_SECONDS: f64 = 0.12; + const SHARED_OBJECT_SHADOW_REAPPLY_POSITION_EPSILON_METERS: f32 = 0.015; + const SHARED_OBJECT_SHADOW_REAPPLY_ORIENTATION_EPSILON_DEGREES: f32 = 1.5; + const SHARED_OBJECT_SHADOW_REAPPLY_LINVEL_EPSILON_MPS: f32 = 0.08; + const SHARED_OBJECT_SHADOW_REAPPLY_ANGVEL_EPSILON_RADPS: f32 = 0.08; + const SHARED_OBJECT_TAKEOVER_DISTANCE_METERS: f32 = 0.18; + const SHARED_OBJECT_TAKEOVER_RELATIVE_SPEED_MAX: f32 = 2.4; + const SHARED_OBJECT_TAKEOVER_EFFECTIVE_DELAY_SECONDS: f64 = 0.12; + const SHARED_OBJECT_TAKEOVER_EFFECTIVE_TICK_OFFSET: u32 = 3; + const SHARED_OBJECT_IMPULSE_DISTANCE_METERS: f32 = 0.16; + const SHARED_OBJECT_IMPULSE_MIN_HAND_SPEED: f32 = 0.65; + const SHARED_OBJECT_IMPULSE_SCALE: f32 = 0.08; + const SHARED_OBJECT_BOOTSTRAP_OWNER_TAG: u64 = 0x626f6f7473747261; + + pub fn status_text(&self) -> &str { + self.diagnostics.status_text() + } + + pub fn connected_peer_count(&self) -> usize { + self.runtime.registry.len() + } + + pub fn local_is_activity_authority(&self) -> bool { + let Some(activity) = self.runtime.accepted_activity else { + return false; + }; + self.runtime + .net_node + .as_ref() + .is_some_and(|node| node.node_id() == activity.changed_by) + } + + pub fn enabled(&self) -> bool { + self.enabled + } + + pub fn auto_alignment_enabled(&self) -> bool { + self.auto_alignment_enabled + } + + pub fn current_activity(&self) -> Option { + Some(self.runtime.accepted_activity?.activity_id) + } + + pub fn spawnable_activity(&self) -> Option { + self.runtime.shared_objects.activity_id() + } + + pub fn shared_object_count(&self) -> usize { + self.runtime.shared_objects.active_count() + } + + pub fn pending_shared_object_control_count(&self) -> usize { + self.runtime.pending_shared_object_controls.len() + } + + pub fn tx_body_spawn_count(&self) -> u64 { + self.runtime.metrics.tx_body_spawn_count + } + + pub fn tx_shared_object_state_count(&self) -> u64 { + self.runtime.metrics.tx_shared_object_state_count + } + + pub fn rx_body_spawn_count(&self) -> u64 { + self.runtime.metrics.rx_body_spawn_count + } + + pub fn rx_shared_object_state_count(&self) -> u64 { + self.runtime.metrics.rx_shared_object_state_count + } + + pub fn remote_shadow_apply_count(&self) -> u64 { + self.runtime.metrics.remote_shadow_apply_count + } + + pub fn last_network_event_label(&self) -> &str { + self.runtime.metrics.last_event_label() + } + + pub fn network_status_text(&self) -> &str { + self.diagnostics.network_status_text() + } + + pub fn clock_synced_peer_count(&self) -> usize { + self.runtime + .registry + .peers + .values() + .filter(|peer| peer.clock_offset_seconds.is_some()) + .count() + } + + pub fn clock_ping_tx_count(&self) -> u64 { + self.runtime.metrics.tx_clock_ping_count + } + + pub fn clock_ping_rx_count(&self) -> u64 { + self.runtime.metrics.rx_clock_ping_count + } + + pub fn clock_pong_tx_count(&self) -> u64 { + self.runtime.metrics.tx_clock_pong_count + } + + pub fn clock_pong_rx_count(&self) -> u64 { + self.runtime.metrics.rx_clock_pong_count + } + + pub fn non_xr_draw_clock_count(&self) -> u64 { + self.runtime.metrics.non_xr_draw_clock_count + } + + pub fn alignment_debug_text(&self) -> &str { + self.diagnostics.alignment_debug_text() + } + + pub fn touch_sync_status_text(&self) -> String { + if !self.enabled { + return "Touch sync: off".to_string(); + } + let manual_status = self.manual_touch_sync_status_text(); + if self.auto_alignment_enabled && manual_status == "Touch sync: idle" { + return "Touch sync: auto align on".to_string(); + } + manual_status + } + + pub fn local_touch_signal_text(&self) -> String { + if !self.enabled { + return "TouchRaw: off".to_string(); + } + let Some(state) = self.runtime.local.latest_xr_state.as_ref() else { + return "TouchRaw: waiting for local XR".to_string(); + }; + Self::touch_signal_text_for_state("TouchRaw", "TouchLF", "TouchRF", state) + } + + pub fn remote_touch_signal_text(&self) -> String { + if !self.enabled { + return "TouchPeer: off".to_string(); + } + let Some((peer_id, peer_state)) = self.runtime.registry.preferred_peer() else { + return "TouchPeer: waiting for peer".to_string(); + }; + let Some(state_frame) = peer_state.latest_state.as_ref() else { + return format!("TouchPeer {:08x}: waiting for peer XR", peer_id.0); + }; + Self::touch_signal_text_for_state( + &format!("TouchPeer {:08x}", peer_id.0), + "TouchPLF", + "TouchPRF", + &state_frame.state, + ) + } + + pub fn alignment_state_text(&self) -> &str { + self.diagnostics.alignment_state_text() + } + + pub fn peer_scene_text(&self) -> &str { + self.diagnostics.peer_scene_text() + } + + pub fn aligned_peer_height_map(&self) -> Option { + let (_, peer_state) = self.runtime.registry.preferred_peer()?; + let transform = peer_state.descriptor_remote_to_local.or_else(|| { + peer_state + .last_solve_diagnostic + .and_then(|diagnostic| diagnostic.best_solution) + .map(|solution| solution.remote_to_local_transform()) + })?; + let descriptor = peer_state.latest_descriptor?.descriptor; + descriptor.transformed(&transform).height_map + } + + pub fn raw_peer_alignment_descriptor( + &self, + ) -> Option<(XrNetPeerId, XrNetAlignmentDescriptorFrame)> { + let (peer_id, peer_state) = self.runtime.registry.preferred_peer()?; + Some((peer_id, peer_state.latest_descriptor?)) + } + + pub fn raw_peer_height_map(&self) -> Option { + let (_, descriptor) = self.raw_peer_alignment_descriptor()?; + descriptor.descriptor.height_map + } + + pub fn raw_alignment_dump_pair(&self) -> Option { + let local_descriptor = self.runtime.local.descriptor.clone()?; + let (peer_id, remote_descriptor) = self.raw_peer_alignment_descriptor()?; + if local_descriptor.descriptor.height_map.is_none() + || remote_descriptor.descriptor.height_map.is_none() + { + return None; + } + Some(XrNetAlignmentDescriptorDumpPair::new( + peer_id, + local_descriptor, + remote_descriptor, + )) + } + + pub fn local_slice_preview(&self) -> Option { + self.runtime.local.slice_preview.clone() + } + + fn apply_surface_analysis_enabled(&self, cx: &mut Cx) { + cx.xr_tsdf() + .set_surface_analysis_enabled(self.enabled && self.auto_alignment_enabled); + } + + pub fn set_enabled(&mut self, cx: &mut Cx, enabled: bool) -> bool { + if self.enabled == enabled { + return self.enabled; + } + self.enabled = enabled; + self.apply_surface_analysis_enabled(cx); + self.runtime = XrPeerSyncRuntime::default(); + self.diagnostics = XrPeerSyncDiagnostics::default(); + + if enabled { + if self.auto_alignment_enabled { + self.runtime.alignment_worker = Some(XrPeopleAlignmentWorker::new(cx.xr_tsdf())); + } + self.ensure_net_node(); + self.diagnostics + .set_enabled_defaults(self.auto_alignment_enabled, self.runtime.net_node.is_some()); + } else { + self.diagnostics.set_disabled(); + } + self.redraw(cx); + self.enabled + } + + pub fn set_auto_alignment_enabled(&mut self, cx: &mut Cx, enabled: bool) -> bool { + if self.auto_alignment_enabled == enabled { + self.apply_surface_analysis_enabled(cx); + return self.auto_alignment_enabled; + } + let restart = self.enabled; + self.auto_alignment_enabled = enabled; + if restart { + self.set_enabled(cx, false); + self.set_enabled(cx, true); + } else { + self.apply_surface_analysis_enabled(cx); + } + self.auto_alignment_enabled + } + + pub fn set_net_config_override(&mut self, config: XrNetConfig) { + self.net_config_override = Some(config); + } + + pub fn set_local_activity( + &mut self, + _cx: &mut Cx, + activity_id: XrActivityId, + ) -> Option { + if !self.enabled { + return None; + } + self.ensure_net_node(); + let changed_at = if self.runtime.local.state_time != 0.0 { + self.runtime.local.state_time + } else { + Cx::time_now() + }; + let local_node_id = self.runtime.net_node.as_ref()?.node_id(); + if self.runtime.accepted_activity.is_some_and(|current| { + current.activity_id == activity_id && current.changed_by == local_node_id + }) { + return self.runtime.accepted_activity; + } + let state = self + .runtime + .net_node + .as_mut()? + .send_activity(activity_id, changed_at); + self.runtime.accepted_activity = Some(state); + self.runtime.registry.clear_remote_activity_poses(); + self.runtime.metrics.record_activity_tx(state); + Some(state) + } + + pub fn set_spawnable_objects(&mut self, activity_id: XrActivityId, bindings: I) -> usize + where + I: IntoIterator, + { + self.runtime + .shared_objects + .replace_spawnables(activity_id, bindings); + self.runtime.shared_objects.len() + } +} + +impl Widget for XrPeerSync { + fn script_call( + &mut self, + vm: &mut ScriptVm, + method: LiveId, + args: ScriptValue, + ) -> ScriptAsyncResult { + if method == live_id!(set_enabled) { + let mut enabled = self.enabled; + if let Some(args_obj) = args.as_object() { + let trap = vm.bx.threads.cur().trap.pass(); + enabled = vm + .bx + .heap + .cast_to_bool(vm.bx.heap.vec_value(args_obj, 0, trap)); + } + let enabled = vm.with_cx_mut(|cx| self.set_enabled(cx, enabled)); + return ScriptAsyncResult::Return(ScriptValue::from_bool(enabled)); + } + if method == live_id!(toggle_enabled) || method == live_id!(toggle_test) { + let enabled = vm.with_cx_mut(|cx| self.set_enabled(cx, !self.enabled)); + return ScriptAsyncResult::Return(ScriptValue::from_bool(enabled)); + } + if method == live_id!(enabled) { + return ScriptAsyncResult::Return(ScriptValue::from_bool(self.enabled)); + } + if method == live_id!(set_auto_alignment_enabled) || method == live_id!(set_auto_align) { + let mut enabled = self.auto_alignment_enabled; + if let Some(args_obj) = args.as_object() { + let trap = vm.bx.threads.cur().trap.pass(); + enabled = vm + .bx + .heap + .cast_to_bool(vm.bx.heap.vec_value(args_obj, 0, trap)); + } + let enabled = vm.with_cx_mut(|cx| self.set_auto_alignment_enabled(cx, enabled)); + return ScriptAsyncResult::Return(ScriptValue::from_bool(enabled)); + } + if method == live_id!(auto_alignment_enabled) || method == live_id!(auto_align) { + return ScriptAsyncResult::Return(ScriptValue::from_bool(self.auto_alignment_enabled)); + } + self.node.script_call(vm, method, args) + } + + fn script_result(&mut self, vm: &mut ScriptVm, id: ScriptAsyncId, result: ScriptValue) { + self.node.script_result(vm, id, result); + } + + fn handle_event(&mut self, cx: &mut Cx, event: &Event, scope: &mut Scope) { + if self.enabled { + if let Event::XrUpdate(update) = event { + self.refresh_from_local_state(cx, update.state.as_ref()); + } else if !cx.in_xr_mode() { + if let Some(local_time) = Self::timed_event_local_time(event) { + self.service_non_xr_local_clock(local_time); + } + } + self.poll_network(cx); + self.apply_alignment_results(cx); + self.refresh_status(); + } + self.node.handle_event(cx, event, scope); + } + + fn draw_3d(&mut self, cx: &mut Cx3d, scope: &mut Scope) -> DrawStep { + if !self.enabled { + return self.node.draw_3d(cx, scope); + } + if cx.scene_state_3d().is_none() { + return DrawStep::done(); + } + let world = if cx.cx.in_xr_mode() { + Mat4f::identity() + } else { + self.node.local_transform() + }; + self.draw_cube.begin_many_instances(cx); + self.draw_pending_sync_anchor_preview(cx, &world); + self.draw_recent_anchor_confirmation(cx, &world); + if self.auto_alignment_enabled && Self::SHOW_LOCAL_DESCRIPTOR_DEBUG { + self.draw_local_descriptor(cx, &world); + } + self.draw_remote_peers(cx, &world); + self.draw_cube.end_many_instances(cx); + self.node.draw_3d(cx, scope) + } + + fn draw_walk(&mut self, _cx: &mut Cx2d, _scope: &mut Scope, _walk: Walk) -> DrawStep { + DrawStep::done() + } +} + +#[cfg(test)] +include!("../tests/scene/xr_peer_sync_descriptor_pair.rs"); +#[cfg(test)] +include!("../tests/scene/xr_peer_sync_alignment.rs"); +#[cfg(test)] +include!("../tests/scene/xr_peer_sync_touch_sync.rs"); +#[cfg(test)] +include!("../tests/scene/xr_peer_sync_shared_objects.rs"); diff --git a/xr/src/tests/algorithms/tsdf_query.rs b/xr/src/tests/algorithms/tsdf_query.rs new file mode 100644 index 0000000000..4d2c75ea56 --- /dev/null +++ b/xr/src/tests/algorithms/tsdf_query.rs @@ -0,0 +1,99 @@ +mod tests { + use super::*; + use std::{collections::HashMap, sync::Arc}; + + fn set_normalized_distance( + chunks: &mut HashMap>, + chunk_edge: i32, + coord: TsdfVoxelCoord, + normalized_distance: f32, + ) { + let cx = coord.x.div_euclid(chunk_edge); + let cy = coord.y.div_euclid(chunk_edge); + let cz = coord.z.div_euclid(chunk_edge); + let lx = coord.x.rem_euclid(chunk_edge) as usize; + let ly = coord.y.rem_euclid(chunk_edge) as usize; + let lz = coord.z.rem_euclid(chunk_edge) as usize; + let edge = chunk_edge as usize; + let id = lx + ly * edge + lz * edge * edge; + let chunk = Arc::make_mut( + chunks + .entry(ChunkKey::new(cx, cy, cz)) + .or_insert_with(|| Arc::new(SparseTsdReadChunk::new(edge * edge * edge))), + ); + chunk.set_value(id, normalized_distance, 8, 1); + } + + fn make_flat_floor_snapshot(voxel_size: f32) -> TsdfPublishedSnapshot { + let chunk_edge = 8; + let mut chunks = HashMap::new(); + let tsd_distance_meters = depth_tsd_distance_meters(voxel_size); + let mut active_value_count = 0usize; + for z in -6..=6 { + for y in -6..=6 { + for x in -6..=6 { + let world_y = voxel_center_axis(voxel_size, y); + let normalized = (world_y / tsd_distance_meters).clamp(-1.0, 1.0); + set_normalized_distance( + &mut chunks, + chunk_edge, + TsdfVoxelCoord::new(x, y, z), + normalized, + ); + active_value_count += 1; + } + } + } + TsdfPublishedSnapshot { + generation: 1, + latest_topology_generation: 1, + update_sequence: 1, + grid: Arc::new(SparseTsdGridReadSnapshot { + voxel_size, + chunk_edge, + chunk_edge_shift: Some(chunk_edge.trailing_zeros() as u8), + chunk_edge_mask: chunk_edge - 1, + chunk_volume: (chunk_edge as usize).pow(3), + active_value_count, + active_bounds: Some(( + vec3f(-6.0 * voxel_size, -6.0 * voxel_size, -6.0 * voxel_size), + vec3f(7.0 * voxel_size, 7.0 * voxel_size, 7.0 * voxel_size), + )), + chunks, + }), + height_map: None, + } + } + + #[test] + fn direct_tsdf_query_finds_flat_floor_support() { + let snapshot = make_flat_floor_snapshot(0.05); + let result = evaluate_tsdf_query( + &snapshot, + DepthQuery { + key: 1, + center: vec3f(0.0, 0.09, 0.0), + predicted_center: vec3f(0.0, 0.07, 0.0), + velocity: vec3f(0.0, -0.2, 0.0), + radius: 0.05, + max_distance: 0.12, + }, + ); + + match result { + DepthQueryResult::Hit(hit) => { + let DepthQueryColliderGeometry::HalfSpace(plane) = hit.collider.geometry; + assert!( + plane.normal.y >= 0.98, + "expected floor support plane, got {plane:?}" + ); + assert!( + plane.point.y.abs() <= 0.03, + "expected plane near y=0, got {:?}", + plane.point + ); + } + DepthQueryResult::Miss { .. } => panic!("expected flat-floor TSDF query hit"), + } + } +} diff --git a/xr/src/tests/obj/shooter.rs b/xr/src/tests/obj/shooter.rs new file mode 100644 index 0000000000..c584f0888c --- /dev/null +++ b/xr/src/tests/obj/shooter.rs @@ -0,0 +1,135 @@ +mod tests { + use super::*; + use std::rc::Rc; + + fn point_pose(base: Vec3f, z: f32) -> Pose { + Pose::new(Quat::default(), base + vec3f(0.0, 0.0, z)) + } + + fn make_pointing_hand() -> XrHand { + let base = vec3f(0.20, 1.22, -0.22); + let mut hand = XrHand::default(); + hand.flags = XrHand::IN_VIEW | XrHand::AIM_VALID | XrHand::DOMINANT_HAND; + hand.tips_active = 1 << XrHand::INDEX_TIP; + hand.tips[XrHand::INDEX_TIP] = 0.038; + + hand.joints[XrHand::CENTER] = Pose::new(Quat::default(), base + vec3f(0.0, -0.03, 0.03)); + hand.joints[XrHand::WRIST] = Pose::new(Quat::default(), base + vec3f(0.0, -0.05, 0.08)); + hand.joints[XrHand::INDEX_BASE] = point_pose(base, 0.0); + hand.joints[XrHand::INDEX_KNUCKLE1] = point_pose(base, -0.041); + hand.joints[XrHand::INDEX_KNUCKLE2] = + point_pose(base + vec3f(0.001, 0.002, 0.0), -0.082); + hand.joints[XrHand::INDEX_KNUCKLE3] = + point_pose(base + vec3f(0.002, 0.004, 0.0), -0.122); + hand.aim_pose = Pose::new(Quat::default(), base + vec3f(0.0, 0.0, -0.16)); + hand + } + + fn make_curled_hand() -> XrHand { + let base = vec3f(0.20, 1.22, -0.22); + let mut hand = XrHand::default(); + hand.flags = XrHand::IN_VIEW | XrHand::AIM_VALID; + hand.tips_active = 1 << XrHand::INDEX_TIP; + hand.tips[XrHand::INDEX_TIP] = 0.030; + + hand.joints[XrHand::CENTER] = Pose::new(Quat::default(), base + vec3f(0.0, -0.03, 0.03)); + hand.joints[XrHand::WRIST] = Pose::new(Quat::default(), base + vec3f(0.0, -0.05, 0.08)); + hand.joints[XrHand::INDEX_BASE] = point_pose(base, 0.0); + hand.joints[XrHand::INDEX_KNUCKLE1] = point_pose(base, -0.030); + hand.joints[XrHand::INDEX_KNUCKLE2] = + Pose::new(Quat::default(), base + vec3f(0.018, -0.012, -0.040)); + hand.joints[XrHand::INDEX_KNUCKLE3] = + Pose::new(Quat::default(), base + vec3f(0.034, -0.030, -0.032)); + hand.aim_pose = Pose::new(Quat::default(), base + vec3f(0.0, 0.0, -0.12)); + hand + } + + fn make_sparse_tracking_hand() -> XrHand { + let base = vec3f(0.20, 1.22, -0.22); + let mut hand = XrHand::default(); + hand.flags = XrHand::IN_VIEW | XrHand::AIM_VALID; + hand.tips_active = XrHand::GRAB_ACTIVE; + hand.joints[XrHand::CENTER] = Pose::new(Quat::default(), base + vec3f(0.0, -0.03, 0.03)); + hand.joints[XrHand::WRIST] = Pose::new(Quat::default(), base + vec3f(0.0, -0.05, 0.08)); + hand.aim_pose = Pose::new(Quat::default(), base + vec3f(0.0, 0.0, -0.12)); + hand + } + + fn make_pointing_hand_with_sideways_aim_pose() -> XrHand { + let mut hand = make_pointing_hand(); + hand.aim_pose.orientation = Quat::from_axis_angle(vec3f(0.0, 1.0, 0.0), 0.75); + hand + } + + #[test] + fn point_gesture_is_considered_an_emit_gesture() { + let hand = make_pointing_hand(); + assert!(Shooter::hand_emit_gesture_active(&hand)); + assert!(Shooter::projectile_emitter_pose(&hand).is_some()); + } + + #[test] + fn generic_grab_bit_does_not_block_emit_gesture_when_pointing_pose_is_valid() { + let mut hand = make_pointing_hand(); + hand.tips_active |= XrHand::GRAB_ACTIVE; + assert!(Shooter::hand_emit_gesture_active(&hand)); + assert!(Shooter::projectile_emitter_pose(&hand).is_some()); + } + + #[test] + fn point_gesture_without_tip_active_bit_still_emits_from_joint_chain() { + let mut hand = make_pointing_hand(); + hand.tips_active &= !(1 << XrHand::INDEX_TIP); + let metrics = Shooter::hand_index_finger_stretch_metrics(&hand) + .expect("joint-chain metrics should still be derivable without the tip-active bit"); + assert!(metrics.max_bend_angle_degrees <= SHOOTER_INDEX_BEND_MAX_DEGREES); + assert!(Shooter::hand_emit_gesture_active(&hand)); + assert!(Shooter::projectile_emitter_pose(&hand).is_some()); + } + + #[test] + fn projectile_direction_follows_finger_chain_not_openxr_aim_ray() { + let hand = make_pointing_hand_with_sideways_aim_pose(); + let (_, direction) = Shooter::projectile_emitter_pose(&hand) + .expect("pointing hand should emit even if aim pose diverges"); + assert!(direction.z < -0.9, "{direction:?}"); + assert!(direction.x.abs() < 0.2, "{direction:?}"); + } + + #[test] + fn curled_index_finger_is_rejected() { + let hand = make_curled_hand(); + assert!( + Shooter::hand_index_finger_stretch_metrics(&hand).is_some_and(|metrics| { + metrics.max_bend_angle_degrees > SHOOTER_INDEX_BEND_MAX_DEGREES + }) + ); + assert!(!Shooter::hand_emit_gesture_active(&hand)); + } + + #[test] + fn sparse_tracking_sample_is_rejected_for_emit_gesture() { + let hand = make_sparse_tracking_hand(); + assert!(Shooter::hand_index_finger_stretch_metrics(&hand).is_none()); + assert!(!Shooter::hand_emit_gesture_active(&hand)); + assert!(Shooter::projectile_emitter_pose(&hand).is_none()); + } + + #[test] + fn dominant_hand_selection_prefers_pointing_hand() { + let mut update = XrUpdateEvent { + state: Rc::new(XrState::default()), + last: Rc::new(XrState::default()), + }; + let mut left = make_pointing_hand(); + left.flags |= XrHand::DOMINANT_HAND; + let right = make_curled_hand(); + Rc::make_mut(&mut update.state).left_hand = left; + Rc::make_mut(&mut update.state).right_hand = right; + let emitter = Shooter::main_projectile_emitter_pose(&update); + assert!(emitter.is_some()); + let (pos, dir) = emitter.unwrap(); + assert!(pos.z < -0.30); + assert!(dir.z < -0.8); + } +} diff --git a/xr/src/tests/obj/tank.rs b/xr/src/tests/obj/tank.rs new file mode 100644 index 0000000000..263a8d1a4e --- /dev/null +++ b/xr/src/tests/obj/tank.rs @@ -0,0 +1,37 @@ +#[test] +fn head_relative_drive_direction_uses_head_yaw() { + let controller = XrController { + buttons: XrController::ACTIVE, + stick: vec2f(0.0, 1.0), + ..XrController::default() + }; + let yaw_right = Quat::from_axis_angle(vec3f(0.0, 1.0, 0.0), -std::f32::consts::FRAC_PI_2); + + let (direction, amount) = stick_deadzone_scaled_direction(&controller, yaw_right, 0.16) + .expect("active stick should produce a drive direction"); + + assert!((direction - vec3f(1.0, 0.0, 0.0)).length() < 0.0001); + assert!((amount - 1.0).abs() < 0.0001); +} + +#[test] +fn track_commands_turn_right_for_target_on_right() { + let pose = Pose::default(); + let desired_direction = vec3f(1.0, 0.0, 0.0); + let (left, right) = + differential_track_commands(pose, vec3f(0.0, 0.0, 0.0), desired_direction, 1.0, 1.35, 1.45); + + assert!(left < 0.0); + assert!(right > 0.0); +} + +#[test] +fn track_commands_reverse_when_target_is_behind_body() { + let pose = Pose::default(); + let desired_direction = vec3f(0.0, 0.0, 1.0); + let (left, right) = + differential_track_commands(pose, vec3f(0.0, 0.0, 0.0), desired_direction, 1.0, 1.35, 1.45); + + assert!(left < 0.0); + assert!(right < 0.0); +} diff --git a/xr/src/tests/scene/xr_depth.rs b/xr/src/tests/scene/xr_depth.rs new file mode 100644 index 0000000000..cce500add4 --- /dev/null +++ b/xr/src/tests/scene/xr_depth.rs @@ -0,0 +1,121 @@ +mod tests { + use super::*; + + #[test] + fn depth_query_plane_supports_body_respects_exact_quad_edge() { + let plane = DepthQuerySupportPlane { + point: vec3f(0.0, 0.0, 0.0), + normal: vec3f(0.0, 1.0, 0.0), + tangent: vec3f(1.0, 0.0, 0.0), + bitangent: vec3f(0.0, 0.0, 1.0), + half_extent_tangent: 0.10, + half_extent_bitangent: 0.08, + }; + + assert!(depth_query_plane_supports_body( + plane, + vec3f(0.099, 0.03, 0.0), + 0.05, + 0.0, + )); + assert!(!depth_query_plane_supports_body( + plane, + vec3f(0.101, 0.03, 0.0), + 0.05, + 0.0, + )); + } + + #[test] + fn retained_support_refresh_requeries_before_body_reaches_quad_edge() { + let plane = DepthQuerySupportPlane { + point: vec3f(0.0, 0.0, 0.0), + normal: vec3f(0.0, 1.0, 0.0), + tangent: vec3f(1.0, 0.0, 0.0), + bitangent: vec3f(0.0, 0.0, 1.0), + half_extent_tangent: 0.10, + half_extent_bitangent: 0.10, + }; + let retained = RetainedDepthQueryHit { + surfaces: std::array::from_fn(|index| { + (index == 0).then_some(RetainedDepthQuerySurface { + target: DepthQuerySurfaceTarget { + collider: DepthQueryCollider { + fingerprint: 1, + geometry: DepthQueryColliderGeometry::HalfSpace(plane), + role: DepthQueryColliderRole::Support, + restitution: 0.0, + }, + }, + misses_left: XR_DEPTH_QUERY_HIT_MISS_GRACE_FRAMES, + }) + }), + }; + + assert!(retained.can_skip_refresh( + vec3f(0.00, 0.03, 0.0), + vec3f(0.02, 0.03, 0.0), + 0.05, + 0.30, + )); + assert!(!retained.can_skip_refresh( + vec3f(0.06, 0.03, 0.0), + vec3f(0.09, 0.03, 0.0), + 0.05, + 0.30, + )); + } + + #[test] + fn slow_support_refresh_margin_is_less_aggressive_than_fast_motion() { + let plane = DepthQuerySupportPlane { + point: vec3f(0.0, 0.0, 0.0), + normal: vec3f(0.0, 1.0, 0.0), + tangent: vec3f(1.0, 0.0, 0.0), + bitangent: vec3f(0.0, 0.0, 1.0), + half_extent_tangent: 0.10, + half_extent_bitangent: 0.10, + }; + let retained = RetainedDepthQueryHit { + surfaces: std::array::from_fn(|index| { + (index == 0).then_some(RetainedDepthQuerySurface { + target: DepthQuerySurfaceTarget { + collider: DepthQueryCollider { + fingerprint: 2, + geometry: DepthQueryColliderGeometry::HalfSpace(plane), + role: DepthQueryColliderRole::Support, + restitution: 0.0, + }, + }, + misses_left: XR_DEPTH_QUERY_HIT_MISS_GRACE_FRAMES, + }) + }), + }; + + assert!(retained.can_skip_refresh( + vec3f(0.082, 0.03, 0.0), + vec3f(0.088, 0.03, 0.0), + 0.05, + 0.05, + )); + assert!(!retained.can_skip_refresh( + vec3f(0.082, 0.03, 0.0), + vec3f(0.088, 0.03, 0.0), + 0.05, + 0.30, + )); + } + + #[test] + fn retained_support_does_not_hide_impact_capable_motion() { + assert!(depth_query_should_refresh_from_tsdf( + false, true, 0.12, true, + )); + assert!(!depth_query_should_refresh_from_tsdf( + false, true, 0.12, false, + )); + assert!(!depth_query_should_refresh_from_tsdf( + true, false, 0.40, true, + )); + } +} diff --git a/xr/src/tests/scene/xr_gesture.rs b/xr/src/tests/scene/xr_gesture.rs new file mode 100644 index 0000000000..ac32527c9a --- /dev/null +++ b/xr/src/tests/scene/xr_gesture.rs @@ -0,0 +1,224 @@ +mod tests { + use super::*; + + fn pose(position: Vec3f) -> Pose { + Pose::new(Quat::default(), position) + } + + fn mirror_x(is_left: bool, x: f32) -> f32 { + if is_left { + -x + } else { + x + } + } + + fn set_curled_finger( + hand: &mut XrHand, + is_left: bool, + base_joint: usize, + knuckle1_joint: usize, + knuckle2_joint: usize, + knuckle3_joint: usize, + x: f32, + y_bias: f32, + ) { + hand.joints[base_joint] = pose(vec3f(mirror_x(is_left, x), 1.18 + y_bias, -0.344)); + hand.joints[knuckle1_joint] = + pose(vec3f(mirror_x(is_left, x * 0.88), 1.174 + y_bias, -0.365)); + hand.joints[knuckle2_joint] = + pose(vec3f(mirror_x(is_left, x * 0.40), 1.153 + y_bias, -0.344)); + hand.joints[knuckle3_joint] = + pose(vec3f(mirror_x(is_left, x * 0.12), 1.156 + y_bias, -0.320)); + } + + fn set_thumbs_up_thumb(hand: &mut XrHand, is_left: bool) { + hand.joints[XrHand::THUMB_BASE] = pose(vec3f(mirror_x(is_left, 0.055), 1.168, -0.332)); + hand.joints[XrHand::THUMB_KNUCKLE1] = + pose(vec3f(mirror_x(is_left, 0.060), 1.196, -0.326)); + hand.joints[XrHand::THUMB_KNUCKLE2] = + pose(vec3f(mirror_x(is_left, 0.064), 1.228, -0.320)); + } + + fn set_open_finger( + hand: &mut XrHand, + is_left: bool, + base_joint: usize, + knuckle1_joint: usize, + knuckle2_joint: usize, + knuckle3_joint: usize, + x: f32, + y_bias: f32, + ) { + hand.joints[base_joint] = pose(vec3f(mirror_x(is_left, x), 1.18 + y_bias, -0.344)); + hand.joints[knuckle1_joint] = + pose(vec3f(mirror_x(is_left, x), 1.182 + y_bias, -0.374)); + hand.joints[knuckle2_joint] = + pose(vec3f(mirror_x(is_left, x), 1.184 + y_bias, -0.404)); + hand.joints[knuckle3_joint] = + pose(vec3f(mirror_x(is_left, x), 1.186 + y_bias, -0.434)); + } + + fn make_ready_fist_hand(is_left: bool) -> XrHand { + let mut hand = XrHand::default(); + hand.flags = XrHand::IN_VIEW | XrHand::AIM_VALID; + hand.joints[XrHand::CENTER] = pose(vec3f(mirror_x(is_left, 0.0), 1.160, -0.300)); + hand.joints[XrHand::WRIST] = pose(vec3f(mirror_x(is_left, 0.0), 1.120, -0.330)); + set_curled_finger( + &mut hand, + is_left, + XrHand::INDEX_BASE, + XrHand::INDEX_KNUCKLE1, + XrHand::INDEX_KNUCKLE2, + XrHand::INDEX_KNUCKLE3, + 0.030, + 0.000, + ); + set_curled_finger( + &mut hand, + is_left, + XrHand::MIDDLE_BASE, + XrHand::MIDDLE_KNUCKLE1, + XrHand::MIDDLE_KNUCKLE2, + XrHand::MIDDLE_KNUCKLE3, + 0.008, + 0.000, + ); + set_curled_finger( + &mut hand, + is_left, + XrHand::RING_BASE, + XrHand::RING_KNUCKLE1, + XrHand::RING_KNUCKLE2, + XrHand::RING_KNUCKLE3, + -0.015, + -0.001, + ); + set_curled_finger( + &mut hand, + is_left, + XrHand::LITTLE_BASE, + XrHand::LITTLE_KNUCKLE1, + XrHand::LITTLE_KNUCKLE2, + XrHand::LITTLE_KNUCKLE3, + -0.038, + -0.002, + ); + hand + } + + fn make_ready_open_hand(is_left: bool) -> XrHand { + let mut hand = XrHand::default(); + hand.flags = XrHand::IN_VIEW | XrHand::AIM_VALID; + hand.joints[XrHand::CENTER] = pose(vec3f(mirror_x(is_left, 0.0), 1.160, -0.300)); + hand.joints[XrHand::WRIST] = pose(vec3f(mirror_x(is_left, 0.0), 1.120, -0.330)); + set_open_finger( + &mut hand, + is_left, + XrHand::INDEX_BASE, + XrHand::INDEX_KNUCKLE1, + XrHand::INDEX_KNUCKLE2, + XrHand::INDEX_KNUCKLE3, + 0.030, + 0.000, + ); + set_open_finger( + &mut hand, + is_left, + XrHand::MIDDLE_BASE, + XrHand::MIDDLE_KNUCKLE1, + XrHand::MIDDLE_KNUCKLE2, + XrHand::MIDDLE_KNUCKLE3, + 0.008, + 0.000, + ); + set_open_finger( + &mut hand, + is_left, + XrHand::RING_BASE, + XrHand::RING_KNUCKLE1, + XrHand::RING_KNUCKLE2, + XrHand::RING_KNUCKLE3, + -0.015, + -0.001, + ); + set_open_finger( + &mut hand, + is_left, + XrHand::LITTLE_BASE, + XrHand::LITTLE_KNUCKLE1, + XrHand::LITTLE_KNUCKLE2, + XrHand::LITTLE_KNUCKLE3, + -0.038, + -0.002, + ); + hand + } + + #[test] + fn ready_fist_shape_accepts_joint_only_closed_pose_without_tip_bits() { + let hand = make_ready_fist_hand(true); + assert!(hand.is_fist()); + } + + #[test] + fn ready_fist_rejects_straightened_index_finger() { + let mut hand = make_ready_fist_hand(true); + hand.joints[XrHand::INDEX_KNUCKLE1] = pose(vec3f(mirror_x(true, 0.028), 1.176, -0.366)); + hand.joints[XrHand::INDEX_KNUCKLE2] = pose(vec3f(mirror_x(true, 0.026), 1.172, -0.390)); + hand.joints[XrHand::INDEX_KNUCKLE3] = pose(vec3f(mirror_x(true, 0.024), 1.168, -0.414)); + assert!(!hand.is_fist()); + } + + #[test] + fn finger_joint_only_helpers_survive_missing_outer_knuckle() { + let mut hand = make_ready_fist_hand(true); + let expected_index_end = hand.joints[XrHand::INDEX_KNUCKLE2].position; + hand.joints[XrHand::INDEX_KNUCKLE3] = Pose::default(); + assert_eq!( + hand.finger_end_joint_position(XrHand::INDEX_TIP), + Some(expected_index_end) + ); + assert!(hand + .finger_max_bend_angle_degrees_joint_only(XrHand::INDEX_TIP) + .is_some()); + } + + #[test] + fn ready_open_hand_accepts_low_bend_pose() { + let hand = make_ready_open_hand(true); + assert!(hand.is_open()); + assert!(hand + .average_open_finger_bend_degrees() + .is_some_and(|average| average <= XrHand::OPEN_MAX_AVERAGE_FINGER_BEND_DEGREES)); + } + + #[test] + fn ready_open_hand_rejects_curled_index_finger() { + let mut hand = make_ready_open_hand(true); + hand.joints[XrHand::INDEX_KNUCKLE1] = pose(vec3f(mirror_x(true, 0.026), 1.178, -0.360)); + hand.joints[XrHand::INDEX_KNUCKLE2] = pose(vec3f(mirror_x(true, 0.016), 1.162, -0.344)); + hand.joints[XrHand::INDEX_KNUCKLE3] = pose(vec3f(mirror_x(true, 0.004), 1.160, -0.322)); + assert!(!hand.is_open()); + } + + #[test] + fn contact_point_uses_tracking_palm_center_for_open_hand() { + let hand = make_ready_open_hand(true); + let forward = vec3f(0.0, 0.0, -1.0); + let expected_point = hand.tracking_pose().expect("tracking pose").position + + forward.scale(OPEN_HAND_SYNC_FORWARD_OFFSET_METERS); + + assert_eq!( + hand_closed_fist_contact_point(&hand, forward, true), + Some(expected_point) + ); + } + + #[test] + fn ready_fist_ignores_thumb_posture() { + let mut hand = make_ready_fist_hand(true); + set_thumbs_up_thumb(&mut hand, true); + assert!(hand.is_fist()); + } +} diff --git a/xr/src/tests/scene/xr_peer_sync_alignment.rs b/xr/src/tests/scene/xr_peer_sync_alignment.rs new file mode 100644 index 0000000000..5f5747043d --- /dev/null +++ b/xr/src/tests/scene/xr_peer_sync_alignment.rs @@ -0,0 +1,374 @@ +mod alignment_tests { + use super::*; + + fn make_solution( + yaw_radians: f32, + translation: Vec3f, + confidence: f32, + residual_meters: f32, + matched_samples: usize, + ) -> XrDepthAlignSolution { + XrDepthAlignSolution { + yaw_radians, + translation, + confidence, + symmetry_confidence: 1.0, + residual_meters, + matched_samples, + } + } + + fn make_diagnostic(solution: XrDepthAlignSolution) -> XrDepthAlignSolveDiagnostic { + XrDepthAlignSolveDiagnostic { + local_wall_samples: 8, + remote_wall_samples: 8, + best_solution: Some(solution), + ..XrDepthAlignSolveDiagnostic::default() + } + } + + fn make_peer(peer_id: u64) -> XrNetPeer { + XrNetPeer { + id: XrNetPeerId(peer_id), + addr: "127.0.0.1:41547".parse().unwrap(), + } + } + + fn make_peer_descriptor( + wall_count: usize, + has_vertical_descriptor: bool, + ) -> XrNetAlignmentDescriptorFrame { + let samples = (0..wall_count) + .map(|index| XrDepthAlignSample { + kind: XrDepthAlignSampleKind::Wall, + point: vec3(index as f32 * 0.2, 0.0, 0.0), + normal: vec3(1.0, 0.0, 0.0), + weight: 1.0, + }) + .collect(); + XrNetAlignmentDescriptorFrame { + seq: 7, + sent_at: 1.0, + descriptor: XrDepthAlignDescriptor { + samples, + vertical_descriptor: has_vertical_descriptor.then_some( + XrDepthAlignVerticalDescriptor { + origin_x: 0.0, + origin_z: 0.0, + cell_size_meters: 0.25, + size: 1, + vertical_surface_masks: vec![1], + clutter_surface_masks: vec![0], + free_space_masks: vec![0], + height_u8: vec![128], + }, + ), + ..XrDepthAlignDescriptor::default() + }, + } + } + + fn reference_dump_pair() -> XrNetAlignmentDescriptorDumpPair { + let path = std::path::PathBuf::from(env!("CARGO_MANIFEST_DIR")) + .join("dump/dumps/align-pair-226a39e4b300-r0097-1774792873191.bin"); + let bytes = std::fs::read(path).expect("reference dump should exist"); + XrNetAlignmentDescriptorDumpPair::from_file_bytes(&bytes) + .expect("reference dump should decode") + } + + #[test] + fn stable_alignment_prefers_existing_solution_over_flip() { + let previous = make_solution(0.42, vec3(0.28, 0.0, -0.64), 0.41, 0.03, 8); + let flipped = make_solution(-2.71, vec3(-0.34, 0.0, 0.71), 0.44, 0.03, 8); + + let chosen = choose_stable_alignment_solution( + Some(previous), + Some(previous), + &make_diagnostic(flipped), + ) + .unwrap(); + + assert_eq!(chosen, previous); + } + + #[test] + fn stable_alignment_accepts_small_refinement() { + let previous = make_solution(0.42, vec3(0.28, 0.0, -0.64), 0.28, 0.06, 6); + let refined = make_solution(0.46, vec3(0.24, 0.0, -0.60), 0.35, 0.03, 8); + + let chosen = choose_stable_alignment_solution( + Some(previous), + Some(previous), + &make_diagnostic(refined), + ) + .unwrap(); + + assert_eq!(chosen, refined); + } + + #[test] + fn stable_alignment_holds_stronger_solution_over_weaker_reacquisition() { + let previous = XrDepthAlignSolution { + yaw_radians: 0.38, + translation: vec3(0.18, 0.0, -0.54), + confidence: 0.74, + symmetry_confidence: 0.82, + residual_meters: 0.03, + matched_samples: 8, + }; + let weaker = XrDepthAlignSolution { + yaw_radians: 0.52, + translation: vec3(0.34, 0.0, -0.30), + confidence: 0.59, + symmetry_confidence: 0.21, + residual_meters: 0.08, + matched_samples: 2, + }; + let mut weaker_diag = make_diagnostic(weaker); + weaker_diag.local_wall_samples = 2; + weaker_diag.remote_wall_samples = 2; + + let chosen = + choose_stable_alignment_solution(Some(previous), Some(previous), &weaker_diag).unwrap(); + + assert_eq!(chosen, previous); + } + + #[test] + fn stable_alignment_switches_when_previous_pose_no_longer_scores_on_current_descriptor() { + let previous = make_solution(-0.41, vec3(0.58, 0.0, -0.44), 0.42, 0.03, 8); + let candidate = make_solution(1.18, vec3(-0.62, 0.0, 0.71), 0.39, 0.03, 8); + let stale_on_current = XrDepthAlignSolution { + yaw_radians: previous.yaw_radians, + translation: previous.translation, + confidence: 0.06, + symmetry_confidence: 0.01, + residual_meters: 0.21, + matched_samples: 1, + }; + + let chosen = choose_stable_alignment_solution( + Some(previous), + Some(stale_on_current), + &make_diagnostic(candidate), + ) + .unwrap(); + + assert_eq!(chosen, candidate); + } + + #[test] + fn stable_alignment_clears_previous_when_current_descriptor_no_longer_supports_it() { + let previous = make_solution(-0.41, vec3(0.58, 0.0, -0.44), 0.42, 0.03, 8); + let stale_on_current = XrDepthAlignSolution { + yaw_radians: previous.yaw_radians, + translation: previous.translation, + confidence: 0.05, + symmetry_confidence: 0.0, + residual_meters: 0.24, + matched_samples: 0, + }; + let rejected = XrDepthAlignSolveDiagnostic { + local_wall_samples: 4, + remote_wall_samples: 4, + local_vertical_descriptor: true, + remote_vertical_descriptor: true, + best_solution: Some(XrDepthAlignSolution { + yaw_radians: 1.18, + translation: vec3(-0.62, 0.0, 0.71), + confidence: 0.10, + symmetry_confidence: 0.03, + residual_meters: 0.18, + matched_samples: 1, + }), + ..XrDepthAlignSolveDiagnostic::default() + }; + + let chosen = + choose_stable_alignment_solution(Some(previous), Some(stale_on_current), &rejected); + + assert_eq!(chosen, None); + } + + #[test] + fn worker_queues_new_local_descriptor_without_interrupting_active_solver() { + let pair = reference_dump_pair(); + let peer = XrNetPeer { + id: pair.remote_peer_id, + addr: "127.0.0.1:41547".parse().unwrap(), + }; + let mut state = AlignmentWorkerState::default(); + let mut updated_local = pair.local_descriptor.clone(); + let updated_height = updated_local + .descriptor + .height_map + .as_mut() + .and_then(|height_map| { + height_map + .heights_meters + .iter_mut() + .find(|height| height.is_finite()) + }) + .expect("reference dump should contain a finite local height sample"); + *updated_height += 0.06; + + state.apply_local_descriptor_update(PendingLocalDescriptorUpdate::Set { + frame: pair.local_descriptor.clone(), + version: (1, 0), + }); + assert!(state.apply_peer_update( + peer.id, + PendingPeerDescriptorUpdate::Set { + peer, + frame: pair.remote_descriptor.clone(), + }, + )); + + let peer_state = state.peers.get(&peer.id).unwrap(); + assert!(peer_state.matcher.is_some()); + assert_eq!(peer_state.active_local_descriptor_version, Some((1, 0))); + assert_eq!( + peer_state.active_remote_descriptor_seq, + Some(pair.remote_descriptor.seq) + ); + assert!(!peer_state.queued_rerun); + + assert!( + state.apply_local_descriptor_update(PendingLocalDescriptorUpdate::Set { + frame: updated_local, + version: (2, 0), + }) + ); + + let peer_state = state.peers.get(&peer.id).unwrap(); + assert!(peer_state.matcher.is_some()); + assert_eq!(peer_state.active_local_descriptor_version, Some((1, 0))); + assert_eq!( + peer_state.active_remote_descriptor_seq, + Some(pair.remote_descriptor.seq) + ); + assert!(peer_state.queued_rerun); + + let mut guard = 0usize; + while state.has_pending_work() && guard < 16 { + let outcome = + state.advance_pending_alignments(Duration::ZERO, XR_ALIGNMENT_CALLBACK_MAX_STEPS); + assert!(outcome.did_work); + guard += 1; + } + + let peer_state = state.peers.get(&peer.id).unwrap(); + assert_eq!( + peer_state.last_solved_local_descriptor_version, + Some((2, 0)) + ); + assert_eq!( + peer_state.last_solved_remote_descriptor_seq, + Some(pair.remote_descriptor.seq) + ); + assert!(peer_state.matcher.is_none()); + assert!(!peer_state.queued_rerun); + } + + #[test] + fn pending_alignment_debug_reports_local_descriptor_before_peer_arrives() { + let text = make_pending_alignment_debug_text( + "AlignDbg: local slice 2 | desc occ 0 v 0 c 0", + &HashMap::new(), + ); + assert_eq!( + text, + "AlignDbg: local slice 2 | desc occ 0 v 0 c 0 | waiting for peer heightmap" + ); + } + + #[test] + fn pending_alignment_debug_reports_solve_pending_once_peer_descriptor_arrives() { + let mut peers = HashMap::new(); + let mut peer = RemotePeerState::new(make_peer(0x2a)); + peer.latest_descriptor = Some(make_peer_descriptor(2, true)); + peer.has_descriptor = true; + peers.insert(peer.peer.id, peer); + + let text = make_pending_alignment_debug_text( + "AlignDbg: local slice 2 | desc occ 0 v 0 c 0", + &peers, + ); + assert_eq!( + text, + "AlignDbg: local slice 2 | desc occ 0 v 0 c 0 | 0000002a: remote map seq 7 missing | solve pending" + ); + } + + #[test] + fn peer_scene_debug_uses_descriptor_payload_before_solver_runs() { + let mut peers = HashMap::new(); + let mut peer = RemotePeerState::new(make_peer(0x2a)); + peer.latest_descriptor = Some(make_peer_descriptor(2, true)); + peer.has_descriptor = true; + peers.insert(peer.peer.id, peer); + + let text = make_peer_scene_debug_text(true, &peers); + assert_eq!( + text, + "PeerMap 0000002a: state no | map yes seq 7 missing | pose raw | solve pending" + ); + } + + #[test] + fn peer_scene_debug_prefers_peer_with_descriptor_over_stale_waiting_peer() { + let mut peers = HashMap::new(); + peers.insert(make_peer(0x01).id, RemotePeerState::new(make_peer(0x01))); + + let mut peer = RemotePeerState::new(make_peer(0x2a)); + peer.latest_descriptor = Some(make_peer_descriptor(2, true)); + peer.has_descriptor = true; + peers.insert(peer.peer.id, peer); + + let text = make_peer_scene_debug_text(true, &peers); + assert_eq!( + text, + "PeerMap 0000002a: state no | map yes seq 7 missing | pose raw | solve pending" + ); + } + + #[test] + fn alignment_state_reports_local_remote_worker_versions() { + let mut peers = HashMap::new(); + let mut peer = RemotePeerState::new(make_peer(0x2a)); + peer.latest_descriptor = Some(make_peer_descriptor(2, true)); + peer.last_solve_ms = 1.7; + peer.last_solved_local_descriptor_version = Some((4, 9)); + peer.last_solved_remote_descriptor_seq = Some(7); + peer.last_solve_diagnostic = Some(XrDepthAlignSolveDiagnostic { + local_wall_samples: 8, + remote_wall_samples: 8, + local_vertical_descriptor: true, + remote_vertical_descriptor: true, + best_solution: Some(make_solution(0.15, vec3(0.2, 0.0, -0.1), 0.42, 0.03, 8)), + ..XrDepthAlignSolveDiagnostic::default() + }); + peers.insert(peer.peer.id, peer); + + let text = make_alignment_state_text(LocalSceneState::Ready, Some((4, 9)), &peers); + assert_eq!( + text, + "AlignState 0000002a: local map yes v4/9 | remote map yes seq 7 | worker lv4/9 rv7 accepted match 1.7ms | pose raw" + ); + } + + #[test] + fn pending_alignment_debug_keeps_worker_diagnostic_text_when_available() { + let mut peers = HashMap::new(); + let mut peer = RemotePeerState::new(make_peer(0x2a)); + peer.has_descriptor = true; + peer.last_solve_diagnostic = Some(XrDepthAlignSolveDiagnostic::default()); + peers.insert(peer.peer.id, peer); + + let text = make_pending_alignment_debug_text( + "AlignDbg: local slice 2 | desc occ 0 v 0 c 0", + &peers, + ); + assert_eq!(text, "AlignDbg: local slice 2 | desc occ 0 v 0 c 0"); + } +} diff --git a/xr/src/tests/scene/xr_peer_sync_descriptor_pair.rs b/xr/src/tests/scene/xr_peer_sync_descriptor_pair.rs new file mode 100644 index 0000000000..1ceac6b870 --- /dev/null +++ b/xr/src/tests/scene/xr_peer_sync_descriptor_pair.rs @@ -0,0 +1,30 @@ +mod descriptor_pair_tests { + use super::descriptor_pair_ready_for_solve; + + #[test] + fn initial_descriptor_pair_solves_immediately() { + assert!(descriptor_pair_ready_for_solve(None, None, (1, 0), 7)); + } + + #[test] + fn one_sided_descriptor_updates_resolve_immediately() { + assert!(descriptor_pair_ready_for_solve( + Some((1, 0)), + Some(7), + (2, 0), + 7, + )); + assert!(descriptor_pair_ready_for_solve( + Some((1, 0)), + Some(7), + (1, 0), + 8, + )); + assert!(descriptor_pair_ready_for_solve( + Some((1, 0)), + Some(7), + (2, 0), + 8, + )); + } +} diff --git a/xr/src/tests/scene/xr_peer_sync_shared_objects.rs b/xr/src/tests/scene/xr_peer_sync_shared_objects.rs new file mode 100644 index 0000000000..1ee4e13f8e --- /dev/null +++ b/xr/src/tests/scene/xr_peer_sync_shared_objects.rs @@ -0,0 +1,122 @@ +mod shared_object_shadow_tests { + use super::*; + + fn make_shared_state( + sent_at: f64, + pose_z: f32, + linvel_z: f32, + angvel_y: f32, + ) -> XrNetSharedObjectState { + XrNetSharedObjectState { + seq: 1, + sent_at, + physics_tick: 1, + object_id: xr_make_shared_object_id(XrNetPeerId(9), XrSharedObjectCounter(7)) + .expect("shared object id should pack"), + epoch: 0, + authority: XrNetPeerId(9), + fidelity: XrSharedObjectFidelity::Interpolated, + mode: XrSharedObjectMode::Dynamic, + pose: Pose::new(Quat::default(), vec3f(0.0, 0.0, pose_z)), + linvel: vec3f(0.0, 0.0, linvel_z), + angvel: vec3f(0.0, angvel_y, 0.0), + } + } + + #[test] + fn shared_object_shadow_prediction_interpolates_between_remote_history_samples() { + let previous = make_shared_state(1.00, -1.0, -4.0, 0.0); + let next = XrNetSharedObjectState { + seq: 2, + sent_at: 1.10, + pose: Pose::new( + Quat::from_axis_angle(vec3f(0.0, 1.0, 0.0), 0.2), + vec3f(0.0, 0.0, -2.0), + ), + linvel: vec3f(0.0, 0.0, -6.0), + angvel: vec3f(0.0, 0.5, 0.0), + ..previous + }; + + let (mode, pose, linvel, angvel) = XrPeerSync::predict_remote_shadow_state_from_history( + 1.05, + next, + &[previous, next], + 1.05, + ); + + assert_eq!(mode, XrSharedObjectMode::Dynamic); + assert!((pose.position.z + 1.5).abs() <= 0.001, "{pose:?}"); + assert!((linvel.z + 5.0).abs() <= 0.001, "{linvel:?}"); + assert!((angvel.y - 0.25).abs() <= 0.001, "{angvel:?}"); + } + + #[test] + fn shared_object_shadow_prediction_extrapolates_latest_sample_with_clamped_horizon() { + let latest = make_shared_state(1.00, -1.0, -10.0, 0.0); + + let (_, pose, linvel, _) = + XrPeerSync::predict_remote_shadow_state_from_history(1.30, latest, &[latest], 1.30); + + let expected_z = -1.0 + -10.0 * XrPeerSync::SHARED_OBJECT_SHADOW_MAX_EXTRAPOLATION_SECONDS; + assert!((pose.position.z - expected_z).abs() <= 0.001, "{pose:?}"); + assert_eq!(linvel.z, -10.0); + } + + #[test] + fn shared_object_shadow_prediction_uses_fallback_local_time_when_sample_time_is_zero() { + let latest = XrNetSharedObjectState { + sent_at: 0.0, + pose: Pose::new(Quat::default(), vec3f(0.0, 0.0, -1.0)), + linvel: vec3f(0.0, 0.0, -2.0), + ..make_shared_state(0.0, -1.0, -2.0, 0.0) + }; + + let (_, pose, linvel, _) = + XrPeerSync::predict_remote_shadow_state_from_history(2.00, latest, &[latest], 1.95); + + assert!((pose.position.z + 1.10).abs() <= 0.001, "{pose:?}"); + assert_eq!(linvel.z, -2.0); + } + + #[test] + fn incoming_shared_object_state_time_is_clamped_to_local_receive_time() { + let normalized = XrPeerSync::clamp_remote_shared_object_local_time(5.40, 5.10); + + assert!((normalized - 5.10).abs() <= f64::EPSILON, "{normalized:?}"); + } + + #[test] + fn incoming_shared_object_state_time_preserves_past_local_sample_time() { + let normalized = XrPeerSync::clamp_remote_shared_object_local_time(5.00, 5.10); + + assert!((normalized - 5.00).abs() <= f64::EPSILON, "{normalized:?}"); + } + + #[test] + fn shared_object_shadow_does_not_reapply_for_seq_only_advance_on_predicted_path() { + let previous = XrAppliedRemoteShadowState { + peer_id: XrNetPeerId(42), + applied_at_local_time: 10.0, + state_seq: Some(7), + mode: XrSharedObjectMode::Dynamic, + pose: Pose::new(Quat::default(), vec3f(0.0, 0.0, -1.0)), + linvel: vec3f(0.0, 0.0, -4.0), + angvel: vec3f(0.0, 0.0, 0.0), + }; + + assert!( + !XrPeerSync::should_reapply_remote_shadow_state( + &previous, + 10.04, + XrNetPeerId(42), + Some(8), + XrSharedObjectMode::Dynamic, + Pose::new(Quat::default(), vec3f(0.0, 0.0, -1.16)), + vec3f(0.0, 0.0, -4.0), + vec3f(0.0, 0.0, 0.0), + ), + "a newer authoritative shared-object seq should not force a correction when the local shadow remains on the predicted path" + ); + } +} diff --git a/xr/src/tests/scene/xr_peer_sync_touch_sync.rs b/xr/src/tests/scene/xr_peer_sync_touch_sync.rs new file mode 100644 index 0000000000..a267500489 --- /dev/null +++ b/xr/src/tests/scene/xr_peer_sync_touch_sync.rs @@ -0,0 +1,693 @@ +mod touch_sync_tests { + use super::*; + + fn make_peer(peer_id: u64) -> XrNetPeer { + XrNetPeer { + id: XrNetPeerId(peer_id), + addr: "127.0.0.1:41547".parse().unwrap(), + } + } + + fn make_state_frame(state: XrState) -> XrNetStateFrame { + XrNetStateFrame { + seq: 1, + sent_at: state.time, + state, + } + } + + fn make_sync_anchor( + id: u32, + captured_at: f64, + extrema: XrSyncAnchorExtrema, + anchor: XrAnchor, + ) -> XrSyncAnchor { + XrSyncAnchor { + id, + captured_at, + extrema, + anchor, + } + } + + fn make_local_sync_state( + now: f64, + anchor: Option, + sync_anchor: Option, + fist_hold_anchor: Option, + ) -> XrPeerSyncLocalState { + let mut local = XrPeerSyncLocalState { + state_time: now, + anchor, + sync_anchor, + fist_hold_anchor, + ..XrPeerSyncLocalState::default() + }; + if let Some(sync_anchor) = sync_anchor { + local.record_sync_anchor(sync_anchor); + } + local + } + + fn pose(position: Vec3f) -> Pose { + Pose::new(Quat::default(), position) + } + + fn make_tracking_hand(position: Vec3f) -> XrHand { + let mut hand = XrHand::default(); + hand.flags = XrHand::IN_VIEW | XrHand::AIM_VALID; + hand.joints[XrHand::CENTER] = pose(position); + hand.joints[XrHand::WRIST] = pose(position + vec3f(0.0, -0.040, 0.030)); + hand + } + + fn make_remote_touch_anchor(local_anchor: XrAnchor, remote_to_local: Mat4f) -> XrAnchor { + let local_to_remote = remote_to_local.invert(); + XrAnchor { + left: XrPeerSync::transform_point(&local_to_remote, local_anchor.right), + right: XrPeerSync::transform_point(&local_to_remote, local_anchor.left), + } + } + + fn assert_touch_anchor_mapping_matches( + solved: Mat4f, + remote_anchor: XrAnchor, + local_anchor: XrAnchor, + ) { + let mirrored_remote = remote_anchor.mirrored(); + let solved_left = XrPeerSync::transform_point(&solved, mirrored_remote.left); + let solved_right = XrPeerSync::transform_point(&solved, mirrored_remote.right); + assert!( + (solved_left - local_anchor.left).length() <= 0.001, + "{solved_left:?}" + ); + assert!( + (solved_right - local_anchor.right).length() <= 0.001, + "{solved_right:?}" + ); + } + + fn assert_anchor_close(left: XrAnchor, right: XrAnchor) { + assert!( + (left.left - right.left).length() <= 1.0e-5, + "{left:?} != {right:?}" + ); + assert!( + (left.right - right.right).length() <= 1.0e-5, + "{left:?} != {right:?}" + ); + } + + fn far_relocated_anchor_override(saved_anchor: XrAnchor, motion_center: Vec3f) -> XrAnchor { + let left_distance = (motion_center - saved_anchor.left).length(); + let right_distance = (motion_center - saved_anchor.right).length(); + if left_distance <= right_distance { + XrAnchor { + left: motion_center, + right: saved_anchor.right, + } + } else { + XrAnchor { + left: saved_anchor.left, + right: motion_center, + } + } + } + + #[test] + fn local_sync_anchor_activity_uses_seen_time_not_capture_time() { + let sync_anchor = make_sync_anchor( + 7, + 10.0, + XrSyncAnchorExtrema::High, + XrAnchor { + left: vec3f(-0.12, 1.1, -0.4), + right: vec3f(0.14, 1.1, -0.4), + }, + ); + let mut local = make_local_sync_state(11.2, None, Some(sync_anchor), None); + + assert_eq!(local.active_sync_anchor(), Some(sync_anchor)); + + local.state_time = 12.7; + assert_eq!(local.active_sync_anchor(), None); + } + + #[test] + fn far_relocated_sync_samples_replace_closest_saved_marker() { + let saved_local_anchor = XrAnchor { + left: vec3f(-0.25, 1.05, -0.30), + right: vec3f(0.22, 1.05, -0.30), + }; + let first_far_anchor = XrAnchor { + left: vec3f(-0.18, 1.18, -1.64), + right: vec3f(0.16, 1.17, -1.60), + }; + let second_far_anchor = XrAnchor { + left: vec3f(-0.20, 1.20, -1.52), + right: vec3f(0.18, 1.19, -1.48), + }; + let mut local = make_local_sync_state(10.0, Some(saved_local_anchor), None, None); + + let first = local.record_matched_sync_anchor(first_far_anchor, 10.0); + let first_center = (first_far_anchor.left + first_far_anchor.right) * 0.5; + assert_anchor_close( + first, + far_relocated_anchor_override(saved_local_anchor, first_center), + ); + + let second = local.record_matched_sync_anchor(second_far_anchor, 10.1); + let second_center = (first_far_anchor.left + + first_far_anchor.right + + second_far_anchor.left + + second_far_anchor.right) + * 0.25; + assert_anchor_close( + second, + far_relocated_anchor_override(saved_local_anchor, second_center), + ); + assert_eq!(local.matched_sync_sample_count(), 2); + } + + #[test] + fn far_relocated_sync_samples_can_replace_left_marker_when_it_is_closest() { + let saved_local_anchor = XrAnchor { + left: vec3f(-0.42, 1.02, -0.32), + right: vec3f(0.34, 1.01, -0.28), + }; + let far_anchor = XrAnchor { + left: vec3f(-0.64, 1.19, -1.68), + right: vec3f(-0.26, 1.18, -1.60), + }; + let mut local = make_local_sync_state(10.0, Some(saved_local_anchor), None, None); + + let matched = local.record_matched_sync_anchor(far_anchor, 10.0); + let far_center = (far_anchor.left + far_anchor.right) * 0.5; + + assert_anchor_close( + matched, + XrAnchor { + left: far_center, + right: saved_local_anchor.right, + }, + ); + assert_eq!(local.matched_sync_sample_count(), 1); + } + + #[test] + fn mirrored_remote_touch_anchor_maps_back_to_local_touch_order() { + let local_anchor = XrAnchor { + left: vec3f(-0.18, 1.12, -0.44), + right: vec3f(0.16, 1.11, -0.40), + }; + let remote_to_local = Pose::new( + Quat::from_axis_angle(vec3f(0.0, 1.0, 0.0), 0.63), + vec3f(-0.31, 0.04, 0.27), + ) + .to_mat4(); + let local_to_remote = remote_to_local.invert(); + let remote_anchor = XrAnchor { + left: XrPeerSync::transform_point(&local_to_remote, local_anchor.right), + right: XrPeerSync::transform_point(&local_to_remote, local_anchor.left), + }; + let mirrored_remote = remote_anchor.mirrored(); + let solved = mirrored_remote.mapping_to(&local_anchor); + + let solved_left = XrPeerSync::transform_point(&solved, mirrored_remote.left); + let solved_right = XrPeerSync::transform_point(&solved, mirrored_remote.right); + + assert!( + (solved_left - local_anchor.left).length() <= 0.001, + "{solved_left:?}" + ); + assert!( + (solved_right - local_anchor.right).length() <= 0.001, + "{solved_right:?}" + ); + assert!( + (XrPeerSync::transform_point(&solved, remote_anchor.left) - local_anchor.right) + .length() + <= 0.001 + ); + } + + #[test] + fn arm_corridor_allows_opposing_vertical_wiggle_within_angle_budget() { + let head_pose = Pose::new(Quat::default(), vec3f(0.0, 1.60, 0.0)); + let left_hand = make_tracking_hand(vec3f(-0.18, 1.40, -0.40)); + let right_hand = make_tracking_hand(vec3f(0.18, 1.75, -0.40)); + + assert!(XrPeerSync::arm_corridor_ready( + head_pose, + &left_hand, + &right_hand + )); + } + + #[test] + fn descriptor_alignment_uses_persistent_anchor_height_override() { + let mut registry = XrPeerRegistry::default(); + let peer = make_peer(0x2a); + let local_anchor = XrAnchor { + left: vec3f(-0.18, 1.31, -0.41), + right: vec3f(0.16, 1.29, -0.39), + }; + let anchor_remote_to_local = Pose::new( + Quat::from_axis_angle(vec3f(0.0, 1.0, 0.0), 0.21), + vec3f(-0.14, 0.28, 0.09), + ) + .to_mat4(); + let remote_anchor = make_remote_touch_anchor(local_anchor, anchor_remote_to_local); + let descriptor_remote_to_local = Pose::new( + Quat::from_axis_angle(vec3f(0.0, 1.0, 0.0), -0.63), + vec3f(0.48, -0.41, -0.26), + ) + .to_mat4(); + + registry.track_state( + peer, + make_state_frame(XrState { + time: 1.0, + anchor: Some(remote_anchor), + ..XrState::default() + }), + 10.0, + ); + registry + .peers + .get_mut(&peer.id) + .expect("peer state should exist") + .descriptor_remote_to_local = Some(descriptor_remote_to_local); + + let mut cx = Cx::new(Box::new(|_, _| {})); + let mut local = make_local_sync_state(10.1, Some(local_anchor), None, None); + let mut recent_anchor_confirmation = None; + let changed = + registry.refresh_transforms(&mut cx, &mut local, &mut recent_anchor_confirmation, 10.1); + + assert!(changed); + let peer_state = registry + .peers + .get(&peer.id) + .expect("peer state should exist"); + let solved_anchor = peer_state + .anchor_remote_to_local + .expect("persistent anchors should resolve"); + assert_touch_anchor_mapping_matches(solved_anchor, remote_anchor, local_anchor); + assert_eq!(local.anchor_override, None); + let mut expected = descriptor_remote_to_local; + expected.v[13] = solved_anchor.v[13]; + assert_eq!(peer_state.remote_to_local, Some(expected)); + assert_eq!( + peer_state.transform_source, + RemoteTransformSource::Descriptor + ); + } + + #[test] + fn descriptor_alignment_stays_unchanged_without_persistent_anchors() { + let mut registry = XrPeerRegistry::default(); + let peer = make_peer(0x2a); + let descriptor_remote_to_local = Pose::new( + Quat::from_axis_angle(vec3f(0.0, 1.0, 0.0), 0.47), + vec3f(-0.36, 0.18, 0.22), + ) + .to_mat4(); + + registry.track_state( + peer, + make_state_frame(XrState { + time: 1.0, + ..XrState::default() + }), + 10.0, + ); + registry + .peers + .get_mut(&peer.id) + .expect("peer state should exist") + .descriptor_remote_to_local = Some(descriptor_remote_to_local); + + let mut cx = Cx::new(Box::new(|_, _| {})); + let mut local = make_local_sync_state(10.1, None, None, None); + let mut recent_anchor_confirmation = None; + let changed = + registry.refresh_transforms(&mut cx, &mut local, &mut recent_anchor_confirmation, 10.1); + + assert!(changed); + let peer_state = registry + .peers + .get(&peer.id) + .expect("peer state should exist"); + assert_eq!(peer_state.anchor_remote_to_local, None); + assert_eq!(peer_state.remote_to_local, Some(descriptor_remote_to_local)); + assert_eq!( + peer_state.transform_source, + RemoteTransformSource::Descriptor + ); + } + + #[test] + fn matched_box_sync_sample_updates_local_anchor_average_immediately() { + let mut registry = XrPeerRegistry::default(); + let peer = make_peer(0x2a); + let saved_local_anchor = XrAnchor { + left: vec3f(-0.25, 1.05, -0.30), + right: vec3f(0.22, 1.05, -0.30), + }; + let fresh_local_anchor = XrAnchor { + left: vec3f(-0.17, 1.16, -0.56), + right: vec3f(0.15, 1.15, -0.52), + }; + let saved_remote_to_local = Pose::new( + Quat::from_axis_angle(vec3f(0.0, 1.0, 0.0), -0.31), + vec3f(0.42, 0.02, -0.18), + ) + .to_mat4(); + let fresh_remote_to_local = Pose::new( + Quat::from_axis_angle(vec3f(0.0, 1.0, 0.0), 0.58), + vec3f(-0.24, -0.01, 0.33), + ) + .to_mat4(); + let saved_remote_anchor = + make_remote_touch_anchor(saved_local_anchor, saved_remote_to_local); + let fresh_remote_anchor = + make_remote_touch_anchor(fresh_local_anchor, fresh_remote_to_local); + + registry.track_state( + peer, + make_state_frame(XrState { + time: 1.0, + anchor: Some(saved_remote_anchor), + sync_anchor: Some(make_sync_anchor( + 41, + 1.0, + XrSyncAnchorExtrema::Low, + fresh_remote_anchor, + )), + ..XrState::default() + }), + 10.0, + ); + { + let peer_state = registry + .peers + .get_mut(&peer.id) + .expect("peer state should exist"); + peer_state.last_fist_hold_anchor = Some(fresh_remote_anchor); + peer_state.last_fist_hold_seen_at = Some(10.0); + } + + let mut cx = Cx::new(Box::new(|_, _| {})); + let local_sync = make_sync_anchor(7, 10.02, XrSyncAnchorExtrema::High, fresh_local_anchor); + let mut local = make_local_sync_state( + 10.1, + Some(saved_local_anchor), + Some(local_sync), + Some(fresh_local_anchor), + ); + let mut recent_anchor_confirmation = None; + let changed = + registry.refresh_transforms(&mut cx, &mut local, &mut recent_anchor_confirmation, 10.1); + + assert!(changed); + let peer_state = registry + .peers + .get(&peer.id) + .expect("peer state should exist"); + let solved = peer_state + .anchor_remote_to_local + .expect("live override should update the active anchor mapping immediately"); + assert_eq!( + solved, + saved_remote_anchor + .mirrored() + .mapping_to(&fresh_local_anchor) + ); + assert_anchor_close( + local + .anchor_override + .expect("anchor override should be present"), + fresh_local_anchor, + ); + assert_eq!(local.matched_sync_sample_count(), 1); + assert_anchor_close( + recent_anchor_confirmation + .map(|confirmation| confirmation.anchor) + .expect("confirmation anchor should be present"), + fresh_local_anchor, + ); + } + + #[test] + fn matched_box_sync_far_from_saved_anchor_replaces_closest_saved_marker() { + let mut registry = XrPeerRegistry::default(); + let peer = make_peer(0x2a); + let saved_local_anchor = XrAnchor { + left: vec3f(-0.42, 1.05, -0.30), + right: vec3f(0.34, 1.05, -0.30), + }; + let far_local_anchor = XrAnchor { + left: vec3f(-0.64, 1.18, -1.64), + right: vec3f(-0.28, 1.17, -1.60), + }; + let expected_override = far_relocated_anchor_override( + saved_local_anchor, + (far_local_anchor.left + far_local_anchor.right) * 0.5, + ); + let saved_remote_to_local = Pose::new( + Quat::from_axis_angle(vec3f(0.0, 1.0, 0.0), -0.31), + vec3f(0.42, 0.02, -0.18), + ) + .to_mat4(); + let far_remote_to_local = Pose::new( + Quat::from_axis_angle(vec3f(0.0, 1.0, 0.0), 0.58), + vec3f(-0.24, -0.01, 0.33), + ) + .to_mat4(); + let saved_remote_anchor = + make_remote_touch_anchor(saved_local_anchor, saved_remote_to_local); + let far_remote_anchor = make_remote_touch_anchor(far_local_anchor, far_remote_to_local); + + registry.track_state( + peer, + make_state_frame(XrState { + time: 1.0, + anchor: Some(saved_remote_anchor), + sync_anchor: Some(make_sync_anchor( + 41, + 1.0, + XrSyncAnchorExtrema::Low, + far_remote_anchor, + )), + ..XrState::default() + }), + 10.0, + ); + { + let peer_state = registry + .peers + .get_mut(&peer.id) + .expect("peer state should exist"); + peer_state.last_fist_hold_anchor = Some(far_remote_anchor); + peer_state.last_fist_hold_seen_at = Some(10.0); + } + + let mut cx = Cx::new(Box::new(|_, _| {})); + let local_sync = make_sync_anchor(7, 10.02, XrSyncAnchorExtrema::High, far_local_anchor); + let mut local = make_local_sync_state( + 10.1, + Some(saved_local_anchor), + Some(local_sync), + Some(far_local_anchor), + ); + let mut recent_anchor_confirmation = None; + let changed = + registry.refresh_transforms(&mut cx, &mut local, &mut recent_anchor_confirmation, 10.1); + + assert!(changed); + let peer_state = registry + .peers + .get(&peer.id) + .expect("peer state should exist"); + let solved = peer_state + .anchor_remote_to_local + .expect("live override should update the active anchor mapping immediately"); + assert_eq!( + solved, + saved_remote_anchor + .mirrored() + .mapping_to(&expected_override) + ); + assert_anchor_close( + local + .anchor_override + .expect("anchor override should be present"), + expected_override, + ); + assert_eq!(local.matched_sync_sample_count(), 1); + assert_anchor_close( + recent_anchor_confirmation + .map(|confirmation| confirmation.anchor) + .expect("confirmation anchor should be present"), + expected_override, + ); + } + + #[test] + fn box_sync_requires_both_sides_to_remain_in_box_pose() { + let mut registry = XrPeerRegistry::default(); + let peer = make_peer(0x2a); + let local_sync_anchor = XrAnchor { + left: vec3f(-0.16, 1.14, -0.58), + right: vec3f(0.14, 1.13, -0.55), + }; + let remote_sync_anchor = XrAnchor { + left: vec3f(0.12, 1.14, -0.54), + right: vec3f(-0.11, 1.13, -0.53), + }; + + registry.track_state( + peer, + make_state_frame(XrState { + time: 1.0, + sync_anchor: Some(make_sync_anchor( + 99, + 1.0, + XrSyncAnchorExtrema::Low, + remote_sync_anchor, + )), + ..XrState::default() + }), + 10.0, + ); + { + let peer_state = registry + .peers + .get_mut(&peer.id) + .expect("peer state should exist"); + peer_state.last_fist_hold_anchor = Some(remote_sync_anchor); + peer_state.last_fist_hold_seen_at = Some(10.0); + } + + let mut cx = Cx::new(Box::new(|_, _| {})); + let mut local = make_local_sync_state( + 10.1, + None, + Some(make_sync_anchor( + 7, + 10.02, + XrSyncAnchorExtrema::Low, + local_sync_anchor, + )), + None, + ); + let mut recent_anchor_confirmation = None; + let changed = + registry.refresh_transforms(&mut cx, &mut local, &mut recent_anchor_confirmation, 10.1); + + assert!(!changed); + assert_eq!(local.anchor_override, None); + assert_eq!(recent_anchor_confirmation, None); + assert_eq!(local.matched_sync_sample_count(), 0); + } + + #[test] + fn box_sync_without_backend_anchor_updates_local_override_but_not_transform() { + let mut registry = XrPeerRegistry::default(); + let peer = make_peer(0x2a); + let fresh_local_anchor = XrAnchor { + left: vec3f(-0.17, 1.16, -0.56), + right: vec3f(0.15, 1.15, -0.52), + }; + let fresh_remote_anchor = XrAnchor { + left: vec3f(0.10, 1.12, -0.48), + right: vec3f(-0.12, 1.12, -0.50), + }; + + registry.track_state( + peer, + make_state_frame(XrState { + time: 1.0, + sync_anchor: Some(make_sync_anchor( + 41, + 1.0, + XrSyncAnchorExtrema::High, + fresh_remote_anchor, + )), + ..XrState::default() + }), + 10.0, + ); + { + let peer_state = registry + .peers + .get_mut(&peer.id) + .expect("peer state should exist"); + peer_state.last_fist_hold_anchor = Some(fresh_remote_anchor); + peer_state.last_fist_hold_seen_at = Some(10.0); + } + + let mut cx = Cx::new(Box::new(|_, _| {})); + let mut local = make_local_sync_state( + 10.1, + None, + Some(make_sync_anchor( + 7, + 10.03, + XrSyncAnchorExtrema::Low, + fresh_local_anchor, + )), + Some(fresh_local_anchor), + ); + let mut recent_anchor_confirmation = None; + let changed = + registry.refresh_transforms(&mut cx, &mut local, &mut recent_anchor_confirmation, 10.1); + + assert!(changed); + let peer_state = registry + .peers + .get(&peer.id) + .expect("peer state should exist"); + assert_eq!(peer_state.anchor_remote_to_local, None); + assert_eq!(peer_state.remote_to_local, None); + assert_anchor_close( + local + .anchor_override + .expect("anchor override should be present"), + fresh_local_anchor, + ); + assert_anchor_close( + recent_anchor_confirmation + .map(|confirmation| confirmation.anchor) + .expect("confirmation anchor should be present"), + fresh_local_anchor, + ); + } + + #[test] + fn recent_remote_sync_anchor_requires_recent_receive_time() { + let sync_anchor = make_sync_anchor( + 42, + 1.25, + XrSyncAnchorExtrema::High, + XrAnchor { + left: vec3f(-0.1, 1.0, -0.3), + right: vec3f(0.1, 1.0, -0.3), + }, + ); + let mut peer = RemotePeerState::new(make_peer(0x2a)); + peer.last_state_received_at = 10.0; + peer.recent_sync_anchors.push_back(TimedRemoteSyncAnchor { + sync: sync_anchor, + first_seen_at_local_time: 10.0, + last_seen_at_local_time: 10.0, + }); + + assert_eq!( + XrPeerSync::recent_remote_sync_anchor(&peer, 10.2), + Some(sync_anchor) + ); + assert_eq!(XrPeerSync::recent_remote_sync_anchor(&peer, 10.6), None); + } +} diff --git a/xr/src/tests/scene/xr_physics.rs b/xr/src/tests/scene/xr_physics.rs new file mode 100644 index 0000000000..bf2f2242bb --- /dev/null +++ b/xr/src/tests/scene/xr_physics.rs @@ -0,0 +1,657 @@ +mod tests { + use super::*; + use crate::algorithms::tsdf_query::{DepthQueryCollider, DepthQuerySupportPlane}; + + fn assert_vec3_close(actual: Vec3f, expected: Vec3f, tolerance: f32) { + assert!( + (actual - expected).length() <= tolerance, + "expected {:?} to be within {} of {:?}", + actual, + tolerance, + expected + ); + } + + #[test] + fn impact_surface_enables_before_current_body_overlaps_quad() { + let plane = DepthQuerySupportPlane { + point: vec3f(1.0, 0.0, 0.0), + normal: vec3f(-1.0, 0.0, 0.0), + tangent: vec3f(0.0, 1.0, 0.0), + bitangent: vec3f(0.0, 0.0, 1.0), + half_extent_tangent: 0.08, + half_extent_bitangent: 0.08, + }; + let target = DepthQuerySurfaceTarget { + collider: DepthQueryCollider { + fingerprint: 1, + geometry: DepthQueryColliderGeometry::HalfSpace(plane), + role: DepthQueryColliderRole::Impact, + restitution: 0.38, + }, + }; + + assert!(depth_query_surface_target_should_enable( + target, + vec3f(0.78, 0.0, 0.0), + vec3f(0.55, 0.0, 0.0), + 0.05, + 0.004, + )); + } + + #[test] + fn respawn_body_applies_shadow_contact_dominated_and_sleeping_modes() { + let mut scene = RapierScene::new(0.0); + let widget_uid = WidgetUid(41); + let pose = Pose::new(Quat::default(), vec3f(0.08, 1.12, -0.44)); + scene.spawn_dynamic_box( + widget_uid, + pose, + vec3f(0.04, 0.04, 0.04), + vec3f(1.0, 1.0, 1.0), + 1.0, + 0.5, + 0.0, + ); + let cube = scene.cubes[0]; + + scene.respawn_body( + widget_uid, + true, + XrSharedObjectMode::Dynamic, + pose, + vec3f(1.0, 2.0, 3.0), + vec3f(4.0, 5.0, 6.0), + ); + let body = scene + .bodies + .get(cube.body) + .expect("spawned cube body should exist"); + assert_eq!(body.body_type(), RigidBodyType::KinematicPositionBased); + assert_eq!( + scene.shadow_body_motion_for_body(cube.body), + Some((vec3f(1.0, 2.0, 3.0), vec3f(4.0, 5.0, 6.0))) + ); + + scene.respawn_body( + widget_uid, + false, + XrSharedObjectMode::ContactDominated { + authority: XrNetPeerId(7), + hand: XrSharedHand::RightHand, + }, + pose, + vec3f(0.5, 0.0, -0.5), + vec3f(0.0, 1.0, 0.0), + ); + let body = scene + .bodies + .get(cube.body) + .expect("cube body should still exist after contact-dominated respawn"); + assert_eq!(body.body_type(), RigidBodyType::KinematicPositionBased); + assert!(!body.is_sleeping()); + + scene.respawn_body( + widget_uid, + false, + XrSharedObjectMode::Sleeping, + pose, + vec3f(0.0, 0.0, 0.0), + vec3f(0.0, 0.0, 0.0), + ); + let body = scene + .bodies + .get(cube.body) + .expect("cube body should still exist after sleeping respawn"); + assert_eq!(body.body_type(), RigidBodyType::Dynamic); + assert_vec3_close( + vec3f(body.linvel().x, body.linvel().y, body.linvel().z), + vec3f(0.0, 0.0, 0.0), + 0.0001, + ); + assert_vec3_close( + vec3f(body.angvel().x, body.angvel().y, body.angvel().z), + vec3f(0.0, 0.0, 0.0), + 0.0001, + ); + } + + #[test] + fn shadow_respawn_keeps_projecting_motion_between_corrections() { + let mut scene = RapierScene::new(0.0); + let widget_uid = WidgetUid(410); + let pose = Pose::new(Quat::default(), vec3f(0.0, 1.0, -0.2)); + scene.spawn_dynamic_box( + widget_uid, + pose, + vec3f(0.05, 0.05, 0.05), + vec3f(1.0, 1.0, 1.0), + 1.0, + 0.5, + 0.0, + ); + let cube = scene.cubes[0]; + + scene.respawn_body( + widget_uid, + true, + XrSharedObjectMode::Dynamic, + pose, + vec3f(0.0, 0.0, -1.5), + vec3f(0.0, 0.0, 0.0), + ); + scene.step(); + + let body = scene + .bodies + .get(cube.body) + .expect("shadow body should exist after a step"); + let stepped_pose = makepad_pose(body.position()); + assert_eq!(body.body_type(), RigidBodyType::KinematicPositionBased); + assert!( + stepped_pose.position.z < pose.position.z - 0.0001, + "shadow body should keep moving forward between network corrections: {stepped_pose:?}" + ); + assert_eq!( + scene + .shadow_body_motion_for_body(cube.body) + .map(|(linvel, _)| linvel) + .unwrap_or_default(), + vec3f(0.0, 0.0, -1.5) + ); + } + + #[test] + fn shadow_dynamic_body_ignores_local_wall_until_extrapolation_runs_out() { + let mut scene = RapierScene::new(0.0); + let projectile_uid = WidgetUid(411); + let wall_uid = WidgetUid(412); + let pose = Pose::new(Quat::default(), vec3f(0.0, 1.0, -0.2)); + scene.spawn_dynamic_sphere( + projectile_uid, + pose, + 0.05, + vec3f(1.0, 1.0, 1.0), + 1.0, + 0.0, + 0.95, + ); + scene.spawn_fixed_box( + wall_uid, + Pose::new(Quat::default(), vec3f(0.0, 1.0, -0.55)), + vec3f(1.0, 1.0, 0.05), + vec3f(1.0, 1.0, 1.0), + 0.0, + 0.95, + ); + let cube = scene.cubes[0]; + + scene.respawn_body( + projectile_uid, + true, + XrSharedObjectMode::Dynamic, + pose, + vec3f(0.0, 0.0, -2.0), + vec3f(0.0, 0.0, 0.0), + ); + for _ in 0..30 { + scene.step(); + } + + let body = scene + .bodies + .get(cube.body) + .expect("shadow projectile should still exist after stepping"); + assert!( + makepad_pose(body.position()).position.z <= -0.39, + "shadow projectile should keep dead-reckoning forward instead of locally bouncing on observer-only walls: {:?}", + makepad_pose(body.position()).position + ); + assert!( + makepad_pose(body.position()).position.z >= -0.45, + "shadow projectile should stop near the extrapolation horizon when no fresh authority samples arrive: {:?}", + makepad_pose(body.position()).position + ); + assert_eq!(body.body_type(), RigidBodyType::KinematicPositionBased); + } + + #[test] + fn apply_impulse_only_affects_dynamic_enabled_bodies() { + let mut scene = RapierScene::new(0.0); + let widget_uid = WidgetUid(42); + let pose = Pose::new(Quat::default(), vec3f(0.0, 1.0, 0.0)); + scene.spawn_dynamic_box( + widget_uid, + pose, + vec3f(0.05, 0.05, 0.05), + vec3f(1.0, 1.0, 1.0), + 1.0, + 0.5, + 0.0, + ); + let cube = scene.cubes[0]; + + assert!(scene.apply_impulse(widget_uid, pose.position, vec3f(0.0, 0.0, -1.0),)); + let body = scene + .bodies + .get(cube.body) + .expect("cube body should exist after impulse"); + assert!(body.linvel().z < 0.0); + + scene.respawn_body( + widget_uid, + true, + XrSharedObjectMode::Dynamic, + pose, + vec3f(0.0, 0.0, 0.0), + vec3f(0.0, 0.0, 0.0), + ); + assert!( + !scene.apply_impulse(widget_uid, pose.position, vec3f(0.0, 0.0, -1.0)), + "shadow bodies should reject dynamic impulses" + ); + + scene.despawn_body(widget_uid); + assert!( + !scene.apply_impulse(widget_uid, pose.position, vec3f(0.0, 0.0, -1.0)), + "disabled bodies should reject impulses" + ); + } + + #[test] + fn spawn_pool_respawn_reenables_body_and_survives_a_step() { + let mut scene = RapierScene::new(0.0); + let widget_uid = WidgetUid(420); + let pose = Pose::new(Quat::default(), vec3f(0.0, 1.0, -0.4)); + scene.spawn_dynamic_sphere(widget_uid, pose, 0.04, vec3f(1.0, 1.0, 1.0), 1.0, 0.5, 0.0); + scene.register_spawn_pool_cube(0); + + let cube = scene.cubes[0]; + let body = scene + .bodies + .get(cube.body) + .expect("projectile body should exist before respawn"); + assert!(!body.is_enabled(), "projectile pool bodies start disabled"); + + scene.respawn_body( + widget_uid, + false, + XrSharedObjectMode::Dynamic, + pose, + vec3f(0.0, 0.0, -8.0), + vec3f(0.0, 0.0, 0.0), + ); + + let body = scene + .bodies + .get(cube.body) + .expect("projectile body should still exist after respawn"); + assert!(body.is_enabled(), "respawn should re-enable the pooled body"); + assert_eq!(body.body_type(), RigidBodyType::Dynamic); + + scene.step(); + + let body = scene + .bodies + .get(cube.body) + .expect("projectile body should still exist after a step"); + assert!( + body.is_enabled(), + "a respawned pooled body should remain enabled after stepping" + ); + } + + #[test] + fn hand_contact_grab_and_release_restores_dynamic_body_velocity() { + let mut scene = RapierScene::new(0.0); + let widget_uid = WidgetUid(43); + let pose = Pose::new(Quat::default(), vec3f(0.0, 1.0, 0.0)); + scene.spawn_dynamic_box( + widget_uid, + pose, + vec3f(0.05, 0.05, 0.05), + vec3f(1.0, 1.0, 1.0), + 1.0, + 0.5, + 0.0, + ); + let cube = scene.cubes[0]; + let hand_pose = Pose::new(Quat::default(), pose.position); + + RapierScene::sync_hand_bodies( + &scene.left_hand, + &[HandCollider::Ball { + center: pose.position, + radius: 0.09, + }], + &mut scene.bodies, + &mut scene.colliders, + ); + scene.left_hand_grab = HandGrabState { + shared_hand: XrSharedHand::LeftHand, + pose: hand_pose, + previous_pose: hand_pose, + linvel: vec3f(0.0, 0.0, 0.0), + tracked: true, + gripping: true, + held_body: None, + grab_offset: Pose::default(), + }; + + scene.step(); + assert_eq!(scene.left_hand_grab.held_body, Some(cube.body)); + let body = scene + .bodies + .get(cube.body) + .expect("cube body should exist after grab"); + assert_eq!(body.body_type(), RigidBodyType::KinematicPositionBased); + assert_eq!( + scene.held_by_for_body(cube.body), + Some(XrSharedHand::LeftHand) + ); + + let release_velocity = vec3f(0.6, 0.0, -0.4); + scene.left_hand_grab.linvel = release_velocity; + scene.left_hand_grab.gripping = false; + scene.apply_held_body_targets(); + + assert_eq!(scene.left_hand_grab.held_body, None); + let body = scene + .bodies + .get(cube.body) + .expect("cube body should still exist after release"); + assert_eq!(body.body_type(), RigidBodyType::Dynamic); + assert_vec3_close( + vec3f(body.linvel().x, body.linvel().y, body.linvel().z), + release_velocity, + 0.0001, + ); + } + + #[test] + fn hand_grab_anchors_body_surface_to_hand_pose_instead_of_body_center() { + let mut scene = RapierScene::new(0.0); + let widget_uid = WidgetUid(430); + let pose = Pose::new(Quat::default(), vec3f(0.0, 1.0, 0.0)); + let half_extents = vec3f(0.05, 0.05, 0.05); + scene.spawn_dynamic_box( + widget_uid, + pose, + half_extents, + vec3f(1.0, 1.0, 1.0), + 1.0, + 0.5, + 0.0, + ); + let cube = scene.cubes[0]; + let acquire_pose = Pose::new(Quat::default(), pose.position + vec3f(0.0, 0.0, 0.11)); + let moved_pose = Pose::new(Quat::default(), pose.position + vec3f(0.16, 0.0, 0.11)); + + RapierScene::sync_hand_bodies( + &scene.left_hand, + &[HandCollider::Ball { + center: acquire_pose.position, + radius: 0.09, + }], + &mut scene.bodies, + &mut scene.colliders, + ); + scene.left_hand_grab = HandGrabState { + shared_hand: XrSharedHand::LeftHand, + pose: acquire_pose, + previous_pose: acquire_pose, + linvel: vec3f(0.0, 0.0, 0.0), + tracked: true, + gripping: true, + held_body: None, + grab_offset: Pose::default(), + }; + + scene.step(); + assert_eq!(scene.left_hand_grab.held_body, Some(cube.body)); + + scene.left_hand_grab.previous_pose = scene.left_hand_grab.pose; + scene.left_hand_grab.pose = moved_pose; + scene.apply_held_body_targets(); + scene.step(); + + let body = scene + .bodies + .get(cube.body) + .expect("cube body should exist after moving the held hand"); + let body_pose = makepad_pose(body.position()); + let center_distance = (body_pose.position - moved_pose.position).length(); + assert!( + (center_distance - half_extents.z).abs() <= 0.012, + "center_distance={center_distance:?} body_pose={body_pose:?} moved_pose={moved_pose:?}" + ); + + let local_anchor = scene.left_hand_grab.grab_offset.invert().position; + assert!( + (local_anchor.z - half_extents.z).abs() <= 0.012, + "local_anchor={local_anchor:?} half_extents={half_extents:?}" + ); + } + + #[test] + fn secondary_hand_can_join_existing_hold_and_keep_body_kinematic() { + let mut scene = RapierScene::new(0.0); + let widget_uid = WidgetUid(44); + let pose = Pose::new(Quat::default(), vec3f(0.0, 1.0, 0.0)); + scene.spawn_dynamic_box( + widget_uid, + pose, + vec3f(0.05, 0.05, 0.05), + vec3f(1.0, 1.0, 1.0), + 1.0, + 0.5, + 0.0, + ); + let cube = scene.cubes[0]; + let left_pose = Pose::new(Quat::default(), pose.position); + let right_pose = Pose::new(Quat::default(), pose.position + vec3f(0.08, 0.0, 0.0)); + + RapierScene::sync_hand_bodies( + &scene.left_hand, + &[HandCollider::Ball { + center: pose.position, + radius: 0.09, + }], + &mut scene.bodies, + &mut scene.colliders, + ); + scene.left_hand_grab = HandGrabState { + shared_hand: XrSharedHand::LeftHand, + pose: left_pose, + previous_pose: left_pose, + linvel: vec3f(0.0, 0.0, 0.0), + tracked: true, + gripping: true, + held_body: None, + grab_offset: Pose::default(), + }; + scene.step(); + assert_eq!(scene.left_hand_grab.held_body, Some(cube.body)); + + RapierScene::sync_hand_bodies( + &scene.right_hand, + &[HandCollider::Ball { + center: pose.position, + radius: 0.09, + }], + &mut scene.bodies, + &mut scene.colliders, + ); + scene.right_hand_grab = HandGrabState { + shared_hand: XrSharedHand::RightHand, + pose: right_pose, + previous_pose: right_pose, + linvel: vec3f(0.0, 0.0, 0.0), + tracked: true, + gripping: true, + held_body: None, + grab_offset: Pose::default(), + }; + scene.step(); + + assert_eq!(scene.left_hand_grab.held_body, Some(cube.body)); + assert_eq!(scene.right_hand_grab.held_body, Some(cube.body)); + let body = scene + .bodies + .get(cube.body) + .expect("cube body should exist while two hands hold it"); + assert_eq!(body.body_type(), RigidBodyType::KinematicPositionBased); + + scene.left_hand_grab.gripping = false; + scene.apply_held_body_targets(); + + assert_eq!(scene.left_hand_grab.held_body, None); + assert_eq!(scene.right_hand_grab.held_body, Some(cube.body)); + let body = scene + .bodies + .get(cube.body) + .expect("cube body should still exist after primary hand drops"); + assert_eq!(body.body_type(), RigidBodyType::KinematicPositionBased); + assert_eq!( + scene.held_by_for_body(cube.body), + Some(XrSharedHand::RightHand) + ); + } + + #[test] + fn sticky_raw_grab_bit_does_not_keep_pointing_hand_in_grab_state() { + let mut scene = RapierScene::new(0.0); + let widget_uid = WidgetUid(45); + let pose = Pose::new(Quat::default(), vec3f(0.0, 1.0, 0.0)); + scene.spawn_dynamic_box( + widget_uid, + pose, + vec3f(0.05, 0.05, 0.05), + vec3f(1.0, 1.0, 1.0), + 1.0, + 0.5, + 0.0, + ); + let cube = scene.cubes[0]; + let hand_pose = Pose::new(Quat::default(), pose.position); + + scene.left_hand_grab = HandGrabState { + shared_hand: XrSharedHand::LeftHand, + pose: hand_pose, + previous_pose: hand_pose, + linvel: vec3f(0.0, 0.0, 0.0), + tracked: true, + gripping: true, + held_body: Some(cube.body), + grab_offset: Pose::default(), + }; + + let mut hand = XrHand::default(); + hand.flags = XrHand::IN_VIEW | XrHand::AIM_VALID; + hand.tips_active = XrHand::GRAB_ACTIVE | (1 << XrHand::INDEX_TIP); + hand.tips[XrHand::INDEX_TIP] = 0.038; + hand.joints[XrHand::CENTER] = Pose::new(Quat::default(), pose.position); + hand.joints[XrHand::WRIST] = + Pose::new(Quat::default(), pose.position + vec3f(0.0, -0.03, 0.05)); + hand.joints[XrHand::INDEX_BASE] = Pose::new(Quat::default(), pose.position); + hand.joints[XrHand::INDEX_KNUCKLE1] = + Pose::new(Quat::default(), pose.position + vec3f(0.0, 0.0, -0.041)); + hand.joints[XrHand::INDEX_KNUCKLE2] = + Pose::new(Quat::default(), pose.position + vec3f(0.001, 0.002, -0.082)); + hand.joints[XrHand::INDEX_KNUCKLE3] = + Pose::new(Quat::default(), pose.position + vec3f(0.002, 0.004, -0.122)); + + scene.sync_tracked_hands(&hand, &XrHand::default()); + scene.apply_held_body_targets(); + + assert_eq!(scene.left_hand_grab.held_body, None); + let body = scene + .bodies + .get(cube.body) + .expect("cube body should still exist after sticky-grab release"); + assert_eq!(body.body_type(), RigidBodyType::Dynamic); + } + + #[test] + fn hand_grab_pose_uses_index_tip_when_pinch_midpoint_is_unavailable() { + let mut scene = RapierScene::new(0.0); + let widget_uid = WidgetUid(46); + let pose = Pose::new(Quat::default(), vec3f(0.0, 1.0, 0.0)); + scene.spawn_dynamic_box( + widget_uid, + pose, + vec3f(0.05, 0.05, 0.05), + vec3f(1.0, 1.0, 1.0), + 1.0, + 0.5, + 0.0, + ); + let cube = scene.cubes[0]; + let hand_pose = Pose::new(Quat::default(), pose.position); + + scene.left_hand_grab = HandGrabState { + shared_hand: XrSharedHand::LeftHand, + pose: hand_pose, + previous_pose: hand_pose, + linvel: vec3f(0.0, 0.0, 0.0), + tracked: true, + gripping: true, + held_body: Some(cube.body), + grab_offset: Pose::default(), + }; + + let mut hand = XrHand::default(); + hand.flags = XrHand::IN_VIEW | XrHand::AIM_VALID; + hand.tips_active = XrHand::GRAB_ACTIVE | (1 << XrHand::INDEX_TIP); + hand.tips[XrHand::INDEX_TIP] = 0.034; + hand.joints[XrHand::CENTER] = Pose::new(Quat::default(), pose.position); + hand.joints[XrHand::WRIST] = + Pose::new(Quat::default(), pose.position + vec3f(0.0, -0.03, 0.05)); + hand.joints[XrHand::INDEX_BASE] = + Pose::new(Quat::default(), pose.position + vec3f(0.0, 0.0, -0.010)); + hand.joints[XrHand::INDEX_KNUCKLE1] = + Pose::new(Quat::default(), pose.position + vec3f(0.0, -0.005, -0.030)); + hand.joints[XrHand::INDEX_KNUCKLE2] = Pose::new( + Quat::default(), + pose.position + vec3f(0.012, -0.018, -0.040), + ); + hand.joints[XrHand::INDEX_KNUCKLE3] = Pose::new( + Quat::default(), + pose.position + vec3f(0.022, -0.032, -0.020), + ); + + assert!(hand.grab_intent(), "curled hand should still report grab intent"); + assert!( + hand.tracking_pose().is_some(), + "palm tracking should still be valid for the sample" + ); + assert!( + hand.pinch_anchor_pose().is_none(), + "sample must not expose a pinch anchor" + ); + let index_tip = hand + .tip_pos_checked(XrHand::INDEX_TIP) + .expect("index tip should still be valid"); + let palm_pose = hand + .tracking_pose() + .expect("tracking pose should still be valid"); + + scene.sync_tracked_hands(&hand, &XrHand::default()); + scene.apply_held_body_targets(); + + assert_eq!(scene.left_hand_grab.held_body, Some(cube.body)); + assert!(scene.left_hand_grab.tracked); + assert!(scene.left_hand_grab.gripping); + assert_vec3_close(scene.left_hand_grab.pose.position, index_tip, 0.0001); + assert!( + (scene.left_hand_grab.pose.position - palm_pose.position).length() > 0.01, + "grab pose should stay on the finger anchor, not the palm" + ); + let body = scene + .bodies + .get(cube.body) + .expect("cube body should still exist after index-tip hold update"); + assert_eq!(body.body_type(), RigidBodyType::KinematicPositionBased); + } +} diff --git a/xr/src/tests/scene/xr_root.rs b/xr/src/tests/scene/xr_root.rs new file mode 100644 index 0000000000..56a6625636 --- /dev/null +++ b/xr/src/tests/scene/xr_root.rs @@ -0,0 +1,45 @@ +mod tests { + use super::*; + + fn sample(captured_at: f64, vertical_split: f32) -> BoxSyncPoseSample { + BoxSyncPoseSample { + anchor: XrAnchor { + left: vec3f(-0.1, vertical_split * 0.5, -0.4), + right: vec3f(0.1, -vertical_split * 0.5, -0.4), + }, + captured_at, + vertical_split, + } + } + + #[test] + fn box_sync_detector_emits_every_full_reversal_extrema() { + let mut runtime = XrSyncAnchorRuntime::default(); + let mut emitted = Vec::new(); + let samples = [ + sample(0.00, 0.00), + sample(0.10, 0.03), + sample(0.20, 0.06), + sample(0.30, 0.03), + sample(0.40, 0.00), + sample(0.50, -0.03), + sample(0.60, -0.06), + sample(0.70, -0.03), + sample(0.80, 0.00), + sample(0.90, 0.03), + sample(1.00, 0.06), + ]; + + for sample in samples { + if let Some(sync_anchor) = runtime.update_detector_sample(Some(sample)) { + emitted.push(sync_anchor); + } + } + + assert_eq!(emitted.len(), 2); + assert_eq!(emitted[0].extrema, XrSyncAnchorExtrema::High); + assert_eq!(emitted[0].captured_at, 0.20); + assert_eq!(emitted[1].extrema, XrSyncAnchorExtrema::Low); + assert_eq!(emitted[1].captured_at, 0.60); + } +} diff --git a/xr/src/tests/scene/xr_shared_object_sync.rs b/xr/src/tests/scene/xr_shared_object_sync.rs new file mode 100644 index 0000000000..ca18a17571 --- /dev/null +++ b/xr/src/tests/scene/xr_shared_object_sync.rs @@ -0,0 +1,711 @@ +mod tests { + use super::*; + + #[test] + fn remote_allocations_are_scoped_per_peer_within_a_pool_group() { + let activity_id = XrActivityId(live_id!(ico_shoot_scene)); + let mut registry = XrSharedObjectRegistry::default(); + registry.replace_spawnables( + activity_id, + [ + XrSpawnableObjectBinding { + object_id: XrSpawnableObjectId(11), + allocation_group_id: XrSpawnableObjectId(91), + widget_uid: WidgetUid(101), + bootstrap_shared: true, + }, + XrSpawnableObjectBinding { + object_id: XrSpawnableObjectId(12), + allocation_group_id: XrSpawnableObjectId(91), + widget_uid: WidgetUid(102), + bootstrap_shared: true, + }, + ], + ); + + let left = registry.resolve_remote_widget_uid( + activity_id, + xr_make_shared_object_id(XrNetPeerId(1), XrSharedObjectCounter(0)).unwrap(), + XrSpawnableObjectId(11), + ); + let right = registry.resolve_remote_widget_uid( + activity_id, + xr_make_shared_object_id(XrNetPeerId(2), XrSharedObjectCounter(0)).unwrap(), + XrSpawnableObjectId(11), + ); + + assert_eq!(left, Some(WidgetUid(101))); + assert_eq!(right, Some(WidgetUid(102))); + } + + #[test] + fn local_allocations_use_hashed_shared_object_ids() { + let activity_id = XrActivityId(live_id!(ico_shoot_scene)); + let mut registry = XrSharedObjectRegistry::default(); + registry.set_local_peer_id(XrNetPeerId(55)); + registry.replace_spawnables( + activity_id, + [XrSpawnableObjectBinding { + object_id: XrSpawnableObjectId(11), + allocation_group_id: XrSpawnableObjectId(91), + widget_uid: WidgetUid(101), + bootstrap_shared: true, + }], + ); + + let allocation = registry + .allocate_local_shared_object(activity_id, WidgetUid(101)) + .expect("local shared-object allocation should succeed"); + assert_eq!( + allocation.shared_object_id, + xr_make_shared_object_id(XrNetPeerId(55), XrSharedObjectCounter(0)) + .expect("hashed shared object id should allocate") + ); + assert_eq!(allocation.spawnable_object_id, XrSpawnableObjectId(11)); + assert_eq!(allocation.epoch, 0); + } + + #[test] + fn pool_overflow_rebind_evicts_stale_remote_record() { + let activity_id = XrActivityId(live_id!(ico_shoot_scene)); + let mut registry = XrSharedObjectRegistry::default(); + registry.replace_spawnables( + activity_id, + [ + XrSpawnableObjectBinding { + object_id: XrSpawnableObjectId(11), + allocation_group_id: XrSpawnableObjectId(91), + widget_uid: WidgetUid(101), + bootstrap_shared: false, + }, + XrSpawnableObjectBinding { + object_id: XrSpawnableObjectId(12), + allocation_group_id: XrSpawnableObjectId(91), + widget_uid: WidgetUid(102), + bootstrap_shared: false, + }, + ], + ); + + let object_a = xr_make_shared_object_id(XrNetPeerId(1), XrSharedObjectCounter(0)).unwrap(); + let object_b = xr_make_shared_object_id(XrNetPeerId(2), XrSharedObjectCounter(0)).unwrap(); + let object_c = xr_make_shared_object_id(XrNetPeerId(3), XrSharedObjectCounter(0)).unwrap(); + + let widget_a = registry + .register_remote_shared_object( + activity_id, + 1.0, + object_a, + 0, + XrNetPeerId(1), + XrSharedObjectFidelity::ImpactCritical, + XrSpawnableObjectId(11), + Pose::new(Quat::default(), vec3f(-0.2, 1.0, -0.5)), + vec3f(0.0, 0.0, 0.0), + vec3f(0.0, 0.0, 0.0), + ) + .expect("first remote object should bind"); + let widget_b = registry + .register_remote_shared_object( + activity_id, + 2.0, + object_b, + 0, + XrNetPeerId(2), + XrSharedObjectFidelity::ImpactCritical, + XrSpawnableObjectId(11), + Pose::new(Quat::default(), vec3f(0.0, 1.0, -0.5)), + vec3f(0.0, 0.0, 0.0), + vec3f(0.0, 0.0, 0.0), + ) + .expect("second remote object should bind"); + let widget_c = registry + .register_remote_shared_object( + activity_id, + 3.0, + object_c, + 0, + XrNetPeerId(3), + XrSharedObjectFidelity::ImpactCritical, + XrSpawnableObjectId(11), + Pose::new(Quat::default(), vec3f(0.2, 1.0, -0.5)), + vec3f(0.0, 0.0, 0.0), + vec3f(0.0, 0.0, 0.0), + ) + .expect("third remote object should rebind into the same pool"); + + assert_eq!(widget_a, WidgetUid(101)); + assert_eq!(widget_b, WidgetUid(102)); + assert!(widget_c == WidgetUid(101) || widget_c == WidgetUid(102)); + assert_eq!(registry.remote_objects.len(), 2); + assert_eq!(registry.remote_object_to_widget.len(), 2); + assert_eq!(registry.remote_widget_to_object.len(), 2); + assert!( + registry.remote_shared_object_snapshot(object_c).is_some(), + "newest remote object should remain tracked" + ); + assert!( + registry.remote_shared_object_snapshot(object_a).is_none() + || registry.remote_shared_object_snapshot(object_b).is_none(), + "one older pooled remote object must be evicted instead of lingering as a stale shadow" + ); + } + + #[test] + fn pooled_local_allocations_reuse_object_id_and_advance_epoch() { + let activity_id = XrActivityId(live_id!(ico_shoot_scene)); + let mut registry = XrSharedObjectRegistry::default(); + registry.set_local_peer_id(XrNetPeerId(55)); + registry.replace_spawnables( + activity_id, + [XrSpawnableObjectBinding { + object_id: XrSpawnableObjectId(11), + allocation_group_id: XrSpawnableObjectId(91), + widget_uid: WidgetUid(101), + bootstrap_shared: true, + }], + ); + + let first = registry + .allocate_local_shared_object(activity_id, WidgetUid(101)) + .expect("first local allocation should succeed"); + let second = registry + .allocate_local_shared_object(activity_id, WidgetUid(101)) + .expect("pooled widget reuse should reuse the shared object id"); + + assert_eq!(first.shared_object_id, second.shared_object_id); + assert_eq!(first.epoch, 0); + assert_eq!(second.epoch, 1); + } + + #[test] + fn local_spawn_prepares_oldest_pooled_object_before_newer_remote_or_local_slots() { + let activity_id = XrActivityId(live_id!(ico_shoot_scene)); + let mut registry = XrSharedObjectRegistry::default(); + registry.set_local_peer_id(XrNetPeerId(55)); + registry.replace_spawnables( + activity_id, + [ + XrSpawnableObjectBinding { + object_id: XrSpawnableObjectId(11), + allocation_group_id: XrSpawnableObjectId(91), + widget_uid: WidgetUid(101), + bootstrap_shared: false, + }, + XrSpawnableObjectBinding { + object_id: XrSpawnableObjectId(12), + allocation_group_id: XrSpawnableObjectId(91), + widget_uid: WidgetUid(102), + bootstrap_shared: false, + }, + ], + ); + + let remote_object_id = + xr_make_shared_object_id(XrNetPeerId(7), XrSharedObjectCounter(3)).unwrap(); + registry + .register_remote_shared_object( + activity_id, + 1.0, + remote_object_id, + 0, + XrNetPeerId(7), + XrSharedObjectFidelity::ImpactCritical, + XrSpawnableObjectId(11), + Pose::new(Quat::default(), vec3f(-0.1, 1.0, -0.5)), + vec3f(0.0, 0.0, 0.0), + vec3f(0.0, 0.0, 0.0), + ) + .expect("remote projectile should bind"); + + let local_recent = registry + .force_local_shared_object_reset( + activity_id, + WidgetUid(102), + 2.0, + Pose::new(Quat::default(), vec3f(0.1, 1.0, -0.4)), + vec3f(0.0, 0.0, 0.0), + vec3f(0.0, 0.0, 0.0), + ) + .expect("local projectile should claim the second slot"); + + let (allocation, reused_remote) = registry + .prepare_local_spawn_allocation( + activity_id, + WidgetUid(102), + 3.0, + Pose::new(Quat::default(), vec3f(0.2, 1.0, -0.3)), + vec3f(0.0, 0.0, 0.0), + vec3f(0.0, 0.0, 0.0), + ) + .expect("pool-full spawn should reclaim the oldest slot"); + + assert!(reused_remote, "oldest pooled slot should come from the remote shadow"); + assert_eq!(allocation.widget_uid, WidgetUid(101)); + assert_eq!(allocation.shared_object_id, remote_object_id); + assert_eq!( + registry + .local_shared_object_snapshot(local_recent.shared_object_id) + .expect("newer local pooled object should remain in place") + .widget_uid, + WidgetUid(102) + ); + } + + #[test] + fn prune_missing_local_shared_objects_returns_despawns() { + let activity_id = XrActivityId(live_id!(ico_shoot_scene)); + let mut registry = XrSharedObjectRegistry::default(); + registry.set_local_peer_id(XrNetPeerId(55)); + registry.replace_spawnables( + activity_id, + [XrSpawnableObjectBinding { + object_id: XrSpawnableObjectId(11), + allocation_group_id: XrSpawnableObjectId(91), + widget_uid: WidgetUid(101), + bootstrap_shared: true, + }], + ); + let allocation = registry + .allocate_local_shared_object(activity_id, WidgetUid(101)) + .expect("allocation should succeed"); + + let despawns = registry.prune_missing_local_shared_objects(&HashMap::new()); + assert_eq!(despawns.len(), 1); + assert_eq!(despawns[0].0, allocation.shared_object_id); + assert_eq!(despawns[0].1, allocation.epoch); + assert_eq!(despawns[0].2, WidgetUid(101)); + assert_eq!(registry.active_count(), 0); + } + + #[test] + fn scheduled_takeover_promotes_remote_object_to_local_authority() { + let activity_id = XrActivityId(live_id!(ico_shoot_scene)); + let mut registry = XrSharedObjectRegistry::default(); + registry.set_local_peer_id(XrNetPeerId(55)); + registry.replace_spawnables( + activity_id, + [XrSpawnableObjectBinding { + object_id: XrSpawnableObjectId(11), + allocation_group_id: XrSpawnableObjectId(91), + widget_uid: WidgetUid(101), + bootstrap_shared: true, + }], + ); + + let object_id = xr_make_shared_object_id(XrNetPeerId(7), XrSharedObjectCounter(3)).unwrap(); + let widget_uid = registry + .register_remote_shared_object( + activity_id, + 0.0, + object_id, + 0, + XrNetPeerId(7), + XrSharedObjectFidelity::ImpactCritical, + XrSpawnableObjectId(11), + Pose::new(Quat::default(), vec3f(0.0, 1.0, -0.5)), + vec3f(0.0, 0.0, 0.0), + vec3f(0.0, 0.0, 0.0), + ) + .expect("remote object should bind into the spawnable pool"); + assert_eq!(widget_uid, WidgetUid(101)); + + assert!(registry.schedule_authority_transfer( + object_id, + 1, + XrNetPeerId(7), + XrNetPeerId(55), + 0.0, + 0, + 17, + Some(XrSharedHand::RightHand), + Pose::new(Quat::default(), vec3f(0.1, 1.0, -0.45)), + vec3f(0.2, 0.0, 0.0), + vec3f(0.0, 0.0, 0.0), + )); + let transfers = registry.apply_scheduled_authority_transfers(0.2, 1); + + assert_eq!(transfers.len(), 1); + assert!(!transfers[0].shadow); + assert_eq!(transfers[0].object_id, object_id); + assert_eq!(transfers[0].widget_uid, WidgetUid(101)); + assert!(registry.remote_shared_object_snapshot(object_id).is_none()); + let local = registry + .local_shared_object_snapshot(object_id) + .expect("takeover should keep the same object id under local authority"); + assert_eq!(local.widget_uid, WidgetUid(101)); + assert_eq!(local.epoch, 1); + assert_eq!(local.authority, XrNetPeerId(55)); + } + + #[test] + fn scheduled_handoff_demotes_local_object_to_remote_shadow() { + let activity_id = XrActivityId(live_id!(ico_shoot_scene)); + let mut registry = XrSharedObjectRegistry::default(); + registry.set_local_peer_id(XrNetPeerId(55)); + registry.replace_spawnables( + activity_id, + [XrSpawnableObjectBinding { + object_id: XrSpawnableObjectId(11), + allocation_group_id: XrSpawnableObjectId(91), + widget_uid: WidgetUid(101), + bootstrap_shared: true, + }], + ); + + let allocation = registry + .allocate_local_shared_object(activity_id, WidgetUid(101)) + .expect("local allocation should succeed"); + assert!(registry.schedule_authority_transfer( + allocation.shared_object_id, + allocation.epoch.wrapping_add(1), + XrNetPeerId(55), + XrNetPeerId(8), + 0.0, + 0, + 23, + Some(XrSharedHand::LeftHand), + Pose::new(Quat::default(), vec3f(-0.1, 1.1, -0.4)), + vec3f(-0.2, 0.0, 0.1), + vec3f(0.0, 0.0, 0.0), + )); + let transfers = registry.apply_scheduled_authority_transfers(0.2, 1); + + assert_eq!(transfers.len(), 1); + assert!(transfers[0].shadow); + assert_eq!(transfers[0].source_authority, XrNetPeerId(55)); + assert!(registry + .local_shared_object_snapshot(allocation.shared_object_id) + .is_none()); + let remote = registry + .remote_shared_object_snapshot(allocation.shared_object_id) + .expect("handoff should preserve the object id as a remote shadow"); + assert_eq!(remote.widget_uid, WidgetUid(101)); + assert_eq!(remote.authority, XrNetPeerId(8)); + assert_eq!(remote.state_source_authority, XrNetPeerId(55)); + } + + #[test] + fn replacing_spawnables_for_same_activity_preserves_active_shared_objects() { + let activity_id = XrActivityId(live_id!(ico_shoot_scene)); + let mut registry = XrSharedObjectRegistry::default(); + registry.set_local_peer_id(XrNetPeerId(55)); + registry.replace_spawnables( + activity_id, + [ + XrSpawnableObjectBinding { + object_id: XrSpawnableObjectId(11), + allocation_group_id: XrSpawnableObjectId(11), + widget_uid: WidgetUid(101), + bootstrap_shared: true, + }, + XrSpawnableObjectBinding { + object_id: XrSpawnableObjectId(12), + allocation_group_id: XrSpawnableObjectId(12), + widget_uid: WidgetUid(102), + bootstrap_shared: true, + }, + ], + ); + + let local = registry + .allocate_local_shared_object(activity_id, WidgetUid(101)) + .expect("local shared object should allocate"); + let remote_object_id = + xr_make_shared_object_id(XrNetPeerId(7), XrSharedObjectCounter(9)).unwrap(); + registry + .register_remote_shared_object( + activity_id, + 0.0, + remote_object_id, + 0, + XrNetPeerId(7), + XrSharedObjectFidelity::ImpactCritical, + XrSpawnableObjectId(12), + Pose::new(Quat::default(), vec3f(0.0, 1.0, -0.4)), + vec3f(0.0, 0.0, -1.0), + vec3f(0.0, 0.0, 0.0), + ) + .expect("remote shared object should bind"); + + registry.replace_spawnables( + activity_id, + [ + XrSpawnableObjectBinding { + object_id: XrSpawnableObjectId(11), + allocation_group_id: XrSpawnableObjectId(11), + widget_uid: WidgetUid(201), + bootstrap_shared: true, + }, + XrSpawnableObjectBinding { + object_id: XrSpawnableObjectId(12), + allocation_group_id: XrSpawnableObjectId(12), + widget_uid: WidgetUid(202), + bootstrap_shared: true, + }, + ], + ); + + assert_eq!( + registry + .local_shared_object_snapshot(local.shared_object_id) + .expect("local shared object should survive refresh") + .widget_uid, + WidgetUid(201) + ); + assert_eq!( + registry + .remote_shared_object_snapshot(remote_object_id) + .expect("remote shared object should survive refresh") + .widget_uid, + WidgetUid(202) + ); + } + + #[test] + fn remote_state_updates_are_rejected_from_the_wrong_authority() { + let activity_id = XrActivityId(live_id!(ico_shoot_scene)); + let mut registry = XrSharedObjectRegistry::default(); + registry.replace_spawnables( + activity_id, + [XrSpawnableObjectBinding { + object_id: XrSpawnableObjectId(11), + allocation_group_id: XrSpawnableObjectId(11), + widget_uid: WidgetUid(101), + bootstrap_shared: true, + }], + ); + + let object_id = xr_make_shared_object_id(XrNetPeerId(7), XrSharedObjectCounter(3)).unwrap(); + registry + .register_remote_shared_object( + activity_id, + 0.0, + object_id, + 0, + XrNetPeerId(7), + XrSharedObjectFidelity::ImpactCritical, + XrSpawnableObjectId(11), + Pose::new(Quat::default(), vec3f(0.0, 1.0, -0.5)), + vec3f(0.0, 0.0, -1.0), + vec3f(0.0, 0.0, 0.0), + ) + .expect("remote object should bind"); + + let stale_state = XrNetSharedObjectState { + seq: 1, + sent_at: 0.5, + physics_tick: 10, + object_id, + epoch: 0, + authority: XrNetPeerId(8), + fidelity: XrSharedObjectFidelity::ImpactCritical, + mode: XrSharedObjectMode::Dynamic, + pose: Pose::new(Quat::default(), vec3f(0.4, 1.0, -0.3)), + linvel: vec3f(1.0, 0.0, 0.0), + angvel: vec3f(0.0, 0.0, 0.0), + }; + assert!(registry + .record_remote_shared_object_state(XrNetPeerId(8), stale_state) + .is_none()); + + let fresh_state = XrNetSharedObjectState { + authority: XrNetPeerId(7), + ..stale_state + }; + assert_eq!( + registry.record_remote_shared_object_state(XrNetPeerId(7), fresh_state), + Some(WidgetUid(101)) + ); + let snapshot = registry + .remote_shared_object_snapshot(object_id) + .expect("remote snapshot should still exist"); + assert_eq!(snapshot.authority, XrNetPeerId(7)); + assert_eq!(snapshot.state_source_authority, XrNetPeerId(7)); + assert_eq!(snapshot.latest_state, Some(fresh_state)); + } + + #[test] + fn remote_state_updates_accept_matching_net_peer_authority_ids() { + let activity_id = XrActivityId(live_id!(ico_shoot_scene)); + let mut registry = XrSharedObjectRegistry::default(); + registry.replace_spawnables( + activity_id, + [XrSpawnableObjectBinding { + object_id: XrSpawnableObjectId(11), + allocation_group_id: XrSpawnableObjectId(11), + widget_uid: WidgetUid(101), + bootstrap_shared: true, + }], + ); + + let sender_node_id = XrNetPeerId(0x1234); + let sender_peer = XrNetPeer { + id: sender_node_id, + addr: "192.168.1.42:41547".parse().unwrap(), + }; + let object_id = xr_make_shared_object_id(sender_node_id, XrSharedObjectCounter(3)) + .expect("shared object id should hash"); + registry + .register_remote_shared_object( + activity_id, + 0.0, + object_id, + 0, + sender_node_id, + XrSharedObjectFidelity::ImpactCritical, + XrSpawnableObjectId(11), + Pose::new(Quat::default(), vec3f(0.0, 1.0, -0.5)), + vec3f(0.0, 0.0, -1.0), + vec3f(0.0, 0.0, 0.0), + ) + .expect("remote object should bind"); + + let updated_state = XrNetSharedObjectState { + seq: 1, + sent_at: 0.5, + physics_tick: 10, + object_id, + epoch: 0, + authority: sender_node_id, + fidelity: XrSharedObjectFidelity::ImpactCritical, + mode: XrSharedObjectMode::Dynamic, + pose: Pose::new(Quat::default(), vec3f(0.4, 1.0, -0.3)), + linvel: vec3f(1.0, 0.0, 0.0), + angvel: vec3f(0.0, 0.0, 0.0), + }; + + assert_eq!( + registry.record_remote_shared_object_state(sender_peer.id, updated_state), + Some(WidgetUid(101)) + ); + } + + #[test] + fn force_local_shared_object_reset_reclaims_remote_object_for_local_resetter() { + let activity_id = XrActivityId(live_id!(ico_shoot_scene)); + let widget_uid = WidgetUid(101); + let mut registry = XrSharedObjectRegistry::default(); + registry.set_local_peer_id(XrNetPeerId(55)); + registry.replace_spawnables( + activity_id, + [XrSpawnableObjectBinding { + object_id: XrSpawnableObjectId(11), + allocation_group_id: XrSpawnableObjectId(11), + widget_uid, + bootstrap_shared: true, + }], + ); + + let object_id = xr_make_shared_object_id(XrNetPeerId(7), XrSharedObjectCounter(3)) + .expect("shared object id should hash"); + registry + .register_remote_shared_object( + activity_id, + 0.0, + object_id, + 2, + XrNetPeerId(7), + XrSharedObjectFidelity::ImpactCritical, + XrSpawnableObjectId(11), + Pose::new(Quat::default(), vec3f(0.0, 1.0, -0.5)), + vec3f(0.0, 0.0, -1.0), + vec3f(0.0, 0.0, 0.0), + ) + .expect("remote block should bind"); + + let reset_pose = Pose::new(Quat::default(), vec3f(0.1, 1.2, -0.3)); + let reset_linvel = vec3f(0.2, 0.0, -0.4); + let reset_angvel = vec3f(0.0, 0.1, 0.0); + let allocation = registry + .force_local_shared_object_reset( + activity_id, + widget_uid, + 0.0, + reset_pose, + reset_linvel, + reset_angvel, + ) + .expect("reset should reclaim the remote object for the local peer"); + + assert_eq!(allocation.shared_object_id, object_id); + assert_eq!(allocation.epoch, 3); + assert!(registry.remote_shared_object_snapshot(object_id).is_none()); + + let local = registry + .local_shared_object_snapshot(object_id) + .expect("reset should promote the reclaimed brick into local authority"); + assert_eq!(local.authority, XrNetPeerId(55)); + assert_eq!(local.widget_uid, widget_uid); + assert_eq!(local.epoch, 3); + let latest_state = local + .latest_state + .expect("reclaimed local brick should carry the reset state"); + assert_eq!(latest_state.pose, reset_pose); + assert_eq!(latest_state.linvel, reset_linvel); + assert_eq!(latest_state.angvel, reset_angvel); + } + + #[test] + fn releasing_remote_shared_objects_uses_current_authority_not_allocator_peer() { + let activity_id = XrActivityId(live_id!(ico_shoot_scene)); + let widget_uid = WidgetUid(101); + let mut registry = XrSharedObjectRegistry::default(); + registry.set_local_peer_id(XrNetPeerId(55)); + registry.replace_spawnables( + activity_id, + [XrSpawnableObjectBinding { + object_id: XrSpawnableObjectId(11), + allocation_group_id: XrSpawnableObjectId(11), + widget_uid, + bootstrap_shared: true, + }], + ); + + let object_id = xr_make_shared_object_id(XrNetPeerId(7), XrSharedObjectCounter(3)) + .expect("shared object id should hash"); + registry + .register_remote_shared_object( + activity_id, + 0.0, + object_id, + 0, + XrNetPeerId(7), + XrSharedObjectFidelity::ImpactCritical, + XrSpawnableObjectId(11), + Pose::new(Quat::default(), vec3f(0.0, 1.0, -0.5)), + vec3f(0.0, 0.0, -1.0), + vec3f(0.0, 0.0, 0.0), + ) + .expect("remote block should bind"); + + assert!(registry.schedule_authority_transfer( + object_id, + 1, + XrNetPeerId(7), + XrNetPeerId(8), + 0.0, + 0, + 17, + None, + Pose::new(Quat::default(), vec3f(0.1, 1.0, -0.4)), + vec3f(0.0, 0.0, -0.5), + vec3f(0.0, 0.0, 0.0), + )); + let transfers = registry.apply_scheduled_authority_transfers(0.1, 1); + assert_eq!(transfers.len(), 1); + assert_eq!(transfers[0].new_authority, XrNetPeerId(8)); + + assert_eq!( + registry.release_remote_shared_objects_by_peer_id(XrNetPeerId(7)), + Vec::::new() + ); + assert!(registry.remote_shared_object_snapshot(object_id).is_some()); + + assert_eq!( + registry.release_remote_shared_objects_by_peer_id(XrNetPeerId(8)), + vec![widget_uid] + ); + assert!(registry.remote_shared_object_snapshot(object_id).is_none()); + } +} diff --git a/xr/src/tests/util/depth_debug_mesh.rs b/xr/src/tests/util/depth_debug_mesh.rs new file mode 100644 index 0000000000..707bc84f7d --- /dev/null +++ b/xr/src/tests/util/depth_debug_mesh.rs @@ -0,0 +1,189 @@ +fn depth_tsd_distance_meters(voxel_size_meters: f32) -> f32 { + voxel_size_meters * 2.0 +} + +mod tests { + use super::*; + use std::{collections::HashMap, sync::Arc}; + + fn packed_position(vertices: &[f32], vertex_index: usize) -> Vec3f { + let base = vertex_index * XR_DEBUG_DEPTH_FLOATS_PER_VERTEX; + vec3f(vertices[base], vertices[base + 1], vertices[base + 2]) + } + + fn packed_barycentric(vertices: &[f32], vertex_index: usize) -> Vec3f { + let base = vertex_index * XR_DEBUG_DEPTH_FLOATS_PER_VERTEX; + vec3f(vertices[base + 4], vertices[base + 5], vertices[base + 6]) + } + + fn set_normalized_distance( + chunks: &mut HashMap>, + chunk_edge: i32, + coord: VoxelCoord, + normalized_distance: f32, + confidence: u8, + ) { + let chunk_key = ChunkKey::new( + coord.x.div_euclid(chunk_edge), + coord.y.div_euclid(chunk_edge), + coord.z.div_euclid(chunk_edge), + ); + let lx = coord.x.rem_euclid(chunk_edge) as usize; + let ly = coord.y.rem_euclid(chunk_edge) as usize; + let lz = coord.z.rem_euclid(chunk_edge) as usize; + let edge = chunk_edge as usize; + let id = lx + ly * edge + lz * edge * edge; + + let chunk = Arc::make_mut( + chunks + .entry(chunk_key) + .or_insert_with(|| Arc::new(SparseTsdReadChunk::new(edge * edge * edge))), + ); + chunk.set_value(id, normalized_distance, confidence, 1); + } + + fn make_flat_floor_snapshot_with_confidence( + voxel_size: f32, + confidence: u8, + ) -> TsdfPublishedSnapshot { + let chunk_edge = 8; + let mut chunks = HashMap::new(); + let tsd_distance_meters = depth_tsd_distance_meters(voxel_size); + let mut active_value_count = 0usize; + for z in -6..=6 { + for y in -6..=6 { + for x in -6..=6 { + let world_y = (y as f32 + 0.5) * voxel_size; + let normalized = (world_y / tsd_distance_meters).clamp(-1.0, 1.0); + set_normalized_distance( + &mut chunks, + chunk_edge, + VoxelCoord::new(x, y, z), + normalized, + confidence, + ); + active_value_count += 1; + } + } + } + TsdfPublishedSnapshot { + generation: 1, + latest_topology_generation: 1, + update_sequence: 1, + grid: Arc::new(SparseTsdGridReadSnapshot { + voxel_size, + chunk_edge, + chunk_edge_shift: Some(chunk_edge.trailing_zeros() as u8), + chunk_edge_mask: chunk_edge - 1, + chunk_volume: (chunk_edge as usize).pow(3), + active_value_count, + active_bounds: Some(( + vec3f(-6.0 * voxel_size, -6.0 * voxel_size, -6.0 * voxel_size), + vec3f(7.0 * voxel_size, 7.0 * voxel_size, 7.0 * voxel_size), + )), + chunks, + }), + height_map: None, + } + } + + fn make_flat_floor_snapshot(voxel_size: f32) -> TsdfPublishedSnapshot { + make_flat_floor_snapshot_with_confidence(voxel_size, 8) + } + + #[test] + fn pack_surface_mesh_debug_vertices_expands_to_triangle_barycentrics() { + let mesh = SurfaceMesh32 { + positions: vec![ + [0.0, 0.0, 0.0], + [1.0, 0.0, 0.0], + [1.0, 1.0, 0.0], + [0.0, 1.0, 0.0], + ], + indices: vec![0, 1, 2, 0, 2, 3], + }; + + let (indices, vertices) = pack_surface_mesh_debug_vertices(&mesh); + + assert_eq!(indices, vec![0, 1, 2, 3, 4, 5]); + assert_eq!(vertices.len(), 6 * XR_DEBUG_DEPTH_FLOATS_PER_VERTEX); + assert_eq!(packed_position(&vertices, 0), vec3f(0.0, 0.0, 0.0)); + assert_eq!(packed_position(&vertices, 2), vec3f(1.0, 1.0, 0.0)); + assert_eq!(packed_position(&vertices, 5), vec3f(0.0, 1.0, 0.0)); + assert_eq!(packed_barycentric(&vertices, 0), vec3f(1.0, 0.0, 0.0)); + assert_eq!(packed_barycentric(&vertices, 1), vec3f(0.0, 1.0, 0.0)); + assert_eq!(packed_barycentric(&vertices, 2), vec3f(0.0, 0.0, 1.0)); + } + + #[test] + fn push_debug_depth_plane_emits_single_quad_mesh() { + let plane = DepthQuerySupportPlane { + point: vec3f(0.0, 0.5, 0.0), + normal: vec3f(0.0, 1.0, 0.0), + tangent: vec3f(1.0, 0.0, 0.0), + bitangent: vec3f(0.0, 0.0, 1.0), + half_extent_tangent: 0.25, + half_extent_bitangent: 0.10, + }; + let mut indices = Vec::new(); + let mut vertices = Vec::new(); + + push_debug_depth_plane(&mut indices, &mut vertices, plane); + + assert_eq!(indices, vec![0, 1, 2, 3, 4, 5]); + assert_eq!(vertices.len(), 6 * XR_DEBUG_DEPTH_FLOATS_PER_VERTEX); + let first = packed_position(&vertices, 0); + let sixth = packed_position(&vertices, 5); + assert_eq!(first, vec3f(-0.25, 0.5, -0.10)); + assert_eq!(sixth, vec3f(-0.25, 0.5, 0.10)); + assert_eq!(packed_barycentric(&vertices, 3), vec3f(1.0, 0.0, 0.0)); + assert_eq!(packed_barycentric(&vertices, 4), vec3f(0.0, 1.0, 0.0)); + assert_eq!(packed_barycentric(&vertices, 5), vec3f(0.0, 0.0, 1.0)); + } + + #[test] + fn debug_depth_mesh_view_plan_extracts_visible_chunk_meshes() { + let snapshot = make_flat_floor_snapshot(0.05); + let view_plan = + debug_depth_mesh_view_plan(&snapshot, Pose::new(Quat::default(), vec3f(0.0, 1.4, 0.0))); + + assert!( + !view_plan.visible_chunks.is_empty(), + "expected at least one visible debug chunk" + ); + + let mut triangulator = DebugDepthMeshTriangulator::default(); + let built_chunks: Vec<_> = view_plan + .visible_chunks + .iter() + .filter_map(|plan| triangulator.build_chunk(&snapshot, view_plan.layout, plan)) + .collect(); + + assert!( + !built_chunks.is_empty(), + "expected triangulated visible chunks for a flat floor snapshot" + ); + assert!(built_chunks + .iter() + .all(|chunk| !chunk.indices.is_empty() && !chunk.vertices.is_empty())); + } + + #[test] + fn debug_depth_mesh_includes_low_confidence_valid_voxels() { + let snapshot = make_flat_floor_snapshot_with_confidence(0.05, 1); + let view_plan = + debug_depth_mesh_view_plan(&snapshot, Pose::new(Quat::default(), vec3f(0.0, 1.4, 0.0))); + + let mut triangulator = DebugDepthMeshTriangulator::default(); + let built_chunks: Vec<_> = view_plan + .visible_chunks + .iter() + .filter_map(|plan| triangulator.build_chunk(&snapshot, view_plan.layout, plan)) + .collect(); + + assert!( + !built_chunks.is_empty(), + "expected triangulated visible chunks even from low-confidence valid TSDF voxels" + ); + } +} diff --git a/xr/src/util/depth_debug_mesh.rs b/xr/src/util/depth_debug_mesh.rs new file mode 100644 index 0000000000..9c379dc072 --- /dev/null +++ b/xr/src/util/depth_debug_mesh.rs @@ -0,0 +1,752 @@ +use crate::algorithms::tsdf_query::{depth_query_plane_quad, DepthQuerySupportPlane}; +use crate::prelude::*; +use std::{ + collections::hash_map::DefaultHasher, + hash::{Hash, Hasher}, + sync::Arc, +}; + +const XR_DEBUG_DEPTH_FLOATS_PER_VERTEX: usize = 8; +const XR_DEBUG_TSDF_MESH_CHUNK_WORLD_SIZE_METERS: f32 = 1.0; +const XR_DEBUG_TSDF_MESH_VIEW_DISTANCE_METERS: f32 = 5.5; +const XR_DEBUG_TSDF_MESH_NEAR_VISIBILITY_METERS: f32 = 1.35; +const XR_DEBUG_TSDF_MESH_VIEW_CONE_DOT: f32 = 0.309_016_88; +const XR_DEBUG_TSDF_MESH_CELL_STRIDE_VOXELS: i32 = 1; +const XR_DEBUG_TSDF_MESH_CHUNK_OVERLAP_CELLS: i32 = 2; +const XR_DEBUG_TSDF_DENSE_HOLE_FILL_MAX_PASSES: usize = 2; + +#[derive(Clone, Copy, Debug, Default, Eq, Hash, PartialEq)] +struct VoxelCoord { + x: i32, + y: i32, + z: i32, +} + +impl VoxelCoord { + const fn new(x: i32, y: i32, z: i32) -> Self { + Self { x, y, z } + } +} + +impl core::ops::Add for VoxelCoord { + type Output = Self; + + fn add(self, rhs: Self) -> Self::Output { + Self::new(self.x + rhs.x, self.y + rhs.y, self.z + rhs.z) + } +} + +impl core::ops::Sub for VoxelCoord { + type Output = Self; + + fn sub(self, rhs: Self) -> Self::Output { + Self::new(self.x - rhs.x, self.y - rhs.y, self.z - rhs.z) + } +} + +#[derive(Clone, Copy, Debug, PartialEq)] +pub(crate) struct DebugDepthMeshLayout { + pub(crate) chunk_edge_voxels: i32, + pub(crate) overlap_voxels: i32, + pub(crate) stride_voxels: i32, + pub(crate) chunk_world_size_meters: f32, + pub(crate) view_distance_meters: f32, + pub(crate) near_distance_meters: f32, + pub(crate) view_cone_dot: f32, +} + +#[derive(Clone, Debug, Default, PartialEq, Eq)] +pub(crate) struct DebugDepthMeshChunkSignature { + pub(crate) sources: Vec<(ChunkKey, usize)>, +} + +#[derive(Clone, Debug)] +pub(crate) struct DebugDepthMeshChunkPlan { + pub(crate) chunk_key: ChunkKey, + pub(crate) signature: DebugDepthMeshChunkSignature, +} + +#[derive(Clone, Debug)] +pub(crate) struct DebugDepthMeshViewPlan { + pub(crate) layout: DebugDepthMeshLayout, + pub(crate) visible_chunks: Vec, +} + +#[derive(Clone, Debug, Default, PartialEq)] +struct SurfaceMesh32 { + positions: Vec<[f32; 3]>, + indices: Vec, +} + +#[derive(Clone, Debug)] +pub(crate) struct DebugDepthMeshChunk { + pub(crate) chunk_key: ChunkKey, + pub(crate) fingerprint: u64, + pub(crate) indices: Vec, + pub(crate) vertices: Vec, +} + +#[derive(Default)] +pub(crate) struct DebugDepthMeshTriangulator { + dense: Vec, + fill_scratch: Vec, +} + +const SURFACE_NET_CORNERS: [VoxelCoord; 8] = [ + VoxelCoord::new(0, 0, 0), + VoxelCoord::new(1, 0, 0), + VoxelCoord::new(1, 0, 1), + VoxelCoord::new(0, 0, 1), + VoxelCoord::new(0, 1, 0), + VoxelCoord::new(1, 1, 0), + VoxelCoord::new(1, 1, 1), + VoxelCoord::new(0, 1, 1), +]; + +const SURFACE_NET_EDGES: [(usize, usize); 12] = [ + (0, 1), + (1, 2), + (2, 3), + (3, 0), + (4, 5), + (5, 6), + (6, 7), + (7, 4), + (0, 4), + (1, 5), + (2, 6), + (3, 7), +]; + +const DEBUG_DEPTH_TRIANGLE_BARYCENTRICS: [[f32; 3]; 3] = + [[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]]; + +fn quantize_f32(value: f32, quantum: f32) -> i32 { + (value / quantum.max(f32::EPSILON)).round() as i32 +} + +fn push_debug_depth_vertex(vertices: &mut Vec, position: Vec3f, barycentric: [f32; 3]) { + vertices.extend_from_slice(&[ + position.x, + position.y, + position.z, + 1.0, + barycentric[0], + barycentric[1], + barycentric[2], + 0.0, + ]); +} + +fn pack_surface_mesh_debug_vertices(mesh: &SurfaceMesh32) -> (Vec, Vec) { + let mut indices = Vec::with_capacity(mesh.indices.len()); + let mut vertices = Vec::with_capacity(mesh.indices.len() * XR_DEBUG_DEPTH_FLOATS_PER_VERTEX); + for triangle in mesh.indices.chunks_exact(3) { + let base = (vertices.len() / XR_DEBUG_DEPTH_FLOATS_PER_VERTEX) as u32; + for (corner, vertex_index) in triangle.iter().enumerate() { + let position = mesh.positions[*vertex_index as usize]; + push_debug_depth_vertex( + &mut vertices, + vec3f(position[0], position[1], position[2]), + DEBUG_DEPTH_TRIANGLE_BARYCENTRICS[corner], + ); + } + indices.extend_from_slice(&[base, base + 1, base + 2]); + } + (indices, vertices) +} + +fn push_debug_depth_triangle(indices: &mut Vec, vertices: &mut Vec, tri: [Vec3f; 3]) { + let base = (vertices.len() / XR_DEBUG_DEPTH_FLOATS_PER_VERTEX) as u32; + for (corner, position) in tri.into_iter().enumerate() { + push_debug_depth_vertex( + vertices, + position, + DEBUG_DEPTH_TRIANGLE_BARYCENTRICS[corner], + ); + } + indices.extend_from_slice(&[base, base + 1, base + 2]); +} + +fn push_debug_depth_quad(indices: &mut Vec, vertices: &mut Vec, quad: [Vec3f; 4]) { + push_debug_depth_triangle(indices, vertices, [quad[0], quad[1], quad[2]]); + push_debug_depth_triangle(indices, vertices, [quad[0], quad[2], quad[3]]); +} + +pub(crate) fn push_debug_depth_plane( + indices: &mut Vec, + vertices: &mut Vec, + plane: DepthQuerySupportPlane, +) { + push_debug_depth_quad(indices, vertices, depth_query_plane_quad(plane)); +} + +fn align_extent(extent: i32, stride: i32) -> i32 { + let stride = stride.max(1); + if extent <= 0 { + 0 + } else { + ((extent + stride - 1) / stride) * stride + } +} + +fn flatten_coord(coord: VoxelCoord, size: VoxelCoord) -> usize { + coord.x as usize + + coord.y as usize * size.x as usize + + coord.z as usize * size.x as usize * size.y as usize +} + +fn dense_corner_coord(base: VoxelCoord, corner: VoxelCoord, stride: i32) -> VoxelCoord { + VoxelCoord::new( + base.x * stride + corner.x * stride, + base.y * stride + corner.y * stride, + base.z * stride + corner.z * stride, + ) +} + +fn snapshot_meshing_distance(grid: &SparseTsdGridReadSnapshot, coord: VoxelCoord) -> Option { + let (chunk_key, id) = grid.chunk_key_and_local_index_xyz(coord.x, coord.y, coord.z); + let chunk = grid.chunks.get(&chunk_key)?; + chunk.value(id) +} + +fn extract_dense_region_into( + grid: &SparseTsdGridReadSnapshot, + start: VoxelCoord, + extent: VoxelCoord, + dense: &mut Vec, +) { + let sx = extent.x.max(0) as usize; + let sy = extent.y.max(0) as usize; + let sz = extent.z.max(0) as usize; + dense.clear(); + dense.resize(sx * sy * sz, f32::NEG_INFINITY); + for z in 0..extent.z.max(0) { + for y in 0..extent.y.max(0) { + for x in 0..extent.x.max(0) { + let coord = VoxelCoord::new(start.x + x, start.y + y, start.z + z); + let value = snapshot_meshing_distance(grid, coord).unwrap_or(f32::NEG_INFINITY); + dense[(x as usize) + (y as usize) * sx + (z as usize) * sx * sy] = value; + } + } + } +} + +fn repair_dense_meshing_holes(dense: &mut Vec, scratch: &mut Vec, extent: VoxelCoord) { + let sx = extent.x.max(0) as usize; + let sy = extent.y.max(0) as usize; + let sz = extent.z.max(0) as usize; + if sx < 3 || sy < 3 || sz < 3 || dense.len() != sx * sy * sz { + return; + } + + scratch.clear(); + scratch.resize(dense.len(), f32::NEG_INFINITY); + + for _ in 0..XR_DEBUG_TSDF_DENSE_HOLE_FILL_MAX_PASSES { + scratch.as_mut_slice().copy_from_slice(dense.as_slice()); + let mut changed = false; + + for z in 1..(sz - 1) { + for y in 1..(sy - 1) { + for x in 1..(sx - 1) { + let coord = VoxelCoord::new(x as i32, y as i32, z as i32); + let index = flatten_coord(coord, extent); + if dense[index].is_finite() { + continue; + } + + let mut pair_sum = 0.0f32; + let mut pair_count = 0usize; + let mut sign_vote = 0i32; + + for (a, b) in [ + (VoxelCoord::new(-1, 0, 0), VoxelCoord::new(1, 0, 0)), + (VoxelCoord::new(0, -1, 0), VoxelCoord::new(0, 1, 0)), + (VoxelCoord::new(0, 0, -1), VoxelCoord::new(0, 0, 1)), + ] { + let value_a = dense[flatten_coord(coord + a, extent)]; + let value_b = dense[flatten_coord(coord + b, extent)]; + if !value_a.is_finite() || !value_b.is_finite() { + continue; + } + pair_sum += value_a + value_b; + pair_count += 1; + sign_vote += if value_a + value_b >= 0.0 { 1 } else { -1 }; + } + + if pair_count < 2 { + continue; + } + + let filled = pair_sum / (pair_count as f32 * 2.0); + if !filled.is_finite() { + continue; + } + scratch[index] = if sign_vote < 0 { + filled.min(-1.0e-4) + } else if sign_vote > 0 { + filled.max(1.0e-4) + } else { + filled + }; + changed = true; + } + } + } + + if !changed { + break; + } + dense.as_mut_slice().copy_from_slice(scratch.as_slice()); + } +} + +fn scaled_voxel_count(voxel_count: VoxelCoord, stride: i32) -> Option { + if voxel_count.x <= 1 || voxel_count.y <= 1 || voxel_count.z <= 1 { + return None; + } + let stride = stride.max(1); + let scaled_count = VoxelCoord::new( + voxel_count.x.div_euclid(stride), + voxel_count.y.div_euclid(stride), + voxel_count.z.div_euclid(stride), + ); + (scaled_count.x > 1 && scaled_count.y > 1 && scaled_count.z > 1).then_some(scaled_count) +} + +fn surface_net_mesh_from_dense( + volume: &[f32], + voxel_count: VoxelCoord, + voxel_size: f32, + start_coord: VoxelCoord, + stride: i32, +) -> Option { + let stride = stride.max(1); + let scaled_count = scaled_voxel_count(voxel_count, stride)?; + + let sample_value = |coord: VoxelCoord| -> f32 { + let raw = volume[flatten_coord(coord, voxel_count)]; + if raw.is_finite() { + raw + } else { + 0.0 + } + }; + let raw_value = |coord: VoxelCoord| -> f32 { volume[flatten_coord(coord, voxel_count)] }; + + let mut positions = Vec::<[f32; 3]>::new(); + let mut indices = Vec::::new(); + let mut coord_vert_map = + vec![i32::MIN; (scaled_count.x * scaled_count.y * scaled_count.z) as usize]; + let mut vert_coords = Vec::::new(); + + for z in 0..scaled_count.z { + for y in 0..scaled_count.y { + for x in 0..scaled_count.x { + if x == scaled_count.x - 1 || y == scaled_count.y - 1 || z == scaled_count.z - 1 { + continue; + } + let coord = VoxelCoord::new(x, y, z); + let mut pos_coord = vec3f(0.0, 0.0, 0.0); + let mut crossings = 0u8; + let mut bad_crossings = 0u8; + + for (a_idx, b_idx) in SURFACE_NET_EDGES { + let coord_a = dense_corner_coord(coord, SURFACE_NET_CORNERS[a_idx], stride); + let coord_b = dense_corner_coord(coord, SURFACE_NET_CORNERS[b_idx], stride); + let value_a = sample_value(coord_a); + let value_b = sample_value(coord_b); + let change = value_a - value_b; + if (value_a < 0.0) == (value_b < 0.0) || change.abs() <= f32::EPSILON { + continue; + } + if !raw_value(coord_a).is_finite() || !raw_value(coord_b).is_finite() { + bad_crossings = bad_crossings.saturating_add(1); + } + let t = value_a / change; + pos_coord += vec3f( + coord_a.x as f32 + (coord_b.x - coord_a.x) as f32 * t, + coord_a.y as f32 + (coord_b.y - coord_a.y) as f32 * t, + coord_a.z as f32 + (coord_b.z - coord_a.z) as f32 * t, + ); + crossings = crossings.saturating_add(1); + } + + if crossings < 3 || crossings == bad_crossings { + continue; + } + + pos_coord = pos_coord.scale(1.0 / crossings as f32); + let world = vec3f( + (start_coord.x as f32 + pos_coord.x + 0.5) * voxel_size, + (start_coord.y as f32 + pos_coord.y + 0.5) * voxel_size, + (start_coord.z as f32 + pos_coord.z + 0.5) * voxel_size, + ); + let vertex_index = positions.len() as u32; + positions.push([world.x, world.y, world.z]); + coord_vert_map[flatten_coord(coord, scaled_count)] = vertex_index as i32; + vert_coords.push(coord); + } + } + } + + for coord in vert_coords { + surface_net_tris_for_axis( + &mut indices, + &coord_vert_map, + scaled_count, + &sample_value, + coord, + VoxelCoord::new(1, 0, 0), + VoxelCoord::new(0, 0, 1), + VoxelCoord::new(0, 1, 0), + stride, + ); + surface_net_tris_for_axis( + &mut indices, + &coord_vert_map, + scaled_count, + &sample_value, + coord, + VoxelCoord::new(0, 1, 0), + VoxelCoord::new(1, 0, 0), + VoxelCoord::new(0, 0, 1), + stride, + ); + surface_net_tris_for_axis( + &mut indices, + &coord_vert_map, + scaled_count, + &sample_value, + coord, + VoxelCoord::new(0, 0, 1), + VoxelCoord::new(0, 1, 0), + VoxelCoord::new(1, 0, 0), + stride, + ); + } + + if indices.is_empty() { + None + } else { + Some(SurfaceMesh32 { positions, indices }) + } +} + +fn surface_net_tris_for_axis( + indices: &mut Vec, + coord_vert_map: &[i32], + size: VoxelCoord, + sample_value: &impl Fn(VoxelCoord) -> f32, + coord: VoxelCoord, + axis: VoxelCoord, + d1: VoxelCoord, + d2: VoxelCoord, + stride: i32, +) { + if coord.x - d1.x < 0 + || coord.y - d1.y < 0 + || coord.z - d1.z < 0 + || coord.x - d2.x < 0 + || coord.y - d2.y < 0 + || coord.z - d2.z < 0 + { + return; + } + let scaled = VoxelCoord::new(coord.x * stride, coord.y * stride, coord.z * stride); + let value_a = sample_value(scaled); + let value_b = + sample_value(scaled + VoxelCoord::new(axis.x * stride, axis.y * stride, axis.z * stride)); + if (value_a < 0.0) == (value_b < 0.0) { + return; + } + let a = coord_vert_map[flatten_coord(coord, size)]; + let b = coord_vert_map[flatten_coord(coord - d1, size)]; + let c = coord_vert_map[flatten_coord(coord - d1 - d2, size)]; + let d = coord_vert_map[flatten_coord(coord - d2, size)]; + if a < 0 || b < 0 || c < 0 || d < 0 { + return; + } + let (a, b, c, d) = (a as u32, b as u32, c as u32, d as u32); + if value_a < 0.0 { + indices.extend_from_slice(&[c, b, a, d, c, a]); + } else { + indices.extend_from_slice(&[a, c, d, a, b, c]); + } +} + +fn mesh_chunk_extent(layout: DebugDepthMeshLayout) -> VoxelCoord { + let edge = layout.chunk_edge_voxels.max(1); + let overlap = layout.overlap_voxels.max(0); + VoxelCoord::new(edge + overlap, edge + overlap, edge + overlap) +} + +fn mesh_chunk_start_coord(chunk_key: ChunkKey, layout: DebugDepthMeshLayout) -> VoxelCoord { + let edge = layout.chunk_edge_voxels.max(1); + VoxelCoord::new(chunk_key.x * edge, chunk_key.y * edge, chunk_key.z * edge) +} + +fn mesh_chunk_world_bounds( + voxel_size: f32, + chunk_key: ChunkKey, + layout: DebugDepthMeshLayout, +) -> (Vec3f, Vec3f) { + let start = mesh_chunk_start_coord(chunk_key, layout); + let edge = layout.chunk_edge_voxels.max(1); + ( + vec3f( + start.x as f32 * voxel_size, + start.y as f32 * voxel_size, + start.z as f32 * voxel_size, + ), + vec3f( + (start.x + edge) as f32 * voxel_size, + (start.y + edge) as f32 * voxel_size, + (start.z + edge) as f32 * voxel_size, + ), + ) +} + +fn aabb_intersects(min_a: Vec3f, max_a: Vec3f, min_b: Vec3f, max_b: Vec3f) -> bool { + min_a.x <= max_b.x + && max_a.x >= min_b.x + && min_a.y <= max_b.y + && max_a.y >= min_b.y + && min_a.z <= max_b.z + && max_a.z >= min_b.z +} + +fn chunk_visible_from_head( + head_position: Vec3f, + head_forward: Vec3f, + world_min: Vec3f, + world_max: Vec3f, + layout: DebugDepthMeshLayout, +) -> bool { + let center = (world_min + world_max).scale(0.5); + let half_extent = (world_max - world_min).scale(0.5); + let radius = half_extent.length(); + let to_center = center - head_position; + let distance = to_center.length(); + if distance > layout.view_distance_meters + radius { + return false; + } + if distance <= layout.near_distance_meters + radius || distance <= 1.0e-4 { + return true; + } + let direction = to_center.scale(1.0 / distance); + direction.dot(head_forward) >= layout.view_cone_dot - (radius / distance).min(0.35) +} + +fn snapshot_region_signature( + grid: &SparseTsdGridReadSnapshot, + start: VoxelCoord, + extent: VoxelCoord, +) -> DebugDepthMeshChunkSignature { + if extent.x <= 0 || extent.y <= 0 || extent.z <= 0 || grid.chunks.is_empty() { + return DebugDepthMeshChunkSignature::default(); + } + let max = VoxelCoord::new( + start.x + extent.x.saturating_sub(1), + start.y + extent.y.saturating_sub(1), + start.z + extent.z.saturating_sub(1), + ); + let min_chunk = VoxelCoord::new( + start.x.div_euclid(grid.chunk_edge), + start.y.div_euclid(grid.chunk_edge), + start.z.div_euclid(grid.chunk_edge), + ); + let max_chunk = VoxelCoord::new( + max.x.div_euclid(grid.chunk_edge), + max.y.div_euclid(grid.chunk_edge), + max.z.div_euclid(grid.chunk_edge), + ); + let mut sources = Vec::new(); + for z in min_chunk.z..=max_chunk.z { + for y in min_chunk.y..=max_chunk.y { + for x in min_chunk.x..=max_chunk.x { + let key = ChunkKey::new(x, y, z); + let Some(chunk) = grid.chunks.get(&key) else { + continue; + }; + sources.push((key, Arc::as_ptr(chunk) as usize)); + } + } + } + DebugDepthMeshChunkSignature { sources } +} + +fn mesh_chunk_fingerprint(chunk_key: ChunkKey, mesh: &SurfaceMesh32) -> u64 { + let mut hasher = DefaultHasher::new(); + chunk_key.hash(&mut hasher); + for position in &mesh.positions { + quantize_f32(position[0], 0.01).hash(&mut hasher); + quantize_f32(position[1], 0.01).hash(&mut hasher); + quantize_f32(position[2], 0.01).hash(&mut hasher); + } + mesh.indices.hash(&mut hasher); + hasher.finish() +} + +pub(crate) fn snapshot_debug_mesh_layout(snapshot: &TsdfPublishedSnapshot) -> DebugDepthMeshLayout { + let voxel_size = snapshot.grid.voxel_size.max(1.0e-5); + let stride = XR_DEBUG_TSDF_MESH_CELL_STRIDE_VOXELS.max(1); + let approx_edge = (XR_DEBUG_TSDF_MESH_CHUNK_WORLD_SIZE_METERS / voxel_size).ceil() as i32; + let chunk_edge_voxels = align_extent(approx_edge.max(stride * 2), stride); + let overlap_voxels = stride * XR_DEBUG_TSDF_MESH_CHUNK_OVERLAP_CELLS.max(1); + DebugDepthMeshLayout { + chunk_edge_voxels, + overlap_voxels, + stride_voxels: stride, + chunk_world_size_meters: chunk_edge_voxels as f32 * voxel_size, + view_distance_meters: XR_DEBUG_TSDF_MESH_VIEW_DISTANCE_METERS, + near_distance_meters: XR_DEBUG_TSDF_MESH_NEAR_VISIBILITY_METERS, + view_cone_dot: XR_DEBUG_TSDF_MESH_VIEW_CONE_DOT, + } +} + +pub(crate) fn debug_depth_mesh_view_plan( + snapshot: &TsdfPublishedSnapshot, + head_pose: Pose, +) -> DebugDepthMeshViewPlan { + let layout = snapshot_debug_mesh_layout(snapshot); + let Some((active_min, active_max)) = snapshot.grid.active_bounds else { + return DebugDepthMeshViewPlan { + layout, + visible_chunks: Vec::new(), + }; + }; + + let mut head_forward = head_pose.orientation.rotate_vec3(&vec3f(0.0, 0.0, -1.0)); + if head_forward.length() > 1.0e-4 { + head_forward = head_forward.normalize(); + } else { + head_forward = vec3f(0.0, 0.0, -1.0); + } + + let (head_voxel_x, head_voxel_y, head_voxel_z) = + snapshot.grid.world_to_voxel_xyz(head_pose.position); + let head_chunk = VoxelCoord::new( + head_voxel_x.div_euclid(layout.chunk_edge_voxels.max(1)), + head_voxel_y.div_euclid(layout.chunk_edge_voxels.max(1)), + head_voxel_z.div_euclid(layout.chunk_edge_voxels.max(1)), + ); + let search_radius = + (layout.view_distance_meters / layout.chunk_world_size_meters.max(0.1)).ceil() as i32 + 1; + let extent = mesh_chunk_extent(layout); + + #[derive(Clone)] + struct VisibilityCandidate { + forward_depth: f32, + distance: f32, + plan: DebugDepthMeshChunkPlan, + } + + let mut candidates = Vec::::new(); + for z in (head_chunk.z - search_radius)..=(head_chunk.z + search_radius) { + for y in (head_chunk.y - search_radius)..=(head_chunk.y + search_radius) { + for x in (head_chunk.x - search_radius)..=(head_chunk.x + search_radius) { + let chunk_key = ChunkKey::new(x, y, z); + let (world_min, world_max) = + mesh_chunk_world_bounds(snapshot.grid.voxel_size, chunk_key, layout); + if !aabb_intersects(world_min, world_max, active_min, active_max) { + continue; + } + if !chunk_visible_from_head( + head_pose.position, + head_forward, + world_min, + world_max, + layout, + ) { + continue; + } + + let signature = snapshot_region_signature( + snapshot.grid.as_ref(), + mesh_chunk_start_coord(chunk_key, layout), + extent, + ); + if signature.sources.is_empty() { + continue; + } + + let center = (world_min + world_max).scale(0.5); + let to_center = center - head_pose.position; + candidates.push(VisibilityCandidate { + forward_depth: to_center.dot(head_forward), + distance: to_center.length(), + plan: DebugDepthMeshChunkPlan { + chunk_key, + signature, + }, + }); + } + } + } + + candidates.sort_by(|left, right| { + left.distance + .total_cmp(&right.distance) + .then_with(|| right.forward_depth.total_cmp(&left.forward_depth)) + .then_with(|| { + ( + left.plan.chunk_key.x, + left.plan.chunk_key.y, + left.plan.chunk_key.z, + ) + .cmp(&( + right.plan.chunk_key.x, + right.plan.chunk_key.y, + right.plan.chunk_key.z, + )) + }) + }); + + DebugDepthMeshViewPlan { + layout, + visible_chunks: candidates + .into_iter() + .map(|candidate| candidate.plan) + .collect(), + } +} + +impl DebugDepthMeshTriangulator { + pub(crate) fn build_chunk( + &mut self, + snapshot: &TsdfPublishedSnapshot, + layout: DebugDepthMeshLayout, + plan: &DebugDepthMeshChunkPlan, + ) -> Option { + if plan.signature.sources.is_empty() { + return None; + } + let start = mesh_chunk_start_coord(plan.chunk_key, layout); + let extent = mesh_chunk_extent(layout); + extract_dense_region_into(snapshot.grid.as_ref(), start, extent, &mut self.dense); + repair_dense_meshing_holes(&mut self.dense, &mut self.fill_scratch, extent); + let mesh = surface_net_mesh_from_dense( + &self.dense, + extent, + snapshot.grid.voxel_size, + start, + layout.stride_voxels, + )?; + let fingerprint = mesh_chunk_fingerprint(plan.chunk_key, &mesh); + let (indices, vertices) = pack_surface_mesh_debug_vertices(&mesh); + if indices.is_empty() || vertices.is_empty() { + return None; + } + Some(DebugDepthMeshChunk { + chunk_key: plan.chunk_key, + fingerprint, + indices, + vertices, + }) + } +} + +#[cfg(test)] +include!("../tests/util/depth_debug_mesh.rs"); diff --git a/xr/src/util/depth_debug_mesh_worker.rs b/xr/src/util/depth_debug_mesh_worker.rs new file mode 100644 index 0000000000..7fffdca33f --- /dev/null +++ b/xr/src/util/depth_debug_mesh_worker.rs @@ -0,0 +1,317 @@ +use super::depth_debug_mesh::{ + debug_depth_mesh_view_plan, DebugDepthMeshChunk, DebugDepthMeshChunkSignature, + DebugDepthMeshTriangulator, +}; +use crate::prelude::*; +use makepad_widgets::makepad_platform::{ + ChunkKey, SparseTsdGridReadSnapshot, TsdfPublishedSnapshot, +}; +use std::{ + collections::{HashMap, VecDeque}, + sync::{Arc, Condvar, Mutex}, + thread::{self, JoinHandle}, +}; + +const XR_DEPTH_DEBUG_MESH_RESULT_BATCH_CHUNKS: usize = 4; +const XR_DEPTH_DEBUG_MESH_RESULT_BATCH_REMOVALS: usize = 16; + +struct DepthDebugMeshWorkerRequest { + request_id: u64, + snapshot: Arc, + head_pose: Pose, +} + +#[derive(Default)] +struct DepthDebugMeshWorkerMailbox { + version: u64, + shutdown: bool, + pending_request: Option, +} + +pub(crate) struct XrDepthDebugMeshVisibleSet { + pub(crate) request_id: u64, + pub(crate) generation: u64, + pub(crate) update_sequence: u64, + pub(crate) snapshot_grid: Arc, + pub(crate) visible_chunk_keys: Vec, +} + +pub(crate) enum XrDepthDebugMeshWorkerResult { + VisibleSet(XrDepthDebugMeshVisibleSet), + ChunkUpserts { + request_id: u64, + chunks: Vec, + }, + ChunkRemovals { + request_id: u64, + chunk_keys: Vec, + }, +} + +#[derive(Default)] +struct CachedDebugDepthMeshChunk { + signature: DebugDepthMeshChunkSignature, + mesh: Option, + sent_to_ui: bool, +} + +struct CachedDebugDepthMeshChunkData { + fingerprint: u64, + indices: Vec, + vertices: Vec, +} + +impl CachedDebugDepthMeshChunkData { + fn from_chunk(chunk: DebugDepthMeshChunk) -> Self { + Self { + fingerprint: chunk.fingerprint, + indices: chunk.indices, + vertices: chunk.vertices, + } + } + + fn to_chunk(&self, chunk_key: ChunkKey) -> DebugDepthMeshChunk { + DebugDepthMeshChunk { + chunk_key, + fingerprint: self.fingerprint, + indices: self.indices.clone(), + vertices: self.vertices.clone(), + } + } +} + +pub(crate) struct XrDepthDebugMeshWorker { + mailbox: Arc<(Mutex, Condvar)>, + pending_results: Arc>>, + join_handle: Option>, + next_request_id: u64, +} + +impl XrDepthDebugMeshWorker { + pub(crate) fn new() -> Self { + let mailbox = Arc::new(( + Mutex::new(DepthDebugMeshWorkerMailbox::default()), + Condvar::new(), + )); + let pending_results = Arc::new(Mutex::new(VecDeque::new())); + let mailbox_thread = mailbox.clone(); + let results_thread = pending_results.clone(); + let join_handle = thread::Builder::new() + .name("makepad-xr-depth-debug-mesh".to_string()) + .spawn(move || depth_debug_mesh_worker_loop(mailbox_thread, results_thread)) + .ok(); + + Self { + mailbox, + pending_results, + join_handle, + next_request_id: 0, + } + } + + pub(crate) fn request_snapshot( + &mut self, + snapshot: Arc, + head_pose: Pose, + ) { + self.next_request_id = self.next_request_id.wrapping_add(1); + if self.next_request_id == 0 { + self.next_request_id = 1; + } + + let (lock, wake) = &*self.mailbox; + if let Ok(mut mailbox) = lock.lock() { + mailbox.pending_request = Some(DepthDebugMeshWorkerRequest { + request_id: self.next_request_id, + snapshot, + head_pose, + }); + mailbox.version = mailbox.version.saturating_add(1); + wake.notify_one(); + } + } + + pub(crate) fn take_next_result(&mut self) -> Option { + self.pending_results + .lock() + .ok() + .and_then(|mut results| results.pop_front()) + } +} + +impl Drop for XrDepthDebugMeshWorker { + fn drop(&mut self) { + let (lock, wake) = &*self.mailbox; + if let Ok(mut mailbox) = lock.lock() { + mailbox.shutdown = true; + mailbox.version = mailbox.version.saturating_add(1); + wake.notify_one(); + } + if let Some(join_handle) = self.join_handle.take() { + let _ = join_handle.join(); + } + } +} + +fn push_worker_result( + pending_results: &Mutex>, + result: XrDepthDebugMeshWorkerResult, +) -> bool { + if let Ok(mut pending_results) = pending_results.lock() { + pending_results.push_back(result); + true + } else { + false + } +} + +fn request_obsolete( + mailbox: &Arc<(Mutex, Condvar)>, + active_version: u64, +) -> bool { + let (lock, _) = &**mailbox; + lock.lock() + .map(|guard| guard.shutdown || guard.version != active_version) + .unwrap_or(true) +} + +fn depth_debug_mesh_worker_loop( + mailbox: Arc<(Mutex, Condvar)>, + pending_results: Arc>>, +) { + let mut seen_version = 0u64; + let mut triangulator = DebugDepthMeshTriangulator::default(); + let mut cache = HashMap::::new(); + + loop { + let (active_version, request) = { + let (lock, wake) = &*mailbox; + let mut guard = match lock.lock() { + Ok(guard) => guard, + Err(_) => return, + }; + while !guard.shutdown && guard.version == seen_version { + guard = match wake.wait(guard) { + Ok(guard) => guard, + Err(_) => return, + }; + } + if guard.shutdown { + return; + } + seen_version = guard.version; + (seen_version, guard.pending_request.take()) + }; + + let Some(request) = request else { + continue; + }; + let view_plan = debug_depth_mesh_view_plan(request.snapshot.as_ref(), request.head_pose); + if request_obsolete(&mailbox, active_version) { + continue; + } + + if !push_worker_result( + &pending_results, + XrDepthDebugMeshWorkerResult::VisibleSet(XrDepthDebugMeshVisibleSet { + request_id: request.request_id, + generation: request.snapshot.generation, + update_sequence: request.snapshot.update_sequence, + snapshot_grid: request.snapshot.grid.clone(), + visible_chunk_keys: view_plan + .visible_chunks + .iter() + .map(|plan| plan.chunk_key) + .collect(), + }), + ) { + return; + } + + let mut removal_batch = Vec::::new(); + let mut upsert_batch = Vec::::new(); + let flush_removals = |chunk_keys: &mut Vec| -> bool { + if chunk_keys.is_empty() { + return true; + } + push_worker_result( + &pending_results, + XrDepthDebugMeshWorkerResult::ChunkRemovals { + request_id: request.request_id, + chunk_keys: std::mem::take(chunk_keys), + }, + ) + }; + let flush_upserts = |chunks: &mut Vec| -> bool { + if chunks.is_empty() { + return true; + } + push_worker_result( + &pending_results, + XrDepthDebugMeshWorkerResult::ChunkUpserts { + request_id: request.request_id, + chunks: std::mem::take(chunks), + }, + ) + }; + + let mut aborted = false; + for plan in &view_plan.visible_chunks { + if request_obsolete(&mailbox, active_version) { + aborted = true; + break; + } + + let entry = cache.entry(plan.chunk_key).or_default(); + if entry.signature == plan.signature { + if !entry.sent_to_ui { + if let Some(mesh) = entry.mesh.as_ref() { + upsert_batch.push(mesh.to_chunk(plan.chunk_key)); + entry.sent_to_ui = true; + if upsert_batch.len() >= XR_DEPTH_DEBUG_MESH_RESULT_BATCH_CHUNKS + && !flush_upserts(&mut upsert_batch) + { + return; + } + } + } + continue; + } + + let was_sent_to_ui = entry.sent_to_ui; + entry.signature = plan.signature.clone(); + entry.mesh = triangulator + .build_chunk(request.snapshot.as_ref(), view_plan.layout, plan) + .map(CachedDebugDepthMeshChunkData::from_chunk); + if let Some(mesh) = entry.mesh.as_ref() { + upsert_batch.push(mesh.to_chunk(plan.chunk_key)); + entry.sent_to_ui = true; + if upsert_batch.len() >= XR_DEPTH_DEBUG_MESH_RESULT_BATCH_CHUNKS + && !flush_upserts(&mut upsert_batch) + { + return; + } + } else { + entry.sent_to_ui = false; + if was_sent_to_ui { + removal_batch.push(plan.chunk_key); + if removal_batch.len() >= XR_DEPTH_DEBUG_MESH_RESULT_BATCH_REMOVALS + && !flush_removals(&mut removal_batch) + { + return; + } + } + } + } + + if aborted { + if !flush_removals(&mut removal_batch) || !flush_upserts(&mut upsert_batch) { + return; + } + continue; + } + if !flush_removals(&mut removal_batch) || !flush_upserts(&mut upsert_batch) { + return; + } + } +} diff --git a/xr/src/util/gltf_bridge.rs b/xr/src/util/gltf_bridge.rs index 7fe48520d3..6b978d1d8a 100644 --- a/xr/src/util/gltf_bridge.rs +++ b/xr/src/util/gltf_bridge.rs @@ -1,14 +1,13 @@ -use crate::makepad_draw::shader::draw_pbr::PbrMeshHandle; -use crate::{ +use makepad_gltf::{ + decode_mesh_primitive, load_gltf_from_bytes, load_gltf_from_path, load_image_bytes, + DecodedPrimitive, GltfDocument, GltfError, GltfNode, LoadedGltf, +}; +use makepad_widgets::{ image_cache::{ handle_image_cache_network_responses, load_image_from_cache, load_image_from_data_async, process_async_image_load, AsyncImageLoad, AsyncLoadResult, ImageError, }, - makepad_draw::*, -}; -use makepad_gltf::{ - decode_mesh_primitive, load_gltf_from_bytes, load_gltf_from_path, load_image_bytes, - DecodedPrimitive, GltfDocument, GltfError, GltfNode, LoadedGltf, + makepad_draw::{shader::draw_pbr::PbrMeshHandle, *}, }; use std::{ collections::HashMap, diff --git a/xr/src/util/mod.rs b/xr/src/util/mod.rs new file mode 100644 index 0000000000..9db39ba80f --- /dev/null +++ b/xr/src/util/mod.rs @@ -0,0 +1,6 @@ +pub mod depth_debug_mesh; +pub(crate) mod depth_debug_mesh_worker; +pub mod gltf_bridge; +pub mod mesh_generators; +pub mod passthrough_env; +pub(crate) mod scene_draw; diff --git a/xr/src/util/scene_draw.rs b/xr/src/util/scene_draw.rs index 1e32fbeb41..261ddb1068 100644 --- a/xr/src/util/scene_draw.rs +++ b/xr/src/util/scene_draw.rs @@ -1,6 +1,6 @@ -use crate::makepad_draw::*; +use makepad_widgets::makepad_draw::*; -pub use crate::makepad_draw::SceneState3D; +pub use makepad_widgets::makepad_draw::SceneState3D; pub fn scene_state_from_cx(cx: &mut Cx3d) -> Option { cx.scene_state_3d() diff --git a/xr/src/xr_net.rs b/xr/src/xr_net.rs deleted file mode 100644 index a2a12066ff..0000000000 --- a/xr/src/xr_net.rs +++ /dev/null @@ -1,1775 +0,0 @@ -use crate::*; -use makepad_widgets::makepad_platform::makepad_micro_serde::*; -#[cfg(test)] -use makepad_widgets::makepad_platform::XrDepthAlignWallFeature; -use std::{ - collections::HashMap, - io::{self, Read, Write}, - net::{IpAddr, Ipv4Addr, Shutdown, SocketAddr, TcpListener, TcpStream, UdpSocket}, - sync::{ - atomic::{AtomicBool, Ordering}, - mpsc, Arc, - }, - thread::{self, JoinHandle}, - time::{Duration, Instant, SystemTime, UNIX_EPOCH}, -}; - -pub const XR_NET_PROTOCOL_VERSION: u16 = 2; -pub const XR_NET_DEFAULT_DISCOVERY_PORT: u16 = 41546; -pub const XR_NET_DEFAULT_DATA_PORT: u16 = 41547; -pub const XR_NET_DEFAULT_SYNC_PORT: u16 = 41548; - -const XR_NET_DEFAULT_DISCOVERY_INTERVAL: Duration = Duration::from_millis(100); -const XR_NET_DEFAULT_POLL_INTERVAL: Duration = Duration::from_millis(10); -const XR_NET_DEFAULT_PEER_TIMEOUT: Duration = Duration::from_secs(2); -const XR_NET_DEFAULT_SYNC_CONNECT_RETRY: Duration = Duration::from_millis(250); -const XR_NET_SYNC_CONNECT_TIMEOUT: Duration = Duration::from_millis(120); -const XR_NET_SYNC_MAX_FRAME_BYTES: usize = 256 * 1024; - -#[derive(Clone, Copy, Debug, Default, PartialEq, Eq, Hash, SerBin, DeBin)] -pub struct XrNetPeerId(pub u64); - -impl XrNetPeerId { - pub fn to_live_id(self) -> LiveId { - LiveId(self.0) - } -} - -#[derive(Clone, Copy, Debug, PartialEq, Eq, Hash)] -pub struct XrNetPeer { - pub id: XrNetPeerId, - pub addr: SocketAddr, -} - -impl XrNetPeer { - pub fn to_live_id(self) -> LiveId { - self.id.to_live_id() - } -} - -#[derive(Clone, Copy, Debug, PartialEq, Eq)] -pub enum XrNetLeaveReason { - Explicit, - Timeout, -} - -#[derive(Clone, Debug, SerBin, DeBin)] -pub struct XrNetStateFrame { - pub seq: u32, - pub sent_at: f64, - pub state: XrState, -} - -#[derive(Clone, Copy, Debug, Default, PartialEq, SerBin, DeBin)] -pub struct XrNetAlignmentFrame { - pub seq: u32, - pub sent_at: f64, - pub anchor: XrAnchor, - pub confidence: f32, -} - -impl XrNetAlignmentFrame { - pub fn remote_to_local_transform( - local: &XrNetAlignmentFrame, - remote: &XrNetAlignmentFrame, - ) -> Option { - if local.confidence <= 0.0 || remote.confidence <= 0.0 { - return None; - } - Some(Mat4f::mul( - &local.anchor.to_pose().to_mat4(), - &remote.anchor.to_pose().to_mat4().invert(), - )) - } -} - -#[derive(Clone, Debug, Default, PartialEq, SerBin, DeBin)] -pub struct XrNetAlignmentDescriptorFrame { - pub seq: u32, - pub sent_at: f64, - pub descriptor: XrDepthAlignDescriptor, -} - -impl XrNetAlignmentDescriptorFrame { - pub fn from_depth_mesh(depth_mesh: &XrDepthMesh, sent_at: f64) -> Option { - Some(Self { - seq: 0, - sent_at, - descriptor: depth_mesh.alignment_descriptor.clone()?, - }) - } - - pub fn transformed(&self, transform: &Mat4f) -> Self { - Self { - descriptor: xr_depth_align_transform_descriptor(&self.descriptor, transform), - ..self.clone() - } - } - - pub fn test_markers(&self) -> Option<[Vec3f; 2]> { - xr_depth_align_test_markers(&self.descriptor) - } - - pub fn solve_remote_to_local( - local: &XrNetAlignmentDescriptorFrame, - remote: &XrNetAlignmentDescriptorFrame, - ) -> Option { - xr_depth_align_solve_remote_to_local(&local.descriptor, &remote.descriptor) - } -} - -pub type XrNetAlignmentSolution = XrDepthAlignSolution; - -#[derive(Clone, Debug)] -pub enum XrNetIncoming { - Join { - peer: XrNetPeer, - }, - Leave { - peer: XrNetPeer, - reason: XrNetLeaveReason, - }, - State { - peer: XrNetPeer, - frame: XrNetStateFrame, - }, - Alignment { - peer: XrNetPeer, - frame: XrNetAlignmentFrame, - }, - AlignmentDescriptor { - peer: XrNetPeer, - frame: XrNetAlignmentDescriptorFrame, - }, -} - -#[derive(Clone, Debug)] -pub struct XrNetConfig { - pub node_id: XrNetPeerId, - pub discovery_bind: SocketAddr, - pub data_bind: SocketAddr, - pub sync_bind: SocketAddr, - pub discovery_targets: Vec, - pub discovery_interval: Duration, - pub peer_timeout: Duration, - pub poll_interval: Duration, -} - -impl Default for XrNetConfig { - fn default() -> Self { - Self { - node_id: XrNetPeerId(default_node_id()), - discovery_bind: SocketAddr::new( - IpAddr::V4(Ipv4Addr::UNSPECIFIED), - XR_NET_DEFAULT_DISCOVERY_PORT, - ), - data_bind: SocketAddr::new(IpAddr::V4(Ipv4Addr::UNSPECIFIED), XR_NET_DEFAULT_DATA_PORT), - sync_bind: SocketAddr::new(IpAddr::V4(Ipv4Addr::UNSPECIFIED), XR_NET_DEFAULT_SYNC_PORT), - discovery_targets: vec![SocketAddr::new( - IpAddr::V4(Ipv4Addr::new(255, 255, 255, 255)), - XR_NET_DEFAULT_DISCOVERY_PORT, - )], - discovery_interval: XR_NET_DEFAULT_DISCOVERY_INTERVAL, - peer_timeout: XR_NET_DEFAULT_PEER_TIMEOUT, - poll_interval: XR_NET_DEFAULT_POLL_INTERVAL, - } - } -} - -#[derive(Clone, Debug, SerBin, DeBin)] -struct XrNetDiscoveryHello { - version: u16, - node_id: XrNetPeerId, - data_port: u16, - sync_port: u16, -} - -#[derive(Clone, Debug, SerBin, DeBin)] -struct XrNetLeavePacket { - version: u16, - node_id: XrNetPeerId, -} - -#[derive(Clone, Debug, SerBin, DeBin)] -enum XrNetDiscoveryPacket { - Hello(XrNetDiscoveryHello), - Leave(XrNetLeavePacket), -} - -#[derive(Clone, Debug, SerBin, DeBin)] -struct XrNetSyncHello { - version: u16, - node_id: XrNetPeerId, -} - -#[derive(Clone, Debug, SerBin, DeBin)] -enum XrNetSyncPacket { - Hello(XrNetSyncHello), - Data(XrNetDataPacket), -} - -#[derive(Clone, Debug, SerBin, DeBin)] -enum XrNetDataPacket { - State { - version: u16, - node_id: XrNetPeerId, - frame: XrNetStateFrame, - }, - Alignment { - version: u16, - node_id: XrNetPeerId, - frame: XrNetAlignmentFrame, - }, - AlignmentDescriptor { - version: u16, - node_id: XrNetPeerId, - frame: XrNetAlignmentDescriptorFrame, - }, - Leave(XrNetLeavePacket), -} - -#[derive(Debug)] -enum XrNetOutgoing { - State(XrNetStateFrame), - Alignment(XrNetAlignmentFrame), - AlignmentDescriptor(XrNetAlignmentDescriptorFrame), - Break, -} - -struct WorkerPeerState { - peer: XrNetPeer, - sync_addr: SocketAddr, - last_seen: Instant, - last_state_seq: Option, - last_alignment_seq: Option, - last_alignment_descriptor_seq: Option, - sync_connection: Option, - next_sync_connect_attempt_at: Instant, -} - -impl WorkerPeerState { - fn new(peer: XrNetPeer, sync_addr: SocketAddr, now: Instant) -> Self { - Self { - peer, - sync_addr, - last_seen: now, - last_state_seq: None, - last_alignment_seq: None, - last_alignment_descriptor_seq: None, - sync_connection: None, - next_sync_connect_attempt_at: now, - } - } -} - -struct XrNetSyncConnection { - stream: TcpStream, - peer_id: Option, - read_buf: Vec, - write_buf: Vec, - handshake_received: bool, -} - -impl XrNetSyncConnection { - fn new(stream: TcpStream, peer_id: Option) -> io::Result { - stream.set_nonblocking(true)?; - let _ = stream.set_nodelay(true); - Ok(Self { - stream, - peer_id, - read_buf: Vec::new(), - write_buf: Vec::new(), - handshake_received: false, - }) - } - - fn queue_packet(&mut self, packet: &XrNetSyncPacket) { - let payload = packet.serialize_bin(); - let frame_len = payload.len().min(u32::MAX as usize) as u32; - self.write_buf.extend_from_slice(&frame_len.to_le_bytes()); - self.write_buf - .extend_from_slice(&payload[..frame_len as usize]); - } - - fn queue_hello(&mut self, node_id: XrNetPeerId) { - self.queue_packet(&XrNetSyncPacket::Hello(XrNetSyncHello { - version: XR_NET_PROTOCOL_VERSION, - node_id, - })); - } - - fn shutdown(&mut self) { - let _ = self.stream.shutdown(Shutdown::Both); - } -} - -#[derive(Debug)] -pub struct XrNetNode { - pub incoming_receiver: mpsc::Receiver, - outgoing_sender: mpsc::Sender, - thread_loop: Arc, - worker_thread: Option>, - next_state_seq: u32, - next_alignment_seq: u32, - next_alignment_descriptor_seq: u32, -} - -impl XrNetNode { - pub fn new() -> io::Result { - Self::with_config(XrNetConfig::default()) - } - - pub fn with_config(config: XrNetConfig) -> io::Result { - let discovery_socket = UdpSocket::bind(config.discovery_bind)?; - discovery_socket.set_nonblocking(true)?; - discovery_socket.set_broadcast(true)?; - - let data_socket = UdpSocket::bind(config.data_bind)?; - data_socket.set_nonblocking(true)?; - - let sync_listener = TcpListener::bind(config.sync_bind)?; - sync_listener.set_nonblocking(true)?; - - let bound_data_addr = data_socket.local_addr()?; - let bound_sync_addr = sync_listener.local_addr()?; - let (incoming_sender, incoming_receiver) = mpsc::channel(); - let (outgoing_sender, outgoing_receiver) = mpsc::channel(); - let thread_loop = Arc::new(AtomicBool::new(true)); - let worker_thread = Some(spawn_worker_thread( - config, - bound_data_addr.port(), - bound_sync_addr.port(), - discovery_socket, - data_socket, - sync_listener, - incoming_sender, - outgoing_receiver, - thread_loop.clone(), - )); - - Ok(Self { - incoming_receiver, - outgoing_sender, - thread_loop, - worker_thread, - next_state_seq: 0, - next_alignment_seq: 0, - next_alignment_descriptor_seq: 0, - }) - } - - pub fn send_state(&mut self, state: XrState) { - let frame = XrNetStateFrame { - seq: self.next_state_seq, - sent_at: state.time, - state, - }; - self.next_state_seq = self.next_state_seq.wrapping_add(1); - let _ = self.outgoing_sender.send(XrNetOutgoing::State(frame)); - } - - pub fn send_alignment(&mut self, anchor: XrAnchor, confidence: f32, sent_at: f64) { - let frame = XrNetAlignmentFrame { - seq: self.next_alignment_seq, - sent_at, - anchor, - confidence, - }; - self.next_alignment_seq = self.next_alignment_seq.wrapping_add(1); - let _ = self.outgoing_sender.send(XrNetOutgoing::Alignment(frame)); - } - - pub fn send_alignment_descriptor(&mut self, mut frame: XrNetAlignmentDescriptorFrame) { - frame.seq = self.next_alignment_descriptor_seq; - self.next_alignment_descriptor_seq = self.next_alignment_descriptor_seq.wrapping_add(1); - let _ = self - .outgoing_sender - .send(XrNetOutgoing::AlignmentDescriptor(frame)); - } -} - -impl Drop for XrNetNode { - fn drop(&mut self) { - self.thread_loop.store(false, Ordering::Relaxed); - let _ = self.outgoing_sender.send(XrNetOutgoing::Break); - if let Some(worker_thread) = self.worker_thread.take() { - let _ = worker_thread.join(); - } - } -} - -fn spawn_worker_thread( - config: XrNetConfig, - bound_data_port: u16, - bound_sync_port: u16, - discovery_socket: UdpSocket, - data_socket: UdpSocket, - sync_listener: TcpListener, - incoming_sender: mpsc::Sender, - outgoing_receiver: mpsc::Receiver, - thread_loop: Arc, -) -> JoinHandle<()> { - thread::spawn(move || { - let mut peers = HashMap::::new(); - let mut pending_sync_connections = Vec::::new(); - let mut cached_state = None; - let mut cached_alignment = None; - let mut cached_alignment_descriptor = None; - let mut next_discovery_at = Instant::now(); - let mut read_buf = [0u8; 65536]; - - while thread_loop.load(Ordering::Relaxed) { - let now = Instant::now(); - if now >= next_discovery_at { - send_discovery_hello( - &discovery_socket, - &config.discovery_targets, - config.node_id, - bound_data_port, - bound_sync_port, - ); - next_discovery_at = now + config.discovery_interval; - } - - let should_break = drain_outgoing_messages( - &data_socket, - &mut peers, - config.node_id, - &outgoing_receiver, - &mut cached_state, - &mut cached_alignment, - &mut cached_alignment_descriptor, - ); - process_discovery_packets( - &discovery_socket, - &data_socket, - &mut read_buf, - &mut peers, - &incoming_sender, - config.node_id, - &cached_state, - &cached_alignment, - &cached_alignment_descriptor, - ); - accept_sync_connections( - &sync_listener, - &mut pending_sync_connections, - config.node_id, - ); - ensure_outbound_sync_connections(&mut peers, config.node_id); - process_pending_sync_connections( - &mut pending_sync_connections, - &mut peers, - &incoming_sender, - config.node_id, - &cached_alignment, - &cached_alignment_descriptor, - ); - process_sync_connections( - &mut peers, - &incoming_sender, - config.node_id, - &cached_alignment, - &cached_alignment_descriptor, - ); - process_data_packets( - &data_socket, - &mut read_buf, - &mut peers, - &incoming_sender, - config.node_id, - ); - expire_timed_out_peers(&mut peers, &incoming_sender, config.peer_timeout); - - if should_break { - break; - } - thread::sleep(config.poll_interval); - } - - broadcast_explicit_leave(&data_socket, &peers, config.node_id); - broadcast_discovery_leave(&discovery_socket, &config.discovery_targets, config.node_id); - for peer_state in peers.values_mut() { - close_sync_connection(peer_state); - } - for pending in &mut pending_sync_connections { - pending.shutdown(); - } - }) -} - -fn send_discovery_hello( - socket: &UdpSocket, - discovery_targets: &[SocketAddr], - node_id: XrNetPeerId, - data_port: u16, - sync_port: u16, -) { - let packet = XrNetDiscoveryPacket::Hello(XrNetDiscoveryHello { - version: XR_NET_PROTOCOL_VERSION, - node_id, - data_port, - sync_port, - }); - let buf = packet.serialize_bin(); - for target in discovery_targets { - let _ = socket.send_to(&buf, target); - } -} - -fn broadcast_discovery_leave( - socket: &UdpSocket, - discovery_targets: &[SocketAddr], - node_id: XrNetPeerId, -) { - let packet = XrNetDiscoveryPacket::Leave(XrNetLeavePacket { - version: XR_NET_PROTOCOL_VERSION, - node_id, - }); - let buf = packet.serialize_bin(); - for target in discovery_targets { - let _ = socket.send_to(&buf, target); - } -} - -fn broadcast_explicit_leave( - socket: &UdpSocket, - peers: &HashMap, - node_id: XrNetPeerId, -) { - let packet = XrNetDataPacket::Leave(XrNetLeavePacket { - version: XR_NET_PROTOCOL_VERSION, - node_id, - }); - let buf = packet.serialize_bin(); - for peer in peers.values() { - let _ = socket.send_to(&buf, peer.peer.addr); - } -} - -fn drain_outgoing_messages( - socket: &UdpSocket, - peers: &mut HashMap, - node_id: XrNetPeerId, - outgoing_receiver: &mpsc::Receiver, - cached_state: &mut Option, - cached_alignment: &mut Option, - cached_alignment_descriptor: &mut Option, -) -> bool { - loop { - match outgoing_receiver.try_recv() { - Ok(XrNetOutgoing::State(frame)) => { - *cached_state = Some(frame.clone()); - let packet = XrNetDataPacket::State { - version: XR_NET_PROTOCOL_VERSION, - node_id, - frame, - }; - let buf = packet.serialize_bin(); - for peer in peers.values() { - let _ = socket.send_to(&buf, peer.peer.addr); - } - } - Ok(XrNetOutgoing::Alignment(frame)) => { - *cached_alignment = Some(frame); - let packet = XrNetDataPacket::Alignment { - version: XR_NET_PROTOCOL_VERSION, - node_id, - frame, - }; - for peer_state in peers.values_mut() { - if let Some(connection) = peer_state.sync_connection.as_mut() { - connection.queue_packet(&XrNetSyncPacket::Data(packet.clone())); - } - } - } - Ok(XrNetOutgoing::AlignmentDescriptor(frame)) => { - *cached_alignment_descriptor = Some(frame.clone()); - let packet = XrNetDataPacket::AlignmentDescriptor { - version: XR_NET_PROTOCOL_VERSION, - node_id, - frame, - }; - for peer_state in peers.values_mut() { - if let Some(connection) = peer_state.sync_connection.as_mut() { - connection.queue_packet(&XrNetSyncPacket::Data(packet.clone())); - } - } - } - Ok(XrNetOutgoing::Break) => return true, - Err(mpsc::TryRecvError::Empty) => return false, - Err(mpsc::TryRecvError::Disconnected) => return true, - } - } -} - -fn process_discovery_packets( - discovery_socket: &UdpSocket, - data_socket: &UdpSocket, - read_buf: &mut [u8], - peers: &mut HashMap, - incoming_sender: &mpsc::Sender, - node_id: XrNetPeerId, - cached_state: &Option, - _cached_alignment: &Option, - _cached_alignment_descriptor: &Option, -) { - loop { - match discovery_socket.recv_from(read_buf) { - Ok((len, source_addr)) => { - let Ok(packet) = XrNetDiscoveryPacket::deserialize_bin(&read_buf[..len]) else { - continue; - }; - match packet { - XrNetDiscoveryPacket::Hello(packet) => { - if packet.version != XR_NET_PROTOCOL_VERSION || packet.node_id == node_id { - continue; - } - let peer_addr = SocketAddr::new(source_addr.ip(), packet.data_port); - let peer_sync_addr = SocketAddr::new(source_addr.ip(), packet.sync_port); - let (peer, is_new) = touch_peer( - peers, - incoming_sender, - packet.node_id, - peer_addr, - peer_sync_addr, - ); - if is_new { - send_cached_state_to_peer(data_socket, peer, node_id, cached_state); - } - } - XrNetDiscoveryPacket::Leave(packet) => { - if packet.version != XR_NET_PROTOCOL_VERSION || packet.node_id == node_id { - continue; - } - remove_peer( - peers, - incoming_sender, - packet.node_id, - XrNetLeaveReason::Explicit, - ); - } - } - } - Err(err) if err.kind() == io::ErrorKind::WouldBlock => return, - Err(_) => return, - } - } -} - -fn process_data_packets( - socket: &UdpSocket, - read_buf: &mut [u8], - peers: &mut HashMap, - incoming_sender: &mpsc::Sender, - node_id: XrNetPeerId, -) { - loop { - match socket.recv_from(read_buf) { - Ok((len, source_addr)) => { - let Ok(packet) = XrNetDataPacket::deserialize_bin(&read_buf[..len]) else { - continue; - }; - match packet { - XrNetDataPacket::State { - version, - node_id: remote_id, - frame, - } => { - if version != XR_NET_PROTOCOL_VERSION || remote_id == node_id { - continue; - } - let sync_addr = peers - .get(&remote_id) - .map(|peer_state| peer_state.sync_addr) - .unwrap_or_else(|| { - SocketAddr::new(source_addr.ip(), XR_NET_DEFAULT_SYNC_PORT) - }); - let (peer, _) = - touch_peer(peers, incoming_sender, remote_id, source_addr, sync_addr); - let Some(peer_state) = peers.get_mut(&remote_id) else { - continue; - }; - if !accept_seq(peer_state.last_state_seq, frame.seq) { - continue; - } - peer_state.last_state_seq = Some(frame.seq); - let _ = incoming_sender.send(XrNetIncoming::State { peer, frame }); - } - XrNetDataPacket::Alignment { - version, - node_id: remote_id, - frame, - } => { - if version != XR_NET_PROTOCOL_VERSION || remote_id == node_id { - continue; - } - let sync_addr = peers - .get(&remote_id) - .map(|peer_state| peer_state.sync_addr) - .unwrap_or_else(|| { - SocketAddr::new(source_addr.ip(), XR_NET_DEFAULT_SYNC_PORT) - }); - let (peer, _) = - touch_peer(peers, incoming_sender, remote_id, source_addr, sync_addr); - let Some(peer_state) = peers.get_mut(&remote_id) else { - continue; - }; - if !accept_seq(peer_state.last_alignment_seq, frame.seq) { - continue; - } - peer_state.last_alignment_seq = Some(frame.seq); - let _ = incoming_sender.send(XrNetIncoming::Alignment { peer, frame }); - } - XrNetDataPacket::AlignmentDescriptor { - version, - node_id: remote_id, - frame, - } => { - if version != XR_NET_PROTOCOL_VERSION || remote_id == node_id { - continue; - } - let sync_addr = peers - .get(&remote_id) - .map(|peer_state| peer_state.sync_addr) - .unwrap_or_else(|| { - SocketAddr::new(source_addr.ip(), XR_NET_DEFAULT_SYNC_PORT) - }); - let (peer, _) = - touch_peer(peers, incoming_sender, remote_id, source_addr, sync_addr); - let Some(peer_state) = peers.get_mut(&remote_id) else { - continue; - }; - if !accept_seq(peer_state.last_alignment_descriptor_seq, frame.seq) { - continue; - } - peer_state.last_alignment_descriptor_seq = Some(frame.seq); - let _ = incoming_sender - .send(XrNetIncoming::AlignmentDescriptor { peer, frame }); - } - XrNetDataPacket::Leave(packet) => { - if packet.version != XR_NET_PROTOCOL_VERSION || packet.node_id == node_id { - continue; - } - remove_peer( - peers, - incoming_sender, - packet.node_id, - XrNetLeaveReason::Explicit, - ); - } - } - } - Err(err) if err.kind() == io::ErrorKind::WouldBlock => return, - Err(_) => return, - } - } -} - -fn touch_peer( - peers: &mut HashMap, - incoming_sender: &mpsc::Sender, - peer_id: XrNetPeerId, - addr: SocketAddr, - sync_addr: SocketAddr, -) -> (XrNetPeer, bool) { - let now = Instant::now(); - if let Some(peer_state) = peers.get_mut(&peer_id) { - let sync_addr_changed = peer_state.sync_addr != sync_addr; - peer_state.peer.addr = addr; - peer_state.sync_addr = sync_addr; - peer_state.last_seen = now; - if sync_addr_changed { - close_sync_connection(peer_state); - peer_state.next_sync_connect_attempt_at = now; - } - (peer_state.peer, false) - } else { - let peer = XrNetPeer { id: peer_id, addr }; - peers.insert(peer_id, WorkerPeerState::new(peer, sync_addr, now)); - let _ = incoming_sender.send(XrNetIncoming::Join { peer }); - (peer, true) - } -} - -fn send_cached_state_to_peer( - socket: &UdpSocket, - peer: XrNetPeer, - node_id: XrNetPeerId, - cached_state: &Option, -) { - if let Some(frame) = cached_state { - let packet = XrNetDataPacket::State { - version: XR_NET_PROTOCOL_VERSION, - node_id, - frame: frame.clone(), - }; - let _ = socket.send_to(&packet.serialize_bin(), peer.addr); - } -} - -fn close_sync_connection(peer_state: &mut WorkerPeerState) { - if let Some(connection) = peer_state.sync_connection.as_mut() { - connection.shutdown(); - } - peer_state.sync_connection = None; -} - -fn should_initiate_sync_connection(local_node_id: XrNetPeerId, peer_id: XrNetPeerId) -> bool { - local_node_id.0 < peer_id.0 -} - -fn accept_sync_connections( - sync_listener: &TcpListener, - pending_sync_connections: &mut Vec, - node_id: XrNetPeerId, -) { - loop { - match sync_listener.accept() { - Ok((stream, _)) => { - let Ok(mut connection) = XrNetSyncConnection::new(stream, None) else { - continue; - }; - connection.queue_hello(node_id); - pending_sync_connections.push(connection); - } - Err(err) if err.kind() == io::ErrorKind::WouldBlock => return, - Err(_) => return, - } - } -} - -fn ensure_outbound_sync_connections( - peers: &mut HashMap, - node_id: XrNetPeerId, -) { - let now = Instant::now(); - let peer_ids = peers.keys().copied().collect::>(); - for peer_id in peer_ids { - let Some(peer_state) = peers.get_mut(&peer_id) else { - continue; - }; - if peer_state.sync_connection.is_some() - || !should_initiate_sync_connection(node_id, peer_id) - || now < peer_state.next_sync_connect_attempt_at - { - continue; - } - peer_state.next_sync_connect_attempt_at = now + XR_NET_DEFAULT_SYNC_CONNECT_RETRY; - let Ok(stream) = - TcpStream::connect_timeout(&peer_state.sync_addr, XR_NET_SYNC_CONNECT_TIMEOUT) - else { - continue; - }; - let Ok(mut connection) = XrNetSyncConnection::new(stream, Some(peer_id)) else { - continue; - }; - connection.queue_hello(node_id); - peer_state.sync_connection = Some(connection); - } -} - -fn process_pending_sync_connections( - pending_sync_connections: &mut Vec, - peers: &mut HashMap, - incoming_sender: &mpsc::Sender, - node_id: XrNetPeerId, - cached_alignment: &Option, - cached_alignment_descriptor: &Option, -) { - let pending_count = pending_sync_connections.len(); - for _ in 0..pending_count { - let mut connection = pending_sync_connections.swap_remove(0); - let Ok(packets) = pump_sync_connection(&mut connection) else { - connection.shutdown(); - continue; - }; - let mut queued_data = Vec::::new(); - let mut invalid = false; - - for packet in packets { - match packet { - XrNetSyncPacket::Hello(hello) => { - if hello.version != XR_NET_PROTOCOL_VERSION || hello.node_id == node_id { - invalid = true; - break; - } - if connection - .peer_id - .is_some_and(|existing| existing != hello.node_id) - { - invalid = true; - break; - } - connection.peer_id = Some(hello.node_id); - connection.handshake_received = true; - } - XrNetSyncPacket::Data(data) => { - if !connection.handshake_received { - invalid = true; - break; - } - queued_data.push(data); - } - } - } - - if invalid { - connection.shutdown(); - continue; - } - - let Some(peer_id) = connection.peer_id else { - pending_sync_connections.push(connection); - continue; - }; - let Some(peer_state) = peers.get_mut(&peer_id) else { - pending_sync_connections.push(connection); - continue; - }; - - close_sync_connection(peer_state); - connection.handshake_received = true; - send_cached_sync_frames_to_connection( - &mut connection, - node_id, - cached_alignment, - cached_alignment_descriptor, - ); - peer_state.sync_connection = Some(connection); - - if let Some(peer_state) = peers.get_mut(&peer_id) { - for data in queued_data { - route_sync_data_packet(peer_state, incoming_sender, node_id, data); - } - } - } -} - -fn process_sync_connections( - peers: &mut HashMap, - incoming_sender: &mpsc::Sender, - node_id: XrNetPeerId, - cached_alignment: &Option, - cached_alignment_descriptor: &Option, -) { - let peer_ids = peers.keys().copied().collect::>(); - for peer_id in peer_ids { - let Some(mut connection) = peers - .get_mut(&peer_id) - .and_then(|peer_state| peer_state.sync_connection.take()) - else { - continue; - }; - - let result = pump_sync_connection(&mut connection); - let Ok(packets) = result else { - connection.shutdown(); - continue; - }; - - let mut invalid = false; - let mut became_ready = false; - let mut queued_data = Vec::::new(); - for packet in packets { - match packet { - XrNetSyncPacket::Hello(hello) => { - if hello.version != XR_NET_PROTOCOL_VERSION || hello.node_id != peer_id { - invalid = true; - break; - } - if !connection.handshake_received { - connection.handshake_received = true; - became_ready = true; - } - } - XrNetSyncPacket::Data(data) => { - if !connection.handshake_received { - invalid = true; - break; - } - queued_data.push(data); - } - } - } - - let Some(peer_state) = peers.get_mut(&peer_id) else { - connection.shutdown(); - continue; - }; - if invalid { - connection.shutdown(); - peer_state.next_sync_connect_attempt_at = - Instant::now() + XR_NET_DEFAULT_SYNC_CONNECT_RETRY; - continue; - } - if became_ready { - send_cached_sync_frames_to_connection( - &mut connection, - node_id, - cached_alignment, - cached_alignment_descriptor, - ); - } - for data in queued_data { - route_sync_data_packet(peer_state, incoming_sender, node_id, data); - } - peer_state.sync_connection = Some(connection); - } -} - -fn send_cached_sync_frames_to_connection( - connection: &mut XrNetSyncConnection, - node_id: XrNetPeerId, - cached_alignment: &Option, - cached_alignment_descriptor: &Option, -) { - if let Some(frame) = cached_alignment { - connection.queue_packet(&XrNetSyncPacket::Data(XrNetDataPacket::Alignment { - version: XR_NET_PROTOCOL_VERSION, - node_id, - frame: *frame, - })); - } - if let Some(frame) = cached_alignment_descriptor { - connection.queue_packet(&XrNetSyncPacket::Data( - XrNetDataPacket::AlignmentDescriptor { - version: XR_NET_PROTOCOL_VERSION, - node_id, - frame: frame.clone(), - }, - )); - } -} - -fn pump_sync_connection(connection: &mut XrNetSyncConnection) -> io::Result> { - flush_sync_connection(connection)?; - - let mut read_chunk = [0u8; 16384]; - loop { - match connection.stream.read(&mut read_chunk) { - Ok(0) => { - return Err(io::Error::new( - io::ErrorKind::UnexpectedEof, - "sync connection closed", - )) - } - Ok(len) => { - connection.read_buf.extend_from_slice(&read_chunk[..len]); - } - Err(err) if err.kind() == io::ErrorKind::WouldBlock => break, - Err(err) => return Err(err), - } - } - drain_sync_packets(&mut connection.read_buf) -} - -fn flush_sync_connection(connection: &mut XrNetSyncConnection) -> io::Result<()> { - while !connection.write_buf.is_empty() { - match connection.stream.write(&connection.write_buf) { - Ok(0) => { - return Err(io::Error::new( - io::ErrorKind::WriteZero, - "sync connection write zero", - )) - } - Ok(written) => { - connection.write_buf.drain(0..written); - } - Err(err) if err.kind() == io::ErrorKind::WouldBlock => break, - Err(err) => return Err(err), - } - } - Ok(()) -} - -fn drain_sync_packets(read_buf: &mut Vec) -> io::Result> { - let mut packets = Vec::::new(); - let mut offset = 0usize; - while read_buf.len().saturating_sub(offset) >= 4 { - let frame_len = u32::from_le_bytes([ - read_buf[offset], - read_buf[offset + 1], - read_buf[offset + 2], - read_buf[offset + 3], - ]) as usize; - if frame_len > XR_NET_SYNC_MAX_FRAME_BYTES { - return Err(io::Error::new( - io::ErrorKind::InvalidData, - "sync frame exceeded max size", - )); - } - if read_buf.len().saturating_sub(offset + 4) < frame_len { - break; - } - let start = offset + 4; - let end = start + frame_len; - let packet = XrNetSyncPacket::deserialize_bin(&read_buf[start..end]).map_err(|_| { - io::Error::new(io::ErrorKind::InvalidData, "failed to decode sync packet") - })?; - packets.push(packet); - offset = end; - } - if offset > 0 { - read_buf.drain(0..offset); - } - Ok(packets) -} - -fn route_sync_data_packet( - peer_state: &mut WorkerPeerState, - incoming_sender: &mpsc::Sender, - node_id: XrNetPeerId, - packet: XrNetDataPacket, -) { - peer_state.last_seen = Instant::now(); - let peer = peer_state.peer; - match packet { - XrNetDataPacket::State { - version, - node_id: remote_id, - frame, - } => { - if version != XR_NET_PROTOCOL_VERSION || remote_id == node_id { - return; - } - if !accept_seq(peer_state.last_state_seq, frame.seq) { - return; - } - peer_state.last_state_seq = Some(frame.seq); - let _ = incoming_sender.send(XrNetIncoming::State { peer, frame }); - } - XrNetDataPacket::Alignment { - version, - node_id: remote_id, - frame, - } => { - if version != XR_NET_PROTOCOL_VERSION || remote_id == node_id { - return; - } - if !accept_seq(peer_state.last_alignment_seq, frame.seq) { - return; - } - peer_state.last_alignment_seq = Some(frame.seq); - let _ = incoming_sender.send(XrNetIncoming::Alignment { peer, frame }); - } - XrNetDataPacket::AlignmentDescriptor { - version, - node_id: remote_id, - frame, - } => { - if version != XR_NET_PROTOCOL_VERSION || remote_id == node_id { - return; - } - if !accept_seq(peer_state.last_alignment_descriptor_seq, frame.seq) { - return; - } - peer_state.last_alignment_descriptor_seq = Some(frame.seq); - let _ = incoming_sender.send(XrNetIncoming::AlignmentDescriptor { peer, frame }); - } - XrNetDataPacket::Leave(packet) => { - if packet.version != XR_NET_PROTOCOL_VERSION || packet.node_id == node_id { - return; - } - } - } -} - -pub fn encode_alignment_descriptor_packet( - node_id: XrNetPeerId, - frame: &XrNetAlignmentDescriptorFrame, -) -> Vec { - XrNetDataPacket::AlignmentDescriptor { - version: XR_NET_PROTOCOL_VERSION, - node_id, - frame: frame.clone(), - } - .serialize_bin() -} - -pub fn decode_alignment_descriptor_packet( - bytes: &[u8], -) -> Option<(XrNetPeerId, XrNetAlignmentDescriptorFrame)> { - match XrNetDataPacket::deserialize_bin(bytes).ok()? { - XrNetDataPacket::AlignmentDescriptor { - version, - node_id, - frame, - } if version == XR_NET_PROTOCOL_VERSION => Some((node_id, frame)), - _ => None, - } -} - -#[cfg(test)] -fn safe_normalize(v: Vec3f) -> Option { - let len = v.length(); - (len > 0.0001).then_some(v * (1.0 / len)) -} - -#[cfg(test)] -fn xz_axis(vector: Vec3f) -> Option { - safe_normalize(vec3f(vector.x, 0.0, vector.z)) -} - -#[cfg(test)] -fn wrap_angle(mut angle: f32) -> f32 { - while angle <= -std::f32::consts::PI { - angle += std::f32::consts::TAU; - } - while angle > std::f32::consts::PI { - angle -= std::f32::consts::TAU; - } - angle -} - -#[cfg(test)] -fn build_wall_feature_histogram( - features: &[XrDepthAlignWallFeature], - bin_count: usize, -) -> Vec { - let bin_count = bin_count.max(1); - let mut histogram = vec![0.0; bin_count]; - for feature in features { - let Some(axis) = xz_axis(feature.normal) else { - continue; - }; - let angle = axis.x.atan2(-axis.z); - let normalized = (angle + std::f32::consts::PI) / std::f32::consts::TAU; - let bin = (normalized * bin_count as f32).floor() as isize; - histogram[bin.rem_euclid(bin_count as isize) as usize] += feature.area.max(0.05); - } - let total = histogram.iter().copied().sum::(); - if total > 0.0 { - for value in &mut histogram { - *value = (*value / total * 100.0).round() / 100.0; - } - } - histogram -} - -fn remove_peer( - peers: &mut HashMap, - incoming_sender: &mpsc::Sender, - peer_id: XrNetPeerId, - reason: XrNetLeaveReason, -) { - let Some(mut peer_state) = peers.remove(&peer_id) else { - return; - }; - close_sync_connection(&mut peer_state); - let _ = incoming_sender.send(XrNetIncoming::Leave { - peer: peer_state.peer, - reason, - }); -} - -fn expire_timed_out_peers( - peers: &mut HashMap, - incoming_sender: &mpsc::Sender, - peer_timeout: Duration, -) { - let now = Instant::now(); - let expired: Vec<_> = peers - .iter() - .filter_map(|(peer_id, peer_state)| { - (now.duration_since(peer_state.last_seen) > peer_timeout).then_some(*peer_id) - }) - .collect(); - for peer_id in expired { - remove_peer(peers, incoming_sender, peer_id, XrNetLeaveReason::Timeout); - } -} - -fn accept_seq(last: Option, next: u32) -> bool { - let Some(last) = last else { - return true; - }; - next != last && next.wrapping_sub(last) < (u32::MAX / 2) -} - -fn default_node_id() -> u64 { - let now = SystemTime::now() - .duration_since(UNIX_EPOCH) - .unwrap_or_default() - .as_nanos(); - let pid = std::process::id() as u128; - LiveId::from_str(&format!("xr-net-{}-{}", now, pid)).0 -} - -#[cfg(test)] -mod tests { - use super::*; - - const TEST_IO_TIMEOUT: Duration = Duration::from_secs(3); - - fn wait_for_event(node: &XrNetNode, mut predicate: F) -> Option - where - F: FnMut(&XrNetIncoming) -> bool, - { - let start = Instant::now(); - while start.elapsed() < TEST_IO_TIMEOUT { - match node - .incoming_receiver - .recv_timeout(Duration::from_millis(50)) - { - Ok(event) if predicate(&event) => return Some(event), - Ok(_) => continue, - Err(mpsc::RecvTimeoutError::Timeout) => continue, - Err(mpsc::RecvTimeoutError::Disconnected) => return None, - } - } - None - } - - fn localhost_config( - node_id: u64, - discovery_port: u16, - data_port: u16, - sync_port: u16, - discovery_targets: Vec, - ) -> XrNetConfig { - XrNetConfig { - node_id: XrNetPeerId(node_id), - discovery_bind: SocketAddr::new(IpAddr::V4(Ipv4Addr::LOCALHOST), discovery_port), - data_bind: SocketAddr::new(IpAddr::V4(Ipv4Addr::LOCALHOST), data_port), - sync_bind: SocketAddr::new(IpAddr::V4(Ipv4Addr::LOCALHOST), sync_port), - discovery_targets: discovery_targets - .into_iter() - .map(|port| SocketAddr::new(IpAddr::V4(Ipv4Addr::LOCALHOST), port)) - .collect(), - discovery_interval: Duration::from_millis(20), - peer_timeout: Duration::from_millis(150), - poll_interval: Duration::from_millis(5), - } - } - - fn make_test_state(head_position: Vec3f) -> XrState { - XrState { - time: 1.0, - head_pose: Pose::new(Quat::default(), head_position), - ..XrState::default() - } - } - - fn transform_point(mat: &Mat4f, point: Vec3f) -> Vec3f { - mat.transform_vec4(vec4f(point.x, point.y, point.z, 1.0)) - .to_vec3f() - } - - fn angle_error(a: f32, b: f32) -> f32 { - wrap_angle(a - b).abs() - } - - fn nearest_wall_distance(samples: &[XrDepthAlignSample], point: Vec3f) -> Option { - samples - .iter() - .filter(|sample| sample.kind == XrDepthAlignSampleKind::Wall) - .map(|sample| (sample.point - point).length()) - .min_by(|left, right| left.total_cmp(right)) - } - - fn make_align_sample( - kind: XrDepthAlignSampleKind, - point: Vec3f, - normal: Vec3f, - weight: f32, - ) -> XrDepthAlignSample { - XrDepthAlignSample { - kind, - point, - normal: normal.normalize(), - weight, - } - } - - fn make_wall_feature( - center: Vec3f, - normal: Vec3f, - half_extent_along: f32, - min_y: f32, - max_y: f32, - ) -> XrDepthAlignWallFeature { - let normal = normal.normalize(); - let along_axis = vec3f(-normal.z, 0.0, normal.x).normalize(); - XrDepthAlignWallFeature { - center, - normal, - along_axis, - plane_distance: center.dot(normal), - half_extent_along, - min_y, - max_y, - area: (half_extent_along * 2.0) * (max_y - min_y).max(0.0), - } - } - - fn make_wall_samples_from_feature( - feature: &XrDepthAlignWallFeature, - weight: f32, - ) -> [XrDepthAlignSample; 2] { - [ - make_align_sample( - XrDepthAlignSampleKind::Wall, - vec3f( - feature.center.x - feature.along_axis.x * feature.half_extent_along * 0.55, - feature.min_y + 0.18, - feature.center.z - feature.along_axis.z * feature.half_extent_along * 0.55, - ), - feature.normal, - weight, - ), - make_align_sample( - XrDepthAlignSampleKind::Wall, - vec3f( - feature.center.x + feature.along_axis.x * feature.half_extent_along * 0.55, - feature.max_y - 0.18, - feature.center.z + feature.along_axis.z * feature.half_extent_along * 0.55, - ), - feature.normal, - weight * 0.96, - ), - ] - } - - fn make_descriptor_frame() -> XrNetAlignmentDescriptorFrame { - let wall_features = vec![ - make_wall_feature( - vec3f(-1.10, 0.95, -0.90), - vec3f(1.0, 0.0, 0.0), - 0.46, - 0.52, - 1.38, - ), - make_wall_feature( - vec3f(0.98, 0.88, -1.26), - vec3f(1.0, 0.0, 0.0), - 0.34, - 0.54, - 1.22, - ), - make_wall_feature( - vec3f(0.08, 0.98, -2.12), - vec3f(0.0, 0.0, 1.0), - 0.38, - 0.58, - 1.34, - ), - make_wall_feature( - vec3f(0.36, 0.92, -0.42), - vec3f(0.0, 0.0, 1.0), - 0.30, - 0.60, - 1.24, - ), - ]; - let mut samples = Vec::new(); - for (index, feature) in wall_features.iter().enumerate() { - samples.extend(make_wall_samples_from_feature( - feature, - 0.92 - index as f32 * 0.04, - )); - } - XrNetAlignmentDescriptorFrame { - seq: 0, - sent_at: 1.0, - descriptor: XrDepthAlignDescriptor { - voxel_size_meters: 0.05, - floor_y: 0.0, - wall_normal_histogram: build_wall_feature_histogram(&wall_features, 48), - wall_features, - samples, - }, - } - } - - fn make_depth_mesh_for_descriptor(descriptor: XrDepthAlignDescriptor) -> XrDepthMesh { - let mut mesh = XrDepthMesh::default(); - mesh.voxel_size_meters = descriptor.voxel_size_meters; - mesh.alignment_descriptor = Some(descriptor.clone()); - mesh.alignment_debug = XrDepthAlignDebug { - near_surface_voxel_count: descriptor.samples.len() as u32 * 6, - floor_candidate_count: descriptor - .samples - .iter() - .filter(|sample| sample.kind == XrDepthAlignSampleKind::Floor) - .count() as u32 - * 3, - wall_candidate_count: descriptor - .samples - .iter() - .filter(|sample| sample.kind == XrDepthAlignSampleKind::Wall) - .count() as u32 - * 3, - floor_sample_count: descriptor - .samples - .iter() - .filter(|sample| sample.kind == XrDepthAlignSampleKind::Floor) - .count() as u32, - wall_sample_count: descriptor - .samples - .iter() - .filter(|sample| sample.kind == XrDepthAlignSampleKind::Wall) - .count() as u32, - }; - mesh - } - - #[test] - fn peer_timeout_emits_leave_without_needing_more_packets() { - let config = localhost_config(1, 42646, 42647, 42648, Vec::new()); - let node = XrNetNode::with_config(config).expect("node should bind localhost test ports"); - - let discovery_socket = - UdpSocket::bind(SocketAddr::new(IpAddr::V4(Ipv4Addr::LOCALHOST), 0)).unwrap(); - let data_socket = - UdpSocket::bind(SocketAddr::new(IpAddr::V4(Ipv4Addr::LOCALHOST), 0)).unwrap(); - - let discovery_packet = XrNetDiscoveryPacket::Hello(XrNetDiscoveryHello { - version: XR_NET_PROTOCOL_VERSION, - node_id: XrNetPeerId(2), - data_port: data_socket.local_addr().unwrap().port(), - sync_port: 42658, - }); - let discovery_target = SocketAddr::new(IpAddr::V4(Ipv4Addr::LOCALHOST), 42646); - let _ = discovery_socket.send_to(&discovery_packet.serialize_bin(), discovery_target); - - let join = wait_for_event(&node, |event| matches!(event, XrNetIncoming::Join { .. })) - .expect("join should arrive from a single hello"); - let peer = match join { - XrNetIncoming::Join { peer } => peer, - _ => unreachable!(), - }; - - let state_packet = XrNetDataPacket::State { - version: XR_NET_PROTOCOL_VERSION, - node_id: XrNetPeerId(2), - frame: XrNetStateFrame { - seq: 0, - sent_at: 1.0, - state: make_test_state(vec3f(0.0, 1.6, 0.0)), - }, - }; - let state_target = SocketAddr::new(IpAddr::V4(Ipv4Addr::LOCALHOST), 42647); - let _ = data_socket.send_to(&state_packet.serialize_bin(), state_target); - - let _ = wait_for_event(&node, |event| { - matches!(event, XrNetIncoming::State { peer: event_peer, .. } if *event_peer == peer) - }) - .expect("state should arrive"); - - let leave = wait_for_event(&node, |event| { - matches!( - event, - XrNetIncoming::Leave { - peer: event_peer, - reason: XrNetLeaveReason::Timeout, - } if *event_peer == peer - ) - }) - .expect("peer should time out without any extra packets"); - match leave { - XrNetIncoming::Leave { reason, .. } => { - assert_eq!(reason, XrNetLeaveReason::Timeout); - } - _ => unreachable!(), - } - } - - #[test] - fn two_test_clients_align_two_cubes_via_protocol() { - let mut left = - XrNetNode::with_config(localhost_config(11, 42746, 42747, 42748, vec![42756])) - .expect("left test client should bind"); - let mut right = - XrNetNode::with_config(localhost_config(22, 42756, 42757, 42758, vec![42746])) - .expect("right test client should bind"); - - let left_cube_world = vec3f(-0.25, 0.82, -0.62); - let right_cube_world = vec3f(0.21, 0.82, -0.38); - let local_anchor = XrAnchor { - left: left_cube_world, - right: right_cube_world, - }; - - let remote_scene_to_local = Pose::new( - Quat::from_axis_angle(vec3f(0.0, 1.0, 0.0), 0.42), - vec3f(-0.34, 0.05, 0.27), - ) - .to_mat4(); - let local_to_remote_scene = remote_scene_to_local.invert(); - let remote_anchor = XrAnchor { - left: transform_point(&local_to_remote_scene, left_cube_world), - right: transform_point(&local_to_remote_scene, right_cube_world), - }; - - left.send_alignment(local_anchor, 0.95, 1.0); - right.send_alignment(remote_anchor, 0.95, 1.0); - - let left_remote = wait_for_event(&left, |event| { - matches!(event, XrNetIncoming::Alignment { .. }) - }) - .expect("left test client should receive remote alignment"); - let right_remote = wait_for_event(&right, |event| { - matches!(event, XrNetIncoming::Alignment { .. }) - }) - .expect("right test client should receive remote alignment"); - - let left_remote_frame = match left_remote { - XrNetIncoming::Alignment { frame, .. } => frame, - _ => unreachable!(), - }; - let right_remote_frame = match right_remote { - XrNetIncoming::Alignment { frame, .. } => frame, - _ => unreachable!(), - }; - - let solved_remote_to_local = XrNetAlignmentFrame::remote_to_local_transform( - &local_anchor_frame(local_anchor), - &left_remote_frame, - ) - .expect("left side should solve a remote->local transform"); - let solved_local_to_remote = XrNetAlignmentFrame::remote_to_local_transform( - &remote_anchor_frame(remote_anchor), - &right_remote_frame, - ) - .expect("right side should solve a local->remote transform"); - - let mapped_left = transform_point(&solved_remote_to_local, remote_anchor.left); - let mapped_right = transform_point(&solved_remote_to_local, remote_anchor.right); - assert!((mapped_left - left_cube_world).length() < 0.02); - assert!((mapped_right - right_cube_world).length() < 0.02); - - let mapped_back_left = transform_point(&solved_local_to_remote, left_cube_world); - let mapped_back_right = transform_point(&solved_local_to_remote, right_cube_world); - assert!((mapped_back_left - remote_anchor.left).length() < 0.02); - assert!((mapped_back_right - remote_anchor.right).length() < 0.02); - } - - #[test] - fn cached_alignment_descriptor_is_sent_to_late_joiner() { - let mut left = - XrNetNode::with_config(localhost_config(101, 42846, 42847, 42848, vec![42856])) - .expect("left test client should bind"); - let right = XrNetNode::with_config(localhost_config(202, 42856, 42857, 42858, vec![42846])) - .expect("right test client should bind"); - - let descriptor = make_descriptor_frame(); - left.send_alignment_descriptor(descriptor.clone()); - - let event = wait_for_event(&right, |event| { - matches!(event, XrNetIncoming::AlignmentDescriptor { .. }) - }) - .expect("late joiner should receive cached descriptor"); - - let received = match event { - XrNetIncoming::AlignmentDescriptor { frame, .. } => frame, - _ => unreachable!(), - }; - assert_eq!( - received.descriptor.samples.len(), - descriptor.descriptor.samples.len() - ); - assert!((received.descriptor.floor_y - descriptor.descriptor.floor_y).abs() < 0.001); - } - - #[test] - fn descriptor_solver_recovers_yaw_and_position_from_wall_samples() { - let local = make_descriptor_frame(); - let ground_truth_remote_to_local = Pose::new( - Quat::from_axis_angle(vec3f(0.0, 1.0, 0.0), 0.58), - vec3f(-0.82, 0.0, 0.67), - ) - .to_mat4(); - let local_to_remote = ground_truth_remote_to_local.invert(); - let mut remote = local.transformed(&local_to_remote); - remote.descriptor.samples = remote - .descriptor - .samples - .into_iter() - .enumerate() - .filter_map(|(index, mut sample)| { - let keep = match sample.kind { - XrDepthAlignSampleKind::Wall => index % 5 != 2, - XrDepthAlignSampleKind::Floor | XrDepthAlignSampleKind::Unknown => false, - }; - if !keep { - return None; - } - match sample.kind { - XrDepthAlignSampleKind::Wall => { - sample.point += vec3f( - ((index % 4) as f32 - 1.5) * 0.012, - (((index * 3) % 5) as f32 - 2.0) * 0.020, - (((index * 5) % 7) as f32 - 3.0) * 0.012, - ); - } - XrDepthAlignSampleKind::Floor | XrDepthAlignSampleKind::Unknown => {} - } - sample.weight = - (sample.weight * (0.84 + 0.03 * (index % 4) as f32)).clamp(0.1, 1.0); - Some(sample) - }) - .collect(); - remote.descriptor.wall_normal_histogram = - build_wall_feature_histogram(&remote.descriptor.wall_features, 48); - - let solution = XrNetAlignmentDescriptorFrame::solve_remote_to_local(&local, &remote) - .expect("descriptor solver should find a transform"); - - assert!(angle_error(solution.yaw_radians, 0.58) < 0.08); - assert!((solution.translation - vec3f(-0.82, 0.0, 0.67)).length() < 0.12); - let mapped_error = remote - .descriptor - .samples - .iter() - .filter(|sample| sample.kind == XrDepthAlignSampleKind::Wall) - .map(|sample| { - let mapped = solution.map_point(sample.point); - nearest_wall_distance(&local.descriptor.samples, mapped) - .expect("local descriptor should have wall samples") - }) - .sum::() - / remote - .descriptor - .samples - .iter() - .filter(|sample| sample.kind == XrDepthAlignSampleKind::Wall) - .count() - .max(1) as f32; - assert!( - mapped_error < 0.10, - "solution={solution:?} mapped_error={mapped_error}" - ); - assert!(solution.confidence > 0.15); - assert!(solution.matched_samples >= 2); - } - - #[test] - fn descriptor_from_depth_mesh_uses_tsdf_alignment_descriptor_snapshot() { - let descriptor = make_descriptor_frame().descriptor; - let mesh = make_depth_mesh_for_descriptor(descriptor.clone()); - let descriptor = XrNetAlignmentDescriptorFrame::from_depth_mesh(&mesh, 1.0) - .expect("descriptor should use the TSDF alignment descriptor from the depth snapshot"); - - assert_eq!(descriptor.descriptor, mesh.alignment_descriptor.unwrap()); - assert!(descriptor.test_markers().is_some()); - } - - fn local_anchor_frame(anchor: XrAnchor) -> XrNetAlignmentFrame { - XrNetAlignmentFrame { - seq: 0, - sent_at: 1.0, - anchor, - confidence: 1.0, - } - } - - fn remote_anchor_frame(anchor: XrAnchor) -> XrNetAlignmentFrame { - XrNetAlignmentFrame { - seq: 0, - sent_at: 1.0, - anchor, - confidence: 1.0, - } - } -}