46 lines
1.3 KiB
Matlab
46 lines
1.3 KiB
Matlab
function [dh, dv] = wallDistance(corners, x, y)
|
|
% Calculates the distances from the walls.
|
|
% - Negative value means colision
|
|
% - Inf value means no wall in this direction
|
|
%
|
|
% Inputs:
|
|
% corners[] : corner matrix (each line is a corner point)
|
|
% x: Car's x position
|
|
% y: Car's y position
|
|
% Outputs:
|
|
% dh: Horizontal distance from wall
|
|
% dv: Vertical distance from wall
|
|
|
|
% Calculate the distances in the x direction
|
|
srt_corners = sortrows(corners, 2);
|
|
active_corners = -Inf(size(corners));
|
|
|
|
for i=1:length(srt_corners)
|
|
if srt_corners(i, 2) >= y
|
|
active_corners = srt_corners(i:end, :);
|
|
break;
|
|
end
|
|
end
|
|
dx = active_corners(:, 1) - x; % Walls are on the right
|
|
dh = min(dx); % Minimum distance in x direction
|
|
|
|
% Calculate the distances in the y direction
|
|
srt_corners = sortrows(corners, 1, 'descend');
|
|
active_corners = -Inf(size(corners));
|
|
|
|
for i=1:length(srt_corners)
|
|
if srt_corners(i, 1) <= x
|
|
active_corners = srt_corners(i:end, :);
|
|
break;
|
|
end
|
|
end
|
|
dy = y - active_corners(:, 2); % Walls are on the bottom
|
|
dv = min(dy); % Minimum distance in y direction
|
|
|
|
if ~isfinite(dh) dh = Inf; end
|
|
if ~isfinite(dv) dv = Inf; end
|
|
end
|
|
|
|
|
|
|