diff --git a/src/dijkstragraph.c b/src/dijkstragraph.c index db7258b..07025e9 100644 --- a/src/dijkstragraph.c +++ b/src/dijkstragraph.c @@ -157,13 +157,18 @@ struct dijkstra_path *dijkstra_path_dup_shallow(struct dijkstra_path *path) return dup; } +float dijkstra_position_get_distance(struct position *position_a, + struct position *position_b) { + return sqrt(pow(position_a->x - position_b->x, 2) + + pow(position_a->y - position_b->y, 2)); +} + dijkstra_cost dijkstra_get_weight_from_distance(struct dijkstra_node *node_a, struct dijkstra_node *node_b) { float dist; - dist = sqrt(pow(node_a->position.x - node_b->position.x, 2) + - pow(node_a->position.y - node_b->position.y, 2)); + dist = dijkstra_position_get_distance(&node_a->position, &node_b->position); if (dist < DIJKSTRA_COST_MIN) dist = DIJKSTRA_COST_MIN; @@ -408,3 +413,23 @@ inline struct dijkstra_node *dijkstra_node_get_connection(struct dijkstra_node * } return (path->destination == node ? path->source : path->destination); } + +struct dijkstra_node *dijkstra_node_find_closest_node(struct dijkstra_solver *solver, + struct position *position) { + GList *l = NULL; + struct dijkstra_node *node = NULL; + struct dijkstra_node *closest_node = NULL; + float closest_distance; + float distance; + + for (l = solver->nodes; l != NULL; l = l->next) { + node = (struct dijkstra_node*) l->data; + distance = dijkstra_position_get_distance(position, &node->position); + if (!closest_node || distance < closest_distance) { + closest_node = node; + closest_distance = distance; + } + } + + return closest_node; +} diff --git a/src/dijkstragraph.h b/src/dijkstragraph.h index 58e1048..b9e4a60 100644 --- a/src/dijkstragraph.h +++ b/src/dijkstragraph.h @@ -49,6 +49,8 @@ void dijkstra_solver_free(struct dijkstra_solver *solver); /* Library functions */ struct dijkstra_node *dijkstra_node_get_connection(struct dijkstra_node *node, struct dijkstra_path *path); +struct dijkstra_node *dijkstra_node_find_closest_node(struct dijkstra_solver *solver, + struct position *position); /* For debugging only */ struct dijkstra_node *dijkstra_node_new_to_list(GList **list, diff --git a/src/dijkstrasearch.c b/src/dijkstrasearch.c index 66de48d..d06d5ca 100644 --- a/src/dijkstrasearch.c +++ b/src/dijkstrasearch.c @@ -203,8 +203,27 @@ GList *dijkstra_search_get_route(struct dijkstra_search *search, return l; } -void dijkstra_search_route_to_geojson(struct dijkstra_search *search, - GList *route) +int printb(char **buf, int *offset, const char *__restrict __format, ...) +{ + va_list vargs; + int len; + + va_start(vargs, __format); + len = vsnprintf(NULL, 0, __format, vargs); + va_end(vargs); + *buf = realloc(*buf, *offset + len + 1); + va_start(vargs, __format); + len = vsprintf(*buf + *offset, __format, vargs); + va_end(vargs); + + *offset += len; + return len; +} + +// TODO: Rewrite to use glib-json... On the other hand, this is probably faster. +int dijkstra_search_route_to_geojson(struct dijkstra_search *search, + GList *route, + char **buf) { GList *l = NULL; struct dijkstra_node *node = NULL; @@ -213,8 +232,11 @@ void dijkstra_search_route_to_geojson(struct dijkstra_search *search, float last_heading = 0; char *relative_direction_str = NULL; char *format_str = NULL; + int offset = 0; - printf("["); + *buf = NULL; + + printb(buf, &offset, "["); for (l = route; l != NULL; l = l->next) { node = (struct dijkstra_node*) l->data; @@ -236,31 +258,33 @@ void dijkstra_search_route_to_geojson(struct dijkstra_search *search, format_str = "Go %s onto %s."; if (node_last != search->start) - printf(","); - printf("\n"); - printf("{\"type\": \"Feature\",\n" + printb(buf, &offset, ","); + printb(buf, &offset, "\n"); + printb(buf, &offset, "{\"type\": \"Feature\",\n" " \"geometry\": {\n" " \"type\": \"LineString\", \"coordinates\": [[%f,%f],[%f,%f]]\n" " },\n", node_last->position.x, node_last->position.y, node->position.x, node->position.y); - printf(" \"properties\": {\n" + printb(buf, &offset, " \"properties\": {\n" " \"heading\": \"%f\",\n" " \"through\": \"%s\",\n", heading, search->states[node->uid]->cheapest_path->name); - printf(" \"description\": \""); - printf(format_str, relative_direction_str, search->states[node->uid]->cheapest_path->name); - printf("\"\n"); + printb(buf, &offset, " \"description\": \""); + printb(buf, &offset, format_str, relative_direction_str, search->states[node->uid]->cheapest_path->name); + printb(buf, &offset, "\"\n"); - printf(" }\n"); - printf("}"); + printb(buf, &offset, " }\n"); + printb(buf, &offset, "}"); last_heading = heading; } node_last = node; } - printf("]\n"); + printb(buf, &offset, "\n]\n"); + + return offset; } void dijkstra_print_path(struct dijkstra_search *search, diff --git a/src/dijkstrasearch.h b/src/dijkstrasearch.h index 218672e..62b9f27 100644 --- a/src/dijkstrasearch.h +++ b/src/dijkstrasearch.h @@ -32,6 +32,7 @@ GList *dijkstra_search_get_route(struct dijkstra_search *search, void dijkstra_print_path(struct dijkstra_search *search, struct dijkstra_node *destination); void dijkstra_print_nodes(struct dijkstra_search *search); -void dijkstra_search_route_to_geojson(struct dijkstra_search *search, - GList *route); +int dijkstra_search_route_to_geojson(struct dijkstra_search *search, + GList *route, + char **buf); #endif // DIJKSTRASEARCH_H diff --git a/src/dijkstraserv.c b/src/dijkstraserv.c new file mode 100644 index 0000000..4255be1 --- /dev/null +++ b/src/dijkstraserv.c @@ -0,0 +1,172 @@ +#include "dijkstraserv.h" + +static int dijkstra_serv_perform_search(struct dijkstra_serv *serv, + struct position *pos_start, + struct position *pos_destination, + char **result) { + struct dijkstra_node *start = NULL; + struct dijkstra_node *destination = NULL; + + struct dijkstra_search *search; + int iterations; + GList *route; + + report(LL_INFO, "Requested route from %f,%f to %f,%f.", + pos_start->x, pos_start->y, + pos_destination->x, pos_destination->y); + + /* Find closest node */ // TODO: This should be closest point on path, but for now this'll do. + start = dijkstra_node_find_closest_node(serv->solver, pos_start); + if (!start) + return 0; + destination = dijkstra_node_find_closest_node(serv->solver, pos_destination); + if (!destination) + return 0; + + report(LL_DEBUG, "Searching route from %f,%f to %f,%f.", + start->position.x, start->position.y, + destination->position.x, destination->position.y); + + search = dijkstra_search_new(serv->solver); + if (!search) { + report(LL_CRITICAL, "Error allocating dijkstra_search"); + return 0; + } + dijkstra_search_set_start(search, start); + if ((iterations = dijkstra_search_find_path(search, destination)) > 0) { + report(LL_INFO, " Found path from %d to %d after %d iterations.", start->uid, destination->uid, iterations); + route = dijkstra_search_get_route(search, destination); + return dijkstra_search_route_to_geojson(search, route, result); + } else { + report(LL_INFO, " Couldn't find path."); + } + + return 0; +} + +static void dijkstra_serv_soup_callback (SoupServer *server, + SoupMessage *msg, + const char *path, + GHashTable *query, + SoupClientContext *client, + gpointer user_data) +{ + struct dijkstra_serv *serv = (struct dijkstra_serv *) user_data; + GError *err = NULL; + GMatchInfo *matchInfo; + GRegex *regex; + char *body; + int body_len; + int i = 0; + struct position pos_start, pos_destination; + + report(LL_INFO, "New request: %s", path); + if (msg->method != SOUP_METHOD_GET) { + soup_message_set_status (msg, SOUP_STATUS_NOT_IMPLEMENTED); + return; + } + + if (strlen(path) > 20) { + soup_message_set_status (msg, SOUP_STATUS_BAD_REQUEST); + return; + } + + regex = g_regex_new("([\\-0-9]+)", 0, 0, &err); + g_regex_match (regex, path, 0, &matchInfo); + + while (g_match_info_matches(matchInfo)) { + float result_num; + gchar *result; + result = g_match_info_fetch(matchInfo, 0); + if (!result) + break; + + result_num = atof(result); + switch (i) { + case 0: + pos_start.x = result_num; + break; + case 1: + pos_start.y = result_num; + break; + case 2: + pos_destination.x = result_num; + break; + case 3: + pos_destination.y = result_num; + break; + } + + g_match_info_next(matchInfo, &err); + g_free(result); + + i++; + if (i >= 4) + break; + } + + g_regex_unref(regex); + + if (i == 4) { + body_len = dijkstra_serv_perform_search(serv, &pos_start, &pos_destination, &body); + if (!body_len) { + body = strdup("Not found."); + body_len = 9; + soup_message_set_status (msg, SOUP_STATUS_NOT_FOUND); + } else { + soup_message_set_status (msg, SOUP_STATUS_OK); + } + + + soup_message_set_response (msg, "application/json", SOUP_MEMORY_COPY, + body, body_len); + free(body); + } else { + report(LL_WARNING, " Invalid request."); + soup_message_set_status (msg, SOUP_STATUS_MALFORMED); + soup_message_set_response (msg, "text/plain", SOUP_MEMORY_COPY, + "Invalid request", 15); + + } +} + +struct dijkstra_serv *dijkstra_serv_new( + struct dijkstra_solver *solver, + int port) +{ + struct dijkstra_serv *serv; + + serv = (struct dijkstra_serv*) malloc(sizeof(*serv)); + if (serv) { + serv->solver = solver; + serv->port = port; + serv->soupserver = soup_server_new(NULL, NULL); + if (!serv->soupserver) { + free(serv); + return NULL; + } + soup_server_add_handler (serv->soupserver, + "/", dijkstra_serv_soup_callback, + serv, NULL); // TODO: The last arg here should probably not be NULL... + } + + return serv; +} + +int dijkstra_serv_run(struct dijkstra_serv *serv) +{ + GError *err = NULL; + + if (!soup_server_listen_all(serv->soupserver, serv->port, 0, &err)) { + report(LL_CRITICAL, "Couldn't start server: %s", err->message); + g_error_free(err); + return -1; + } + + report(LL_INFO, "Listening on port %d.", serv->port); + + GMainLoop *loop; + loop = g_main_loop_new(NULL, TRUE); + g_main_loop_run(loop); // TODO: Add way to gracefully exit loop + return 0; +} diff --git a/src/dijkstraserv.h b/src/dijkstraserv.h new file mode 100644 index 0000000..05f8d21 --- /dev/null +++ b/src/dijkstraserv.h @@ -0,0 +1,22 @@ +#ifndef DIJKSTRASERV_H +#define DIJKSTRASERV_H + +#include "dijkstragraph.h" +#include "dijkstrasearch.h" +#include "logging.h" + +#include + +struct dijkstra_serv { + struct dijkstra_solver *solver; + int port; + + SoupServer *soupserver; +}; + +struct dijkstra_serv *dijkstra_serv_new( + struct dijkstra_solver *solver, + int port); +int dijkstra_serv_run(struct dijkstra_serv *serv); + +#endif // DIJKSTRASERV_H diff --git a/src/main.c b/src/main.c index 404e10d..8dc166f 100644 --- a/src/main.c +++ b/src/main.c @@ -5,6 +5,7 @@ #include "dijkstragraph.h" #include "dijkstrasearch.h" +#include "dijkstraserv.h" #include "geojson.h" #include "logging.h" @@ -48,16 +49,14 @@ int main(int argc, char *argv[]) struct dijkstra_node *node; struct dijkstra_path *path; - struct dijkstra_search *search; - int iterations; - - GList *route = NULL; + int port = 6802; + struct dijkstra_serv *serv; /* Init stuff */ set_log_level(LL_DEBUG); /* Process arguments */ - while((opt = getopt(argc, argv, "vs")) != -1) { + while((opt = getopt(argc, argv, "vsp")) != -1) { switch(opt) { case 'v': @@ -66,6 +65,9 @@ int main(int argc, char *argv[]) case 's': set_log_level(0); break; + case 'p': + port = atoi(argv[i]); + break; case ':': case '?': help(argv[0]); @@ -92,13 +94,6 @@ int main(int argc, char *argv[]) add_geojson_to_dijkstra(solver, argv[i]); } - // add_test_map(solver); - // TODO: This start / end coordinates are only for testing - start = dijkstra_node_new_to_list(&solver->nodes, -856, -393); - dest = dijkstra_node_new_to_list(&solver->nodes, 388, 99); - //start = dijkstra_node_new_to_list(&solver->nodes, -957, -467); - //dest = dijkstra_node_new_to_list(&solver->nodes, -712, -467); - if (should_report(LL_NOISY)) { for (l = solver->nodes; l != NULL; l = l->next) { node = (struct dijkstra_node*) l->data; @@ -121,25 +116,14 @@ int main(int argc, char *argv[]) g_list_length(solver->nodes), g_list_length(solver->paths)); - /* Perform search */ - search = dijkstra_search_new(solver); - if (!search) { - report(LL_ERROR, "Error allocating dijkstra_search"); + /* Start server */ + serv = dijkstra_serv_new(solver, port); + if (!serv) { dijkstra_solver_free(solver); - return -3; - } - dijkstra_search_set_start(search, start); - if ((iterations = dijkstra_search_find_path(search, dest)) > 0) { - report(LL_INFO, "Found path from %d to %d after %d iterations.", start->uid, dest->uid, iterations); - route = dijkstra_search_get_route(search, dest); - dijkstra_search_route_to_geojson(search, route); - // dijkstra_print_path(search, dest); - // dijkstra_print_nodes(search); - } else { - report(LL_WARNING, "Couldn't find path."); + return -4; } + dijkstra_serv_run(serv); - dijkstra_search_free(search); dijkstra_solver_free(solver); return 0;