// map.cc - Classe Map // robert - Programme du robot APBteam // Copyright (C) 2005 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.hh" #include #include using namespace std; #define TABLE_WIDTH 2100 #define TABLE_HEIGHT 2400 /// Constructeurs. Map::Map (Space *space) : lock (0); treeFound (0), checkCurBall (0) // : motor (Motor::getInstance ()), tracker (motor.getTracker ()) { oconfig = OConfig::GetInstance (); Map::oconfig = oconfig; Map::space = space; posGoal[0] = 105; posGoal[1] = 0; } /// Destructeur. Map::~Map (void) { } /// Donne la position dans le référentiel de la table 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); // Vue de la balle partiel ou en bas de l'ecran 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) { // Parcours de toutes les balles déjà trouvées 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); } /// Balle lockée ? bool Map::IsLock() { return lock; } /// Lock ou unlock 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] + 35; y = curBall->position[1]; cout << "position balle: " << x << " " << y << endl; 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]; double centYMax = 900; // On supprime l'ancienne liste de balle ball.clear (); checkCurBall = 0; while (pCur) { //XXX a enlever XXX pCur->centery = oconfig->height - pCur->centery; // On choppe la position par rapport au robot space->GetLoc (pCur->centerx, pCur->centery, pos[0], pos[1]); AddBall(pos, pCur); checkCurBall = 1; // Place la balle la plus proche dans le pointeur curBall if (pCur->centery < centYMax) { curBall = ball.end (); curBall--; centYMax = pCur->centery; } pCur = pCur->next; } // Detection de poteau ou de goal pCur = group->zoneListGoal; if (pCur) { treeFound = 1; space->GetLoc (pCur->centerx, pCur->ymax, 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] << endl; i++; } cout << endl; }