src/robofsm/eb2010misc.cc

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