【数组】求两个匀速运动质点的相交或最小距离

题目描述

两个质点初始位置，朝向角和速度分别为：（x0,y0,yaw0,v0），（x1,y1,yaw1,v1），它们在t=0时刻匀速直行运动，问它们是否会在同一时刻相交，如果不会，找出它们距离最小的时刻。相交判断距离小于1e-6，算法要求高效。

代码实现

import matheps = 1e-6def check(x0, y0, yaw0, v0, x1, y1, yaw1, v1):# 计算两个质点的速度分量v0_x = v0 * math.cos(yaw0)v0_y = v0 * math.sin(yaw0)v1_x = v1 * math.cos(yaw1)v1_y = v1 * math.sin(yaw1)# 相对速度分量（质点0相对于质点1）dv_x = v0_x - v1_xdv_y = v0_y - v1_y# 初始相对位置dx = x0 - x1dy = y0 - y1# 如果相对速度为0if dv_x == 0 and dv_y == 0:# 距离会始终不变，计算距离distance = math.sqrt(dx ** 2 + dy ** 2)return (True, 0.0) if distance < eps else (False, 0.0)# 如果相对速度不为0else:# 计算最小距离对应的时刻t_mint_min = -(dx * dv_x + dy * dv_y) / (dv_x ** 2 + dv_y ** 2)# 时刻不能为负数t_min = max(t_min, 0.0)# 计算t_min时刻的距离x0_t = x0 + v0_x * t_miny0_t = y0 + v0_y * t_minx1_t = x1 + v1_x * t_miny1_t = y1 + v1_y * t_mindistance = math.sqrt((x0_t - x1_t)**2 + (y0_t - y1_t)**2)# 判断是否相交（小于eps）is_intersect = distance < epsreturn (is_intersect, t_min)

t_min = -(dx * dv_x + dy * dv_y) / (dv_x ** 2 + dv_y ** 2)

要理解这个公式，我们需要从距离的数学表达和极值求解的角度来分析。

1. 距离的平方公式

设时刻 t 时，两个质点的位置分别为：

• 质点0： $(x_0(t),y_0(t))=(x_0+v_{0x}\cdot t,y_0+v_{0y}\cdot t)$

• 质点1： $(x_1(t),y_1(t))=(x_1+v_{1x}\cdot t,y_1+v_{1y}\cdot t)$

两质点的距离平方为：

$d^2(t)={\left[x_0(t)-x_1(t)\right]}^2+{\left[y_0(t)-y_1(t)\right]}^2$

将位置表达式代入，展开后得到：

$d^2(t)={\left[(x_0-x_1)+(v_{0x}-v_{1x})\cdot t\right]}^2+{\left[(y_0-y_1)+(v_{0y}-v_{1y})\cdot t\right]}^2$

2. 定义简化变量

为了简化推导，定义：

• 初始位置差： $dx=x_0-x_1$ ， $dy=y_0-y_1$

• 相对速度分量： $d{v}_x=v_{0x}-v_{1x}，d{v}_y=v_{0y}-v_{1y}$

此时，距离平方可简化为：

$d^2(t)={\left(dx+d{v}_x\cdot t\right)}^2+{\left(dy+d{v}_y\cdot t\right)}^2$

3. 求距离的最小值（极值）

距离 d(t) 是非负连续函数，且 d(t) 与 $d^2(t)$ 的单调性一致（因为平方是单调递增函数）。因此，最小化 d(t) 等价于最小化 $d^2(t)$ 。

对 $d^2(t)$ 关于 t 求导，并令导数为0（极值点的必要条件）：

步骤1：展开 $d^2(t)$

$d^2(t)=(dx{)}^2+2\cdot dx\cdot d{v}_x\cdot t+(d{v}_x{)}^2\cdot t^2+(dy{)}^2+2\cdot dy\cdot d{v}_y\cdot t+(d{v}_y{)}^2\cdot t^2$

步骤2：求导

对 t 求导：

$\frac{d}{dt}\left[d^2(t)\right]=2\cdot dx\cdot d{v}_x+2\cdot (d{v}_x{)}^2\cdot t+2\cdot dy\cdot d{v}_y+2\cdot (d{v}_y{)}^2\cdot t$

步骤3：令导数为0，解 t

令 $\frac{d}{dt}\left[d^2(t)\right]=0$ ，消去公因子 2 后：

$dx\cdot d{v}_x+(d{v}_x{)}^2\cdot t+dy\cdot d{v}_y+(d{v}_y{)}^2\cdot t=0$

整理关于 t 的项：

$t\cdot \left[(d{v}_x{)}^2+(d{v}_y{)}^2\right]+\left(dx\cdot d{v}_x+dy\cdot d{v}_y\right)=0$

解得：

$t=-\frac{dx\cdot d{v}_x+dy\cdot d{v}_y}{(d{v}_x{)}^2+(d{v}_y{)}^2}$

这就是最小距离对应的时刻 $t_{\text{min}}$ 。

4. 物理意义

• 公式中的分子 $dx\cdot d{v}_x+dy\cdot d{v}_y$ 是初始位置差与相对速度的“点积”，反映了“位置差”与“相对速度”的对齐程度。

• 分母 $(d{v}_x{)}^2+(d{v}_y{)}^2$ 是相对速度的大小平方，保证分母非负。

当 $t_{\text{min}}\ge 0$ 时，该时刻对应两质点距离最小的时刻；若 $t_{\text{min}}<0$ ，则最小距离出现在 t=0 （因为质点从 t=0 开始运动，更早的时刻无意义）。

总结

公式 $t_{\text{min}}=-\frac{dx\cdot d{v}_x+dy\cdot d{v}_y}{(d{v}_x{)}^2+(d{v}_y{)}^2}$ 是通过求导找极值推导得到的，它给出了两质点距离最小时的时刻（仅考虑 $t\ge 0$ 的情况）。