src/motion/turn.cc

   1 #include "trgen.h"
   2 #include "trgendbg.h"
   3
   4 namespace Segment {
   5
   6 /**
   7  * Segment representing turn at one place.
   8  *
   9  * Speed related variables @c v1, @c v2 and @c acc represents
  10  * angular speed here.
  11  *
  12  */
  13     Turn::Turn(Point *_center, double _startHeading, double _turnBy) :
  14                 center(_center), turnBy(_turnBy), startHeading(_startHeading)
  15     {}
  16
  17
  18     void Turn::setMaxV(const TrajectoryConstraints &constr) {
  19             maxv = constr.maxomega;
  20             v1 = constr.maxomega;
  21             v2 = constr.maxomega;
  22     }
  23
  24
  25     double Turn::getDistance(double time) const {
  26         time -= t1;
  27         return 0.5*acc*time*time + v1*time;
  28     }
  29
  30
  31     void Turn::getPointAt(double distance, Point *p) {
  32         *p = *center;
  33     }
  34
  35     TrajectorySegment* Turn::splitAt(double distance, Point *newEnd) {
  36         if (distance <= 0 || distance >= fabs(turnBy)) {
  37             dbgPrintf("splitAt: distance=%g turnBy=%g\n", distance, turnBy);
  38             return NULL;
  39         }
  40
  41         Turn *ns = new Turn(*this);
  42         if (turnBy < 0)
  43                 distance = -distance;
  44         turnBy = distance;
  45         ns->startHeading+=distance;
  46         ns->turnBy -= distance;
  47         return ns;
  48     }
  49
  50     TrajectorySegment* Turn::splitAtByTime(double time, Point *newEnd) {
  51         if (time <= t1 || time >= t2) {
  52             dbgPrintf("splitAt: distance=%g turnBy=%g\n", time, turnBy);
  53             return NULL;
  54         }
  55
  56         Turn *ns = new Turn(*this);
  57         double distance = 0.5 * acc * time * time + v1 * time;
  58         if (turnBy < 0)
  59                 distance = -distance;
  60         turnBy = distance;
  61         ns->startHeading+=distance;
  62         ns->turnBy -= distance;
  63         return ns;
  64     }
  65
  66     void Turn::getRefPos(double time, Pos &rp) {
  67         double t = time-t1;
  68         double fraction = t/(t2-t1);
  69         double distance = (v1 + 0.5*acc*t) * t;
  70         rp.x = center->x;
  71         rp.y = center->y;
  72         if (turnBy > 0)
  73             rp.phi = startHeading + distance;
  74         else
  75             rp.phi = startHeading - distance;
  76         rp.v=0;
  77         if (time < t2) {
  78                 rp.omega = v1+fraction*(v2-v1);
  79                 if (turnBy < 0) rp.omega = -rp.omega;
  80         }
  81         else
  82                 rp.omega = 0;
  83     }
  84
  85 } // namespace Segment