obstacle avoidance

2025-12-23 14:57:13 -08:00
parent 4fa942564a
commit 8315b6c511
6 changed files with 53 additions and 9 deletions
--- a/geometries/@rectangularPrism/closestToPoint.m
+++ b/geometries/@rectangularPrism/closestToPoint.m
@@ -0,0 +1,19 @@
+function cPos = closestToPoint(obj, pos)
+    arguments (Input)
+        obj (1, 1) {mustBeA(obj, 'rectangularPrism')};
+        pos (:, 3) double;
+    end
+    arguments (Output)
+        cPos (:, 3) double;
+    end
+    cPos = NaN(1, 3);
+    for ii = 1:3
+        if pos(ii) < obj.minCorner(ii)
+            cPos(ii) = obj.minCorner(ii);
+        elseif pos(ii) > obj.maxCorner(ii)
+            cPos(ii) = obj.maxCorner(ii);
+        else
+            cPos(ii) = pos(ii);
+        end
+    end
+end