Sirisian

#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;
    }
}