// map.cc - Classe Map // nono - Programme du robot Efrei Robotique I1-I2 2004 // Copyright (C) 2004 Olivier Gaillard /// @file map.cc Fourni la liste des balles presentes sur le terrain et permet le choix rapide et efficace de la prochaine balle a aller chercher #include "map.h" #include #include using namespace std; #define TABLE_WIDTH 2100 #define TABLE_HEIGHT 2400 /// Constructeurs. Map::Map (Space *space) // : motor (Motor::getInstance ()), tracker (motor.getTracker ()) { oconfig = OConfig::GetInstance (); lock = 0; treeFound = 0; checkCurBall = 0; Map::oconfig = oconfig; Map::space = space; posGoal[0] = 105; posGoal[1] = 0; } /// Destructeur. Map::~Map (void) { } void Map::GetPosFromLoc (int locImgX, int locImgY, double &posX, double &posY) { double locX, locY; double posRobotX, posRobotY, angleRobot; space->GetLoc(locImgX, locImgY, locX, locY); // tracker.getPos(posRobotX, posRobotY, angleRobot); space->GetPos(locX, locY, posRobotX, posRobotY, angleRobot, posX, posY); } /// Ajoute une balle a la map void Map::AddBall(double *pos, ZONE *pZone) { tBALL ballTmp; ballTmp.position[0] = pos[0]; ballTmp.position[1] = pos[1]; // definit la position de la balle // ballTmp.zone = (double)((int)(pos[0])%300 + ((int)(pos[1])%300)*7); ballTmp.partial = pZone->partial; if (pZone->bottom) ballTmp.bottom = oconfig->ball_bottom_time_out ; else ballTmp.bottom = 0; ballTmp.skepticism = 0; ballTmp.precision = pZone->centery; // calcul du score partie ballTmp.preScore = 0; // oconfig->distance_ball_goal_weight * Dist (pos, posGoal); ball.push_back(ballTmp); } /// Supprime une balle de la map void Map::DelBall(list::iterator &iter) { ball.erase(iter); checkCurBall = 0; } /// Test si une balle trouvee correspond a une balle de la map int Map::TestSimilarBall(ZONE *pBall, list::iterator &iter) { for(iter = ball.begin(); iter != ball.end(); iter++) { double pos[2] = {pBall->centerx, pBall->centery}; if (Dist(iter->position, pos) < ((iter->partial || pBall->partial)?oconfig->map_error_part:oconfig->map_error)) return 1; } return 0; } /// Calcul de distance double Map::Dist(double pos1X, double pos1Y, double pos2X, double pos2Y) { double x = pos2X - pos1X; double y = pos2Y - pos1Y; return sqrt((double)(x * x + y * y)); } double Map::Dist(double *pos1, double *pos2) { double x = pos2[0]-pos1[0]; double y = pos2[1]-pos1[1]; return sqrt((double)(x * x + y * y)); } double Map::Angle (double ballPosY, double robotPosY, double distRobotBall) { return acos ((ballPosY - robotPosY)/distRobotBall); } /// Retourne si une balle est locke bool Map::IsLock() { return lock; } /// Lock une balle pour savoir quel balle le robot suit void Map::SetLock(bool value) { lock = value; } /// Donne la position de la balle la plus proche ou locke bool Map::getCurBallPos (double &x, double &y) { if (checkCurBall == true) { x = curBall->position[0]; y = curBall->position[1]; return 1; } return false; } /// Retourne si un palmier est sur l'image bool Map::IsTree () { return treeFound; } /// Donne la position du palmier ŕ l'image bool Map::GetTreePos (double &x, double &y) { if (treeFound == true) { x = posTree[0]; y = posTree[1]; return 1; } return 0; } /// Met a jour la map avec une nouvelle liste de balle void Map::AddBallsToMap(Group *group) { ZONE *pCur = group->zoneListBall; double pos[2]; bool ballLost = 1; while (pCur) { // TODO passer par la classe space //GetPosFromLoc pos[0] = pCur->centerx; pos[1] = pCur->centery; // Cherche si la balle est deja dans la liste list::iterator iCur; // On regarde si le group trouvé n'est pas en dehors de la table if (1) //TODO (pos[0] < TABLE_WIDTH) && (pos[1] < TABLE_HEIGHT)) { // Si la balle n'est pas dans la liste on l'ajoute if (!TestSimilarBall(pCur, iCur)) { AddBall(pos, pCur); } // Si elle l'est else { // on regarde si c'est la balle qui a ete locké if (IsLock ()) if (iCur == curBall) ballLost = 0; // on incremente son marqueur de viabilite if (iCur->skepticism < oconfig->skepticism_max) iCur->skepticism++; if (pCur->centery <= iCur->precision) { ///TODO mis a jour de la position iCur->position[0] = pCur->centerx; iCur->position[1] = pCur->centery; iCur->bottom = pCur->bottom * oconfig->ball_bottom_time_out; } } } pCur = pCur->next; } // TODO decremente d'autre marqueur if (IsLock () && ballLost) { // si la balle sort par le bas pour etre prise if (curBall->bottom) curBall->bottom--; else curBall->skepticism -= oconfig->ball_lost_weight; } // presence robot ennemi // Detection de poteau ou de goal pCur = group->zoneListGoal; if (pCur) { treeFound = 1; space->GetLoc (pCur->centerx, pCur->ymin, posTree[0], posTree[1]); } else treeFound = 0; } /// Met a jour les scores void Map::UpdateMap() { double robotPosX, robotPosY, robotAngle; double distRobotBall; // On met a jour tous les scores list::iterator iter; for(iter = ball.begin(); iter != ball.end(); iter++) { distRobotBall = Dist(robotPosX, robotPosY, iter->position[0], iter->position[1]); iter->score = iter->preScore + oconfig->distance_ball_robot_weight * distRobotBall // + oconfig->angle_ball_weight * abs (robotAngle - Angle (iter->position[2], robotPosY, distRobotBall)); // + oconfig->ball_density_weight * // + oconfig->ennemy_presence_weight * + oconfig->skepticism_weight * iter->skepticism; // + oconfig->ball_precision_weight * // + ... iter->partial; if (checkCurBall) if ((!IsLock()) && (iter->score > curBall->score)) { curBall = iter; checkCurBall = 1; } } } /// Affiche les balles de la map void Map::ShowBalls() { list::iterator iter; int i=0; cout << "balles: (" << ball.size () << ")" << endl;; /// Parcours de la liste for(iter = ball.begin(); iter != ball.end(); iter++) { /// Affichage des infos de la balle cout << i << ": " << iter->position[0] << "\t" << iter->position[1] << "\t" << iter->skepticism << "\t" << iter->score << "\t" << iter->bottom << endl; i++; } cout << endl; }