src/motion/arc.cc

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