From 96c91c398879d190bb6b4e6154f6384fd55137f7 Mon Sep 17 00:00:00 2001
From: Kevin D <krdee1@gmail.com>
Date: Thu, 4 Dec 2025 18:24:49 -0800
Subject: [PATCH] added collision barrier function and gradient

---
 @agent/agent.m                                |  2 +
 @agent/run.m                                  |  6 +++
 .../@rectangularPrism/distanceGradient.m      | 42 +++++++++++++++++++
 .../@rectangularPrism/rectangularPrism.m      |  1 +
 4 files changed, 51 insertions(+)
 create mode 100644 geometries/@rectangularPrism/distanceGradient.m

diff --git a/@agent/agent.m b/@agent/agent.m
index b14c679..9337f55 100644
--- a/@agent/agent.m
+++ b/@agent/agent.m
@@ -20,6 +20,8 @@ classdef agent
 
         % Collision
         collisionGeometry;
+        barrierFunction;
+        dBarrierFunction;
 
         % FOV cone
         fovGeometry;
diff --git a/@agent/run.m b/@agent/run.m
index 16566c1..1668fb0 100644
--- a/@agent/run.m
+++ b/@agent/run.m
@@ -8,6 +8,12 @@ function obj = run(obj, domain, partitioning, t)
     arguments (Output)
         obj (1, 1) {mustBeA(obj, 'agent')};
     end
+    
+    % Update collision barrier function
+    % first part evaluates to +/-1 if the point is outside/inside the collision geometry
+    % Second part determines the distance from the point to the boundary of the collision geometry
+    obj.barrierFunction = @(x) (1 - 2 * obj.collisionGeometry.contains(x)) * obj.collisionGeometry.distance(x); % x is 1x3
+    obj.dBarrierFunction = @(x) obj.collisionGeometry.distanceGradient(x); % x is 1x3
 
     % Collect objective function values across partition
     partitionMask = partitioning == obj.index;
diff --git a/geometries/@rectangularPrism/distanceGradient.m b/geometries/@rectangularPrism/distanceGradient.m
new file mode 100644
index 0000000..c4e43a7
--- /dev/null
+++ b/geometries/@rectangularPrism/distanceGradient.m
@@ -0,0 +1,42 @@
+function g = distanceGradient(obj, pos)
+    arguments (Input)
+        obj (1, 1) {mustBeA(obj, 'rectangularPrism')};
+        pos (:, 3) double;
+    end
+    arguments (Output)
+        g (:, 3) double
+    end
+    
+    % find nearest point on surface to query position
+    q = min(max(pos, obj.minCorner), obj.maxCorner);
+
+    % Find distance and direction between pos and q
+    v = pos - q;
+    vNorm = norm(v);
+
+    % position is outside geometry
+    if vNorm > 0
+        % gradient is normalized vector from q to p
+        g = v / vNorm;
+        return;
+    end
+
+    % position is on or in geometry
+    % find distances to each face in each dimension
+    distances = [pos(1) - obj.minCorner(1), obj.maxCorner(1) - pos(1), pos(2) - obj.minCorner(2), obj.maxCorner(2) - pos(2), pos(3) - obj.minCorner(3), obj.maxCorner(3) - pos(3)];
+    [~, idx] = min(distances);
+
+    % I think there needs to be additional handling here for the
+    % edge/corner cases, where there are ways to balance or resolve ties
+    % when two faces are equidistant to the query position
+    assert(sum(idx) == idx, "Implement edge case handling");
+
+    % select gradient that brings us quickest to the nearest face
+    g = [ 1,  0,  0; ...
+         -1,  0,  0; ...
+          0,  1,  0; ...
+          0, -1,  0; ...
+          0,  0,  1; ...
+          0,  0, -1;];
+    g = g(idx, :);
+end
\ No newline at end of file
diff --git a/geometries/@rectangularPrism/rectangularPrism.m b/geometries/@rectangularPrism/rectangularPrism.m
index 7c124bd..901aa06 100644
--- a/geometries/@rectangularPrism/rectangularPrism.m
+++ b/geometries/@rectangularPrism/rectangularPrism.m
@@ -32,6 +32,7 @@ classdef rectangularPrism
         [r     ] = random(obj);
         [c     ] = contains(obj, pos);
         [d     ] = distance(obj, pos);
+        [g     ] = distanceGradient(obj, pos);
         [c     ] = containsLine(obj, pos1, pos2);
         [obj, f] = plotWireframe(obj, ind, f);
     end