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@41:b5a2fbc20beb, 2016-04-13 (annotated)

Committer:
sype
Date:
Wed Apr 13 11:27:34 2016 +0000
Revision:
41:b5a2fbc20beb
Parent:
39:309f38d1e49c 
Impl?mentation des dispositifs du robot

Who changed what in which revision?

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