double x_ = 0.0;
double y_ = 0.0;
public:
- Point(double x, double y);
Point();
+ Point(double x, double y);
/*! Get horizontal coordinate. */
double x() const;
/*! \brief Return `true` if on the right side of the plane.
*
- * The plane is given by the line `li`, where `li->fp()` is the base
- * point and the direction is given by `li->lp() - li->fp()`.
+ * The plane is given by the line `li`, where `li->b()` is the base
+ * point and the direction is given by `li->e() - li->b()`.
*
* \param li The plane to consider is given by `li`.
*/
*/
void rotate(Point const& c, double const angl);
+ /*! Return Euclidean distance to `p`. */
+ double edist(Point const& p) const;
+
friend std::ostream& operator<<(std::ostream& out, Point const& p);
};
class Line {
private:
- Point first;
- Point last;
- Point intersection1;
- Point intersection2;
+ Point b_;
+ Point e_;
+ Point i1_;
+ Point i2_;
public:
Line(Point const& fp, Point const& lp);
- /*! Get first point. */
- Point fp() const&;
+ /*! Get beginning point. */
+ Point b() const&;
- /*! Get last point. */
- Point lp() const&;
+ /*! Get end point. */
+ Point e() const&;
/*! Get intersection point. */
- Point in1() const&;
+ Point i1() const&;
/*! Get intersection point. */
- Point in2() const&;
+ Point i2() const&;
/*! \brief Return if `this` line intersects with line `li`.
*
* If the method returns `true`, the intersection `Point` is available
- * in `this->in1()`.
+ * in `this->i1()`.
*
* \see https://en.wikipedia.org/wiki/Line%E2%80%93line_intersection
*
/*! \brief Return intersections of `this` (infinite) line and circle.
*
* If the method returns `true`, the intersection `Point`s are available
- * in `this->in1()` and `this->in2()`.
+ * in `this->i1()` and `this->i2()`.
*
* \see https://mathworld.wolfram.com/Circle-LineIntersection.html
*
bool intersects_with(Point const& c, double const r);
double len() const;
+
+ friend std::ostream& operator<<(std::ostream& out, Line const& li);
};
/*! Store coordinates `x`, `y`, and heading `h`. */
private:
double h_ = 0.0;
public:
+ using Point::Point;
Pose(double x, double y, double h);
- Pose();
/*! Get heading in the interval [-pi, +pi] radians. */
double h() const;
*/
class CarSize {
private:
- double curb_to_curb = 10.820;
- double width = 1.625;
- double wheelbase = 2.450;
- double distance_to_front = 3.105;
- double length = 3.760;
+ double curb_to_curb_ = 10.820;
+ double width_ = 1.625;
+ double wheelbase_ = 2.450;
+ double distance_to_front_ = 3.105;
+ double length_ = 3.760;
public:
/*! Get curb-to-curb distance. */
double ctc() const;
/*! \brief Get minimum turning radius.
*
* Please, note that the method returns really _minimum turning radius_,
- * which is the distance from the reare axle center to the center of
+ * which is the distance from the rear axle center to the center of
* left or right rotation given by the kinematics constrants, i.e.
* _wheelbase_ and _curb-to-curb_ distance.
*
/*! Store car motion. */
class CarMove {
private:
- double speed = 0.0;
- double steer = 0.0;
+ double speed_ = 0.0;
+ double steer_ = 0.0;
public:
/*! Get speed. */
double sp() const;
/*! Get frame's right front y coordinate. */
double rfy() const;
+ /*! Get frame's left front point. */
+ Point lf() const;
+
+ /*! Get frame's left rear point. */
+ Point lr() const;
+
+ /*! Get frame's right rear point. */
+ Point rr() const;
+
+ /*! Get frame's right front point. */
+ Point rf() const;
+
+ /*! Get frame's left side. */
+ Line left() const;
+
+ /*! Get frame's rear side. */
+ Line rear() const;
+
+ /*! Get frame's right side. */
+ Line right() const;
+
+ /*! Get frame's front side. */
+ Line front() const;
+
/*! Get rear axle's left x coordinate. */
double ralx() const;
#define BCAR_PSLOT_H
#include <ostream>
+#include <vector>
#include "bcar.hh"
namespace bcar {
*/
class ParkingSlot {
private:
+ double offset_ = 0.001; // to avoid collision during init
Point border_[4];
Line entry_;
Line rear_;
ParkingSlot(double lrx, double lry, double rrx, double rry, double rfx,
double rfy, double lfx, double lfy);
+ /*! Get slot's length. */
+ double len() const;
+
+ /*! Get slot's width. */
+ double w() const;
+
/*! Get slot's left front x coordinate. */
double lfx() const;
/*! Change side of the parking slot. */
void swap_side();
+ /*! Return `true` if car `c` is parking in slot `this`. */
+ bool parked(BicycleCar const& c) const;
+
+ /*! Return `true` if `c`'s car frame collide with `this` border. */
+ bool collide(BicycleCar const& c) const;
+
+ /*! \brief Drive car `c` into the parking slot `this`.
+ *
+ * \param c Starting bicycle car.
+ * \param max Maximum number of backward-forward direction changes.
+ */
+ std::vector<BicycleCar> drive_in_slot(BicycleCar c, unsigned int& max);
+
+ /*! Find entry. */
+ PoseRange fe();
+
friend std::ostream& operator<<(std::ostream& o, ParkingSlot const& s);
};
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)
{
}
bool
Point::on_right_side_of(Line const& li) const
{
- auto x1 = li.fp().x();
- auto y1 = li.fp().y();
- auto x2 = li.lp().x();
- auto y2 = li.lp().y();
+ 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) {
this->y(ny + c.y());
}
+double
+Point::edist(Point const& p) const
+{
+ return sqrt(pow(p.x() - this->x_, 2.0) + pow(p.y() - this->y_, 2.0));
+}
+
std::ostream&
operator<<(std::ostream& out, Point const& p)
{
return out;
}
-Line::Line(Point const& fp, Point const& lp): first(fp), last(lp),
- intersection1(Point(0.0, 0.0)), intersection2(Point(0.0, 0.0))
+Line::Line(Point const& b, Point const& e): b_(b), e_(e)
{
}
Point
-Line::fp() const&
+Line::b() const&
{
- return this->first;
+ return this->b_;
}
Point
-Line::lp() const&
+Line::e() const&
{
- return this->last;
+ return this->e_;
}
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;
if (t < 0.0 || t > 1.0 || u < 0.0 || u > 1.0) {
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;
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;
}
double
Line::len() const
{
- double dx = this->lp().x() - this->fp().x();
- double dy = this->lp().y() - this->fp().y();
- return sqrt(dx * dx + dy * dy);
+ return this->b_.edist(this->e_);
}
-Pose::Pose() : Point()
+std::ostream&
+operator<<(std::ostream& out, Line const& li)
{
+ out << "[" << li.b_ << "," << li.e_ << "]";
+ return out;
}
Pose::Pose(double x, double y, double h) : Point(x, y), h_(h)
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::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
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
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
{
{
}
+double
+ParkingSlot::len() const
+{
+ return this->entry_.len();
+}
+
+double
+ParkingSlot::w() const
+{
+ return this->rear_.len();
+}
+
double
ParkingSlot::lfx() const
{
this->border_[2].rotate(this->border_[3], M_PI);
}
+bool
+ParkingSlot::parked(BicycleCar const& c) const
+{
+ auto b_len = sizeof(this->border_) / sizeof(this->border_[0]);
+ std::vector<Point> b(this->border_, this->border_ + b_len);
+ return c.lf().inside_of(b) && c.lr().inside_of(b)
+ && c.rr().inside_of(b) && c.rf().inside_of(b);
+}
+
+bool
+ParkingSlot::collide(BicycleCar const& c) const
+{
+ return c.left().intersects_with(this->rear_)
+ && c.left().intersects_with(this->curb_)
+ && c.left().intersects_with(this->front_)
+ && c.rear().intersects_with(this->rear_)
+ && c.rear().intersects_with(this->curb_)
+ && c.rear().intersects_with(this->front_)
+ && c.right().intersects_with(this->rear_)
+ && c.right().intersects_with(this->curb_)
+ && c.right().intersects_with(this->front_)
+ && c.front().intersects_with(this->rear_)
+ && c.front().intersects_with(this->curb_)
+ && c.front().intersects_with(this->front_);
+}
+
+std::vector<BicycleCar>
+ParkingSlot::drive_in_slot(BicycleCar c, unsigned int& max)
+{
+ assert(this->parallel());
+ assert(this->right());
+ assert(c.len() < this->len());
+ assert(c.w() < this->w());
+ std::vector<BicycleCar> path;
+ path.reserve(max + 2);
+ path.push_back(c);
+ unsigned int cusp = 0;
+ while (cusp < max) {
+ if (c.h() < this->h()) {
+ return std::vector<BicycleCar>();
+ }
+ if (this->parked(c)) {
+ if (cusp < max) {
+ max = cusp;
+ }
+ break;
+ }
+ c.next();
+ if (this->collide(c)) {
+ c.sp(c.sp() * -1.0);
+ c.next();
+ path.push_back(c);
+ c.st(c.st() * -1.0);
+ cusp += 1;
+ }
+ }
+ path.push_back(c);
+ return path;
+}
+
+PoseRange
+ParkingSlot::fe()
+{
+ assert(this->parallel());
+ assert(this->right());
+ BicycleCar c;
+ c.h(this->h());
+ double clen = this->offset_ + this->len() - c.df();
+ double cw = this->offset_ + c.w() / 2.0;
+ c.x(this->lrx() + clen * cos(c.h()) + cw * cos(c.h() + M_PI / 2.0));
+ c.y(this->lry() + clen * sin(c.h()) + cw * sin(c.h() + M_PI / 2.0));
+ c.set_max_steer();
+ c.sp(-0.01);
+ auto const& b3 = this->border_[3];
+ this->curb_.intersects_with(b3, c.len());
+ double max_to_slot;
+ auto const& rr = c.rr();
+ auto const& i1 = this->curb_.i1();
+ auto const& i2 = this->curb_.i2();
+ if (rr.edist(i1) < rr.edist(i2)) {
+ max_to_slot = rr.min_angle_between(b3, i1);
+ } else {
+ max_to_slot = rr.min_angle_between(b3, i2);
+ }
+ std::vector<BicycleCar> starts;
+ double a_to_slot = 0.0;
+ while (a_to_slot < max_to_slot) {
+ a_to_slot += 0.001;
+ c.rotate(b3, 0.001);
+ starts.push_back(c);
+ }
+ std::vector<std::vector<BicycleCar>> entries;
+ unsigned int max_cusp = 10;
+ for (auto s: starts) {
+ auto r = this->drive_in_slot(s, max_cusp);
+ if (r.size() > 0) {
+ entries.push_back(r);
+ }
+ }
+ assert(entries.size() > 0);
+ auto& c1 = entries.front().front();
+ auto& c2 = entries.back().front();
+ double b = std::min(c1.h(), c2.h());
+ double e = std::max(c1.h(), c2.h());
+ clen = c.len();
+ Point b1(c1.x() - clen * cos(c1.h()), c1.y() - clen * sin(c1.h()));
+ Point b2(c2.x() - clen * cos(c2.h()), c2.y() - clen * sin(c2.h()));
+ Point e1(c1.x() + clen * cos(c1.h()), c1.y() + clen * sin(c1.h()));
+ Point e2(c2.x() + clen * cos(c2.h()), c2.y() + clen * sin(c2.h()));
+ Line li1(b1, e1);
+ Line li2(b2, e2);
+ li1.intersects_with(li2);
+ PoseRange pr;
+ pr.x(li1.i1().x());
+ pr.y(li1.i1().y());
+ pr.b(b);
+ pr.e(e);
+ return pr;
+}
+
std::ostream&
operator<<(std::ostream& o, ParkingSlot const& s)
{
auto li1 = Line(Point(1.0, 1.0), Point(3.0, 3.0));
auto li2 = Line(Point(1.0, 3.0), Point(3.0, 1.0));
WVPASS(li1.intersects_with(li2));
- WVPASSEQ_DOUBLE(li1.in1().x(), 2.0, 0.00001);
- WVPASSEQ_DOUBLE(li1.in1().y(), 2.0, 0.00001);
+ WVPASSEQ_DOUBLE(li1.i1().x(), 2.0, 0.00001);
+ WVPASSEQ_DOUBLE(li1.i1().y(), 2.0, 0.00001);
}
{
auto li1 = Line(Point(1.0, 1.0), Point(1.0, 3.0));
projects / hubacji1 / bcar.git / commitdiff