function [axes] = getAxes(platformVertices)
for i = 1:size(platformVertices,1)
%Get the normals of the edges
axes(i, 1) = (platformVertices(mod(i, size(platformVertices,1)) + 1, 2) - platformVertices(i,2));
axes(i, 2) = -(platformVertices(mod(i, size(platformVertices,1)) + 1, 1) - platformVertices(i,1));
axes(i, :) = axes(i, :) / norm(axes(i, :));
end
end
function [projection] = project2D(vertices, ax)
min = dot(ax, vertices(1, :));
max = min;
for i = 2:size(vertices,1)
temp = dot(ax, vertices(i, :));
if temp < min
min = temp;
elseif temp > max
max = temp;
end
end
projection = [min, max];
end
function [overlap, minOverlap, maxOverlap] = findOverlap(projectionA, projectionB)
if projectionB(2) <= projectionA(1) || ...
projectionA(2) <= projectionB(1)
%Separating Axis Found
overlap = 0;
minOverlap = 0;
maxOverlap = 0;
else
overlap = 1;
minOverlap = max([projectionA(1), projectionB(1)]); %Get the biggest min
maxOverlap = min([projectionA(2), projectionB(2)]); %Get the smallest max
end
end
%% The start scipt is below
%Squares
plat = [-1 1; -1 -1; 1 -1; 1 1];
offset = 2 * ones(4,2);
plat1 = plat + .75 * [1 0; 1 0; 1 0; 1 0] + offset;
plat2 = plat - .5 * [1 1; 1 1; 1 1; 1 1] + offset;
plat3 = plat + .5 * [1 1; 1 1; 1 1; 1 1] + offset;
%Plotting stuff
figure
axis equal
%plat1
line([plat1(1,1) plat1(2,1)], [plat1(1,2) plat1(2,2)], 'Color', [0 1 0]);
line([plat1(2,1) plat1(3,1)], [plat1(2,2) plat1(3,2)], 'Color', [0 1 0]);
line([plat1(3,1) plat1(4,1)], [plat1(3,2) plat1(4,2)], 'Color', [0 1 0]);
line([plat1(4,1) plat1(1,1)], [plat1(4,2) plat1(1,2)], 'Color', [0 1 0]);
%plat2
line([plat2(1,1) plat2(2,1)], [plat2(1,2) plat2(2,2)], 'Color', [1 0 0]);
line([plat2(2,1) plat2(3,1)], [plat2(2,2) plat2(3,2)], 'Color', [1 0 0]);
line([plat2(3,1) plat2(4,1)], [plat2(3,2) plat2(4,2)], 'Color', [1 0 0]);
line([plat2(4,1) plat2(1,1)], [plat2(4,2) plat2(1,2)], 'Color', [1 0 0]);
%plat3
line([plat3(1,1) plat3(2,1)], [plat3(1,2) plat3(2,2)], 'Color', [0 0 1]);
line([plat3(2,1) plat3(3,1)], [plat3(2,2) plat3(3,2)], 'Color', [0 0 1]);
line([plat3(3,1) plat3(4,1)], [plat3(3,2) plat3(4,2)], 'Color', [0 0 1]);
line([plat3(4,1) plat3(1,1)], [plat3(4,2) plat3(1,2)], 'Color', [0 0 1]);
%End Plotting stuff
wedgeX = [plat1(:,1) plat2(:,1) plat3(:,1)];
wedgeY = [plat1(:,2) plat2(:,2) plat3(:,2)];
NumFaces = max([size(plat1, 1), size(plat2, 1), size(plat3, 1)]);
NumDim = 2;
NumPlatforms = 3;
axes = NaN(NumFaces, NumDim, NumPlatforms);
%For a wedge we only have to find the normals of 3 sides since 2 sides are
%parallel
for i = 1:NumPlatforms
axes(:, :, i) = getAxes([wedgeX(1:4, i) wedgeY(1:4, i)]);
end
overlaps = NaN(NumFaces * NumPlatforms, 4);
for i = 1:NumPlatforms
for j = 1:NumFaces
projA = project2D([wedgeX(:, i), wedgeY(:, i)], axes(j, :, i));
projB = project2D([wedgeX(:, mod(i, NumPlatforms) + 1), wedgeY(:, mod(i, NumPlatforms) + 1)], axes(j, :, i));
[overlap, minOverlap, maxOverlap] = findOverlap(projA, projB);
line([axes(j, 1, i) * -3, axes(j, 1, i) * 3], [axes(j, 2, i) * -3, axes(j, 2, i) * 3]);
if overlap == 0
'Not overlapping!'
return
else
% [overlaps((i-1) * NumFaces + j, :)] = project2D([minOverlap, maxOverlap], [axes(j, 2, i), -axes(j, 1, i)]);
%startOfLine
overlaps((i - 1) * NumFaces + j, 1) = minOverlap * axes(j, 1, i);
overlaps((i - 1) * NumFaces + j, 2) = minOverlap * axes(j, 2, i);
%endOfLine
overlaps((i - 1) * NumFaces + j, 3) = maxOverlap * axes(j, 1, i);
overlaps((i - 1) * NumFaces + j, 4) = maxOverlap * axes(j, 2, i);
hold on
% plot(mean(overlaps(:, 1)), mean(overlaps(:, 2)), '*r');
plot(overlaps(:, 1), overlaps(:, 2), 'xr');
plot(overlaps(:, 3), overlaps(:, 4), 'og');
midpoint = [overlaps(:,1) + (overlaps(:, 3) - overlaps(:,1)) / 2, overlaps(:,2) + (overlaps(:, 4) - overlaps(:,2)) / 2];
plot(midpoint(:,1), midpoint(:,2), 'dk');
end
end
end
hold on
plot(mean(midpoint((midpoint(:,1) ~= 0),1)), mean(midpoint((midpoint(:,2) ~= 0),2)), 'm*');
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