BicycleCar gc_;
ParkingSlot ps_;
- // find entry to slot by reverse approach
+ // find entry to slot
void fe_parallel();
void fe_perpendicular();
+ // find entry to slot by reverse approach
void fer_parallel();
void fer_perpendicular();
+ // set goal car
+ void gc_to_4();
public:
/*! \brief Return `true` if there is collision.
return false;
}
+void PSPlanner::gc_to_4()
+{
+ double angl_slot = atan2(
+ this->ps().y3() - this->ps().y4(),
+ this->ps().x3() - this->ps().x4()
+ );
+ double angl_delta = M_PI / 2;
+ if (this->ps().right())
+ angl_delta = -M_PI / 2;
+ double x = this->ps().x4();
+ double y = this->ps().y4();
+ x += (this->gc().dr() + 0.01) * cos(angl_slot);
+ y += (this->gc().dr() + 0.01) * sin(angl_slot);
+ x += (this->gc().w() / 2 + 0.01) * cos(angl_slot + angl_delta);
+ y += (this->gc().w() / 2 + 0.01) * sin(angl_slot + angl_delta);
+ this->gc().x(x);
+ this->gc().y(y);
+ this->gc().h(angl_slot);
+}
+
void PSPlanner::guess_gc()
{
double x = this->ps().x1();