X-Git-Url: https://rtime.felk.cvut.cz/gitweb/hubacji1/psp.git/blobdiff_plain/232ea49983f32a1e362baeea311d1fe7835012fd..b1e5ff694daf6b2ef32ebb1dc821914798655440:/src/psp.cc

diff --git a/src/psp.cc b/src/psp.cc
index f0d2f31..dc81f84 100644
--- a/src/psp.cc
+++ b/src/psp.cc
@@ -21,16 +21,70 @@ bool PSPlanner::collide()
 
 bool PSPlanner::forward()
 {
-        double heading = this->ps().heading();
+        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;
-        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)
+        if (std::abs(heading - h) < M_PI / 4)
                 return true;
-        else
-                return false;
+        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);
+}
+
+std::tuple<double, double, double, double> circle_line_intersection(
+        double cx, double cy, double r,
+        double x1, double y1,
+        double x2, double y2
+)
+{
+        double t = (y2 - y1) / (x2 - x1);
+        //double a = 1 + pow(t, 2);
+        //double b = - 2 * cx - 2 * pow(t, 2) * x1 + 2 * t * y1 - 2 * t * cy;
+        //double c = pow(cx, 2) + pow(t, 2) * pow(x1, 2) - 2 * t * y1 * x1
+        //        + pow(y1, 2) + 2 * t * cy * x1 - 2 * y1 * cy + pow(cy, 2)
+        //        - pow(r, 2);
+        double a = 1 + pow(t, 2);
+        double b = - 2 * cx + 2 * t * (-t * x1 + y1) - 2 * cy * t;
+        double c = pow(cx, 2) + pow(cy, 2) - pow(r, 2);
+        c += pow(-t * x1 + y1, 2);
+        c += 2 * cy * t * x1 - 2 * cy * y1;
+        double D = pow(b, 2) - 4 * a * c;
+        if (D < 0)
+                return std::make_tuple(cx, cy, cx, cy);
+        double res_x1 = (-b + sqrt(D)) / (2 * a);
+        double res_y1 = t * (res_x1 - x1) + y1;
+        double res_x2 = (-b - sqrt(D)) / (2 * a);
+        double res_y2 = t * (res_x2 - x1) + y1;
+        return std::make_tuple(res_x1, res_y1, res_x2, res_y2);
+}
+
+double edist(double x1, double y1, double x2, double y2)
+{
+        return sqrt(pow(x2 - x1, 2) + pow(y2 - y1, 2));
 }
 
 void PSPlanner::guess_gc()
@@ -42,19 +96,48 @@ void PSPlanner::guess_gc()
         if (this->ps().right())
                 dts = - M_PI / 2;
         if (this->ps().parallel()) {
-                x += (this->gc().w() + 0.01) * cos(h + dts);
+                dts *= 0.99; // precision workaround
+                x += (this->gc().w() / 2 + 0.01) * cos(h + dts);
                 x += (this->gc().dr() + 0.01) * cos(h);
-                y += (this->gc().w() + 0.01) * sin(h + dts);
+                y += (this->gc().w() / 2 + 0.01) * sin(h + dts);
                 y += (this->gc().dr() + 0.01) * sin(h);
         } else {
-                x += (this->ps().x4() - this->ps().x1()) / 2;
-                x += (this->gc().df() + 0.01) * cos(h + dts);
-                y += (this->ps().y4() - this->ps().y1()) / 2;
-                y += (this->gc().df() + 0.01) * sin(h + dts);
-                if (this->ps().right())
-                        h += M_PI / 2;
-                else
-                        h -= M_PI / 2;
+                if (std::abs(
+                        atan2(
+                                this->ps().y2() - this->ps().y1(),
+                                this->ps().x2() - this->ps().x1()
+                        )
+                        - this->ps().heading()
+                ) < M_PI / 2) {
+                        // forward parking
+                        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 {
+                        dts = atan2(
+                                this->ps().y2() - this->ps().y1(),
+                                this->ps().x2() - this->ps().x1()
+                        );
+                        dts *= 1.01; // precision workaround
+                        // backward parking
+                        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;
@@ -112,13 +195,16 @@ bool PSPlanner::parked()
                 && inside(this->gc().rfx(), this->gc().rfy(), slot);
 }
 
-std::vector<BicycleCar> PSPlanner::possible_inits()
+std::vector<BicycleCar> PSPlanner::possible_inits(
+        unsigned int cnt,
+        double dist
+)
 {
         std::vector<BicycleCar> pi;
-        this->cc().sp(this->cc().sp() * -1);
-        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 < 20; i++) {
+        for (unsigned int i = 0; i < cnt; i++) {
                 this->cc().next();
                 pi.push_back(BicycleCar(this->cc()));
         }
@@ -156,6 +242,11 @@ void PSPlanner::fe_parallel()
         dist_angl += (this->ps().right()) ? + M_PI / 2 : - M_PI / 2;
         bci.x(bci.x() + bci.w() / 2 * cos(dist_angl));
         bci.y(bci.y() + bci.w() / 2 * sin(dist_angl));
+        // set default speed, steer
+        bci.st(bci.wb() / bci.mtr());
+        if (!this->ps().right())
+                bci.st(bci.st() * -1);
+        bci.sp(-0.01);
         // BFS - init all starts
         // see https://courses.cs.washington.edu/courses/cse326/03su/homework/hw3/bfs.html
         double dist_diag = sqrt(pow(bci.w() / 2, 2) + pow(bci.df(), 2));
@@ -185,21 +276,20 @@ void PSPlanner::fe_parallel()
                 }
                 this->cc().h(this->ps().heading() + dist_angl - DIST_ANGL);
                 if (!this->collide()) {
-                        this->cc().st(this->cc().wb() / this->cc().mtr());
-                        if (!this->ps().right())
-                                this->cc().st(this->cc().st() * -1);
-                        this->cc().sp(-0.01);
                         q.push(BicycleCar(this->cc()));
                 }
                 dist_angl += (this->ps().right()) ? + 0.01 : - 0.01;
         }
         // BFS - find entry current car `cc` and corresponding goal car `gc`
-        unsigned int iter_cntr;
+        unsigned int iter_cntr = 0;
         while (!q.empty() && iter_cntr < 9) {
                 this->cc() = BicycleCar(q.front());
                 q.pop();
                 while (
                         !this->collide()
+                        && (std::abs(
+                                this->cc().h() - this->ps().heading()
+                        ) > M_PI / 32)
                         && (std::abs(
                                 this->cc().h() - this->ps().heading()
                         ) < M_PI / 2)
@@ -231,7 +321,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()
@@ -349,9 +445,9 @@ void PSPlanner::fer_perpendicular()
         this->cc().h(cc_h);
         // get current car `cc` out of slot
         if (this->forward())
-                this->cc().sp(-0.1);
+                this->cc().sp(-0.01);
         else
-                this->cc().sp(0.1);
+                this->cc().sp(0.01);
         this->cc().st(this->cc().wb() / this->cc().mtr());
         if (this->ps().right())
                 this->cc().st(this->cc().st() * -1);