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

geometries/@rectangularPrism/initialize.m

@@ -24,6 +24,10 @@ function obj = initialize(obj, bounds, tag, label, objectiveFunction, discretiza
     % Compute center
     obj.center = obj.minCorner + obj.dimensions ./ 2;
 
+    % Compute a (fake) radius
+    % fully contains the rectangular prism from the center
+    obj.radius = (1/2) * sqrt(sum(obj.dimensions.^2));
+
     % Compute vertices
     obj.vertices = [obj.minCorner;
                     obj.maxCorner(1), obj.minCorner(2:3);
@@ -44,4 +48,13 @@ function obj = initialize(obj, bounds, tag, label, objectiveFunction, discretiza
     if tag == REGION_TYPE.DOMAIN
         obj.objective = sensingObjective;
     end
+
+    % Initialize CBF
+    % 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
+    % gradient of barrier function 
+    obj.dBarrierFunction = @(x) obj.collisionGeometry.distanceGradient(x); % x is 1x3
+    % as long as the collisionGeometry object is updated during runtime,
+    % these functions never have to be updated again
 end

geometries/@rectangularPrism/rectangularPrism.m

@@ -11,6 +11,7 @@ classdef rectangularPrism
         dimensions = NaN(1, 3);
         center = NaN;
         footprint = NaN(4, 2);
+        radius = NaN; % fake radius
 
         % Graph
         vertices = NaN(8, 3);
@@ -20,6 +21,10 @@ classdef rectangularPrism
 
         % Plotting
         lines;
+
+        % collision
+        barrierFunction;
+        dBarrierFunction;
     end
     properties (SetAccess = public, GetAccess = public)
         % Sensing objective (for DOMAIN region type only)

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

geometries/@spherical/containsLine.m

@@ -0,0 +1,28 @@
+function c = containsLine(obj, pos1, pos2)
+    arguments (Input)
+        obj (1, 1) {mustBeA(obj, 'spherical')};
+        pos1 (1, 3) double;
+        pos2 (1, 3) double;
+    end
+    arguments (Output)
+        c (1, 1) logical
+    end
+    
+    d = pos2 - pos1;
+    f = pos1 - obj.center;
+
+    a = dot(d, d);
+    b = 2 * dot(f, d);
+    c = dot(f, f) - obj.radius^2;
+
+    disc = b^2 - 4*a*c;
+
+    if disc < 0
+        c = false;
+        return;
+    end
+
+    t = [(-b - sqrt(disc)) / (2 * a), (-b + sqrt(disc)) / (2 * a)];
+
+    c = (t(1) >= 0 && t(1) <= 1) || (t(2) >= 0 && t(2) <= 1);
+end

geometries/@spherical/initialize.m

@@ -0,0 +1,42 @@
+function obj = initialize(obj, center, radius, tag, label)
+    arguments (Input)
+        obj (1, 1) {mustBeA(obj, 'spherical')};
+        center (1, 3) double;
+        radius  (1, 1) double;
+        tag (1, 1) REGION_TYPE = REGION_TYPE.INVALID;
+        label (1, 1) string = "";
+    end
+    arguments (Output)
+        obj (1, 1) {mustBeA(obj, 'spherical')};
+    end
+
+    obj.tag = tag;
+    obj.label = label;
+
+    % Define geometry
+    obj.center = center;
+    obj.radius = radius;
+    obj.diameter = 2 * obj.radius;
+
+    % Initialize CBF
+    obj.barrierFunction = @(x) NaN;
+    % gradient of barrier function 
+    obj.dBarrierFunction = @(x) NaN;
+
+    % fake vertices in a cross pattern
+    obj.vertices = [obj.center + [obj.radius, 0, 0]; ...
+                    obj.center - [obj.radius, 0, 0]; ...
+                    obj.center + [0, obj.radius, 0]; ...
+                    obj.center - [0, obj.radius, 0]; ...
+                    obj.center + [0, 0, obj.radius]; ...
+                    obj.center - [0, 0, obj.radius]];
+    % fake edges in two perpendicular rings
+    obj.edges = [1, 3; ...
+                 3, 2; ...
+                 2, 4; ...
+                 4, 1; ...
+                 1, 5; ...
+                 5, 2; ...
+                 2, 6; ...
+                 6, 1];
+end

geometries/@spherical/plotWireframe.m

@@ -0,0 +1,43 @@
+function [obj, f] = plotWireframe(obj, ind, f)
+    arguments (Input)
+        obj (1, 1) {mustBeA(obj, 'spherical')};
+        ind (1, :) double = NaN;
+        f (1, 1) {mustBeA(f, 'matlab.ui.Figure')} = figure;
+    end
+    arguments (Output)
+        obj (1, 1) {mustBeA(obj, 'spherical')};
+        f (1, 1) {mustBeA(f, 'matlab.ui.Figure')};
+    end
+
+    % Create axes if they don't already exist
+    f = firstPlotSetup(f);
+
+    % Create plotting inputs
+    [X, Y, Z] = sphere(8);
+    % Scale
+    X = X * obj.radius;
+    Y = Y * obj.radius;
+    Z = Z * obj.radius;
+    % Shift
+    X = X + obj.center(1);
+    Y = Y + obj.center(2);
+    Z = Z + obj.center(3);
+
+    % Plot the boundaries of the geometry into 3D view
+    if isnan(ind)
+        o = plot3(f.CurrentAxes, X, Y, Z, '-', 'Color', obj.tag.color, 'LineWidth', 2);
+    else
+        hold(f.Children(1).Children(ind(1)), "on");
+        o = plot3(f.Children(1).Children(ind(1)), X, Y, Z, '-', 'Color', obj.tag.color, 'LineWidth', 2);
+        hold(f.Children(1).Children(ind(1)), "off");
+    end
+
+    % Copy to other requested tiles
+    if numel(ind) > 1
+        for ii = 2:size(ind, 2)
+            o = [o, copyobj(o(:, 1), f.Children(1).Children(ind(ii)))];
+        end
+    end
+
+    obj.lines = o;
+end

geometries/@spherical/random.m

@@ -0,0 +1,15 @@
+function r = random(obj)
+    arguments (Input)
+        obj (1, 1) {mustBeA(obj, 'spherical')};
+    end
+    arguments (Output)
+        r (1, 3) double
+    end
+    y = (rand - 0.5) * 2; % uniform draw on [-1, 1]
+    R = sqrt(1 - y^2);
+    lon = (rand - 0.5) * 2 * pi; % uniform draw on [-pi, pi]
+    s = [R * sin(lon), y, R * cos(lon)]; % random point on surface
+    r = s * rand^(1/3); % scaled to random normalized radius [0, 1]
+
+    r = obj.center + obj.radius * r;
+end

geometries/@spherical/spherical.m

@@ -0,0 +1,37 @@
+classdef spherical
+    % Rectangular prism geometry
+    properties (SetAccess = private, GetAccess = public)
+        % Meta
+        tag = REGION_TYPE.INVALID;
+        label = "";
+
+        % Spatial
+        center = NaN;
+        radius = NaN;
+        diameter = NaN;
+
+        vertices; % fake vertices
+        edges; % fake edges
+
+        % Plotting
+        lines;
+
+        % collision
+        barrierFunction;
+        dBarrierFunction;
+    end
+    properties (SetAccess = public, GetAccess = public)
+        % Sensing objective (for DOMAIN region type only)
+        objective;
+    end
+
+    methods (Access = public)
+        [obj   ] = initialize(obj, center, radius, tag, label);
+        [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
+end