From 24af775b5041cbed67dfc84f3a0d67850a4b6a1b Mon Sep 17 00:00:00 2001 From: Aldrik Ramaekers Date: Sun, 11 Dec 2022 16:14:54 +0100 Subject: pathfinding --- pathfinding.c | 308 ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 308 insertions(+) create mode 100644 pathfinding.c (limited to 'pathfinding.c') diff --git a/pathfinding.c b/pathfinding.c new file mode 100644 index 0000000..b69d420 --- /dev/null +++ b/pathfinding.c @@ -0,0 +1,308 @@ +#include "include/pathfinding.h" + +static float distance_between(vec2f v1, vec2f v2) +{ + return ((v1.x-v2.x)*(v1.x-v2.x)+(v1.y-v2.y)*(v1.y-v2.y)); +} + +bool can_walk_at(float x, float y) +{ + for (int i = 0; i < max_objects; i++) { + object o = objects[i]; + if (!o.active) continue; + if (x >= o.position.x && x < o.position.x + o.size.x && y >= o.position.y && y < o.position.y + o.size.y) return false; + } + return true; +} + +bool find_path_to(vec2f start_pos, vec2f end_pos, array *to_fill, pathfinding_request* request) +{ + struct path_node { + struct path_node *parent; + vec2f position; + s32 g; + s32 h; + s32 f; + }; + + list open_array = list_create(sizeof(struct path_node)); + list closed_array = list_create(sizeof(struct path_node)); + + struct path_node *start_node = mem_alloc(sizeof(struct path_node)); + start_node->g = 0; + start_node->h = 0; + start_node->f = 0; + start_node->parent = 0; + start_node->position = start_pos; + + list_push(&open_array, (uint8_t*)start_node); + + struct path_node *current_node = 0; + while(open_array.count > 0 && !request->cancelled) + { + // Get the current node + current_node = list_at(&open_array, 0); + s32 current_index = 0; + + for (s32 i = 0; i < open_array.count; i++) + { + struct path_node *item = list_at(&open_array, i); + if (item->f < current_node->f) + { + current_node = item; + current_index = i; + } + } + + //Pop current off open array, add to closed array + list_push(&closed_array, (uint8_t*)current_node); + + if (closed_array.count > 1000) return false; + if (open_array.count > 1000) return false; + + // Found the goal + if (distance_between(current_node->position, end_pos) <= 0) + { + if (to_fill) + to_fill->length = 0; + struct path_node *current = current_node; + + while(current != 0) + { + if (to_fill) + { + vec2f v; + v.x = current->position.x + 0.5; + v.y = current->position.y + 0.5; + array_push(to_fill, (uint8_t*)&v); + } + + struct path_node *prev = current; + current = current->parent; + mem_free(prev); + } + + if (to_fill) { + array_remove_at(to_fill, to_fill->length-1); + request->done = true; + } + + list_destroy(&open_array); + list_destroy(&closed_array); + + return true; + } + + vec2 adjecent[8] = { {0, -1}, {0, 1}, {-1, 0}, {1, 0}, {-1, -1}, {-1, 1}, {1, -1}, {1, 1} }; + + list children = list_create(sizeof(struct path_node)); + + // Generate children + for (s32 i = 0; i < 8; i++) + { + //Get node position + vec2f node_position; + node_position.x = current_node->position.x + adjecent[i].x; + node_position.y = current_node->position.y + adjecent[i].y; + + //Make sure within range + if (!is_in_bounds(node_position.x, node_position.y) || !can_walk_at(node_position.x, node_position.y)) continue; + + // if diagonal move, check if not cutting off building + if (i >= 4) { + if (i == 4) // top left + { + if (!can_walk_at(node_position.x+1, node_position.y)) continue; + if (!can_walk_at(node_position.x, node_position.y+1)) continue; + } + if (i == 5) // down left + { + if (!can_walk_at(node_position.x+1, node_position.y)) continue; + if (!can_walk_at(node_position.x, node_position.y-1)) continue; + } + if (i == 6) // top right + { + if (!can_walk_at(node_position.x-1, node_position.y)) continue; + if (!can_walk_at(node_position.x, node_position.y+1)) continue; + } + if (i == 7) // down right + { + if (!can_walk_at(node_position.x-1, node_position.y)) continue; + if (!can_walk_at(node_position.x, node_position.y-1)) continue; + } + } + + // Create new node + struct path_node *new_node = mem_alloc(sizeof(struct path_node)); + new_node->g = 0; + new_node->h = 0; + new_node->f = 0; + new_node->parent = current_node; + new_node->position = node_position; + + if (node_position.x == current_node->position.x && + node_position.y == current_node->position.y) + continue; + + // Child is on the closed array + for (s32 c = 0; c < closed_array.count; c++) + { + struct path_node *closed_child = list_at(&closed_array, c); + + if (closed_child->position.x == new_node->position.x && + closed_child->position.y == new_node->position.y) + { + goto skip_adjecent; + } + } + + list_push(&children,(uint8_t*)new_node); + + skip_adjecent:; + } + + // Loop through children + for (s32 i = 0; i < children.count; i++) + { + struct path_node *child = list_at(&children, i); + + // Child is on the closed array + for (s32 c = 0; c < closed_array.count; c++) + { + struct path_node *closed_child = list_at(&closed_array, c); + + if (closed_child->position.x == child->position.x && + closed_child->position.y == child->position.y) + { + goto skip_child; + } + } + + // Create the f, g, and h values + child->g = current_node->g + 1; + child->h = distance_between(child->position, end_pos); + child->f = child->g + child->h; + + // Child is already in the open array + for (s32 c = 0; c < open_array.count; c++) + { + struct path_node *open_child = list_at(&open_array, c); + + if ((open_child->position.x == child->position.x && + open_child->position.y == child->position.y) && + child->g >= open_child->g) + { + goto skip_child; + } + } + + // Add the child to the open array + list_push(&open_array, (uint8_t*)child); + + skip_child:; + } + + list_remove_at(&open_array, current_index); + } + + return false; +} + +vec2f get_open_tile_next_to_target(float x, float y) +{ + vec2f adjecent[8] = { {0, -1}, {0, 1}, {-1, 0}, {1, 0}, {-1, -1}, {-1, 1}, {1, -1}, {1, 1} }; + + vec2f v_s; + v_s.x = x; + v_s.y = y; + + s32 closest_index = -1; + s32 closest_dist = 999999; + vec2f v; + for (s32 i = 0; i < 8; i++) + { + if (can_walk_at(x + adjecent[i].x, y + adjecent[i].y)) + { + vec2f vv; + vv.x = x + adjecent[i].x; + vv.y = y + adjecent[i].y; + + s32 dist = distance_between(v_s, vv); + if (dist < closest_dist) + { + v = vv; + closest_dist = dist; + closest_index = i; + } + } + } + + if (closest_index != -1) + { + return v; + } + + v.x = -1; + v.y = -1; + return v; +} + +void pathfinding_init() +{ + global_pathfinding_queue = array_create(sizeof(pathfinding_request)); + array_reserve(&global_pathfinding_queue, 1000); +} + +void pathfinding_destroy() +{ + array_destroy(&global_pathfinding_queue); +} + +void* pathfinding_thread(void *args) +{ + while(1) + { + pathfinding_request request; + + mutex_lock(&global_pathfinding_queue.mutex); + if (global_pathfinding_queue.length) + { + pathfinding_request* request = *(pathfinding_request**)array_at(&global_pathfinding_queue, 0); + array_remove_at(&global_pathfinding_queue, 0); + mutex_unlock(&global_pathfinding_queue.mutex); + + request->cancelled = false; + find_path_to(request->start, request->end, request->to_fill, request); + + continue; + } + else + { + mutex_unlock(&global_pathfinding_queue.mutex); + continue; + } + + thread_sleep(100); + } + + return 0; +} + +void make_pathfinding_request(vec2f start, vec2f end, array *to_fill, pathfinding_request *request) +{ + mutex_lock(&global_pathfinding_queue.mutex); + + request->cancelled = true; + + start.x = (int)start.x; + start.y = (int)start.y; + end.x = (int)end.x; + end.y = (int)end.y; + + request->start = start; + request->end = end; + request->to_fill = to_fill; + request->done = false; + array_push(&global_pathfinding_queue, (uint8_t*)&request); + mutex_unlock(&global_pathfinding_queue.mutex); +} \ No newline at end of file -- cgit v1.2.3-70-g09d2