Initializing domain, obstacles, objective, and agents

validators/mustBeAgents.m

@@ -0,0 +1,10 @@
+function mustBeAgents(agents)
+    validGeometries = ["rectangularPrismConstraint";];
+    if isa(agents, 'cell')
+        for ii = 1:size(agents, 1)
+            assert(isa(agents{ii}, "agent"), "Agent in index %d is not a valid agent class", ii);
+        end
+    else
+        assert(isa(agents, validGeometries), "Agent is not a valid agent class");
+    end
+end

validators/mustBeConstraintGeometries.m

@@ -0,0 +1,10 @@
+function mustBeConstraintGeometries(constraintGeometry)
+    validGeometries = ["rectangularPrismConstraint";];
+    if isa(constraintGeometry, 'cell')
+        for ii = 1:size(constraintGeometry, 1)
+            assert(isa(constraintGeometry{ii}, validGeometries), "Constraint geometry in index %d is not a valid constraint geometry class", ii);
+        end
+    else
+        assert(isa(constraintGeometry, validGeometries), "Constraint geometry is not a valid constraint geometry class");
+    end
+end

validators/mustBeDcm.m

@@ -0,0 +1,12 @@
+function mustBeDcm(dcm)
+    % Assert 2D
+    assert(numel(size(dcm)) == 2, "DCM is not 2D");
+    % Assert square
+    assert(size(unique(size(dcm)), 1) == 1, "DCM is not a square matrix");
+
+    epsilon = 1e-9;
+    % Assert inverse equivalent to transpose
+    assert(all(abs(inv(dcm) - dcm') < epsilon, "all"), "DCM inverse is not equivalent to transpose");
+    % Assert determinant is 1
+    assert(det(dcm) > 1 - epsilon && det(dcm) < 1 + epsilon, "DCM has determinant not equal to 1");
+end