fixed guidance only pulling things towards the middle and added CA QP CBF code

2025-12-05 16:04:02 -08:00
parent 96c91c3988
commit 95ea19e546
31 changed files with 342 additions and 14 deletions
--- a/geometries/@spherical/contains.m
+++ b/geometries/@spherical/contains.m
@@ -0,0 +1,10 @@
+function c = contains(obj, pos)
+    arguments (Input)
+        obj (1, 1) {mustBeA(obj, 'spherical')};
+        pos (:, 3) double;
+    end
+    arguments (Output)
+        c (:, 1) logical
+    end
+    c = norm(obj.center - pos) <= obj.radius;
+end