%% Ray in a polygon
% Definition of the four vertices
v1 = [-3 -4]';  % lowerleft
v2 = [2 -3]';   % lowerright
v3 = [1 2]';    % upperright
v4 = [-2 1]';   % upperleft

% Definition of the polygon
polygon = [v1 v2 v3 v4 v1];
u1 = v2-v1;
u2 = v3-v2;
u3 = v4-v3;
u4 = v1-v4;

% Rotation matix
r90 = [0 -1
       1  0];

% Get vector perpendicular to sides
n1 = r90*u1/norm(u1);
n2 = r90*u2/norm(u2);
n3 = r90*u3/norm(u3);
n4 = r90*u4/norm(u4);

% Correct scale
scale=1.2*max(max(abs(polygon)));
figure(gcf)

% Clear prev plot
clf
axis([-scale scale -scale scale]), axis ('equal')
hold on
%% Plot the polygon
plot(polygon(1,:),polygon(2,:))
r0=[0;0];
r=r0;
c=[.3;.2];
%c=[0.34 0.27]';
dt=.1*scale;
kmax=100;
rg=r0;
for t=0:dt:dt*kmax
    r=r+c*dt;
    pause(0.01)

    % Negative dot product means outside polygone
    if dot(n1,r-v1)<=0
        cn=dot(c,n1)*n1;
        c=c-2*cn;
        r=rg;
    elseif dot(n2,r-v2)<=0
        cn=dot(c,n2)*n2;
        c=c-2*cn;
        r=rg;
    elseif dot(n3,r-v3)<=0
        cn=dot(c,n3)*n3;
        c=c-2*cn;
        r=rg;
    elseif dot(n4,r-v4)<=0
        cn=dot(c,n4)*n4;
        c=c-2*cn;
        r=rg;
    else
        rg=r;
    end
    plot(r(1),r(2),'.r')
end
hold off
title('Ray reflection in a polygone')
xlabel('x-axis, distance [m]')
ylabel('y-axis, distance [m]')