Diff: Map/Map.cpp

Revision:

119:c45efcd706d9

Parent:

117:f8c147141a0c

Child:

123:55e5e9acc541

--- a/Map/Map.cpp	Wed May 06 11:23:08 2015 +0000
+++ b/Map/Map.cpp	Wed May 06 15:17:16 2015 +0000
@@ -52,11 +52,23 @@
     //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();
+    #if LOG_LEVEL >= 3
+        logger.printf("[info - pathfinder] Start point (%.1f,%.1f)\r\n",x,y);
+        logger.printf("[info - pathfinder] Goal point (%.1f,%.1f)\r\n",goal_x,goal_y);
+    #endif
+    
     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);
+            logger.printf("[warning - pathfinder] Unreachable point (%.1f,%.1f) %d\r\n",goal_x,goal_y,getHeight(goal_x,goal_y));
+            
+            for(unsigned int i=0;i<obstacles.size();i++)
+            {
+                logger.printf("%d %d\r\n",i,obstacles[i]->height(goal_x,goal_y));
+            }
+            
         #endif
         return 4;
     }
@@ -230,7 +242,7 @@
                     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));
+                    float step = (mpc*0.5f)*cos(atan(a));
                     
                     #if LOG_LEVEL >= 4 & LOG_TENDEUR
                         logger.printf("[info - tendeur] X: a=%.2f b=%.2f\r\n",a,b);
@@ -239,7 +251,7 @@
                     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
+                        #if LOG_LEVEL >= 5 & 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
@@ -265,7 +277,7 @@
                     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));
+                    float step = (mpc*0.5f)*cos(atan(a));
                     
                     #if LOG_LEVEL >= 4 & LOG_TENDEUR
                         logger.printf("[info - tendeur] Y: a=%.2f b=%.2f\r\n",a,b);
@@ -274,7 +286,7 @@
                     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
+                        #if LOG_LEVEL >= 5 & 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