Te he hecho un programita en c++ que calcula el camino más corto entre dos celdas de un array.
Tienes obstaculos que se representan con un 1.
La función "getShortestPath" es la que lo hace todo y en el main lo que se hace es probar un ejemplo.
Código
#include<iostream> #include<vector> #include<queue> using namespace std; class Cell{ int x; int y; public: int getx(){return x;} int gety(){return y;} void setx(int x){this->x=x;} void sety(int y){this->y=y;} bool operator==(Cell o){return x==o.x && y==o.y;} Cell operator=(Cell o){x=o.x; y=o.y; return *this;} Cell(int x,int y):x(x),y(y){} Cell():x(0),y(0){} }; vector<Cell> getShortestPath(Cell ori, Cell dest, int array[], int width, int height); int main(){ int ejemplo[]={ 0,1,0,1,0,0,0,0, //0: void 0,1,0,1,0,0,0,0, //1: obstacle 0,1,0,1,0,0,0,0, 0,1,0,1,0,0,0,0, 0,1,0,1,0,0,0,0, 0,1,0,1,0,0,0,0, 0,0,0,1,0,0,0,0, 0,0,0,0,0,0,0,0}; vector<Cell> camino= getShortestPath(Cell(0,0),Cell(7,0),ejemplo,8,8); for(int i=0;i<camino.size();i++){ cout<<"("<<camino[i].getx()<<", "<<camino[i].gety()<<")"<<endl; } } vector<Cell> getShortestPath(Cell ori, Cell dest, int array[], int width, int height){ if(ori==dest) return vector<Cell>(); unsigned int *sizes=new unsigned int[width*height]; Cell *prev=new Cell[width*height]; for(int i=0;i<width*height;i++){sizes[i]=-1; prev[i]=Cell(-1,-1);} sizes[ori.getx()+ori.gety()*width]=0; prev[ori.getx()+ori.gety()*width]=ori; queue<Cell> porVisitar; porVisitar.push(ori); while(!porVisitar.empty()){ Cell cur=porVisitar.front(); porVisitar.pop(); cout<<porVisitar.size()<<endl; for(int i=-1;i<2;i++) for(int j=-1;j<2;j++) if( (cur.getx()+j)>=0 && (cur.getx()+j)<width && (cur.gety()+i)>=0 && (cur.gety()+i)<height && //is not out of bounds array[(cur.getx()+j)+(cur.gety()+i)*width]==0 && //is not an obstable sizes[cur.getx()+cur.gety()*width]+1 < sizes[(cur.getx()+j)+(cur.gety()+i)*width] //there is not a better path ){ sizes[(cur.getx()+j)+(cur.gety()+i)*width]=sizes[cur.getx()+cur.gety()*width]+1; prev[(cur.getx()+j)+(cur.gety()+i)*width]=Cell(cur.getx(),cur.gety()); porVisitar.push(Cell(cur.getx()+j,cur.gety()+i)); } } if(prev[dest.getx()+dest.gety()*width]==Cell(-1,-1)) return vector<Cell>(); Cell pp=dest; vector<Cell> res(sizes[dest.getx()+dest.gety()*width]+1); for(int i=res.size()-1; !(pp==ori); i--){ res[i]=pp; pp=prev[pp.getx()+pp.gety()*width]; } return res; }
Lo que se hace es considerar que:
1) Toda celda del array es un nodo.
2) Todas las celdas estan conectadas a sus vecinas.
3) Todas las aristas son bidireccionales.
4) Todas las aristas miden lo mismo: 1.
Y con esto lo que se hace es implementar dijkstra en un caso particular más sencillo.
Te pongo un pastebin para que puedas copiar y pegar:
http://pastebin.com/unRpQgK5