A* simplify

1 files changed, 21 insertions, 21 deletions

diff --git a/source/kit/astar.inl.h b/source/kit/astar.inl.h
index 7d6d99e..a844fb2 100644
--- a/source/kit/astar.inl.h
+++ b/source/kit/astar.inl.h
@@ -89,15 +89,25 @@ static void astar_iteration(astar_state_t *state) {
   assert(state->closed.values != NULL);
 
   //  FIXME
-  //  Use abstractions for set element erasing
+  //  Use abstractions for set element search
   //
 
-  //  Remove a node from the open set, the last node is a node with
-  //  the lowest estimated distance from the source to the destination
+  //  Find a node that is closest to the destination
   //
-  astar_node_t nearest_node =
-      state->open.values[state->open.size - 1];
-  --state->open.size;
+  astar_node_t nearest_node;
+  {
+    i64 index_nearest = 0;
+    for (i64 i = 1; i < state->open.size; i++)
+      if (state->open.values[i].estimated_source_to_destination <
+          state->open.values[index_nearest]
+              .estimated_source_to_destination)
+        index_nearest = i;
+    nearest_node = state->open.values[index_nearest];
+    if (index_nearest != state->open.size - 1)
+      state->open.values[index_nearest] =
+          state->open.values[state->open.size - 1];
+    --state->open.size;
+  }
 
   //  Enumerate all neighbors
   //
@@ -199,8 +209,9 @@ static void astar_iteration(astar_state_t *state) {
 
       //  Remove this node from the open set
       //
-      for (i64 i = index_in_open + 1; i < state->open.size; ++i)
-        state->open.values[i - 1] = state->open.values[i];
+      if (index_in_open != state->open.size - 1)
+        state->open.values[index_in_open] =
+            state->open.values[state->open.size - 1];
       --state->open.size;
     }
 
@@ -208,21 +219,10 @@ static void astar_iteration(astar_state_t *state) {
     //  Use abstractions to set element adding
     //
 
-    //  Add this node to the open set, sorted by the estimated
-    //  distance from the source to the destination, so the last node
-    //  in the set will be a node with the lowest estimate
+    //  Add this node to the open set
     //
     assert(state->open.size + 1 <= ASTAR_SET_SIZE);
-    i64 index_insert = 0;
-    for (; index_insert < state->open.size; ++index_insert) {
-      if (state->open.values[index_insert]
-              .estimated_source_to_destination >
-          neighbor_node.estimated_source_to_destination)
-        continue;
-    }
-    for (i64 i = state->open.size; i > index_insert; --i)
-      state->open.values[i] = state->open.values[i - 1];
-    state->open.values[index_insert] = neighbor_node;
+    state->open.values[state->open.size] = neighbor_node;
     ++state->open.size;
   }