平面上两点和方位角计算目标交点的算法（应该可用于 MC 末地传送门三角定位）

前言

之前看 MC 视频的时候有个人说了怎么用三角定位来确定末地传送门的位置。
说的是在两个不同的位置扔出末影珍珠，然后记住这两个位置和末影珍珠的方位角，就可以计算出来。
肯定有人已经把算法弄出来了，但是我想自己试试，就得出了自己的一个算法。
当然这个算法是 xOy 平面的，MC 的平面应该是 xOz 平面；而且 MC 的方位角的计算我也不知道是怎么算的，以后有空了想起来再对应一下吧。

算法

在 xOy 平面上，有原点 \(O\)、已知位置 \(P_1(x_1,y_1)\)、\(P_2(x_2,y_2)\) 和未知位置 \(P_t(x_t,y_t)\)。
定义方位角在 x 轴正方向上为 0，方位角正方向从 x 轴到 y 轴。对于 \(P_1\) 和 \(P_2\) 的局部方位角有相同的定义，只是该方位角以该点为原点。

现有条件：以 \(P_1\) 为原点方位角为 \(\theta_1\) 的射线与以 \(P_2\) 为原点方位角为 \(\theta_2\) 的射线交于点 \(P_t\)（当然，这三个点不能共线）
则 \(P_t\) 的坐标 \((x_t,y_t)\) 可以这样计算出来：

\[\begin{align} x_t &= \frac{y_2-y_1+x_1\tan\theta_1-x_2\tan\theta_2}{\tan\theta_1-\tan\theta_2}, \\ y_t &= (x_t - x_1)\tan\theta_1 + y_1 \\ &= (x_t - x_2)\tan\theta_2 + y_2 \end{align} \]

这个算法有一个硬伤，那就是 \(\theta_1\) 和 \(\theta_2\) 不能等于 \((2n+1)\frac{\pi}{2}\)，其中 \(n\in\mathbb{Z}\)。

代码

def calc(p1, theta1, p2, theta2):
    import math
    #p1 = (0, 0)
    #theta1 = 0
    #p2 = (0, 0)
    #theta2 = 0

    x1 = p1[0]
    y1 = p1[1]
    x2 = p2[0]
    y2 = p2[1]

    theta1rad = math.radians(theta1)
    theta2rad = math.radians(theta2)
    tantheta1 = math.tan(theta1rad)
    tantheta2 = math.tan(theta2rad)
    x = (y2 - y1 + x1 * tantheta1 - x2 * tantheta2) / (tantheta1 - tantheta2)
    y = (x - x1) * tantheta1 + y1

    return (x, y)

算法推导

在前文所说的条件下，
以 \(P_0\) 为原点方位角为 \(\theta_0\) 的射线上的点的表达式为：

\[y=(x-x_0)\tan\theta_0+y_0 \]

同理，对于 \(P_1\) 和 \(P_2\) 及其对应方位角 \(\theta_1\) 和 \(\theta_2\)，有：

\[\begin{equation} \label{y1} y=(x-x_1)\tan\theta_1+y_1 \end{equation} \]

和

\[\begin{equation} \label{y2} y=(x-x_2)\tan\theta_2+y_2 \end{equation} \]

当两条射线交于点 \(P_t\) 时，让式 \ref{y1} 等于式 \ref{y2}，代入 \(x_t\) 和 \(y_t\)，有：

\[\begin{align} (x_t-x_1)\tan\theta_1+y_1 &= (x_t-x_2)\tan\theta_2+y_2 \\ x_t\tan\theta_1 - x_1\tan\theta_1 + y_1 &= x_t\tan\theta_2 - x_2\tan\theta_2 + y_2 \\ x_t(\tan\theta_1 - \tan\theta_2) &= x_1\tan\theta_1 - y_1 - x_2\tan\theta_2 + y_2 \\ x_t &= \frac{x_1\tan\theta_1 - y_1 - x_2\tan\theta_2 + y_2}{\tan\theta_1 - \tan\theta_2} \end{align} \]

和

\[\begin{align} y_t &= (x_t - x_1)\tan\theta_1 + y_1 \\ &= (x_t - x_2)\tan\theta_2 + y_2 \end{align} \]