www.xbdev.net xbdev - software development
Monday December 10, 2018
home | about | contact | Donations

     
 

The Maths of 3D

You can't have 3D without a little maths...

 

A* PathFinding Algorithm

 

 

Have you ever wondered how you can make the ghosts in Pac-man, smart enough to find the shortest route to Pac-man and not go down any paths that are dead ends etc?  Well one way is to use the A* Algorithm.....pronounced.. "A-Star".  There is a load of stuff on the internet on it - and is used in numerous 3D and 2D games because of its effectiveness and simplistic nature.

 

Now some of the C code that I've put together to implement the A* Demo's, uses a bubble sort to find values with the lowest value....while again this works...its not always the most optimised... but you can always modify the code if you prefer :)  I've tried to keep the tutorial as simple as possible...but would be grateful for any feedback.

 

Tinny Bit of Theory First!

 

If we assume a 2D grid...or map it might be called for starters...moving onto 3D sometime in the future, where we would only move in the x-z axis in 3D.

 

We have a starting point and an ending point!  Our mission is to find the shortest path from the start to the goal without to much overhead.  Of course, the problem is tricky because we have obsticles in our way...which you can see I've added to the image on the right....

A quick sketch always helps to visualise things.

 

But there is a formula!... Yup an equation to help us on our journey...it is:

 

F = G + H

where G - is the movement cost from the starting point to the current square

H - cost of movement from the current square to the goal...usually an estimate

 

F is used to choose our next square...usually deciding which of the 8 surrounding squares has the lowest F, is the one we'll choose.

 

 

So lets make each square a node...and of course each node has 8 possible choices for the next node. 

 

 

 

From node to node we can calculate our way... but we must keep track of dead ends and record only the shortest path.  This is where list's come into it.... a list is just an array of data, and we use two of them.  Of of them is called the closed list, and the other the open list.  The open list will eventually contain our shortest path from the start to the end, and the closed list will keep track of where we have been, so while the algorithm is running it doesn't go down the same wrong path again.

 

 

Putting a Code Example Together

Hmmm....well it all seems simple in theory doesn't it... but when it sometimes come's to coding it, its not always as easy as that.  Well hopefully my demo C code will clarify things and give you a starting point from which you can build a more robust and flexible A* Engine that will plug into anything.

 

For the two lists, I decided to use the STL library's....but there's nothing stopping you changing the code so that it uses a large allocated space of memory for you own custom array.  I just decided to use the STL library's so it looks simplier.

 

I start with a text file (map.txt) which contains a 10x10 grid of characters.... '0' for space... '1' for wall....'2' for start and of course '3' for the goal.  Its only a simple dos application....I tried to make it as simple as possible...as understanding is the biggest concern here.  Later I'll add a GUI windows version that you can see it travel its journey....and later on again a Pac-Man game, so you can see how good those ghosts are at getting to you :)

 

Txt File: map.txt

 

A word to the wiser..if your worried about the length of the code don't be... it looks long...but if you break it up, you'll be okay....for example the ReadMap() function does what the name says, and only reads the data from the txt file into a global array called map[10][10]....

The program code starts and finishes from the function main()....and most of the function names are pretty descriptive...I would recommend printing a copy out and doing some scribbling on the paper if you get stuck.

 

Again if you have any serious problems, just give me a shout, and I'll be glad to give you a hand if I can.

 

DownloadSourceCode

 

#include <stdio.h>   // used for reading in the txt file and for printf output

#include <assert.h>  // some debugging code extra's

#include <math.h>    // for sqrt function

#include <vector>    // STL for our two arrays.

using namespace std;

 

// Our data from our text file - map.txt, is stored in this array.

char map[10][10];

 

 

void ReadMap()

{

      FILE* fp = fopen("map.txt", "r");

      char c;

      int i=0, j=0;

      while( fread(&c, 1, 1, fp) != EOF )

      {

            if( (c != ' ') && (c!='\n') ) // skip white spaces

            {

                  map[i][j]=c;

                  j++;

            }

            if( j== 10 && i==9 ) break;

            if( j== 10 ){ j=0; i++;}

 

      }// End while loop

      fclose(fp);

}// End of ReadMap()

 

// Used for debugging, to check taht we have read in the map

// correctly - else its unused.

void PrintMap()

{

      printf("\n");

      for(int y=0; y<10; y++)

      {

            for(int x=0; x<10; x++)

            {

                  printf("%c ", map[y][x] );

            }// End inner for loop

            printf("\n");

      }// End outer for loop

 

}// End of PrintMap()

 

struct node

{

      node* parent;

      int x, y;

      char c;

      float f, g, h;

 

      node()

      {

            parent=NULL;

      }

};

 

 

char GetPosValue(int x, int y)

{

      // top, top_right, right, bottom_right, bottom, left_bottom, left, top_left

      if( y < 0 ) return '1'; // outside the map, hence return wall;

      if( y > 9 ) return '1';

      if( x < 0 ) return '1';

      if( x > 9 ) return '1';

 

      return map[y][x];

}// end of closed

 

 

 

void FindEndPoint(int* xend, int* yend)

{

      *xend = 0;

      *yend = 0;

      // Find our end point -3- in this case

      for(int y=0; y<10; y++)

      {

            for(int x=0; x<10; x++)

            {

                  if( map[y][x] == '3')

                  {

                        *xend = x;

                        *yend = y;

                        break;

                  }

            }// End inner for loop

      }// End of outer for loop

 

}// End of FindEndPoint()

 

float DistanceToTarget(int xpoint, int ypoint,

                                 int xend,   int yend)

{

      float dist = (float)((xend - xpoint)*(xend - xpoint) + (yend - ypoint)*(yend - ypoint));

      dist = (float)sqrtf((float)dist);

 

      return dist;

}// End of DistanceToTarget()

 

// It takes the parent node, and an array of 8 nodes e.g. n[8]...

// it then fills in the values for the 8 surounding nodes.

void GenerateSuccessors(node* n, node* p, int xend, int yend)

{

      int x   = p->x;

      int y   = p->y;

      float g = p->g;

 

      /*

      (top_left)     (top)      (top_right)

      (left)          (p)           (right)

      (bottom_left) (bottom) (bottom_right)

      */

 

      n[0].x = x;

      n[0].y = y-1;

      n[0].c = GetPosValue(x,   y-1); // top

      n[0].parent = p;

      n[0].g = g + 1;

      int k = 0;

      n[0].h = DistanceToTarget(n[k].x, n[k].y,    xend,yend);

      n[0].f = n[k].g + n[k].h;

 

      n[1].c = GetPosValue(x+1, y-1); // top_right

      n[1].x = x+1;

      n[1].y = y-1;

      n[1].parent = p;

      n[1].g = g + sqrtf(2.0f);//0.5f;

      k = 1;

      n[1].h = DistanceToTarget(n[k].x, n[k].y,   xend,yend);

      n[1].f = n[k].g + n[k].h;

 

      n[2].c = GetPosValue(x+1, y);   // right

      n[2].x = x+1;

      n[2].y = y;

      n[2].parent = p;

      n[2].g = g + 1;

      k = 2;

      n[2].h = DistanceToTarget(n[k].x, n[k].y,   xend,yend);

      n[2].f = n[k].g + n[k].h;

 

      n[3].c = GetPosValue(x+1,   y+1); // bottom_right

      n[3].x = x+1;

      n[3].y = y+1;

      n[3].parent = p;

      n[3].g = g + sqrtf(2.0f);;

      k = 3;

      n[3].h = DistanceToTarget(n[k].x, n[k].y,   xend,yend);

      n[3].f = n[k].g + n[k].h;

 

      n[4].c = GetPosValue(x, y+1);   // bottom

      n[4].x = x;

      n[4].y = y+1;

      n[4].parent = p;

      n[4].g = g + 1;

      k = 4;

      n[4].h = DistanceToTarget(n[k].x, n[k].y,   xend,yend);

      n[4].f = n[k].g + n[k].h;

 

      n[5].c = GetPosValue(x-1, y+1); // bottom_left

      n[5].x = x-1;

      n[5].y = y+1;

      n[5].parent = p;

      n[5].g = g + sqrtf(2.0f);;

      k = 5;

      n[5].h = DistanceToTarget(n[k].x, n[k].y,   xend,yend);

      n[5].f = n[k].g + n[k].h;

 

      n[6].c = GetPosValue(x-1, y);   // left

      n[6].x = x-1;

      n[6].y = y;

      n[6].parent = p;

      n[6].g = g + 1;

      k = 6;

      n[6].h = DistanceToTarget(n[k].x, n[k].y,   xend,yend);

      n[6].f = n[k].g + n[k].h;

 

      n[7].c = GetPosValue(x-1, y-1); // top_left

      n[7].x = x-1;

      n[7].y = y-1;

      n[7].parent = p;

      n[7].g = g + sqrtf(2.0f);;

      k = 7;

      n[7].h = DistanceToTarget(n[k].x, n[k].y,   xend,yend);

      n[7].f = n[k].g + n[k].h;

 

}// End of GenerateSuccessors(..)

 

 

int GetNodeWithLeastDist(node* n)

{

      // Find the first node, which isn't a wall, e.g. not '1'

      int sel=0;

      while( n[sel].c != '0' ) sel++;

 

      // Possible bug, so I'll put an assert here - incase we silly enough

      // put all walls around our starting point.

      assert( !(sel >= 8) && ("Error no walls in GetNodeWithLeastDist(..)") );

 

      // Now compare our value with the others, of course checking

      // that its not a wall.

      for( int i=1; i<8; i++)

      {

            if( (n[i].f < n[sel].f) && (n[i].c == '0') )

            {

                  sel = i;

            }

 

      }// End for loop

      return sel;

}// End of GetNodeWithLeastDist(..)

 

// You could probably fix the one line that needs this function, but it

// makes the program easier to follow - its only got one line different

// from the LeastDist version above.

int GetNodeWithLargestDist(node* n)

{

      // Find the first node, which isn't a wall, e.g. not '1'

      int sel=0;

      while( n[sel].c != '0' ) sel++;

 

      // Now compare our value with the others, of course checking

      // that its not a wall.

      for( int i=1; i<8; i++)

      {

            if( (n[i].f > n[sel].f) && (n[i].c == '0') )

            {

                  sel = i;

            }

 

      }// End for loop

      return sel;

}// End of GetNodeWithLargestDist(..)

 

 

// Go through, or read in map of characters, and find the starting

// position, which in this case is a '2'

void GetStartPos(int* xstart, int* ystart)

{

      // Find our starting point -2- in this case

      for(int y=0; y<10; y++)

      {

            for(int x=0; x<10; x++)

            {

                  if( map[y][x] == '2')

                  {

                        *xstart = x;

                        *ystart = y;

                        break;

                  }

            }// End inner for loop

      }// End of outer for loop

 

}// End of GetStartPos(..)

 

// Determines if the passed node is in the closed list, true

// for yes...and false for no.

bool inClosedList(vector<node>* closedlist, node n)

{

      int count = (int)closedlist->size();

     

      for (vector<node>::iterator nn = closedlist->begin(); nn != closedlist->end(); nn++)

    {

        if( (nn->x == n.x)&&(nn->y == n.y) )

            {

                  return true;                    

            }// end if

    }// end for loop nn

 

      return false; // not in our closedlist

}// End inClosedList(..)

 

// Returns true, if the node we have passed in, is at our goal

// position - hence success!

bool CheckForSuccess(node* n, int* iWhich)

{

      for( int i=0; i<8; i++ )

      {

            if( n[i].c == '3' ) // reached goal

            {

                  *iWhich = i;

                  return true;

            }

      }// end for loop

      return false;

}// End of CheckForSuccess(..)

 

 

// program entry point

void main()

{

 

      // Read in our 10x10 map from a text file

      ReadMap();

 

      // Debug - print our data to the screen, just to make sure its read

      // in correctly.

      //PrintMap();

 

      int xstart=0;

      int ystart=0;

      node startnode;

 

      GetStartPos(&xstart, &ystart);

 

      int xend, yend;

      FindEndPoint(&xend, &yend);

 

      vector<node> openlist;

      vector<node> closelist;

 

 

 

      startnode.x = xstart;

      startnode.y = ystart;

      startnode.c = 2;

      startnode.f = 0;

      startnode.g = DistanceToTarget(xstart, ystart,  xend,yend);

 

 

      openlist.push_back(startnode);

     

      bool bFound = false;

 

      int s = (int)openlist.size();

      while( (s > 0) && (bFound==false) )

      {

            int h = (int)openlist.size();

            node q = openlist[h-1];

 

            // Generate the 8 successors for the start node, and fill

            // in there values

            node n[8];

            GenerateSuccessors(n, &q, xend,yend);

 

 

            // Check if any of the successors have reached the goal

            int iWhich = 0;

            bool success = CheckForSuccess(n, &iWhich);

            if( success == true )

            {

                  closelist.push_back(q);

                  closelist.push_back(n[iWhich]);

                  openlist.push_back(n[iWhich]);

                  bFound= true;

                  break;

            }

           

            // Loop through each node, and pick the node thats the least

            // distance and is not a wall ...and of course a node we havn't

            // been down before, which is stored in close list.

            bool bPicked = false;

            for(int i=0; i<8; i++)

            {

 

                  // returns the node with the least distance, and doesn't

                  // return a wall!..checks that its not a '1'

                  int sel = GetNodeWithLeastDist(n);

                 

 

                  //Check if a node with this position is in the closed list

                  bool bCheck = inClosedList(&closelist, n[sel]);

                  if( bCheck )

                  {

                        // Sort of a hack fix, so that the next time round, we don't just

                        // pick the same node with the lowest value, instead we make this one

                        // the largest.

                        n[sel].f = n[GetNodeWithLargestDist(n)].f+1;

                        continue;

                  }

                  else

                  {

                        openlist.push_back( n[sel] );

                        bPicked = true;

                        //Debug - uncomment line to see which nodes are picked as the

                        //program itterates along.

                        //printf("node: %d %d\n", n[sel].x, n[sel].y);

                        break;

                  }

            }// End for loop

 

            if( bPicked==false )

            {

                  openlist.pop_back();

                  //openlist.erase( openlist.begin() + 0 );

            }

 

            closelist.push_back(q);

            s = (int)openlist.size();

      }// End of while loop

 

      // Displays the most optimal path from start to end.

      printf("\n\nOpenList (x,y coords of shortest path)\n");

      for(unsigned int i=0; i< openlist.size(); i++)

      {

            printf("node: %d %d\n", openlist[i].x, openlist[i].y);

      }

     

     

      // Displays the complete path that was searched, step by step

      printf("\n\nCloseList (x,y of how we got our optimised path)\n");

      for(unsigned int i=0; i< closelist.size(); i++)

      {

            printf("node: %d %d\n", closelist[i].x, closelist[i].y);

      }

 

     

}// End main()

 

 

Output:
OpenList (x,y coords of shortest path)
node: 1 1
node: 2 2
node: 3 2
node: 2 3
node: 3 4
node: 4 5
node: 5 5
node: 6 6
node: 7 6
node: 8 6
node: 9 7


CloseList (x,y of how we got our optimised path)
node: 1 1
node: 2 2
node: 3 2
node: 4 2
node: 5 2
node: 4 2
node: 3 2
node: 2 3
node: 3 4
node: 4 5
node: 5 5
node: 6 6
node: 7 6
node: 8 6
node: 9 7

 

 

Wow...a lot of code.... but it's a cool ability to find the shortest path...or even a path from a start point to an end point....using closelist array for example.... as sometimes you don't want your enemy to come straight for you...you could have it slowly work its way towards you....but you want it to get there in the end. (Pac-man basics soon).

 

Notice how you can move in any of 8 directions...top, top_right, right, bottom_right, bottom, bottom_left, left, top_left......will very little work you can change this to only up, left,right and down...4 possible directions...just remember the A* Algorithm is very flexible as long as you understand it.

 

One thing I think is worth mentioning...is how far to go before stopping.....a good solution, would be to check the size of the CloseList or OpenList array...and if it reaches a certain size, then to stop....maybe have your character...or object move to there before doing the calculation again etc.... always a good idea.

 

Optimisations are always important in games....some ideas off the top of my head on improving the speed of the code, would be to use a large allocated block of memory for the arrays...as its slightly faster than linked lists...all that allocating and deleting.  Also maybe use the build in sort algorithms in the STL librarys....or write improved ones for searching for the smallest F value.... there's loads of little tweaks you can work on.

 

 

Well I think the next step is to put our A* code into another file....a_star.cpp/.h I think and implement a graphical implementation so you can drewl over just how cool it really is.

 

 

 

 

 

 

 

 

 

 
 Visitor: 9534626  { 209.237.238.175 } Copyright (c) 2002-2017 xbdev.net - All rights reserved.
Designated tutorial and software are the property of their respective owners.