Matlab implementation of airspace conflict detection and resolution based on GJK algorithm

Between airspace requirements, the Minkowski difference set between airspaces is calculated pairwise, judging its inclusion relationship with the origin, and detecting whether there is a conflict between pairs of airspaces. If the triangular image obtained does not contain the origin, then it is judged whether the horizontal interval between the two airspace requirements is less than the minimum horizontal safety interval. If it is less than the minimum safety interval, it is considered that there is a horizontal conflict between the two airspace demands. Further, it can be judged that there is a horizontal conflict between the two airspace demands. Airspace conflict.

3. Matlab implementation of GJK algorithm

3.1 Main function part

The input sequence shap_1 and shap_2 involve different images, including circles, polygons and runway shapes. The first item in the sequence is the identification item, 1 is a circle, 2 is a quadrilateral, 3 is a pentagon, 4 is a hexagon, and 5 is a runway shape. The second and third items of the circle have no data, the fourth and fifth items are the coordinates of the center of the circle (x, y), and the sixth item is the radius. For the remaining images, the second and third items represent the geometric center of each image, followed by the vertex coordinates (x, y) of each image. The runway shape is represented by the vertices of its circumscribed rectangle.

The output out indicates different situations. 1 indicates that the iteration process failed to pass the origin detection and exited the iteration; 2 indicates that the triangle formed can surround the origin; 3 indicates that the triangle formed by iteration is repeated before and after.

mindiastance is the minimum distance

ci represents the number of loop iterations

function [out,mindistance,ci] = GJK(shap_1,shap_2)
% initialize simplex
out = 0;
simple = [];
origin = [0,0]; % origin
n = 1; % record the number of iterations

% first iteration
direction = getInitDirection(shap_1,shap_2); % Get the initial direction
vertex = getNewVertex(shap_1,shap_2,direction); % Get the new vertex of the simplex based on the given direction and polygon
simple(n,1:2) = vertex;
% second iteration
n = n + 1;
direction = diff_1(origin,vertex);
vertex = getNewVertex(shap_1,shap_2,direction);
% Pass origin check
if isCrossingOriginVector(diff_1([0 0],simple(1,1:2)),vertex) == 1
    out = 1; % At this point it can be proven that no collision can occur
end
simple(n,1:2) = vertex;
n = n + 1;
% third iteration
direction = getLineFaceToOriginVector(simple(1,1:2),simple(2,1:2));
direction = direction
vertex = getNewVertex(shap_1,shap_2,direction);
% Pass origin check
if isCrossingOriginVector(direction,vertex) == 1
    out = 1; % At this point it can be proven that no collision can occur
    n1 = 1
end
% if isCrossingOriginVector(simple(2,1:2),vertex) == 0
% out = 1; % At this time, it can be proved that the origin has been passed, and it can be proved that a collision will occur, and it is an edge collision.
% end
simple(n,1:2) = vertex;
n = n + 1;
ci = 0;
% disp(simple);
if out == 0
    % start loop
    for i = 1:10000
        % Determine whether the triangle formed by the three vertices of the current simplex contains the origin
        if isContainOrigin(simple(1,1:2),simple(2,1:2),simple(3,1:2)) == 1
           out = 2; % At this time, it can be proved that it has passed the origin, it can be proved that a collision will occur, and it is an absolute collision.
           break;
        end
        % Find the side of the triangle closest to the origin
        minIndex1 = -1;
        minIndex2 = -1;
        for x = 1:length(simple)-1
            for y = x + 1:length(simple)
                distance = calcPointToLineDistance([0 0],simple(x,1:2),simple(y,1:2));
                if minIndex1 == -1 || distance < minDistance
                    minDistance = distance;
                    minIndex1 = x;
                    minIndex2 = y;
                end
            end
        end
        % Find direction
        direction = getLineFaceToOriginVector(simple(minIndex1,1:2),simple(minIndex2,1:2));
        vertex = getNewVertex(shap_1,shap_2,direction);
        % Whether there is a current simplex check
        for z = 1: length(simple)
           if isEquals(simple(z,1:2),vertex) == 1
               out = 3;
           end
        end
        if out == 3
           break; % If the simplex check is not passed, break out of the loop
        end
        % Pass origin check
        if isCrossingOriginVector(direction,vertex) == 1
            out = 1; % At this time, it can be proved that the origin detection has not been passed.
            % update simplex
            vertex1 = simple(minIndex1,1:2);
            vertex2 = simple(minIndex2,1:2);
            simple(1,1:2) = vertex1;
            simple(2,1:2) = vertex2;
            simple(3,1:2) = vertex;
            break;
        end

        % update simplex
        vertex1 = simple(minIndex1,1:2);
        vertex2 = simple(minIndex2,1:2);
        simple(1,1:2) = vertex1;
        simple(2,1:2) = vertex2;
        simple(3,1:2) = vertex;
    end
    ci = i;
end
% Find the side of the triangle closest to the origin
mindistance = 0;
minIndex1 = -1;
for x = 1:length(simple)-1
    for y = x + 1:length(simple)
        distance = calcPointToLineDistance([0 0],simple(x,1:2),simple(y,1:2));
        if minIndex1 == -1 || distance < mindistance
            mindistance = distance;
            minIndex1 = x;
        end
    end
end
disp(simple);
end

3.2 Get initial direction

Get the direction between the initial two geometries, with the centers of the two geometries subtracted.

function [out] = getInitDirection(shap_1,shap_2)
if(shap_1(1) == 1 )
    out_1(1:2) = shap_1(4:5);
else
    out_1(1:2) = shap_1(2:3);
end
if(shap_2(1) == 1 )
    out_2(1:2) = shap_2(4:5);
else
    out_2(1:2) = shap_2(2:3);
end
out = out_1 - out_2;
end

3.3 According to the given direction and polygon, get the new vertex of the simplex

% Get new simplex vertices based on the given direction and polygon.
% First consider the runway trajectory as a rectangle
function [out] = getNewVertex(shap_1,shap_2,direction)
if shap_1(1) == 1
    supportPoint1 = supportCircle(shap_1,direction);
elseif shap_1(1) == 5
    supportPoint1 = supportP(shap_1,direction);
else
    supportPoint1 = support(shap_1,direction);
end
if shap_2(1) == 1
    supportPoint2 = supportCircle(shap_2,-direction);
elseif shap_2(1) == 5
    supportPoint2 = supportP(shap_2,-direction);
else
    supportPoint2 = support(shap_2,-direction);
end
supportPoint1 = supportPoint1
supportPoint2 = supportPoint2
out = diff_1(supportPoint1,supportPoint2);
end

If the shape is a circle

% Support function (circle)

function [out] = supportCircle(shap,direction)
theta = calc2DVectorsAngle(direction,[1,0]);
if(direction(2)<0)
    theta = 2*pi - theta;
end
out = [shap(4) + shap(6)*cos(theta),shap(5) + shap(6)*sin(theta)];
end

% Calculate the angle between two two-dimensional vectors [0,PI]

function [out] = calc2DVectorsAngle(direction,shap)
d1 = norm(direction,2);
d2 = norm(shap,2);
out = acos((direction(1)*shap(1) + direction(2)*shap(2))/(d1*d2));
end

If the shape is a polygon

% support function (regular polygon)

function [out] = support(shap,direction)
maxIndex = 0;
maxDot = dot_1([shap(4),shap(5)],direction);
if shap(1) == 2 || shap(1) == 5
    n = 4;
elseif shap(1) == 3
    n = 5;
elseif shap(1) == 4
    n = 6;
end
for i = 1:n-1
   d = dot_1([shap(4 + 2*i),shap(5 + 2*i)],direction);
   if d > maxDot
       maxDot = d;
       maxIndex = 2*i;
   end
end
    out = [shap(4 + maxIndex),shap(5 + maxIndex)]; % Output the polygon and the vertices in the given direction
end

%Two-dimensional vector dot product

function [out] = dot_1(v1,v2)
out = v1(1)*v2(1) + v1(2)*v2(2);
end

If the image is a runway

% runway graphics

function [out] = supportP(shap,direction)
r = (shap(11)-shap(9))/2;
low = shap(8) + r;high = shap(4)-r;
% shap_1 = [0 0 0 high shap(5) high shap(7) low shap(7) low shap(5)];
% row = diff_1([low shap(9) + r],[shap(2) shap(3)]);
% row_1 = diff_1([high shap(9) + r],[shap(2) shap(3)]);
if direction(1) <= 0
    out = supportCircle([1,0,0,low,shap(9) + r,r],direction);
else
    out = supportCircle([1,0,0,high shap(9) + r,r],direction);
end
end

% Two-dimensional vector subtraction

function [out] = diff_1(shap_1,shap_2)
out = shap_1 - shap_2;
end

3.4 Origin crossing detection function

% Pass origin check

function [out] = isCrossingOriginVector(v1,v2)
if dot_1(v1,v2)>=0
    out = 0;
else
    out = 1;
end
end

3.5 Find the direction based on two support points

% Pass in two points and get the normal vector of the edge they form facing the origin.

function [out] = getLineFaceToOriginVector(A,B)
if A(1) == B(1)
    if A(1) >= 0
        out = [-1 0];
    else
        out = [1 0];
    end
elseif A(2) == B(2)
    if A(2) >= 0
        out = [0 -1];
    else
        out = [0 1];
    end
else
    k = (A(2) - B(2))/(A(1) - B(1));
    k1 = -1/k;
    b = A(2) - k*A(1);
    x = -k*b/(k*k + 1);
    y = k1*x;
    out = diff_1([0 0],[x y]);
end
end

3.6 Determine whether the triangle contains the origin

% Pass in three points and determine whether the triangle composed of three points contains the origin.

function [out] = isContainOrigin(p1,p2,p3)
origin = [0 0];
a = calcTriangleArea(origin,p1,p2);
b = calcTriangleArea(origin,p1,p3);
c = calcTriangleArea(origin,p2,p3);
s = calcTriangleArea(p3,p1,p2);
if abs(a + b + c-s) <0.00001
    out = 1; % prove that the origin is within the triangle
else
    out = 0; % prove that the origin is not within the triangle
end
end

% Pass in three points and calculate the area of the triangle according to Heron's formula

function [out] = calcTriangleArea(p1,p2,p3)
a = calcPointToPointDistance(p1,p2);
b = calcPointToPointDistance(p1,p3);
c = calcPointToPointDistance(p2,p3);
p = (a + b + c)/2;
out = sqrt(p*(p-a)*(p-b)*(p-c));
end

% Calculate the distance between two points

function [out] = calcPointToPointDistance(p1,p2)
out = sqrt((p1(1)-p2(1))^2 + (p1(2)-p2(2))^2);
end

3.7 Find the distance from the origin to the edge

% Find the distance from the origin to an edge

function [out] = calcPointToLineDistance(p0,p1,p2)
out2 = norm(p1,2);
out3 = norm(p2,2);
if p2(1) - p1(1) == 0
    out = abs(p2(1));
else
    a = (p2(2)-p1(2))/(p2(1)-p1(1));
    b = -1;
    c = p1(2)-a*p1(1);
    out1 = abs(a*p0(1) + b*p0(2) + c)/sqrt(a*a + b*b);
    x = (-1*a*c)/(a*a + 1);
    if (x >= p1(1) & amp; & amp; x <= p2(1)) || (x >= p2(1) & amp; & amp; x <= p1(1))
        out = out1;
    elseif out2 <= out3
        out = out2;
    else
        out = out3;
    end
end
end

3.8 Determine whether the new support points formed by the previous and subsequent iterations are repeated

% Determine whether two-dimensional vectors are equal

function [out] = isEquals(p1,p2)
if abs(p1(1) - p2(1)) == 0 & amp; & amp; abs(p1(2) - p2(2)) == 0
   out = 1;
else
    out = 0;
end
end