Diff: Map/map.cpp

Revision:

46:8eae88c45a78

diff -r b53ae54062c6 -r 8eae88c45a78 Map/map.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Map/map.cpp	Wed Apr 20 13:13:37 2016 +0000
@@ -0,0 +1,159 @@
+#include "map.h"
+
+map::map (Odometry* nodo) : Codo(nodo) {
+}
+
+void map::addObs (obsCarr nobs) {
+    obs.push_back (nobs);
+}
+
+void map::FindWay (float depX, float depY, float arrX, float arrY) {
+    point depart(depX, depY);
+    point arrivee(arrX, arrY);
+    FindWay(depart, arrivee);
+}
+
+void map::FindWay (point dep, point arr) {
+    //logger.printf("On a cherche un chemin\n\r");
+    nVector<pointParcours> open;
+    nVector<pointParcours> close;
+    points4 tmp;
+    bool val[4] = {true,true,true,true};
+    int os = obs.size ();
+    int i, j;
+    bool ended=false;   // On teste tous les points ajoutes dans l'open list pour savoir s'il y a intersection avec un obstacle. Ended passe à true quand aucun ne coupe un obstacle.
+    endedParc = false;
+
+    path.clear();
+    
+    pointParcours depP (dep, NULL, arr);
+    int indTMP1=0;  // Le point actuel
+    int PointEnding = 0;    
+    open.push_back (depP);
+
+    while (!ended && !open.empty ()) {
+        for (i = 0; i < open.size (); ++i) {
+            if (open[i].getP2 () < open[indTMP1].getP2 ())
+                indTMP1 = i;
+        }
+
+        close.push_first (open[indTMP1]);
+        open.erase (indTMP1);
+        indTMP1 = 0;
+
+        ended = true;
+        for (i = 0; i < os; ++i) {
+            if (obs[i].getCroisement (close[indTMP1].getX (), close[indTMP1].getY (), arr)) {
+                ended = false;
+                tmp = obs[i].getPoints ();
+
+                // On vérifie si le point croise un obstacle
+                for (j = 0; j < os; ++j)
+                    if (obs[j].getCroisement (tmp.p0, close[indTMP1]))
+                        val[0] = false;
+                // On vérifie si le point existe déjà dans la liste ouverte
+                for (j = 0; j < open.size (); ++j) {
+                    if (open[j] == tmp.p0)
+                        val[0] = false;
+                }
+                // On vérifie si le point existe déjà dans la liste fermée
+                for (j = 0; j < close.size (); ++j) {
+                    if (close[j] == tmp.p0)
+                        val[0] = false;
+                }
+                if (val[0]) {
+                    open.push_back (pointParcours (tmp.p0, &close[indTMP1], arr));
+                }
+
+                // On repete l'operation pour le second point
+                for (j = 0; j < os; ++j)
+                    if (obs[j].getCroisement (tmp.p1, close[indTMP1]))
+                        val[1] = false;
+                for (j = 0; j < open.size (); ++j) {
+                    if (open[j] == tmp.p1)
+                        val[1] = false;
+                }
+                for (j = 0; j < close.size (); ++j) {
+                    if (close[j] == tmp.p1)
+                        val[1] = false;
+                }
+                if (val[1]) {
+                    open.push_back (pointParcours (tmp.p1, &close[indTMP1], arr));
+                }
+
+                // On répète l'opération pour le troisième point
+                for (j = 0; j < os; ++j)
+                    if (obs[j].getCroisement (tmp.p2, close[indTMP1]))
+                        val[2] = false;
+                for (j = 0; j < open.size (); ++j) {
+                    if (open[j] == tmp.p2)
+                        val[2] = false;
+                }
+                for (j = 0; j < close.size (); ++j) {
+                    if (close[j] == tmp.p2)
+                        val[2] = false;
+                }
+                if (val[2]) {
+                    open.push_back (pointParcours (tmp.p2, &close[indTMP1], arr));
+                }
+
+                // On répète l'opération pour le quatrieme point
+                for (j = 0; j < os; ++j)
+                    if (obs[j].getCroisement (tmp.p3, close[indTMP1]))
+                        val[3] = false;
+                for (j = 0; j < open.size (); ++j) {
+                    if (open[j] == tmp.p3)
+                        val[3] = false;
+                }
+                for (j = 0; j < close.size (); ++j) {
+                    if (close[j] == tmp.p3)
+                        val[3] = false;
+                }
+                if (val[3]) {
+                    open.push_back (pointParcours (tmp.p3, &close[indTMP1], arr));
+                }
+
+                val[0] = true;
+                val[1] = true;
+                val[2] = true;
+                val[3] = true;
+            }
+        }
+    }
+
+    /* L'algorithme n'est pas bon. Je devrais prendre ici le plus court chemin vers l'arrivée pour ceux qui ne sont pas bloqués, et pas un aléatoire ... */
+    if (ended) {
+        pointParcours* pente;
+        pente = &close[0];
+        while (pente != NULL) {
+            path.push_first (*pente);
+            pente = pente->getPere ();
+        }
+        path.push_back (pointParcours(arr, NULL, arr));
+        path.erase(0);
+        endedParc = true;
+        /*
+        if (path.size() > 1)
+            path.erase(0);*/
+    }
+}
+
+void map::Execute(float XObjectif, float YObjectif) {
+    logger.printf("Findway %f-%f -> %f-%f\n\r", Codo->getX(), Codo->getY(), XObjectif, YObjectif);
+    FindWay (Codo->getX(), Codo->getY(), XObjectif, YObjectif);
+    
+    if (endedParc) {
+        //logger.printf("\n\r");
+        for (int i = 0; i < path.size (); i++) {
+            logger.printf("Goto %d/%d [%f, %f]... \n\r", i, path.size()-1, path[i].getX(), path[i].getY());
+            //the = (float) atan2((double) (p[i].gety() - odo.getY()), (double) (p[i].getx() - odo.getX())); 
+            Codo->GotoXY((double)path[i].getX(), (double)path[i].getY());
+            logger.printf("Goto Fini\n\r");
+        }
+        //logger.printf("Chemin fini !\n\r");
+    }
+    else {
+        logger.printf("Chemin pas trouve ...\n\r");
+    }
+    endedParc = false;
+}
\ No newline at end of file