137 bad next, 48115 bad distances

Author: Trolldemorted

navpointmatrixcli/src/main.rs

@@ -1,8 +1,11 @@
 use std::fs::{self};
 
+use ordered_f32::OrderedF32;
 use p3_api::data::{navigation_matrix::NavigationMatrix, navigation_vector::NavigationVector, navpoint_matrix::NavpointMatrix};
 use pathfinding::prelude::{build_path, dijkstra_all};
 
+pub mod ordered_f32;
+
 pub struct DirectlyConnectedNodes {
     pub connected_nodes: Vec<(u16, u16)>,
 }
@@ -38,35 +41,38 @@ fn main() {
         for target_index in 0..navigation_vector.points.len() as u16 {
             if source_index != target_index {
                 let path = build_path(&(target_index), &parents);
-                let distance = 0;
-                //println!("{source_index} -> {target_index} {path:?}");
+                let distance = navigation_vector.get_path_length(&path);
                 new_navpoint_matrix.set_next(source_index, target_index, path[1], distance, navigation_vector.length)
             } else {
                 new_navpoint_matrix.set_next(source_index, source_index, source_index, 0, navigation_vector.length)
             }
         }
     }
 
-    {
-        println!(
-            "{} {} {}",
-            navigation_vector.get_distance(0, 14),
-            navigation_vector.get_distance(0, 9),
-            navigation_vector.get_distance(9, 14)
-        );
-    }
-
-    //assert_eq!(original_navpoint_matrix, new_navpoint_matrix)
     assert_eq!(original_navpoint_matrix.matrix.len(), new_navpoint_matrix.matrix.len());
+
     println!("Asserting {} cells", original_navpoint_matrix.matrix.len());
-    //println!("{:?}", &original_navpoint_matrix.matrix[0..10]);
-    //println!("{:?}", &new_navpoint_matrix.matrix[0..10]);
+    let mut bad_next_cells = 0;
     for i in 0..original_navpoint_matrix.matrix.len() {
         let orig_next = original_navpoint_matrix.matrix[i].next;
         let calculated_next = new_navpoint_matrix.matrix[i].next;
-        println!("cell {i}: {orig_next} == {calculated_next}");
-        assert_eq!(orig_next, calculated_next);
+        if orig_next != calculated_next {
+            println!("cell {i}: next {orig_next} != {calculated_next}");
+            bad_next_cells += 1;
+        }
     }
+    println!("{bad_next_cells} bad next entries");
+
+    let mut bad_distance_cells = 0;
+    for i in 0..original_navpoint_matrix.matrix.len() {
+        let orig_distance = original_navpoint_matrix.matrix[i].distance;
+        let calculated_distance = new_navpoint_matrix.matrix[i].distance;
+        if orig_distance != calculated_distance {
+            println!("cell {i}: distance {orig_distance} != {calculated_distance}");
+            bad_distance_cells += 1;
+        }
+    }
+    println!("{bad_distance_cells} bad distances");
 }
 
 fn is_connected(p0: (i16, i16), p1: (i16, i16), navigation_matrix: &NavigationMatrix) -> bool {
@@ -116,11 +122,11 @@ impl DirectlyConnectedNodes {
         buf
     }
 
-    pub fn get_neighbours(&self, node_index: u16, nav_vec: &NavigationVector) -> Vec<(u16, i128)> {
+    pub fn get_neighbours(&self, node_index: u16, nav_vec: &NavigationVector) -> Vec<(u16, OrderedF32)> {
         let mut neighbours = vec![];
         for n in &self.connected_nodes {
             if n.0 == node_index {
-                neighbours.push((n.1, nav_vec.get_distance(n.0 as _, n.1 as _)));
+                neighbours.push((n.1, nav_vec.get_distance(n.0 as _, n.1 as _).into()));
             }
         }
 
@@ -167,3 +173,29 @@ impl DirectlyConnectedNodes {
         DirectlyConnectedNodes { connected_nodes: nodes }
     }
 }
+
+#[test]
+fn test1() {
+    let _navigation_matrix = NavigationMatrix::deserialize(&fs::read(r"C:\Users\Benni\Patrician 3_workbench\navdata\nav_matrix.dat").unwrap());
+    let navigation_vector = NavigationVector::deserialize(&fs::read(r"C:\Users\Benni\Patrician 3_workbench\navdata\nav_vec.dat").unwrap());
+    let original_navpoint_matrix = NavpointMatrix::deserialize(&fs::read(r"C:\Users\Benni\Patrician 3_workbench\navdata\matrix_int.dat").unwrap());
+    let cell4 = &original_navpoint_matrix.matrix[6];
+    let cell4_distance = cell4.distance;
+
+    let (x1, y1) = navigation_vector.points[0];
+    let (x2, y2) = navigation_vector.points[6];
+    let dx = (x2 - x1) as f32;
+    let dy = (y2 - y1) as f32;
+
+    let calculated_distance = ((dx * dx + dy * dy).sqrt() * 65536.0) as i32;
+
+    println!("{cell4_distance} {calculated_distance}");
+    assert_eq!(cell4.distance, calculated_distance)
+}
+
+#[test]
+fn test2() {
+    let navigation_vector = NavigationVector::deserialize(&fs::read(r"C:\Users\Benni\Patrician 3_workbench\navdata\nav_vec.dat").unwrap());
+    let path_0_2 = [0, 9, 20, 34, 33, 25, 24, 29, 31, 59, 65, 64, 44, 2];
+    assert_eq!(19936074, navigation_vector.get_path_length(&path_0_2))
+}

navpointmatrixcli/src/ordered_f32.rs

@@ -0,0 +1,56 @@
+use std::{cmp::Ordering, ops::Add};
+
+#[derive(Copy, Clone)]
+pub struct OrderedF32 {
+    value: f32,
+}
+
+impl From<f32> for OrderedF32 {
+    fn from(value: f32) -> Self {
+        Self { value }
+    }
+}
+
+impl pathfinding::num_traits::Zero for OrderedF32 {
+    fn zero() -> Self {
+        Self { value: 0.0 }
+    }
+
+    fn is_zero(&self) -> bool {
+        self.value == 0.0
+    }
+}
+
+impl Add for OrderedF32 {
+    type Output = OrderedF32;
+
+    fn add(self, rhs: Self) -> Self::Output {
+        Self { value: self.value + rhs.value }
+    }
+}
+
+impl Ord for OrderedF32 {
+    fn cmp(&self, other: &Self) -> std::cmp::Ordering {
+        if self.value < other.value {
+            Ordering::Less
+        } else if self.value > other.value {
+            Ordering::Greater
+        } else {
+            Ordering::Equal
+        }
+    }
+}
+
+impl PartialOrd for OrderedF32 {
+    fn partial_cmp(&self, other: &Self) -> Option<std::cmp::Ordering> {
+        Some(self.cmp(other))
+    }
+}
+
+impl PartialEq for OrderedF32 {
+    fn eq(&self, other: &Self) -> bool {
+        self.value == other.value
+    }
+}
+
+impl Eq for OrderedF32 {}

p3-api/src/data/navigation_vector.rs

@@ -17,10 +17,94 @@ impl NavigationVector {
         NavigationVector { length, points }
     }
 
-    pub fn get_distance(&self, source_index: usize, destination_index: usize) -> i128 {
-        let s = self.points[source_index];
-        let d = self.points[destination_index];
-        let i_square = (s.0 as i64 - d.0 as i64).pow(2) + (s.1 as i64 - d.1 as i64).pow(2);
-        ((i_square as f64).sqrt()) as i128
+    pub fn get_distance(&self, source_index: usize, destination_index: usize) -> f32 {
+        let x1 = self.points[source_index].0 as f32;
+        let y1 = self.points[source_index].1 as f32;
+        let x2 = self.points[destination_index].0 as f32;
+        let y2 = self.points[destination_index].1 as f32;
+        let i_square = (x2 - x1).powf(2.0) + (y2 - y1).powf(2.0);
+        i_square.sqrt()
+    }
+
+    pub fn get_path_length2(&self, path: &[u16]) -> i32 {
+        let mut distance = 0;
+        let mut i = 0;
+        while i < path.len() - 1 {
+            let x1 = self.points[path[i] as usize].0 as i64;
+            let y1 = self.points[path[i] as usize].1 as i64;
+            let x2 = self.points[path[i + 1] as usize].0 as i64;
+            let y2 = self.points[path[i + 1] as usize].1 as i64;
+            let i_square = ((x2 << 16) - (x1 << 16)).pow(2) + ((y2 << 16) - (y1 << 16)).pow(2);
+            distance += i_square.isqrt() as i32;
+            i += 1;
+        }
+
+        distance
+    }
+
+    pub fn get_path_length3(&self, path: &[u16]) -> i32 {
+        let mut distance = 0;
+        let mut i = 0;
+        while i < path.len() - 1 {
+            let x1 = self.points[path[i] as usize].0 as i32;
+            let y1 = self.points[path[i] as usize].1 as i32;
+            let x2 = self.points[path[i + 1] as usize].0 as i32;
+            let y2 = self.points[path[i + 1] as usize].1 as i32;
+            let i_square = (x2 - x1).pow(2) + (y2 - y1).pow(2);
+            distance += i_square.isqrt() << 16;
+            i += 1;
+        }
+
+        distance
+    }
+
+    /// F64, multiplication and rounding at the end
+    pub fn get_path_length_48k(&self, path: &[u16]) -> i32 {
+        let mut distance = 0.0;
+        let mut i = 0;
+        while i < path.len() - 1 {
+            let x1 = self.points[path[i] as usize].0 as i32;
+            let y1 = self.points[path[i] as usize].1 as i32;
+            let x2 = self.points[path[i + 1] as usize].0 as i32;
+            let y2 = self.points[path[i + 1] as usize].1 as i32;
+            let dx = (x2 - x1) as f64;
+            let dy = (y2 - y1) as f64;
+            distance += (dx * dx + dy * dy).sqrt();
+            i += 1;
+        }
+
+        (distance * 65536.0).round() as i32
+    }
+
+    pub fn get_path_length_66k(&self, path: &[u16]) -> i32 {
+        let mut distance = 0.0;
+        let mut i = 0;
+        while i < path.len() - 1 {
+            let x1 = self.points[path[i] as usize].0 as i32;
+            let y1 = self.points[path[i] as usize].1 as i32;
+            let x2 = self.points[path[i + 1] as usize].0 as i32;
+            let y2 = self.points[path[i + 1] as usize].1 as i32;
+            let dx = (x2 - x1) as f32;
+            let dy = (y2 - y1) as f32;
+            distance += (dx * dx + dy * dy).sqrt();
+            i += 1;
+        }
+
+        (distance * 65536.0).round() as i32
+    }
+
+    pub fn get_path_length(&self, path: &[u16]) -> i32 {
+        let mut distance = 0.0;
+        for i in 0..path.len() - 1 {
+            let x1 = self.points[path[i] as usize].0 as i32;
+            let y1 = self.points[path[i] as usize].1 as i32;
+            let x2 = self.points[path[i + 1] as usize].0 as i32;
+            let y2 = self.points[path[i + 1] as usize].1 as i32;
+            let dx = (x2 - x1) as f64;
+            let dy = (y2 - y1) as f64;
+            distance += (dx * dx + dy * dy).sqrt();
+        }
+
+        (distance * 65536.0).round() as i32
     }
 }