Add set goal car position method

[hubacji1/psp.git] / src / psp.cc

diff --git a/src/psp.cc b/src/psp.cc
index 395deecffc0436ea5cb4cc1f096978a2bf302708..5992e3ec54a55599abd5232ac7d803bf5c82eafc 100644 (file)
--- a/src/psp.cc
+++ b/src/psp.cc
@@ -35,6 +35,26 @@ bool PSPlanner::forward()
         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();
@@ -141,13 +161,8 @@ std::vector<BicycleCar> PSPlanner::possible_inits(
 )
 {
         std::vector<BicycleCar> pi;
-        if (!this->ps().parallel()) {
-                this->cc().sp(this->cc().sp() * -dist);
-                this->cc().st(this->cc().st() * -1);
-        } else {
-                this->cc().sp(this->cc().sp() * dist);
-                this->cc().st(this->cc().st() * 1);
-        }
+        this->cc().sp(this->cc().sp() * dist);
+        this->cc().st(this->cc().st() * 1);
         BicycleCar orig_cc(this->cc());
         for (unsigned int i = 0; i < cnt; i++) {
                 this->cc().next();
@@ -266,7 +281,13 @@ void PSPlanner::fe_perpendicular()
         //
         //      Another approach could be testing angles from the
         //      beginning of the escape parkig slot maneuver.
-        return fer_perpendicular();
+        if (this->forward())
+                this->cc().sp(-0.01);
+        else
+                this->cc().sp(0.01);
+        while (!this->left())
+                this->cc().next();
+        return;
 }
 
 void PSPlanner::fer()