From 6e7d99953aafc3d4ec189314e6307dc0afe9dcc8 Mon Sep 17 00:00:00 2001 From: chazmm Date: Sun, 6 Sep 2026 18:37:48 +0100 Subject: [PATCH] Implement native biped contact classification and world query pipeline --- .../src/animation/foot_ik/post_contact.rs | 2 +- .../src/player/offboard/contact_candidates.rs | 212 +++++++++++++ .../src/player/offboard/contact_classify.rs | 287 ++++++++++++++++++ .../src/player/offboard/contact_geometry.rs | 83 +++++ .../player/offboard/contact_intersections.rs | 194 ++++++++++++ .../src/player/offboard/contact_output.rs | 213 +++++++++++++ .../src/player/offboard/contact_promotion.rs | 174 +++++++---- .../src/player/offboard/contact_segments.rs | 284 ++++++++++++----- .../src/player/offboard/contact_simplify.rs | 114 +++++++ .../src/player/offboard/contact_sort.rs | 174 +++++++++++ .../src/player/offboard/contact_toolkit.rs | 271 +++++++++++++++++ crates/skate-core/src/player/offboard/mod.rs | 8 + .../src/physics/offboard/contact_queries.rs | 222 ++++++++++++-- docs/RUNOUT.md | 23 +- 14 files changed, 2089 insertions(+), 172 deletions(-) create mode 100644 crates/skate-core/src/player/offboard/contact_candidates.rs create mode 100644 crates/skate-core/src/player/offboard/contact_classify.rs create mode 100644 crates/skate-core/src/player/offboard/contact_geometry.rs create mode 100644 crates/skate-core/src/player/offboard/contact_intersections.rs create mode 100644 crates/skate-core/src/player/offboard/contact_output.rs create mode 100644 crates/skate-core/src/player/offboard/contact_simplify.rs create mode 100644 crates/skate-core/src/player/offboard/contact_sort.rs create mode 100644 crates/skate-core/src/player/offboard/contact_toolkit.rs diff --git a/crates/skate-core/src/animation/foot_ik/post_contact.rs b/crates/skate-core/src/animation/foot_ik/post_contact.rs index af122be0..525a143a 100644 --- a/crates/skate-core/src/animation/foot_ik/post_contact.rs +++ b/crates/skate-core/src/animation/foot_ik/post_contact.rs @@ -67,7 +67,7 @@ pub(super) fn target( ///Scalar constructor path through original8252D980. Its argument is already ///clamped to [.01,1.5607964]; coefficients come from822FE3C0..E0. -fn tangent(angle: f32) -> f32 { +pub(crate) fn tangent(angle: f32) -> f32 { let quadrant = (angle * f32::from_bits(0x3F22_F983)).round_ties_even(); let reduced = (-quadrant).mul_add(f32::from_bits(0x3FC9_0FDB), angle); let reduced = (-quadrant).mul_add(f32::from_bits(0x2E85_A309), reduced); diff --git a/crates/skate-core/src/player/offboard/contact_candidates.rs b/crates/skate-core/src/player/offboard/contact_candidates.rs new file mode 100644 index 00000000..7ae1ba57 --- /dev/null +++ b/crates/skate-core/src/player/offboard/contact_candidates.rs @@ -0,0 +1,212 @@ +//! Native candidate searches82D84D38/85070 and distance fallback82D84B98. +use super::{ + contact_geometry, + contact_queries::{Input, V}, + contact_segments::{Candidate, Kind, Segments}, +}; +use crate::physics::native_arithmetic::dot3; +fn sub(a: V, b: V) -> V { + std::array::from_fn(|i| a[i] - b[i]) +} +fn project(input: Input, p: V) -> [f32; 2] { + let delta = sub(p, input.position); + [ + dot3(input.surface_forward, delta), + dot3(input.surface_up, delta), + ] +} +fn next_normal(input: Input, segments: &Segments, i: usize) -> V { + if i + 1 == segments.last_surface_end { + input.surface_up + } else { + segments.items[i + 1].normal + } +} +pub fn rising( + input: Input, + segments: &Segments, + index: usize, + slope: f32, + flags: u32, +) -> Option { + let anchor = project(input, segments.items[index].end); + let end = [-1., (-1. - anchor[0]).mul_add(slope, anchor[1])]; + for i in (0..index).rev() { + let s = segments.items[i]; + let fraction = contact_geometry::intersect( + project(input, s.start), + project(input, s.end), + anchor, + end, + ); + if fraction < 0. { + if i == 0 { + return Some(Candidate::from_point( + input, + segments, + input.position, + input.surface_up, + Some(index), + flags, + 1, + )); + } + continue; + } + if !(fraction <= 1.) { + continue; + } + if s.kind != Kind::Surface { + if dot3(input.surface_up, sub(s.start, input.position)) > 0.75 { + return None; + } + return Some(Candidate::from_point( + input, + segments, + s.start, + next_normal(input, segments, i), + Some(index), + flags, + 1, + )); + } + let distance = s.length * fraction; + let position = std::array::from_fn(|k| s.direction[k].mul_add(distance, s.start[k])); + if dot3(input.surface_up, sub(position, input.position)) > 0.75 { + return None; + } + return Some(Candidate::from_point( + input, + segments, + position, + s.normal, + Some(index), + flags, + 1, + )); + } + None +} +pub fn falling( + input: Input, + segments: &Segments, + index: Option, + slope: f32, + flags: u32, + kind: u32, + forward_limit: f32, +) -> Option { + let anchor = project( + input, + index.map_or(input.position, |i| segments.items[i].start), + ); + let x = f32::from_bits(0x4033_3333); + let end = [x, (x - anchor[0]).mul_add(slope, anchor[1])]; + for i in index.map_or(0, |i| i + 1)..segments.last_surface_end { + let s = segments.items[i]; + let fraction = contact_geometry::intersect( + project(input, s.start), + project(input, s.end), + anchor, + end, + ); + if fraction < 0. || !(fraction <= 1.) { + continue; + } + let (position, normal) = if s.kind != Kind::Surface { + if dot3(input.surface_forward, sub(s.end, input.position)) + > forward_limit - f32::from_bits(0x3eb3_3333) + { + return None; + } + (s.end, next_normal(input, segments, i)) + } else { + let distance = s.length * fraction; + ( + std::array::from_fn(|k| s.direction[k].mul_add(distance, s.start[k])), + s.normal, + ) + }; + return Some(Candidate::from_point( + input, segments, position, normal, index, flags, kind, + )); + } + None +} +pub fn distance_fallback( + input: Input, + segments: &Segments, + mut distance: f32, +) -> Option { + for i in 0..segments.last_surface_end { + let s = segments.items[i]; + if s.kind == Kind::Falling { + distance += s.length / contact_geometry::slope_limit(input, s.length, false); + continue; + } + distance -= s.length; + if !(distance <= 0.) { + continue; + } + let (position, normal) = if s.kind == Kind::Rising { + (s.end, next_normal(input, segments, i)) + } else { + ( + std::array::from_fn(|k| s.direction[k].mul_add(distance, s.end[k])), + s.normal, + ) + }; + return Some(Candidate::from_point( + input, + segments, + position, + normal, + Some(i), + 0, + 9, + )); + } + None +} + +#[cfg(test)] +mod tests { + use super::*; + use crate::player::offboard::contact_records::Record; + fn input() -> Input { + Input { + position: [0.; 4], + surface_forward: [0., 0., 1., 0.], + surface_up: [0., 1., 0., 0.], + surface_right: [1., 0., 0., 0.], + velocity: [0., 0., 2., 0.], + animation_up: [0., 1., 0., 0.], + animation_right: [1., 0., 0., 0.], + } + } + fn r(y: f32, z: f32) -> Record { + Record { + position: [0., y, z, 0.], + normal: [0., 1., 0., 0.], + coordinates: [z, y, z, z], + flags: 0, + distance: z, + } + } + #[test] + fn searches_intersect_actual_surface_segments() { + let mut segments = Segments::default(); + segments.rebuild(input(), &[r(0., 0.), r(0., 1.), r(0.5, 1.), r(0.5, 2.)]); + let candidate = rising(input(), &segments, 1, 1., 128).unwrap(); + assert_eq!(candidate.position, [0., 0., 0.5, 0.]); + assert_eq!(candidate.flags, 128); + assert_eq!(candidate.segment_index, Some(1)); + let candidate = distance_fallback(input(), &segments, 0.25).unwrap(); + assert_eq!(candidate.position, [0., 0., 0.25, 0.]); + assert_eq!(candidate.kind, 9); + segments.rebuild(input(), &[r(0., 0.), r(0., 1.), r(-0.5, 1.), r(-0.5, 2.)]); + let candidate = falling(input(), &segments, Some(1), -1., 0, 4, 100.).unwrap(); + assert_eq!(candidate.position, [0., -0.5, 1.5, 0.]); + assert_eq!(candidate.kind, 4); + } +} diff --git a/crates/skate-core/src/player/offboard/contact_classify.rs b/crates/skate-core/src/player/offboard/contact_classify.rs new file mode 100644 index 00000000..6c51920e --- /dev/null +++ b/crates/skate-core/src/player/offboard/contact_classify.rs @@ -0,0 +1,287 @@ +//! Candidate generation82D837C8. Uses the recovered sorted/reduced segments; +//! final candidate selection and contact packet publication happen afterward. +use super::{ + contact_candidates, + contact_geometry::{slope_between, slope_limit}, + contact_packet::Packet, + contact_queries::{Input, V}, + contact_segments::{Candidate, Kind, Segments, length_inverse}, +}; +use crate::physics::native_arithmetic::dot3; +fn sub(a: V, b: V) -> V { + std::array::from_fn(|i| a[i] - b[i]) +} +fn height(input: Input, p: V) -> f32 { + dot3(input.surface_up, sub(p, input.position)) +} +fn forward(input: Input, p: V) -> f32 { + dot3(input.surface_forward, sub(p, input.position)) +} +pub fn candidates( + input: Input, + packet: &Packet, + segments: &Segments, + forward_limit: f32, + previous_flags: u32, +) -> Vec { + let mut out = Vec::new(); + let supported = packet.flags & 1 != 0; + if supported { + let up_speed = dot3(input.surface_up, input.velocity); + let planar = std::array::from_fn(|i| input.velocity[i] - input.surface_up[i] * up_speed); + if !(length_inverse(planar).0 >= f32::from_bits(0x3c23_d70a)) { + let a = input.surface_right; + let b = packet.normal; + let tangent = [ + (-a[2]).mul_add(b[1], a[1] * b[2]), + (-a[0]).mul_add(b[2], a[2] * b[0]), + (-a[1]).mul_add(b[0], a[0] * b[1]), + (-a[3]).mul_add(b[3], a[3] * b[3]), + ]; + out.push(Candidate::new( + input, + segments, + packet.position, + packet.normal, + tangent, + None, + 0, + 0, + )); + return out; + } + } + let count = segments.last_surface_end; + if count == 0 { + return out; + } + let mut occluded = false; + let mut passed_drop = false; + let mut highest: Option = None; + for i in 0..count { + let s = segments.items[i]; + let normal = if i + 1 < count { + let next = segments.items[i + 1]; + if s.kind != Kind::Surface && next.kind != Kind::Surface { + input.surface_up + } else { + next.normal + } + } else { + s.normal + }; + let was_drop = passed_drop; + if !passed_drop { + let previous = highest.map_or(input.position, |j| segments.items[j].end); + if dot3(sub(s.end, previous), input.surface_up) > 0. { + highest = Some(i); + } + } + if s.kind == Kind::Rising && s.length > f32::from_bits(0x3d4c_cccd) { + let tall = s.length > f32::from_bits(0x3ecc_cccd); + let mut bridged = false; + let mut blocked = false; + let mut slope = slope_limit(input, s.length, true); + let mut j = i + 1; + while j < count { + let next = segments.items[j]; + if !blocked && next.kind == Kind::Falling { + blocked = + forward(input, next.start) < forward_limit - f32::from_bits(0x3eb3_3333); + } + if next.kind == Kind::Surface + || (next.kind == Kind::Rising && next.length < f32::from_bits(0x3d4c_cccd)) + { + if !blocked { + blocked = dot3(next.direction, input.surface_forward) + > f32::from_bits(0x3f34_fdf4) + && forward(input, next.start) + < forward_limit - f32::from_bits(0x3eb3_3333); + } + j += 1; + continue; + } + break; + } + if j < count && segments.items[j].kind == Kind::Rising { + let between = slope_between(input, s.end, segments.items[j].end); + if between > slope_limit(input, s.length, true) * 0.5 { + slope = between; + bridged = true; + if !occluded + && slope_between(input, input.position, segments.items[j].end) + < slope_limit(input, s.length, true) * 0.5 + { + bridged = false; + } + } + } + if !occluded { + occluded = blocked; + } + if !blocked { + continue; + } + if let Some(c) = + contact_candidates::rising(input, segments, i, slope, if tall { 128 } else { 0 }) + { + out.push(c); + } + out.push(Candidate::from_point( + input, + segments, + s.end, + normal, + Some(i), + 64 | if bridged || tall { 256 } else { 0 }, + if bridged { 2 } else { 3 }, + )); + } else if s.kind == Kind::Falling && s.length > f32::from_bits(0x3d4c_cccd) { + let mut bridged = false; + let tall = s.length > f32::from_bits(0x3ecc_cccd); + let mut slope = -slope_limit(input, s.length, false); + let mut j = i + 1; + while j < count { + let next = segments.items[j]; + if next.kind == Kind::Surface + || (next.kind == Kind::Falling && next.length < f32::from_bits(0x3d4c_cccd)) + { + j += 1; + continue; + } + break; + } + if j < count && segments.items[j].kind == Kind::Falling { + let between = slope_between(input, s.start, segments.items[j].start); + if between < slope_limit(input, s.length, false) * -0.5 { + bridged = true; + slope = between; + } + } + let mut reject = !was_drop + && !occluded + && highest.is_some_and(|j| { + height(input, segments.items[j].end) > f32::from_bits(0x3ca3_d70a) + }); + if i == 1 && segments.items[0].kind == Kind::Surface { + reject = false; + } + passed_drop = true; + if reject { + continue; + } + if !bridged { + if forward(input, s.start) > 0.75 { + continue; + } + if let Some(c) = contact_candidates::falling( + input, + segments, + Some(i), + slope, + if tall { 256 } else { 0 }, + 4, + forward_limit, + ) { + out.push(c); + } + } + out.push(Candidate::from_point( + input, + segments, + s.start, + normal, + Some(i), + 64 | if bridged || tall { 128 } else { 0 }, + if bridged { 5 } else { 6 }, + )); + } + } + if supported && input.position[1] - packet.position[1] > f32::from_bits(0x3c23_d70a) { + let mut flags = previous_flags & 0x140; + if !(input.position[1] - packet.position[1] < f32::from_bits(0x3ecc_cccd)) { + flags |= 64; + } + let empty = out.is_empty(); + if empty || !passed_drop { + if let Some(c) = contact_candidates::falling( + input, + segments, + None, + -slope_limit(input, f32::from_bits(0x3f4c_cccd), false), + flags, + if empty { 7 } else { 8 }, + forward_limit, + ) { + out.push(c); + } + } + } else if out.is_empty() { + if let Some(c) = contact_candidates::distance_fallback( + input, + segments, + length_inverse(input.velocity).0 * f32::from_bits(0x3c88_8889), + ) { + out.push(c); + } + } + out +} + +#[cfg(test)] +mod tests { + use super::*; + use crate::player::offboard::contact_records::Record; + fn input(speed: f32) -> Input { + Input { + position: [0.; 4], + surface_forward: [0., 0., 1., 0.], + surface_up: [0., 1., 0., 0.], + surface_right: [1., 0., 0., 0.], + velocity: [0., 0., speed, 0.], + animation_up: [0., 1., 0., 0.], + animation_right: [1., 0., 0., 0.], + } + } + fn r(y: f32, z: f32) -> Record { + Record { + position: [0., y, z, 0.], + normal: [0., 1., 0., 0.], + coordinates: [z, y, z, z], + flags: 0, + distance: z, + } + } + #[test] + fn stationary_support_and_moving_flat_ground_take_distinct_native_paths() { + let packet = Packet { + flags: 1, + ..Packet::default() + }; + let stationary = candidates(input(0.), &packet, &Segments::default(), 100., 0); + assert_eq!(stationary.len(), 1); + assert_eq!(stationary[0].kind, 0); + let mut segments = Segments::default(); + segments.rebuild(input(3.), &[r(0., 0.), r(0., 2.)]); + let moving = candidates(input(3.), &packet, &segments, 100., 0); + assert_eq!(moving.len(), 1); + assert_eq!(moving[0].kind, 9); + assert!((moving[0].position[2] - 0.05).abs() < 1e-6); + } + #[test] + fn step_up_with_forward_clearance_produces_approach_and_top_candidates() { + let packet = Packet { + flags: 1, + ..Packet::default() + }; + let mut segments = Segments::default(); + segments.rebuild(input(3.), &[r(0., 0.), r(0., 0.5), r(0.3, 0.5), r(0.3, 2.)]); + let candidates = candidates(input(3.), &packet, &segments, 100., 0); + assert_eq!( + candidates.iter().map(|c| c.kind).collect::>(), + vec![1, 3] + ); + assert_eq!(candidates[1].flags, 64); + assert_eq!(candidates[1].position, [0., 0.3, 0.5, 0.]); + } +} diff --git a/crates/skate-core/src/player/offboard/contact_geometry.rs b/crates/skate-core/src/player/offboard/contact_geometry.rs new file mode 100644 index 00000000..9b3e9731 --- /dev/null +++ b/crates/skate-core/src/player/offboard/contact_geometry.rs @@ -0,0 +1,83 @@ +//! Native classifier math82D85360/85488/855A8/85C48. +use super::{ + contact_queries::{Input, V}, + contact_segments::{length_inverse, reciprocal}, +}; +use crate::physics::native_arithmetic::dot3; +pub fn slope_limit(input: Input, height: f32, rising: bool) -> f32 { + let large = height > f32::from_bits(0x3ecc_cccd); + let end = if large { + if rising { 35. } else { 30. } + } else { + 15. + }; + let start = if large { 50. } else { 30. }; + let speed = length_inverse(input.velocity).0; + // Scalar fsel clamp order retains native unordered behavior. + let lower = if 2. - speed >= 0. { 2. } else { speed }; + let clamped = if 6. - lower >= 0. { lower } else { 6. }; + let degrees = ((clamped - 2.) * (end - start)).mul_add(0.25, start); + crate::animation::foot_ik::post_contact::tangent(degrees * f32::from_bits(0x3c8e_fa35)) +} +pub fn slope_between(input: Input, start: V, end: V) -> f32 { + let delta = std::array::from_fn(|i| end[i] - start[i]); + let distance = dot3(input.surface_forward, delta); + if f32::from_bits(0x3a83_126f) > distance { + 1000. + } else { + dot3(input.surface_up, delta) * reciprocal(distance) + } +} +/// Infinite 2D line intersection parameter along a..b. Parallel lines return +///the native finite sentinel; callers perform their own [0,1] interval tests. +pub fn intersect(a: [f32; 2], b: [f32; 2], c: [f32; 2], d: [f32; 2]) -> f32 { + let ab = [b[0] - a[0], b[1] - a[1]]; + let cd = [d[0] - c[0], d[1] - c[1]]; + let determinant = cd[1] * ab[0] - cd[0] * ab[1]; + if !(determinant.abs() >= f32::from_bits(0x3727_c5ac)) { + return f32::from_bits(0x5015_02f9); + } + let ac = [a[0] - c[0], a[1] - c[1]]; + (1. / determinant) * (cd[0] * ac[1] - cd[1] * ac[0]) +} +#[cfg(test)] +mod tests { + use super::*; + fn input(speed: f32) -> Input { + Input { + position: [0.; 4], + surface_forward: [0., 0., 1., 0.], + surface_up: [0., 1., 0., 0.], + surface_right: [1., 0., 0., 0.], + velocity: [0., 0., speed, 0.], + animation_up: [0., 1., 0., 0.], + animation_right: [1., 0., 0., 0.], + } + } + #[test] + fn stock_speed_clamps_and_height_branch_select_expected_angles() { + for (speed, height, rising, angle) in [ + (0., 0.4, true, 30.0f64), + (6., 0.4, true, 15.), + (2., 0.5, true, 50.), + (6., 0.5, true, 35.), + (8., 0.5, false, 30.), + ] { + assert!( + (slope_limit(input(speed), height, rising) - angle.to_radians().tan() as f32).abs() + < 1e-5 + ); + } + } + #[test] + fn line_parameters_and_parallel_sentinel() { + assert_eq!(intersect([0., 0.], [2., 0.], [1., -1.], [1., 1.]), 0.5); + assert_eq!(intersect([0., 0.], [1., 0.], [2., -1.], [2., 1.]), 2.); + assert_eq!( + intersect([0., 0.], [1., 0.], [0., 1.], [1., 1.]).to_bits(), + 0x501502f9 + ); + assert_eq!(slope_between(input(0.), [0.; 4], [0., 1., 2., 0.]), 0.5); + assert_eq!(slope_between(input(0.), [0.; 4], [0., 1., 0., 0.]), 1000.); + } +} diff --git a/crates/skate-core/src/player/offboard/contact_intersections.rs b/crates/skate-core/src/player/offboard/contact_intersections.rs new file mode 100644 index 00000000..3de452e0 --- /dev/null +++ b/crates/skate-core/src/player/offboard/contact_intersections.rs @@ -0,0 +1,194 @@ +//! Normal constraints82D82940/82E0A258 and obstacle intersections82D82AF0. +use super::{ + contact_queries::{Input, V}, + contact_records::{Direction, Record, Records, Source}, + contact_segments::length_inverse, +}; +use crate::physics::native_arithmetic::dot3; +fn sub(a: V, b: V) -> V { + std::array::from_fn(|i| a[i] - b[i]) +} +fn cross(a: V, b: V) -> V { + [ + (-a[2]).mul_add(b[1], a[1] * b[2]), + (-a[0]).mul_add(b[2], a[2] * b[0]), + (-a[1]).mul_add(b[0], a[0] * b[1]), + (-a[3]).mul_add(b[3], a[3] * b[3]), + ] +} +fn unit(a: V) -> V { + let (length, inverse) = length_inverse(a); + if length > f32::from_bits(0x3586_37bd) { + a.map(|x| x * inverse) + } else { + [0.; 4] + } +} +fn negate(a: V) -> V { + a.map(|x| f32::from_bits(x.to_bits() ^ 0x80000000)) +} +pub fn clamp_normal(a: V, b: V, normal: &mut V) -> bool { + let a = unit(a); + let b = unit(b); + let axis = unit(cross(a, b)); + let dot = dot3(axis, *normal); + let projected = unit(std::array::from_fn(|i| normal[i] - axis[i] * dot)); + let product = ((dot3(axis, axis) * dot3(projected, projected)) * dot3(a, a)) * dot3(b, b); + if !(product >= f32::from_bits(0x3dcc_cccd)) { + return false; + } + *normal = if dot3(cross(a, projected), axis) > 0. && dot3(cross(projected, b), axis) > 0. { + projected + } else if dot3(projected, a) > dot3(projected, b) { + a + } else { + b + }; + true +} +///82E0A060 accepts strictly opposite sides; endpoint-on-plane is not a hit. +pub fn plane_segment(origin: V, normal: V, start: V, end: V) -> Option { + let a = dot3(normal, sub(start, origin)); + let b = dot3(normal, sub(end, origin)); + if a * b >= 0. { + return None; + } + let total = b.abs() + a.abs(); + let wa = b.abs() / total; + let wb = a.abs() / total; + Some(std::array::from_fn(|i| start[i].mul_add(wa, end[i] * wb))) +} +pub fn constrain(input: Input, records: &mut Records) { + let mut previous = input.position; + for record in &mut records.surface { + let height = dot3(sub(record.position, previous), input.surface_up); + if !(height.abs() >= f32::from_bits(0x3a83_126f)) { + record.normal = input.surface_up; + } else { + let forward = if height > 0. { + negate(input.surface_forward) + } else { + input.surface_forward + }; + clamp_normal(forward, input.surface_up, &mut record.normal); + } + //82940 loads this record's position into the next iteration's base. + previous = record.position; + } + let count = records.surface.len(); + for i in 1..count { + let a = records.surface[i - 1]; + let b = records.surface[i]; + for j in 0..records.obstacle.len() { + let obstacle = records.obstacle[j]; + if obstacle.flags & 3 != 0 + || a.coordinates[0] > obstacle.coordinates[0] + || obstacle.coordinates[0] > b.coordinates[0] + { + continue; + } + insert_intersections(input, records, a, b, j); + } + } + super::contact_sort::sort(&mut records.surface); +} +fn insert_intersections(input: Input, records: &mut Records, a: Record, b: Record, index: usize) { + let obstacle = records.obstacle[index]; + let delta = sub(b.position, a.position); + let close = f32::from_bits(0x3c23_d70a); + if dot3(a.normal, b.normal) > f32::from_bits(0x3f7d_70a4) + && dot3(a.normal, obstacle.normal) > f32::from_bits(0x3f7d_70a4) + && close > dot3(delta, b.normal).abs() + { + let mut delta = sub(obstacle.position, a.position); + if close > length_inverse(delta).0 { + delta = sub(obstacle.position, b.position); + } + if close > dot3(delta, b.normal).abs() { + return; + } + } + let mut normal = obstacle.normal; + let forward = if b.coordinates[1] > a.coordinates[1] { + negate(input.surface_forward) + } else { + input.surface_forward + }; + clamp_normal(forward, input.surface_up, &mut normal); + let first_dot = dot3(a.normal, delta); + let second_dot = dot3(b.normal, delta); + let extent = f32::from_bits(0x3f8c_cccd); + let extended_first = std::array::from_fn(|i| { + (delta[i] - a.normal[i] * first_dot).mul_add(extent, a.position[i]) + }); + let extended_second = + std::array::from_fn(|i| b.position[i] - (delta[i] - b.normal[i] * second_dot) * extent); + let hits = [ + plane_segment(obstacle.position, normal, a.position, extended_first), + plane_segment(obstacle.position, normal, extended_second, b.position), + ]; + let low = if b.coordinates[1] > a.coordinates[1] { + a.coordinates[1] + } else { + b.coordinates[1] + }; + let high = if a.coordinates[1] > b.coordinates[1] { + a.coordinates[1] + } else { + b.coordinates[1] + }; + let length = length_inverse(delta).0; + for point in hits.into_iter().flatten() { + let d = sub(point, input.position); + let forward = dot3(input.surface_forward, d); + let height = dot3(input.surface_up, d); + if !(forward >= a.coordinates[0] + && b.coordinates[0] >= forward + && height >= low + && high >= height) + { + continue; + } + let fraction = length_inverse(sub(point, a.position)).0 / length; + let distance = fraction.mul_add(b.distance - a.distance, a.distance); + let mut n = normal; + records.insert( + input, + point, + &mut n, + Source::Support, + Direction::Reverse, + distance, + ); + records.obstacle[index].flags |= 4; + } +} +#[cfg(test)] +mod tests { + use super::*; + #[test] + fn normal_is_projected_then_constrained_to_the_native_wedge() { + let a = [0., 0., -1., 0.]; + let b = [0., 1., 0., 0.]; + let mut n = [1., 1., -1., 0.]; + assert!(clamp_normal(a, b, &mut n)); + assert_eq!(n[0], 0.); + assert!(n[1] > 0.7 && n[2] < -0.7); + let mut outside = [0., -1., 0., 0.]; + clamp_normal(a, b, &mut outside); + assert_eq!(outside, a); + } + #[test] + fn plane_intersection_rejects_endpoint_contacts() { + assert_eq!( + plane_segment( + [0.; 4], + [0., 1., 0., 0.], + [0., -1., 0., 0.], + [0., 1., 0., 0.] + ), + Some([0.; 4]) + ); + assert!(plane_segment([0.; 4], [0., 1., 0., 0.], [0.; 4], [0., 1., 0., 0.]).is_none()); + } +} diff --git a/crates/skate-core/src/player/offboard/contact_output.rs b/crates/skate-core/src/player/offboard/contact_output.rs new file mode 100644 index 00000000..5eba9a5e --- /dev/null +++ b/crates/skate-core/src/player/offboard/contact_output.rs @@ -0,0 +1,213 @@ +//! Contact publication82D84060 and slope history82D848D0. +use super::{ + contact_packet::Packet, + contact_queries::{Input, V}, + contact_records::Records, + contact_segments::{Candidate, Kind, Segments, length_inverse}, + contact_simplify::Reduction, +}; +use crate::physics::native_arithmetic::dot3; +#[derive(Default)] +pub struct History { + pub retained_candidate: Option, + pub stable_kind: u32, + pub samples: [u32; 3], + pub cursor: usize, +} +fn sub(a: V, b: V) -> V { + std::array::from_fn(|i| a[i] - b[i]) +} +fn forward(input: Input, p: V) -> f32 { + dot3(input.surface_forward, sub(p, input.position)) +} + +pub fn publish( + input: Input, + packet: &mut Packet, + records: &Records, + segments: &Segments, + reduction: &Reduction, + candidates: &mut [Candidate], + history: &mut History, +) { + edge_output(input, packet, records, segments, reduction); + super::contact_sort::sort(candidates); + let near_distance = length_inverse(input.velocity).0 * f32::from_bits(0x3c88_8889); + let mut selected = None; + for (i, c) in candidates.iter().copied().enumerate() { + let near = i + 1 < candidates.len() && forward(input, c.position) < near_distance; + if c.kind == 1 { + let mut interrupted = false; + if !near { + for next in &candidates[i + 1..] { + if next.minimum_distance > c.maximum_distance { + break; + } + if matches!(next.kind, 4 | 7 | 8) { + interrupted = true; + break; + } + } + } + if near || interrupted { + selected = Some( + candidates[i + 1..] + .iter() + .find(|next| next.segment_index == c.segment_index) + .copied() + .unwrap_or(c), + ); + break; + } + } + if !near { + selected = Some(c); + break; + } + } + if let Some(c) = selected { + history.retained_candidate = Some(c); + packet.target_position = c.position; + packet.target_normal = c.normal; + packet.flags |= c.flags | 2; + if packet.flags & 1 == 0 { + packet.flags |= 57; + packet.position = input.position; + packet.normal = input.surface_up; + } + publish_slope(packet, c.tangent, history); + } else if reduction.forward_limit < f32::from_bits(0x3e99_999a) { + packet.flags |= 1024; + } else { + packet.flags &= !1; + } +} + +fn edge_output( + input: Input, + packet: &mut Packet, + records: &Records, + segments: &Segments, + reduction: &Reduction, +) { + packet.distance_168 = 0.; + packet.edge_position = input.position; + packet.edge_normal = input.surface_up; + if segments.last_surface_end == 0 { + return; + } + let distance = forward(input, segments.items[segments.last_surface_end - 1].end); + packet.distance_168 = if distance < f32::from_bits(0x3fd3_3333) { + distance + } else { + f32::from_bits(0x5015_02f9) + }; + if let Some(s) = segments + .items + .iter() + .find(|s| s.kind != Kind::Surface && s.length > f32::from_bits(0x3d4c_cccd)) + { + packet.distance_168 = forward(input, s.start); + } + if packet.flags & 1 != 0 + && input.surface_forward[1] > f32::from_bits(0x3e4c_cccd) + && f32::from_bits(0x3f33_3333) > packet.normal[1] + { + let condition = |r: &&super::contact_records::Record| { + r.normal[1] > f32::from_bits(0x3f73_3333) || r.flags & 16 != 0 + }; + let first = records + .surface + .iter() + .chain(&reduction.inactive_records) + .take(records.inserted_surface_count) + .find(condition); + if first.is_some_and(|r| dot3(sub(r.position, input.position), input.surface_up) > 0.) { + packet.flags |= 512; + } + let first = records.obstacle.iter().find(condition); + if first.is_some_and(|r| dot3(sub(r.position, input.position), input.surface_up) > 0.) { + packet.flags |= 512; + } + } + let mut remaining = length_inverse(input.velocity).0 * 0.25; + for s in &segments.items { + if s.kind != Kind::Surface && !(s.normal[1] > f32::from_bits(0x3f66_6666)) { + packet.edge_position = s.end; + packet.flags |= 4; + break; + } + packet.edge_normal = s.normal; + remaining -= s.length; + if remaining <= 0. { + packet.edge_position = + std::array::from_fn(|i| s.direction[i].mul_add(remaining, s.end[i])); + break; + } + } +} + +fn publish_slope(packet: &mut Packet, tangent: V, history: &mut History) { + // The native scalar path fuses x*x + z*z and clamps its horizontal length. + let sq = tangent[0].mul_add(tangent[0], tangent[2] * tangent[2]); + let mut inverse = crate::physics::reciprocal_sqrt::estimate(sq); + for _ in 0..2 { + inverse = (inverse * 0.5).mul_add((-sq).mul_add(inverse * inverse, 1.), inverse); + } + let length = if sq == 0. { 0. } else { sq * inverse }; + let minimum = f32::from_bits(0x3a83_126f); + let denominator = if minimum - length >= 0. { + minimum + } else { + length + }; + let slope = (1. / denominator) * tangent[1]; + packet.value_160 = slope; + let tan = crate::animation::foot_ik::post_contact::tangent; + let low = tan(f32::from_bits(0x3dd6_7750)); + let high = tan(f32::from_bits(0x3f0e_fa35)); + let offset = if packet.flags & 64 != 0 { 0 } else { 4 }; + let kind = if !(slope.abs() <= high) { + offset + if slope > 0. { 2 } else { 4 } + } else if !(slope.abs() <= low) { + offset + if slope > 0. { 1 } else { 3 } + } else { + 0 + }; + if kind == 0 { + history.cursor = 0; + history.stable_kind = 0; + history.samples = [0; 3]; + packet.kind_164 = 0; + return; + } + if history.samples.iter().all(|&previous| previous == kind) { + history.stable_kind = kind; + packet.kind_164 = kind; + } else { + history.cursor = (history.cursor + 1) % 3; + history.samples[history.cursor] = kind; + packet.kind_164 = history.stable_kind; + } +} + +#[cfg(test)] +mod tests { + use super::*; + #[test] + fn slope_kind_requires_three_prior_matching_samples_and_flat_resets_history() { + let mut packet = Packet { + flags: 64, + ..Packet::default() + }; + let mut history = History::default(); + for expected in [0, 0, 0, 2] { + publish_slope(&mut packet, [0., 1., 1., 0.], &mut history); + assert_eq!(packet.kind_164, expected); + } + publish_slope(&mut packet, [0., 0., 1., 0.], &mut history); + assert_eq!(packet.kind_164, 0); + assert_eq!(history.samples, [0; 3]); + assert_eq!(history.cursor, 0); + } +} diff --git a/crates/skate-core/src/player/offboard/contact_promotion.rs b/crates/skate-core/src/player/offboard/contact_promotion.rs index 4996aab4..a8643710 100644 --- a/crates/skate-core/src/player/offboard/contact_promotion.rs +++ b/crates/skate-core/src/player/offboard/contact_promotion.rs @@ -1,80 +1,140 @@ -//! Obstacle-to-surface promotion82D82498. Native sorting is a separate stage; -//! this routine consumes the same sorted input order and appends promoted hits. -use super::{contact_queries::Input,contact_records::{Records,Source,Direction}}; +//! Obstacle-to-surface promotion82D82498. Consumes sorted input order, appends +//! promoted hits and invokes the native record sort before returning. +use super::{ + contact_queries::Input, + contact_records::{Direction, Records, Source}, +}; -pub fn promote(input: Input,records: &mut Records) { - let original_count=records.surface.len(); +pub fn promote(input: Input, records: &mut Records) { + let original_count = records.surface.len(); for next in 1..original_count { - let previous=records.surface[next-1]; - let current=records.surface[next]; - let a=previous.coordinates; - let b=current.coordinates; - let mut low=if b[1]>a[1] {a[1]} else {b[1]}; - let mut high=if a[1]>b[1] {a[1]} else {b[1]}; - let mut index=0; - while indexp[0] || p[0]>b[0] {index+=1;continue;} - if low>p[1] { - records.obstacle[index].flags|=2; - index+=1;continue; + let previous = records.surface[next - 1]; + let current = records.surface[next]; + let a = previous.coordinates; + let b = current.coordinates; + let mut low = if b[1] > a[1] { a[1] } else { b[1] }; + let mut high = if a[1] > b[1] { a[1] } else { b[1] }; + let mut index = 0; + while index < records.obstacle.len() { + let obstacle = records.obstacle[index]; + let p = obstacle.coordinates; + if a[0] > p[0] || p[0] > b[0] { + index += 1; + continue; } - if high>p[1] || obstacle.distancecurrent.distance { - index+=1;continue; + if low > p[1] { + records.obstacle[index].flags |= 2; + index += 1; + continue; } - records.obstacle[index].flags|=2; - let mut lower=index; - let mut upper=index; - index+=1; - while indexf32::from_bits(0x3a83_126f) {break;} - records.obstacle[index].flags|=2; - if p[1]>=high && records.obstacle[lower].coordinates[1]>p[1] {lower=index;} - if p[1]>records.obstacle[upper].coordinates[1] {upper=index;} - index+=1; + if high > p[1] + || obstacle.distance < previous.distance + || obstacle.distance > current.distance + { + index += 1; + continue; } - records.obstacle[lower].flags=(records.obstacle[lower].flags & !2)|1; - records.obstacle[upper].flags=(records.obstacle[upper].flags & !2)|1; - let mut insert=|index: usize,low: &mut f32,high: &mut f32| { - let r=records.obstacle[index]; - let mut normal=r.normal; - records.insert(input,r.position,&mut normal,Source::Support,Direction::Forward,r.distance); - if r.coordinates[1]>*high {*low=*high;*high=r.coordinates[1];} - else if r.coordinates[1]>*low {*low=r.coordinates[1];} + records.obstacle[index].flags |= 2; + let mut lower = index; + let mut upper = index; + index += 1; + while index < records.obstacle.len() { + let p = records.obstacle[index].coordinates; + if (records.obstacle[lower].coordinates[0] - p[0]).abs() + > f32::from_bits(0x3a83_126f) + { + break; + } + records.obstacle[index].flags |= 2; + if p[1] >= high && records.obstacle[lower].coordinates[1] > p[1] { + lower = index; + } + if p[1] > records.obstacle[upper].coordinates[1] { + upper = index; + } + index += 1; + } + records.obstacle[lower].flags = (records.obstacle[lower].flags & !2) | 1; + records.obstacle[upper].flags = (records.obstacle[upper].flags & !2) | 1; + let mut insert = |index: usize, low: &mut f32, high: &mut f32| { + let r = records.obstacle[index]; + let mut normal = r.normal; + records.insert( + input, + r.position, + &mut normal, + Source::Support, + Direction::Forward, + r.distance, + ); + if r.coordinates[1] > *high { + *low = *high; + *high = r.coordinates[1]; + } else if r.coordinates[1] > *low { + *low = r.coordinates[1]; + } }; - insert(lower,&mut low,&mut high); - if upper!=lower {insert(upper,&mut low,&mut high);} + insert(lower, &mut low, &mut high); + if upper != lower { + insert(upper, &mut low, &mut high); + } //82D82900 increments even after the grouping loop has advanced the //cursor. Preserve this native skip of the first nonmatching record. - index+=1; + index += 1; } } + super::contact_sort::sort(&mut records.surface); } #[cfg(test)] mod tests { use super::*; use crate::player::offboard::contact_records::Record; - fn input() -> Input {Input {position:[0.;4],surface_forward:[0.,0.,1.,0.],surface_up:[0.,1.,0.,0.],surface_right:[1.,0.,0.,0.],velocity:[0.;4],animation_up:[0.,1.,0.,0.],animation_right:[1.,0.,0.,0.]}} - fn hit(y: f32,z: f32) -> Record {Record {position:[0.,y,z,0.],normal:[0.,1.,0.,0.],coordinates:[z,y,z,z],flags:0,distance:z}} + fn input() -> Input { + Input { + position: [0.; 4], + surface_forward: [0., 0., 1., 0.], + surface_up: [0., 1., 0., 0.], + surface_right: [1., 0., 0., 0.], + velocity: [0.; 4], + animation_up: [0., 1., 0., 0.], + animation_right: [1., 0., 0., 0.], + } + } + fn hit(y: f32, z: f32) -> Record { + Record { + position: [0., y, z, 0.], + normal: [0., 1., 0., 0.], + coordinates: [z, y, z, z], + flags: 0, + distance: z, + } + } #[test] fn below_surface_is_marked_and_vertical_group_promotes_only_extremes() { - let mut records=Records {surface:vec![hit(0.,0.),hit(0.,2.)], - obstacle:vec![hit(-0.1,0.5),hit(0.2,1.),hit(0.4,1.),hit(0.3,1.)],inserted_surface_count:2}; - promote(input(),&mut records); - assert_eq!(records.obstacle.iter().map(|r|r.flags).collect::>(),vec![2,1,1,2]); - assert_eq!(records.surface.len(),4); - assert_eq!(records.surface[2].position,[0.,0.2,1.,0.]); - assert_eq!(records.surface[3].position,[0.,0.4,1.,0.]); + let mut records = Records { + surface: vec![hit(0., 0.), hit(0., 2.)], + obstacle: vec![hit(-0.1, 0.5), hit(0.2, 1.), hit(0.4, 1.), hit(0.3, 1.)], + inserted_surface_count: 2, + }; + promote(input(), &mut records); + assert_eq!( + records.obstacle.iter().map(|r| r.flags).collect::>(), + vec![2, 1, 1, 2] + ); + assert_eq!(records.surface.len(), 4); + assert_eq!(records.surface[1].position, [0., 0.2, 1., 0.]); + assert_eq!(records.surface[2].position, [0., 0.4, 1., 0.]); } #[test] fn promotion_retains_native_group_cursor_skip() { - let mut records=Records {surface:vec![hit(0.,0.),hit(0.,2.)], - obstacle:vec![hit(0.2,0.5),hit(0.3,1.),hit(0.4,1.5)],inserted_surface_count:2}; - promote(input(),&mut records); - assert_eq!(records.obstacle[1].flags,0); - assert_eq!(records.surface.len(),4); + let mut records = Records { + surface: vec![hit(0., 0.), hit(0., 2.)], + obstacle: vec![hit(0.2, 0.5), hit(0.3, 1.), hit(0.4, 1.5)], + inserted_surface_count: 2, + }; + promote(input(), &mut records); + assert_eq!(records.obstacle[1].flags, 0); + assert_eq!(records.surface.len(), 4); } } diff --git a/crates/skate-core/src/player/offboard/contact_segments.rs b/crates/skate-core/src/player/offboard/contact_segments.rs index 8e8550bb..431dec37 100644 --- a/crates/skate-core/src/player/offboard/contact_segments.rs +++ b/crates/skate-core/src/player/offboard/contact_segments.rs @@ -1,10 +1,17 @@ //! Surface segment construction82D83438 and candidate insertion82D82380. //! Input records must already have passed the earlier native classifiers. -use super::{contact_queries::{Input,V}, contact_records::Record}; -use crate::physics::native_arithmetic::{dot3,reciprocal_estimate}; +use super::{ + contact_queries::{Input, V}, + contact_records::Record, +}; +use crate::physics::native_arithmetic::{dot3, reciprocal_estimate}; #[derive(Clone, Copy, Debug, PartialEq, Eq)] -pub enum Kind { Surface, Rising, Falling } +pub enum Kind { + Surface, + Rising, + Falling, +} #[derive(Clone, Copy, Debug)] pub struct Segment { pub kind: Kind, @@ -20,53 +27,86 @@ pub struct Segments { ///25232 is one past the last Surface segment, not the number of all segments. pub last_surface_end: usize, } -fn sub(a: V,b: V) -> V { std::array::from_fn(|i|a[i]-b[i]) } -fn scale(a: V,s: f32) -> V { a.map(|x|x*s) } -fn cross(a: V,b: V) -> V { - [(-a[2]).mul_add(b[1],a[1]*b[2]),(-a[0]).mul_add(b[2],a[2]*b[0]), - (-a[1]).mul_add(b[0],a[0]*b[1]),(-a[3]).mul_add(b[3],a[3]*b[3])] +fn sub(a: V, b: V) -> V { + std::array::from_fn(|i| a[i] - b[i]) } -fn length_inverse(v: V) -> (f32,f32) { - let sq=dot3(v,v); - let mut inverse=crate::physics::reciprocal_sqrt::estimate(sq); - for _ in 0..2 { inverse=(inverse*0.5).mul_add((-sq).mul_add(inverse*inverse,1.),inverse); } - (if sq==0. {0.} else {sq*inverse},inverse) +fn scale(a: V, s: f32) -> V { + a.map(|x| x * s) } -fn reciprocal(v: f32) -> f32 { - let mut r=reciprocal_estimate(v); - for _ in 0..2 { r=r.mul_add((-r).mul_add(v,1.),r); } +fn cross(a: V, b: V) -> V { + [ + (-a[2]).mul_add(b[1], a[1] * b[2]), + (-a[0]).mul_add(b[2], a[2] * b[0]), + (-a[1]).mul_add(b[0], a[0] * b[1]), + (-a[3]).mul_add(b[3], a[3] * b[3]), + ] +} +pub(super) fn length_inverse(v: V) -> (f32, f32) { + let sq = dot3(v, v); + let mut inverse = crate::physics::reciprocal_sqrt::estimate(sq); + for _ in 0..2 { + inverse = (inverse * 0.5).mul_add((-sq).mul_add(inverse * inverse, 1.), inverse); + } + (if sq == 0. { 0. } else { sq * inverse }, inverse) +} +pub(super) fn reciprocal(v: f32) -> f32 { + let mut r = reciprocal_estimate(v); + for _ in 0..2 { + r = r.mul_add((-r).mul_add(v, 1.), r); + } r } -fn normal(delta: V,input: Input) -> V { - let cross=cross(delta,input.surface_right); - let (len,inv)=length_inverse(cross); +fn normal(delta: V, input: Input) -> V { + let cross = cross(delta, input.surface_right); + let (len, inv) = length_inverse(cross); //830BD350 is initialized by82F826F8 from82181A88, not its zero dump image. - if len>f32::from_bits(0x3586_37bd) { scale(cross,inv) } else { input.surface_up } + if len > f32::from_bits(0x3586_37bd) { + scale(cross, inv) + } else { + input.surface_up + } } impl Segments { pub fn rebuild(&mut self, input: Input, records: &[Record]) { - self.items.clear(); self.last_surface_end=0; + self.items.clear(); + self.last_surface_end = 0; for pair in records.windows(2) { - let start=pair[0].position; - let end=pair[1].position; - let delta=sub(end,start); - let (length,_)=length_inverse(delta); - if length0.5 {Kind::Surface} - else if dot3(input.surface_up,direction)<0. {Kind::Falling} else {Kind::Rising}; - if kind!=Kind::Surface { - if let Some(previous)=self.items.last_mut().filter(|s|s.kind==kind) { - previous.end=end; - let delta=sub(end,previous.start); - previous.length=length_inverse(delta).0; - previous.direction=scale(delta,reciprocal(previous.length)); - previous.normal=normal(delta,input); + let start = pair[0].position; + let end = pair[1].position; + let delta = sub(end, start); + let (length, _) = length_inverse(delta); + if length < f32::from_bits(0x38d1_b717) { + continue; + } + let direction = scale(delta, reciprocal(length)); + let kind = if dot3(input.surface_forward, direction).abs() > 0.5 { + Kind::Surface + } else if dot3(input.surface_up, direction) < 0. { + Kind::Falling + } else { + Kind::Rising + }; + if kind != Kind::Surface { + if let Some(previous) = self.items.last_mut().filter(|s| s.kind == kind) { + previous.end = end; + let delta = sub(end, previous.start); + previous.length = length_inverse(delta).0; + previous.direction = scale(delta, reciprocal(previous.length)); + previous.normal = normal(delta, input); continue; } } - self.items.push(Segment {kind,start,end,direction,normal:normal(delta,input),length}); - if kind==Kind::Surface { self.last_surface_end=self.items.len(); } + self.items.push(Segment { + kind, + start, + end, + direction, + normal: normal(delta, input), + length, + }); + if kind == Kind::Surface { + self.last_surface_end = self.items.len(); + } } } } @@ -84,22 +124,75 @@ pub struct Candidate { pub kind: u32, } impl Candidate { + ///82D82288 uses input forward for a point closer than .02; otherwise the + ///un-normalized displacement is retained as the candidate tangent. + pub fn from_point( + input: Input, + segments: &Segments, + position: V, + normal: V, + segment_index: Option, + flags: u32, + kind: u32, + ) -> Self { + let delta = sub(position, input.position); + let tangent = if !(length_inverse(delta).0 >= f32::from_bits(0x3ca3_d70a)) { + input.surface_forward + } else { + delta + }; + Self::new( + input, + segments, + position, + normal, + tangent, + segment_index, + flags, + kind, + ) + } /// Caller supplies the actual native segment index; absent means r7=-1. - pub fn new(input: Input,segments: &Segments,position: V,normal: V,tangent: V, - segment_index: Option,flags: u32,kind: u32) -> Self - { - let distance=dot3(input.surface_forward,sub(position,input.position)); - let mut result=Self {position,normal,tangent,flags,minimum_distance:distance, - maximum_distance:distance,position_distance:distance,segment_index,kind}; - if let Some(index)=segment_index { - let segment=segments.items[index]; - if segment.kind!=Kind::Surface { - let endpoint=if segment.kind==Kind::Falling {segment.start} else {segment.end}; - let edge_distance=dot3(input.surface_forward,sub(endpoint,input.position)); - if edge_distance, + flags: u32, + kind: u32, + ) -> Self { + let distance = dot3(input.surface_forward, sub(position, input.position)); + let mut result = Self { + position, + normal, + tangent, + flags, + minimum_distance: distance, + maximum_distance: distance, + position_distance: distance, + segment_index, + kind, + }; + if let Some(index) = segment_index { + let segment = segments.items[index]; + if segment.kind != Kind::Surface { + let endpoint = if segment.kind == Kind::Falling { + segment.start + } else { + segment.end + }; + let edge_distance = dot3(input.surface_forward, sub(endpoint, input.position)); + if edge_distance < distance { + result.minimum_distance = edge_distance; + } else { + result.maximum_distance = edge_distance; + } } - } else {result.minimum_distance=0.;} + } else { + result.minimum_distance = 0.; + } result } } @@ -108,35 +201,76 @@ impl Candidate { mod tests { use super::*; fn input() -> Input { - Input {position:[0.;4],surface_forward:[0.,0.,1.,0.],surface_up:[0.,1.,0.,0.], - surface_right:[1.,0.,0.,0.],velocity:[0.;4],animation_up:[0.,1.,0.,0.],animation_right:[1.,0.,0.,0.]} + Input { + position: [0.; 4], + surface_forward: [0., 0., 1., 0.], + surface_up: [0., 1., 0., 0.], + surface_right: [1., 0., 0., 0.], + velocity: [0.; 4], + animation_up: [0., 1., 0., 0.], + animation_right: [1., 0., 0., 0.], + } } - fn record(y: f32,z: f32) -> Record { - Record {position:[0.,y,z,0.],normal:[0.,1.,0.,0.],coordinates:[z,y,z,z],flags:0,distance:z} + fn record(y: f32, z: f32) -> Record { + Record { + position: [0., y, z, 0.], + normal: [0., 1., 0., 0.], + coordinates: [z, y, z, z], + flags: 0, + distance: z, + } } #[test] fn consecutive_vertical_segments_merge_but_surface_segments_remain_distinct() { - let mut segments=Segments::default(); - segments.rebuild(input(),&[record(0.,0.),record(0.,1.),record(1.,1.),record(2.,1.),record(2.,2.),record(2.,3.)]); - assert_eq!(segments.items.len(),4); - assert_eq!(segments.last_surface_end,4); - assert_eq!(segments.items[1].kind,Kind::Rising); - assert_eq!(segments.items[1].length,2.); - assert_eq!(segments.items[1].normal,[0.,0.,-1.,0.]); - assert_eq!(segments.items[0].normal,[0.,1.,0.,0.]); - segments.rebuild(input(),&[record(0.,0.),record(1.,0.)]); - assert_eq!(segments.last_surface_end,0); + let mut segments = Segments::default(); + segments.rebuild( + input(), + &[ + record(0., 0.), + record(0., 1.), + record(1., 1.), + record(2., 1.), + record(2., 2.), + record(2., 3.), + ], + ); + assert_eq!(segments.items.len(), 4); + assert_eq!(segments.last_surface_end, 4); + assert_eq!(segments.items[1].kind, Kind::Rising); + assert_eq!(segments.items[1].length, 2.); + assert_eq!(segments.items[1].normal, [0., 0., -1., 0.]); + assert_eq!(segments.items[0].normal, [0., 1., 0., 0.]); + segments.rebuild(input(), &[record(0., 0.), record(1., 0.)]); + assert_eq!(segments.last_surface_end, 0); } #[test] fn candidates_expand_only_toward_the_native_selected_endpoint() { - let mut segments=Segments::default(); - segments.rebuild(input(),&[record(0.,0.),record(2.,0.5)]); - let candidate=Candidate::new(input(),&segments,[0.,1.,0.25,0.],[0.,1.,0.,0.],[1.,0.,0.,0.],Some(0),64,4); - assert_eq!(candidate.minimum_distance,0.25); - assert_eq!(candidate.maximum_distance,0.5); - assert_eq!(candidate.position_distance,0.25); - let no_segment=Candidate::new(input(),&segments,[0.,1.,0.25,0.],[0.;4],[0.;4],None,0,0); - assert_eq!(no_segment.minimum_distance,0.); - assert_eq!(no_segment.maximum_distance,0.25); + let mut segments = Segments::default(); + segments.rebuild(input(), &[record(0., 0.), record(2., 0.5)]); + let candidate = Candidate::new( + input(), + &segments, + [0., 1., 0.25, 0.], + [0., 1., 0., 0.], + [1., 0., 0., 0.], + Some(0), + 64, + 4, + ); + assert_eq!(candidate.minimum_distance, 0.25); + assert_eq!(candidate.maximum_distance, 0.5); + assert_eq!(candidate.position_distance, 0.25); + let no_segment = Candidate::new( + input(), + &segments, + [0., 1., 0.25, 0.], + [0.; 4], + [0.; 4], + None, + 0, + 0, + ); + assert_eq!(no_segment.minimum_distance, 0.); + assert_eq!(no_segment.maximum_distance, 0.25); } } diff --git a/crates/skate-core/src/player/offboard/contact_simplify.rs b/crates/skate-core/src/player/offboard/contact_simplify.rs new file mode 100644 index 00000000..40de23a4 --- /dev/null +++ b/crates/skate-core/src/player/offboard/contact_simplify.rs @@ -0,0 +1,114 @@ +//! Native record reduction82D830A8. Marks discarded distances, invokes the +//! native sort and truncates to the retained count; neighbors are not rewritten. +use super::{ + contact_queries::Input, + contact_records::Record, + contact_segments::{length_inverse, reciprocal}, +}; +use crate::physics::native_arithmetic::dot3; + +/// Returns Toolkit284's forward limit, reset even for fewer than two records. +pub struct Reduction { + pub forward_limit: f32, + /// Native count14928 shrinks, but84860 still scans count14932 including + ///the sorted tail in the fixed backing array. Retain these physical records. + pub inactive_records: Vec, +} +pub fn simplify(input: Input, records: &mut Vec) -> Reduction { + let mut limit = f32::from_bits(0x5015_02f9); + let original = records.len(); + if original < 2 { + return Reduction { + forward_limit: limit, + inactive_records: Vec::new(), + }; + } + let sentinel = records[original - 1].distance + 1.; + let mut retained = original; + for i in 1..original - 1 { + if records[i].flags & 8 != 0 { + retained -= original - i - 1; + limit = dot3( + std::array::from_fn(|k| records[i].position[k] - input.position[k]), + input.surface_forward, + ); + break; + } + let a = records[i - 1].coordinates; + let b = records[i].coordinates; + let c = records[i + 1].coordinates; + let incoming = [b[0] - a[0], b[1] - a[1], 0., 0.]; + let outgoing = [c[0] - b[0], c[1] - b[1], 0., 0.]; + let in_length = length_inverse(incoming).0; + let out_length = length_inverse(outgoing).0; + let mut remove = in_length < f32::from_bits(0x3ca3_d70a); + if !remove && !(out_length < f32::from_bits(0x3ca3_d70a)) { + if !(dot3(incoming, outgoing) < 0. && dot3(incoming, input.surface_up) >= 0.) { + let a = incoming.map(|v| v * reciprocal(in_length)); + let b = outgoing.map(|v| v * reciprocal(out_length)); + let cross = [ + (-a[2]).mul_add(b[1], a[1] * b[2]), + (-a[0]).mul_add(b[2], a[2] * b[0]), + (-a[1]).mul_add(b[0], a[0] * b[1]), + (-a[3]).mul_add(b[3], a[3] * b[3]), + ]; + remove = !(length_inverse(cross).0 >= f32::from_bits(0x3d4c_cccd)); + } + } + if remove { + records[i].distance = sentinel; + retained -= 1; + } + } + super::contact_sort::sort(records); + let inactive_records = records.split_off(retained); + Reduction { + forward_limit: limit, + inactive_records, + } +} + +#[cfg(test)] +mod tests { + use super::*; + fn input() -> Input { + Input { + position: [0.; 4], + surface_forward: [0., 0., 1., 0.], + surface_up: [0., 1., 0., 0.], + surface_right: [1., 0., 0., 0.], + velocity: [0.; 4], + animation_up: [0., 1., 0., 0.], + animation_right: [1., 0., 0., 0.], + } + } + fn r(y: f32, z: f32) -> Record { + Record { + position: [0., y, z, 0.], + normal: [0., 1., 0., 0.], + coordinates: [z, y, z, z], + flags: 0, + distance: z, + } + } + #[test] + fn flat_samples_reduce_to_endpoints_but_step_corner_survives() { + let mut records = vec![r(0., 0.), r(0., 0.5), r(0., 1.), r(0., 1.5)]; + let reduced = simplify(input(), &mut records); + assert_eq!(reduced.forward_limit.to_bits(), 0x501502f9); + assert_eq!(reduced.inactive_records.len(), 2); + assert_eq!(records.len(), 2); + assert_eq!(records[1].distance, 1.5); + let mut records = vec![r(0., 0.), r(0., 1.), r(1., 1.), r(1., 2.)]; + simplify(input(), &mut records); + assert_eq!(records.len(), 4); + } + #[test] + fn elevated_record_caps_forward_search_after_prior_removals() { + let mut records = vec![r(0., 0.), r(0., 0.5), r(0., 1.), r(1., 1.5), r(1., 2.)]; + records[3].flags = 8; + assert_eq!(simplify(input(), &mut records).forward_limit, 1.5); + assert_eq!(records.len(), 3); + assert_eq!(records[2].position, [0., 1., 1.5, 0.]); + } +} diff --git a/crates/skate-core/src/player/offboard/contact_sort.rs b/crates/skate-core/src/player/offboard/contact_sort.rs new file mode 100644 index 00000000..56b4e3ce --- /dev/null +++ b/crates/skate-core/src/player/offboard/contact_sort.rs @@ -0,0 +1,174 @@ +//! Record sort82D95390, introsort82D95500, partition82D95AB0 and heap fallback. +//! Comparator82D81028 uses distance52 only. Equal distances must retain the +//! original algorithm's order because later classifiers scan vertical groups. +use super::{contact_records::Record, contact_segments::Candidate}; +pub trait Distance: Copy { + fn distance(self) -> f32; +} +impl Distance for Record { + fn distance(self) -> f32 { + self.distance + } +} +impl Distance for Candidate { + fn distance(self) -> f32 { + self.position_distance + } +} +fn less(a: T, b: T) -> bool { + a.distance() < b.distance() +} + +pub fn sort(records: &mut [T]) { + if records.is_empty() { + return; + } + let depth = 2 * (usize::BITS - 1 - records.len().leading_zeros()); + introsort(records, depth); + //82D95650 sorts the first28, then95390 inserts each remaining record. + //A bounded host cursor also avoids out-of-bounds reads for unordered input. + insertion(records); +} +fn insertion(a: &mut [T]) { + for index in 1..a.len() { + let value = a[index]; + let mut hole = index; + while hole > 0 && less(value, a[hole - 1]) { + a[hole] = a[hole - 1]; + hole -= 1; + } + a[hole] = value; + } +} +fn introsort(mut a: &mut [T], mut depth: u32) { + while a.len() > 28 && depth > 0 { + let first = a[0]; + let middle = a[a.len() / 2]; + let last = a[a.len() - 1]; + let pivot = if less(first, middle) { + if less(middle, last) { + middle + } else if less(first, last) { + last + } else { + first + } + } else if less(first, last) { + first + } else if less(middle, last) { + last + } else { + middle + }; + let mut left = 0; + let mut right = a.len(); + let cut = loop { + while less(a[left], pivot) { + left += 1; + } + right -= 1; + while less(pivot, a[right]) { + right -= 1; + } + if left >= right { + break left; + } + a.swap(left, right); + left += 1; + }; + depth -= 1; + let (lower, upper) = a.split_at_mut(cut); + introsort(upper, depth); + a = lower; + } + if depth == 0 { + heap_sort(a); + } +} +fn heap_sort(a: &mut [T]) { + if a.len() < 2 { + return; + } + for index in (0..=(a.len() - 2) / 2).rev() { + let value = a[index]; + adjust_heap(a, index, value); + } + for end in (1..a.len()).rev() { + let value = a[end]; + a[end] = a[0]; + adjust_heap(&mut a[..end], 0, value); + } +} +fn adjust_heap(a: &mut [T], top: usize, value: T) { + let mut hole = top; + let mut child = 2 * (hole + 1); + while child < a.len() { + //96070 chooses the RIGHT child on equal distances. + if less(a[child], a[child - 1]) { + child -= 1; + } + a[hole] = a[child]; + hole = child; + child = 2 * (hole + 1); + } + if child == a.len() { + a[hole] = a[child - 1]; + hole = child - 1; + } + while hole > top { + let parent = (hole - 1) / 2; + if !less(a[parent], value) { + break; + } + a[hole] = a[parent]; + hole = parent; + } + a[hole] = value; +} + +#[cfg(test)] +mod tests { + use super::*; + fn records(n: usize) -> Vec { + (0..n) + .map(|i| Record { + position: [i as f32, 0., 0., 0.], + normal: [0.; 4], + coordinates: [0.; 4], + flags: 0, + distance: 1., + }) + .collect() + } + #[test] + fn equal_distance_order_matches_native_partition_threshold() { + for n in [28, 32, 64] { + let mut a = records(n); + sort(&mut a); + let expected: Vec<_> = match n { + 28 => (0..28).collect(), + 32 => (0..32).rev().collect(), + 64 => (32..64).chain(0..32).collect(), + _ => unreachable!(), + }; + assert_eq!( + a.iter().map(|r| r.position[0] as usize).collect::>(), + expected + ); + } + } + #[test] + fn sort_and_heap_fallback_order_finite_distances() { + for n in 0..=128 { + let mut a = records(n); + for (i, r) in a.iter_mut().enumerate() { + r.distance = ((i * 17 + 3) % 29) as f32 - 14.; + } + let mut heap = a.clone(); + sort(&mut a); + heap_sort(&mut heap); + assert!(a.windows(2).all(|w| w[0].distance <= w[1].distance)); + assert!(heap.windows(2).all(|w| w[0].distance <= w[1].distance)); + } + } +} diff --git a/crates/skate-core/src/player/offboard/contact_toolkit.rs b/crates/skate-core/src/player/offboard/contact_toolkit.rs new file mode 100644 index 00000000..a1cf45e3 --- /dev/null +++ b/crates/skate-core/src/player/offboard/contact_toolkit.rs @@ -0,0 +1,271 @@ +//! BipedToolkit refresh82D81610. Query observations are owned by the scene; +//! this owner retains native contact history and executes the classifier chain. +use super::{ + contact_intersections::plane_segment, + contact_output::History, + contact_packet::{Packet, SupportHit}, + contact_queries::{Input, Layout, V}, + contact_records::{Direction, Records, Source}, + contact_segments::Segments, + ground_query::Edge, +}; +use crate::{math::Vector3, physics::native_arithmetic::dot3}; +#[derive(Clone, Copy, Debug)] +pub struct Hit { + pub position: V, + pub normal: V, +} +pub struct Observations { + pub input: Input, + pub support: [Option; 3], + /// Provider IDs from Layout, including the reverse horizontal queries. + pub obstacles: [Option; 44], + /// Actual82C1EAD8 results in native provider order and capped at40. + pub edges: Vec, +} +#[derive(Default)] +pub struct Toolkit { + pub packet: Packet, + pub history: History, + pub contact_age: i32, + pub pending: Option, +} +fn vector(v: Vector3) -> V { + [v.x, v.y, v.z, 0.] +} +fn xyz(v: V) -> Vector3 { + Vector3::new(v[0], v[1], v[2]) +} +fn sub(a: V, b: V) -> V { + std::array::from_fn(|i| a[i] - b[i]) +} +impl Toolkit { + /// Player Update82DB4048 completes the preceding Sync's submission before + /// any physical-state PreUpdate. The source decrements320, then refreshes + /// unconditionally (refresh itself publishes30 even with no pending query). + pub fn begin_player_update(&mut self, layout: &Layout) { + self.contact_age = self.contact_age.saturating_sub(1).max(0); + self.refresh(layout); + } + /// Reset82D30BD0 / Exit82D30CC0 must finish outstanding providers before + /// clearing the completion latch and classifier history. + pub fn reset_contacts(&mut self, layout: &Layout) { + self.refresh(layout); + self.contact_age = 0; + self.history = History::default(); + } + pub fn submit(&mut self, observations: Observations) { + self.pending = Some(observations); + self.contact_age = 0; + } + pub fn refresh(&mut self, layout: &Layout) { + self.packet = Packet::default(); + self.contact_age = 30; + let Some(observations) = self.pending.take() else { + return; + }; + let input = observations.input; + let previous_flags = self.history.retained_candidate.map_or(0, |c| c.flags); + let mut records = Records::default(); + self.packet + .consume_support(input, observations.support, previous_flags, &mut records); + gather(layout, &observations, &mut records); + super::contact_sort::sort(&mut records.surface); + super::contact_sort::sort(&mut records.obstacle); + super::contact_promotion::promote(input, &mut records); + super::contact_intersections::constrain(input, &mut records); + let reduction = super::contact_simplify::simplify(input, &mut records.surface); + self.packet.distance_172 = reduction.forward_limit; + let mut segments = Segments::default(); + segments.rebuild(input, &records.surface); + let mut candidates = super::contact_classify::candidates( + input, + &self.packet, + &segments, + reduction.forward_limit, + previous_flags, + ); + super::contact_output::publish( + input, + &mut self.packet, + &records, + &segments, + &reduction, + &mut candidates, + &mut self.history, + ); + } +} + +/// Bounds at19056/19072, used by the scene to obtain real query edges. +pub fn edge_bounds(input: Input) -> (V, V) { + let center: V = std::array::from_fn(|i| { + input.surface_up[i].mul_add( + f32::from_bits(0x3e99_9999), + input.surface_forward[i].mul_add(f32::from_bits(0x3f66_6666), input.position[i]), + ) + }); + let extent: V = std::array::from_fn(|i| { + input.surface_forward[i].abs().mul_add( + f32::from_bits(0x3f66_6666), + input.surface_up[i].abs().mul_add( + f32::from_bits(0x3f8c_cccd), + input.surface_right[i].abs() * f32::from_bits(0x3d19_999a), + ), + ) + }); + ( + std::array::from_fn(|i| center[i] - extent[i]), + std::array::from_fn(|i| center[i] + extent[i]), + ) +} +fn edge_angle(delta: V, forward: V) -> f32 { + let angle = crate::physics::board_ground::angle_between(xyz(delta), xyz(forward)); + let turns = angle * f32::from_bits(0x3e22_f983); + let fraction = turns - turns.floor(); + let wrapped = (fraction - if fraction > 0.5 { 1. } else { 0. }) * f32::from_bits(0x40c9_0fdb); + let sign = if wrapped > 0. { 1. } else { -1. }; + let absolute = wrapped * sign; + sign * if absolute > f32::from_bits(0x3fc9_0fdb) { + absolute - f32::from_bits(0x4049_0fdb) + } else { + absolute + } +} +fn gather(layout: &Layout, observations: &Observations, records: &mut Records) { + let input = observations.input; + let (_, world) = layout.world_probes(input); + let mut overrides: Vec> = vec![None; layout.vertical.len()]; + for edge in observations.edges.iter().take(40) { + let start = vector(edge.start); + let end = vector(edge.end); + let delta = sub(end, start); + if edge_angle(delta, input.surface_forward).abs() > 30. * f32::from_bits(0x3c8e_fa35) { + if let Some(position) = plane_segment(input.position, input.surface_right, start, end) { + let mut normal = input.surface_up; + records.insert( + input, + position, + &mut normal, + Source::Intersection, + Direction::None, + -1., + ); + } + continue; + } + let a = dot3(sub(start, input.position), input.surface_forward); + let b = dot3(sub(end, input.position), input.surface_forward); + let crossing: V = std::array::from_fn(|i| delta[i].mul_add(a / (a - b), start[i])); + if dot3(input.surface_right, sub(crossing, input.position)).abs() > 0.5 { + continue; + } + let (low, high) = if a - b >= 0. { (b, a) } else { (a, b) }; + for (i, probe) in layout.vertical.iter().enumerate() { + let forward = probe.start[2]; + if !(forward >= low && high >= forward) { + continue; + } + let t = (forward - a) / (b - a); + let edge_point: V = std::array::from_fn(|k| delta[k].mul_add(t, start[k])); + let base = world[i].start; + let h = dot3(input.surface_up, sub(edge_point, base)); + let point: V = std::array::from_fn(|k| input.surface_up[k].mul_add(h, base[k])); + if dot3(input.surface_right, sub(point, edge_point)).abs() + >= f32::from_bits(0x3d4c_cccd) + { + continue; + } + if overrides[i].is_none_or(|previous| point[1] > previous[1]) { + overrides[i] = Some(point); + } + } + } + for (i, probe) in layout.vertical.iter().enumerate() { + let hit = observations.obstacles[probe.id]; + let chosen = match (hit, overrides[i]) { + (Some(hit), Some(position)) if position[1] > hit.position[1] => Some(Hit { + position, + normal: input.surface_up, + }), + (Some(hit), _) => Some(hit), + (None, Some(position)) => Some(Hit { + position, + normal: input.surface_up, + }), + (None, None) => None, + }; + if let Some(hit) = chosen { + let mut normal = hit.normal; + records.insert( + input, + hit.position, + &mut normal, + Source::Support, + Direction::None, + -1., + ); + } + } + for probe in &layout.horizontal { + for id in std::iter::once(probe.id).chain(probe.reverse_id) { + if let Some(hit) = observations.obstacles[id] { + let mut normal = hit.normal; + records.insert( + input, + hit.position, + &mut normal, + Source::Probe, + Direction::None, + -1., + ); + } + } + } +} + +#[cfg(test)] +mod tests { + use super::*; + #[test] + fn reduced_forward_limit_reaches_the_retained_ground_packet() { + let input = Input { + position: [0.; 4], + surface_right: [1., 0., 0., 0.], + surface_up: [0., 1., 0., 0.], + surface_forward: [0., 0., 1., 0.], + animation_right: [1., 0., 0., 0.], + animation_up: [0., 1., 0., 0.], + velocity: [0., 0., 2., 0.], + }; + let layout = Layout::default(); + let mut obstacles = [None; 44]; + for i in 0..3 { + obstacles[layout.vertical[i].id] = Some(Hit { + position: [ + 0., + if i == 0 { 0. } else { 1. }, + layout.vertical[i].start[2], + 0., + ], + normal: input.surface_up, + }); + } + let support = SupportHit { + position: [0.; 4], + normal: input.surface_up, + frame: Packet::default().support_frame, + support_id: 1, + }; + let mut toolkit = Toolkit::default(); + toolkit.submit(Observations { + input, + support: [Some(support), None, None], + obstacles, + edges: Vec::new(), + }); + toolkit.refresh(&layout); + assert_eq!(toolkit.packet.distance_172, layout.vertical[1].start[2]); + assert!(toolkit.packet.position.iter().all(|x| x.is_finite())); + } +} diff --git a/crates/skate-core/src/player/offboard/mod.rs b/crates/skate-core/src/player/offboard/mod.rs index e182330e..954665e1 100644 --- a/crates/skate-core/src/player/offboard/mod.rs +++ b/crates/skate-core/src/player/offboard/mod.rs @@ -25,3 +25,11 @@ pub mod contact_packet; pub mod contact_segments; pub mod contact_promotion; pub mod ground_job; +pub mod contact_sort; +pub mod contact_simplify; +pub mod contact_geometry; +pub mod contact_candidates; +pub mod contact_classify; +pub mod contact_output; +pub mod contact_intersections; +pub mod contact_toolkit; diff --git a/crates/skate-game/src/physics/offboard/contact_queries.rs b/crates/skate-game/src/physics/offboard/contact_queries.rs index 0272b57b..eef6ec26 100644 --- a/crates/skate-game/src/physics/offboard/contact_queries.rs +++ b/crates/skate-game/src/physics/offboard/contact_queries.rs @@ -2,8 +2,11 @@ //! packet owns observations; the world's geometry and acceleration tree stay shared. use skate_core::{ math::Vector3, - physics::{board_world::{BoardWorld, query_metadata::QueryPool}, - drive_frames::RetailAffineTransform, triangle_query::{TriangleLineHit, triangle_segment}}, + physics::{ + board_world::{BoardWorld, query_metadata::QueryPool}, + drive_frames::RetailAffineTransform, + triangle_query::{TriangleLineHit, triangle_segment}, + }, player::offboard::contact_queries::{Input, Layout, Probe}, }; @@ -21,35 +24,130 @@ pub(crate) struct Completed { pub obstacles: Vec>, } -pub(crate) fn submit(world: &BoardWorld, layout: &Layout, input: Input, matching_id: u32) - -> Result -{ +/// Connect actual canonical scene observations to the recovered classifier. +pub(crate) fn submit_toolkit( + world: &BoardWorld, + layout: &Layout, + input: Input, + matching_id: u32, +) -> Result { + use skate_core::player::offboard::{ + contact_packet::SupportHit, contact_toolkit, ground_query::Edge, + }; + let completed = submit(world, layout, input, matching_id)?; + let v = |p: Vector3| [p.x, p.y, p.z, 0.]; + let support = completed.support.map(|h| { + h.map(|h| { + let f = h.support_frame; + let b = f.basis.columns; + SupportHit { + position: v(h.geometry.position), + normal: v(h.geometry.normal), + frame: [ + [b[0][0], b[0][1], b[0][2], 0.], + [b[1][0], b[1][1], b[1][2], 0.], + [b[2][0], b[2][1], b[2][2], 0.], + v(f.translation), + ], + // Guest query136 identifies the supporting body. Canonical mesh + // identities replace guest pointers in this authored static scene. + support_id: u32::try_from(h.mesh_index + 1) + .expect("canonical map mesh identity fits u32"), + } + }) + }); + let obstacles: Vec<_> = completed + .obstacles + .into_iter() + .map(|h| { + h.map(|h| contact_toolkit::Hit { + position: v(h.geometry.position), + normal: v(h.geometry.normal), + }) + }) + .collect(); + let obstacles = obstacles.try_into().map_err(|_: Vec<_>| { + "Biped query provider did not complete all44 native query slots".to_owned() + })?; + let metadata = world.query_metadata().map_err(str::to_owned)?; + let (min, max) = contact_toolkit::edge_bounds(input); + let bounds = skate_core::physics::board_world::query_metadata::Bounds { + min: xyz(min), + max: xyz(max), + }; + // This BoardWorld owns an authored static scene; it has no dynamic/vehicle + // edge providers. Preserve static provider order and the native shared cap. + let edges = metadata + .static_edges + .iter() + .filter(|edge| edge.local_bounds.overlaps(bounds)) + .take(40) + .map(|edge| Edge { + start: edge.start, + end: edge.end, + }) + .collect(); + Ok(contact_toolkit::Observations { + input, + support, + obstacles, + edges, + }) +} + +pub(crate) fn submit( + world: &BoardWorld, + layout: &Layout, + input: Input, + matching_id: u32, +) -> Result { let (support, obstacles) = layout.world_probes(input); let support = support.map(|probe| query(world, probe, matching_id)); - let [a,b,c] = support; + let [a, b, c] = support; let mut hits = vec![None; obstacles.iter().map(|p| p.id + 1).max().unwrap_or(0)]; for probe in obstacles { hits[probe.id] = query(world, probe, matching_id)?; } - Ok(Completed { input, support: [a?,b?,c?], obstacles: hits }) + Ok(Completed { + input, + support: [a?, b?, c?], + obstacles: hits, + }) } -fn xyz(v: [f32;4]) -> Vector3 { Vector3::new(v[0],v[1],v[2]) } +fn xyz(v: [f32; 4]) -> Vector3 { + Vector3::new(v[0], v[1], v[2]) +} fn query(world: &BoardWorld, probe: Probe, matching_id: u32) -> Result, String> { let metadata = world.query_metadata().map_err(str::to_owned)?; let start = xyz(probe.start); let end = xyz(probe.end); - let delta = Vector3::new(end.x-start.x,end.y-start.y,end.z-start.z); + let delta = Vector3::new(end.x - start.x, end.y - start.y, end.z - start.z); let mut nearest: Option = None; // Accelerate the canonical identity-transform map representation. Authored // transformed meshes retain the same geometry path without incorrect culling. - let candidates: Vec<_> = if metadata.meshes.iter().all(|m| m.local_to_world == RetailAffineTransform::IDENTITY) { - world.line_candidates(start, end, probe.radius).map(|(i,_)| i).collect() - } else { (0..world.triangles().len()).collect() }; + let candidates: Vec<_> = if metadata + .meshes + .iter() + .all(|m| m.local_to_world == RetailAffineTransform::IDENTITY) + { + world + .line_candidates(start, end, probe.radius) + .map(|(i, _)| i) + .collect() + } else { + (0..world.triangles().len()).collect() + }; for pool in [QueryPool::Ground, QueryPool::Island, QueryPool::Conditional] { - if pool == QueryPool::Conditional && metadata.island_flags != 3 { continue; } + if pool == QueryPool::Conditional && metadata.island_flags != 3 { + continue; + } for (mesh_index, mesh) in metadata.meshes.iter().enumerate() { - if mesh.pool != pool || !(mesh.matching_group == -1 || mesh.matching_group == matching_id as i32) { continue; } + if mesh.pool != pool + || !(mesh.matching_group == -1 || mesh.matching_group == matching_id as i32) + { + continue; + } let first = candidates.partition_point(|&i| i < mesh.triangle_range.start); let last = candidates.partition_point(|&i| i < mesh.triangle_range.end); for &index in &candidates[first..last] { @@ -59,18 +157,34 @@ fn query(world: &BoardWorld, probe: Probe, matching_id: u32) -> Result