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@20:b2f3757fd7ee, 2016-01-13 (annotated)

Committer:
IceTeam
Date:
Wed Jan 13 19:10:51 2016 +0100
Revision:
20:b2f3757fd7ee
Parent:
12:5355aed288b0
Child:
23:3a34b8b70da6
Test odo

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