python calculation space line surface intersection

import numpy as np 

''' Find the direction vector of a line [m,n,p] 


    def Direction_vector(x1, y1, z1, x2, y2, z2): 
    m = x1 - x2 
    n = y1 - y2 
    p = z1 - z2 
    print ('m', m) 
    print('n', n) 
    print('p', p) 
    return m, n, p''' 

'''Three-point normal plane equation solution''' 


def Find_plane_equation(xo1, yo1 , zo1, xo2, yo2, zo2, xo3, yo3, zo3): 
    a = (yo2 - yo1) * (zo3 - zo1) - (zo2 - zo1) * (yo3 - yo1) 
    b = (xo3 - xo1) * ( zo2 - zo1) - (xo2 - xo1) * (zo3 - zo1) 
    c = (xo2 - xo1) * (yo3 - yo1) - (xo3 - xo1) * (yo2 - yo1) 
    d = -(a * xo1 ​​+ b * yo1 + c * zo1) 
    Equation_parameters = np. array([a, b, c, d])
    print('Plane equation parameters', Equation_parameters) 

    return Equation_parameters


'''Find the intersection point of line and plane''' 


def Find_intersection(x1, y1, z1, x2, y2, z2, a, b, c, d): 
    m = x1 - x2 
    n = y1 - y2 
    p = z1 - z2 
    t = (-a * x1 - b * y1 - c * z1 - d) / (a ​​* m + b * n + c * p) x = m 

    * t + x1 
    y = n * t + y1 
    z = p * t + z1 
    X = np.array([x, y, z]) 
    print('Intersection', X) 

    return X 


a, b, c, d = Find_plane_equation(727.34, 143.62, 0, 544.14, 161.87, 0, 471.0, 120.24, 0) 

X = Find_intersection(555.45, 120.24, 155.28, 511.61, 120.24, 74.67, a, b, c, d)

Guess you like

Origin blog.csdn.net/m0_61509658/article/details/122351028