Merge branch 'feature/fer-perpendicullar'

diff --git a/CHANGELOG.md b/CHANGELOG.md
index 3b344e954691ebd66bb5c6f018107d29b96eb759..41a97235dfdaf88ea122c71821b3af66aacc7df7 100644 (file)
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -14,5 +14,6 @@ The format is based on [Keep a Changelog][] and this project adheres to
 - [WvTest][] unit testing framework.
 - Link `bcar` and `pslot` libraries.
 - Implement find entry by reverse method.
+- Find entry by reverse method for perpendicular parking slots.
 
 [WvTest]: https://github.com/apenwarr/wvtest

diff --git a/api/psp.h b/api/psp.h
index 529c04d7ecbe6ce0ec1f97ee0596cf556dbbc8ce..57a0c07b2fe5d2e3f2a6caec5c664bd16fc0d5ab 100644 (file)
--- a/api/psp.h
+++ b/api/psp.h
@@ -17,6 +17,10 @@ class PSPlanner {
                 BicycleCar cc_;
                 BicycleCar gc_;
                 ParkingSlot ps_;
+
+                // find entry to slot by reverse approach
+                void fer_parallel();
+                void fer_perpendicular();
         public:
                 /*! \brief Return `true` if there is collision.
 
@@ -30,6 +34,12 @@ class PSPlanner {
                 (parking slot lines).
                 */
                 bool collide();
+                /*! \brief Return parking direction
+
+                Return `true` if the direction of the parking in the
+                slot is forward.
+                */
+                bool forward();
                 /*! \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 a138944d283514e21c8f94358840e3087a232ced..369dea2df88e77334886d647cb3101a0b84d06b3 100644 (file)
--- a/src/psp.cc
+++ b/src/psp.cc
@@ -1,3 +1,4 @@
+#include <cmath>
 #include "psp.h"
 
 bool PSPlanner::collide()
@@ -93,23 +94,34 @@ bool PSPlanner::left()
 {
        double lfx = this->cc().lfx();
        double lfy = this->cc().lfy();
+       double lrx = this->cc().lrx();
+       double lry = this->cc().lry();
+       double rrx = this->cc().rrx();
+       double rry = this->cc().rry();
        double rfx = this->cc().rfx();
        double rfy = this->cc().rfy();
-       double ccx = this->cc().x();
-       double ccy = this->cc().y();
        double lfs = sgn(
                (lfx - this->ps().x1()) * (this->ps().y4() - this->ps().y1())
                - (lfy - this->ps().y1()) * (this->ps().x4() - this->ps().x1())
        );
+       double lrs = sgn(
+               (lrx - this->ps().x1()) * (this->ps().y4() - this->ps().y1())
+               - (lry - this->ps().y1()) * (this->ps().x4() - this->ps().x1())
+       );
+       double rrs = sgn(
+               (rrx - this->ps().x1()) * (this->ps().y4() - this->ps().y1())
+               - (rry - this->ps().y1()) * (this->ps().x4() - this->ps().x1())
+       );
        double rfs = sgn(
                (rfx - this->ps().x1()) * (this->ps().y4() - this->ps().y1())
                - (rfy - this->ps().y1()) * (this->ps().x4() - this->ps().x1())
        );
-       double ccs = sgn(
-               (ccx - this->ps().x1()) * (this->ps().y4() - this->ps().y1())
-               - (ccy - this->ps().y1()) * (this->ps().x4() - this->ps().x1())
-       );
-       return lfs == rfs && lfs != ccs;
+        if (this->ps().parallel())
+                return lfs == rfs && (lfs != lrs || lfs != rrs);
+        else if (!this->forward())
+                return lfs == rfs && (lfs != lrs || lfs != rrs);
+        else
+                return lrs == rrs && (lrs != lfs || lrs != rfs);
 }
 
 // find entry
@@ -118,15 +130,107 @@ void PSPlanner::fe()
 }
 
 void PSPlanner::fer()
+{
+        if (this->ps().parallel())
+                return this->fer_parallel();
+        else
+                return this->fer_perpendicular();
+}
+
+void PSPlanner::fer_parallel()
 {
         this->cc().st(this->cc().wb() / this->cc().mtr());
         if (!this->ps().right())
                 this->cc().st(this->cc().st() * -1);
         this->cc().sp(0.1);
         while (!this->left()) {
-                while (!this->collide() && !this->left()) {
+                while (!this->collide() && !this->left())
+                        this->cc().next();
+                if (this->left() && !this->collide()) {
+                        break;
+                } else {
+                        this->cc().sp(this->cc().sp() * -1);
                         this->cc().next();
+                        this->cc().st(this->cc().st() * -1);
                 }
+        }
+}
+
+void PSPlanner::fer_perpendicular()
+{
+        double cc_h = this->cc().h();
+        double x;
+        double y;
+        // check inner radius
+        if (this->forward()) {
+                x = this->ps().x1();
+                y = this->ps().y1();
+        } else {
+                x = this->ps().x4();
+                y = this->ps().y4();
+        }
+        double x1;
+        double y1;
+        if (this->ps().right()) {
+                x1 = this->cc().ccr().x();
+                y1 = this->cc().ccr().y();
+        } else {
+                x1 = this->cc().ccl().x();
+                y1 = this->cc().ccl().y();
+        }
+        double IR = this->cc().iradi();
+        double a = 1;
+        double b;
+        if (this->forward())
+                b = (x - x1) * 2 * cos(cc_h) + (y - y1) * 2 * sin(cc_h);
+        else
+                b = (x1 - x) * 2 * cos(cc_h) + (y1 - y) * 2 * sin(cc_h);
+        double c = pow(x - x1, 2) + pow(y - y1, 2) - pow(IR, 2);
+        double D = D = pow(b, 2) - 4 * a * c;
+        double delta;
+        delta = -b - sqrt(D);
+        delta /= 2 * a;
+        double delta_1 = delta;
+        // check outer radius
+        if (this->forward()) {
+                x = this->ps().x4();
+                y = this->ps().y4();
+        } else {
+                x = this->ps().x1();
+                y = this->ps().y1();
+        }
+        IR = this->cc().ofradi();
+        a = 1;
+        if (this->forward())
+                b = (x - x1) * 2 * cos(cc_h) + (y - y1) * 2 * sin(cc_h);
+        else
+                b = (x1 - x) * 2 * cos(cc_h) + (y1 - y) * 2 * sin(cc_h);
+        c = pow(x - x1, 2) + pow(y - y1, 2) - pow(IR, 2);
+        D = pow(b, 2) - 4 * a * c;
+        if (this->forward()) {
+                delta = -b + sqrt(D);
+                delta /= 2 * a;
+        }
+        double delta_2 = delta;
+        delta = -b - sqrt(D);
+        delta /= 2 * a;
+        double delta_3 = delta;
+        delta = std::max(delta_1, std::max(delta_2, delta_3));
+        // current car `cc` can get out of slot with max steer
+        this->cc().x(this->cc().x() + delta * cos(cc_h));
+        this->cc().y(this->cc().y() + delta * sin(cc_h));
+        this->cc().h(cc_h);
+        // get current car `cc` out of slot
+        if (this->forward())
+                this->cc().sp(-0.1);
+        else
+                this->cc().sp(0.1);
+        this->cc().st(this->cc().wb() / this->cc().mtr());
+        if (this->ps().right())
+                this->cc().st(this->cc().st() * -1);
+        while (!this->left()) {
+                while (!this->collide() && !this->left())
+                        this->cc().next();
                 if (this->left() && !this->collide()) {
                         break;
                 } else {
@@ -137,6 +241,20 @@ void PSPlanner::fer()
         }
 }
 
+bool PSPlanner::forward()
+{
+        double heading = this->ps().heading();
+        while (heading < 0) heading += 2 * M_PI;
+        if (!this->ps().parallel())
+                heading -= M_PI / 2;
+        double h = this->gc().h();
+        while (h < 0) h += 2 * M_PI;
+        if (-0.00001 < heading - h && heading - h < 0.00001)
+                return true;
+        else
+                return false;
+}
+
 PSPlanner::PSPlanner()
 {
 }

diff --git a/ut/psp.t.cc b/ut/psp.t.cc
index 94e8ccabed8b103182029814bb9294f855afef4d..d1ea09467ce2c08e8fa5c75a9da2c1a70718c08a 100644 (file)
--- a/ut/psp.t.cc
+++ b/ut/psp.t.cc
@@ -3,7 +3,7 @@
 
 #include "psp.h"
 
-WVTEST_MAIN("parking slot planner basic test")
+WVTEST_MAIN("parallel parking slot planner")
 {
         PSPlanner psp;
         psp.ps().border(3, 3, 5, 3, 5, 8, 3, 8);
@@ -21,6 +21,7 @@ WVTEST_MAIN("parking slot planner basic test")
 
         // init orientation
         WVPASS(!psp.collide());
+        WVPASS(psp.forward());
         WVPASSEQ_DOUBLE(psp.ps().heading(), psp.gc().h(), 0.00001);
 
         // entry point found by reverse
@@ -44,3 +45,57 @@ WVTEST_MAIN("parking slot planner basic test")
         tpsp.ps().border(3, 4.1, 3, 2.1, 8, 2.1, 8, 4.1);
         WVPASS(tpsp.left());
 }
+
+WVTEST_MAIN("backward perpendicullar parking slot planner")
+{
+        PSPlanner psp;
+        psp.ps().border(3, 3, 8, 3, 8, 5, 3, 5);
+        psp.gc().x(7);
+        psp.gc().y(4);
+        psp.gc().h(M_PI);
+        psp.gc().mtr(10);
+        psp.gc().wb(2);
+        psp.gc().w(1);
+        psp.gc().l(3);
+        psp.gc().he(1.5);
+        psp.gc().df(2 + 0.5);
+        psp.gc().dr(0.5);
+        psp.cc() = BicycleCar(psp.gc());
+
+        // init orientation
+        WVPASS(!psp.collide());
+        WVPASS(!psp.forward());
+        WVPASSEQ_DOUBLE(psp.ps().heading() + M_PI / 2, psp.gc().h(), 0.00001);
+
+        // entry point found by reverse
+        WVPASS(!psp.left());
+        psp.fer();
+        WVPASS(psp.left());
+}
+
+WVTEST_MAIN("forward perpendicullar parking slot planner")
+{
+        PSPlanner psp;
+        psp.ps().border(3, 3, 8, 3, 8, 5, 3, 5);
+        psp.gc().x(4);
+        psp.gc().y(4);
+        psp.gc().h(0);
+        psp.gc().mtr(10);
+        psp.gc().wb(2);
+        psp.gc().w(1);
+        psp.gc().l(3);
+        psp.gc().he(1.5);
+        psp.gc().df(2 + 0.5);
+        psp.gc().dr(0.5);
+        psp.cc() = BicycleCar(psp.gc());
+
+        // init orientation
+        WVPASS(!psp.collide());
+        WVPASS(psp.forward());
+        WVPASSEQ_DOUBLE(psp.ps().heading() - M_PI / 2, psp.gc().h(), 0.00001);
+
+        // entry point found by reverse
+        WVPASS(!psp.left());
+        psp.fer();
+        WVPASS(psp.left());
+}