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