From fee446cb7d90179e7c935362c3d87fb26a952518 Mon Sep 17 00:00:00 2001
From: Nolan Darilek <nolan@thewordnerd.info>
Date: Wed, 21 Jul 2021 15:20:02 -0500
Subject: [PATCH] Pathfinding optimizations.

---
 src/pathfinding.rs | 60 +++++++++++++++++++++++++++-------------------
 1 file changed, 36 insertions(+), 24 deletions(-)

diff --git a/src/pathfinding.rs b/src/pathfinding.rs
index cb9d119..ef2dc3d 100644
--- a/src/pathfinding.rs
+++ b/src/pathfinding.rs
@@ -176,7 +176,7 @@ fn find_path_for_shape(
             } else {
                 None
             })
-            .expect("Channel should exist");
+            .ok();
     })
     .detach();
 }
@@ -188,10 +188,7 @@ fn calculate_path(
     obstructions: Query<&MapObstruction>,
     collider_query: QueryPipelineColliderComponentsQuery,
     mut calculating: Local<HashMap<Entity, Receiver<Option<Path>>>>,
-    query: Query<
-        (Entity, &Destination, &Coordinates, &ColliderShape),
-        Or<(Without<Path>, Changed<Destination>)>,
-    >,
+    query: Query<(Entity, &Destination, &Coordinates, &ColliderShape), Changed<Destination>>,
     destinations: Query<&Destination>,
     map: Query<&Map>,
 ) {
@@ -211,23 +208,29 @@ fn calculate_path(
         }
     }
     for (entity, destination, coordinates, shape) in query.iter() {
-        if let std::collections::hash_map::Entry::Vacant(e) = calculating.entry(entity) {
-            let (tx, rx) = unbounded();
-            e.insert(rx);
-            for map in map.iter() {
-                find_path_for_shape(
-                    &*pool,
-                    query_pipeline.clone(),
-                    entity,
-                    coordinates,
-                    destination,
-                    &map,
-                    &collider_query,
-                    &obstructions,
-                    shape,
-                    &tx,
-                );
-            }
+        if calculating.contains_key(&entity) {
+            continue;
+        }
+        if coordinates.i32() == **destination {
+            commands.entity(entity).remove::<Path>();
+            commands.entity(entity).remove::<Destination>();
+            continue;
+        }
+        let (tx, rx) = unbounded();
+        calculating.insert(entity, rx);
+        for map in map.iter() {
+            find_path_for_shape(
+                &*pool,
+                query_pipeline.clone(),
+                entity,
+                coordinates,
+                destination,
+                &map,
+                &collider_query,
+                &obstructions,
+                shape,
+                &tx,
+            );
         }
     }
 }
@@ -257,8 +260,17 @@ fn negotiate_path(
                 new_path = vec![start_i32];
                 new_path.append(&mut upcoming.to_vec());
             } else {
-                commands.entity(entity).remove::<Path>();
-                velocity.linvel = Vec2::ZERO.into();
+                let start = Vec2::new(
+                    position.position.translation.x,
+                    position.position.translation.y,
+                );
+                let next = path[1];
+                let next = Vec2::new(next.0 as f32 + 0.5, next.1 as f32 + 0.5);
+                let mut direction = next - start;
+                direction = direction.normalize();
+                direction *= **speed;
+                velocity.linvel = direction.into();
+                continue;
             }
         } else {
             commands.entity(entity).remove::<Path>();