File content as of revision 76:cd8c7da76768:

#include "map.h"

/* Dernier Changement : Romain 20h30 */

void map::Build_Objectives() {
    if (objectifs.empty()) {
        logger.printf("map::Build_Objectives Creation des Objectifs ...\n\r");
        if (couleur == VERT) {
            addObj (objectif (OBJ_BLOC_PRISE, 120, 1000, 0, Codo, Parasol, pince));
            addObj (objectif (OBJ_BLOC_LACHE, 120, 1000, 0, Codo, Parasol, pince, 50));
        }
        else {
            addObj (objectif (OBJ_BLOC_PRISE, 3000-350, 850, 0, Codo, Parasol, pince));
            addObj (objectif (RIEN, 3000-550, 850, 0, Codo, Parasol, pince));
            addObj (objectif (OBJ_BLOC_LACHE, 3000-1050, 1100, 0, Codo, Parasol, pince));
            addObj (objectif (RIEN, 3000-200, 850, 0, Codo, Parasol, pince));
        }
    }
}

map::map (Odometry* nodo, AX12 * np, ControlleurPince * npince, int ncouleur, int nformation) : Codo(nodo), Parasol(np), pince(npince) {
    couleur = ncouleur;
    formation = nformation;

    if (couleur == VERT) {
        Codo->setPos(X_START_VERT, Y_START, 0);
    }
    else {
        Codo->setPos(X_START_VIOLET, Y_START, -PI);
    }
    Build();
    Build_Objectives();
}

void map::addObs (obsCarr nobs) {
    obs.push_back (nobs);
}
void map::addObj (objectif nobj) {
    objectifs.push_back(nobj);
}

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
    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 est sur la table
                if (tmp.p0.getX() < min_x_table || tmp.p0.getY() < min_y_table || tmp.p0.getX() > max_x_table || tmp.p0.getY() > max_y_table)
                    val[0] = false;
                // On vérifie si le point croise un obstacle
                for (j = 0; j < os && val[0]; ++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 () && val[0]; ++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 () && val[0]; ++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
                if (tmp.p1.getX() < min_x_table || tmp.p1.getY() < min_y_table || tmp.p1.getX() > max_x_table || tmp.p1.getY() > max_y_table)
                    val[1] = false;
                for (j = 0; j < os && val[1]; ++j)
                    if (obs[j].getCroisement (tmp.p1, close[indTMP1]))
                        val[1] = false;
                for (j = 0; j < open.size () && val[1]; ++j) {
                    if (open[j] == tmp.p1)
                        val[1] = false;
                }
                for (j = 0; j < close.size () && val[1]; ++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
                if (tmp.p2.getX() < min_x_table || tmp.p2.getY() < min_y_table || tmp.p2.getX() > max_x_table || tmp.p2.getY() > max_y_table)
                    val[2] = false;
                for (j = 0; j < os && val[2]; ++j)
                    if (obs[j].getCroisement (tmp.p2, close[indTMP1]))
                        val[2] = false;
                for (j = 0; j < open.size () && val[2]; ++j) {
                    if (open[j] == tmp.p2)
                        val[2] = false;
                }
                for (j = 0; j < close.size () && val[2]; ++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
                if (tmp.p3.getX() < min_x_table || tmp.p3.getY() < min_y_table || tmp.p3.getX() > max_x_table || tmp.p3.getY() > max_y_table)
                    val[3] = false;
                for (j = 0; j < os && val[3]; ++j)
                    if (obs[j].getCroisement (tmp.p3, close[indTMP1]))
                        val[3] = false;
                for (j = 0; j < open.size () && val[3]; ++j) {
                    if (open[j] == tmp.p3)
                        val[3] = false;
                }
                for (j = 0; j < close.size () && val[3]; ++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;
}

void map::Execute(int obj) {
    objectif o = objectifs[obj];
    logger.printf("map::Execute(int obj) Realisation de l'objectif %d\n\r", obj);
    logger.printf("Depart [%f, %f] Objectif [%f, %f]\n\r", Codo->getX(), Codo->getY(), o.getX(), o.getY());
    // logger.printf("Findway %f-%f -> %f-%f\n\r", Codo->getX(), Codo->getY(), XObjectif, YObjectif);
    FindWay (Codo->getX(), Codo->getY(), o.getX(), o.getY());
    
    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());
            Codo->Stop();
            // logger.printf("Goto Fini\n\r");
        }
        Codo->GotoThet(o.getThet());
        o.Action();
        //logger.printf("Chemin fini !\n\r");
    }
    else {
        logger.printf("Chemin pas trouve ...\n\r");
    }
    endedParc = false;
}

void map::Execute() {
    for (int i = 0; i < objectifs.size();++i) {
        logger.printf("map::Execute() Realisation de l'objectif %d\n\r", i);
        Execute(i);
    }
}

void map::Build () {
    logger.printf("map::Build Creation des obstacles ...\n\r");
    if (couleur == VERT) {
        max_x_table = 1400;
        max_y_table = 1900;
        min_x_table = 100;
        min_y_table = 100;
    }
    else {
        max_x_table = 2900;
        max_y_table = 1900;
        min_x_table = 1600;
        min_y_table = 100;
    }

    if (couleur == VERT) {
        // Attention les commentaires sont inversées par rapport aux valeur en x et y des obstacles.
        // Il faut lire les commentaires de la façon dont la carte est présentée dans le règlement
        // Un / signifie un point de départ. Milieu/Haut/Haut signifie que l'on part du milieu, que l'on va en haut puis encore en haut  

        addObs(obsCarr (0, 2000, 250, 150));  // Coté haut droite
        addObs(obsCarr (200, 2000, 200, 50));

        addObs(obsCarr (800, 100, 100, 15));    // Petit obstacle en haut à gauche
    }
    else {
        addObs(obsCarr (3000, 2000, 250, 150));     // Coté bas droite
        addObs(obsCarr (2800, 2000, 200, 50));

        addObs(obsCarr (2200, 100, 100, 15));   // Petit Obstacle en haut à gauche
    }

    addObs(obsCarr (1500, 750, 1100, 15));      // Obstacle du milieu à la verticale
    addObs(obsCarr (1500, 1050, 20, 300));      // Vitre du milieu (horizontale)
    
    if (formation == 1) { 
        Build_formation_1 (couleur);
    }
    if (formation == 2) {
        Build_formation_2 (couleur);        
    }
    if (formation == 3) {
        Build_formation_3 (couleur);   
    }
    else {
        addObs(obsCarr (1250, 1000, 220, 220)); // Obstacles du test standard hors-coupe. A ignorer
        addObs(obsCarr (1500, 750, 220, 220));
        addObs(obsCarr (1500, 1250, 220, 220));   
    }
}

void map::Build_formation_1 (int couleur) {
    logger.printf("map::Build_formation1 Ajout des coquillages de la formation 1 ...\n\r");
    if (couleur == VERT) {
        addObs(obsCarr (200, 2000-450, 40, 40));    // Coquillage du haut - droite
        addObs(obsCarr (200, 2000-750, 40, 40));    // Coquillage sur le même axe horizontal
        
        addObs(obsCarr (900, 2000-550, 40, 40));    // Coqullage du milieu/haut/haut
        addObs(obsCarr (1200, 2000-350, 40, 40));   // Coquillage du milieu/haut
        
        addObs(obsCarr (1500, 2000-550, 40, 40));   // Coquillage du milieu gauche
        addObs(obsCarr (1500, 2000-350, 40, 40));   // Coquillage du milieu droit
        
        //addObs(obsCarr (3000-900, 2000-550, 40, 40));
        addObs(obsCarr (3000-1200, 2000-350, 40, 40));   // Coquillage du milieu bas
        
        //addObs(obsCarr (3000-200, 2000-450, 40, 40));   // Coquillages du bas droite
        //addObs(obsCarr (3000-200, 2000-750, 40, 40));        
    }
    else {
        //addObs(obsCarr (200, 2000-450, 40, 40));   // Coquillages du haut droit
        //addObs(obsCarr (200, 2000-750, 40, 40));
        
        //addObs(obsCarr (900, 2000-550, 40, 40));
        addObs(obsCarr (1200, 2000-350, 40, 40)); // Coquillages du milieu/haut
        
        addObs(obsCarr (1500, 2000-550, 40, 40));   // Coquillage du milieu gauche
        addObs(obsCarr (1500, 2000-350, 40, 40));   // Coquillage du milieu droite
        
        addObs(obsCarr (3000-900, 2000-550, 40, 40));   // Coquillage du milieu/bas 
        addObs(obsCarr (3000-1200, 2000-350, 40, 40));  // Coquillage du milieu/bas/bas
        
        addObs(obsCarr (3000-200, 2000-450, 40, 40));   // Coquillages du bas droite
        addObs(obsCarr (3000-200, 2000-750, 40, 40));   // Coquillage sur le même axe horizontal       
    }
}

void map::Build_formation_2 (int couleur) {
    if (couleur == VERT) {
        addObs(obsCarr (200, 2000-450, 40, 40));    // Coquillage du haut gauche
        addObs(obsCarr (200, 2000-750, 40, 40));    // Coquillage du haut droite

        addObs(obsCarr (600, 2000-450, 40, 40));    // Coquillage du milieu/haut/haut droite
        addObs(obsCarr (600, 2000-750, 40, 40));    // Coquillage du milieu/haut/haut sur le même axe horizontal

        addObs(obsCarr (1200, 2000-350, 40, 40));   // Coquillage du milieu/haut droite
    }
    else {
        //addObs(obsCarr (200, 2000-450, 40, 40));   // Coquillages du haut droit
        //addObs(obsCarr (200, 2000-750, 40, 40));
        
        //addObs(obsCarr (900, 2000-550, 40, 40));
        addObs(obsCarr (3000-200, 2000-450, 40, 40));    // Coquillage du bas gauche
        addObs(obsCarr (3000-200, 2000-750, 40, 40));    // Coquillage du bas droite

        addObs(obsCarr (3000-600, 2000-450, 40, 40));    // Coquillage du milieu bas/bas droite
        addObs(obsCarr (3000-600, 2000-750, 40, 40));    // Coquillage du milieu bas/bas sur le même axe horizontal

        addObs(obsCarr (1800, 2000-350, 40, 40));   // Coquillage du milieu/bas droite
    }
}

void map::Build_formation_3 (int couleur) {
    if (couleur == VERT) {
        addObs(obsCarr (200, 2000-450, 40, 40));    // Coquillage du haut gauche
        addObs(obsCarr (200, 2000-750, 40, 40));    // Coquillage du haut droite

        addObs(obsCarr (600, 2000-450, 40, 40));    // Coquillage du milieu/haut/haut droite
        addObs(obsCarr (600, 2000-750, 40, 40));    // Coquillage du milieu/haut/haut sur le même axe horizontal

        addObs(obsCarr (600, 2000-150, 40, 40));   // Coquillage du milieu/haut droite
    }
    else {
        //addObs(obsCarr (200, 2000-450, 40, 40));   // Coquillages du haut droit
        //addObs(obsCarr (200, 2000-750, 40, 40));
        
        //addObs(obsCarr (900, 2000-550, 40, 40));
        // Inverser bas et haut
        addObs(obsCarr (3000-200, 2000-450, 40, 40));    // Coquillage du bas gauche
        addObs(obsCarr (3000-200, 2000-750, 40, 40));    // Coquillage du bas droite

        addObs(obsCarr (3000-600, 2000-450, 40, 40));    // Coquillage du milieu bas/bas droite
        addObs(obsCarr (3000-600, 2000-750, 40, 40));    // Coquillage du milieu bas/bas sur le même axe horizontal

        addObs(obsCarr (3000-600, 2000-150, 40, 40));   // Coquillage du milieu/bas droite
    }
}