42 lines
1.3 KiB
Matlab
42 lines
1.3 KiB
Matlab
function g = distanceGradient(obj, pos)
|
|
arguments (Input)
|
|
obj (1, 1) {mustBeA(obj, 'rectangularPrism')};
|
|
pos (:, 3) double;
|
|
end
|
|
arguments (Output)
|
|
g (:, 3) double
|
|
end
|
|
|
|
% find nearest point on surface to query position
|
|
q = min(max(pos, obj.minCorner), obj.maxCorner);
|
|
|
|
% Find distance and direction between pos and q
|
|
v = pos - q;
|
|
vNorm = norm(v);
|
|
|
|
% position is outside geometry
|
|
if vNorm > 0
|
|
% gradient is normalized vector from q to p
|
|
g = v / vNorm;
|
|
return;
|
|
end
|
|
|
|
% position is on or in geometry
|
|
% find distances to each face in each dimension
|
|
distances = [pos(1) - obj.minCorner(1), obj.maxCorner(1) - pos(1), pos(2) - obj.minCorner(2), obj.maxCorner(2) - pos(2), pos(3) - obj.minCorner(3), obj.maxCorner(3) - pos(3)];
|
|
[~, idx] = min(distances);
|
|
|
|
% I think there needs to be additional handling here for the
|
|
% edge/corner cases, where there are ways to balance or resolve ties
|
|
% when two faces are equidistant to the query position
|
|
assert(sum(idx) == idx, "Implement edge case handling");
|
|
|
|
% select gradient that brings us quickest to the nearest face
|
|
g = [ 1, 0, 0; ...
|
|
-1, 0, 0; ...
|
|
0, 1, 0; ...
|
|
0, -1, 0; ...
|
|
0, 0, 1; ...
|
|
0, 0, -1;];
|
|
g = g(idx, :);
|
|
end |