Merge branch 'main' of https://github.uconn.edu/tjs20011/SDP-37-Model

raytrace.py

@@ -0,0 +1,122 @@
+import numpy as np
+import stl
+# TODO: Documentation
+
+
+def load_triangle_from_stl(path):
+    mesh = stl.Mesh.from_file(path)
+    triangle_list = np.array([mesh.v0, mesh.v1, mesh.v2]).swapaxes(0, 1)
+    return triangle_list
+
+
+def simple_raytrace(triangle_list, direction_vector, camera_origin):
+    best_dist = float('inf')
+
+    for triangle in triangle_list:
+        dist = triangle_ray_intersection(
+            triangle, direction_vector, camera_origin
+        )
+        if dist == -1:
+            continue
+        if dist < best_dist:
+            best_dist = dist
+    return best_dist
+def array_raytrace(triangle_list, direction_vector, camera_origin):
+    out = batch_triangle_ray_intersection(triangle_list, direction_vector, camera_origin)
+    out[out < 0] = np.inf
+    return np.min(out)
+
+def triangle_ray_intersection(triangle, ray, ray_origin = np.zeros(3)):
+    # Translated from https://en.wikipedia.org/wiki/M%C3%B6ller%E2%80%93Trumbore_intersection_algorithm#C++_implementation
+    epsilon = 1e-10
+
+    v0, v1, v2 = triangle
+    edge1 = v1 - v0
+    edge2 = v2 - v0
+    ray_cross_e2 = np.cross(ray, 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, 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
+def batch_triangle_ray_intersection(triangle_array, ray, ray_origin = np.zeros(3)):
+    # Translated from https://en.wikipedia.org/wiki/M%C3%B6ller%E2%80%93Trumbore_intersection_algorithm#C++_implementation
+    epsilon = 1e-10
+    tr = triangle_array[:]
+    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)
+
+    ray_cross_e2 = np.cross(ray, v2)
+    det = np.vecdot(v1, ray_cross_e2)
+
+    # if -epsilon < det < epsilon:
+    #     return None  # This ray is parallel to this triangle.
+    np.seterr(divide='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, 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)
+    return t
+
+
+def vector_square_length(vec):
+    return vec.dot(vec)
+def vector_length(vec):
+    return np.sqrt(vector_square_length(vec))
+
+if __name__ == '__main__':
+    origin = np.array([0, 0, 0])
+    ray = np.array([0, 0, 1])
+
+    # triangle_list = np.array([[[-2, -2, 2], [2, -2, 2], [0, 4, 2]]])
+    triangle_list = load_triangle_from_stl("sample_Hull_Mesh.stl")
+
+    # Triangles that intersect 
+    # When origin = (0, 0, 0) and ray = (0, 0, 1)
+    # [20252, 102434, 149679, 171530, 253712, 300957, 302603]
+
+    import time
+    start_time = time.time()
+    distance = array_raytrace(triangle_list, ray, origin)
+    end_time = time.time()
+
+    print('Triangles:', triangle_list.shape[0])
+    print('Distance:', distance)
+    print('Time:', end_time - start_time, 'seconds')