Created AUVSim

Created a module to generate basic AUV paths to follow the face of hulls

AUVSim/.gitignore

@@ -0,0 +1 @@
+__pycache__/

AUVSim/__init__.py

@@ -0,0 +1,5 @@
+from .auv import (
+    AUV,
+    AUVPath,
+    AUVPathGen
+)

AUVSim/auv.py

@@ -0,0 +1,59 @@
+from Raytrace.TriangleMesh import TriangleMesh, Ray
+from Raytrace.SideScan import SideScan
+import numpy as np
+from typing import Any
+class AUV:
+    ideal_distance:float # Currently unused
+    start_pos:np.ndarray
+    start_facing:np.ndarray
+    def __init__(self, position:np.ndarray, facing:np.ndarray):
+        self.start_pos = position
+        self.start_facing = facing
+class AUVPath:
+    positions:np.ndarray[float, Any]
+    facings:np.ndarray[float, Any]
+
+    def __init__(self, positions:np.ndarray, facings:np.ndarray):
+        self.positions = positions
+        self.facings = facings
+    def as_rays(self):
+        return [Ray(i, j) for i, j in zip(self.facings, self.positions)]
+
+class AUVPathGen:
+    mesh:TriangleMesh
+    auv:AUV
+    def __init__(self, mesh:TriangleMesh, auv:AUV):
+        self.mesh = mesh
+        self.auv = auv
+    def get_path(self, travel_distance:float, samples:int) -> AUVPath:
+        travel_step = travel_distance / (samples - 1)
+        positions = [self.auv.start_pos]
+        facings = [self.auv.start_facing]
+
+        # Utility functions
+        # HACK: There should be a function to generate one side ray
+        #       instead of generating a list with length one.
+        get_side_ray = lambda face, pos:\
+            SideScan.generate_rays(Ray(face, pos), 0, 1, 2)[0]
+        get_dist = lambda ray: self.mesh.raytrace(ray)
+        normalize = lambda vector: vector / np.sqrt(np.dot(vector, vector))
+
+        for _ in range(samples - 1):
+            # find distance at current state
+            prev_dist = get_dist(get_side_ray(facings[-1], positions[-1]))
+            # move frowards
+            positions.append(positions[-1] + facings[-1] * travel_step)
+            # shoot side ray
+            # NOTE: facing has not yet been updated
+            side_ray = get_side_ray(facings[-1], positions[-1])
+            ray_dist = self.mesh.raytrace(side_ray)
+            # point in the direction that the hull is sloping
+            rise = (ray_dist - prev_dist) * side_ray.direction
+            if not (np.isfinite(ray_dist) and np.isfinite(prev_dist)):
+                rise = np.zeros(3)
+            new_facing = normalize(facings[-1] + rise)
+            facings.append(new_facing)
+
+        np_positions = np.array(positions)
+        np_facings = np.array(facings)
+        return AUVPath(np_positions, np_facings)