diff --git a/@miSim/initializeFromInits.m b/@miSim/initializeFromInits.m new file mode 100644 index 0000000..792ec27 --- /dev/null +++ b/@miSim/initializeFromInits.m @@ -0,0 +1,87 @@ +function obj = initializeFromInits(obj, initsPath) +% INITIALIZEFROMINITS Initialize miSim from a saved simInits matfile. +% +% Loads all simulation parameters and initial agent states written by +% writeInits(), reconstructs domain, objective, agents, and obstacles, then +% calls the standard obj.initialize() method. Plots and video are disabled. +% +% Usage: +% sim = sim.initializeFromInits('sandbox/2025_01_01_12_00_00_miSimInits.mat'); + +arguments (Input) + obj (1, 1) {mustBeA(obj, 'miSim')}; + initsPath (1, 1) string; +end +arguments (Output) + obj (1, 1) {mustBeA(obj, 'miSim')}; +end + +inits = load(initsPath); + +% ---- Build domain ------------------------------------------------------------ +dom = rectangularPrism; +dom = dom.initialize([inits.domainMin; inits.domainMax], REGION_TYPE.DOMAIN, "Domain"); + +% ---- Build sensing objective ------------------------------------------------- +dom.objective = sensingObjective; +% reshape guards against MATLAB flattening the 1×2×2 singleton dimension on load +objSigma = reshape(inits.objectiveSigma, [1 2 2]); +objFcn = objectiveFunctionWrapper(inits.objectivePos, objSigma); +dom.objective = dom.objective.initialize(objFcn, dom, ... + inits.discretizationStep, inits.protectedRange, inits.sensorPerformanceMinimum, ... + inits.objectivePos, objSigma); + +% ---- Build agents ------------------------------------------------------------ +numAgents = inits.numAgents; +agentList = cell(numAgents, 1); +for ii = 1:numAgents + pos = inits.pos(ii, :); + + sensor = sigmoidSensor; + sensor = sensor.initialize(inits.alphaDist(ii), inits.betaDist(ii), ... + inits.alphaTilt(ii), inits.betaTilt(ii)); + + geom = spherical; + geom = geom.initialize(pos, inits.collisionRadius(ii), REGION_TYPE.COLLISION, ... + sprintf("UAV %d Collision", ii)); + ag = agent; + ag = ag.initialize(pos, geom, sensor, inits.comRange(ii), inits.maxIter, ... + inits.initialStepSize(ii), sprintf("UAV %d", ii)); + agentList{ii} = ag; +end + +% ---- Build obstacles --------------------------------------------------------- +numObstacles = inits.numObstacles; +obstacleList = cell(numObstacles, 1); +if numObstacles > 0 + for ii = 1:numObstacles + obs = rectangularPrism; + obs = obs.initialize([inits.obsMinCorners(ii, :); inits.obsMaxCorners(ii, :)], ... + REGION_TYPE.OBSTACLE, sprintf("Obstacle %d", ii)); + obstacleList{ii} = obs; + end +end + +% ---- Optional backward-compat fields ----------------------------------------- +if isfield(inits, 'useDoubleIntegrator') + useDoubleIntegrator = logical(inits.useDoubleIntegrator); +else + useDoubleIntegrator = false; +end +if isfield(inits, 'dampingCoeff') + dampingCoeff = inits.dampingCoeff; +else + dampingCoeff = 2.0; +end +if isfield(inits, 'useFixedTopology') + useFixedTopology = logical(inits.useFixedTopology); +else + useFixedTopology = false; +end + +% ---- Initialize simulation (plots and video disabled) ------------------------ +obj = obj.initialize(dom, agentList, inits.barrierGain, inits.barrierExponent, ... + inits.minAlt, inits.timestep, inits.maxIter, obstacleList, ... + false, false, useDoubleIntegrator, dampingCoeff, useFixedTopology); + +end diff --git a/@miSim/miSim.m b/@miSim/miSim.m index 37806a7..55f3545 100644 --- a/@miSim/miSim.m +++ b/@miSim/miSim.m @@ -71,6 +71,7 @@ classdef miSim end [obj] = initialize(obj, domain, agents, barrierGain, barrierExponent, minAlt, timestep, maxIter, obstacles, makePlots, makeVideo); [obj] = initializeFromCsv(obj, csvPath); + [obj] = initializeFromInits(obj, initsPath); [obj] = run(obj); [obj] = lesserNeighbor(obj); [obj] = constrainMotion(obj); diff --git a/@miSim/writeInits.m b/@miSim/writeInits.m index 56543aa..baab094 100644 --- a/@miSim/writeInits.m +++ b/@miSim/writeInits.m @@ -5,28 +5,35 @@ function writeInits(obj) arguments (Output) end + % User-supplied obstacles only: initialize() appends a floor obstacle at + % the end when minAlt > 0, so exclude it here to avoid double-counting on + % reconstruction (initializeFromInits re-adds the floor via minAlt). + numInputObs = size(obj.obstacles, 1) - (obj.minAlt > 0); + userObstacles = obj.obstacles(1:numInputObs); + % Collect agent parameters collisionRadii = cellfun(@(x) x.collisionGeometry.radius, obj.agents); alphaDist = cellfun(@(x) x.sensorModel.alphaDist, obj.agents); betaDist = cellfun(@(x) x.sensorModel.betaDist, obj.agents); alphaTilt = cellfun(@(x) x.sensorModel.alphaTilt, obj.agents); - betaTilt = cellfun(@(x) x.sensorModel.alphaDist, obj.agents); + betaTilt = cellfun(@(x) x.sensorModel.betaTilt, obj.agents); comRanges = cellfun(@(x) x.commsGeometry.radius, obj.agents); initialStepSize = cellfun(@(x) x.initialStepSize, obj.agents); pos = cell2mat(cellfun(@(x) x.pos, obj.agents, 'UniformOutput', false)); - obsMinCorners = cell2mat(cellfun(@(x) x.minCorner, obj.obstacles, 'UniformOutput', false)); - obsMaxCorners = cell2mat(cellfun(@(x) x.maxCorner, obj.obstacles, 'UniformOutput', false)); + obsMinCorners = cell2mat(cellfun(@(x) x.minCorner, userObstacles, 'UniformOutput', false)); + obsMaxCorners = cell2mat(cellfun(@(x) x.maxCorner, userObstacles, 'UniformOutput', false)); % Combine with simulation parameters - inits = struct("timestep", obj.timestep, "maxIter", obj.maxIter, "minAlt", obj.obstacles{end}.maxCorner(3), ... + inits = struct("timestep", obj.timestep, "maxIter", obj.maxIter + 1, "minAlt", obj.minAlt, ... "discretizationStep", obj.domain.objective.discretizationStep, "protectedRange", obj.domain.objective.protectedRange, ... "sensorPerformanceMinimum", obj.domain.objective.sensorPerformanceMinimum, "initialStepSize", initialStepSize, ... - "barrierGain", obj.barrierGain, "barrierExponent", obj.barrierExponent, "numObstacles", size(obj.obstacles, 1), ... + "barrierGain", obj.barrierGain, "barrierExponent", obj.barrierExponent, "numObstacles", numInputObs, ... "numAgents", size(obj.agents, 1), "collisionRadius", collisionRadii, "comRange", comRanges, ... - "useDoubleIntegrator", obj.useDoubleIntegrator, "dampingCoeff", obj.dampingCoeff, ... + "useDoubleIntegrator", obj.useDoubleIntegrator, "dampingCoeff", obj.dampingCoeff, "useFixedTopology", obj.useFixedTopology, ... "alphaDist", alphaDist, "betaDist", betaDist, "alphaTilt", alphaTilt, "betaTilt", betaTilt, ... ... % ^^^ PARAMETERS ^^^ | vvv STATES vvv "pos", pos, "objectivePos", obj.domain.objective.groundPos, "objectiveSigma", obj.domain.objective.objectiveSigma, ... + "domainMin", obj.domain.minCorner, "domainMax", obj.domain.maxCorner, ... "obsMinCorners", obsMinCorners, "obsMaxCorners", obsMaxCorners, ... "objectiveIntegral", sum(obj.domain.objective.values(:))); @@ -34,4 +41,4 @@ function writeInits(obj) initsFile = strcat(obj.artifactName, "_miSimInits"); initsFile = fullfile(matlab.project.rootProject().RootFolder, "sandbox", initsFile); save(initsFile, "-struct", "inits"); -end \ No newline at end of file +end diff --git a/test/test_miSim.m b/test/test_miSim.m index 4f474a2..dbef0a4 100644 --- a/test/test_miSim.m +++ b/test/test_miSim.m @@ -728,6 +728,128 @@ classdef test_miSim < matlab.unittest.TestCase 0, 0, 0, 1, 1, 1, 1; 0, 0, 0, 0, 0, 1, 1; ])); end + function miSim_initializeFromInits(tc) + % Build a minimal valid simulation, write it to a matfile, reload + % via initializeFromInits, assert round-trip consistency, then + % delete the file. No plotting or video at any stage. + + % Obstacles + nGeom = tc.minNumObstacles + randi(tc.maxNumObstacles - tc.minNumObstacles); + tc.obstacles = cell(nGeom, 1); + for ii = 1:nGeom + badCandidate = true; + while badCandidate + tc.obstacles{ii} = rectangularPrism; + tc.obstacles{ii} = tc.obstacles{ii}.initializeRandom(REGION_TYPE.OBSTACLE, ... + sprintf("Obstacle %d", ii), tc.minObstacleSize, tc.maxObstacleSize, ... + tc.domain, tc.minAlt); + if ~tc.obstacleCollisionCheck(tc.obstacles(1:(ii - 1)), tc.obstacles{ii}) + badCandidate = false; + end + end + end + + % Agents + nAgents = tc.minAgents; + tc.agents = cell(nAgents, 1); + tc.collisionRanges = tc.minCollisionRange + rand(nAgents, 1) * (tc.maxCollisionRange - tc.minCollisionRange); + tc.commsRanges = tc.minCommsRange + rand(nAgents, 1) * (tc.maxCommsRange - tc.minCommsRange); + + for ii = 1:nAgents + initInvalid = true; + while initInvalid + if ii == 1 + candidatePos = tc.domain.random(); + candidatePos(3) = tc.minAlt + rand * 3; + while agentsCrowdObjective(tc.domain.objective, candidatePos, mean(tc.domain.dimensions) / 2) + candidatePos = tc.domain.random(); + candidatePos(3) = tc.minAlt + rand * 3; + end + else + candidatePos = tc.agents{randi(ii - 1)}.pos + sign(randn([1, 3])) .* (rand(1, 3) .* tc.commsRanges(ii) / sqrt(2)); + candidatePos(3) = tc.minAlt + rand * 3; + end + + if ~tc.domain.contains(candidatePos), continue; end + if agentsCrowdObjective(tc.domain.objective, candidatePos, mean(tc.domain.dimensions) / 2), continue; end + + % Connectivity check + connections = false(1, ii - 1); + for jj = 1:(ii - 1) + if norm(tc.agents{jj}.pos - candidatePos) <= min(tc.commsRanges([ii, jj])) + connections(jj) = true; + for kk = 1:size(tc.obstacles, 1) + if tc.obstacles{kk}.containsLine(tc.agents{jj}.pos, candidatePos) + connections(jj) = false; + end + end + end + end + if ii ~= 1 && ~any(connections), continue; end + + geom = spherical; + geom = geom.initialize(candidatePos, tc.collisionRanges(ii), REGION_TYPE.COLLISION); + tc.sensor = sigmoidSensor; + tc.sensor = tc.sensor.initialize( ... + tc.alphaDistMin + rand * (tc.alphaDistMax - tc.alphaDistMin), ... + tc.betaDistMin + rand * (tc.betaDistMax - tc.betaDistMin), ... + tc.alphaTiltMin + rand * (tc.alphaTiltMax - tc.alphaTiltMin), ... + tc.betaTiltMin + rand * (tc.betaTiltMax - tc.betaTiltMin)); + newAgent = agent; + newAgent = newAgent.initialize(candidatePos, geom, tc.sensor, tc.commsRanges(ii), tc.maxIter, tc.initialStepSize); + + % Domain / obstacle / agent collision checks + violation = false; + for jj = 1:size(newAgent.collisionGeometry.vertices, 1) + if ~tc.domain.contains(newAgent.collisionGeometry.vertices(jj, 1:3)) + violation = true; break; + end + end + if violation, continue; end + for kk = 1:size(tc.obstacles, 1) + if geometryIntersects(tc.obstacles{kk}, newAgent.collisionGeometry) + violation = true; break; + end + end + if violation, continue; end + for kk = 1:(ii - 1) + if geometryIntersects(tc.agents{kk}.collisionGeometry, newAgent.collisionGeometry) + violation = true; break; + end + end + if newAgent.pos(3) - newAgent.collisionGeometry.radius <= tc.minAlt + violation = true; + end + if violation, continue; end + + initInvalid = false; + tc.agents{ii} = newAgent; + end + end + + % Initialize first sim (no plots / video) + sim1 = miSim; + sim1 = sim1.initialize(tc.domain, tc.agents, tc.barrierGain, tc.barrierExponent, ... + tc.minAlt, tc.timestep, tc.maxIter, tc.obstacles, false, false, ... + tc.useDoubleIntegrator, tc.dampingCoeff, tc.useFixedTopology); + + % Write inits and build file path + sim1.writeInits(); + initsFile = fullfile(matlab.project.rootProject().RootFolder, "sandbox", ... + strcat(sim1.artifactName, "_miSimInits.mat")); + + % Load via initializeFromInits + sim2 = miSim; + sim2 = sim2.initializeFromInits(initsFile); + + % Assertions + tc.assertEqual(size(sim2.agents, 1), size(sim1.agents, 1)); + tc.assertEqual(sim2.maxIter, sim1.maxIter); + tc.assertEqual(sim2.barrierGain, sim1.barrierGain); + + % Cleanup + delete(initsFile); + end end methods