X-Git-Url: https://rtime.felk.cvut.cz/gitweb/hubacji1/psp.git/blobdiff_plain/262ca6bf5b1dedba858e0d65fd7df8b4c19e9b5e..1e54f83b2a117d6803b214df1bc7537ed603ccd9:/src/psp.cc diff --git a/src/psp.cc b/src/psp.cc index 5992e3e..9258012 100644 --- a/src/psp.cc +++ b/src/psp.cc @@ -55,6 +55,38 @@ void PSPlanner::gc_to_4() this->gc().h(angl_slot); } +std::tuple 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(); @@ -63,31 +95,93 @@ void PSPlanner::guess_gc() double dts = + M_PI / 2; // direction to slot if (this->ps().right()) dts = - M_PI / 2; - dts *= 0.99; // precision workaround 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 { - dts = atan2( - this->ps().y2() - this->ps().y1(), - this->ps().x2() - this->ps().x1() - ); - dts *= 1.01; // precision workaround - if (std::abs(dts - this->ps().heading()) < M_PI / 2) { - 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); + if (std::abs( + atan2( + this->ps().y2() - this->ps().y1(), + this->ps().x2() - this->ps().x1() + ) + - this->ps().heading() + ) < M_PI / 2) { + // forward parking + this->gc_to_4(); + double bx; + double by; + double cx; + double cy; + if (this->ps().right()) { + bx = this->gc().lfx(); + by = this->gc().lfy(); + cx = this->gc().ccr().x(); + cy = this->gc().ccr().y(); + } else { + bx = this->gc().rfx(); + by = this->gc().rfy(); + cx = this->gc().ccl().x(); + cy = this->gc().ccl().y(); + } + double radi_angl = atan2(by - cy, bx - cx); + radi_angl += dts; + double angl_delta = this->gc().h() - radi_angl; + this->gc().rotate(bx, by, angl_delta); + // TODO there is a bug somewhere :/ + // + // cli returns not exact intersection, therefore the + // distance to x1, y1 of border is shorter. Then, when + // moving, the distance `dist_o` is not sufficient and + // car still collide with parking slot. It shouldn't be + // problem until it collides with obstacle. + // + if (this->ps().right()) { + cx = this->gc().ccr().x(); + cy = this->gc().ccr().y(); + } else { + cx = this->gc().ccl().x(); + cy = this->gc().ccl().y(); + } + auto cli = circle_line_intersection( + cx, cy, this->gc().iradi(), + this->ps().x1(), this->ps().y1(), + this->ps().x2(), this->ps().y2() + ); + double d1 = edist( + this->ps().x1(), this->ps().y1(), + std::get<0>(cli), std::get<1>(cli) + ); + double d2 = edist( + this->ps().x1(), this->ps().y1(), + std::get<2>(cli), std::get<3>(cli) + ); + double dist_o = std::min(d1, d2); + double angl_o = atan2( + this->ps().y4() - this->ps().y3(), + this->ps().x4() - this->ps().x3() + ); + // projection + double angl_d = atan2( + this->ps().y1() - this->ps().y2(), + this->ps().x1() - this->ps().x2() + ); + angl_d -= angl_o; + dist_o *= cos(angl_d); + this->gc().x(this->gc().x() + dist_o * cos(angl_o)); + this->gc().y(this->gc().y() + dist_o * sin(angl_o)); + // --- ENDTODO --- + this->gc().sp(-0.01); + this->gc().st(dts); + return; } 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); @@ -155,12 +249,16 @@ bool PSPlanner::parked() && inside(this->gc().rfx(), this->gc().rfy(), slot); } -std::vector PSPlanner::possible_inits( +std::vector PSPlanner::possible_goals( unsigned int cnt, double dist ) { std::vector pi; + if (this->cc().sp() > 0) + this->cc().sp(1); + else + this->cc().sp(-1); this->cc().sp(this->cc().sp() * dist); this->cc().st(this->cc().st() * 1); BicycleCar orig_cc(this->cc()); @@ -183,66 +281,49 @@ void PSPlanner::fe() 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)); - // set default speed, steer - bci.st(bci.wb() / bci.mtr()); + this->cc() = BicycleCar(); + this->cc().sp(-0.01); + this->cc().set_max_steer(); 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)); - 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> q; - while ( - ( - this->ps().right() - && dist_angl < DIST_ANGL + 3 * M_PI / 4 + 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()) { - 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> q; + while (!this->collide()) { + q.push(this->cc()); + this->cc().rotate( + this->ps().x4(), + this->ps().y4() - 0.01, + 0.01 + ); } // BFS - find entry current car `cc` and corresponding goal car `gc` unsigned int iter_cntr = 0; - while (!q.empty() && iter_cntr < 9) { + while (!q.empty() && iter_cntr < 30) { this->cc() = BicycleCar(q.front()); q.pop(); while ( @@ -304,6 +385,7 @@ void PSPlanner::fer_parallel() 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(); @@ -313,8 +395,14 @@ 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() @@ -408,7 +496,7 @@ void PSPlanner::fer_perpendicular() this->cc().sp(-0.01); else this->cc().sp(0.01); - this->cc().st(this->cc().wb() / this->cc().mtr()); + this->cc().set_max_steer(); if (this->ps().right()) this->cc().st(this->cc().st() * -1); while (!this->left()) {