Geometric test whether ray intersects sphere

.gitignore

@@ -30,3 +30,6 @@ pip-log.txt
 
 # vi swap files
 *.swp
+
+# Pycharm ide
+*.idea

pyrr/geometric_tests.py

@@ -6,7 +6,7 @@
 import math
 import numpy as np
 from . import rectangle, vector, vector3
-from .utils import all_parameters_as_numpy_arrays, parameters_as_numpy_arrays
+from .utils import all_parameters_as_numpy_arrays, parameters_as_numpy_arrays, solve_quadratic_equation
 
 """
 TODO: line_intersect_plane
@@ -399,3 +399,41 @@ def sphere_penetration_sphere(s1, s2):
     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

pyrr/utils.py

@@ -74,3 +74,32 @@ def wrapper(*args, **kwargs):
             return fn(*args, **kwargs)
         return wrapper
     return decorator
+
+def solve_quadratic_equation(a, b, c):
+    """Quadratic equation solver.
+    Solve function of form f(x) = ax^2 + bx + c
+
+    :param float a: Quadratic part of equation.
+    :param float b: Linear part of equation.
+    :param float c: Static part of equation.
+    :rtype: list
+    :return: List contains either two elements for two solutions, one element for one solution, or is empty if
+        no solution for the quadratic equation exists.
+    """
+    delta = b * b - 4 * a * c
+    if delta > 0:
+        # Two solutions
+        # See https://www.scratchapixel.com/lessons/3d-basic-rendering/minimal-ray-tracer-rendering-simple-shapes/ray-sphere-intersection
+        # Why not use simple form:
+        # s1 = (-b + math.sqrt(delta)) / (2 * a)
+        # s2 = (-b - math.sqrt(delta)) / (2 * a)
+        q = -0.5 * (b + np.math.sqrt(delta)) if b > 0 else -0.5 * (b - np.math.sqrt(delta))
+        s1 = q / a
+        s2 = c / q
+        return [s1, s2]
+    elif delta == 0:
+        # One solution
+        return [-b / (2 * a)]
+    else:
+        # No solution exists
+        return list()

tests/test_geometric_tests.py

@@ -1,3 +1,5 @@
+from pyrr.geometric_tests import ray_intersect_sphere
+
 try:
     import unittest2 as unittest
 except:
@@ -259,6 +261,42 @@ def test_sphere_penetration_sphere_4(self):
         s2 = sphere.create([3.,0.,0.], 1.0)
         self.assertEqual(gt.sphere_penetration_sphere(s1, s2), 0.0)
 
+    def test_ray_intersect_sphere_no_solution_1(self):
+        r = ray.create([0, 2, 0], [1, 0, 0])
+        s = sphere.create([0, 0, 0], 1)
+        intersections = ray_intersect_sphere(r, s)
+        self.assertEqual(len(intersections), 0)
+
+    def test_ray_intersect_sphere_no_solution_2(self):
+        r = ray.create([0, 0, 0], [1, 0, 0])
+        s = sphere.create([0, 2, 0], 1)
+        intersections = ray_intersect_sphere(r, s)
+        self.assertEqual(len(intersections), 0)
+
+    def test_ray_intersect_sphere_one_solution_1(self):
+        r = ray.create([0, 0, 0], [1, 0, 0])
+        s = sphere.create([0, 0, 0], 1)
+        intersections = ray_intersect_sphere(r, s)
+        self.assertEqual(len(intersections), 1)
+        np.testing.assert_array_almost_equal(intersections[0], np.array([1, 0, 0]), decimal=2)
+
+    def test_ray_intersect_sphere_two_solutions_1(self):
+        r = ray.create([-2, 0, 0], [1, 0, 0])
+        s = sphere.create([0, 0, 0], 1)
+        intersections = ray_intersect_sphere(r, s)
+        self.assertEqual(len(intersections), 2)
+        np.testing.assert_array_almost_equal(intersections[0], np.array([1, 0, 0]), decimal=2)
+        np.testing.assert_array_almost_equal(intersections[1], np.array([-1, 0, 0]), decimal=2)
+
+    def test_ray_intersect_sphere_two_solutions_2(self):
+        r = ray.create([2.48, 1.45, 1.78], [-3.1, 0.48, -3.2])
+        s = sphere.create([1, 1, 0], 1)
+        intersections = ray_intersect_sphere(r, s)
+        self.assertEqual(len(intersections), 2)
+        np.testing.assert_array_almost_equal(intersections[0], np.array([0.44, 1.77, -0.32]), decimal=2)
+        np.testing.assert_array_almost_equal(intersections[1], np.array([1.41, 1.62, 0.67]), decimal=2)
+
+
 
 if __name__ == '__main__':
     unittest.main()