Recover native biped contact promotion, segments and ground job production

crates/skate-core/src/player/offboard/contact_promotion.rs

@@ -0,0 +1,80 @@
+//! 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}};
+
+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 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 low>p[1] {
+                records.obstacle[index].flags|=2;
+                index+=1;continue;
+            }
+            if high>p[1] || obstacle.distance<previous.distance || obstacle.distance>current.distance {
+                index+=1;continue;
+            }
+            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);}
+            //82D82900 increments even after the grouping loop has advanced the
+            //cursor. Preserve this native skip of the first nonmatching record.
+            index+=1;
+        }
+    }
+}
+
+#[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}}
+    #[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<_>>(),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.]);
+    }
+    #[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);
+    }
+}

crates/skate-core/src/player/offboard/contact_segments.rs

@@ -0,0 +1,142 @@
+//! 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};
+
+#[derive(Clone, Copy, Debug, PartialEq, Eq)]
+pub enum Kind { Surface, Rising, Falling }
+#[derive(Clone, Copy, Debug)]
+pub struct Segment {
+    pub kind: Kind,
+    pub start: V,
+    pub end: V,
+    pub direction: V,
+    pub normal: V,
+    pub length: f32,
+}
+#[derive(Default)]
+pub struct Segments {
+    pub items: Vec<Segment>,
+    ///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 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 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);
+    //830BD350 is initialized by82F826F8 from82181A88, not its zero dump image.
+    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;
+        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 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(); }
+        }
+    }
+}
+
+#[derive(Clone, Copy, Debug)]
+pub struct Candidate {
+    pub position: V,
+    pub normal: V,
+    pub tangent: V,
+    pub flags: u32,
+    pub minimum_distance: f32,
+    pub maximum_distance: f32,
+    pub position_distance: f32,
+    pub segment_index: Option<usize>,
+    pub kind: u32,
+}
+impl Candidate {
+    /// 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<usize>,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.;}
+        result
+    }
+}
+
+#[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 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);
+    }
+    #[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);
+    }
+}

crates/skate-core/src/player/offboard/ground_job.rs

@@ -0,0 +1,135 @@
+//! BipedGround PreUpdate82D30D30 job production. The scene/host completes
+//! contact queries before this call and retains the resulting contact packet.
+use super::{contact_packet::Packet,controller::{GroundJob,Frame,Vector},
+    ground_entry::State,ground_input::{self,GroundInput},ground_query};
+use crate::{math::Vector3,point_graph::PointGraph};
+
+pub struct Input {
+    pub previous_state: u32,
+    pub frames_since_teleport: u32,
+    pub fallback_line_hit: Option<Vector>,
+    pub controls: GroundInput,
+    pub flags_2476: u32,
+    pub flags_2480: u32,
+    pub flags_2484: u32,
+    pub flags_2488: u32,
+    pub animation_motion_2864: Vector,
+    pub duration_2896: f32,
+    pub phase_2900: f32,
+    pub override_duration_2904: f32,
+    pub position_592: Vector,
+    pub velocity_608: Vector,
+    pub skeleton_motion_16320: Vector,
+    pub skeleton_displacements_16288_16304: [Vector;2],
+}
+pub struct Prepared {
+    pub job: GroundJob,
+    /// Retained until FillPhysOut82D32D38, including754 which is not a controller input.
+    pub geometry_flags_752_to_754: [bool;3],
+}
+fn xyz(v: Vector) -> Vector3 {Vector3::new(v[0],v[1],v[2])}
+fn vector(v: Vector3) -> Vector {[v.x,v.y,v.z,0.]}
+fn frame(f: Frame) -> ground_query::Frame {
+    ground_query::Frame {right:xyz(f[0]),up:xyz(f[1]),forward:xyz(f[2]),position:xyz(f[3])}
+}
+
+/// A completed toolkit packet exists only when the native contact age320>0.
+/// Missing completion preserves retained contact fields, except for the original
+/// preceding-riding-state line fallback. No synthetic floor hit is introduced.
+pub fn prepare(state: &mut State,retained: &mut Packet,completed: Option<Packet>,
+    geometry: Option<ground_query::GroundGeometry>,input: &Input,
+    movement_curve: &PointGraph<8>,turn_curve: &PointGraph<8>) -> Prepared
+{
+    let timestep=f32::from_bits(0x3c88_8889);
+    state.distance_164+=timestep;
+    let mut both_feet=true;
+    if let Some(packet)=completed {
+        *retained=packet;
+        both_feet=retained.flags & 0x30 == 0x30;
+    } else if input.previous_state!=501 {
+        if let Some(position)=input.fallback_line_hit {
+            retained.flags=1;
+            retained.position=position;
+            retained.normal=state.frame_80[1];
+        }
+    }
+    let suppress_minimum=input.frames_since_teleport<20;
+    if suppress_minimum {retained.flags &= !8;}
+    let mut controls=input.controls;
+    controls.frame_forward_112=[state.frame_80[2][0],state.frame_80[2][1],state.frame_80[2][2]];
+    let control=ground_input::calculate(&controls,movement_curve,turn_curve);
+    let adjustment=ground_query::consume_geometry(ground_query::ConsumeInput {
+        frame_80:frame(state.frame_80),contact_position_192:xyz(retained.position),
+        contact_flags_368:retained.flags,reach_364:retained.distance_172,
+        previous_input_up_416:xyz(retained.normal),
+    },geometry);
+    if !both_feet && !adjustment.state_753 && retained.flags & 8 == 0 {state.distance_164=0.;}
+    let target=adjustment.frame_768;
+    let job=GroundJob {
+        contact_position:retained.position,
+        contact_normal:[adjustment.input_up_416.x,adjustment.input_up_416.y,adjustment.input_up_416.z,retained.normal[3]],
+        support_frame:retained.support_frame,target_position:retained.target_position,
+        target_normal:retained.target_normal,edge_position:retained.edge_position,edge_normal:retained.edge_normal,
+        flags:retained.flags,support_id:retained.support_id,
+        collision_displacements:input.skeleton_displacements_16288_16304,
+        animation_motion:input.animation_motion_2864,animation_velocity:input.skeleton_motion_16320,
+        desired_direction:control.state_656,
+        animation_position:std::array::from_fn(|i|input.velocity_608[i].mul_add(timestep,input.position_592[i])),
+        requested_duration:input.duration_2896,mirrored:input.flags_2476 & 4 != 0,
+        requested_phase:input.phase_2900,override_duration:input.override_duration_2904,
+        animation_directed:input.flags_2488 & 0x0800_0000 != 0,
+        movement:control.state_708,steering:control.state_712,sprint_pressed:input.flags_2484 & 0x20000 != 0,
+        suppress_lean:input.flags_2480 & 128 != 0,suppress_minimum,
+        target_frame_present:adjustment.state_752,edge_active:adjustment.state_753,
+        target_frame:[vector(target.right),vector(target.up),vector(target.forward),vector(target.position)],
+        ignore_obstacle:input.controls.processed_flags_2472 & 0x1000_0000 != 0,
+    };
+    Prepared {job,geometry_flags_752_to_754:[adjustment.state_752,adjustment.state_753,adjustment.state_754]}
+}
+
+#[cfg(test)]
+mod tests {
+    use super::*;
+    fn input() -> Input {
+        Input {previous_state:500,frames_since_teleport:20,fallback_line_hit:None,
+            controls:GroundInput {processed_flags_2472:0x1000_0000,processed_direct_2684:0.75,
+                processed_direct_2680:-0.5,processed_stick_2692:0.,processed_stick_2688:0.,
+                processed_scale_2912:1.,processed_scale_2908:1.,frame_forward_112:[0.,0.,1.]},
+            flags_2476:4,flags_2480:128,flags_2484:0x20000,flags_2488:0x0800_0000,
+            animation_motion_2864:[1.,2.,3.,4.],duration_2896:0.25,phase_2900:0.75,override_duration_2904:1.5,
+            position_592:[10.,20.,30.,0.],velocity_608:[60.,120.,180.,0.],skeleton_motion_16320:[4.,3.,2.,1.],
+            skeleton_displacements_16288_16304:[[1.;4],[2.;4]]}
+    }
+    fn curves() -> PointGraph<8> {PointGraph {x:std::array::from_fn(|i|i as f32),y:[1.;8]}}
+    #[test]
+    fn delayed_contact_has_priority_and_suppression_expires_at_twenty() {
+        let curves=curves();
+        for age in [19,20] {
+            let mut state=State::default();
+            let mut retained=Packet::default();
+            let mut input=input(); input.frames_since_teleport=age;input.fallback_line_hit=Some([99.;4]);
+            let mut completed=Packet::default();completed.flags=57;completed.position=[0.,-1.,0.,0.];
+            let prepared=prepare(&mut state,&mut retained,Some(completed),None,&input,&curves,&curves);
+            let job=prepared.job;
+            assert_eq!(job.contact_position,completed.position);
+            assert_eq!(job.suppress_minimum,age==19);
+            assert_eq!(job.flags,if age==19 {49} else {57});
+            assert_eq!(job.animation_position,[11.,22.,33.,0.]);
+            assert_eq!(job.collision_displacements,[[1.;4],[2.;4]]);
+            assert_eq!(job.movement,0.75); assert_eq!(job.steering,-0.5);
+            assert!(job.mirrored && job.sprint_pressed && job.suppress_lean && job.animation_directed && job.ignore_obstacle);
+        }
+    }
+    #[test]
+    fn fallback_is_only_used_when_no_completed_contact_and_previous_state_is_not_biped_air() {
+        let curves=curves();
+        for previous in [500,501] {
+            let mut state=State::default();
+            let mut retained=Packet::default();
+            let mut input=input();input.previous_state=previous;input.fallback_line_hit=Some([0.,-1.,0.,0.]);
+            let result=prepare(&mut state,&mut retained,None,None,&input,&curves,&curves);
+            assert_eq!(result.job.flags,if previous==501 {0} else {1});
+            assert_eq!(retained.position,if previous==501 {[0.;4]} else {[0.,-1.,0.,0.]});
+        }
+    }
+}

crates/skate-core/src/player/offboard/mod.rs

@@ -22,3 +22,6 @@ pub mod reset_animation;
 pub mod contact_queries;
 pub mod contact_records;
 pub mod contact_packet;
+pub mod contact_segments;
+pub mod contact_promotion;
+pub mod ground_job;