Romain Ame / Mbed 2 deprecated Robot2016_2-0_STATIC

Voili voilou

Dependencies:   RoboClaw StepperMotor mbed

Fork of Robot2016_2-0 by ARES

Map/Map.cpp@19:30942f018252, 2016-01-07 (annotated)

Committer:
IceTeam
Date:
Thu Jan 07 18:22:38 2016 +0000
Revision:
19:30942f018252
Parent:
12:5355aed288b0 
Une version toute pourrie, pleine de bug. Toute les verification d'accessibilit? avec getHeight ont ete mise en commentaire, il faudra les decommenter;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
IceTeam 11:9c70a7f4d7aa 1 #include "Map.h"
IceTeam 11:9c70a7f4d7aa 2
IceTeam 11:9c70a7f4d7aa 3 #include "Obs_circle.h"
IceTeam 11:9c70a7f4d7aa 4 #include "Obs_rect.h"
IceTeam 11:9c70a7f4d7aa 5
IceTeam 11:9c70a7f4d7aa 6 #ifdef CODEBLOCK
IceTeam 11:9c70a7f4d7aa 7 using namespace std;
IceTeam 11:9c70a7f4d7aa 8 #include <iostream>
IceTeam 11:9c70a7f4d7aa 9 #include <fstream>
IceTeam 11:9c70a7f4d7aa 10 #else
IceTeam 11:9c70a7f4d7aa 11 #include "mbed.h"
IceTeam 11:9c70a7f4d7aa 12 extern Serial logger;
IceTeam 19:30942f018252 13 extern Serial pc;
IceTeam 11:9c70a7f4d7aa 14 #endif
IceTeam 11:9c70a7f4d7aa 15 #include <math.h>
IceTeam 11:9c70a7f4d7aa 16
IceTeam 11:9c70a7f4d7aa 17 Map::Map()
IceTeam 11:9c70a7f4d7aa 18 {
IceTeam 11:9c70a7f4d7aa 19
IceTeam 11:9c70a7f4d7aa 20 }
IceTeam 11:9c70a7f4d7aa 21
IceTeam 11:9c70a7f4d7aa 22 Map::~Map()
IceTeam 11:9c70a7f4d7aa 23 {
IceTeam 11:9c70a7f4d7aa 24 for(unsigned int i=0;i<obstacles.size();i++)
IceTeam 11:9c70a7f4d7aa 25 delete obstacles[i];
IceTeam 11:9c70a7f4d7aa 26 }
IceTeam 11:9c70a7f4d7aa 27
IceTeam 11:9c70a7f4d7aa 28 void Map::build()
IceTeam 11:9c70a7f4d7aa 29 {
IceTeam 12:5355aed288b0 30 /*obstacles.push_back(new Obs_rect(ROBOTRADIUS,IDO_MG,-100,-100,2100,0));// MG
IceTeam 11:9c70a7f4d7aa 31 obstacles.push_back(new Obs_rect(ROBOTRADIUS,IDO_MB,2100,-100,2000,3100));// MB
IceTeam 11:9c70a7f4d7aa 32 obstacles.push_back(new Obs_rect(ROBOTRADIUS,IDO_MH,-100,-100,0,3100));// MH
IceTeam 11:9c70a7f4d7aa 33 obstacles.push_back(new Obs_rect(ROBOTRADIUS,IDO_MD,2100,3000,-100,3100));// MD
IceTeam 11:9c70a7f4d7aa 34
IceTeam 11:9c70a7f4d7aa 35
IceTeam 11:9c70a7f4d7aa 36 obstacles.push_back(new Obs_rect(ROBOTRADIUS,IDO_M1,778,0,800,400));// M1
IceTeam 11:9c70a7f4d7aa 37 obstacles.push_back(new Obs_rect(ROBOTRADIUS,IDO_M2,1200,0,1222,400));// M2
IceTeam 11:9c70a7f4d7aa 38 obstacles.push_back(new Obs_rect(ROBOTRADIUS,IDO_M3,800,0,1200,70));// M3
IceTeam 11:9c70a7f4d7aa 39 obstacles.push_back(new Obs_rect(ROBOTRADIUS,IDO_M4,1222,2600,1200,3000));// M4
IceTeam 11:9c70a7f4d7aa 40 obstacles.push_back(new Obs_rect(ROBOTRADIUS,IDO_M5,1200,2930,800,3000));// M5
IceTeam 11:9c70a7f4d7aa 41 obstacles.push_back(new Obs_rect(ROBOTRADIUS,IDO_M6,800,2600,778,3000));// M6
IceTeam 11:9c70a7f4d7aa 42
IceTeam 11:9c70a7f4d7aa 43 obstacles.push_back(new Obs_rect(ROBOTRADIUS,IDO_E,580,967,0,2033));// E
IceTeam 11:9c70a7f4d7aa 44 obstacles.push_back(new Obs_rect(ROBOTRADIUS,IDO_S,2000,1200,1900,1800));// S
IceTeam 11:9c70a7f4d7aa 45
IceTeam 11:9c70a7f4d7aa 46 obstacles.push_back(new Obs_rect(ROBOTRADIUS,IDO_D1,70,265,0,335));// D1
IceTeam 11:9c70a7f4d7aa 47 obstacles.push_back(new Obs_rect(ROBOTRADIUS,IDO_D2,70,565,0,636));// D2
IceTeam 11:9c70a7f4d7aa 48 obstacles.push_back(new Obs_rect(ROBOTRADIUS,IDO_D3,70,2365,0,2435));// D3
IceTeam 11:9c70a7f4d7aa 49 obstacles.push_back(new Obs_rect(ROBOTRADIUS,IDO_D4,70,2665,0,2735));// D4
IceTeam 11:9c70a7f4d7aa 50
IceTeam 11:9c70a7f4d7aa 51
IceTeam 11:9c70a7f4d7aa 52 obstacles.push_back(new Obs_circle(ROBOTRADIUS,IDO_PC1,800,910,35));// PC1
IceTeam 11:9c70a7f4d7aa 53 obstacles.push_back(new Obs_circle(ROBOTRADIUS,IDO_PC3,1650,1500,35));// PC3
IceTeam 11:9c70a7f4d7aa 54 obstacles.push_back(new Obs_circle(ROBOTRADIUS,IDO_PC2,1750,250,35));// PC2
IceTeam 11:9c70a7f4d7aa 55 obstacles.push_back(new Obs_circle(ROBOTRADIUS,IDO_PC4,1750,3000-250,35));// PC4
IceTeam 11:9c70a7f4d7aa 56 obstacles.push_back(new Obs_circle(ROBOTRADIUS,IDO_PC5,800,3000-910,35));// PC5
IceTeam 11:9c70a7f4d7aa 57
IceTeam 11:9c70a7f4d7aa 58 obstacles.push_back(new Obs_circle(ROBOTRADIUS,IDO_P1,200,90,30));// P1
IceTeam 11:9c70a7f4d7aa 59 obstacles.push_back(new Obs_circle(ROBOTRADIUS,IDO_P2,100,850,30));// P2
IceTeam 11:9c70a7f4d7aa 60 obstacles.push_back(new Obs_circle(ROBOTRADIUS,IDO_P3,200,850,30));// P3
IceTeam 11:9c70a7f4d7aa 61 obstacles.push_back(new Obs_circle(ROBOTRADIUS,IDO_P4,1355,870,30));// P4
IceTeam 11:9c70a7f4d7aa 62 obstacles.push_back(new Obs_circle(ROBOTRADIUS,IDO_P5,1750,90,30));// P5
IceTeam 11:9c70a7f4d7aa 63 obstacles.push_back(new Obs_circle(ROBOTRADIUS,IDO_P6,1850,90,30));// P6
IceTeam 11:9c70a7f4d7aa 64 obstacles.push_back(new Obs_circle(ROBOTRADIUS,IDO_P7,1770,1100,30));// P7
IceTeam 11:9c70a7f4d7aa 65 obstacles.push_back(new Obs_circle(ROBOTRADIUS,IDO_P8,1400,1300,30));// P8
IceTeam 11:9c70a7f4d7aa 66 obstacles.push_back(new Obs_circle(ROBOTRADIUS,IDO_P9,200,3000-90,30));// P9
IceTeam 11:9c70a7f4d7aa 67 obstacles.push_back(new Obs_circle(ROBOTRADIUS,IDO_P10,100,3000-850,30));// P10
IceTeam 11:9c70a7f4d7aa 68 obstacles.push_back(new Obs_circle(ROBOTRADIUS,IDO_P11,200,3000-850,30));// P11
IceTeam 11:9c70a7f4d7aa 69 obstacles.push_back(new Obs_circle(ROBOTRADIUS,IDO_P12,1355,3000-870,30));// P12
IceTeam 11:9c70a7f4d7aa 70 obstacles.push_back(new Obs_circle(ROBOTRADIUS,IDO_P13,1750,3000-90,30));// P13
IceTeam 11:9c70a7f4d7aa 71 obstacles.push_back(new Obs_circle(ROBOTRADIUS,IDO_P14,1850,3000-90,30));// P14
IceTeam 12:5355aed288b0 72 obstacles.push_back(new Obs_circle(ROBOTRADIUS,IDO_P15,1770,3000-1100,30));// P15*/
IceTeam 19:30942f018252 73 obstacles.push_back(new Obs_circle(ROBOTRADIUS,IDO_P16,1000+1000/20,1000+1000/20,30/20));// P16
IceTeam 11:9c70a7f4d7aa 74 }
IceTeam 11:9c70a7f4d7aa 75
IceTeam 11:9c70a7f4d7aa 76 int Map::getHeight(float x, float y)
IceTeam 11:9c70a7f4d7aa 77 {
IceTeam 11:9c70a7f4d7aa 78 int height = 0;
IceTeam 11:9c70a7f4d7aa 79 for(unsigned int i=0;i<obstacles.size();i++)
IceTeam 11:9c70a7f4d7aa 80 height += obstacles[i]->height(x,y);
IceTeam 11:9c70a7f4d7aa 81 return height;
IceTeam 11:9c70a7f4d7aa 82 }
IceTeam 11:9c70a7f4d7aa 83
IceTeam 11:9c70a7f4d7aa 84 float dist(Point *p1,Point *p2)
IceTeam 11:9c70a7f4d7aa 85 {
IceTeam 11:9c70a7f4d7aa 86 int dx = p1->getx()-p2->getx();
IceTeam 11:9c70a7f4d7aa 87 if(dx<0) dx=-dx;
IceTeam 11:9c70a7f4d7aa 88 int dy = p1->gety()-p2->gety();
IceTeam 11:9c70a7f4d7aa 89 if(dy<0) dy=-dy;
IceTeam 11:9c70a7f4d7aa 90 return sqrt((float)dx*dx+dy*dy);
IceTeam 11:9c70a7f4d7aa 91 }
IceTeam 11:9c70a7f4d7aa 92
IceTeam 11:9c70a7f4d7aa 93 char Map::AStar(float x, float y, float goal_x, float goal_y, float mpc)
IceTeam 11:9c70a7f4d7aa 94 {
IceTeam 11:9c70a7f4d7aa 95 /*! http://www.gamedev.net/page/resources/_/technical/artificial-intelligence/a-pathfinding-for-beginners-r2003 !*/
IceTeam 11:9c70a7f4d7aa 96 //float dx,dy; // Permet de diminuer l'erreur par rapport au centre des cases
IceTeam 11:9c70a7f4d7aa 97 //dx = ((((int)(x/mpc))*mpc-mpc/2)+(((int)(goal_x/mpc))*mpc-mpc/2))/2;
IceTeam 11:9c70a7f4d7aa 98
IceTeam 11:9c70a7f4d7aa 99 path.clear();
IceTeam 11:9c70a7f4d7aa 100
IceTeam 11:9c70a7f4d7aa 101 Point goal(goal_x/mpc,goal_y/mpc);
IceTeam 19:30942f018252 102 /*if(getHeight(goal_x,goal_y) >= 32000)
IceTeam 11:9c70a7f4d7aa 103 {
IceTeam 11:9c70a7f4d7aa 104 #if LOG_LEVEL >= 2
IceTeam 11:9c70a7f4d7aa 105 logger.printf("[warning - pathfinder] Unreachable point (%.3f,%.3f)\r\n",goal_x,goal_y);
IceTeam 11:9c70a7f4d7aa 106 #endif
IceTeam 11:9c70a7f4d7aa 107 return 4;
IceTeam 11:9c70a7f4d7aa 108 }
IceTeam 11:9c70a7f4d7aa 109
IceTeam 11:9c70a7f4d7aa 110 if(getHeight(x,y) >= 32000)
IceTeam 11:9c70a7f4d7aa 111 {
IceTeam 11:9c70a7f4d7aa 112 #if LOG_LEVEL >= 2
IceTeam 11:9c70a7f4d7aa 113 logger.printf("[warning - pathfinder] Unstartable point (%.3f,%.3f)\r\n",x,y);
IceTeam 11:9c70a7f4d7aa 114 #endif
IceTeam 11:9c70a7f4d7aa 115 return 5;
IceTeam 19:30942f018252 116 }*/
IceTeam 11:9c70a7f4d7aa 117
IceTeam 11:9c70a7f4d7aa 118
IceTeam 11:9c70a7f4d7aa 119 unsigned int i=0;
IceTeam 11:9c70a7f4d7aa 120 unsigned int instanceDePoint=0;
IceTeam 11:9c70a7f4d7aa 121
IceTeam 11:9c70a7f4d7aa 122 std::vector<Point*> openList;
IceTeam 11:9c70a7f4d7aa 123 openList.push_back(new Point(x/mpc,y/mpc));
IceTeam 11:9c70a7f4d7aa 124 openList[0]->setG(0);
IceTeam 11:9c70a7f4d7aa 125 openList[0]->setH(dist(openList[0],&goal));
IceTeam 11:9c70a7f4d7aa 126 openList[0]->setParent();
IceTeam 11:9c70a7f4d7aa 127
IceTeam 11:9c70a7f4d7aa 128 std::vector<Point*> closeList;
IceTeam 11:9c70a7f4d7aa 129
IceTeam 11:9c70a7f4d7aa 130 Point* current;
IceTeam 19:30942f018252 131 pc.printf("@Map::AStar l.130 : Debut du do\n\r");
IceTeam 11:9c70a7f4d7aa 132 do
IceTeam 11:9c70a7f4d7aa 133 {
IceTeam 11:9c70a7f4d7aa 134 // On cherche le plus petit F dans la liste ouverte
IceTeam 11:9c70a7f4d7aa 135 current = openList[0];
IceTeam 11:9c70a7f4d7aa 136
IceTeam 11:9c70a7f4d7aa 137 unsigned int pos = 0;
IceTeam 11:9c70a7f4d7aa 138 for(i=0;i<openList.size();i++)
IceTeam 11:9c70a7f4d7aa 139 if(openList[i]->getF() < current->getF())
IceTeam 11:9c70a7f4d7aa 140 {
IceTeam 11:9c70a7f4d7aa 141 current = openList[i];
IceTeam 11:9c70a7f4d7aa 142 pos = i;
IceTeam 11:9c70a7f4d7aa 143 }
IceTeam 11:9c70a7f4d7aa 144
IceTeam 11:9c70a7f4d7aa 145 // On le place dans la liste fermé
IceTeam 11:9c70a7f4d7aa 146 closeList.push_back(current);
IceTeam 11:9c70a7f4d7aa 147 openList.erase(openList.begin()+pos);
IceTeam 11:9c70a7f4d7aa 148
IceTeam 11:9c70a7f4d7aa 149 #if LOG_LEVEL >= 4 && LOG_ASTAR
IceTeam 11:9c70a7f4d7aa 150 logger.printf("[info - pathfinder] Adding (%d,%d) in the closed list\r\n",current->getx(),current->gety());
IceTeam 11:9c70a7f4d7aa 151 logger.printf("[info - pathfinder] Closed list : %d elements\r\n[info - pathfinder] Opened list : %d elements\r\n",openList.size(),closeList.size());
IceTeam 11:9c70a7f4d7aa 152 #endif
IceTeam 11:9c70a7f4d7aa 153
IceTeam 11:9c70a7f4d7aa 154 // On ajoute tous ses voisins viable das la liste ouverte
IceTeam 19:30942f018252 155 pc.printf("@Map::AStar l.155 : Debut du for d'ajout des voisins dans la liste ouverte\n\r");
IceTeam 11:9c70a7f4d7aa 156 for(int dx=-1;dx<=1;dx++)
IceTeam 11:9c70a7f4d7aa 157 {
IceTeam 11:9c70a7f4d7aa 158 for(int dy=-1;dy<=1;dy++)
IceTeam 11:9c70a7f4d7aa 159 {
IceTeam 11:9c70a7f4d7aa 160 if(dx==0 && dy==0)
IceTeam 11:9c70a7f4d7aa 161 continue;
IceTeam 11:9c70a7f4d7aa 162 if(!(dx == 0 || dy == 0)) // On skype les mouvement diagoneaux
IceTeam 11:9c70a7f4d7aa 163 continue;
IceTeam 11:9c70a7f4d7aa 164
IceTeam 11:9c70a7f4d7aa 165 Point *p = new Point(current->getx()+dx,current->gety()+dy);
IceTeam 11:9c70a7f4d7aa 166 instanceDePoint++;
IceTeam 11:9c70a7f4d7aa 167 if(p == 0 || instanceDePoint >= MAXPOINT) // Overload !!!
IceTeam 11:9c70a7f4d7aa 168 {
IceTeam 11:9c70a7f4d7aa 169 for(unsigned int i=0;i<openList.size();i++)
IceTeam 11:9c70a7f4d7aa 170 delete openList[i];
IceTeam 11:9c70a7f4d7aa 171 for(unsigned int i=0;i<closeList.size();i++)
IceTeam 11:9c70a7f4d7aa 172 delete closeList[i];
IceTeam 11:9c70a7f4d7aa 173
IceTeam 11:9c70a7f4d7aa 174 path.clear();
IceTeam 19:30942f018252 175
IceTeam 19:30942f018252 176 logger.printf("[error - pathfinder] Overload (%d,%d)\r\n",openList.size(),closeList.size());
IceTeam 11:9c70a7f4d7aa 177
IceTeam 11:9c70a7f4d7aa 178 return 3;
IceTeam 11:9c70a7f4d7aa 179 }
IceTeam 11:9c70a7f4d7aa 180
IceTeam 11:9c70a7f4d7aa 181 if(p->in(closeList)) // On ignore le point si il est déjà dans la liste fermé
IceTeam 11:9c70a7f4d7aa 182 {
IceTeam 11:9c70a7f4d7aa 183 delete p;
IceTeam 11:9c70a7f4d7aa 184 continue;
IceTeam 11:9c70a7f4d7aa 185 }
IceTeam 11:9c70a7f4d7aa 186
IceTeam 11:9c70a7f4d7aa 187 int height = getHeight((current->getx()+dx)*mpc,(current->gety()+dy)*mpc);
IceTeam 19:30942f018252 188 /*if(height>=32000) // On ignore le point, il n'est pas accessible
IceTeam 11:9c70a7f4d7aa 189 {
IceTeam 19:30942f018252 190 pc.printf("@Map::AStar l.190 : Point inaccessible\n\r");
IceTeam 11:9c70a7f4d7aa 191 delete p;
IceTeam 11:9c70a7f4d7aa 192 continue; // On ignore le point si il est déjà dans la liste fermé
IceTeam 19:30942f018252 193 }*/
IceTeam 11:9c70a7f4d7aa 194
IceTeam 11:9c70a7f4d7aa 195 if(p->in(openList,pos))
IceTeam 11:9c70a7f4d7aa 196 {
IceTeam 11:9c70a7f4d7aa 197 if(dx == 0 || dy == 0) // Mouvement non diagonnal
IceTeam 11:9c70a7f4d7aa 198 {
IceTeam 11:9c70a7f4d7aa 199 if(current->getG() + NDIAG_COST < openList[pos]->getG())
IceTeam 11:9c70a7f4d7aa 200 {
IceTeam 11:9c70a7f4d7aa 201 openList[pos]->setG(current->getG() + NDIAG_COST);
IceTeam 11:9c70a7f4d7aa 202 openList[pos]->setParent(current);
IceTeam 11:9c70a7f4d7aa 203 }
IceTeam 11:9c70a7f4d7aa 204 }
IceTeam 11:9c70a7f4d7aa 205 else
IceTeam 11:9c70a7f4d7aa 206 {
IceTeam 11:9c70a7f4d7aa 207 if(current->getG() + DIAG_COST < openList[pos]->getG())
IceTeam 11:9c70a7f4d7aa 208 {
IceTeam 11:9c70a7f4d7aa 209 openList[pos]->setG(current->getG() + DIAG_COST);
IceTeam 11:9c70a7f4d7aa 210 openList[pos]->setParent(current);
IceTeam 11:9c70a7f4d7aa 211 }
IceTeam 11:9c70a7f4d7aa 212 }
IceTeam 11:9c70a7f4d7aa 213
IceTeam 11:9c70a7f4d7aa 214 delete p;
IceTeam 11:9c70a7f4d7aa 215 }
IceTeam 11:9c70a7f4d7aa 216 else
IceTeam 11:9c70a7f4d7aa 217 {
IceTeam 11:9c70a7f4d7aa 218 openList.push_back(p);
IceTeam 11:9c70a7f4d7aa 219 if(dx == 0 || dy == 0) // Mouvement non diagonnal
IceTeam 11:9c70a7f4d7aa 220 p->setG(current->getG() + NDIAG_COST);
IceTeam 11:9c70a7f4d7aa 221 else
IceTeam 11:9c70a7f4d7aa 222 p->setG(current->getG() + DIAG_COST);
IceTeam 11:9c70a7f4d7aa 223 p->setH(height + dist(p,&goal));
IceTeam 11:9c70a7f4d7aa 224 p->setParent(current);
IceTeam 11:9c70a7f4d7aa 225 }
IceTeam 11:9c70a7f4d7aa 226 }
IceTeam 11:9c70a7f4d7aa 227 }
IceTeam 19:30942f018252 228 pc.printf("@Map::AStar l.229 : Fin du while\n\r");
IceTeam 19:30942f018252 229 if(!openList.empty())
IceTeam 19:30942f018252 230 pc.printf("@Map::AStar l.231 : openList.size = %f\n\r", openList.size());
IceTeam 19:30942f018252 231 pc.printf("@Map::AStar l.233 : dist(closeList.back(),goal)=%f\n\r", dist(closeList.back(),&goal));
IceTeam 11:9c70a7f4d7aa 232 }
IceTeam 11:9c70a7f4d7aa 233 while(dist(closeList.back(),&goal) && !openList.empty()); // Tant qu'on a pas atteint la case et qu'on a des choix
IceTeam 11:9c70a7f4d7aa 234
IceTeam 11:9c70a7f4d7aa 235
IceTeam 11:9c70a7f4d7aa 236 #if LOG_LEVEL >= 3
IceTeam 11:9c70a7f4d7aa 237 logger.printf("[info - pathfinder] Closed list : %d elements\r\n[info - pathfinder] Opened list : %d elements\r\n",openList.size(),closeList.size());
IceTeam 11:9c70a7f4d7aa 238 #endif
IceTeam 19:30942f018252 239 pc.printf("@Map::AStar l.155 : Debut du tendage\n\r");
IceTeam 11:9c70a7f4d7aa 240 if(!openList.empty())
IceTeam 11:9c70a7f4d7aa 241 {
IceTeam 11:9c70a7f4d7aa 242 #ifdef CODEBLOCK
IceTeam 11:9c70a7f4d7aa 243 ofstream f_openlist("openlist.txt");
IceTeam 11:9c70a7f4d7aa 244 for(i=0;i<openList.size();i++)
IceTeam 11:9c70a7f4d7aa 245 {
IceTeam 11:9c70a7f4d7aa 246 f_openlist << i << "," << openList[i]->getx()*mpc << "," << openList[i]->gety()*mpc << endl;
IceTeam 11:9c70a7f4d7aa 247 delete openList[i];
IceTeam 11:9c70a7f4d7aa 248 }
IceTeam 11:9c70a7f4d7aa 249 f_openlist.close();
IceTeam 11:9c70a7f4d7aa 250
IceTeam 11:9c70a7f4d7aa 251 ofstream f_closelist("closelist.txt");
IceTeam 11:9c70a7f4d7aa 252 for(i=0;i<closeList.size();i++)
IceTeam 11:9c70a7f4d7aa 253 {
IceTeam 11:9c70a7f4d7aa 254 f_closelist << i << "," << closeList[i]->getx()*mpc << "," << closeList[i]->gety()*mpc << endl;
IceTeam 11:9c70a7f4d7aa 255 }
IceTeam 11:9c70a7f4d7aa 256 f_closelist.close();
IceTeam 11:9c70a7f4d7aa 257 #endif
IceTeam 11:9c70a7f4d7aa 258
IceTeam 11:9c70a7f4d7aa 259 // On reconstruit le chemin
IceTeam 11:9c70a7f4d7aa 260 #if LOG_LEVEL >= 3
IceTeam 11:9c70a7f4d7aa 261 logger.printf("[info - pathfinder] Recontruction du chemin ... ");
IceTeam 11:9c70a7f4d7aa 262 #endif
IceTeam 11:9c70a7f4d7aa 263 path.clear();
IceTeam 11:9c70a7f4d7aa 264 Point* c = closeList.back();
IceTeam 11:9c70a7f4d7aa 265 while(c != 0)
IceTeam 11:9c70a7f4d7aa 266 {
IceTeam 11:9c70a7f4d7aa 267 path.insert(path.begin(),SimplePoint(c->getx()*mpc,c->gety()*mpc));
IceTeam 11:9c70a7f4d7aa 268 c = c->getParent();
IceTeam 11:9c70a7f4d7aa 269 }
IceTeam 11:9c70a7f4d7aa 270
IceTeam 11:9c70a7f4d7aa 271 path.front().x=x;
IceTeam 11:9c70a7f4d7aa 272 path.front().y=y;
IceTeam 11:9c70a7f4d7aa 273
IceTeam 11:9c70a7f4d7aa 274 path.back().x=goal_x;
IceTeam 11:9c70a7f4d7aa 275 path.back().y=goal_y;
IceTeam 11:9c70a7f4d7aa 276
IceTeam 11:9c70a7f4d7aa 277 #ifdef CODEBLOCK
IceTeam 11:9c70a7f4d7aa 278 ofstream f_path("path.txt");
IceTeam 11:9c70a7f4d7aa 279 for(i=0;i<path.size();i++)
IceTeam 11:9c70a7f4d7aa 280 {
IceTeam 11:9c70a7f4d7aa 281 f_path << i << "," << path[i].x << "," << path[i].y << endl;
IceTeam 11:9c70a7f4d7aa 282 }
IceTeam 11:9c70a7f4d7aa 283 f_path.close();
IceTeam 11:9c70a7f4d7aa 284 #endif
IceTeam 11:9c70a7f4d7aa 285
IceTeam 11:9c70a7f4d7aa 286 for(i=0;i<closeList.size();i++)
IceTeam 11:9c70a7f4d7aa 287 delete closeList[i];
IceTeam 11:9c70a7f4d7aa 288
IceTeam 11:9c70a7f4d7aa 289 #if LOG_LEVEL >= 3
IceTeam 11:9c70a7f4d7aa 290 logger.printf("[done] %d elements\r\n",path.size());
IceTeam 11:9c70a7f4d7aa 291 logger.printf("[info - pathfinder] Tendage du chemin ... ");
IceTeam 11:9c70a7f4d7aa 292 #endif
IceTeam 11:9c70a7f4d7aa 293
IceTeam 11:9c70a7f4d7aa 294
IceTeam 11:9c70a7f4d7aa 295 // Algo de 'tendage' du chemin
IceTeam 11:9c70a7f4d7aa 296 bool continuer = true;
IceTeam 11:9c70a7f4d7aa 297 unsigned int pointValide = 0;
IceTeam 11:9c70a7f4d7aa 298
IceTeam 11:9c70a7f4d7aa 299 #ifdef CODEBLOCK
IceTeam 11:9c70a7f4d7aa 300 ofstream f_tendage("tendage.txtt");
IceTeam 11:9c70a7f4d7aa 301 #endif // CODEBLOCK
IceTeam 11:9c70a7f4d7aa 302
IceTeam 11:9c70a7f4d7aa 303
IceTeam 11:9c70a7f4d7aa 304 while(continuer)
IceTeam 11:9c70a7f4d7aa 305 {
IceTeam 11:9c70a7f4d7aa 306 continuer = false;
IceTeam 11:9c70a7f4d7aa 307
IceTeam 11:9c70a7f4d7aa 308 for(unsigned int i1=pointValide;i1<path.size();i1++)
IceTeam 11:9c70a7f4d7aa 309 {
IceTeam 11:9c70a7f4d7aa 310 bool necessaire = false;
IceTeam 11:9c70a7f4d7aa 311
IceTeam 11:9c70a7f4d7aa 312 for(unsigned int i2=i1+2;i2<path.size();i2++)
IceTeam 11:9c70a7f4d7aa 313 {
IceTeam 11:9c70a7f4d7aa 314 //cout << "Entre " << i1 << " et " << i2 << endl;
IceTeam 11:9c70a7f4d7aa 315 if(path[i1].x!=path[i2].x && atan((path[i1].y-path[i2].y)/(path[i1].x-path[i2].x)) < PI/4)
IceTeam 11:9c70a7f4d7aa 316 {
IceTeam 11:9c70a7f4d7aa 317 float a = (path[i1].y-path[i2].y)/(path[i1].x-path[i2].x);
IceTeam 11:9c70a7f4d7aa 318 float b = path[i1].y - a*path[i1].x;
IceTeam 11:9c70a7f4d7aa 319 float step = (mpc*0.5f)*cos(atan(a));
IceTeam 11:9c70a7f4d7aa 320
IceTeam 11:9c70a7f4d7aa 321 necessaire = false;
IceTeam 11:9c70a7f4d7aa 322
IceTeam 11:9c70a7f4d7aa 323 for(x=min(path[i1].x,path[i2].x);x<max(path[i1].x,path[i2].x);x+=step)
IceTeam 11:9c70a7f4d7aa 324 {
IceTeam 11:9c70a7f4d7aa 325 y=a*x+b;
IceTeam 11:9c70a7f4d7aa 326
IceTeam 19:30942f018252 327 /*#ifdef CODEBLOCK
IceTeam 11:9c70a7f4d7aa 328 f_tendage << getHeight(x,y) << "," << x << "," << y << endl;
IceTeam 11:9c70a7f4d7aa 329 #endif // CODEBLOCK
IceTeam 11:9c70a7f4d7aa 330
IceTeam 11:9c70a7f4d7aa 331 if(getHeight(x,y) >= 32000)
IceTeam 11:9c70a7f4d7aa 332 {
IceTeam 11:9c70a7f4d7aa 333 necessaire = true;
IceTeam 11:9c70a7f4d7aa 334 break;
IceTeam 19:30942f018252 335 }*/
IceTeam 11:9c70a7f4d7aa 336 }
IceTeam 11:9c70a7f4d7aa 337
IceTeam 11:9c70a7f4d7aa 338 if(!necessaire)
IceTeam 11:9c70a7f4d7aa 339 {
IceTeam 11:9c70a7f4d7aa 340 // Ca n'a pas touché, on peut supprimmer le point entre les deux
IceTeam 11:9c70a7f4d7aa 341 continuer = true;
IceTeam 11:9c70a7f4d7aa 342 path.erase(path.begin()+i2-1);
IceTeam 11:9c70a7f4d7aa 343 }
IceTeam 11:9c70a7f4d7aa 344 else
IceTeam 11:9c70a7f4d7aa 345 pointValide++;
IceTeam 11:9c70a7f4d7aa 346 break;
IceTeam 11:9c70a7f4d7aa 347 }
IceTeam 11:9c70a7f4d7aa 348 else
IceTeam 11:9c70a7f4d7aa 349 {
IceTeam 11:9c70a7f4d7aa 350 // x=a*y+b
IceTeam 11:9c70a7f4d7aa 351 float a = (path[i1].x-path[i2].x)/(path[i1].y-path[i2].y);
IceTeam 11:9c70a7f4d7aa 352 float b = path[i1].x - a*path[i1].y;
IceTeam 11:9c70a7f4d7aa 353 float step = (mpc*0.5f)*cos(atan(a));
IceTeam 11:9c70a7f4d7aa 354
IceTeam 11:9c70a7f4d7aa 355 necessaire = false;
IceTeam 11:9c70a7f4d7aa 356
IceTeam 11:9c70a7f4d7aa 357 for(y=min(path[i1].y,path[i2].y);y<max(path[i1].y,path[i2].y);y+=step)
IceTeam 11:9c70a7f4d7aa 358 {
IceTeam 11:9c70a7f4d7aa 359 x=a*y+b;
IceTeam 11:9c70a7f4d7aa 360
IceTeam 19:30942f018252 361 /*#ifdef CODEBLOCK
IceTeam 11:9c70a7f4d7aa 362 f_tendage << getHeight(x,y) << "," << x << "," << y << endl;
IceTeam 11:9c70a7f4d7aa 363 #endif // CODEBLOCK
IceTeam 11:9c70a7f4d7aa 364
IceTeam 11:9c70a7f4d7aa 365 if(getHeight(x,y) >= 32000)
IceTeam 11:9c70a7f4d7aa 366 {
IceTeam 11:9c70a7f4d7aa 367 necessaire = true;
IceTeam 11:9c70a7f4d7aa 368 break;
IceTeam 19:30942f018252 369 }*/
IceTeam 11:9c70a7f4d7aa 370 }
IceTeam 11:9c70a7f4d7aa 371
IceTeam 11:9c70a7f4d7aa 372 if(!necessaire)
IceTeam 11:9c70a7f4d7aa 373 {
IceTeam 11:9c70a7f4d7aa 374 // Ca n'a pas touché, on peut supprimmer le point entre les deux
IceTeam 11:9c70a7f4d7aa 375 continuer = true;
IceTeam 11:9c70a7f4d7aa 376 path.erase(path.begin()+i2-1);
IceTeam 11:9c70a7f4d7aa 377 }
IceTeam 11:9c70a7f4d7aa 378 else
IceTeam 11:9c70a7f4d7aa 379 pointValide++;
IceTeam 11:9c70a7f4d7aa 380 break;
IceTeam 11:9c70a7f4d7aa 381
IceTeam 11:9c70a7f4d7aa 382 }
IceTeam 11:9c70a7f4d7aa 383 }
IceTeam 11:9c70a7f4d7aa 384 if(continuer)
IceTeam 11:9c70a7f4d7aa 385 break;
IceTeam 11:9c70a7f4d7aa 386 }
IceTeam 11:9c70a7f4d7aa 387 }
IceTeam 11:9c70a7f4d7aa 388
IceTeam 11:9c70a7f4d7aa 389 #ifdef CODEBLOCK
IceTeam 11:9c70a7f4d7aa 390 f_tendage.close();
IceTeam 11:9c70a7f4d7aa 391 #endif
IceTeam 11:9c70a7f4d7aa 392
IceTeam 11:9c70a7f4d7aa 393
IceTeam 11:9c70a7f4d7aa 394
IceTeam 11:9c70a7f4d7aa 395 #if LOG_LEVEL >= 3
IceTeam 11:9c70a7f4d7aa 396 logger.printf("[done] %d elements\r\n",path.size());
IceTeam 11:9c70a7f4d7aa 397 #endif
IceTeam 11:9c70a7f4d7aa 398
IceTeam 11:9c70a7f4d7aa 399 return 1;
IceTeam 11:9c70a7f4d7aa 400 }
IceTeam 11:9c70a7f4d7aa 401 else
IceTeam 11:9c70a7f4d7aa 402 {
IceTeam 11:9c70a7f4d7aa 403 for(i=0;i<openList.size();i++)
IceTeam 11:9c70a7f4d7aa 404 delete openList[i];
IceTeam 11:9c70a7f4d7aa 405
IceTeam 11:9c70a7f4d7aa 406 for(i=0;i<closeList.size();i++)
IceTeam 11:9c70a7f4d7aa 407 delete closeList[i];
IceTeam 11:9c70a7f4d7aa 408
IceTeam 11:9c70a7f4d7aa 409 path.clear();
IceTeam 11:9c70a7f4d7aa 410
IceTeam 11:9c70a7f4d7aa 411 return 2;
IceTeam 11:9c70a7f4d7aa 412 }
IceTeam 11:9c70a7f4d7aa 413 }
IceTeam 11:9c70a7f4d7aa 414
IceTeam 11:9c70a7f4d7aa 415
IceTeam 11:9c70a7f4d7aa 416
IceTeam 11:9c70a7f4d7aa 417