Autonomous Maze Solving Robot and Faulty Path Sorting

Question

Intro

I'm building a maze runner robot, whose goal is to be capable of navigating through a maze from a given starting point to a given ending point. I designed the algorithm myself - I insist on getting my own implementation working before looking at previous work.

Problem

Part of the algorithm involves sorting the directions(using insertion sort, because the array is so small) - forward, backward, up, and down in terms of how promising they are.

I'm having some problems with the sorting, which sorts directions based on three factors

Status: If the path in direction d was already explored?
Distance: The distance of the square at end of path p to the endpoint - the minimum of the x and y different
Length: How long the paths actually are.

When sorting, I to compare factor 1. If they are equal, I compare factor 2. If factor 2 is equal, I continue to 3. If any two factors are either less than or more than, I return that. For some reason, sometimes a path with a lower status gets pushed to the back. Something is going wrong with my sorting of the paths.

enter code here

Any ideas?

Code

    /*------------( Getting the best direction to go in )----------------    */
    /*
     * 0: North
     * 1: East
     * 2: South
     * 3: West
     */
    int  bestDirection(Point _curr_pos){
    Vec4 statuses/*1 for unexplored(better), 2 for explored(worse)*/,
        distances ,
        lengths = collectDistancesToWalls(),
        availablities = {POINT_REACHABLE, POINT_REACHABLE, POINT_REACHABLE, POINT_REACHABLE},
        directions = {0,1,2,3};

    //Give directions a length rating and distance rating and then sorts the directions, leaving the best direction in the front.

    //If we discover that we have already been in a location, we should block off the passage behind us leading to that location because it must be a loop.
    //Collecting the distance and length data. 
      for (int i=0; i < 4; i++) {
        Point direction_translated = translateOnAxisInDirection(_curr_pos, i, 1);

     //Converts the point to a reachable square - unnecessary to describe here. There is a domain specific problem which necessitates this function to be used. 
        Point heading_direction = pointToReachableSquare(direction_translated , i);

        //Distance from end of path to "headinglocation", the goal
        Point vec_to_end = subVec(heading_direction, headingLocation);
        distances[i]    =  min(absv(vec_to_end.x), absv(vec_to_end.y));
        statuses[i]     =  history[heading_direction.x][heading_direction.y].status;

        //If path is unreachable because of wall or something, then mark it as unreachable. 
        if (cmpVec(heading_direction, _curr_pos) || history[heading_direction.x][heading_direction.y].classification == WALL || !pointInIndBounds(direction_translated)) {
            availablities[i] = POINT_UNREACHABLE;
        }
    }

    //Insertion sort the distances.
    for (int i = 1; i < 4; i++) {
        int j = i - 1;
        while (
            comparePathOptions(
            statuses[i],
            distances[i],
            lengths[i],
            statuses[j],
            distances[j],
            lengths[j]
               ) == LESS_THAN && (j >= 0)) {
                int temp = directions[i];
                directions[i] = directions[j];
                directions[j] = temp;
                j--;
        }
    }

    //Return the first reachable direction. 
    int ind = 0;

    int dir = directions[ind];

    while (availablities[ directions[ind] ] == POINT_UNREACHABLE && (ind<4)) {
        dir = directions[ind+1];
        ind++;
    }

    return dir;
}

The comparison functions:

int relationship(int a, int b){
    if (a < b) return LESS_THAN;
    if (a > b) return MORE_THAN;
    return EQUAL;
}

//Edit this function
//TODO: Edit comparePathOptions.
//NOTE: Something
int comparePathOptions(int n_explored, int n_closeness, int n_length,
                 int b_explored, int b_closeness, int b_length){

    int objs[][3] = {
        {n_explored, n_closeness, n_length},
        {b_explored, b_closeness, b_length}
    };

    for (int i = 0; i < 3; i++){
        int rel = relationship(objs[1][i],objs[0][i]);
        if (rel!= EQUAL ) return rel;

    }
    return EQUAL;
}

Solved

Thanks to @Kittsil I've gotten the algorithm to work! Instead of accessing statuses, lengths, and distances by j and i, you do it by directions[i or j] because i and j stop referring to the current direction when directions array is changed.

The edited code:

   while ( (j >= 0) &&
        comparePathOptions(
        statuses[  directions[i] ],
        distances[ directions[i] ],
        lengths[   directions[i] ],
        statuses[  directions[j] ],
        distances[ directions[j] ],
        lengths[   directions[j] ]
           ) == MORE_THAN ) {
            int temp = directions[i];
            directions[i] = directions[j];
            directions[j] = temp;
            j--;
    }

And the solved maze:

x: 0, y: 0
H: 5, W:5, Ss:1
4|#####|
3|#####|
2|#####|
1|#####|
0|*::::|
  01234 
4|#####|
3|#####|
2|#####|
1|#####|
0| *:::|
  01234 
4|#####|
3|#####|
2|#####|
1|#####|
0|  *::|
  01234 
4|#####|
3|#####|
2|#####|
1|#####|
0|   *:|
  01234 
4|#####|
3|#####|
2|####:|
1|####:|
0|    *|
  01234 
4|#####|
3|#####|
2|####:|
1|####*|
0|     |
  01234 
4|#####|
3|#####|
2|::::*|
1|#### |
0|     |
  01234 
4|#####|
3|#####|
2|:::* |
1|#### |
0|     |
  01234 
4|#####|
3|#####|
2|::*  |
1|#### |
0|     |
  01234 
4|#####|
3|#####|
2|:*   |
1|#### |
0|     |
  01234 
4|:####|
3|:####|
2|*    |
1|#### |
0|     |
  01234 
4|:####|
3|*####|
2|     |
1|#### |
0|     |
  01234 
4|*####|
3| ####|
2|     |
1|#### |
0|     |
  01234

Autonomous Maze Solving Robot and Faulty Path Sorting

Intro

Problem

Code

Solved

Answers (1)

Related Questions