Tobias Berger
Tobis Programm forked to not destroy your golden files
Fork of
Robocode
by 
PES 2 - Gruppe 1
Diff: source/pathfinding.cpp
- Revision:
- 18:a82994e67297
- Parent:
- 16:4a20536c9bb8
- Child:
- 32:777976c4d733
diff -r 581f5fd20f62 -r a82994e67297 source/pathfinding.cpp
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/source/pathfinding.cpp Thu Mar 02 08:16:07 2017 +0000
@@ -0,0 +1,273 @@
+/**
+* Pathfinding function library
+* Handels Pathfinding inside the Arena
+* Version 3.0.0
+**/
+
+/**
+* Release notes:
+* Storage and run time optimized
+* Starts from target to reduce code and run time
+**/
+
+/**
+* Storage table:
+* 80kB uint16_t [40000] open_list (contains open f values as well as closed list value = max of 16 bit = 65535)
+* 80kB uint16_t [40000] g_list (contains g values from A to current incl. target, needed for pathtracing)
+* 40kB uint8_t [40000] obstacle_list (contains hard surface as well as red and green LEGO stones)
+* 2kB uint8_t [2000] walkpath (contains the path coordinates in a list, [0] = start, [n] = target)
+**/
+
+//#include "mbed.h"
+#include "pathfinding.h"
+#include <stdio.h>
+#include <iostream>
+
+#define row 100
+#define col 100
+
+
+using namespace std;
+
+//will be replaced with
+uint8_t obstacle_list[row][col] = { 0 };
+
+//global
+position walkpath[5 * row] = { 0 };
+
+//should be local
+static uint16_t open_list[row][col] = { 0 }; // contains open slots marked with their F value (G + H)
+static uint16_t g_list[row][col] = { 0 }; // G = A to square (C) in absolut number
+
+static uint16_t open_list_count = 0;
+static uint16_t counter;
+
+
+/************************************************************************************************************
+Main a_star handling
+************************************************************************************************************/
+void pathfinding() {
+ define_obst();
+ position start = { 1,1 }; //start
+ position target = { 198,198 }; //target
+ position current = { 0 }; //current pos
+
+ memset(open_list, 0, sizeof(open_list));
+ memset(g_list, 0, sizeof(g_list));
+ memset(walkpath, 0, sizeof(walkpath));
+ open_list_count = 0;
+ counter = 0;
+
+ // check if position is reachable
+ if (obstacle_list[start.x][start.y] == 1 || start.x > row || start.y > col ||
+ obstacle_list[target.x][target.y] == 1 || target.x > row || target.y > col) {
+ printf("Fatal Error, no path calculation possible\n");
+ }
+ else {
+ position diff;
+ diff.x = abs(target.x - start.x);
+ diff.y = abs(target.y - start.y);
+ uint8_t diagonal;
+ if (diff.x > diff.y) {
+ diagonal = diff.x - (diff.x - diff.y);
+ }
+ else {
+ diagonal = diff.y - (diff.y - diff.x);
+ }
+ open_list[target.x][target.y] = diff.x * 2 + diff.y * 2 - diagonal;
+ open_list_count += 1;
+ g_list[target.x][target.y] = 1;
+
+ do {
+
+ current = openList_lowest_F(); // Get the square with the lowest F score
+ open_list[current.x][current.y] = 65535; // add position to the closed list; open_list 1111 1111 1111 1111 ^= closed_list = 1
+ open_list_count -= 1;
+
+ if (open_list[start.x][start.y] == 65535) { // if we added the destination to the closed list, we've found a path
+ printf("Path found\n\n"); // PATH FOUND
+ break; // break the loop
+ }
+ calc_F(target, start, current); // Calculates F for each neighbour of current lowest F
+
+ } while (!(open_list_count == 0)); // Continue until there is no more available square in the open list (which means there is no path)
+ if (open_list[start.x][start.y] != 65535) { // if we added the destination to the closed list, we've found a path
+ printf("No Path possible\n");
+ }
+ else {
+ mapp_path(start); //mapps the path onto the open_list, be replaced by position vector array;
+ printf("\nPath from (%d:%d) to (%d:%d) has a total of %d steps\n\n", start.x, start.y, target.x, target.y, counter);
+ }
+ }
+}
+
+/************************************************************************************************************
+only needed for test, will be done by mapping function, delete for use on robot
+************************************************************************************************************/
+static void define_obst() {
+
+ for (int i = 0; i < row; i++) {
+ obstacle_list[i][0] = 1;
+ obstacle_list[i][col - 1] = 1;
+ }
+
+ for (int i = 0; i < row; i++) {
+ obstacle_list[0][i] = 1;
+ obstacle_list[row - 1][i] = 1;
+ }
+
+ obstacle_list[1][5] = 1;
+ obstacle_list[2][5] = 1;
+ //obstacle_list[3][5] = 1;
+ obstacle_list[4][5] = 1;
+ //obstacle_list[5][5] = 1;
+
+ obstacle_list[5][0] = 1;
+ obstacle_list[5][1] = 1;
+ obstacle_list[5][2] = 1;
+ obstacle_list[5][3] = 1;
+ obstacle_list[5][4] = 1;
+
+ obstacle_list[7][6] = 1;
+ obstacle_list[7][4] = 1;
+ obstacle_list[7][5] = 1;
+ obstacle_list[7][6] = 1;
+ obstacle_list[7][7] = 1;
+ obstacle_list[7][8] = 1;
+}
+
+
+/************************************************************************************************************
+Essential function for calculating the distance
+************************************************************************************************************/
+static void calc_F(position a, position b, position current) {
+
+ position adjacent = current;
+ position corner1 = { 0 }, corner2 = { 0 };
+ uint16_t numb = 2;
+ for (int i = 0; i < 8; i++) {
+ switch (i)
+ {
+ case 0: adjacent.x = current.x - 1; adjacent.y = current.y; // top
+ break;
+ case 1: adjacent.x = current.x + 1; adjacent.y = current.y; // bottom
+ break;
+ case 2: adjacent.x = current.x; adjacent.y = current.y - 1; // left
+ break;
+ case 3: adjacent.x = current.x; adjacent.y = current.y + 1; // right
+ break;
+ case 4: adjacent.x = current.x - 1; adjacent.y = current.y - 1; numb = 3; // top left
+ corner1.x = current.x - 1; corner1.y = current.y;
+ corner2.x = current.x; corner2.y = current.y - 1;
+ break;
+ case 5: adjacent.x = current.x - 1; adjacent.y = current.y + 1; // top right
+ corner2.y = current.y + 1;
+ break;
+ case 6: adjacent.x = current.x + 1; adjacent.y = current.y + 1; // bottom right
+ corner1.x = current.x + 1;
+ break;
+ case 7: adjacent.x = current.x + 1; adjacent.y = current.y - 1; // bottom left
+ corner2.y = current.y - 1;
+ break;
+ default:
+ printf("Fatal Error, unknown position");
+ break;
+ }
+ if (obstacle_list[adjacent.x][adjacent.y] == 0 && ((obstacle_list[corner1.x][corner1.y] == 0 && obstacle_list[corner2.x][corner2.y] == 0) ||
+ (corner1.x == 0 && corner1.y == 0 && corner2.x == 0 && corner2.y == 0))) {
+ if (open_list[adjacent.x][adjacent.y] != 65535) { // if this adjacent square is already in the closed list ignore it
+ uint16_t g_value = g_list[current.x][current.y] + numb;
+ position diff;
+ diff.x = abs(adjacent.x - b.x);
+ diff.y = abs(adjacent.y - b.y);
+ uint8_t diagonal;
+ if (diff.x > diff.y) {
+ diagonal = diff.x - (diff.x - diff.y);
+ }
+ else {
+ diagonal = diff.y - (diff.y - diff.x);
+ }
+ if (open_list[adjacent.x][adjacent.y] == 0) { // if its not in the open list
+ open_list_count += 1;
+ g_list[adjacent.x][adjacent.y] = g_value; // compute its score, set the parent
+ open_list[adjacent.x][adjacent.y] = g_value + (diff.x * 2 + diff.y * 2 - diagonal);
+ }
+ else { // if its already in the open list
+ if (g_value < g_list[adjacent.x][adjacent.y]) { // test if new path is better
+ g_list[adjacent.x][adjacent.y] = g_value;
+ open_list[adjacent.x][adjacent.y] = g_list[adjacent.x][adjacent.y] + (diff.x * 2 + diff.y * 2 - diagonal);
+ }
+ }
+ }
+ }
+ }
+}
+
+/************************************************************************************************************
+Essential function for getting the next position
+************************************************************************************************************/
+static position openList_lowest_F() {
+ uint16_t lowest = row * col;
+ position lowest_pos;
+ for (uint8_t i = 0; i < row; i++) {
+ for (uint8_t j = 0; j < col; j++) {
+ if (open_list[i][j] > 1 && open_list[i][j] < lowest && open_list[i][j] != 65535) {
+ lowest_pos.x = i;
+ lowest_pos.y = j;
+ lowest = open_list[i][j];
+ }
+ }
+ }
+ return lowest_pos;
+}
+
+/************************************************************************************************************
+Essential function for getting the next position
+************************************************************************************************************/
+static void mapp_path(position b) {
+ // write path to walkpath array
+
+ position x = b; // lower value
+ position y = b; // higher value
+ counter = 0;
+ walkpath[0] = y;
+ printf("(%d | %d) \n", walkpath[0].x, walkpath[0].y);
+ while (g_list[y.x][y.y] > 1) {
+ counter += 1;
+ position s = b;
+ for (int i = 0; i < 8; i++) {
+ switch (i)
+ {
+ case 0: x.x = y.x - 1; x.y = y.y;
+ break;
+ case 1: x.x = y.x + 1; x.y = y.y;
+ break;
+ case 2: x.x = y.x; x.y = y.y - 1;
+ break;
+ case 3: x.x = y.x; x.y = y.y + 1;
+ break;
+ case 4: x.x = y.x - 1; x.y = y.y - 1;
+ break;
+ case 5: x.x = y.x - 1; x.y = y.y + 1;
+ break;
+ case 6: x.x = y.x + 1; x.y = y.y - 1;
+ break;
+ case 7: x.x = y.x + 1; x.y = y.y + 1;
+ break;
+ default:
+ printf("Fatal Error, unknown position");
+ break;
+ }
+ if (g_list[x.x][x.y] < g_list[y.x][y.y] && g_list[x.x][x.y] != 0 && g_list[s.x][s.y] > g_list[x.x][x.y]) {
+ s = x;
+ }
+ }
+ y = s;
+ walkpath[counter].x = y.x;
+ walkpath[counter].y = y.y;
+ printf("(%d | %d) \n", walkpath[counter].x, walkpath[counter].y);
+
+ }
+}
+
+