motion_planning/findClosestPoint.m

6 lines
274 B
Matlab

function [closestPoint, closestIdx] = findClosestPoint(tree, point)
% Finds the closest point in the tree to a given random point
distances = sqrt(sum((tree - point).^2, 1));
[minDistance, closestIdx] = min(distances);
closestPoint = tree(:, closestIdx);
end