- if(std::get<0>(intersect(
- this->cc().lfx(), this->cc().lfy(),
- this->cc().lrx(), this->cc().lry(),
- this->ps().x1(), this->ps().y1(),
- this->ps().x2(), this->ps().y2()
- )))
- return true;
- if(std::get<0>(intersect(
- this->cc().rfx(), this->cc().rfy(),
- this->cc().rrx(), this->cc().rry(),
- this->ps().x1(), this->ps().y1(),
- this->ps().x2(), this->ps().y2()
- )))
- return true;
- if(std::get<0>(intersect(
- this->cc().lfx(), this->cc().lfy(),
- this->cc().rfx(), this->cc().rfy(),
- this->ps().x1(), this->ps().y1(),
- this->ps().x2(), this->ps().y2()
- )))
- return true;
- if(std::get<0>(intersect(
- this->cc().lrx(), this->cc().lry(),
- this->cc().rrx(), this->cc().rry(),
- this->ps().x1(), this->ps().y1(),
- this->ps().x2(), this->ps().y2()
- )))
- return true;
- if(std::get<0>(intersect(
- this->cc().lfx(), this->cc().lfy(),
- this->cc().lrx(), this->cc().lry(),
- this->ps().x2(), this->ps().y2(),
- this->ps().x3(), this->ps().y3()
- )))
- return true;
- if(std::get<0>(intersect(
- this->cc().rfx(), this->cc().rfy(),
- this->cc().rrx(), this->cc().rry(),
- this->ps().x2(), this->ps().y2(),
- this->ps().x3(), this->ps().y3()
- )))
- return true;
- if(std::get<0>(intersect(
- this->cc().lfx(), this->cc().lfy(),
- this->cc().rfx(), this->cc().rfy(),
- this->ps().x2(), this->ps().y2(),
- this->ps().x3(), this->ps().y3()
- )))
- return true;
- if(std::get<0>(intersect(
- this->cc().lrx(), this->cc().lry(),
- this->cc().rrx(), this->cc().rry(),
- this->ps().x2(), this->ps().y2(),
- this->ps().x3(), this->ps().y3()
- )))
- return true;
- if(std::get<0>(intersect(
- this->cc().lfx(), this->cc().lfy(),
- this->cc().lrx(), this->cc().lry(),
- this->ps().x3(), this->ps().y3(),
- this->ps().x4(), this->ps().y4()
- )))
- return true;
- if(std::get<0>(intersect(
- this->cc().rfx(), this->cc().rfy(),
- this->cc().rrx(), this->cc().rry(),
- this->ps().x3(), this->ps().y3(),
- this->ps().x4(), this->ps().y4()
- )))
- return true;
- if(std::get<0>(intersect(
- this->cc().lfx(), this->cc().lfy(),
- this->cc().rfx(), this->cc().rfy(),
- this->ps().x3(), this->ps().y3(),
- this->ps().x4(), this->ps().y4()
- )))
- return true;
- if(std::get<0>(intersect(
- this->cc().lrx(), this->cc().lry(),
- this->cc().rrx(), this->cc().rry(),
- this->ps().x3(), this->ps().y3(),
- this->ps().x4(), this->ps().y4()
- )))
+ std::vector<std::tuple<double, double>> 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<std::tuple<double, double>> 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;
+ 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)