python求点到面的距离

使用方法：

1、print(getDistanceBetweenPointAndFace(1,1,0,[[1,0,0],[0,1,0],[0,0,1]]))

2、print(getDistanceBetweenPointAndFace(1,1,1,[[1,0,0],[0,1,0],[0,0,1]]))

注：本代码考虑了点在面上的投影是否在面上的情况，不在的话转化为求点到线的距离，如果该点的投影点也不在线上，转化为求点到点的。

如果你只想求点到平面的（无论点在面上的投影是否在面上），可以在getDistanceBetweenPointAndFace末尾直接返回getDistanceBetweenTwoPoints(x,y,z,X0,Y0,Z0)，无需点投影是否在面上的判断

# refer to https://www.cnblogs.com/nobodyzhou/archive/2016/12/08/6145030.html
# x,y,z is the point, tha face consists of three points
# example print(getDistanceBetweenPointAndFace(1,1,0,[[1,0,0],[0,1,0],[0,0,1]]))
def getDistanceBetweenPointAndFace(x,y,z,face):
    #(a,b,c) is the normal vector
    a = (face[1][1]-face[0][1])*(face[2][2]-face[0][2]) - (face[2][1]-face[0][1])*(face[1][2]-face[0][2])
    b = (face[2][0]-face[0][0])*(face[1][2]-face[0][2]) - (face[1][0]-face[0][0])*(face[2][2]-face[0][2])
    c = (face[1][0]-face[0][0])*(face[2][1]-face[0][1]) - (face[2][0]-face[0][0])*(face[1][1]-face[0][1])

    t = (a*face[0][0] + b*face[0][1] + c*face[0][2] - (a*x + b*y + c*z))/(a**2+b**2+c**2)
    # t_same1 = (a*face[1][0] + b*face[1][1] + c*face[1][2] - (a*x + b*y + c*z))/(a**2+b**2+c**2)
    # t_same2 = (a*face[2][0] + b*face[2][1] + c*face[2][2] - (a*x + b*y + c*z))/(a**2+b**2+c**2)
    # assert (t-t_same1)<0.0000001 and (t-t_same2)<0.0000001

    #X0,Y0,Z0 is the projection point
    X0 = x + a*t
    Y0 = y + b*t
    Z0 = z + c*t

    if      getInnerProduct(face[0][0]-X0, face[0][1]-Y0, face[0][2]-Z0, face[1][0]-X0, face[1][1]-Y0, face[1][2]-Z0)<=0\
        and getInnerProduct(face[0][0]-X0, face[0][1]-Y0, face[0][2]-Z0, face[2][0]-X0, face[2][1]-Y0, face[2][2]-Z0)<=0\
        and getInnerProduct(face[1][0]-X0, face[1][1]-Y0, face[1][2]-Z0, face[2][0]-X0, face[2][1]-Y0, face[2][2]-Z0)<=0:
        # print(str(X0)+' '+str(Y0)+' '+ str(Z0)+'On face')
        return getDistanceBetweenTwoPoints(x,y,z,X0,Y0,Z0)
    else:
        # print(str(X0)+' '+str(Y0)+' '+ str(Z0)+'Not on face')
        return min([getDistanceBetweenPointAndLine(face[0][0],face[0][1],face[0][2],face[1][0],face[1][1],face[1][2],x,y,z),
                     getDistanceBetweenPointAndLine(face[0][0],face[0][1],face[0][2],face[2][0],face[2][1],face[2][2],x,y,z),
                     getDistanceBetweenPointAndLine(face[1][0],face[1][1],face[1][2],face[2][0],face[2][1],face[2][2],x,y,z)])

#refer to https://blog.csdn.net/gf771115/article/details/26721055/
#X3,Y3,Z3 is the point
def getDistanceBetweenPointAndLine(X1,Y1,Z1,X2,Y2,Z2,X3,Y3,Z3):
    k = ((X3-X1)*(X2-X1)+(Y3-Y1)*(Y2-Y1)+(Z3-Z1)*(Z2-Z1))/(getDistanceBetweenTwoPoints(X1,Y1,Z1,X2,Y2,Z2)**2)
    k_same = getInnerProduct(X3-X1,Y3-Y1,Z3-Z1, X2-X1,Y2-Y1,Z2-Z1)/(getDistanceBetweenTwoPoints(X1,Y1,Z1,X2,Y2,Z2)**2)
    assert k==k_same

    X0 = X1 + k*(X2-X1)
    Y0 = Y1 + k*(Y2-Y1)
    Z0 = Z1 + k*(Z2-Z1)
    #X0,Y0,Z0 is Projection point

    # is on line.
    if getInnerProduct(X1-X0, Y1-Y0, Z1-Z0, X2-X0, Y2-Y0, Z2-Z0)<=0:
        return getDistanceBetweenTwoPoints(X3,Y3,Z3,X0,Y0,Z0)
    else:# not on line.
        return min([getDistanceBetweenTwoPoints(X1,Y1,Z1,X3,Y3,Z3),getDistanceBetweenTwoPoints(X2,Y2,Z2,X3,Y3,Z3)])

def getDistanceBetweenTwoPoints(X1,Y1,Z1,X2,Y2,Z2):
    return ((X1-X2)**2 + (Y1-Y2)**2 + (Z1-Z2)**2)**0.5

def getInnerProduct(DetaX1,DetaY1,DetaZ1,DetaX2,DetaY2,DetaZ2):
    return DetaX1*DetaX2 + DetaY1*DetaY2 + DetaZ1*DetaZ2