fixed cone plotting all the way to ground

@agent/initialize.m

@@ -33,5 +33,5 @@ function obj = initialize(obj, pos, vel, pan, tilt, collisionGeometry, sensorMod
 
     % Initialize FOV cone
     obj.fovGeometry = cone;
-    obj.fovGeometry = obj.fovGeometry.initialize([obj.pos(1:2), 0], tand(obj.sensorModel.alphaTilt) * obj.pos(3), obj.pos(3), REGION_TYPE.FOV, sprintf("%s FOV", obj.label));
+    obj.fovGeometry = obj.fovGeometry.initialize([obj.pos(1:3)], tand(obj.sensorModel.alphaTilt) * obj.pos(3), obj.pos(3), REGION_TYPE.FOV, sprintf("%s FOV", obj.label));
 end

@agent/plot.m

@@ -36,5 +36,6 @@ function [obj, f] = plot(obj, ind, f)
     end
 
     % Plot FOV geometry
-    [obj.fovGeometry, f] = obj.fovGeometry.plot(ind, f);
+    maxAlt = f.Children(1).Children(end).ZLim(2); % to avoid scaling the FOV geometry as the sim runs, let's just make it really big and hide the excess under the floor of the domain. Check the domain altitude to figure out how big it needs to be to achieve this deception.
+    [obj.fovGeometry, f] = obj.fovGeometry.plot(ind, f, maxAlt);
 end

@agent/run.m

@@ -67,7 +67,7 @@ function obj = run(obj, domain, partitioning, timestepIndex, index, agents)
 
     % Compute scaling factor
     targetRate = 0.2 - 0.0008 * timestepIndex; % slow down as you get closer
-    rateFactor = targetRate / norm(gradC); 
+    rateFactor = targetRate / norm(gradC);
 
     % Compute unconstrained next position
     pNext = obj.pos + rateFactor * gradC;

@agent/updatePlots.m

@@ -39,6 +39,7 @@ function updatePlots(obj)
     % Update FOV geometry surfaces
     for jj = 1:size(obj.fovGeometry.surface, 2)
         % Update each plot
+        % obj.fovGeometry = obj.fovGeometry.plot(obj.spatialPlotIndices)
         obj.fovGeometry.surface(jj).XData = obj.fovGeometry.surface(jj).XData + deltaPos(1);
         obj.fovGeometry.surface(jj).YData = obj.fovGeometry.surface(jj).YData + deltaPos(2);
         obj.fovGeometry.surface(jj).ZData = obj.fovGeometry.surface(jj).ZData + deltaPos(3);