# Source code for pyrr.geometric_tests

    # -*- coding: utf-8 -*-
    """Defines a number of functions to test interactions between
    various forms data types.
    """
    from __future__ import absolute_import, division, print_function
    import math
    import numpy as np
    from . import rectangle, vector, vector3, plane
    from .utils import all_parameters_as_numpy_arrays, parameters_as_numpy_arrays, solve_quadratic_equation

    """
    TODO: line_intersect_plane
    TODO: line_segment_intersect_plane
    TODO: ray_intersect_ray
    TODO: line_intersect_line
    TODO: line_segment_intersect_line_segment
    """

    @all_parameters_as_numpy_arrays
    def point_intersect_line(point, line):
        """Calculates the intersection point of a point and aline.

        Performed by checking if the cross-product
        of the point relative to the line is
        0.
        """
        rl = line[1] - line[0]
        rp = point - line[0]
        cross = vector3.cross(rl, rp)

        # check if the cross product is zero
        if np.count_nonzero(cross) > 0:
            return None
        return point

    @all_parameters_as_numpy_arrays
    def point_intersect_line_segment(point, line):
        """Calculates the intersection point of a point and a line segment.

        Performed by checking if the cross-product
        of the point relative to the line is
        0 and if the dot product of the point
        relative to the line start AND the end
        point relative to the line start is
        less than the segment's squared length.
        """
        rl = line[1] - line[0]
        rp = point - line[0]
        cross = vector3.cross(rl, rp)
        dot = vector.dot(rp, rl)
        squared_length = vector.squared_length(rl)

        if np.count_nonzero(cross) > 0:
            return None

        if dot < 0.0 or dot > squared_length:
            return None
        return point

    @all_parameters_as_numpy_arrays
    def point_intersect_rectangle(point, rect):
        """Calculates the intersection point of a point and a 2D rectangle.

        For 3D points, the Z axis will be ignored.

        :return: Returns True if the point is touching
        or within the rectangle.
        """
        left, right, bottom, top = rectangle.bounds(rect)
        if \
            point[0] < left or \
            point[0] > right or \
            point[1] < bottom or \
            point[1] > top:
            return None
        return point

    @parameters_as_numpy_arrays('ray', 'pl')
    def ray_intersect_plane(ray, pl, front_only=False):
        """Calculates the intersection point of a ray and a plane.

        :param numpy.array ray: The ray to test for intersection.
        :param numpy.array pl: The plane to test for intersection.
        :param boolean front_only: Specifies if the ray should
        only hit the front of the plane.
        Collisions from the rear of the plane will be
        ignored.

        :return The intersection point, or None
        if the ray is parallel to the plane.
        Returns None if the ray intersects the back
        of the plane and front_only is True.
        """
        """
        Distance to plane is defined as
        t = (pd - p0.n) / rd.n
        where:
        rd is the ray direction
        pd is the point on plane . plane normal
        p0 is the ray position
        n is the plane normal

        if rd.n == 0, the ray is parallel to the
        plane.
        """
        p = plane.position(pl)
        n = plane.normal(pl)
        rd_n = vector.dot(ray[1], n)

        if rd_n == 0.0:
            return None

        if front_only == True:
            if rd_n >= 0.0:
                return None

        pd = vector.dot(p, n)
        p0_n = vector.dot(ray[0], n)
        t = (pd - p0_n) / rd_n
        return ray[0] + (ray[1] * t)

    @all_parameters_as_numpy_arrays
    def point_closest_point_on_ray(point, ray):
        """Calculates the point on a ray that is closest to a point.

        :param numpy.array point: The point to check with.
        :param numpy.array ray: The ray to check against.
        :rtype: numpy.array
        :return: The closest point on the ray to the point.
        """
        """
        t = (p - rp).n
        cp = rp + (n * t)
        where
        p is the point
        rp is the ray origin
        n is the ray normal of unit length
        t is the distance along the ray to the point
        """
        normalized_n = vector.normalize(ray[1])
        relative_point = (point - ray[0])
        t = vector.dot(relative_point, normalized_n)
        return ray[0] + (normalized_n * t)

    @all_parameters_as_numpy_arrays
    def point_closest_point_on_line(point, line):
        """Calculates the point on the line that is closest to
        the specified point.

        :param numpy.array point: The point to check with.
        :param numpy.array line: The line to check against.
        :rtype: numpy.array
        :return: The closest point on the line to the point.
        """
        """
        rl = va->b (relative line)
        rp = va->p (relative point)
        u' = u / |u| (normalize)
        cp = a + (u' * (u'.v))
        where:
        a = line start
        b = line end
        p = point
        cp = closest point
        """
        rl = line[1] - line[0]
        rp = point - line[0]
        rl = vector.normalize(rl)
        dot = vector.dot(rl, rp)
        return line[0] + (rl * dot)

    @all_parameters_as_numpy_arrays
    def point_closest_point_on_line_segment(point, segment):
        """Calculates the point on the line segment that is closest
        to the specified point.

        This is similar to point_closest_point_on_line, except this
        is against the line segment of finite length. Whereas point_closest_point_on_line
        checks against a line of infinite length.

        :param numpy.array point: The point to check with.
        :param numpy.array line_segment: The finite line segment to check against.
        :rtype: numpy.array
        :return: The closest point on the line segment to the point.
        """
        # check if the line has any length
        rl = segment[1] - segment[0]
        squared_length = vector.squared_length(rl)
        if squared_length == 0.0:
            return segment[0]

        rp = point - segment[0]
        # check that / squared_length is correct
        dot = vector.dot(rp, rl) / squared_length;

        if dot < 0.0:
            return segment[0]
        elif dot > 1.0:
            return segment[1]

        # within segment
        # perform the same calculation as closest_point_on_line
        return segment[0] + (rl * dot)

    @all_parameters_as_numpy_arrays
    def vector_parallel_vector(v1, v2):
        """Checks if two vectors are parallel.

        :param numpy.array v1, v2: The vectors to check.
        :rtype: boolean
        :return: Returns True if the two vectors are parallel.
        """
        # we cross product the 2 vectors
        # if the result is 0, then they are parallel
        cross = vector3.cross(v1, v2)
        return 0 == np.count_nonzero(cross)

    @all_parameters_as_numpy_arrays
    def ray_parallel_ray(ray1, ray2):
        """Checks if two rays are parallel.

        :param numpy.array ray1, ray2: The rays to check.
        :rtype: boolean
        :return: Returns True if the two rays are parallel.
        """
        # we use a cross product in-case the ray direction
        # isn't unit length
        return vector_parallel_vector(ray1[ 1 ], ray2[ 1 ])

    @all_parameters_as_numpy_arrays
    def ray_coincident_ray(ray1, ray2):
        """Check if rays are coincident.

        Rays must not only be parallel to each other, but reside
        along the same vector.

        :param numpy.array ray1, ray2: The rays to check.
        :rtype: boolean
        :return: Returns True if the two rays are co-incident.
        """
        # ensure the ray's directions are the same
        if ray_parallel_ray(ray1, ray2):
            # get the delta between the two ray's start point
            delta = ray2[0] - ray1[0]

            # get the cross product of the ray delta and
            # the direction of the rays
            cross = vector3.cross(delta, ray2[1])

            # if the cross product is zero, the start of the
            # second ray is in line with the direction of the
            # first ray
            if np.count_nonzero(cross) > 0:
                return False

            return True
        return False

    @all_parameters_as_numpy_arrays
    def ray_intersect_aabb(ray, aabb):
        """Calculates the intersection point of a ray and an AABB

        :param numpy.array ray1: The ray to check.
        :param numpy.array aabb: The Axis-Aligned Bounding Box to check against.
        :rtype: numpy.array
        :return: Returns a vector if an intersection occurs.
            Returns None if no intersection occurs.
        """
        """
        http://gamedev.stackexchange.com/questions/18436/most-efficient-aabb-vs-ray-collision-algorithms
        """
        # this is basically "numpy.divide( 1.0, ray[ 1 ] )"
        # except we're trying to avoid a divide by zero warning
        # so where the ray direction value is 0.0, just use infinity
        # which is what we want anyway
        direction = ray[1]
        dir_fraction = np.empty(3, dtype = ray.dtype)
        dir_fraction[direction == 0.0] = np.inf
        dir_fraction[direction != 0.0] = np.divide(1.0, direction[direction != 0.0])

        t1 = (aabb[0,0] - ray[0,0]) * dir_fraction[ 0 ]
        t2 = (aabb[1,0] - ray[0,0]) * dir_fraction[ 0 ]
        t3 = (aabb[0,1] - ray[0,1]) * dir_fraction[ 1 ]
        t4 = (aabb[1,1] - ray[0,1]) * dir_fraction[ 1 ]
        t5 = (aabb[0,2] - ray[0,2]) * dir_fraction[ 2 ]
        t6 = (aabb[1,2] - ray[0,2]) * dir_fraction[ 2 ]

        tmin = max(min(t1, t2), min(t3, t4), min(t5, t6))
        tmax = min(max(t1, t2), max(t3, t4), max(t5, t6))

        # if tmax < 0, ray (line) is intersecting AABB
        # but the whole AABB is behind the ray start
        if tmax < 0:
            return None

        # if tmin > tmax, ray doesn't intersect AABB
        if tmin > tmax:
            return None

        # t is the distance from the ray point
        # to intersection

        t = min(x for x in [tmin, tmax] if x >= 0)
        point = ray[0] + (ray[1] * t)
        return point

    @all_parameters_as_numpy_arrays
    def point_height_above_plane(point, pl):
        """Calculates how high a point is above a plane.

        :param numpy.array point: The point to check.
        :param numpy.array plane: The plane to check.
        :rtype: float
        :return: The height above the plane as a float. The value will be
            negative if the point is behind the plane.
        """
        """
        Because we store normalised normal, we can simply
        use: n . (p - p0)
        where:
            n is the plane normal
            p is the plane position
            p0 is the point
        """
        return vector.dot(plane.normal(pl), point - plane.position(pl))

    @all_parameters_as_numpy_arrays
    def point_closest_point_on_plane(point, pl):
        """Calculates the point on a plane that is closest to a point.

        :param numpy.array point: The point to check with.
        :param numpy.array plane: The infinite plane to check against.
        :rtype: numpy.array
        :return: The closest point on the plane to the point.
        """
        """
        point on plane is defined as:
        q' = q + (d - q.n)n
        where:
        q' is the point on the plane
        q is the point we are checking
        d is the value of normal dot position
        n is the plane normal
        """
        n = plane.normal(pl)
        p = n * plane.distance(pl)
        d = np.dot(p, n)
        qn = np.dot(point, n)
        return point + (n * (d - qn))

    @all_parameters_as_numpy_arrays
    def sphere_does_intersect_sphere(s1, s2):
        """Checks if two spheres overlap.

        Note: This will return True if the two spheres are
        touching perfectly but sphere_penetration_sphere
        will return 0.0 as the touch but don't penetrate.

        This is faster than circle_penetrate_amount_circle
        as it avoids a square root calculation.

        :param numpy.array s1: The first circle.
        :param numpy.array s2: The second circle.
        :rtype: boolean
        :return: Returns True if the circles overlap.
            Otherwise, returns False.
        """
        delta = s2[:3] - s1[:3]
        distance_squared = vector.squared_length(delta)

        radii_squared = math.pow(s1[3] + s2[3], 2.0)

        if distance_squared > radii_squared:
            return False
        return True

    @all_parameters_as_numpy_arrays
    def sphere_penetration_sphere(s1, s2):
        """Calculates the distance two spheres have penetrated
        into one another.

        :param numpy.array s1: The first circle.
        :param numpy.array s2: The second circle.
        :rtype: float
        :return: The total overlap of the two spheres.
            This is essentially:
            r1 + r2 - distance
            Where r1 and r2 are the radii of circle 1 and 2
            and distance is the length of the vector p2 - p1.
            Will return 0.0 if the circles do not overlap.
        """
        delta = s2[:3] - s1[:3]
        distance = vector.length(delta)

        combined_radii = s1[3] + s2[3]
        penetration = combined_radii - distance

        if penetration <= 0.0:
            return 0.0
        return penetration

    @all_parameters_as_numpy_arrays
    def ray_intersect_sphere(ray, sphere):
        """ Returns the intersection points of a ray and a sphere.
        See: https://www.scratchapixel.com/lessons/3d-basic-rendering/minimal-ray-tracer-rendering-simple-shapes/ray-sphere-intersection
        The ray is defined via the following equation O+tD. Where O is the origin point and D is a direction vector.
        A sphere is defined as |P−C|^2=R2 where P is the origin and C is the center of the sphere.
        R is the radius of the sphere.

        Args:
            ray: Ray geometry
            sphere: Sphere geometry

        Returns:
            list: Intersection points as 3D vector list

        :param numpy.array ray: Ray parameter.
        :param numpy.array sphere: Sphere parameter.
        :rtype: float
        :return: Intersection points as a list of points.
        """
        sphere_center = sphere[:3]
        sphere_radius = sphere[3]
        ray_origin = ray[0]
        ray_direction = ray[1]

        a = 1
        b = 2 * np.dot(ray_direction, (ray_origin - sphere_center))
        c = np.dot(ray_origin - sphere_center, ray_origin - sphere_center) - sphere_radius * sphere_radius

        t_list = solve_quadratic_equation(a, b, c)

        ret = list()
        for t in t_list:
            # We are calculating intersection for ray not line! Use only positive t for ray.
            if t >= 0:
                ret.append(ray_origin + ray_direction * t)
        return ret