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