File content as of revision 18:a82994e67297:

/**
* 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);

    }
}