Compare commits
	
		
			2 Commits
		
	
	
		
			25a80f7fa9
			...
			0b755401b8
		
	
	| Author | SHA1 | Date | |
|---|---|---|---|
| 0b755401b8 | |||
| d060afea0d | 
| @ -89,18 +89,6 @@ struct dijkstra_node *dijkstra_node_new_to_list(GList **list, | ||||
| 	return node; | ||||
| } | ||||
| 
 | ||||
| int dijkstra_connect_dangling_node_to_path(struct dijkstra_node **node, | ||||
| 					      struct dijkstra_path *path) | ||||
| { | ||||
| 	if (!path || !node) | ||||
| 		return -1; | ||||
| 
 | ||||
| 	(*node)->paths = g_list_append((*node)->paths, (gpointer) path); | ||||
| 	path->destination = *node; | ||||
| 
 | ||||
| 	return 0; | ||||
| } | ||||
| 
 | ||||
| int dijkstra_connect_destination_node_to_path(struct dijkstra_node *destination, | ||||
| 					      struct dijkstra_path *path) | ||||
| { | ||||
| @ -311,8 +299,10 @@ int dijkstra_path_intersect(struct dijkstra_solver *solver, | ||||
| 
 | ||||
| 			disconnected_node = dijkstra_disconnect_node_from_path(path_a, path_a->destination); | ||||
| 			dijkstra_connect_destination_node_to_path(node_new, path_a); | ||||
| 			path_a->weight = dijkstra_get_weight_from_distance(path_a->source, path_a->destination); | ||||
| 
 | ||||
| 			dijkstra_connect_nodes_to_path(node_new, disconnected_node, path_new); | ||||
| 			path_new->weight = dijkstra_get_weight_from_distance(path_new->source, path_new->destination); | ||||
| 		} | ||||
| 
 | ||||
| 		// Now do the same thing for path b
 | ||||
| @ -322,8 +312,10 @@ int dijkstra_path_intersect(struct dijkstra_solver *solver, | ||||
| 
 | ||||
| 			disconnected_node = dijkstra_disconnect_node_from_path(path_b, path_b->destination); | ||||
| 			dijkstra_connect_destination_node_to_path(node_new, path_b); | ||||
| 			path_b->weight = dijkstra_get_weight_from_distance(path_b->source, path_b->destination); | ||||
| 
 | ||||
| 			dijkstra_connect_nodes_to_path(node_new, disconnected_node, path_new); | ||||
| 			path_new->weight = dijkstra_get_weight_from_distance(path_new->source, path_new->destination); | ||||
| 		} | ||||
| 
 | ||||
| 		return 1; | ||||
| @ -402,6 +394,10 @@ void dijkstra_solver_free(struct dijkstra_solver *solver) | ||||
| inline struct dijkstra_node *dijkstra_node_get_connection(struct dijkstra_node *node, | ||||
| 							  struct dijkstra_path *path) | ||||
| { | ||||
| 	if (!node || !path) { | ||||
| 		report(LL_CRITICAL, "Called get_connection with NULL node (%p) or path (%p).", | ||||
| 		       node, path); | ||||
| 	} | ||||
| 	if (node != path->destination && node != path->source) { | ||||
| 		report(LL_WARNING, "get_connection requested for invalid node / path pair: %d with %p (%s).", node->uid, path, path->name); | ||||
| 		for (GList *l = node->paths; l != NULL; l = l->next) { | ||||
|  | ||||
| @ -8,6 +8,7 @@ void dijkstra_search_reset(struct dijkstra_search *search) | ||||
| 	for (i=0; i < search->node_count; i++) { | ||||
| 		search->states[i]->cost = DIJKSTRA_COST_MAX; | ||||
| 		search->states[i]->visited = 0; | ||||
| 		search->states[i]->cheapest_path = NULL; | ||||
| 	} | ||||
| 
 | ||||
| 	if (search->start) { | ||||
| @ -28,6 +29,7 @@ struct dijkstra_search *dijkstra_search_new(struct dijkstra_solver *solver) | ||||
| 
 | ||||
| 	search = malloc(sizeof(*search)); | ||||
| 	if (search) { | ||||
| 		search->solver = solver; | ||||
| 		search->states = malloc(sizeof(*search->states) * g_list_length(solver->nodes)); | ||||
| 		search->node_count = g_list_length(solver->nodes); | ||||
| 		search->start = NULL; | ||||
| @ -94,11 +96,16 @@ struct dijkstra_node *dijkstra_search_process_queue(struct dijkstra_search *sear | ||||
| 	} | ||||
| 	node = cheapest_node; | ||||
| 	state = search->states[node->uid]; | ||||
| 	report(LL_DEBUG, "Current cheapest node is %d", node->uid); | ||||
| 	report(LL_NOISY, "Current cheapest node is %d", node->uid); | ||||
| 
 | ||||
| 	// 2. Mark the current node as visited and remove it from the actives
 | ||||
| 	state->visited = 1; | ||||
| 	search->active_nodes = g_list_remove(search->active_nodes, node); | ||||
| 	if (!state->cheapest_path && node != search->start) { | ||||
| 		report(LL_CRITICAL, "DEBUG: marked current node as visited, even though it doesn't have a path to it. Cost is %d.", | ||||
| 		       state->cost); | ||||
| 		getchar(); | ||||
| 	} | ||||
| 
 | ||||
| 	// 3. Update connecting nodes
 | ||||
| 	for (l = node->paths; l != NULL; l = l->next) { | ||||
| @ -106,13 +113,14 @@ struct dijkstra_node *dijkstra_search_process_queue(struct dijkstra_search *sear | ||||
| 		cost = state->cost + path->weight; | ||||
| 
 | ||||
| 		connection = dijkstra_node_get_connection(node, path); | ||||
| 
 | ||||
| 		// 3.1 Update cost if cheaper
 | ||||
| 		if (cost < search->states[connection->uid]->cost) { | ||||
| 			search->states[connection->uid]->cost = cost; | ||||
| 			search->states[connection->uid]->cheapest_path = path; | ||||
| 			report(LL_DEBUG, "  Set cost of node %d to %d (backpath from %d to %d)", connection->uid, cost, connection->uid, node->uid); | ||||
| 			report(LL_NOISY, "  Set cost of node %d to %d (backpath from %d to %d)", connection->uid, cost, connection->uid, node->uid); | ||||
| 			if (search->states[connection->uid]->visited) { | ||||
| 				report(LL_WARNING, "  Reset cost of visited node!"); // TODO: not necessary if algorithm OK
 | ||||
| 				report(LL_CRITICAL, "  Reset cost of visited node!"); // TODO: not necessary if algorithm OK
 | ||||
| 			} | ||||
| 		} | ||||
| 		// 3.2. Add connecting nodes to active nodes if not already visited
 | ||||
| @ -120,7 +128,10 @@ struct dijkstra_node *dijkstra_search_process_queue(struct dijkstra_search *sear | ||||
| 			if (!g_list_find(search->active_nodes, connection)) { | ||||
| 				search->active_nodes = g_list_append(search->active_nodes, | ||||
| 								     (gpointer) connection); | ||||
| 				report(LL_DEBUG, "  Append %d as an active node.", connection->uid); | ||||
| 				report(LL_NOISY, "  Append %d as an active node.", connection->uid); | ||||
| 				if (!search->states[connection->uid]->cheapest_path) { | ||||
| 					report(LL_CRITICAL, "Appended a node without a path!"); | ||||
| 				} | ||||
| 			} | ||||
| 		} | ||||
| 	} | ||||
| @ -151,28 +162,59 @@ int dijkstra_search_find_path(struct dijkstra_search *search, | ||||
| 	return -1; | ||||
| } | ||||
| 
 | ||||
| int dijkstra_print_path(struct dijkstra_search *search, | ||||
| void dijkstra_print_path(struct dijkstra_search *search, | ||||
| 			 struct dijkstra_node *destination) | ||||
| { | ||||
| 	struct dijkstra_node *node = NULL; | ||||
| 	GList *l; | ||||
| 	int i=500; // TODO: This should not be necessary. Plus the number is arbitrary.
 | ||||
| 	unsigned int i = 0; | ||||
| 
 | ||||
| 	if (!destination || !search->states[destination->uid]->visited) | ||||
| 		return 0; | ||||
| 		return; | ||||
| 
 | ||||
| 	node = destination; | ||||
| 	while (node != search->start) { | ||||
| 	while (1) { | ||||
| 		if (node != destination) | ||||
| 			printf(","); | ||||
| 		printf("[%d,%d]", (int) node->position.x, (int) node->position.y); | ||||
| 
 | ||||
| 	//	printf("%u [%d]-> ", node->uid, search->states[node->uid]->cost);
 | ||||
| 		if (node == search->start) | ||||
| 			break; | ||||
| 		node = dijkstra_node_get_connection(node, search->states[node->uid]->cheapest_path); | ||||
| 		if (!i--) { | ||||
| 		if (i++ > search->node_count) { | ||||
| 			report(LL_CRITICAL, "Iteration limit for backsolver. This should never happen."); | ||||
| 			break; | ||||
| 		} | ||||
| 	} | ||||
| 
 | ||||
| 	printf("\n"); | ||||
| } | ||||
| 
 | ||||
| void dijkstra_print_nodes(struct dijkstra_search *search) | ||||
| { | ||||
| 	GList *l; | ||||
| 	struct dijkstra_node *node; | ||||
| 
 | ||||
| 	printf("["); | ||||
| 	for (l = search->solver->nodes; l != NULL; l = l->next) { | ||||
| 		node = (struct dijkstra_node*) l->data; | ||||
| 		printf("{ \"type\": \"Feature\",\n" | ||||
| 		       "    \"geometry\" : {\n" | ||||
| 		       "        \"type\" : \"Point\", \"coordinates\" : [%f, %f]\n" | ||||
| 		       "    },\n" | ||||
| 		       "    \"properties\" : {\n", node->position.x, node->position.y); | ||||
| 		if (search->states[node->uid]->cheapest_path) { | ||||
| 			printf("        \"name\" : \"Node %d (cost %d, from %d)\"\n", | ||||
| 			       node->uid, search->states[node->uid]->cost, dijkstra_node_get_connection(node, search->states[node->uid]->cheapest_path)->uid); | ||||
| 		} else if (node == search->start) { | ||||
| 			printf("        \"name\" : \"Starting node %d\"\n", | ||||
| 			       node->uid); | ||||
| 		} else { | ||||
| 			printf("        \"name\" : \"Unvisited node %d\"\n", | ||||
| 			       node->uid); | ||||
| 		} | ||||
| 		printf("    }\n" | ||||
| 		       "},\n"); | ||||
| 
 | ||||
| 	} | ||||
| 	printf("{}]\n"); | ||||
| } | ||||
|  | ||||
| @ -12,6 +12,7 @@ struct dijkstra_state { | ||||
| }; | ||||
| 
 | ||||
| struct dijkstra_search { | ||||
| 	struct dijkstra_solver *solver; | ||||
| 	struct dijkstra_state **states; | ||||
| 	unsigned int node_count; | ||||
| 	struct dijkstra_node *start; | ||||
| @ -24,6 +25,7 @@ void dijkstra_search_set_start(struct dijkstra_search *search, | ||||
| 			       struct dijkstra_node *start); | ||||
| int dijkstra_search_find_path(struct dijkstra_search *search, | ||||
| 			      struct dijkstra_node *destination); | ||||
| int dijkstra_print_path(struct dijkstra_search *search, | ||||
| void dijkstra_print_path(struct dijkstra_search *search, | ||||
| 			struct dijkstra_node *destination); | ||||
| void dijkstra_print_nodes(struct dijkstra_search *search); | ||||
| #endif // DIJKSTRASEARCH_H
 | ||||
|  | ||||
							
								
								
									
										12
									
								
								src/main.c
									
									
									
									
									
								
							
							
						
						
									
										12
									
								
								src/main.c
									
									
									
									
									
								
							| @ -92,21 +92,24 @@ int main(int argc, char *argv[]) | ||||
| 
 | ||||
| 	// add_test_map(solver);
 | ||||
| 	// TODO: This start / end coordinates are only for testing
 | ||||
| 	start = dijkstra_node_new_to_list(&solver->nodes, -1213, -305); | ||||
| 	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; | ||||
| 			printf("Node %5d @%p: %f,\t%f [%d paths]\n", node->uid, node, node->position.x, node->position.y, g_list_length(node->paths)); | ||||
| 			fprintf(stderr, "Node %5d @%p: %f,\t%f [%d paths]\n", node->uid, node, node->position.x, node->position.y, g_list_length(node->paths)); | ||||
| 		} | ||||
| 		printf("\n"); | ||||
| 
 | ||||
| 		for (l = solver->paths; l != NULL; l = l->next) { | ||||
| 			path = (struct dijkstra_path*) l->data; | ||||
| 			printf("Path %p: %5d (%f,%f) -> %5d (%f,%f) \t\"%s\"\n", path, | ||||
| 			fprintf(stderr, "Path %p: %5d (%f,%f) -> %5d (%f,%f) cost %d \t\"%s\"\n", path, | ||||
| 			       path->source->uid, path->source->position.x, path->source->position.y, | ||||
| 			       path->destination->uid, path->destination->position.x, path->destination->position.y, | ||||
| 			       path->weight, | ||||
| 			       path->name); | ||||
| 		} | ||||
| 		printf("\n"); | ||||
| @ -125,8 +128,9 @@ int main(int argc, char *argv[]) | ||||
| 	} | ||||
| 	dijkstra_search_set_start(search, start); | ||||
| 	if ((iterations = dijkstra_search_find_path(search, dest)) > 0) { | ||||
| 		report(LL_INFO, "Found path after %d iterations.", iterations); | ||||
| 		report(LL_INFO, "Found path from %d to %d after %d iterations.", start->uid, dest->uid, iterations); | ||||
| 		dijkstra_print_path(search, dest); | ||||
| 		// dijkstra_print_nodes(search);
 | ||||
| 	} else { | ||||
| 		report(LL_WARNING, "Couldn't find path."); | ||||
| 	} | ||||
|  | ||||
		Loading…
	
	
			
			x
			
			
		
	
		Reference in New Issue
	
	Block a user