namespace bcar {
ParkingSlot::ParkingSlot(Point p, double h, double W, double L) :
namespace bcar {
ParkingSlot::ParkingSlot(Point p, double h, double W, double L) :
Point(p.x() + W * cos(h - M_PI / 2.0),
p.y() + W * sin(h - M_PI / 2.0)),
Point(p.x() + W * cos(h - M_PI / 2.0) + L * cos(h),
p.y() + W * sin(h - M_PI / 2.0) + L * sin(h)),
Point(p.x() + L * cos(h), p.y() + L * sin(h))},
Point(p.x() + W * cos(h - M_PI / 2.0),
p.y() + W * sin(h - M_PI / 2.0)),
Point(p.x() + W * cos(h - M_PI / 2.0) + L * cos(h),
p.y() + W * sin(h - M_PI / 2.0) + L * sin(h)),
Point(p.x() + L * cos(h), p.y() + L * sin(h))},
- entry_(border_[0], border_[3]),
- rear_(border_[0], border_[1]),
- curb_(border_[1], border_[2]),
- front_(border_[2], border_[3])
+ _entry(_border[0], _border[3]),
+ _rear(_border[0], _border[1]),
+ _curb(_border[1], _border[2]),
+ _front(_border[2], _border[3])
{
}
ParkingSlot::ParkingSlot(double lrx, double lry, double rrx, double rry,
double rfx, double rfy, double lfx, double lfy) :
{
}
ParkingSlot::ParkingSlot(double lrx, double lry, double rrx, double rry,
double rfx, double rfy, double lfx, double lfy) :
- border_{Point(lrx, lry), Point(rrx, rry),
+ _border{Point(lrx, lry), Point(rrx, rry),
Point(rfx, rfy), Point(lfx, lfy)},
Point(rfx, rfy), Point(lfx, lfy)},
- entry_(border_[0], border_[3]),
- rear_(border_[0], border_[1]),
- curb_(border_[1], border_[2]),
- front_(border_[2], border_[3])
+ _entry(_border[0], _border[3]),
+ _rear(_border[0], _border[1]),
+ _curb(_border[1], _border[2]),
+ _front(_border[2], _border[3])
- 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]);
+ 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]);
- auto b_len = sizeof(this->border_) / sizeof(this->border_[0]);
- std::vector<Point> b(this->border_, this->border_ + b_len);
+ auto b_len = sizeof(this->_border) / sizeof(this->_border[0]);
+ std::vector<Point> b(this->_border, this->_border + b_len);
- 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_);
+ 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);
assert(c.len() < this->len());
assert(c.w() < this->w());
std::vector<BicycleCar> path;
assert(c.len() < this->len());
assert(c.w() < this->w());
std::vector<BicycleCar> path;
- auto b_len = sizeof(this->border_) / sizeof(this->border_[0]);
- std::vector<Point> b(this->border_, this->border_ + b_len);
- while (cusp < this->max_cusp_ + 1) {
+ auto b_len = sizeof(this->_border) / sizeof(this->_border[0]);
+ std::vector<Point> b(this->_border, this->_border + b_len);
+ while (cusp < this->_max_cusp + 1) {
- c.x(this->entry_.m().x() + gd * cos(this->rear_.h()));
- c.y(this->entry_.m().y() + gd * sin(this->rear_.h()));
+ c.x(this->_entry.m().x() + gd * cos(this->_rear.h()));
+ c.y(this->_entry.m().y() + gd * sin(this->_rear.h()));
- this->rear_.intersects_with(cc, radi);
- dd = std::min(this->border_[0].edist(this->rear_.i1()),
- this->border_[0].edist(this->rear_.i2()));
+ this->_rear.intersects_with(cc, radi);
+ dd = std::min(this->_border[0].edist(this->_rear.i1()),
+ this->_border[0].edist(this->_rear.i2()));
return PoseRange(c.x(), c.y(), c.h(), c.h());
}
bool swapped = false;
return PoseRange(c.x(), c.y(), c.h(), c.h());
}
bool swapped = false;
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();
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();
if (rr.edist(i1) < rr.edist(i2)) {
max_to_slot = rr.min_angle_between(rc, i1);
} else {
if (rr.edist(i1) < rr.edist(i2)) {
max_to_slot = rr.min_angle_between(rc, i1);
} else {
- a_to_slot += this->delta_angle_to_slot_;
- c.rotate(rc, this->delta_angle_to_slot_);
+ a_to_slot += this->_delta_angle_to_slot;
+ c.rotate(rc, this->_delta_angle_to_slot);
ParkingSlot::recompute_entry(PoseRange p)
{
p.rotate(Point(0.0, 0.0), this->h());
ParkingSlot::recompute_entry(PoseRange p)
{
p.rotate(Point(0.0, 0.0), this->h());
- o << s.border_[0] << ",";
- o << s.border_[1] << ",";
- o << s.border_[2] << ",";
- o << s.border_[3];
+ o << s._border[0] << ",";
+ o << s._border[1] << ",";
+ o << s._border[2] << ",";
+ o << s._border[3];