3维直线和面的交点计算

简介

3为之间和面交点的计算，其实百度百科上讲的比较清楚了

link

百度百科 链接 https://baike.baidu.com/item/线面交点/23119069?fr=aladdin
讲的真的很好

python code

# coding=utf-8
from scipy.optimize import fsolve #导入求解方程组的函数 非线性方程组
import numpy as np
import matplotlib as mpl
mpl.use('TkAgg')
import matplotlib.pyplot as plt
from scipy import linalg
import math
class Point2D:
    # AX+BY+C = 0
    # 二维的一般式方程
    def __init__(self, x, y):
        self.x = x
        self.y = y

class Line2D:
    # AX+BY+C = 0
#   # 二维的一般式方程
    def __init__(self, A, B, C):
        self.A = A
        self.B = B
        self.C = C
    def init_from_two_point2d(self, a, b):
        self.A = b.y - a.y
        self.B = a.x - b.x
        self.C = b.x * a.y - a.x * b.y
    def getABC(self):
        return self.A, self.B, self.C


class Point3D:
    def __init__(self, x, y, z):
        self.x = x
        self.y = y
        self.z = z

class Vec3D:
    def __init__(self, x, y, z):
        self.x = x
        self.y = y
        self.z = z
        # self.norm()
    def norm(self):
        # 向量单位化后反而增加了误差
        self.x = self.x / math.sqrt(self.x * self.x + self.y * self.y + self.z * self.z)
        self.z = self.z / math.sqrt(self.x * self.x + self.y * self.y + self.z * self.z)
        self.y = self.y / math.sqrt(self.x * self.x + self.y * self.y + self.z * self.z)

class Face3D:
    # 一般表示形式
    def __init__(self):
        self.points = []
        self.n = [0,0,0]
    def init_from_three_point3d(self, a, b, c):
        # https://zhidao.baidu.com/question/456206521205069445.html
        # 首先求出平面的法向量 Point3D 类型
        u = [a.x - b.x, a.y - b.y, a.z - b.z]

        v = [a.x - c.x, a.y - c.y, a.z - c.z]
        self.n = Vec3D(u[1] * v[2] - u[2] * v[1],
                       u[2] * v[0] - u[0] * v[2],
                       u[0] * v[1] - u[1] * v[0])
        rlt = u[0] * self.n.x + u[1] * self.n.y + u[2] * self.n.z
        print('debug', self.n.x, self.n.y, self.n.z)
        print('debug-rlt', rlt)
        self.points.append(a)
        self.points.append(b)
        self.points.append(c)

        # https://baike.baidu.com/item/%E7%BA%BF%E9%9D%A2%E4%BA%A4%E7%82%B9/23119069?fr=aladdin
        # (p - p_0)*n = 0

class Line3D:
    def __init__(self):
        self.l = [0, 0, 0]
        self.points = []
    def init_from_two_point3d(self, a, b):
        # p = d * l + l_0
        self.l = Vec3D(a.x - b.x, a.y - b.y, a.z - b.z)
        self.points.append(a)
        self.points.append(b)


def intersection_line3D_Face3D(line, face):
    # https://baike.baidu.com/item/%E7%BA%BF%E9%9D%A2%E4%BA%A4%E7%82%B9/23119069?fr=aladdin
    l = line.l
    n = face.n
    if l.x * n.x + l.y * n.y + l.z * n.z == 0:
        print("[DEBUG] 平行 暂时不考虑重合")
        return

    d = ((face.points[0].x - line.points[0].x) * n.x + (face.points[0].y - line.points[0].y) * n.y
        + (face.points[0].z - line.points[0].z) * n.z) / (l.x * n.x + l.y * n.y + l.z * n.z)
    print("[DEBUG] intersection point[x, y, z] ", [d * line.l.x + line.points[0].x, d * line.l.y + line.points[0].y
                                          , d * line.l.z + line.points[0].z])
    return [d * line.l.x + line.points[0].x, d * line.l.y + line.points[0].y
                                          , d * line.l.z + line.points[0].z]




def intersection_line2D_line2D(line1, line2):
    A,B,C = line1.getABC()
    D,E,F = line2.getABC()
    # 判断这两条直线是否是重合的 或者平行的
    w = np.array([[A, B, C], [D, E, F]])
    if(np.linalg.matrix_rank(w) != 2):
        print('[ERROR] coincide  重合')
        return
    ww = np.array([[A, B], [D, E]])
    if(np.linalg.matrix_rank(ww) == 1 and C != F):
        print('[ERROR] parallel 平行')
        return
    AA = np.array([[A, B], [D, E]])
    BB = np.array([-C, -F])
    x = linalg.solve(AA, BB)
    print("[DEBUG] intersection on the point[x,y]", x)

def draw(l, f, in_p):
    mpl.rcParams['legend.fontsize'] = 10
    fig = plt.figure()
    ax = fig.add_subplot(111, projection='3d')
    # ax = fig.gca(projection='3d')
    # 画线
    ax.plot([l.points[0].x, l.points[1].x], [l.points[0].y, l.points[1].y], [l.points[0].z, l.points[1].z], color='r')
    # 画面
    ax.plot([f.points[0].x, f.points[1].x], [f.points[0].y, f.points[1].y], [f.points[0].z, f.points[1].z], color='b')
    ax.plot([f.points[2].x, f.points[1].x], [f.points[2].y, f.points[1].y], [f.points[2].z, f.points[1].z], color='b')
    ax.plot([f.points[0].x, f.points[2].x], [f.points[0].y, f.points[2].y], [f.points[0].z, f.points[2].z], color='b')
    # 画交点
    ax.scatter(in_p[0], in_p[1], in_p[2], color='b')

    # 绘制法向量
    ax.plot([f.points[0].x, f.points[0].x + f.n.x], [f.points[0].y, f.n.y + f.points[0].y], [f.points[0].z, f.points[0].z + f.n.z], color='b')
    ax.text(f.points[0].x, f.points[0].y, f.points[0].z, 'f0')
    ax.plot([l.points[0].x, l.points[0].x + l.l.x], [l.points[0].y, l.points[0].y + l.l.y], [l.points[0].z,l.points[0].z + l.l.z], color='r')
    ax.text(l.points[0].x, l.points[0].y, l.points[0].z, 'l0')
    ax.text(l.points[1].x, l.points[1].y, l.points[1].z, 'l1')
    plt.show()

if __name__ == "__main__":
    l0 = Point3D(37.55893681871618, 38.05750310452609, -84.50415801897957)
    l1 = Point3D(-37.043790813096156, 83.71664782625285, -40.24028378292983)
    l = Line3D()
    l.init_from_two_point3d(l0, l1)
    f0 = Point3D(40.52,54.87,-87.45)
    f1 = Point3D(28.23,43.26,-98.14)
    f2 = Point3D(20.72,35.64, -81.24)
    f = Face3D()
    f.init_from_three_point3d(f0, f1, f2)
    out = intersection_line3D_Face3D(l, f)
    draw(l, f, out)
    # l0 = Point3D(2,2,0.5)
    # l1 = Point3D(4,5,1)
    # l = Line3D()
    # l.init_from_two_point3d(l0, l1)
    # f0 = Point3D(0,0,0)
    # f1 = Point3D(4,1,0)
    # f2 = Point3D(2,3,0)
    # f = Face3D()
    # f.init_from_three_point3d(f0, f1, f2)
    # out =intersection_line3D_Face3D(l, f)
    # draw(l, f, out)