src/robofsm/eb2010misc.cc

   1 #include <map.h>
   2 #include <trgen.h>
   3 #include "robot.h"
   4 #include "corns_configs.h"
   5 #include <ul_log.h>
   6
   7 UL_LOG_CUST(ulogd_eb2010misc); /* Log domain name = ulogd + name of the file */
   8
   9 #include "eb2010misc.h"
  10
  11 /************************************************************************
  12  * Functions to manipulate map, compute distances, choose corns etc.
  13  ************************************************************************/
  14 double x_coord(double x, enum which_side which_side)
  15 {
  16         switch(which_side) {
  17                 case MINE:
  18                         return x;
  19                         break;
  20                 case OPPONENTS:
  21                         return PLAYGROUND_WIDTH_M - x;
  22                         break;
  23                 default:
  24                         goto error;
  25         }
  26 error:
  27                         ul_logfatal("ERROR enum which_side definition changed!!\n");
  28                         robot_exit();
  29                         return 0; // avoid warning
  30 }
  31
  32 // returns pointer to next real (non-fake) corn which was not yet collected
  33 // TODO: note: use this for "short_time_to_end: situation only, otherwise
  34 // it is better to try to rush more corns at once
  35 struct corn * choose_next_corn()
  36 {
  37         Point cornPosition;
  38
  39         double maxDistance = 0; // 2 * PLAYGROUND_HEIGHT_M; // "infinity"
  40         struct corn *minCorn = NULL, *corn;
  41         // robot.corns->corns[NUM_OF_FAKE_CORNS] is first non-fake corn in the robot.corns structure
  42         for(corn = &robot.corns->corns[NUM_OF_FAKE_CORNS]; corn < &robot.corns->corns[robot.corns->corns_count]; corn++) {
  43                 cornPosition.x = corn->position.x;
  44                 cornPosition.y = corn->position.y;
  45                 double distance = cornPosition.distanceTo(containerPosition);
  46                 ul_logmsg("maxDistance = %.3f; new corn dist = %.3f, it x position = %.3f; %s\n",
  47                         maxDistance, distance, corn->position.x, (corn->was_collected?"W":"N"));
  48
  49                 if (distance > maxDistance && corn->position.x > 2.2 && corn->was_collected == false) {
  50                         maxDistance = distance;
  51                         minCorn = corn;
  52                         ul_logmsg("\tchoose it\n");
  53                 }
  54         }
  55
  56         if (minCorn) ul_logmsg("\tmin distance was: %.3f  ", maxDistance);
  57         return minCorn;
  58 }
  59
  60 /**
  61  * Computes and returns line to point distance
  62  * @param[in] p the point coords
  63  * @param[in] lp1 coords of one of the points on the line
  64  * @param[in] lp2 coords of the second point on the line
  65  */
  66 double get_point_to_line_distance(const Point &p, const Point &lp1, const Point &lp2)
  67 {
  68         double distance = fabs((lp2.x - lp1.x)*(lp1.y-p.y) - (lp1.x - p.x)*(lp2.y - lp1.y))
  69                 / sqrt((lp2.x - lp1.x)*(lp2.x - lp1.x) + (lp2.y - lp1.y)*(lp2.y - lp1.y));
  70         return distance;
  71 }
  72
  73 Pos * get_corn_approach_position(struct corn *corn)
  74 {
  75         const Point approxContainerPosition(PLAYGROUND_WIDTH_M - 0.15, 0.02); // blue container Position
  76         Pos *p = new Pos; // robot position result
  77
  78         Point cornPosition(corn->position.x, corn->position.y);
  79         double a = approxContainerPosition.angleTo(cornPosition);
  80
  81         p->x = cornPosition.x + cos(a)*(CORN_NEIGHBOURHOOD_RADIUS_M + 0.07);
  82         p->y = cornPosition.y + sin(a)*(CORN_NEIGHBOURHOOD_RADIUS_M + 0.07);
  83         p->phi = a + M_PI;
  84
  85         return p;
  86 }
  87
  88 void remove_wall_around_corn(const double x, const double y)
  89 {
  90         ShmapSetCircleFlag(x, y, CORN_NEIGHBOURHOOD_RADIUS_M, 0, MAP_FLAG_WALL);
  91 }
  92
  93