more accurate distances

Author: Trolldemorted

navpointmatrixcli/src/cli.rs

@@ -15,6 +15,7 @@ pub enum Command {
     ConnectedNodesFromNavpointMatrix(ConnectedNodesFromNavpointMatrixArgs),
     ConnectedNodesFromNavigationData(ConnectedNodesFromNavigationDataArgs),
     NavpointMatrix(BuildNavpointMatrixArgs),
+    ShowPath(ShowPathArgs),
 }
 
 #[derive(Args, Debug)]
@@ -67,3 +68,14 @@ pub enum ConnectedNodesMode {
     BresenhamLine,
     //P3,
 }
+
+#[derive(Args, Debug)]
+pub struct ShowPathArgs {
+    /// Source node index
+    #[arg(value_name = "source index", required = true)]
+    pub source_index: u16,
+
+    /// Destination node index
+    #[arg(value_name = "destination index", required = true)]
+    pub destination_index: u16,
+}

navpointmatrixcli/src/main.rs

@@ -1,5 +1,5 @@
 use clap::Parser;
-use cli::{BuildNavpointMatrixArgs, Cli, Command, ConnectedNodesFromNavigationDataArgs, ConnectedNodesFromNavpointMatrixArgs};
+use cli::{BuildNavpointMatrixArgs, Cli, Command, ConnectedNodesFromNavigationDataArgs, ConnectedNodesFromNavpointMatrixArgs, ShowPathArgs};
 use connected_nodes::ConnectedNodes;
 use p3_api::data::{navigation_matrix::NavigationMatrix, navigation_vector::NavigationVector, navpoint_matrix::NavpointMatrix};
 use pathfinding::prelude::{build_path, dijkstra_all};
@@ -18,6 +18,7 @@ fn main() {
         Command::ConnectedNodesFromNavpointMatrix(args) => build_connected_nodes_from_navpoint_matrix(args),
         Command::ConnectedNodesFromNavigationData(args) => build_connected_nodes_from_navigation_data(args),
         Command::Test => test(),
+        Command::ShowPath(show_path_args) => show_path(show_path_args),
     }
 }
 
@@ -56,10 +57,26 @@ fn build_connected_nodes_from_navigation_data(args: ConnectedNodesFromNavigation
     fs::write(args.output_file, connected_nodes.serialize()).unwrap();
 }
 
+fn show_path(args: ShowPathArgs) {
+    let navpoint_matrix = NavpointMatrix::deserialize(&fs::read(r"C:\Users\Benni\Patrician 3_workbench\navdata\matrix_int.dat.orig").unwrap());
+    let navigation_vector = NavigationVector::deserialize(&fs::read(r"C:\Users\Benni\Patrician 3_workbench\navdata\nav_vec.dat.orig").unwrap());
+    let mut current = args.source_index;
+    let current_point = navigation_vector.points[current as usize];
+    let mut buf = format!("{current} {current_point:x?}");
+    let distance = navpoint_matrix.get(current, args.destination_index, 350).distance;
+    while current != args.destination_index {
+        let cell = navpoint_matrix.get(current, args.destination_index, 350);
+        current = cell.next;
+        let current_point = navigation_vector.points[current as usize];
+        buf.push_str(&format!(" -> {current} {current_point:x?}"));
+    }
+    println!("{buf} (declared distance: {distance})");
+}
+
 fn test() {
     let start = Instant::now();
-    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 navigation_vector = NavigationVector::deserialize(&fs::read(r"C:\Users\Benni\Patrician 3_workbench\navdata\nav_vec.dat.orig").unwrap());
+    let original_navpoint_matrix = NavpointMatrix::deserialize(&fs::read(r"C:\Users\Benni\Patrician 3_workbench\navdata\matrix_int.dat.orig").unwrap());
     let connected_nodes = ConnectedNodes::from_navpoint_matrix(&original_navpoint_matrix);
     let mut new_navpoint_matrix = NavpointMatrix::new(navigation_vector.length);
 
@@ -99,7 +116,8 @@ fn test() {
         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}");
+            let (source, destination) = new_navpoint_matrix.get_source_and_destination(i, navigation_vector.length);
+            println!("cell {i}: distance {orig_distance} != {calculated_distance} ({source} -> {destination})");
             bad_distance_cells += 1;
         }
     }

p3-api/src/data/navigation_vector.rs

@@ -1,3 +1,5 @@
+use std::ops::Shl;
+
 #[derive(Debug)]
 pub struct NavigationVector {
     pub length: u16,
@@ -18,17 +20,18 @@ impl NavigationVector {
     }
 
     pub fn get_path_length(&self, path: &[u16]) -> i32 {
-        let mut distance = 0.0;
+        let mut distance = 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();
+            let dx = (x2 - x1) as i64;
+            let dy = (y2 - y1) as i64;
+            let squared = dx * dx + dy * dy;
+            distance += ((squared.shl(32) as f64).sqrt() + 0.5) as i64;
         }
 
-        (distance * 65536.0).round() as i32
+        (distance) as i32
     }
 }

p3-api/src/data/navpoint_matrix.rs

@@ -46,4 +46,19 @@ impl NavpointMatrix {
     pub fn set_next(&mut self, source: u16, destination: u16, next: u16, distance: i32, width: u16) {
         self.matrix[source as usize * width as usize + destination as usize] = NavpointMatrixCell { distance, next }
     }
+
+    pub fn get_next(&self, source: u16, destination: u16, width: u16) -> u16 {
+        self.matrix[source as usize * width as usize + destination as usize].next
+    }
+
+    pub fn get(&self, source: u16, destination: u16, width: u16) -> &NavpointMatrixCell {
+        &self.matrix[source as usize * width as usize + destination as usize]
+    }
+
+    pub fn get_source_and_destination(&self, cell: usize, width: u16) -> (u16, u16) {
+        let width = width as usize;
+        let destination = cell % width;
+        let source = cell / width;
+        (source as _, destination as _)
+    }
 }