function separationDist = vppblock_CheckCollision(geom1, geom2)
%#codegen
% This is a utility function for collision checking of the Vehicle Path
% Planner (VPP) block. 
%
% Simplified collision detection routine: detect if two objects collide and
% find the minimal distance in case of collision
%
%   Assumption:
%       1: The objects are both of rectangle-shapes
%       2: The minimal distance approximates the real minimal distance.
%
%   Algorithm:
%       This routine uses the separating axis theorem to check if two
%       objects are collided or not.
%       In case of non-collision, the minimal distance is approximated by
%       the projected distance of the objects onto the separating axes.
%
%   Inputs:
%       geom1, geom2: vector of the centroid position (x,y), orientation
%       (yaw), and the size (length,width)
%
%   Output:
%       separationDist: approximated minimal distance of the two objects,
%       -1 if collided
%
% see also: vppblock_CreateNonlinearMPC, vppblock_IneqConFcn

% Copyright 2021-2022 The MathWorks, Inc.

%% Get vertices of each object
[xVtx1,yVtx1] = getVertex(geom1);
[xVtx2,yVtx2] = getVertex(geom2);

%% Rotate both objects to align first object vertically (yaw = pi/2)
[xRot11,yRot11] = rotate(xVtx1,yVtx1,geom1(1),geom1(2),0.5*pi-geom1(3));
[xRot21,yRot21] = rotate(xVtx2,yVtx2,geom1(1),geom1(2),0.5*pi-geom1(3));
% check for collision in this orientation
[isCollided1,dist1] = customCheckCollision(xRot11,yRot11,xRot21,yRot21);

%% Rotate both objects to align second object vertically (yaw = pi/2)
[xRot12,yRot12] = rotate(xVtx1,yVtx1,geom2(1),geom2(2),0.5*pi-geom2(3));
[xRot22,yRot22] = rotate(xVtx2,yVtx2,geom2(1),geom2(2),0.5*pi-geom2(3));
% check for collision in this orientation
[isCollided2,dist2] = customCheckCollision(xRot12,yRot12,xRot22,yRot22);

%% Conbine the results
if ~isCollided1
    if ~isCollided2
        % If both projections are non-collided, take the maximum distance
        separationDist = max(dist1,dist2);
    else
        % Only first projection is non-collided
        separationDist = dist1;
    end
elseif ~isCollided2
    % Only second projecttion is non-collided
    separationDist = dist2;
else
    % Neither are non-collided
    separationDist = -1;
end
end
%% Utility functions
function [isCollided,dist] = customCheckCollision(x1_vertex,y1_vertex,x2_vertex,y2_vertex)
minx1 = min(x1_vertex);
maxx1 = max(x1_vertex);
miny1 = min(y1_vertex);
maxy1 = max(y1_vertex);
minx2 = min(x2_vertex);
maxx2 = max(x2_vertex);
miny2 = min(y2_vertex);
maxy2 = max(y2_vertex);
dist = zeros(1,4);
% Collision checks using AABB formula
if minx1 > maxx2
    dist(1) = minx1 - maxx2;
end
if minx2 > maxx1
    dist(2) = minx2 - maxx1;
end
if miny1 > maxy2
    dist(3) = miny1 - maxy2;
end
if miny2 > maxy1
    dist(4) = miny2 - maxy1;
end
if ~any(dist)
    isCollided = true;
    dist = -1;
else
    isCollided = false;
    % minimum distance can be approximated by the norm-2 distance of an x-y
    % pair of the projected distance
    dist = norm(dist);
end
end
%% Rotate [xVtx,yVtx] by thetaRot angle with respect to [xC,yC]
function [xRot,yRot] = rotate(xVtx,yVtx,xC,yC,thetaRot)
xRot = zeros(1,length(xVtx));
yRot = xRot;
for i = 1:length(xVtx)
    Rtmp = sqrt((xVtx(i)-xC)^2+(yVtx(i)-yC)^2);
    thetaTmp = atan2(yC-yVtx(i),xC-xVtx(i));
    xRot(i) = xC - Rtmp*cos(thetaTmp+thetaRot);
    yRot(i) = yC - Rtmp*sin(thetaTmp+thetaRot);
end
end
%% Get vertices of a rectangle-shape object
function [xVtx,yVtx] = getVertex(geom)
x     = geom(1);
y     = geom(2);
theta = geom(3);
l     = geom(4);
w     = geom(5);
xTmp1 = x - w/2*sin(theta);
yTmp1 = y + w/2*cos(theta);
xTmp2 = x + w/2*sin(theta);
yTmp2 = y - w/2*cos(theta);
x1 = xTmp1 + l/2*cos(theta);
y1 = yTmp1 + l/2*sin(theta);
x2 = xTmp1 - l/2*cos(theta);
y2 = yTmp1 - l/2*sin(theta);
x3 = xTmp2 - l/2*cos(theta);
y3 = yTmp2 - l/2*sin(theta);
x4 = xTmp2 + l/2*cos(theta);
y4 = yTmp2 + l/2*sin(theta);
xVtx = [x1, x2, x3, x4];
yVtx = [y1, y2, y3, y4];
end