summaryrefslogtreecommitdiff
path: root/2004/i/nono/src/ovision/mapOld.cc
diff options
context:
space:
mode:
Diffstat (limited to '2004/i/nono/src/ovision/mapOld.cc')
-rw-r--r--2004/i/nono/src/ovision/mapOld.cc299
1 files changed, 299 insertions, 0 deletions
diff --git a/2004/i/nono/src/ovision/mapOld.cc b/2004/i/nono/src/ovision/mapOld.cc
new file mode 100644
index 0000000..9dcb5c0
--- /dev/null
+++ b/2004/i/nono/src/ovision/mapOld.cc
@@ -0,0 +1,299 @@
+// 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 <iostream>
+#include <math.h>
+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<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)
+{
+ 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<tBALL>::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<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] << "\t" << iter->skepticism << "\t" << iter->score << "\t" << iter->bottom << endl;
+ i++;
+ }
+ cout << endl;
+}