+bool PSPlanner::forward()
+{
+ if (this->ps().parallel())
+ return false;
+ double heading = atan2(
+ this->ps().y2() - this->ps().y1(),
+ this->ps().x2() - this->ps().x1()
+ );
+ while (heading < 0) heading += 2 * M_PI;
+ double h = this->gc().h();
+ while (h < 0) h += 2 * M_PI;
+ if (std::abs(heading - h) < M_PI / 4)
+ return true;
+ return false;
+}
+
+void PSPlanner::guess_gc()
+{
+ double x = this->ps().x1();
+ double y = this->ps().y1();
+ double h = this->ps().heading();
+ double dts = + M_PI / 2; // direction to slot
+ if (this->ps().right())
+ dts = - M_PI / 2;
+ dts *= 0.99; // precision workaround
+ if (this->ps().parallel()) {
+ x += (this->gc().w() / 2 + 0.01) * cos(h + dts);
+ 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);
+ } else {
+ dts = atan2(
+ this->ps().y2() - this->ps().y1(),
+ this->ps().x2() - this->ps().x1()
+ );
+ dts *= 1.01; // precision workaround
+ if (std::abs(dts - this->ps().heading()) < M_PI / 2) {
+ x = this->ps().x4();
+ y = this->ps().y4();
+ h = dts;
+ x += (this->gc().dr() + 0.01) * cos(h);
+ y += (this->gc().dr() + 0.01) * sin(h);
+ if (this->ps().right())
+ dts -= M_PI / 2;
+ else
+ dts += M_PI / 2;
+ x += (this->gc().w() / 2 + 0.01) * cos(dts);
+ y += (this->gc().w() / 2 + 0.01) * sin(dts);
+ } else {
+ h = dts + M_PI;
+ x += -(this->gc().df() + 0.01) * cos(h);
+ y += -(this->gc().df() + 0.01) * sin(h);
+ if (this->ps().right())
+ dts += M_PI / 2;
+ else
+ dts -= M_PI / 2;
+ x += (this->gc().w() / 2 + 0.01) * cos(dts);
+ y += (this->gc().w() / 2 + 0.01) * sin(dts);
+ }
+ }
+ while (h > M_PI)
+ h -= 2 * M_PI;
+ while (h <= -M_PI)
+ h += 2 * M_PI;
+ this->gc().x(x);
+ this->gc().y(y);
+ this->gc().h(h);
+}
+