メルカトル図法上の二点間の最短航路を幾何学的に求める(実装とシミュレーション)

前回記事ではシミュレーションに必要な各図法の紹介と、それを組み合わせてメルカトル図法上の二点間を結ぶ最短経路曲線の式を立式した。
今回は、それをもとに実装してシミュレーションしてみよう。

## 文字テクスチャが刻まれた球を作る

球面に等幅フォントのランダムな文字列を同じスケールで描画して、メルカトル図法上のスケール変化が一目で分かるようにするようなテクスチャとしよう。
まずは一枚の、まんべんなく文字を描画した画像を用意する。
それをサンソン図法の地図だと解釈して、有効範囲を球座標へ変換(非線形変換)する処理をつくろう。



```python
import numpy as np
import cv2

def makeTextureMap(sz=900):
	# 一面背景色の画像生成.
	txmap=np.zeros((sz,sz*2,3))
	txmap[:]=(0.1,0.2,0.4)
	# 適当に文字を羅列する.
	sps=["QuickBrownFoxJumpsOverThe",
	"JugemujugemuGokounoSuriki",
	"ThisIsASentenceThatHasNoMean",
	"SphereHasCircleSurfaceof3D"]*2
	# 文字描画設定.
	fsz=3.95*sz/900
	left=int(round(2*sz/90))
	top=int(round(sz/9))
	stride=int(round(sz*11/90))
	thic=max(int(round(sz/100)),1)
	# 文字描画.
	for h in range(8):
		cv2.putText(txmap, sps[h],(left,top+h*stride),cv2.FONT_HERSHEY_SIMPLEX, fsz, (0.5,1,0.2),thic)
	# 各ピクセルに対応するy,xのindexを用意.	idx=np.indices(txmap.shape[:2]).astype(float)
	# uは正規化X軸、vは正規化y軸とする.
	v=(idx[0]+1e-5)/(txmap.shape[0]+2e-5)
	u=(idx[1]/txmap.shape[1]-0.5)
	return u, v, txmap
```

描画すると以下。
![merca_imUv.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2902764/8ec1a71b-c446-f468-d748-30ab623870cf.png)

これを横に細い糸の集まりだとして、緯度が同じところに巻きつけていけば球にテクスチャをはるかことができる。一方、球の極に近い部分なほど必要な糸は短くて済むので、球のテクスチャとして必要なピクセルのみを描画するとサンソン図法が得られる。

サンソン図法で解釈するには、規格化座標をそれぞれ$\theta,\phi$に対応づければいい。
$\phi$は一周分以上はいらない点に留意する。


```python
def clipmap(theta, phi, tx, lmt=(None, None)):
	def wrkfunc(x, lmt):
		if lmt is None:
			return x==x
		return ((x>=lmt[0])&(x<=lmt[1]) & (x!=np.nan))
	tb, pb=[wrkfunc(e, lm) for e,lm in zip((theta,phi),lmt)]
	t = np.where(tb & pb)
	return theta[t], phi[t], tx[t]

def asSansonThetaPhi(u,v,txmap):
	theta=v*np.pi
	phi=u/np.sin(theta)*2*np.pi
	return clipmap(theta,phi,txmap,(None,(-np.pi,np.pi)))
```


後半でラクをしたいので、地図を生成する汎用関数を作っておこう。


```python
def dfunc(x, dir, sz):
	# 正規化座標xを指定したdirでalignして画像描画範囲かどうかの判定とともに返す.
	# dirは絶対値が0をどこに描画するかに対応(-1なら0を左上へ、-2なら0を中央へ). 負は逆さまに描画.
	if dir>0:
		x=x+(dir-1)*0.5
	if dir<0:
		x=(-dir-1)*0.5-x
	x=np.round(x*sz).astype(int)
	# 描画先の有効範囲かを計算.
	b=((x>=0)&(x<sz))
	return x, b
	
def makePos(u,v,im,dir):
	# 縦横それぞれ描画先を計算.
	(u, bu), (v,bv) = [dfunc(*xyprm) for xyprm in zip((u,v),dir,im.shape[:2][::-1])]
	# 縦横ともに友好的なピクセルが描画対象.
	p=np.where(bu&bv)
	return u, v, p

def makeMap(u,v,tx,sz,dir=(0,0)):
	# 黒無地の画像を作る.
	im=np.zeros(list(sz[::-1])+[3])
	# 描画先と描画対象ピクセルを計算.
	u,v,p=makePos(u,v,im,dir)
	# 描画対象のみを列挙.
	u,v,tx=[e[*p] for e in (u,v,tx)]
	# 列挙した位置と対応pixelを一気に描画.
	im[v,u]=tx
	return im
```

サンソン図法とyz面への正射図法で投影する関数を定義する。


```python
def sansonProjection(theta, phi, tx, sz=256):
	return makeMap(phi/2/np.pi*np.sin(theta),theta/np.pi,tx,(sz*2,sz),(2,1))

def yzProjection(theta, phi, tx, sz=256):
	theta, phi, tx=clipmap(theta, phi, tx, (None, (-np.pi/2, np.pi/2)))
	y=np.sin(theta)*np.sin(phi)/2
	z=np.cos(theta)/2
	return makeMap(y,z,tx,(sz,sz),(2,-2))
```

さて、テクスチャ生成からここまでを描画する処理を書こう。


```python
u,v,txmap=makeTextureMap()
imUv=makeMap(u,v,txmap,(512,256),(2,1))
theta,phi,txmap=asSansonThetaPhi(u,v,txmap)
imSs=sansonProjection(theta,phi,txmap)
imyz=yzProjection(theta,phi,txmap)
```

処理結果。

サンソン図法:
![merca_imSs.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2902764/5915fec1-a935-95e4-b6bb-8cec7367d891.png)

正射図法:
![merca_imyz.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2902764/fc43ce5a-3e8d-256b-6f83-eb6af50bdf5b.png)


### メルカトル図法と正距方位図法で描画

$\theta,\phi$から各座標上の$u,v$に変換する方法は先ほどと同様にできるので、前記事で求めた各図法への変換計算式に従って$(u,v)$を求め、画像として描画しよう。

あとで最短経路の描画を行うことを考慮して、投影関数を汎用化しておこう。


```python
def projection(u, v, tx, sz, dir, im, wgt, method, loops):
	# 汎用投影関数. methodは0が地図画像生成, 1が開始終了点描画, 2が折れ線描画. 
	# loopsはx,yそれぞれ左右上下端がつながっているかをtrue/false指定する.
	if method==0:
		return makeMap(u, v, tx, sz, dir)
	# 円か線の描画.
	u,v,_=makePos(u, v, im, dir)
	clr=np.array(tx)
	if clr.ndim==1:
		clr=clr[None]+0*u[:,None]
	if method==1:
		# 指定点の数だけ円を描画する.
		for Xc in zip(u,v,clr):
			c=[float(e) for e in Xc[2]]
			cv2.circle(im, Xc[:2], wgt*2, c, -1)
	else:
		# 一連の指定点を折れ線でつなぐ.
		whs=im.shape[:2][::-1]
		for sx,sy,ex,ey,c in zip(u[:-1],v[:-1],u[1:],v[1:],clr[:-1]):
			c=[float(e) for e in c]
			fixs=[]
			for sxy,exy,lp,wh in zip((sx,sy),(ex,ey),loops,whs):
				# loopsがtrueの軸は、端がつながっていることを考慮.
				if lp and abs(exy-sxy)>wh/2:
					fixs.append(sxy + wh * (1 if exy>=sxy else -1))
				else:
					fixs.append(sxy)
			cv2.line(im, fixs, (ex, ey), c, wgt)

def mercatorProjection(theta, phi, tx, sz=256, im=None, wgt=2, method=0):
	v=np.log(np.tan(theta/2))/5
	u=phi/np.pi/2
	if sz is not None:
		sz=(2*sz, sz)
	return projection(u, v, tx, sz, (2,2), im, wgt, method, (True, False))

def azimuthProjection(theta, phi, tx, sz=256, im=None, wgt=2, method=0):
	u=theta*np.cos(phi)/np.pi/2
	v=theta*np.sin(phi)/np.pi/2
	if sz is not None:
		sz=(sz,sz)
	return projection(u, v, tx, sz, (2, -2), im, wgt, method, (False, False))
```

メルカトル図法:
![merca_immr.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2902764/61e2ac42-5d0b-b20e-a8af-04c1c191f495.png)

正距方位図法:
![merca_imaz.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2902764/12574454-35ab-9b23-b57d-067a5d9a727a.png)


ただし、ここで求めた正距方位図法の中心点は極に限定していることに注意である。

## 最短経路のシミュレーション

シミュレーションの準備が整ったので、最短経路を求める関数を実装していこう。
例として、球座標上のある二点を結ぶ最短経路を求めよう。
例題の点はメルカトル図法上で以下のマゼンタ円を始点、青円を終点で規定しよう。


```python
sted=degs([[43,65],[-92,70]])
mmap=mercatorProjection(theta, phi, txmap)
mercatorProjection(*sted, ((1,0,1),(0,0.5,1)), im=mmap, method=1, wgt=4)
```

![merca_sted.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2902764/4e10aba0-c16e-9ba3-3985-345aae2d9dcd.png)


正距方位図法上で中心点からの最短経路は直線で表現できるのだったので、任意の点を中心に据えた描画ができれば問題の半分は解決である。

任意の点を中心として描画するには、前記事であったように三次元座標への変換、座標回転、球座標へ戻すという操作である。
それぞれ定義式に従って関数にしよう。


```python
def Jmat(rad, axis):
	# 指定軸周りの三次元回転に対応する回転行列計算.
	t=rad
	# 二次元上の回転行列をつくる.
	minimat=np.array([[np.cos(t), np.sin(t)],[-np.sin(t), np.cos(t)]])
	# 3次元上の単位行列生成.
	E=np.eye(3)
	# 二次元上の回転行列をどの成分に代入するか.
	poses=np.indices((2,2))
	# axis=0なら、(0,0),(0,1),(1,0),(1,1)を代入先にする。つまりz軸まわりの回転.
	# 一方axis=nなら、代入位置をn足して3で割った余りを代入先にする. これは、代入先を右下方向に回転させる対応になる.
	# それはつまり、axis=1ならx軸、2ならu.y軸まわりの回転に対応する.
	poses=(poses+axis) % 3
	# 代入先を指定して2x2の回転行列成分を書き込む.
	E[poses[0],poses[1]]=minimat
	return E

def Unitary(theta, phi):
	# 座標の三次元回転を与えるユニタリ行列計算.
	# z軸まわりの回転.
	Jz=Jmat(phi, 0)
	# y軸まわりの回転.
	Jy=Jmat(theta, 2)
	# z軸回してからy軸回す.
	return Jy@Jz

def to3dPos(theta, phi):
	px=np.sin(theta)*np.cos(phi)
	py=np.sin(theta)*np.sin(phi)
	pz=np.cos(theta)
	return np.stack((px,py,pz))

def toAzimuth(pos):
	# 指定した三次元座標を球座標に変換.
	theta=np.arccos(np.clip(pos[2],-1,1))
	sq=np.sqrt(1-np.clip(pos[2]*pos[2],0,1))
	# あまりにz軸に近かったら、0と見なす.ためにログっておく.
	bsq=(sq<1e-10)
	cP=pos[0]/np.maximum(sq, 1e-15)
	phi=np.arccos(np.clip(cP,-1,1))
	phi=phi[None]
	# ログった位置に0を代入.
	phi[:,bsq]=0
	phi[0,pos[1]<0]*=-1
	phi=phi[0]
	return theta, phi


def rotateAzimuth(theta, phi, theta0, phi0, anti=False):
	# 指定した球座標から、第三第四引数で指定した座標回転した後の球座標に変換する.
	# theta==theta0,phi==phi0指定で変換後は極になるように回転される.
	# 座標回転に対応するユニタリ行列計算.
	U=Unitary(theta0, phi0)
	# 逆回転は転置に対応する.
	if anti:
		U=U.T
	# xyzにして座標回転.
	poses=U@to3dPos(theta, phi)
	# 角度にもどす.
	rho, psi=toAzimuth(poses)
	return rho, psi

```

この`rotateAzimuth`関数を使えば任意の点を極にする回転ができるので、先ほどの極を中心点とする正距方位図法への変換と組み合わせれば、任意の点を中心にできる。



```python
rho, psi=rotateAzimuth(theta, phi, *sted[:,0])
st_rho, st_phi=rotateAzimuth(*sted, *sted[:,0])
azm=azimuthProjection(rho, psi, txmap)
azimuthProjection(st_rho, st_phi, ((1,0,1),(0,0.5,1)), im=azm, method=1)
```

![merca_azsted.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2902764/43ee4e6c-0b58-885a-74ad-98028d6043b4.png)

球面のテクスチャをみると、メルカトル図法上と同じ点がプロットされていることがわかり、始点が中心点になっていることもわかる。

あとはこの二点を結ぶ線分を式にすれぼ正距方位図法上での最短経路が求まる。
最終的にメルカトル図法上での最短経路が曲線になることを考慮して、線分を細かく刻んだ線素の集まりとして定義する.


```python
# 始点を中心とした正距方位図法として2点を求める.
serp=rotateAzimuth(*sted, *sted[:,0])
# 中心と任意の一点を結ぶ線は、方位は常に一定、緯度方向に進行する.
psil=serp[1][1]
# 線分を32分割した線素の集まりで定義. 各線素の緯度を計算しておく.
rhol=np.linspace(0,1,32)*(serp[0][1]-serp[0][0])+serp[0][0]
# 方位は緯度に依存しないが、要素数を合わせるため0掛け.
psil=psil+rhol*0

# 1つ前のコードで生成した正距方位図法地図に、線分を描画.
azimuthProjection(rhol,psil,(1,0,0),im=azm,method=2)
```

これで、二点を結ぶ最短経路が描画される.

![merca_azln.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2902764/d6b16cef-1d70-a408-e037-9b6154bdd466.png)

次は、元の球座標に逆回転で戻してメルカトル図法として線分を描画すれば、得たい曲線が求まる.

```python
# 元の球座標に逆変換.
tpl=rotateAzimuth(rhol,psil,*sted[:,0],True)
# メルカトル図法として線分を描画.
mercatorProjection(*tpl, (1,0,0), im=mmap, method=2)
```

![merca_mrln.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2902764/d184267f-10a9-67cf-a481-6802e2fcd9eb.png)