X-Git-Url: https://rtime.felk.cvut.cz/gitweb/hubacji1/psp.git/blobdiff_plain/12fde3bad5354702571c90ae6e8c91b07f482b35..6d2a78b51d95fd09f7e70943f02921f74a866a38:/src/psp.cc diff --git a/src/psp.cc b/src/psp.cc index 6d90ce4..8bc3ac2 100644 --- a/src/psp.cc +++ b/src/psp.cc @@ -1,17 +1,812 @@ +#include +#include +#include #include "psp.h" bool PSPlanner::collide() { + std::vector> bc; + bc.push_back(std::make_tuple(this->cc().lfx(), this->cc().lfy())); + bc.push_back(std::make_tuple(this->cc().lrx(), this->cc().lry())); + bc.push_back(std::make_tuple(this->cc().rrx(), this->cc().rry())); + bc.push_back(std::make_tuple(this->cc().rfx(), this->cc().rfy())); + bc.push_back(std::make_tuple(this->cc().lfx(), this->cc().lfy())); + std::vector> ps; + ps.push_back(std::make_tuple(this->ps().x1(), this->ps().y1())); + ps.push_back(std::make_tuple(this->ps().x2(), this->ps().y2())); + ps.push_back(std::make_tuple(this->ps().x3(), this->ps().y3())); + ps.push_back(std::make_tuple(this->ps().x4(), this->ps().y4())); + return std::get<0>(::collide(bc, ps)); +} + +bool PSPlanner::forward() +{ + 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; + double h = this->gc().h(); + while (h < 0) h += 2 * M_PI; + if (std::abs(heading - h) < M_PI / 4) + return true; 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 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 PSPlanner::last_maneuver() +{ + std::vector 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(); + 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 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()) + ); + 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); +} + +bool PSPlanner::parked() +{ + std::vector> slot; + slot.push_back(std::make_tuple(this->ps().x1(), this->ps().y1())); + slot.push_back(std::make_tuple(this->ps().x2(), this->ps().y2())); + slot.push_back(std::make_tuple(this->ps().x3(), this->ps().y3())); + slot.push_back(std::make_tuple(this->ps().x4(), this->ps().y4())); + return inside(this->gc().lfx(), this->gc().lfy(), slot) + && inside(this->gc().lrx(), this->gc().lry(), slot) + && inside(this->gc().rrx(), this->gc().rry(), slot) + && inside(this->gc().rfx(), this->gc().rfy(), slot); +} + +std::vector PSPlanner::possible_goals( + unsigned int cnt, + double dist +) +{ + std::vector 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() { + this->c_ = 0; + if (this->ps().parallel()) { + return this->fe_parallel(); + } 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() +{ + BicycleCar bco = BicycleCar(this->gc()); + 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->cc().df() + 0.01) * cos( + this->ps().heading() + M_PI + ) + ); + 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, + ((this->ps().right()) ? 0.001 : -0.001) + ); + } + // BFS - find entry current car `cc` and corresponding goal car `gc` + unsigned int iter_cntr = 0; + while (!q.empty() && iter_cntr < 30) { + this->cc() = BicycleCar(q.front()); + q.pop(); + if (this->ps().right() && this->cc().sp() < 0) { + double cclx = this->cc().ccl().x(); + double ccly = this->cc().ccl().y(); + + double r1 = sqrt( + pow(this->cc().lry() - ccly, 2) + + pow(this->cc().lrx() - cclx, 2) + ); + auto cli1 = ::intersect( + cclx, ccly, r1, + this->ps().x1(), this->ps().y1(), + this->ps().x2(), this->ps().y2() + ); + double a11 = ::angle_between_three_points( + this->cc().lrx(), this->cc().lry(), + cclx, ccly, + std::get<1>(cli1), std::get<2>(cli1) + ); + double a12 = ::angle_between_three_points( + this->cc().lrx(), this->cc().lry(), + cclx, ccly, + std::get<3>(cli1), std::get<4>(cli1) + ); + double a1 = std::min(a11, a12); +{ + double rf = edist(cclx, ccly, this->ps().x1(), this->ps().y1()); + if ( + edist(cclx, ccly, this->cc().lrx(), this->cc().lry()) < rf + && rf < edist(cclx, ccly, this->cc().rrx(), this->cc().rry()) + ) { + auto clif = ::intersect( + cclx, ccly, rf, + this->cc().lrx(), this->cc().lry(), + this->cc().rrx(), this->cc().rry() + ); + double xf = std::get<1>(clif); + double yf = std::get<2>(clif); + if ( + edist( + std::get<3>(clif), std::get<4>(clif), + this->cc().x(), this->cc().y() + + ) + < edist(xf, yf, this->cc().x(), this->cc().y()) + ) { + xf = std::get<3>(clif); + yf = std::get<4>(clif); + } + double af = ::intersection( + xf, yf, + cclx, ccly, + this->ps().x1(), this->ps().y1() + ); + a1 = af; + } +} + + double r2 = sqrt( + pow(this->cc().rry() - ccly, 2) + + pow(this->cc().rrx() - cclx, 2) + ); + auto cli2 = ::intersect( + cclx, ccly, r2, + this->ps().x1(), this->ps().y1(), + this->ps().x2(), this->ps().y2() + ); + double a21 = ::angle_between_three_points( + this->cc().rrx(), this->cc().rry(), + cclx, ccly, + std::get<1>(cli2), std::get<2>(cli2) + ); + double a22 = ::angle_between_three_points( + this->cc().rrx(), this->cc().rry(), + cclx, ccly, + std::get<3>(cli2), std::get<4>(cli2) + ); + double a2 = std::min(a21, a22); + + double r3 = sqrt( + pow(this->cc().rry() - ccly, 2) + + pow(this->cc().rrx() - cclx, 2) + ); + auto cli3 = ::intersect( + cclx, ccly, r3, + this->ps().x2(), this->ps().y2(), + this->ps().x3(), this->ps().y3() + ); + double a31 = ::angle_between_three_points( + this->cc().rrx(), this->cc().rry(), + cclx, ccly, + std::get<1>(cli3), std::get<2>(cli3) + ); + double a32 = ::angle_between_three_points( + this->cc().rrx(), this->cc().rry(), + cclx, ccly, + std::get<3>(cli3), std::get<4>(cli3) + ); + double a3 = std::min(a31, a32); + + if (std::get<0>(cli1) && ( + (!std::get<0>(cli2) && !std::get<0>(cli3)) + || (a1 < a2 && !std::get<0>(cli3)) + || (a1 < a3 && !std::get<0>(cli2)) + || (a1 < a2 && a1 < a3) + )) { + this->cc().rotate(cclx, ccly, -a1); + } else if (std::get<0>(cli2) && ( + (!std::get<0>(cli1) && !std::get<0>(cli3)) + || (a2 < a1 && !std::get<0>(cli3)) + || (a2 < a3 && !std::get<0>(cli1)) + || (a2 < a1 && a2 < a3) + )) { + this->cc().rotate(cclx, ccly, -a2); + } else if (std::get<0>(cli3) && ( + (!std::get<0>(cli1) && !std::get<0>(cli2)) + || (a3 < a1 && !std::get<0>(cli2)) + || (a3 < a2 && !std::get<0>(cli1)) + || (a3 < a1 && a3 < a2) + )) { + this->cc().rotate(cclx, ccly, -a3); + } else { + continue; + } + if (::right_side_of_line( + this->cc().x(), this->cc().y(), + this->cc().x() + cos(this->ps().heading()), + this->cc().y() + sin(this->ps().heading()), + this->cc().x() + cos(this->cc().h()), + this->cc().y() + sin(this->cc().h()) + )) + continue; + } else if (this->ps().right() && this->cc().sp() > 0) { + double ccrx = this->cc().ccr().x(); + double ccry = this->cc().ccr().y(); +{ + double rf = sqrt( + pow(this->cc().lfy() - ccry, 2) + + pow(this->cc().lfx() - ccrx, 2) + ); + auto clif = ::intersect( + ccrx, ccry, rf, + this->ps().x1(), this->ps().y1(), + this->ps().x4(), this->ps().y4() + ); + if (std::get<0>(clif)) { + double xf = std::get<1>(clif); + double yf = std::get<2>(clif); + if ( + edist( + this->ps().x4(), + this->ps().y4(), + std::get<3>(clif), + std::get<4>(clif) + ) < edist( + this->ps().x4(), + this->ps().y4(), + xf, yf + ) + ) { + xf = std::get<3>(clif); + yf = std::get<4>(clif); + } + auto af = ::angle_between_three_points( + this->cc().lfx(), + this->cc().lfy(), + ccrx, ccry, + xf, yf + ); + auto tmp_cc = BicycleCar(this->cc()); + this->cc().rotate(ccrx, ccry, -af); + if ( + !this->collide() + && (edist( + this->ps().x1(), this->ps().y1(), + xf, yf + ) < edist( + this->ps().x1(), this->ps().y1(), + this->ps().x4(), this->ps().y4() + )) + ) { + this->cc().sp(-0.01); + this->cc().set_max_steer(); + this->cc().st(this->cc().st() * -1); + this->gc() = BicycleCar(this->cc()); + goto successfinish; + } else { + this->cc() = BicycleCar(tmp_cc); + } + } else { + // should be parked and found in + // previous iteration + } +} + double r1 = sqrt( + pow(this->cc().rfy() - ccry, 2) + + pow(this->cc().rfx() - ccrx, 2) + ); + auto cli1 = ::intersect( + ccrx, ccry, r1, + this->ps().x3(), this->ps().y3(), + this->ps().x4(), this->ps().y4() + ); + double a11 = ::angle_between_three_points( + this->cc().lrx(), this->cc().lry(), + ccrx, ccry, + std::get<1>(cli1), std::get<2>(cli1) + ); + double a12 = ::angle_between_three_points( + this->cc().lrx(), this->cc().lry(), + ccrx, ccry, + std::get<3>(cli1), std::get<4>(cli1) + ); + double a1 = std::min(a11, a12); + + double r2 = sqrt( + pow(this->cc().lfy() - ccry, 2) + + pow(this->cc().lfx() - ccrx, 2) + ); + auto cli2 = ::intersect( + ccrx, ccry, r2, + this->ps().x3(), this->ps().y3(), + this->ps().x4(), this->ps().y4() + ); + double a21 = ::angle_between_three_points( + this->cc().rrx(), this->cc().rry(), + ccrx, ccry, + std::get<1>(cli2), std::get<2>(cli2) + ); + double a22 = ::angle_between_three_points( + this->cc().rrx(), this->cc().rry(), + ccrx, ccry, + std::get<3>(cli2), std::get<4>(cli2) + ); + double a2 = std::min(a21, a22); + + double r3 = sqrt( + pow(this->cc().rfy() - ccry, 2) + + pow(this->cc().rfx() - ccrx, 2) + ); + auto cli3 = ::intersect( + ccrx, ccry, r3, + this->ps().x3(), this->ps().y3(), + this->ps().x2(), this->ps().y2() + ); + double a31 = ::angle_between_three_points( + this->cc().rrx(), this->cc().rry(), + ccrx, ccry, + std::get<1>(cli3), std::get<2>(cli3) + ); + double a32 = ::angle_between_three_points( + this->cc().rrx(), this->cc().rry(), + ccrx, ccry, + std::get<3>(cli3), std::get<4>(cli3) + ); + double a3 = std::min(a31, a32); + + if (std::get<0>(cli1) && ( + (!std::get<0>(cli2) && !std::get<0>(cli3)) + || (a1 < a2 && !std::get<0>(cli3)) + || (a1 < a3 && !std::get<0>(cli2)) + || (a1 < a2 && a1 < a3) + )) { + this->cc().rotate(ccrx, ccry, -a1); + } else if (std::get<0>(cli2) && ( + (!std::get<0>(cli1) && !std::get<0>(cli3)) + || (a2 < a1 && !std::get<0>(cli3)) + || (a2 < a3 && !std::get<0>(cli1)) + || (a2 < a1 && a2 < a3) + )) { + this->cc().rotate(ccrx, ccry, -a2); + } else if (std::get<0>(cli3) && ( + (!std::get<0>(cli1) && !std::get<0>(cli2)) + || (a3 < a1 && !std::get<0>(cli2)) + || (a3 < a2 && !std::get<0>(cli1)) + || (a3 < a1 && a3 < a2) + )) { + this->cc().rotate(ccrx, ccry, -a3); + } else { + continue; + } + } else { + // TODO left parking slot (both forward, backward) + } + this->cc().sp(this->cc().sp() * -1); + this->cc().next(); + this->gc() = BicycleCar(this->cc()); + if (this->parked()) + goto successfinish; + this->cc().st(this->cc().st() * -1); + q.push(BicycleCar(this->cc())); + if (sgn(this->cc().st()) == sgn(q.front().st())) + iter_cntr++; + } + // fallback to fer + this->gc() = BicycleCar(bco); +successfinish: + return this->fer_parallel(); +} + +void PSPlanner::fe_perpendicular() +{ + // TODO Try multiple angles when going from parking slot. + // + // Do not use just the maximum steer angle. Test angles + // until the whole current car `cc` is out of the parking + // slot `ps`. + // + // Another approach could be testing angles from the + // beginning of the escape parkig slot maneuver. + if (this->forward()) + this->cc().sp(-0.01); + else + this->cc().sp(0.01); + while (!this->left()) + this->cc().next(); + return; } void PSPlanner::fer() { + 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 { + return this->fer_perpendicular(); + } +} + +void PSPlanner::fer_parallel() +{ + this->cusps_.clear(); + while (!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); + 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; + // 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 = 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(); + 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; + if (D < 0) + delta_use[1] = false; + delta = -b - sqrt(D); + delta /= 2 * a; + double delta_3 = delta; + 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.01); + else + this->cc().sp(0.01); + this->cc().set_max_steer(); + 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 { + this->cc().sp(this->cc().sp() * -1); + this->cc().next(); + this->cc().st(this->cc().st() * -1); + } + } } PSPlanner::PSPlanner()