fh-2022算法考试编程题-3

在三维坐标系中存在a1(x1,y1,z1)、a2(x2,y2,z2)、b1(x3 ,y3 ,z3)、b2(x4 ,y4 ,z4 )四个点，a1、a2为直线A上两点，b1、b2为直线B上两点。在给定坐标点a1、a2、b1、b2的情况下，请计算出直线A上任意一点与直线B上任一点之间距离的最小值，并给出距离最小的两个点的坐标（如平行，给出一组即可）。

测试用例：

[1]输入：a1(1,0,0),a2(2,0,0),b1(0,1,2),b2(0,2,2)

  输出：2,(0 0 0),(0,0,2)

[2]输入：a1(1,0,0),a2(1,0,1),b1(0,1,2),b2(0,2,2)

  输出：1,(1 0 2),(0 0 2)

[3]输入：a1(1,2,1),a2(3,2,1),b1(1,3,4),b2(0,2,4)

  输出：3,(0 2 1),(0 2 4)

# -*- coding:utf-8 -*-
# file: fun3.py
# author: 
# datetime: 2022/9/17
# software: PyCharm
import math
import numpy as np


def dot_line_distance(dot: tuple, line: tuple):
    """
    求点到直线最短距离
    点(x0,y0,z0)与空间直线x-x1/l=y-y1/m=z-z1/n
    :param dot: 输入坐标，（x0，y0，z0）
    :param line: 三维中的直线，经过点（x_0，y_0，z_0），方向向量（m，n，p）
    :return: distance: 点到直线距离
            dot_out：线上的点
    """
    # 假设垂直点为p(line[1][0]*t+line[0][0], line[1][1]*t+line[0][1], line[1][2]*t+line[0][2])
    # dot和p点的直线则表示为
    # (line[1][0]*t+line[0][0]-dot[0], line[1][1]*t+line[0][1]-dot[0], line[1][2]*t+line[0][2]-dot[0])
    # 直线line的方向向量和dot—p向量垂直，相乘为0，可求出t
    up_div = (line[0][0]-dot[0])*line[1][0]+(line[0][1]-dot[1])*line[1][1]+(line[0][2]-dot[2])*line[1][2]
    down_div = line[1][0]*line[1][0]+line[1][1]*line[1][1]+line[1][2]*line[1][2]
    t = -up_div/down_div
    # print(t)

    # 根据t，求出坐标
    dot_out = (line[1][0]*t+line[0][0], line[1][1]*t+line[0][1], line[1][2]*t+line[0][2])
    # 计算距离
    distance = math.sqrt(pow((dot_out[0]-dot[0]), 2)+pow((dot_out[1]-dot[1]), 2)+pow((dot_out[2]-dot[2]), 2))

    print(distance, dot, dot_out)
    return distance, dot, dot_out


def line_line_distance(line_1: tuple, line_2: tuple):
    """
    求不平行的两条直线的最短距离，并返回距离和所在直线上的点
    :param line_1: 直线1
    :param line_2: 直线2
    :return:
    """
    # 直线1,2的方向向量
    line_1_v = np.array(line_1[1])
    line_2_v = np.array(line_2[1])

    # 叉乘获取公垂向量
    vec_high = np.cross(line_1_v, line_2_v)

    # 在直线1,2上任取一个点，得到向量3
    line_3 = np.array(line_1[0]) - np.array(line_2[0])

    # 向量3在公垂向量方向的投影即为异面直线的最短距离
    distance = abs(np.dot(vec_high, line_3))/np.linalg.norm(vec_high)

    # 坐标表示法（x_0+mt，y_0+nt，z_0+pt），设两点分别取值s,t,则st直线垂直line_1,line_2,可求解方程获取t，s
    t = (np.cross((np.array(line_2[0])-np.array(line_1[0])), line_2_v).dot(np.cross(line_1_v, line_2_v)))/np.sum(np.cross(line_1_v, line_2_v)**2)
    s = (np.cross((np.array(line_2[0])-np.array(line_1[0])), line_1_v).dot(np.cross(line_1_v, line_2_v)))/np.sum(np.cross(line_1_v, line_2_v)**2)
    # t = -t
    # 映射回实际坐标
    dot_1 = (line_1[1][0]*t+line_1[0][0], line_1[1][1]*t+line_1[0][1], line_1[1][2]*t+line_1[0][2])
    dot_2 = (line_2[1][0]*s+line_2[0][0], line_2[1][1]*s+line_2[0][1], line_2[1][2]*s+line_2[0][2])
    print(distance, dot_1, dot_2)
    return distance, dot_1, dot_2


def get_line_equation(dot_1: tuple, dot_2: tuple) -> tuple:
    """
    根据三维坐标中的两点，求直线方程
    :param dot_1:坐标1，（x1，y1，z1）
    :param dot_2:坐标2，（x2，y2，z2）
    :return:
    空间直线两点式（x-x_0）/m = (y-y_0)/n = (z-z_0)/p，返回（x_0，y_0，z_0），（m，n，p）
    """
    # x=x_0+mt
    x_0 = dot_1[0]
    m = dot_2[0]-dot_1[0]

    # y=y_0+nt
    y_0 = dot_1[1]
    n = dot_2[1]-dot_1[1]

    # z=z_0+pt
    z_0 = dot_1[2]
    p = dot_2[2]-dot_1[2]

    out = ((x_0, y_0, z_0), (m, n, p))

    return out


def main(dot_1, dot_2, dot_3, dot_4):
    """
    执行主函数，主逻辑部分
    :param dot_1: 直线k的一点
    :param dot_2: 直线k的另一点
    :param dot_3: 直线s的一点
    :param dot_4: 直线s的另一点
    :return:
    """
    # 获取直线方程
    line_k = get_line_equation(dot_1, dot_2)
    line_s = get_line_equation(dot_3, dot_4)

    # 判断直线是否平行,如果叉积为0则平行
    if np.linalg.norm(np.cross(np.array(line_k[1]), np.array(line_s[1]))) == 0:
        # 平行时，随便取直线k上一点，计算该点到直线的垂直距离即最短距离
        # distance, dot_line_k, dot_line_s = dot_line_distance(line_k[0], line_s)
        return dot_line_distance(line_k[0], line_s)
    else:
        # 不平行时
        # distance, dot_line_k, dot_line_s = line_line_distance(line_k, line_s)
        return line_line_distance(line_k, line_s)


if __name__ == '__main__':
    # 测试用例
    # 用例1
    # a_1 = (1, 0, 0)
    # a_2 = (2, 0, 0)
    # b_1 = (0, 1, 2)
    # b_2 = (0, 2, 2)

    # 用例2
    a_1 = (1, 0, 0)
    a_2 = (1, 0, 1)
    b_1 = (0, 1, 2)
    b_2 = (0, 2, 2)

    # 用例3
    # a_1 = (1, 2, 1)
    # a_2 = (3, 2, 1)
    # b_1 = (1, 3, 4)
    # b_2 = (0, 2, 4)

    _, _, _ = main(a_1, a_2, b_1, b_2)


    # dot_line_distance((-6,1,21),((-4,-5,-1),(3,1,1)))