44 lines
1.6 KiB
Matlab
44 lines
1.6 KiB
Matlab
function validate(obj)
|
|
arguments (Input)
|
|
obj (1, 1) {mustBeA(obj, "miSim")};
|
|
end
|
|
arguments (Output)
|
|
end
|
|
|
|
%% Mode Validators
|
|
if obj.useRoutingTopology && ~obj.useSinrComms
|
|
error("Routing topology requires SINR comms mode (useSinrComms)");
|
|
end
|
|
|
|
if obj.useRoutingTopology && obj.useFixedTopology
|
|
error("useRoutingTopology and useFixedTopology are mutually exclusive topology selectors");
|
|
end
|
|
|
|
%% Communications Network Validators
|
|
if max(conncomp(graph(obj.adjacency))) ~= 1
|
|
error("Network is not connected");
|
|
end
|
|
|
|
% In routing (CALSN) mode the MAINTAINED topology itself must form a
|
|
% single connected component (guaranteed by the connectivity bridge)
|
|
if obj.useRoutingTopology && max(conncomp(graph(obj.constraintAdjacencyMatrix))) ~= 1
|
|
error("Maintained routing topology (A_control) is not connected");
|
|
end
|
|
|
|
if any(obj.adjacency - obj.constraintAdjacencyMatrix < 0, "all")
|
|
error("Eliminated network connections that were necessary");
|
|
end
|
|
|
|
%% Obstacle Validators
|
|
% Agent-Obstacle Collision Detection
|
|
for jj = 1:size(obj.obstacles, 1)
|
|
for kk = 1:size(obj.agents, 1)
|
|
P = min(max(obj.agents{kk}.pos, obj.obstacles{jj}.minCorner), obj.obstacles{jj}.maxCorner);
|
|
d = obj.agents{kk}.pos - P;
|
|
if dot(d, d) < obj.agents{kk}.collisionGeometry.radius^2 - 1e-3
|
|
error("%s colliding with %s by %d", obj.agents{kk}.label, obj.obstacles{jj}.label, - dot(d, d) + obj.agents{kk}.collisionGeometry.radius^2); % this will cause quadprog to fail
|
|
end
|
|
end
|
|
end
|
|
end
|