From 745ce096a016aa8a5621b7c8fe67368409ae27eb Mon Sep 17 00:00:00 2001
From: Martin Indra <martin.indra@mgn.cz>
Date: Mon, 9 Oct 2023 20:34:01 +0200
Subject: [PATCH] Possible fix

---
 crates/pathing/src/interval.rs | 45 ++++++++++++++++++++++------------
 1 file changed, 29 insertions(+), 16 deletions(-)

diff --git a/crates/pathing/src/interval.rs b/crates/pathing/src/interval.rs
index bc1fe46bf..50cf4fba6 100644
--- a/crates/pathing/src/interval.rs
+++ b/crates/pathing/src/interval.rs
@@ -140,8 +140,10 @@ impl SegmentInterval {
         let b = RayProjection::calculate(ray_b, target);
 
         let side = which_side(ray_a.origin, ray_a.point_at(1.), ray_b.point_at(1.));
-        debug_assert!(side != Side::Straight || ray_a.dir.dot(&ray_b.dir) < 0.);
-        SegmentProjection::new(a, b, side)
+        // TODO: for some reason this assert fails due to the ray origin and
+        // self.segment lying on a line.
+        // debug_assert!(side != Side::Straight || ray_a.dir.dot(&ray_b.dir) < 0.);
+        SegmentProjection::new(a, b, side, target.length())
     }
 
     /// Returns a ray with a given origin and pointing towards the endpoint a
@@ -197,11 +199,19 @@ pub(crate) struct SegmentProjection {
     a: RayProjection,
     b: RayProjection,
     ray_b_side: Side,
+    // TODO rename (everywhere to scale?)
+    size: f32,
 }
 
 impl SegmentProjection {
-    fn new(a: RayProjection, b: RayProjection, ray_b_side: Side) -> Self {
-        Self { a, b, ray_b_side }
+    // TODO docs
+    fn new(a: RayProjection, b: RayProjection, ray_b_side: Side, size: f32) -> Self {
+        Self {
+            a,
+            b,
+            ray_b_side,
+            size,
+        }
     }
 
     // TODO document
@@ -217,7 +227,7 @@ impl SegmentProjection {
             0.
         };
 
-        ParamPair::ordered(first, second)
+        ParamPair::ordered(first, second, self.size)
     }
 
     // TODO document
@@ -233,7 +243,7 @@ impl SegmentProjection {
             0.
         };
 
-        ParamPair::ordered(first, second)
+        ParamPair::ordered(first, second, self.size)
     }
 
     // TODO document
@@ -243,7 +253,7 @@ impl SegmentProjection {
         }
 
         match (self.a.parameter(), self.b.parameter()) {
-            (Some(a), Some(b)) => ParamPair::ordered(a, b),
+            (Some(a), Some(b)) => ParamPair::ordered(a, b, self.size),
             (None, None) => {
                 if self.a.endpoint_a_side() == self.b.endpoint_a_side() {
                     None
@@ -257,7 +267,7 @@ impl SegmentProjection {
                 } else {
                     0.
                 };
-                ParamPair::ordered(first, second)
+                ParamPair::ordered(first, second, self.size)
             }
         }
     }
@@ -267,14 +277,17 @@ impl SegmentProjection {
 pub(crate) struct ParamPair(f32, f32);
 
 impl ParamPair {
-    fn round(parameter: f32) -> f32 {
-        // TODO use constants
-
+    // TODO docs
+    fn round(parameter: f32, size: f32) -> f32 {
         // Due to the nature of the algorithm, the ray and the segment
         // frequently intersect near one of the endpoints. To avoid rounding issues,
-        if parameter < 0.0001 {
+
+        let scaled = parameter * size;
+
+        // TODO use constants (negligible distance)
+        if parameter < 0.01 {
             0.
-        } else if parameter > 0.9999 {
+        } else if parameter > 0.99 {
             1.
         } else {
             parameter
@@ -282,9 +295,9 @@ impl ParamPair {
     }
 
     // TODO document
-    fn ordered(a: f32, b: f32) -> Option<Self> {
-        let a = Self::round(a);
-        let b = Self::round(b);
+    fn ordered(a: f32, b: f32, size: f32) -> Option<Self> {
+        let a = Self::round(a, size);
+        let b = Self::round(b, size);
 
         if a < b {
             Some(Self::new(a, b))