Merge remote-tracking branch 'origin/pathfinder-25'

# Conflicts:
#	lunabotics/lunabot/src/apps/sim.rs

lunabotics/lunabot/src/apps/sim.rs

@@ -1,4 +1,5 @@
 use core::str;
+use std::ops::Deref;
 use std::{
     cmp::Ordering,
     collections::VecDeque,
@@ -19,6 +20,7 @@ use gputter::{
 };
 use lunabot_ai::{run_ai, Action, Input, PollWhen};
 use nalgebra::{Isometry3, UnitQuaternion, UnitVector3, Vector2, Vector3, Vector4};
+use pathfinding::Pathfinder;
 use serde::{Deserialize, Serialize};
 use thalassic::DepthProjectorBuilder;
 use urobotics::{app::define_app, log::{log_to_console, Level}, tokio};
@@ -88,7 +90,6 @@ pub struct LunasimbotApp {
 }
 
 impl Runnable for LunasimbotApp {
-
     fn run(mut self) {
         log_to_console([("wgpu_hal::vulkan::instance", Level::Info), ("wgpu_core::device::resource", Level::Info)]);
         log_teleop_messages();
@@ -312,6 +313,19 @@ impl Runnable for LunasimbotApp {
             lunasim_stdin2.write(bytes);
         }));
 
+        // Add callback for pathfinding
+        const CELL_COUNT: usize = 64 * 128;
+        let height_map: Arc<Mutex<[f32; CELL_COUNT]>> =
+            Arc::new(Mutex::new([0f32; CELL_COUNT]));
+        let height_map_copy = height_map.clone();
+        heightmap_callbacks.add_dyn_fn_mut(Box::new(move |heights| {
+            if let Ok(mut map_arr) = height_map_copy.try_lock() {
+                *map_arr = heights.try_into().unwrap();
+            }
+        }));
+        let mut finder = Pathfinder::new(Vector2::new(32.0, 16.0), 0.0625);
+        let gradient_map: &[f32] = &[0f32; CELL_COUNT]; // Replace this with gradient map callback
+
         let lunabot_stage = Arc::new(AtomicCell::new(LunabotStage::SoftStop));
 
         let (packet_builder, mut from_lunabase_rx, mut connected) = create_packet_builder(
@@ -338,8 +352,14 @@ impl Runnable for LunasimbotApp {
                         lunasim_stdin.write(bytes);
                     }
                     Action::CalculatePath { from, to, mut into } => {
-                        into.push(from);
-                        into.push(to);
+                        finder.append_path(
+                            from,
+                            to,
+                            height_map.lock().unwrap().deref(),
+                            gradient_map,
+                            1.0,
+                            &mut into,
+                        );
                         inputs.push(Input::PathCalculated(into));
                     }
                 },

lunabotics/pathfinding/src/astar.rs

@@ -32,24 +32,16 @@ pub(crate) fn astar(
     mut start: Vector2<f64>,
     mut goal: Vector2<f64>,
     map_dimension: Vector2<f64>,
-    offset: Vector2<f64>,
     step_size: f64,
     mut is_safe: impl FnMut(Vector2<f64>, Vector2<f64>) -> bool,
 ) -> Vec<Vector2<f64>> {
     let startf = start;
     let goalf = goal;
     let max_index = Vector2::new(
-        (map_dimension.x as f64 / step_size).round() as u32,
-        (map_dimension.y as f64 / step_size).round() as u32,
+        (map_dimension.x / step_size).round() as u32,
+        (map_dimension.y / step_size).round() as u32,
     );
 
-    start -= offset;
-    if start.x < 0.0 {
-        start.x = 0.0;
-    }
-    if start.y < 0.0 {
-        start.y = 0.0;
-    }
     let mut start = Vector2::new(
         (start.x / step_size).round() as u32,
         (start.y / step_size).round() as u32,
@@ -61,13 +53,6 @@ pub(crate) fn astar(
         start.y = max_index.y;
     }
 
-    goal -= offset;
-    if goal.x < 0.0 {
-        goal.x = 0.0;
-    }
-    if goal.y < 0.0 {
-        goal.y = 0.0;
-    }
     let mut goal = Vector2::new(
         (goal.x / step_size).round() as u32,
         (goal.y / step_size).round() as u32,
@@ -110,10 +95,7 @@ pub(crate) fn astar(
             let node_parent = parents.get(&node).unwrap();
             let mut successors = heapless::Vec::<_, 8>::new();
             let mut try_add = |next: Vector2<u32>, successor_parent: Parent, cost: usize| {
-                if is_safe(
-                    step_size * node.cast() + offset,
-                    step_size * next.cast() + offset,
-                ) {
+                if is_safe(step_size * node.cast(), step_size * next.cast()) {
                     successors.push((next, successor_parent, cost)).unwrap();
                 }
             };
@@ -177,6 +159,11 @@ pub(crate) fn astar(
         }
     }
 
+    // End here if pathfinding was not successful
+    if !parents.contains_key(&goal) {
+        return vec![startf];
+    }
+
     let mut path = vec![goalf; best_cost_so_far.length];
 
     for i in (1..best_cost_so_far.length - 1).rev() {
@@ -196,7 +183,7 @@ pub(crate) fn astar(
             };
         }
         traverse!();
-        path[i] = step_size * best_so_far.cast() + offset;
+        path[i] = step_size * best_so_far.cast();
 
         #[cfg(debug_assertions)]
         {