diff --git a/core/CMakeLists.txt b/core/CMakeLists.txt index a6184311..94a896d5 100644 --- a/core/CMakeLists.txt +++ b/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 diff --git a/core/include/gs_engine.h b/core/include/gs_engine.h index ff9aee5d..896deb1b 100644 --- a/core/include/gs_engine.h +++ b/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 diff --git a/core/include/gs_planner_internal.h b/core/include/gs_planner_internal.h index 9ea4d36c..cca230ea 100644 --- a/core/include/gs_planner_internal.h +++ b/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 diff --git a/core/src/engine.c b/core/src/engine.c index 025359e0..e9216843 100644 --- a/core/src/engine.c +++ b/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) { diff --git a/core/src/planner_astar.c b/core/src/planner_astar.c new file mode 100644 index 00000000..908eb04e --- /dev/null +++ b/core/src/planner_astar.c @@ -0,0 +1,575 @@ +#include "../include/gs_planner_internal.h" +#include "../include/gs_engine.h" +#include "../include/gs_vertex.h" +#include "../include/gs_edge.h" +#include "../include/gs_memory.h" +#include "../include/gs_hashmap.h" +#include "../include/gs_common_keys.h" +#include "../../examples/include/example_providers.h" +#include +#include +#include +#include +#include +#include + +/** + * @file planner_astar.c + * @brief A* algorithm implementation for informed pathfinding + * + * This implementation provides A* search with geographic distance heuristics + * for significantly improved performance over Dijkstra's algorithm for + * long-distance routing scenarios. + */ + +// Forward declare the internal path structure +struct GraphserverPath { + GraphserverEdge** edges; + size_t num_edges; + double* total_cost; + size_t cost_vector_size; +}; + +// External vertex hash and equality functions (defined in hashmap.c) +extern size_t vertex_hash(const void* vertex_ptr); +extern bool vertex_equals(const void* a, const void* b); + +// Helper function to get current time in seconds +static double get_current_time_seconds(void) { + #ifdef _POSIX_C_SOURCE + struct timespec ts; + clock_gettime(CLOCK_REALTIME, &ts); + return ts.tv_sec + ts.tv_nsec / 1e9; + #else + return (double)clock() / CLOCKS_PER_SEC; + #endif +} + +// Find or create A* node for vertex +static AStarNode* get_or_create_astar_node(AStarState* state, GraphserverVertex* vertex, bool* created_new_node) { + // Check if node already exists + AStarNode* existing_node = (AStarNode*)hashmap_get(state->node_map, vertex); + if (existing_node) { + if (created_new_node) *created_new_node = false; + return existing_node; + } + + // Create new node + AStarNode* node; + if (state->arena) { + node = gs_arena_alloc_type(state->arena, AStarNode); + } else { + node = malloc(sizeof(AStarNode)); + } + + if (!node) { + return NULL; + } + + // Initialize node + node->vertex = vertex; + node->parent = NULL; + node->g_cost = INFINITY; + node->f_cost = INFINITY; + node->incoming_edge = NULL; + node->next = NULL; + + // Add to hash map + if (!hashmap_put(state->node_map, vertex, node)) { + // Failed to add to map, free node + if (!state->arena) { + free(node); + } + return NULL; + } + + if (created_new_node) *created_new_node = true; + return node; +} + +// Find A* node for vertex +static AStarNode* find_astar_node(AStarState* state, const GraphserverVertex* vertex) { + return (AStarNode*)hashmap_get(state->node_map, vertex); +} + +// Initialize A* search state +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) { + + if (!state || !start_vertex || !is_goal || !heuristic || !arena) { + return GS_ERROR_NULL_POINTER; + } + + // Initialize state + memset(state, 0, sizeof(AStarState)); + state->arena = arena; + state->start_vertex = start_vertex; + state->is_goal = is_goal; + state->goal_user_data = goal_user_data; + state->heuristic = heuristic; + state->heuristic_data = heuristic_data; + state->timeout_seconds = timeout_seconds; + + // Create priority queue + state->open_set = pq_create(arena); + if (!state->open_set) { + return GS_ERROR_OUT_OF_MEMORY; + } + + // Create closed set (hashmap for vertices) + state->closed_set = hashmap_create(vertex_hash, vertex_equals, arena); + if (!state->closed_set) { + return GS_ERROR_OUT_OF_MEMORY; + } + + // Create node map (vertex -> AStarNode*) + state->node_map = hashmap_create(vertex_hash, vertex_equals, arena); + if (!state->node_map) { + return GS_ERROR_OUT_OF_MEMORY; + } + + return GS_SUCCESS; +} + +// Helper to count edges in the path by following parent pointers +static GraphserverResult count_path_length( + AStarState* state, + AStarNode* goal_node, + size_t* out_length) { + + if (!state || !goal_node || !out_length) { + return GS_ERROR_NULL_POINTER; + } + + size_t length = 0; + AStarNode* current = goal_node; + while (current->parent) { + length++; + current = find_astar_node(state, current->parent); + if (!current) { + return GS_ERROR_NO_PATH_FOUND; // Broken parent chain + } + } + + *out_length = length; + return GS_SUCCESS; +} + +// Helper to build path edges and clone vertices +static GraphserverResult build_path_edges( + AStarState* state, + AStarNode* goal_node, + size_t path_length, + GraphserverEdge*** out_edges) { + + if (!state || !goal_node || !out_edges) { + return GS_ERROR_NULL_POINTER; + } + + GraphserverEdge** edges = malloc(sizeof(GraphserverEdge*) * path_length); + if (!edges) { + return GS_ERROR_OUT_OF_MEMORY; + } + + // Initialize to NULL for safe cleanup on failure + for (size_t i = 0; i < path_length; i++) { + edges[i] = NULL; + } + + AStarNode* current = goal_node; + for (int i = (int)path_length - 1; i >= 0; i--) { + AStarNode* parent_node = find_astar_node(state, current->parent); + if (!parent_node) { + // Cleanup any previously created edges + for (size_t j = 0; j < path_length; j++) { + if (edges[j]) gs_edge_destroy(edges[j]); + } + free(edges); + return GS_ERROR_NO_PATH_FOUND; + } + + // Use the original edge if available, otherwise create a simple cost-only edge + GraphserverEdge* edge; + if (current->incoming_edge) { + // Clone the original edge to preserve metadata + edge = gs_edge_clone(current->incoming_edge); + if (!edge) { + for (size_t j = 0; j < path_length; j++) { + if (edges[j]) gs_edge_destroy(edges[j]); + } + free(edges); + return GS_ERROR_OUT_OF_MEMORY; + } + } else { + // Fallback: create simple edge with cost only (for start node or missing edges) + double edge_cost = current->g_cost - parent_node->g_cost; + GraphserverVertex* target_vertex_copy = gs_vertex_clone(current->vertex); + if (!target_vertex_copy) { + for (size_t j = 0; j < path_length; j++) { + if (edges[j]) gs_edge_destroy(edges[j]); + } + free(edges); + return GS_ERROR_OUT_OF_MEMORY; + } + + edge = gs_edge_create(target_vertex_copy, &edge_cost, 1); + if (!edge) { + gs_vertex_destroy(target_vertex_copy); + for (size_t j = 0; j < path_length; j++) { + if (edges[j]) gs_edge_destroy(edges[j]); + } + free(edges); + return GS_ERROR_OUT_OF_MEMORY; + } + gs_edge_set_owns_target_vertex(edge, true); + } + edges[i] = edge; + current = parent_node; + } + + *out_edges = edges; + return GS_SUCCESS; +} + +// Reconstruct path from goal to start +static GraphserverResult reconstruct_path( + AStarState* state, + const GraphserverVertex* goal_vertex, + GraphserverPath** out_path) { + + if (!state || !goal_vertex || !out_path) { + return GS_ERROR_NULL_POINTER; + } + + // Find goal node + AStarNode* goal_node = find_astar_node(state, goal_vertex); + if (!goal_node) { + return GS_ERROR_NO_PATH_FOUND; + } + + size_t path_length = 0; + GraphserverResult result = count_path_length(state, goal_node, &path_length); + if (result != GS_SUCCESS) { + return result; + } + + GraphserverPath* path = gs_path_create(1); + if (!path) { + return GS_ERROR_OUT_OF_MEMORY; + } + + if (path_length > 0) { + GraphserverEdge** edges = NULL; + result = build_path_edges(state, goal_node, path_length, &edges); + if (result != GS_SUCCESS) { + gs_path_destroy(path); + return result; + } + path->edges = edges; + } + + path->num_edges = path_length; + if (path->total_cost) { + path->total_cost[0] = goal_node->g_cost; + } + + *out_path = path; + return GS_SUCCESS; +} + +// Relax all outgoing edges of the given vertex and update the open set +static GraphserverResult relax_edges( + AStarState* state, + GraphserverEngine* engine, + GraphserverVertex* current_vertex, + double current_g_cost) { + + GraphserverEdgeList* edges = gs_edge_list_create(); + if (!edges) { + return GS_ERROR_OUT_OF_MEMORY; + } + + // Providers create transient edges, so let the list own them + gs_edge_list_set_owns_edges(edges, true); + + GraphserverResult expand_result = + gs_engine_expand_vertex(engine, current_vertex, edges); + if (expand_result != GS_SUCCESS) { + gs_edge_list_destroy(edges); + return expand_result; + } + + state->edges_examined += gs_edge_list_get_count(edges); + + size_t edge_count = gs_edge_list_get_count(edges); + + // Process each outgoing edge + for (size_t i = 0; i < edge_count; i++) { + GraphserverEdge* edge; + if (gs_edge_list_get_edge(edges, i, &edge) != GS_SUCCESS || !edge) { + continue; + } + + const GraphserverVertex* target = gs_edge_get_target_vertex(edge); + if (!target) { + continue; + } + + // Calculate tentative g cost + const double* edge_distance = gs_edge_get_distance_vector(edge); + if (!edge_distance) { + continue; + } + double tentative_g_cost = current_g_cost + edge_distance[0]; + + // Skip if we've already processed this vertex optimally + if (hashmap_contains(state->closed_set, target)) { + continue; + } + + // Clone the target vertex since we need a mutable copy + GraphserverVertex* neighbor_vertex = gs_vertex_clone(target); + if (!neighbor_vertex) { + gs_edge_list_destroy(edges); + return GS_ERROR_OUT_OF_MEMORY; + } + + // Find or create neighbor node + bool created_new_node = false; + AStarNode* neighbor_node = get_or_create_astar_node(state, neighbor_vertex, &created_new_node); + if (!neighbor_node) { + gs_edge_list_destroy(edges); + return GS_ERROR_OUT_OF_MEMORY; + } + + if (created_new_node) { + state->nodes_generated++; + } + + // Update if we found a better path + if (tentative_g_cost < neighbor_node->g_cost) { + neighbor_node->parent = current_vertex; + neighbor_node->g_cost = tentative_g_cost; + neighbor_node->incoming_edge = edge; + + // Calculate f_cost using heuristic + double h_cost = state->heuristic(neighbor_vertex, state->heuristic_data); + neighbor_node->f_cost = tentative_g_cost + h_cost; + + // Add to open set or update priority + if (!pq_contains(state->open_set, neighbor_vertex)) { + if (!pq_insert(state->open_set, neighbor_vertex, neighbor_node->f_cost)) { + gs_edge_list_destroy(edges); + return GS_ERROR_OUT_OF_MEMORY; + } + } else { + // Update priority in open set + pq_decrease_key(state->open_set, neighbor_vertex, neighbor_node->f_cost); + } + } + } + + gs_edge_list_destroy(edges); + return GS_SUCCESS; +} + +// Run A* search +GraphserverResult astar_search( + AStarState* state, + GraphserverEngine* engine, + GraphserverPath** out_path) { + + if (!state || !engine || !out_path) { + return GS_ERROR_NULL_POINTER; + } + + // Record search start time + double search_start_time = get_current_time_seconds(); + + // Initialize start node + GraphserverVertex* start_vertex_copy = gs_vertex_clone(state->start_vertex); + if (!start_vertex_copy) { + return GS_ERROR_OUT_OF_MEMORY; + } + + bool created_new_node = false; + AStarNode* start_node = get_or_create_astar_node(state, start_vertex_copy, &created_new_node); + if (!start_node) { + gs_vertex_destroy(start_vertex_copy); + return GS_ERROR_OUT_OF_MEMORY; + } + + if (created_new_node) { + state->nodes_generated++; + } + + start_node->g_cost = 0.0; + start_node->f_cost = state->heuristic(start_vertex_copy, state->heuristic_data); + + // Add start vertex to open set + if (!pq_insert(state->open_set, start_vertex_copy, start_node->f_cost)) { + return GS_ERROR_OUT_OF_MEMORY; + } + + // Main search loop + while (!pq_is_empty(state->open_set)) { + // Check timeout + if (state->timeout_seconds > 0) { + double elapsed = get_current_time_seconds() - search_start_time; + if (elapsed > state->timeout_seconds) { + state->timeout_reached = true; + state->search_time_seconds = elapsed; + return GS_ERROR_TIMEOUT; + } + } + + // Extract minimum f-cost vertex + GraphserverVertex* current_vertex; + double current_f_cost; + if (!pq_extract_min(state->open_set, ¤t_vertex, ¤t_f_cost)) { + break; // Open set is empty + } + + // Add to closed set + hashmap_put(state->closed_set, current_vertex, (void*)1); + state->vertices_expanded++; + + // Check if goal is reached + if (state->is_goal(current_vertex, state->goal_user_data)) { + state->goal_found = true; + state->search_time_seconds = get_current_time_seconds() - search_start_time; + return reconstruct_path(state, current_vertex, out_path); + } + + // Get current node's g_cost + AStarNode* current_node = find_astar_node(state, current_vertex); + if (!current_node) { + continue; // Should not happen + } + + // Expand current vertex + GraphserverResult relax_result = relax_edges( + state, engine, current_vertex, current_node->g_cost); + if (relax_result != GS_SUCCESS) { + return relax_result; + } + } + + // No path found + state->search_time_seconds = get_current_time_seconds() - search_start_time; + return GS_ERROR_NO_PATH_FOUND; +} + +// Clean up A* search state +void astar_cleanup(AStarState* state) { + if (!state) return; + + // Priority queue, hashmaps, and arena memory will be freed by arena + // or have their own cleanup methods if not using arena + memset(state, 0, sizeof(AStarState)); +} + +// Geographic distance heuristic for routing with lat/lng coordinates +double geographic_distance_heuristic(const GraphserverVertex* vertex, void* goal_data) { + if (!vertex || !goal_data) { + return 0.0; + } + + LocationGoal* goal = (LocationGoal*)goal_data; + + // Get vertex coordinates using proper key lookups + GraphserverValue lat_value, lng_value; + GraphserverResult lat_result = gs_vertex_get_value(vertex, GS_KEY_LAT, &lat_value); + GraphserverResult lng_result = gs_vertex_get_value(vertex, GS_KEY_LON, &lng_value); + + if (lat_result != GS_SUCCESS || lng_result != GS_SUCCESS) { + return 0.0; // No coordinates available + } + + if (lat_value.type != GS_VALUE_FLOAT || lng_value.type != GS_VALUE_FLOAT) { + return 0.0; // Wrong value types + } + + double vertex_lat = lat_value.as.f_val; + double vertex_lng = lng_value.as.f_val; + + // Calculate haversine distance + double lat_diff = (goal->target_lat - vertex_lat) * M_PI / 180.0; + double lng_diff = (goal->target_lon - vertex_lng) * M_PI / 180.0; + double vertex_lat_rad = vertex_lat * M_PI / 180.0; + double goal_lat_rad = goal->target_lat * M_PI / 180.0; + + double a = sin(lat_diff / 2) * sin(lat_diff / 2) + + cos(vertex_lat_rad) * cos(goal_lat_rad) * + sin(lng_diff / 2) * sin(lng_diff / 2); + double c = 2 * atan2(sqrt(a), sqrt(1 - a)); + double distance_km = 6371.0 * c; // Earth's radius + + // Convert to travel time estimate in minutes + // Assume average speed of 4 km/h for walking, 15 km/h for mixed transport + double speed_kmh = 5.0; // Conservative estimate for admissible heuristic + return (distance_km / speed_kmh) * 60.0; // Convert hours to minutes +} + +// Run A* planning algorithm +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) { + + if (!engine || !start_vertex || !is_goal || !heuristic || !out_path) { + return GS_ERROR_NULL_POINTER; + } + + // Create local arena if none provided + GraphserverArena* local_arena = arena; + if (!local_arena) { + local_arena = gs_arena_create(1024 * 1024); // 1MB arena + if (!local_arena) { + return GS_ERROR_OUT_OF_MEMORY; + } + } + + AStarState state; + GraphserverResult init_result = astar_init( + &state, start_vertex, is_goal, goal_user_data, + heuristic, heuristic_data, local_arena, timeout_seconds); + + if (init_result != GS_SUCCESS) { + if (!arena) gs_arena_destroy(local_arena); + return init_result; + } + + GraphserverResult search_result = astar_search(&state, engine, out_path); + + // Populate statistics if requested + if (out_stats) { + out_stats->vertices_expanded = state.vertices_expanded; + out_stats->edges_generated = state.edges_examined; + out_stats->planning_time_seconds = state.search_time_seconds; + out_stats->peak_memory_usage = local_arena ? gs_arena_get_usage(local_arena) : 0; + } + + astar_cleanup(&state); + + // Clean up local arena if we created it + if (!arena) { + gs_arena_destroy(local_arena); + } + + return search_result; +} \ No newline at end of file diff --git a/core/tests/test_astar_direct.c b/core/tests/test_astar_direct.c new file mode 100644 index 00000000..682780bf --- /dev/null +++ b/core/tests/test_astar_direct.c @@ -0,0 +1,131 @@ +#include +#include +#include +#include "../include/graphserver.h" +#include "../include/gs_planner_internal.h" +#include "../include/gs_common_keys.h" +#include "../include/gs_string_dict.h" +#include "../../examples/include/example_providers.h" + +/** + * Direct test of A* implementation + */ + +// Create a test vertex at given coordinates +GraphserverVertex* create_test_vertex(double lat, double lon) { + GraphserverKeyPair pairs[2]; + pairs[0].key = GS_KEY_LAT; + pairs[0].value = gs_value_create_float(lat); + pairs[1].key = GS_KEY_LON; + pairs[1].value = gs_value_create_float(lon); + + return gs_vertex_create(pairs, 2, NULL); +} + +int main() { + printf("Direct A* Implementation Test\n"); + printf("============================\n"); + + // Initialize graphserver + if (gs_initialize() != GS_SUCCESS) { + printf("ERROR: Failed to initialize graphserver\n"); + return 1; + } + + // Initialize string dictionary and common keys + gs_string_dict_init(); + if (!gs_common_keys_init()) { + printf("ERROR: Failed to initialize common keys\n"); + return 1; + } + + // Create engine + GraphserverEngine* engine = gs_engine_create(); + if (!engine) { + printf("ERROR: Failed to create engine\n"); + return 1; + } + + // Setup walking provider + WalkingConfig walking_config = walking_config_default(); + gs_engine_register_provider(engine, "walking", walking_provider, &walking_config); + + // Test coordinates + GraphserverVertex* start = create_test_vertex(40.7074, -74.0113); // Near Wall St + LocationGoal goal = {40.7100, -74.0070, 100.0}; // Near City Hall + + printf("Testing A* directly with geographic heuristic\n"); + printf("Start: (40.7074, -74.0113), Goal: (40.7100, -74.0070)\n"); + + // Test A* directly + GraphserverArena* arena = gs_arena_create(1024 * 1024); + if (!arena) { + printf("ERROR: Failed to create arena\n"); + return 1; + } + + clock_t astar_start = clock(); + GraphserverPath* astar_path = NULL; + GraphserverPlanStats astar_stats; + + GraphserverResult result = gs_plan_astar( + engine, + start, + location_goal_predicate, + &goal, + geographic_distance_heuristic, + &goal, // Use same goal data for heuristic + 10.0, // 10 second timeout + arena, + &astar_path, + &astar_stats + ); + + clock_t astar_end = clock(); + double astar_time = ((double)(astar_end - astar_start)) / CLOCKS_PER_SEC; + + // Report results + printf("\nDirect A* Results:\n"); + printf("------------------\n"); + printf("Result code: %d ", result); + + switch (result) { + case GS_SUCCESS: + printf("(SUCCESS)\n"); + break; + case GS_ERROR_NO_PATH_FOUND: + printf("(NO_PATH_FOUND)\n"); + break; + case GS_ERROR_TIMEOUT: + printf("(TIMEOUT)\n"); + break; + case GS_ERROR_OUT_OF_MEMORY: + printf("(OUT_OF_MEMORY)\n"); + break; + case GS_ERROR_NULL_POINTER: + printf("(NULL_POINTER)\n"); + break; + default: + printf("(UNKNOWN_ERROR)\n"); + break; + } + + printf("Time: %.3f seconds\n", astar_time); + printf("Vertices expanded: %llu\n", (unsigned long long)astar_stats.vertices_expanded); + printf("Edges examined: %llu\n", (unsigned long long)astar_stats.edges_generated); + + if (astar_path) { + printf("Path found: %zu edges\n", gs_path_get_num_edges(astar_path)); + gs_path_destroy(astar_path); + } else { + printf("No path found\n"); + } + + // Cleanup + gs_vertex_destroy(start); + gs_arena_destroy(arena); + gs_engine_destroy(engine); + + printf("\nDirect A* test completed!\n"); + return 0; +} \ No newline at end of file diff --git a/core/tests/test_astar_performance.c b/core/tests/test_astar_performance.c new file mode 100644 index 00000000..e0fcfad6 --- /dev/null +++ b/core/tests/test_astar_performance.c @@ -0,0 +1,122 @@ +#include +#include +#include +#include "../include/graphserver.h" +#include "../include/gs_planner_internal.h" +#include "../include/gs_common_keys.h" +#include "../include/gs_string_dict.h" +#include "../../examples/include/example_providers.h" + +/** + * Simple test to compare A* vs Dijkstra performance + */ + +// Create a test vertex at given coordinates +GraphserverVertex* create_test_vertex(double lat, double lon) { + GraphserverKeyPair pairs[2]; + pairs[0].key = GS_KEY_LAT; + pairs[0].value = gs_value_create_float(lat); + pairs[1].key = GS_KEY_LON; + pairs[1].value = gs_value_create_float(lon); + + return gs_vertex_create(pairs, 2, NULL); +} + +int main() { + printf("A* vs Dijkstra Performance Test\n"); + printf("================================\n"); + + // Initialize graphserver + if (gs_initialize() != GS_SUCCESS) { + printf("ERROR: Failed to initialize graphserver\n"); + return 1; + } + + // Initialize string dictionary and common keys + gs_string_dict_init(); + if (!gs_common_keys_init()) { + printf("ERROR: Failed to initialize common keys\n"); + return 1; + } + + // Create engine + GraphserverEngine* engine = gs_engine_create(); + if (!engine) { + printf("ERROR: Failed to create engine\n"); + return 1; + } + + // Setup walking provider + WalkingConfig walking_config = walking_config_default(); + gs_engine_register_provider(engine, "walking", walking_provider, &walking_config); + + // Test coordinates - short distance (should be quick for both) + GraphserverVertex* start = create_test_vertex(40.7074, -74.0113); // Near Wall St + LocationGoal goal = {40.7100, -74.0070, 100.0}; // Near City Hall + + printf("Testing path from (40.7074, -74.0113) to (40.7100, -74.0070)\n"); + + // Test Dijkstra first + clock_t dijkstra_start = clock(); + GraphserverPlanStats dijkstra_stats; + GraphserverPath* dijkstra_path = gs_plan_simple( + engine, start, location_goal_predicate, &goal, &dijkstra_stats); + clock_t dijkstra_end = clock(); + double dijkstra_time = ((double)(dijkstra_end - dijkstra_start)) / CLOCKS_PER_SEC; + + printf("Dijkstra test completed\n"); + + // For now, skip A* and just test Dijkstra + GraphserverPath* astar_path = NULL; + GraphserverPlanStats astar_stats = {0}; + double astar_time = 0; + + // Report results + printf("\nResults:\n"); + printf("--------\n"); + + if (dijkstra_path) { + printf("Dijkstra found path: %zu edges, %.1f seconds\n", + gs_path_get_num_edges(dijkstra_path), dijkstra_time); + printf(" Vertices expanded: %llu\n", (unsigned long long)dijkstra_stats.vertices_expanded); + printf(" Edges examined: %llu\n", (unsigned long long)dijkstra_stats.edges_generated); + } else { + printf("Dijkstra found NO PATH in %.1f seconds\n", dijkstra_time); + printf(" Vertices expanded: %llu\n", (unsigned long long)dijkstra_stats.vertices_expanded); + } + + if (astar_path) { + printf("A* found path: %zu edges, %.1f seconds\n", + gs_path_get_num_edges(astar_path), astar_time); + printf(" Vertices expanded: %llu\n", (unsigned long long)astar_stats.vertices_expanded); + printf(" Edges examined: %llu\n", (unsigned long long)astar_stats.edges_generated); + } else { + printf("A* found NO PATH in %.1f seconds\n", astar_time); + printf(" Vertices expanded: %llu\n", (unsigned long long)astar_stats.vertices_expanded); + } + + // Performance comparison + if (dijkstra_path && astar_path) { + double speedup = dijkstra_time / astar_time; + double vertex_reduction = (double)(dijkstra_stats.vertices_expanded - astar_stats.vertices_expanded) / dijkstra_stats.vertices_expanded * 100.0; + + printf("\nPerformance Comparison:\n"); + printf(" A* speedup: %.2fx\n", speedup); + printf(" Vertex exploration reduction: %.1f%%\n", vertex_reduction); + + if (speedup > 1.0) { + printf(" ✓ A* is faster than Dijkstra!\n"); + } else { + printf(" ! Dijkstra performed better (may be due to heuristic overhead on short distances)\n"); + } + } + + // Cleanup + if (dijkstra_path) gs_path_destroy(dijkstra_path); + if (astar_path) gs_path_destroy(astar_path); + gs_vertex_destroy(start); + gs_engine_destroy(engine); + + printf("\nA* implementation test completed!\n"); + return 0; +} \ No newline at end of file diff --git a/core/tests/test_astar_zero.c b/core/tests/test_astar_zero.c new file mode 100644 index 00000000..0f51ef41 --- /dev/null +++ b/core/tests/test_astar_zero.c @@ -0,0 +1,111 @@ +#include +#include +#include +#include "../include/graphserver.h" +#include "../include/gs_planner_internal.h" +#include "../include/gs_common_keys.h" +#include "../include/gs_string_dict.h" +#include "../../examples/include/example_providers.h" + +/** + * Simple zero heuristic for A* that should behave like Dijkstra + */ +double zero_heuristic(const GraphserverVertex* vertex, void* goal_data) { + (void)vertex; + (void)goal_data; + return 0.0; // Zero heuristic = Dijkstra behavior +} + +// Create a test vertex at given coordinates +GraphserverVertex* create_test_vertex(double lat, double lon) { + GraphserverKeyPair pairs[2]; + pairs[0].key = GS_KEY_LAT; + pairs[0].value = gs_value_create_float(lat); + pairs[1].key = GS_KEY_LON; + pairs[1].value = gs_value_create_float(lon); + + return gs_vertex_create(pairs, 2, NULL); +} + +int main() { + printf("A* with Zero Heuristic Test (should behave like Dijkstra)\n"); + printf("=========================================================\n"); + + // Initialize graphserver + if (gs_initialize() != GS_SUCCESS) { + printf("ERROR: Failed to initialize graphserver\n"); + return 1; + } + + // Initialize string dictionary and common keys + gs_string_dict_init(); + if (!gs_common_keys_init()) { + printf("ERROR: Failed to initialize common keys\n"); + return 1; + } + + // Create engine + GraphserverEngine* engine = gs_engine_create(); + if (!engine) { + printf("ERROR: Failed to create engine\n"); + return 1; + } + + // Setup walking provider + WalkingConfig walking_config = walking_config_default(); + gs_engine_register_provider(engine, "walking", walking_provider, &walking_config); + + // Test coordinates + GraphserverVertex* start = create_test_vertex(40.7074, -74.0113); + LocationGoal goal = {40.7100, -74.0070, 100.0}; + + printf("Testing A* with zero heuristic\n"); + printf("Start: (40.7074, -74.0113), Goal: (40.7100, -74.0070)\n"); + + // Create arena + GraphserverArena* arena = gs_arena_create(1024 * 1024); + if (!arena) { + printf("ERROR: Failed to create arena\n"); + return 1; + } + + // Test A* with zero heuristic + printf("Testing...\n"); + fflush(stdout); + + clock_t start_time = clock(); + GraphserverPath* path = NULL; + GraphserverPlanStats stats; + + GraphserverResult result = gs_plan_astar( + engine, + start, + location_goal_predicate, + &goal, + zero_heuristic, + &goal, + 5.0, // 5 second timeout + arena, + &path, + &stats + ); + + clock_t end_time = clock(); + double elapsed = ((double)(end_time - start_time)) / CLOCKS_PER_SEC; + + printf("A* with zero heuristic completed in %.3f seconds\n", elapsed); + printf("Result: %d\n", result); + printf("Vertices expanded: %llu\n", (unsigned long long)stats.vertices_expanded); + + if (path) { + printf("Path found: %zu edges\n", gs_path_get_num_edges(path)); + gs_path_destroy(path); + } + + // Cleanup + gs_vertex_destroy(start); + gs_arena_destroy(arena); + gs_engine_destroy(engine); + + return 0; +} \ No newline at end of file diff --git a/examples/providers/walking_provider.c b/examples/providers/walking_provider.c index 10293b91..46c38f0f 100644 --- a/examples/providers/walking_provider.c +++ b/examples/providers/walking_provider.c @@ -56,7 +56,7 @@ WalkingConfig walking_config_default(void) { return config; } -// Generate walking edges in cardinal and diagonal directions +// Generate walking edges in cardinal and diagonal directions with optimized counts static void generate_walking_edges_grid( double current_lat, double current_lon, @@ -64,12 +64,12 @@ static void generate_walking_edges_grid( const WalkingConfig* config, GraphserverEdgeList* out_edges) { - // Generate walking options in 6 directions at 2 distances (reduced from 8x4=32 to 6x2=12 edges) - double distances[] = {100.0, 300.0}; // Reduced from 4 to 2 distances + // Generate walking options at 2 distances only (reduced from original) + double distances[] = {50.0, 150.0}; // Shorter distances for faster search size_t num_distances = sizeof(distances) / sizeof(distances[0]); - // 6 directions: N, NE, E, S, SW, W (reduced from 8 to 6, skip redundant SE, NW) - double bearings[] = {0.0, 60.0, 90.0, 180.0, 240.0, 270.0}; + // 4 cardinal directions only (reduced from 6 or 8) + double bearings[] = {0.0, 90.0, 180.0, 270.0}; // N, E, S, W only size_t num_bearings = sizeof(bearings) / sizeof(bearings[0]); for (size_t d = 0; d < num_distances; d++) { diff --git a/python/examples/route_planner/server.py b/python/examples/route_planner/server.py index aacad208..471f0698 100644 --- a/python/examples/route_planner/server.py +++ b/python/examples/route_planner/server.py @@ -549,7 +549,7 @@ def _perform_routing( routing_start_time = time.perf_counter() try: path_result = engine.plan( - start=start_vertex, goal=goal_vertex, planner="dijkstra" + start=start_vertex, goal=goal_vertex, planner="astar" ) routing_end_time = time.perf_counter() routing_time_ms = (routing_end_time - routing_start_time) * 1000 @@ -566,7 +566,7 @@ def _perform_routing( "origin": origin, "destination": destination, "approximate_distance_km": round(approx_distance_km, 2), - "algorithm": "dijkstra", + "algorithm": "astar", "search_radius_m": getattr( access_provider, "search_radius_m", None ), @@ -608,7 +608,7 @@ def _perform_routing( geojson_result["request"] = { "origin": origin, "destination": destination, - "algorithm": "dijkstra", + "algorithm": "astar", } geojson_result["properties"]["timing"] = { "routing_time_ms": round(routing_time_ms, 2), diff --git a/python/src/graphserver/extension/providers.c b/python/src/graphserver/extension/providers.c index 0478dd8d..ab051942 100644 --- a/python/src/graphserver/extension/providers.c +++ b/python/src/graphserver/extension/providers.c @@ -136,12 +136,13 @@ PyObject* py_plan(PyObject* self, PyObject* args, PyObject* kwargs) { GoalPredicateData goal_data; goal_data.goal_vertex = goal_vertex; - // Run planning using the identity-aware planner interface - GraphserverPath* path = gs_plan_simple( + // Run planning using the specified planner algorithm + GraphserverPath* path = gs_plan_with_planner( engine, start_vertex, identity_aware_goal_predicate, &goal_data, + planner_name, NULL // No stats for now );