]> rtime.felk.cvut.cz Git - hubacji1/bcar.git/blobdiff - src/bcar.cc
Add gen plot for lra and rra points
[hubacji1/bcar.git] / src / bcar.cc
index ef7300e2ecdee573a3d951dab061b3a0f387b392..4e635d6c395a9a8f72519f1c94ea6d2fb7b5252b 100644 (file)
@@ -1,38 +1,44 @@
+/*
+ * SPDX-FileCopyrightText: 2021 Jiri Vlasak <jiri.vlasak.2@cvut.cz>
+ *
+ * SPDX-License-Identifier: GPL-3.0-only
+ */
+
 #include <cmath>
 #include "bcar.hh"
 
-using namespace bcar;
+namespace bcar {
 
-Point::Point(double x, double y) : x_(x), y_(y)
+Point::Point()
 {
 }
 
-Point::Point() : Point::Point(0.0, 0.0)
+Point::Point(double x, double y) : _x(x), _y(y)
 {
 }
 
 double
 Point::x() const
 {
-       return this->x_;
+       return this->_x;
 }
 
 void
 Point::x(double x)
 {
-       this->x_ = x;
+       this->_x = x;
 }
 
 double
 Point::y() const
 {
-       return this->y_;
+       return this->_y;
 }
 
 void
 Point::y(double y)
 {
-       this->y_ = y;
+       this->_y = y;
 }
 
 double
@@ -79,46 +85,123 @@ Point::inside_of(std::vector<Point> const& poly) const
        return c;
 }
 
-Line::Line(Point const& fp, Point const& lp): first(fp), last(lp),
-               intersection1(Point(0.0, 0.0)), intersection2(Point(0.0, 0.0))
+bool
+Point::on_right_side_of(Line const& li) const
+{
+       auto x1 = li.b().x();
+       auto y1 = li.b().y();
+       auto x2 = li.e().x();
+       auto y2 = li.e().y();
+       auto x3 = this->_x;
+       auto y3 = this->_y;
+       if (sgn((x3 - x1) * (y2 - y1) - (y3 - y1) * (x2 - x1)) < 0.0) {
+               return false;
+       } else {
+               return true;
+       }
+}
+
+void
+Point::translate(Point const& p)
 {
+       this->_x += p.x();
+       this->_y += p.y();
+}
+
+void
+Point::rotate(Point const& c, double const angl)
+{
+       double px = this->x();
+       double py = this->y();
+       px -= c.x();
+       py -= c.y();
+       double nx = px * cos(angl) - py * sin(angl);
+       double ny = px * sin(angl) + py * cos(angl);
+       this->x(nx + c.x());
+       this->y(ny + c.y());
+}
+
+void
+Point::reflect(Line const& li)
+{
+       this->rotate(li.b(), -li.h());
+       this->_y -= li.b().y();
+       this->_y *= -1.0;
+       this->_y += li.b().y();
+       this->rotate(li.b(), li.h());
+}
+
+double
+Point::edist(Point const& p) const
+{
+       return sqrt(pow(p.x() - this->_x, 2.0) + pow(p.y() - this->_y, 2.0));
+}
+
+void
+Point::gen_gnuplot_to(std::ostream& out)
+{
+       out << this->_x << " " << this->_y << std::endl;
+}
+
+bool
+Point::operator==(Point const& p)
+{
+       return this->x() == p.x() && this->y() == p.y();
+}
+
+std::ostream&
+operator<<(std::ostream& out, Point const& p)
+{
+       out << "[" << p.x() << "," << p.y() << "]";
+       return out;
+}
+
+Line::Line(Point const& b, Point const& e): _b(b), _e(e)
+{
+}
+
+Point
+Line::b() const&
+{
+       return this->_b;
 }
 
 Point
-Line::fp() const&
+Line::e() const&
 {
-       return this->first;
+       return this->_e;
 }
 
 Point
-Line::lp() const&
+Line::m() const
 {
-       return this->last;
+       return Point((this->_b.x() + this->_e.x()) / 2.0,
+               (this->_b.y() + this->_e.y()) / 2.0);
 }
 
 Point
-Line::in1() const&
+Line::i1() const&
 {
-       return this->intersection1;
+       return this->_i1;
 }
 
 Point
-Line::in2() const&
+Line::i2() const&
 {
-       return this->intersection2;
+       return this->_i2;
 }
 
 bool
 Line::intersects_with(Line const& li)
 {
-       auto x1 = this->fp().x();
-       auto y1 = this->fp().y();
-       auto x2 = this->lp().x();
-       auto y2 = this->lp().y();
-       auto x3 = li.fp().x();
-       auto y3 = li.fp().y();
-       auto x4 = li.lp().x();
-       auto y4 = li.lp().y();
+       auto x1 = this->_b.x();
+       auto y1 = this->_b.y();
+       auto x2 = this->_e.x();
+       auto y2 = this->_e.y();
+       auto x3 = li.b().x();
+       auto y3 = li.b().y();
+       auto x4 = li.e().x();
+       auto y4 = li.e().y();
        double deno = (x1 - x2) * (y3 - y4) - (y1 - y2) * (x3 - x4);
        if (deno == 0.0) {
                return false;
@@ -129,20 +212,20 @@ Line::intersects_with(Line const& li)
        u *= -1.0;
        u /= deno;
        if (t < 0.0 || t > 1.0 || u < 0.0 || u > 1.0) {
-               false;
+               return false;
        }
-       this->intersection1.x(x1 + t * (x2 - x1));
-       this->intersection1.y(y1 + t * (y2 - y1));
+       this->_i1.x(x1 + t * (x2 - x1));
+       this->_i1.y(y1 + t * (y2 - y1));
        return true;
 }
 
 bool
 Line::intersects_with(Point const& c, double const r)
 {
-       auto x1 = this->fp().x();
-       auto y1 = this->fp().y();
-       auto x2 = this->lp().x();
-       auto y2 = this->lp().y();
+       auto x1 = this->_b.x();
+       auto y1 = this->_b.y();
+       auto x2 = this->_e.x();
+       auto y2 = this->_e.y();
        auto cx = c.x();
        auto cy = c.y();
        x2 -= cx;
@@ -168,57 +251,48 @@ Line::intersects_with(Point const& c, double const r)
        iy1 += cy;
        double iy2 = (-D*dx - std::abs(dy)*sqrt(r*r * dr*dr - D*D)) / (dr*dr);
        iy2 += cy;
-       this->intersection1.x(ix1);
-       this->intersection1.y(iy1);
-       this->intersection2.x(ix2);
-       this->intersection2.y(iy2);
+       this->_i1.x(ix1);
+       this->_i1.y(iy1);
+       this->_i2.x(ix2);
+       this->_i2.y(iy2);
        return true;
 }
 
-bool
-Line::is_on_right_side(Point const& p) const
-{
-       auto x1 = this->fp().x();
-       auto y1 = this->fp().y();
-       auto x2 = this->lp().x();
-       auto y2 = this->lp().y();
-       auto x3 = p.x();
-       auto y3 = p.y();
-       if (sgn((x3 - x1) * (y2 - y1) - (y3 - y1) * (x2 - x1)) < 0.0) {
-               return false;
-       } else {
-               return true;
-       }
+double
+Line::len() const
+{
+       return this->_b.edist(this->_e);
 }
 
 double
-Pose::x() const
+Line::h() const
 {
-       return this->x_;
+       return atan2(this->_e.y() - this->_b.y(), this->_e.x() - this->_b.x());
 }
 
 void
-Pose::x(double x)
+Line::gen_gnuplot_to(std::ostream& out)
 {
-       this->x_ = x;
+       this->b().gen_gnuplot_to(out);
+       this->e().gen_gnuplot_to(out);
+       out << std::endl;
 }
 
-double
-Pose::y() const
+std::ostream&
+operator<<(std::ostream& out, Line const& li)
 {
-       return this->y_;
+       out << "[" << li._b << "," << li._e << "]";
+       return out;
 }
 
-void
-Pose::y(double y)
+Pose::Pose(double x, double y, double h) : Point(x, y), _h(h)
 {
-       this->y_ = y;
 }
 
 double
 Pose::h() const
 {
-       return this->h_;
+       return this->_h;
 }
 
 void
@@ -230,21 +304,36 @@ Pose::h(double h)
        while (h > +M_PI) {
                h -= 2 * M_PI;
        }
-       this->h_ = h;
+       this->_h = h;
+}
+
+void
+Pose::set_pose(Pose const& p)
+{
+       this->x(p.x());
+       this->y(p.y());
+       this->h(p.h());
 }
 
 void
 Pose::rotate(Point const& c, double const angl)
 {
-       double px = this->x();
-       double py = this->y();
-       px -= c.x();
-       py -= c.y();
-       double nx = px * cos(angl) - py * sin(angl);
-       double ny = px * sin(angl) + py * cos(angl);
+       Point::rotate(c, angl);
        this->h(this->h() + angl);
-       this->x(nx + c.x());
-       this->y(ny + c.y());
+}
+
+void
+Pose::reflect(Line const& li)
+{
+       Point::reflect(li);
+       double dh = li.h() - this->h();
+       this->h(this->h() + 2.0 * dh);
+}
+
+bool
+Pose::operator==(Pose const& p)
+{
+       return this->x() == p.x() && this->y() == p.y() && this->h() == p.h();
 }
 
 std::ostream&
@@ -254,41 +343,96 @@ operator<<(std::ostream& out, Pose const& p)
        return out;
 }
 
-double
-PoseRange::b() const
+void
+PoseRange::set_xyh()
+{
+       double clen = 10.0;
+       double bpbx = this->_bp.x() - clen * cos(this->_bp.h());
+       double bpby = this->_bp.y() - clen * sin(this->_bp.h());
+       double bpfx = this->_bp.x() + clen * cos(this->_bp.h());
+       double bpfy = this->_bp.y() + clen * sin(this->_bp.h());
+       Line li1(Point(bpbx, bpby), Point(bpfx, bpfy));
+       double epbx = this->_ep.x() - clen * cos(this->_ep.h());
+       double epby = this->_ep.y() - clen * sin(this->_ep.h());
+       double epfx = this->_ep.x() + clen * cos(this->_ep.h());
+       double epfy = this->_ep.y() + clen * sin(this->_ep.h());
+       Line li2(Point(epbx, epby), Point(epfx, epfy));
+       li1.intersects_with(li2);
+       this->x(li1.i1().x());
+       this->y(li1.i1().y());
+       double bh = this->b();
+       while (bh < 0.0) {
+               bh += 2.0 * M_PI;
+       }
+       this->_bp.h(bh);
+       double eh = this->e();
+       while (eh < 0.0) {
+               eh += 2.0 * M_PI;
+       }
+       this->_ep.h(eh);
+       this->h((this->b() + this->e()) / 2.0);
+}
+
+PoseRange::PoseRange(Pose bp, Pose ep) : _bp(bp), _ep(ep)
 {
-       return this->h();
+       if (this->_bp == this->_ep) {
+               this->set_pose(this->_ep);
+       } else {
+               this->set_xyh();
+       }
 }
 
-void
-PoseRange::b(double b)
+PoseRange::PoseRange(double x, double y, double b, double e)
+               : PoseRange(Pose(x, y, b), Pose(x, y, e))
+{
+}
+
+Pose
+PoseRange::bp() const
 {
-       this->h(b);
+       return this->_bp;
+}
+
+Pose
+PoseRange::ep() const
+{
+       return this->_ep;
+}
+
+double
+PoseRange::b() const
+{
+       return std::min(this->_bp.h(), this->_ep.h());
 }
 
 double
 PoseRange::e() const
 {
-       return this->e_;
+       return std::max(this->_bp.h(), this->_ep.h());
 }
 
 void
-PoseRange::e(double e)
+PoseRange::translate(Point const& p)
 {
-       while (e < -M_PI) {
-               e += 2 * M_PI;
-       }
-       while (e > +M_PI) {
-               e -= 2 * M_PI;
-       }
-       this->e_ = e;
+       this->_bp.translate(p);
+       this->_ep.translate(p);
+       this->set_xyh();
 }
 
 void
 PoseRange::rotate(Point const& c, double const angl)
 {
-       Pose::rotate(c, angl);
-       this->e(this->e() + angl);
+       this->_bp.rotate(c, angl);
+       this->_ep.rotate(c, angl);
+       this->set_xyh();
+}
+
+void
+PoseRange::reflect(Line const& li)
+{
+       this->_bp.reflect(li);
+       this->_ep.reflect(li);
+       this->set_xyh();
 }
 
 std::ostream&
@@ -302,61 +446,73 @@ operator<<(std::ostream& out, PoseRange const& p)
 double
 CarSize::ctc() const
 {
-       return this->curb_to_curb;
+       return this->_curb_to_curb;
 }
 
 void
 CarSize::ctc(double ctc)
 {
-       this->curb_to_curb = ctc;
+       this->_curb_to_curb = ctc;
 }
 
 double
 CarSize::wb() const
 {
-       return this->wheelbase;
+       return this->_wheelbase;
 }
 
 void
 CarSize::wb(double wb)
 {
-       this->wheelbase = wb;
+       this->_wheelbase = wb;
 }
 
 double
 CarSize::w() const
 {
-       return this->width;
+       return this->_width;
 }
 
 void
 CarSize::w(double w)
 {
-       this->width = w;
+       this->_width = w;
+}
+
+double
+CarSize::wwm() const
+{
+       return this->_width_with_mirrors;
+}
+
+void
+CarSize::wwm(double w)
+{
+       this->_width_with_mirrors = w;
 }
 
 double
 CarSize::len() const
 {
-       return this->length;
+       return this->_length;
 }
 
 void
 CarSize::len(double len)
 {
-       this->length = len;
+       this->_length = len;
 }
 
 double
 CarSize::df() const
 {
-       return this->distance_to_front;
+       return this->_distance_to_front;
 }
 
 void
 CarSize::df(double df)
 {
-       this->distance_to_front = df;
+       this->_distance_to_front = df;
 }
 
 double
@@ -365,12 +521,24 @@ CarSize::dr() const
        return this->len() - this->df();
 }
 
+void
+CarSize::ft(double ft)
+{
+       this->_front_track = ft;
+}
+
+double
+CarSize::ft() const
+{
+       return this->_front_track;
+}
+
 double
 CarSize::mtr() const
 {
        auto ctc2 = pow(this->ctc() / 2.0, 2.0);
        auto wb2 = pow(this->wb(), 2.0);
-       return sqrt(ctc2 - wb2) - this->w() / 2.0;
+       return sqrt(ctc2 - wb2) - this->ft() / 2.0;
 }
 
 double
@@ -395,13 +563,29 @@ CarSize::orradi() const
        return sqrt(mtrw2 + dr2);
 }
 
+double
+CarSize::imradi() const
+{
+       auto mtrw2 = pow(this->mtr() - this->wwm() / 2.0, 2.0);
+       auto df2 = pow(this->wb(), 2.0);
+       return sqrt(mtrw2 + df2);
+}
+
+double
+CarSize::omradi() const
+{
+       auto mtrw2 = pow(this->mtr() + this->wwm() / 2.0, 2.0);
+       auto df2 = pow(this->wb(), 2.0);
+       return sqrt(mtrw2 + df2);
+}
+
 double
 CarSize::perfect_parking_slot_len() const
 {
        auto r = this->ctc() / 2.0;
        auto l = this->wb();
        auto k = this->df() - this->wb();
-       auto w = this->w();
+       auto w = this->w(); // FIXME use wwm()?
        auto r2l2 = r * r - l * l;
        auto s = r2l2 + pow(l + k, 2.0) - pow(sqrt(r2l2) - w, 2.0);
        return this->len() + sqrt(s) - l - k;
@@ -410,48 +594,42 @@ CarSize::perfect_parking_slot_len() const
 double
 CarMove::sp() const
 {
-       return this->speed;
+       return this->_speed;
 }
 
 void
 CarMove::sp(double sp)
 {
-       this->speed = sp;
+       this->_speed = sp;
 }
 
 double
 CarMove::st() const
 {
-       return this->steer;
+       return this->_steer;
 }
 
 void
 CarMove::st(double st)
 {
-       this->steer = st;
+       this->_steer = st;
 }
 
 bool
 BicycleCar::drivable(Pose const& p) const
 {
-       PoseRange pr;
-       pr.x(p.x());
-       pr.y(p.y());
-       pr.b(p.h());
-       pr.e(p.h());
-       return this->drivable(pr);
+       return this->drivable(PoseRange(p, p));
 }
 
 bool
 BicycleCar::drivable(PoseRange const& p) const
 {
-       double h = (p.b() + p.e()) / 2.0;
        double a_1 = atan2(p.y() - this->y(), p.x() - this->x()) - this->h();
        while (a_1 < -M_PI)
                a_1 += 2 * M_PI;
        while (a_1 > +M_PI)
                a_1 -= 2 * M_PI;
-       double h_d = h - this->h();
+       double h_d = p.h() - this->h();
        while (h_d < -M_PI)
                h_d += 2 * M_PI;
        while (h_d > +M_PI)
@@ -462,9 +640,9 @@ BicycleCar::drivable(PoseRange const& p) const
        } else if (0 < a_1 && a_1 <= M_PI/2) { // left front
                BicycleCar z(*this); // zone border
                z.h(p.e());
-               h_d = h - this->h();
+               h_d = p.h() - this->h();
                z.rotate(this->ccl(), h_d);
-               // assert z.h() == h
+               // assert z.h() == p.h()
                if (p.y() == z.y() && p.x() == z.x()) // p on zone border
                        return true;
                a_2 = atan2(p.y() - z.y(), p.x() - z.x());
@@ -477,9 +655,9 @@ BicycleCar::drivable(PoseRange const& p) const
        } else if (M_PI/2 < a_1 && a_1 <= M_PI) { // left rear
                BicycleCar z(*this); // zone border
                z.h(p.e());
-               h_d = h - this->h();
+               h_d = p.h() - this->h();
                z.rotate(this->ccl(), h_d);
-               // assert z.h() == h
+               // assert z.h() == p.h()
                if (p.y() == z.y() && p.x() == z.x()) // p on zone border
                        return true;
                a_2 = atan2(p.y() - z.y(), p.x() - z.x());
@@ -493,9 +671,9 @@ BicycleCar::drivable(PoseRange const& p) const
        } else if (0 > a_1 && a_1 >= -M_PI/2) { // right front
                BicycleCar z(*this); // zone border
                z.h(p.b());
-               h_d = h - this->h();
+               h_d = p.h() - this->h();
                z.rotate(this->ccr(), h_d);
-               // assert z.h() == h
+               // assert z.h() == p.h()
                if (p.y() == z.y() && p.x() == z.x()) // p on zone border
                        return true;
                a_2 = atan2(p.y() - z.y(), p.x() - z.x());
@@ -508,9 +686,9 @@ BicycleCar::drivable(PoseRange const& p) const
        } else if (-M_PI/2 > a_1 && a_1 >= -M_PI) { // right rear
                BicycleCar z(*this); // zone border
                z.h(p.b());
-               h_d = h - this->h();
+               h_d = p.h() - this->h();
                z.rotate(this->ccr(), h_d);
-               // assert z.h() == h
+               // assert z.h() == p.h()
                if (p.y() == z.y() && p.x() == z.x()) // p on zone border
                        return true;
                a_2 = atan2(p.y() - z.y(), p.x() - z.x());
@@ -605,15 +783,63 @@ BicycleCar::rfy() const
        return rfy;
 }
 
+Point
+BicycleCar::lf() const
+{
+       return Point(this->lfx(), this->lfy());
+}
+
+Point
+BicycleCar::lr() const
+{
+       return Point(this->lrx(), this->lry());
+}
+
+Point
+BicycleCar::rr() const
+{
+       return Point(this->rrx(), this->rry());
+}
+
+Point
+BicycleCar::rf() const
+{
+       return Point(this->rfx(), this->rfy());
+}
+
+Line
+BicycleCar::left() const
+{
+       return Line(this->lr(), this->lf());
+}
+
+Line
+BicycleCar::rear() const
+{
+       return Line(this->lr(), this->rr());
+}
+
+Line
+BicycleCar::right() const
+{
+       return Line(this->rr(), this->rf());
+}
+
+Line
+BicycleCar::front() const
+{
+       return Line(this->rf(), this->lf());
+}
+
 double
-BicycleCar::ralx() const
+BicycleCar::lrax() const
 {
        double lrx = this->x();
        lrx += (this->w() / 2.0) * cos(this->h() + M_PI / 2.0);
        return lrx;
 }
 double
-BicycleCar::raly() const
+BicycleCar::lray() const
 {
        double lry = this->y();
        lry += (this->w() / 2.0) * sin(this->h() + M_PI / 2.0);
@@ -621,7 +847,7 @@ BicycleCar::raly() const
 }
 
 double
-BicycleCar::rarx() const
+BicycleCar::rrax() const
 {
        double rrx = this->x();
        rrx += (this->w() / 2.0) * cos(this->h() - M_PI / 2.0);
@@ -629,13 +855,127 @@ BicycleCar::rarx() const
 }
 
 double
-BicycleCar::rary() const
+BicycleCar::rray() const
 {
        double rry = this->y();
        rry += (this->w() / 2.0) * sin(this->h() - M_PI / 2.0);
        return rry;
 }
 
+Point
+BicycleCar::lra() const
+{
+       return Point(this->lrax(), this->lray());
+}
+
+Point
+BicycleCar::rra() const
+{
+       return Point(this->rrax(), this->rray());
+}
+
+double
+BicycleCar::lfax() const
+{
+       return this->lrax() + this->wb() * cos(this->h());
+}
+
+double
+BicycleCar::lfay() const
+{
+       return this->lray() + this->wb() * sin(this->h());
+}
+
+double
+BicycleCar::rfax() const
+{
+       return this->rrax() + this->wb() * cos(this->h());
+}
+
+double
+BicycleCar::rfay() const
+{
+       return this->rray() + this->wb() * sin(this->h());
+}
+
+Point
+BicycleCar::lfa() const
+{
+       return Point(this->lfax(), this->lfay());
+}
+
+Point
+BicycleCar::rfa() const
+{
+       return Point(this->rfax(), this->rfay());
+}
+
+double
+BicycleCar::lfmx() const
+{
+       double x = this->x();
+       x += (this->wwm() / 2.0) * cos(this->h() + M_PI / 2.0);
+       x += this->wb() * cos(this->h());
+       return x;
+}
+
+double
+BicycleCar::lfmy() const
+{
+       double y = this->y();
+       y += (this->wwm() / 2.0) * sin(this->h() + M_PI / 2.0);
+       y += this->wb() * sin(this->h());
+       return y;
+}
+
+double
+BicycleCar::rfmx() const
+{
+       double x = this->x();
+       x += (this->wwm() / 2.0) * cos(this->h() - M_PI / 2.0);
+       x += this->wb() * cos(this->h());
+       return x;
+}
+
+double
+BicycleCar::rfmy() const
+{
+       double y = this->y();
+       y += (this->wwm() / 2.0) * sin(this->h() - M_PI / 2.0);
+       y += this->wb() * sin(this->h());
+       return y;
+}
+
+Point
+BicycleCar::lfm() const
+{
+       return Point(this->lfmx(), this->lfmy());
+}
+
+Point
+BicycleCar::rfm() const
+{
+       return Point(this->rfmx(), this->rfmy());
+}
+
+double
+BicycleCar::cfx() const
+{
+       return this->x() + this->df() * cos(this->h());
+}
+
+double
+BicycleCar::cfy() const
+{
+       return this->y() + this->df() * sin(this->h());
+}
+
+Point
+BicycleCar::cf() const
+{
+       return Point(this->cfx(), this->cfy());
+}
+
 Point
 BicycleCar::ccl() const
 {
@@ -661,3 +1001,112 @@ BicycleCar::next()
        this->y(this->y() + this->sp() * sin(this->h()));
        this->h(this->h() + this->sp() / this->wb() * tan(this->st()));
 }
+
+void
+BicycleCar::gen_gnuplot_to(std::ostream& out, GenPlotOpts opts)
+{
+       if (opts.ALL) {
+               opts.CAR = true;
+               opts.MIRRORS = true;
+       }
+       if (opts.MIRRORS) {
+               opts.LEFT_MIRROR = true;
+               opts.RIGHT_MIRROR = true;
+       }
+       if (opts.CAR) {
+               opts.FRAME = true;
+               opts.CROSS = true;
+               opts.ARROW = true;
+       }
+       if (opts.FRAME) {
+               opts.LEFT = true;
+               opts.RIGHT = true;
+               opts.REAR = true;
+               opts.FRONT = true;
+       }
+       if (opts.LF_POINT) {
+               this->lf().gen_gnuplot_to(out);
+       }
+       if (opts.LR_POINT) {
+               this->lr().gen_gnuplot_to(out);
+       }
+       if (opts.RR_POINT) {
+               this->rr().gen_gnuplot_to(out);
+       }
+       if (opts.RF_POINT) {
+               this->rf().gen_gnuplot_to(out);
+       }
+       if (opts.LFM_POINT) {
+               this->lfm().gen_gnuplot_to(out);
+       }
+       if (opts.RFM_POINT) {
+               this->rfm().gen_gnuplot_to(out);
+       }
+       if (opts.CRA_POINT || opts.CAR_POINT) {
+               Point::gen_gnuplot_to(out);
+       }
+       if (opts.LRA_POINT) {
+               this->lra().gen_gnuplot_to(out);
+       }
+       if (opts.RRA_POINT) {
+               this->rra().gen_gnuplot_to(out);
+       }
+       if (opts.LEFT) {
+               this->lf().gen_gnuplot_to(out);
+               this->lr().gen_gnuplot_to(out);
+               out << std::endl;
+       }
+       if (opts.RIGHT) {
+               this->rf().gen_gnuplot_to(out);
+               this->rr().gen_gnuplot_to(out);
+               out << std::endl;
+       }
+       if (opts.REAR) {
+               this->lr().gen_gnuplot_to(out);
+               this->rr().gen_gnuplot_to(out);
+               out << std::endl;
+       }
+       if (opts.FRONT) {
+               this->lf().gen_gnuplot_to(out);
+               this->rf().gen_gnuplot_to(out);
+               out << std::endl;
+       }
+       if (opts.ARROW) {
+               this->cf().gen_gnuplot_to(out);
+               this->lfa().gen_gnuplot_to(out);
+               this->rfa().gen_gnuplot_to(out);
+               this->cf().gen_gnuplot_to(out);
+               out << std::endl;
+       }
+       if (opts.CROSS) {
+               double lx = this->x() + 0.2 * cos(this->h() + M_PI/2);
+               double rx = this->x() - 0.2 * cos(this->h() + M_PI/2);
+               double fx = this->x() + 0.2 * cos(this->h());
+               double bx = this->x() - 0.2 * cos(this->h()); // rear is back
+               double ly = this->y() + 0.2 * sin(this->h() + M_PI/2);
+               double ry = this->y() - 0.2 * sin(this->h() + M_PI/2);
+               double fy = this->y() + 0.2 * sin(this->h());
+               double by = this->y() - 0.2 * sin(this->h()); // rear is back
+               out << lx << " " << ly << std::endl;
+               out << rx << " " << ry << std::endl;
+               out << std::endl;
+               out << fx << " " << fy << std::endl;
+               out << bx << " " << by << std::endl;
+               out << std::endl;
+       }
+       if (opts.LEFT_MIRROR) {
+               this->lf().gen_gnuplot_to(out);
+               this->lfm().gen_gnuplot_to(out);
+               this->lr().gen_gnuplot_to(out);
+               out << std::endl;
+
+       }
+       if (opts.RIGHT_MIRROR) {
+               this->rf().gen_gnuplot_to(out);
+               this->rfm().gen_gnuplot_to(out);
+               this->rr().gen_gnuplot_to(out);
+               out << std::endl;
+       }
+}
+
+} // namespace bcar