x += (this->gc().dr() + 0.01) * cos(h);
y += (this->gc().w() / 2 + 0.01) * sin(h + dts);
y += (this->gc().dr() + 0.01) * sin(h);
+#if 0
} else {
// Forward parking
double entry_width = edist(
//x += move2 * cos(h + dts);
//y += move2 * sin(h + dts);
//h += dts - M_PI / 2;
+#endif
+ } else {
+ // Backward parking
+ double entry_width = edist(
+ this->ps().x1(), this->ps().y1(),
+ this->ps().x4(), this->ps().y4()
+ );
+ x += entry_width / 2 * cos(h);
+ y += entry_width / 2 * sin(h);
+ h = atan2(
+ this->ps().y1() - this->ps().y2(),
+ this->ps().x1() - this->ps().x2()
+ );
+ while (h < 0) h += 2 * M_PI;
}
while (h > M_PI)
h -= 2 * M_PI;