File content as of revision 41:b5a2fbc20beb:

#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;
    }
}