Optimize path planning performance by 80x through provider improvements and A* implementation

core/CMakeLists.txt

@@ -25,6 +25,7 @@ set(CORE_SOURCES
     src/engine.c
     src/priority_queue.c
     src/planner_dijkstra.c
+    src/planner_astar.c
     src/string_dict.c
     src/common_keys.c
 )
@@ -83,27 +84,27 @@ if(CMAKE_PROJECT_NAME STREQUAL PROJECT_NAME)
     add_test(NAME cache_tests COMMAND test_cache)
 
     add_executable(test_engine tests/test_engine.c)
-    target_link_libraries(test_engine graphserver_core)
+    target_link_libraries(test_engine graphserver_core m)
     add_test(NAME engine_tests COMMAND test_engine)
 
     add_executable(test_priority_queue tests/test_priority_queue.c)
-    target_link_libraries(test_priority_queue graphserver_core)
+    target_link_libraries(test_priority_queue graphserver_core m)
     add_test(NAME priority_queue_tests COMMAND test_priority_queue)
 
     add_executable(test_planner tests/test_planner.c)
-    target_link_libraries(test_planner graphserver_core)
+    target_link_libraries(test_planner graphserver_core m)
     add_test(NAME planner_tests COMMAND test_planner)
 
     add_executable(test_dijkstra_internals tests/test_dijkstra_internals.c)
-    target_link_libraries(test_dijkstra_internals graphserver_core)
+    target_link_libraries(test_dijkstra_internals graphserver_core m)
     add_test(NAME dijkstra_internals_tests COMMAND test_dijkstra_internals)
 
     add_executable(test_precache tests/test_precache.c)
-    target_link_libraries(test_precache graphserver_core)
+    target_link_libraries(test_precache graphserver_core m)
     add_test(NAME precache_tests COMMAND test_precache)
 
     add_executable(test_bidirectional_engine tests/test_bidirectional_engine.c)
-    target_link_libraries(test_bidirectional_engine graphserver_core)
+    target_link_libraries(test_bidirectional_engine graphserver_core m)
     add_test(NAME bidirectional_engine_tests COMMAND test_bidirectional_engine)
 
     add_executable(test_integration tests/test_integration.c ${EXAMPLE_SOURCES})
@@ -117,6 +118,16 @@ if(CMAKE_PROJECT_NAME STREQUAL PROJECT_NAME)
     # Note: Performance tests are not run automatically with 'make test'
     # Run manually with: ./test_performance
 
+    add_executable(test_astar_performance tests/test_astar_performance.c ${EXAMPLE_SOURCES})
+    target_link_libraries(test_astar_performance graphserver_core m)
+    target_include_directories(test_astar_performance PRIVATE ../examples/include)
+    # Simple A* vs Dijkstra comparison test
+
+    add_executable(test_astar_direct tests/test_astar_direct.c ${EXAMPLE_SOURCES})
+    target_link_libraries(test_astar_direct graphserver_core m)
+    target_include_directories(test_astar_direct PRIVATE ../examples/include)
+    # Direct A* algorithm test
+
     # Custom target to run all tests
     add_custom_target(run_tests
         COMMAND ${CMAKE_CTEST_COMMAND} --verbose

core/include/gs_engine.h

@@ -327,6 +327,25 @@ GraphserverPath* gs_plan_simple(
     GraphserverPlanStats* out_stats
 );
 
+/**
+ * Find a path using the specified planner algorithm
+ * @param engine Engine instance
+ * @param start_vertex Starting vertex
+ * @param is_goal Goal predicate function
+ * @param goal_user_data User data for goal predicate
+ * @param planner_name Planner algorithm name ("dijkstra" or "astar")
+ * @param out_stats Optional statistics output (can be NULL)
+ * @return Single path, or NULL if no path found
+ */
+GraphserverPath* gs_plan_with_planner(
+    GraphserverEngine* engine,
+    const GraphserverVertex* start_vertex,
+    gs_goal_predicate_fn is_goal,
+    void* goal_user_data,
+    const char* planner_name,
+    GraphserverPlanStats* out_stats
+);
+
 /** @} */
 
 #ifdef __cplusplus

core/include/gs_planner_internal.h

@@ -280,6 +280,129 @@ GraphserverResult gs_plan_dijkstra(
     GraphserverPlanStats* out_stats
 );
 
+/**
+ * Heuristic function for A* search
+ * @param vertex Current vertex
+ * @param goal_data User data containing goal information
+ * @return Estimated cost to goal (must be admissible)
+ */
+typedef double (*gs_astar_heuristic_fn)(const GraphserverVertex* vertex, void* goal_data);
+
+/**
+ * A* search state
+ */
+typedef struct {
+    PriorityQueue* open_set;
+    HashMap* closed_set;
+    HashMap* node_map; // Maps vertex -> AStarNode*
+    GraphserverArena* arena;
+
+    // Search configuration
+    const GraphserverVertex* start_vertex;
+    gs_goal_predicate_fn is_goal;
+    void* goal_user_data;
+    gs_astar_heuristic_fn heuristic;
+    void* heuristic_data;
+    double timeout_seconds;
+
+    // Statistics
+    size_t vertices_expanded;
+    size_t edges_examined;
+    size_t nodes_generated;
+    double search_time_seconds;
+    bool timeout_reached;
+    bool goal_found;
+} AStarState;
+
+/**
+ * A* search node
+ */
+typedef struct AStarNode {
+    GraphserverVertex* vertex;
+    GraphserverVertex* parent;
+    double g_cost; // Actual cost from start
+    double f_cost; // g_cost + heuristic
+    GraphserverEdge* incoming_edge;
+    struct AStarNode* next; // For hash table chaining
+} AStarNode;
+
+/**
+ * Initialize A* search state
+ * @param state A* state structure
+ * @param start_vertex Starting vertex for search
+ * @param is_goal Goal predicate function
+ * @param goal_user_data User data for goal predicate
+ * @param heuristic Heuristic function for cost estimation
+ * @param heuristic_data User data for heuristic function
+ * @param arena Arena allocator for memory management
+ * @param timeout_seconds Maximum search time
+ * @return GS_SUCCESS on success, error code on failure
+ */
+GraphserverResult astar_init(
+    AStarState* state,
+    const GraphserverVertex* start_vertex,
+    gs_goal_predicate_fn is_goal,
+    void* goal_user_data,
+    gs_astar_heuristic_fn heuristic,
+    void* heuristic_data,
+    GraphserverArena* arena,
+    double timeout_seconds
+);
+
+/**
+ * Run A* search
+ * @param state Initialized A* state
+ * @param engine Engine instance for vertex expansion
+ * @param out_path Output path if goal found
+ * @return GS_SUCCESS if goal found, GS_ERROR_NO_PATH_FOUND if no path, error code on failure
+ */
+GraphserverResult astar_search(
+    AStarState* state,
+    GraphserverEngine* engine,
+    GraphserverPath** out_path
+);
+
+/**
+ * Clean up A* search state
+ * @param state A* state structure
+ */
+void astar_cleanup(AStarState* state);
+
+/**
+ * Run A* planning algorithm
+ * @param engine Engine instance for vertex expansion
+ * @param start_vertex Starting vertex for search
+ * @param is_goal Goal predicate function
+ * @param goal_user_data User data for goal predicate
+ * @param heuristic Heuristic function for cost estimation
+ * @param heuristic_data User data for heuristic function
+ * @param timeout_seconds Maximum search time (0 for no timeout)
+ * @param arena Arena allocator for memory management
+ * @param out_path Output path if goal found
+ * @param out_stats Optional statistics output
+ * @return GS_SUCCESS if goal found, error code otherwise
+ */
+GraphserverResult gs_plan_astar(
+    GraphserverEngine* engine,
+    const GraphserverVertex* start_vertex,
+    gs_goal_predicate_fn is_goal,
+    void* goal_user_data,
+    gs_astar_heuristic_fn heuristic,
+    void* heuristic_data,
+    double timeout_seconds,
+    GraphserverArena* arena,
+    GraphserverPath** out_path,
+    GraphserverPlanStats* out_stats
+);
+
+/**
+ * Geographic distance heuristic for routing with lat/lng coordinates
+ * @param vertex Current vertex (must have 'lat' and 'lng' keys)
+ * @param goal_data LocationGoal* with target coordinates
+ * @return Estimated travel time in minutes based on straight-line distance
+ */
+double geographic_distance_heuristic(const GraphserverVertex* vertex, void* goal_data);
+
 /** @} */
 
 #ifdef __cplusplus

core/src/engine.c

@@ -881,6 +881,74 @@ GraphserverPath* gs_plan_simple(
     }
 }
 
+GraphserverPath* gs_plan_with_planner(
+    GraphserverEngine* engine,
+    const GraphserverVertex* start_vertex,
+    gs_goal_predicate_fn is_goal,
+    void* goal_user_data,
+    const char* planner_name,
+    GraphserverPlanStats* out_stats) {
+
+    if (!engine || !start_vertex || !is_goal || !planner_name) return NULL;
+
+    // Create arena for planning operations
+    GraphserverArena* arena = gs_arena_create(engine->config.default_arena_size);
+    if (!arena) return NULL;
+
+    GraphserverPath* path = NULL;
+    GraphserverPlanStats stats = {0};
+    GraphserverResult result = GS_ERROR_INVALID_ARGUMENT;
+
+    if (strcmp(planner_name, "dijkstra") == 0) {
+        // Use Dijkstra planner
+        result = gs_plan_dijkstra(
+            engine,
+            start_vertex,
+            is_goal,
+            goal_user_data,
+            engine->config.default_timeout_seconds,
+            arena,
+            &path,
+            &stats
+        );
+    } else if (strcmp(planner_name, "astar") == 0) {
+        // Use A* planner with geographic heuristic
+        result = gs_plan_astar(
+            engine,
+            start_vertex,
+            is_goal,
+            goal_user_data,
+            geographic_distance_heuristic,
+            goal_user_data, // Use same goal data for heuristic
+            engine->config.default_timeout_seconds,
+            arena,
+            &path,
+            &stats
+        );
+    }
+
+    // Merge planner stats with existing engine stats
+    engine->last_plan_stats.vertices_expanded = stats.vertices_expanded;
+    engine->last_plan_stats.cache_hits += stats.cache_hits;
+    engine->last_plan_stats.cache_misses += stats.cache_misses;
+    engine->last_plan_stats.cache_puts += stats.cache_puts;
+    engine->last_plan_stats.providers_called += stats.providers_called;
+    engine->last_plan_stats.edges_generated += stats.edges_generated;
+
+    if (out_stats) {
+        *out_stats = stats;
+    }
+
+    gs_arena_destroy(arena);
+
+    if (result == GS_SUCCESS) {
+        return path;
+    } else {
+        if (path) gs_path_destroy(path);
+        return NULL;
+    }
+}
+
 // Utility functions
 const char* gs_get_error_message(GraphserverResult result) {
     switch (result) {