X-Git-Url: https://rtime.felk.cvut.cz/gitweb/hubacji1/psp.git/blobdiff_plain/60a893dc49a745e913bccd4b3c2fd57208caf81d..3231095bbfd687390fb681dc60ea4d9022cca054:/src/psp.cc diff --git a/src/psp.cc b/src/psp.cc index 35e92ac..9021d26 100644 --- a/src/psp.cc +++ b/src/psp.cc @@ -21,20 +21,11 @@ bool PSPlanner::collide() 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; +#if FORWARD_PARKING > 0 + return true; +#else return false; +#endif } void PSPlanner::gc_to_4() @@ -102,8 +93,8 @@ 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 { +#if FORWARD_PARKING > 0 // Forward parking double entry_width = edist( this->ps().x1(), this->ps().y1(), @@ -116,35 +107,9 @@ 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; -#endif - } else { + x += this->gc().dr() * cos(h + M_PI); + y += this->gc().dr() * sin(h + M_PI); +#else // Backward parking double entry_width = edist( this->ps().x1(), this->ps().y1(), @@ -157,6 +122,9 @@ void PSPlanner::guess_gc() 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); +#endif } while (h > M_PI) h -= 2 * M_PI; @@ -298,6 +266,29 @@ std::vector PSPlanner::possible_goals( return pi; } +void PSPlanner::shrink_to_perfect_len() +{ + if (!this->ps().parallel()) + return; + double perfect_len = this->gc().perfect_parking_slot_len(); + if (edist( + this->ps().x1(), this->ps().y1(), + this->ps().x4(), this->ps().y4() + ) < perfect_len) + return; + double h = this->ps().heading(); + h -= M_PI; + while (h < 0) h += 2 * M_PI; + double ch = perfect_len * cos(h); + double sh = perfect_len * sin(h); + this->ps().border( + this->ps().x4() + ch, this->ps().y4() + sh, + this->ps().x3() + ch, this->ps().y3() + sh, + this->ps().x3(), this->ps().y3(), + this->ps().x4(), this->ps().y4() + ); +} + // find entry void PSPlanner::fe() { @@ -328,6 +319,7 @@ double angle_between_closer_point( void PSPlanner::fe_parallel() { + this->shrink_to_perfect_len(); BicycleCar bco = BicycleCar(this->gc()); this->cc() = BicycleCar(); this->cc().sp(-0.01); @@ -583,6 +575,216 @@ void PSPlanner::fe_parallel() } else { continue; } + } else if (!this->ps().right() && this->cc().sp() < 0) { + double ccrx = this->cc().ccr().x(); + double ccry = this->cc().ccr().y(); + double ccr_rr = edist( + ccrx, ccry, + this->cc().rrx(), this->cc().rry() + ); + double ccr_lr = edist( + ccrx, ccry, + this->cc().lrx(), this->cc().lry() + ); + double ccr_p1 = edist( + ccrx, ccry, + this->ps().x1(), this->ps().y1() + ); + if (ccr_lr < ccr_p1) { + // pass parking slot + continue; + } else if (ccr_lr >= ccr_p1 && ccr_rr < ccr_p1) { + // partially out of parking slot + auto cli1 = ::intersect( + ccrx, ccry, ccr_p1, + this->cc().lrx(), this->cc().lry(), + this->cc().rrx(), this->cc().rry() + ); + double a1 = ::angle_between_closer_point( + this->ps().x1(), this->ps().y1(), + ccrx, ccry, + std::get<1>(cli1), std::get<2>(cli1), + std::get<3>(cli1), std::get<4>(cli1) + ); + auto cli2 = ::intersect( + ccrx, ccry, ccr_lr, + this->ps().x2(), this->ps().y2(), + this->ps().x3(), this->ps().y3() + ); + double a2 = angle_between_closer_point( + this->cc().lrx(), this->cc().lry(), + ccrx, ccry, + std::get<1>(cli2), std::get<2>(cli2), + std::get<3>(cli2), std::get<4>(cli2) + ); + if (std::get<0>(cli1) && ( + !std::get<0>(cli2) + || a1 < a2 + )) { + this->cc().rotate(ccrx, ccry, 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 + )) { + this->cc().rotate(ccrx, ccry, a2); + } else { + continue; + } + } else if (ccr_rr >= ccr_p1) { + // in parking slot + auto cli1 = ::intersect( + ccrx, ccry, ccr_rr, + this->ps().x1(), this->ps().y1(), + this->ps().x2(), this->ps().y2() + ); + double a1 = angle_between_closer_point( + this->cc().rrx(), this->cc().rry(), + ccrx, ccry, + std::get<1>(cli1), std::get<2>(cli1), + std::get<3>(cli1), std::get<4>(cli1) + ); + auto cli2 = ::intersect( + ccrx, ccry, ccr_lr, + this->ps().x2(), this->ps().y2(), + this->ps().x3(), this->ps().y3() + ); + double a2 = angle_between_closer_point( + this->cc().lrx(), this->cc().lry(), + ccrx, ccry, + std::get<1>(cli2), std::get<2>(cli2), + std::get<3>(cli2), std::get<4>(cli2) + ); + if (std::get<0>(cli1) && ( + !std::get<0>(cli2) + || a1 < a2 + )) { + this->cc().rotate(ccrx, ccry, 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 + )) { + this->cc().rotate(ccrx, ccry, a2); + } else { + continue; + } + } + } else if (!this->ps().right() && this->cc().sp() > 0) { + double cclx = this->cc().ccl().x(); + double ccly = this->cc().ccl().y(); + double ccl_rf = edist( + cclx, ccly, + this->cc().rfx(), this->cc().rfy() + ); + double ccl_lf = edist( + cclx, ccly, + this->cc().lfx(), this->cc().lfy() + ); + { + double af = std::abs( + this->ps().heading() + - this->cc().h() + ); + auto tmp_cc = BicycleCar(this->cc()); + this->cc().rotate(cclx, ccly, 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( + cclx, ccly, ccl_lf, + this->ps().x3(), this->ps().y3(), + this->ps().x4(), this->ps().y4() + ); + double a1 = angle_between_closer_point( + this->cc().lfx(), this->cc().lfy(), + cclx, ccly, + std::get<1>(cli1), std::get<2>(cli1), + std::get<3>(cli1), std::get<4>(cli1) + ); + auto cli2 = ::intersect( + cclx, ccly, ccl_lf, + this->ps().x2(), this->ps().y2(), + this->ps().x3(), this->ps().y3() + ); + double a2 = angle_between_closer_point( + this->cc().lfx(), this->cc().lfy(), + cclx, ccly, + std::get<1>(cli2), std::get<2>(cli2), + std::get<3>(cli2), std::get<4>(cli2) + ); + auto cli3 = ::intersect( + cclx, ccly, ccl_rf, + this->ps().x3(), this->ps().y3(), + this->ps().x4(), this->ps().y4() + ); + double a3 = angle_between_closer_point( + this->cc().rfx(), this->cc().rfy(), + cclx, ccly, + std::get<1>(cli3), std::get<2>(cli3), + std::get<3>(cli3), std::get<4>(cli3) + ); + 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; + } } else { // TODO left parking slot (both forward, backward) }