kdee/miSim
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

added sensor pointing by gradient ascent

Browse Source
This commit is contained in:
Kevin Dee
2026-05-07 09:04:52 -07:00
parent 740b42eba4
commit b44df40c7e
17 changed files with 296 additions and 148 deletions
Download Patch File Download Diff File Expand all files Collapse all files
@agent/updatePlots.m
+51 -28
View File
@@ -7,45 +7,68 @@ function updatePlots(obj)
% Find change in agent position since last timestep
deltaPos = obj.pos - obj.lastPos;
if all(isnan(deltaPos)) || all(deltaPos == zeros(1, 3))
% Agent did not move, so nothing has to move on the plots
posChanged = ~(all(isnan(deltaPos)) || all(deltaPos == zeros(1, 3)));
orientChanged = obj.sensorModel.tilt ~= obj.fovGeometry.tilt || ...
obj.sensorModel.azimuth ~= obj.fovGeometry.azimuth;
if ~posChanged && ~orientChanged
return;
end
% Scatterplot point positions
for ii = 1:size(obj.scatterPoints, 1)
obj.scatterPoints(ii).XData = obj.pos(1);
obj.scatterPoints(ii).YData = obj.pos(2);
obj.scatterPoints(ii).ZData = obj.pos(3);
end
% Collision geometry edges
for jj = 1:size(obj.collisionGeometry.lines, 2)
% Update plotting
for ii = 1:size(obj.collisionGeometry.lines(:, jj), 1)
obj.collisionGeometry.lines(ii, jj).XData = obj.collisionGeometry.lines(ii, jj).XData + deltaPos(1);
obj.collisionGeometry.lines(ii, jj).YData = obj.collisionGeometry.lines(ii, jj).YData + deltaPos(2);
obj.collisionGeometry.lines(ii, jj).ZData = obj.collisionGeometry.lines(ii, jj).ZData + deltaPos(3);
if posChanged
% Scatterplot point positions
for ii = 1:size(obj.scatterPoints, 1)
obj.scatterPoints(ii).XData = obj.pos(1);
obj.scatterPoints(ii).YData = obj.pos(2);
obj.scatterPoints(ii).ZData = obj.pos(3);
end
end
% Communications geometry edges
if obj.plotCommsGeometry
for jj = 1:size(obj.commsGeometry.lines, 2)
% Collision geometry edges
for jj = 1:size(obj.collisionGeometry.lines, 2)
for ii = 1:size(obj.collisionGeometry.lines(:, jj), 1)
obj.collisionGeometry.lines(ii, jj).XData = obj.collisionGeometry.lines(ii, jj).XData + deltaPos(1);
obj.collisionGeometry.lines(ii, jj).YData = obj.collisionGeometry.lines(ii, jj).YData + deltaPos(2);
obj.collisionGeometry.lines(ii, jj).ZData = obj.collisionGeometry.lines(ii, jj).ZData + deltaPos(3);
end
end
% Communications geometry edges
if obj.plotCommsGeometry
for jj = 1:size(obj.commsGeometry.lines, 2)
for ii = 1:size(obj.collisionGeometry.lines(:, jj), 1)
obj.collisionGeometry.lines(ii, jj).XData = obj.collisionGeometry.lines(ii, jj).XData + deltaPos(1);
obj.collisionGeometry.lines(ii, jj).YData = obj.collisionGeometry.lines(ii, jj).YData + deltaPos(2);
obj.collisionGeometry.lines(ii, jj).ZData = obj.collisionGeometry.lines(ii, jj).ZData + deltaPos(3);
end
end
end
end
% 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);
% FOV cone: recompute full mesh whenever position or orientation changes
if ~isempty(obj.fovGeometry.surface)
% Sync fovGeometry state to current agent position and sensor orientation
obj.fovGeometry = obj.fovGeometry.initialize( ...
obj.pos, obj.fovGeometry.radius, obj.fovGeometry.height, ...
obj.fovGeometry.tag, obj.fovGeometry.label, ...
obj.sensorModel.tilt, obj.sensorModel.azimuth);
% Recompute cone mesh (mirrors cone.plot logic)
maxAlt = obj.fovGeometry.surface(1).Parent.ZLim(2);
scalingFactor = maxAlt / obj.fovGeometry.height;
[X, Y, Z] = cylinder([scalingFactor * obj.fovGeometry.radius, 0], obj.fovGeometry.n);
Z = Z * maxAlt;
Ry = [cosd(obj.fovGeometry.tilt), 0, -sind(obj.fovGeometry.tilt); 0, 1, 0; sind(obj.fovGeometry.tilt), 0, cosd(obj.fovGeometry.tilt)];
Rz = [sind(obj.fovGeometry.azimuth), -cosd(obj.fovGeometry.azimuth), 0; cosd(obj.fovGeometry.azimuth), sind(obj.fovGeometry.azimuth), 0; 0, 0, 1];
R = Rz * Ry;
pts = R * [X(:)'; Y(:)'; Z(:)' - maxAlt];
X = reshape(pts(1, :), size(X)) + obj.pos(1);
Y = reshape(pts(2, :), size(Y)) + obj.pos(2);
Z = reshape(pts(3, :) + maxAlt, size(Z)) + obj.pos(3) - maxAlt;
for jj = 1:size(obj.fovGeometry.surface, 2)
obj.fovGeometry.surface(jj).XData = X;
obj.fovGeometry.surface(jj).YData = Y;
obj.fovGeometry.surface(jj).ZData = Z;
end
end
end
end