X-Git-Url: https://rtime.felk.cvut.cz/gitweb/hubacji1/psp.git/blobdiff_plain/da8d8301b0d2a9e734b8fbd5b4cff07d38f9b77a..3231095bbfd687390fb681dc60ea4d9022cca054:/src/psp.cc diff --git a/src/psp.cc b/src/psp.cc index d36c084..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() @@ -103,6 +94,7 @@ void PSPlanner::guess_gc() y += (this->gc().w() / 2 + 0.01) * sin(h + dts); y += (this->gc().dr() + 0.01) * sin(h); } else { +#if FORWARD_PARKING > 0 // Forward parking double entry_width = edist( this->ps().x1(), this->ps().y1(), @@ -115,33 +107,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); +#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); +#endif } while (h > M_PI) h -= 2 * M_PI; @@ -283,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() { @@ -299,8 +305,21 @@ void PSPlanner::fe() } } +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 +) { + 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() { + this->shrink_to_perfect_len(); BicycleCar bco = BicycleCar(this->gc()); this->cc() = BicycleCar(); this->cc().sp(-0.01); @@ -349,280 +368,420 @@ void PSPlanner::fe_parallel() 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 ccl_lr = edist( + cclx, ccly, + this->cc().lrx(), this->cc().lry() ); - double a11 = ::angle_between_three_points( - this->cc().lrx(), this->cc().lry(), - cclx, ccly, - std::get<1>(cli1), std::get<2>(cli1) + double ccl_rr = edist( + cclx, ccly, + this->cc().rrx(), this->cc().rry() ); - double a12 = ::angle_between_three_points( - this->cc().lrx(), this->cc().lry(), - cclx, ccly, - std::get<3>(cli1), std::get<4>(cli1) + double ccl_p1 = edist( + cclx, ccly, + this->ps().x1(), this->ps().y1() ); - 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() + if (ccl_rr < ccl_p1) { + // pass parking slot + continue; + } else if (ccl_rr >= ccl_p1 && ccl_lr < ccl_p1) { + // partially out of parking slot + auto cli1 = ::intersect( + cclx, ccly, ccl_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(), + cclx, ccly, + std::get<1>(cli1), std::get<2>(cli1), + std::get<3>(cli1), std::get<4>(cli1) + ); + auto cli2 = ::intersect( + cclx, ccly, ccl_rr, + this->ps().x2(), this->ps().y2(), + this->ps().x3(), this->ps().y3() + ); + double a2 = angle_between_closer_point( + this->cc().rrx(), this->cc().rry(), + cclx, ccly, + 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(cclx, ccly, -a1); + if (::right_side_of_line( + 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 = ::angle_between_three_points( - xf, yf, - cclx, ccly, - this->ps().x1(), this->ps().y1() - ); - a1 = af; - } -} + 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(cclx, ccly, -a2); + } else { + continue; + } + } else if (ccl_lr >= ccl_p1) { + // in parking slot + auto cli1 = ::intersect( + cclx, ccly, ccl_lr, + this->ps().x1(), this->ps().y1(), + this->ps().x2(), this->ps().y2() + ); + double a1 = angle_between_closer_point( + this->cc().lrx(), this->cc().lry(), + cclx, ccly, + std::get<1>(cli1), std::get<2>(cli1), + std::get<3>(cli1), std::get<4>(cli1) + ); + auto cli2 = ::intersect( + cclx, ccly, ccl_rr, + this->ps().x2(), this->ps().y2(), + this->ps().x3(), this->ps().y3() + ); + double a2 = angle_between_closer_point( + this->cc().rrx(), this->cc().rry(), + cclx, ccly, + 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(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()), - double r2 = sqrt( - pow(this->cc().rry() - ccly, 2) - + pow(this->cc().rrx() - cclx, 2) + 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(cclx, ccly, -a2); + } 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_lf = edist( + ccrx, ccry, + this->cc().lfx(), this->cc().lfy() ); - auto cli2 = ::intersect( - cclx, ccly, r2, - this->ps().x1(), this->ps().y1(), - this->ps().x2(), this->ps().y2() + double ccr_rf = edist( + ccrx, ccry, + this->cc().rfx(), this->cc().rfy() ); - double a21 = ::angle_between_three_points( - this->cc().rrx(), this->cc().rry(), - cclx, ccly, - std::get<1>(cli2), std::get<2>(cli2) + { + 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 a22 = ::angle_between_three_points( - this->cc().rrx(), this->cc().rry(), - cclx, ccly, - std::get<3>(cli2), std::get<4>(cli2) + 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) ); - double a2 = std::min(a21, a22); - - double r3 = sqrt( - pow(this->cc().rry() - ccly, 2) - + pow(this->cc().rrx() - cclx, 2) + auto cli2 = ::intersect( + ccrx, ccry, ccr_rf, + this->ps().x2(), this->ps().y2(), + this->ps().x3(), this->ps().y3() ); - auto cli3 = ::intersect( - cclx, ccly, r3, - this->ps().x2(), this->ps().y2(), - this->ps().x3(), this->ps().y3() + 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) ); - double a31 = ::angle_between_three_points( - this->cc().rrx(), this->cc().rry(), - cclx, ccly, - std::get<1>(cli3), std::get<2>(cli3) + auto cli3 = ::intersect( + 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(), - cclx, ccly, - 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)) || (a1 < a3 && !std::get<0>(cli2)) || (a1 < a2 && a1 < a3) )) { - this->cc().rotate(cclx, ccly, -a1); + 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(cclx, ccly, -a2); + 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(cclx, ccly, -a3); + this->cc().rotate(ccrx, ccry, -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) { + } 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 ccr_rr = edist( + ccrx, ccry, + this->cc().rrx(), this->cc().rry() ); - double a11 = ::angle_between_three_points( - this->cc().lrx(), this->cc().lry(), - ccrx, ccry, - std::get<1>(cli1), std::get<2>(cli1) + double ccr_lr = edist( + ccrx, ccry, + this->cc().lrx(), this->cc().lry() ); - double a12 = ::angle_between_three_points( - this->cc().lrx(), this->cc().lry(), - ccrx, ccry, - std::get<3>(cli1), std::get<4>(cli1) + double ccr_p1 = edist( + ccrx, ccry, + this->ps().x1(), this->ps().y1() ); - double a1 = std::min(a11, a12); + 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(), - double r2 = sqrt( - pow(this->cc().lfy() - ccry, 2) - + pow(this->cc().lfx() - ccrx, 2) + 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() ); - auto cli2 = ::intersect( - ccrx, ccry, r2, - this->ps().x3(), this->ps().y3(), - this->ps().x4(), this->ps().y4() + double ccl_lf = edist( + cclx, ccly, + this->cc().lfx(), this->cc().lfy() ); - double a21 = ::angle_between_three_points( - this->cc().rrx(), this->cc().rry(), - ccrx, ccry, - std::get<1>(cli2), std::get<2>(cli2) + { + 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 a22 = ::angle_between_three_points( - this->cc().rrx(), this->cc().rry(), - ccrx, ccry, - std::get<3>(cli2), std::get<4>(cli2) + 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) ); - double a2 = std::min(a21, a22); - - double r3 = sqrt( - pow(this->cc().rfy() - ccry, 2) - + pow(this->cc().rfx() - ccrx, 2) + auto cli2 = ::intersect( + cclx, ccly, ccl_lf, + this->ps().x2(), this->ps().y2(), + this->ps().x3(), this->ps().y3() ); - auto cli3 = ::intersect( - ccrx, ccry, r3, - this->ps().x3(), this->ps().y3(), - this->ps().x2(), this->ps().y2() + 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) ); - double a31 = ::angle_between_three_points( - this->cc().rrx(), this->cc().rry(), - ccrx, ccry, - std::get<1>(cli3), std::get<2>(cli3) + auto cli3 = ::intersect( + cclx, ccly, ccl_rf, + 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().rfx(), this->cc().rfy(), + cclx, ccly, + 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)) || (a1 < a3 && !std::get<0>(cli2)) || (a1 < a2 && a1 < a3) )) { - this->cc().rotate(ccrx, ccry, -a1); + 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(ccrx, ccry, -a2); + 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(ccrx, ccry, -a3); + this->cc().rotate(cclx, ccly, a3); } else { continue; }