二维直线路径 LOS 制导

题目

无限直线路径 $A\to B$：$$\chi=\operatorname{atan2}(B_y-A_y,B_x-A_x),\quad e=-(x-A_x)\sin\chi+(y-A_y)\cos\chi,$$ $$\psi=\operatorname{wrap}_{[-\pi,\pi]}\left(\chi-\operatorname{atan2}(e,\Delta)\right).$$ 若 $\lVert B-A\rVert_2<10^{-10}$，输出 DEGENERATE。不是航点切换、避障或海流补偿。 判题对首个输出角度使用环绕角误差 $|\operatorname{atan2}(\sin(\psi-\psi^*),\cos(\psi-\psi^*))|$，因此 $-\pi$ 与 $+\pi$ 等价；仍要求输出在 $[-\pi,\pi]$ 内（容差允许边界舍入）。第二个输出横向误差按普通浮点规则比较。

回到相关教材 →

输入

$A_x,A_y,B_x,B_y,x,y,\Delta$；$\Delta>0$；角度单位 rad。

输出

$\psi,e$；退化路径输出 DEGENERATE。

示例

输入

-3.75083734625231 1.3242494039467196 -3.75083734625231 1.3242494039467196 3.6306381965027157 0.1420738550790599 0.10053092483031315

期望输出

DEGENERATE

#include <iostream>
#include <iomanip>
#include <cmath>
#include <algorithm>
#include <vector>
#include <array>
#include <queue>
#include <tuple>
#include <limits>
using namespace std;

// Reference implementation: optional; solve the starter before opening.
int main() {
    cout << setprecision(17);
    double ax,ay,bx,by,x,y,D;
    cin>>ax>>ay>>bx>>by>>x>>y>>D;
    if(hypot(bx-ax,by-ay)<1e-10) {
        cout<<"DEGENERATE";
        return 0;
    }
    double chi=atan2(by-ay,bx-ax),e=-(x-ax)*sin(chi)+(y-ay)*cos(chi),p=chi-atan2(e,D);
    cout<<atan2(sin(p),cos(p))<<" "<<e;
}