Simplify costmaps and add bounds checks to prevent overflows.

src/pathfinding.rs

@@ -2,6 +2,7 @@ use bevy::{
     ecs::entity::Entities,
     prelude::*,
     tasks::{prelude::*, Task},
+    utils::HashMap,
 };
 use bevy_rapier2d::{
     na::{Isometry2, Vector2},
@@ -58,32 +59,9 @@ pub struct NoPath;
 #[reflect(Component)]
 pub struct Path(pub Vec<(i32, i32)>);
 
-#[derive(Component, Clone, Debug, Default, Reflect)]
+#[derive(Component, Clone, Debug, Default, Reflect, Deref, DerefMut)]
 #[reflect(Component)]
-pub struct CostMap {
-    width: usize,
-    costs: Vec<f32>,
-}
-
-impl CostMap {
-    pub fn new(width: usize, height: usize) -> Self {
-        let count = width * height;
-        Self {
-            width,
-            costs: vec![1.; count],
-        }
-    }
-
-    pub fn modifier(&self, x: i32, y: i32) -> Option<&f32> {
-        let idx = (y as usize) * self.width + (x as usize);
-        self.costs.get(idx)
-    }
-
-    pub fn set_modifier(&mut self, x: usize, y: usize, cost: f32) {
-        let idx = y * self.width + x;
-        self.costs[idx] = cost;
-    }
-}
+pub struct CostMap(pub HashMap<(i32, i32), f32>);
 
 pub fn find_path<D: 'static + Clone + Default + Send + Sync>(
     start: &dyn PointLike,
@@ -95,12 +73,14 @@ pub fn find_path<D: 'static + Clone + Default + Send + Sync>(
         &start.into(),
         |p| {
             let mut successors: Vec<((i32, i32), u32)> = vec![];
+            if p.0 >= 0 && p.1 >= 0 {
                 if let Some(tile) = map.at(p.0 as usize, p.1 as usize) {
                     if tile.is_walkable() {
                         for tile in map.get_available_exits(p.0 as usize, p.1 as usize) {
                             let mut cost = tile.2 * 100.;
                             if let Some(cost_map) = cost_map {
-                            if let Some(modifier) = cost_map.modifier(tile.0 as i32, tile.1 as i32)
+                                if let Some(modifier) =
+                                    cost_map.get(&(tile.0 as i32, tile.1 as i32))
                                 {
                                     cost *= modifier;
                                 }
@@ -109,6 +89,7 @@ pub fn find_path<D: 'static + Clone + Default + Send + Sync>(
                         }
                     }
                 }
+            }
             successors
         },
         |p| (p.distance_squared(destination) * 100.) as u32,
@@ -161,7 +142,7 @@ fn find_path_for_shape(
                 if should_push {
                     let mut cost = exit.1 * 100.;
                     if let Some(cost_map) = cost_map {
-                        if let Some(modifier) = cost_map.modifier(exit.0 .0, exit.0 .1) {
+                        if let Some(modifier) = cost_map.get(&(exit.0 .0, exit.0 .1)) {
                             cost *= modifier;
                         }
                     }