三维空间中 点线面距离计算

def dist_O_A_B(O,A,B):
    # r.t. https://blog.csdn.net/qq_32867925/article/details/114294753
    # 计算点O到A,B连线的垂足点
    xo,yo,zo=O
    x1,y1,z1=A
    x2,y2,z2=B

    k=-( (x1-xo)*(x2-x1)+(y1-yo)*(y2-y1)+(z1-zo)*(z2-z1) ) / ( (x2-x1)**2+(y2-y1)**2+(z2-z1)**2)
    xn=k*(x2-x1)+x1
    yn=k*(y2-y1)+y1
    zn=k*(z2-z1)+z1
    N=[xn,yn,zn]
    return N

def points_to_screen(points):
    # r.t. https://blog.csdn.net/qq_29912325/article/details/106917141
    # 4个点计算平面方程，拟合求解
    n=points.shape[0]
    A=np.ones(shape=[n,3])
    b=np.zeros(shape=[n])
    for i in range(n):
        A[i,0]=points[i][0]
        A[i,1]=points[i][1]
        b[i]=points[i][2]

    coefficients, residuals, rank, singular_values = np.linalg.lstsq(A, b, rcond=None)
    # # 提取平面方程的系数
    A_screen, B_screen, D_screen = coefficients
    C_screen = 1  # 设定 C 为 1

    #print("平面方程：{}x + {}y + {}z + {} = 0".format(A_screen, B_screen, C_screen, D_screen))
    return A_screen,B_screen,C_screen,D_screen

def dist_point_to_screen(p,v,A,B,C,D):
    # r.t. https://blog.csdn.net/qq_42679415/article/details/136740895
    # 向量和平面的交点计算
    x0,y0,z0=p
    a,b,c=v
    t=-( A*x0+B*y0+c*z0+D ) / (a*A+b*B+c*C)

    C=p-t*v

    return C

def angle_two_vector(vec1,vec2):
    x1,y1,z1=vec1
    x2,y2,z2=vec2
    n1=np.linalg.norm(vec1)
    n2=np.linalg.norm(vec2)
    dot=np.dot(vec1,vec2)
    cos_=dot/( n1*n2 )
    return np.arccos(cos_)