function ijk = points_to_idx(map, xyz)
% convert 3d points xyz (Nx3) into indices in occupancy grid
% @author: Yiren Lu
% @date: 02/13/2017
% @input:   map         map generated by load_map
%           xyz         Nx3 3d points in actual space
% @output:  ijk         Nx3 indices in map's occupancy grid
ijk = ones(size(xyz,1),3);
if size(xyz,1) > 0
ijk(:,1) = min(max(floor((xyz(:,1) - map.boundary(1))/map.xy_res + 1),1), map.nx);
ijk(:,2) = min(max(floor((xyz(:,2) - map.boundary(2))/map.xy_res + 1),1), map.ny);
ijk(:,3) = min(max(floor((xyz(:,3) - map.boundary(3))/map.z_res + 1),1), map.nz);
end
end