src/motion/turn.cc

   1 //     Copyright 2009 Michal Sojka <sojkam1@fel.cvut.cz>
   2 //     Copyright 2009 Petr Beneš
   3 //
   4 //     This file is part of Trgen library.
   5 //
   6 //     Trgen is free software: you can redistribute it and/or modify
   7 //     it under the terms of the GNU General Public License as published by
   8 //     the Free Software Foundation, either version 3 of the License, or
   9 //     (at your option) any later version.
  10 //
  11 //     Trgen is distributed in the hope that it will be useful,
  12 //     but WITHOUT ANY WARRANTY; without even the implied warranty of
  13 //     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  14 //     GNU General Public License for more details.
  15 //
  16 //     You should have received a copy of the GNU General Public License
  17 //     along with Trgen.  If not, see <http://www.gnu.org/licenses/>.
  18
  19 #include "trgen.h"
  20 #include "trgendbg.h"
  21 #include <stdio.h>
  22
  23 namespace Segment {
  24
  25 /**
  26  * Segment representing turn at one place.
  27  *
  28  * Speed related variables @c v1, @c v2 and @c acc represents
  29  * angular speed here.
  30  *
  31  */
  32     Turn::Turn(Point *_center, double _startHeading, double _turnBy) :
  33                 center(_center), turnBy(_turnBy), startHeading(_startHeading)
  34     {}
  35
  36
  37     void Turn::setMaxV(const TrajectoryConstraints &constr) {
  38             maxv = constr.maxomega;
  39             v1 = constr.maxomega;
  40             v2 = constr.maxomega;
  41     }
  42
  43
  44     double Turn::getDistance(double time) const {
  45         time -= t1;
  46         return 0.5*acc*time*time + v1*time;
  47     }
  48
  49
  50     void Turn::getPointAt(double distance, Point *p) {
  51         *p = *center;
  52     }
  53
  54     TrajectorySegment* Turn::splitAt(double distance, Point *newEnd) {
  55         if (distance <= 0 || distance >= fabs(turnBy)) {
  56             dbgPrintf("splitAt: distance=%g turnBy=%g\n", distance, turnBy);
  57             return NULL;
  58         }
  59
  60         Turn *ns = new Turn(*this);
  61         if (turnBy < 0)
  62                 distance = -distance;
  63         turnBy = distance;
  64         ns->startHeading+=distance;
  65         ns->turnBy -= distance;
  66         return ns;
  67     }
  68
  69     TrajectorySegment* Turn::splitAtByTime(double time, Point *newEnd) {
  70         if (time <= t1 || time >= t2) {
  71             dbgPrintf("splitAt: distance=%g turnBy=%g\n", time, turnBy);
  72             return NULL;
  73         }
  74
  75         Turn *ns = new Turn(*this);
  76         double distance = 0.5 * acc * time * time + v1 * time;
  77         if (turnBy < 0)
  78                 distance = -distance;
  79         turnBy = distance;
  80         ns->startHeading+=distance;
  81         ns->turnBy -= distance;
  82         return ns;
  83     }
  84
  85     void Turn::getRefPos(double time, Pos &rp) {
  86         double t = time-t1;
  87         double fraction = t/(t2-t1);
  88         double distance = (v1 + 0.5*acc*t) * t;
  89         rp.x = center->x;
  90         rp.y = center->y;
  91         if (turnBy > 0)
  92             rp.phi = startHeading + distance;
  93         else
  94             rp.phi = startHeading - distance;
  95         rp.v=0;
  96         if (time < t2) {
  97                 rp.omega = v1+fraction*(v2-v1);
  98                 if (turnBy < 0) rp.omega = -rp.omega;
  99         }
 100         else
 101                 rp.omega = 0;
 102     }
 103
 104 } // namespace Segment