trgen.cc

Go to the documentation of this file.

#include <exception>
#include <typeinfo>
#include <iterator>
#include "trgen.h"

//#define DEBUG

#ifdef DEBUG
  #ifdef MATLAB_MEX_FILE
    #define dbgPrintf(format, ...) ssPrintf(format, ##__VA_ARGS__)
  #else
    #define dbgPrintf(format, ...) printf(format, ##__VA_ARGS__)
  #endif
#else
#define dbgPrintf(format, ...)
#endif

#ifdef MATLAB_MEX_FILE
#define S_FUNCTION_LEVEL 2
#define S_FUNCTION_NAME  sf_trgen
#include "simstruc.h"
#endif

namespace Segment {

class Line : public TrajectorySegment 
{
    Point *p1, *p2;
    double angle;               // uhle v globalnich souradnicich
    double length;              // delka segmentu
    double cosphi, sinphi;
public:
    Line(Point *_p1, Point *_p2) : p1(_p1), p2(_p2) {
        length = p1->distanceTo(*p2);
        angle = p1->angleTo(*p2);
        sinphi = (p2->y-p1->y)/length;
        cosphi = (p2->x-p1->x)/length;
    }
    virtual ~Line() {};
  
    virtual void setMaxV(const TrajectoryConstraints &constr) {
        maxv = constr.maxv;
    }

    virtual double getLength() const {return length;}
    double getAngle() {return angle;}

    virtual void getPointAt(double distance, Point *p) {
        double ratio;
        if (distance > 0) {
            ratio = distance/length;
        } else {
            ratio = (length+distance)/length;
        }
        p->x = p1->x + ratio*(p2->x - p1->x);
        p->y = p1->y + ratio*(p2->y - p1->y);
    }

    virtual void shortenBy(double distance, Point *newEnd) {
        getPointAt(-distance, newEnd);
        if (distance > 0) {
            p2 = newEnd;
        } else {
            distance = -distance;
            p1 = newEnd;
        }
        length -= distance;
    }

    virtual TrajectorySegment* splitAt(double distance, Point *newEnd) {
        if (distance <= 0 || distance >= length) {
            dbgPrintf("splitAt: distance=%g length=%g\n", distance, length);
            return NULL;
        }

        getPointAt(distance, newEnd);
        Line *ns = new Line(*this);
        p2 = newEnd;
        length = distance;
        ns->length -= distance;
        ns->p1 = newEnd;
        return ns;
    }

    virtual void getRefPos(double time, Pos &rp) {
        double t = time-t1;
        double fraction = t/(t2-t1);
        double distance = (v1 + 0.5*acc*t) * t;
        rp.x = p1->x + distance*cosphi;
        rp.y = p1->y + distance*sinphi;
        rp.phi = angle;
        
        rp.v = v1+fraction*(v2-v1);
        rp.omega = 0;
        
    }
#ifdef MATLAB_MEX_FILE
    virtual void plot(const char *style) {
        char cmd[300];
        sprintf(cmd, "plot([%g %g], [%g %g], '%so')",
                p1->x, p2->x, p1->y, p2->y, style);
        mexEvalString(cmd);
    };
#endif
};

class Arc : public TrajectorySegment 
{
    Point *p1, *p2;
    double angle;               // vrcholovy uhel [-pi,+pi]
    double startAngle;          // uhel od stredu k p1
    double radius;              // polomer
    double length;              // delka segmentu
    Point center;
public:
    Arc(Point *_p1, Point *_p2, double _radius) :
            p1(_p1), p2(_p2), radius(fabs(_radius)) {

        // Find the center of the arc
        double angp1p2 = p1->angleTo(*p2);
        double m = p1->distanceTo(*p2)/2.0;
        if (radius < m) {
            radius = m; 
            dbgPrintf("EEEERRRRRRRRROOOOOOOOORRRRRRRRR!!!!!!!!!!\n");
        }
        double angcen = acos(m/radius);
        if (_radius < 0) angcen = -angcen;
        center = Point(p1->x + radius * cos(angp1p2+angcen),
                       p1->y + radius * sin(angp1p2+angcen));

        startAngle = center.angleTo(*p1);
        angle = center.angleTo(*p2) - startAngle;
        if (angle < -M_PI) angle += 2.0*M_PI;
        if (angle > +M_PI) angle -= 2.0*M_PI;

        length = fabs(angle*radius);
    }
    virtual ~Arc() {};

    
    virtual void setMaxV(const TrajectoryConstraints &constr) {
        double r = radius;
        maxv = fmin(constr.maxv, constr.maxomega * r);
        maxv = fmin(maxv, sqrt(constr.maxcenacc * r));
    }

    virtual double getLength() const {return length;}

    virtual void getPointAt(double distance, Point *p) {
        double ratio, a;
        if (distance > 0) {
            ratio = distance/length;
        } else {
            distance = -distance;
            ratio = (length-distance)/length;
        }
        a = startAngle + ratio*angle;
        p->x = center.x + radius*cos(a);
        p->y = center.y + radius*sin(a);
    }
    virtual void shortenBy(double distance, Point *newEnd) {
        getPointAt(-distance, newEnd);
        if (distance > 0) {
            angle *= (length-distance)/length;
            p2 = newEnd;
        } else {
            distance = -distance;
            startAngle = startAngle + distance/length*angle;
            angle *= (length-distance)/length;
            p1 = newEnd;
        }
        length -= distance;
    }

    virtual TrajectorySegment* splitAt(double distance, Point *newEnd) {
        if (distance <= 0 || distance >= length)
            return NULL;

        getPointAt(distance, newEnd);
        Arc *ns = new Arc(*this);

        double a = distance/length*angle;
        angle = a;
        length = distance;
        p2 = newEnd;

        ns->startAngle += a;
        ns->angle -= a;
        ns->length -= distance;
        ns->p1 = newEnd;
        return ns;
    }

    virtual void getRefPos(double time, Pos &rp) {
        double t = time-t1;
        double fraction = t/(t2-t1);
        double distance = (v1 + 0.5*acc*t) * t;
        double a = startAngle + distance/length*angle;
        rp.x = center.x + radius*cos(a);
        rp.y = center.y + radius*sin(a);
        if (angle > 0)
            rp.phi = a + M_PI/2.0;
        else
            rp.phi = a - M_PI/2.0;

        rp.v = v1+fraction*(v2-v1);
        rp.omega =rp.v/radius;
        if (angle < 0) rp.omega = -rp.omega;
        
    }
#ifdef MATLAB_MEX_FILE
    virtual void plot(const char *style) {
        char cmd[300];
        const int len = 10;
        int i;
        Pos rp;
        mxArray *x = mxCreateDoubleMatrix(1, len, mxREAL);
        mxArray *y = mxCreateDoubleMatrix(1, len, mxREAL);
        mxArray *s = mxCreateCharMatrixFromStrings(1, &style);
        mxArray *rhs[] = {x,y,s};
        
        for (i=0; i < len; i++) {
            getRefPos(t1+(t2-t1)*i/(len-1), rp);
            mxGetPr(x)[i] = rp.x;
            mxGetPr(y)[i] = rp.y;
        }
        mexCallMATLAB(0, NULL, 3, rhs, "plot");
        sprintf(cmd, "plot([%g %g], [%g %g], 'mo')",
                p1->x, p2->x, p1->y, p2->y);
        mexEvalString(cmd);
        mxDestroyArray(x);
        mxDestroyArray(y);
        mxDestroyArray(s);
    };
#endif
};

class Turn : public TrajectorySegment 
{
    Point *center;
    double turnBy;            
    double startHeading;      
public:
    Turn(Point *_center, double _startHeading, double _turnBy) :
        center(_center), turnBy(_turnBy), startHeading(_startHeading) {}
    virtual ~Turn() {};

    
    virtual void setMaxV(const TrajectoryConstraints &constr) {
            maxv = constr.maxomega;
    }

    virtual double getMaxAcc(const TrajectoryConstraints &constr) const {
        return constr.maxangacc;
    }

    virtual bool isTurn() const { return true; };

    virtual double getLength() const {return 0;}
    virtual double getUnifiedLength() const {
        return fabs(turnBy);
    }

    virtual void getPointAt(double distance, Point *p) {
        *p = *center;
    }
    virtual void shortenBy(double distance, Point *newEnd) {}

    virtual TrajectorySegment* splitAt(double distance, Point *newEnd) {
        if (distance <= 0 || distance >= fabs(turnBy)) {
            dbgPrintf("splitAt: distance=%g turnBy=%g\n", distance, turnBy);
            return NULL;
        }

        Turn *ns = new Turn(*this);
        if (turnBy < 0) 
                distance = -distance;
        turnBy = distance;
        ns->startHeading+=distance;
        ns->turnBy -= distance;
        return ns;
    }

    virtual void getRefPos(double time, Pos &rp) {
        double t = time-t1;
        double fraction = t/(t2-t1);
        double distance = (v1 + 0.5*acc*t) * t;
        rp.x = center->x;
        rp.y = center->y;
        if (turnBy > 0)
            rp.phi = startHeading + distance;
        else
            rp.phi = startHeading - distance;
        rp.v=0;
        if (time < t2)
                rp.omega = v1+fraction*(v2-v1);
        else
                rp.omega = 0;
    }

#ifdef MATLAB_MEX_FILE
    virtual void plot(const char *style) {};
#endif
};

} // namespace Segment


using namespace Segment;

bool 
Trajectory::splitSegment(iterator &seg, double distance) 
{
    TrajectorySegment *ns;
    Point *np = NULL;
    if (typeid(**seg) != typeid(Turn)) 
        np = new Point;

    ns = (*seg)->splitAt(distance, np);
    if (ns == NULL) {
        dbgPrintf("ERROR\n");
        delete(np);
        return false;
    }
    if (np) 
        wayPoints.push_back(np);
    seg = insert(++seg, ns);
    return true;
}
    
//     iterator insert(reverse_iterator it, const value_type& val) {
//         return insert((--it).base(), val);
//     }

bool
Trajectory::points2segments() 
{
    TrajectoryPoints::iterator p1, p2;
    Point ip(initPos.x, initPos.y);

    if (*wayPoints.front() != ip) {
        initialPoint = new Point(ip);
        wayPoints.push_front(initialPoint);
    } else
        initialPoint = wayPoints.front();

    // Delete old segments
    deleteSegments();

    if (wayPoints.size() < 2) {
        return false;
    }
    for (p1 = wayPoints.begin(), p2 = ++wayPoints.begin();
         p2 != wayPoints.end();
         p1++, p2++) {
        push_back(new Line(*p1, *p2));
    }
    return true;
}


// \f{center}
// \frame{\begin{tikzpicture}
//  \draw (-5cm,0) coordinate(p1) node[left]{$p_1$} --
//   node[sloped,above]{$\mathrm seg_1$} (0,0) coordinate(p2)
//   node[above]{$p_2$} -- node[sloped,above]{$\mathrm seg_2$} +(-30:5cm)
//   node[right]{$p_3$} coordinate (p3);
//   \coordinate[shift=(-105:5cm)] (c) at (p2);
//   \draw (p2) -- (c);
//   \draw (c) -- (c |- p2);
//   \draw (c) -- (intersection of c--40:10cm and p2--p3);
// \end{tikzpicture}}
//    \f}

void
Trajectory::corners2arcs() 
{
    iterator seg, segNext, segPrev;

    if (size() < 2) return;
        
    // Find radiuses to meet e <= maxe
    for (segPrev = begin(), segNext = ++begin();
         segNext != end();
         segPrev++, segNext++) {
            
        double alpha;       // curve angle (-pi <= alpha <= +pi)
        double r;           // radius of proposed arc. (alpha < 0 -> r < 0)
        double s;           // length of the segment replaced by the arc (s > 0)
        try {
            alpha = dynamic_cast<Line*>(*segNext)->getAngle() -
                dynamic_cast<Line*>(*segPrev)->getAngle();
            if (alpha > +M_PI) alpha -= 2.0*M_PI;
            if (alpha < -M_PI) alpha += 2.0*M_PI;

            if (fabs(alpha) < M_PI/180.0) continue;

            r = constr.maxe / (1/cos(alpha/2.0) - 1);
            if (alpha < 0) r = -r;
            s = r*tan(alpha/2.0);
            if (s > (*segPrev)->getLength()) {
                s = (*segPrev)->getLength();
                r = s/tan(alpha/2.0);
            }
            if (s > (*segNext)->getLength()) {
                s = (*segNext)->getLength();
                r = s/tan(alpha/2.0);
            }
            (*segPrev)->r = r;                
            (*segPrev)->s = s;
            (*segPrev)->alphahalf = alpha/2.0;
        } 
        catch (std::exception& e) {} // Skip wrong dynamic_cast
    }

    if (size() >= 3) {
        // Reduce overlapping arcs by decreasing radiuses
        double s1, s2, r1, r2, l, alphahalf1, alphahalf2;
        bool wantSameR;
        for (segPrev = begin(), segNext = ++begin();
             segNext != --end();
             segPrev++, segNext++) {
            l = (*segNext)->getLength();
            s1 = (*segPrev)->s;
            s2 = (*segNext)->s;
            r1 = (*segPrev)->r;
            r2 = (*segNext)->r;
            alphahalf1 = (*segPrev)->alphahalf;
            alphahalf2 = (*segNext)->alphahalf;
            if (s1 + s2 > l) {
                if (fabs(r1) > fabs(r2)) {
                    s1 = l - s2;
                    r1 = s1/tan(alphahalf1);
                    wantSameR = (fabs(r1) < fabs(r2));
                } else {
                    s2 = l - s1;
                    r2 = s2/tan(alphahalf2);
                    wantSameR = (fabs(r1) > fabs(r2));
                }
                if (wantSameR) {
                    if (alphahalf1 < 0 ^ alphahalf2 < 0)
                        s1 = l*tan(alphahalf1)/(tan(alphahalf1)-tan(alphahalf2));
                    else
                        s1 = l*tan(alphahalf1)/(tan(alphahalf1)+tan(alphahalf2));
                    s2 = l - s1;
                    r1 = s1/tan(alphahalf1);
                    r2 = s2/tan(alphahalf2);
                }
                (*segPrev)->s = s1;
                (*segNext)->s = s2;
                (*segPrev)->r = r1;
                (*segNext)->r = r2;
            }
        }
    }

    // replace corners by arcs with previously computed radiuses
    for (segPrev = begin(), segNext = ++begin();
         segNext != end();
         ++segPrev, ++segNext) {
        if (fabs((*segPrev)->r) > 0) {
            Point *newEnd1, *newEnd2;
            double s = (*segPrev)->s;
            newEnd1 = new Point();
            newEnd2 = new Point();
            (*segPrev)->shortenBy(+s, newEnd1);
            (*segNext)->shortenBy(-s, newEnd2);
            wayPoints.push_back(newEnd1);
            wayPoints.push_back(newEnd2);
            segPrev = insert(segNext, new Arc(newEnd1, newEnd2,
                                                        (*segPrev)->r));
        }
    }

    //delete zero-length segments
    seg = begin();
    while (seg != end()) {
        if ((*seg)->getLength() <= 1e-9) {
            delete(*seg);
            seg = erase(seg);
        } else
            seg++;
    }
}


void
Trajectory::calcSpeeds() 
{
    if (size() == 0) return;

    // Assign maximum speed
    for (iterator seg = begin(); seg != end(); seg++) {
        (*seg)->setMaxV(constr);
        (*seg)->v1 = (*seg)->maxv;
        (*seg)->v2 = (*seg)->maxv;
    }
#ifdef MATLAB_MEX_FILE
    plotSpeed("r-o");
#endif

    double lastv;
    bool turning = false;       // FIXME: turning je integer podle smeru otaceni

    // Accelerare where possible
    lastv = initPos.v;
    for (iterator seg = begin(); seg != end(); seg++) {
        if (turning != (*seg)->isTurn()) {
            turning = (*seg)->isTurn();
            lastv = 0;
        }
        double v1 = (*seg)->v1;
        double v2 = (*seg)->v2;
        double maxv = (*seg)->maxv;
        //dbgPrintf("processing: %p v1=%g\n", *seg, v1);
        if (v1 > lastv) {
            v1 = lastv;
            double l = (*seg)->getUnifiedLength();
            double a = (*seg)->getMaxAcc(constr);
            double t;

            t = (-v1+sqrt(v1*v1 + 2.0*a*l))/a;
            v2 = v1+a*t;

            //dbgPrintf("t=%g v2=%g l=%g\n", t, v2, l);
            if (v2 > maxv) {    // split this segment
                v2 = maxv;
                t = (v2 - v1)/a;
                l = v1*t+0.5*a*t*t;
                //dbgPrintf("t=%g v2=%g l=%g\n", t, v2, l);
                iterator ns = seg;
                if (splitSegment(ns, l)) {
                    (*ns)->v1 = v2;
                }
            }

            (*seg)->v1 = v1;
            (*seg)->v2 = v2;
        }
        lastv = (*seg)->v2;
    }
#ifdef MATLAB_MEX_FILE
    plotSpeed("g-o");
#endif        

    // Deccelerate where needed
    turning = false;
    lastv = 0;              // Final speed
    for (reverse_iterator seg = rbegin(); seg != rend(); seg++) {
        if (turning != (*seg)->isTurn()) {
            turning = (*seg)->isTurn();
            lastv = 0;
        }
        double v1 = (*seg)->v1;
        double v2 = (*seg)->v2;
        double maxv = (*seg)->maxv;
        dbgPrintf("processing: %p v2=%g\n", *seg, v2);
        if (v2 > lastv) {
            v2 = lastv;
            double l = (*seg)->getUnifiedLength();
            double a = (*seg)->getMaxAcc(constr);
            double t;

            t = (-v2+sqrt(v2*v2 + 2.0*a*l))/a;
            v1 = v2+a*t;

            //dbgPrintf("t=%g v1=%g v2=%g l=%g\n", t, v1, v2, l);
            if (v1 > maxv && (*seg)->v1 == maxv) {
                v1 = maxv;
                t = (v1 - v2)/a;
                double l2 = v2*t+0.5*a*t*t;
                //dbgPrintf("t=%g v1=%g v2=%g l=%g l2=%g\n", t, v1, v2, l, l2);
                iterator ns = --(seg.base());
                //dbgPrintf("seg=%p ns=%p\n", *seg, *ns);
                if (splitSegment(ns, l-l2)) {
                    //dbgPrintf("ns=%p\n", *ns);
                    (*ns)->v2 = v1;
                    (*seg)->v1 = v1;
                }
            } else if (v1 > (*seg)->v1) {
                // Acceleration and deacceleration within one segment
                // Where is the crossing af acceleration and decceleration?
                v1 = (*seg)->v1;
                double s1 = l/2.0 + (v2*v2 - v1*v1)/4.0*a;
                    
                if (s1 > l/1000 && s1 < l - l/1000) {
                    //dbgPrintf("t=%g v1=%g v2=%g l=%g s1=%g\n", t, v1, v2, l, s1);
                    iterator ns = --(seg.base());
                    //dbgPrintf("seg=%p ns=%p (before split)\n", *seg, *ns);
                    if (splitSegment(ns, s1)) {
                        ++seg; //move to the earlier subsegment of the splited one
                        //dbgPrintf("seg=%p ns=%p (after split)\n", *seg, *ns);
                        v1 = sqrt(v1*v1 + 2.0*a*s1);
                        (*ns)->v1 = v1;
                        (*ns)->v2 = v2;
                        v2 = v1;
                        v1 = (*seg)->v1;
                    }
                }
            } else {
                (*seg)->v1 = v1;
            }
            (*seg)->v2 = v2;
        }
        lastv = (*seg)->v1;
    }
#ifdef MATLAB_MEX_FILE
    plotSpeed("b-o");
#endif
    for (iterator seg = begin(); seg != end(); ++seg) {
        dbgPrintf("final: %-16s %p v1=%-8.2g v2=%-8.2g l=%-8.2g\n", 
                  typeid(**seg).name(),
                  *seg, (*seg)->v1, (*seg)->v2, (*seg)->getLength());
    }
}

    
double
Trajectory::calcLength() 
{
    double totLen = 0;
    for (iterator seg = begin(); seg != end(); ++seg) {
        totLen += (*seg)->getLength();
    }
    return totLen;
}

static double minimizeAngle(double rad)
{
    rad = fmod(rad, 2.0*M_PI);
    if (rad > +M_PI/2.0) rad -= 2.0*M_PI;
    if (rad < -M_PI/2.0) rad += 2.0*M_PI;
    return rad;
}

void Trajectory::addTurns()
{
    Pos p;
    TrajectorySegment *seg;
    double initialHeading = initPos.phi;
    double turnBy;
    if (backward) 
        initialHeading -= M_PI;

    if (size() == 0) {
        // We have to turn only. No movement.
        dbgPrintf("*** Adding only the turn - start: %5.2f, end %5.2f\n", initialHeading/M_PI*180, finalHeading/M_PI*180);
        turnBy = minimizeAngle(finalHeading - initialHeading);
        if (fabs(turnBy) > 1e-3) {
            push_front(new Turn(initialPoint, initialHeading, 
                                finalHeading - initialHeading));
        }
    } else {
        // Add turn at beginning
        seg = *begin();
        seg->v1 = 1;           // Hack - speeds are not calculated yet
        seg->v2 = 1;
        seg->startAt(0);        // Assign times so we can use
                                // getRefPos();
        seg->getRefPos(0, p);
        dbgPrintf("*** Adding initial turn - start: %5.2f, end %5.2f\n", initialHeading/M_PI*180, p.phi/M_PI*180);
        turnBy = minimizeAngle(p.phi - initialHeading);
        if (fabs(turnBy) > 1e-3) {
            push_front(new Turn(initialPoint, initialHeading, turnBy));
        }

        // Add turn at the end
        seg = *(--end());
        seg->v1 = 1;           // Hack - speeds are not calculated yet
        seg->v2 = 1;
        seg->startAt(0);            // Assign times so we can use getRefPos();
        seg->getRefPos(seg->getT2(), p);
        turnBy = minimizeAngle(p.phi - finalHeading);
        if (fabs(turnBy) > 1e-3) { // NaN in finalHeading makes the condition false
            dbgPrintf("*** Adding final turn - start: %5.2f, end %5.2f\n", p.phi/M_PI*180, finalHeading/M_PI*180);
            push_back(new Turn(finalPoint, p.phi, turnBy));
        }
    }
}

bool
Trajectory::prepare(Pos _initPos) 
{
    if (constr.maxv      <= 0 || constr.maxv      > 10 ||
        constr.maxomega  <= 0 || constr.maxomega  > 10 ||
        constr.maxangacc <= 0 || constr.maxangacc > 10 ||
        constr.maxacc    <= 0 || constr.maxacc    > 10 ||
        constr.maxcenacc <= 0 || constr.maxcenacc > 10 ||
        constr.maxe      <= 0 || constr.maxe      > 10) {
        dbgPrintf("wrong constraints!!!!\n");
    }

    if (wayPoints.size() == 0) {
        dbgPrintf("No point in trajectory!\n");
        return false;
    }

    initPos = _initPos;
    if (points2segments() == false) {
        dbgPrintf("points2segments error!\n");
        return false;
    }
    corners2arcs();
    addTurns();
    if (size() == 0)
        return false;
    calcSpeeds();

    // Assign times to segments
    double t=0;
    for (iterator seg = begin(); seg != end(); ++seg) {
        t = (*seg)->startAt(t);
    }
    currentSeg = begin();
    prepared = true;
    return true;
}

bool
Trajectory::getRefPos(double time, Pos &rp) 
{
    int d;
    bool ret = false;
    TrajectorySegment *seg;

    if (!prepared) {
        ret = true;
    } else {
        do {
            d=(*currentSeg)->containsTime(time);
            if (d==0) break;
            if (currentSeg == --end() && d > 0) break;
            if (currentSeg == begin() && d < 0) break;
            if (d>0) ++currentSeg;
            else if (d<0) --currentSeg;
        } while (1);

        seg = *currentSeg;
        if (d==0) 
            getSegRefPos(seg, time, rp);
        else if (d<0)
            getSegRefPos(seg, seg->getT1(), rp);
        else {
            getSegRefPos(seg, seg->getT2(), rp);
            ret = true;             // We are at the end.
        }
    }
    
    return ret;
}

void
Trajectory::getSegRefPos(TrajectorySegment *seg, double time, Pos &rp) 
{
    seg->getRefPos(time, rp);

    if (backward) {
        rp.phi += M_PI;
        rp.v = -rp.v;
    }
}

#ifdef MATLAB_MEX_FILE
void
Trajectory::plot(const char *style) 
{
    for (iterator seg = begin(); seg != end(); ++seg) {
        (*seg)->plot(style);
    }
};
void
Trajectory::plotSpeed(const char *style) 
{
    char *f2 = "figure(2);";
    char *f1 = "hold on; figure(1)";
    double t = 0;
    mexEvalString(f2);
    for (iterator seg = begin(); seg != end(); ++seg) {
        t = (*seg)->startAt(t);
        (*seg)->plotSpeed(style);
    }
    mexEvalString(f1);
};
#endif

---