Add last maneuver computation

diff --git a/api/psp.h b/api/psp.h
index 7ee2bb81a90528f1d9bf279bcd2955329f151618..b6049845a6af2cd9b3a40cf22b067d8c4c6ff035 100644 (file)
--- a/api/psp.h
+++ b/api/psp.h
@@ -54,6 +54,12 @@ class PSPlanner {
                 Set the goal car guessed from the parking slot.
                 */
                 void guess_gc();
+                /*! \brief Return last maneuver to the parking slot.
+
+                Return path from entry point towards parking slot, such
+                that ``cc`` is inside the parking slot at the end.
+                */
+                std::vector<BicycleCar> last_maneuver();
                 /*! \brief Has current car `cc` left?
 
                 Return `true` if the current car `cc` left the parking

diff --git a/src/psp.cc b/src/psp.cc
index df6b58ea4468af08217f11082763db1297e134bc..9f5ea8df41ee6e43ee29a4eafb8275df04c44fd2 100644 (file)
--- a/src/psp.cc
+++ b/src/psp.cc
@@ -152,6 +152,46 @@ void PSPlanner::guess_gc()
         this->gc().h(h);
 }
 
+std::vector<BicycleCar> PSPlanner::last_maneuver()
+{
+        std::vector<BicycleCar> lm;
+        if (this->ps().parallel()) {
+                // zig-zag out from the slot
+                this->cc() = BicycleCar(this->gc());
+                this->cc().sp(0.1);
+                while (!this->left()) {
+                        while (!this->collide() && !this->left()) {
+                                this->cc().next();
+                                lm.push_back(BicycleCar(this->cc()));
+                        }
+                        if (this->left() && !this->collide()) {
+                                break;
+                        } else {
+                                lm.pop_back();
+                                this->cc().sp(this->cc().sp() * -1);
+                                this->cc().next();
+                                this->cc().st(this->cc().st() * -1);
+                                this->c_++;
+                                lm.push_back(BicycleCar(this->cc()));
+                        }
+                }
+                if (this->cc().st() < 0) {
+                        this->c_++;
+                        lm.push_back(BicycleCar(this->cc()));
+                }
+        } else {
+                // go 1 m forward
+                this->cc().sp(0.1);
+                BicycleCar orig_cc(this->cc());
+                for (unsigned int i = 0; i < 10; i++) {
+                        this->cc().next();
+                        lm.push_back(BicycleCar(this->cc()));
+                }
+                this->cc() = BicycleCar(orig_cc);
+        }
+        return lm;
+}
+
 bool PSPlanner::left()
 {
        double lfx = this->cc().lfx();