6.contour_dis_el.md

# 距离等值线\俯仰等值线计算与绘制

上一篇文章中，我们采用HPR坐标系里的向量旋转，在地表绘制了螺旋线.
![2](../images/example03.png)
在复杂多样的现实应用需求中，还有一种更为普遍的计算需求，就是求取地表到全向光源的距离为D的所有点的集合（用多边形组成的近似椭圆区域）。一种类似的约束，还有站在地表的角度，要求求取观测空中的光源仰角为特定值的位置集合。

# 1. 问题描述

问题：已知一个点 T 位于空中，求取所有海拔为H的点A组成的集合，使得点A距离T的直线距离为D米。

如果是在一个正球中，这个问题是非常容易解决的，直接求解三角函数即可。而且，在正球下，二者的图形都是圆，且仰角与距离是等效的概念。椭球中，情况就复杂起来了，因为各个方向上的曲率是不同的，仰角也不同。相同仰角下，距离也不同。

下面为了方便，假设光源T的经度 $$$\theta$$$=0,纬度为 $$$\varphi$$$，高度为Hb。要计算的椭球的高度是Ha。光源的ECEF坐标为 $$$\vec T=\{A,C,E\}$$$，已经提前计算完毕。

![2](../images/contour_distance.svg)

# 2. 投射推导
由于椭球的曲率和经度无关，假设手电的经度为0.  对于全向光源，姿态为 Heading=0, Pitch=-90, Roll = 0, 也就是手电垂直指向地面。

此时，HPR坐标系的逆运算矩阵变为：
$$
T'_{enu}=\left[\begin{matrix}\sin{\left(h \right)} \cos{\left(p \right)} & \cos{\left(h \right)} \cos{\left(p \right)} &\sin{\left(p \right)}\\- \sin{\left(h \right)} \sin{\left(p \right)} \cos{\left(r \right)} + \sin{\left(r \right)} \cos{\left(h \right)} & - \sin{\left(h \right)} \sin{\left(r \right)} - \sin{\left(p \right)} \cos{\left(h \right)} \cos{\left(r \right)} & \cos{\left(p \right)} \cos{\left(r \right)}\\\sin{\left(h \right)} \sin{\left(p \right)} \sin{\left(r \right)} + \cos{\left(h \right)} \cos{\left(r \right)} & - \sin{\left(h \right)} \cos{\left(r \right)} + \sin{\left(p \right)} \sin{\left(r \right)} \cos{\left(h \right)} & - \sin{\left(r \right)} \cos{\left(p \right)}\end{matrix}\right]
$$

$$
Tt=T'^T_{enu}=\left[\begin{matrix}0 & 0 & 1\\0 & -1 & 0\\1 & 0 & 0\end{matrix}\right]
$$

而 ENU的逆运算矩阵变为：

$$R = \begin{bmatrix}
   {-\sin \theta} & {\cos \theta} & 0\\ 
   {-\sin \varphi cos\theta} & {-\sin \varphi \sin \theta} &  {\cos \varphi} \\
   \cos \varphi cos\theta & \cos \varphi \sin \theta & \sin \varphi
\end{bmatrix}$$

$$
R^T=\left[\begin{matrix}0 & - \sin{\left( \varphi\right)} & \cos{\left( \varphi \right)}\\1 & 0 & 0\\0 & \cos{\left( \varphi \right)} & \sin{\left( \varphi \right)}\end{matrix}\right]
$$

而 $\alpha \beta$坐标下，HPR光线的矢量为：

$$
K_{hpr}=\left[\begin{matrix}\cos{\left(\alpha \right)} \\ \sin{\left(\alpha \right)} \cos{\left(\beta \right)} \\ \sin{\left(\alpha \right)} \sin{\left(\beta \right)}\end{matrix}\right]^T
$$

变换到 ECEF：

$$
K_{ecef}=K_{hpr} \times T'^T_{enu} \times R^T
$$

化简：

$$
K_{ecef}=\left[\begin{matrix}- \sin{\left(\alpha \right)} \cos{\left(\beta \right)} \\ - \sin{\left(\alpha \right)} \sin{\left(\beta \right)} \sin{\left(\varphi \right)} + \cos{\left(\alpha \right)} \cos{\left(\varphi \right)} \\ \sin{\left(\alpha \right)} \sin{\left(\beta \right)} \cos{\left(\varphi \right)} + \sin{\left(\varphi \right)} \cos{\left(\alpha \right)}\end{matrix}\right]^T
$$

这就是在投射角度 $\alpha \beta$ 在ECEF坐标下的单位方向矢量。

利用参数方程, 可以得到过 T的直线为

$$
\vec {TM} = \vec T+K_{ecef} \cdot {t}
$$

t 是距离量，单位是米。


根据上一章的推导，我们知道 t 的根有0、1或者2个。在带入了$$$\beta$$$，$$$\alpha$的情况下，可以求解方程。


# 3 模型求解

现在，要求这个直线和椭球交汇为一点A，且交会时，距离 t = D，已知 $$$\beta$$$，求取 $$$\alpha$$$
对一个高度为H的标准椭球，其方程为：

$$
\frac{x^2}{(a+H)^2}+\frac{y^2}{(a+H)^2}+\frac{z^2}{(b+H)^2}=1
$$

为了便于展开，设 $$$G=\frac{1}{(a+H)^2}$$$，$$$L=\frac{1}{(b+H)^2}$$$
按照上一章直线与椭球的交点结论，直接给出方程为：

$$t[1]=\frac{2 \cdot (4 A G \sin{(\alpha )} \cos{(\beta )} + 4 C G \sin{(\alpha )} \sin{(\beta )} \sin{(\varphi )}- 4 C G \cos{(\alpha )} \cos{(\varphi )} - 4 E L \sin{(\alpha )} \sin{(\beta )} \cos{(\varphi )} - 4 E L \sin{(\varphi )} \cos{(\alpha )} + \sqrt{2} \sqrt{(- A^{2} G - C^{2} G - E^{2} L + 1) (- G (\sin{(- 2 \alpha + \beta + 2 \varphi )} + \sin{(2 \alpha - \beta + 2 \varphi )} + \sin{(2 \alpha + \beta - 2 \varphi )} - \sin{(2 \alpha + \beta + 2 \varphi )}) + 8 G \sin^{2}{(\alpha )} \sin^{2}{(\beta )} \sin^{2}{(\varphi )} + 8 G \sin^{2}{(\alpha )} \cos^{2}{(\beta )} + 8 G \cos^{2}{(\alpha )} \cos^{2}{(\varphi )} + L (\sin{(- 2 \alpha + \beta + 2 \varphi )} + \sin{(2 \alpha - \beta + 2 \varphi )} + \sin{(2 \alpha + \beta - 2 \varphi )} - \sin{(2 \alpha + \beta + 2 \varphi )}) + 8 L \sin^{2}{(\alpha )} \sin^{2}{(\beta )} \cos^{2}{(\varphi )} + 8 L \sin^{2}{(\varphi )} \cos^{2}{(\alpha )}) + 8 (- A G \sin{(\alpha )} \cos{(\beta )} - C G \sin{(\alpha )} \sin{(\beta )} \sin{(\varphi )} + C G \cos{(\alpha )} \cos{(\varphi )} + E L \sin{(\alpha )} \sin{(\beta )} \cos{(\varphi )} + E L \sin{(\varphi )} \cos{(\alpha )})^{2}})}{- G (\sin{(- 2 \alpha + \beta + 2 \varphi )} + \sin{(2 \alpha - \beta + 2 \varphi )} + \sin{(2 \alpha + \beta - 2 \varphi )} - \sin{(2 \alpha + \beta + 2 \varphi )}) + 8 G \sin^{2}{(\alpha )} \sin^{2}{(\beta )}\sin^{2}{(\varphi )} + 8 G \sin^{2}{(\alpha )} \cos^{2}{(\beta )} + 8 G \cos^{2}{(\alpha )} \cos^{2}{(\varphi )} + L (\sin{(- 2 \alpha + \beta + 2 \varphi )} + \sin{(2 \alpha - \beta + 2 \varphi )} + \sin{(2 \alpha + \beta - 2 \varphi )} - \sin{(2 \alpha + \beta + 2 \varphi )}) + 8 L \sin^{2}{(\alpha )} \sin^{2}{(\beta )} \cos^{2}{(\varphi )} + 8 L \sin^{2}{(\varphi )} \cos^{2}{(\alpha )}}$$

$$t[2]=\frac{2 (\sqrt{2} \sqrt{(- A^{2} G - C^{2} G - E^{2} L + 1) (- G (\sin{(- 2 \alpha + \beta + 2 \varphi )} + \sin{(2 \alpha - \beta + 2 \varphi )} + \sin{(2 \alpha + \beta - 2 \varphi )} -\sin{(2 \alpha + \beta + 2 \varphi )}) + 8 G \sin^{2}{(\alpha )} \sin^{2}{(\beta )} \sin^{2}{(\varphi )} + 8 G \sin^{2}{(\alpha )} \cos^{2}{(\beta )} + 8 G \cos^{2}{(\alpha )} \cos^{2}{(\varphi )} + L (\sin{(- 2 \alpha + \beta + 2 \varphi )} + \sin{(2 \alpha - \beta + 2 \varphi )} +\sin{(2 \alpha + \beta - 2 \varphi )} - \sin{(2 \alpha + \beta + 2 \varphi )}) + 8 L \sin^{2}{(\alpha )} \sin^{2}{(\beta )} \cos^{2}{(\varphi )} + 8 L \sin^{2}{(\varphi )} \cos^{2}{(\alpha )}) + 8 (- A G \sin{(\alpha )} \cos{(\beta )} - C G \sin{(\alpha )} \sin{(\beta )} \sin{(\varphi )} + C G \cos{(\alpha )} \cos{(\varphi )} + E L \sin{(\alpha )} \sin{(\beta )} \cos{(\varphi )} + E L \sin{(\varphi )} \cos{(\alpha )})^{2}} (G (\sin{(- 2 \alpha + \beta + 2 \varphi )} + \sin{(2 \alpha - \beta + 2 \varphi )} + \sin{(2 \alpha + \beta - 2 \varphi )} - \sin{(2 \alpha + \beta + 2 \varphi )}) - 8 G \sin^{2}{(\alpha )} \sin^{2}{(\beta )} \sin^{2}{(\varphi )} - 8 G \sin^{2}{(\alpha )} \cos^{2}{(\beta )} - 8 G \cos^{2}{(\alpha )} \cos^{2}{(\varphi )} - L (\sin{(- 2 \alpha + \beta + 2 \varphi )} + \sin{(2 \alpha - \beta + 2 \varphi )} + \sin{(2 \alpha + \beta - 2 \varphi )} - \sin{(2 \alpha + \beta + 2 \varphi )}) - 8 L \sin^{2}{(\alpha )} \sin^{2}{(\beta )} \cos^{2}{(\varphi )} - 8 L \sin^{2}{(\varphi )} \cos^{2}{(\alpha )}) - 4 (- A G \sin{(\alpha )} \cos{(\beta )} - C G \sin{(\alpha )} \sin{(\beta )} \sin{(\varphi )} + C G \cos{(\alpha )} \cos{(\varphi )} + E L \sin{(\alpha )} \sin{(\beta )} \cos{(\varphi)} + E L \sin{(\varphi )} \cos{(\alpha )}) (- G (\sin{(- 2 \alpha + \beta + 2 \varphi )} + \sin{(2 \alpha - \beta + 2 \varphi )} + \sin{(2 \alpha + \beta - 2 \varphi )} - \sin{(2 \alpha + \beta + 2 \varphi )}) + 8 G \sin^{2}{(\alpha )} \sin^{2}{(\beta )} \sin^{2}{(\varphi )} + 8 G \sin^{2}{(\alpha )} \cos^{2}{(\beta )} + 8 G \cos^{2}{(\alpha )} \cos^{2}{(\varphi )} + L (\sin{(- 2 \alpha + \beta + 2 \varphi )} + \sin{(2 \alpha - \beta + 2 \varphi )} + \sin{(2 \alpha + \beta - 2 \varphi )} - \sin{(2 \alpha + \beta + 2 \varphi )}) + 8 L \sin^{2}{(\alpha )} \sin^{2}{(\beta )} \cos^{2}{(\varphi )} + 8 L \sin^{2}{(\varphi )} \cos^{2}{(\alpha )}))}{(- G (\sin{(- 2 \alpha + \beta + 2 \varphi )} + \sin{(2 \alpha - \beta + 2 \varphi )} + \sin{(2 \alpha + \beta - 2 \varphi )} - \sin{(2 \alpha + \beta + 2 \varphi )}) + 8 G \sin^{2}{(\alpha )} \sin^{2}{(\beta )} \sin^{2}{(\varphi )} + 8 G \sin^{2}{(\alpha )} \cos^{2}{(\beta )} + 8 G \cos^{2}{(\alpha )} \cos^{2}{(\varphi )} + L (\sin{(- 2 \alpha + \beta + 2 \varphi )} + \sin{(2 \alpha - \beta + 2 \varphi )} + \sin{(2 \alpha + \beta - 2 \varphi )} - \sin{(2 \alpha + \beta + 2 \varphi )}) + 8 L \sin^{2}{(\alpha )} \sin^{2}{(\beta )} \cos^{2}{(\varphi )} + 8 L \sin^{2}{(\varphi )} \cos^{2}{(\alpha )})^{2}}$$

Octave 符号计算的程序:

```octave
clc
clear
pkg load symbolic

a = sym('a');
b = sym('b');

h = sym('0');
p = sym('pi/2');
r = sym('0');

TT = [
sin(h)*cos(p), -sin(h)*sin(p)*cos(r)+sin(r)*cos(h), sin(h)*sin(p)*sin(r)+cos(h)*cos(r);
cos(h)*cos(p), -sin(h)*sin(r)-sin(p)*cos(h)*cos(r),-sin(h)*cos(r)+sin(p)*sin(r)*cos(h);
sin(p), cos(p)*cos(r), -sin(r)*cos(p)
];



ts = sym('0');
tf = sym('tf');

TR = [
-sin(ts),-sin(tf)*cos(ts),cos(tf)*cos(ts);
cos(ts),-sin(tf)*sin(ts),cos(tf)*sin(ts);
0,cos(tf),sin(tf)
];

Khpr = [cos(a),sin(a)*cos(b),sin(a)*sin(b)];

Kecef = Khpr * TT * TR;

G = sym('G');
L = sym('L');
A = sym('A');
B = Kecef(1);
C = sym('C');
D = Kecef(2);
E = sym('E');
F = Kecef(3);
t = sym('t');
Tt=solve(G*(A+B*t)^2 + G*(C+D*t)^2 + K*(E+F*t)^2==1,t);
Ts=simplify(Tt);
latex(Ts)
```

注意，上面的方程中，是写成了 t 的形式，但未知数却是 $$$\alpha$$$，它是一个非线性的高次三角方程。

但对于定义域在[0,-90]度的角度 来说，可以研究一下它的单调性，以采用计算方法进行求解。这个关于$$$\alpha$$$的方程，在俯视时，角度与距离的关系是非线性递增的。

![角度-距离](../images/function_alpha_d.jpg)

我们可以采用二分查找的方法，较为迅速的锁定角度值。对于求取距离等值线，约束设置为距离；对于求取仰角等值线，设置为仰角。要注意仰角函数相对于$\alpha$是递减的。

# 4 主要接口

下面这个函数可以按照给定的$\beta$求取位置。距离的最小值，就是手电到椭球的最短距离（H）。距离的最大值，是各个方向上做切线，切线段的长度。满足值域范围的进行迭代计算；不满足的，返回最小或者最大值。

```cpp
/*!
 * \brief contour_distance 计算到空间位置LLA坐标 T 距离为D的地面位置集合。
 * \param lla_torch		全向光源的位置
 * \param queryD		距离等值线的距离。D=0会返回最短距离， D超过最大距离，则返回最大距离。
 * \param earth_H		地表高度
 * \param vec_beta		给定的HPR beta，正北是0，顺时针递增
 * \param vec_lat		存储纬度的向量
 * \param vec_lon		存储经度的向量
 * \param vec_D			存储距离的向量
 * \param derrMeter		迭代误差，默认1mm
 * \param rad			弧度true/度 false
 * \param latfirst		纬度经度高度 true/经度 纬度 高度 false
 */
void contour_distance(const double lla_torch [3],
					  const double queryD,
					  const double earth_H,
					  const std::vector<double> vec_beta,
					  std::vector<double> * vec_lat,
					  std::vector<double> * vec_lon,
					  std::vector<double> * vec_D,
					  const double derrMeter = 1e-3,
					  const bool rad = false,
					  const bool latfirst = true);


/*!
 * \brief contour_elevation 计算到空间位置LLA坐标 T 仰角为el的地面位置集合。
 * \param lla_torch		全向光源的位置
 * \param queryEl		角度等值线的仰角。El=90会返回最短距离， El=0返回切面最大距离。
 * \param earth_H		地表高度
 * \param vec_beta		给定的HPR beta，正北是0，顺时针递增
 * \param vec_lat		存储纬度的向量
 * \param vec_lon		存储经度的向量
 * \param vec_D			存储距离的向量
 * \param vec_Az		存储方位角的向量
 * \param vec_El		存储俯仰角的向量
 * \param ATMOSPHERIC_CORRECTION		大气折射矫正
 * \param derrMeter		迭代误差，默认1mm
 * \param rad			弧度true/度 false
 * \param latfirst		纬度经度高度 true/经度 纬度 高度 false
 */
inline void contour_elevation(const double lla_torch [3],
					  const double queryEl,
					  const double earth_H,
					  const std::vector<double> vec_beta,
					  std::vector<double> * vec_lat,
					  std::vector<double> * vec_lon,
					  std::vector<double> * vec_D,
					  std::vector<double> * vec_Az,
					  std::vector<double> * vec_El,
					  const bool ATMOSPHERIC_CORRECTION = false,
					  const double derrMeter = 1e-3,
					  const bool rad = false,
					  const bool latfirst = true);
```


注意，椭球上，各个$\beta$方向上的最大距离是不同的。

# 5. 例子

我们求取不同距离上的围线：

```cpp
void demo_contour_distance()
{
	printf ("\n\n========%s=======\n",__FUNCTION__);
	using namespace CES_GEOCALC;
	//光源的LLA
	const double lla_torch [3] = {35.273, 117.121, 10000};

	std::vector<double> betas;
	for (int i=0;i<360;i+=5)
		betas.push_back(i);

	std::vector<double> lats,lons,Ds;

	contour_distance(lla_torch,123000,100,betas,&lats,&lons,&Ds);

	const size_t res_sz = lats.size();

	for (size_t i=0;i<res_sz;++i)
	{
		printf("%.6lf,%.6lf\n",lats[i],lons[i]);
	}

}

```

效果如下图：

![Rings](../images/example04.png)
![Rings](../images/example05.png)