1
import numpy as np
2

3
def cal_Distance_Point2Point(p1:np.array, p2:np.array) -> np.array:
4
    """
5
    @brief 计算 p1 到 p2 的距离
6
    @param p1 [[x, y], ...]
7
    @param p2 [[x, y], ...]
8
    @return np.array([distance1, ....])
9
    """
10
    return np.sqrt(np.power(p1[:,0]-p2[:,0],2) + np.power(p1[:,1]-p2[:,1],2))
11

12
def cal_Distance_Point2Line(p1:np.array, p2:np.array, p3:np.array, absolute:bool = True) -> np.array:
13
    """
14
    @brief 计算 p1 到 p2 和 p3 连线的距离
15
    @param p1, p2, p3 [[x, y], ...]
16
    @param absolute True(只输出距离) False(带方向的距离)
17
    @return np.array([distance1, ....])
18
    """
19
    d1 = (p2[:,1]-p3[:,1]) * p1[:,0] + (p3[:,0]-p2[:,0]) * p1[:,1] + p2[:,0]*p3[:,1] - p2[:,1]*p3[:,0]
20
    d2 = np.sqrt(np.power(p2[:,0]-p3[:,0],2) + np.power(p2[:,1]-p3[:,1],2))
21
    distance = d1 / d2
22
    return abs(distance) if absolute else distance
23

24
def calA