IntersectionFunctions.hpp

IntersectionFunctions.hpp

#pragma once

#include <vector>
#include <cmath>

#include "Vector2.hpp"

namespace IntersectionFunctions
{
	struct IntersectionObject
	{
		std::vector<Vector2> points;

		void InsertSolution(float x, float y)
		{
			points.push_back(Vector2(x, y));
		}

		void InsertSolution(Vector2 v)
		{
			points.push_back(v);
		}

		int NumberOfSolutions()
		{
			return points.size();
		}
	};

	// Circle to Circle
	IntersectionObject CircleToCircleIntersection(Vector2 circlePosition1, float radius1,
	                                              Vector2 circlePosition2, float radius2)
	{
		IntersectionObject result;
		float a, distSq, dist, h;
		Vector2 d, r, v2;

		// d is the vertical and horizontal distances between the circle centers
		d = circlePosition1 - circlePosition2;

		//distance squared between the circles
		distSq = d.LengthSq();

		// Check for equality and infinite intersections exist
		if (distSq == 0 && radius1 == radius2)
		{
			return result;
		}

		//Check for solvability
		if (distSq > (radius1 + radius2) * (radius1 + radius2))
		{
			// no solution. circles do not intersect
			return result;
		}

		if (distSq < abs(radius1 - radius2) * abs(radius1 - radius2))
		{
			// no solution. one circle is contained in the other
			return result;
		}
		
		if (distSq == (radius1 + radius2) * (radius1 + radius2))
		{
			//one solution
			result.InsertSolution((circlePosition1 - circlePosition2) / (radius1 + radius2) * radius1 + circlePosition1);
			return result;
		}

		dist = sqrt(distSq);

		// 'point 2' is the point where the line through the circle
		// intersection points crosses the line between the circle
		// centers.

		// Determine the distance from point 0 to point 2
		a = ((radius1 * radius1) - (radius2 * radius2) + distSq) / (2.0f * dist);

		// Determine the coordinates of point 2
		v2 = circlePosition1 + d * (a / dist);

		// Determine the distance from point 2 to either of the intersection points
		h = sqrt((radius1 * radius1) - (a * a));

		// Now determine the offsets of the intersection points from point 2
		r = d * (h / dist);
		r = r.Normal();

		// Determine the absolute intersection points
		result.InsertSolution(v2 + r);
		result.InsertSolution(v2 - r);

		return result;
	}

	// Used only within this namespace
	int PrivateLineToCircleIntersection(Vector2 vertex1, Vector2 vertex2,
	                                    Vector2 circlePosition, float radius,
	                                    Vector2 & solution1, Vector2 & solution2)
	{
		// Vector from point 1 to point 2
		Vector2 vertex1to2 = vertex2 - vertex1;
		// Vector from point 1 to the circle's center
		Vector2 circleToVertex1 = circlePosition - vertex1;

		float dot = vertex1to2.Dot(circleToVertex1);
		Vector2 proj1 = vertex1to2 * (dot / vertex1to2.LengthSq());

		Vector2 midpt = vertex1 + proj1;
		Vector2 circleToMidpt = midpt - circlePosition;
		float distSqToCenter = circleToMidpt.LengthSq();
		if (distSqToCenter > radius * radius) return 0;
		if (distSqToCenter == radius * radius)
		{
			solution1 = midpt;
			return 1;
		}
		float distToIntersection;
		if (distSqToCenter == 0)
		{
			distToIntersection = radius;
		}
		else
		{
			distToIntersection = sqrt(radius * radius - distSqToCenter);
		}
		vertex1to2 = vertex1to2.Normalize();
		vertex1to2 *= distToIntersection;

		solution1 = midpt + vertex1to2;
		solution2 = midpt - vertex1to2;
		return 2;
	}

	//Line to Circle
	IntersectionObject LineToCircleIntersection(Vector2 vertex1, Vector2 vertex2,
	                                            Vector2 circlePosition, float radius)
	{
		IntersectionObject result;
		Vector2 solution1, solution2;
		switch(PrivateLineToCircleIntersection(vertex1, vertex2, circlePosition, radius, solution1, solution2))
		{
		case 2:
			result.InsertSolution(solution2);
		case 1:
			result.InsertSolution(solution1);
			break;
		}
		return result;
	}

	// Circle to Line
	IntersectionObject CircleToLineIntersection(Vector2 circlePosition, float radius,
	                                            Vector2 vertex1, Vector2 vertex2)
	{
		return LineToCircleIntersection(vertex1, vertex2, circlePosition, radius);
	}

	// LineSegment to Circle
	IntersectionObject LineSegmentToCircleIntersection(Vector2 vertex1, Vector2 vertex2,
	                                                   Vector2 circlePosition, float radius)
	{
		IntersectionObject result;
		Vector2 solution1, solution2;
		Vector2 vertex1to2 = vertex2 - vertex1;
		Vector2 vertex1ToSolution1, vertex2ToSolution1, vertex1ToSolution2, vertex2ToSolution2;
		switch(PrivateLineToCircleIntersection(vertex1, vertex2, circlePosition, radius, solution1, solution2))
		{
		case 2:
			vertex1ToSolution2 = solution2 - vertex1;
			vertex2ToSolution2 = solution2 - vertex2;
			if (vertex1ToSolution2.Dot(vertex1to2) > 0 &&
				vertex2ToSolution2.Dot(vertex1to2) < 0)
			{
				result.InsertSolution(solution2);
			}
		case 1:
			vertex1ToSolution1 = solution1 - vertex1;
			vertex2ToSolution1 = solution1 - vertex2;
			if (vertex1ToSolution1.Dot(vertex1to2) > 0 &&
				vertex2ToSolution1.Dot(vertex1to2) < 0)
			{
				result.InsertSolution(solution1);
			}
			break;
		}
		return result;
	}

	// Circle to LineSegment
	IntersectionObject CircleToLineSegmentIntersection(Vector2 circlePosition, float radius,
	                                                   Vector2 vertex1, Vector2 vertex2)
	{
		return LineSegmentToCircleIntersection(vertex1, vertex2, circlePosition, radius);
	}

	// Ray to Circle
	IntersectionObject RayToCircleIntersection(Vector2 vertex1, Vector2 vertex2,
	                                           Vector2 circlePosition, float radius)
	{
		IntersectionObject result;
		Vector2 solution1, solution2;
		Vector2 vertex1to2 = vertex2 - vertex1;
		Vector2 vertex1ToSolution1, vertex1ToSolution2; 
		switch(PrivateLineToCircleIntersection(vertex1, vertex2, circlePosition, radius, solution1, solution2))
		{
		case 2:
			vertex1ToSolution2 = solution2 - vertex1;
			if (vertex1ToSolution2.Dot(vertex1to2) > 0)
			{
				result.InsertSolution(solution2);
			}
		case 1:
			vertex1ToSolution1 = solution1 - vertex1;
			if (vertex1ToSolution1.Dot(vertex1to2) > 0)
			{
				result.InsertSolution(solution1);
			}
			break;
		}
		return result;
	}

	// Circle to Ray
	IntersectionObject CircleToRayIntersection(Vector2 circlePosition, float radius,
	                                           Vector2 vertex1, Vector2 vertex2)
	{
		return RayToCircleIntersection(vertex1, vertex2, circlePosition, radius);
	}

	// Used only within this namespace
	bool PrivateLineToLineIntersection(Vector2 vertex1, Vector2 vertex2,
	                                   Vector2 vertex3, Vector2 vertex4, float & r, float & s)
	{
		float d;
		//Make sure the lines aren't parallel
		Vector2 vertex1to2 = vertex2 - vertex1;
		Vector2 vertex3to4 = vertex4 - vertex3;
		//if (vertex1to2.x * -vertex3to4.y + vertex1to2.y * vertex3to4.x != 0)
		//{
		if(vertex1to2.y / vertex1to2.x != vertex3to4.y / vertex3to4.x)
		{
			d = vertex1to2.x * vertex3to4.y - vertex1to2.y * vertex3to4.x;
			if (d != 0)
			{
				Vector2 vertex3to1 = vertex1 - vertex3;
				r = (vertex3to1.y * vertex3to4.x - vertex3to1.x * vertex3to4.y) / d;
				s = (vertex3to1.y * vertex1to2.x - vertex3to1.x * vertex1to2.y) / d;
				return true;
			}
		}
		return false;
	}

	// Line to Line
	IntersectionObject LineToLineIntersection(Vector2 vertex1, Vector2 vertex2,
	                                          Vector2 vertex3, Vector2 vertex4)
	{
		IntersectionObject result;
		float r, s;
		if(PrivateLineToLineIntersection(vertex1, vertex2, vertex3, vertex4, r, s))
		{
			result.InsertSolution(vertex1 + (vertex2 - vertex1) * r);
		}
		return result;
	}

	// LineSegment to LineSegment
	IntersectionObject LineSegmentToLineSegmentIntersection(Vector2 vertex1, Vector2 vertex2,
	                                                        Vector2 vertex3, Vector2 vertex4)
	{
		IntersectionObject result;
		float r, s;
		if(PrivateLineToLineIntersection(vertex1, vertex2, vertex3, vertex4, r, s))
		{
			if (r >= 0 && r <= 1)
			{
				if (s >= 0 && s <= 1)
				{
					result.InsertSolution(vertex1 + (vertex2 - vertex1) * r);
				}
			}
		}
		return result;
	}

	// LineSegment to Line
	IntersectionObject LineSegmentToLineIntersection(Vector2 vertex1, Vector2 vertex2,
	                                                 Vector2 vertex3, Vector2 vertex4)
	{
		IntersectionObject result;
		float r, s;
		if(PrivateLineToLineIntersection(vertex1, vertex2, vertex3, vertex4, r, s))
		{
			if (r >= 0 && r <= 1)
			{
				result.InsertSolution(vertex1 + (vertex2 - vertex1) * r);
			}
		}
		return result;
	}

	// Line to LineSement
	IntersectionObject LineToLineSegmentIntersection(Vector2 vertex1, Vector2 vertex2,
	                                                 Vector2 vertex3, Vector2 vertex4)
	{
		return LineSegmentToLineIntersection(vertex3, vertex4, vertex1, vertex2);
	}

	// Ray to LineSegment
	IntersectionObject RayToLineSegmentIntersection(Vector2 vertex1, Vector2 vertex2,
	                                                Vector2 vertex3, Vector2 vertex4)
	{
		IntersectionObject result;
		float r, s;
		if(PrivateLineToLineIntersection(vertex1, vertex2, vertex3, vertex4, r, s))
		{
			if (r >= 0)
			{
				if (s >= 0 && s <= 1)
				{
					result.InsertSolution(vertex1 + (vertex2 - vertex1) * r);
				}
			}
		}
		return result;
	}

	// LineSegment to Ray
	IntersectionObject LineSegmentToRayIntersection(Vector2 vertex1, Vector2 vertex2,
	                                                Vector2 vertex3, Vector2 vertex4)
	{
		return RayToLineSegmentIntersection(vertex3, vertex4, vertex1, vertex2);
	}

	// Ray to Line
	IntersectionObject RayToLineIntersection(Vector2 vertex1, Vector2 vertex2,
	                                         Vector2 vertex3, Vector2 vertex4)
	{
		IntersectionObject result;
		float r, s;
		if(PrivateLineToLineIntersection(vertex1, vertex2, vertex3, vertex4, r, s))
		{
			if (r >= 0)
			{
				result.InsertSolution(vertex1 + (vertex2 - vertex1) * r);
			}
		}
		return result;
	}

	// Line to Ray
	IntersectionObject LineToRayIntersection(Vector2 vertex1, Vector2 vertex2,
	                                         Vector2 vertex3, Vector2 vertex4)
	{
		return RayToLineIntersection(vertex3, vertex4, vertex1, vertex2);
	}

	// Ray to Ray
	IntersectionObject RayToRayIntersection(Vector2 vertex1, Vector2 vertex2,
	                                        Vector2 vertex3, Vector2 vertex4)
	{
		IntersectionObject result;
		float r, s;
		if(PrivateLineToLineIntersection(vertex1, vertex2, vertex3, vertex4, r, s))
		{
			if (r >= 0)
			{
				if (s >= 0)
				{
					result.InsertSolution(vertex1 + (vertex2 - vertex1) * r);
				}
			}
		}
		return result;
	}
}