Matlab计算两线段/向量交点

Matlab计算两线段/向量交点

p11、p12表示一线段或向量（lim=1表示为线段计算）

p21、p22表示另一线段或向量

输入四个端点，是否为向量，输出交点坐标（不存在时输出nan）

function [point] = vector2corss_point(p11,p12,p21,p22,lim)
% 2024-1-10
% xp
% 通过平面内两组点计算两个向量
% 再通过平面内两个向量计算交点

k1 = (p12(2)-p11(2))./(p12(1)-p11(1));
b1 = p11(2) - k1 .* p11(1);
k2 = (p22(2)-p21(2))./(p22(1)-p21(1));
b2 = p21(2) - k2 .* p21(1);
x = (b2-b1) ./ (k1-k2);
y = k1 .* x + b1;

if lim ~= 1
    % 奇异点修正
    if p11(1) == p12(1)&&(p21(1) ~= p22(1))
        x = p11(1);
        y = k2 .* x + b2;
    end
    if (p11(1) ~= p12(1))&&(p21(1) == p22(1))
        x = p21(1);
        y = k1 .* x + b1;
    end
    % 若两根线一般共线
    if (k1 == k2)&&(b1 == b2)
        point = p11;
    end
    if (k1 == k2)&&(b1 == b2)
        return
    end
    % 若两根线竖直共线
    if (isinf(k1) && isinf(k2))||(isnan(k1) && isnan(k2))
        if (p11(1) == p21(1))
            point = p11;
        end
        if (p11(1) == p21(1))
            return
        end
    end
end

if lim == 1
    % 奇异点修正
    if (p11(1) == p12(1))&&(p21(1) ~= p22(1))
        x = p11(1);
        y = k2 .* x + b2;
    end
    value1 = (y-p11(2))*(y-p12(2));
    if value1 > 0
        x = nan;
        y = nan;
    end
    
    if (p11(1) ~= p12(1))&&(p21(1) == p22(1))
        x = p21(1);
        y = k1 .* x + b1;
    end
    value1 = (y-p21(2))*(y-p22(2));
    if value1 > 0
        x = nan;
        y = nan;
    end
    
    % 若两根线一般共线
    if (k1 == k2)&&(b1 == b2)
        % p1是否满足要求
        value1 = (p11(1)-p21(1))*(p11(1)-p22(1));
        % p2是否满足要求
        value2 = (p12(1)-p21(1))*(p12(1)-p22(1));
        if (value1 <= 0)
            point = p11;
        end
        if (value1 <= 0)
            return
        end
        if (value2 <= 0)
            point = p12;
        end
        if (value2 <= 0)
            return
        end
    end
    % 若两根线竖直共线
    if (isinf(k1) && isinf(k2))||(isnan(k1) && isnan(k2))
        if (p11(1) == p21(1))
            value1 = (p11(2)-p21(2))*(p11(2)-p22(2));
            value2 = (p12(2)-p21(2))*(p12(2)-p22(2));
            if (value1 <= 0)
                point = p11;
            end
            if (value1 <= 0)
                return
            end
            if (value2 <= 0)
                point = p12;
            end
            if (value2 <= 0)
                return
            end
        end
    end
end

if isnan(x)
    y = nan;
end
if isnan(y)
    x = nan;
end

% 若以线段来计算
if lim == 1
    % 且计算出有效值
    no_exist = sum(isnan(x)+isinf(x)+isnan(y)+isinf(y));
    if no_exist <= 0
        %则判断该点是否在线段定义下仍有效
        value1 = (((x-p21(1))*(x-p22(1)))<=0);
        value2 = (((x-p11(1))*(x-p12(1)))<=0);
        value3 = (((y-p21(2))*(y-p22(2)))<=0);
        value4 = (((y-p11(2))*(y-p12(2)))<=0);
        value0 = value1.*value2;
        value00 = value3.*value4;
        if (value0 <= 0) || (value00 <= 0)
            x = nan;
            y = nan;
        end
    end
    if no_exist > 0
        x = nan;
        y = nan;
    end
end

point = [x,y]';
end