%% Get Ray Intersection Points on Occupancy Grid
% Get the ray intersection points on an occupancy grid that has obstacles in the map. The rays are defined ranges and angles from a starting robot pose.
% Create an occupancy grid map. Add obstacles and inflate them. A lower resolution map is used to illustrate the importance of using grid cells. Show the map.

map = robotics.OccupancyGrid(10,10,2);
obstacles = [4 10; 3 5; 7 7];
setOccupancy(map,obstacles,ones(length(obstacles),1))
inflate(map,0.25)
show(map)

%
% Find the intersection point for rays that emit from the given robot pose. The max range and angles for these rays are specified. The last ray does not intersect with an obstacle within the max range, thus it has no collision point.

maxrange = 6;
angles = [pi/4,-pi/4,0,-pi/8];
robotPose = [4,4,pi/2];
intsectionPts = rayIntersection(map,robotPose,angles,maxrange,0.7)

%
% Plot the intersection points and rays from the pose.

hold on
plot(intsectionPts(:,1),intsectionPts(:,2) , '*r') % Intersection points
plot(robotPose(1),robotPose(2),'ob') % Robot pose
for i = 1:3
    plot([robotPose(1),intsectionPts(i,1)],...
        [robotPose(2),intsectionPts(i,2)],'-b') % Plot intersecting rays
end
plot([robotPose(1),robotPose(1)-6*sin(angles(4))],...
    [robotPose(2),robotPose(2)+6*cos(angles(4))],'-b') % No intersection ray

legend('Collision Points','Robot Position','Rays','Location','SouthEast')

%
% Copyright 2012 The MathWorks, Inc.