1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167 168 169 170 171 172 173 174 175 176 177 178 179 180 181
|
#include <iostream>
#include <algorithm>
#include <vector>
#include <math.h>
#include <stdlib.h>
using namespace std;
int main(){
try{
Coord* g=createGraph(60,60);//retourne un pointeur vers un tableau de Coord(noeuds)
list<Coord> path=findPath(g,5,50,55,25);//retourne une liste de Coord(Noeuds) qui menent du point de depart à la fin
//trace path
}
catch(int i){
if(i==1){
system.log("path not found");
}
}
return 0;
}
struct Coord{
int x,y;
Node* element;
bool operator==(const Coord& lhs, const Coord& rhs){
return lhs.x==rhs.x&&lhs.y==rhs.y;
}
};
class Node{
private:
int closed;
int priority;//length from start to this+estimate length to end
int lengthFromStart;
int estimateToEnd;
Coord come_from;
public:
Node(){
closed=0;
priority=0;//length from start to this+estimate length to end
lengthFromStart=0;
estimateToEnd=0;
come_from=NULL;
};
void setComeFrom(Coord c){
this->come_from=c;
}
void setPriority(){
//to do
}
void setLengthFromStart(){
//to do
}
void setEstimateToEnd(){
//to do
}
Coord getComeFrom(){
return come_from;
}
int getPriority(){
return priority;
}
int getLengthFromStart(){
return lengthFromStart;
}
int getEstimateToEnd(){
return estimateToEnd;
}
};
Coord* createGraph(int width, int heigth){//retourn un tableau 2d de Coord
Coord* g=new Coord[][];
//Aucun obstacle pour linstant
for(int i=0;i<height;i++){
for(int j=0;j<width;j++){
Coord c;
c.x=j;
c.y=i;
c.element=new Node();
g[i][j]=c;
}
}
return g;
}
list<Coord> findPath(Coord* g,int xstart,int ystart,int xend,int yend){//g est le tableau de coord(struct) contenant les objets nodes et la position
list<coord> open_nodes,closed_nodes;
int xs=xstart;
int ys=ystart;
int xe=xend;
int ye=yend;
Coord start=g[xs][ys];
open_nodes.push_back(start);
while(open_nodes.empty()==false){
Coord current=find_maxPriority(open_nodes);//current est le noeud ds open_node avec la meilleure estimation pour arriver à end node
if (current.x==xe && current.y==ye){
//reconstruct path
list<Coord> path;
Coord curr=current;
while(curr.element.getComeFrom()!=NULL){
curr=current.element.getComeFrom();
path.push_back(curr);
}
return path;
}
open_nodes.remove(current);
closed_nodes.push_back(current);
list<Coord> neighbors=find_neighbors(g,current);
//voir ts les voisins et updater priority lengthFromStart estimateToEnd etc...
for (list<Coord>::iterator it = neighbors.begin(); it != neighbors.end(); it++){
auto closed=find(it.begin(), it.end(), closed_nodes);
if (closed == it.end())//si le voisin est deja ds closed nodes on fait rien
continue;
//calculer lengthFromStart
int priority=it.element.getLengthFromStart()+estimateTime(it.x,it.y,xe,ye);
auto open=find(it.begin(), it.end(), open_nodes);
if (open==it.end() || priority<current.element.getPriority()){//si neighbor n'est pas ds open_node ou si la nouv priorité<ancienne priorité du neighbor
it.element.setComeFrom(current);//on modifie le "parent" du neighbor
if(it.x==current.x || it.y==current.y){//assignation de la nouvelle longueur depuis depart:+10 si horiz ou vert
it.element.setLengthFromStart(current.getLengthFromStart()+10);
it.setPriority(it.getLengthFromStart()+estimateTime(it.x,it.y,xe,ye));
}else{//+14 si diagonal
it.element.setLengthFromStart(current.getLengthFromStart()+14);
it.setPriority(it.getLengthFromStart()+estimateTime(it.x,it.y,xe,ye));
}
if(open==it.end()){//si it n'est pas ds open_node, on lajoute
open_nodes.push_back(it);
}
}
}
}
int notFound=1;
throw notFound;
}
int estimateTime(int x1, int y1,int x2,int y2){
int width=math.abs(x1-x2);
int height=math.abs(y1-y2);
int hyp=math.sqrt(math.pow(width,2.0),math.pow(height,2.0));
return hyp;
}
Coord find_maxPriority(list<Coord>& liste){
Coord priority;
list<Coord>::const_iterator iterator;
int cpt=0;
for (iterator = liste.begin(); iterator != liste.end(); ++iterator) {
if(cpt==0){
priority=iterator;
cpt++;
}else{
if(iterator.element.getPriority()<priority.element.getPriority()){
priority=iterator;
}
}
}
return priority;
}
list<Coord> find_neighbors(Coord* g, Coord& current){
list<Coord> liste;
int x=current.x;
int y=current.y;
liste.push_back(g[x+1][y]);
liste.push_back(g[x+1][y+1]);
liste.push_back(g[x][y+1]);
liste.push_back(g[x-1][y+1]);
liste.push_back(g[x-1][y]);
liste.push_back(g[x-1][y-1]);
liste.push_back(g[x][y-1]);
liste.push_back(g[x+1][y-1]);
return liste;
}
|