summaryrefslogtreecommitdiff
path: root/2005/i/robert/src/ovision/see/map.cc
diff options
context:
space:
mode:
Diffstat (limited to '2005/i/robert/src/ovision/see/map.cc')
-rw-r--r--2005/i/robert/src/ovision/see/map.cc269
1 files changed, 269 insertions, 0 deletions
diff --git a/2005/i/robert/src/ovision/see/map.cc b/2005/i/robert/src/ovision/see/map.cc
new file mode 100644
index 0000000..a05563a
--- /dev/null
+++ b/2005/i/robert/src/ovision/see/map.cc
@@ -0,0 +1,269 @@
+// 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 <iostream>
+#include <math.h>
+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<tBALL>::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<tBALL>::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 = 296 - 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<tBALL>::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<tBALL>::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;
+}