State Dubins::getState(myFloat len) const { Arc path1 = getFirstArc(); myFloat l1 = path1.getLength(); if (len < l1) { return path1.getState(len); } myFloat l2; if (isCCC) { Arc ca = getCenterArc(); l2 = ca.getLength(); if (len < l1 + l2) { return ca.getState(len - l1); } } else { Line cl = getCenter(); l2 = cl.getLength(); if (len < l1 + l2) { return cl.getState(len - l1); } } Arc path3 = getSecondArc(); return path3.getState(len - l1 - l2); }
bool Polygon::intersectBool(const Arc& arc) const { myFloat rad = arc.getLength() / 2 + maxRadius; Point arcCenter = (arc.position.point + arc.getEnd().point) / 2; if ((arcCenter - center).length() > rad) { return false; } Point last = pnts.back(); for (Point act : pnts) { if (!arc.intersectionPoint(Line(last, act)).position.invalid()) { return true; } last = act; } return false; }