ARES / Mbed 2 deprecated TestMyPathFind

Test du path finding

Dependencies:   RoboClaw mbed

Fork of TestMyPathFind by Romain Ame

map/map.cpp@41:53d5990ff99d, 2016-04-13 (annotated)

Committer:
IceTeam
Date:
Wed Apr 13 16:17:19 2016 +0000
Revision:
41:53d5990ff99d
Parent:
39:ca4dd3faffa8 
Correction odometrie;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
IceTeam 39:ca4dd3faffa8 1 #include "map.h"
IceTeam 39:ca4dd3faffa8 2
IceTeam 39:ca4dd3faffa8 3 map::map () {
IceTeam 39:ca4dd3faffa8 4 }
IceTeam 39:ca4dd3faffa8 5
IceTeam 39:ca4dd3faffa8 6 void map::addObs (obsCarr nobs) {
IceTeam 39:ca4dd3faffa8 7 obs.push_back (nobs);
IceTeam 39:ca4dd3faffa8 8 }
IceTeam 39:ca4dd3faffa8 9
IceTeam 39:ca4dd3faffa8 10 void map::FindWay (point dep, point arr) {
IceTeam 39:ca4dd3faffa8 11 nVector<pointParcours> open;
IceTeam 39:ca4dd3faffa8 12 nVector<pointParcours> close;
IceTeam 39:ca4dd3faffa8 13 points4 tmp;
IceTeam 39:ca4dd3faffa8 14 bool val[4] = {true,true,true,true};
IceTeam 39:ca4dd3faffa8 15 int os = obs.size ();
IceTeam 39:ca4dd3faffa8 16 int i, j;
IceTeam 39:ca4dd3faffa8 17 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.
IceTeam 39:ca4dd3faffa8 18 endedParc = false;
IceTeam 39:ca4dd3faffa8 19
IceTeam 39:ca4dd3faffa8 20 pointParcours depP (dep, NULL, &arr);
IceTeam 39:ca4dd3faffa8 21 int indTMP1=0; // Le point actuel
IceTeam 39:ca4dd3faffa8 22 int PointEnding = 0;
IceTeam 39:ca4dd3faffa8 23 open.push_back (depP);
IceTeam 39:ca4dd3faffa8 24
IceTeam 39:ca4dd3faffa8 25 while (!ended && !open.empty ()) {
IceTeam 39:ca4dd3faffa8 26 for (i = 0; i < open.size (); ++i) {
IceTeam 39:ca4dd3faffa8 27 if (open[i].getP2 () < open[indTMP1].getP2 ())
IceTeam 39:ca4dd3faffa8 28 indTMP1 = i;
IceTeam 39:ca4dd3faffa8 29 }
IceTeam 39:ca4dd3faffa8 30
IceTeam 39:ca4dd3faffa8 31 close.push_first (open[indTMP1]);
IceTeam 39:ca4dd3faffa8 32 open.erase (indTMP1);
IceTeam 39:ca4dd3faffa8 33 indTMP1 = 0;
IceTeam 39:ca4dd3faffa8 34
IceTeam 39:ca4dd3faffa8 35 ended = true;
IceTeam 39:ca4dd3faffa8 36 for (i = 0; i < os; ++i) {
IceTeam 39:ca4dd3faffa8 37 if (obs[i].getCroisement (close[indTMP1].getX (), close[indTMP1].getY (), arr)) {
IceTeam 39:ca4dd3faffa8 38 ended = false;
IceTeam 39:ca4dd3faffa8 39 tmp = obs[i].getPoints ();
IceTeam 39:ca4dd3faffa8 40
IceTeam 39:ca4dd3faffa8 41 // On vérifie si le point croise un obstacle
IceTeam 39:ca4dd3faffa8 42 for (j = 0; j < os; ++j)
IceTeam 39:ca4dd3faffa8 43 if (obs[j].getCroisement (tmp.p0, close[indTMP1]))
IceTeam 39:ca4dd3faffa8 44 val[0] = false;
IceTeam 39:ca4dd3faffa8 45 // On vérifie si le point existe déjà dans la liste ouverte
IceTeam 39:ca4dd3faffa8 46 for (j = 0; j < open.size (); ++j) {
IceTeam 39:ca4dd3faffa8 47 if (open[j] == tmp.p0)
IceTeam 39:ca4dd3faffa8 48 val[0] = false;
IceTeam 39:ca4dd3faffa8 49 }
IceTeam 39:ca4dd3faffa8 50 // On vérifie si le point existe déjà dans la liste fermée
IceTeam 39:ca4dd3faffa8 51 for (j = 0; j < close.size (); ++j) {
IceTeam 39:ca4dd3faffa8 52 if (close[j] == tmp.p0)
IceTeam 39:ca4dd3faffa8 53 val[0] = false;
IceTeam 39:ca4dd3faffa8 54 }
IceTeam 39:ca4dd3faffa8 55 if (val[0]) {
IceTeam 39:ca4dd3faffa8 56 open.push_back (pointParcours (tmp.p0, &close[indTMP1], &arr));
IceTeam 39:ca4dd3faffa8 57 }
IceTeam 39:ca4dd3faffa8 58
IceTeam 39:ca4dd3faffa8 59 // On repete l'operation pour le second point
IceTeam 39:ca4dd3faffa8 60 for (j = 0; j < os; ++j)
IceTeam 39:ca4dd3faffa8 61 if (obs[j].getCroisement (tmp.p1, close[indTMP1]))
IceTeam 39:ca4dd3faffa8 62 val[1] = false;
IceTeam 39:ca4dd3faffa8 63 for (j = 0; j < open.size (); ++j) {
IceTeam 39:ca4dd3faffa8 64 if (open[j] == tmp.p1)
IceTeam 39:ca4dd3faffa8 65 val[1] = false;
IceTeam 39:ca4dd3faffa8 66 }
IceTeam 39:ca4dd3faffa8 67 for (j = 0; j < close.size (); ++j) {
IceTeam 39:ca4dd3faffa8 68 if (close[j] == tmp.p1)
IceTeam 39:ca4dd3faffa8 69 val[1] = false;
IceTeam 39:ca4dd3faffa8 70 }
IceTeam 39:ca4dd3faffa8 71 if (val[1]) {
IceTeam 39:ca4dd3faffa8 72 open.push_back (pointParcours (tmp.p1, &close[indTMP1], &arr));
IceTeam 39:ca4dd3faffa8 73 }
IceTeam 39:ca4dd3faffa8 74
IceTeam 39:ca4dd3faffa8 75 // On répète l'opération pour le troisième point
IceTeam 39:ca4dd3faffa8 76 for (j = 0; j < os; ++j)
IceTeam 39:ca4dd3faffa8 77 if (obs[j].getCroisement (tmp.p2, close[indTMP1]))
IceTeam 39:ca4dd3faffa8 78 val[2] = false;
IceTeam 39:ca4dd3faffa8 79 for (j = 0; j < open.size (); ++j) {
IceTeam 39:ca4dd3faffa8 80 if (open[j] == tmp.p2)
IceTeam 39:ca4dd3faffa8 81 val[2] = false;
IceTeam 39:ca4dd3faffa8 82 }
IceTeam 39:ca4dd3faffa8 83 for (j = 0; j < close.size (); ++j) {
IceTeam 39:ca4dd3faffa8 84 if (close[j] == tmp.p2)
IceTeam 39:ca4dd3faffa8 85 val[2] = false;
IceTeam 39:ca4dd3faffa8 86 }
IceTeam 39:ca4dd3faffa8 87 if (val[2]) {
IceTeam 39:ca4dd3faffa8 88 open.push_back (pointParcours (tmp.p2, &close[indTMP1], &arr));
IceTeam 39:ca4dd3faffa8 89 }
IceTeam 39:ca4dd3faffa8 90
IceTeam 39:ca4dd3faffa8 91 // On répète l'opération pour le quatrieme point
IceTeam 39:ca4dd3faffa8 92 for (j = 0; j < os; ++j)
IceTeam 39:ca4dd3faffa8 93 if (obs[j].getCroisement (tmp.p3, close[indTMP1]))
IceTeam 39:ca4dd3faffa8 94 val[3] = false;
IceTeam 39:ca4dd3faffa8 95 for (j = 0; j < open.size (); ++j) {
IceTeam 39:ca4dd3faffa8 96 if (open[j] == tmp.p3)
IceTeam 39:ca4dd3faffa8 97 val[3] = false;
IceTeam 39:ca4dd3faffa8 98 }
IceTeam 39:ca4dd3faffa8 99 for (j = 0; j < close.size (); ++j) {
IceTeam 39:ca4dd3faffa8 100 if (close[j] == tmp.p3)
IceTeam 39:ca4dd3faffa8 101 val[3] = false;
IceTeam 39:ca4dd3faffa8 102 }
IceTeam 39:ca4dd3faffa8 103 if (val[3]) {
IceTeam 39:ca4dd3faffa8 104 open.push_back (pointParcours (tmp.p3, &close[indTMP1], &arr));
IceTeam 39:ca4dd3faffa8 105 }
IceTeam 39:ca4dd3faffa8 106
IceTeam 39:ca4dd3faffa8 107 val[0] = true;
IceTeam 39:ca4dd3faffa8 108 val[1] = true;
IceTeam 39:ca4dd3faffa8 109 val[2] = true;
IceTeam 39:ca4dd3faffa8 110 val[3] = true;
IceTeam 39:ca4dd3faffa8 111
IceTeam 39:ca4dd3faffa8 112 /*
IceTeam 39:ca4dd3faffa8 113 if (val[0]) { // Si le point est accessible et n'existe pas deja
IceTeam 39:ca4dd3faffa8 114 open.push_back (pointParcours (tmp.p0, &close[indTMP1], &arr)); // On le rajoute dans la liste ouverte
IceTeam 39:ca4dd3faffa8 115 ended = true; // On suppose que le parcours s'arrete ici
IceTeam 39:ca4dd3faffa8 116 for (int a = 0; a < os && ended; ++a)
IceTeam 39:ca4dd3faffa8 117 if (obs[a].getCroisement (tmp.p0, arr)) { // Si on croise un obstacle, le parcours ne s'arrete pas !
IceTeam 39:ca4dd3faffa8 118 ended = false;
IceTeam 39:ca4dd3faffa8 119 std::cout << "Croisement de " << tmp.p0.getX() << " - " << tmp.p0.getY() << " l'obstacle " << a << std::endl;
IceTeam 39:ca4dd3faffa8 120 }
IceTeam 39:ca4dd3faffa8 121 if (ended)
IceTeam 39:ca4dd3faffa8 122 PointEnding = open.size ();
IceTeam 39:ca4dd3faffa8 123 }
IceTeam 39:ca4dd3faffa8 124 if (val[1] && !ended) {
IceTeam 39:ca4dd3faffa8 125 ended = true;
IceTeam 39:ca4dd3faffa8 126 open.push_back (pointParcours (tmp.p1, &close[indTMP1], &arr));
IceTeam 39:ca4dd3faffa8 127 for (int a = 0; a < os && ended; ++a)
IceTeam 39:ca4dd3faffa8 128 if (obs[a].getCroisement (tmp.p1, arr)) {
IceTeam 39:ca4dd3faffa8 129 ended = false;
IceTeam 39:ca4dd3faffa8 130 std::cout << "Croisement de " << tmp.p1.getX () << " - " << tmp.p1.getY () << " l'obstacle " << a << std::endl;
IceTeam 39:ca4dd3faffa8 131 }
IceTeam 39:ca4dd3faffa8 132 if (ended)
IceTeam 39:ca4dd3faffa8 133 PointEnding = open.size ();
IceTeam 39:ca4dd3faffa8 134 }
IceTeam 39:ca4dd3faffa8 135 if (val[2] && !ended) {
IceTeam 39:ca4dd3faffa8 136 ended = true;
IceTeam 39:ca4dd3faffa8 137 open.push_back (pointParcours (tmp.p2, &close[indTMP1], &arr));
IceTeam 39:ca4dd3faffa8 138 for (int a = 0; a < os && ended; ++a)
IceTeam 39:ca4dd3faffa8 139 if (obs[a].getCroisement (tmp.p2, arr)) {
IceTeam 39:ca4dd3faffa8 140 ended = false;
IceTeam 39:ca4dd3faffa8 141 std::cout << "Croisement de " << tmp.p2.getX () << " - " << tmp.p2.getY () << " l'obstacle " << a << std::endl;
IceTeam 39:ca4dd3faffa8 142 }
IceTeam 39:ca4dd3faffa8 143 if (ended)
IceTeam 39:ca4dd3faffa8 144 PointEnding = open.size ();
IceTeam 39:ca4dd3faffa8 145 }
IceTeam 39:ca4dd3faffa8 146 if (val[3] && !ended) {
IceTeam 39:ca4dd3faffa8 147 ended = true;
IceTeam 39:ca4dd3faffa8 148 open.push_back (pointParcours (tmp.p3, &close[indTMP1], &arr));
IceTeam 39:ca4dd3faffa8 149 for (int a = 0; a < os && ended; ++a)
IceTeam 39:ca4dd3faffa8 150 if (obs[a].getCroisement (tmp.p3, arr)) {
IceTeam 39:ca4dd3faffa8 151 ended = false;
IceTeam 39:ca4dd3faffa8 152 std::cout << "Croisement de " << tmp.p3.getX () << " - " << tmp.p3.getY () << " l'obstacle " << a << std::endl;
IceTeam 39:ca4dd3faffa8 153 }
IceTeam 39:ca4dd3faffa8 154 if (ended)
IceTeam 39:ca4dd3faffa8 155 PointEnding = open.size ();
IceTeam 39:ca4dd3faffa8 156 }
IceTeam 39:ca4dd3faffa8 157
IceTeam 39:ca4dd3faffa8 158 val[0] = true;
IceTeam 39:ca4dd3faffa8 159 val[1] = true;
IceTeam 39:ca4dd3faffa8 160 val[2] = true;
IceTeam 39:ca4dd3faffa8 161 val[3] = true;*/
IceTeam 39:ca4dd3faffa8 162 }
IceTeam 39:ca4dd3faffa8 163 }
IceTeam 39:ca4dd3faffa8 164 }
IceTeam 39:ca4dd3faffa8 165
IceTeam 39:ca4dd3faffa8 166 /* 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 ... */
IceTeam 39:ca4dd3faffa8 167 if (ended) {
IceTeam 39:ca4dd3faffa8 168 pointParcours* pente;
IceTeam 39:ca4dd3faffa8 169 pente = &close[0];
IceTeam 39:ca4dd3faffa8 170 while (pente != NULL) {
IceTeam 39:ca4dd3faffa8 171 path.push_first (*pente);
IceTeam 39:ca4dd3faffa8 172 pente = pente->getPere ();
IceTeam 39:ca4dd3faffa8 173 }
IceTeam 39:ca4dd3faffa8 174 path.push_back (pointParcours(arr, &path[path.size()-1], &arr));
IceTeam 39:ca4dd3faffa8 175 endedParc = true;
IceTeam 39:ca4dd3faffa8 176 }
IceTeam 39:ca4dd3faffa8 177 }