wip

Author: Trolldemorted

.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,11 @@
+[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"

navpointmatrixcli/src/main.rs

@@ -0,0 +1,169 @@
+use std::fs::{self};
+
+use p3_api::data::{navigation_matrix::NavigationMatrix, navigation_vector::NavigationVector, navpoint_matrix::NavpointMatrix};
+use pathfinding::prelude::{build_path, dijkstra_all};
+
+pub struct DirectlyConnectedNodes {
+    pub connected_nodes: Vec<(u16, u16)>,
+}
+
+fn main() {
+    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 original_connected_nodes = DirectlyConnectedNodes::from_navpoint_matrix(&original_navpoint_matrix);
+    fs::write("./connections.orig.dat", original_connected_nodes.serialize()).unwrap();
+    for (source, destination) in &original_connected_nodes.connected_nodes {
+        println!("{source} {destination}")
+    }
+    */
+
+    /*
+    let calculated_connected_nodes = DirectlyConnectedNodes::from_navigation_vector(&navigation_vector, &navigation_matrix);
+    fs::write("./connections.calculated.dat", calculated_connected_nodes.serialize()).unwrap();
+    for (source, destination) in &calculated_connected_nodes.connected_nodes {
+        println!("{source} {destination}")
+    }
+    */
+
+    //let connected_nodes = DirectlyConnectedNodes::from_file(&fs::read("./connections.orig.dat").unwrap());
+
+    let mut new_navpoint_matrix = NavpointMatrix::new(navigation_vector.length);
+    let connected_nodes = DirectlyConnectedNodes::from_navpoint_matrix(&original_navpoint_matrix);
+    for (source_index, _node) in navigation_vector.points.iter().enumerate() {
+        let source_index = source_index as u16;
+        let parents = dijkstra_all(&source_index, |n| connected_nodes.get_neighbours(*n, &navigation_vector));
+        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:?}");
+                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]);
+    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);
+    }
+}
+
+fn is_connected(p0: (i16, i16), p1: (i16, i16), navigation_matrix: &NavigationMatrix) -> bool {
+    if p0 == p1 {
+        return true;
+    }
+
+    // Bresenham's Line Algorithm
+    let (mut x0, mut y0) = p0;
+    let (x1, y1) = p1;
+    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 {
+        if navigation_matrix.data[x0 as usize + navigation_matrix.width as usize * y0 as usize] == 1 {
+            return false;
+        }
+        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;
+        }
+    }
+
+    navigation_matrix.data[x0 as usize + navigation_matrix.width as usize * y0 as usize] == 0
+}
+
+impl DirectlyConnectedNodes {
+    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 get_neighbours(&self, node_index: u16, nav_vec: &NavigationVector) -> Vec<(u16, i128)> {
+        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
+    }
+
+    pub fn from_file(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..i + 2].try_into().unwrap());
+            let destination = u16::from_le_bytes(data[i + 2..i + 4].try_into().unwrap());
+            connected_nodes.push((source, destination));
+        }
+
+        Self { connected_nodes }
+    }
+
+    pub fn from_navigation_vector(navigation_vector: &NavigationVector, navigation_matrix: &NavigationMatrix) -> 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) {
+                    nodes.push((source_index as _, destination_index as _))
+                }
+            }
+        }
+
+        DirectlyConnectedNodes { 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 _));
+                }
+            }
+        }
+        DirectlyConnectedNodes { connected_nodes: nodes }
+    }
+}

p3-api/src/data/mod.rs

@@ -8,6 +8,9 @@ pub mod convoy;
 pub mod enums;
 pub mod mission;
 pub mod missions;
+pub mod navigation_matrix;
+pub mod navigation_vector;
+pub mod navpoint_matrix;
 pub mod office;
 pub mod p3_ptr;
 pub mod screen_game_ini_anim;

p3-api/src/data/navigation_matrix.rs

@@ -0,0 +1,15 @@
+#[derive(Debug)]
+pub struct NavigationMatrix {
+    pub width: u16,
+    pub height: u16,
+    pub data: Vec<u8>,
+}
+
+impl NavigationMatrix {
+    pub fn deserialize(data: &[u8]) -> Self {
+        let width: u16 = u16::from_le_bytes(data[0..2].try_into().unwrap());
+        let height: u16 = u16::from_le_bytes(data[2..4].try_into().unwrap());
+        let data = data[4..].to_vec();
+        NavigationMatrix { width, height, data }
+    }
+}

p3-api/src/data/navigation_vector.rs

@@ -0,0 +1,26 @@
+#[derive(Debug)]
+pub struct NavigationVector {
+    pub length: u16,
+    pub points: Vec<(i16, i16)>,
+}
+
+impl NavigationVector {
+    pub fn deserialize(data: &[u8]) -> Self {
+        let length: u16 = u16::from_le_bytes(data[0..2].try_into().unwrap());
+        let mut points = vec![];
+        for i in 0..length as usize {
+            let x = i16::from_le_bytes(data[4 + 4 * i..4 + 4 * i + 2].try_into().unwrap());
+            let y = i16::from_le_bytes(data[4 + 4 * i + 2..4 + 4 * i + 4].try_into().unwrap());
+            points.push((x, y));
+        }
+
+        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
+    }
+}

p3-api/src/data/navpoint_matrix.rs

@@ -0,0 +1,40 @@
+#[derive(Debug, Eq, PartialEq)]
+pub struct NavpointMatrix {
+    pub matrix: Vec<NavpointMatrixCell>,
+}
+
+#[derive(Debug, Eq, PartialEq, Clone)]
+pub struct NavpointMatrixCell {
+    pub distance: i32,
+    pub next: u16,
+}
+
+impl Default for NavpointMatrixCell {
+    fn default() -> Self {
+        Self { distance: -1, next: u16::MAX }
+    }
+}
+
+impl NavpointMatrix {
+    pub fn new(width: u16) -> Self {
+        Self {
+            matrix: vec![Default::default(); (width as usize).pow(2)],
+        }
+    }
+
+    pub fn deserialize(data: &[u8]) -> Self {
+        let elements = data.len() / 6;
+        let mut matrix = Vec::with_capacity(elements);
+        for i in 0..elements {
+            let distance = i32::from_le_bytes(data[i * 6..i * 6 + 4].try_into().unwrap());
+            let next = u16::from_le_bytes(data[i * 6 + 4..i * 6 + 4 + 2].try_into().unwrap());
+            matrix.push(NavpointMatrixCell { distance, next })
+        }
+
+        NavpointMatrix { matrix }
+    }
+
+    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 }
+    }
+}