diff options
| -rw-r--r-- | source/kit/astar.inl.h | 42 |
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; } |