static bool inCone(int i, int n, const int* verts, const int* pj)
{
const int* pi = &verts[i * 4];
const int* pi1 = &verts[next(i, n) * 4];
const int* pin1 = &verts[prev(i, n) * 4];
// If P[i] is a convex vertex [ i+1 left or on (i-1,i) ].
if (leftOn(pin1, pi, pi1))
return left(pi, pj, pin1) && left(pj, pi, pi1);
// Assume (i-1,i,i+1) not collinear.
// else P[i] is reflex.
return !(leftOn(pi, pj, pi1) && leftOn(pj, pi, pin1));
}
/*
* Pauses the car for one second
*/
void pauseBoth(){
stopRight();
stopLeft();
__delay_cycles(STRAIGHTTIME);
rightOn();
leftOn();
}
bool Polygon::canSee(int i, int j) const {
if(reflex(i)) {
if(leftOn(at(i), at(i - 1), at(j)) && rightOn(at(i), at(i + 1), at(j))) return false;
} else {
if(rightOn(at(i), at(i + 1), at(j)) || leftOn(at(i), at(i - 1), at(j))) return false;
}
if(reflex(j)) {
if(leftOn(at(j), at(j - 1), at(i)) && rightOn(at(j), at(j + 1), at(i))) return false;
} else {
if(rightOn(at(j), at(j + 1), at(i)) || leftOn(at(j), at(j - 1), at(i))) return false;
}
for(int k = 0; k < size(); ++k) {
if((k + 1) % size() == i || k == i || (k + 1) % size() == j || k == j) {
continue; // ignore incident edges
}
if(QLineF(at(i), at(j)).intersect(QLineF(at(k), at(k + 1)), NULL) == QLineF::BoundedIntersection) {
return false;
}
}
return true;
}
/*
* Sets the car up for any kind of right turn
*/
void turnRight(){
leftOn();
stopRight();
}
/*
* Makes both motors move forwards.
*/
void bothOn(){
rightOn();
leftOn();
}
bool Polygon::leftOn(int i) const {
return leftOn(at(i - 1), at(i), at(i + 1));
}
QList<Polygon> Polygon::convexPartition() const { // precondition: ccw; see mnbayazit.com/406/bayazit for details about how this works
QList<Polygon> list;
qreal d, dist1, dist2;
QPointF ip, ip1, ip2; // intersection points
int ind1, ind2;
Polygon poly1, poly2;
for(int i = 0; i < size(); ++i) {
if(reflex(i)) {
dist1 = dist2 = std::numeric_limits<qreal>::max();
for(int j = 0; j < size(); ++j) {
if(left(at(i - 1), at(i), at(j)) && rightOn(at(i - 1), at(i), at(j - 1))) { // if ray (i-1)->(i) intersects with edge (j, j-1)
QLineF(at(i - 1), at(i)).intersect(QLineF(at(j), at(j - 1)), &ip);
if(right(at(i + 1), at(i), ip)) { // intersection point isn't caused by backwards ray
d = sqdist(at(i), ip);
if(d < dist1) { // take the closest intersection so we know it isn't blocked by another edge
dist1 = d;
ind1 = j;
ip1 = ip;
}
}
}
if(left(at(i + 1), at(i), at(j + 1)) && rightOn(at(i + 1), at(i), at(j))) { // if ray (i+1)->(i) intersects with edge (j+1, j)
QLineF(at(i + 1), at(i)).intersect(QLineF(at(j), at(j + 1)), &ip);
if(left(at(i - 1), at(i), ip)) {
d = sqdist(at(i), ip);
if(d < dist2) {
dist2 = d;
ind2 = j;
ip2 = ip;
}
}
}
}
if(ind1 == (ind2 + 1) % size()) { // no vertices in range
QPointF sp((ip1 + ip2) / 2);
poly1 = copy(i, ind2);
poly1.append(sp);
poly2 = copy(ind1, i);
poly2.append(sp);
} else {
double highestScore = 0, bestIndex = ind1, score;
while(ind2 < ind1) ind2 += size();
for(int j = ind1; j <= ind2; ++j) {
if(canSee(i, j)) {
score = 1 / (sqdist(at(i), at(j)) + 1);
if(reflex(j)) {
if(rightOn(at(j - 1), at(j), at(i)) && leftOn(at(j + 1), at(j), at(i))) {
score += 3;
} else {
score += 2;
}
} else {
score += 1;
}
if(score > highestScore) {
bestIndex = j;
highestScore = score;
}
}
}
poly1 = copy(i, bestIndex);
poly2 = copy(bestIndex, i);
}
list += poly1.convexPartition();
list += poly2.convexPartition();
return list;
}
}
// polygon is already convex
if(size() > b2_maxPolygonVertices) {
poly1 = copy(0, size() / 2);
poly2 = copy(size() / 2, 0);
list += poly1.convexPartition();
list += poly2.convexPartition();
} else list.append(*this);
return list;
}