ARES / Mbed 2 deprecated Robot 2015

Robot's source code

Dependencies:   mbed

Map/Map.cpp@123:55e5e9acc541, 2015-05-11 (annotated)

Committer:
Jagang
Date:
Mon May 11 20:32:11 2015 +0000
Revision:
123:55e5e9acc541
Parent:
119:c45efcd706d9 
Maj AI; Repositionnement du d?part (1000,177); Ajout de l'action sur les claps

Who changed what in which revision?

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