File content as of revision 117:f8c147141a0c:

#include "Map.h"

#include "mbed.h"
#include "Obs_circle.h"
#include "Obs_rect.h"

extern Serial logger;

Map::Map()
{
    
}

Map::~Map()
{
    for(unsigned int i=0;i<obstacles.size();i++)
        delete obstacles[i];
}

void Map::build()
{
    //obstacles.push_back(new Obs_circle(ROBOTRADIUS,2,2,1));
    obstacles.push_back(new Obs_rect(ROBOTRADIUS,-100,-100,2100,0)); // MG
    
    
    obstacles.push_back(new Obs_rect(ROBOTRADIUS,778,0,800,400)); // M1
    obstacles.push_back(new Obs_rect(ROBOTRADIUS,1200,0,1222,400)); // M2
    obstacles.push_back(new Obs_rect(ROBOTRADIUS,800,0,1200,70)); // M3
    
}

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;
    
    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;
                
                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())
    {
        for(i=0;i<openList.size();i++)
            delete openList[i];
        
        // 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();
        }
        
        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;
        std::vector<SimplePoint> tempPath;
        while(continuer)
        {
            for(i=1;i<path.size();i+=2)
            {
                tempPath.push_back(path[i-1]);
                int height = (getHeight(path[i-1].x*mpc,path[i-1].y*mpc)+getHeight(path[i+1].x*mpc,path[i+1].y*mpc))/2;
                
                #if LOG_LEVEL >= 4 & LOG_TENDEUR
                    logger.printf("[info - tendeur] Testing (%.3f,%.3f) %d\r\n",path[i].x,path[i].y,height);
                #endif
                
                if(path[i-1].x!=path[i+1].x)
                {
                    // y=a*x+b
                    float a = (path[i-1].y-path[i+1].y)/(path[i-1].x-path[i+1].x);
                    float b = path[i-1].y - a*path[i-1].x;
                    
                    float step = (mpc*0.1f)*cos(atan(a));
                    
                    #if LOG_LEVEL >= 4 & LOG_TENDEUR
                        logger.printf("[info - tendeur] X: a=%.2f b=%.2f\r\n",a,b);
                        logger.printf("[info - tendeur] x [%.3f,%.3f] step=%.6f\r\n",path[i-1].x,path[i+1].x,step);
                    #endif
                    float x;
                    for(x=min(path[i-1].x,path[i+1].x);x<max(path[i-1].x,path[i+1].x);x+=step)
                    {
                        #if LOG_LEVEL >= 4 & LOG_TENDEUR
                            logger.printf("%.3f\t%.3f\t%.3f\r\n",getHeight(x,(a*x+b)),x,(a*x+b));
                        #endif
                        if(getHeight(x,(a*x+b)) > height) // Ca ne passe pas sans ce point
                        {
                            tempPath.push_back(path[i]);
                            
                            #if LOG_LEVEL >= 4 & LOG_TENDEUR
                                logger.printf("[info - tendeur] Adding (%.3f,%.3f) to the path\r\n",path[i].x,path[i].y);
                            #endif
                            break;
                        }
                    }
                    #if LOG_LEVEL >= 4 & LOG_TENDEUR
                        if(x >= max(path[i-1].x,path[i+1].x))
                        {
                            logger.printf("[info - tendeur] Skipping (%.3f,%.3f) to the path\r\n",path[i].x,path[i].y);
                        }
                    #endif
                }
                else if(path[i-1].y!=path[i+1].y)
                {
                    // x=a*y+b
                    float a = (path[i-1].x-path[i+1].x)/(path[i-1].y-path[i+1].y);
                    float b = path[i-1].x - a*path[i-1].y;
                    
                    float step = (mpc*0.1f)*cos(atan(a));
                    
                    #if LOG_LEVEL >= 4 & LOG_TENDEUR
                        logger.printf("[info - tendeur] Y: a=%.2f b=%.2f\r\n",a,b);
                        logger.printf("[info - tendeur] y [%.3f,%.3f] step=%.6f\r\n",path[i-1].y,path[i+1].y,step);
                    #endif
                    float y;
                    for(y=min(path[i-1].y,path[i+1].y);y<max(path[i-1].y,path[i+1].y);y+=step)
                    {
                        #if LOG_LEVEL >= 4 & LOG_TENDEUR
                            logger.printf("%.3f\t%.3f\t%.3f\t%.3f\r\n",getHeight((a*y+b),y),height,(a*y+b),y);
                        #endif
                        if(getHeight((a*y+b),y) > height) // Ca ne passe pas sans ce point
                        {
                            tempPath.push_back(path[i]);
                            
                            #if LOG_LEVEL >= 4 & LOG_TENDEUR
                                logger.printf("[info - tendeur] Adding (%.3f,%.3f) to the path\r\n",path[i].x,path[i].y);
                            #endif
                            break;
                        }
                    }
                    #if LOG_LEVEL >= 4 & LOG_TENDEUR
                        if(y >= max(path[i-1].y,path[i+1].y))
                        {
                            logger.printf("[info - tendeur] Skipping (%.3f,%.3f) to the path\r\n",path[i].x,path[i].y);
                        }
                    #endif
                }
            }
            if(path.back() != tempPath.back())
                tempPath.push_back(path.back());
            
            if(tempPath.size() < path.size()) // On a réussit a enlever un point
            {
                path.clear();
                for(i=0;i<tempPath.size();i++)
                    path.push_back(tempPath[i]);
                continuer = true;
            }
            else
                continuer = false;
            
            tempPath.clear();
        }
        
        #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;
    }
}