motion_planning/findPointIndex.m

12 lines
350 B
Matlab

function idx = findPointIndex(pointMap, point)
% Find the index of a point in the pointMap
keys = cell2mat(pointMap.keys);
values = cell2mat(pointMap.values);
for i = 1:length(keys)
if isequal(values(:, i), point)
idx = keys(i);
return;
end
end
error('Point not found in the map.');
end