From 8583a0f1376350711940b1c80070d17e3870f08a Mon Sep 17 00:00:00 2001 From: JoeBell Date: Thu, 20 Feb 2025 18:57:47 -0500 Subject: [PATCH 1/5] Moved raytracing code into its own folder --- Raytrace/TriangleMesh.py | 117 +++++++++++++++++++++++++++++++++++++ Raytrace/__init__.py | 0 raytrace.py | 122 --------------------------------------- 3 files changed, 117 insertions(+), 122 deletions(-) create mode 100644 Raytrace/TriangleMesh.py create mode 100644 Raytrace/__init__.py delete mode 100644 raytrace.py diff --git a/Raytrace/TriangleMesh.py b/Raytrace/TriangleMesh.py new file mode 100644 index 0000000..950671f --- /dev/null +++ b/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 diff --git a/Raytrace/__init__.py b/Raytrace/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/raytrace.py b/raytrace.py deleted file mode 100644 index 72eaae4..0000000 --- a/raytrace.py +++ /dev/null @@ -1,122 +0,0 @@ -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') \ No newline at end of file From f923eac970e339135a7b3031c84421fe5e741f68 Mon Sep 17 00:00:00 2001 From: JoeBell Date: Thu, 20 Feb 2025 18:59:31 -0500 Subject: [PATCH 2/5] Implemented BVH Implemented the BVH algorithm to allow for much faster collision tests at the cost of having to compile the mesh --- Raytrace/BVHMesh.py | 178 ++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 178 insertions(+) create mode 100644 Raytrace/BVHMesh.py diff --git a/Raytrace/BVHMesh.py b/Raytrace/BVHMesh.py new file mode 100644 index 0000000..6edb52e --- /dev/null +++ b/Raytrace/BVHMesh.py @@ -0,0 +1,178 @@ +from typing import Self, Literal +import numpy as np +from Raytrace.TriangleMesh import TriangleMesh, Ray + +# Adapted from https://jacco.ompf2.com/2022/04/13/how-to-build-a-bvh-part-1-basics/ +class BVHAABB: + min_pos:np.ndarray + max_pos:np.ndarray + def __init__(self): + self.min_pos = np.array([np.nan]*3) + self.max_pos = np.array([np.nan]*3) + @staticmethod + def from_array(triangles:np.ndarray) -> 'BVHAABB': + out = BVHAABB() + if triangles.size == 0: return out + triangles = triangles.reshape((-1, 3)) + out.min_pos = triangles.min(axis = 0) + out.max_pos = triangles.max(axis = 0) + return out + def raytrace(self, ray:Ray) -> float: + old_error_state = np.seterr(divide='ignore') + t1 = (self.min_pos - ray.origin) * ray.inverse_direction + t2 = (self.max_pos - ray.origin) * ray.inverse_direction + np.seterr(**old_error_state) + t = np.stack([t1, t2]) + tmin:float = t.min(axis=0).max() + tmax:float = t.max(axis=0).min() + return tmin if tmax >= tmin and tmax > 0 else np.inf + def grow(self, point:np.ndarray): + self.min_pos = np.nanmin(np.stack([point, self.min_pos]), axis=0) + self.max_pos = np.nanmax(np.stack([point, self.max_pos]), axis=0) + def grow_aabb(self, other:Self): + self.grow(other.min_pos) + self.grow(other.max_pos) + def area(self) -> float: + e = self.max_pos - self.min_pos + return e[0] * e[1] + e[1] * e[2] + e[2] * e[0] + def __str__(self): + return f'({float(self.min_pos[0])}, {float(self.min_pos[1])}, {float(self.min_pos[2])})#({float(self.max_pos[0])}, {float(self.max_pos[1])}, {float(self.max_pos[2])})' +class BVHNode: + aabb:BVHAABB + left:Self + right:Self + start_index:int + tri_count:int + def get_subarray(self, triangles:np.ndarray) -> np.ndarray: + return triangles[self.start_index:self.start_index + self.tri_count] + def update_bounds(self, triangles:np.ndarray): + self.aabb = BVHAABB.from_array(self.get_subarray(triangles)) + def is_leaf(self) -> bool: + return self.tri_count > 0 +class BVHMesh(TriangleMesh): + centroids:np.ndarray + root:BVHNode + node_count:int + def __init__(self, *args, min_node_size = 2, **kwargs) -> None: + super().__init__(*args, **kwargs) + self.build_BVH(min_node_size) + def build_BVH(self, min_node_size = 100) -> None: + # calculate triangle centroids for partitioning + self.centroids = self.triangles.sum(axis = 1) / 3 + # assign all triangles to root node + self.node_count = 1 + self.root = root = BVHNode() + root.start_index = 0 + root.tri_count = self.triangles.shape[0] + if root.tri_count == 0: return + root.update_bounds(self.triangles) + # subdivide recursively + self.subdivide(root, min_node_size) + def subdivide(self, node:BVHNode, min_node_size = 100) -> None: + # determine split axis using SAH + print(self.node_count, node.tri_count,end=' \r') + if node.tri_count < min_node_size: return + best_cost, axis, split_pos = self.find_best_split(node) + if best_cost >= node.tri_count * node.aabb.area(): return + # in-place partition + i = node.start_index + j = i + node.tri_count - 1 + while i <= j: + if self.centroids[i, axis] < split_pos: i += 1 + else: + self.triangles[[i,j]] = self.triangles[[j,i]] + self.centroids[[i,j]] = self.centroids[[j,i]] + j -= 1 + # abort split if one of the sides is empty + left_count = i - node.start_index + if left_count == 0 or left_count == node.tri_count: return + self.node_count += 2 + # create child nodes + node.left = left = BVHNode() + left.start_index = node.start_index + left.tri_count = left_count + + node.right = right = BVHNode() + right.start_index = i + right.tri_count = node.tri_count - left_count + + node.tri_count = 0 + left.update_bounds(self.triangles) + right.update_bounds(self.triangles) + # recurse + self.subdivide(left) + self.subdivide(right) + + def raytrace(self, ray:Ray) -> float: + return self.BVH_raytrace(ray) + + def BVH_raytrace(self, ray:Ray) -> float: + node:BVHNode = self.root + stack:list[BVHNode] = [] + best:float = np.inf + while 1: + if node.is_leaf(): + intersections = self.batch_triangle_ray_intersection(node.get_subarray(self.triangles), ray) + intersections[intersections < 0] = np.inf + best = min(best, np.min(intersections)) + if len(stack) == 0: break + node = stack.pop() + continue + child1, child2 = node.left, node.right + dist1 = node.left.aabb.raytrace(ray) + dist2 = node.right.aabb.raytrace(ray) + if dist1 > dist2: + dist1, dist2 = dist2, dist1 + child1, child2 = child2, child1 + if not np.isfinite(dist1): + if len(stack) == 0: break + node = stack.pop() + else: + node = child1 + if np.isfinite(dist2): + stack.append(child2) + return best + def find_best_split(self, node:BVHNode) -> tuple[float, int, float]: + BINS = 8 + axis:int = -1 + split_pos:float = 0 + best_cost:float = np.inf + triangles:np.ndarray[tuple[int, Literal[3], Literal[3]], np.dtype[np.float32]] = node.get_subarray(self.triangles) + centroids:np.ndarray[tuple[int, Literal[3]], np.dtype[np.float32]] = node.get_subarray(self.centroids) + for axis_i in range(3): + bounds_min = float(np.min(centroids[:,axis_i])) + bounds_max = float(np.max(centroids[:,axis_i])) + + if bounds_min == bounds_max: continue + # populate the bins + scale:float = BINS / (bounds_max - bounds_min) + bin_idx = ((centroids[:,axis_i] - bounds_min) * scale).astype(int) + bin_idx[bin_idx > BINS - 1] = BINS - 1 + bin_bounds = [BVHAABB.from_array(triangles[bin_idx == n]) for n in range(BINS)] + bin_counts = [np.count_nonzero(bin_idx == n) for n in range(BINS)] + # gather data for the 7 planes between the 8 bins + left_area = np.zeros(BINS - 1, float) + right_area = np.zeros(BINS - 1, float) + left_count = np.zeros(BINS - 1, int) + right_count = np.zeros(BINS - 1, int) + left_aabb = BVHAABB() + right_aabb = BVHAABB() + left_sum = right_sum = 0 + for i in range(BINS - 1): + left_sum += bin_counts[i] + left_count[i] = left_sum + left_aabb.grow_aabb(bin_bounds[i]) + left_area[i] = left_aabb.area() + right_sum += bin_counts[BINS - 1 - i] + right_count[BINS - 2 - i] = right_sum + right_aabb.grow_aabb(bin_bounds[BINS - 1 - i]) + right_area[BINS - 2 - i] = right_aabb.area() + # calculate SAH cost for the 7 planes + scale = 1. / scale + for i in range(BINS - 1): + cost:float = left_count[i] * left_area[i] + right_count[i] * right_area[i] + if cost < best_cost: + split_pos = bounds_min + scale * (i + 1) + axis = axis_i + best_cost = cost + return best_cost, axis, split_pos \ No newline at end of file From e72b80899ce6509e1551048da4122bbd8dbf935a Mon Sep 17 00:00:00 2001 From: JoeBell Date: Thu, 20 Feb 2025 19:00:18 -0500 Subject: [PATCH 3/5] Created simulation for side scan sonar --- Raytrace/SideScan.py | 96 ++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 96 insertions(+) create mode 100644 Raytrace/SideScan.py diff --git a/Raytrace/SideScan.py b/Raytrace/SideScan.py new file mode 100644 index 0000000..598a084 --- /dev/null +++ b/Raytrace/SideScan.py @@ -0,0 +1,96 @@ +from Raytrace.TriangleMesh import TriangleMesh, Ray +import numpy as np +import time + +class ScanReading: + distances:np.ndarray + intersections:np.ndarray + origins:np.ndarray + directions:np.ndarray + finite:np.ndarray + intersection_count:int + + smooth_dist:float + result_reselution:int + min_dist:float + max_dist:float + + result:np.ndarray + + start_time:float = -1 + end_time:float = -1 + + def __init__(self, + distances:np.ndarray, rays:list[Ray], + smooth_dist:float = 0.05, + result_reselution:int = 1000, + min_dist:float = 0, + max_dist:float = 2 + ): + old_error_state = np.seterr(all='ignore') + self.distances = distances + self.origins = np.array([r.origin for r in rays]) + self.directions = np.array([r.direction for r in rays]) + self.intersections = self.origins + self.directions * self.distances.reshape((-1,1)) + self.finite = np.isfinite(self.distances) + self.intersection_count = np.count_nonzero(self.finite) + + self.smooth_dist = smooth_dist + self.result_reselution = result_reselution + self.min_dist = min_dist + self.max_dist = max_dist + self.convert_distances() + np.seterr(**old_error_state) + + def convert_distances(self) -> None: + old_error_state = np.seterr(all='ignore') + norm = (self.distances[self.finite] - self.min_dist) / (self.max_dist - self.min_dist) + + ldist = norm - (np.arange(0,1,1/self.result_reselution) + 0.5/self.result_reselution).reshape((-1,1)) + smooth_val = self.smooth_dist / (self.max_dist - self.min_dist) + lval = np.pow(np.maximum(0, np.square(smooth_val) - np.square(ldist)),3) / (32/35*smooth_val**7) / len(self.distances) + + self.result = np.sum(lval, axis = 1) + np.seterr(**old_error_state) + def print_summary(self) -> None: + print('Intersections:', self.intersection_count , '/', len(self.distances)) + if self.end_time == -1 or self.start_time == -1: return + print('Time:', self.end_time - self.start_time, 'seconds') + print('Speed:', len(self.distances)/(self.end_time - self.start_time), 'rays/seconds') +class SideScan: + mesh:TriangleMesh + smooth_dist:float + result_reselution:int + def __init__(self, mesh:TriangleMesh, smooth_dist:float = 0.05, result_reselution:int = 1000) -> None: + self.mesh = mesh + self.smooth_dist = smooth_dist + self.result_reselution = result_reselution + def scan_rays(self, rays:list[Ray]) -> ScanReading: + distances = np.empty((len(rays),), np.float32) + + start_time = time.time() + for n, ray in enumerate(rays): + distances[n] = self.mesh.raytrace(ray) + end_time = time.time() + + out = ScanReading(distances, rays) + + out.start_time = start_time + out.end_time = end_time + + return out + @staticmethod + def generate_rays(orientation:Ray, min_angle:float, max_angle:float, angle_reselution:int) -> list[Ray]: + angle_step = (max_angle-min_angle)/(angle_reselution-1) + angles = np.arange(min_angle, max_angle + angle_step/2, angle_step) + origin = orientation.origin + pitch = np.arctan2(orientation.direction[2],np.sqrt(np.dot(orientation.direction[0:2], orientation.direction[0:2]))) + yaw = np.arctan2(orientation.direction[1], orientation.direction[0]) # yaw = 0 => facing due +x + + # Precomputed rotation matrix + sin_comp = np.array([-np.sin(pitch) * np.cos(yaw), np.sin(pitch) * np.sin(yaw), np.cos(pitch)]) + cos_comp = np.array([np.sin(yaw), np.cos(yaw), 0]) + + return [Ray(np.sin(a) * sin_comp + np.cos(a) * cos_comp, origin) for a in angles] + + \ No newline at end of file From ddce711517086427ffedfd725dc79ea382f824a3 Mon Sep 17 00:00:00 2001 From: JoeBell Date: Thu, 20 Feb 2025 19:01:04 -0500 Subject: [PATCH 4/5] Added helper functions visualizing --- Raytrace/PlotRays.py | 59 ++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 59 insertions(+) create mode 100644 Raytrace/PlotRays.py diff --git a/Raytrace/PlotRays.py b/Raytrace/PlotRays.py new file mode 100644 index 0000000..bb7e01a --- /dev/null +++ b/Raytrace/PlotRays.py @@ -0,0 +1,59 @@ +from Raytrace.TriangleMesh import TriangleMesh +from Raytrace.SideScan import ScanReading +import numpy as np +try: + import plotly.graph_objs as go # type: ignore +except ModuleNotFoundError: + print('plotly not installed, please run "pip install plotly"') + exit() + +def plot_mesh(mesh:TriangleMesh, **kwargs) -> go.Mesh3d: + x, y, z = mesh.triangles.reshape((-1,3)).swapaxes(0, 1) + i, j, k = [list(range(i,x.shape[0],3)) for i in range(3)] + return go.Mesh3d(x=x, y=y, z=z, i=i, j=j, k=k, **kwargs) + +def plot_rays(reading:ScanReading, **kwargs) -> go.Scatter3d: + origin = reading.origins[reading.finite] + inters = reading.intersections[reading.finite] + + x, y, z = np.stack((origin, inters, origin)).swapaxes(0,1).reshape((-1,3)).swapaxes(0,1) + return go.Scatter3d(x=x, y=y, z=z, **kwargs) + +def plot_reading(reading:ScanReading) -> go.Figure: + return go.Figure(data = [ + go.Scatter({ + 'name': 'Smoothed', + 'showlegend': True, + 'mode': 'lines', + 'x': np.arange(reading.result_reselution)/reading.result_reselution*(reading.max_dist-reading.min_dist)+reading.min_dist, + 'y': reading.result, + }), + go.Scatter({ + 'mode': 'markers', + 'name': 'Raw', + 'showlegend': True, + 'x': reading.distances[reading.finite], + 'y': np.zeros(reading.intersection_count), + }) + ]) +def plot_intersections(reading:ScanReading, **kwargs) -> go.Scatter3d: + inters = reading.intersections[reading.finite] + + x, y, z = inters.reshape((-1,3)).swapaxes(0,1) + return go.Scatter3d(x=x, y=y, z=z, **kwargs) + +def plot_intersections_list(readings:list[ScanReading], **kwargs) -> list[go.Scatter3d]: + return [plot_intersections(reading, **kwargs) for reading in readings] + +def plot_readings_heatmap(readings) -> go.Figure: + return go.Figure(data = [ + go.Heatmap({ + 'y': np.arange(readings[0].min_dist, readings[0].max_dist, (readings[0].max_dist - readings[0].min_dist) / readings[0].result_reselution), + 'z': np.array([reading.result for reading in readings]).swapaxes(0,1), + }) + ], + layout = { + 'xaxis_title' : 'Reading Index', + 'yaxis_title' : 'Distance (units)', + } + ) \ No newline at end of file From 4c24da07478b184b640c081ef559c139b81d60ca Mon Sep 17 00:00:00 2001 From: JoeBell Date: Thu, 20 Feb 2025 19:03:31 -0500 Subject: [PATCH 5/5] Added examples --- ExampleMultipleSonarReadings.ipynb | 103 ++++++++++++++++++++++++++ ExampleSingleSonarReading.ipynb | 114 +++++++++++++++++++++++++++++ 2 files changed, 217 insertions(+) create mode 100644 ExampleMultipleSonarReadings.ipynb create mode 100644 ExampleSingleSonarReading.ipynb diff --git a/ExampleMultipleSonarReadings.ipynb b/ExampleMultipleSonarReadings.ipynb new file mode 100644 index 0000000..f45c0f0 --- /dev/null +++ b/ExampleMultipleSonarReadings.ipynb @@ -0,0 +1,103 @@ +{ + "cells": [ + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "from Raytrace.TriangleMesh import Ray\n", + "from Raytrace.BVHMesh import BVHMesh\n", + "from Raytrace.SideScan import SideScan, ScanReading\n", + "import numpy as np\n", + "import time" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "stl_path = 'sample_Hull_Mesh.stl'\n", + "start_time = time.time()\n", + "mesh = BVHMesh(stl_path = stl_path, min_node_size = 100)\n", + "end_time = time.time()\n", + "\n", + "print('Build Time:', end_time - start_time, 'seconds')" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "facing = np.array([1, 0, 0])\n", + "min_angle = 0\n", + "max_angle = -np.pi/2\n", + "sample_ray_count = 1000\n", + "\n", + "origin_start = np.array([5, -1, 1])\n", + "origin_end = np.array([9, -1, 1])\n", + "readings_count = 100\n", + "# liniar interpolation between the two for this simple demo\n", + "origins = [origin_start * (1-i) + origin_end * i for i in np.arange(0, 1+1/(readings_count-1)/2, 1/(readings_count-1))]\n", + "\n", + "orientations = [Ray(facing, origin) for origin in origins]\n", + "\n", + "rays_list = [SideScan.generate_rays(orientation, min_angle, max_angle, sample_ray_count) for orientation in orientations]\n", + "\n", + "readings:list[ScanReading] = []\n", + "for n,rays in enumerate(rays_list):\n", + " print(n + 1, len(rays_list), sep='/', end='\\r')\n", + " readings.append(SideScan(mesh).scan_rays(rays))\n", + "\n", + "print()\n", + "\n", + "# 100 readings at 1000 rays each takes just over 2 minutes on my machine\n", + "print('Done')" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "import plotly.graph_objs as go\n", + "from Raytrace.PlotRays import plot_readings_heatmap" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "plot_readings_heatmap(readings)" + ] + } + ], + "metadata": { + "kernelspec": { + "display_name": "Python 3", + "language": "python", + "name": "python3" + }, + "language_info": { + "codemirror_mode": { + "name": "ipython", + "version": 3 + }, + "file_extension": ".py", + "mimetype": "text/x-python", + "name": "python", + "nbconvert_exporter": "python", + "pygments_lexer": "ipython3", + "version": "3.11.5" + } + }, + "nbformat": 4, + "nbformat_minor": 2 +} diff --git a/ExampleSingleSonarReading.ipynb b/ExampleSingleSonarReading.ipynb new file mode 100644 index 0000000..5787eb4 --- /dev/null +++ b/ExampleSingleSonarReading.ipynb @@ -0,0 +1,114 @@ +{ + "cells": [ + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "from Raytrace.TriangleMesh import Ray\n", + "from Raytrace.BVHMesh import BVHMesh\n", + "from Raytrace.SideScan import SideScan\n", + "import numpy as np\n", + "import time" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "stl_path = 'sample_Hull_Mesh.stl'\n", + "start_time = time.time()\n", + "mesh = BVHMesh(stl_path = stl_path, min_node_size = 100)\n", + "end_time = time.time()\n", + "\n", + "print('Build Time:', end_time - start_time, 'seconds')" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "origin = np.array([5, -1, 1])\n", + "facing = np.array([1, 0, 0])\n", + "min_angle = 0\n", + "max_angle = -np.pi/2\n", + "sample_ray_count = 1000\n", + "\n", + "orientation = Ray(facing, origin)\n", + "\n", + "rays = SideScan.generate_rays(orientation, min_angle, max_angle, sample_ray_count)\n", + "\n", + "reading = SideScan(mesh).scan_rays(rays)\n", + "\n", + "print('Triangles:', mesh.triangles.shape[0])\n", + "reading.print_summary()" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "import plotly.graph_objs as go\n", + "from PlotRays import plot_mesh, plot_rays, plot_reading" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "mesh_plot = plot_mesh(mesh, color='lightpink', opacity=1)\n", + "rays_plot = plot_rays(reading, line={'color':'Blue'})" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "fig = go.Figure(data=[mesh_plot, rays_plot])\n", + "fig.update_layout(autosize=False, width=1000, height=500, margin=dict(l=20, r=20, t=20, b=20), scene = {'aspectmode':'data'})\n", + "fig.show()" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "plot_reading(reading)" + ] + } + ], + "metadata": { + "kernelspec": { + "display_name": "Python 3", + "language": "python", + "name": "python3" + }, + "language_info": { + "codemirror_mode": { + "name": "ipython", + "version": 3 + }, + "file_extension": ".py", + "mimetype": "text/x-python", + "name": "python", + "nbconvert_exporter": "python", + "pygments_lexer": "ipython3", + "version": "3.11.5" + } + }, + "nbformat": 4, + "nbformat_minor": 2 +}