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