Jag behöver någon som kan köra min A* algoritm. Jag har läst mig på hur A* fungerar.
Om ni inte vet vad A* är så är det en sökalgoritm för koordinater. Du börjar på en position och sedan söker du den kortaste vägen till din slutdestination. Här är C-koden. Orsaken varför jag vänder mig till er har mycket med att det är bättre om flera testar än bara jag testar. Kom ihop att indexeringen är från 0 när man bestämmer koordinaterna. Kom också ihåg att -1 är väggar och där kan ni inte placera era koordinater.
Frågor som ni ställer:
Vad kan vi använda A* till?
Svar:
A* används mycket inom navigering. T.ex. om ni har en radiostyrd bil som ni vill att den ska köra själv. Då kan ni hitta en sökväg för er så kallade framtida robot. Jag har även utvecklat Adaptive Model Predictive Control i rent C, utan att fuskat med malloc, calloc eller recalloc. Model Predictive Control används mycket inom Telsa-bilarna för att hitta den mest optimala körstilen för framtid navigering av fordonet. Notera att Model Predictive Control är olinjär reglering då den tar hänsyn till begränsningar.
Innehåller:
- Eliminerar duplicerade vägar t.ex. om algoritmen hoppar fram och tillbaka, och sedan går den till mål. Rätt öndigt att hoppa fram och tillbaka
- Sick-sack. Bättre att gå en rak fin väg
- L2-Norm för bästa sökning
- L1-Norm för bästa sökning
Kod: Markera allt
/*
============================================================================
Name : Astar.c
Author : Daniel
Version : 1.0
Copyright : MIT
Description : A* algorithm
============================================================================
*/
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <string.h>
#include <time.h>
#define height_map 15
#define width_map 10
void print_int(int* A, int row, int column) {
for (int i = 0; i < row; i++) {
for (int j = 0; j < column; j++) {
printf("%d\t", *(A++));
}
printf("\n");
}
printf("\n");
}
static void heuristic_map(int *map, int x_stop, int y_stop, int height, int width, int norm_mode);
/*
* This is A* algorithm. An AI-algorithm in other words.
* It find the optimal(at least try too) path from your source to your destination.
* See working example how to use.
* I wrote this C code because I don't like calloc, malloc and recalloc in embedded.
*/
void Astar(int *map, int *path_x, int *path_y, int x_start, int y_start, int x_stop, int y_stop, int height, int width, int norm_mode, int* steps) {
// Clear first our path
memset(path_x, -1, height * width * sizeof(int));
memset(path_y, -1, height * width * sizeof(int));
// Create map with L2-norm
heuristic_map(map, x_stop, y_stop, height, width, norm_mode);
// Set some initial parameters
int direction[4];
const int x_directions[4] = { -1, 1, 0, 0 };
const int y_directions[4] = { 0, 0, -1, 1 };
int position = 0;
int minValue = 0;
int x = x_start;
int y = y_start;
//printf("Current start coordinate: x = %d, y = %d\n", x, y);
for (int k = 0; k < height * width; k++) {
// Look to the left, right, up and down
for (int index = 0; index < 4; index++)
direction[index] = *(map + (y + y_directions[index]) * width + x + x_directions[index]);
// Take the decision where to go by looking at direction array
minValue = direction[0];
position = 0;
for (int index = 0; index < 4; index++) {
// If our minimal value is -1, change it
if (minValue == -1) {
minValue = direction[index];
position = index;
} else {
// Check if we have found the smallest number now
if (direction[index] != -1 && direction[index] < minValue) {
minValue = direction[index];
position = index;
}
}
}
// Prevent repeating by increasing the weights
*(map + y * width + x) += *(map + y * width + x);
// Save path
*(path_x + k) = x;
*(path_y + k) = y;
// Where to go - position variable tells
if (position == 0) x--; // Go one step left
if (position == 1) x++; // Go one step right
if (position == 2) y--; // Go one step up
if (position == 3) y++; // Go one step down
// Check if we are at our goal
if (*(map + y * width + x) == 0) {
*(path_x + k + 1) = x;
*(path_y + k + 1) = y;
break;
}
}
//printf("Current goal coordinate: x = %d, y = %d\n", x, y);
// Filter the path_x and path_y from duplicates
for (int i = 0; i < height * width; i++) {
position = 0;
x = *(path_x + i);
y = *(path_y + i);
if (x != -1 && y != -1) {
// Search for the last same x and y in path_x and path_y
for (int j = i + 1; j < height * width; j++) {
if (x == *(path_x + j) && y == *(path_y + j)) {
position = j; // Remember this position
}
}
// If we got duplicates - delete them
if (position > 0) {
memset(path_x + i, -1, (position - i) * sizeof(int));
memset(path_y + i, -1, (position - i) * sizeof(int));
i = position; // Jump
}
} else {
//printf("Break at %i\n", i);
position = i; // Remember that too
break; // The rest is just -1's
}
}
// Pack path_x and path_y together now
for (int i = 0; i < position; i++) {
// If we have a deleted coordinate
if (*(path_x + i) == -1 && *(path_y + i) == -1) {
// Move them up
for (int j = i; j < height * width; j++) {
if (*(path_x + j) != -1 && *(path_y + j) != -1) {
*(path_x + i) = *(path_x + j);
*(path_x + j) = -1;
*(path_y + i) = *(path_y + j);
*(path_y + j) = -1;
break;
}
}
}
}
// Filter the path_x and path_y from zigzag
for (int i = 0; i < height * width; i++) {
position = 0;
x = *(path_x + i);
y = *(path_y + i);
if (x != -1 && y != -1) {
// Search for a jump
for (int index = 0; index < 4; index++) {
for (int j = i + 1; j < height * width; j++) {
if (x + x_directions[index] == *(path_x + j) && y + y_directions[index] == *(path_y + j)) {
if (j > position)
position = j; // We want to have the largest "gap", which is the zigzag
}
}
// If we got zigzag. We need to have + 1 because we cannot accept a neighbor step as zigzag
if (position > i + 1) {
memset(path_x + i + 1, -1, (position - i - 1) * sizeof(int));
memset(path_y + i + 1, -1, (position - i - 1) * sizeof(int));
i = position; // Jump
}
}
} else {
//printf("Break at %i\n", i);
position = i; // Remember that too
break; // The rest is just -1's
}
}
// Pack path_x and path_y together again
for (int i = 0; i < position; i++) {
// If we have a deleted coordinate
if (*(path_x + i) == -1 && *(path_y + i) == -1) {
// Move them up
for (int j = i; j < height * width; j++) {
if (*(path_x + j) != -1 && *(path_y + j) != -1) {
*(path_x + i) = *(path_x + j);
*(path_x + j) = -1;
*(path_y + i) = *(path_y + j);
*(path_y + j) = -1;
break;
}
}
}
}
// How many steps have we done
//printf("position is = %d\n", position);
*steps = position;
//printf("Done\n");
}
// norm_mode = 2 -> L2 norm
// norm_mode = 1 -> L1 norm
static void heuristic_map(int *map, int x_stop, int y_stop, int height, int width, int norm_mode) {
for (int i = 0; i < width; i++) {
for (int j = 0; j < height; j++) {
if (*(map + j * width + i) != -1) {
int dx = (j + x_stop) - (j + i); // Distance we want to go in x-axis
int dy = (y_stop + i) - (j + i); // Distance we want to go in y-axis
if (norm_mode == 1)
*(map + j * width + i) = abs(dx) + abs(dy); // L1-Norm
if (norm_mode == 2)
*(map + j * width + i) = dx * dx + dy * dy; // L2-Norm (without square root, not needed due to integers)
// You can add more heuristic here!
}
}
}
}
void show_path(int* map, int* path_x, int* path_y, int height, int width){
for(int k = 0; k < height*width; k++){
int x = *(path_x + k);
int y = *(path_y + k);
if(x != -1 && y != -1)
*(map + y*width + x) = -100 - k;
}
print_int(map, height, width);
}
int main() {
// Beginning coordinates
int x_start = 1;
int y_start = 8;
// End coordinates
int x_stop = 8;
int y_stop = 8;
// Path - Our goal is to find them
int path_x[height_map * width_map];
int path_y[height_map * width_map];
// Norm modes
int norm1 = 1; // L1-Norm
int norm2 = 2; // L2-Norm
// Steps
int steps1 = 0;
int steps2 = 0;
// Map size
int map[height_map*width_map] = {-1, -1, -1, -1, -1, -1, -1, -1, -1, -1,
-1, -1, -1, -1, -1, -1, -1, -1, -1, -1,
-1, -1, 0, 0, 0, 0, 0, 0, 0, -1,
-1, -1, 0, -1, -1, -1, -1, -1, 0, -1,
-1, -1, 0, -1, 0, 0, 0, -1, 0, -1,
-1, -1, 0, -1, 0, -1, 0, -1, 0, -1,
-1, -1, 0, -1, 0, -1, 0, -1, 0, -1,
-1, -1, 0, -1, 0, -1, 0, -1, 0, -1,
-1, 0, 0, 0, 0, -1, 0, -1, 0, -1,
-1, -1, -1, -1, -1, -1, 0, -1, 0, -1,
-1, -1, 0, 0, 0, 0, 0, -1, 0, -1,
-1, -1, 0, 0, -1, -1, -1, -1, 0, -1,
-1, -1, 0, 0, 0, 0, 0, 0, 0, -1,
-1, -1, -1, -1, -1, 0, -1, -1, 0, -1,
-1, -1, -1, -1, -1, -1, -1, -1, -1, -1};
// Print the map
printf("Initial map\n");
print_int(map, height_map, width_map);
// Compute the "shortest" path
printf("Compute the coordinates\n");
clock_t start, end;
float cpu_time_used;
start = clock();
// First compute with L1-Norm and check the steps
Astar(map, path_x, path_y, x_start, y_start, x_stop, y_stop, height_map, width_map, norm1, &steps1);
// Then compute with L2-norm and check the steps
Astar(map, path_x, path_y, x_start, y_start, x_stop, y_stop, height_map, width_map, norm2, &steps2);
// Check the steps now - Which of the norms results less steps
if(steps2 > steps1){
Astar(map, path_x, path_y, x_start, y_start, x_stop, y_stop, height_map, width_map, norm1, &steps1); // Get the path again
printf("Shortest step is = %d\n", steps1);
}else{
printf("Shortest step is = %d\n", steps2);
}
end = clock();
cpu_time_used = ((float) (end - start)) / CLOCKS_PER_SEC;
printf("\nTotal speed was %f,", cpu_time_used);
// Show the path
printf("\nComputed map\n");
show_path(map, path_x, path_y, height_map, width_map);
// Show the path
for (int i = 0; i < height_map * width_map; i++)
printf("x = %d, y = %d\n", path_x[i], path_y[i]);
return 0;
}