Fixed corrected position finder and remade find_low_inside code

kite_test.py

@@ -19,9 +19,9 @@ def read_maze(file_name: str) -> List[List[int]]:
 
 maze = read_maze("tests/empty10x10.txt")
 pf = sc2pathlibp.PathFinder(maze)
-pf.normalize_influence(1)
+pf.normalize_influence(10)
 enemy_pos = (4, 0)
-start_pos = (5, 5)
+start_pos = (5, 2)
 pf.add_influence_walk([enemy_pos], 100, 7)
 end_result = pf.find_low_inside_walk(start_pos, enemy_pos, 5)
 print(end_result)

src/helpers/mod.rs

@@ -3,3 +3,9 @@ pub fn round_point2(point: (f32, f32)) -> (usize, usize) {
     let y = point.1.round() as usize;
     return (x, y);
 }
+
+pub fn point2_f32(point: (usize, usize)) -> (f32, f32) {
+    let x = point.0 as f32;
+    let y = point.1 as f32;
+    return (x, y);
+}

src/path_find/mod.rs

@@ -1,6 +1,10 @@
+use std::f32::MAX;
+
 use pathfinding::prelude::{absdiff, astar, dijkstra_all, dijkstra_partial};
 use pyo3::prelude::*;
 
+use crate::helpers::point2_f32;
+
 mod angles;
 pub mod pos;
 mod pos_large;
@@ -235,7 +239,7 @@ impl PathFind {
     }
 
     /// Adds influence based on euclidean distance
-    pub fn add_influence_flat(&mut self, positions: Vec<(usize, usize)>, max: f32, distance: f32) -> PyResult<()> {
+    pub fn add_influence_flat(&mut self, positions: Vec<(usize, usize)>, max: f32, distance: f32) {
         let value = max as usize;
         let mult_distance = distance * pos::MULTF32;
 
@@ -253,12 +257,10 @@ impl PathFind {
                 }
             }
         }
-
-        Ok(())
     }
 
     /// Adds influence based on walk distance
-    pub fn add_walk_influence(&mut self, positions: Vec<(usize, usize)>, max: f32, distance: f32) -> PyResult<()> {
+    pub fn add_walk_influence(&mut self, positions: Vec<(usize, usize)>, max: f32, distance: f32) {
         let mult = 1.0 / distance;
         let max_int = max as usize;
 
@@ -280,8 +282,6 @@ impl PathFind {
                 }
             }
         }
-
-        Ok(())
     }
 
     /// Adds influence based on walk distance
@@ -869,7 +869,7 @@ impl PathFind {
 
     #[inline]
     fn get_closest_pathable(&self, start: (usize, usize)) -> (usize, usize) {
-        if self.auto_correct || self.map[start.0][start.1] > 0 {
+        if !self.auto_correct || self.map[start.0][start.1] > 0 {
             start
         } else {
             self.free_finder.find_free(start, &self.map, self.width, self.height)
@@ -885,65 +885,49 @@ impl PathFind {
 
         let corrected_start = self.get_closest_pathable(start_int);
         let corrected_target = self.get_closest_pathable(target_int);
-        let angle = angles::angle_between_f32(start, target);
-        let u_distance = distance as usize;
-        let rect = rectangle::Rectangle::init_from_center2(corrected_target,
-                                                           (u_distance, u_distance),
-                                                           self.width,
-                                                           self.height);
-
-        let mut destinations = Vec::<((usize, usize), usize)>::new();
-
-        for x in rect.x..rect.x_end {
-            for y in rect.y..rect.y_end {
-                let new_val = self.map[x][y];
-                if new_val > 0 {
-                    destinations.push(((x, y), new_val));
-                }
-            }
+        // let angle = angles::angle_between_f32(start, target);
+        // let u_distance = distance as usize;
+
+        if current_distance > distance + 2.0 {
+            // Just do normal influence pathfinding to near the target.
+            let path =
+                self.find_path_influence_inline_closer_than(corrected_start, corrected_target, Some(1u8), distance);
+            if path.1 >= 0f32 {
+                // dbg!(path);
+                // dbg!(path.0);
+                // dbg!(path.1);
+                return (point2_f32(*path.0.last().unwrap()), path.1);
+            }
+            return ((0.0, 0.0), -1.0); // Failed
         }
 
-        if destinations.is_empty() {
-            // Cannot find path to target
-            return ((0.0, 0.0), -1.0);
-        } else {
-            let mut best_target: ((f32, f32), f32) = ((0.0, 0.0), -1.0);
+        let destinations = self.find_destinations_in_inline(corrected_target, distance);
 
-            // Get a backup position that's closest to start up position
-            for destination in destinations {
-                let point = destination.0;
-                let distance_from_start = octile_distance_f32(start_int, point);
+        let mut best_target: ((f32, f32), f32) = (point2_f32(corrected_start), 0.0);
+        let mut best_influence = f32::MAX;
+        if current_distance < distance {
+            best_influence = self.map[corrected_start.0][corrected_start.1] as f32;
+        }
 
-                if distance_from_start < best_target.1 || best_target.1 < 0.0 {
-                    let point_f32 = (point.0 as f32 + 0.5, point.1 as f32 + 0.5);
-                    best_target = (point_f32, distance_from_start);
-                }
+        for destination in destinations {
+            let distance_from_target = octile_distance_f32(corrected_target, destination.0);
+            if distance_from_target > distance {
+                continue;
             }
+
+            let distance_from_start= octile_distance_f32(corrected_start, destination.0);
+            // Use magic distance constant here to not move without reason.
+            // Let's take the distance into account so that same influence value is better when it's closer.
+            let distance_value = distance_from_start * self.normal_influence as f32 * 0.25;
+            let influence = self.map[(destination.0).0][(destination.0).1] as f32 + distance_value;
 
-            if current_distance < distance + 4.0 {
-                let best_influence = self.map[(best_target.0).0 as usize][(best_target.0).1 as usize];
-                //let mut best_distance_from_target = octile_distance_f64(best_target.0, target_int);
-                let destinations_from_start = self.find_destinations_in_inline(corrected_start, 5.0);
-                let mut angle_distance =
-                    angles::angle_distance(angle, angles::angle_between_f32(best_target.0, target));
-                let mut best_score = best_influence as f32 * (1.0 + angle_distance * 0.25);
-
-                for destination in destinations_from_start {
-                    let point = destination.0;
-                    let point_f32 = (point.0 as f32 + 0.5, point.1 as f32 + 0.5);
-                    let influence = self.map[point.0][point.1];
-                    angle_distance = angles::angle_distance(angle, angles::angle_between_f32(point_f32, target));
-                    let score = influence as f32 * (1.0 + angle_distance * 0.25);
-
-                    if score < best_score {
-                        best_score = score;
-                        best_target = (point_f32, destination.1);
-                    }
-                }
+            if influence < best_influence {
+                best_target = (point2_f32(destination.0), distance);
+                best_influence = influence;
             }
-
-            return best_target;
         }
+
+        return best_target;
     }
 
     pub fn invert_djiktra(&self, start: (f32, f32), distance: f32) -> Vec<((usize, usize), f32)> {

tests/test_pathfinding.rs

@@ -1,4 +1,8 @@
 use common::get_pathfind;
+use sc2pathlib::helpers::point2_f32;
+use sc2pathlib::helpers::round_point2;
+use sc2pathlib::path_find::octile_distance;
+use sc2pathlib::path_find::octile_distance_f32;
 
 mod common;
 
@@ -25,3 +29,100 @@ fn test_find_path_10x10() {
     let (_, distance) = r;
     assert_eq!(distance, 12.3136);
 }
+
+#[test]
+fn test_find_low_inside1() {
+    // Assign
+    let mut path_find = get_pathfind("tests/empty10x10.txt");
+    path_find.normalize_influence(1);
+    let enemy_pos = (4usize, 0usize);
+    let start_pos = (5f32, 0f32);
+    let all_pos: Vec<(usize, usize)> = vec![enemy_pos];
+    path_find.add_walk_influence(all_pos, 100f32, 7f32);
+    // Act
+    let r = path_find.find_low_inside_walk(start_pos, point2_f32(enemy_pos), 8f32);
+    // Assert
+    let pos = round_point2(r.0);
+    let influence = path_find.map[pos.0][pos.1];
+    let distance = octile_distance_f32(enemy_pos, pos);
+
+    assert!(distance <= 8f32);
+    assert_eq!(influence, 1);
+}
+
+#[test]
+fn test_find_low_inside2() {
+    // Assign
+    let mut path_find = get_pathfind("tests/empty10x10.txt");
+    path_find.normalize_influence(1);
+    let enemy_pos = (4usize, 0usize);
+    let start_pos = (5f32, 0f32);
+    let all_pos: Vec<(usize, usize)> = vec![enemy_pos];
+    path_find.add_walk_influence(all_pos, 100f32, 7f32);
+    // Act
+    let r = path_find.find_low_inside_walk(start_pos, point2_f32(enemy_pos), 6f32);
+    // Assert
+    let pos = round_point2(r.0);
+    let influence = path_find.map[pos.0][pos.1];
+    let distance = octile_distance_f32(enemy_pos, pos);
+    assert!(distance <= 6f32);
+    assert_eq!(influence, 15);
+}
+
+#[test]
+fn test_find_low_inside3() {
+    // Assign
+    let mut path_find = get_pathfind("tests/empty10x10.txt");
+    path_find.normalize_influence(1);
+    let enemy_pos = (4usize, 0usize);
+    let start_pos = (8f32, 4f32);
+    let all_pos: Vec<(usize, usize)> = vec![enemy_pos];
+    path_find.add_walk_influence(all_pos, 100f32, 7f32);
+    // Act
+    let r = path_find.find_low_inside_walk(start_pos, point2_f32(enemy_pos), 8f32);
+    // Assert
+    let pos = round_point2(r.0);
+    let influence = path_find.map[pos.0][pos.1];
+    let distance = octile_distance_f32(enemy_pos, pos);
+
+    assert!(distance <= 8f32);
+    assert_eq!(influence, 1);
+}
+
+#[test]
+fn test_find_low_inside_far1() {
+    // Assign
+    let mut path_find = get_pathfind("tests/empty10x10.txt");
+    path_find.normalize_influence(1);
+    let enemy_pos = (4usize, 0usize);
+    let start_pos = (9f32, 9f32);
+    let all_pos: Vec<(usize, usize)> = vec![enemy_pos];
+    path_find.add_walk_influence(all_pos, 100f32, 7f32);
+    // Act
+    let r = path_find.find_low_inside_walk(start_pos, point2_f32(enemy_pos), 6f32);
+    // Assert
+    let pos = round_point2(r.0);
+    let influence = path_find.map[pos.0][pos.1];
+    let distance = octile_distance_f32(enemy_pos, pos);
+    assert!(distance <= 6f32);
+    assert_eq!(influence, 17);
+}
+
+#[test]
+fn test_find_low_inside_far2() {
+    // Assign
+    let mut path_find = get_pathfind("tests/empty10x10.txt");
+    path_find.normalize_influence(1);
+    let enemy_pos = (4usize, 0usize);
+    let start_pos = (9f32, 9f32);
+    let all_pos: Vec<(usize, usize)> = vec![enemy_pos];
+    path_find.add_walk_influence(all_pos, 100f32, 7f32);
+    // Act
+    let r = path_find.find_low_inside_walk(start_pos, point2_f32(enemy_pos), 8f32);
+    // Assert
+    let pos = round_point2(r.0);
+    let influence = path_find.map[pos.0][pos.1];
+    let distance = octile_distance_f32(enemy_pos, pos);
+    assert!(distance <= 8f32);
+    assert_eq!(influence, 1);
+}