initial commit
This commit is contained in:
Magnus Küderli
@@ -0,0 +1,14 @@
|
||||
SRC = src/*
|
||||
TARGET = target
|
||||
LIB = -lraylib -lm -lpthread -ldl -lrt -lX11
|
||||
all: $(TARGET)
|
||||
|
||||
$(TARGET):
|
||||
gcc $(SRC) -o $(TARGET) $(LIB)
|
||||
|
||||
clean:
|
||||
rm -f $(TARGET)
|
||||
|
||||
run: $(TARGET)
|
||||
gcc $(SRC) -o $(TARGET) $(LIB)
|
||||
./$(TARGET)
|
||||
@@ -0,0 +1,26 @@
|
||||
[
|
||||
{
|
||||
"arguments": [
|
||||
"/usr/bin/gcc",
|
||||
"-c",
|
||||
"-o",
|
||||
"target",
|
||||
"src/astar.c"
|
||||
],
|
||||
"directory": "/home/magnus/code/c/projects/astar",
|
||||
"file": "/home/magnus/code/c/projects/astar/src/astar.c",
|
||||
"output": "/home/magnus/code/c/projects/astar/target"
|
||||
},
|
||||
{
|
||||
"arguments": [
|
||||
"/usr/bin/gcc",
|
||||
"-c",
|
||||
"-o",
|
||||
"target",
|
||||
"src/main.c"
|
||||
],
|
||||
"directory": "/home/magnus/code/c/projects/astar",
|
||||
"file": "/home/magnus/code/c/projects/astar/src/main.c",
|
||||
"output": "/home/magnus/code/c/projects/astar/target"
|
||||
}
|
||||
]
|
||||
@@ -0,0 +1,40 @@
|
||||
# pragma once
|
||||
|
||||
# include <stdbool.h>
|
||||
# include <raylib.h>
|
||||
# include <raymath.h>
|
||||
|
||||
typedef struct ANode {
|
||||
Vector2 parent;
|
||||
|
||||
int h;
|
||||
int g;
|
||||
|
||||
int f;
|
||||
|
||||
bool is_collapsed;
|
||||
bool is_obstacle;
|
||||
} anode_t;
|
||||
|
||||
typedef struct AField {
|
||||
int rows;
|
||||
int columns;
|
||||
|
||||
anode_t** map;
|
||||
} afield_t;
|
||||
|
||||
afield_t generate_astar_field(int rows, int columns, int** col_map);
|
||||
|
||||
void get_a_star_path(afield_t* field, Vector2 start, Vector2 goal);
|
||||
|
||||
void setup_start_field(afield_t* field, Vector2 start, Vector2 target);
|
||||
|
||||
Vector2 get_best_node(afield_t *field);
|
||||
void collapse_node_at(afield_t *field, Vector2 node_pos);
|
||||
int assign_node_as_parent(afield_t *field, Vector2 target, Vector2 node_pos);
|
||||
|
||||
void debug_print_field_collisions(afield_t* field);
|
||||
|
||||
void debug_print_field(afield_t* field, bool show_parent);
|
||||
|
||||
int adjusted_euclidean(Vector2 a, Vector2 b);
|
||||
+275
@@ -0,0 +1,275 @@
|
||||
#include <assert.h>
|
||||
#include <math.h>
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
|
||||
#include "../include/astar.h"
|
||||
|
||||
void reset_field(afield_t *field, int** map) {
|
||||
for (int r = 0; r < field->rows; r++) {
|
||||
for (int c = 0; c < field->columns; c++) {
|
||||
bool is_obstacle = map[r][c] != -1 ? true : false;
|
||||
|
||||
field->map[r][c] = (anode_t){
|
||||
.parent = {-1, -1},
|
||||
.h = -1,
|
||||
.g = -1,
|
||||
.f = -1,
|
||||
.is_collapsed = false,
|
||||
.is_obstacle = is_obstacle,
|
||||
};
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
afield_t generate_astar_field(int rows, int columns, int** col_map) {
|
||||
anode_t **map = calloc(sizeof(anode_t *), rows);
|
||||
assert(map);
|
||||
for (int r = 0; r < rows; r++) {
|
||||
map[r] = (anode_t *)calloc(sizeof(anode_t), columns);
|
||||
assert(map[r]);
|
||||
}
|
||||
|
||||
afield_t field = {
|
||||
rows = rows,
|
||||
columns = columns,
|
||||
map = map,
|
||||
};
|
||||
|
||||
reset_field(&field, col_map);
|
||||
|
||||
return field;
|
||||
}
|
||||
|
||||
int adjusted_euclidean(Vector2 a, Vector2 b) {
|
||||
int dx = b.x - a.x;
|
||||
int dy = b.y - a.y;
|
||||
return (int)round(sqrt(dx * dx + dy * dy) * 10.0);
|
||||
}
|
||||
|
||||
int octile_heuristic(Vector2 a, Vector2 b) {
|
||||
int dx = abs((int)b.x - (int)a.x);
|
||||
int dy = abs((int)b.y - (int)a.y);
|
||||
int min_d = dx < dy ? dx : dy;
|
||||
int max_d = dx > dy ? dx : dy;
|
||||
return 14 * min_d + 10 * (max_d - min_d);
|
||||
}
|
||||
|
||||
void update_node_values(afield_t *field, Vector2 target, Vector2 node_pos, Vector2 parent_pos) {
|
||||
anode_t *node = &field->map[(int)node_pos.y][(int)node_pos.x];
|
||||
|
||||
int traversal_cost = (node_pos.x == parent_pos.x) || (node_pos.y == parent_pos.y) ? 10 : 14;
|
||||
int new_g = field->map[(int)parent_pos.y][(int)parent_pos.x].g + traversal_cost;
|
||||
|
||||
if (node->parent.y != -1) {
|
||||
// new potential cost
|
||||
if (node->g <= new_g) {
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
// set parent
|
||||
node->parent = parent_pos;
|
||||
|
||||
// set values
|
||||
node->g = new_g;
|
||||
node->h = octile_heuristic(node_pos, target);
|
||||
node->f = node->g + node->h;
|
||||
|
||||
}
|
||||
|
||||
bool is_valid_child(afield_t* field, Vector2 parent, Vector2 child) {
|
||||
int child_x = child.x;
|
||||
int child_y = child.y;
|
||||
|
||||
// if in bounds set note as parent
|
||||
if (child_x < 0 || child_y < 0 || child_x >= field->columns || child_y >= field->rows) {
|
||||
// printf("out of bounds node at: %d %d\n", child_x, child_y);
|
||||
return false;
|
||||
}
|
||||
|
||||
if (field->map[child_y][child_x].is_collapsed) { return false; }
|
||||
|
||||
if (field->map[child_y][child_x].is_obstacle) { return false; }
|
||||
|
||||
|
||||
// dont allow diagonal passing trough obstacles
|
||||
|
||||
if (child.x == parent.x || child.y == parent.y) {
|
||||
// no diagonal movement
|
||||
return true;
|
||||
}
|
||||
|
||||
Vector2 dir = Vector2Subtract(child, parent);
|
||||
// printf("Direction: %i %i\n", (int)dir.x, (int)dir.y);
|
||||
|
||||
bool is_x_obstacle = field->map[(int)(parent.y+dir.y)][(int)parent.x].is_obstacle;
|
||||
bool is_y_obstacle = field->map[(int)parent.y][(int)(parent.x+dir.x)].is_obstacle;
|
||||
|
||||
// printf("obstacles: %i %i\n", is_x_obstacle, is_y_obstacle);
|
||||
if (is_x_obstacle || is_y_obstacle) {
|
||||
return false;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
int assign_node_as_parent(afield_t* field, Vector2 target, Vector2 node_pos) {
|
||||
Vector2 offsets[3][3] = {
|
||||
{(Vector2){-1.0f, -1.0f}, (Vector2){0.0f, -1.0f}, (Vector2){1.0f, -1.0f}},
|
||||
{(Vector2){-1.0f, 0.0f}, (Vector2){0.0f, 0.0f}, (Vector2){1.0f, 0.0f}},
|
||||
{(Vector2){-1.0f, 1.0f}, (Vector2){0.0f, 1.0f}, (Vector2){1.0f, 1.0f}},
|
||||
};
|
||||
|
||||
for (int y = 0; y < 3; y++) {
|
||||
for (int x = 0; x < 3; x++) {
|
||||
if (x == 1 && y == 1) { continue; } // skip parent
|
||||
|
||||
int child_x = node_pos.x + offsets[y][x].x;
|
||||
int child_y = node_pos.y + offsets[y][x].y;
|
||||
|
||||
if (!is_valid_child(field, node_pos, (Vector2) { child_x, child_y } )) {
|
||||
continue;
|
||||
}
|
||||
|
||||
if (child_x == (int)target.x && child_y == (int)target.y) {
|
||||
// target found!!
|
||||
field->map[(int)target.y][(int)target.x].is_collapsed = true;
|
||||
field->map[(int)target.y][(int)target.x].parent = node_pos;
|
||||
return 1;
|
||||
}
|
||||
|
||||
update_node_values(field, target, (Vector2) {child_x, child_y}, node_pos);
|
||||
}
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
void collapse_node_at(afield_t *field, Vector2 node_pos) {
|
||||
field->map[(int)node_pos.y][(int)node_pos.x].is_collapsed = true;
|
||||
}
|
||||
|
||||
Vector2 get_best_node(afield_t *field) {
|
||||
Vector2 best_pos = {-1, -1};
|
||||
int best_f = __INT_MAX__;
|
||||
int best_h = __INT_MAX__;
|
||||
|
||||
for (int y = 0; y < field->rows; y++) {
|
||||
for (int x = 0; x < field->columns; x++) {
|
||||
anode_t *test_node = &field->map[y][x];
|
||||
|
||||
if (test_node->f == -1) { continue; }
|
||||
|
||||
// TODO what if two nodes have same f and same heuristic? (symetric
|
||||
// problem)
|
||||
if ((
|
||||
(test_node->f < best_f) ||
|
||||
(test_node->f == best_f && test_node->h < best_h)
|
||||
) &&
|
||||
!test_node->is_collapsed
|
||||
&&
|
||||
!test_node->is_obstacle
|
||||
) {
|
||||
// new best
|
||||
best_pos = (Vector2){x, y};
|
||||
best_f = test_node->f;
|
||||
best_h = test_node->h;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// will return -1 -1 when no uncollapsed node was found
|
||||
return best_pos;
|
||||
}
|
||||
|
||||
void get_a_star_path(afield_t *field, Vector2 start, Vector2 goal) {
|
||||
// TODO: check if start or end node is obstacle
|
||||
|
||||
if (start.x == goal.x && start.y == goal.y) {
|
||||
// no path required
|
||||
return; // TODO: Return valid empty path
|
||||
}
|
||||
|
||||
// setup start node
|
||||
anode_t *start_node_p = &field->map[(int)start.y][(int)start.x];
|
||||
start_node_p->g = 0;
|
||||
int heuristic = octile_heuristic(start, goal);
|
||||
start_node_p->h = heuristic;
|
||||
start_node_p->f = heuristic;
|
||||
start_node_p->is_collapsed = true;
|
||||
|
||||
// set values of surrounding nodes
|
||||
assign_node_as_parent(field, goal, start);
|
||||
|
||||
bool found_target = false;
|
||||
while (!found_target) {
|
||||
// get node with best f score
|
||||
Vector2 best = get_best_node(field);
|
||||
if (best.x == -1 && best.y == -1) {
|
||||
printf("Target can not be reached!!");
|
||||
}
|
||||
// collase node with best f score and recalculate children
|
||||
collapse_node_at(field, best);
|
||||
assign_node_as_parent(field, goal, best);
|
||||
// repeat until child is target
|
||||
}
|
||||
}
|
||||
|
||||
void setup_start_field(afield_t* field, Vector2 start, Vector2 target) {
|
||||
int heuristic = octile_heuristic(start, target);
|
||||
|
||||
field->map[(int)start.y][(int)start.x].g = 0;
|
||||
field->map[(int)start.y][(int)start.x].h = heuristic;
|
||||
field->map[(int)start.y][(int)start.x].f = heuristic;
|
||||
}
|
||||
|
||||
void debug_print_field_collisions(afield_t* field) {
|
||||
printf("\nAstar Collision:\n");
|
||||
for (int r = 0; r < field->rows; r++) {
|
||||
for (int c = 0; c < field->columns; c++) {
|
||||
printf("%i", field->map[r][c].is_obstacle);
|
||||
}
|
||||
printf("\n");
|
||||
}
|
||||
printf("\n");
|
||||
}
|
||||
|
||||
void debug_print_field(afield_t* field, bool show_parent) {
|
||||
char* arrows[3][3] = {
|
||||
{"↖", "↑", "↗"},
|
||||
{"←", "•", "→"},
|
||||
{"↙", "↓", "↘"}
|
||||
};
|
||||
|
||||
printf("\nAstar Field:\n");
|
||||
for (int r = 0; r < field->rows; r++) {
|
||||
for (int c = 0; c < field->columns; c++) {
|
||||
printf(" |");
|
||||
if (field->map[r][c].is_obstacle) {
|
||||
printf("#");
|
||||
}
|
||||
// else if (field->map[r][c].is_collapsed) {
|
||||
// printf("[]");
|
||||
// }
|
||||
else {
|
||||
if (show_parent) {
|
||||
Vector2 parent = field->map[r][c].parent;
|
||||
if (parent.x != -1 && field->map[r][c].is_collapsed) {
|
||||
Vector2 par_offs = Vector2Subtract(parent, (Vector2) {c, r});
|
||||
printf("%s", arrows[(int)par_offs.y+1][(int)par_offs.x+1]);
|
||||
}
|
||||
else {
|
||||
printf(" ");
|
||||
}
|
||||
}
|
||||
else {
|
||||
printf("%2i", field->map[r][c].f);
|
||||
}
|
||||
}
|
||||
printf("| ");
|
||||
}
|
||||
printf("\n");
|
||||
}
|
||||
printf("\n");
|
||||
|
||||
}
|
||||
+70
@@ -0,0 +1,70 @@
|
||||
#include <raylib.h>
|
||||
#include <stdlib.h>
|
||||
#include <stdio.h>
|
||||
#include <assert.h>
|
||||
|
||||
#include "../include/astar.h"
|
||||
|
||||
int** create_array(int x, int y) {
|
||||
int rows = y, cols = x;
|
||||
int** myArray = malloc(rows * sizeof(int*));
|
||||
for (int i = 0; i < rows; i++) {
|
||||
myArray[i] = malloc(cols * sizeof(int));
|
||||
}
|
||||
|
||||
int values[10][10] = {
|
||||
{-1,-1,-1,-1,-1,-1,-1,-1,-1,-1},
|
||||
{-1,-1,-1,-1,-1,-1,-1,-1,-1,-1},
|
||||
{-1,-1,-1,-1,-1,-1,-1,-1,-1,-1},
|
||||
{-1,-1,-1,-1,-1,-1,-1,-1,-1,-1},
|
||||
{-1,-1,-1,-1,-1,-1,-1,-1,-1,-1},
|
||||
{-1,-1,-1,-1,-1,-1,-1,-1,-1,-1},
|
||||
{-1,-1,-1,-1,-1,-1,-1,-1,-1,-1},
|
||||
{-1,-1,-1,-1,-1,-1,-1,-1,-1,-1},
|
||||
{-1,-1,-1,-1,-1,-1,-1,-1,-1,-1},
|
||||
{-1,-1,-1,-1,-1,-1,-1,-1,-1,-1}
|
||||
};
|
||||
|
||||
for (int i = 0; i < rows; i++) {
|
||||
for (int j = 0; j < cols; j++) {
|
||||
myArray[i][j] = values[i][j];
|
||||
}
|
||||
}
|
||||
|
||||
return myArray;
|
||||
}
|
||||
|
||||
int main() {
|
||||
int** dyn_array = create_array(10, 10);
|
||||
afield_t field = generate_astar_field(10, 10, dyn_array);
|
||||
|
||||
Vector2 start = { 0, 0 };
|
||||
Vector2 goal = { 9, 0 };
|
||||
|
||||
setup_start_field(&field, start, goal);
|
||||
collapse_node_at(&field, start);
|
||||
|
||||
assign_node_as_parent(&field, goal, start);
|
||||
debug_print_field(&field, true);
|
||||
|
||||
for (int x = 0; x < 100; x++) {
|
||||
Vector2 best = get_best_node(&field);
|
||||
if (best.x == -1 || best.y == -1) {
|
||||
// all search paths exausted
|
||||
debug_print_field(&field, true);
|
||||
printf("TARGET NOT REACHABLE!\n");
|
||||
return 0;
|
||||
}
|
||||
// printf("Best node: %d %d\n", (int)best.x, (int)best.y);
|
||||
|
||||
int found = assign_node_as_parent(&field, goal, best);
|
||||
collapse_node_at(&field, best);
|
||||
// printf("collapsed node at: %d %d", (int)best.x, (int)best.y);
|
||||
|
||||
if (found) {
|
||||
debug_print_field(&field, true);
|
||||
printf("FOUND TARGET!\n");
|
||||
return 0;
|
||||
}
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user