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`.
*/
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
*
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;
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) {
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_);
}
std::ostream&
operator<<(std::ostream& out, Line const& li)
{
- out << "[" << li.first << "," << li.last << "]";
+ out << "[" << li.b_ << "," << li.e_ << "]";
return out;
}
-Pose::Pose() : Point()
-{
-}
-
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
this->curb_.intersects_with(b3, c.len());
double max_to_slot;
auto const& rr = c.rr();
- auto const& i1 = this->curb_.in1();
- auto const& i2 = this->curb_.in2();
+ 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 {
Line li2(b2, e2);
li1.intersects_with(li2);
PoseRange pr;
- pr.x(li1.in1().x());
- pr.y(li1.in1().y());
+ pr.x(li1.i1().x());
+ pr.y(li1.i1().y());
pr.b(b);
pr.e(e);
return pr;
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