三维坐标数学计算公式

三维坐标公式记录：

import math

import numpy as np


def euclidean_distance(p1, p2) -> float:
    """
    三维向量欧几里得距离
    Args:
        p1: [x, y, z, confidence]
        p2: [x, y, z, confidence]

    Returns:
        两个向量的欧几里得距离[浮点数]
    """
    return np.linalg.norm(p1[:3] - p2[:3])


def calculate_angle_degree(a1, a2) -> float:
    # 两边向量的夹角
    # 创建向量a和b
    a = np.array(a1)
    b = np.array(a2)

    # 计算两个向量的模
    l_a = np.sqrt(a.dot(a))
    l_b = np.sqrt(b.dot(b))
    print("向量的模:", l_a, l_b)

    # 向量的点积
    dian = a.dot(b)
    print("向量的点积=", dian)

    # 计算夹角的cos值
    cos_ = dian / (l_a * l_b)
    print("夹角的cos值=", cos_)

    # 求得夹角(弧度)
    angle_hu = np.arccos(round(cos_, 1))
    print(f"弧度 = ", angle_hu)

    # 转换为角度
    angle_d = angle_hu * 180 / np.pi
    print(f"夹角 = ", angle_d)

    return angle_d


def get_vector(a, b):
    """获取空间向量

    Args:
        a: [x1, y1, z1]
        b: [x2, y2, z2]

    Returns:
        [x2-x1, y2-y1, z2-z1]
    """
    return [b1 - a1 for a1, b1 in zip(a, b)]


def cal_angle(point_a, point_b, point_c):
    """
    根据三点坐标计算夹角

                  点a
           点b ∠
                   点c
    Args:
        point_a: 数据类型为list,二维坐标形式[x、y]或三维坐标形式[x、y、z]
        point_b: 数据类型为list,二维坐标形式[x、y]或三维坐标形式[x、y、z]
        point_c: 数据类型为list,二维坐标形式[x、y]或三维坐标形式[x、y、z]

    Returns:
        返回角点b的夹角值

    --
        数学原理：
        设m,n是两个不为0的向量，它们的夹角为<m,n> (或用α ,β, θ ,..,字母表示)

        1、由向量公式：cos<m,n>=m.n/|m||n|

        2、若向量用坐标表示，m=(x1,y1,z1), n=(x2,y2,z2),

        则,m.n=(x1x2+y1y2+z1z2).

        |m|=√(x1^2+y1^2+z1^2), |n|=√(x2^2+y2^2+z2^2).

        将这些代入②得到：

        cos<m,n>=(x1x2+y1y2+z1z2)/[√(x1^2+y1^2+z1^2)*√(x2^2+y2^2+z2^2)]

        上述公式是以空间三维坐标给出的，令坐标中的z=0,则得平面向量的计算公式。

        两个向量夹角的取值范围是：[0,π].

        夹角为锐角时，cosθ>0；夹角为钝角时,cosθ<0.

    """
    a_x, b_x, c_x = point_a[0], point_b[0], point_c[0]  # 点a、b、c的x坐标
    a_y, b_y, c_y = point_a[1], point_b[1], point_c[1]  # 点a、b、c的y坐标
    a_z, b_z, c_z = point_a[2], point_b[2], point_c[2]  # 点a、b、c的z坐标

    # 向量 m=(x1,y1,z1), n=(x2,y2,z2)
    x1, y1, z1 = (a_x - b_x), (a_y - b_y), (a_z - b_z)
    x2, y2, z2 = (c_x - b_x), (c_y - b_y), (c_z - b_z)

    # 两个向量的夹角，即角点b的夹角余弦值
    cos_b = (x1 * x2 + y1 * y2 + z1 * z2) / (
        math.sqrt(x1**2 + y1**2 + z1**2)
        * (math.sqrt(x2**2 + y2**2 + z2**2))
    )  # 角点b的夹角余弦值
    degree = math.degrees(math.acos(round(cos_b, 1)))  # 角点b的夹角值
    return degree


if __name__ == "__main__":
    a = [1, 2, 3]
    b = [2, 3, 4]
    c = [3, 4, 5]

    print(cal_angle(a, b, c))
    a_b = get_vector(a, b)
    b_c = get_vector(c, b)
    print(f"a_b: {a_b}")
    print(f"b_c: {b_c}")
    print(calculate_angle_degree(a_b, b_c))