src/motion/line.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     Line::Line(Point *_p1, Point *_p2) : p1(_p1), p2(_p2) {
  26         length = p1->distanceTo(*p2);
  27         angle = p1->angleTo(*p2);
  28         sinphi = (p2->y-p1->y)/length;
  29         cosphi = (p2->x-p1->x)/length;
  30     }
  31
  32     //Line::~Line() {};
  33
  34     void Line::setMaxV(const TrajectoryConstraints &constr) {
  35         maxv = constr.maxv;
  36         v1 = maxv;
  37         v2 = maxv;
  38     }
  39
  40     double Line::getDistance(double time) const {
  41         time -= t1;
  42         return (0.5*acc*time*time + v1*time);
  43     }
  44
  45     void Line::getPointAt(double distance, Point *p) {
  46         double ratio;
  47         if (distance > 0) {
  48             ratio = distance/length;
  49         } else {
  50             ratio = (length+distance)/length;
  51         }
  52         p->x = p1->x + ratio*(p2->x - p1->x);
  53         p->y = p1->y + ratio*(p2->y - p1->y);
  54     }
  55
  56     void Line::shortenBy(double distance, Point *newEnd) {
  57         getPointAt(-distance, newEnd);
  58
  59         if (distance > 0) {     // end cut off
  60                 p2 = newEnd;
  61         } else {                // beginning cut off
  62                 distance = -distance;
  63                 p1 = newEnd;
  64         }
  65         length -= distance;
  66     }
  67
  68     TrajectorySegment* Line::splitAtByTime(double time, Point *newEnd) {
  69         if (time <= t1 || time >= t2) {
  70             dbgPrintf("splitAt: time=%g length=%g\n", time, length);
  71             return NULL;
  72         }
  73         time -= t1;
  74         double dst = 0.5 * acc * time * time + v1 * time; // where to split from beginning
  75         getPointAt(dst, newEnd);
  76         Line *ns = new Line(*this);
  77         p2 = newEnd;
  78         length = dst;
  79         ns->length -= dst;
  80         ns->p1 = newEnd;
  81
  82         ns->v1 = v2 = v1 + acc*time; // need to update speeds
  83
  84         return ns;
  85     }
  86
  87     TrajectorySegment* Line::splitAt(double distance, Point *newEnd) {
  88         if (distance <= 0 || distance >= length) {
  89             dbgPrintf("splitAt: distance=%g length=%g\n", distance, length);
  90             return NULL;
  91         }
  92
  93         getPointAt(distance, newEnd);
  94         Line *ns = new Line(*this);
  95         p2 = newEnd;
  96         double orig_length = length;
  97         length = distance;
  98         ns->length -= distance;
  99         ns->p1 = newEnd;
 100
 101         // s=1/2 a t2   t2 = 2as   t=sqrt(2as) time
 102         v2     =     v1 + (    v2-    v1) * (    length/orig_length); // need to update speeds
 103         ns->v1 = ns->v2 - (ns->v2-ns->v1) * (ns->length/orig_length);
 104
 105         return ns;
 106     }
 107
 108     void Line::getRefPos(double time, Pos &rp) {
 109         double t = time-t1;
 110         double fraction = t/(t2-t1);
 111         double distance = (v1 + 0.5*acc*t) * t;
 112         rp.x = p1->x + distance*cosphi;
 113         rp.y = p1->y + distance*sinphi;
 114         rp.phi = angle;
 115
 116         rp.v = v1+fraction*(v2-v1);
 117         rp.omega = 0;
 118
 119     }
 120 #ifdef MATLAB_MEX_FILE
 121     void Line::plot(const char *style) {
 122         char cmd[300];
 123         sprintf(cmd, "plot([%g %g], [%g %g], '%so')",
 124                 p1->x, p2->x, p1->y, p2->y, style);
 125         mexEvalString(cmd);
 126     };
 127 #endif
 128
 129 } // namespace Segment