From 0f78ed421d14695f68985967d36e887e9c6053ea Mon Sep 17 00:00:00 2001
From: Nolan Darilek <nolan@thewordnerd.info>
Date: Tue, 6 Sep 2022 17:25:14 -0500
Subject: [PATCH] Add ability to store path cost modifiers as a component.

---
 src/pathfinding.rs | 50 +++++++++++++++++++++++++++++++++++++++++++---
 1 file changed, 47 insertions(+), 3 deletions(-)

diff --git a/src/pathfinding.rs b/src/pathfinding.rs
index 9e5b1e8..1b3c4bb 100644
--- a/src/pathfinding.rs
+++ b/src/pathfinding.rs
@@ -39,10 +39,38 @@ pub struct NoPath;
 #[reflect(Component)]
 pub struct Path(pub Vec<(i32, i32)>);
 
+#[derive(Component, Clone, Debug, Default, Reflect)]
+#[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: usize, y: usize) -> 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 as usize) * self.width + (x as usize);
+        self.costs[idx] = cost;
+    }
+}
+
 pub fn find_path<D: 'static + Clone + Default + Send + Sync>(
     start: &dyn PointLike,
     destination: &dyn PointLike,
     map: &Map<D>,
+    cost_map: &Option<CostMap>,
 ) -> Option<(Vec<(i32, i32)>, u32)> {
     astar(
         &start.into(),
@@ -51,7 +79,13 @@ pub fn find_path<D: 'static + Clone + Default + Send + Sync>(
             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) {
-                        successors.push(((tile.0 as i32, tile.1 as i32), (tile.2 * 100.) as u32));
+                        let mut cost = tile.2 * 100.;
+                        if let Some(cost_map) = cost_map {
+                            if let Some(modifier) = cost_map.modifier(tile.0, tile.1) {
+                                cost *= modifier;
+                            }
+                        }
+                        successors.push(((tile.0 as i32, tile.1 as i32), cost as u32));
                     }
                 }
             }
@@ -67,6 +101,7 @@ fn find_path_for_shape<D: 'static + Clone + Default + Send + Sync>(
     start: Transform,
     destination: Destination,
     map: Map<D>,
+    cost_map: &Option<CostMap>,
     query_pipeline: QueryPipeline,
     collider_set: ColliderSet,
     rigid_body_set: RigidBodySet,
@@ -97,8 +132,14 @@ fn find_path_for_shape<D: 'static + Clone + Default + Send + Sync>(
                             },
                         );
                         if should_push {
+                            let mut cost = tile.2 * 100.;
+                            if let Some(cost_map) = cost_map {
+                                if let Some(modifier) = cost_map.modifier(tile.0, tile.1) {
+                                    cost *= modifier;
+                                }
+                            }
                             successors
-                                .push(((tile.0 as i32, tile.1 as i32), (tile.2 * 100.) as u32));
+                                .push(((tile.0 as i32, tile.1 as i32), cost as u32));
                         }
                     }
                 }
@@ -126,12 +167,13 @@ fn calculate_path<D: 'static + Clone + Default + Send + Sync>(
             &Destination,
             &Transform,
             &Collider,
+            Option<&CostMap>,
         ),
         Changed<Destination>,
     >,
     map: Query<&Map<D>>,
 ) {
-    for (entity, handle, destination, coordinates, shape) in query.iter() {
+    for (entity, handle, destination, coordinates, shape, cost_map) in &query {
         if coordinates.i32() == **destination {
             commands
                 .entity(entity)
@@ -147,6 +189,7 @@ fn calculate_path<D: 'static + Clone + Default + Send + Sync>(
             let destination_clone = *destination;
             let query_pipeline = rapier_context.query_pipeline.clone();
             let map_clone = map.clone();
+            let cost_map_clone = cost_map.cloned();
             let handle_clone = *handle;
             let mut collider_set = rapier_context.colliders.clone();
             let mut to_remove = vec![];
@@ -170,6 +213,7 @@ fn calculate_path<D: 'static + Clone + Default + Send + Sync>(
                     coordinates_clone,
                     destination_clone,
                     map_clone,
+                    &cost_map_clone,
                     query_pipeline,
                     collider_set,
                     bodies,