Created simulation for side scan sonar

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]
+
+