// static
LineIntersection::IntersectionResult LineIntersection::raySegmentIntersection( const Vector2f& rayOrigin, const Vector2f& rayDirection,
const Vector2f& segmentBegin, const Vector2f& segmentEnd,
Vector2f* intersectionPoint )
{
float tp;
float tq;
// first intersect the lines
Vector2f p0 = rayOrigin;
Vector2f p1 = rayOrigin + rayDirection; // find two points on the ray
Vector2f q0 = segmentBegin;
Vector2f q1 = segmentEnd;
IntersectionResult result = lineLineIntersection( p0, p1, q0, q1, &tp, &tq );
if( result == INTERESECTING )
{
if( tq >= 0.f && tq <= 1.f && tp > 0.f )
{
intersectionPoint->x = q0.x + tq * ( q1.x - q0.x );
intersectionPoint->y = q0.y + tq * ( q1.y - q0.y );
return INTERESECTING;
}
}
return NOT_INTERESECTING;
}
// static
LineIntersection::IntersectionResult LineIntersection::segmentSegmentIntersection( const Vector2f& p0, const Vector2f& p1,
const Vector2f& q0, const Vector2f& q1,
Vector2f* intersectionPoint )
{
float tp;
float tq;
IntersectionResult result = lineLineIntersection( p0, p1, q0, q1, &tp, &tq );
if( result == INTERESECTING )
{
if( tp >= 0.0f && tp <= 1.0f && tq >= 0.0f && tq <= 1.0f )
{
// get the intersection point
intersectionPoint->x = p0.x + tp * ( p1.x - p0.x );
intersectionPoint->y = p0.y + tp * ( p1.y - p0.y );
return INTERESECTING;
}
}
return NOT_INTERESECTING;
}
bool Obstacles::linePolygonIntersection(LineSegment lineA, VertexType lineAType, Polygon *polyB, Vector2 *intersectionPoint, int *intersectionIndex) {
bool hasIntersection = false;
float nearestIntersectionDistance = 0.0f;
for (int i = 0; i != polyB->verticeCount; ++i) {
LineSegment lineB;
lineB.start = polyB->vertices[i];
lineB.end = polyB->vertices[(i+1) % polyB->verticeCount];
VertexType lineBType = polyB->vertexType[i];
Vector2 newIntersectionPoint;
if (lineLineIntersection(lineA, lineB, &newIntersectionPoint)) {
if ((lineAType == TOP_RIGHT && lineBType == TOP_LEFT)
|| (lineAType == BOTTOM_RIGHT && lineBType == TOP_RIGHT)
|| (lineAType == BOTTOM_LEFT && lineBType == BOTTOM_RIGHT)
|| (lineAType == TOP_LEFT && lineBType == BOTTOM_LEFT)
) {
if (!WITHIN_TOLERANCE(lineB.end.x, intersectionPoint->x)
|| !WITHIN_TOLERANCE(lineB.end.y, intersectionPoint->y)) {
if (newIntersectionPoint != *intersectionPoint) {
float newIntersectionDistance = getLength(lineA.start.x, lineA.start.y, newIntersectionPoint.x, newIntersectionPoint.y);
if (!hasIntersection || newIntersectionDistance < nearestIntersectionDistance) {
hasIntersection = true;
nearestIntersectionDistance = newIntersectionDistance;
*intersectionPoint = newIntersectionPoint;
*intersectionIndex = i;
}
}
}
}
}
}
return hasIntersection;
}
