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