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

created project file

Browse Source
This commit is contained in:
Kevin D
2025-10-24 22:59:30 -07:00
parent 371656fb39
commit 7fff074a5c
71 changed files with 250 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files

18
geometries/REGION_TYPE.m Normal file
View File

@@ -0,0 +1,18 @@
classdef REGION_TYPE
properties
id
color
end
enumeration
INVALID (0, [255, 127, 255]); % default value
DOMAIN (1, [0, 0, 0]); % domain region
OBSTACLE (2, [255, 127, 127]); % obstacle region
COLLISION (3, [255, 255, 128]); % collision avoidance region
end
methods
function obj = REGION_TYPE(id, color)
obj.id = id;
obj.color = color./ 255;
end
end
end

106
geometries/rectangularPrismConstraint.m Normal file
View File

@@ -0,0 +1,106 @@
classdef rectangularPrismConstraint
% Rectangular prism constraint geometry
properties (SetAccess = private, GetAccess = public)
tag = REGION_TYPE.INVALID;
label = "";
minCorner = NaN(3, 1);
maxCorner = NaN(3, 1);
dimensions = NaN(3, 1);
center = NaN;
vertices = NaN(8, 3);
footprint = NaN(2, 4);
end
methods (Access = public)
function obj = initialize(obj, bounds, tag, label)
arguments (Input)
obj (1, 1) {mustBeA(obj, 'rectangularPrismConstraint')};
bounds (3, 2) double;
tag (1, 1) REGION_TYPE = REGION_TYPE.INVALID;
label (1, 1) string = "";
end
arguments (Output)
obj (1, 1) {mustBeA(obj, 'rectangularPrismConstraint')};
end
obj.tag = tag;
obj.label = label;
%% Define geometry bounds by LL corner and UR corner
obj.minCorner = bounds(:, 1);
obj.maxCorner = bounds(:, 2);
% Compute L, W, H
obj.dimensions = [obj.maxCorner(1) - obj.minCorner(1), obj.maxCorner(2) - obj.minCorner(2), obj.maxCorner(3) - obj.minCorner(3)];
% Compute center
obj.center = obj.minCorner + obj.dimensions ./ 2;
% Compute vertices
obj.vertices = [obj.minCorner';
obj.maxCorner(1), obj.minCorner(2:3)';
obj.maxCorner(1:2)', obj.minCorner(3);
obj.minCorner(1), obj.maxCorner(2), obj.minCorner(3);
obj.minCorner(1:2)', obj.maxCorner(3);
obj.maxCorner(1), obj.minCorner(2), obj.maxCorner(3);
obj.minCorner(1), obj.maxCorner(2:3)'
obj.maxCorner';];
% Compute footprint
obj.footprint = [obj.minCorner(1:2, 1), ...
[obj.minCorner(1, 1); obj.maxCorner(2, 1)], ...
[obj.maxCorner(1, 1); obj.minCorner(2, 1)], ...
obj.maxCorner(1:2, 1)];
end
function r = random(obj)
arguments (Input)
obj (1, 1) {mustBeA(obj, 'rectangularPrismConstraint')};
end
arguments (Output)
r (1, 3) double
end
r = (obj.vertices(1, 1:3) + rand(1, 3) .* obj.vertices(8, 1:3) - obj.vertices(1, 1:3))';
end
function c = contains(obj, pos)
arguments (Input)
obj (1, 1) {mustBeA(obj, 'rectangularPrismConstraint')};
pos (:, 3) double;
end
arguments (Output)
c (1, 1) logical
end
c = all(pos >= repmat(obj.minCorner', size(pos, 1), 1), 2) & all(pos <= repmat(obj.maxCorner', size(pos, 1), 1), 2);
end
function f = plotWireframe(obj, f)
arguments (Input)
obj (1, 1) {mustBeA(obj, 'rectangularPrismConstraint')};
f (1, 1) {mustBeA(f, 'matlab.ui.Figure')} = figure;
end
arguments (Output)
f (1, 1) {mustBeA(f, 'matlab.ui.Figure')};
end
% Create axes if they don't already exist
f = firstPlotSetup(f);
edges = [1 2; 2 3; 3 4; 4 1; % bottom square
5 6; 6 8; 8 7; 7 5; % top square
1 5; 2 6; 3 8; 4 7]; % vertical edges
% Create plotting inputs from vertices and edges
X = [obj.vertices(edges(:,1),1), obj.vertices(edges(:,2),1)]';
Y = [obj.vertices(edges(:,1),2), obj.vertices(edges(:,2),2)]';
Z = [obj.vertices(edges(:,1),3), obj.vertices(edges(:,2),3)]';
% Plot the boundaries of the constraint geometry
hold(f.CurrentAxes, "on");
plot3(X, Y, Z, '-', 'Color', obj.tag.color, 'LineWidth', 2);
hold(f.CurrentAxes, "off");
end
end
end