--- /dev/null
+root = true
+
+[*.{cc,hh}]
+end_of_line = lf
+insert_final_newline = true
+charset = utf-8
+trim_trailing_whitespace = true
+indent_style = tab
+indent_size = 8
+max_line_length = 80
\param st Steering of the car.
*/
class BicycleCar {
- private:
- // coordinates
- double x_ = 0;
- double y_ = 0;
- double h_ = 0;
- // kinematic constraints
- double ctc_ = 10.820; // curb-to-curb
- // FIXME is not mtr; curb-to-curb is 10.820
- double mtr_ = 10.820;
- double wb_ = 2.450;
- // dimensions
- double w_ = 1.625;
- double l_ = 3.760;
- double he_ = 1.450;
- double sd_ = 0;
- double df_ = 3.105;
- double dr_ = 0.655;
- // moving
- double sp_ = 0;
- double st_ = 0;
- public:
- // kinematic constraints
- /*! \brief Return `false` if `bc` is not achievable.
-
- When `false` is returned the `bc` may still be drivable,
- because only "line segment - circle arc - line segment"
- paths are considered in ``drivable`` method.
-
- \param[in] bc The bicycle car to achieve.
- */
- bool drivable(const BicycleCar &bc) const;
- bool drivable(const BicycleCar &bc, double b, double e) const;
- /*! \brief Return inner radius.
-
- The inner radius is the distance from minimum turning
- radius circle center to the nearest point on the car. In
- this case, the nearest points on the car are rear axle
- endpoints.
- */
- double iradi() const;
- /*! \brief Return outer front radius.
-
- The outer front radius is the distance from minimum
- turning radius circle center to the farthest point on
- the front (from the rear axle view) part of the car.
- */
- double ofradi() const;
- /*! \brief Return outer rear radius.
-
- The outer rear radius is the distance from minimum
- turning radius circle center to the farthest point on
- the rear (from the rear axle view) part of the car.
- */
- double orradi() const;
- /*! \brief Return length of perfect parking slot.
-
- The width of the slot is the same as the width of the
- car.
- */
- double perfect_parking_slot_len() const;
- /*! \brief Set maximum steering angle.
- */
- void set_max_steer();
-
- // car frame
- double lfx() const; double lfy() const;
- double lrx() const; double lry() const;
- double rrx() const; double rry() const;
- double rfx() const; double rfy() const;
-
- double ralx() const; double raly() const;
- double rarx() const; double rary() const;
-
- /*! \brief Min. turning radius circle center on left.
-
- Important are coordinates `x` and `y`. The heading `h`
- is set as the heading of `this->h()`.
- */
- BicycleCar ccl() const;
- /*! \brief Min. turning radius circle center on rigth.
-
- Important are coordinates `x` and `y`. The heading `h`
- is set as the heading of `this->h()`.
- */
- BicycleCar ccr() const;
-
- // moving
- /*! \brief Next car position based on `sp` and `st`.
-
- Where `sp` is speed and `st` is steering of the car.
- */
- void next();
- /*! \brief Rotate self around the point.
-
- \param cx Horizontal coordinate of rotation center.
- \param cy Vertical coordinate of rotation center.
- \param angl Angle of rotation.
- */
- void rotate(double cx, double cy, double angl);
-
- // getters, setters
- double x() const { return this->x_; }
- void x(double x) { this->x_ = x; }
-
- double y() const { return this->y_; }
- void y(double y) { this->y_ = y; }
-
- double h() const { return this->h_; }
- void h(double h)
- {
- while (h < -M_PI)
- h += 2 * M_PI;
- while (h > +M_PI)
- h -= 2 * M_PI;
- this->h_ = h;
- }
-
- double ctc() const { return this->ctc_; }
- void ctc(double ctc) { this->ctc_ = ctc; }
-
- double mtr() const { return this->mtr_; }
- void mtr(double mtr) { this->mtr_ = mtr; }
-
- double wb() const { return this->wb_; }
- void wb(double wb) { this->wb_ = wb; }
-
- double w() const { return this->w_; }
- void w(double w) { this->w_ = w; }
-
- double l() const { return this->l_; }
- void l(double l) { this->l_ = l; }
-
- double he() const { return this->he_; }
- void he(double he) { this->he_ = he; }
-
- double sd() const { return this->sd_; }
- void sd(double sd) { this->sd_ = sd; }
-
- double df() const { return this->df_; }
- void df(double df) { this->df_ = df; }
-
- double dr() const { return this->dr_; }
- void dr(double dr) { this->dr_ = dr; }
-
- double sp() const { return this->sp_; }
- void sp(double sp) { this->sp_ = sp; }
-
- double st() const { return this->st_; }
- void st(double st) { this->st_ = st; }
-
- BicycleCar();
- friend std::ostream &operator<<(
- std::ostream &out,
- const BicycleCar &bc
- )
- {
- out << "[" << bc.x();
- out << "," << bc.y();
- out << "," << bc.h();
- out << "]";
- return out;
- }
+private:
+ // coordinates
+ double x_ = 0;
+ double y_ = 0;
+ double h_ = 0;
+ // kinematic constraints
+ double ctc_ = 10.820; // curb-to-curb
+ // FIXME is not mtr; curb-to-curb is 10.820
+ double mtr_ = 10.820;
+ double wb_ = 2.450;
+ // dimensions
+ double w_ = 1.625;
+ double l_ = 3.760;
+ double he_ = 1.450;
+ double sd_ = 0;
+ double df_ = 3.105;
+ double dr_ = 0.655;
+ // moving
+ double sp_ = 0;
+ double st_ = 0;
+public:
+ // kinematic constraints
+ /*! \brief Return `false` if `bc` is not achievable.
+
+ When `false` is returned the `bc` may still be drivable,
+ because only "line segment - circle arc - line segment"
+ paths are considered in ``drivable`` method.
+
+ \param[in] bc The bicycle car to achieve.
+ */
+ bool drivable(const BicycleCar &bc) const;
+ bool drivable(const BicycleCar &bc, double b, double e) const;
+ /*! \brief Return inner radius.
+
+ The inner radius is the distance from minimum turning
+ radius circle center to the nearest point on the car. In
+ this case, the nearest points on the car are rear axle
+ endpoints.
+ */
+ double iradi() const;
+ /*! \brief Return outer front radius.
+
+ The outer front radius is the distance from minimum
+ turning radius circle center to the farthest point on
+ the front (from the rear axle view) part of the car.
+ */
+ double ofradi() const;
+ /*! \brief Return outer rear radius.
+
+ The outer rear radius is the distance from minimum
+ turning radius circle center to the farthest point on
+ the rear (from the rear axle view) part of the car.
+ */
+ double orradi() const;
+ /*! \brief Return length of perfect parking slot.
+
+ The width of the slot is the same as the width of the
+ car.
+ */
+ double perfect_parking_slot_len() const;
+ /*! \brief Set maximum steering angle.
+ */
+ void set_max_steer();
+
+ // car frame
+ double lfx() const; double lfy() const;
+ double lrx() const; double lry() const;
+ double rrx() const; double rry() const;
+ double rfx() const; double rfy() const;
+
+ double ralx() const; double raly() const;
+ double rarx() const; double rary() const;
+
+ /*! \brief Min. turning radius circle center on left.
+
+ Important are coordinates `x` and `y`. The heading `h`
+ is set as the heading of `this->h()`.
+ */
+ BicycleCar ccl() const;
+ /*! \brief Min. turning radius circle center on rigth.
+
+ Important are coordinates `x` and `y`. The heading `h`
+ is set as the heading of `this->h()`.
+ */
+ BicycleCar ccr() const;
+
+ // moving
+ /*! \brief Next car position based on `sp` and `st`.
+
+ Where `sp` is speed and `st` is steering of the car.
+ */
+ void next();
+ /*! \brief Rotate self around the point.
+
+ \param cx Horizontal coordinate of rotation center.
+ \param cy Vertical coordinate of rotation center.
+ \param angl Angle of rotation.
+ */
+ void rotate(double cx, double cy, double angl);
+
+ // getters, setters
+ double x() const { return this->x_; }
+ void x(double x) { this->x_ = x; }
+
+ double y() const { return this->y_; }
+ void y(double y) { this->y_ = y; }
+
+ double h() const { return this->h_; }
+ void h(double h)
+ {
+ while (h < -M_PI)
+ h += 2 * M_PI;
+ while (h > +M_PI)
+ h -= 2 * M_PI;
+ this->h_ = h;
+ }
+
+ double ctc() const { return this->ctc_; }
+ void ctc(double ctc) { this->ctc_ = ctc; }
+
+ double mtr() const { return this->mtr_; }
+ void mtr(double mtr) { this->mtr_ = mtr; }
+
+ double wb() const { return this->wb_; }
+ void wb(double wb) { this->wb_ = wb; }
+
+ double w() const { return this->w_; }
+ void w(double w) { this->w_ = w; }
+
+ double l() const { return this->l_; }
+ void l(double l) { this->l_ = l; }
+
+ double he() const { return this->he_; }
+ void he(double he) { this->he_ = he; }
+
+ double sd() const { return this->sd_; }
+ void sd(double sd) { this->sd_ = sd; }
+
+ double df() const { return this->df_; }
+ void df(double df) { this->df_ = df; }
+
+ double dr() const { return this->dr_; }
+ void dr(double dr) { this->dr_ = dr; }
+
+ double sp() const { return this->sp_; }
+ void sp(double sp) { this->sp_ = sp; }
+
+ double st() const { return this->st_; }
+ void st(double st) { this->st_ = st; }
+
+ BicycleCar();
+ friend std::ostream &operator<<(
+ std::ostream &out,
+ const BicycleCar &bc
+ )
+ {
+ out << "[" << bc.x();
+ out << "," << bc.y();
+ out << "," << bc.h();
+ out << "]";
+ return out;
+ }
};
/*! \brief Does two polygons collide?
*/
std::tuple<bool, unsigned int, unsigned int>
collide(
- std::vector<std::tuple<double, double>> &p1,
- std::vector<std::tuple<double, double>> &p2
+ std::vector<std::tuple<double, double>> &p1,
+ std::vector<std::tuple<double, double>> &p2
);
/*! \brief Is `x, y` coordinate in polygon `poly`?
*/
std::tuple<bool, double, double>
intersect(
- double x1, double y1,
- double x2, double y2,
- double x3, double y3,
- double x4, double y4
+ double x1, double y1,
+ double x2, double y2,
+ double x3, double y3,
+ double x4, double y4
);
/*! \brief Return intersections of (infinite) line and circle.
*/
std::tuple<bool, double, double, double, double>
intersect(
- double cx, double cy, double r,
- double x1, double y1,
- double x2, double y2
+ double cx, double cy, double r,
+ double x1, double y1,
+ double x2, double y2
);
/*! \brief Return the smallest angle between three points.
*/
double
angle_between_three_points(
- double x1, double y1,
- double x2, double y2,
- double x3, double y3
+ double x1, double y1,
+ double x2, double y2,
+ double x3, double y3
);
/*! \brief Return if point is on the right side of plane.
*/
bool
right_side_of_line(
- double x1, double y1,
- double x2, double y2,
- double x3, double y3
+ double x1, double y1,
+ double x2, double y2,
+ double x3, double y3
);
#endif /* BCAR_H */
\param border Array of 4 `x`, `y` values - the borderd of the parking slot.
*/
class ParkingSlot {
- private:
- double border_[4][2] = {
- {0, 0},
- {1, 0},
- {1, 2},
- {0, 2}
- };
- public:
- /*! \brief Reverse order of border coordinates.
- */
- void reverse_border();
+private:
+ double border_[4][2] = {
+ {0, 0},
+ {1, 0},
+ {1, 2},
+ {0, 2}
+ };
+public:
+ /*! \brief Reverse order of border coordinates.
+ */
+ void reverse_border();
- // slot info
- /*! \brief Return orientation of the parking slot.
+ // slot info
+ /*! \brief Return orientation of the parking slot.
- The orientation of the parking slot is computed as the
- direction from the first to the last border coordinates.
- */
- double heading() const;
- /*! \brief Return `true` if slot is parallel.
+ The orientation of the parking slot is computed as the
+ direction from the first to the last border coordinates.
+ */
+ double heading() const;
+ /*! \brief Return `true` if slot is parallel.
- There are two slot types - parallel and perpendicular.
- */
- bool parallel() const;
- /*! \brief Return `true` if slot is on the right.
+ There are two slot types - parallel and perpendicular.
+ */
+ bool parallel() const;
+ /*! \brief Return `true` if slot is on the right.
- The slot could be on right or the left side.
- */
- bool right() const;
+ The slot could be on right or the left side.
+ */
+ bool right() const;
- // getters, setters
- double x1() const { return this->border_[0][0]; }
- double y1() const { return this->border_[0][1]; }
- double x2() const { return this->border_[1][0]; }
- double y2() const { return this->border_[1][1]; }
- double x3() const { return this->border_[2][0]; }
- double y3() const { return this->border_[2][1]; }
- double x4() const { return this->border_[3][0]; }
- double y4() const { return this->border_[3][1]; }
- /*! \brief Set parking slot border.
+ // getters, setters
+ double x1() const { return this->border_[0][0]; }
+ double y1() const { return this->border_[0][1]; }
+ double x2() const { return this->border_[1][0]; }
+ double y2() const { return this->border_[1][1]; }
+ double x3() const { return this->border_[2][0]; }
+ double y3() const { return this->border_[2][1]; }
+ double x4() const { return this->border_[3][0]; }
+ double y4() const { return this->border_[3][1]; }
+ /*! \brief Set parking slot border.
- \param x1 First `x` coordinate.
- \param y1 First `y` coordinate.
- \param x2 Second `x` coordinate.
- \param y2 Second `y` coordinate.
- \param x3 Third `x` coordinate.
- \param y3 Third `y` coordinate.
- \param x4 The last (fourth) `x` coordinate.
- \param y4 The last (fourth) `y` coordinate.
- */
- void border(
- double x1, double y1,
- double x2, double y2,
- double x3, double y3,
- double x4, double y4
- ) {
- this->border_[0][0] = x1;
- this->border_[0][1] = y1;
- this->border_[1][0] = x2;
- this->border_[1][1] = y2;
- this->border_[2][0] = x3;
- this->border_[2][1] = y3;
- this->border_[3][0] = x4;
- this->border_[3][1] = y4;
- };
- /*! \brief Set parking slot.
+ \param x1 First `x` coordinate.
+ \param y1 First `y` coordinate.
+ \param x2 Second `x` coordinate.
+ \param y2 Second `y` coordinate.
+ \param x3 Third `x` coordinate.
+ \param y3 Third `y` coordinate.
+ \param x4 The last (fourth) `x` coordinate.
+ \param y4 The last (fourth) `y` coordinate.
+ */
+ void border(
+ double x1, double y1,
+ double x2, double y2,
+ double x3, double y3,
+ double x4, double y4
+ ) {
+ this->border_[0][0] = x1;
+ this->border_[0][1] = y1;
+ this->border_[1][0] = x2;
+ this->border_[1][1] = y2;
+ this->border_[2][0] = x3;
+ this->border_[2][1] = y3;
+ this->border_[3][0] = x4;
+ this->border_[3][1] = y4;
+ };
+ /*! \brief Set parking slot.
- \param x Horizontal coordinate of entry side center.
- \param y Vertical coordinate of entry side center.
- \param h The heading towards slot.
- \param w The width of the slot.
- \param l The length of the slot.
- */
- void set_slot(
- double x,
- double y,
- double h,
- double w,
- double l
- );
+ \param x Horizontal coordinate of entry side center.
+ \param y Vertical coordinate of entry side center.
+ \param h The heading towards slot.
+ \param w The width of the slot.
+ \param l The length of the slot.
+ */
+ void set_slot(
+ double x,
+ double y,
+ double h,
+ double w,
+ double l
+ );
- ParkingSlot();
- friend std::ostream &operator<<(
- std::ostream &out,
- const ParkingSlot &ps
- )
- {
- out << "[[" << ps.x1() << "," << ps.y1() << "]";
- out << ",[" << ps.x2() << "," << ps.y2() << "]";
- out << ",[" << ps.x3() << "," << ps.y3() << "]";
- out << ",[" << ps.x4() << "," << ps.y4() << "]";
- out << "]";
- return out;
- }
+ ParkingSlot();
+ friend std::ostream &operator<<(
+ std::ostream &out,
+ const ParkingSlot &ps
+ )
+ {
+ out << "[[" << ps.x1() << "," << ps.y1() << "]";
+ out << ",[" << ps.x2() << "," << ps.y2() << "]";
+ out << ",[" << ps.x3() << "," << ps.y3() << "]";
+ out << ",[" << ps.x4() << "," << ps.y4() << "]";
+ out << "]";
+ return out;
+ }
};
template <typename T> int sgn(T val) {
- return (T(0) < val) - (val < T(0));
+ return (T(0) < val) - (val < T(0));
}
#endif /* PSLOT_H */
}
bool BicycleCar::drivable(const BicycleCar &bc, double b, double e) const
{
- // assert bc.h() == (b + e) / 2.0
- double a_1 = atan2(bc.y() - this->y(), bc.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 = bc.h() - this->h();
- while (h_d < -M_PI)
- h_d += 2 * M_PI;
- while (h_d > +M_PI)
- h_d -= 2 * M_PI;
- double a_2 = 0;
- if (h_d == 0 && (a_1 == 0 || a_2 == M_PI || a_2 == -M_PI)) {
- return true;
- } else if (0 < a_1 && a_1 <= M_PI/2) { // left front
- BicycleCar z(*this); // zone border
- z.h(e);
- h_d = bc.h() - this->h();
- z.rotate(this->ccl().x(), this->ccl().y(), h_d);
- // assert z.h() == bc.h()
- if (bc.y() == z.y() && bc.x() == z.x()) // bc on zone border
- return true;
- a_2 = atan2(bc.y() - z.y(), bc.x() - z.x());
- while (a_2 < -M_PI)
- a_2 += 2 * M_PI;
- while (a_2 > +M_PI)
- a_2 -= 2 * M_PI;
- if (z.h() >= a_2 && a_2 >= this->h())
- return true;
- } else if (M_PI/2 < a_1 && a_1 <= M_PI) { // left rear
- BicycleCar z(*this); // zone border
- z.h(e);
- h_d = bc.h() - this->h();
- z.rotate(this->ccl().x(), this->ccl().y(), h_d);
- // assert z.h() == bc.h()
- if (bc.y() == z.y() && bc.x() == z.x()) // bc on zone border
- return true;
- a_2 = atan2(bc.y() - z.y(), bc.x() - z.x());
- a_2 -= M_PI;
- while (a_2 < -M_PI)
- a_2 += 2 * M_PI;
- while (a_2 > +M_PI)
- a_2 -= 2 * M_PI;
- if (this->h() >= a_2 && a_2 >= z.h())
- return true;
- } else if (0 > a_1 && a_1 >= -M_PI/2) { // right front
- BicycleCar z(*this); // zone border
- z.h(b);
- h_d = bc.h() - this->h();
- z.rotate(this->ccr().x(), this->ccr().y(), h_d);
- // assert z.h() == bc.h()
- if (bc.y() == z.y() && bc.x() == z.x()) // bc on zone border
- return true;
- a_2 = atan2(bc.y() - z.y(), bc.x() - z.x());
- while (a_2 < -M_PI)
- a_2 += 2 * M_PI;
- while (a_2 > +M_PI)
- a_2 -= 2 * M_PI;
- if (this->h() >= a_2 && a_2 >= z.h())
- return true;
- } else if (-M_PI/2 > a_1 && a_1 >= -M_PI) { // right rear
- BicycleCar z(*this); // zone border
- z.h(b);
- h_d = bc.h() - this->h();
- z.rotate(this->ccr().x(), this->ccr().y(), h_d);
- // assert z.h() == bc.h()
- if (bc.y() == z.y() && bc.x() == z.x()) // bc on zone border
- return true;
- a_2 = atan2(bc.y() - z.y(), bc.x() - z.x());
- a_2 -= M_PI;
- while (a_2 < -M_PI)
- a_2 += 2 * M_PI;
- while (a_2 > +M_PI)
- a_2 -= 2 * M_PI;
- if (z.h() >= a_2 && a_2 >= this->h())
- return true;
- } else {
- // Not happenning, as ``-pi <= a <= pi``.
- }
- return false;
+ // assert bc.h() == (b + e) / 2.0
+ double a_1 = atan2(bc.y() - this->y(), bc.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 = bc.h() - this->h();
+ while (h_d < -M_PI)
+ h_d += 2 * M_PI;
+ while (h_d > +M_PI)
+ h_d -= 2 * M_PI;
+ double a_2 = 0;
+ if (h_d == 0 && (a_1 == 0 || a_2 == M_PI || a_2 == -M_PI)) {
+ return true;
+ } else if (0 < a_1 && a_1 <= M_PI/2) { // left front
+ BicycleCar z(*this); // zone border
+ z.h(e);
+ h_d = bc.h() - this->h();
+ z.rotate(this->ccl().x(), this->ccl().y(), h_d);
+ // assert z.h() == bc.h()
+ if (bc.y() == z.y() && bc.x() == z.x()) // bc on zone border
+ return true;
+ a_2 = atan2(bc.y() - z.y(), bc.x() - z.x());
+ while (a_2 < -M_PI)
+ a_2 += 2 * M_PI;
+ while (a_2 > +M_PI)
+ a_2 -= 2 * M_PI;
+ if (z.h() >= a_2 && a_2 >= this->h())
+ return true;
+ } else if (M_PI/2 < a_1 && a_1 <= M_PI) { // left rear
+ BicycleCar z(*this); // zone border
+ z.h(e);
+ h_d = bc.h() - this->h();
+ z.rotate(this->ccl().x(), this->ccl().y(), h_d);
+ // assert z.h() == bc.h()
+ if (bc.y() == z.y() && bc.x() == z.x()) // bc on zone border
+ return true;
+ a_2 = atan2(bc.y() - z.y(), bc.x() - z.x());
+ a_2 -= M_PI;
+ while (a_2 < -M_PI)
+ a_2 += 2 * M_PI;
+ while (a_2 > +M_PI)
+ a_2 -= 2 * M_PI;
+ if (this->h() >= a_2 && a_2 >= z.h())
+ return true;
+ } else if (0 > a_1 && a_1 >= -M_PI/2) { // right front
+ BicycleCar z(*this); // zone border
+ z.h(b);
+ h_d = bc.h() - this->h();
+ z.rotate(this->ccr().x(), this->ccr().y(), h_d);
+ // assert z.h() == bc.h()
+ if (bc.y() == z.y() && bc.x() == z.x()) // bc on zone border
+ return true;
+ a_2 = atan2(bc.y() - z.y(), bc.x() - z.x());
+ while (a_2 < -M_PI)
+ a_2 += 2 * M_PI;
+ while (a_2 > +M_PI)
+ a_2 -= 2 * M_PI;
+ if (this->h() >= a_2 && a_2 >= z.h())
+ return true;
+ } else if (-M_PI/2 > a_1 && a_1 >= -M_PI) { // right rear
+ BicycleCar z(*this); // zone border
+ z.h(b);
+ h_d = bc.h() - this->h();
+ z.rotate(this->ccr().x(), this->ccr().y(), h_d);
+ // assert z.h() == bc.h()
+ if (bc.y() == z.y() && bc.x() == z.x()) // bc on zone border
+ return true;
+ a_2 = atan2(bc.y() - z.y(), bc.x() - z.x());
+ a_2 -= M_PI;
+ while (a_2 < -M_PI)
+ a_2 += 2 * M_PI;
+ while (a_2 > +M_PI)
+ a_2 -= 2 * M_PI;
+ if (z.h() >= a_2 && a_2 >= this->h())
+ return true;
+ } else {
+ // Not happenning, as ``-pi <= a <= pi``.
+ }
+ return false;
}
double BicycleCar::iradi() const
{
- return this->mtr() - this->w() / 2;
+ return this->mtr() - this->w() / 2;
}
double BicycleCar::ofradi() const
{
- return sqrt(pow(this->mtr() + this->w() / 2, 2) + pow(this->df(), 2));
+ return sqrt(pow(this->mtr() + this->w() / 2, 2) + pow(this->df(), 2));
}
double BicycleCar::orradi() const
{
- return sqrt(pow(this->mtr() + this->w() / 2, 2) + pow(this->dr(), 2));
+ return sqrt(pow(this->mtr() + this->w() / 2, 2) + pow(this->dr(), 2));
}
double BicycleCar::perfect_parking_slot_len() const
{
- // see Simon R. Blackburn *The Geometry of Perfect Parking*
- // see https://www.ma.rhul.ac.uk/SRBparking
- double r = this->ctc() / 2;
- double l = this->wb();
- double k = this->df() - this->wb();
- double w = this->w();
- return
- this->l()
- + sqrt(
- (r*r - l*l)
- + pow(l + k, 2)
- - pow(sqrt(r*r - l*l) - w, 2)
- )
- - l
- - k
- ;
+ // see Simon R. Blackburn *The Geometry of Perfect Parking*
+ // see https://www.ma.rhul.ac.uk/SRBparking
+ double r = this->ctc() / 2;
+ double l = this->wb();
+ double k = this->df() - this->wb();
+ double w = this->w();
+ return
+ this->l()
+ + sqrt(
+ (r*r - l*l)
+ + pow(l + k, 2)
+ - pow(sqrt(r*r - l*l) - w, 2)
+ )
+ - l
+ - k
+ ;
}
void BicycleCar::set_max_steer()
{
- this->st(atan(this->wb() / this->mtr()));
+ this->st(atan(this->wb() / this->mtr()));
}
// car frame
double BicycleCar::lfx() const
{
- double lfx = this->x();
- lfx += (this->w() / 2) * cos(this->h() + M_PI / 2);
- lfx += this->df() * cos(this->h());
- lfx += this->sd() * cos(this->h());
- return lfx;
+ double lfx = this->x();
+ lfx += (this->w() / 2) * cos(this->h() + M_PI / 2);
+ lfx += this->df() * cos(this->h());
+ lfx += this->sd() * cos(this->h());
+ return lfx;
}
double BicycleCar::lfy() const
{
- double lfy = this->y();
- lfy += (this->w() / 2) * sin(this->h() + M_PI / 2);
- lfy += this->df() * sin(this->h());
- lfy += this->sd() * sin(this->h());
- return lfy;
+ double lfy = this->y();
+ lfy += (this->w() / 2) * sin(this->h() + M_PI / 2);
+ lfy += this->df() * sin(this->h());
+ lfy += this->sd() * sin(this->h());
+ return lfy;
}
double BicycleCar::lrx() const
{
- double lrx = this->x();
- lrx += (this->w() / 2) * cos(this->h() + M_PI / 2);
- lrx += -this->dr() * cos(this->h());
- lrx += -this->sd() * cos(this->h());
- return lrx;
+ double lrx = this->x();
+ lrx += (this->w() / 2) * cos(this->h() + M_PI / 2);
+ lrx += -this->dr() * cos(this->h());
+ lrx += -this->sd() * cos(this->h());
+ return lrx;
}
double BicycleCar::lry() const
{
- double lry = this->y();
- lry += (this->w() / 2) * sin(this->h() + M_PI / 2);
- lry += -this->dr() * sin(this->h());
- lry += -this->sd() * sin(this->h());
- return lry;
+ double lry = this->y();
+ lry += (this->w() / 2) * sin(this->h() + M_PI / 2);
+ lry += -this->dr() * sin(this->h());
+ lry += -this->sd() * sin(this->h());
+ return lry;
}
double BicycleCar::rrx() const
{
- double rrx = this->x();
- rrx += (this->w() / 2) * cos(this->h() - M_PI / 2);
- rrx += -this->dr() * cos(this->h());
- rrx += -this->sd() * cos(this->h());
- return rrx;
+ double rrx = this->x();
+ rrx += (this->w() / 2) * cos(this->h() - M_PI / 2);
+ rrx += -this->dr() * cos(this->h());
+ rrx += -this->sd() * cos(this->h());
+ return rrx;
}
double BicycleCar::rry() const
{
- double rry = this->y();
- rry += (this->w() / 2) * sin(this->h() - M_PI / 2);
- rry += -this->dr() * sin(this->h());
- rry += -this->sd() * sin(this->h());
- return rry;
+ double rry = this->y();
+ rry += (this->w() / 2) * sin(this->h() - M_PI / 2);
+ rry += -this->dr() * sin(this->h());
+ rry += -this->sd() * sin(this->h());
+ return rry;
}
double BicycleCar::rfx() const
{
- double rfx = this->x();
- rfx += (this->w() / 2) * cos(this->h() - M_PI / 2);
- rfx += this->df() * cos(this->h());
- rfx += this->sd() * cos(this->h());
- return rfx;
+ double rfx = this->x();
+ rfx += (this->w() / 2) * cos(this->h() - M_PI / 2);
+ rfx += this->df() * cos(this->h());
+ rfx += this->sd() * cos(this->h());
+ return rfx;
}
double BicycleCar::rfy() const
{
- double rfy = this->y();
- rfy += (this->w() / 2) * sin(this->h() - M_PI / 2);
- rfy += this->df() * sin(this->h());
- rfy += this->sd() * sin(this->h());
- return rfy;
+ double rfy = this->y();
+ rfy += (this->w() / 2) * sin(this->h() - M_PI / 2);
+ rfy += this->df() * sin(this->h());
+ rfy += this->sd() * sin(this->h());
+ return rfy;
}
double BicycleCar::ralx() const
{
- double lrx = this->x();
- lrx += (this->w() / 2) * cos(this->h() + M_PI / 2);
- return lrx;
+ double lrx = this->x();
+ lrx += (this->w() / 2) * cos(this->h() + M_PI / 2);
+ return lrx;
}
double BicycleCar::raly() const
{
- double lry = this->y();
- lry += (this->w() / 2) * sin(this->h() + M_PI / 2);
- return lry;
+ double lry = this->y();
+ lry += (this->w() / 2) * sin(this->h() + M_PI / 2);
+ return lry;
}
double BicycleCar::rarx() const
{
- double rrx = this->x();
- rrx += (this->w() / 2) * cos(this->h() - M_PI / 2);
- return rrx;
+ double rrx = this->x();
+ rrx += (this->w() / 2) * cos(this->h() - M_PI / 2);
+ return rrx;
}
double BicycleCar::rary() const
{
- double rry = this->y();
- rry += (this->w() / 2) * sin(this->h() - M_PI / 2);
- return rry;
+ double rry = this->y();
+ rry += (this->w() / 2) * sin(this->h() - M_PI / 2);
+ return rry;
}
BicycleCar BicycleCar::ccl() const
{
- BicycleCar bc;
- bc.x(this->x() + this->mtr() * cos(this->h() + M_PI / 2));
- bc.y(this->y() + this->mtr() * sin(this->h() + M_PI / 2));
- bc.h(this->h());
- return bc;
+ BicycleCar bc;
+ bc.x(this->x() + this->mtr() * cos(this->h() + M_PI / 2));
+ bc.y(this->y() + this->mtr() * sin(this->h() + M_PI / 2));
+ bc.h(this->h());
+ return bc;
}
BicycleCar BicycleCar::ccr() const
{
- BicycleCar bc;
- bc.x(this->x() + this->mtr() * cos(this->h() - M_PI / 2));
- bc.y(this->y() + this->mtr() * sin(this->h() - M_PI / 2));
- bc.h(this->h());
- return bc;
+ BicycleCar bc;
+ bc.x(this->x() + this->mtr() * cos(this->h() - M_PI / 2));
+ bc.y(this->y() + this->mtr() * sin(this->h() - M_PI / 2));
+ bc.h(this->h());
+ return bc;
}
// moving
void BicycleCar::next()
{
- this->x(this->x() + this->sp() * cos(this->h()));
- this->y(this->y() + this->sp() * sin(this->h()));
- this->h(this->h() + this->sp() / this->wb() * tan(this->st()));
+ this->x(this->x() + this->sp() * cos(this->h()));
+ this->y(this->y() + this->sp() * sin(this->h()));
+ this->h(this->h() + this->sp() / this->wb() * tan(this->st()));
}
void BicycleCar::rotate(double cx, double cy, double angl)
{
- double px = this->x();
- double py = this->y();
- px -= cx;
- py -= cy;
- double nx = px * cos(angl) - py * sin(angl);
- double ny = px * sin(angl) + py * cos(angl);
- this->h(this->h() + angl);
- this->x(nx + cx);
- this->y(ny + cy);
+ double px = this->x();
+ double py = this->y();
+ px -= cx;
+ py -= cy;
+ double nx = px * cos(angl) - py * sin(angl);
+ double ny = px * sin(angl) + py * cos(angl);
+ this->h(this->h() + angl);
+ this->x(nx + cx);
+ this->y(ny + cy);
}
BicycleCar::BicycleCar()
{
- // TODO according to mtr_ FIXME
- this->mtr_ = sqrt(
- pow(10.82 / 2, 2)
- - pow(this->wb(), 2)
- )
- - this->w() / 2
- ;
+ // TODO according to mtr_ FIXME
+ this->mtr_ = sqrt(
+ pow(10.82 / 2, 2)
+ - pow(this->wb(), 2)
+ )
+ - this->w() / 2
+ ;
}
std::tuple<bool, unsigned int, unsigned int>
collide(
- std::vector<std::tuple<double, double>> &p1,
- std::vector<std::tuple<double, double>> &p2
+ std::vector<std::tuple<double, double>> &p1,
+ std::vector<std::tuple<double, double>> &p2
)
{
- for (unsigned int i = 0; i < p1.size() - 1; i++) {
- for (unsigned int j = 0; j < p2.size() - 1; j++) {
- auto x = intersect(
- std::get<0>(p1[i]),
- std::get<1>(p1[i]),
- std::get<0>(p1[i + 1]),
- std::get<1>(p1[i + 1]),
- std::get<0>(p2[j]),
- std::get<1>(p2[j]),
- std::get<0>(p2[j + 1]),
- std::get<1>(p2[j + 1])
- );
- if (std::get<0>(x))
- return std::make_tuple(true, i, j);
- }
- }
- return std::make_tuple(false, 0, 0);
+ for (unsigned int i = 0; i < p1.size() - 1; i++) {
+ for (unsigned int j = 0; j < p2.size() - 1; j++) {
+ auto x = intersect(
+ std::get<0>(p1[i]),
+ std::get<1>(p1[i]),
+ std::get<0>(p1[i + 1]),
+ std::get<1>(p1[i + 1]),
+ std::get<0>(p2[j]),
+ std::get<1>(p2[j]),
+ std::get<0>(p2[j + 1]),
+ std::get<1>(p2[j + 1])
+ );
+ if (std::get<0>(x))
+ return std::make_tuple(true, i, j);
+ }
+ }
+ return std::make_tuple(false, 0, 0);
}
bool
inside(double x, double y, std::vector<std::tuple<double, double>> &poly)
{
- unsigned int i = 0;
- unsigned int j = 3;
- bool inside = false;
- for (i = 0; i < 4; i++) {
- if (
- (std::get<1>(poly[i]) > y) != (std::get<1>(poly[j]) > y)
- && (
- x < std::get<0>(poly[i])
- + (std::get<0>(poly[j]) - std::get<0>(poly[i]))
- * (y - std::get<1>(poly[i]))
- / (std::get<1>(poly[j]) - std::get<1>(poly[i]))
- )
- )
- inside = !inside;
- j = i;
- }
- return inside;
+ unsigned int i = 0;
+ unsigned int j = 3;
+ bool inside = false;
+ for (i = 0; i < 4; i++) {
+ if (
+ (std::get<1>(poly[i]) > y) != (std::get<1>(poly[j]) > y)
+ && (
+ x < std::get<0>(poly[i])
+ + (std::get<0>(poly[j]) - std::get<0>(poly[i]))
+ * (y - std::get<1>(poly[i]))
+ / (std::get<1>(poly[j]) - std::get<1>(poly[i]))
+ )
+ )
+ inside = !inside;
+ j = i;
+ }
+ return inside;
}
std::tuple<bool, double, double>
intersect(
- double x1, double y1,
- double x2, double y2,
- double x3, double y3,
- double x4, double y4
+ double x1, double y1,
+ double x2, double y2,
+ double x3, double y3,
+ double x4, double y4
)
{
- double deno = (x1 - x2) * (y3 - y4) - (y1 - y2) * (x3 - x4);
- if (deno == 0)
- return std::make_tuple(false, 0, 0);
- double t = (x1 - x3) * (y3 - y4) - (y1 - y3) * (x3 - x4);
- t /= deno;
- double u = (x1 - x2) * (y1 - y3) - (y1 - y2) * (x1 - x3);
- u *= -1;
- u /= deno;
- if (t < 0 || t > 1 || u < 0 || u > 1)
- return std::make_tuple(false, 0, 0);
- return std::make_tuple(true, x1 + t * (x2 - x1), y1 + t * (y2 - y1));
+ double deno = (x1 - x2) * (y3 - y4) - (y1 - y2) * (x3 - x4);
+ if (deno == 0)
+ return std::make_tuple(false, 0, 0);
+ double t = (x1 - x3) * (y3 - y4) - (y1 - y3) * (x3 - x4);
+ t /= deno;
+ double u = (x1 - x2) * (y1 - y3) - (y1 - y2) * (x1 - x3);
+ u *= -1;
+ u /= deno;
+ if (t < 0 || t > 1 || u < 0 || u > 1)
+ return std::make_tuple(false, 0, 0);
+ return std::make_tuple(true, x1 + t * (x2 - x1), y1 + t * (y2 - y1));
}
std::tuple<bool, double, double, double, double>
intersect(
- double cx, double cy, double r,
- double x1, double y1,
- double x2, double y2
+ double cx, double cy, double r,
+ double x1, double y1,
+ double x2, double y2
) {
- x2 -= cx;
- x1 -= cx;
- y2 -= cy;
- y1 -= cy;
- if (y1 == y2)
- y1 += 0.00001;
- double dx = x2 - x1;
- double dy = y2 - y1;
- double dr = sqrt(dx*dx + dy*dy);
- double D = x1*y2 - x2*y1;
- if (r*r * dr*dr - D*D < 0)
- return std::make_tuple(false, 0, 0, 0, 0);
- // intersection coordinates
- double ix1 = (D*dy + sgn(dy)*dx*sqrt(r*r * dr*dr - D*D)) / (dr*dr);
- ix1 += cx;
- double ix2 = (D*dy - sgn(dy)*dx*sqrt(r*r * dr*dr - D*D)) / (dr*dr);
- ix2 += cx;
- double iy1 = (-D*dx + std::abs(dy)*sqrt(r*r * dr*dr - D*D)) / (dr*dr);
- iy1 += cy;
- double iy2 = (-D*dx - std::abs(dy)*sqrt(r*r * dr*dr - D*D)) / (dr*dr);
- iy2 += cy;
- return std::make_tuple(true, ix1, iy1, ix2, iy2);
+ x2 -= cx;
+ x1 -= cx;
+ y2 -= cy;
+ y1 -= cy;
+ if (y1 == y2)
+ y1 += 0.00001;
+ double dx = x2 - x1;
+ double dy = y2 - y1;
+ double dr = sqrt(dx*dx + dy*dy);
+ double D = x1*y2 - x2*y1;
+ if (r*r * dr*dr - D*D < 0)
+ return std::make_tuple(false, 0, 0, 0, 0);
+ // intersection coordinates
+ double ix1 = (D*dy + sgn(dy)*dx*sqrt(r*r * dr*dr - D*D)) / (dr*dr);
+ ix1 += cx;
+ double ix2 = (D*dy - sgn(dy)*dx*sqrt(r*r * dr*dr - D*D)) / (dr*dr);
+ ix2 += cx;
+ double iy1 = (-D*dx + std::abs(dy)*sqrt(r*r * dr*dr - D*D)) / (dr*dr);
+ iy1 += cy;
+ double iy2 = (-D*dx - std::abs(dy)*sqrt(r*r * dr*dr - D*D)) / (dr*dr);
+ iy2 += cy;
+ return std::make_tuple(true, ix1, iy1, ix2, iy2);
}
double
angle_between_three_points(
- double x1, double y1,
- double x2, double y2,
- double x3, double y3
+ double x1, double y1,
+ double x2, double y2,
+ double x3, double y3
) {
- double d1x = x2 - x1;
- double d1y = y2 - y1;
- double d2x = x3 - x2;
- double d2y = y3 - y2;
+ double d1x = x2 - x1;
+ double d1y = y2 - y1;
+ double d2x = x3 - x2;
+ double d2y = y3 - y2;
- double dot = d1x*d2x + d1y*d2y;
- double d1 = sqrt(d1x*d1x + d1y*d1y);
- double d2 = sqrt(d2x*d2x + d2y*d2y);
+ double dot = d1x*d2x + d1y*d2y;
+ double d1 = sqrt(d1x*d1x + d1y*d1y);
+ double d2 = sqrt(d2x*d2x + d2y*d2y);
- double delta = acos(dot / (d1 * d2));
- return std::min(delta, M_PI - delta);
+ double delta = acos(dot / (d1 * d2));
+ return std::min(delta, M_PI - delta);
}
bool
right_side_of_line(
- double x1, double y1,
- double x2, double y2,
- double x3, double y3
+ double x1, double y1,
+ double x2, double y2,
+ double x3, double y3
) {
- if (sgn((x3 - x1) * (y2 - y1) - (y3 - y1) * (x2 - x1)) < 0)
- return false;
- else
- return true;
+ if (sgn((x3 - x1) * (y2 - y1) - (y3 - y1) * (x2 - x1)) < 0)
+ return false;
+ else
+ return true;
}
void ParkingSlot::reverse_border()
{
- this->border(
- this->x4(), this->y4(),
- this->x3(), this->y3(),
- this->x2(), this->y2(),
- this->x1(), this->y1()
- );
+ this->border(
+ this->x4(), this->y4(),
+ this->x3(), this->y3(),
+ this->x2(), this->y2(),
+ this->x1(), this->y1()
+ );
}
// slot info
double ParkingSlot::heading() const
{
- return atan2(this->y4() - this->y1(), this->x4() - this->x1());
+ return atan2(this->y4() - this->y1(), this->x4() - this->x1());
}
bool ParkingSlot::parallel() const
{
- double d1 = sqrt(
- pow(this->x2() - this->x1(), 2)
- + pow(this->y2() - this->y1(), 2)
- );
- double d2 = sqrt(
- pow(this->x3() - this->x2(), 2)
- + pow(this->y3() - this->y2(), 2)
- );
- if (d1 < d2)
- return true;
- else
- return false;
+ double d1 = sqrt(
+ pow(this->x2() - this->x1(), 2)
+ + pow(this->y2() - this->y1(), 2)
+ );
+ double d2 = sqrt(
+ pow(this->x3() - this->x2(), 2)
+ + pow(this->y3() - this->y2(), 2)
+ );
+ if (d1 < d2)
+ return true;
+ else
+ return false;
}
bool ParkingSlot::right() const
{
- if (sgn(
- (this->x2() - this->x1()) * (this->y4() - this->y1())
- - (this->y2() - this->y1()) * (this->x4() - this->x1())
- ) < 0)
- return false;
- else
- return true;
+ if (sgn(
+ (this->x2() - this->x1()) * (this->y4() - this->y1())
+ - (this->y2() - this->y1()) * (this->x4() - this->x1())
+ ) < 0)
+ return false;
+ else
+ return true;
}
//getters, setters
void ParkingSlot::set_slot(
- double x,
- double y,
- double h,
- double w,
- double l
+ double x,
+ double y,
+ double h,
+ double w,
+ double l
)
{
- double x1 = x + w/2 * cos(h - M_PI/2);
- double y1 = y + w/2 * sin(h - M_PI/2);
- double x2 = x + l * cos(h) + w/2 * cos(h - M_PI/2);
- double y2 = y + l * sin(h) + w/2 * sin(h - M_PI/2);
- double x3 = x + l * cos(h) + w/2 * cos(h + M_PI/2);
- double y3 = y + l * sin(h) + w/2 * sin(h + M_PI/2);
- double x4 = x + w/2 * cos(h + M_PI/2);
- double y4 = y + w/2 * sin(h + M_PI/2);
- this->border(x1, y1, x2, y2, x3, y3, x4, y4);
+ double x1 = x + w/2 * cos(h - M_PI/2);
+ double y1 = y + w/2 * sin(h - M_PI/2);
+ double x2 = x + l * cos(h) + w/2 * cos(h - M_PI/2);
+ double y2 = y + l * sin(h) + w/2 * sin(h - M_PI/2);
+ double x3 = x + l * cos(h) + w/2 * cos(h + M_PI/2);
+ double y3 = y + l * sin(h) + w/2 * sin(h + M_PI/2);
+ double x4 = x + w/2 * cos(h + M_PI/2);
+ double y4 = y + w/2 * sin(h + M_PI/2);
+ this->border(x1, y1, x2, y2, x3, y3, x4, y4);
}
ParkingSlot::ParkingSlot()
WVTEST_MAIN("bcar basic geometry")
{
- BicycleCar bc;
- bc.x(1);
- bc.y(1);
- bc.h(M_PI / 2);
- bc.mtr(10);
- bc.wb(2);
- bc.w(1);
- bc.l(3);
- bc.he(1.5);
- bc.df(2 + 0.5);
- bc.dr(0.5);
+ BicycleCar bc;
+ bc.x(1);
+ bc.y(1);
+ bc.h(M_PI / 2);
+ bc.mtr(10);
+ bc.wb(2);
+ bc.w(1);
+ bc.l(3);
+ bc.he(1.5);
+ bc.df(2 + 0.5);
+ bc.dr(0.5);
- // car frame
- WVPASSEQ_DOUBLE(bc.l(), bc.df() + bc.dr(), 0.00001);
- WVPASSEQ_DOUBLE(0.5, bc.lfx(), 0.00001);
- WVPASSEQ_DOUBLE(0.5, bc.lrx(), 0.00001);
- WVPASSEQ_DOUBLE(1.5, bc.rrx(), 0.00001);
- WVPASSEQ_DOUBLE(1.5, bc.rfx(), 0.00001);
- WVPASSEQ_DOUBLE(3.5, bc.lfy(), 0.00001);
- WVPASSEQ_DOUBLE(0.5, bc.lry(), 0.00001);
- WVPASSEQ_DOUBLE(0.5, bc.rry(), 0.00001);
- WVPASSEQ_DOUBLE(3.5, bc.rfy(), 0.00001);
- WVPASSEQ_DOUBLE(0.5, bc.ralx(), 0.00001);
- WVPASSEQ_DOUBLE(1.5, bc.rarx(), 0.00001);
- WVPASSEQ_DOUBLE(1, bc.raly(), 0.00001);
- WVPASSEQ_DOUBLE(1, bc.rary(), 0.00001);
+ // car frame
+ WVPASSEQ_DOUBLE(bc.l(), bc.df() + bc.dr(), 0.00001);
+ WVPASSEQ_DOUBLE(0.5, bc.lfx(), 0.00001);
+ WVPASSEQ_DOUBLE(0.5, bc.lrx(), 0.00001);
+ WVPASSEQ_DOUBLE(1.5, bc.rrx(), 0.00001);
+ WVPASSEQ_DOUBLE(1.5, bc.rfx(), 0.00001);
+ WVPASSEQ_DOUBLE(3.5, bc.lfy(), 0.00001);
+ WVPASSEQ_DOUBLE(0.5, bc.lry(), 0.00001);
+ WVPASSEQ_DOUBLE(0.5, bc.rry(), 0.00001);
+ WVPASSEQ_DOUBLE(3.5, bc.rfy(), 0.00001);
+ WVPASSEQ_DOUBLE(0.5, bc.ralx(), 0.00001);
+ WVPASSEQ_DOUBLE(1.5, bc.rarx(), 0.00001);
+ WVPASSEQ_DOUBLE(1, bc.raly(), 0.00001);
+ WVPASSEQ_DOUBLE(1, bc.rary(), 0.00001);
- // min. turning radius circle centers
- WVPASSEQ_DOUBLE(bc.h(), bc.ccl().h(), 0.00001);
- WVPASSEQ_DOUBLE(M_PI / 2, bc.ccl().h(), 0.00001);
- WVPASSEQ_DOUBLE(-9, bc.ccl().x(), 0.00001);
- WVPASSEQ_DOUBLE(1, bc.ccl().y(), 0.00001);
- WVPASSEQ_DOUBLE(bc.h(), bc.ccr().h(), 0.00001);
- WVPASSEQ_DOUBLE(M_PI / 2, bc.ccr().h(), 0.00001);
- WVPASSEQ_DOUBLE(11, bc.ccr().x(), 0.00001);
- WVPASSEQ_DOUBLE(1, bc.ccr().y(), 0.00001);
+ // min. turning radius circle centers
+ WVPASSEQ_DOUBLE(bc.h(), bc.ccl().h(), 0.00001);
+ WVPASSEQ_DOUBLE(M_PI / 2, bc.ccl().h(), 0.00001);
+ WVPASSEQ_DOUBLE(-9, bc.ccl().x(), 0.00001);
+ WVPASSEQ_DOUBLE(1, bc.ccl().y(), 0.00001);
+ WVPASSEQ_DOUBLE(bc.h(), bc.ccr().h(), 0.00001);
+ WVPASSEQ_DOUBLE(M_PI / 2, bc.ccr().h(), 0.00001);
+ WVPASSEQ_DOUBLE(11, bc.ccr().x(), 0.00001);
+ WVPASSEQ_DOUBLE(1, bc.ccr().y(), 0.00001);
- // car radiuses (inner radius, outer front radius, outer rear radius)
- bc.h(1.2345);
- WVPASSEQ_DOUBLE(bc.iradi(), 9.5, 0.00001);
- WVPASSEQ_DOUBLE(bc.ofradi(), 10.793516572461451, 0.00001);
- WVPASSEQ_DOUBLE(bc.orradi(), 10.51189802081432, 0.00001);
- bc.h(M_PI / 2);
+ // car radiuses (inner radius, outer front radius, outer rear radius)
+ bc.h(1.2345);
+ WVPASSEQ_DOUBLE(bc.iradi(), 9.5, 0.00001);
+ WVPASSEQ_DOUBLE(bc.ofradi(), 10.793516572461451, 0.00001);
+ WVPASSEQ_DOUBLE(bc.orradi(), 10.51189802081432, 0.00001);
+ bc.h(M_PI / 2);
- // moving
- bc.sp(1);
- bc.st(0);
- bc.next();
- WVPASSEQ_DOUBLE(1, bc.x(), 0.00001);
- WVPASSEQ_DOUBLE(2, bc.y(), 0.00001);
+ // moving
+ bc.sp(1);
+ bc.st(0);
+ bc.next();
+ WVPASSEQ_DOUBLE(1, bc.x(), 0.00001);
+ WVPASSEQ_DOUBLE(2, bc.y(), 0.00001);
- bc.set_max_steer();//bc.st(M_PI);
- bc.next();
- WVPASSEQ_DOUBLE(0.2, bc.st(), 0.01);
- bc.st(bc.st() * -1);
- bc.next();
- WVPASSEQ_DOUBLE(-0.2, bc.st(), 0.01);
+ bc.set_max_steer();//bc.st(M_PI);
+ bc.next();
+ WVPASSEQ_DOUBLE(0.2, bc.st(), 0.01);
+ bc.st(bc.st() * -1);
+ bc.next();
+ WVPASSEQ_DOUBLE(-0.2, bc.st(), 0.01);
- // rotate
- bc.x(-1);
- bc.y(1);
- bc.h(0);
- bc.rotate(-1, 1, M_PI);
- WVPASSEQ_DOUBLE(-1, bc.x(), 0.00001);
- WVPASSEQ_DOUBLE(1, bc.y(), 0.00001);
- WVPASSEQ_DOUBLE(M_PI, bc.h(), 0.00001);
- bc.rotate(0, 1, -M_PI / 2);
- WVPASSEQ_DOUBLE(0, bc.x(), 0.00001);
- WVPASSEQ_DOUBLE(2, bc.y(), 0.00001);
- WVPASSEQ_DOUBLE(M_PI / 2, bc.h(), 0.00001);
+ // rotate
+ bc.x(-1);
+ bc.y(1);
+ bc.h(0);
+ bc.rotate(-1, 1, M_PI);
+ WVPASSEQ_DOUBLE(-1, bc.x(), 0.00001);
+ WVPASSEQ_DOUBLE(1, bc.y(), 0.00001);
+ WVPASSEQ_DOUBLE(M_PI, bc.h(), 0.00001);
+ bc.rotate(0, 1, -M_PI / 2);
+ WVPASSEQ_DOUBLE(0, bc.x(), 0.00001);
+ WVPASSEQ_DOUBLE(2, bc.y(), 0.00001);
+ WVPASSEQ_DOUBLE(M_PI / 2, bc.h(), 0.00001);
}
WVTEST_MAIN("test collide functions")
{
- std::vector<std::tuple<double, double>> p1;
- p1.push_back(std::make_tuple(1, 1));
- p1.push_back(std::make_tuple(1, 3));
- p1.push_back(std::make_tuple(3, 3));
- p1.push_back(std::make_tuple(3, 1));
- WVPASS(inside(2, 2, p1));
- WVPASS(!inside(4, 4, p1));
- auto tmpi1 = intersect(1, 1, 3, 3, 1, 3, 3, 1);
- WVPASS(std::get<0>(tmpi1));
- WVPASSEQ_DOUBLE(std::get<1>(tmpi1), 2, 0.00001);
- WVPASSEQ_DOUBLE(std::get<2>(tmpi1), 2, 0.00001);
- auto tmpi2 = intersect(1, 1, 1, 3, 3, 1, 3, 3);
- WVPASS(!std::get<0>(tmpi2));
- std::vector<std::tuple<double, double>> p2;
- p2.push_back(std::make_tuple(2.5, 1));
- p2.push_back(std::make_tuple(3.5, 3));
- p2.push_back(std::make_tuple(2, 4));
- p2.push_back(std::make_tuple(1, 2));
- auto col1 = collide(p1, p2);
- WVPASS(std::get<0>(col1));
- WVPASSEQ(std::get<1>(col1), 0); // first segment (indexing from 0)
- WVPASSEQ(std::get<2>(col1), 2); // the last segment
- std::vector<std::tuple<double, double>> p3;
- p3.push_back(std::make_tuple(2, 2));
- p3.push_back(std::make_tuple(2, 0));
- p3.push_back(std::make_tuple(4, 0));
- p3.push_back(std::make_tuple(4, 2));
- WVPASS(!std::get<0>(collide(p1, p3)));
- auto tmpi3 = intersect(1, 1, 3, 0, 0, 5, 5);
- WVPASS(std::get<0>(tmpi3));
- auto tmpi4 = intersect(1, 1, 3, 0, 0, -5, 5);
- WVPASS(std::get<0>(tmpi4));
- auto tmpi5 = intersect(1, 1, 3, 0, 0, -5, -5);
- WVPASS(std::get<0>(tmpi5));
- auto tmpi6 = intersect(1, 1, 3, 0, 0, 5, -5);
- WVPASS(std::get<0>(tmpi6));
- auto tmpi7 = intersect(1, 1, 1, -5, 5, 5, 5);
- WVPASS(!std::get<0>(tmpi7));
- auto tmpi8 = intersect(1, 1, 1, -5, -5, 5, -5);
- WVPASS(!std::get<0>(tmpi8));
- auto tmpi9 = intersect(1, 1, 1, -5, -5, -5, 5);
- WVPASS(!std::get<0>(tmpi9));
- auto tmpi10 = intersect(1, 1, 1, 5, -5, 5, 5);
- WVPASS(!std::get<0>(tmpi10));
+ std::vector<std::tuple<double, double>> p1;
+ p1.push_back(std::make_tuple(1, 1));
+ p1.push_back(std::make_tuple(1, 3));
+ p1.push_back(std::make_tuple(3, 3));
+ p1.push_back(std::make_tuple(3, 1));
+ WVPASS(inside(2, 2, p1));
+ WVPASS(!inside(4, 4, p1));
+ auto tmpi1 = intersect(1, 1, 3, 3, 1, 3, 3, 1);
+ WVPASS(std::get<0>(tmpi1));
+ WVPASSEQ_DOUBLE(std::get<1>(tmpi1), 2, 0.00001);
+ WVPASSEQ_DOUBLE(std::get<2>(tmpi1), 2, 0.00001);
+ auto tmpi2 = intersect(1, 1, 1, 3, 3, 1, 3, 3);
+ WVPASS(!std::get<0>(tmpi2));
+ std::vector<std::tuple<double, double>> p2;
+ p2.push_back(std::make_tuple(2.5, 1));
+ p2.push_back(std::make_tuple(3.5, 3));
+ p2.push_back(std::make_tuple(2, 4));
+ p2.push_back(std::make_tuple(1, 2));
+ auto col1 = collide(p1, p2);
+ WVPASS(std::get<0>(col1));
+ WVPASSEQ(std::get<1>(col1), 0); // first segment (indexing from 0)
+ WVPASSEQ(std::get<2>(col1), 2); // the last segment
+ std::vector<std::tuple<double, double>> p3;
+ p3.push_back(std::make_tuple(2, 2));
+ p3.push_back(std::make_tuple(2, 0));
+ p3.push_back(std::make_tuple(4, 0));
+ p3.push_back(std::make_tuple(4, 2));
+ WVPASS(!std::get<0>(collide(p1, p3)));
+ auto tmpi3 = intersect(1, 1, 3, 0, 0, 5, 5);
+ WVPASS(std::get<0>(tmpi3));
+ auto tmpi4 = intersect(1, 1, 3, 0, 0, -5, 5);
+ WVPASS(std::get<0>(tmpi4));
+ auto tmpi5 = intersect(1, 1, 3, 0, 0, -5, -5);
+ WVPASS(std::get<0>(tmpi5));
+ auto tmpi6 = intersect(1, 1, 3, 0, 0, 5, -5);
+ WVPASS(std::get<0>(tmpi6));
+ auto tmpi7 = intersect(1, 1, 1, -5, 5, 5, 5);
+ WVPASS(!std::get<0>(tmpi7));
+ auto tmpi8 = intersect(1, 1, 1, -5, -5, 5, -5);
+ WVPASS(!std::get<0>(tmpi8));
+ auto tmpi9 = intersect(1, 1, 1, -5, -5, -5, 5);
+ WVPASS(!std::get<0>(tmpi9));
+ auto tmpi10 = intersect(1, 1, 1, 5, -5, 5, 5);
+ WVPASS(!std::get<0>(tmpi10));
}
WVTEST_MAIN("auxiliary angle between three points")
{
- double a;
- a = angle_between_three_points(1, 0, 0, 0, 0, 1);
- WVPASSEQ_DOUBLE(a, M_PI/2, 0.00001);
- a = angle_between_three_points(0, 1, 0, 0, 1, 0);
- WVPASSEQ_DOUBLE(a, M_PI/2, 0.00001);
- a = angle_between_three_points(2, 2, 1, 1, 0, 0);
- WVPASSEQ_DOUBLE(a, 0, 0.00001);
- a = angle_between_three_points(-2, 2, -1, 1, -1, 2);
- WVPASSEQ_DOUBLE(a, M_PI/4, 0.00001);
- a = angle_between_three_points(-1, 2, -1, 1, -2, 2);
- WVPASSEQ_DOUBLE(a, M_PI/4, 0.00001);
+ double a;
+ a = angle_between_three_points(1, 0, 0, 0, 0, 1);
+ WVPASSEQ_DOUBLE(a, M_PI/2, 0.00001);
+ a = angle_between_three_points(0, 1, 0, 0, 1, 0);
+ WVPASSEQ_DOUBLE(a, M_PI/2, 0.00001);
+ a = angle_between_three_points(2, 2, 1, 1, 0, 0);
+ WVPASSEQ_DOUBLE(a, 0, 0.00001);
+ a = angle_between_three_points(-2, 2, -1, 1, -1, 2);
+ WVPASSEQ_DOUBLE(a, M_PI/4, 0.00001);
+ a = angle_between_three_points(-1, 2, -1, 1, -2, 2);
+ WVPASSEQ_DOUBLE(a, M_PI/4, 0.00001);
- bool r;
- r = right_side_of_line(-1, -1, 1, 1, 2, 1);
- WVPASS(r);
- r = right_side_of_line(-1, -1, 1, 1, 1, 2);
- WVPASS(!r);
- r = right_side_of_line(-1, 1, 1, -1, 2, 1);
- WVPASS(!r);
- r = right_side_of_line(-1, 1, 1, -1, 1, 2);
- WVPASS(!r);
- r = right_side_of_line(-1, 1, 1, -1, 2, -1);
- WVPASS(!r);
- r = right_side_of_line(-1, 1, 1, -1, -1, 2);
- WVPASS(!r);
- r = right_side_of_line(-1, 1, 1, -1, -2, 1);
- WVPASS(r);
+ bool r;
+ r = right_side_of_line(-1, -1, 1, 1, 2, 1);
+ WVPASS(r);
+ r = right_side_of_line(-1, -1, 1, 1, 1, 2);
+ WVPASS(!r);
+ r = right_side_of_line(-1, 1, 1, -1, 2, 1);
+ WVPASS(!r);
+ r = right_side_of_line(-1, 1, 1, -1, 1, 2);
+ WVPASS(!r);
+ r = right_side_of_line(-1, 1, 1, -1, 2, -1);
+ WVPASS(!r);
+ r = right_side_of_line(-1, 1, 1, -1, -1, 2);
+ WVPASS(!r);
+ r = right_side_of_line(-1, 1, 1, -1, -2, 1);
+ WVPASS(r);
}
WVTEST_MAIN("drivable")
{
- double tmp_double_1 = 0;
- double tmp_double_2 = 0;
- BicycleCar g;
- // TODO set g.x, g.y to different values
- // TODO set g.h to cover all 4 quadrants
- BicycleCar n;
- n.x(g.x());
- n.y(g.y());
- n.h(g.h());
- WVPASS(g.drivable(n)); // pass the same pose
+ double tmp_double_1 = 0;
+ double tmp_double_2 = 0;
+ BicycleCar g;
+ // TODO set g.x, g.y to different values
+ // TODO set g.h to cover all 4 quadrants
+ BicycleCar n;
+ n.x(g.x());
+ n.y(g.y());
+ n.h(g.h());
+ WVPASS(g.drivable(n)); // pass the same pose
- n = BicycleCar(g);
- n.rotate(g.ccr().x(), g.ccr().y(), -M_PI/2);
- WVPASSEQ_DOUBLE(n.h(), g.h() - M_PI/2, 0.00001);
- tmp_double_1 = sqrt(pow(n.x() - g.x(), 2) + pow(n.y() - g.y(), 2));
- tmp_double_2 = std::abs(g.mtr() * 2 * sin(-M_PI/2 / 2));
- WVPASSEQ_DOUBLE(tmp_double_1, tmp_double_2, 0.00001);
- WVPASS(g.drivable(n)); // pass right corner case
+ n = BicycleCar(g);
+ n.rotate(g.ccr().x(), g.ccr().y(), -M_PI/2);
+ WVPASSEQ_DOUBLE(n.h(), g.h() - M_PI/2, 0.00001);
+ tmp_double_1 = sqrt(pow(n.x() - g.x(), 2) + pow(n.y() - g.y(), 2));
+ tmp_double_2 = std::abs(g.mtr() * 2 * sin(-M_PI/2 / 2));
+ WVPASSEQ_DOUBLE(tmp_double_1, tmp_double_2, 0.00001);
+ WVPASS(g.drivable(n)); // pass right corner case
- n = BicycleCar(g);
- n.rotate(g.ccl().x(), g.ccl().y(), M_PI/2);
- WVPASSEQ_DOUBLE(n.h(), g.h() + M_PI/2, 0.00001);
- tmp_double_1 = sqrt(pow(n.x() - g.x(), 2) + pow(n.y() - g.y(), 2));
- tmp_double_2 = std::abs(g.mtr() * 2 * sin(M_PI/2 / 2));
- WVPASSEQ_DOUBLE(tmp_double_1, tmp_double_2, 0.00001);
- WVPASS(g.drivable(n)); // pass left corner case
- n.rotate(g.ccl().x(), g.ccl().y(), 0.01);
- WVPASS(!g.drivable(n)); // fail left corner case
+ n = BicycleCar(g);
+ n.rotate(g.ccl().x(), g.ccl().y(), M_PI/2);
+ WVPASSEQ_DOUBLE(n.h(), g.h() + M_PI/2, 0.00001);
+ tmp_double_1 = sqrt(pow(n.x() - g.x(), 2) + pow(n.y() - g.y(), 2));
+ tmp_double_2 = std::abs(g.mtr() * 2 * sin(M_PI/2 / 2));
+ WVPASSEQ_DOUBLE(tmp_double_1, tmp_double_2, 0.00001);
+ WVPASS(g.drivable(n)); // pass left corner case
+ n.rotate(g.ccl().x(), g.ccl().y(), 0.01);
+ WVPASS(!g.drivable(n)); // fail left corner case
- n = BicycleCar(g);
- n.sp(std::abs(g.mtr() * 2 * sin(M_PI/2 / 2)));
- n.st(0);
- n.next();
- WVPASS(g.drivable(n)); // pass forward corner case
+ n = BicycleCar(g);
+ n.sp(std::abs(g.mtr() * 2 * sin(M_PI/2 / 2)));
+ n.st(0);
+ n.next();
+ WVPASS(g.drivable(n)); // pass forward corner case
- for (double a = 0; a > -M_PI/2; a -= 0.01) {
- n = BicycleCar(g);
- n.rotate(g.ccr().x(), g.ccr().y(), a);
- WVPASS(g.drivable(n)); // pass drivable border
- }
- for (double a = 0; a > -M_PI/2 + 0.1; a -= 0.01) {
- // + 0.1 -- compensate for Euclid. dist. check
- n = BicycleCar(g);
- n.x(n.x() + 0.1*cos(n.h()));
- n.y(n.y() + 0.1*sin(n.h()));
- n.rotate(n.ccr().x(), n.ccr().y(), a);
- WVPASS(g.drivable(n)); // pass near drivable border
- }
- for (double a = -0.1; a > -M_PI/2; a -= 0.01) {
- // = -0.1 -- compensate for near goal
- n = BicycleCar(g);
- n.x(n.x() - 0.1*cos(n.h()));
- n.y(n.y() - 0.1*sin(n.h()));
- n.rotate(n.ccr().x(), n.ccr().y(), a);
- WVPASS(!g.drivable(n)); // fail near drivable border
- }
- for (double a = 0; a < M_PI / 2; a += 0.01) {
- n = BicycleCar(g);
- n.rotate(g.ccl().x(), g.ccl().y(), a);
- WVPASS(g.drivable(n)); // pass drivable border
- }
- for (double a = 0; a < M_PI / 2 - 0.1; a += 0.01) {
- // - 0.1 -- compensate for Euclid. dist. check
- n = BicycleCar(g);
- n.x(n.x() + 0.1*cos(n.h()));
- n.y(n.y() + 0.1*sin(n.h()));
- n.rotate(n.ccl().x(), n.ccl().y(), a);
- WVPASS(g.drivable(n)); // pass near drivable border
- }
- for (double a = 0.1; a < M_PI / 2; a += 0.01) {
- // = 0.1 -- compensate for near goal
- n = BicycleCar(g);
- n.x(n.x() - 0.1*cos(n.h()));
- n.y(n.y() - 0.1*sin(n.h()));
- n.rotate(n.ccl().x(), n.ccl().y(), a);
- WVPASS(!g.drivable(n)); // fail near drivable border
- }
+ for (double a = 0; a > -M_PI/2; a -= 0.01) {
+ n = BicycleCar(g);
+ n.rotate(g.ccr().x(), g.ccr().y(), a);
+ WVPASS(g.drivable(n)); // pass drivable border
+ }
+ for (double a = 0; a > -M_PI/2 + 0.1; a -= 0.01) {
+ // + 0.1 -- compensate for Euclid. dist. check
+ n = BicycleCar(g);
+ n.x(n.x() + 0.1*cos(n.h()));
+ n.y(n.y() + 0.1*sin(n.h()));
+ n.rotate(n.ccr().x(), n.ccr().y(), a);
+ WVPASS(g.drivable(n)); // pass near drivable border
+ }
+ for (double a = -0.1; a > -M_PI/2; a -= 0.01) {
+ // = -0.1 -- compensate for near goal
+ n = BicycleCar(g);
+ n.x(n.x() - 0.1*cos(n.h()));
+ n.y(n.y() - 0.1*sin(n.h()));
+ n.rotate(n.ccr().x(), n.ccr().y(), a);
+ WVPASS(!g.drivable(n)); // fail near drivable border
+ }
+ for (double a = 0; a < M_PI / 2; a += 0.01) {
+ n = BicycleCar(g);
+ n.rotate(g.ccl().x(), g.ccl().y(), a);
+ WVPASS(g.drivable(n)); // pass drivable border
+ }
+ for (double a = 0; a < M_PI / 2 - 0.1; a += 0.01) {
+ // - 0.1 -- compensate for Euclid. dist. check
+ n = BicycleCar(g);
+ n.x(n.x() + 0.1*cos(n.h()));
+ n.y(n.y() + 0.1*sin(n.h()));
+ n.rotate(n.ccl().x(), n.ccl().y(), a);
+ WVPASS(g.drivable(n)); // pass near drivable border
+ }
+ for (double a = 0.1; a < M_PI / 2; a += 0.01) {
+ // = 0.1 -- compensate for near goal
+ n = BicycleCar(g);
+ n.x(n.x() - 0.1*cos(n.h()));
+ n.y(n.y() - 0.1*sin(n.h()));
+ n.rotate(n.ccl().x(), n.ccl().y(), a);
+ WVPASS(!g.drivable(n)); // fail near drivable border
+ }
- n = BicycleCar(g);
- n.sp(std::abs(g.mtr() * 2 * sin(M_PI/2 / 2)));
- n.sp(n.sp() * -1);
- n.st(0);
- n.next();
- WVPASS(g.drivable(n)); // pass backward corner case
+ n = BicycleCar(g);
+ n.sp(std::abs(g.mtr() * 2 * sin(M_PI/2 / 2)));
+ n.sp(n.sp() * -1);
+ n.st(0);
+ n.next();
+ WVPASS(g.drivable(n)); // pass backward corner case
- n = BicycleCar(g);
- n.rotate(g.ccr().x(), g.ccr().y(), M_PI/2);
- WVPASSEQ_DOUBLE(n.h(), g.h() + M_PI/2, 0.00001);
- tmp_double_1 = sqrt(pow(n.x() - g.x(), 2) + pow(n.y() - g.y(), 2));
- tmp_double_2 = std::abs(g.mtr() * 2 * sin(-M_PI/2 / 2));
- WVPASSEQ_DOUBLE(tmp_double_1, tmp_double_2, 0.00001);
- WVPASS(g.drivable(n)); // pass right corner case
+ n = BicycleCar(g);
+ n.rotate(g.ccr().x(), g.ccr().y(), M_PI/2);
+ WVPASSEQ_DOUBLE(n.h(), g.h() + M_PI/2, 0.00001);
+ tmp_double_1 = sqrt(pow(n.x() - g.x(), 2) + pow(n.y() - g.y(), 2));
+ tmp_double_2 = std::abs(g.mtr() * 2 * sin(-M_PI/2 / 2));
+ WVPASSEQ_DOUBLE(tmp_double_1, tmp_double_2, 0.00001);
+ WVPASS(g.drivable(n)); // pass right corner case
- n = BicycleCar(g);
- n.rotate(g.ccl().x(), g.ccl().y(), -M_PI/2);
- WVPASSEQ_DOUBLE(n.h(), g.h() - M_PI/2, 0.00001);
- tmp_double_1 = sqrt(pow(n.x() - g.x(), 2) + pow(n.y() - g.y(), 2));
- tmp_double_2 = std::abs(g.mtr() * 2 * sin(M_PI/2 / 2));
- WVPASSEQ_DOUBLE(tmp_double_1, tmp_double_2, 0.00001);
- WVPASS(g.drivable(n)); // pass left corner case
+ n = BicycleCar(g);
+ n.rotate(g.ccl().x(), g.ccl().y(), -M_PI/2);
+ WVPASSEQ_DOUBLE(n.h(), g.h() - M_PI/2, 0.00001);
+ tmp_double_1 = sqrt(pow(n.x() - g.x(), 2) + pow(n.y() - g.y(), 2));
+ tmp_double_2 = std::abs(g.mtr() * 2 * sin(M_PI/2 / 2));
+ WVPASSEQ_DOUBLE(tmp_double_1, tmp_double_2, 0.00001);
+ WVPASS(g.drivable(n)); // pass left corner case
- for (double a = 0; a < M_PI / 2; a += 0.01) {
- n = BicycleCar(g);
- n.rotate(g.ccr().x(), g.ccr().y(), a);
- WVPASS(g.drivable(n)); // pass drivable border
- }
- for (double a = 0; a < M_PI / 2 - 0.1; a += 0.01) {
- // - 0.1 -- compensate for Euclid. dist. check
- n = BicycleCar(g);
- n.x(n.x() - 0.1*cos(n.h()));
- n.y(n.y() - 0.1*sin(n.h()));
- n.rotate(n.ccr().x(), n.ccr().y(), a);
- WVPASS(g.drivable(n)); // pass near drivable border
- }
- for (double a = 0.1; a < M_PI / 2; a += 0.01) {
- // = 0.1 -- compensate for near goal
- n = BicycleCar(g);
- n.x(n.x() + 0.1*cos(n.h()));
- n.y(n.y() + 0.1*sin(n.h()));
- n.rotate(n.ccr().x(), n.ccr().y(), a);
- WVPASS(!g.drivable(n)); // fail near drivable border
- }
- for (double a = 0; a > -M_PI/2; a -= 0.01) {
- n = BicycleCar(g);
- n.rotate(g.ccl().x(), g.ccl().y(), a);
- WVPASS(g.drivable(n)); // pass drivable border
- }
- for (double a = 0; a > -M_PI/2 + 0.1; a -= 0.01) {
- // + 0.1 -- compensate for Euclid. dist. check
- n = BicycleCar(g);
- n.x(n.x() - 0.1*cos(n.h()));
- n.y(n.y() - 0.1*sin(n.h()));
- n.rotate(n.ccl().x(), n.ccl().y(), a);
- WVPASS(g.drivable(n)); // pass near drivable border
- }
- for (double a = -0.1; a > -M_PI/2; a -= 0.01) {
- // = -0.1 -- compensate for near goal
- n = BicycleCar(g);
- n.x(n.x() + 0.1*cos(n.h()));
- n.y(n.y() + 0.1*sin(n.h()));
- n.rotate(n.ccl().x(), n.ccl().y(), a);
- WVPASS(!g.drivable(n)); // fail near drivable border
- }
+ for (double a = 0; a < M_PI / 2; a += 0.01) {
+ n = BicycleCar(g);
+ n.rotate(g.ccr().x(), g.ccr().y(), a);
+ WVPASS(g.drivable(n)); // pass drivable border
+ }
+ for (double a = 0; a < M_PI / 2 - 0.1; a += 0.01) {
+ // - 0.1 -- compensate for Euclid. dist. check
+ n = BicycleCar(g);
+ n.x(n.x() - 0.1*cos(n.h()));
+ n.y(n.y() - 0.1*sin(n.h()));
+ n.rotate(n.ccr().x(), n.ccr().y(), a);
+ WVPASS(g.drivable(n)); // pass near drivable border
+ }
+ for (double a = 0.1; a < M_PI / 2; a += 0.01) {
+ // = 0.1 -- compensate for near goal
+ n = BicycleCar(g);
+ n.x(n.x() + 0.1*cos(n.h()));
+ n.y(n.y() + 0.1*sin(n.h()));
+ n.rotate(n.ccr().x(), n.ccr().y(), a);
+ WVPASS(!g.drivable(n)); // fail near drivable border
+ }
+ for (double a = 0; a > -M_PI/2; a -= 0.01) {
+ n = BicycleCar(g);
+ n.rotate(g.ccl().x(), g.ccl().y(), a);
+ WVPASS(g.drivable(n)); // pass drivable border
+ }
+ for (double a = 0; a > -M_PI/2 + 0.1; a -= 0.01) {
+ // + 0.1 -- compensate for Euclid. dist. check
+ n = BicycleCar(g);
+ n.x(n.x() - 0.1*cos(n.h()));
+ n.y(n.y() - 0.1*sin(n.h()));
+ n.rotate(n.ccl().x(), n.ccl().y(), a);
+ WVPASS(g.drivable(n)); // pass near drivable border
+ }
+ for (double a = -0.1; a > -M_PI/2; a -= 0.01) {
+ // = -0.1 -- compensate for near goal
+ n = BicycleCar(g);
+ n.x(n.x() + 0.1*cos(n.h()));
+ n.y(n.y() + 0.1*sin(n.h()));
+ n.rotate(n.ccl().x(), n.ccl().y(), a);
+ WVPASS(!g.drivable(n)); // fail near drivable border
+ }
}
WVTEST_MAIN("pslot basic geometry")
{
- ParkingSlot ps = ParkingSlot();
- ps.set_slot(0.5, 1.5, M_PI/4, sqrt(2), sqrt(8));
+ ParkingSlot ps = ParkingSlot();
+ ps.set_slot(0.5, 1.5, M_PI/4, sqrt(2), sqrt(8));
- // slot info
- WVPASSEQ_DOUBLE(ps.x1(), 1, 0.00001);
- WVPASSEQ_DOUBLE(ps.y1(), 1, 0.00001);
- WVPASSEQ_DOUBLE(ps.x2(), 3, 0.00001);
- WVPASSEQ_DOUBLE(ps.y2(), 3, 0.00001);
- WVPASSEQ_DOUBLE(ps.x3(), 2, 0.00001);
- WVPASSEQ_DOUBLE(ps.y3(), 4, 0.00001);
- WVPASSEQ_DOUBLE(ps.x4(), 0, 0.00001);
- WVPASSEQ_DOUBLE(ps.y4(), 2, 0.00001);
- WVPASS(ps.right());
- WVPASS(!ps.parallel());
- WVPASSEQ_DOUBLE(ps.heading(), M_PI * 3 / 4, 0.00001);
- ps.reverse_border();
- WVPASSEQ_DOUBLE(ps.x1(), 0, 0.00001);
- WVPASSEQ_DOUBLE(ps.y1(), 2, 0.00001);
- WVPASSEQ_DOUBLE(ps.x2(), 2, 0.00001);
- WVPASSEQ_DOUBLE(ps.y2(), 4, 0.00001);
- WVPASSEQ_DOUBLE(ps.x3(), 3, 0.00001);
- WVPASSEQ_DOUBLE(ps.y3(), 3, 0.00001);
- WVPASSEQ_DOUBLE(ps.x4(), 1, 0.00001);
- WVPASSEQ_DOUBLE(ps.y4(), 1, 0.00001);
- WVPASS(!ps.right());
- WVPASS(!ps.parallel());
- WVPASSEQ_DOUBLE(ps.heading(), -M_PI / 4, 0.00001);
+ // slot info
+ WVPASSEQ_DOUBLE(ps.x1(), 1, 0.00001);
+ WVPASSEQ_DOUBLE(ps.y1(), 1, 0.00001);
+ WVPASSEQ_DOUBLE(ps.x2(), 3, 0.00001);
+ WVPASSEQ_DOUBLE(ps.y2(), 3, 0.00001);
+ WVPASSEQ_DOUBLE(ps.x3(), 2, 0.00001);
+ WVPASSEQ_DOUBLE(ps.y3(), 4, 0.00001);
+ WVPASSEQ_DOUBLE(ps.x4(), 0, 0.00001);
+ WVPASSEQ_DOUBLE(ps.y4(), 2, 0.00001);
+ WVPASS(ps.right());
+ WVPASS(!ps.parallel());
+ WVPASSEQ_DOUBLE(ps.heading(), M_PI * 3 / 4, 0.00001);
+ ps.reverse_border();
+ WVPASSEQ_DOUBLE(ps.x1(), 0, 0.00001);
+ WVPASSEQ_DOUBLE(ps.y1(), 2, 0.00001);
+ WVPASSEQ_DOUBLE(ps.x2(), 2, 0.00001);
+ WVPASSEQ_DOUBLE(ps.y2(), 4, 0.00001);
+ WVPASSEQ_DOUBLE(ps.x3(), 3, 0.00001);
+ WVPASSEQ_DOUBLE(ps.y3(), 3, 0.00001);
+ WVPASSEQ_DOUBLE(ps.x4(), 1, 0.00001);
+ WVPASSEQ_DOUBLE(ps.y4(), 1, 0.00001);
+ WVPASS(!ps.right());
+ WVPASS(!ps.parallel());
+ WVPASSEQ_DOUBLE(ps.heading(), -M_PI / 4, 0.00001);
}
projects / hubacji1 / bcar.git / commitdiff