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@31:8bcc3a0bfa8a, 2016-01-26 (annotated)

Committer:
IceTeam
Date:
Tue Jan 26 16:39:43 2016 +0000
Revision:
31:8bcc3a0bfa8a
Parent:
23:3a34b8b70da6
Child:
36:2d7357a385bc
Le commit d'une truc pourri;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
IceTeam 11:9c70a7f4d7aa 1 #include "Map.h"
IceTeam 11:9c70a7f4d7aa 2
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 23:3a34b8b70da6 71 obstacles.push_back(new Obs_circle(ROBOTRADIUS,IDO_P16,700,1000,1));// P16
IceTeam 11:9c70a7f4d7aa 72 }
IceTeam 11:9c70a7f4d7aa 73
IceTeam 11:9c70a7f4d7aa 74 int Map::getHeight(float x, float y)
IceTeam 11:9c70a7f4d7aa 75 {
IceTeam 11:9c70a7f4d7aa 76 int height = 0;
IceTeam 11:9c70a7f4d7aa 77 for(unsigned int i=0;i<obstacles.size();i++)
IceTeam 11:9c70a7f4d7aa 78 height += obstacles[i]->height(x,y);
IceTeam 11:9c70a7f4d7aa 79 return height;
IceTeam 11:9c70a7f4d7aa 80 }
IceTeam 11:9c70a7f4d7aa 81
IceTeam 11:9c70a7f4d7aa 82 float dist(Point *p1,Point *p2)
IceTeam 11:9c70a7f4d7aa 83 {
IceTeam 11:9c70a7f4d7aa 84 int dx = p1->getx()-p2->getx();
IceTeam 11:9c70a7f4d7aa 85 if(dx<0) dx=-dx;
IceTeam 11:9c70a7f4d7aa 86 int dy = p1->gety()-p2->gety();
IceTeam 11:9c70a7f4d7aa 87 if(dy<0) dy=-dy;
IceTeam 11:9c70a7f4d7aa 88 return sqrt((float)dx*dx+dy*dy);
IceTeam 11:9c70a7f4d7aa 89 }
IceTeam 11:9c70a7f4d7aa 90
IceTeam 11:9c70a7f4d7aa 91 char Map::AStar(float x, float y, float goal_x, float goal_y, float mpc)
IceTeam 11:9c70a7f4d7aa 92 {
IceTeam 11:9c70a7f4d7aa 93 /*! http://www.gamedev.net/page/resources/_/technical/artificial-intelligence/a-pathfinding-for-beginners-r2003 !*/
IceTeam 11:9c70a7f4d7aa 94 //float dx,dy; // Permet de diminuer l'erreur par rapport au centre des cases
IceTeam 11:9c70a7f4d7aa 95 //dx = ((((int)(x/mpc))*mpc-mpc/2)+(((int)(goal_x/mpc))*mpc-mpc/2))/2;
IceTeam 11:9c70a7f4d7aa 96
IceTeam 11:9c70a7f4d7aa 97 path.clear();
IceTeam 11:9c70a7f4d7aa 98
IceTeam 11:9c70a7f4d7aa 99 Point goal(goal_x/mpc,goal_y/mpc);
IceTeam 11:9c70a7f4d7aa 100 if(getHeight(goal_x,goal_y) >= 32000)
IceTeam 11:9c70a7f4d7aa 101 {
IceTeam 11:9c70a7f4d7aa 102 #if LOG_LEVEL >= 2
IceTeam 11:9c70a7f4d7aa 103 logger.printf("[warning - pathfinder] Unreachable point (%.3f,%.3f)\r\n",goal_x,goal_y);
IceTeam 11:9c70a7f4d7aa 104 #endif
IceTeam 11:9c70a7f4d7aa 105 return 4;
IceTeam 11:9c70a7f4d7aa 106 }
IceTeam 11:9c70a7f4d7aa 107
IceTeam 11:9c70a7f4d7aa 108 if(getHeight(x,y) >= 32000)
IceTeam 11:9c70a7f4d7aa 109 {
IceTeam 11:9c70a7f4d7aa 110 #if LOG_LEVEL >= 2
IceTeam 11:9c70a7f4d7aa 111 logger.printf("[warning - pathfinder] Unstartable point (%.3f,%.3f)\r\n",x,y);
IceTeam 11:9c70a7f4d7aa 112 #endif
IceTeam 11:9c70a7f4d7aa 113 return 5;
IceTeam 11:9c70a7f4d7aa 114 }
IceTeam 11:9c70a7f4d7aa 115
IceTeam 11:9c70a7f4d7aa 116
IceTeam 11:9c70a7f4d7aa 117 unsigned int i=0;
IceTeam 11:9c70a7f4d7aa 118 unsigned int instanceDePoint=0;
IceTeam 11:9c70a7f4d7aa 119
IceTeam 11:9c70a7f4d7aa 120 std::vector<Point*> openList;
IceTeam 11:9c70a7f4d7aa 121 openList.push_back(new Point(x/mpc,y/mpc));
IceTeam 11:9c70a7f4d7aa 122 openList[0]->setG(0);
IceTeam 11:9c70a7f4d7aa 123 openList[0]->setH(dist(openList[0],&goal));
IceTeam 11:9c70a7f4d7aa 124 openList[0]->setParent();
IceTeam 11:9c70a7f4d7aa 125
IceTeam 11:9c70a7f4d7aa 126 std::vector<Point*> closeList;
IceTeam 11:9c70a7f4d7aa 127
IceTeam 11:9c70a7f4d7aa 128 Point* current;
IceTeam 11:9c70a7f4d7aa 129 do
IceTeam 11:9c70a7f4d7aa 130 {
IceTeam 11:9c70a7f4d7aa 131 // On cherche le plus petit F dans la liste ouverte
IceTeam 11:9c70a7f4d7aa 132 current = openList[0];
IceTeam 11:9c70a7f4d7aa 133
IceTeam 11:9c70a7f4d7aa 134 unsigned int pos = 0;
IceTeam 11:9c70a7f4d7aa 135 for(i=0;i<openList.size();i++)
IceTeam 11:9c70a7f4d7aa 136 if(openList[i]->getF() < current->getF())
IceTeam 11:9c70a7f4d7aa 137 {
IceTeam 11:9c70a7f4d7aa 138 current = openList[i];
IceTeam 11:9c70a7f4d7aa 139 pos = i;
IceTeam 11:9c70a7f4d7aa 140 }
IceTeam 11:9c70a7f4d7aa 141
IceTeam 11:9c70a7f4d7aa 142 // On le place dans la liste fermé
IceTeam 11:9c70a7f4d7aa 143 closeList.push_back(current);
IceTeam 11:9c70a7f4d7aa 144 openList.erase(openList.begin()+pos);
IceTeam 11:9c70a7f4d7aa 145
IceTeam 11:9c70a7f4d7aa 146 #if LOG_LEVEL >= 4 && LOG_ASTAR
IceTeam 11:9c70a7f4d7aa 147 logger.printf("[info - pathfinder] Adding (%d,%d) in the closed list\r\n",current->getx(),current->gety());
IceTeam 11:9c70a7f4d7aa 148 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 149 #endif
IceTeam 11:9c70a7f4d7aa 150
IceTeam 11:9c70a7f4d7aa 151 // On ajoute tous ses voisins viable das la liste ouverte
IceTeam 11:9c70a7f4d7aa 152 for(int dx=-1;dx<=1;dx++)
IceTeam 11:9c70a7f4d7aa 153 {
IceTeam 11:9c70a7f4d7aa 154 for(int dy=-1;dy<=1;dy++)
IceTeam 11:9c70a7f4d7aa 155 {
IceTeam 11:9c70a7f4d7aa 156 if(dx==0 && dy==0)
IceTeam 11:9c70a7f4d7aa 157 continue;
IceTeam 11:9c70a7f4d7aa 158 if(!(dx == 0 || dy == 0)) // On skype les mouvement diagoneaux
IceTeam 11:9c70a7f4d7aa 159 continue;
IceTeam 11:9c70a7f4d7aa 160
IceTeam 11:9c70a7f4d7aa 161 Point *p = new Point(current->getx()+dx,current->gety()+dy);
IceTeam 11:9c70a7f4d7aa 162 instanceDePoint++;
IceTeam 11:9c70a7f4d7aa 163 if(p == 0 || instanceDePoint >= MAXPOINT) // Overload !!!
IceTeam 11:9c70a7f4d7aa 164 {
IceTeam 11:9c70a7f4d7aa 165 for(unsigned int i=0;i<openList.size();i++)
IceTeam 11:9c70a7f4d7aa 166 delete openList[i];
IceTeam 11:9c70a7f4d7aa 167 for(unsigned int i=0;i<closeList.size();i++)
IceTeam 11:9c70a7f4d7aa 168 delete closeList[i];
IceTeam 11:9c70a7f4d7aa 169
IceTeam 11:9c70a7f4d7aa 170 path.clear();
IceTeam 11:9c70a7f4d7aa 171
IceTeam 11:9c70a7f4d7aa 172 #if LOG_LEVEL >= 1
IceTeam 11:9c70a7f4d7aa 173 logger.printf("[error - pathfinder] Overload (%d,%d)\r\n",openList.size(),closeList.size());
IceTeam 11:9c70a7f4d7aa 174 #endif
IceTeam 11:9c70a7f4d7aa 175
IceTeam 11:9c70a7f4d7aa 176 return 3;
IceTeam 11:9c70a7f4d7aa 177 }
IceTeam 11:9c70a7f4d7aa 178
IceTeam 11:9c70a7f4d7aa 179 if(p->in(closeList)) // On ignore le point si il est déjà dans la liste fermé
IceTeam 11:9c70a7f4d7aa 180 {
IceTeam 11:9c70a7f4d7aa 181 delete p;
IceTeam 11:9c70a7f4d7aa 182 continue;
IceTeam 11:9c70a7f4d7aa 183 }
IceTeam 11:9c70a7f4d7aa 184
IceTeam 11:9c70a7f4d7aa 185 int height = getHeight((current->getx()+dx)*mpc,(current->gety()+dy)*mpc);
IceTeam 11:9c70a7f4d7aa 186 if(height>=32000) // On ignore le point, il n'est pas accessible
IceTeam 11:9c70a7f4d7aa 187 {
IceTeam 11:9c70a7f4d7aa 188 delete p;
IceTeam 11:9c70a7f4d7aa 189 continue; // On ignore le point si il est déjà dans la liste fermé
IceTeam 11:9c70a7f4d7aa 190 }
IceTeam 11:9c70a7f4d7aa 191
IceTeam 11:9c70a7f4d7aa 192 if(p->in(openList,pos))
IceTeam 11:9c70a7f4d7aa 193 {
IceTeam 11:9c70a7f4d7aa 194 if(dx == 0 || dy == 0) // Mouvement non diagonnal
IceTeam 11:9c70a7f4d7aa 195 {
IceTeam 11:9c70a7f4d7aa 196 if(current->getG() + NDIAG_COST < openList[pos]->getG())
IceTeam 11:9c70a7f4d7aa 197 {
IceTeam 11:9c70a7f4d7aa 198 openList[pos]->setG(current->getG() + NDIAG_COST);
IceTeam 11:9c70a7f4d7aa 199 openList[pos]->setParent(current);
IceTeam 11:9c70a7f4d7aa 200 }
IceTeam 11:9c70a7f4d7aa 201 }
IceTeam 11:9c70a7f4d7aa 202 else
IceTeam 11:9c70a7f4d7aa 203 {
IceTeam 11:9c70a7f4d7aa 204 if(current->getG() + DIAG_COST < openList[pos]->getG())
IceTeam 11:9c70a7f4d7aa 205 {
IceTeam 11:9c70a7f4d7aa 206 openList[pos]->setG(current->getG() + DIAG_COST);
IceTeam 11:9c70a7f4d7aa 207 openList[pos]->setParent(current);
IceTeam 11:9c70a7f4d7aa 208 }
IceTeam 11:9c70a7f4d7aa 209 }
IceTeam 11:9c70a7f4d7aa 210
IceTeam 11:9c70a7f4d7aa 211 delete p;
IceTeam 11:9c70a7f4d7aa 212 }
IceTeam 11:9c70a7f4d7aa 213 else
IceTeam 11:9c70a7f4d7aa 214 {
IceTeam 11:9c70a7f4d7aa 215 openList.push_back(p);
IceTeam 11:9c70a7f4d7aa 216 if(dx == 0 || dy == 0) // Mouvement non diagonnal
IceTeam 11:9c70a7f4d7aa 217 p->setG(current->getG() + NDIAG_COST);
IceTeam 11:9c70a7f4d7aa 218 else
IceTeam 11:9c70a7f4d7aa 219 p->setG(current->getG() + DIAG_COST);
IceTeam 11:9c70a7f4d7aa 220 p->setH(height + dist(p,&goal));
IceTeam 11:9c70a7f4d7aa 221 p->setParent(current);
IceTeam 11:9c70a7f4d7aa 222 }
IceTeam 11:9c70a7f4d7aa 223 }
IceTeam 11:9c70a7f4d7aa 224 }
IceTeam 11:9c70a7f4d7aa 225 }
IceTeam 11:9c70a7f4d7aa 226 while(dist(closeList.back(),&goal) && !openList.empty()); // Tant qu'on a pas atteint la case et qu'on a des choix
IceTeam 11:9c70a7f4d7aa 227
IceTeam 11:9c70a7f4d7aa 228
IceTeam 11:9c70a7f4d7aa 229 #if LOG_LEVEL >= 3
IceTeam 11:9c70a7f4d7aa 230 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 231 #endif
IceTeam 11:9c70a7f4d7aa 232
IceTeam 11:9c70a7f4d7aa 233 if(!openList.empty())
IceTeam 11:9c70a7f4d7aa 234 {
IceTeam 11:9c70a7f4d7aa 235 #ifdef CODEBLOCK
IceTeam 11:9c70a7f4d7aa 236 ofstream f_openlist("openlist.txt");
IceTeam 11:9c70a7f4d7aa 237 for(i=0;i<openList.size();i++)
IceTeam 11:9c70a7f4d7aa 238 {
IceTeam 11:9c70a7f4d7aa 239 f_openlist << i << "," << openList[i]->getx()*mpc << "," << openList[i]->gety()*mpc << endl;
IceTeam 11:9c70a7f4d7aa 240 delete openList[i];
IceTeam 11:9c70a7f4d7aa 241 }
IceTeam 11:9c70a7f4d7aa 242 f_openlist.close();
IceTeam 11:9c70a7f4d7aa 243
IceTeam 11:9c70a7f4d7aa 244 ofstream f_closelist("closelist.txt");
IceTeam 11:9c70a7f4d7aa 245 for(i=0;i<closeList.size();i++)
IceTeam 11:9c70a7f4d7aa 246 {
IceTeam 11:9c70a7f4d7aa 247 f_closelist << i << "," << closeList[i]->getx()*mpc << "," << closeList[i]->gety()*mpc << endl;
IceTeam 11:9c70a7f4d7aa 248 }
IceTeam 11:9c70a7f4d7aa 249 f_closelist.close();
IceTeam 11:9c70a7f4d7aa 250 #endif
IceTeam 11:9c70a7f4d7aa 251
IceTeam 11:9c70a7f4d7aa 252 // On reconstruit le chemin
IceTeam 11:9c70a7f4d7aa 253 #if LOG_LEVEL >= 3
IceTeam 11:9c70a7f4d7aa 254 logger.printf("[info - pathfinder] Recontruction du chemin ... ");
IceTeam 11:9c70a7f4d7aa 255 #endif
IceTeam 11:9c70a7f4d7aa 256 path.clear();
IceTeam 11:9c70a7f4d7aa 257 Point* c = closeList.back();
IceTeam 11:9c70a7f4d7aa 258 while(c != 0)
IceTeam 11:9c70a7f4d7aa 259 {
IceTeam 11:9c70a7f4d7aa 260 path.insert(path.begin(),SimplePoint(c->getx()*mpc,c->gety()*mpc));
IceTeam 11:9c70a7f4d7aa 261 c = c->getParent();
IceTeam 11:9c70a7f4d7aa 262 }
IceTeam 11:9c70a7f4d7aa 263
IceTeam 11:9c70a7f4d7aa 264 path.front().x=x;
IceTeam 11:9c70a7f4d7aa 265 path.front().y=y;
IceTeam 11:9c70a7f4d7aa 266
IceTeam 11:9c70a7f4d7aa 267 path.back().x=goal_x;
IceTeam 11:9c70a7f4d7aa 268 path.back().y=goal_y;
IceTeam 11:9c70a7f4d7aa 269
IceTeam 11:9c70a7f4d7aa 270 #ifdef CODEBLOCK
IceTeam 11:9c70a7f4d7aa 271 ofstream f_path("path.txt");
IceTeam 11:9c70a7f4d7aa 272 for(i=0;i<path.size();i++)
IceTeam 11:9c70a7f4d7aa 273 {
IceTeam 11:9c70a7f4d7aa 274 f_path << i << "," << path[i].x << "," << path[i].y << endl;
IceTeam 11:9c70a7f4d7aa 275 }
IceTeam 11:9c70a7f4d7aa 276 f_path.close();
IceTeam 11:9c70a7f4d7aa 277 #endif
IceTeam 11:9c70a7f4d7aa 278
IceTeam 20:b2f3757fd7ee 279 for(i=0; i<closeList.size(); i++)
IceTeam 11:9c70a7f4d7aa 280 delete closeList[i];
IceTeam 11:9c70a7f4d7aa 281
IceTeam 11:9c70a7f4d7aa 282 #if LOG_LEVEL >= 3
IceTeam 11:9c70a7f4d7aa 283 logger.printf("[done] %d elements\r\n",path.size());
IceTeam 11:9c70a7f4d7aa 284 logger.printf("[info - pathfinder] Tendage du chemin ... ");
IceTeam 11:9c70a7f4d7aa 285 #endif
IceTeam 11:9c70a7f4d7aa 286
IceTeam 11:9c70a7f4d7aa 287
IceTeam 11:9c70a7f4d7aa 288 // Algo de 'tendage' du chemin
IceTeam 11:9c70a7f4d7aa 289 bool continuer = true;
IceTeam 11:9c70a7f4d7aa 290 unsigned int pointValide = 0;
IceTeam 11:9c70a7f4d7aa 291
IceTeam 11:9c70a7f4d7aa 292 #ifdef CODEBLOCK
IceTeam 11:9c70a7f4d7aa 293 ofstream f_tendage("tendage.txtt");
IceTeam 11:9c70a7f4d7aa 294 #endif // CODEBLOCK
IceTeam 11:9c70a7f4d7aa 295
IceTeam 11:9c70a7f4d7aa 296
IceTeam 11:9c70a7f4d7aa 297 while(continuer)
IceTeam 11:9c70a7f4d7aa 298 {
IceTeam 11:9c70a7f4d7aa 299 continuer = false;
IceTeam 11:9c70a7f4d7aa 300
IceTeam 11:9c70a7f4d7aa 301 for(unsigned int i1=pointValide;i1<path.size();i1++)
IceTeam 11:9c70a7f4d7aa 302 {
IceTeam 11:9c70a7f4d7aa 303 bool necessaire = false;
IceTeam 11:9c70a7f4d7aa 304
IceTeam 11:9c70a7f4d7aa 305 for(unsigned int i2=i1+2;i2<path.size();i2++)
IceTeam 11:9c70a7f4d7aa 306 {
IceTeam 11:9c70a7f4d7aa 307 //cout << "Entre " << i1 << " et " << i2 << endl;
IceTeam 11:9c70a7f4d7aa 308 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 309 {
IceTeam 11:9c70a7f4d7aa 310 float a = (path[i1].y-path[i2].y)/(path[i1].x-path[i2].x);
IceTeam 11:9c70a7f4d7aa 311 float b = path[i1].y - a*path[i1].x;
IceTeam 11:9c70a7f4d7aa 312 float step = (mpc*0.5f)*cos(atan(a));
IceTeam 11:9c70a7f4d7aa 313
IceTeam 11:9c70a7f4d7aa 314 necessaire = false;
IceTeam 11:9c70a7f4d7aa 315
IceTeam 11:9c70a7f4d7aa 316 for(x=min(path[i1].x,path[i2].x);x<max(path[i1].x,path[i2].x);x+=step)
IceTeam 11:9c70a7f4d7aa 317 {
IceTeam 11:9c70a7f4d7aa 318 y=a*x+b;
IceTeam 11:9c70a7f4d7aa 319
IceTeam 11:9c70a7f4d7aa 320 #ifdef CODEBLOCK
IceTeam 11:9c70a7f4d7aa 321 f_tendage << getHeight(x,y) << "," << x << "," << y << endl;
IceTeam 11:9c70a7f4d7aa 322 #endif // CODEBLOCK
IceTeam 11:9c70a7f4d7aa 323
IceTeam 11:9c70a7f4d7aa 324 if(getHeight(x,y) >= 32000)
IceTeam 11:9c70a7f4d7aa 325 {
IceTeam 11:9c70a7f4d7aa 326 necessaire = true;
IceTeam 11:9c70a7f4d7aa 327 break;
IceTeam 11:9c70a7f4d7aa 328 }
IceTeam 11:9c70a7f4d7aa 329 }
IceTeam 11:9c70a7f4d7aa 330
IceTeam 11:9c70a7f4d7aa 331 if(!necessaire)
IceTeam 11:9c70a7f4d7aa 332 {
IceTeam 11:9c70a7f4d7aa 333 // Ca n'a pas touché, on peut supprimmer le point entre les deux
IceTeam 11:9c70a7f4d7aa 334 continuer = true;
IceTeam 11:9c70a7f4d7aa 335 path.erase(path.begin()+i2-1);
IceTeam 11:9c70a7f4d7aa 336 }
IceTeam 11:9c70a7f4d7aa 337 else
IceTeam 11:9c70a7f4d7aa 338 pointValide++;
IceTeam 11:9c70a7f4d7aa 339 break;
IceTeam 11:9c70a7f4d7aa 340 }
IceTeam 11:9c70a7f4d7aa 341 else
IceTeam 11:9c70a7f4d7aa 342 {
IceTeam 11:9c70a7f4d7aa 343 // x=a*y+b
IceTeam 11:9c70a7f4d7aa 344 float a = (path[i1].x-path[i2].x)/(path[i1].y-path[i2].y);
IceTeam 11:9c70a7f4d7aa 345 float b = path[i1].x - a*path[i1].y;
IceTeam 11:9c70a7f4d7aa 346 float step = (mpc*0.5f)*cos(atan(a));
IceTeam 11:9c70a7f4d7aa 347
IceTeam 11:9c70a7f4d7aa 348 necessaire = false;
IceTeam 11:9c70a7f4d7aa 349
IceTeam 11:9c70a7f4d7aa 350 for(y=min(path[i1].y,path[i2].y);y<max(path[i1].y,path[i2].y);y+=step)
IceTeam 11:9c70a7f4d7aa 351 {
IceTeam 11:9c70a7f4d7aa 352 x=a*y+b;
IceTeam 11:9c70a7f4d7aa 353
IceTeam 11:9c70a7f4d7aa 354 #ifdef CODEBLOCK
IceTeam 11:9c70a7f4d7aa 355 f_tendage << getHeight(x,y) << "," << x << "," << y << endl;
IceTeam 11:9c70a7f4d7aa 356 #endif // CODEBLOCK
IceTeam 11:9c70a7f4d7aa 357
IceTeam 11:9c70a7f4d7aa 358 if(getHeight(x,y) >= 32000)
IceTeam 11:9c70a7f4d7aa 359 {
IceTeam 11:9c70a7f4d7aa 360 necessaire = true;
IceTeam 11:9c70a7f4d7aa 361 break;
IceTeam 11:9c70a7f4d7aa 362 }
IceTeam 11:9c70a7f4d7aa 363 }
IceTeam 11:9c70a7f4d7aa 364
IceTeam 11:9c70a7f4d7aa 365 if(!necessaire)
IceTeam 11:9c70a7f4d7aa 366 {
IceTeam 11:9c70a7f4d7aa 367 // Ca n'a pas touché, on peut supprimmer le point entre les deux
IceTeam 11:9c70a7f4d7aa 368 continuer = true;
IceTeam 11:9c70a7f4d7aa 369 path.erase(path.begin()+i2-1);
IceTeam 11:9c70a7f4d7aa 370 }
IceTeam 11:9c70a7f4d7aa 371 else
IceTeam 11:9c70a7f4d7aa 372 pointValide++;
IceTeam 11:9c70a7f4d7aa 373 break;
IceTeam 11:9c70a7f4d7aa 374
IceTeam 11:9c70a7f4d7aa 375 }
IceTeam 11:9c70a7f4d7aa 376 }
IceTeam 11:9c70a7f4d7aa 377 if(continuer)
IceTeam 11:9c70a7f4d7aa 378 break;
IceTeam 11:9c70a7f4d7aa 379 }
IceTeam 11:9c70a7f4d7aa 380 }
IceTeam 11:9c70a7f4d7aa 381
IceTeam 11:9c70a7f4d7aa 382 #ifdef CODEBLOCK
IceTeam 11:9c70a7f4d7aa 383 f_tendage.close();
IceTeam 11:9c70a7f4d7aa 384 #endif
IceTeam 11:9c70a7f4d7aa 385
IceTeam 11:9c70a7f4d7aa 386
IceTeam 11:9c70a7f4d7aa 387
IceTeam 11:9c70a7f4d7aa 388 #if LOG_LEVEL >= 3
IceTeam 11:9c70a7f4d7aa 389 logger.printf("[done] %d elements\r\n",path.size());
IceTeam 11:9c70a7f4d7aa 390 #endif
IceTeam 11:9c70a7f4d7aa 391
IceTeam 11:9c70a7f4d7aa 392 return 1;
IceTeam 11:9c70a7f4d7aa 393 }
IceTeam 11:9c70a7f4d7aa 394 else
IceTeam 11:9c70a7f4d7aa 395 {
IceTeam 11:9c70a7f4d7aa 396 for(i=0;i<openList.size();i++)
IceTeam 11:9c70a7f4d7aa 397 delete openList[i];
IceTeam 11:9c70a7f4d7aa 398
IceTeam 11:9c70a7f4d7aa 399 for(i=0;i<closeList.size();i++)
IceTeam 11:9c70a7f4d7aa 400 delete closeList[i];
IceTeam 11:9c70a7f4d7aa 401
IceTeam 11:9c70a7f4d7aa 402 path.clear();
IceTeam 11:9c70a7f4d7aa 403
IceTeam 11:9c70a7f4d7aa 404 return 2;
IceTeam 11:9c70a7f4d7aa 405 }
IceTeam 11:9c70a7f4d7aa 406 }
IceTeam 11:9c70a7f4d7aa 407
IceTeam 11:9c70a7f4d7aa 408
IceTeam 11:9c70a7f4d7aa 409
IceTeam 11:9c70a7f4d7aa 410