src/motion/arc.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
  20 #include "trgen.h"
  21 #include "trgendbg.h"
  22 #include <stdio.h>
  23
  24 namespace Segment {
  25
  26     Arc::Arc(Point *_p1, Point *_p2, double _radius) :
  27             p1(_p1), p2(_p2), radius(fabs(_radius)) {
  28
  29         // Find the center of the arc
  30         double angp1p2 = p1->angleTo(*p2);
  31         double m = p1->distanceTo(*p2)/2.0;
  32         if (radius < m) {
  33             radius = m;
  34             dbgPrintf("EEEERRRRRRRRROOOOOOOOORRRRRRRRR!!!!!!!!!!\n");
  35         }
  36         double angcen = acos(m/radius);
  37         if (_radius < 0) angcen = -angcen;
  38         center = Point(p1->x + radius * cos(angp1p2+angcen),
  39                        p1->y + radius * sin(angp1p2+angcen));
  40
  41         startAngle = center.angleTo(*p1);
  42         angle = center.angleTo(*p2) - startAngle;
  43         if (angle < -M_PI) angle += 2.0*M_PI;
  44         if (angle > +M_PI) angle -= 2.0*M_PI;
  45
  46         length = fabs(angle*radius);
  47     }
  48
  49     void Arc::setMaxV(const TrajectoryConstraints &constr) {
  50         double r = radius;
  51         maxv = fmin(constr.maxv, constr.maxomega * r);
  52         maxv = fmin(maxv, sqrt(constr.maxcenacc * r));
  53     }
  54
  55     /**
  56      * not implemented, because this class is not used any more
  57      */
  58     double Arc::getDistance(double time) const {
  59         return -1.0;
  60     }
  61
  62     void Arc::getPointAt(double distance, Point *p) {
  63         double ratio, a;
  64         if (distance > 0) {
  65             ratio = distance/length;
  66         } else {
  67             distance = -distance;
  68             ratio = (length-distance)/length;
  69         }
  70         a = startAngle + ratio*angle;
  71         p->x = center.x + radius*cos(a);
  72         p->y = center.y + radius*sin(a);
  73     }
  74     void Arc::shortenBy(double distance, Point *newEnd) {
  75         getPointAt(-distance, newEnd);
  76         if (distance > 0) {
  77             angle *= (length-distance)/length;
  78             p2 = newEnd;
  79         } else {
  80             distance = -distance;
  81             startAngle = startAngle + distance/length*angle;
  82             angle *= (length-distance)/length;
  83             p1 = newEnd;
  84         }
  85         length -= distance;
  86     }
  87
  88     TrajectorySegment* Arc::splitAt(double distance, Point *newEnd) {
  89         if (distance <= 0 || distance >= length)
  90             return NULL;
  91
  92         getPointAt(distance, newEnd);
  93         Arc *ns = new Arc(*this);
  94
  95         double a = distance/length*angle;
  96         angle = a;
  97         length = distance;
  98         p2 = newEnd;
  99
 100         ns->startAngle += a;
 101         ns->angle -= a;
 102         ns->length -= distance;
 103         ns->p1 = newEnd;
 104         return ns;
 105     }
 106
 107     void Arc::getRefPos(double time, Pos &rp) {
 108         double t = time-t1;
 109         double fraction = t/(t2-t1);
 110         double distance = (v1 + 0.5*acc*t) * t;
 111         double a = startAngle + distance/length*angle;
 112         rp.x = center.x + radius*cos(a);
 113         rp.y = center.y + radius*sin(a);
 114         if (angle > 0)
 115             rp.phi = a + M_PI/2.0;
 116         else
 117             rp.phi = a - M_PI/2.0;
 118
 119         rp.v = v1+fraction*(v2-v1);
 120         rp.omega =rp.v/radius;
 121         if (angle < 0) rp.omega = -rp.omega;
 122
 123     }
 124 #ifdef MATLAB_MEX_FILE
 125     void Arc::plot(const char *style) {
 126         char cmd[300];
 127         const int len = 10;
 128         int i;
 129         Pos rp;
 130         mxArray *x = mxCreateDoubleMatrix(1, len, mxREAL);
 131         mxArray *y = mxCreateDoubleMatrix(1, len, mxREAL);
 132         mxArray *s = mxCreateCharMatrixFromStrings(1, &style);
 133         mxArray *rhs[] = {x,y,s};
 134
 135         for (i=0; i < len; i++) {
 136             getRefPos(t1+(t2-t1)*i/(len-1), rp);
 137             mxGetPr(x)[i] = rp.x;
 138             mxGetPr(y)[i] = rp.y;
 139         }
 140         mexCallMATLAB(0, NULL, 3, rhs, "plot");
 141         sprintf(cmd, "plot([%g %g], [%g %g], 'mo')",
 142                 p1->x, p2->x, p1->y, p2->y);
 143         mexEvalString(cmd);
 144         mxDestroyArray(x);
 145         mxDestroyArray(y);
 146         mxDestroyArray(s);
 147     };
 148 #endif
 149
 150 } // namespace Segment