I'm working on a simple pathfinder. I've a small grid-based map.
'1' is a floor
'0' is a wall
'G' is one of the goals
Features:
- there isn't diagonal movement
- path cost always equals 1
- there can be many goals
My full code:
#include <vector>
#include <iostream>
#include <map>
#include <algorithm>
using namespace std;
const int MapWidth = 10, MapHeight = 10;
// Y pos X pos
char Map[MapHeight][MapWidth] = {
{ '1', '1', 'G', '1', '1', '1', '1', '1', '1', '1' },
{ '1', '1', '1', '1', '1', '1', '1', '1', '1', '1' },
{ '1', '1', '0', '1', '1', '1', '1', '1', '0', 'G' },
{ '1', '1', '0', '0', '1', '1', '1', '1', '0', '1' },
{ '1', '1', '1', '1', '1', '1', '1', '1', '1', '1' },
{ '1', '1', '1', '1', '1', '1', '1', '0', '0', '1' },
{ '1', '1', '1', '1', '1', '1', '1', '0', '1', '1' },
{ '1', '1', '1', '0', '0', '1', '1', '0', '1', '1' },
{ '1', '1', '0', '0', '0', '1', '1', '0', '0', '0' },
{ '1', '1', '1', 'G', '0', '1', '1', '1', '1', '1' }
};
struct Pos
{
int X;
int Y;
};
bool PosIsUsed(vector <Pos> Position, int Y, int X)
{
for (vector <Pos>::iterator it = Position.begin(); it != Position.end(); ++it)
{
if (it->X == X && it->Y == Y)
{
return true;
}
}
return false;
}
void SearchNext(int Y, int X, vector <Pos> Position)
{
Pos NewStep = { X, Y };
Position.push_back(NewStep);
//North
if (Y >= 1 && !PosIsUsed(Position, Y - 1, X))
{
vector <Pos> NewPosition = Position;
switch (Map[Y - 1][X])
{
case '1':
SearchNext(Y - 1, X, NewPosition);
break;
case 'G':
printf("Found!\n");
break;
}
}
//East
if (X + 1 < MapWidth && !PosIsUsed(Position, Y, X + 1))
{
vector <Pos> NewPosition = Position;
switch (Map[Y][X + 1])
{
case '1':
SearchNext(Y, X + 1, NewPosition);
break;
case 'G':
printf("Found!\n");
break;
}
}
//Soth
if (Y + 1 < MapHeight && !PosIsUsed(Position, Y + 1, X))
{
vector <Pos> NewPosition = Position;
switch (Map[Y + 1][X])
{
case '1':
SearchNext(Y + 1, X, NewPosition);
break;
case 'G':
printf("Found!\n");
break;
}
}
//West
if (X >= 1 && !PosIsUsed(Position, Y, X - 1))
{
vector <Pos> NewPosition = Position;
switch (Map[Y][X - 1])
{
case '1':
SearchNext(Y, X - 1, NewPosition);
break;
case 'G':
printf("Found!\n");
break;
}
}
}
int main()
{
vector <Pos> Pos;
SearchNext(9, 9, Pos); //We start here (Pos [9][9])
printf("End\n");
system("PAUSE");
return 0;
}
There's a problem. It just never ends. Could you tell me, what I'm doing wrong?
I switched X and Y params order in SearchNext function. After this change, program displays the infinity "Found!".
Working code (not mine):
#include <vector>
#include <iostream>
#include <map>
#include <algorithm>
#include <string>
using namespace std;
const char FLOOR = '1' ;
const char WALL = '0' ;
const char GOAL = 'G' ;
const int MapWidth = 10, MapHeight = 10;
// Y pos X pos
char Map[MapHeight][MapWidth] = {
{ '1', '1', 'G', '1', '1', '1', '1', '1', '1', '1' },
{ '1', '1', '1', '1', '1', '1', '1', '1', '1', '1' },
{ '1', '1', '0', '1', '1', '1', '1', '1', '0', 'G' },
{ '1', '1', '0', '0', '1', '1', '1', '1', '0', '1' },
{ '1', '1', '1', '1', '1', '1', '1', '1', '1', '1' },
{ '1', '1', '1', '1', '1', '1', '1', '0', '0', '1' },
{ '1', '1', '1', '1', '1', '1', '1', '0', '1', '1' },
{ '1', '1', '1', '0', '0', '1', '1', '0', '1', '1' },
{ '1', '1', '0', '0', '0', '1', '1', '0', '0', '0' },
{ '1', '1', '1', 'G', '0', '1', '1', '1', '1', '1' }
};
struct Pos
{
short x,y;
Pos operator + ( Pos p ) const { return Pos(x+p.x,y+p.y); }
bool operator < ( Pos p ) const { return ( y==p.y ) ? (x<p.x) : (y<p.y) ; }
bool operator != ( Pos p ) const { return ( y!=p.y ) || (x!=p.x) ; }
Pos(short x=0,short y=0) : x(x), y(y) {}
};
bool valid(Pos p) { return p.x>=0 && p.x<MapWidth && p.y>=0 && p.y<MapHeight; }
enum Dir { d_beg, d_up=d_beg, d_rg, d_dn, d_lf, d_end };
Pos deltas[d_end] = { {0,-1}, {+1,0}, {0,+1}, {-1,0} };
Dir& operator ++ ( Dir& d ) { d = (Dir) ( 1+(int)d ) ; return d; }
Dir other(Dir d)
{
switch(d)
{
case d_up: return d_dn;
case d_rg: return d_lf;
case d_dn: return d_up;
case d_lf: return d_rg;
default: return d_end;
}
}
struct SearchMapItem
{
bool traversble;
bool goal;
bool visited;
int cost_here;
Dir came_from;
bool paths[d_end];
};
map<Pos,SearchMapItem> search_map;
typedef map<Pos,SearchMapItem>::iterator SMII;
void MakeMap()
{
search_map.clear();
Pos p;
for(p.y=0;p.y<MapHeight;++p.y) for(p.x=0;p.x<MapWidth;++p.x)
{
SearchMapItem smi;
smi.visited = false;
smi.cost_here = -1;
smi.came_from = d_end;
if( Map[p.y][p.x] == WALL )
{
smi.traversble = false;
}
else if( Map[p.y][p.x] == GOAL )
{
smi.traversble = true;
smi.goal = true;
}
else if( Map[p.y][p.x] == FLOOR )
{
smi.traversble = true;
smi.goal = false;
for( Dir d = d_beg; d != d_end; ++d )
{
Pos p2 = p + deltas[d];
smi.paths[d] = valid(p2) && (Map[p2.y][p2.x] != WALL) ;
}
}
search_map[p] = smi;
}
}
void FindGoalFrom( Pos start )
{
vector<SMII> found;
{
SMII smii = search_map.find(start);
if(smii==search_map.end()) { cout << "starting outside map\n"; return; }
if(smii->second.goal) { cout << "already at target\n"; return; }
if(!smii->second.traversble) { cout << "starting in a wall\n"; return; }
smii->second.visited = true;
smii->second.cost_here = 0;
found.push_back(smii);
}
int cost_so_far = 0;
bool did_find = false;
while(!did_find)
{
vector<SMII> candidates;
for( SMII smii : found )
{
for( Dir d = d_beg; d != d_end; ++d )
{
if( ! smii->second.paths[d] ) continue;
Pos p = smii->first + deltas[d];
if(!valid(p)) continue;
SMII cand = search_map.find(p);
if(cand==search_map.end()) continue;
if(cand->second.visited) continue;
cand->second.came_from=d;
candidates.push_back(cand);
}
}
++cost_so_far;
if( candidates.empty() ) break;
for( SMII smii : candidates )
{
smii->second.visited = true;
smii->second.cost_here = cost_so_far;
found.push_back(smii);
if( smii->second.goal ) { did_find = true; break; }
}
}
if( ! did_find ) { cout << "no goal reachable\n"; return; }
SMII pos = found.back();
vector<Dir> path;
while( pos->first != start )
{
Dir d = pos->second.came_from;
path.push_back(d);
Pos p = pos->first + deltas[ other(d) ];
pos = search_map.find(p);
}
for( auto itr = path.rbegin(); itr != path.rend(); ++itr )
{
const char* dir_names[] = { "Up", "Right", "Down", "Left" } ;
cout << "Walk " << dir_names[*itr] << endl;
}
cout << "Then you are at goal in total " << to_string(cost_so_far) << " steps" << endl;
}
int main()
{
MakeMap();
FindGoalFrom( Pos(5,9) );
cout << "\ndone\n";
}
Lets say there are at average 2.5 directions to choose from, then
your algorithm will take 2.5n steps to complete, where n is the
number of squares. That will be an incredible large number, even
for fairly small n. You need to look into Djikstra's algorithm
This is assuming you eventually want to find the cheapest route to
a goal, not just any route.