From: Jiri Vlasak Date: Wed, 1 Jul 2020 15:58:27 +0000 (+0200) Subject: FIX 2877ad5 X-Git-Tag: v0.4.0~4^2~29 X-Git-Url: https://rtime.felk.cvut.cz/gitweb/hubacji1/psp.git/commitdiff_plain/b4e818f97fc3330b6966a2687c33bc90b0eade8b FIX 2877ad5 --- diff --git a/src/psp.cc b/src/psp.cc index 1e61784..3489606 100644 --- a/src/psp.cc +++ b/src/psp.cc @@ -344,99 +344,100 @@ void PSPlanner::fe_parallel() // BFS - find entry current car `cc` and corresponding goal car `gc` unsigned int iter_cntr = 0; while (!q.empty() && iter_cntr < 30) { + std::cerr << "backw " << iter_cntr << std::endl; 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 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 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 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 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); + 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 (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 if (this->ps().right() && this->cc().sp() > 0) { std::cerr << "forw " << iter_cntr << std::endl; double ccrx = this->cc().ccr().x();