// Intersect a square with center at (x,y), orientation theta, and the given side length with the set of rectangles.
// If the square lies outside of all obstacles, return true
bool isValidSquare(double x, double y, double theta, double sideLength, const std::vector <Rectangle> &obstacles) {
double halfLength = sideLength / 2;
double upperX, upperY, lowerX, lowerY;
double blX, blY;
double tlX, tlY;
double trX, trY;
double brX, brY;
upperX = halfLength;
upperY = halfLength;
lowerX = -halfLength;
lowerY = -halfLength;
//rotate the square, calculate bottom left X, bottom left Y, top left X, etc...
blX = rotatedX(lowerX, lowerY, theta, x, y);
blY = rotatedY(lowerX, lowerY, theta, x, y);
tlX = rotatedX(lowerX, upperY, theta, x, y);
tlY = rotatedY(lowerX, upperY, theta, x, y);
trX = rotatedX(upperX, upperY, theta, x, y);
trY = rotatedY(upperX, upperY, theta, x, y);
brX = rotatedX(upperX, lowerY, theta, x, y);
brY = rotatedY(upperX, lowerY, theta, x, y);
int rectCount = -1;
for (Rectangle rect: obstacles) {
rectCount++;
// check if square's center is inside rectangle
if (between(rect.x, rect.x + rect.width, x) && between(rect.y, rect.y + rect.height, y))
return false;
// check if left side of robot intersects the rectangle
else if (checkLineToRect(blX, blY, tlX, tlY, rect))
return false;
// check top side
else if (checkLineToRect(tlX, tlY, trX, trY, rect))
return false;
// check bottom side
else if (checkLineToRect(blX, blY, brX, brY, rect))
return false;
// check right side
else if (checkLineToRect(brX, brY, trX, trY, rect))
return false;
}
return true;
}
//=============================================================================
// Compute corners of rotated box, projection edges and min and max projections
// 0---1 corner numbers
// | |
// 3---2
//=============================================================================
void Entity::computeRotatedBox()
{
if(rotatedBoxReady)
return;
float projection;
VECTOR2 rotatedX(cos(spriteData.angle), sin(spriteData.angle));
VECTOR2 rotatedY(-sin(spriteData.angle), cos(spriteData.angle));
const VECTOR2 *center = getCenter();
corners[0] = *center + rotatedX * ((float)edge.left*getScale()) +
rotatedY * ((float)edge.top*getScale());
corners[1] = *center + rotatedX * ((float)edge.right*getScale()) +
rotatedY * ((float)edge.top*getScale());
corners[2] = *center + rotatedX * ((float)edge.right*getScale()) +
rotatedY * ((float)edge.bottom*getScale());
corners[3] = *center + rotatedX * ((float)edge.left*getScale()) +
rotatedY * ((float)edge.bottom*getScale());
// corners[0] is used as origin
// The two edges connected to corners[0] are used as the projection lines
edge01 = VECTOR2(corners[1].x - corners[0].x, corners[1].y - corners[0].y);
graphics->Vector2Normalize(&edge01);
edge03 = VECTOR2(corners[3].x - corners[0].x, corners[3].y - corners[0].y);
graphics->Vector2Normalize(&edge03);
// this entities min and max projection onto edges
projection = graphics->Vector2Dot(&edge01, &corners[0]);
edge01Min = projection;
edge01Max = projection;
// project onto edge01
projection = graphics->Vector2Dot(&edge01, &corners[1]);
if (projection < edge01Min)
edge01Min = projection;
else if (projection > edge01Max)
edge01Max = projection;
// project onto edge03
projection = graphics->Vector2Dot(&edge03, &corners[0]);
edge03Min = projection;
edge03Max = projection;
projection = graphics->Vector2Dot(&edge03, &corners[3]);
if (projection < edge03Min)
edge03Min = projection;
else if (projection > edge03Max)
edge03Max = projection;
rotatedBoxReady = true;
}