target_include_directories(pslot PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/incl)
target_link_libraries(pslot bcar)
+add_executable(compute_pslot_table src/compute_pslot_table.cc)
+target_link_libraries(compute_pslot_table pslot)
+
if (SKIP_UT)
return()
endif()
*/
bool on_right_side_of(Line const& li) const;
+ /*! \brief Translate self.
+ *
+ * \param p `Point` offset to translate by.
+ */
+ void translate(Point const& p);
+
/*! \brief Rotate self around the point.
\param c Rotation center `Point`.
*/
void rotate(Point const& c, double const angl);
+ /*! \brief Compute reflection of `this` around the `Line`.
+ *
+ * \param li The plane to reflect around is given by `li`.
+ */
+ void reflect(Line const& li);
+
/*! Return Euclidean distance to `p`. */
double edist(Point const& p) const;
+ bool operator==(Point const& p);
friend std::ostream& operator<<(std::ostream& out, Point const& p);
};
double len() const;
+ double h() const;
+
friend std::ostream& operator<<(std::ostream& out, Line const& li);
};
void rotate(Point const& c, double const angl);
+ void reflect(Line const& li);
+
+ bool operator==(Pose const& p);
friend std::ostream& operator<<(std::ostream& out, Pose const& p);
};
class PoseRange : public virtual Pose {
private:
- double e_ = 0.0;
- using Pose::h;
+ Pose bp_;
+ Pose ep_;
+ void set_xyh();
public:
+ PoseRange(Pose bp, Pose ep);
+ PoseRange(double x, double y, double b, double e);
+
+ Pose bp() const;
+ Pose ep() const;
+
/*! Get heading's begin in the interval [-pi, +pi] radians. */
double b() const;
- /*! Set heading's begin in radians. It's recomputed to [-pi, +pi]. */
- void b(double b);
-
/*! Get heading's end in the interval [-pi, +pi] radians. */
double e() const;
- /*! Set heading's end in radians. It's recomputed to [-pi, +pi]. */
- void e(double e);
-
+ void translate(Point const& p);
void rotate(Point const& c, double const angl);
+ void reflect(Line const& li);
friend std::ostream& operator<<(std::ostream& out, PoseRange const& p);
};
public virtual CarMove {
private:
public:
- /*! \brief Return `false` if `bc` is not achievable.
+ /*! \brief Return `true` if `this` can drive to `p` trivially.
*
- * When `false` is returned the `bc` may still be drivable, but not
- * trivially, i.e. by "line segment - circle arc - line segment".
+ * Trivially means that `this` can drive to `p` by line segment - circle
+ * arc - line segment.
*
* \param p `PoseRange` (resp. `Pose`) to achieve.
*/
class ParkingSlot {
private:
double offset_ = 0.001; // to avoid collision during init
+ double parking_speed_ = -0.1;
+ unsigned int max_cusp_ = 10;
+ double delta_angle_to_slot_ = 0.001;
Point border_[4];
Line entry_;
Line rear_;
/*! Return parking slot's orientation. */
double h() const;
+ /*! Get parking slot's left front point. */
+ Point lf() const;
+
+ /*! Get parking slot's left rear point. */
+ Point lr() const;
+
+ /*! Get parking slot's right rear point. */
+ Point rr() const;
+
+ /*! Get parking slot's right front point. */
+ Point rf() const;
+
+ /*! Get parking slot's entry side. */
+ Line entry() const;
+
+ /*! Get parking slot's rear side. */
+ Line rear() const;
+
+ /*! Get parking slot's curb side. */
+ Line curb() const;
+
+ /*! Get parking slot's front side. */
+ Line front() const;
+
+ /*! Car's next iteration distance. (Negative for backward.) */
+ void set_parking_speed(double s);
+
+ /*! Maximum allowed number of cusp inside the parking slot. */
+ void set_max_cusp(unsigned int m);
+
+ /*! Angle's increment when creating start positions. */
+ void set_delta_angle_to_slot(double d);
+
/*! Return `true` for the parallel parking slot. */
bool parallel() 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);
+ std::vector<BicycleCar> drive_in_slot(BicycleCar c);
- /*! Find entry. */
- PoseRange fe();
+ /*! \brief Steer car `c` into the parking slot `this`.
+ *
+ * `steer_in_slot` returns the complete path as the list of `Pose`s, not
+ * just cusp `BicycleCar`s as `drive_to_slot`.
+ *
+ * \param c Starting bicycle car.
+ */
+ std::vector<Pose> steer_in_slot(BicycleCar c);
+
+ /*! \brief Find entry.
+ *
+ * \param c For which `BicycleCar` should entry be found?
+ */
+ PoseRange fe(BicycleCar c);
+
+ /*! \brief Recompute zero slot's `PoseRange` entry for `this`.
+ *
+ * The _zero_ slot is the `ParkingSlot(Point(0.0, 0.0), 0.0, W, L);`.
+ *
+ * \param p Computed `PoseRange` entry.
+ */
+ PoseRange recompute_entry(PoseRange p);
friend std::ostream& operator<<(std::ostream& o, ParkingSlot const& s);
};
}
}
+void
+Point::translate(Point const& p)
+{
+ this->x_ += p.x();
+ this->y_ += p.y();
+}
+
void
Point::rotate(Point const& c, double const angl)
{
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));
}
+bool
+Point::operator==(Point const& p)
+{
+ return this->x() == p.x() && this->y() == p.y();
+}
+
std::ostream&
operator<<(std::ostream& out, Point const& p)
{
return this->b_.edist(this->e_);
}
+double
+Line::h() const
+{
+ return atan2(this->e_.y() - this->b_.y(), this->e_.x() - this->b_.x());
+}
+
std::ostream&
operator<<(std::ostream& out, Line const& li)
{
this->h(this->h() + angl);
}
+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&
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());
+ this->h((this->b() + this->e()) / 2.0);
+}
+
+PoseRange::PoseRange(Pose bp, Pose ep) : bp_(bp), ep_(ep)
+{
+ if (this->bp_ == this->ep_) {
+ this->set_pose(this->ep_);
+ } else {
+ this->set_xyh();
+ }
+}
+
+PoseRange::PoseRange(double x, double y, double b, double e)
+ : PoseRange(Pose(x, y, b), Pose(x, y, e))
{
- return this->h();
}
-void
-PoseRange::b(double b)
+Pose
+PoseRange::bp() const
+{
+ return this->bp_;
+}
+
+Pose
+PoseRange::ep() const
{
- this->h(b);
+ 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&
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)
} 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());
} 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());
} 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());
} 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());
--- /dev/null
+#include <iostream>
+#include <vector>
+#include "pslot.hh"
+
+#define CAR "fiat_punto"
+#define CAR_CURB_TO_CURB 10.820
+#define CAR_WIDTH 1.625
+#define CAR_WHEELBASE 2.450
+#define CAR_DISTANCE_FROM_REAR_AXLE_TO_FRONT 3.105
+#define CAR_LENGTH 3.760
+
+#define SLOT_MAX_LENGTH 6.5
+#define SLOT_MAX_WIDTH 2.2
+#define SLOT_STEP_LENGTH 0.01
+#define SLOT_STEP_WIDTH 0.01
+
+#define PARKING_SPEED -0.001
+#define MAX_CUSP 10
+#define DELTA_ANGLE_TO_SLOT 0.0001
+
+struct entry {
+ double len = 0.0;
+ double w = 0.0;
+ bcar::PoseRange pr;
+};
+
+int main()
+{
+ std::cout << std::fixed;
+ std::cerr << std::fixed;
+
+ bcar::BicycleCar c;
+ c.ctc(CAR_CURB_TO_CURB);
+ c.w(CAR_WIDTH);
+ c.wb(CAR_WHEELBASE);
+ c.df(CAR_DISTANCE_FROM_REAR_AXLE_TO_FRONT);
+ c.len(CAR_LENGTH);
+
+ bcar::Point zp(0.0, 0.0);
+ double zh = 0.0;
+ double len = SLOT_MAX_LENGTH;
+ double w = SLOT_MAX_WIDTH;
+
+ std::vector<struct entry> entries;
+ while (len > c.len()) {
+ if (len > c.perfect_parking_slot_len() + SLOT_STEP_LENGTH) {
+ len -= SLOT_STEP_LENGTH;
+ continue;
+ }
+ w = SLOT_MAX_WIDTH;
+ while (w > c.w()) {
+ bcar::ParkingSlot s(zp, zh, w, len);
+ s.set_parking_speed(PARKING_SPEED);
+ s.set_max_cusp(MAX_CUSP);
+ s.set_delta_angle_to_slot(DELTA_ANGLE_TO_SLOT);
+ auto pr = s.fe(c);
+ if (!(pr.x() == 0.0 && pr.y() == 0.0 && pr.b() == 0.0
+ && pr.e() == 0.0)) {
+ struct entry e{len, w, pr};
+ entries.push_back(e);
+ }
+ w -= SLOT_STEP_WIDTH;
+ }
+ std::cerr << "Slot length " << len << " done." << std::endl;
+ len -= SLOT_STEP_LENGTH;
+ }
+
+ using namespace std;
+ cout << "/* Generated by `./compute_pslot_table` */" << endl;
+ cout << "#ifndef PSLOT_" << CAR << "_TABLE" << endl;
+ cout << "#define PSLOT_" << CAR << "_TABLE" << endl;
+ cout << "#include \"bcar.hh\"" << endl;
+ cout << "bcar::PoseRange " << "get_" << CAR << "_entry(double len,";
+ cout << "double w)" << endl;
+ cout << "{" << endl;
+ for (auto e: entries) {
+ cout << "\tif (len >= " << e.len;
+ cout << " && w >= " << e.w << ") {" << endl;
+ cout << "\t\treturn bcar::PoseRange(";
+ cout << e.pr.x() << "," << e.pr.y() << ",";
+ cout << e.pr.b() << "," << e.pr.e() << ");" << endl;
+ cout << "\t}" << endl;
+ }
+ cout << "\treturn bcar::PoseRange(0.0,0.0,0.0,0.0);" << endl;
+ cout << "}" << endl;
+ cout << "#endif /* PSLOT_" << CAR << "_TABLE */" << endl;
+ return 0;
+}
return atan2(this->lfy() - this->lry(), this->lfx() - this->lrx());
}
+Point
+ParkingSlot::lf() const
+{
+ return Point(this->lfx(), this->lfy());
+}
+
+Point
+ParkingSlot::lr() const
+{
+ return Point(this->lrx(), this->lry());
+}
+
+Point
+ParkingSlot::rr() const
+{
+ return Point(this->rrx(), this->rry());
+}
+
+Point
+ParkingSlot::rf() const
+{
+ return Point(this->rfx(), this->rfy());
+}
+
+Line
+ParkingSlot::entry() const
+{
+ return this->entry_;
+}
+
+Line
+ParkingSlot::rear() const
+{
+ return this->rear_;
+}
+
+Line
+ParkingSlot::curb() const
+{
+ return this->curb_;
+}
+
+Line
+ParkingSlot::front() const
+{
+ return this->front_;
+}
+
+void
+ParkingSlot::set_parking_speed(double s)
+{
+ this->parking_speed_ = s;
+}
+
+void
+ParkingSlot::set_max_cusp(unsigned int m)
+{
+ this->max_cusp_ = m;
+}
+
+void
+ParkingSlot::set_delta_angle_to_slot(double d)
+{
+ this->delta_angle_to_slot_ = d;
+}
+
bool
ParkingSlot::parallel() const
{
{
this->border_[1].rotate(this->border_[0], M_PI);
this->border_[2].rotate(this->border_[3], M_PI);
+ this->entry_ = Line(this->border_[0], this->border_[3]);
+ this->rear_ = Line(this->border_[0], this->border_[1]);
+ this->curb_ = Line(this->border_[1], this->border_[2]);
+ this->front_ = Line(this->border_[2], this->border_[3]);
}
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_);
+ || 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)
+ParkingSlot::drive_in_slot(BicycleCar c)
{
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.reserve(this->max_cusp_ + 2);
path.push_back(c);
unsigned int cusp = 0;
- while (cusp < max) {
- if (c.h() < this->h()) {
- return std::vector<BicycleCar>();
- }
+ while (cusp < this->max_cusp_ + 1) {
if (this->parked(c)) {
- if (cusp < max) {
- max = cusp;
+ if (cusp < this->max_cusp_) {
+ this->max_cusp_ = cusp;
}
- break;
+ path.push_back(c);
+ return path;
+ }
+ if (c.h() < this->h()) {
+ return std::vector<BicycleCar>();
}
c.next();
if (this->collide(c)) {
cusp += 1;
}
}
- path.push_back(c);
+ return std::vector<BicycleCar>();
+}
+
+std::vector<Pose>
+ParkingSlot::steer_in_slot(BicycleCar c)
+{
+ std::vector<Pose> path;
+ while (!this->parked(c)) {
+ path.push_back(c);
+ c.next();
+ if (this->collide(c)) {
+ c.sp(c.sp() * -1.0);
+ c.next();
+ c.st(c.st() * -1.0);
+ }
+ }
return path;
}
PoseRange
-ParkingSlot::fe()
+ParkingSlot::fe(BicycleCar c)
{
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;
+ double clen = -this->offset_ + this->len() - c.df();
+ double cw = 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());
+ c.sp(this->parking_speed_);
+ auto const rc = c.rf();
+ this->curb_.intersects_with(rc, 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);
+ max_to_slot = rr.min_angle_between(rc, i1);
} else {
- max_to_slot = rr.min_angle_between(b3, i2);
+ max_to_slot = rr.min_angle_between(rc, 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);
+ a_to_slot += this->delta_angle_to_slot_;
+ c.rotate(rc, this->delta_angle_to_slot_);
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);
+ auto r = this->drive_in_slot(s);
if (r.size() > 0) {
entries.push_back(r);
}
}
- assert(entries.size() > 0);
+ if (entries.size() == 0) {
+ return PoseRange(Pose(0.0, 0.0, 0.0), Pose(0.0, 0.0, 0.0));
+ }
+ if (entries.size() == 1) {
+ auto f = entries.front().front();
+ return PoseRange(f, f);
+ }
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;
+ return PoseRange(c1, c2);
+}
+
+PoseRange
+ParkingSlot::recompute_entry(PoseRange p)
+{
+ p.rotate(Point(0.0, 0.0), this->h());
+ p.translate(this->border_[0]);
+ if (!this->right()) {
+ p.reflect(this->entry_);
+ }
+ return p;
}
std::ostream&
projects / hubacji1 / bcar.git / commitdiff