Diff: Map/Map.cpp

Revision:

47:be4eebf40568

Parent:

45:b53ae54062c6

Child:

49:5e2f7323f280

--- a/Map/Map.cpp	Wed Apr 13 12:47:47 2016 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,412 +0,0 @@
-#include "Map.h"
-#include "../Odometry/Odometry.h"
-#include "Obstacles/Obs_circle.h"
-
-#ifdef CODEBLOCK
-    using namespace std;
-    #include <iostream>
-    #include <fstream>
-#else
-    #include "mbed.h"
-    extern Serial logger;
-#endif
-#include <math.h>
-
-Map::Map()
-{
-    
-}
-
-Map::~Map()
-{
-    for(unsigned int i=0;i<obstacles.size();i++)
-        delete obstacles[i];
-}
-
-void Map::build()
-{
-    /*obstacles.push_back(new Obs_rect(ROBOTRADIUS,IDO_MG,-100,-100,2100,0));// MG
-    obstacles.push_back(new Obs_rect(ROBOTRADIUS,IDO_MB,2100,-100,2000,3100));// MB
-    obstacles.push_back(new Obs_rect(ROBOTRADIUS,IDO_MH,-100,-100,0,3100));// MH
-    obstacles.push_back(new Obs_rect(ROBOTRADIUS,IDO_MD,2100,3000,-100,3100));// MD
-    
-    
-    obstacles.push_back(new Obs_rect(ROBOTRADIUS,IDO_M1,778,0,800,400));// M1
-    obstacles.push_back(new Obs_rect(ROBOTRADIUS,IDO_M2,1200,0,1222,400));// M2
-    obstacles.push_back(new Obs_rect(ROBOTRADIUS,IDO_M3,800,0,1200,70));// M3
-    obstacles.push_back(new Obs_rect(ROBOTRADIUS,IDO_M4,1222,2600,1200,3000));// M4
-    obstacles.push_back(new Obs_rect(ROBOTRADIUS,IDO_M5,1200,2930,800,3000));// M5
-    obstacles.push_back(new Obs_rect(ROBOTRADIUS,IDO_M6,800,2600,778,3000));// M6
-    
-    obstacles.push_back(new Obs_rect(ROBOTRADIUS,IDO_E,580,967,0,2033));// E
-    obstacles.push_back(new Obs_rect(ROBOTRADIUS,IDO_S,2000,1200,1900,1800));// S
-    
-    obstacles.push_back(new Obs_rect(ROBOTRADIUS,IDO_D1,70,265,0,335));// D1
-    obstacles.push_back(new Obs_rect(ROBOTRADIUS,IDO_D2,70,565,0,636));// D2
-    obstacles.push_back(new Obs_rect(ROBOTRADIUS,IDO_D3,70,2365,0,2435));// D3
-    obstacles.push_back(new Obs_rect(ROBOTRADIUS,IDO_D4,70,2665,0,2735));// D4
-    
-    
-    obstacles.push_back(new Obs_circle(ROBOTRADIUS,IDO_PC1,800,910,35));// PC1
-    obstacles.push_back(new Obs_circle(ROBOTRADIUS,IDO_PC3,1650,1500,35));// PC3
-    obstacles.push_back(new Obs_circle(ROBOTRADIUS,IDO_PC2,1750,250,35));// PC2
-    obstacles.push_back(new Obs_circle(ROBOTRADIUS,IDO_PC4,1750,3000-250,35));// PC4
-    obstacles.push_back(new Obs_circle(ROBOTRADIUS,IDO_PC5,800,3000-910,35));// PC5
-    
-    obstacles.push_back(new Obs_circle(ROBOTRADIUS,IDO_P1,200,90,30));// P1
-    obstacles.push_back(new Obs_circle(ROBOTRADIUS,IDO_P2,100,850,30));// P2
-    obstacles.push_back(new Obs_circle(ROBOTRADIUS,IDO_P3,200,850,30));// P3
-    obstacles.push_back(new Obs_circle(ROBOTRADIUS,IDO_P4,1355,870,30));// P4
-    obstacles.push_back(new Obs_circle(ROBOTRADIUS,IDO_P5,1750,90,30));// P5
-    obstacles.push_back(new Obs_circle(ROBOTRADIUS,IDO_P6,1850,90,30));// P6
-    obstacles.push_back(new Obs_circle(ROBOTRADIUS,IDO_P7,1770,1100,30));// P7
-    obstacles.push_back(new Obs_circle(ROBOTRADIUS,IDO_P8,1400,1300,30));// P8
-    obstacles.push_back(new Obs_circle(ROBOTRADIUS,IDO_P9,200,3000-90,30));// P9
-    obstacles.push_back(new Obs_circle(ROBOTRADIUS,IDO_P10,100,3000-850,30));// P10
-    obstacles.push_back(new Obs_circle(ROBOTRADIUS,IDO_P11,200,3000-850,30));// P11
-    obstacles.push_back(new Obs_circle(ROBOTRADIUS,IDO_P12,1355,3000-870,30));// P12
-    obstacles.push_back(new Obs_circle(ROBOTRADIUS,IDO_P13,1750,3000-90,30));// P13
-    obstacles.push_back(new Obs_circle(ROBOTRADIUS,IDO_P14,1850,3000-90,30));// P14
-    obstacles.push_back(new Obs_circle(ROBOTRADIUS,IDO_P15,1770,3000-1100,30));// P15*/
-    obstacles.push_back(new Obs_circle(ROBOTRADIUS,IDO_P16,700,1000,100));// P16
-    obstacles.push_back(new Obs_circle(ROBOTRADIUS,IDO_P15,1100,600,100));// P16
-    obstacles.push_back(new Obs_circle(ROBOTRADIUS,IDO_P15,1100,1000,100));// P16
-}
-
-int Map::getHeight(float x, float y)
-{
-    int height = 0;
-    for(unsigned int i=0;i<obstacles.size();i++)
-        height += obstacles[i]->height(x,y);
-    return height;
-}
-
-float dist(Point *p1,Point *p2)
-{
-    int dx = p1->getx()-p2->getx();
-    if(dx<0) dx=-dx;
-    int dy = p1->gety()-p2->gety();
-    if(dy<0) dy=-dy;
-    return sqrt((float)dx*dx+dy*dy);
-}
-
-char Map::AStar(float x, float y, float goal_x, float goal_y, float mpc)
-{
-    /*! http://www.gamedev.net/page/resources/_/technical/artificial-intelligence/a-pathfinding-for-beginners-r2003 !*/
-    //float dx,dy; // Permet de diminuer l'erreur par rapport au centre des cases
-    //dx = ((((int)(x/mpc))*mpc-mpc/2)+(((int)(goal_x/mpc))*mpc-mpc/2))/2;
-    
-    path.clear();
-    
-    Point goal(goal_x/mpc,goal_y/mpc);
-    if(getHeight(goal_x,goal_y) >= 32000)
-    {
-        #if LOG_LEVEL >= 2
-            logger.printf("[warning - pathfinder] Unreachable point (%.3f,%.3f)\r\n",goal_x,goal_y);
-        #endif
-        return 4;
-    }
-    
-    if(getHeight(x,y) >= 32000)
-    {
-        #if LOG_LEVEL >= 2
-            logger.printf("[warning - pathfinder] Unstartable point (%.3f,%.3f)\r\n",x,y);
-        #endif
-        return 5;
-    }
-    
-    
-    unsigned int i=0;
-    unsigned int instanceDePoint=0;
-    
-    std::vector<Point*> openList;
-    openList.push_back(new Point(x/mpc,y/mpc));
-    openList[0]->setG(0);
-    openList[0]->setH(dist(openList[0],&goal));
-    openList[0]->setParent();
-    
-    std::vector<Point*> closeList;
-    
-    Point* current;
-    do
-    {
-        // On cherche le plus petit F dans la liste ouverte
-        current = openList[0];
-        
-        unsigned int pos = 0;
-        for(i=0;i<openList.size();i++)
-            if(openList[i]->getF() < current->getF())
-            {
-                current = openList[i];
-                pos = i;
-            }
-        
-        // On le place dans la liste fermé
-        closeList.push_back(current);
-        openList.erase(openList.begin()+pos);
-        
-        #if LOG_LEVEL >= 4 && LOG_ASTAR
-            logger.printf("[info - pathfinder] Adding (%d,%d) in the closed list\r\n",current->getx(),current->gety());
-            logger.printf("[info - pathfinder] Closed list : %d elements\r\n[info - pathfinder] Opened list : %d elements\r\n",openList.size(),closeList.size());
-        #endif
-        
-        // On ajoute tous ses voisins viable das la liste ouverte
-        for(int dx=-1;dx<=1;dx++)
-        {
-            for(int dy=-1;dy<=1;dy++)
-            {
-                if(dx==0 && dy==0)
-                    continue;
-                if(!(dx == 0 || dy == 0)) // On skype les mouvement diagoneaux
-                    continue;
-                
-                Point *p = new Point(current->getx()+dx,current->gety()+dy);
-                instanceDePoint++;
-                if(p == 0 || instanceDePoint >= MAXPOINT) // Overload !!!
-                {
-                    for(unsigned int i=0;i<openList.size();i++)
-                        delete openList[i];
-                    for(unsigned int i=0;i<closeList.size();i++)
-                        delete closeList[i];
-                    
-                    path.clear();
-                        
-                    #if LOG_LEVEL >= 1
-                        logger.printf("[error - pathfinder] Overload (%d,%d)\r\n",openList.size(),closeList.size());
-                    #endif
-                    
-                    return 3;
-                }
-                
-                if(p->in(closeList)) // On ignore le point si il est déjà dans la liste fermé
-                {
-                    delete p;
-                    continue; 
-                }
-                
-                int height = getHeight((current->getx()+dx)*mpc,(current->gety()+dy)*mpc);
-                if(height>=32000)  // On ignore le point, il n'est pas accessible
-                {
-                    delete p;
-                    continue; // On ignore le point si il est déjà dans la liste fermé
-                }
-                
-                if(p->in(openList,pos))
-                {
-                    if(dx == 0 || dy == 0) // Mouvement non diagonnal
-                    {
-                        if(current->getG() + NDIAG_COST < openList[pos]->getG())
-                        {
-                            openList[pos]->setG(current->getG() + NDIAG_COST);
-                            openList[pos]->setParent(current);
-                        }
-                    }
-                    else
-                    {
-                        if(current->getG() + DIAG_COST < openList[pos]->getG())
-                        {
-                            openList[pos]->setG(current->getG() + DIAG_COST);
-                            openList[pos]->setParent(current);
-                        }
-                    }
-                    
-                    delete p;
-                }
-                else
-                {
-                    openList.push_back(p);
-                    if(dx == 0 || dy == 0) // Mouvement non diagonnal
-                        p->setG(current->getG() + NDIAG_COST);
-                    else
-                        p->setG(current->getG() + DIAG_COST);
-                    p->setH(height + dist(p,&goal));
-                    p->setParent(current);
-                }
-            }
-        }
-    }
-    while(dist(closeList.back(),&goal) && !openList.empty()); // Tant qu'on a pas atteint la case et qu'on a des choix
-    
-    
-    #if LOG_LEVEL >= 3
-        logger.printf("[info - pathfinder] Closed list : %d elements\r\n[info - pathfinder] Opened list : %d elements\r\n",openList.size(),closeList.size());
-    #endif
-    
-    if(!openList.empty())
-    {
-        #ifdef CODEBLOCK
-            ofstream f_openlist("openlist.txt");
-            for(i=0;i<openList.size();i++)
-            {
-                f_openlist << i << "," << openList[i]->getx()*mpc << "," << openList[i]->gety()*mpc << endl;
-                delete openList[i];
-            }
-            f_openlist.close();
-            
-            ofstream f_closelist("closelist.txt");
-            for(i=0;i<closeList.size();i++)
-            {
-                f_closelist << i << "," << closeList[i]->getx()*mpc << "," << closeList[i]->gety()*mpc << endl;
-            }
-            f_closelist.close();
-        #endif
-        
-        // On reconstruit le chemin
-        #if LOG_LEVEL >= 3
-            logger.printf("[info - pathfinder] Recontruction du chemin ... ");
-        #endif
-        path.clear();
-        Point* c = closeList.back();
-        while(c != 0)
-        {
-            path.insert(path.begin(),SimplePoint(c->getx()*mpc,c->gety()*mpc));
-            c = c->getParent();
-        }
-        
-        path.front().x=x;
-        path.front().y=y;
-        
-        path.back().x=goal_x;
-        path.back().y=goal_y;
-        
-        #ifdef CODEBLOCK
-            ofstream f_path("path.txt");
-            for(i=0;i<path.size();i++)
-            {
-                f_path << i << "," << path[i].x << "," <<  path[i].y << endl;
-            }
-            f_path.close();
-        #endif
-        
-        for(i=0; i<closeList.size(); i++)
-            delete closeList[i];
-        
-        #if LOG_LEVEL >= 3
-            logger.printf("[done] %d elements\r\n",path.size());
-            logger.printf("[info - pathfinder] Tendage du chemin ... ");
-        #endif
-        
-        
-        // Algo de 'tendage' du chemin
-        bool continuer = true;
-        unsigned int pointValide = 0;
-        
-        #ifdef CODEBLOCK
-            ofstream f_tendage("tendage.txtt");
-        #endif // CODEBLOCK
-        
-        
-        while(continuer)
-        {
-            continuer = false;
-            
-            for(unsigned int i1=pointValide;i1<path.size();i1++)
-            {
-                bool necessaire = false;
-                
-                for(unsigned int i2=i1+2;i2<path.size();i2++)
-                {
-                    //cout << "Entre " << i1 << " et " << i2 << endl;
-                    if(path[i1].x!=path[i2].x && atan((path[i1].y-path[i2].y)/(path[i1].x-path[i2].x)) < PI/4)
-                    {
-                        float a = (path[i1].y-path[i2].y)/(path[i1].x-path[i2].x);
-                        float b = path[i1].y - a*path[i1].x;
-                        float step = (mpc*0.5f)*cos(atan(a));
-                        
-                        necessaire = false;
-                        
-                        for(x=min(path[i1].x,path[i2].x);x<max(path[i1].x,path[i2].x);x+=step)
-                        {
-                            y=a*x+b;
-                            
-                            #ifdef CODEBLOCK
-                                f_tendage << getHeight(x,y) << "," << x << "," <<  y << endl;
-                            #endif // CODEBLOCK
-                            
-                            if(getHeight(x,y) >= 32000)
-                            {   
-                                necessaire = true;
-                                break;
-                            }
-                        }
-                        
-                        if(!necessaire)
-                        {
-                            // Ca n'a pas touché, on peut supprimmer le point entre les deux
-                            continuer = true;
-                            path.erase(path.begin()+i2-1);
-                        }
-                        else
-                            pointValide++;
-                        break;
-                    }
-                    else
-                    {
-                        // x=a*y+b
-                        float a = (path[i1].x-path[i2].x)/(path[i1].y-path[i2].y);
-                        float b = path[i1].x - a*path[i1].y;
-                        float step = (mpc*0.5f)*cos(atan(a));
-                        
-                        necessaire = false;
-                        
-                        for(y=min(path[i1].y,path[i2].y);y<max(path[i1].y,path[i2].y);y+=step)
-                        {
-                            x=a*y+b;
-                            
-                            #ifdef CODEBLOCK
-                                f_tendage << getHeight(x,y) << "," << x << "," <<  y << endl;
-                            #endif // CODEBLOCK
-                            
-                            if(getHeight(x,y) >= 32000)
-                            {
-                                necessaire = true;
-                                break;
-                            }
-                        }
-                        
-                        if(!necessaire)
-                        {
-                            // Ca n'a pas touché, on peut supprimmer le point entre les deux
-                            continuer = true;
-                            path.erase(path.begin()+i2-1);
-                        }
-                        else
-                            pointValide++;
-                        break;
-                        
-                    }
-                }
-                if(continuer)
-                    break;
-            }
-        }
-        
-        #ifdef CODEBLOCK
-            f_tendage.close();
-        #endif
-        
-        
-        
-        #if LOG_LEVEL >= 3
-            logger.printf("[done] %d elements\r\n",path.size());
-        #endif
-        
-        return 1;
-    }
-    else
-    {
-        for(i=0;i<openList.size();i++)
-            delete openList[i];
-
-        for(i=0;i<closeList.size();i++)
-            delete closeList[i];
-        
-        path.clear();
-        
-        return 2;
-    }
-}
-
-
-
-