X-Git-Url: https://rtime.felk.cvut.cz/gitweb/hubacji1/psp.git/blobdiff_plain/9fd797a028de1d657eae59f7fd207ac2ca64162a..43359b55f3853e82249843e3ec19c2a6f618303b:/src/psp.cc diff --git a/src/psp.cc b/src/psp.cc index 34d4b7c..f69e1f2 100644 --- a/src/psp.cc +++ b/src/psp.cc @@ -102,6 +102,7 @@ void PSPlanner::guess_gc() 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); +#if 0 } else { // Forward parking double entry_width = edist( @@ -115,33 +116,24 @@ void PSPlanner::guess_gc() 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; + x += this->gc().dr() * cos(h + M_PI); + y += this->gc().dr() * sin(h + M_PI); +#endif + } else { + // Backward 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().y1() - this->ps().y2(), + this->ps().x1() - this->ps().x2() + ); + while (h < 0) h += 2 * M_PI; + x += this->gc().df() * cos(h + M_PI); + y += this->gc().df() * sin(h + M_PI); } while (h > M_PI) h -= 2 * M_PI; @@ -299,15 +291,16 @@ void PSPlanner::fe() } } -double smaller_angle_of_two( +double angle_between_closer_point( double sx, double sy, // common start point double cx, double cy, // common middle point double x1, double y1, // first ending point double x2, double y2 // second ending point ) { - double a1 = ::angle_between_three_points(sx, sy, cx, cy, x1, y1); - double a2 = ::angle_between_three_points(sx, sy, cx, cy, x2, y2); - return std::min(a1, a2); + if (edist(sx, sy, x1, y1) < edist(sx, sy, x2, y2)) + return ::angle_between_three_points(sx, sy, cx, cy, x1, y1); + else + return ::angle_between_three_points(sx, sy, cx, cy, x2, y2); } void PSPlanner::fe_parallel() @@ -382,7 +375,7 @@ void PSPlanner::fe_parallel() this->cc().lrx(), this->cc().lry(), this->cc().rrx(), this->cc().rry() ); - double a1 = ::smaller_angle_of_two( + double a1 = ::angle_between_closer_point( this->ps().x1(), this->ps().y1(), cclx, ccly, std::get<1>(cli1), std::get<2>(cli1), @@ -393,7 +386,7 @@ void PSPlanner::fe_parallel() this->ps().x2(), this->ps().y2(), this->ps().x3(), this->ps().y3() ); - double a2 = smaller_angle_of_two( + double a2 = angle_between_closer_point( this->cc().rrx(), this->cc().rry(), cclx, ccly, std::get<1>(cli2), std::get<2>(cli2), @@ -404,6 +397,21 @@ void PSPlanner::fe_parallel() || a1 < a2 )) { this->cc().rotate(cclx, ccly, -a1); + 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 (std::get<0>(cli2) && ( !std::get<0>(cli1) || a2 < a1 @@ -419,7 +427,7 @@ void PSPlanner::fe_parallel() this->ps().x1(), this->ps().y1(), this->ps().x2(), this->ps().y2() ); - double a1 = smaller_angle_of_two( + double a1 = angle_between_closer_point( this->cc().lrx(), this->cc().lry(), cclx, ccly, std::get<1>(cli1), std::get<2>(cli1), @@ -430,7 +438,7 @@ void PSPlanner::fe_parallel() this->ps().x2(), this->ps().y2(), this->ps().x3(), this->ps().y3() ); - double a2 = smaller_angle_of_two( + double a2 = angle_between_closer_point( this->cc().rrx(), this->cc().rry(), cclx, ccly, std::get<1>(cli2), std::get<2>(cli2), @@ -441,6 +449,21 @@ void PSPlanner::fe_parallel() || a1 < a2 )) { this->cc().rotate(cclx, ccly, -a1); + 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 (std::get<0>(cli2) && ( !std::get<0>(cli1) || a2 < a1 @@ -453,128 +476,66 @@ void PSPlanner::fe_parallel() } 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 or continue - // with the parking process - } -} - 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 ccr_lf = edist( + ccrx, ccry, + this->cc().lfx(), this->cc().lfy() ); - double a11 = ::angle_between_three_points( - this->cc().lrx(), this->cc().lry(), - ccrx, ccry, - std::get<1>(cli1), std::get<2>(cli1) + double ccr_rf = edist( + ccrx, ccry, + this->cc().rfx(), this->cc().rfy() ); - double a12 = ::angle_between_three_points( - this->cc().lrx(), this->cc().lry(), - ccrx, ccry, - std::get<3>(cli1), std::get<4>(cli1) + { + double af = std::abs( + this->ps().heading() + - this->cc().h() + ); + auto tmp_cc = BicycleCar(this->cc()); + this->cc().rotate(ccrx, ccry, -af); + this->gc() = BicycleCar(this->cc()); + if ( + !this->collide() + && this->parked() + ) { + this->cc().sp(this->cc().sp() * -1); + this->gc() = BicycleCar(this->cc()); + goto successfinish; + } else { + this->cc() = BicycleCar(tmp_cc); + } + } + auto cli1 = ::intersect( + ccrx, ccry, ccr_rf, + this->ps().x3(), this->ps().y3(), + this->ps().x4(), this->ps().y4() ); - double a1 = std::min(a11, a12); - - double r2 = sqrt( - pow(this->cc().lfy() - ccry, 2) - + pow(this->cc().lfx() - ccrx, 2) + double a1 = angle_between_closer_point( + this->cc().rfx(), this->cc().rfy(), + ccrx, ccry, + std::get<1>(cli1), std::get<2>(cli1), + std::get<3>(cli1), std::get<4>(cli1) ); 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) + ccrx, ccry, ccr_rf, + this->ps().x2(), this->ps().y2(), + this->ps().x3(), this->ps().y3() ); - 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) + double a2 = angle_between_closer_point( + this->cc().rfx(), this->cc().rfy(), + ccrx, ccry, + std::get<1>(cli2), std::get<2>(cli2), + std::get<3>(cli2), std::get<4>(cli2) ); 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) + ccrx, ccry, ccr_lf, + this->ps().x3(), this->ps().y3(), + this->ps().x4(), this->ps().y4() ); - double a32 = ::angle_between_three_points( - this->cc().rrx(), this->cc().rry(), - ccrx, ccry, - std::get<3>(cli3), std::get<4>(cli3) + double a3 = angle_between_closer_point( + this->cc().lfx(), this->cc().lfy(), + ccrx, ccry, + std::get<1>(cli3), std::get<2>(cli3), + 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))