三维空间线段的交点

二维平面线段交点 代码

def find2dCrossPoint(point1,point2,point3,point4):
    deltax_s1 = point1[0]-point2[0]
    deltax_s2 = point3[0]-point4[0]
    deltay_s1 = point1[1]-point2[1]
    deltay_s2 = point3[1]-point4[1]
    
    direct_s1 = point1-point2
    direct_s2 = point3-point4
    
    dir_cross = np.cross(direct_s1,direct_s2)
    
    px = (np.cross(point1,point2) * deltax_s2 - np.cross(point3,point4) * deltax_s1) / dir_cross
    py = (np.cross(point1,point2) * deltay_s2 - np.cross(point3,point4) * deltay_s1) / dir_cross
    # 如果不存在交点 [px, py]=[nan, -inf]
    if px==px:
        # 判断交点在不在线段上
        isInS1 = min(point1[0],point2[0])<= px and max(point1[0],point2[0])>= px and min(point1[1],point2[1])<= py and max(point1[1],point2[1])>= py
        isInS2 = min(point3[0],point4[0])<= px and max(point3[0],point4[0])>= px and min(point3[1],point4[1])<= py and max(point3[1],point4[1])>= py
        
        if isInS1 and isInS2:
            return np.array([px, py])
        else:
            return -1
    else:
        return -1

测试

• 如果存在交点

point1=np.array([0,1])
point2=np.array([1,0])
point3=np.array([0,0])
point4=np.array([1,1])

find2dCrossPoint(point1,point2,point3,point4)

output：array([0.5, 0.5])

• 如果不存在交点

point1=np.array([0,1])
point2=np.array([0,0])
point3=np.array([1,0])
point4=np.array([1,1])
find2dCrossPoint(point1,point2,point3,point4)

output：-1

三维空间线段交点 代码

def find3dCrossPoint(point1,point2,point3,point4):

    # 空间直线方程 参数形式
    
    # 1> 先求解方程组：
    # x1+(x2-x1)t1=x3+(x4-x3)t2
    # y1+(y2-y1)t1=y3+(y4-y3)t2
    # 解得(t1,t2)
    
    # 2> (t1,t2)代入第三个方程检验是否成立：
    # z1+(z2-z1)t1=z3+(z4-z3)t2

    # 3> 如果第三个方程成立，则两直线相交
    # 交点坐标为（x1+(x2-x1)t1，y1+(y2-y1)t1，z1+(z2-z1)t1）

    deltax_s1 = point2[0]-point1[0]
    deltay_s1 = point2[1]-point1[1]
    deltaz_s1 = point2[2]-point1[2]
    
    deltax_s2 = point4[0]-point3[0]
    deltay_s2 = point4[1]-point3[1]    
    deltaz_s2 = point4[2]-point3[2]
    
    # 方程组
    # point1[0] + deltax_s1 * t1 = point3[0] + deltax_s2 * t2
    # point1[1] + deltay_s1 * t1 = point3[1] + deltay_s2 * t2
    # 解方程组
    
    x0 = [deltax_s1,-deltax_s2]
    b0 = point3[0]-point1[0]
    x1 = [deltay_s1,-deltay_s2]
    b1 = point3[1]-point1[1]
    x2 = [deltaz_s1,-deltaz_s2]
    b2 = point3[2]-point1[2]
    
    tempDic = [{'thex':[x0,x1],'theb':[b0,b1],'theIsCross':2},
              {'thex':[x0,x2],'theb':[b0,b2],'theIsCross':1},
              {'thex':[x1,x2],'theb':[b1,b2],'theIsCross':0}]
    # 系数矩阵
    x = np.mat(tempDic[0]['thex'])
    # 常数项
    b = np.mat(tempDic[0]['theb']).T
    isExitX = False
    theIsCrossNum = 2
    try:  
        for i in range(3):
            x = np.mat(tempDic[i]['thex'])
            b = np.mat(tempDic[i]['theb']).T
            theIsCrossNum = 2-i
            if np.linalg.det(x) != 0:
                isExitX = True
                break
        if not isExitX:
            return -1
        else:
            theSolve = np.linalg.solve(x,b)
            t1 = theSolve[0]
            t2 = theSolve[1]

            # 要求0<=t1,t2<=1，否则还是不相交
            if t1>=0 and t2<=1:
                theIsCross = [point1[2] + deltaz_s1 * t1 == point3[2] + deltaz_s2 * t2,
                              point1[1] + deltay_s1 * t1 == point3[1] + deltay_s2 * t2,
                              point1[0] + deltax_s1 * t1 == point3[0] + deltax_s2 * t2]

                isCross = theIsCross[theIsCrossNum]

                if isCross:
                    crossPointX = point1[0]+deltax_s1 * t1
                    crossPointY = point1[1]+deltay_s1 * t1
                    crossPointZ = point1[2]+deltaz_s1 * t1

                    crossPoint = np.array([crossPointX,crossPointY,crossPointZ]).flatten()

                    # 交点在两条线段上
                    if isPointOnLine(crossPoint,(point1,point2)) and isPointOnLine(crossPoint,(point3,point4)): 
                    
                        return crossPoint
                    else:
                        return -1
            else:
                return -1
    except:
        print('异常')
        
    return -1



def isPointOnLine(point,line):
    # 已知点在线上
    p1 = line[0]
    p2 = line[1]
    xmax,xmin = max(p1[0],p2[0]),min(p1[0],p2[0])
    ymax,ymin = max(p1[1],p2[1]),min(p1[1],p2[1])
    zmax,zmin = max(p1[2],p2[2]),min(p1[2],p2[2])
    # 点与线段共线，但是在延长线上
    # 横纵坐标在线段两点的最大最小值之间
    if point[0]<=xmax and point[0]>=xmin and point[1]<=ymax and point[1]>=ymin and point[2]<=zmax and point[2]>=zmin:
        return True
    else:
        return False

测试

point1=np.array([0,1,0])
point2=np.array([1,0,0])
point3=np.array([0,0,0])
point4=np.array([1,1,0])

find3dCrossPoint(point1,point2,point3,point4)


point1=np.array([0,0,1])
point2=np.array([1,0,0])
point3=np.array([0,0,0])
point4=np.array([1,0,1])

find3dCrossPoint(point1,point2,point3,point4)

output: array([0.5, 0.5, 0. ])

添加维度判断

定义

def findCrossPoint(point1,point2,point3,point4):
    dim = len(point1)
    if dim == 2:
        return find2dCrossPoint(point1,point2,point3,point4)
    if dim == 3:
        return find3dCrossPoint(point1,point2,point3,point4)

使用

point1=np.array([0,1,0])
point2=np.array([1,0,0])
point3=np.array([0,0,0])
point4=np.array([1,1,0])

findCrossPoint(point1,point2,point3,point4)

---

相关文章：

1. 奇技淫巧系列：向量叉乘
2. numpy api cross
3. NumPy叉乘
4. 计算几何入门一：点，线段，直线，向量
5. 求三维空间中两直线(或线段)的交点
6. （计算几何）判断一个点是否在线段上
7. 几何算法–python求解空间线段交点坐标
8. 用python解方程组
9. numpy.linalg.LinAlgError: Singular matrix 问题解决
10. 计算几何—判断点是否在线段上