Exemple #1
0
    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;
}