[WIP] Navpointmatrixcli

.gitignore

@@ -8,3 +8,5 @@ p2arch0_eng
 generated.*.toml
 /deploy*.bat
 /deploy*.ps1
+*.log
+*.dat

Cargo.toml

@@ -19,6 +19,7 @@ members = [
     "mod-tavern-show-all-sailors",
     "mod-town-hall-details",
     "mod-trading-office-prices-synchronization",
+    "navpointmatrixcli",
     #"p3-agent",
     "p3-agent-loader",
     "p3-aim",
@@ -42,3 +43,5 @@ sysinfo = "0.29"
 win_dbg_logger = "0.1"
 num-traits = "0.2"
 num-derive = "0.4"
+byteorder = "1.5.0"
+pathfinding = "4.11.0"

navpointmatrixcli/Cargo.toml

@@ -0,0 +1,14 @@
+[package]
+name = "navpointmatrixcli"
+edition = "2021"
+version.workspace = true
+
+[dependencies]
+p3-api = { path = "../p3-api" }
+clap = { workspace = true }
+log = { workspace = true }
+pathfinding = { workspace = true }
+geo = "0.29.3"
+
+[dev-dependencies]
+regex = "1.11.1"

navpointmatrixcli/README.md

@@ -0,0 +1,8 @@
+# Navpoint Matrix CLI
+
+## Build Connected Nodes
+- `cargo run --bin navpointmatrixcli -- connected-nodes-from-navpoint-matrix --input "C:\Users\Benni\Patrician 3_workbench\navdata\matrix_int.dat"  --output ./connected_nodes.navpointmatrix.dat`
+- `cargo run --bin navpointmatrixcli -- connected-nodes-from-navigation-data --navigation-matrix "C:\Users\Benni\Patrician 3_workbench\navdata\nav_matrix.dat" --navigation-vector "C:\Users\Benni\Patrician 3_workbench\navdata\nav_vec.dat" --output ./connected_nodes.navpointdata.dat`
+
+## Build Navpoint Matrix
+- `cargo run --release --bin navpointmatrixcli -- navpoint-matrix --navigation-vector "C:\Users\Benni\Patrician 3_workbench\navdata\nav_vec.dat" --connected-nodes .\connected_nodes.navpointmatrix.dat --output ./matrix_int.dat`

navpointmatrixcli/src/cli.rs

@@ -0,0 +1,69 @@
+use clap::{Args, Parser, Subcommand, ValueEnum};
+use std::path::PathBuf;
+
+/// Patrician 3 navpoint matrix calculation
+#[derive(Parser, Debug)]
+#[command(version, about, long_about = None)]
+pub struct Cli {
+    #[command(subcommand)]
+    pub command: Command,
+}
+
+#[derive(Subcommand, Debug)]
+pub enum Command {
+    Test,
+    ConnectedNodesFromNavpointMatrix(ConnectedNodesFromNavpointMatrixArgs),
+    ConnectedNodesFromNavigationData(ConnectedNodesFromNavigationDataArgs),
+    NavpointMatrix(BuildNavpointMatrixArgs),
+}
+
+#[derive(Args, Debug)]
+pub struct BuildNavpointMatrixArgs {
+    /// Path to the connected nodes file
+    #[arg(long = "connected-nodes", value_name = "connected-nodes-file")]
+    pub connected_nodes_file: PathBuf,
+
+    /// Path to the navigation vector file
+    #[arg(long = "navigation-vector", value_name = "navigation-vector-file")]
+    pub navigation_vector_file: PathBuf,
+
+    /// Path to the output navpoint matrix file
+    #[arg(long = "output", value_name = "output-file")]
+    pub output_file: PathBuf,
+}
+
+#[derive(Args, Debug)]
+pub struct ConnectedNodesFromNavpointMatrixArgs {
+    /// Path to the input navpoint matrix file
+    #[arg(long = "input", value_name = "input-file")]
+    pub navpoint_matrix_file: PathBuf,
+
+    /// Path to the output connected nodes file
+    #[arg(long = "output", value_name = "output-file")]
+    pub output_file: PathBuf,
+}
+
+#[derive(Args, Debug)]
+pub struct ConnectedNodesFromNavigationDataArgs {
+    /// Path to the input navigation matrix file
+    #[arg(long = "navigation-matrix", value_name = "navigation-matrix-file")]
+    pub navigation_matrix_file: PathBuf,
+
+    /// Path to the input navigation vector file
+    #[arg(long = "navigation-vector", value_name = "navigation-vector-file")]
+    pub navigation_vector_file: PathBuf,
+
+    /// F
+    #[arg(long = "calculation-mode", value_name = "calculation-mode")]
+    pub mode: ConnectedNodesMode,
+
+    /// Path to the output connected nodes file
+    #[arg(long = "output", value_name = "output-file")]
+    pub output_file: PathBuf,
+}
+
+#[derive(Debug, Clone, Copy, ValueEnum)]
+pub enum ConnectedNodesMode {
+    BresenhamLine,
+    //P3,
+}

navpointmatrixcli/src/connected_nodes.rs

@@ -0,0 +1,193 @@
+use p3_api::data::{navigation_matrix::NavigationMatrix, navigation_vector::NavigationVector, navpoint_matrix::NavpointMatrix};
+
+use crate::cli::ConnectedNodesMode;
+
+#[derive(Debug, Clone)]
+pub struct ConnectedNodes {
+    pub connected_nodes: Vec<(u16, u16)>,
+}
+
+impl ConnectedNodes {
+    pub fn serialize(&self) -> Vec<u8> {
+        let mut buf = vec![];
+        for pair in &self.connected_nodes {
+            buf.extend_from_slice(&pair.0.to_le_bytes());
+            buf.extend_from_slice(&pair.1.to_le_bytes());
+        }
+        buf
+    }
+
+    pub fn deserialize(data: &[u8]) -> Self {
+        let len = data.len() / 4;
+        let mut connected_nodes = Vec::with_capacity(len);
+        for i in 0..len {
+            let source = u16::from_le_bytes(data[i * 4..i * 4 + 2].try_into().unwrap());
+            let destination = u16::from_le_bytes(data[i * 4 + 2..i * 4 + 4].try_into().unwrap());
+            connected_nodes.push((source, destination));
+        }
+
+        Self { connected_nodes }
+    }
+
+    pub fn get_neighbours(&self, node_index: u16, nav_vec: &NavigationVector) -> Vec<(u16, i32)> {
+        let mut neighbours = vec![];
+        for n in &self.connected_nodes {
+            if n.0 == node_index {
+                neighbours.push((n.1, nav_vec.get_path_length(&[n.0 as _, n.1 as _])));
+            }
+        }
+
+        neighbours
+    }
+
+    pub fn from_navigation_matrix_p3(navigation_vector: &NavigationVector, navigation_matrix: &NavigationMatrix, mode: ConnectedNodesMode) -> Self {
+        let mut nodes = vec![];
+        for (source_index, source) in navigation_vector.points.iter().enumerate() {
+            println!("Calculating neighbours for node {source_index}");
+            for (destination_index, destination) in navigation_vector.points.iter().enumerate() {
+                if is_connected(*source, *destination, navigation_matrix, mode) {
+                    nodes.push((source_index as _, destination_index as _))
+                }
+            }
+        }
+
+        ConnectedNodes { connected_nodes: nodes }
+    }
+
+    pub fn from_navpoint_matrix(navpoint_matrix: &NavpointMatrix) -> Self {
+        let mut nodes = vec![];
+        let nodes_count = navpoint_matrix.matrix.len().isqrt();
+        for source_index in 0..nodes_count {
+            for destination_index in 0..nodes_count {
+                let cell = &navpoint_matrix.matrix[source_index * nodes_count + destination_index];
+                if cell.next == destination_index as u16 {
+                    nodes.push((source_index as _, destination_index as _));
+                }
+            }
+        }
+        ConnectedNodes { connected_nodes: nodes }
+    }
+}
+
+fn is_connected(p0: (i16, i16), p1: (i16, i16), navigation_matrix: &NavigationMatrix, mode: ConnectedNodesMode) -> bool {
+    if p0 == p1 {
+        return true;
+    }
+
+    let line = match mode {
+        ConnectedNodesMode::BresenhamLine => get_bresenham_line(p0.0, p0.1, p1.0, p0.1),
+        //ConnectedNodesMode::P3 => get_p3_line(p0.0, p0.1, p1.0, p0.1),
+    };
+    for point in line {
+        if navigation_matrix.data[point.0 as usize + navigation_matrix.width as usize * point.1 as usize] == 1 {
+            return false;
+        }
+    }
+    true
+}
+
+pub fn get_bresenham_line(mut x0: i16, mut y0: i16, x1: i16, y1: i16) -> Vec<(i16, i16)> {
+    let mut path = Vec::with_capacity(42);
+    let dx = (x1 - x0).abs();
+    let sx = if x0 < x1 { 1 } else { -1 };
+    let dy = -((y1 - y0).abs());
+    let sy = if y0 < y1 { 1 } else { -1 };
+    let mut error = dx + dy;
+    loop {
+        path.push((x0, y0));
+        let e2 = 2 * error;
+        if e2 >= dy {
+            if x0 == x1 {
+                break;
+            }
+            error += dy;
+            x0 += sx;
+        }
+        if e2 <= dx {
+            if y0 == y1 {
+                break;
+            }
+            error += dx;
+            y0 += sy;
+        }
+    }
+    path
+}
+
+pub fn get_p3_line(mut x0: i16, mut y0: i16, x1: i16, y1: i16) -> Vec<(i16, i16)> {
+    let mut path = Vec::with_capacity(42);
+    path.push((x0, y0));
+    let dx_abs = (x1 - x0).abs();
+    let dy_abs = (y1 - y0).abs();
+    let sx = if x0 < x1 { 1 } else { -1 };
+    let sy = if y0 < y1 { 1 } else { -1 };
+
+    let mut diff = dx_abs;
+    let steps = dx_abs;
+    for _ in 0..steps {
+        // 0x0044477A
+        x0 += sx; // Apply x step
+        diff += 2 * dy_abs; // Add 2*dy to diff
+        if diff >= 2 * dx_abs {
+            // 0x00444784
+            diff -= 2 * dx_abs;
+            if diff == 0 {
+                path.push((x0, y0));
+                y0 += sy;
+            } else {
+                y0 += sy;
+                path.push((x0, y0));
+            }
+        } else {
+            // 0x00444793
+            path.push((x0, y0));
+        }
+    }
+
+    path
+}
+
+#[cfg(test)]
+mod tests {
+    use std::{
+        fs,
+        path::{Path, PathBuf},
+    };
+
+    use regex::Regex;
+
+    use super::*;
+
+    #[test]
+    fn it_works() {
+        test_file(&PathBuf::from("tests/lines/line1.txt"));
+        test_file(&PathBuf::from("tests/lines/line2.txt"));
+        test_file(&PathBuf::from("tests/lines/line3.txt"));
+    }
+
+    fn test_file(path: &Path) {
+        let data = fs::read_to_string(path).unwrap();
+        let mut lines: Vec<&str> = data.split("\n").collect();
+        let header = lines.remove(0);
+        let header_re = Regex::new(r"^\((\d+)\, (\d+)\) -> \((\d+)\, (\d+)\)").unwrap();
+        let line_re = Regex::new(r"^\((\d+)\, (\d+)\)").unwrap();
+        let header_captures = header_re.captures(header).unwrap();
+        let x0: i16 = header_captures.get(1).unwrap().as_str().parse().unwrap();
+        let y0: i16 = header_captures.get(2).unwrap().as_str().parse().unwrap();
+        let x1: i16 = header_captures.get(3).unwrap().as_str().parse().unwrap();
+        let y1: i16 = header_captures.get(4).unwrap().as_str().parse().unwrap();
+
+        let mut calculated_line = get_p3_line(x1, y1, x0, y0);
+        calculated_line.remove(0);
+        calculated_line.pop();
+        println!("{calculated_line:?}");
+        for (i, line) in lines.iter().enumerate() {
+            let line_captures = line_re.captures(line).unwrap();
+            let x: i16 = line_captures.get(1).unwrap().as_str().parse().unwrap();
+            let y: i16 = line_captures.get(2).unwrap().as_str().parse().unwrap();
+            println!("({}, {})", calculated_line[i].0, calculated_line[i].1);
+            assert_eq!(x, calculated_line[i].0);
+            assert_eq!(y, calculated_line[i].1);
+        }
+    }
+}