X-Git-Url: https://rtime.felk.cvut.cz/gitweb/hubacji1/psp.git/blobdiff_plain/eeb71223b656e52daa696cb125c117b5473c8f78..802b4fe9bfc4f3e75b28cebf3ce5013b436a5362:/src/psp.cc

diff --git a/src/psp.cc b/src/psp.cc
index 606c6f6..31d4a74 100644
--- a/src/psp.cc
+++ b/src/psp.cc
@@ -21,16 +21,175 @@ bool PSPlanner::collide()
 
 bool PSPlanner::forward()
 {
-        double heading = this->ps().heading();
+        if (this->ps().parallel())
+                return false;
+        else
+                return true;
+        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()
+{
+        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;
+        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 {
+                // Forward parking
+                double entry_width = edist(
+                        this->ps().x1(), this->ps().y1(),
+                        this->ps().x4(), this->ps().y4()
+                );
+                x += entry_width / 2 * cos(h);
+                y += entry_width / 2 * sin(h);
+                h = atan2(
+                        this->ps().y2() - this->ps().y1(),
+                        this->ps().x2() - this->ps().x1()
+                );
+                while (h < 0) h += 2 * M_PI;
+
+                //// This is for backward parking only.
+                //double entry_width = edist(
+                //        this->ps().x1(), this->ps().y1(),
+                //        this->ps().x4(), this->ps().y4()
+                //);
+                //double dist_l =
+                //        this->gc().orradi()
+                //        - (this->gc().mtr() + this->gc().w() / 2)
+                //;
+                //double move1 = dist_l + this->gc().w() / 2;
+                //double dist_r = entry_width - this->gc().w() - dist_l;
+                //double move2 = sqrt(
+                //        pow(this->gc().iradi(), 2)
+                //        - pow(this->gc().iradi() - dist_r, 2)
+                //);
+                //move2 -= this->gc().dr() / 2; // workaround
+                //x += move1 * cos(h);
+                //y += move1 * sin(h);
+                //dts = atan2(
+                //        this->ps().y2() - this->ps().y1(),
+                //        this->ps().x2() - this->ps().x1()
+                //);
+                //while (dts < 0) dts += 2 * M_PI;
+                //x += move2 * cos(h + dts);
+                //y += move2 * sin(h + dts);
+                //h += dts - M_PI / 2;
+        }
+        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);
+}
+
+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()
@@ -80,85 +239,113 @@ bool PSPlanner::parked()
                 && inside(this->gc().rfx(), this->gc().rfy(), slot);
 }
 
+std::vector<BicycleCar> PSPlanner::possible_goals(
+        unsigned int cnt,
+        double dist
+)
+{
+        std::vector<BicycleCar> pi;
+        if (this->ps().parallel())
+                this->cc().sp(1);
+        else
+                this->cc().sp(-1);
+        this->cc().sp(this->cc().sp() * dist);
+        BicycleCar orig_cc(this->cc());
+        for (unsigned int i = 0; i < cnt; i++) {
+                this->cc().next();
+                pi.push_back(BicycleCar(this->cc()));
+        }
+        this->cc() = BicycleCar(orig_cc);
+        if (this->ps().parallel()) {
+                this->cc().st(0);
+                for (unsigned int i = 0; i < cnt; i++) {
+                        this->cc().next();
+                        pi.push_back(BicycleCar(this->cc()));
+                }
+                this->cc() = BicycleCar(orig_cc);
+        } else {
+                if (!this->ps().right()) {
+                        this->cc().set_max_steer();
+                        for (unsigned int i = 0; i < cnt; i++) {
+                                this->cc().next();
+                                pi.push_back(BicycleCar(this->cc()));
+                        }
+                } else {
+                        this->cc().set_max_steer();
+                        this->cc().st(this->cc().st() * -1);
+                        for (unsigned int i = 0; i < cnt; i++) {
+                                this->cc().next();
+                                pi.push_back(BicycleCar(this->cc()));
+                        }
+                }
+                this->cc() = BicycleCar(orig_cc);
+        }
+        return pi;
+}
+
 // find entry
 void PSPlanner::fe()
 {
-        if (this->ps().parallel())
+        this->c_ = 0;
+        if (this->ps().parallel()) {
                 return this->fe_parallel();
-        else
-                return this->fe_perpendicular();
+        } else {
+                this->guess_gc();
+                this->cc() = BicycleCar(this->gc());
+                //this->cc().set_max_steer();
+                //if (this->ps().right())
+                //        this->cc().st(this->cc().st() * -1);
+                this->cc().sp(-0.2);
+        }
 }
 
 void PSPlanner::fe_parallel()
 {
-        // angle for distance from "entry" corner
-        double dist_angl = this->ps().heading() + M_PI;
-        dist_angl += (this->ps().right()) ? - M_PI / 4 : + M_PI / 4;
-        // set bicycle car `bci` basic dimensions and heading
-        BicycleCar bci = BicycleCar(this->gc());
         BicycleCar bco = BicycleCar(this->gc());
-        bci.h(this->ps().heading());
-        // move 0.01 from the "entry" corner
-        bci.x(this->ps().x4() + 0.01 * cos(dist_angl));
-        bci.y(this->ps().y4() + 0.01 * sin(dist_angl));
-        // align with parking "top" of slot (move backward)
-        dist_angl = bci.h() + M_PI;
-        bci.x(bci.x() + bci.df() * cos(dist_angl));
-        bci.y(bci.y() + bci.df() * sin(dist_angl));
-        // align with "entry" to pakring slot (move outside)
-        dist_angl = this->ps().heading();
-        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));
-        // 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));
-        if (this->ps().right())
-                dist_angl = atan2(bci.y() - bci.rfy(), bci.x() - bci.rfx());
-        else
-                dist_angl = atan2(bci.y() - bci.lfy(), bci.x() - bci.lfx());
-        double DIST_ANGL = dist_angl;
-        std::queue<BicycleCar, std::list<BicycleCar>> q;
-        while (
-                (
-                        this->ps().right()
-                        && dist_angl < DIST_ANGL + 3 * M_PI / 4
+        this->cc() = BicycleCar();
+        this->cc().sp(-0.01);
+        this->cc().set_max_steer();
+        if (!this->ps().right())
+                this->cc().st(this->cc().st() * -1);
+        this->cc().h(this->ps().heading());
+        double angl_in_slot = this->ps().heading() - M_PI / 4;
+        if (!this->ps().right())
+                angl_in_slot += M_PI / 2;
+        this->cc().x(
+                this->ps().x4()
+                + this->cc().w()/2 * cos(
+                        this->ps().heading()
+                        + (this->ps().right() ? + M_PI / 2 : - M_PI / 2)
                 )
-                || (
-                        !this->ps().right()
-                        && dist_angl > DIST_ANGL - 3 * M_PI / 4
+                + (this->cc().df() + 0.01) * cos(
+                        this->ps().heading() + M_PI
                 )
-        ) {
-                this->cc() = BicycleCar(bci);
-                if (this->ps().right()) {
-                        this->cc().x(bci.rfx() + dist_diag * cos(dist_angl));
-                        this->cc().y(bci.rfy() + dist_diag * sin(dist_angl));
-                } else {
-                        this->cc().x(bci.lfx() + dist_diag * cos(dist_angl));
-                        this->cc().y(bci.lfy() + dist_diag * sin(dist_angl));
-                }
-                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;
+        );
+        this->cc().y(
+                this->ps().y4()
+                + this->cc().w()/2 * sin(
+                        this->ps().heading()
+                        + (this->ps().right() ? + M_PI / 2 : - M_PI / 2)
+                )
+                + (this->cc().df() + 0.01) * sin(
+                        this->ps().heading() + M_PI
+                )
+        );
+
+        std::queue<BicycleCar, std::list<BicycleCar>> q;
+        while (!this->collide()) {
+                q.push(this->cc());
+                this->cc().rotate(
+                        this->ps().x4(),
+                        this->ps().y4() - 0.01,
+                        ((this->ps().right()) ? 0.001 : -0.001)
+                );
         }
         // BFS - find entry current car `cc` and corresponding goal car `gc`
-        unsigned int iter_cntr;
-        while (!q.empty() && iter_cntr < 9) {
+        unsigned int iter_cntr = 0;
+        while (!q.empty() && iter_cntr < 30) {
                 this->cc() = BicycleCar(q.front());
                 q.pop();
-                while (
-                        !this->collide()
-                        && (std::abs(
-                                this->cc().h() - this->ps().heading()
-                        ) < M_PI / 2)
-                )
-                        this->cc().next();
                 this->cc().sp(this->cc().sp() * -1);
                 this->cc().next();
                 this->gc() = BicycleCar(this->cc());
@@ -185,23 +372,34 @@ 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()
 {
-        if (this->ps().parallel())
+        this->c_ = 0;
+        if (this->ps().parallel()) {
+                this->guess_gc();
+                this->cc() = BicycleCar(this->gc());
+                this->cc().set_max_steer();
+                if (!this->ps().right())
+                        this->cc().st(this->cc().st() * -1);
+                this->cc().sp(0.01);
                 return this->fer_parallel();
-        else
+        } 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.01);
+        this->cusps_.clear();
         while (!this->left()) {
                 while (!this->collide() && !this->left())
                         this->cc().next();
@@ -211,12 +409,19 @@ void PSPlanner::fer_parallel()
                         this->cc().sp(this->cc().sp() * -1);
                         this->cc().next();
                         this->cc().st(this->cc().st() * -1);
+                        this->c_++;
+                        this->cusps_.push_back(this->cc());
                 }
         }
+        if (this->cc().st() < 0) {
+                this->c_++;
+                this->cusps_.push_back(this->cc());
+        }
 }
 
 void PSPlanner::fer_perpendicular()
 {
+        bool delta_use[] = {true, true, true};
         double cc_h = this->cc().h();
         double x;
         double y;
@@ -245,11 +450,13 @@ void PSPlanner::fer_perpendicular()
         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 D = pow(b, 2) - 4 * a * c;
         double delta;
         delta = -b - sqrt(D);
         delta /= 2 * a;
         double delta_1 = delta;
+        if (D < 0)
+                delta_use[0] = false;
         // check outer radius
         if (this->forward()) {
                 x = this->ps().x4();
@@ -271,20 +478,39 @@ void PSPlanner::fer_perpendicular()
                 delta /= 2 * a;
         }
         double delta_2 = delta;
+        if (D < 0)
+                delta_use[1] = false;
         delta = -b - sqrt(D);
         delta /= 2 * a;
         double delta_3 = delta;
-        delta = std::max(delta_1, std::max(delta_2, delta_3));
+        if (D < 0)
+                delta_use[2] = false;
+        if (delta_use[0] && delta_use[1] && delta_use[2])
+                delta = std::max(delta_1, std::max(delta_2, delta_3));
+        else if (delta_use[0] && delta_use[1])
+                delta = std::max(delta_1, delta_2);
+        else if (delta_use[0] && delta_use[2])
+                delta = std::max(delta_1, delta_3);
+        else if (delta_use[1] && delta_use[2])
+                delta = std::max(delta_2, delta_3);
+        else if (delta_use[0])
+                delta = delta_1;
+        else if (delta_use[1])
+                delta = delta_2;
+        else if (delta_use[2])
+                delta = delta_3;
+        else
+                return;
         // 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);
+                this->cc().sp(-0.01);
         else
-                this->cc().sp(0.1);
-        this->cc().st(this->cc().wb() / this->cc().mtr());
+                this->cc().sp(0.01);
+        this->cc().set_max_steer();
         if (this->ps().right())
                 this->cc().st(this->cc().st() * -1);
         while (!this->left()) {