diff --git a/include/omath/pathfinding/NavigationMesh.h b/include/omath/pathfinding/NavigationMesh.h
index 1af4b06..c37da96 100644
--- a/include/omath/pathfinding/NavigationMesh.h
+++ b/include/omath/pathfinding/NavigationMesh.h
@@ -15,7 +15,7 @@ namespace omath::pathfinding
     struct NavigationVertex
     {
         Vector3 origin;
-        std::vector<Vector3*> connections;
+        std::vector<NavigationVertex*> connections;
     };
 
 
diff --git a/source/pathfinding/Astar.cpp b/source/pathfinding/Astar.cpp
index f211de7..12c3b55 100644
--- a/source/pathfinding/Astar.cpp
+++ b/source/pathfinding/Astar.cpp
@@ -3,15 +3,15 @@
 //
 #include "omath/pathfinding/Astar.h"
 #include <unordered_map>
-
+#include <unordered_set>
 
 
 namespace omath::pathfinding
 {
     struct PathNode final
     {
-        PathNode* cameFrom;
-        const NavigationVertex* navVertex;
+        Vector3 cameFrom;
+        NavigationVertex const* navVertex;
         float gCost = 0.f;
     };
 
@@ -24,11 +24,23 @@ namespace omath::pathfinding
         const auto& startVertex = navMesh.GetClossestVertex(start).value();
         const auto& endVertex = navMesh.GetClossestVertex(end).value();
 
-        openList.emplace(startVertex.origin, PathNode{nullptr, &startVertex, 0.f});
+        openList.emplace(startVertex.origin, PathNode{startVertex.origin, &startVertex, 0.f});
 
         while (!openList.empty())
         {
-            
+            const auto [cord, node] = *std::ranges::min_element(openList,
+                [&endVertex](const auto& a, const auto& b) -> bool
+                {
+                    const auto aCost = a.second.gCost + a.second.navVertex->origin.DistTo(endVertex.origin);
+                    const auto bCost = b.second.gCost + b.second.navVertex->origin.DistTo(endVertex.origin);
+                    return aCost < bCost;
+                });
+
+            openList.erase(cord);
+
+            for (const auto& neighbor : node.navVertex->connections)
+                if (!closedList.contains(neighbor->origin))
+                    closedList.emplace(neighbor->origin, PathNode{cord, neighbor, neighbor->origin.DistTo(cord) + node.gCost});
         }
 
         return {};