Moved raytracing code into its own folder

Raytrace/TriangleMesh.py

@@ -0,0 +1,117 @@
+import numpy as np
+
+
+class Ray:
+    direction:np.ndarray
+    origin:np.ndarray
+    inverse_direction:np.ndarray
+    def __init__(self, direction:np.ndarray, origin:np.ndarray) -> None:
+        self.direction = direction
+        self.origin = origin
+        old_error_state = np.seterr(divide='ignore', invalid='ignore')
+        self.inverse_direction = np.ones(3) / direction
+        np.seterr(**old_error_state)
+
+class TriangleMesh:
+    # An n by 3 by 3 array of floats representing a list of triangles in the form triangle_index, vertex_index, axis_index
+    triangles:np.ndarray
+
+    def __init__(self, triangles: np.ndarray|None = None, stl_path: str|None = None):
+        if not stl_path is None:
+            import stl
+            mesh = stl.Mesh.from_file(stl_path)
+            triangles = np.array([mesh.v0, mesh.v1, mesh.v2]).swapaxes(0, 1)
+        self.triangles = np.ndarray((0,3), float) if triangles is None else triangles
+
+    def raytrace(self, ray:Ray) -> float:
+        return self.array_raytrace(ray)
+
+    def simple_raytrace(self, ray:Ray) -> float:
+        best_dist = np.inf
+
+        for triangle in self.triangles:
+            dist = self.triangle_ray_intersection(
+                triangle, ray
+            )
+            if dist == -1:
+                continue
+            if dist < best_dist:
+                best_dist = dist
+        return best_dist
+    def array_raytrace(self, ray:Ray) -> float:
+        out = self.batch_triangle_ray_intersection(self.triangles, ray)
+        out[out < 0] = np.inf
+
+        return np.min(out)
+
+    @staticmethod
+    def triangle_ray_intersection(triangle, ray:Ray, epsilon = 1e-10) -> float:
+        # Translated from https://en.wikipedia.org/wiki/M%C3%B6ller%E2%80%93Trumbore_intersection_algorithm#C++_implementation
+
+        v0, v1, v2 = triangle
+        edge1 = v1 - v0
+        edge2 = v2 - v0
+        ray_cross_e2 = np.cross(ray.direction, edge2)
+        det = np.dot(edge1, ray_cross_e2)
+
+        if -epsilon < det < epsilon:
+            return -1  # This ray is parallel to this triangle.
+
+        inv_det = 1.0 / det
+        s = ray.origin - v0
+        u = inv_det * np.dot(s, ray_cross_e2)
+
+        if u < -epsilon or 1 < u:
+            return -1
+
+        s_cross_e1 = np.cross(s, edge1)
+        v = inv_det * np.dot(ray.direction, s_cross_e1)
+
+        if v < -epsilon or 1 < u + v:
+            return -1
+
+        # At this stage we can compute t to find out where the intersection point is on the line.
+        t = inv_det * np.dot(edge2, s_cross_e1)
+
+        if t > epsilon:  # ray intersection
+            return t
+
+        else:  # This means that there is a line intersection but not a ray intersection.
+            return -1
+    @staticmethod
+    def batch_triangle_ray_intersection(triangle_array, ray:Ray, epsilon = 1e-10) -> np.ndarray:
+        # Translated from https://en.wikipedia.org/wiki/M%C3%B6ller%E2%80%93Trumbore_intersection_algorithm#C++_implementation
+
+        triangle_array_comp = triangle_array.swapaxes(0, 1)
+        v0, v1, v2 = triangle_array_comp[0], triangle_array_comp[1], triangle_array_comp[2]
+
+        # np.subtract(v1, v0, out= v1)
+        # np.subtract(v2, v0, out= v2)
+        v1 = v1 - v0
+        v2 = v2 - v0
+
+        ray_cross_e2 = np.cross(ray.direction, v2)
+        det = np.vecdot(v1, ray_cross_e2)
+        # if -epsilon < det < epsilon:
+        #     return None  # This ray is parallel to this triangle.
+        old_error_state = np.seterr(divide='ignore', invalid='ignore')
+        inv_det = 1.0 / det
+        s = ray.origin - v0
+        u = inv_det * np.vecdot(s, ray_cross_e2)
+
+        s_cross_e1 = np.cross(s, v1)
+        v = inv_det * np.vecdot(ray.direction, s_cross_e1)
+
+        valid = u < -epsilon
+        np.logical_or(valid, 1 < u, out=valid)
+        np.logical_or(valid, v < -epsilon, out=valid)
+        np.logical_or(valid, 1 < u + v, out=valid)
+        np.invert(valid, out=valid)
+
+        t = np.empty(triangle_array.shape[0], dtype=np.float64)
+        t.fill(-1)
+
+        np.multiply(inv_det, np.vecdot(v2, s_cross_e1), out = t, where = valid)
+
+        np.seterr(**old_error_state)
+        return t