Diff: Map/Map.cpp

Revision:

109:53918ba98306

Parent:

12:235b5545ff41

Child:

117:f8c147141a0c

--- a/Map/Map.cpp	Mon May 04 13:00:28 2015 +0000
+++ b/Map/Map.cpp	Tue May 05 16:35:53 2015 +0000
@@ -0,0 +1,335 @@
+#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;
+    
+    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);
+                if(p == 0) // 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;
+    }
+}
+
+
+
+
+
+