Improve ECB behaviour, add ECB interpolation, wall/ceiling support etc (#94)

Reviewed-on: #94
Co-authored-by: Álex Román Núñez <eirexe@eirteam.moe>

src/fighter/collision.rs

@@ -1,11 +1,17 @@
 //! Resolving an integrated fighter position against the stage.
 
-use crate::fighter::ecb::FighterPreviousECB;
-use crate::fighter::{FighterECB, FighterPreviousTranslation, FighterTranslation};
+use crate::fighter::ecb::{self as ecb_module, FighterDesiredECB};
+use crate::fighter::{FighterPreviousTranslation, FighterTranslation};
+use crate::fighter::FighterVelocity;
+#[cfg(test)]
+use crate::fighter::{FighterECB, ecb::FighterPreviousECB};
+use crate::math::int::FGi32;
 use crate::math::rect::FGRect;
 use crate::math::segment::FGSegment2d;
 use crate::math::vec::FGVec2;
-use crate::stage::line::{StageCollision, StageLineID, StagePolyType, StageVertexID};
+use crate::stage::line::{
+    StageCollision, StageLineID, StagePolyLineSegmentType, StagePolyType, StageVertexID,
+};
 /*
 pub fn collide_fighter_with_scene(
     fighters: Query<(&FighterECB, &mut FighterVelocity, &mut FighterTranslation)>,
@@ -36,10 +42,14 @@ pub fn collide_fighter_with_scene(
 pub struct AirCollisionWithStageResult {
     pub line_id: StageLineID,
     pub hit_position: FGVec2,
+    /// Fighter origin at the collision substep. The ECB remains local to this
+    /// origin; callers must not rebase the ECB by the frame-sized movement.
+    pub resolved_translation: FGVec2,
     pub hit_normal: FGVec2,
 }
 
-pub fn air_collide_with_stage(
+#[cfg(test)]
+fn air_collide_with_stage(
     prev_translation: &FighterPreviousTranslation,
     translation: &FighterTranslation,
     prev_ecb: &FighterPreviousECB,
@@ -48,42 +58,298 @@ pub fn air_collide_with_stage(
     ignored_polys: &[usize],
     can_ignore_soft_platforms: bool,
 ) -> Option<AirCollisionWithStageResult> {
-    let ray_segment = FGSegment2d::new(
-        prev_translation.0 + prev_ecb.get_bottom_point(),
-        translation.0 + ecb.get_bottom_point(),
-    );
+    let mut translation = *translation;
+    let mut velocity = FighterVelocity::default();
+    air_collide_with_stage_substepped(
+        prev_translation,
+        &mut translation,
+        &mut velocity,
+        &mut ecb_module::FighterECB {
+            top: prev_ecb.top,
+            bottom: prev_ecb.bottom,
+            left: prev_ecb.left,
+            right: prev_ecb.right,
+        },
+        &FighterDesiredECB::from(*ecb),
+        stage_collision,
+        ignored_polys,
+        can_ignore_soft_platforms,
+    )
+}
 
+/// Sweeps movement and ECB shape changes together.
+pub(crate) fn collision_step_count(max_change: FGi32) -> u32 {
+    let six = FGi32::lit("6.0");
+    if max_change <= six {
+        return 1;
+    }
+
+    let mut quotient = 0;
+    let mut remainder = max_change;
+    while remainder >= six {
+        quotient += 1;
+        remainder -= six;
+    }
+    quotient + 1
+}
+
+pub fn air_collide_with_stage_substepped(
+    prev_translation: &FighterPreviousTranslation,
+    translation: &mut FighterTranslation,
+    velocity: &mut FighterVelocity,
+    ecb: &mut crate::fighter::FighterECB,
+    desired: &FighterDesiredECB,
+    stage_collision: &StageCollision,
+    ignored_polys: &[usize],
+    can_ignore_soft_platforms: bool,
+) -> Option<AirCollisionWithStageResult> {
+    let requested_translation = translation.0;
+    let movement = requested_translation - prev_translation.0;
+    let mut largest_change = movement.x.abs().max(movement.y.abs());
+    largest_change = largest_change.max(ecb_module::ecb_max_delta(ecb, desired));
+    let steps = collision_step_count(largest_change);
+
+    let step_count = FGi32::from_num(steps);
+    let movement_per_step = movement / step_count;
+    let mut current_position = prev_translation.0;
+    let mut current_ecb = *ecb;
+    let mut collision_offset = FGVec2::ZERO;
+
+    for step in 0..steps {
+        let previous_position = current_position;
+        let previous_ecb = current_ecb;
+        let remaining_steps = FGi32::from_num(steps - step);
+        ecb_module::interpolate_ecb(&mut current_ecb, *desired, FGi32::ONE / remaining_steps);
+        current_position = if step + 1 == steps {
+            // Fixed-point division may truncate. Always make the final
+            // substep land exactly on the requested frame endpoint.
+            requested_translation + collision_offset
+        } else {
+            prev_translation.0
+                + movement_per_step * FGi32::from_num(step + 1)
+                + collision_offset
+        };
+
+        let lower_edges = [
+            (
+                previous_ecb.bottom,
+                previous_ecb.left,
+                current_ecb.bottom,
+                current_ecb.left,
+            ),
+            (
+                previous_ecb.bottom,
+                previous_ecb.right,
+                current_ecb.bottom,
+                current_ecb.right,
+            ),
+        ];
+        if let Some(result) = sweep_edges(
+            stage_collision,
+            previous_position,
+            current_position,
+            &lower_edges,
+            StagePolyLineSegmentType::Floor,
+            ignored_polys,
+            can_ignore_soft_platforms,
+        ) {
+            *ecb = current_ecb;
+            translation.0 = current_position + result.correction;
+            velocity.0 -= velocity.0.project_onto_normalized(result.hit_normal);
+            let bottom_contact = translation.0 + current_ecb.bottom;
+            return Some(AirCollisionWithStageResult {
+                line_id: result.line_id,
+                hit_position: bottom_contact,
+                resolved_translation: translation.0,
+                hit_normal: result.hit_normal,
+            });
+        }
+
+        let upper_edges = [
+            (
+                previous_ecb.top,
+                previous_ecb.left,
+                current_ecb.top,
+                current_ecb.left,
+            ),
+            (
+                previous_ecb.top,
+                previous_ecb.right,
+                current_ecb.top,
+                current_ecb.right,
+            ),
+        ];
+        if let Some(contact) = sweep_edges(
+            stage_collision,
+            previous_position,
+            current_position,
+            &upper_edges,
+            StagePolyLineSegmentType::Ceiling,
+            ignored_polys,
+            true,
+        ) {
+            current_position += contact.correction;
+            collision_offset += contact.correction;
+            velocity.0 -= velocity.0.project_onto_normalized(contact.hit_normal);
+        }
+
+        let side_edges = [
+            (
+                previous_ecb.bottom,
+                previous_ecb.right,
+                current_ecb.bottom,
+                current_ecb.right,
+            ),
+            (
+                previous_ecb.right,
+                previous_ecb.top,
+                current_ecb.right,
+                current_ecb.top,
+            ),
+            (
+                previous_ecb.top,
+                previous_ecb.left,
+                current_ecb.top,
+                current_ecb.left,
+            ),
+            (
+                previous_ecb.left,
+                previous_ecb.bottom,
+                current_ecb.left,
+                current_ecb.bottom,
+            ),
+        ];
+        if let Some(contact) = sweep_edges(
+            stage_collision,
+            previous_position,
+            current_position,
+            &side_edges,
+            StagePolyLineSegmentType::Wall,
+            ignored_polys,
+            true,
+        ) {
+            current_position += contact.correction;
+            collision_offset += contact.correction;
+            velocity.0 -= velocity.0.project_onto_normalized(contact.hit_normal);
+        }
+    }
+
+    *ecb = current_ecb;
+    translation.0 = current_position;
+    None
+}
+
+struct PointSweepResult {
+    line_id: StageLineID,
+    hit_normal: FGVec2,
+    correction: FGVec2,
+}
+
+fn sweep_edges(
+    stage_collision: &StageCollision,
+    previous_position: FGVec2,
+    current_position: FGVec2,
+    edges: &[(FGVec2, FGVec2, FGVec2, FGVec2)],
+    segment_type: StagePolyLineSegmentType,
+    ignored_polys: &[usize],
+    ignore_platforms: bool,
+) -> Option<PointSweepResult> {
     stage_collision
         .stage_polys
         .iter()
         .enumerate()
         .filter(|(poly_idx, poly)| {
             !ignored_polys.contains(poly_idx)
-                && !(can_ignore_soft_platforms && matches!(poly.poly_type, StagePolyType::Platform))
+                && !(ignore_platforms && matches!(poly.poly_type, StagePolyType::Platform))
         })
-        .filter_map(|(poly_idx, poly)| {
-            poly.intersect_ray(ray_segment).map(|intersect_result| {
-                (
-                    AirCollisionWithStageResult {
-                        line_id: {
-                            StageLineID {
-                                polygon: poly_idx,
-                                segment: intersect_result.segment_idx,
+        .flat_map(|(poly_idx, poly)| {
+            poly.segments
+                .iter()
+                .enumerate()
+                .filter(move |(_, segment)| segment.segment_type == segment_type)
+                .filter_map(move |(segment_idx, segment)| {
+                    edges
+                        .iter()
+                        .filter_map(|(previous_a, previous_b, current_a, current_b)| {
+                            let previous_a = previous_position + *previous_a;
+                            let previous_b = previous_position + *previous_b;
+                            let current_a = current_position + *current_a;
+                            let current_b = current_position + *current_b;
+                            let previous_min = segment
+                                .normal
+                                .dot(previous_a - segment.segment.point1())
+                                .min(segment.normal.dot(previous_b - segment.segment.point1()));
+                            let current_a_distance =
+                                segment.normal.dot(current_a - segment.segment.point1());
+                            let current_b_distance =
+                                segment.normal.dot(current_b - segment.segment.point1());
+                            let current_min = current_a_distance.min(current_b_distance);
+                            if previous_min < crate::math::int::FGWide::ZERO
+                                || current_min > crate::math::int::FGWide::ZERO
+                            {
+                                return None;
                             }
-                        },
-                        hit_position: intersect_result.position,
-                        hit_normal: intersect_result.normal,
-                    },
-                    intersect_result
-                        .position
-                        .distance_squared(ray_segment.point2()),
-                )
-            })
+
+                            let boundary = [
+                                FGSegment2d::new(previous_a, previous_b),
+                                FGSegment2d::new(previous_a, current_a),
+                                FGSegment2d::new(previous_b, current_b),
+                                FGSegment2d::new(current_a, current_b),
+                            ];
+                            let quad = [previous_a, previous_b, current_b, current_a];
+                            let position = boundary
+                                .iter()
+                                .filter_map(|edge| segment.segment.segment_intersection(edge))
+                                .max_by_key(|position| position.distance_squared(current_a))
+                                .or_else(|| {
+                                    [segment.segment.point1(), segment.segment.point2()]
+                                        .into_iter()
+                                        .find(|point| point_in_convex_quad(*point, quad))
+                                })?;
+                            let penetration = FGi32::from_num(-current_min);
+                            Some((position, segment.normal * penetration))
+                        })
+                        .max_by_key(|(position, _)| {
+                            position.distance_squared(segment.segment.point1())
+                        })
+                        .map(|(position, correction)| {
+                            (
+                                PointSweepResult {
+                                    line_id: StageLineID {
+                                        polygon: poly_idx,
+                                        segment: segment_idx,
+                                    },
+                                    hit_normal: segment.normal,
+                                    correction,
+                                },
+                                position.distance_squared(segment.segment.point2()),
+                            )
+                        })
+                })
         })
         .max_by(|(_, a), (_, b)| a.cmp(b))
         .map(|(result, _)| result)
 }
 
+fn point_in_convex_quad(point: FGVec2, quad: [FGVec2; 4]) -> bool {
+    let mut has_positive = false;
+    let mut has_negative = false;
+    for (a, b) in quad.into_iter().zip(quad.into_iter().cycle().skip(1)).take(4) {
+        let edge = b - a;
+        let offset = point - a;
+        let cross = crate::math::int::FGWide::from_num(edge.x)
+            * crate::math::int::FGWide::from_num(offset.y)
+            - crate::math::int::FGWide::from_num(edge.y)
+                * crate::math::int::FGWide::from_num(offset.x);
+        has_positive |= cross > crate::math::int::FGWide::ZERO;
+        has_negative |= cross < crate::math::int::FGWide::ZERO;
+        if has_positive && has_negative {
+            return false;
+        }
+    }
+    true
+}
+
 #[derive(Clone)]
 pub struct AirCollisionWithLedgeResult {
     pub ledge: StageVertexID,
@@ -93,9 +359,11 @@ pub fn air_collide_with_ledges(
     ledge_box: &FGRect,
     stage_collision: &StageCollision,
 ) -> Option<AirCollisionWithLedgeResult> {
-    stage_collision.intersect_ledges(ledge_box).map(|result| {
-        AirCollisionWithLedgeResult { ledge: result.vertex_id }
-    })
+    stage_collision
+        .intersect_ledges(ledge_box)
+        .map(|result| AirCollisionWithLedgeResult {
+            ledge: result.vertex_id,
+        })
 }
 
 #[cfg(test)]
@@ -126,6 +394,173 @@ mod tests {
         )
     }
 
+    fn single_segment(
+        start: FGVec2,
+        end: FGVec2,
+        segment_type: StagePolyLineSegmentType,
+    ) -> StagePoly {
+        let segment = FGSegment2d::new(start, end);
+        StagePoly {
+            poly_type: StagePolyType::Closed,
+            segments: vec![crate::stage::line::StagePolyLineSegment {
+                segment,
+                normal: segment.left_normal(),
+                segment_type,
+            }],
+        }
+    }
+
+    #[test]
+    fn melee_step_count_adds_one_step_after_each_six_unit_block() {
+        assert_eq!(collision_step_count(FGi32::lit("6")), 1);
+        assert_eq!(collision_step_count(FGi32::lit("6.01")), 2);
+        assert_eq!(collision_step_count(FGi32::lit("12")), 3);
+        assert_eq!(collision_step_count(FGi32::lit("18")), 4);
+    }
+
+    #[test]
+    fn collision_result_keeps_ecb_local_to_resolved_origin() {
+        let stage_collision = StageCollision {
+            stage_polys: vec![horizontal_platform("0")],
+            ..StageCollision::default()
+        };
+        let previous_translation = FighterPreviousTranslation(FGVec2::lit("0", "2"));
+        let mut translation = FighterTranslation(FGVec2::lit("0", "-1"));
+        let mut velocity = FighterVelocity(FGVec2::lit("0", "-3"));
+        let mut ecb = FighterECB {
+            top: FGVec2::lit("0", "2"),
+            bottom: FGVec2::lit("0", "-1"),
+            left: FGVec2::lit("-1", "0.5"),
+            right: FGVec2::lit("1", "0.5"),
+        };
+        let desired = FighterDesiredECB::from(ecb);
+        let collision = air_collide_with_stage_substepped(
+            &previous_translation,
+            &mut translation,
+            &mut velocity,
+            &mut ecb,
+            &desired,
+            &stage_collision,
+            &[],
+            false,
+        )
+        .expect("the floor should be hit");
+
+        assert_eq!(
+            collision.resolved_translation + ecb.bottom,
+            collision.hit_position
+        );
+        assert_eq!(velocity.0.y, FGi32::ZERO);
+    }
+
+    #[test]
+    fn airborne_side_point_resolves_wall_and_projects_velocity() {
+        let stage_collision = StageCollision {
+            stage_polys: vec![single_segment(
+                FGVec2::lit("2", "2"),
+                FGVec2::lit("2", "-2"),
+                StagePolyLineSegmentType::Wall,
+            )],
+            ..StageCollision::default()
+        };
+        let previous = FighterPreviousTranslation(FGVec2::lit("4", "0"));
+        let mut translation = FighterTranslation(FGVec2::lit("0", "0"));
+        let mut velocity = FighterVelocity(FGVec2::lit("-4", "0"));
+        let mut ecb = FighterECB {
+            top: FGVec2::lit("0", "1"),
+            bottom: FGVec2::lit("0", "-1"),
+            left: FGVec2::lit("-1", "0"),
+            right: FGVec2::lit("1", "0"),
+        };
+        let desired = FighterDesiredECB::from(ecb);
+
+        assert!(air_collide_with_stage_substepped(
+            &previous,
+            &mut translation,
+            &mut velocity,
+            &mut ecb,
+            &desired,
+            &stage_collision,
+            &[],
+            false,
+        )
+        .is_none());
+        assert_eq!(translation.0, FGVec2::lit("3", "0"));
+        assert_eq!(velocity.0.x, FGi32::ZERO);
+    }
+
+    #[test]
+    fn stage_corner_cannot_pass_through_middle_of_diamond_edge() {
+        let stage_collision = StageCollision {
+            stage_polys: vec![single_segment(
+                FGVec2::lit("2", "0.6"),
+                FGVec2::lit("2", "0.4"),
+                StagePolyLineSegmentType::Wall,
+            )],
+            ..StageCollision::default()
+        };
+        let previous = FighterPreviousTranslation(FGVec2::lit("4", "0"));
+        let mut translation = FighterTranslation(FGVec2::ZERO);
+        let mut velocity = FighterVelocity(FGVec2::lit("-4", "0"));
+        let mut ecb = FighterECB {
+            top: FGVec2::lit("0", "1"),
+            bottom: FGVec2::lit("0", "-1"),
+            left: FGVec2::lit("-1", "0"),
+            right: FGVec2::lit("1", "0"),
+        };
+        let desired = FighterDesiredECB::from(ecb);
+
+        assert!(air_collide_with_stage_substepped(
+            &previous,
+            &mut translation,
+            &mut velocity,
+            &mut ecb,
+            &desired,
+            &stage_collision,
+            &[],
+            false,
+        )
+        .is_none());
+        assert_eq!(translation.0.x, FGi32::lit("3"));
+        assert_eq!(velocity.0.x, FGi32::ZERO);
+    }
+
+    #[test]
+    fn airborne_top_point_resolves_ceiling_and_projects_velocity() {
+        let stage_collision = StageCollision {
+            stage_polys: vec![single_segment(
+                FGVec2::lit("2", "2"),
+                FGVec2::lit("-2", "2"),
+                StagePolyLineSegmentType::Ceiling,
+            )],
+            ..StageCollision::default()
+        };
+        let previous = FighterPreviousTranslation(FGVec2::ZERO);
+        let mut translation = FighterTranslation(FGVec2::lit("0", "4"));
+        let mut velocity = FighterVelocity(FGVec2::lit("0", "4"));
+        let mut ecb = FighterECB {
+            top: FGVec2::lit("0", "1"),
+            bottom: FGVec2::lit("0", "-1"),
+            left: FGVec2::lit("-1", "0"),
+            right: FGVec2::lit("1", "0"),
+        };
+        let desired = FighterDesiredECB::from(ecb);
+
+        assert!(air_collide_with_stage_substepped(
+            &previous,
+            &mut translation,
+            &mut velocity,
+            &mut ecb,
+            &desired,
+            &stage_collision,
+            &[],
+            false,
+        )
+        .is_none());
+        assert_eq!(translation.0, FGVec2::lit("0", "1"));
+        assert_eq!(velocity.0.y, FGi32::ZERO);
+    }
+
     #[test]
     fn airborne_collision_skips_ignored_polygons() {
         let stage_collision = StageCollision {
@@ -183,16 +618,18 @@ mod tests {
             right: FGVec2::ZERO,
         };
 
-        assert!(air_collide_with_stage(
-            &previous_translation,
-            &translation,
-            &previous_ecb,
-            &ecb,
-            &stage_collision,
-            &[],
-            true,
-        )
-        .is_none());
+        assert!(
+            air_collide_with_stage(
+                &previous_translation,
+                &translation,
+                &previous_ecb,
+                &ecb,
+                &stage_collision,
+                &[],
+                true,
+            )
+            .is_none()
+        );
 
         assert_eq!(
             air_collide_with_stage(